0 专栏介绍

本专栏旨在通过对ROS的系统学习，掌握ROS底层基本分布式原理，并具有机器人建模和应用ROS进行实际项目的开发和调试的工程能力。

🚀详情：《ROS从入门到精通》

---

1 局部规划插件制作框架

参考ROS从入门到精通5-3：插件库与开发+实例分析开发插件。首先创建功能包my_planner用于生成自定义全局路径规划插件

1. 构造基类：由于局部路径规划插件全部继承于nav_core功能包的BaseLocalPlanner类，因此无需构造

2. 构造插件类：在local_planner/include中新建local_planner.h，继承自基类nav_core::BaseLocalPlanner，与全局规划器不同，这里需要实现更多接口

   • initialize：规划器初始化接口
   • setPlan：设置全局规划器规划的路径
   • computeVelocityCommands：计算运动指令，驱动机器人实际运动，在全局规划中只是规划了路径点，并没有指导机器人应该如何运动
   • isGoalReached：判断机器人是否到达终点

   namespace local_planner{
   	class LocalPlanner : public nav_core::BaseLocalPlanner{
   	public:
   	
   	    LocalPlanner();
   	    LocalPlanner(std::string name, tf2_ros::Buffer* tf,
   	                 costmap_2d::Costmap2DROS* costmap_ros);
   	    ~LocalPlanner();
   	
   	    void initialize(std::string name, tf2_ros::Buffer* tf,
   	                    costmap_2d::Costmap2DROS* costmap_ros);
   	
   	    bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan);
   	
   	    bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);
   	
   	    bool isGoalReached();
   	private:
   	    costmap_2d::Costmap2DROS* costmap_ros_;
   	    tf2_ros::Buffer* tf_;
   	    bool initialized_;
   	};
   };
   

3. 注册插件：在local_planner/src中新建local_planner.cpp使用PLUGINLIB_EXPORT_CLASS宏注册插件，限于篇幅不列出完整代码。

   PLUGINLIB_EXPORT_CLASS(local_planner::LocalPlanner, nav_core::BaseLocalPlanner)
   
   namespace local_planner{
   	LocalPlanner::LocalPlanner() : costmap_ros_(NULL), tf_(NULL), initialized_(false) {}
   	LocalPlanner::LocalPlanner(std::string name, tf2_ros::Buffer* tf,
   	                           costmap_2d::Costmap2DROS* costmap_ros)
   	    : costmap_ros_(NULL), tf_(NULL), initialized_(false)
   	{
   	    initialize(name, tf, costmap_ros);
   	}
   	
   	LocalPlanner::~LocalPlanner() {}
   	
   	void LocalPlanner::initialize(std::string name, tf2_ros::Buffer* tf,
   	                              costmap_2d::Costmap2DROS* costmap_ros)
   	{
   	    if(!initialized_)
   	    {
   	        tf_ = tf;
   	        costmap_ros_ = costmap_ros;
   	        initialized_ = true;
   	    }
   	}
   	...
   }
   

4. 构建插件库.so：编译此功能包local_planner将会在根目录devel/lib中生成插件liblocal_planner.so

5. 集成插件库到ROS：在功能包local_planner下创建local_planner_plugin.xml描述插件信息和库路径

   <library path="lib/liblocal_planner">
   	<class name ="local_planner/LocalPlanner" type ="local_planner::LocalPlanner" base_class_type= "nav_core::BaseLocalPlanner">
       <description>
         My planner
       </description>
     </class>
   </library>
   

   在功能包local_planner下package.xml中导出local_planner_plugin.xml

    <depend>nav_core</depend><!-- 注意此依赖，否则找不到自定义规划器 -->
    <!-- The export tag contains other, unspecified, tags -->
    <export>
        <nav_core plugin="${prefix}/local_planner_plugin.xml" />
    </export>
   

6. 使用插件：在turtlebots_navigation中的move_base.launch新增一行，声明使用自定义插件

   <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
      <param name="base_local_planner" value="local_planner/LocalPlanner"/>
   	...
   </node>
   

所有局部规划插件的编写均遵从上述框架。

2 DWA算法源码分析

DWA算法的核心逻辑集中在computeVelocityCommands函数中，接下来重点分析

2.1 全局路径裁剪

std::vector<geometry_msgs::PoseStamped> transformed_plan;
if (!planner_util_.getLocalPlan(current_pose_, transformed_plan))
{
  ROS_ERROR("Could not get local plan");
  return false;
}

这里裁剪函数getLocalPlan来自base_local_planner提供的工具类LocalPlannerUtil，这个类封装了规划器常用的数据结构——例如地图、tf树，以及一些常用的工具函数

getLocalPlan中分为两步：

• transformGlobalPlan()：将全局规划的路径通过tf坐标变换到局部代价地图坐标系
• prunePlan()：实际的裁剪函数，将机器人背后的路径点裁掉，因为这部分路径在规划中不使用

2.2 更新局部代价

因为每时每刻机器人的局部代价地图和运行的全局路径点不同，因此要进行更新

dp_->updatePlanAndLocalCosts(current_pose_, transformed_plan, costmap_ros_->getRobotFootprint());

