sr_subscriber.cpp
Go to the documentation of this file.
00001 
00029 #include <iostream>
00030 
00031 #include "sr_hand/sr_subscriber.h"
00032 #include <boost/algorithm/string.hpp>
00033 
00034 using namespace std;
00035 
00036 namespace shadowrobot
00037 {
00038 
00040 //    CONSTRUCTOR/DESTRUCTOR   //
00042 
00043 SRSubscriber::SRSubscriber( boost::shared_ptr<SRArticulatedRobot> sr_art_robot ) :
00044     n_tilde("~")
00045 {
00046     sr_articulated_robot = sr_art_robot;
00048     // Initialize the subscribers
00050     SRSubscriber::init();
00051 }
00052 
00053 SRSubscriber::~SRSubscriber()
00054 {
00055 }
00056 
00057 void SRSubscriber::init()
00058 {
00059     // subscribe to sendupdate topic, set sendupdateCallback function
00060     std::string prefix;
00061     std::string searched_param;
00062     n_tilde.searchParam("shadowhand_prefix", searched_param);
00063     n_tilde.param(searched_param, prefix, std::string());
00064     std::string full_topic = prefix + "sendupdate";
00065 
00066     sendupdate_sub = node.subscribe(full_topic, 2, &SRSubscriber::sendupdateCallback, this);
00067 
00068     // subscribe to contrlr topic, set contrlrCallback function
00069     full_topic = prefix + "contrlr";
00070     contrlr_sub = node.subscribe(full_topic, 2, &SRSubscriber::contrlrCallback, this);
00071 
00072     // subscribe to config topic, set configCallback function
00073     full_topic = prefix + "config";
00074     config_sub = node.subscribe(full_topic, 2, &SRSubscriber::configCallback, this);
00075 }
00076 
00078 //         CALLBACK            //
00080 void SRSubscriber::sendupdateCallback( const sr_robot_msgs::sendupdateConstPtr& msg )
00081 {
00082     //loop on all the sendupdate messages received (if > 0)
00083     int sendupdate_length = msg->sendupdate_length;
00084     if( sendupdate_length == 0 )
00085     {
00086         ROS_DEBUG("Received empty sendupdate command.");
00087         return;
00088     }
00089     //OK, not empty => loop to process all the sendupdate messages
00090     for( unsigned short index_msg = 0; index_msg < msg->sendupdate_length; ++index_msg )
00091     {
00092         float target = msg->sendupdate_list[index_msg].joint_target;
00093         string sensor_name = msg->sendupdate_list[index_msg].joint_name;
00094 
00095         ROS_DEBUG("Received sendupdate Command [%s : %f]", sensor_name.c_str(), target);
00096         sr_articulated_robot->sendupdate(sensor_name, (double)target);
00097     }
00098 
00099 }
00100 
00101 void SRSubscriber::contrlrCallback( const sr_robot_msgs::contrlrConstPtr& msg )
00102 {
00103 
00104     vector<string> list_of_parameters = msg->list_of_parameters;
00105 
00106     JointControllerData ctrl_data;
00107 
00108     //parses all the parameters transmitted in the msg
00109     for( unsigned int index_param = 0; index_param < msg->length_of_list; ++index_param )
00110     {
00111         //split the string (around ":")
00112         vector<string> splitted_string;
00113         boost::split(splitted_string, msg->list_of_parameters[index_param], boost::is_any_of(":"));
00114 
00115         Parameters param;
00116         param.name = splitted_string[0];
00117         param.value = splitted_string[1];
00118 
00119         ctrl_data.data.push_back(param);
00120     }
00121 
00122     sr_articulated_robot->setContrl(msg->contrlr_name, ctrl_data);
00123 }
00124 
00125 void SRSubscriber::configCallback( const sr_robot_msgs::configConstPtr& msg )
00126 {
00127     ROS_ERROR("Configuration command callback not implemented yet");
00128 }
00129 
00130 } // end namespace
00131 


sr_hand
Author(s): Ugo Cupcic / ugo@shadowrobot.com, contact@shadowrobot.com
autogenerated on Fri Jan 3 2014 12:03:25