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);
}