在updatePlanAndLocalCosts()内部主要进行了代价函数的更新，这些代价函数将用于轨迹评价，具体地，DWA中包含以下代价函数

对象名称	数据类型	说明
oscillation_costs_	OscillationCostFunction	尽量降低机器人在原地晃动的情况
obstacle_costs_	ObstacleCostFunction	防止机器人撞到障碍物
path_costs_	MapGridCostFunction	使机器人尽可能的贴近全局轨迹
goal_costs_	MapGridCostFunction	更倾向于选择接近局部目标点的轨迹
goal_front_costs_	MapGridCostFunction	尽可能地让机器人朝向全局目标
alignment_costs_	MapGridCostFunction	尽可能地让机器人朝向全局轨迹
twirling_costs_	TwirlingCostFunction	尽量不让机器人原地打转

2.3 运行DWA算法

dp_->updatePlanAndLocalCosts(current_pose_, transformed_plan, costmap_ros_->getRobotFootprint());

这个函数内部运行逻辑包含以下几步

2.3.1 构造动态窗口

generator_.initialise(pos,
        vel,
        goal,
        &limits,
        vsamples_);

在这个函数内部构造了采样窗口

max_vel[0] = std::min(max_vel_x, vel[0] + acc_lim[0] * sim_period_);
max_vel[1] = std::min(max_vel_y, vel[1] + acc_lim[1] * sim_period_);
max_vel[2] = std::min(max_vel_th, vel[2] + acc_lim[2] * sim_period_);

min_vel[0] = std::max(min_vel_x, vel[0] - acc_lim[0] * sim_period_);
min_vel[1] = std::max(min_vel_y, vel[1] - acc_lim[1] * sim_period_);
min_vel[2] = std::max(min_vel_th, vel[2] - acc_lim[2] * sim_period_);

再通过迭代器

VelocityIterator x_it(min_vel[0], max_vel[0], vsamples[0]);
VelocityIterator y_it(min_vel[1], max_vel[1], vsamples[1]);
VelocityIterator th_it(min_vel[2], max_vel[2], vsamples[2]);

遍历所有可能的速度组合，存入sample_params_中

2.3.2 生成最优轨迹

std::vector<base_local_planner::Trajectory> all_explored;
scored_sampling_planner_.findBestTrajectory(result_traj_, &all_explored);

在findBestTrajectory中主要分为两步：

• 轨迹生成

  gen_->nextTrajectory(loop_traj);
  

  这里的gen_就是2.3.1节的generator_，所以它会通过已构造的sample_params_遍历所有可能的速度组合，通过其generateTrajectory()函数生成轨迹返回
• 轨迹评价

  loop_traj_cost = scoreTrajectory(loop_traj, best_traj_cost);
  

  这里实际上调用代价函数栈对轨迹进行逐一评价

  for (std::vector<TrajectoryCostFunction *>::iterator score_function = critics_.begin(); score_function != critics_.end(); ++score_function)
  {
    TrajectoryCostFunction *score_function_p = *score_function;
    double cost = score_function_p->scoreTrajectory(traj);
    ...
  }
  

  而这些代价函数就是2.2节所列出的，它们已经在上一步完成了更新。

最后选择代价最小的轨迹返回即可。

2.4 终点规划

在机器人靠近终点时，可以简化规划流程，直接朝着终点前进，LatchedStopRotateController就是base_local_planner提供用于执行该功能的工具类，其中包含若干工具函数，在编写自定义插件时很有用：

• isPositionReached：判断机器人是否到达指定位置
• isGoalReached：判断机器人是否到达指定位姿
• stopWithAccLimits：机器人逐渐减速到指定位置
• rotateToGoal：机器人旋转到指定位姿
• computeVelocityCommandsStopRotate：机器人逐渐减速到指定位置，并旋转到指定位姿

在DWA中应用如下：

1. 临近终点时的规划

   if (latchedStopRotateController_.isPositionReached(&planner_util_, current_pose_))
   {
     // publish an empty plan because we've reached our goal position
     std::vector<geometry_msgs::PoseStamped> local_plan;
     std::vector<geometry_msgs::PoseStamped> transformed_plan;
     publishGlobalPlan(transformed_plan);
     publishLocalPlan(local_plan);
     base_local_planner::LocalPlannerLimits limits = planner_util_.getCurrentLimits();
     return latchedStopRotateController_.computeVelocityCommandsStopRotate(
         cmd_vel,
         limits.getAccLimits(),
         dp_->getSimPeriod(),
         &planner_util_,
         odom_helper_,
         current_pose_,
         [this](auto pos, auto vel, auto vel_samples)
         { return dp_->checkTrajectory(pos, vel, vel_samples); });
   }
   

2. 判断是否到达终点

   bool DWAPlannerROS::isGoalReached() {
      ...
       if (latchedStopRotateController_.isGoalReached(&planner_util_, odom_helper_, current_pose_))
       {
         ROS_INFO("Goal reached");
         return true;
       }
       else
       {
         return false;
       }
     }
   

3 算法测试

本文的完整工程代码联系下方博主名片获取

---

🔥 更多精彩专栏：

• 《ROS从入门到精通》
• 《Pytorch深度学习实战》
• 《机器学习强基计划》
• 《运动规划实战精讲》
• …

👇源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系👇