planning in moveit - ompl interface

//definition in `move_group_interface.h`
struct Plan
{
        moveit_msgs::RobotState start_state_;
        moveit_msgs::RobotTrajectory trajectory_;
        double planning_time_;
};

//调用了impl_的同名函数
//MoveGroupInterfaceImpl* impl_;
MoveGroupInterface::plan(Plan& plan)
{
        return impl_->plan(plan);
}

//definition in `move_group_interface.cpp`
    MoveItErrorCode plan(Plan& plan)
    {
        if (!move_action_client_)
        {
            return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
        }
        if (!move_action_client_->isServerConnected())
        {
            return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
        }

        moveit_msgs::MoveGroupGoal goal;
        constructGoal(goal);
        goal.planning_options.plan_only = true;
        goal.planning_options.look_around = false;
        goal.planning_options.replan = false;
        goal.planning_options.planning_scene_diff.is_diff = true;
        goal.planning_options.planning_scene_diff.robot_state.is_diff = true;

        move_action_client_->sendGoal(goal);//move_action_client_
        if (!move_action_client_->waitForResult())
        {
            ROS_INFO_STREAM_NAMED("move_group_interface", "MoveGroup action returned early");
        }
        if (move_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
        {
            plan.trajectory_ = move_action_client_->getResult()->planned_trajectory;
            plan.start_state_ = move_action_client_->getResult()->trajectory_start;
            plan.planning_time_ = move_action_client_->getResult()->planning_time;
            return MoveItErrorCode(move_action_client_->getResult()->error_code);
        }
        else
        {
            ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << move_action_client_->getState().toString() << ": "
                                                                                                                         << move_action_client_->getState().getText());
            return MoveItErrorCode(move_action_client_->getResult()->error_code);
        }
    }

//definition in `move_group_interface.cpp`
move_action_client_.reset(
                new actionlib::SimpleActionClient<moveit_msgs::MoveGroupAction>(node_handle_, move_group::MOVE_ACTION, false));

MoveGroupMoveAction::executeMoveCallback_PlanAndExecute(){
    context_->plan_execution_->planAndExecute(plan, planning_scene_diff, opt);
}

struct MoveGroupContext{
    plan_execution::PlanExecutionPtr plan_execution_;
}
//move_group_client最终转化为request
context_->planning_pipeline_->generatePlan(the_scene, goal->request, res);

planning_interface::PlannerManagerPtr planner_instance_;
bool PlanningPipeline::generatePlan(...)const
{
    ...
    planning_interface::PlanningContextPtr context = planner_instance_->getPlanningContext(planning_scene, req, res.error_code_);
    solved = context ? context->solve(res) : false;
    ...
}

OMPLPlannerManager : public planning_interface::PlannerManager{
    getPlanningContext(const  planning_scene,const  req,error_code) const
  {
    return ompl_interface_->getPlanningContext(planning_scene, req, error_code);
  }
};

OMPLInterface::getPlanningContext(){
    ModelBasedPlanningContextPtr ctx = context_manager_.getPlanningContext(planning_scene, req, error_code);
    configureContext(ctx);
}

PlanningContextManager::getPlanningContext(){
    //初始化context
}

context->solve(res);
bool ompl_interface::ModelBasedPlanningContext::solve(planning_interface::MotionPlanResponse& res){
    solve(res.time,res.attempts);
    res.trajectory_=...
}
bool ompl_interface::ModelBasedPlanningContext::solve(double timeout, unsigned int count)
{
    preSolve();
    ompl_simple_setup_->solve();//ompl_parallel_plan_->solve();
    postSolve();
}

class ModelBasedPlanningContext : public planning_interface::PlanningContext{
    //构造函数中将spec中ompl_simple_setup指针赋值给ompl_simple_setup
    ModelBasedPlanningContext(const std::string &name, const ModelBasedPlanningContextSpecification &spec) ;
    solve(double timeout,unsigned int count){
    //SimpleSetup 将调用自身属性base::PlannerPtr planner_的solve()函数。
    ompl_simple_setup_->solve(ob::PlannerTerminationCondiction ptc);
}
    ompl::geometry::SimpleSetupPtr ompl_simple_setup_;
};
//PlanningContext定义在planning_interface.h里面
//该类中有设置goal/statevaliditychecker/projectevaluator

struct ModelBasedPlanningContextSpecification{
    ...
    ModelBasedStateSpacePtr state_space_;
    og::SimpleSetupPtr ompl_simple_setup_;
    ...

};

class ModelBasedStateSpace : public ompl::base::StateSpace{

};

void ModelBasedPlanningContext::convertPath(const ompl::geometric::PathGeometric& pg,
        robot_trajectory::RobotTrajectory& traj) const
{
  robot_state::RobotState ks = complete_initial_robot_state_;
  for (std::size_t i = 0; i < pg.getStateCount(); ++i)
  {
    spec_.state_space_->copyToRobotState(ks, pg.getState(i));
    traj.addSuffixWayPoint(ks, 0.0);
  }
}

void ModelBasedStateSpace::copyToRobotState(robot_state::RobotState& rstate,
    const ompl::base::State* state) const
{
  rstate.setJointGroupPositions(spec_.joint_model_group_, state->as<StateType>()->values);
  rstate.update();
}

struct ModelBasedStateSpaceSpecification
{
    robot_model::RobotModelConstPtr robot_model_;
    const robot_model::JointModelGroup* joint_model_group_;
    robot_model::JointBoundsVector joint_bounds_;//关节极限值
};
class ModelBasedStateSpace : public ompl::base::StateSpace
{
    protected:
        ModelBasedStateSpaceSpecification spec_;
}
class StateType : public ompl::base::State{
        double *values;
};

//以joint_model_group作为关节空间的值
double ompl_interface::ModelBasedStateSpace::distance(const ompl::base::State* state1,
                                                const ompl::base::State* state2) const
{
    if (distance_function_)
        return distance_function_(state1, state2);
    else
        return spec_.joint_model_group_->distance(state1->as<StateType>()->values, state2->as<StateType>()->values);
}

bool ompl_interface::ModelBasedStateSpace::equalStates(const ompl::base::State* state1,
                                                                                                             const ompl::base::State* state2) const
{
    for (unsigned int i = 0; i < variable_count_; ++i)
        if (fabs(state1->as<StateType>()->values[i] - state2->as<StateType>()->values[i]) >
                std::numeric_limits<double>::epsilon())
            return false;
    return true;
}

void ompl_interface::ModelBasedStateSpace::enforceBounds(ompl::base::State* state) const
{
    spec_.joint_model_group_->enforcePositionBounds(state->as<StateType>()->values, spec_.joint_bounds_);
}

bool ompl_interface::ModelBasedStateSpace::satisfiesBounds(const ompl::base::State* state) const
{
    return spec_.joint_model_group_->satisfiesPositionBounds(state->as<StateType>()->values, spec_.joint_bounds_,
                                                                                                                     std::numeric_limits<double>::epsilon());
}

class JointModelGroup{
    std::vector< const JointModel * >   active_joint_model_vector_;
    JointModelGroup(...const std::vector<const JointModel*>& unsorted_group_joints..){
        joint_model_vector_ = unsorted_group_joints;
        std::sort(joint_model_vector_.begin(), joint_model_vector_.end(), OrderJointsByIndex());
        ....
        active_joint_model_vector_.push_back(joint_model_vector_[i]);
        ....
    }
};

class RevoluteJointModel : public JointModel{
    bool RevoluteJointModel::enforcePositionBounds(double* values, const Bounds& bounds) const{
        //与SO2StateSpace::enforceBounds(State *state)函数类似定义
    }
}