client要操作机械臂，首先调用MoveGroupInterfaceImpl::plan(Plan& plan)为此次操作做规划，内中会发送moveit_msgs::MoveGroupGoal请求action。对应到server会触发MoveGroupMoveAction回调move服务的处理例程：executeMoveCallback。
- 规划轨迹先是存在ompl_simple_setup_，然后转换到robot_trajectory::RobotTrajectory类型，存放在res.trajectory_，最后通过convertToMsg转换成client希望的moveit_msgs::RobotTrajectory类型，并存储在action_res.planned_trajectory。
- 规划要使用moveit三大插件之一的运动规划器。默认使用OMPLPlannerManager。
- 运动规划器插件包括两个部分：一个派生于PlannerManager的插件对象（OMPL：OMPLPlannerManager），一个派生于PlanningContext的（OMPL：ModelBasedPlanningContext），后者负责规划出轨迹。
一、MoveGroupMoveAction::executeMoveCallback
@goal[IN]：client想让机械臂移动到的位置。
void MoveGroupMoveAction::executeMoveCallback(const moveit_msgs::MoveGroupGoalConstPtr& goal)
{
setMoveState(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::MoveGroupResult action_res;
if (goal->planning_options.plan_only || !context_->allow_trajectory_execution_)
{
if (!goal->planning_options.plan_only)
ROS_WARN_NAMED(getName(), "This instance of MoveGroup is not allowed to execute trajectories "
"but the goal request has plan_only set to false. "
"Only a motion plan will be computed anyway.");参数plan_only默认值true(1)，allow_trajectory_execution_来自参数allow_trajectory_execution，默认true。经过executeMoveCallbackPlanOnly后，action_res.planned_trajectory应该存储着要返回给client的、此次规划出的轨迹。
executeMoveCallbackPlanOnly(goal, action_res);
}
else
executeMoveCallbackPlanAndExecute(goal, action_res);
bool planned_trajectory_empty = trajectory_processing::isTrajectoryEmpty(action_res.planned_trajectory);
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)
move_action_server_->setSucceeded(action_res, response);
else
{
if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::PREEMPTED)
move_action_server_->setPreempted(action_res, response);
else
move_action_server_->setAborted(action_res, response);
}
setMoveState(IDLE);
preempt_requested_ = false;
}
二、MoveGroupMoveAction::executeMoveCallbackPlanOnly
@goal[IN]：client想让机械臂移动到的位置。
void MoveGroupMoveAction::executeMoveCallbackPlanOnly(const moveit_msgs::MoveGroupGoalConstPtr& goal,
moveit_msgs::MoveGroupResult& action_res)
{
ROS_INFO_NAMED(getName(), "Planning request received for MoveGroup action. Forwarding to planning pipeline.");
// lock the scene so that it does not modify the world representation while diff() is called
planning_scene_monitor::LockedPlanningSceneRO lscene(context_->planning_scene_monitor_);
const planning_scene::PlanningSceneConstPtr& the_scene =
(moveit::core::isEmpty(goal->planning_options.planning_scene_diff)) ?
static_cast(lscene) :
lscene->diff(goal->planning_options.planning_scene_diff);
planning_interface::MotionPlanResponse res;
if (preempt_requested_)
{
ROS_INFO_NAMED(getName(), "Preempt requested before the goal is planned.");
action_res.error_code.val = moveit_msgs::MoveItErrorCodes::PREEMPTED;
return;
}
// Select planning_pipeline to handle request
const planning_pipeline::PlanningPipelinePtr planning_pipeline = resolvePlanningPipeline(goal->request.pipeline_id);
if (!planning_pipeline)
{
action_res.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
return;
}
try
{
planning_pipeline->generatePlan(the_scene, goal->request, res);
generatePlan(...)执行规划任务，规划轨迹存放在res.trajectory_。
}
catch (std::exception& ex)
{
ROS_ERROR_NAMED(getName(), "Planning pipeline threw an exception: %s", ex.what());
res.error_code_.val = moveit_msgs::MoveItErrorCodes::FAILURE;
}
convertToMsg(res.trajectory_, action_res.trajectory_start, action_res.planned_trajectory);res.trajectory_存储着规划轨迹，但类型是robot_trajectory::RobotTrajectory，convertToMsg要把它转成client希望的moveit_msgs::RobotTrajectory，并存储在action_res.planned_trajectory。转换可这么认为waypoints_[n].position_对应到points[n].positions，waypoints_[n].velocities_对应到points[n].velocities。
action_res.error_code = res.error_code_; action_res.planning_time = res.planning_time_; }
三、PlanningPipeline::generatePlan
bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene,
const planning_interface::MotionPlanRequest& req,
planning_interface::MotionPlanResponse& res) const
{
std::vector<std::size_t> dummy;
return generatePlan(planning_scene, req, res, dummy);
}- @req[IN]：来自中goal的request。
- @res[OUT]：规划出的结果将存放在它的res.trajectory_，类型是robot_trajectory::RobotTrajectory。
bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene,
const planning_interface::MotionPlanRequest& req,
planning_interface::MotionPlanResponse& res,
std::vector<std::size_t>& adapter_added_state_index) const
{
// broadcast the request we are about to work on, if needed
if (publish_received_requests_)
received_request_publisher_.publish(req);
adapter_added_state_index.clear();
if (!planner_instance_)
{
ROS_ERROR("No planning plugin loaded. Cannot plan.");
return false;
}
bool solved = false;
try
{
if (adapter_chain_)
{
solved = adapter_chain_->adaptAndPlan(planner_instance_, planning_scene, req, res, adapter_added_state_index);
if (!adapter_added_state_index.empty())
{
std::stringstream ss;
for (std::size_t added_index : adapter_added_state_index)
ss << added_index << " ";
ROS_INFO("Planning adapters have added states at index positions: [ %s]", ss.str().c_str());
}
}
else
{
planning_interface::PlanningContextPtr context =
planner_instance_->getPlanningContext(planning_scene, req, res.error_code_);
solved = context ? context->solve(res) : false;planner_instance_就是三大插件之一的运动规划器插件，默认是OMPLPlannerManager。
context->solve(res)要计算出规划，规划轨迹放在res.trajectory_，但这时数据格式是robot_trajectory::RobotTrajectory。可认为它就是最后要返回的规划轨迹。
}
}
catch (std::exception& ex)
{
ROS_ERROR("Exception caught: '%s'", ex.what());
return false;
}
bool valid = true;
...
return solved && valid;
}
四、ModelBasedPlanningContext::solve
<libros>/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp
------
bool ompl_interface::ModelBasedPlanningContext::solve(planning_interface::MotionPlanResponse& res)
{
if (solve(request_.allowed_planning_time, request_.num_planning_attempts))
{solve(5, 1)执行规划，规划出的轨迹存储在ompl_simple_setup_。
double ptime = getLastPlanTime();
if (simplify_solutions_)
{
simplifySolution(request_.allowed_planning_time - ptime);
ptime += getLastSimplifyTime();
}
if (interpolate_)
interpolateSolution();
// fill the response
ROS_DEBUG_NAMED(LOGNAME, "%s: Returning successful solution with %lu states", getName().c_str(),
getOMPLSimpleSetup()->getSolutionPath().getStateCount());
res.trajectory_ = std::make_shared<robot_trajectory::RobotTrajectory>(getRobotModel(), getGroupName());
getSolutionPath(*res.trajectory_);上面已把规划出的轨迹存在ompl_simple_setup_，接下要把它转换到res.trajectory_，让看下RobotTrajectory中4个字段的赋值情况。
- moveit::core::RobotModelConstPtr robot_model_。make_shared时第一个参数getRobotModel()。
- const moveit::core::JointModelGroup* group_。make_shared时第一个参数getGroupName()。ompl把轨迹存放在ompl::geometric::PathGeometric类型的pdef_->solutions_。
- std::deque<moveit::core::RobotStatePtr> waypoints_。getSolutionPath时赋值，长度和duration_from_previous_的一样。
- std::deque<double> duration_from_previous_。getSolutionPath时赋值，长度和waypoints_的一样。
res.planning_time_ = ptime;
return true;
}
else
{
ROS_INFO_NAMED(LOGNAME, "Unable to solve the planning problem");
res.error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
return false;
}
}