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