ROS与C++入门教程-navigation-实现路径规划

ROS与C++入门教程-navigation-实现路径规划

说明：

• 介绍如何编写全局路径规划器作为ROS中的插件

描述：

• 在本教程中，我将介绍在ROS中编写和使用全局路径规划器的步骤。
• 首先要知道的是，为ROS添加一个新的全局路径规划器，新的路径规划器必须遵守在nav_core包中定义的nav_core::BaseGlobalPlanner C ++接口。
• 一旦编写了全局路径规划器，它必须作为插件添加到ROS中，以便它可以被move_base包使用。
• 在本教程中，我将提供从编写路径规划器类开始直到将其部署为插件的所有步骤。
• 我将使用Turtlebot作为机器人的一个例子来部署新的路径规划器。
• 有关如何将真实GA计划程序集成为ROS插件的教程，请参阅在ROS中将遗传算法全局路径规划器添加为插件。
• 此链接中还提供了教程，视频教程

编写路径规划器类

（1）类头文件

• 第一步是为nav_core::BaseGlobalPlanner中的路径规划器编写一个新类。
• 类似的示例可以在carrot_planner.h中找到作为参考。
• 为此，您需要创建一个头文件，我们将在本例中调用global_planner.h。
• 我将只介绍添加插件的最小代码，这是添加任何全局计划程序的必要和常见步骤。
• 最小头文件定义如下：

/** include the libraries you need in your planner here */
 /** for global path planner interface */
 #include <ros/ros.h>
 #include <costmap_2d/costmap_2d_ros.h>
 #include <costmap_2d/costmap_2d.h>
 #include <nav_core/base_global_planner.h>
 #include <geometry_msgs/PoseStamped.h>
 #include <angles/angles.h>
 #include <base_local_planner/world_model.h>
 #include <base_local_planner/costmap_model.h>

 using std::string;

 #ifndef GLOBAL_PLANNER_CPP
 #define GLOBAL_PLANNER_CPP

 namespace global_planner {

 class GlobalPlanner : public nav_core::BaseGlobalPlanner {
 public:

  GlobalPlanner();
  GlobalPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros);

  /** overridden classes from interface nav_core::BaseGlobalPlanner **/
  void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
  bool makePlan(const geometry_msgs::PoseStamped& start,
                const geometry_msgs::PoseStamped& goal,
                std::vector<geometry_msgs::PoseStamped>& plan
               );
  };
 };
 #endif

代码解释：

• 代码：

 #include <ros/ros.h>
 #include <costmap_2d/costmap_2d_ros.h>
 #include <costmap_2d/costmap_2d.h>
 #include <nav_core/base_global_planner.h>
 #include <geometry_msgs/PoseStamped.h>
 #include <angles/angles.h>
 #include <base_local_planner/world_model.h>
 #include <base_local_planner/costmap_model.h>

• 解释：

  • 有必要包括路径规划器所需的核心ROS库。
  • 需要使用<costmap_2d/costmap_2d_ros.h>和<costmap_2d/costmap_2d.h>来使用将由路径计划程序用作输入映射的costmap_2d::Costmap2D类。
  • 当定义为插件时，路径计划器类将自动访问此地图。
  • 没有必要从ROS订阅costmap2d获取代码地图映射。
  • <nav_core/base_global_planner.h>用于导入接口nav_core::BaseGlobalPlanner，插件必须遵守。

• 代码：

namespace  global_planner { 

 class  GlobalPlanner：public  nav_core :: BaseGlobalPlanner {

• 解释：

  • 这是一个好的习惯，虽然不是必要的，为你的类定义命名空间。
  • 在这里，我们将名称空间定义为GlobalPlanner类的global_planner。
  • 命名空间用于定义对类的完全引用，如global_planner::GlobalPlanner。
  • 然后定义类GlobalPlanner，并从接口nav_core::BaseGlobalPlanner继承。
  • 在nav_core::BaseGlobalPlanner中定义的所有方法都必须被新类GlobalPlanner覆盖。

• 代码：

public:

  GlobalPlanner();
  GlobalPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
   /** overriden classes from interface nav_core::BaseGlobalPlanner **/
  void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
  bool makePlan(const geometry_msgs::PoseStamped& start,
                const geometry_msgs::PoseStamped& goal,
                std::vector<geometry_msgs::PoseStamped>& plan
               );

• 解释：

  • 构造函数GlobalPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros)用于初始化costmap，即将用于计划的地图（costmap_ros）和计划器名称（name）。

