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 __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()
