The Head "Wobbler" Example randomly moves the head to demonstrate using the head pan .
Sawyer's head can rotate left-to-right in pan mode. The pan motion swings Sawyer's 'face' (a.k.a. screen) to a settable angle, rotating around the vertical axes of the head.
The head wobbler example is intented to be a simple demo whose code can be examined to learn how to use the head and demonstrate the use of the intera_interface Head class.
From an RSDK Shell, run the head_wobbler.py demo from the intera_examples package:
$ rosrun intera_examples head_wobbler.py
Sawyer's head will begin panning left and right to random angles.
Now, let's break down the code.
30 import argparse 31 import random 32 33 import rospy 34 35 import intera_interface 36 37 from intera_interface import CHECK_VERSION
This imports the intera interface for accessing the head class.
40 class Wobbler(object): 41 42 def __init__(self): 43 """ 44 'Wobbles' the head 45 """ 46 self._done = False 47 self._head = intera_interface.Head()
The _head is an object of the Head class within the interface_interface. This creates a subscriber to the topic robot/head/head_state and publishers to the topics robot/head/command_head_pan. The object _head, would be used to control the head panning.
49 # verify robot is enabled 50 print("Getting robot state... ") 51 self._rs = intera_interface.RobotEnable(CHECK_VERSION) 52 self._init_state = self._rs.state().enabled 53 print("Enabling robot... ") 54 self._rs.enable() 55 print("Running. Ctrl-c to quit")
intera_interface.RobotEnable(CHECK_VERSION) checks if the sdk version updated in the settings is compatible with the sdk version loaded on the param server of the Robot and creates an object _rs of the RobotEnable class. The next line sets the _init_state of the robot to its current state. The enable() performs the actual enabling of the robot. It is important to note that the robot should be in enabled state before attempting to move its joints.
57 def clean_shutdown(self): 58 """ 59 Exits example cleanly by moving head to neutral position and 60 maintaining start state 61 """ 62 print("\nExiting example...") 63 if self._done: 64 self.set_neutral() 65 if not self._init_state and self._rs.state().enabled: 66 print("Disabling robot...") 67 self._rs.disable()
This function performs a clean shutdown of the program. The _done variable gets updated once the head finishes its wobbling. Then, the head is moved to its neutral position and checks if the _init_state was in disabled state and the current state is enabled. If so, it resets the robot to the disable state.
69 def set_neutral(self): 70 """ 71 Sets the head back into a neutral pose 72 """ 73 self._head.set_pan(0.0)
The set_pan function moves the head to the desired joint position. It employs a PID position controller to move the head joint to that position. In order to move the head to the neutral position, the pan is set to 0.
75 def wobble(self): 76 self.set_neutral() 77 """ 78 Performs the wobbling 79 """ 80 command_rate = rospy.Rate(1) 81 control_rate = rospy.Rate(100) 82 start = rospy.get_time()
This is where the actual wobbling happens. Before the robot starts to wobble its head, it moves its head to the neutral position.
83 while not rospy.is_shutdown() and (rospy.get_time() - start < 10.0): 84 angle = random.uniform(-2.0, 0.95)
This retrieves a random floating point number between -2.0 and 0.95. These random angles are generated for 10 seconds.
85 while (not rospy.is_shutdown() and 86 not (abs(self._head.pan() - angle) <= 87 intera_interface.HEAD_PAN_ANGLE_TOLERANCE)):
This ensures that it loops till the current head pan position and the random angle generated above is within the
88 self._head.set_pan(angle, speed=0.3, timeout=0) 89 control_rate.sleep() 90 command_rate.sleep()
This publishes the corresponding position and the speed to the
92 self._done = True 93 rospy.signal_shutdown("Example finished.")
Once the wobbling is completed, the
_done variable is updated as True and a shutdown signal is sent.
96 def main(): 97 """RSDK Head Example: Wobbler 98 Nods the head and pans side-to-side towards random angles. 99 Demonstrates the use of the intera_interface.Head class. 100 """ 101 arg_fmt = argparse.RawDescriptionHelpFormatter 102 parser = argparse.ArgumentParser(formatter_class=arg_fmt, 103 description=main.__doc__) 104 parser.parse_args(rospy.myargv()[1:]) 105 106 print("Initializing node... ") 107 rospy.init_node("rsdk_head_wobbler") 108 109 wobbler = Wobbler()
An object of the
Wobbler class is generated and this initializes the head interface, checks for the appropriate version and enables the robot.
111 rospy.on_shutdown(wobbler.clean_shutdown) 112 print("Wobbling... ") 113 wobbler.wobble() 114 print("Done.") 115 116 if __name__ == '__main__': 117 main()
Panning happens when the
wobble() function is called.