6 from actionlib
import SimpleActionClient
7 import moveit_commander
8 from moveit_msgs.msg
import MoveItErrorCodes, MoveGroupGoal, Constraints, JointConstraint, MoveGroupAction
16 self._move_client.wait_for_server()
18 moveit_commander.roscpp_initialize(sys.argv)
19 group_name = moveit_commander.RobotCommander().get_group_names()[0]
20 group = moveit_commander.MoveGroupCommander(group_name)
24 self._goal.request.group_name = group_name
25 self._goal.request.max_velocity_scaling_factor = 0.1
26 self._goal.request.max_acceleration_scaling_factor = 0.1
27 self._goal.request.start_state.is_diff =
True 29 goal_constraint = Constraints()
30 joint_values = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]
31 joint_names = group.get_active_joints()
32 for i
in range(len(joint_names)):
33 joint_constraint = JointConstraint()
34 joint_constraint.joint_name = joint_names[i]
35 joint_constraint.position = joint_values[i]
36 joint_constraint.weight = 1.0
37 goal_constraint.joint_constraints.append(joint_constraint)
39 self._goal.request.goal_constraints.append(goal_constraint)
40 self._goal.planning_options.planning_scene_diff.robot_state.is_diff =
True 44 self._move_client.send_goal(self.
_goal)
47 self._move_client.cancel_goal()
50 self._move_client.wait_for_result()
55 result = self._move_client.get_result()
60 self.assertTrue(result.error_code.val == MoveItErrorCodes.PREEMPTED
or result.error_code.val == 0)
64 self._goal.planning_options.plan_only =
True 67 self._move_client.send_goal(self.
_goal)
70 self._move_client.cancel_goal()
73 self._move_client.wait_for_result()
78 result = self._move_client.get_result()
83 self.assertTrue(result.error_code.val == MoveItErrorCodes.PREEMPTED
or result.error_code.val == 0)
87 self._move_client.send_goal(self.
_goal)
90 self._move_client.cancel_goal()
93 self._move_client.send_goal(self.
_goal)
96 self._move_client.wait_for_result()
100 while result
is None:
101 result = self._move_client.get_result()
105 self.assertEqual(result.error_code.val, MoveItErrorCodes.SUCCESS)
108 if __name__ ==
'__main__':
110 rospy.init_node(
'cancel_before_plan_execution')
111 rostest.rosrun(
'moveit_ros_move_group',
'test_cancel_before_plan_execution', TestMoveActionCancelDrop)
def test_cancel_drop_plan_only(self)
def test_cancel_resend(self)
def test_cancel_drop_plan_execution(self)