  • 默认构造函数GlobalPlanner()也是这样，它使用默认值初始化计划器属性。

  • 该方法initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros)是用于一个初始化函数BaseGlobalPlanner，初始化costmap，那就是将被用于规划的地图（costmap_ros）和计划器名称（name）。

  • 必须重写方法bool makePlan(start, goal, plan)

  • 最终计划将存储在方法的参数std::vector<geometry_msgs::PoseStamped>& plan

  • 此计划将通过插件作为主题自动发布。carrot_planner的makePlan方法的实现可以在此链接中找到作为参考。

  • 对于的具体情况carrot_planner的initialize方法的实现如下：

• 代码：

void CarrotPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
     if(!initialized_){
       costmap_ros_ = costmap_ros; //initialize the costmap_ros_ attribute to the parameter.
       costmap_ = costmap_ros_->getCostmap(); //get the costmap_ from costmap_ros_

      // initialize other planner parameters
       ros::NodeHandle private_nh("~/" + name);
       private_nh.param("step_size", step_size_, costmap_->getResolution());
       private_nh.param("min_dist_from_robot", min_dist_from_robot_, 0.10);
       world_model_ = new base_local_planner::CostmapModel(*costmap_);

       initialized_ = true;
     }
     else
       ROS_WARN("This planner has already been initialized... doing nothing");
   }

（2）类实现

• 接下来，我将介绍在实现全局规划器作为插件时要考虑的主要问题。
• 我不会描述一个完整的路径规划算法。
• 我将使用makePlan() 方法的路径规划的虚拟示例仅用于说明目的（这可以在将来改进）。
• 这里是全局计划程序（global_planner.cpp）的最小代码实现，它总是生成一个虚拟静态计划。
• 代码：

#include <pluginlib/class_list_macros.h>
 #include "global_planner.h"

 //register this planner as a BaseGlobalPlanner plugin
 PLUGINLIB_EXPORT_CLASS(global_planner::GlobalPlanner, nav_core::BaseGlobalPlanner)

 using namespace std;

 //Default Constructor
 namespace global_planner {

 GlobalPlanner::GlobalPlanner (){

 }

 GlobalPlanner::GlobalPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
   initialize(name, costmap_ros);
 }


 void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros){

 }

 bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,  std::vector<geometry_msgs::PoseStamped>& plan ){

    plan.push_back(start);
   for (int i=0; i<20; i++){
     geometry_msgs::PoseStamped new_goal = goal;
     tf::Quaternion goal_quat = tf::createQuaternionFromYaw(1.54);

      new_goal.pose.position.x = -2.5+(0.05*i);
      new_goal.pose.position.y = -3.5+(0.05*i);

      new_goal.pose.orientation.x = goal_quat.x();
      new_goal.pose.orientation.y = goal_quat.y();
      new_goal.pose.orientation.z = goal_quat.z();
      new_goal.pose.orientation.w = goal_quat.w();

   plan.push_back(new_goal);
   }
   plan.push_back(goal);
  return true;
 }
 };

• 构造器遵守计划器的要求和规范来实现。
• 在该特定说明性示例中不考虑它们的实现。
• 有几个重要的事情需要考虑：
  • 将计划器注册为BaseGlobalPlanner插件：
    • 通过PLUGINLIB_EXPORT_CLASS(global_planner::GlobalPlanner, nav_core::BaseGlobalPlanner)实现
    • 所以必需包含#include <pluginlib/class_list_macros.h>
  • 实现makePlan()方法：
    • 在start和goal参数分别用于获得初始位置和目标位置。
    • 在该说明性实现中，利用开始位置（plan.push_back(start)）来初始化平面向量。
    • 然后，将在for循环中的计划中静态插入20个新的虚拟位置，然后将目标位置作为最后位置插入计划中。
    • 这个计划路径将被发送到move_base全局计划程序模块，它将通过ROS主题nav_msgs/Path发布它，然后由本地计划程序模块接收。
• 现在你的全局计划器类已经完成，你已经准备好第二步，即创建全局规划器的插件，将其集成到move_base包的全局计划器模块nav_core::BaseGlobalPlanner。

（3）编译

• 要编译上面创建的全局计划器库，它必须添加到CMakeLists.txt。
• 这是要添加的代码：

 add_library(global_planner_lib src/path_planner/global_planner/global_planner.cpp)

• 然后，在终端运行catkin_make在catkin工作区目录下生成二进制文件。
• 这将在lib目录 ~/catkin_ws/devel/lib/libglobal_planner_lib中创建库文件。
• 观察到“lib”被附加到在CMakeLists.txt中声明的库名global_planner_lib

