diff --git a/src/dynamixelserver.cpp b/src/dynamixelserver.cpp index c34965e2ca875498acc301906d5ec6d2cb312bde..dfea759629db4d2296396b9186fac726b4dcd6ef 100644 --- a/src/dynamixelserver.cpp +++ b/src/dynamixelserver.cpp @@ -390,42 +390,44 @@ CDynamixel *CDynamixelServer::get_device(int dev_id) if(dynamixel==NULL) { // try to ping the desired device - try{ - if(this->bus_info.baud_rate==-1) - { + if(this->bus_info.baud_rate==-1) + { + /* handle exceptions */ + throw CDynamixelServerException(_HERE_,"No baudrate has been selected."); + } + else + { + try{ + this->ping(dev_id,500); +// }catch(CEventTimeoutException &e){ + }catch(...){ /* handle exceptions */ - throw CDynamixelServerException(_HERE_,"No baudrate has been selected."); + throw CDynamixelServerException(_HERE_,"No Dynamixel device found with the specified identifier"); } - else + this->dynamixel_access.enter(); + device_name.str(""); + device_name << "dynamixel_bus_" << this->bus_info.bus_id << "_dev_" << dev_id; + name=device_name.str(); + dynamixel=new CDynamixel(name); + dynamixel->usb_dev=this->comm_dev; + dynamixel->usb_access=&this->dynamixel_access; + dynamixel->node_address=dev_id; + dynamixel->usb_rx_event_id=this->comm_dev->get_rx_event_id(); + for(i=0;i<devices.size();i++) { - this->ping(dev_id,500); - this->dynamixel_access.enter(); - device_name.str(""); - device_name << "dynamixel_bus_" << this->bus_info.bus_id << "_dev_" << dev_id; - name=device_name.str(); - dynamixel=new CDynamixel(name); - dynamixel->usb_dev=this->comm_dev; - dynamixel->usb_access=&this->dynamixel_access; - dynamixel->node_address=dev_id; - dynamixel->usb_rx_event_id=this->comm_dev->get_rx_event_id(); - for(i=0;i<devices.size();i++) - if(this->devices[i].id==dev_id) - { - this->devices[i].used=true; - updated=true; - } - if(!updated) + if(this->devices[i].id==dev_id) { - device.id=dev_id; - device.used=true; - this->devices.push_back(device); + this->devices[i].used=true; + updated=true; } - this->dynamixel_access.exit(); } - }catch(CEventTimeoutException &e){ + if(!updated) + { + device.id=dev_id; + device.used=true; + this->devices.push_back(device); + } this->dynamixel_access.exit(); - /* handle exceptions */ - throw CDynamixelServerException(_HERE_,"No Dynamixel device found with the specified identifier"); } } @@ -570,7 +572,7 @@ void CDynamixelServer::ping(int dev_id,int time) }while(read<6); }catch(CEventTimeoutException &e){ this->dynamixel_access.exit(); - throw; + throw e; } } this->dynamixel_access.exit();