diff --git a/src/dynamixel_motor_group.cpp b/src/dynamixel_motor_group.cpp index 4136c561b4cdbf6d9db82ff24e0af19f5271b31a..9c4df814689acf3964ecb370ed8ec3960afcfc0b 100644 --- a/src/dynamixel_motor_group.cpp +++ b/src/dynamixel_motor_group.cpp @@ -674,50 +674,61 @@ void *CDynamixelMotorGroup::sequence_thread(void *param) while(!end) { try{ - try{ - event_id=mtn_seq->event_server->wait_first(events,to); - mtn_seq->motion_access.enter(); - switch(event_id) - { - case -1: mtn_seq->sequence_current_step++; - to=mtn_seq->load_motion(mtn_seq->sequence_current_step); - break; - case 0: /* finish the thread */ - end=true; - break; - case 1: /* start sequence */ - to=mtn_seq->load_motion(mtn_seq->sequence_current_step); - break; - case 2: /* stop sequence */ - mtn_seq->stop(); - mtn_seq->sequence_error_msg="Motion sequence stopped by user"; - mtn_seq->sequence_current_step=0; - to=-1; - if(!mtn_seq->event_server->event_is_set(mtn_seq->sequence_complete_event_id)) - mtn_seq->event_server->set_event(mtn_seq->sequence_complete_event_id); - break; - case 3: /* resume sequence */ - mtn_seq->sequence_current_step++; - to=mtn_seq->load_motion(mtn_seq->sequence_current_step); - break; - } - mtn_seq->motion_access.exit(); - }catch(CEventTimeoutException &e){ - if(!mtn_seq->event_server->event_is_set(mtn_seq->pause_sequence_event_id)) - { - /* load and execute the next step */ - mtn_seq->motion_access.enter(); - mtn_seq->sequence_current_step++; - to=mtn_seq->load_motion(mtn_seq->sequence_current_step); - mtn_seq->motion_access.exit(); - } - else - { - mtn_seq->event_server->reset_event(mtn_seq->pause_sequence_event_id); - to=-1; - } + event_id=mtn_seq->event_server->wait_first(events,to); + mtn_seq->motion_access.enter(); + switch(event_id) + { + case -1: mtn_seq->sequence_current_step++; + to=mtn_seq->load_motion(mtn_seq->sequence_current_step); + break; + case 0: /* finish the thread */ + end=true; + break; + case 1: /* start sequence */ + to=mtn_seq->load_motion(mtn_seq->sequence_current_step); + break; + case 2: /* stop sequence */ + mtn_seq->stop(); + mtn_seq->sequence_error_msg="Motion sequence stopped by user"; + mtn_seq->sequence_current_step=0; + to=-1; + if(!mtn_seq->event_server->event_is_set(mtn_seq->sequence_complete_event_id)) + mtn_seq->event_server->set_event(mtn_seq->sequence_complete_event_id); + break; + case 3: /* resume sequence */ + mtn_seq->sequence_current_step++; + to=mtn_seq->load_motion(mtn_seq->sequence_current_step); + break; + } + mtn_seq->motion_access.exit(); + }catch(CEventTimeoutException &e){ + if(!mtn_seq->event_server->event_is_set(mtn_seq->pause_sequence_event_id)) + { + /* load and execute the next step */ + mtn_seq->motion_access.enter(); + mtn_seq->sequence_current_step++; + try{ + to=mtn_seq->load_motion(mtn_seq->sequence_current_step); + }catch(CException &e){ + to=-1; + mtn_seq->stop(); + mtn_seq->sequence_state=mtn_loaded; + mtn_seq->sequence_error_msg=e.what(); + // signal the user + if(!mtn_seq->event_server->event_is_set(mtn_seq->sequence_error_event_id)) + mtn_seq->event_server->set_event(mtn_seq->sequence_error_event_id); + if(!mtn_seq->event_server->event_is_set(mtn_seq->sequence_complete_event_id)) + mtn_seq->event_server->set_event(mtn_seq->sequence_complete_event_id); + } + mtn_seq->motion_access.exit(); + } + else + { + mtn_seq->event_server->reset_event(mtn_seq->pause_sequence_event_id); + to=-1; } }catch(CException &e){ + to=-1; mtn_seq->motion_access.exit(); mtn_seq->stop(); mtn_seq->sequence_state=mtn_loaded;