Add test for joint trajectory action cancel overwrite with samplerobot

Author: pazeshun

hrpsys_ros_bridge/test/test-samplerobot.py

@@ -224,6 +224,135 @@ def test_joint_angles_duration_0(self): # https://github.com/start-jsk/rtmros_co
         rospy.logwarn("difference between two angles %r %r"%(array([30,30,30,-90,-40,-30])-array(goal_angles),linalg.norm(array([30,30,30,-90,-40,-30])-array(goal_angles))))
         self.assertAlmostEqual(linalg.norm(array([30,30,30,-90,-40,-30])-array(goal_angles)), 0, delta=0.1)
 
+    # https://github.com/start-jsk/rtmros_common/pull/765#issuecomment-120208947
+    def test_jta_cancel_goal(self):
+        # for < kinetic
+        if os.environ['ROS_DISTRO'] >= 'kinetic' :
+            return True
+        from pr2_controllers_msgs.msg import JointTrajectoryAction, JointTrajectoryGoal
+        larm = actionlib.SimpleActionClient("/larm_controller/joint_trajectory_action", JointTrajectoryAction)
+        self.impl_test_jta_cancel_goal(larm, JointTrajectoryGoal())
+
+    def test_fjta_cancel_goal(self):
+        # for >= kinetic
+        if os.environ['ROS_DISTRO'] < 'kinetic' :
+            return True
+        from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
+        larm = actionlib.SimpleActionClient("/larm_controller/follow_joint_trajectory_action", FollowJointTrajectoryAction)
+        self.impl_test_jta_cancel_goal(larm, FollowJointTrajectoryGoal())
+
+    def impl_test_jta_cancel_goal(self, larm, goal):
+        larm.wait_for_server()
+
+        # 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")
+        point = JointTrajectoryPoint()
+        point.positions = [0,0,0,0,0,0]
+        point.time_from_start = rospy.Duration(5.0)
+        goal.trajectory.points.append(point)
+        larm.send_goal(goal)
+        larm.wait_for_result()
+
+        try:
+            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")
+        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)
+        larm.send_goal(goal)
+        rospy.sleep(2.5)
+        (trans1,rot1) = self.listener.lookupTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(0))
+        rospy.logwarn("tf_echo /WAIST_LINK0 /LARM_LINK7 %r %r"%(trans1,rot1))
+        larm.cancel_goal()
+        rospy.sleep(2.5)
+        (trans2,rot2) = self.listener.lookupTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(0))
+        rospy.logwarn("tf_echo /WAIST_LINK0 /LARM_LINK7 %r %r"%(trans2,rot2))
+        larm.wait_for_result()
+        rospy.logwarn("difference between two /LARM_LINK7 %r %r"%(array(trans1)-array(trans2),linalg.norm(array(trans1)-array(trans2))))
+        self.assertAlmostEqual(linalg.norm(array(trans1)-array(trans2)), 0, delta=0.1)
+
+    # https://github.com/start-jsk/rtmros_common/pull/765#issuecomment-392741195
+    def test_jta_overwrite_goal(self):
+        # for < kinetic
+        if os.environ['ROS_DISTRO'] >= 'kinetic' :
+            return True
+        from pr2_controllers_msgs.msg import JointTrajectoryAction, JointTrajectoryGoal
+        larm = actionlib.SimpleActionClient("/larm_controller/joint_trajectory_action", JointTrajectoryAction)
+        self.impl_test_jta_overwrite_goal(larm, JointTrajectoryGoal())
+
+    def test_fjta_overwrite_goal(self):
+        # for >= kinetic
+        if os.environ['ROS_DISTRO'] < 'kinetic' :
+            return True
+        from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
+        larm = actionlib.SimpleActionClient("/larm_controller/follow_joint_trajectory_action", FollowJointTrajectoryAction)
+        self.impl_test_jta_overwrite_goal(larm, FollowJointTrajectoryGoal())
+
+    def impl_test_jta_overwrite_goal(self, larm, goal):
+        larm.wait_for_server()
+
+        # 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")
+        point = JointTrajectoryPoint()
+        point.positions = [0,0,0,0,0,0]
+        point.time_from_start = rospy.Duration(5.0)
+        goal.trajectory.points.append(point)
+        larm.send_goal(goal)
+        larm.wait_for_result()
+
+        try:
+            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")
+        (trans1,rot1) = self.listener.lookupTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(0))
+        rospy.logwarn("tf_echo /WAIST_LINK0 /LARM_LINK7 %r %r"%(trans1,rot1))
+        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)
+        # send first goal
+        larm.send_goal(goal)
+        rospy.sleep(2.5)
+        goal.trajectory.header.stamp = rospy.get_rostime()
+        point.positions = [0,0,0,0,0,0]
+        point.time_from_start = rospy.Duration(2.5)
+        del goal.trajectory.points[:]
+        goal.trajectory.points.append(point)
+        # send second goal
+        larm.send_goal(goal)
+        jt_pos1 = rospy.wait_for_message('/joint_states', JointState)
+        rospy.logwarn("joint position just after sending second goal: %r"%(array(jt_pos1.position)))
+        rospy.sleep(0.5)
+        jt_pos2 = rospy.wait_for_message('/joint_states', JointState)
+        rospy.logwarn("joint position 0.5 sec after sending second goal: %r"%(array(jt_pos2.position)))
+        rospy.sleep(2)
+        (trans2,rot2) = self.listener.lookupTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(0))
+        rospy.logwarn("tf_echo /WAIST_LINK0 /LARM_LINK7 %r %r"%(trans2,rot2))
+        larm.wait_for_result()
+        rospy.logwarn("difference between two joint positions %r"%(array(jt_pos2.position)-array(jt_pos1.position)))
+        # if robot suddenly stops when goal is overwritten, joint position immediately starts to decrease (heading for new goal (zero)).
+        # joint_states includes unchanged hand joints, so using assertGreaterEqual instead of assertGreater is necessary.
+        for x in array(jt_pos2.position)-array(jt_pos1.position):
+            self.assertGreaterEqual(x, 0)
+        rospy.logwarn("difference between two /LARM_LINK7 %r %r"%(array(trans1)-array(trans2),linalg.norm(array(trans1)-array(trans2))))
+        self.assertAlmostEqual(linalg.norm(array(trans1)-array(trans2)), 0, delta=0.1)
+
 
 #unittest.main()
 if __name__ == '__main__':