编写插件

• 基本上，重要的是按照插件说明页面中所述的创建新插件所需的所有步骤。
• 有五个步骤：

（1）插件注册

• 首先，您需要通过导出将全局计划程序类注册为插件。
• 为了允许类被动态加载，它必须被标记为导出类。
• 这是通过特殊宏PLUGINLIB_EXPORT_CLASS来完成的。
• 这个宏可以放在构成插件库的任何源（.cpp）文件中，但通常放在导出类的.cpp文件的末尾。
• 这已经在global_planner.cpp中用指令完成了。
• 示例：

PLUGINLIB_EXPORT_CLASS(global_planner::GlobalPlanner, nav_core::BaseGlobalPlanner)

• 这将使global_planner::GlobalPlanner类注册为move_base的nav_core::BaseGlobalPlanner的插件

（2）插件说明文件

• 第二步是在描述文件中描述插件。
• 插件描述文件是一个XML文件，用于以机器可读格式存储有关插件的所有重要信息。
• 它包含有关插件所在的库的信息，插件的名称，插件的类型等。
• 在我们的全局计划程序的情况下，您需要创建一个新文件并将其保存在包中的特定位置我们的例子global_planner包），并给它一个名称，例如global_planner_plugin.xml。
• 插件描述文件（global_planner_plugin.xml）的内容如下所示：

 <library path="lib/libglobal_planner_lib">
  <class name="global_planner/GlobalPlanner" type="global_planner::GlobalPlanner" base_class_type="nav_core::BaseGlobalPlanner">
    <description>This is a global planner plugin by iroboapp project.</description>
  </class>
 </library>

• 在第一行<library path =“lib / libglobal_planner_lib”>中，我们指定了插件库的路径。

• 在这种情况下，路径是lib/libglobal_planner_lib，其中lib是目录 ~/catkin_ws/devel/中的文件夹（请参阅上面的编译部分）。

• 在此行中,我们首先指定global_planner插件的名称，稍后我们将在move_base启动文件中用作参数,它指定要在nav_core中使用的全局计划器。

• 通常使用命名空间（global_planner），后跟/斜杠，然后使用类的名称（ GlobalPlanner）来指定插件的名称。

• 如果不指定名称，则名称将等于类型，在这种情况下将是 global_planner::GlobalPlanner。

• 它建议指定名称以避免混淆。

• 该type指定实现插件的名称，我们示例里是，类global_planner::GlobalPlanner，而该base_class_type指定实现插件名称的基类，我们示例里是，基类nav_core::BaseGlobalPlanner。

• 在 标签提供有关该插件的简要说明。

• 有关插件描述文件及其相关标签/属性的详细描述，请参阅以下文档。

• 为什么我们需要此文件？

  • 我们需要这个文件除了代码宏，允许ROS系统自动发现，加载和解释插件。
  • 插件描述文件还包含重要信息，如插件的描述，不适合在宏中。

（3）使用ROS包系统注册插件

• 为了让pluginlib查询跨所有ROS包的系统上的所有可用插件，每个包必须显式指定它导出的插件和哪些包库包含这些插件。
• 插件提供程序必须在package.xml中指向其export标记块中的插件描述文件。
• 注意，如果您有其他导出，他们都必须在同一导出字段。
• 在我们的全局计划器示例中，相关行将如下所示：

  <export>
  <nav_core plugin="${prefix}/global_planner_plugin.xml" />
  </export>

• 在${prefix}/将自动确定的完整路径文件global_planner_plugin.xml。

• 有关导出插件的详细说明，请参阅以下文档

• 重要说明：为了使上述export命令正常工作，提供包必须直接依赖于包含插件接口的包，在全局计划器的情况下为nav_core。

• 因此，global_planner包必须在其package.xml中包含以下行：

<build_depend>nav_core</build_depend>
<run_depend>nav_core</run_depend>

• 这将告诉编译器关于nav_core包的依赖

（4）查询ROS包系统可用的插件

• 可以通过rospack查询ROS包系统，以查看任何给定包可用的插件。例如：

rospack plugins --attrib = plugin nav_core

• 这将返回从nav_core包导出的所有插件。这里是一个执行的例子：

