move_base的 局部路径规划代码研究

• base_local_planner
• teb_local_planner
  • parameter
  • code
    • g2o

base_local_planner

ROS wiki

Given a plan to follow and a costmap, the controller produces velocity commands to send to a mobile base.

他的功能是给一个global plan和local costmap，局部路径规划器计算出可行的速度发送给机器人

base_local_planner::TrajectoryPlanner provides implementations of the DWA and Trajectory Rollout

It should be possible to create custom local planners using the dwa_local_planner as template and just adding own cost functions or trajectory generators.

你可以参照DWA实现自己的局部规路径算法

算法主要是在局部的costmap中模拟计算沿着不同的方向进行定义的cost函数的大小，选择一个cost小的正的的方向前进。

主要是进行计算cost函数，每个cost可以有weight参数调整，这个可以算是灵活和也可以说是不稳定的因素(要自己调试)

• ObstacleCostFunction
• MapGridCostFunction
• OscillationCostFunction
• PreferForwardCostFunction

teb_local_planner

ROS wiki

优化轨迹执行的时间，与障碍物的距离，满足最大的速度与加速度的要求

Support of holonomic robots is included since Kinetic

parameter

参数分为一下几类(记住有些参数他在ros wiki里面是没有说明的，在代码里面有的，不是所有的参数都可以通过rqt_reconfigure配置的，有很少的一部分是不行的):
所有的参数你都可以在teb_config.h中找到初始值和含义

• robot configuration

  跟机器人底盘是圆形，多边形，car-like有关，在后面的优化有用到，要设置正确

~<name>/max_vel_x_backwards (double, default: 0.2) 
Maximum absolute translational velocity of the robot while driving backwards in meters/sec. See optimization parameter weight_kinematics_forward_drive

• goal tolerance

~<name>/xy_goal_tolerance (double, default: 0.2) 

~<name>/yaw_goal_tolerance (double, default: 0.2) 
#Remove the goal velocity constraint such that the robot can arrive at the goal with maximum speed 
~<name>/free_goal_vel (bool, default: false) 

• trajectory configuration

# 轨迹的空间分辨率，只是一个参考值，真实的分辨率跟别的还有关
~<name>/dt_ref (double, default: 0.3) 

• obstacles

#距离障碍物的最短距离
~<name>/min_obstacle_dist (double, default: 0.5) 
#Limit the occupied local costmap obstacles taken into account for planning behind the robot (specify distance in meters). 
~<name>/costmap_obstacles_behind_robot_dist (double, default: 1.0) 

#障碍物会影响的pose的个数,
#bool legacy_obstacle_association; //!< If true, the old association strategy is used (for each obstacle, find the nearest TEB pose), otherwise the new one (for each teb pose, find only "relevant" obstacles).
~<name>/obstacle_poses_affected (int, default: 30) 

• optimization

#只允许前进的权重
~<name>/weight_kinematics_forward_drive (double, default: 1.0) 
#远离障碍物至少min_obstacle_dist的权重
~<name>/weight_obstacle (double, default: 50.0) 
#紧跟global plan的权重
~<name>/weight_viapoint (double, default: 1.0) 

• parallel planning in distinctive topologies

#允许并进计算，消耗更多的计算资源
~<name>/enable_homotopy_class_planning (bool, default: true) 
~<name>/enable_multithreading (bool, default: true) 


#Specify how much trajectory cost must a new candidate have w.r.t. a previously selected trajectory in order to be selected (selection if new_cost < old_cost*factor). 
~<name>/selection_cost_hysteresis (double, default: 1.0) 
#Extra scaling of obstacle cost terms just for selecting the 'best' candidate. 
~<name>/selection_obst_cost_scale (double, default: 100.0) 
#Extra scaling of via-point cost terms just for selecting the 'best' candidate. New in version 0.4
~<name>/selection_viapoint_cost_scale (double, default: 1.0) 

• miscellaneous parameters

code

void TebLocalPlannerROS::initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros)
{
    // create the planner instance
    if (cfg_.hcp.enable_homotopy_class_planning)
    {
      planner_ = PlannerInterfacePtr(new HomotopyClassPlanner(cfg_, &obstacles_, robot_model, visualization_, &via_points_));
      ROS_INFO("Parallel planning in distinctive topologies enabled.");
    }
    else
    {
      planner_ = PlannerInterfacePtr(new TebOptimalPlanner(cfg_, &obstacles_, robot_model, visualization_, &via_points_));
      ROS_INFO("Parallel planning in distinctive topologies disabled.");
    }
}

bool TebLocalPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
{
    // prune global plan to cut off parts of the past (spatially before the robot)
    pruneGlobalPlan(*tf_, robot_pose, global_plan_);

    // Transform global plan to the frame of interest (w.r.t. the local costmap)
    if (!transformGlobalPlan(*tf_, global_plan_, robot_pose, *costmap_, global_frame_, cfg_.trajectory.max_global_plan_lookahead_dist, 
                            transformed_plan, &goal_idx, &tf_plan_to_global))
    {
        ROS_WARN("Could not transform the global plan to the frame of the controller");
        return false;
    }

    // check if we should enter any backup mode and apply settings
    configureBackupModes(transformed_plan, goal_idx);

    updateObstacleContainerWithCostmap();

    // Now perform the actual planning
    bool success = planner_->plan(transformed_plan, &robot_vel_, cfg_.goal_tolerance.free_goal_vel);

    bool feasible = planner_->isTrajectoryFeasible(costmap_model_.get(), footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius, cfg_.trajectory.feasibility_check_no_poses);

    if (!planner_->getVelocityCommand(cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z)){

    }
}

