void move_group::MoveGroupPickPlaceAction::executePlaceCallbackPlanAndExecute(
const moveit_msgs::PlaceGoalConstPtr& goal, moveit_msgs::PlaceResult& action_res)
{
plan_execution::PlanExecution::Options opt;
opt.replan_ = goal->planning_options.replan;
opt.replan_attempts_ = goal->planning_options.replan_attempts;
opt.replan_delay_ = goal->planning_options.replan_delay;
opt.before_execution_callback_ = boost::bind(&MoveGroupPickPlaceAction::startPlaceExecutionCallback, this);
opt.plan_callback_ =
boost::bind(&MoveGroupPickPlaceAction::planUsingPickPlacePlace, this, boost::cref(*goal), &action_res, _1);
if (goal->planning_options.look_around && context_->plan_with_sensing_)
{
opt.plan_callback_ = boost::bind(&plan_execution::PlanWithSensing::computePlan, context_->plan_with_sensing_.get(),
_1, opt.plan_callback_, goal->planning_options.look_around_attempts,
goal->planning_options.max_safe_execution_cost);
context_->plan_with_sensing_->setBeforeLookCallback(
boost::bind(&MoveGroupPickPlaceAction::startPlaceLookCallback, this));
}
plan_execution::ExecutableMotionPlan plan;
context_->plan_execution_->planAndExecute(plan, goal->planning_options.planning_scene_diff, opt);
convertToMsg(plan.plan_components_, action_res.trajectory_start, action_res.trajectory_stages);
action_res.trajectory_descriptions.resize(plan.plan_components_.size());
for (std::size_t i = 0; i < plan.plan_components_.size(); ++i)
action_res.trajectory_descriptions[i] = plan.plan_components_[i].description_;
action_res.error_code = plan.error_code_;
}
void move_group::MoveGroupPickPlaceAction::executePickupCallbackPlanOnly(const moveit_msgs::PickupGoalConstPtr& goal,
moveit_msgs::PickupResult& action_res)
{
pick_place::PickPlanPtr plan;
try
{
planning_scene_monitor::LockedPlanningSceneRO ps(context_->planning_scene_monitor_);
plan = pick_place_->planPick(ps, *goal);
}
catch (std::exception& ex)
{
ROS_ERROR_NAMED("manipulation", "Pick&place threw an exception: %s", ex.what());
}
if (plan)
{
const std::vector<pick_place::ManipulationPlanPtr>& success = plan->getSuccessfulManipulationPlans();
if (success.empty())
{
action_res.error_code = plan->getErrorCode();
}
else
{
const pick_place::ManipulationPlanPtr& result = success.back();
convertToMsg(result->trajectories_, action_res.trajectory_start, action_res.trajectory_stages);
action_res.trajectory_descriptions.resize(result->trajectories_.size());
for (std::size_t i = 0; i < result->trajectories_.size(); ++i)
action_res.trajectory_descriptions[i] = result->trajectories_[i].description_;
if (result->id_ < goal->possible_grasps.size())
action_res.grasp = goal->possible_grasps[result->id_];
action_res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
action_res.planning_time = plan->getLastPlanTime();
}
}
else
{
action_res.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
}
}
void move_group::MoveGroupPickPlaceAction::executePlaceCallback_PlanOnly(const moveit_msgs::PlaceGoalConstPtr& goal, moveit_msgs::PlaceResult &action_res)
{
pick_place::PlacePlanPtr plan;
try
{
planning_scene_monitor::LockedPlanningSceneRO ps(planning_scene_monitor_);
plan = pick_place_->planPlace(ps, *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 (plan)
{
const std::vector<pick_place::ManipulationPlanPtr> &success = plan->getSuccessfulManipulationPlans();
if (success.empty())
{
action_res.error_code = plan->getErrorCode();
}
else
{
const pick_place::ManipulationPlanPtr &result = success.back();
convertToMsg(result->trajectories_, action_res.trajectory_start, action_res.trajectory_stages);
action_res.trajectory_descriptions.resize(result->trajectories_.size());
for (std::size_t i = 0 ; i < result->trajectories_.size() ; ++i)
action_res.trajectory_descriptions[i] = result->trajectories_[i].description_;
action_res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
}
}
else
{
action_res.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
}
}
