如何更改move_base功能包中的全局路径规划器
-
我仿照了ros-wiki给出的教程将示例插件运用在仿真环境中。在global_planner.cpp中的makePlan()函数中添加了一行ROS_INFO以此来证明插件成功运行，最终终端中没有显示，但rviz中有该插件发布的话题GlobalPlanner/plan。
Q：我的插件是运行成功了嘛？可终端中没有显示我的日志，此外“Could not transform the global plan to the frame of the controller”该错误该如何消除呢？
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 #endifglobal_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_plannerglobal_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”该错误该如何消除呢？
-
@924453613 这两天比较忙，简单看了眼，打印不出来可能是日志级别导致，尝试使用WARN或者ERROR打印。关于transforms错误请检查tf树是否正常。
