def on_enter(self, userdata):
self._planning_failed = False
self._control_failed = False
self._success = False
# TODO check userdata
# if not isinstance(userdata.pose, PoseStamped):
#Logger.logwarn('userdata.pose must be geomery_msgs.msg.PoseStamped. `%s` received' % str(type(userdata.pose)))
#self._planning_failed = True
#return
check_type('pose', 'geometry_msgs/PoseStamped', userdata.pose)
# request planing and execution
action_goal = MoveGroupGoal()
# set palnning options
action_goal.planning_options.plan_only = self._plan_only
action_goal.planning_options.replan = False
# action_goal.planning_options.planning_scene_diff.is_diff = True
# action_goal.planning_options.planning_scene_diff.robot_state.is_diff = True
# setup request
action_goal.request.group_name = self._move_group
action_goal.request.num_planning_attempts = 3
action_goal.request.allowed_planning_time = 1.0
action_goal.request.max_velocity_scaling_factor = 1.0
action_goal.request.max_acceleration_scaling_factor = 1.0
# start pose
action_goal.request.start_state.is_diff = True
# pose constraint
goal_constraint = Constraints(name='')
# set target position
constraint = PositionConstraint()
constraint.header = Header(frame_id=userdata.pose.header.frame_id)
constraint.link_name = self._end_effector
constraint.constraint_region = BoundingVolume()
constraint.constraint_region.primitives = [
SolidPrimitive(type=SolidPrimitive.SPHERE,
dimensions=[self._position_tolerance])
]
constraint.constraint_region.primitive_poses = [
Pose(position=userdata.pose.pose.position)
]
constraint.weight = 1.0
goal_constraint.position_constraints.append(constraint)
# set target orientation
constraint = OrientationConstraint()
constraint.header = Header(frame_id=userdata.pose.header.frame_id)
constraint.link_name = self._end_effector
constraint.orientation = userdata.pose.pose.orientation
constraint.absolute_x_axis_tolerance = self._orientation_tolerance
constraint.absolute_y_axis_tolerance = self._orientation_tolerance
constraint.absolute_z_axis_tolerance = self._orientation_tolerance
constraint.weight = 0.1
goal_constraint.orientation_constraints.append(constraint)
# add goal_constraint
action_goal.request.goal_constraints.append(goal_constraint)
try:
self._client.send_goal('move_group', action_goal)
except Exception as e:
Logger.logwarn(
'MoveitToPose: Failed to send MoveGroupAction goal for group: %s\n%s'
% (self._move_group, str(e)))
self._planning_failed = True
