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
00042
00043 SRSubscriber::SRSubscriber( boost::shared_ptr<SRArticulatedRobot> sr_art_robot ) :
00044 n_tilde("~")
00045 {
00046 sr_articulated_robot = sr_art_robot;
00047
00049
00051 SRSubscriber::init();
00052 }
00053
00054 SRSubscriber::~SRSubscriber()
00055 {
00056
00057
00058 }
00059
00060 void SRSubscriber::init()
00061 {
00062
00063 std::string prefix;
00064 std::string searched_param;
00065 n_tilde.searchParam("shadowhand_prefix", searched_param);
00066 n_tilde.param(searched_param, prefix, std::string());
00067 std::string full_topic = prefix + "sendupdate";
00068
00069 sendupdate_sub = node.subscribe(full_topic, 2, &SRSubscriber::sendupdateCallback, this);
00070
00071
00072 full_topic = prefix + "contrlr";
00073 contrlr_sub = node.subscribe(full_topic, 2, &SRSubscriber::contrlrCallback, this);
00074
00075
00076 full_topic = prefix + "config";
00077 config_sub = node.subscribe(full_topic, 2, &SRSubscriber::configCallback, this);
00078 }
00079
00081
00083 void SRSubscriber::sendupdateCallback( const sr_robot_msgs::sendupdateConstPtr& msg )
00084 {
00085
00086 int sendupdate_length = msg->sendupdate_length;
00087 if( sendupdate_length == 0 )
00088 {
00089 ROS_WARN("Received empty sendupdate command.");
00090 return;
00091 }
00092
00093 for( unsigned short index_msg = 0; index_msg < msg->sendupdate_length; ++index_msg )
00094 {
00095 float target = msg->sendupdate_list[index_msg].joint_target;
00096 string sensor_name = msg->sendupdate_list[index_msg].joint_name;
00097
00098 ROS_DEBUG("Received sendupdate Command [%s : %f]", sensor_name.c_str(), target);
00099 sr_articulated_robot->sendupdate(sensor_name, (double)target);
00100 }
00101
00102 }
00103
00104 void SRSubscriber::contrlrCallback( const sr_robot_msgs::contrlrConstPtr& msg )
00105 {
00106
00107 vector<string> list_of_parameters = msg->list_of_parameters;
00108
00109 JointControllerData ctrl_data;
00110
00111
00112 for( unsigned int index_param = 0; index_param < msg->length_of_list; ++index_param )
00113 {
00114
00115 vector<string> splitted_string;
00116 boost::split(splitted_string, msg->list_of_parameters[index_param], boost::is_any_of(":"));
00117
00118 Parameters param;
00119 param.name = splitted_string[0];
00120 param.value = splitted_string[1];
00121
00122 ctrl_data.data.push_back(param);
00123 }
00124
00125 sr_articulated_robot->setContrl(msg->contrlr_name, ctrl_data);
00126 }
00127
00128 void SRSubscriber::configCallback( const sr_robot_msgs::configConstPtr& msg )
00129 {
00130 ROS_ERROR("Configuration command callback not implemented yet");
00131 }
00132
00133 }