send_targets.cpp
Go to the documentation of this file.
00001 
00027 #include <ros/ros.h>
00028 
00029 //we store the publishers in a map.
00030 #include <map>
00031 
00032 //You need to send a Float64 message to the joints.
00033 #include <std_msgs/Float64.h>
00034 
00035 class TargetSender
00036 {
00037 public:
00043   TargetSender()
00044   {
00049     static const std::string joints[20] = {"ffj0", "ffj3", "ffj4",
00050                                            "mfj0", "mfj3", "mfj4",
00051                                            "rfj0", "rfj3", "rfj4",
00052                                            "lfj0", "lfj3", "lfj4", "lfj5",
00053                                            "thj1", "thj2", "thj3", "thj4", "thj5",
00054                                            "wrj1", "wrj2"};
00055 
00056     static const std::string prefix = "/sh_";
00057     static const std::string suffix = "_position_controller/command";
00058 
00059     /*
00060      * We now build the map of publishers: there's one topic per joint.
00061      *  to send a target to a joint, we need to publish on /sh_ffj3_position_controller/command
00062      *  (replacing ffj3 by the other joint names, or position controller by the controller
00063      *  you want to use: you can run rostopic list to see which topics are available).
00064      *
00065      * The topic type is Float64.
00066      */
00067     for( unsigned int index_joint=0; index_joint < 20; ++index_joint )
00068     {
00069       std::string joint_tmp = joints[index_joint];
00070       publisher_map_[joint_tmp] = node_handle_.advertise<std_msgs::Float64>( prefix + joint_tmp + suffix , 2 );
00071     }
00072   };
00073 
00074   ~TargetSender()
00075   {};
00076 
00085   bool sendupdate(std::string joint_name, double target)
00086   {
00087     //first we find the joint in the map (or return an error if we
00088     // can't find it)
00089     std::map<std::string, ros::Publisher>::iterator publisher_iterator;
00090     publisher_iterator = publisher_map_.find(joint_name);
00091 
00092     if( publisher_iterator == publisher_map_.end() )
00093     {
00094       ROS_WARN_STREAM("Joint "<< joint_name << " not found.");
00095       return false;
00096     }
00097 
00098     //now we build the message:
00099     // the target must be send in RADIANS
00100     std_msgs::Float64 msg_to_send;
00101     msg_to_send.data = target * 3.14159 / 360.0;
00102     publisher_iterator->second.publish( msg_to_send );
00103 
00104     return true;
00105   };
00106 
00107 protected:
00109   std::map<std::string, ros::Publisher> publisher_map_;
00111   ros::NodeHandle node_handle_;
00112 };
00113 
00114 
00127 int main(int argc, char **argv)
00128 {
00129   //Initialize the ROS node.
00130   ros::init (argc, argv, "send_targets");
00131 
00132   //Instantiate the target sender
00133   TargetSender target_sender = TargetSender();
00134 
00135   //Builds the vector of targets
00136   static const unsigned int length_targets = 1000;
00137   std::vector<double> targets;
00138 
00139   for( unsigned int i = 0; i < length_targets; ++i)
00140   {
00141     double target = static_cast<double>(i) / 1000.0 * 90.0;
00142     targets.push_back( target );
00143   }
00144 
00145   //Send the targets until the node is killed.
00146   unsigned int step = 0;
00147   while( ros::ok() )
00148   {
00149     target_sender.sendupdate("ffj3", targets[step % length_targets] );
00150     usleep(10000);
00151 
00152     ++ step;
00153   }
00154 
00155   return 0;
00156 }
00157 
00158 /* For the emacs weenies in the crowd.
00159 Local Variables:
00160    c-basic-offset: 2
00161 End:
00162 */


sr_edc_ethercat_drivers
Author(s): Yann Sionneau, Ugo Cupcic / ugo@shadowrobot.com, software@shadowrobot.com
autogenerated on Mon Oct 6 2014 07:38:25