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