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()
class Teleop(object): 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() def _reset_gripper(self, gripper): gripper.reboot() gripper.set_force(10) gripper.set_holding_force(20) gripper.set_dead_band(5) if not gripper.ready(): gripper.calibrate() gripper.set_position(0) def _reset_grippers(self, event): rospy.loginfo('Resetting grippers') self._reset_gripper(self.gripper_right) self._reset_gripper(self.gripper_left) def _enable(self): rospy.loginfo("Enabling robot... ") self.rs.enable() rospy.Timer(rospy.Duration(0.1), self._reset_grippers, oneshot=True) self.status_display.set_image('happy') def _hydra_cb(self, msg): with self.hydra_msg_lock: self.hydra_msg = msg def _main_loop(self, event): if self.rs.state().estop_button == 1: self.status_display.set_image('dead') return else: if not self.rs.state().enabled: self.status_display.set_image('indifferent') with self.hydra_msg_lock: msg = self.hydra_msg self._terminate_if_pressed(msg) self.mover_left.update(False, 1 - self.gripper_left.position() / 100) self.mover_right.update(False, 1 - self.gripper_right.position() / 100) if not self.rs.state().enabled: if msg.paddles[0].buttons[0] or msg.paddles[1].buttons[0]: self._enable() return if not rospy.is_shutdown(): happy0 = self.mover_left.update( msg.paddles[0].buttons[0], 1 - self.gripper_left.position() / 100) happy1 = self.mover_right.update( msg.paddles[1].buttons[0], 1 - self.gripper_right.position() / 100) if happy0 and happy1: self.happy_count += 1 if self.happy_count > 200: self.status_display.set_image('happy') else: self.happy_count = 0 self.status_display.set_image('confused') self.mover_head.parse_joy(msg.paddles[0]) self.gripper_left.set_position( 100 * (1 - msg.paddles[0].trigger)) self.gripper_right.set_position( 100 * (1 - msg.paddles[1].trigger)) def _terminate_if_pressed(self, hydra): if(sum(hydra.paddles[0].buttons[1:] + hydra.paddles[1].buttons[1:])): self._cleanup() def _cleanup(self): rospy.loginfo("Disabling robot... ") self.rs.disable() self.mover_left.stop_thread() self.mover_right.stop_thread()
class Teleop(object): 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() def _reset_gripper(self, gripper): gripper.reboot() gripper.set_force(10) gripper.set_holding_force(20) gripper.set_dead_band(5) if not gripper.ready(): gripper.calibrate() gripper.set_position(0) def _reset_grippers(self, event): rospy.loginfo('Resetting grippers') self._reset_gripper(self.gripper_right) self._reset_gripper(self.gripper_left) def _enable(self): rospy.loginfo("Enabling robot... ") self.rs.enable() rospy.Timer(rospy.Duration(0.1), self._reset_grippers, oneshot=True) self.status_display.set_image('happy') def _hydra_cb(self, msg): with self.hydra_msg_lock: self.hydra_msg = msg def _main_loop(self, event): if self.rs.state().estop_button == 1: self.status_display.set_image('dead') return else: if not self.rs.state().enabled: self.status_display.set_image('indifferent') with self.hydra_msg_lock: msg = self.hydra_msg self._terminate_if_pressed(msg) self.mover_left.update(False, 1 - self.gripper_left.position() / 100) self.mover_right.update(False, 1 - self.gripper_right.position() / 100) if not self.rs.state().enabled: if msg.paddles[0].buttons[0] or msg.paddles[1].buttons[0]: self._enable() return if not rospy.is_shutdown(): happy0 = self.mover_left.update( msg.paddles[0].buttons[0], 1 - self.gripper_left.position() / 100) happy1 = self.mover_right.update( msg.paddles[1].buttons[0], 1 - self.gripper_right.position() / 100) if happy0 and happy1: self.happy_count += 1 if self.happy_count > 200: self.status_display.set_image('happy') else: self.happy_count = 0 self.status_display.set_image('confused') self.mover_head.parse_joy(msg.paddles[0]) self.gripper_left.set_position(100 * (1 - msg.paddles[0].trigger)) self.gripper_right.set_position(100 * (1 - msg.paddles[1].trigger)) def _terminate_if_pressed(self, hydra): if (sum(hydra.paddles[0].buttons[1:] + hydra.paddles[1].buttons[1:])): self._cleanup() def _cleanup(self): rospy.loginfo("Disabling robot... ") self.rs.disable() self.mover_left.stop_thread() self.mover_right.stop_thread()