test_jog_frame.py
Go to the documentation of this file.
1 #! /usr/bin/env python
2 # -*- coding: utf-8 -*-
3 import actionlib
4 from control_msgs.msg import (
5  FollowJointTrajectoryAction,
6  FollowJointTrajectoryGoal,
7 )
8 import math
9 import rospy
10 import sys
11 from trajectory_msgs.msg import (
12  JointTrajectoryPoint,
13 )
14 from jog_msgs.msg import JogFrame
15 import rospy
16 import rostest
17 from sensor_msgs.msg import JointState
18 import tf
19 import unittest
20 
21 class TestJogFrameNode(unittest.TestCase):
22  @classmethod
23  def setUpClass(cls):
24  rospy.init_node('test_jog_frame_node')
25 
26  def setUp(self):
27  # Get parameters
28  self.controller_name = rospy.get_param('~controller_name', None)
29  self.action_name = rospy.get_param('~action_name', None)
30  self.joint_names = rospy.get_param('~joint_names', [])
31  self.home_positions = rospy.get_param('~home_positions', [])
32  self.frame_id = rospy.get_param('~frame_id', 'base_link')
33  self.group_name = rospy.get_param('~group_name', 'manipulator')
34  self.link_name = rospy.get_param('~link_name', 'ee_link')
35  self.linear_delta = rospy.get_param('~linear_delta', 0.005)
36  self.angular_delta = rospy.get_param('~angular_delta', 0.01)
37  # TF listener
39  # Publishers
40  self.pub = rospy.Publisher('jog_frame', JogFrame, queue_size=10)
41  # Actionlib
42  self.client = None
43  if self.controller_name and self.action_name:
45  self.controller_name + '/' + self.action_name, FollowJointTrajectoryAction)
46  self.client.wait_for_server()
47  # Move to home position
48  self.move_to_home()
49  rospy.sleep(3.0)
50 
51  def move_to_home(self):
52  goal = FollowJointTrajectoryGoal()
53  goal.trajectory.header.stamp = rospy.Time.now()
54  goal_time_tolerance = rospy.Time(0.1)
55  goal.goal_time_tolerance = rospy.Time(0.1)
56  goal.trajectory.joint_names = self.joint_names
57  point = JointTrajectoryPoint()
58  point.positions = self.home_positions
59  point.time_from_start = rospy.Duration(1.0)
60  goal.trajectory.points.append(point)
61 
62  if self.client:
63  self.client.send_goal(goal)
64  self.assertTrue(self.client.wait_for_result(timeout=rospy.Duration(5.0)))
65  rospy.sleep(1.0)
66 
68  '''Test to jog a command with linear delta'''
69  # Get current posture of link_name
70  self.listener.waitForTransform(
71  self.frame_id, self.link_name, rospy.Time(), rospy.Duration(10))
72  (start_pos, start_rot) = self.listener.lookupTransform(
73  self.frame_id, self.link_name, rospy.Time(0))
74 
75  # Create jog frame command
76  jog = JogFrame()
77  jog.header.stamp = rospy.Time.now()
78  jog.header.frame_id = self.frame_id
79  jog.group_name = self.group_name
80  jog.link_name = self.link_name
81  jog.linear_delta.x = self.linear_delta
82  jog.linear_delta.y = self.linear_delta
83  jog.linear_delta.z = self.linear_delta
84  self.pub.publish(jog)
85  rospy.sleep(3.0)
86 
87  # Get current posture of link_name
88  self.listener.waitForTransform(
89  self.frame_id, self.link_name, rospy.Time(), rospy.Duration(10))
90  (pos, rot) = self.listener.lookupTransform(
91  self.frame_id, self.link_name, rospy.Time(0))
92  # Check if the position is jogged by delta
93  for i in range(3):
94  self.assertAlmostEqual(pos[i], start_pos[i] + self.linear_delta, delta=0.0001)
95  # Check if the rotaion is not changed
96  rot_diff = tf.transformations.quaternion_multiply(
97  tf.transformations.quaternion_inverse(start_rot), rot)
98  for i in range(4):
99  self.assertAlmostEqual(start_rot[i], rot[i], delta=0.0001)
100 
102  '''Test to jog a command with angular delta'''
103  # Get current posture of link_name
104  self.listener.waitForTransform(
105  self.frame_id, self.link_name, rospy.Time(), rospy.Duration(10))
106  (start_pos, start_rot) = self.listener.lookupTransform(
107  self.frame_id, self.link_name, rospy.Time(0))
108 
109  # Create jog frame command
110  jog = JogFrame()
111  jog.header.stamp = rospy.Time.now()
112  jog.header.frame_id = self.frame_id
113  jog.group_name = self.group_name
114  jog.link_name = self.link_name
115  jog.angular_delta.x = self.angular_delta / math.sqrt(3.0)
116  jog.angular_delta.y = self.angular_delta / math.sqrt(3.0)
117  jog.angular_delta.z = self.angular_delta / math.sqrt(3.0)
118  self.pub.publish(jog)
119  rospy.sleep(3.0)
120 
121  # Get current posture of link_name
122  self.listener.waitForTransform(
123  self.frame_id, self.link_name, rospy.Time.now(), rospy.Duration(10))
124  (pos, rot) = self.listener.lookupTransform(
125  self.frame_id, self.link_name, rospy.Time(0))
126  # Check if the position is not changed
127  for i in range(3):
128  self.assertAlmostEqual(pos[i], start_pos[i], delta=0.0001)
129  # Check if the rotaion is jogged by delta
130  jog_q = tf.transformations.quaternion_about_axis(self.angular_delta, [1,1,1])
131  ans_rot = tf.transformations.quaternion_multiply(jog_q, start_rot)
132  for i in range(4):
133  self.assertAlmostEqual(rot[i], ans_rot[i], delta=0.0001)
134 
136  '''Test to jog ten commands with linear delta'''
137  # Get current posture of link_name
138  self.listener.waitForTransform(
139  self.frame_id, self.link_name, rospy.Time(), rospy.Duration(10))
140  (start_pos, start_rot) = self.listener.lookupTransform(
141  self.frame_id, self.link_name, rospy.Time(0))
142 
143  for i in range(10):
144  # Create jog frame command
145  jog = JogFrame()
146  jog.header.stamp = rospy.Time.now()
147  jog.header.frame_id = self.frame_id
148  jog.group_name = self.group_name
149  jog.link_name = self.link_name
150  jog.linear_delta.x = self.linear_delta
151  jog.linear_delta.y = self.linear_delta
152  jog.linear_delta.z = self.linear_delta
153  self.pub.publish(jog)
154  rospy.sleep(3.0)
155 
156  # Get current posture of link_name
157  self.listener.waitForTransform(
158  self.frame_id, self.link_name, rospy.Time(), rospy.Duration(10))
159  (pos, rot) = self.listener.lookupTransform(
160  self.frame_id, self.link_name, rospy.Time(0))
161  # Check if the position is jogged by delta
162  for i in range(3):
163  self.assertAlmostEqual(pos[i], start_pos[i] + self.linear_delta*10, delta=0.0001)
164  # Check if the rotaion is not changed
165  for i in range(4):
166  self.assertAlmostEqual(start_rot[i], rot[i], delta=0.0001)
167 
169  '''Test to jog ten commands with angular delta'''
170  # Get current posture of link_name
171  self.listener.waitForTransform(
172  self.frame_id, self.link_name, rospy.Time(), rospy.Duration(10))
173  (start_pos, start_rot) = self.listener.lookupTransform(
174  self.frame_id, self.link_name, rospy.Time(0))
175 
176  for i in range(10):
177  # Create jog frame command
178  jog = JogFrame()
179  jog.header.stamp = rospy.Time.now()
180  jog.header.frame_id = self.frame_id
181  jog.group_name = self.group_name
182  jog.link_name = self.link_name
183  jog.angular_delta.x = self.angular_delta / math.sqrt(3.0)
184  jog.angular_delta.y = self.angular_delta / math.sqrt(3.0)
185  jog.angular_delta.z = self.angular_delta / math.sqrt(3.0)
186  self.pub.publish(jog)
187  rospy.sleep(3.0)
188 
189  # Get current posture of link_name
190  self.listener.waitForTransform(
191  self.frame_id, self.link_name, rospy.Time(), rospy.Duration(10))
192  (pos, rot) = self.listener.lookupTransform(
193  self.frame_id, self.link_name, rospy.Time(0))
194  # Check if the position is not changed
195  for i in range(3):
196  self.assertAlmostEqual(pos[i], start_pos[i], delta=0.0001)
197  # Check if the rotaion is jogged by delta
198  jog_q = tf.transformations.quaternion_about_axis(self.angular_delta*10, [1,1,1])
199  ans_rot = tf.transformations.quaternion_multiply(jog_q, start_rot)
200  for i in range(4):
201  self.assertAlmostEqual(rot[i], ans_rot[i], delta=0.0001)
202 
203 if __name__ == '__main__':
204  rostest.rosrun('jog_controller', 'test_jog_frame_node', TestJogFrameNode)


jog_controller
Author(s): Ryosuke Tajima
autogenerated on Sun May 17 2020 03:25:01