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 test_a_jog_with_angular_delta(self):
'''Test to jog a command with angular delta'''
# Get current posture of link_name
(start_pos, start_rot) = self.listener.lookupTransform(
self.frame_id, self.link_name, rospy.Time(0))
# Create jog frame command
jog = JogFrame()
jog.header.stamp = rospy.Time.now()
jog.header.frame_id = self.frame_id
jog.group_name = self.group_name
jog.link_name = self.link_name
jog.angular_delta.x = self.angular_delta / math.sqrt(3.0)
jog.angular_delta.y = self.angular_delta / math.sqrt(3.0)
jog.angular_delta.z = self.angular_delta / math.sqrt(3.0)
self.pub.publish(jog)
rospy.sleep(3.0)
(pos, rot) = self.listener.lookupTransform(
self.frame_id, self.link_name, rospy.Time(0))
# Check if the position is not changed
for i in range(3):
self.assertAlmostEqual(pos[i], start_pos[i], delta=0.0001)
# Check if the rotaion is jogged by delta
jog_q = tf.transformations.quaternion_about_axis(self.angular_delta, [1,1,1])
ans_rot = tf.transformations.quaternion_multiply(jog_q, start_rot)
for i in range(4):
self.assertAlmostEqual(rot[i], ans_rot[i], delta=0.0001)
def test_ten_jogs_with_linear_delta(self):
'''Test to jog ten commands with linear delta'''
# Get current posture of link_name
(start_pos, start_rot) = self.listener.lookupTransform(
self.frame_id, self.link_name, rospy.Time(0))
for i in range(10):
# Create jog frame command
jog = JogFrame()
jog.header.stamp = rospy.Time.now()
jog.header.frame_id = self.frame_id
jog.group_name = self.group_name
jog.link_name = self.link_name
jog.linear_delta.x = self.linear_delta
jog.linear_delta.y = self.linear_delta
jog.linear_delta.z = self.linear_delta
self.pub.publish(jog)
rospy.sleep(3.0)
(pos, rot) = self.listener.lookupTransform(
self.frame_id, self.link_name, rospy.Time(0))
# Check if the position is jogged by delta
for i in range(3):
self.assertAlmostEqual(pos[i], start_pos[i] + self.linear_delta*10, delta=0.0001)
# Check if the rotaion is not changed
for i in range(4):
self.assertAlmostEqual(start_rot[i], rot[i], delta=0.0001)
def test_a_jog_with_linear_delta(self):
'''Test to jog a command with linear delta'''
# Get current posture of link_name
(start_pos, start_rot) = self.listener.lookupTransform(
self.frame_id, self.link_name, rospy.Time(0))
# Create jog frame command
jog = JogFrame()
jog.header.stamp = rospy.Time.now()
jog.header.frame_id = self.frame_id
jog.group_name = self.group_name
jog.link_name = self.link_name
jog.linear_delta.x = self.linear_delta
jog.linear_delta.y = self.linear_delta
jog.linear_delta.z = self.linear_delta
self.pub.publish(jog)
rospy.sleep(3.0)
(pos, rot) = self.listener.lookupTransform(
self.frame_id, self.link_name, rospy.Time(0))
# Check if the position is jogged by delta
for i in range(3):
self.assertAlmostEqual(pos[i], start_pos[i] + self.linear_delta, delta=0.0001)
# Check if the rotaion is not changed
rot_diff = tf.transformations.quaternion_multiply(
tf.transformations.quaternion_inverse(start_rot), rot)
for i in range(4):
self.assertAlmostEqual(start_rot[i], rot[i], delta=0.0001)
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)