test_cancel_before_plan_execution.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import sys
4 import rospy
5 import unittest
6 from actionlib import SimpleActionClient
7 import moveit_commander
8 from moveit_msgs.msg import (
9  MoveItErrorCodes,
10  MoveGroupGoal,
11  Constraints,
12  JointConstraint,
13  MoveGroupAction,
14 )
15 
16 
17 class TestMoveActionCancelDrop(unittest.TestCase):
18  def setUp(self):
19  # create a action client of move group
20  self._move_client = SimpleActionClient("move_group", MoveGroupAction)
21  self._move_client.wait_for_server()
22 
23  moveit_commander.roscpp_initialize(sys.argv)
24  group_name = moveit_commander.RobotCommander().get_group_names()[0]
25  group = moveit_commander.MoveGroupCommander(group_name)
26 
27  # prepare a joint goal
28  self._goal = MoveGroupGoal()
29  self._goal.request.group_name = group_name
30  self._goal.request.max_velocity_scaling_factor = 0.1
31  self._goal.request.max_acceleration_scaling_factor = 0.1
32  self._goal.request.start_state.is_diff = True
33 
34  goal_constraint = Constraints()
35  joint_values = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]
36  joint_names = group.get_active_joints()
37  for i in range(len(joint_names)):
38  joint_constraint = JointConstraint()
39  joint_constraint.joint_name = joint_names[i]
40  joint_constraint.position = joint_values[i]
41  joint_constraint.weight = 1.0
42  goal_constraint.joint_constraints.append(joint_constraint)
43 
44  self._goal.request.goal_constraints.append(goal_constraint)
45  self._goal.planning_options.planning_scene_diff.robot_state.is_diff = True
46 
48  # send the goal
49  self._move_client.send_goal(self._goal)
50 
51  # cancel the goal
52  self._move_client.cancel_goal()
53 
54  # wait for result
55  self._move_client.wait_for_result()
56 
57  # polling the result, since result can come after the state is Done
58  result = None
59  while result is None:
60  result = self._move_client.get_result()
61  rospy.sleep(0.1)
62 
63  # check the error code in result
64  # error code is 0 if the server ends with RECALLED status
65  self.assertTrue(
66  result.error_code.val == MoveItErrorCodes.PREEMPTED
67  or result.error_code.val == 0
68  )
69 
71  # set the plan only flag
72  self._goal.planning_options.plan_only = True
73 
74  # send the goal
75  self._move_client.send_goal(self._goal)
76 
77  # cancel the goal
78  self._move_client.cancel_goal()
79 
80  # wait for result
81  self._move_client.wait_for_result()
82 
83  # polling the result, since result can come after the state is Done
84  result = None
85  while result is None:
86  result = self._move_client.get_result()
87  rospy.sleep(0.1)
88 
89  # check the error code in result
90  # error code is 0 if the server ends with RECALLED status
91  self.assertTrue(
92  result.error_code.val == MoveItErrorCodes.PREEMPTED
93  or result.error_code.val == 0
94  )
95 
96  def test_cancel_resend(self):
97  # send the goal
98  self._move_client.send_goal(self._goal)
99 
100  # cancel the goal
101  self._move_client.cancel_goal()
102 
103  # send the goal again
104  self._move_client.send_goal(self._goal)
105 
106  # wait for result
107  self._move_client.wait_for_result()
108 
109  # polling the result, since result can come after the state is Done
110  result = None
111  while result is None:
112  result = self._move_client.get_result()
113  rospy.sleep(0.1)
114 
115  # check the error code in result
116  self.assertEqual(result.error_code.val, MoveItErrorCodes.SUCCESS)
117 
118 
119 if __name__ == "__main__":
120  import rostest
121 
122  rospy.init_node("cancel_before_plan_execution")
123  rostest.rosrun(
124  "moveit_ros_move_group",
125  "test_cancel_before_plan_execution",
126  TestMoveActionCancelDrop,
127  )
test_cancel_before_plan_execution.TestMoveActionCancelDrop._move_client
_move_client
Definition: test_cancel_before_plan_execution.py:20
test_cancel_before_plan_execution.TestMoveActionCancelDrop
Definition: test_cancel_before_plan_execution.py:17
test_cancel_before_plan_execution.TestMoveActionCancelDrop.test_cancel_drop_plan_only
def test_cancel_drop_plan_only(self)
Definition: test_cancel_before_plan_execution.py:70
actionlib::SimpleActionClient
test_cancel_before_plan_execution.TestMoveActionCancelDrop.test_cancel_resend
def test_cancel_resend(self)
Definition: test_cancel_before_plan_execution.py:96
test_cancel_before_plan_execution.TestMoveActionCancelDrop._goal
_goal
Definition: test_cancel_before_plan_execution.py:28
test_cancel_before_plan_execution.TestMoveActionCancelDrop.test_cancel_drop_plan_execution
def test_cancel_drop_plan_execution(self)
Definition: test_cancel_before_plan_execution.py:47
test_cancel_before_plan_execution.TestMoveActionCancelDrop.setUp
def setUp(self)
Definition: test_cancel_before_plan_execution.py:18


move_group
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Mon May 27 2024 02:27:46