Using the GUI
Please refer to the sr_control_gui wiki page.
Writing a simple node
This is a simple program that links one finger joint to another: it subscribes to the topic publishing the hand 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 at the same time as its parent is moving.
NB: To send a new target to one or more joint(s), you need to publish a sr_robot_msgs/sendupdate message to the robot sendupdate topic. As explained on sr_robot_msgs wiki page, the sendupdate message contains a list of joints. You just need to specify the joint_name and the joint_target for each of those joints.
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 sr_robot_msgs.msg import sendupdate, joint, joints_data
4 from sensor_msgs.msg import *
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 /srh/shadowhand_data
14
15 @param data: the message
16 """
17 message=[]
18 if data.joints_list_length == 0:
19 return
20 # loop on the joints in the message
21 for joint_data in data.joints_list:
22 # if its the parent joint, read the target and send it to the child
23 if joint_data.joint_name == parent_name:
24 message.append(joint(joint_name=child_name, joint_target=joint_data.joint_target))
25
26 # publish the message to the /srh/sendupdate topic.
27 pub = rospy.Publisher('srh/sendupdate', sendupdate)
28 pub.publish(sendupdate(len(message), message))
29
30 def listener():
31 """
32 The main function
33 """
34 # init the ros node
35 rospy.init_node('joints_link_test', anonymous=True)
36
37 # init the subscriber: subscribe to the topic
38 # /srh/shadowhand_data, using the callback function
39 # callback()
40 rospy.Subscriber("srh/shadowhand_data", joints_data, callback)
41
42 # subscribe until interrupted
43 rospy.spin()
44
45
46 if __name__ == '__main__':
47 listener()
Let's look at this code.
We need to import the messages to send / receive messages from the ROS interface. Most of the messages we use are defined in the sr_robot_msgs package.
1 from sr_robot_msgs.msg import sendupdate, joint, joints_data
- We subscribe to the shadowhand_data topic which contains the joint_data, including the current targets for each joints. Each time we receive a message on this topic, we'll call the callback function.
- In the callback function, we loop on the joints contained in the message, until we find the parent joint. When the parent joint is found, we create a sendupdate message to specify the targets for the child:
At last we send the message to the /srh/sendupdate topic:
Please note that you can find more examples in the sr_hand/examples directory.
Command Line Interactions
To quickly check things, you can use the excellent rostopic command. For example the following command will print the current state for the joints of the hands.
$ rostopic echo /srh/shadowhand_data
You can also send a target to the hand like this:
$ rostopic pub /srh/sendupdate sr_robot_msgs/sendupdate "{sendupdate_length: 1 ,sendupdate_list: [{joint_name: FFJ0,joint_target: 180}]}"