[Documentation] [TitleIndex] [WordIndex

(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Link two joints [cpp]

Description: Reads the current position from one joint and publishes it to another joint.

Tutorial Level:

Reading and sending data from Cpp

This is a simple program that links one finger joint to another: it subscribes to the topic publishing a parent joints position data, and publishes the data of the selected parent joint as a target for it's child joint. This will simply make the child joint move together with the parent joint.

NB: To send a new target to a joint, you simply need to publish a std_msgs/Float64 message to the appropriate controllers command topic.

The full list of joints to send targets to the hand is: wrj1, wrj2, ffj4, ffj3, ffj0, mfj4, mfj3, mfj0, rfj4, rfj3, rfj0, lfj5, lfj4, lfj3, lfj0, thj5, thj4, thj3, thj2, thj1.

   1 #include <ros/ros.h>
   2 #include <string>
   3 #include <std_msgs/Float64.h>
   4 #include <pr2_controllers_msgs/JointControllerState.h>
   5 
   6 std::string parent_name = "ffj3";
   7 std::string child_name  = "mfj3";
   8 ros::Subscriber sub;
   9 ros::Publisher pub;
  10 
  11 void callback(const pr2_controllers_msgs::JointControllerStateConstPtr& msg)
  12 {
  13   //publish the message
  14   std_msgs::Float64 command;
  15   command.data = msg->set_point;
  16   pub.publish(command);
  17 }
  18 
  19 int main(int argc, char** argv)
  20 {
  21   //init the ros node
  22   ros::init(argc, argv, "link_joints_example");
  23   ros::NodeHandle node;
  24 
  25   pub = node.advertise<std_msgs::Float64>("sh_" + child_name + "_position_control/command", 2);
  26   sub = node.subscribe("sh_" + parent_name + controller_type + "/state", 2,  callback);
  27 
  28   while( ros::ok() )
  29     ros::spin();
  30 
  31   return 0;
  32 }

Let's look at this code.

   1 #include <std_msgs/Float64.h>
   2 #include <pr2_controllers_msgs/JointControllerState.h>
   3 

   1 int main(int argc, char** argv)
   2 {
   3   [...]
   4     //the publisher
   5     pub = node.advertise<std_msgs::Float64>("sh_" + child_name + "_posion_control/command", 2);
   6     // the subscriber
   7     sub = node.subscribe("sh_" + parent_name + controller_type + "/state", 2,  callback);
   8   [...]
   9 }

   1 void callback(const pr2_controllers_msgs::JointControllerStateConstPtr& msg)
   2 {
   3   //publish the message
   4   std_msgs::Float64 command;
   5   command.data = msg->set_point;
   6   pub.publish(command);
   7 }

Compiling the Code

Now that we've written the code, we need to compile it. More information regarding how to properly compile using the catkin build system can be found here In the CMakeLists.txt, you'll need to add the following lines:

add_executable(link_joints src/link_joints.cpp)
add_dependencies(link_joints ${catkin_EXPORTED_TARGETS})
target_link_libraries(link_joints ${Boost_LIBRARIES} ${catkin_LIBRARIES})

You're now ready to compile using catkin_make in the root of your workspace.

$ roscd
$ cd ..
$ catkin_make

Running the example

To test this example, you need to first start the simulator:

$ roslaunch sr_hand gazebo_arm_and_hand.launch

Then we run this code (which can be found in the sr_example package).

$ rosrun sr_example link_joints

Now if you send a target to FFJ3, MFJ3 will follow:

$ rostopic pub /sh_ffj3_position_controller/command std_msgs/Float64 1.5

Please note that you can find more examples in the sr_example package.


2019-09-14 13:15