Head Movement Example

Revision as of 13:06, 24 November 2016 by Swang (talk | contribs) (Edit whole page)
Jump to: navigation , search

The Head "Wobbler" Example randomly moves the head to demonstrate using the head pan .

Introduction

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.

Usage

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.

Code Walkthrough

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()
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")
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()
69     def set_neutral(self):
70         """
71         Sets the head back into a neutral pose
72         """
73         self._head.set_pan(0.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()
83         while not rospy.is_shutdown() and (rospy.get_time() - start < 10.0):
84             angle = random.uniform(-2.0, 0.95)


85             while (not rospy.is_shutdown() and
86                    not (abs(self._head.pan() - angle) <=
87                        intera_interface.HEAD_PAN_ANGLE_TOLERANCE)):
88                 self._head.set_pan(angle, speed=0.3, timeout=0)
89                 control_rate.sleep()
90             command_rate.sleep()
92         self._done = True
93         rospy.signal_shutdown("Example finished.")
 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()
111     rospy.on_shutdown(wobbler.clean_shutdown)
112     print("Wobbling... ")
113     wobbler.wobble()
114     print("Done.")
115 
116 if __name__ == '__main__':
117     main()