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::executePlaceCallback(const moveit_msgs::PlaceGoalConstPtr& goal)
{
setPlaceState(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::PlaceResult 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 place "
"goal request has plan_only set to false. Only a motion plan will be computed "
"anyway.");
executePlaceCallbackPlanOnly(goal, action_res);
}
else
executePlaceCallbackPlanAndExecute(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);
}
bool move_group::MoveGroupPickPlaceAction::planUsingPickPlacePlace(const moveit_msgs::PlaceGoal& goal,
moveit_msgs::PlaceResult* action_res,
plan_execution::ExecutableMotionPlan& plan)
{
setPlaceState(PLANNING);
planning_scene_monitor::LockedPlanningSceneRO ps(plan.planning_scene_monitor_);
pick_place::PlacePlanPtr place_plan;
try
{
place_plan = pick_place_->planPlace(plan.planning_scene_, goal);
}
catch (std::exception& ex)
{
ROS_ERROR_NAMED("manipulation", "Pick&place threw an exception: %s", ex.what());
}
if (place_plan)
{
const std::vector<pick_place::ManipulationPlanPtr>& success = place_plan->getSuccessfulManipulationPlans();
if (success.empty())
{
plan.error_code_ = place_plan->getErrorCode();
}
else
{
const pick_place::ManipulationPlanPtr& result = success.back();
plan.plan_components_ = result->trajectories_;
if (result->id_ < goal.place_locations.size())
action_res->place_location = goal.place_locations[result->id_];
plan.error_code_.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
action_res->planning_time = place_plan->getLastPlanTime();
}
}
else
{
plan.error_code_.val = moveit_msgs::MoveItErrorCodes::FAILURE;
}
return plan.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS;
}
void move_group::MoveGroupPickPlaceAction::startPlaceLookCallback()
{
setPlaceState(LOOK);
}
void move_group::MoveGroupPickPlaceAction::startPlaceExecutionCallback()
{
setPlaceState(MONITOR);
}
