[Documentation] [TitleIndex] [WordIndex

Note: This tutorial assumes that you have completed the previous tutorials: Getting Started: Running the Hand and Arm Interface.
(!) 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.

Moving the Robot

Description: First examples on how to interact with the Robot, through a GUI or simple programs.

Tutorial Level: BEGINNER

  Show EOL distros: 

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.

   1 def listener():
   2   [...]
   3     rospy.Subscriber("srh/shadowhand_data", joints_data, callback)
  • 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:

   1 def callback(data):
   2   [...]
   3     for joint_data in data.joints_list:
   4         # if its the parent joint, read the target and send it to the child
   5         if joint_data.joint_name == parent_name:
   6             message.append(joint(joint_name=child_name, joint_target=joint_data.joint_target))
  • At last we send the message to the /srh/sendupdate topic:

   1     pub = rospy.Publisher('srh/sendupdate', sendupdate) 
   2     pub.publish(sendupdate(len(message), message))

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}]}"

Using the GUI

The hand can be controlled from the ROS rqt_gui in a number of ways using the plugins in the sr_visualization stack.

Writing a simple node

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 sr_robot_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+'_mixed_position_velocity_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+'_mixed_position_velocity_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 sr_robot_msgs for the joint status messages from which we extract the parent joint position.

   1 from sr_robot_msgs.msg import JointControllerState
   2 from std_msgs.msg import Float64
  • 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.

   1 def listener():
   2   [...]
   3     rospy.Subscriber('/sh_'+parent_name+'_mixed_position_velocity_controller/state', JointControllerState, callback)
  • 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 topic:

   1 def callback(data):
   2   [...]
   3     pub = rospy.Publisher('/sh_'+child_name+'_mixed_position_velocity_controller/command', Float64)
   4     pub.publish(data.set_point)

Please note that you can find more examples in the sr_hand/examples directory.

Command Line Interactions

You can use the rostopic command to quickly check up on the system. For example the following command will print the current state for the selected joint (in this case ffj0).

$ rostopic echo /sh_ffj0_mixed_position_velocity_controller/state

You can also send a target to the hand like this:

$ rostopic pub /sh_ffj0_mixed_position_velocity_controller/command Float64 0.5

Our stacks have not yet been released under Groovy, please use the Fuerte versions.

Our stacks have not yet been released under Hydro, please use the Fuerte versions.


2019-11-30 13:17