Head Action Client - Code Walkthrough

From sdk-wiki
Jump to: navigation, search


This example demonstrates ability to move Baxter's head by panning its head to a preset angle at a given velocity and waiting for the robot to complete its trajectory. The code consists of a main() function that initializes the ros node and creates an instance of the HeadClient class. In the __init__() function, a single joint position goal is created. The command() function is then called, which performs a single head pan at a given velocity before proceeding to the next head joint angle.
Interfaces -

  • hc = HeadClient()
  • hc.command(position=0.0, velocity=100.0)
  • hc.wait()

Code Walkthrough

Now, let's break down the code.

33 import sys
34 import argparse
36 import rospy
38 import actionlib
40 from control_msgs.msg import (
41     SingleJointPositionAction,
42     SingleJointPositionGoal,
43 )
45 import baxter_interface
47 from baxter_interface import CHECK_VERSION

This imports the actionlib creating a default client to speak with the Head Action Server. The control_msgs provides the messaging action-goal contract between server and client. The CHECK_VERSION is imported to check if the software running on the robot would be compatible with this local version. It is not necessary to check the version in custom programs.

40 class HeadClient(object):
41     def __init__(self):
42         ns = 'robot/head/head_action'
43         self._client = actionlib.SimpleActionClient(
44             ns,
45             SingleJointPositionAction
46         )
47         self._goal = SingleJointPositionGoal()
49         # Wait 10 Seconds for the head action server to start or exit
50         if not self._client.wait_for_server(rospy.Duration(10.0)):
51             rospy.logerr("Exiting - Head Action Server Not Found")
52             rospy.signal_shutdown("Action Server not found")
53             sys.exit(1)
54         self.clear()

The _goal is an object of the HeadClient class that stores a single goal position. The _client is created from a SimpleActionClient acting on the namespace robot/head/head_action. The client function wait_for_server attempts to wait for 10 seconds for the Head Action server to come up. If no server is detected, this example will report an error and exit. If you see this error, make sure that your Client example node and Server node are speaking to the same rosmaster.

65     def command(self, position, velocity):
66         self._goal.position = position
67         self._goal.max_velocity = velocity
68         self._client.send_goal(self._goal)

HeadClient.command() takes as arguments a position and velocity. They are packed into the goal object and sent to the Head Action Server.

70     def stop(self):
71         self._client.cancel_goal()
73     def wait(self, timeout=5.0):
74         self._client.wait_for_result(timeout=rospy.Duration(timeout))
75         return self._client.get_result()
77     def clear(self):
78         self._goal = SingleJointPositionGoal()

The HeadClient.stop() function cancels any current goal (whether or not the trajectory has completed). The HeadClient.wait() function will wait for a result from the server up to a specified amount of time before returning. The HeadClient.clear() function resets the goal member variable with default zero values.

80 def main():
81     """RSDK Head Example: Action Client
82     Demonstrates creating a client of the Head Action Server,
83     which enables sending commands of standard action type
84     control_msgs/SingleJointPosition.
85     The example will command the head to a position.
86     Be sure to start Baxter's head_action_server before running this example.
87     """
88     arg_fmt = argparse.RawDescriptionHelpFormatter
89     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
90                                      description=main.__doc__)
91     parser.parse_args(rospy.myargv()[1:])

The beginning of the main function parses any parameters passed into this example when it is invoked from the command line.

 95     print("Initializing node... ")
 96     rospy.init_node("rsdk_head_action_client")
 97     print("Getting robot state... ")
 98     rs = baxter_interface.RobotEnable(CHECK_VERSION)
 99     print("Enabling robot... ")
100     rs.enable()
101     print("Running. Ctrl-c to quit")

Here, the ROS node is initialized - a requirement for your code to communicate over ROS. Additionally, the version of your SDK is checked against the version running on the robot, and the robot is enabled via the standard RobotEnable function from baxter_interface.

103     hc = HeadClient()
104     hc.command(position=0.0, velocity=100.0)
105     hc.wait()
106     hc.command(position=1.57, velocity=10.0)
107     hc.wait()
108     hc.command(position=0.0, velocity=80.0)
109     hc.wait()
110     hc.command(position=-1.0, velocity=40.0)
111     hc.wait()
112     hc.command(position=0.0, velocity=60.0)
113     print hc.wait()
114     print "Exiting - Head Action Test Example Complete"
116 if __name__ == "__main__":
117     main()

This section of main performs the meat and potatoes of head movement. The HeadClient is created, and a series of commands are presented to the Head Action Server, with waits in between to ensure the goal state is reached before moving onto the next goal.