|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 [python]Description: Reads the current position from one joint and publishes it to another joint.
Reading and sending data from Python
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 import roslib; roslib.load_manifest('sr_hand') 2 import rospy 3 from pr2_controller_msgs.msg import JointControllerState 4 from std_msgs.msg import Float64 5 6 parent_name = "ffj3" 7 child_name = "mfj3" 8 9 10 def callback(data): 11 """ 12 The callback function: called each time a message is received on the 13 topic parent joint controller state topic 14 15 @param data: the message 16 """ 17 # publish the message to the child joint controller command topic. 18 # here we insert the joint name into the topic name 19 pub = rospy.Publisher('/sh_'+child_name+'_position_controller/command', Float64) 20 pub.publish(data.set_point) 21 22 23 def listener(): 24 """ 25 The main function 26 """ 27 # init the ros node 28 rospy.init_node('joints_link_test', anonymous=True) 29 30 # init the subscriber: subscribe to the 31 # child joint controller topic, using the callback function 32 # callback() 33 rospy.Subscriber('/sh_'+parent_name+'_position_controller/state', JointControllerState, callback) 34 # subscribe until interrupted 35 rospy.spin() 36 37 38 if __name__ == '__main__': 39 listener()
Let's look at this code.
We need to import the messages to send / receive messages from the ROS interface. That's Float64 from std_msgs for the command messages sent to the child joint and jointcontrollerstate from pr2_controller_msgs for the joint status messages from which we extract the parent joint position.
- We subscribe to the parent joint controller state topic which contains information about the current state of the joint, including the current target. Each time we receive a message on this topic, we'll call the callback function.
- In the callback function we create a publisher to the child joint controller command topic then take the set_point target value from the parent joint status message and publish it to the parent command topic:
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.py
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.