ROS与navigation教程-编写自定义全局路径规划

ROS与navigation教程-编写自定义全局路径规划

说明：

介绍在ROS中编写和使用全局路径规划器的步骤，提供从编写路径计划程序类开始直到将其部署为插件的所有步骤。

简介：

首先要知道的是，为了向ROS添加一个新的全局路径规划器，新的路径规划器必须遵守nav_core包中定义的nav_core::BaseGlobalPlanner C ++接口。一旦编写了全局路径规划器，它必须作为插件添加到ROS中，以便它可以被move_base包使用。

我将使用Turtlebot作为机器人的一个例子来部署新的路径规划器。有关如何将真正的GA规划程序集成为ROS插件的教程，参考在ROS中添加遗传算法全局路径规划器作为插件。此链接中还提供了视频教程

(1)编写路径规划类

Class Header/类头文件

第一步是为遵循nav_core::BaseGlobalPlanner的路径规划器编写一个新类。在carrot_planner.h中可以找到一个类似的例子作为参考。为此，您需要创建一个将在本例中调用的global_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类作为输入的地图类。当定义为插件时，路径规划器类将自动访问此地图。没有必要订阅costmap2d来获取ROS的代价地图。<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_ros）以及规划器的名称（name）。默认构造函数GlobalPlanner()即使用默认值作为初始化。方法initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros)是BaseGlobalPlanner的初始化函数，用于初始化costmap，参数为用于规划的代价地图（costmap_ros）和规划器的名称(name)。

对于carrot_planner的具体情况，初始化方法实现如下：

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");
   }

接着，bool makePlan（start，goal，plan）的方法必须要重写。最终规划将存储在方法的参数std::vector<geometry_msgs::PoseStamped>& plan 中。该规划将通过插件自动发布为话题。carrot_planner的makePlan方法的参考。

Class Implementation/类实现

接下来，我将介绍在实施全局规划插件时要考虑的主要问题。我将不会描述完整的路径规划算法。为了说明的目的，我将使用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;
 }
 };

可以根据计划人员的要求和规格实施构造函数。在这个具体的说明性例子中不考虑它们的实现。有几件重要的事情要考虑：

1.注册规划器作为BaseGlobalPlanner插件：这是通过指令PLUGINLIB_EXPORT_CLASS(global_planner::GlobalPlanner, nav_core::BaseGlobalPlanner)完成的。为此，必须包含库 #include <pluginlib/class_list_macros.h>

2.makePlan()方法实现：在开始和目标参数用来获取初始位置和目标位置。在该说明性实现中，以起始位置（plan.push_back(start))）作为开始的规划变量。然后，在for循环中，将在规划中静态插入20个新的虚拟位置，然后将目标位置作为最后一个位置插入规划。然后，此规划路径将发送到move_base全局规划模块，该模块将通过ROS话题nav_msgs/Path进行发布，然后将由本地规划模块接收。

现在，您的全局规划类已经完成，您已准备好进行第二步，即为全局规划类创建插件，将其集成到move_base包的全局规划模块nav_core::BaseGlobalPlanner中。

编译

要编译上面创建的全局规划库，必须将其添加到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

(2)编写你的插件

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

插件注册

首先，您需要通过导出将全局规划类注册为插件。为了允许一个类被动态加载，它必须被标记为一个导出的类。这是通过特殊宏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类的插件。

插件描述文件

第二步是在描述文件中描述该插件。插件描述文件是一个XML文件，用于以机器可读格式存储有关插件的所有重要信息。它包含有关插件库的信息，插件的名称，插件的类型等。以我们的全局规划类为例，您需要创建一个新文件并将其保存在您的包中的某个位置（在我们的case_ 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/（参见上面的编译部分）。在此行中，我们首先指定我们将在move_base启动文件中使用的global_planner插件的名称作为参数，在此行中，它指定了要在nav_core中使用的全局规划器。通常使用命名空间（global_planner）后跟斜杠，然后使用类（ GlobalPlanner）的名称来指定插件的名称。如果不指定名称，那么名称将等于该类型，在这种情况下将是global_planner::GlobalPlanner。它建议指定名称以避免混淆。

该类型指定了实现插件的类的名称，在我们的例子中是global_planner::GlobalPlanner，而base_class_type指定了实现插件的基类的名称，在我们的例子中是nav_core::BaseGlobalPlanner。所述<description> 标记提供关于插件的简要说明。有关插件描述文件及其相关标签/属性的详细说明，请参阅以下文档。

为什么我们需要这个文件？除了代码宏，我们需要这个文件，以允许ROS系统自动发现，加载。插件描述文件还包含重要信息，如插件的描述，不适合加在宏定义里。

注册插件到ROS包系统

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

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

在${PREFIX}/自动确定global_planner_plugin.xml文件完整的路径。有关导出插件的详细讨论，请参阅以下文档。

注意：为了使上述导出命令正常工作，提供程序包必须直接依赖于包含插件接口的程序包，这在全局规划程序的情况下是nav_core。因此，global_planner软件包在其package.xml中必须包含以下内容：

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

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

在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算法。

现在，你的插件就可以使用了。

(3)在Turtlebot上运行插件

在Turtlebot上运行您的规划器有几个步骤。首先，您需要将包含您的全局规划程序（在我们的示例中为global_planner）的软件包复制到Turtlebot的catkin工作区（例如catkin_ws）。然后，您需要运行catkin_make将您的插件导出到您的turtlebot ROS环境中。其次，您需要对move_base配置进行一些修改，以指定要使用的新计划。为此，请按照以下三个步骤：

1.在ROS Hydro版本中，转到此文件夹/opt/ros/hydro/share/turtlebot_navigation/launch/includes，其他版本方法通用。

$ roscd turtlebot_navigation/
$ cd launch/includes/

2.打开文件 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文件名称一致。现在，您已准备好使用新的计划。

3.你现在必须启动你的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文件中所做的更改。

(4)用RVIZ测试计划

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

$ roslaunch turtlebot_rviz_launchers view_navigation.launch --screen

效果图：

请输入图片描述

您现在可以通过点击按钮"Add"（在底部）添加要在rviz中显示的所有信息。您将看到以下窗口：

请输入图片描述

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

现在点击 “2D nav goal”按钮（在顶部），并选择目标位置。你现在可以看到你的机器人走向目标。

请输入图片描述

测试地图：

willow_garage_map.pgm
willow_garage_map.yaml

参考：

http://wiki.ros.org/navigation/Tutorials/Writing%20A%20Global%20Path%20Planner%20As%20Plugin%20in%20ROS

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号