def __init__(self):
rospy.init_node("baxter_hydra_teleop")
self.status_display = baxter_faces.FaceImage()
rospy.loginfo("Getting robot state... ")
self.rs = baxter_interface.RobotEnable()
self.hydra_msg = Hydra()
self.hydra_msg_lock = threading.Lock()
self.gripper_left = baxter_interface.Gripper("left")
self.gripper_right = baxter_interface.Gripper("right")
self.mover_left = LimbMover("left")
self.mover_right = LimbMover("right")
self.mover_head = HeadMover()
self.happy_count = 0 # Need inertia on how long unhappy is displayed
self.hydra_msg = Hydra()
rospy.on_shutdown(self._cleanup)
rospy.Subscriber("/hydra_calib", Hydra, self._hydra_cb)
rospy.Timer(rospy.Duration(1.0 / 30), self._main_loop)
rospy.loginfo(
"Press left or right button on Hydra to start the teleop")
while not self.rs.state().enabled and not rospy.is_shutdown():
rospy.Rate(10).sleep()
self.mover_left.enable()
self.mover_right.enable()
self.mover_head.set_pose()
