diff --git a/src/dynamixel_motor.cpp b/src/dynamixel_motor.cpp index 662ca88094931bb197df49cddcca4d079deb9c06..974c13a92de957fafd3e7821b4b9ce74fa39c4ae 100644 --- a/src/dynamixel_motor.cpp +++ b/src/dynamixel_motor.cpp @@ -1468,9 +1468,13 @@ control_mode CDynamixelMotor::get_control_mode(void) CDynamixelMotor::~CDynamixelMotor() { /* stop the motor */ - this->stop(); - /* disable the motor */ - this->disable(); + try{ + this->stop(); + /* disable the motor */ + this->disable(); + }catch(CException &e){ + /* do nothing */ + } if(this->dynamixel_dev!=NULL) { this->dyn_server->free_device(this->dynamixel_dev->get_id()); diff --git a/src/dynamixel_motor_group.cpp b/src/dynamixel_motor_group.cpp index 276cc36728d92c40fe9a17aed34fb7e94852b68a..f612e3d22e871b11f81a1d3194a3d37591220429 100644 --- a/src/dynamixel_motor_group.cpp +++ b/src/dynamixel_motor_group.cpp @@ -992,6 +992,7 @@ TDynamixelGroup_config CDynamixelMotorGroup::read_config(std::string &filename) std::auto_ptr<dyn_motor_group_config_t> cfg(dyn_motor_group_config(filename.c_str(), xml_schema::flags::dont_validate)); config.bus_id=cfg->bus_id(); config.baudrate=cfg->baudrate(); + config.id.resize(cfg->dyn_motor_config().size()); config.dyn_config.resize(cfg->dyn_motor_config().size()); config.pid_control.resize(cfg->dyn_motor_config().size()); config.compliance_control.resize(cfg->dyn_motor_config().size()); @@ -999,7 +1000,7 @@ TDynamixelGroup_config CDynamixelMotorGroup::read_config(std::string &filename) path=filename.substr(0,found+1); for(iterator=cfg->dyn_motor_config().begin(),i=0;iterator!=cfg->dyn_motor_config().end();iterator++,i++) { - config.bus_id[i]=iterator->id(); + config.id[i]=iterator->id(); full_path=path+iterator->config_file(); CDynamixelMotor::read_config(full_path,config.dyn_config[i],config.compliance_control[i],config.pid_control[i]); } @@ -1947,10 +1948,15 @@ double CDynamixelMotorGroup::get_sequence_completed_percentage(void) CDynamixelMotorGroup::~CDynamixelMotorGroup() { /* stop the motor */ - this->stop(); - /* stop any active sequence */ - if(!this->event_server->event_is_set(this->sequence_complete_event_id)) - this->stop_sequence(); + try{ + this->stop(); + /* stop any active sequence */ + if(!this->event_server->event_is_set(this->sequence_complete_event_id)) + this->stop_sequence(); + this->disable(); + }catch(CException &e){ + /* do nothing */ + } /* stop the internal trhead */ this->event_server->set_event(this->finish_thread_event_id); this->thread_server->end_thread(this->sequence_thread_id); @@ -1972,6 +1978,5 @@ CDynamixelMotorGroup::~CDynamixelMotorGroup() this->sequence_complete_event_id=""; this->event_server->delete_event(this->sequence_error_event_id); this->sequence_error_event_id=""; - this->disable(); this->clear(); }