48.在ROS中实现local planner（1）- 实现一个可以用的模板

有了之前45.在ROS中实现global planner（1）- 实现一个可以用模板的global planner的经验, 现在再去创建一个local planner的包就容易多了

1. 创建包

• 创建

cd ~/pibot_ros/ros_ws/src  # 这里可以使用自己的ros workspace
catkin_create_pkg sample_local_planner

• 添加类
  我们需要实现一个从nav_core::BaseLocalPlanner继承的类, nav_core::BaseLocalPlanner接口类定义在这里base_local_planner.h#L50)可以看到

• 修改编译
  修改CMakeLists.txt,添加相关编译参数和选项

• 添加bgp_plugin.xml文件
  指定导出的类名称

<library path="lib/libsample_local_planner">
  <class name="sample_local_planner/LocalPlanner" type="sample_local_planner::LocalPlanner" base_class_type="nav_core::BaseLocalPlanner">
    <description>
      A sample implementation of a grid local planner 
    </description>
  </class>
</library>

目录结构这样

❯ tree sample_local_planner
sample_local_planner
├── bgp_plugin.xml
├── CMakeLists.txt
├── include
│   └── sample_local_planner
│       └── planner_node.h
├── package.xml
└── src
    └── planner_node.cpp

• 导出类
  参考navigation里面, 添加宏导出该类

PLUGINLIB_EXPORT_CLASS(sample_local_planner::LocalPlanner, nav_core::BaseLocalPlanner)

2. 接口实现

2.1 接口

base_local_planner.h#L50)可以看到接口类

namespace nav_core {
  class BaseLocalPlanner{
    public:
      virtual bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel) = 0;
      virtual bool isGoalReached() = 0;
      virtual bool setPlan(const std::vector<geometry_msgs::PoseStamped>& plan) = 0;
      virtual void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros) = 0;
  };
};  // namespace nav_core

通过命名大概就知道其定义，

• initialize
  初始化接口，给我们传相关功能接口的，如tf， costmap
• setPlan
  规划控制接口，给我们提供一个plan，这个应该是global planner的输出，通过move_base转了一手给到我们，后面可以看下move_base源码
• computeVelocityCommands
  计算速度，传入的参数是一个引用，应该是输出函数，我们把计算好的速度填进去就可以
• isGoalReached
  获取是否以及到达目标点

2.2 不同ros版本接口差异

BaseLocalPlanner在ros kinetic 中的initialize接口稍有差异 见base_local_planner.h#L78

// kinetic
virtual void initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros) = 0;

// melodic&noetic
virtual void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros) = 0;

后面我们以melodic&noetic实现

2.3 实现

主要代码如下，stopwatch_为计时器，我们在setPlan调用后，设置变量，computeVelocityCommands接口中设置固定的速度，在时间到达后，输出0，同时isGoalReached接口返回true

void LocalPlanner::initialize(std::string name, tf::TransformListener *tf,
                                  costmap_2d::Costmap2DROS *costmap_ros)
    {
        ROS_INFO("LocalPlanner initialize");
    }

    bool LocalPlanner::computeVelocityCommands(geometry_msgs::Twist &cmd_vel)
    {
        ROS_INFO("LocalPlanner computeVelocityCommands");
        if (start_flag_) {
	        cmd_vel.linear.x = 0.2;
		    cmd_vel.linear.y = 0;
		    cmd_vel.angular.z = 0.8;
        } else {
	        cmd_vel.linear.x = 0;
		    cmd_vel.linear.y = 0;
		    cmd_vel.angular.z = 0;
        }
        
        return true;
    }

    bool LocalPlanner::setPlan(const std::vector<geometry_msgs::PoseStamped> &orig_global_plan)
    {
        ROS_INFO("LocalPlanner setPlan");

        if (!start_flag_) {
            start_flag_ = true;
            stopwatch_.reset();
        }

        return true;
    }

    bool LocalPlanner::isGoalReached()
    {
        if (stopwatch_.elapsed(std::chrono::seconds(2)))
        {
            ROS_INFO("LocalPlanner GoalReached");
            return true;
        }

        return false;
    }

通过查看move_base源码，上面几个接口是在同一个线程被调用，所有后续不需要考虑资源竞争，即变量无需加锁

3. 测试

3.1 编译

cd ~/pibot_ros/ros_ws
catkin_make

3.2 测试

修改~/pibot_ros/src/pibot_simulator/move_base_params.yaml

# base_local_planner: "dwa_local_planner/DWAPlannerROS"
base_local_planner: sample_local_planner/LocalPlanner

dwa_local_planner/DWAPlannerROS ----> sample_local_planner/LocalPlanner

• 启动模拟器

pibot_simulator

• 查看当前的local_planner

❯ rosparam get /move_base/base_local_planner
sample_local_planner/LocalPlanner  # 输出sample_local_planner/LocalPlanner表示插件已经被正确加载

• 启动rviz发送点位，选点导航测试

pibot_view

3.3 测试结果

[ INFO] [1676647988.863610652]: make plan start:[0.000000 0.000000], goal:[-2.986773 4.282055]
[ INFO] [1676647989.063781836]: LocalPlanner setPlan
[ INFO] [1676647989.064015702]: LocalPlanner computeVelocityCommands
[ INFO] [1676647989.263707871]: LocalPlanner computeVelocityCommands
[ INFO] [1676647989.463771479]: LocalPlanner computeVelocityCommands
[ INFO] [1676647989.663754028]: LocalPlanner computeVelocityCommands
[ INFO] [1676647989.863583610]: LocalPlanner computeVelocityCommands
[ INFO] [1676647989.864067517]: make plan start:[0.000000 0.000000], goal:[-2.986773 4.282055]
[ INFO] [1676647990.063701815]: LocalPlanner setPlan
[ INFO] [1676647990.063874092]: LocalPlanner computeVelocityCommands
[ INFO] [1676647990.263710418]: LocalPlanner computeVelocityCommands
[ INFO] [1676647990.463773749]: LocalPlanner computeVelocityCommands
[ INFO] [1676647990.663630163]: LocalPlanner computeVelocityCommands
[ INFO] [1676647990.863635728]: LocalPlanner computeVelocityCommands
[ INFO] [1676647990.864087581]: make plan start:[0.000000 0.000000], goal:[-2.986773 4.282055]
[ INFO] [1676647991.063713670]: LocalPlanner setPlan
[ INFO] [1676647991.063894899]: LocalPlanner computeVelocityCommands
[ INFO] [1676647991.263639509]: LocalPlanner GoalReached

通过日志可以看出

• 在全局规划(make plan start是我们前面文章新增的astar planner输出)后LocalPlanner的接口setPlan被调用
• computeVelocityCommands函数没0.2s被调用一次, 期间机器人也在做圆周运动
• 全局规划再次被调用(move_bsae里配置了规划频率1hz，这里可以看到间隔1s全局规划一次)，重复前面的
• 直到超时GoalReached返回true完成

4. 总结

本文简单实现了一个local planner的插件，显然实际没啥用，不过可以作为一个模板，基于该模板实现自己的算法。后面我们将基于该模板实现可用的局部规划控制。

本文代码见sample_local_planner