鱼香ROS社区
    • 版块
    • 最新
    • 未解决
    • 已解决
    • 群组
    • 注册
    • 登录
    紧急通知：禁止一切关于政治&VPN翻墙等话题，发现相关帖子会立马删除封号
     小鱼 ROS 2 新书上线！点击链接查看， 新书配套视频点击链接查看。
    提问前必看的发帖注意事项—— 提问前必看！不符合要求的问题拒绝回答！！
    社区使用指南—如何添加标签修改密码

    如何更改move_base功能包中的全局路径规划器

    已定时 已固定 已锁定 已移动 未解决
    综合问题
    movebase nav
    2
    2
    881
    正在加载更多帖子
    • 从旧到新
    • 从新到旧
    • 最多赞同
    回复
    • 在新帖中回复
    登录后回复
    此主题已被删除。只有拥有主题管理权限的用户可以查看。
    • 9
      924453613
      最后由 924453613 编辑

      我仿照了ros-wiki给出的教程将示例插件运用在仿真环境中。在global_planner.cpp中的makePlan()函数中添加了一行ROS_INFO以此来证明插件成功运行，最终终端中没有显示，但rviz中有该插件发布的话题GlobalPlanner/plan。

      Q：我的插件是运行成功了嘛？可终端中没有显示我的日志，此外“Could not transform the global plan to the frame of the controller”该错误该如何消除呢？

      2022年4月02日23:48:10.png

      global_planner.h

      /** include the libraries you need in your planner here */
/** for global path planner interface */
//需要包含路径规划器所需的核心ROS库
#include <angles/angles.h>
#include <base_local_planner/costmap_model.h>
#include <base_local_planner/world_model.h>
#include <costmap_2d/costmap_2d.h>      //路径规划器需要使用
#include <costmap_2d/costmap_2d_ros.h>  //路径规划器需要使用
#include <geometry_msgs/PoseStamped.h>
#include <nav_core/base_global_planner.h>  //导入接口nav_core::BaseGlobalPlanner插件必须要继承的父类
#include <ros/ros.h>

using std::string;

#ifndef GLOBAL_PLANNER_CPP
#define GLOBAL_PLANNER_CPP
//定义命名空间：将为GlobalPlanner类定义命名空间为global_planner
namespace global_planner {
//定义类GlobalPlanner并继承接口nav_core::BaseGlobalPlanner
class GlobalPlanner : public nav_core::BaseGlobalPlanner {
 public:
  GlobalPlanner();  //默认构造函数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);  // BaseGlobalPlanner的初始化函数，用于初始化costmap
  bool makePlan(
      const geometry_msgs::PoseStamped &start,
      const geometry_msgs::PoseStamped &goal,
      std::vector<geometry_msgs::PoseStamped> &
          plan);  //最终规划将存储在方法的参数std::vector<geometry_msgs::PoseStamped>&
                  // plan 中
};
};  // namespace global_planner
#endif

      global_planner.cpp
      在其中添加了一行ROS_INFO在终端中打印出来（想以此来证明插件的成功运行）

      #include "global_planner.h"

#include <pluginlib/class_list_macros.h>  //注册规划器需要使用
#include <tf/transform_broadcaster.h>

// register this planner as a BaseGlobalPlanner plugin
PLUGINLIB_EXPORT_CLASS(
    global_planner::GlobalPlanner,
    nav_core::BaseGlobalPlanner)  //注册规划器作为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) {}

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

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);
  ROS_INFO("GlobalPlanner is working"); //插件成功运行
  return true;
}
};  // namespace global_planner

      global_planner_plugin.xml

      <!--指定插件库的路径-->
<library path="lib/libglobal_planner_lib">
    <!--指定我们将在move_base启动文件中使用的global_planner插件的名称作为参数-->
    <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>

      path_turtlebot.launch

      <!--全局路径规划-->
<launch>
    <!--先启动gazebo_world.launch将机器人模型上传至参数服务器-->
    <!--1.读取地图数据-->
    <include file="$(find irobot)/launch/map_server.launch"/>
    <!--2.定位-->
    <include file="$(find irobot)/launch/amcl_diff.launch"/>
    <!--3.路径规划-->
    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
        <!--全局规划器base_global_planner指定为global_planner-->
        <param name="base_global_planner" value="global_planner/GlobalPlanner"/>
        <!--全局路径规划参数-->
        <rosparam file="$(find irobot)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <!--本地路径规划参数-->
        <rosparam file="$(find irobot)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
        <!--全局代价地图参数-->
        <rosparam file="$(find irobot)/param/global_costmap_params.yaml" command="load" />
        <!--局部代价地图参数-->
        <rosparam file="$(find irobot)/param/local_costmap_params.yaml" command="load" />
        <!--局部规划器参数-->
        <rosparam file="$(find irobot)/param/base_local_planner_params.yaml" command="load" />
    </node>
    <!--4.启动rviz-->
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find irobot)/config/path.rviz" />
</launch>

      package.xml

      <?xml version="1.0"?>
