void move_group::MoveGroupPickPlaceAction::executePlaceCallback(const moveit_msgs::PlaceGoalConstPtr& goal) { setPlaceState(PLANNING); planning_scene_monitor_->updateFrameTransforms(); moveit_msgs::PlaceResult action_res; if (goal->planning_options.plan_only || !allow_trajectory_execution_) { if (!goal->planning_options.plan_only) ROS_WARN("This instance of MoveGroup is not allowed to execute trajectories but the place goal request has plan_only set to false. Only a motion plan will be computed anyway."); executePlaceCallback_PlanOnly(goal, action_res); } else executePlaceCallback_PlanAndExecute(goal, action_res); bool planned_trajectory_empty = action_res.trajectory_stages.empty(); std::string response = getActionResultString(action_res.error_code, planned_trajectory_empty, goal->planning_options.plan_only); if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS) place_action_server_->setSucceeded(action_res, response); else { if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::PREEMPTED) place_action_server_->setPreempted(action_res, response); else place_action_server_->setAborted(action_res, response); } setPlaceState(IDLE); }
void move_group::MoveGroupPickPlaceAction::executePickupCallback(const moveit_msgs::PickupGoalConstPtr& input_goal) { setPickupState(PLANNING); // before we start planning, ensure that we have the latest robot state received... context_->planning_scene_monitor_->waitForCurrentRobotState(ros::Time::now()); context_->planning_scene_monitor_->updateFrameTransforms(); moveit_msgs::PickupGoalConstPtr goal; if (input_goal->possible_grasps.empty()) { moveit_msgs::PickupGoal* copy(new moveit_msgs::PickupGoal(*input_goal)); goal.reset(copy); fillGrasps(*copy); } else goal = input_goal; moveit_msgs::PickupResult action_res; if (goal->planning_options.plan_only || !context_->allow_trajectory_execution_) { if (!goal->planning_options.plan_only) ROS_WARN_NAMED("manipulation", "This instance of MoveGroup is not allowed to execute trajectories but the pick " "goal request has plan_only set to false. Only a motion plan will be computed " "anyway."); executePickupCallbackPlanOnly(goal, action_res); } else executePickupCallbackPlanAndExecute(goal, action_res); bool planned_trajectory_empty = action_res.trajectory_stages.empty(); std::string response = getActionResultString(action_res.error_code, planned_trajectory_empty, goal->planning_options.plan_only); if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS) pickup_action_server_->setSucceeded(action_res, response); else { if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::PREEMPTED) pickup_action_server_->setPreempted(action_res, response); else pickup_action_server_->setAborted(action_res, response); } setPickupState(IDLE); }