tibi_arm_node_driver_node.cpp
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   //init class attributes if necessary
00013   this->loop_rate_ = 100;//in [Hz]
00014 
00015   // [init publishers]
00016   this->joint_feedback_publisher_ = this->public_node_handle_.advertise<sensor_msgs::JointState>("joint_feedback", 100);
00017   
00018   // [init subscribers]
00019   
00020   // [init services]
00021   
00022   // [init clients]
00023   
00024   // [init action servers]
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   // [init action clients]
00041   // [init action clients]
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   //lock access to driver if necessary
00084   this->driver_.lock();
00085 
00086   // update the loop rate
00087   this->loop_rate_ = this->driver_.get_feedback_rate();//in [Hz]
00088 
00089   // [fill msg Header if necessary]
00090   this->JointState_msg_.header.stamp = ros::Time::now();
00091   //<publisher_name>.header.frame_id = "<publisher_topic_name>";
00092 
00093   // [fill msg structures]
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     // convert from degrees to radians
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   // [fill srv structure and make request to the server]
00105 
00106   // [fill action structure and make request to the action server]
00107 
00108   // [publish messages]
00109   this->joint_feedback_publisher_.publish(this->JointState_msg_);
00110 
00111   //unlock access to driver if previously blocked
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 /*  [subscriber callbacks] */
00144 
00145 /*  [service callbacks] */
00146 
00147 /*  [action callbacks] */
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   //lock access to driver if necessary
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         // generate and execute the motion sequence
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   //unlock access to driver if necessary
00216   this->driver_.unlock();
00217 }
00218 
00219 void TibiArmNodeDriverNode::joint_motionStopCallback(void) 
00220 { 
00221   driver_.lock(); 
00222     //stop action 
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   //lock access to driver if necessary
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   //unlock access to driver if necessary
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   //lock access to driver if necessary
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   //unlock access to driver if necessary
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   //lock access to driver if necessary
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   //unlock access to driver if necessary
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   //lock access to driver if necessary
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         // add the full path to the incomming filename
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   //unlock access to driver if necessary
00351   this->driver_.unlock();
00352 } 
00353 
00354 void TibiArmNodeDriverNode::motion_sequenceStopCallback(void) 
00355 { 
00356   //lock access to driver if necessary
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   //unlock access to driver if necessary
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   //lock access to driver if necessary
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   //unlock access to driver if necessary
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   //lock access to driver if necessary
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   //unlock access to driver if necessary
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   //lock access to driver if necessary
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   //unlock access to driver if necessary
00427   this->driver_.unlock();
00428 } 
00429 
00430 void TibiArmNodeDriverNode::motion_sequenceGetFeedbackCallback(tibi_dabo_msgs::sequenceFeedbackPtr& feedback) 
00431 { 
00432   //lock access to driver if necessary
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   //unlock access to driver if necessary
00447   this->driver_.unlock();
00448 }
00449 
00450 /*  [action requests] */
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 //  ros::shutdown();
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   // [free dynamic memory]
00499   this->driver_.lock();
00500   // [free dynamic memory]
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 /* main function */
00510 int main(int argc,char *argv[])
00511 {
00512   return driver_base::main<TibiArmNodeDriverNode>(argc, argv, "tibi_arm_node_driver_node");
00513 }


tibi_arm_node
Author(s): Sergi Hernandez Juan
autogenerated on Fri Dec 6 2013 22:58:42