<package format="2">
  <name>irobot</name>
  <version>0.0.0</version>
  <description>The irobot package</description>

  <maintainer email="wumen@todo.todo">wumen</maintainer>

  <license>TODO</license>

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>gazebo_plugins</build_depend>
  ......
  <exec_depend>move_base</exec_depend>

  <export>
    <!-- Other tools can request additional information be placed here -->
    <!--STEP2-3.registering plugin with ROS package system-->
    <nav_core plugin="${prefix}/global_planner_plugin.xml" />
  </export>
</package>

      CMakeLists.txt

      cmake_minimum_required(VERSION 3.0.2)
project(irobot)
find_package(catkin REQUIRED COMPONENTS)
catkin_package()
include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)
add_library(global_planner_lib
  src/global_planner.cpp
)

      map_server.launch

      <!--读取地图-->
<launch>
    <!--1.启动map_server功能中读取在map文件夹的地图数据-->
    <arg name="map" default="map.yaml" />
    <node name="map_server" pkg="map_server" type="map_server" args="$(find irobot)/map/$(arg map)"/>
</launch>

      amcl_diff.launch

      <!--amcl差分移动机器人配置-->
<launch>
    <node pkg="amcl" type="amcl" name="amcl" output="screen">
        <param name="odom_model_type" value="diff"/><!--差分移动机器人-->
        <param name="odom_alpha5" value="0.1"/>
        <param name="transform_tolerance" value="0.2" />
        <param name="gui_publish_rate" value="10.0"/>
        <param name="laser_max_beams" value="30"/>
        <param name="min_particles" value="500"/>
        <param name="max_particles" value="5000"/>
        <param name="kld_err" value="0.05"/>
        <param name="kld_z" value="0.99"/>
        <param name="odom_alpha1" value="0.2"/>
        <param name="odom_alpha2" value="0.2"/>
        <param name="odom_alpha3" value="0.8"/>
        <param name="odom_alpha4" value="0.2"/>
        <param name="laser_z_hit" value="0.5"/>
        <param name="laser_z_short" value="0.05"/>
        <param name="laser_z_max" value="0.05"/>
        <param name="laser_z_rand" value="0.5"/>
        <param name="laser_sigma_hit" value="0.2"/>
        <param name="laser_lambda_short" value="0.1"/>
        <param name="laser_lambda_short" value="0.1"/>
        <param name="laser_model_type" value="likelihood_field"/>
        <param name="laser_likelihood_max_dist" value="2.0"/>
        <param name="update_min_d" value="0.2"/>
        <param name="update_min_a" value="0.5"/>
        <param name="odom_frame_id" value="odom"/><!--里程计坐标系-->
        <param name="base_frame_id" value="base_footprint"/><!--底盘坐标系-->
        <param name="global_frame_id" value="map"/><!--地图坐标系-->
        <param name="resample_interval" value="1"/>
        <param name="transform_tolerance" value="0.1"/>
        <param name="recovery_alpha_slow" value="0.0"/>
        <param name="recovery_alpha_fast" value="0.0"/>
    </node>
</launch>

      path_turtlebot.launch

      <!--全局路径规划-->
<launch>
    <!--先启动gazebo_world.launch将机器人模型上传至参数服务器-->
    <!--1.读取地图数据-->
    <include file="$(find irobot)/launch/map_server.launch"/>
    <!--2.定位-->
    <include file="$(find irobot)/launch/amcl_diff.launch"/>
    <!--3.路径规划-->
    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
        <!--全局规划器base_global_planner指定为global_planner-->
        <param name="base_global_planner" value="global_planner/GlobalPlanner"/>
        <!--全局路径规划参数-->
        <rosparam file="$(find irobot)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <!--本地路径规划参数-->
        <rosparam file="$(find irobot)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
        <!--全局代价地图参数-->
        <rosparam file="$(find irobot)/param/global_costmap_params.yaml" command="load" />
        <!--局部代价地图参数-->
        <rosparam file="$(find irobot)/param/local_costmap_params.yaml" command="load" />
        <!--局部规划器参数-->
        <rosparam file="$(find irobot)/param/base_local_planner_params.yaml" command="load" />
    </node>
    <!--4.启动rviz-->
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find irobot)/config/path.rviz" />
</launch>

      Q：我的插件是运行成功了嘛？可终端中没有显示我的日志，此外“Could not transform the global plan to the frame of the controller”该错误该如何消除呢？
      2022年4月02日23:48:10.png

      小鱼小 1 条回复 最后回复 回复 引用 0
      • 小鱼小
        小鱼 技术大佬 @924453613
        最后由 编辑

        @924453613 这两天比较忙，简单看了眼，打印不出来可能是日志级别导致，尝试使用WARN或者ERROR打印。关于transforms错误请检查tf树是否正常。

        小鱼 ROS 2 新书上线！书籍购买链接： https://m.tb.cn/h.gFDBx39
        新书配套视频：https://www.bilibili.com/video/BV1GW42197Ck/

        1 条回复 最后回复 回复 引用 1
        • 第一个帖子
          最后一个帖子
        皖ICP备16016415号-7
        Powered by NodeBB | 鱼香ROS