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);
}
bool move_group::MoveGroupPickPlaceAction::planUsingPickPlacePickup(const moveit_msgs::PickupGoal& goal,
moveit_msgs::PickupResult* action_res,
plan_execution::ExecutableMotionPlan& plan)
{
setPickupState(PLANNING);
planning_scene_monitor::LockedPlanningSceneRO ps(plan.planning_scene_monitor_);
pick_place::PickPlanPtr pick_plan;
try
{
pick_plan = pick_place_->planPick(plan.planning_scene_, goal);
}
catch (std::exception& ex)
{
ROS_ERROR_NAMED("manipulation", "Pick&place threw an exception: %s", ex.what());
}
if (pick_plan)
{
const std::vector<pick_place::ManipulationPlanPtr>& success = pick_plan->getSuccessfulManipulationPlans();
if (success.empty())
{
plan.error_code_ = pick_plan->getErrorCode();
}
else
{
const pick_place::ManipulationPlanPtr& result = success.back();
plan.plan_components_ = result->trajectories_;
if (result->id_ < goal.possible_grasps.size())
action_res->grasp = goal.possible_grasps[result->id_];
plan.error_code_.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
action_res->planning_time = pick_plan->getLastPlanTime();
}
}
else
{
plan.error_code_.val = moveit_msgs::MoveItErrorCodes::FAILURE;
}
return plan.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS;
}
bool move_group::MoveGroupPickPlaceAction::planUsingPickPlace_Pickup(const moveit_msgs::PickupGoal& goal, plan_execution::ExecutableMotionPlan &plan)
{
setPickupState(PLANNING);
planning_scene_monitor::LockedPlanningSceneRO ps(plan.planning_scene_monitor_);
pick_place::PickPlanPtr pick_plan;
try
{
pick_plan = pick_place_->planPick(plan.planning_scene_, goal);
}
catch(std::runtime_error &ex)
{
ROS_ERROR("Pick&place threw an exception: %s", ex.what());
}
catch(...)
{
ROS_ERROR("Pick&place threw an exception");
}
if (pick_plan)
{
const std::vector<pick_place::ManipulationPlanPtr> &success = pick_plan->getSuccessfulManipulationPlans();
if (success.empty())
{
plan.error_code_ = pick_plan->getErrorCode();
}
else
{
const pick_place::ManipulationPlanPtr &result = success.back();
plan.plan_components_ = result->trajectories_;
plan.error_code_.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
}
}
else
{
plan.error_code_.val = moveit_msgs::MoveItErrorCodes::FAILURE;
}
return plan.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS;
}
void move_group::MoveGroupPickPlaceAction::startPickupLookCallback()
{
setPickupState(LOOK);
}
void move_group::MoveGroupPickPlaceAction::startPickupExecutionCallback()
{
setPickupState(MONITOR);
}
