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