test_joint_movement.cpp
Go to the documentation of this file.
00001 
00027 #include "sr_self_test/test_joint_movement.hpp"
00028 #include <stdio.h>
00029 #include "boost/iostreams/device/file_descriptor.hpp"
00030 #include "boost/iostreams/stream.hpp"
00031 
00032 
00033 namespace shadow_robot
00034 {
00035   TestJointMovement::TestJointMovement(std::string joint_name, shadowrobot::HandCommander* hand_commander)
00036     : mse(0.0), nh_tilde_("~")
00037   {
00038     joint_name_ = joint_name;
00039 
00040     //subscribes to the mean square error (published by movement pub below)
00041     mse_sub_ = nh_tilde_.subscribe("mse_out", 1, &TestJointMovement::mse_cb_, this);
00042 
00043     //initialises the movement publisher
00044     std::string img_path;
00045     nh_tilde_.getParam("image_path", img_path);
00046 
00047     mvt_from_img_.reset(new shadowrobot::MovementFromImage(img_path) );
00048 
00049     double publish_rate;
00050     unsigned int repetition, nb_mvt_step;
00051     publish_rate = 100.0;
00052     repetition = 1;
00053     nb_mvt_step = 10000;
00054 
00055     if( hand_commander != NULL )
00056       hand_commander_.reset(hand_commander);
00057     else
00058       hand_commander_.reset(new shadowrobot::HandCommander());
00059 
00060     std::string controller_state_topic = hand_commander_->get_controller_state_topic(joint_name);
00061     std::string controller_state_topic_type = get_ROS_topic_type(controller_state_topic);
00062     std::string controller_type = "";
00063     if(controller_state_topic_type.compare("pr2_controllers_msgs/JointControllerState") == 0)
00064     {
00065       controller_type = "pr2";
00066     }
00067     else if(controller_state_topic_type.compare("sr_robot_msgs/JointControllerState") == 0)
00068     {
00069       controller_type = "sr";
00070     }
00071     else
00072     {
00073       ROS_ERROR_STREAM("Unknown controller state type: " << controller_state_topic_type << " trying to use pr2 type instead");
00074       controller_type = "pr2";
00075     }
00076 
00077     mvt_pub_.reset(new shadowrobot::MovementPublisher(joint_name, publish_rate, repetition,
00078                                                       nb_mvt_step, controller_type, false,
00079                                                       hand_commander ));
00080     mvt_pub_->add_movement( *mvt_from_img_.get() );
00081 
00082     if(controller_type.compare("pr2") == 0)
00083     {
00084       pr2_sub_state_ = nh_tilde_.subscribe( mvt_pub_->get_subscriber_topic(), nb_mvt_step,
00085                                             &TestJointMovement::pr2_state_cb_, this );
00086     }
00087     else
00088     {
00089       sub_state_ = nh_tilde_.subscribe( mvt_pub_->get_subscriber_topic(), nb_mvt_step,
00090                                         &TestJointMovement::state_cb_, this );
00091     }
00092 
00093     mvt_pub_->start();
00094   }
00095 
00096   void TestJointMovement::state_cb_(const sr_robot_msgs::JointControllerState::ConstPtr& msg)
00097   {
00098     values[joint_name_ +" position"].push_back(msg->process_value);
00099     values[joint_name_ +" target"].push_back(msg->set_point);
00100     values[joint_name_ +" error"].push_back(msg->error);
00101   }
00102 
00103   void TestJointMovement::pr2_state_cb_(const pr2_controllers_msgs::JointControllerState::ConstPtr& msg)
00104   {
00105     values[joint_name_ +" position"].push_back(msg->process_value);
00106     values[joint_name_ +" target"].push_back(msg->set_point);
00107     values[joint_name_ +" error"].push_back(msg->error);
00108   }
00109 
00110   void TestJointMovement::mse_cb_(const std_msgs::Float64::ConstPtr& msg)
00111   {
00112     mse = msg->data;
00113 
00114     //unsubscribe after receiving the message
00115     mse_sub_.shutdown();
00116     sub_state_.shutdown();
00117   }
00118 
00119   std::string TestJointMovement::get_ROS_topic_type(std::string topic_name)
00120   {
00121     typedef boost::iostreams::file_descriptor_source boost_fd;
00122     typedef boost::iostreams::stream<boost_fd> boost_stream; 
00123     
00124     FILE *myfile;
00125     std::string cmd;
00126 
00127     cmd = "rostopic type " + topic_name;
00128 
00129     // make sure to popen and it succeeds
00130     if(!(myfile = popen(cmd.c_str(), "r")))
00131     {
00132       ROS_ERROR_STREAM("Command failed: " << cmd);
00133     }
00134 
00135     boost_fd fd(fileno(myfile), boost::iostreams::never_close_handle);
00136     boost_stream stream(fd);
00137     //stream.set_auto_close(false); // https://svn.boost.org/trac/boost/ticket/3517
00138     std::string topic_type;
00139     if( !std::getline(stream,topic_type))
00140     {
00141       ROS_ERROR_STREAM("Could nod read line from get_ROS_topic_type command");
00142     }
00143 
00144     pclose(myfile);
00145     return topic_type;
00146   }
00147 }
00148 
00149 /* For the emacs weenies in the crowd.
00150    Local Variables:
00151    c-basic-offset: 2
00152    End:
00153 */
00154 


sr_self_test
Author(s): Ugo Cupcic
autogenerated on Mon Oct 6 2014 07:52:52