Go to the documentation of this file.00001 #include "tibi_arm_node_driver_node.h"
00002
00003 TibiArmNodeDriverNode::TibiArmNodeDriverNode(ros::NodeHandle &nh) :
00004 iri_base_driver::IriBaseNodeDriver<TibiArmNodeDriver>(nh),
00005 joint_motion_aserver_(public_node_handle_, "joint_motion"),
00006 motion_sequence_aserver_(public_node_handle_, "motion_sequence")
00007 {
00008 XmlRpc::XmlRpcValue files_param_list;
00009 XmlRpc::XmlRpcValue param_list_item;
00010 unsigned int i=0;
00011
00012
00013 this->loop_rate_ = 100;
00014
00015
00016 this->joint_feedback_publisher_ = this->public_node_handle_.advertise<sensor_msgs::JointState>("joint_feedback", 100);
00017
00018
00019
00020
00021
00022
00023
00024
00025 joint_motion_aserver_.registerStartCallback(boost::bind(&TibiArmNodeDriverNode::joint_motionStartCallback, this, _1));
00026 joint_motion_aserver_.registerStopCallback(boost::bind(&TibiArmNodeDriverNode::joint_motionStopCallback, this));
00027 joint_motion_aserver_.registerIsFinishedCallback(boost::bind(&TibiArmNodeDriverNode::joint_motionIsFinishedCallback, this));
00028 joint_motion_aserver_.registerHasSucceedCallback(boost::bind(&TibiArmNodeDriverNode::joint_motionHasSucceedCallback, this));
00029 joint_motion_aserver_.registerGetResultCallback(boost::bind(&TibiArmNodeDriverNode::joint_motionGetResultCallback, this, _1));
00030 joint_motion_aserver_.registerGetFeedbackCallback(boost::bind(&TibiArmNodeDriverNode::joint_motionGetFeedbackCallback, this, _1));
00031 joint_motion_aserver_.start();
00032 motion_sequence_aserver_.registerStartCallback(boost::bind(&TibiArmNodeDriverNode::motion_sequenceStartCallback, this, _1));
00033 motion_sequence_aserver_.registerStopCallback(boost::bind(&TibiArmNodeDriverNode::motion_sequenceStopCallback, this));
00034 motion_sequence_aserver_.registerIsFinishedCallback(boost::bind(&TibiArmNodeDriverNode::motion_sequenceIsFinishedCallback, this));
00035 motion_sequence_aserver_.registerHasSucceedCallback(boost::bind(&TibiArmNodeDriverNode::motion_sequenceHasSucceedCallback, this));
00036 motion_sequence_aserver_.registerGetResultCallback(boost::bind(&TibiArmNodeDriverNode::motion_sequenceGetResultCallback, this, _1));
00037 motion_sequence_aserver_.registerGetFeedbackCallback(boost::bind(&TibiArmNodeDriverNode::motion_sequenceGetFeedbackCallback, this, _1));
00038 motion_sequence_aserver_.start();
00039
00040
00041
00042 files_param_list.setSize(this->driver_.get_num_motion_seq_files());
00043 for(i=0;i<this->driver_.get_num_motion_seq_files();i++)
00044 {
00045 param_list_item=this->driver_.get_motion_seq_file(i);
00046 files_param_list[i]=param_list_item;
00047 }
00048 this->public_node_handle_.setParam("motion_files",files_param_list);
00049 files_param_list.clear();
00050 files_param_list.setSize(this->driver_.get_num_config_files());
00051 for(i=0;i<this->driver_.get_num_config_files();i++)
00052 {
00053 param_list_item=this->driver_.get_config_file(i);
00054 files_param_list[i]=param_list_item;
00055 }
00056 this->public_node_handle_.setParam("config_files",files_param_list);
00057 files_param_list.clear();
00058
00059 this->public_node_handle_.getParam("side_id",this->side_id);
00060 this->JointState_msg_.name.clear();
00061 if(this->side_id=="left")
00062 {
00063 this->JointState_msg_.name.push_back("left_shoulder_yaw");
00064 this->JointState_msg_.name.push_back("left_shoulder_roll");
00065 }
00066 else if(this->side_id=="right")
00067 {
00068 this->JointState_msg_.name.push_back("right_shoulder_yaw");
00069 this->JointState_msg_.name.push_back("right_shoulder_roll");
00070 }
00071 else
00072 {
00073 this->JointState_msg_.name.resize(2);
00074 }
00075 }
00076
00077 void TibiArmNodeDriverNode::mainNodeThread(void)
00078 {
00079 std::vector<double> position(4);
00080 std::vector<double> velocity(4);
00081 unsigned int i=0;
00082
00083
00084 this->driver_.lock();
00085
00086
00087 this->loop_rate_ = this->driver_.get_feedback_rate();
00088
00089
00090 this->JointState_msg_.header.stamp = ros::Time::now();
00091
00092
00093
00094 this->driver_.get_position(position);
00095 this->driver_.get_velocity(velocity);
00096 this->JointState_msg_.position.clear();
00097 this->JointState_msg_.velocity.clear();
00098 for(i=0;i<position.size();i++)
00099 {
00100
00101 this->JointState_msg_.position.push_back(position[i]*3.14159/180.0);
00102 this->JointState_msg_.velocity.push_back(velocity[i]*3.14159/180.0);
00103 }
00104
00105
00106
00107
00108
00109 this->joint_feedback_publisher_.publish(this->JointState_msg_);
00110
00111
00112 this->driver_.unlock();
00113 }
00114
00115 void TibiArmNodeDriverNode::check_motion_sequence_status(diagnostic_updater::DiagnosticStatusWrapper &stat)
00116 {
00117 CEventServer *event_server=CEventServer::instance();
00118
00119 if(driver_.isRunning())
00120 {
00121 if(this->driver_.get_motion_sequence_complete_event_id()!="")
00122 {
00123 if(event_server->event_is_set(this->driver_.get_motion_sequence_complete_event_id()))
00124 {
00125 if(event_server->event_is_set(this->driver_.get_motion_sequence_error_event_id()))
00126 {
00127 stat.summary(1,"Sequence has finished with errors");
00128 stat.add("Reported error",this->driver_.get_motion_sequence_error_message());
00129 }
00130 else
00131 {
00132 stat.summary(0,"Sequence has finished ok");
00133 }
00134 }
00135 else
00136 stat.summary(0,"Sequence is in progress");
00137 }
00138 else
00139 stat.summary(0,"No sequence has been executed");
00140 }
00141 }
00142
00143
00144
00145
00146
00147
00148 void TibiArmNodeDriverNode::joint_motionStartCallback(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal)
00149 {
00150 control_msgs::FollowJointTrajectoryResult result;
00151 std::vector<TPTUD46MotionStep> seq;
00152 unsigned int i=0,j=0;
00153 TPTUD46MotionStep step;
00154
00155
00156 this->driver_.lock();
00157
00158 if( driver_.isRunning() )
00159 {
00160 if(this->motion_sequence_aserver_.isActive())
00161 this->joint_motion_aserver_.setAborted(result,std::string("A motion sequence has been started"));
00162 else
00163 {
00164 try{
00165
00166 for(i=0;i<goal->trajectory.points.size();i++)
00167 {
00168 step.position.resize(2,0.0);;
00169 step.velocity.resize(2,0.0);
00170 for(j=0;j<goal->trajectory.joint_names.size();j++)
00171 {
00172 if(this->side_id=="left")
00173 {
00174 if(goal->trajectory.joint_names[j]=="left_shoulder_yaw")
00175 {
00176 step.position[0]=goal->trajectory.points[i].positions[j]*180.0/3.14159;
00177 step.velocity[0]=goal->trajectory.points[i].velocities[j]*180.0/3.14159;
00178 }
00179 else if(goal->trajectory.joint_names[j]=="left_shoulder_roll")
00180 {
00181 step.position[1]=goal->trajectory.points[i].positions[j]*180.0/3.14159;
00182 step.velocity[1]=goal->trajectory.points[i].velocities[j]*180.0/3.14159;
00183 }
00184 else
00185 this->joint_motion_aserver_.setAborted(result);
00186 }
00187 else if(this->side_id=="right")
00188 {
00189 if(goal->trajectory.joint_names[j]=="right_shoulder_yaw")
00190 {
00191 step.position[0]=goal->trajectory.points[i].positions[j]*180.0/3.14159;
00192 step.velocity[0]=goal->trajectory.points[i].velocities[j]*180.0/3.14159;
00193 }
00194 else if(goal->trajectory.joint_names[j]=="Right_shoulder_roll")
00195 {
00196 step.position[1]=goal->trajectory.points[i].positions[j]*180.0/3.14159;
00197 step.velocity[1]=goal->trajectory.points[i].velocities[j]*180.0/3.14159;
00198 }
00199 else
00200 this->joint_motion_aserver_.setAborted(result);
00201 }
00202 else
00203 this->joint_motion_aserver_.setAborted(result);
00204 }
00205 step.delay=goal->trajectory.points[i].time_from_start.toSec()*1000;
00206 seq.push_back(step);
00207 }
00208 this->driver_.start_motion_sequence(seq);
00209 }catch(CException &e){
00210 this->joint_motion_aserver_.setAborted(result);
00211 }
00212 }
00213 }
00214
00215
00216 this->driver_.unlock();
00217 }
00218
00219 void TibiArmNodeDriverNode::joint_motionStopCallback(void)
00220 {
00221 driver_.lock();
00222
00223 if( driver_.isRunning() )
00224 {
00225 try{
00226 this->driver_.stop();
00227 }catch(CException &e){
00228 this->driver_.unlock();
00229 throw;
00230 }
00231 }
00232
00233 driver_.unlock();
00234 }
00235
00236 bool TibiArmNodeDriverNode::joint_motionIsFinishedCallback(void)
00237 {
00238 CEventServer *event_server=CEventServer::instance();
00239 bool finished=false;
00240
00241 driver_.lock();
00242
00243 if( driver_.isRunning() )
00244 {
00245 finished=event_server->event_is_set(this->driver_.get_motion_sequence_complete_event_id());
00246 }
00247
00248 driver_.unlock();
00249
00250 return finished;
00251 }
00252
00253 bool TibiArmNodeDriverNode::joint_motionHasSucceedCallback(void)
00254 {
00255 CEventServer *event_server=CEventServer::instance();
00256
00257
00258 this->driver_.lock();
00259 bool success=false;
00260
00261 if( driver_.isRunning() )
00262 {
00263 success=!event_server->event_is_set(this->driver_.get_motion_sequence_error_event_id());
00264 }
00265
00266
00267 this->driver_.unlock();
00268
00269 return success;
00270 }
00271
00272 void TibiArmNodeDriverNode::joint_motionGetResultCallback(control_msgs::FollowJointTrajectoryResultPtr& result)
00273 {
00274 CEventServer *event_server=CEventServer::instance();
00275
00276
00277 this->driver_.lock();
00278
00279 if( driver_.isRunning() )
00280 {
00281 if(event_server->event_is_set(this->driver_.get_motion_sequence_error_event_id()))
00282 result->error_code=-1;
00283 else
00284 result->error_code=0;
00285 }
00286
00287
00288 this->driver_.unlock();
00289 }
00290
00291 void TibiArmNodeDriverNode::joint_motionGetFeedbackCallback(control_msgs::FollowJointTrajectoryFeedbackPtr& feedback)
00292 {
00293 std::vector<double> position,velocity;
00294 unsigned int i=0;
00295
00296
00297 this->driver_.lock();
00298
00299 if( driver_.isRunning() )
00300 {
00301 try{
00302 this->driver_.get_position(position);
00303 this->driver_.get_velocity(velocity);
00304 for(i=0;i<position.size();i++)
00305 {
00306 feedback->actual.positions.push_back(position[i]);
00307 feedback->actual.velocities.push_back(velocity[i]);
00308 }
00309 }catch(CException &e){
00310 this->driver_.unlock();
00311 throw;
00312 }
00313 }
00314
00315
00316 this->driver_.unlock();
00317 }
00318
00319 void TibiArmNodeDriverNode::motion_sequenceStartCallback(const tibi_dabo_msgs::sequenceGoalConstPtr& goal)
00320 {
00321 std::string file;
00322 tibi_dabo_msgs::sequenceResult result;
00323
00324
00325 this->driver_.lock();
00326
00327 if( driver_.isRunning() )
00328 {
00329 if(this->joint_motion_aserver_.isActive())
00330 {
00331 result.successful.push_back(false);
00332 result.observations.push_back(std::string("A joint motion is in progress"));
00333 this->motion_sequence_aserver_.setAborted(result);
00334 }
00335 else
00336 {
00337 try{
00338
00339 ROS_INFO("Arm motion sequence file %s",goal->sequence_file[0].c_str());
00340 file=goal->sequence_file[0];
00341 this->driver_.start_motion_sequence(file);
00342 }catch(CException &e){
00343 result.successful.push_back(false);
00344 result.observations.push_back(e.what());
00345 this->motion_sequence_aserver_.setAborted(result);
00346 }
00347 }
00348 }
00349
00350
00351 this->driver_.unlock();
00352 }
00353
00354 void TibiArmNodeDriverNode::motion_sequenceStopCallback(void)
00355 {
00356
00357 this->driver_.lock();
00358
00359 if( driver_.isRunning() )
00360 {
00361 try{
00362 this->driver_.stop_motion_sequence();
00363 }catch(CException &e){
00364 this->driver_.unlock();
00365 throw;
00366 }
00367 }
00368
00369
00370 this->driver_.unlock();
00371 }
00372
00373 bool TibiArmNodeDriverNode::motion_sequenceIsFinishedCallback(void)
00374 {
00375 CEventServer *event_server=CEventServer::instance();
00376 bool finished=false;
00377
00378
00379 this->driver_.lock();
00380
00381 if( driver_.isRunning() )
00382 {
00383 finished=event_server->event_is_set(this->driver_.get_motion_sequence_complete_event_id());
00384 }
00385
00386
00387 this->driver_.unlock();
00388
00389 return finished;
00390 }
00391
00392 bool TibiArmNodeDriverNode::motion_sequenceHasSucceedCallback(void)
00393 {
00394 CEventServer *event_server=CEventServer::instance();
00395 bool success=false;
00396
00397
00398 this->driver_.lock();
00399
00400 if( driver_.isRunning() )
00401 {
00402 success=!event_server->event_is_set(this->driver_.get_motion_sequence_error_event_id());
00403 }
00404
00405
00406 this->driver_.unlock();
00407
00408 return success;
00409 }
00410
00411 void TibiArmNodeDriverNode::motion_sequenceGetResultCallback(tibi_dabo_msgs::sequenceResultPtr& result)
00412 {
00413 CEventServer *event_server=CEventServer::instance();
00414
00415
00416 this->driver_.lock();
00417
00418 if( driver_.isRunning() )
00419 {
00420 result->successful.clear();
00421 result->observations.clear();
00422 result->successful.push_back(!event_server->event_is_set(this->driver_.get_motion_sequence_error_event_id()));
00423 result->observations.push_back(this->driver_.get_motion_sequence_error_message());
00424 }
00425
00426
00427 this->driver_.unlock();
00428 }
00429
00430 void TibiArmNodeDriverNode::motion_sequenceGetFeedbackCallback(tibi_dabo_msgs::sequenceFeedbackPtr& feedback)
00431 {
00432
00433 this->driver_.lock();
00434
00435 if( driver_.isRunning() )
00436 {
00437 try{
00438 feedback->percentage.clear();
00439 feedback->percentage.push_back(this->driver_.get_motion_sequence_completed_percentage());
00440 }catch(CException &e){
00441 this->driver_.unlock();
00442 throw;
00443 }
00444 }
00445
00446
00447 this->driver_.unlock();
00448 }
00449
00450
00451
00452 void TibiArmNodeDriverNode::postNodeOpenHook(void)
00453 {
00454 motion_sequence_aserver_.start();
00455 ROS_INFO("TibiArmDriverNode:: Motion sequence server started!");
00456 joint_motion_aserver_.start();
00457 ROS_INFO("TibiArmDriverNode:: Joint motion server started!");
00458 }
00459
00460 void TibiArmNodeDriverNode::preNodeCloseHook(void)
00461 {
00462 if(this->motion_sequence_aserver_.isStarted())
00463 {
00464 this->motion_sequence_aserver_.shutdown();
00465 ROS_INFO("TibiHeadDriverNode:: Motion sequence server stopped!");
00466 }
00467 if(this->joint_motion_aserver_.isStarted())
00468 {
00469 this->joint_motion_aserver_.shutdown();
00470 ROS_INFO("TibiHeadDriverNode:: Joint motion server stopped!");
00471 }
00472
00473 }
00474
00475 void TibiArmNodeDriverNode::addNodeDiagnostics(void)
00476 {
00477 diagnostic_.add("Motion sequence status", this, &TibiArmNodeDriverNode::check_motion_sequence_status);
00478 }
00479
00480 void TibiArmNodeDriverNode::addNodeOpenedTests(void)
00481 {
00482 }
00483
00484 void TibiArmNodeDriverNode::addNodeStoppedTests(void)
00485 {
00486 }
00487
00488 void TibiArmNodeDriverNode::addNodeRunningTests(void)
00489 {
00490 }
00491
00492 void TibiArmNodeDriverNode::reconfigureNodeHook(int level)
00493 {
00494 }
00495
00496 TibiArmNodeDriverNode::~TibiArmNodeDriverNode(void)
00497 {
00498
00499 this->driver_.lock();
00500
00501 if(this->motion_sequence_aserver_.isActive())
00502 {
00503 this->motion_sequence_aserver_.setAborted();
00504 this->driver_.stop_motion_sequence();
00505 }
00506 this->driver_.unlock();
00507 }
00508
00509
00510 int main(int argc,char *argv[])
00511 {
00512 return driver_base::main<TibiArmNodeDriverNode>(argc, argv, "tibi_arm_node_driver_node");
00513 }