Skip to content
Snippets Groups Projects
Commit 544638d7 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Solved a bug in the get_device() function. The function get blocked when no...

Solved a bug in the get_device() function. The function get blocked when no device was found with the desired id.
parent a02dc61e
No related branches found
No related tags found
No related merge requests found
...@@ -390,42 +390,44 @@ CDynamixel *CDynamixelServer::get_device(int dev_id) ...@@ -390,42 +390,44 @@ CDynamixel *CDynamixelServer::get_device(int dev_id)
if(dynamixel==NULL) if(dynamixel==NULL)
{ {
// try to ping the desired device // 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 */ /* 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); if(this->devices[i].id==dev_id)
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)
{ {
device.id=dev_id; this->devices[i].used=true;
device.used=true; updated=true;
this->devices.push_back(device);
} }
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(); 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) ...@@ -570,7 +572,7 @@ void CDynamixelServer::ping(int dev_id,int time)
}while(read<6); }while(read<6);
}catch(CEventTimeoutException &e){ }catch(CEventTimeoutException &e){
this->dynamixel_access.exit(); this->dynamixel_access.exit();
throw; throw e;
} }
} }
this->dynamixel_access.exit(); this->dynamixel_access.exit();
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment