def joy_cb(self, msg):
# Convenience
side = self.side
b = msg.buttons
lb = self.last_buttons
if (rospy.Time.now() - self.last_hand_cmd) < rospy.Duration(0.1):
return
# Update enabled fingers
self.move_f[0] = self.move_f[0] ^ (b[self.B1[side]] and not lb[self.B1[side]])
self.move_f[1] = self.move_f[1] ^ (b[self.B2[side]] and not lb[self.B2[side]])
self.move_f[2] = self.move_f[2] ^ (b[self.B3[side]] and not lb[self.B3[side]])
self.move_spread = self.move_spread ^ (b[self.B4[side]] and not lb[self.B4[side]])
self.move_all = self.move_all ^ (b[self.B_CENTER[side]] and not lb[self.B_CENTER[side]])
# Check if the deadman is engaged
if msg.axes[self.DEADMAN[side]] < 0.01:
if self.deadman_engaged:
self.hand_cmd.mode = [oro_barrett_msgs.msg.BHandCmd.MODE_VELOCITY] * 4
self.hand_cmd.cmd = [0.0, 0.0, 0.0, 0.0]
self.hand_pub.publish(self.hand_cmd)
self.last_hand_cmd = rospy.Time.now()
self.deadman_engaged = False
self.deadman_max = 0.0
else:
self.handle_hand_cmd(msg)
self.handle_cart_cmd(msg)
self.last_buttons = msg.buttons
self.last_axes = msg.axes
# Broadcast the command if it's defined
if self.cmd_frame:
# Broadcast command frame
tform = toTf(self.cmd_frame)
self.broadcaster.sendTransform(tform[0], tform[1], rospy.Time.now(), self.cmd_frame_id, 'world')
# Broadcast telemanip command
telemanip_cmd = TelemanipCommand()
telemanip_cmd.header.frame_id = 'world'
telemanip_cmd.header.stamp = rospy.Time.now()
telemanip_cmd.posetwist.pose = toMsg(self.cmd_frame)
telemanip_cmd.resync_pose = msg.buttons[self.THUMB_CLICK[side]] == 1
telemanip_cmd.deadman_engaged = self.deadman_engaged
if msg.axes[self.THUMB_Y[side]] > 0.5:
telemanip_cmd.grasp_opening = 0.0
elif msg.axes[self.THUMB_Y[side]] < -0.5:
telemanip_cmd.grasp_opening = 1.0
else:
telemanip_cmd.grasp_opening = 0.5
telemanip_cmd.estop = False # TODO: add master estop control
self.telemanip_cmd_pub.publish(telemanip_cmd)
def publish_cmd(self, resync_pose, augmenter_engaged, grasp_opening, time):
"""publish the raw tf frame and the telemanip command"""
if not self.cmd_frame:
return
# Broadcast command frame
tform = toTf(self.cmd_frame)
self.broadcaster.sendTransform(tform[0], tform[1], time, self.cmd_frame_id, 'world')
# Broadcast telemanip command
telemanip_cmd = TelemanipCommand()
telemanip_cmd.header.frame_id = 'world'
telemanip_cmd.header.stamp = time
telemanip_cmd.posetwist.pose = toMsg(self.cmd_frame)
telemanip_cmd.resync_pose = resync_pose
telemanip_cmd.deadman_engaged = self.deadman_engaged
telemanip_cmd.augmenter_engaged = augmenter_engaged
telemanip_cmd.grasp_opening = grasp_opening
telemanip_cmd.estop = False # TODO: add master estop control
self.telemanip_cmd_pub.publish(telemanip_cmd)
def joy_cb(self, msg):
# Convenience
side = self.side
b = msg.buttons
lb = self.last_buttons
if (rospy.Time.now() - self.last_hand_cmd) < rospy.Duration(0.03):
return
# Update enabled fingers
self.move_f[0] = self.move_f[0] ^ (b[self.B1[side]] and not lb[self.B1[side]])
self.move_f[1] = self.move_f[1] ^ (b[self.B2[side]] and not lb[self.B2[side]])
self.move_f[2] = self.move_f[2] ^ (b[self.B3[side]] and not lb[self.B3[side]])
self.move_spread = self.move_spread ^ (b[self.B4[side]] and not lb[self.B4[side]])
self.move_all = self.move_all ^ (b[self.B_CENTER[side]] and not lb[self.B_CENTER[side]])
# Check if the deadman is engaged
if msg.axes[self.DEADMAN[side]] < 0.01:
if self.deadman_engaged:
self.hand_cmd.mode = [oro_barrett_msgs.msg.BHandCmd.MODE_VELOCITY] * 4
self.hand_cmd.cmd = [0.0, 0.0, 0.0, 0.0]
self.hand_pub.publish(self.hand_cmd)
self.last_hand_cmd = rospy.Time.now()
self.deadman_engaged = False
self.deadman_max = 0.0
else:
self.handle_hand_cmd(msg)
self.handle_cart_cmd(msg)
self.last_buttons = msg.buttons
self.last_axes = msg.axes
# republish markers
for i,m in enumerate(self.master_target_markers.markers):
m.header.stamp = rospy.Time.now()
if (i <3 and not self.move_f[i]) or (i==3 and not self.move_spread):
m.color = self.color_orange
elif not self.move_all:
m.color = self.color_red
else:
if self.deadman_engaged:
m.color = self.color_green
else:
m.color = self.color_gray
if i < 3:
if i != 2:
p = m.pose.position
s = (-1.0 if i == 0 else 1.0)
p.x, p.y, p.z = finger_point(
self.hand_position[3] * s,
l = 0.025 * s)
self.marker_pub.publish(self.master_target_markers)
# Broadcast the command if it's defined
if self.cmd_frame:
# Broadcast command frame
tform = toTf(self.cmd_frame)
self.broadcaster.sendTransform(tform[0], tform[1], rospy.Time.now(), self.cmd_frame_id, 'world')
# Broadcast telemanip command
telemanip_cmd = TelemanipCommand()
telemanip_cmd.header.frame_id = 'world'
telemanip_cmd.header.stamp = rospy.Time.now()
telemanip_cmd.posetwist.pose = toMsg(self.cmd_frame)
telemanip_cmd.resync_pose = msg.buttons[self.THUMB_CLICK[side]] == 1
telemanip_cmd.deadman_engaged = self.deadman_engaged
if msg.axes[self.THUMB_Y[side]] > 0.5:
telemanip_cmd.grasp_opening = 0.0
elif msg.axes[self.THUMB_Y[side]] < -0.5:
telemanip_cmd.grasp_opening = 1.0
else:
telemanip_cmd.grasp_opening = 0.5
telemanip_cmd.estop = False # TODO: add master estop control
self.telemanip_cmd_pub.publish(telemanip_cmd)