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

纠错,疑问,交流: 请进入讨论区点击加入Q群

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


标签: ros与navigation教程, navigation教程