有了之前45.在ROS中實現(xiàn)global planner（1）- 實現(xiàn)一個可以用模板的global planner的經驗, 現(xiàn)在再去創(chuàng)建一個local planner的包就容易多了

1. 創(chuàng)建包

• 創(chuàng)建

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

• 添加類
  我們需要實現(xiàn)一個從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. 接口實現(xiàn)

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
  規(guī)劃控制接口，給我們提供一個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實現(xiàn)

2.3 實現(xiàn)

主要代碼如下肠虽，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源碼，上面幾個接口是在同一個線程被調用陆馁，所有后續(xù)不需要考慮資源競爭找颓，即變量無需加鎖

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發(fā)送點位，選點導航測試

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

通過日志可以看出

• 在全局規(guī)劃(make plan start是我們前面文章新增的astar planner輸出)后LocalPlanner的接口setPlan被調用
• computeVelocityCommands函數沒0.2s被調用一次, 期間機器人也在做圓周運動
• 全局規(guī)劃再次被調用(move_bsae里配置了規(guī)劃頻率1hz叮贩，這里可以看到間隔1s全局規(guī)劃一次)击狮，重復前面的
• 直到超時GoalReached返回true完成

4. 總結

本文簡單實現(xiàn)了一個local planner的插件佛析，顯然實際沒啥用，不過可以作為一個模板彪蓬，基于該模板實現(xiàn)自己的算法寸莫。后面我們將基于該模板實現(xiàn)可用的局部規(guī)劃控制。

本文代碼見sample_local_planner