Introduction
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
35
36 import rospy
37
38 import actionlib
39
40 from control_msgs.msg import (
41 SingleJointPositionAction,
42 SingleJointPositionGoal,
43 )
44
45 import baxter_interface
46
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()
48
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()
72
73 def wait(self, timeout=5.0):
74 self._client.wait_for_result(timeout=rospy.Duration(timeout))
75 return self._client.get_result()
76
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"
115
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.
