创建全局路经规划插件global_planner步骤
创建全局路经规划插件global_planner步骤
1.创建一个 ROS 包：
cd ~/catkin_ws/src
catkin_create_pkg my_global_planner_plugin roscpp nav_core angles tf2_geometry_msgs costmap_2d dynamic_reconfigure geometry_msgs nav_msgs navfn pluginlib tf2_ros
2.在该包中创建一个名为 "include" 的文件夹和一个名为 "src" 的文件夹。
cd my_global_planner_plugin
mkdir include
mkdir src
3.在 "include" 文件夹中创建一个名为 "my_global_planner_plugin" 的文件夹，然后在其中创建一个名为 "my_global_planner_plugin.h" 的头文件，这将是你的插件的接口文件。
cd include
mkdir my_global_planner_plugin
cd my_global_planner_plugin
touch my_global_planner_plugin.h
4.打开 "my_global_planner_plugin.h" 文件并添加以下内容：
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
using std::string;
#ifndef MY_GLOBAL_PLANNER_PLUGIN_H
#define MY_GLOBAL_PLANNER_PLUGIN_H
namespace my_global_planner_plugin
{
class MyGlobalPlannerPlugin : public nav_core::BaseGlobalPlanner
{
protected:
/**
* @brief Store a copy of the current costmap in \a costmap. Called by makePlan.
*/
costmap_2d::Costmap2D* costmap_;
costmap_2d::Costmap2DROS* costmap_ros_;
std::string frame_id_;
ros::Publisher plan_pub_;
double step_size_,min_dist_from_robot_;
bool initialized_=false;
public:
MyGlobalPlannerPlugin();
void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
bool makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, std::vector& plan);
};
}
#endif
5.在 "src" 文件夹中创建一个名为 "my_global_planner_plugin.cpp" 的文件，这将是你的插件的实现文件。
cd ../src
touch my_global_planner_plugin.cpp
6.打开 "my_global_planner_plugin.cpp" 文件并添加以下内容：
#include
#include
namespace my_global_planner_plugin
{
MyGlobalPlannerPlugin::MyGlobalPlannerPlugin() {}
void MyGlobalPlannerPlugin::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) {}
bool MyGlobalPlannerPlugin::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, std::vector& plan)
{
// Your global planning algorithm goes here
// Return true if a valid plan was found, false otherwise
return true;
}
}
7.在 "CMakeLists.txt" 文件中添加以下内容：
cmake_minimum_required(VERSION 2.8.3)
project(my_global_planner_plugin)
find_package(catkin REQUIRED COMPONENTS
angles
costmap_2d
dynamic_reconfigure
geometry_msgs
nav_core
navfn
nav_msgs
pluginlib
roscpp
tf2_geometry_msgs
tf2_ros
)
catkin_package(
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}
CATKIN_DEPENDS
costmap_2d
dynamic_reconfigure
geometry_msgs
nav_core
navfn
nav_msgs
pluginlib
roscpp
tf2_ros
)
include_directories(
include
${catkin_INCLUDE_DIRS}
)
add_library(my_global_planner_plugin src/my_global_planner_plugin.cpp)
target_link_libraries(my_global_planner_plugin ${catkin_LIBRARIES})
install(TARGETS my_global_planner_plugin
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
install(FILES my_global_planner.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
8.创建一个全局规划器插件 my_global_planner.xml 文件：
在 my_global_planner_plugin 包的 目录中创建一个 XML 描述文件
例如：
cd my_global_planner_plugin
touch my_global_planner.xml my_global_planner.xml，并将以下内容添加到该文件中：
This plugin provides a custom global planner for navigation.
其中，path 属性是插件库的路径，type 属性是插件的名称和命名空间，base_class_type 属性是插件类的基类。
插件中 my_global_planner_plugin 与CMakeLists.txt 中add_library(my_global_planner_plugin src/my_global_planner_plugin.cpp) 中的俄节点名保持一至。
path中的 libmy_global_planner_plugin是与add_library(my_global_planner_plugin src/my_global_planner_plugin.cpp) 中的俄节点名前加上lib
9. 在 "package.xml" 文件中添加以下内容：
angles
tf2_geometry_msgs
costmap_2d
dynamic_reconfigure
geometry_msgs
nav_core
nav_msgs
navfn
pluginlib
roscpp
tf2_ros
包括注册插件 my_global_planner.xml
10.将插件注册到 ROS：
在 "my_global_planner_plugin.cpp" 文件中，我们使用了一个名为 "PLUGINLIB_EXPORT_CLASS" 的宏来注册我们的插件。这个宏来自于 "pluginlib/class_list_macros.h" 头文件。
#include 它允许我们将插件注册到 ROS 的插件库中，使得 ROS 可以在运行时动态加载插件。
"PLUGINLIB_EXPORT_CLASS" 宏的语法如下：
PLUGINLIB_EXPORT_CLASS(package::Class, base_class::BaseClass)
其中，"package::Class" 是你的插件类名，"base_class::BaseClass" 是你的插件所继承的基类名。在本例中，我们的插件类名为 "my_global_planner_plugin::MyGlobalPlannerPlugin"，继承自 "nav_core::BaseGlobalPlanner" 基类。
将这行代码添加到 "my_global_planner_plugin.cpp" 的文件中：
PLUGINLIB_EXPORT_CLASS(my_global_planner_plugin::MyGlobalPlannerPlugin, nav_core::BaseGlobalPlanner)
11.在 "my_global_planner_plugin.cpp" 文件中实现插件接口：
在 "my_global_planner_plugin.h" 文件中，我们定义了插件类 "MyGlobalPlannerPlugin" 继承自 "nav_core::BaseGlobalPlanner" 基类。"nav_core::BaseGlobalPlanner" 是 move_base 中定义的全局规划器接口，它包含了规划器需要实现的三个函数：initialize、makePlan 和 getName。我们需要在插件中实现这三个函数。
打开 "my_global_planner_plugin.cpp" 文件并添加以下内容：
#include
#include
PLUGINLIB_EXPORT_CLASS(my_global_planner_plugin::MyGlobalPlannerPlugin, nav_core::BaseGlobalPlanner)
namespace my_global_planner_plugin
{
MyGlobalPlannerPlugin::MyGlobalPlannerPlugin() {}
void MyGlobalPlannerPlugin::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) {}
bool MyGlobalPlannerPlugin::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, std::vector& plan)
{
// Your global planning algorithm goes here
// Return true if a valid plan was found, false otherwise
return true;
}
std::string MyGlobalPlannerPlugin::getName() const
{
return "my_global_planner_plugin";
}
}
12.在 "my_global_planner_plugin.cpp" 文件中实现路径规划算法：
现在，我们可以在 "makePlan" 函数中实现我们的全局路径规划算法。在这个函数中，我们需要将起始点和目标点转换为像素坐标，然后使用这些坐标在 costmap 中搜索可行的路径。如果找到了一条路径，我们需要将它转换为全局坐标系中的路径点，并存储在 "plan" 中。
下面是一个简单的示例，它使用 A* 算法在二维网格中搜索最短路径：
bool MyGlobalPlannerPlugin::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, std::vector& plan)
{
// Convert start and goal pose to pixel coordinates
unsigned int start_x, start_y, goal_x, goal_y;
if (!costmap_->worldToMap(start.pose.position.x, start.pose.position.y, start_x, start_y))
{
ROS_WARN("Start pose lies outside the costmap.");
return false;
}
if (!costmap_->worldToMap(goal.pose.position.x, goal.pose.position.y, goal_x, goal_y))
{
ROS_WARN("Goal pose lies outside the costmap.");
return false;
}
// Define a 2D array to store the search space
std::vector> grid(costmap_->getSizeInCellsX(), std::vector(costmap_->getSizeInCellsY(), 0));
// Initialize the grid with costmap data
for (unsigned int x = 0; x < costmap_->getSizeInCellsX(); x++)
{
for (unsigned int y = 0; y < costmap_->getSizeInCellsY(); y++)
{
unsigned int index = costmap_->getIndex(x, y);
if (costmap_->getCost(x, y) == costmap_2d::LETHAL_OBSTACLE || costmap_->getCost(x, y) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
{
grid[x][y] = 1;
}
}
}
// Run A* search to find the shortest path
std::vector> path;
astar_search(grid, start_x, start_y, goal_x, goal_y, path);
// Convert path to world coordinates and store in plan
for (unsigned int i = 0; i < path.size(); i++)
{
double x, y;
costmap_->mapToWorld(path[i].first, path[i].second, x, y);
geometry_msgs::PoseStamped pose;
pose.pose.position.x = x;
pose.pose.position.y = y;
pose.pose.orientation.w = 1.0;
pose.header.stamp = ros::Time::now();
pose.header.frame_id = costmap_ros_->getGlobalFrameID();
plan.push_back(pose);
}
return true;
}
官方提供的示列：
#include
#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){
if(!initialized_){
// initialize other planner parameters
ros::NodeHandle private_nh("~/" + name);
costmap_ros_ = costmap_ros; //initialize the costmap_ros_ attribute to the parameter.
costmap_ = costmap_ros_->getCostmap(); //get the costmap_ from costmap_ros_
//路径发布
plan_pub_ = private_nh.advertise("plan", 1);
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 GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, std::vector& 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);
tf2::Quaternion goal_quat;
// Create a quaternion from roll/pitch/yaw in radians (0, 0, 0)
goal_quat.setRPY(0, 0, 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);
//create a message for the plan
nav_msgs::Path gui_path;
gui_path.poses.resize(plan.size());
gui_path.header.frame_id = frame_id_;
gui_path.header.stamp = ros::Time::now();
// Extract the plan in world co-ordinates, we assume the path is all in the same frame
for (unsigned int i = 0; i < plan.size(); i++) {
gui_path.poses[i] = plan[i];
}
plan_pub_.publish(gui_path);
return true;
}
}; initialize 方法的实现，用于从ROS参数服务器中读取参数：
bool MyGlobalPlannerPlugin::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
{
if(!initialized_){
// initialize other planner parameters
ros::NodeHandle private_nh("~/" + name);
costmap_ros_ = costmap_ros; //initialize the costmap_ros_ attribute to the parameter.
costmap_ = costmap_ros_->getCostmap(); //get the costmap_ from costmap_ros_
//路径发布
plan_pub_ = private_nh.advertise("plan", 1);
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");
}
官方提供的参考示例：
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");
}13.编译和安装插件：
运行以下命令在终端中编译你的插件：
catkin build my_global_planner_plugin
这将会在 catkin workspace 中构建你的包和插件。如果编译成功，你会看到以下输出：
-- +++ processing catkin package: 'my_global_planner_plugin'
-- ==> add_subdirectory(my_global_planner_plugin)
-- Configuring done
-- Generating done
-- Build files have been written to: /path/to/catkin_workspace/build
现在，你可以将你的插件安装到系统中，以便让 ROS 运行时能够找到它。在终端中运行以下命令：
catkin build my_global_planner_plugin --make-args install
输出：
-- Installing: /path/to/catkin_workspace/install/lib/libmy_global_planner_plugin.so
-- Installing: /path/to/catkin_workspace/install/include/my_global_planner_plugin/my_global_planner_plugin.h
14.验证插件是否成功：
rospack plugins --attrib=plugin nav_core输出：输出：
teb_local_planner /opt/ros/noetic/share/teb_local_planner/teb_local_planner_plugin.xml
dwa_local_planner /home/sukai/workspace/workspace_ros_car_noetic/src/navigation-noetic/dwa_local_planner/blp_plugin.xml
rotate_recovery /home/sukai/workspace/workspace_ros_car_noetic/src/navigation-noetic/rotate_recovery/rotate_plugin.xml
waypoint_global_planner /home/sukai/workspace/workspace_ros_car_noetic/src/navigation-noetic/navigation_planner/waypoint-global-planner/bgp_plugin.xml
eband_local_planner /home/sukai/workspace/workspace_ros_car_noetic/src/navigation-noetic/eband_local_planner/elp_plugin.xml
navfn /home/sukai/workspace/workspace_ros_car_noetic/src/navigation-noetic/navfn/bgp_plugin.xml
move_slow_and_clear /home/sukai/workspace/workspace_ros_car_noetic/src/navigation-noetic/move_slow_and_clear/recovery_plugin.xml
global_planner /home/sukai/workspace/workspace_ros_car_noetic/src/navigation-noetic/global_planner/bgp_plugin.xml
base_local_planner /home/sukai/workspace/workspace_ros_car_noetic/src/navigation-noetic/base_local_planner/blp_plugin.xml
my_global_planner_plugin /home/sukai/workspace/workspace_ros_car_noetic/src/navigation-noetic/my_global_planner_plugin/my_global_planner.xml
pathfollow_local_planner /home/sukai/workspace/workspace_ros_car_noetic/src/navigation-noetic/navigation_planner/pathfollow_local_planner/pathfollow_local_planner_plugin.xml
carrot_planner /home/sukai/workspace/workspace_ros_car_noetic/src/navigation-noetic/carrot_planner/bgp_plugin.xml
clear_costmap_recovery /home/sukai/workspace/workspace_ros_car_noetic/src/navigation-noetic/clear_costmap_recovery/ccr_plugin.xml
其中：
my_global_planner_plugin /home/sukai/workspace/workspace_ros_car_noetic/src/navigation-noetic/my_global_planner_plugin/my_global_planner.xml10. 创建一个名为 "params" 的文件夹，并在其中创建一个名为 "my_global_planner_params.yaml" 的文件，用于设置你的插件的参数。
cd ~/catkin_ws/src/my_global_planner_plugin
mkdir params
touch params/my_global_planner_params.yaml打开 "my_global_planner_params.yaml" 文件并添加以下内容：
my_global_planner_plugin:
tolerance: 0.2在 launch 文件中使用你的插件：
报错
ailed to create the my_global_planner_plugin/MyGlobalPlannerPlugin planner, are you sure it is properly registered and that the containing library is built? Exception: According to the loaded plugin descriptions the class my_global_planner_plugin/MyGlobalPlannerPlugin with base class type nav_core::BaseGlobalPlanner does not exist. Declared types are carrot_planner/CarrotPlanner global_planner/GlobalPlanner my_global_planner_plugin::MyGlobalPlannerPlugin navfn/NavfnROS waypoint_global_planner/WaypointGlobalPlanner
这个错误是在加载ROS（机器人操作系统）中的全局路径规划插件时出现的。它说找不到一个名为my_global_planner_plugin/MyGlobalPlannerPlugin的类，这个类应该是从nav_core::BaseGlobalPlanner类继承而来的。可能原因是这个类没有被正确地注册或者它所在的库文件没有被正确地编译。根据已经加载的插件描述，可用的类包括carrot_planner/CarrotPlanner、global_planner/GlobalPlanner、my_global_planner_plugin::MyGlobalPlannerPlugin、navfn/NavfnROS和waypoint_global_planner/WaypointGlobalPlanner。
解决：
要解决这个问题，需要检查以下几个方面：
-
检查插件是否正确注册：在使用该插件前，需要在launch文件或代码中注册该插件。检查一下是否正确地注册了名为my_global_planner_plugin/MyGlobalPlannerPlugin的插件。
-
检查库文件是否正确编译：确保该插件所在的库文件已经正确地编译并且已经被ROS正确地加载。如果库文件没有正确地编译，可以尝试重新编译库文件。
-
检查类的继承关系是否正确：确保该插件的类从nav_core::BaseGlobalPlanner类正确地继承。如果继承关系不正确，可以尝试修改代码以确保正确的继承关系。
-
检查宏 是否准确
#include
PLUGINLIB_EXPORT_CLASS(my_global_planner_plugin::MyGlobalPlannerPlugin, nav_core::BaseGlobalPlanner)
- 全局规划器插件 my_global_planner.xml 文件中缺失 name 属性：name="my_global_planner_plugin/MyGlobalPlannerPlugin" 也会出现此类问题
如果以上步骤都没有解决问题，可以尝试重新安装
参考资料
navigation/Tutorials/Writing A Global Path Planner As Plugin in ROS - ROS Wiki