def callback(self, twist):
msg = JogFrame()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = rospy.get_param('~frame_id', 'base_link')
msg.group_name = rospy.get_param('~group_name', 'manipulator')
msg.link_name = rospy.get_param('~link_name', 'tool0')
if rospy.get_param('~dominant_mode', True):
twist = self.dominantAxisMode(twist)
if rospy.get_param('~axes_remap', True):
twist = self.axesRemap(twist)
msg.angular_delta.x = self.scale_angular['x'] * twist.angular.x
msg.angular_delta.y = self.scale_angular['y'] * twist.angular.y
msg.angular_delta.z = self.scale_angular['z'] * twist.angular.z
msg.linear_delta.x = self.scale_linear['x'] * twist.linear.x
msg.linear_delta.y = self.scale_linear['y'] * twist.linear.y
msg.linear_delta.z = self.scale_linear['z'] * twist.linear.z
msg.avoid_collisions = True
self.pub.publish(msg)
def callback(self, joy):
if not joy.buttons[self.enable_button]:
return
msg = JogFrame()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = rospy.get_param('~frame_id', 'base_link')
msg.group_name = rospy.get_param('~group_name', 'manipulator')
msg.link_name = rospy.get_param('~link_name', 'tool0')
if joy.buttons[self.angular_button]:
msg.angular_delta.x = self.scale_angular['x'] * joy.axes[
self.axis_angular['x']]
msg.angular_delta.y = self.scale_angular['y'] * joy.axes[
self.axis_angular['y']]
msg.angular_delta.z = self.scale_angular['z'] * joy.axes[
self.axis_angular['z']]
else:
# These buttons are binary
msg.linear_delta.x = self.scale_linear['x'] * joy.axes[
self.axis_linear['x']]
msg.linear_delta.y = self.scale_linear['y'] * joy.axes[
self.axis_linear['y']]
msg.linear_delta.z = self.scale_linear['z'] * joy.axes[
self.axis_linear['z']]
msg.avoid_collisions = True
self.pub.publish(msg)