akoubaa@anis-vbox:~/catkin_ws$ rospack plugins --attrib=plugin nav_core
 rotate_recovery /opt/ros/hydro/share/rotate_recovery/rotate_plugin.xml
 navfn /opt/ros/hydro/share/navfn/bgp_plugin.xml
 base_local_planner /opt/ros/hydro/share/base_local_planner/blp_plugin.xml
 move_slow_and_clear /opt/ros/hydro/share/move_slow_and_clear/recovery_plugin.xml
 global_planner /home/akoubaa/catkin_ws/src/global_planner/global_planner_plugin.xml
 dwa_local_planner /opt/ros/hydro/share/dwa_local_planner/blp_plugin.xml
 clear_costmap_recovery /opt/ros/hydro/share/clear_costmap_recovery/ccr_plugin.xml
 carrot_planner /opt/ros/hydro/share/carrot_planner/bgp_plugin.xml

• 观察我们的插件现在可以在包global_planner下获得
• 并且在文件/home/akoubaa/catkin_ws/src/global_planner/global_planner_plugin.xml中指定。
• 您还可以观察nav_core包中现有的其他插件，包括carrot_planner/CarrotPlanner和navfn，其实现Dijkstra算法。
• 现在，你的插件就可以使用了。

在Turtlebot上运行插件

• 在turtlebot运行你的计划有几个步骤。
• 首先，需要将包含全局计划程序（在本例中为global_planner）的包复制到Turtlebot的catkin工作空间（例如catkin_ws）。
• 然后，您需要运行catkin_make将您的插件导出到您的turtlebot ROS环境。
• 其次，您需要对move_base配置进行一些修改，以指定要使用的新计划程序。
• 为此，请按照以下三个步骤：
• 在ROS Hydro版本中，转到此文件夹/opt/ros/hydro/share/turtlebot_navigation/launch/includes：

$ roscd turtlebot_navigation/
$ cd launch/includes/

• 打开文件move_base.launch.xml（您可能需要sudo来打开并能够保存），并将新的计划程序添加为全局计划程序的参数
• 如下所示：

<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<param name="base_global_planner" value="global_planner/GlobalPlanner"/>

• 保存并关闭move_base.launch.xml。

• 请注意，计划程序的名称是global_planner/GlobalPlanner，与global_planner_plugin.xml中指定的名称相同。

• 现在，您可以使用您的新计划。

• 你现在必须启动你的turtlebot。

• 您需要启动minimal.launch，3dsensor.launch，amcl.launch.xml和move_base.launch.xml。

• 以下是可用于此目的的启动文件的示例。

 <launch>
 <include file="$(find turtlebot_bringup)/launch/minimal.launch"></include>

   <include file="$(find turtlebot_bringup)/launch/3dsensor.launch">
     <arg name="rgb_processing" value="false" />
     <arg name="depth_registration" value="false" />
     <arg name="depth_processing" value="false" />
     <arg name="scan_topic" value="/scan" />
   </include>

   <!-- Map server -->
   <arg name="map_file" default="your_map_folder/your_map_file.yaml"/>
   <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />

   <!-- Localization -->
   <arg name="initial_pose_x" default="0.0"/>
   <arg name="initial_pose_y" default="0.0"/>
   <arg name="initial_pose_a" default="0.0"/>
   <include file="$(find turtlebot_navigation)/launch/includes/amcl.launch.xml">
     <arg name="initial_pose_x" value="$(arg initial_pose_x)"/>
     <arg name="initial_pose_y" value="$(arg initial_pose_y)"/>
     <arg name="initial_pose_a" value="$(arg initial_pose_a)"/>
   </include>

   <!-- Move base -->
   <include file="$(find turtlebot_navigation)/launch/includes/move_base.launch.xml"/>

 </launch>

• 请注意，在使用此启动文件启动turtlebot时，将要考虑在文件move_base.launch.xml中所做的更改。

用RVIZ测试计划器

• 在你启动你的turtlebot后，你可以使用这个命令启动rviz（在新的终端）

$ roslaunch turtlebot_rviz_launchers view_navigation.launch --screen

• 效果：
  

• 现在，您可以通过单击按钮“添加”（在底部）添加要显示在rviz中的所有信息。

• 您将看到以下窗口：
  

• 例如，如果要显示全局路径，请单击“By topic”选项卡，在“move_base”类别下选择“/global_plan”，然后选择“Path”，添加显示名称，如果需要，然后单击确定。您可以以相同的方式添加本地路径。

• 现在点击“2D导航目标”按钮（在顶部），并选择一个目标位置。

• 你现在可以看到你的机器人移动到它的目标。
  

• 终端显示：
  

• 相关地图：willow_garage_map.pgm ， willow_garage_map.yaml