bool TebOptimalPlanner::plan(const std::vector<geometry_msgs::PoseStamped>& initial_plan, const geometry_msgs::Twist* start_vel, bool free_goal_vel)
{
    if (!teb_.isInit()){
        // init trajectory
        teb_.initTEBtoGoal(initial_plan, cfg_->trajectory.dt_ref, cfg_->trajectory.global_plan_overwrite_orientation, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);
    }
    else{
        PoseSE2 start_(initial_plan.front().pose);
        PoseSE2 goal_(initial_plan.back().pose);
        if (teb_.sizePoses()>0 && (goal_.position() - teb_.BackPose().position()).norm() < cfg_->trajectory.force_reinit_new_goal_dist) // actual warm start!
        teb_.updateAndPruneTEB(start_, goal_, cfg_->trajectory.min_samples); // update TEB
        else // goal too far away -> reinit
        {
            ROS_DEBUG("New goal: distance to existing goal is higher than the specified threshold. Reinitalizing trajectories.");
            teb_.clearTimedElasticBand();
            teb_.initTEBtoGoal(initial_plan, cfg_->trajectory.dt_ref, true, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);
        }
    }
    
    // now optimize
    return optimizeTEB(cfg_->optim.no_inner_iterations, cfg_->optim.no_outer_iterations);

}

bool TebOptimalPlanner::optimizeTEB(int iterations_innerloop, int iterations_outerloop, bool compute_cost_afterwards,
                                    double obst_cost_scale, double viapoint_cost_scale, bool alternative_time_cost)
{
    for(int i=0; i<iterations_outerloop; ++i)
    {
        if (cfg_->trajectory.teb_autosize)
            teb_.autoResize(cfg_->trajectory.dt_ref, cfg_->trajectory.dt_hysteresis, cfg_->trajectory.min_samples, cfg_->trajectory.max_samples);
        
        //构建图
        success = buildGraph(weight_multiplier);
        if (!success) 
        {
            clearGraph();
            return false;
        }
        //优化图
        success = optimizeGraph(iterations_innerloop, false);
        if (!success) 
        {
            clearGraph();
            return false;
        }
        
        if (compute_cost_afterwards && i==iterations_outerloop-1) // compute cost vec only in the last iteration
            computeCurrentCost(obst_cost_scale, viapoint_cost_scale, alternative_time_cost);
      
        clearGraph();
    
        weight_multiplier *= cfg_->optim.weight_adapt_factor;

    }
}

bool TebOptimalPlanner::buildGraph(double weight_multiplier)
{
     // add TEB vertices
    AddTEBVertices();
    
    // add Edges (local cost functions)
    if (cfg_->obstacles.legacy_obstacle_association)
        AddEdgesObstaclesLegacy(weight_multiplier);
    else
        AddEdgesObstacles(weight_multiplier);
    
    //AddEdgesDynamicObstacles();
    
    AddEdgesViaPoints();
    
    AddEdgesVelocity();
    
    AddEdgesAcceleration();

    AddEdgesTimeOptimal();	
    
    if (cfg_->robot.min_turning_radius == 0 || cfg_->optim.weight_kinematics_turning_radius == 0)
        AddEdgesKinematicsDiffDrive(); // we have a differential drive robot
    else
        AddEdgesKinematicsCarlike(); // we have a carlike robot since the turning radius is bounded from below.

        
    AddEdgesPreferRotDir();
    
}

bool TebOptimalPlanner::optimizeGraph(int no_iterations,bool clear_after)
{
    if (cfg_->robot.max_vel_x<0.01)
    {
        ROS_WARN("optimizeGraph(): Robot Max Velocity is smaller than 0.01m/s. Optimizing aborted...");
        if (clear_after) clearGraph();
        return false;	
    }
    
    if (!teb_.isInit() || teb_.sizePoses() < cfg_->trajectory.min_samples)
    {
        ROS_WARN("optimizeGraph(): TEB is empty or has too less elements. Skipping optimization.");
        if (clear_after) clearGraph();
        return false;	
    }
    
    //  boost::shared_ptr<g2o::SparseOptimizer> optimizer_; //!< g2o optimizer for trajectory optimization
    optimizer_->setVerbose(cfg_->optim.optimization_verbose);
    optimizer_->initializeOptimization();

    int iter = optimizer_->optimize(no_iterations);
    
    if(!iter)
    {
        ROS_ERROR("optimizeGraph(): Optimization failed! iter=%i", iter);
        return false;
    }

    if (clear_after) clearGraph();	
}

g2o

boost::shared_ptr<g2o::SparseOptimizer> TebOptimalPlanner::initOptimizer()
{
  // Call register_g2o_types once, even for multiple TebOptimalPlanner instances (thread-safe)
  static boost::once_flag flag = BOOST_ONCE_INIT;
  boost::call_once(&registerG2OTypes, flag);  

  // allocating the optimizer
  boost::shared_ptr<g2o::SparseOptimizer> optimizer = boost::make_shared<g2o::SparseOptimizer>();
  //typedef g2o::LinearSolverCSparse<TEBBlockSolver::PoseMatrixType> TEBLinearSolver;
  TEBLinearSolver* linearSolver = new TEBLinearSolver(); // see typedef in optimization.h
  linearSolver->setBlockOrdering(true);
  //typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> >  TEBBlockSolver;
  TEBBlockSolver* blockSolver = new TEBBlockSolver(linearSolver);
  g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(blockSolver);

  optimizer->setAlgorithm(solver);
  
  optimizer->initMultiThreading(); // required for >Eigen 3.1
  
  return optimizer;
}