test_cancel_before_plan_execution.py

Go to the documentation of this file.

#!/usr/bin/env python
 
import sys
import rospy
import unittest
from actionlib import SimpleActionClient
import moveit_commander
from moveit_msgs.msg import (
    MoveItErrorCodes,
    MoveGroupGoal,
    Constraints,
    JointConstraint,
    MoveGroupAction,
)
 
 
class TestMoveActionCancelDrop(unittest.TestCase):
    def setUp(self):
        # create a action client of move group
        self._move_client = SimpleActionClient("move_group", MoveGroupAction)
        self._move_client.wait_for_server()
 
        moveit_commander.roscpp_initialize(sys.argv)
        group_name = moveit_commander.RobotCommander().get_group_names()[0]
        group = moveit_commander.MoveGroupCommander(group_name)
 
        # prepare a joint goal
        self._goal = MoveGroupGoal()
        self._goal.request.group_name = group_name
        self._goal.request.max_velocity_scaling_factor = 0.1
        self._goal.request.max_acceleration_scaling_factor = 0.1
        self._goal.request.start_state.is_diff = True
 
        goal_constraint = Constraints()
        joint_values = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]
        joint_names = group.get_active_joints()
        for i in range(len(joint_names)):
            joint_constraint = JointConstraint()
            joint_constraint.joint_name = joint_names[i]
            joint_constraint.position = joint_values[i]
            joint_constraint.weight = 1.0
            goal_constraint.joint_constraints.append(joint_constraint)
 
        self._goal.request.goal_constraints.append(goal_constraint)
        self._goal.planning_options.planning_scene_diff.robot_state.is_diff = True
 
    def test_cancel_drop_plan_execution(self):
        # send the goal
        self._move_client.send_goal(self._goal)
 
        # cancel the goal
        self._move_client.cancel_goal()
 
        # wait for result
        self._move_client.wait_for_result()
 
        # polling the result, since result can come after the state is Done
        result = None
        while result is None:
            result = self._move_client.get_result()
            rospy.sleep(0.1)
 
        # check the error code in result
        # error code is 0 if the server ends with RECALLED status
        self.assertTrue(
            result.error_code.val == MoveItErrorCodes.PREEMPTED
            or result.error_code.val == 0
        )
 
    def test_cancel_drop_plan_only(self):
        # set the plan only flag
        self._goal.planning_options.plan_only = True
 
        # send the goal
        self._move_client.send_goal(self._goal)
 
        # cancel the goal
        self._move_client.cancel_goal()
 
        # wait for result
        self._move_client.wait_for_result()
 
        # polling the result, since result can come after the state is Done
        result = None
        while result is None:
            result = self._move_client.get_result()
            rospy.sleep(0.1)
 
        # check the error code in result
        # error code is 0 if the server ends with RECALLED status
        self.assertTrue(
            result.error_code.val == MoveItErrorCodes.PREEMPTED
            or result.error_code.val == 0
        )
 
    def test_cancel_resend(self):
        # send the goal
        self._move_client.send_goal(self._goal)
 
        # cancel the goal
        self._move_client.cancel_goal()
 
        # send the goal again
        self._move_client.send_goal(self._goal)
 
        # wait for result
        self._move_client.wait_for_result()
 
        # polling the result, since result can come after the state is Done
        result = None
        while result is None:
            result = self._move_client.get_result()
            rospy.sleep(0.1)
 
        # check the error code in result
        self.assertEqual(result.error_code.val, MoveItErrorCodes.SUCCESS)
 
 
if __name__ == "__main__":
    import rostest
 
    rospy.init_node("cancel_before_plan_execution")
    rostest.rosrun(
        "moveit_ros_move_group",
        "test_cancel_before_plan_execution",
        TestMoveActionCancelDrop,
    )