Add two points trajectory to test of joint trajectory action cancel

start-jsk · Nov 30, 2019 · 61137eb · 61137eb
1 parent 675c9ec
commit 61137eb
Show file tree

Hide file tree

Showing 2 changed files with 36 additions and 22 deletions.
diff --git a/hrpsys_ros_bridge/test/test-pa10.py b/hrpsys_ros_bridge/test/test-pa10.py
@@ -137,19 +137,25 @@ def impl_test_jta_cancel_goal(self, larm, goal):
             self.listener.waitForTransform('/BASE_LINK', '/J7_LINK', rospy.Time(), rospy.Duration(120))
         except tf.Exception:
             self.assertTrue(None, "could not found tf from /BASE_LINK to /J7_LINK")
-        goal.trajectory.header.stamp = rospy.get_rostime()
-        goal.trajectory.joint_names.append("J1")
-        goal.trajectory.joint_names.append("J2")
-        goal.trajectory.joint_names.append("J3")
-        goal.trajectory.joint_names.append("J4")
-        goal.trajectory.joint_names.append("J5")
-        goal.trajectory.joint_names.append("J6")
-        goal.trajectory.joint_names.append("J7")
+        goal.trajectory.joint_names = ["J1","J2","J3","J4","J5","J6","J7"]
 
-        point = JointTrajectoryPoint()
-        point.positions = [ x * math.pi / 180.0 for x in [10,20,30,40,50,60,70] ]
-        point.time_from_start = rospy.Duration(5.0)
-        goal.trajectory.points.append(point)
+        point1 = JointTrajectoryPoint()
+        point2 = JointTrajectoryPoint()
+        # cancel one point trajectory (:angle-vector)
+        point1.positions = [ x * math.pi / 180.0 for x in [10,20,30,40,50,60,70] ]
+        point1.time_from_start = rospy.Duration(5.0)
+        self.jta_cancel_goal_template(larm, goal, [point1])
+
+        # cancel two points trajectory (:angle-vector-sequence)
+        point1.positions = [ x * math.pi / 180.0 for x in [5,10,15,20,25,30,35] ]
+        point1.time_from_start = rospy.Duration(2.5)
+        point2.positions = [ x * math.pi / 180.0 for x in [10,20,30,40,50,60,70] ]
+        point2.time_from_start = rospy.Duration(5.0)
+        self.jta_cancel_goal_template(larm, goal, [point1, point2])
+
+    def jta_cancel_goal_template(self, larm, goal, points):
+        goal.trajectory.header.stamp = rospy.get_rostime()
+        goal.trajectory.points = points
         larm.send_goal(goal)
         rospy.sleep(2.5)
         (trans1,rot1) = self.listener.lookupTransform('/BASE_LINK', '/J7_LINK', rospy.Time(0))

diff --git a/hrpsys_ros_bridge/test/test-samplerobot.py b/hrpsys_ros_bridge/test/test-samplerobot.py
@@ -246,12 +246,7 @@ def impl_test_jta_cancel_goal(self, larm, goal):
 
         # initialize
         goal.trajectory.header.stamp = rospy.get_rostime()
-        goal.trajectory.joint_names.append("LARM_SHOULDER_P")
-        goal.trajectory.joint_names.append("LARM_SHOULDER_R")
-        goal.trajectory.joint_names.append("LARM_SHOULDER_Y")
-        goal.trajectory.joint_names.append("LARM_ELBOW")
-        goal.trajectory.joint_names.append("LARM_WRIST_Y")
-        goal.trajectory.joint_names.append("LARM_WRIST_P")
+        goal.trajectory.joint_names = ["LARM_SHOULDER_P","LARM_SHOULDER_R","LARM_SHOULDER_Y","LARM_ELBOW","LARM_WRIST_Y","LARM_WRIST_P"]
         point = JointTrajectoryPoint()
         point.positions = [0,0,0,0,0,0]
         point.time_from_start = rospy.Duration(5.0)
@@ -263,11 +258,24 @@ def impl_test_jta_cancel_goal(self, larm, goal):
             self.listener.waitForTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(), rospy.Duration(120))
         except tf.Exception:
             self.assertTrue(None, "could not found tf from /WAIST_LINK0 to /LARM_LINK7")
+
+        point1 = JointTrajectoryPoint()
+        point2 = JointTrajectoryPoint()
+        # cancel one point trajectory (:angle-vector)
+        point1.positions = [ x * math.pi / 180.0 for x in [20,20,20,20,20,20] ]
+        point1.time_from_start = rospy.Duration(5.0)
+        self.jta_cancel_goal_template(larm, goal, [point1])
+
+        # cancel two points trajectory (:angle-vector-sequence)
+        point1.positions = [ x * math.pi / 180.0 for x in [10,10,10,10,10,10] ]
+        point1.time_from_start = rospy.Duration(2.5)
+        point2.positions = [ x * math.pi / 180.0 for x in [20,20,20,20,20,20] ]
+        point2.time_from_start = rospy.Duration(5.0)
+        self.jta_cancel_goal_template(larm, goal, [point1, point2])
+
+    def jta_cancel_goal_template(self, larm, goal, points):
         goal.trajectory.header.stamp = rospy.get_rostime()
-        point.positions = [ x * math.pi / 180.0 for x in [20,20,20,20,20,20] ]
-        point.time_from_start = rospy.Duration(5.0)
-        del goal.trajectory.points[:]
-        goal.trajectory.points.append(point)
+        goal.trajectory.points = points
         larm.send_goal(goal)
         rospy.sleep(2.5)
         (trans1,rot1) = self.listener.lookupTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(0))