Astar实现自定义的全局路径规划插件
文章目录
本篇代码： astar_plugin
参考链接： https://www.ncnynl.com/archives/201708/1887.html
参考视频： https://www.bilibili.com/video/BV1JF41137RH?spm_id_from=333.337.search-card.all.click
相信看过前几篇关于Astar算法的原理和使用，大家基本对算法本身有了一定了解，那么怎么把自己实现的路径规划放到真实机器人中，让机器人按照你实现的全局路径规划算法进行运动。
ROS具有及其丰富的插件机制，让众多开发者只需关注算法本身的实现，然后通过插件注册机制。本文会一步一步进行插件机制如何使用，并不会过多介绍算法本体。
1.插件功能包的建立
首先给出本文实现的Astar插件功能包的文件索引图
相关文件夹和文件说明如下
include：算法本体和插件机制实现的头文件
src：算法本体和插件机制实现的源文件
config：定位和导航的配置文件，测试时使用
params：导航和自身插件所需要的参数设置文件
launch：测试插件的启动文件
rviz：rviz配置文件
maps：测试所需要的仿真地图文件
CMakeLists.txt：编译插件包
package.xml：编译和注册插件包
astar_plugin.xml：插件包的说明
2. 相关步骤
2.1 建立工作空间和环境变量的配置
在根目录下
mkdir -p astar_ws/src
cd astar_ws/src
catkin_init_worksapce
cd ..
catkin_make
配置环境变量
sudo gedit ~/.bashrc
在打开的文件下方添加
source ~/astar_ws/devel/setup.bash
最后在配置环境变量
source ~/.bashrc
2.2 建立功能包
cd astar_ws/src
catkin_create_pkg astar_plugin nav_core roscpp rospy std
catkin_make
注意在创建功能包一定需要添加nav_core消息类型
2.3 添加源文件与头文件
include头文件/src源文件包括
Astar.h/Astar.cpp：Astar算法本体文件,就是Astar与C++可视化在RVIZ的二维栅格地图章的算法
AstarNode.h/AstarNode.cpp：插件机制与对外接口函数
2.3.1AstarNode.h头文件
头文件基本框架如下，首先定义一个命名空间Astar_planner,然后定义一个C++类AstarPlannerRos ，该类一定要继承nav_core::BaseGlobalPlanner。然后在该类中重写initialize，makePlan方法。
#pragma once
#ifndef __ROS_Astar__
#define __ROS_Astar__
#include <iostream>
#include <stdio.h>
#include <string.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h>
#include <string>
#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <move_base_msgs/MoveBaseGoal.h>
#include <move_base_msgs/MoveBaseActionGoal.h>
#include "sensor_msgs/LaserScan.h"
#include "sensor_msgs/PointCloud2.h"
#include <nav_msgs/Odometry.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/GetPlan.h>
#include <tf/tf.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
#include <boost/foreach.hpp>
#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 <base_local_planner/world_model.h>
#include <base_local_planner/costmap_model.h>
#include <set>
using namespace std;
namespace Astar_planner
{
class AstarPlannerRos:public nav_core::BaseGlobalPlanner
{
public:
AstarPlannerRos();
AstarPlannerRos(ros::NodeHandle &);
AstarPlannerRos(std::string name,costmap_2d::Costmap2DROS* costmap_ros);
ros::NodeHandle ROSNodeHandle;
void loadRosParamFromNodeHandle(const ros::NodeHandle& nh);
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);
std::vector<int> WorldTomap(double wx,double wy);
std::vector<double> MapToWorld(int my, int mx);
costmap_2d::Costmap2DROS* costmap_ros_;
costmap_2d::Costmap2D* costmap_;
bool initialize_;
private:
std::vector<int>m_mapData;
int m_width;
int m_hight;
std::string m_distance;
double m_weight_a;
double m_weight_b;
};
};
#endif
2.3.2AstarNode.cpp源文件
源文件主要对头文件中各函数进行实现，其中比较重要的是
loadRosParamFromNodeHandle：该函数用于加载外部的参数文件，实现源代码与参数解耦。自定义函数
initialize：主要用于地图相关数据的读取，地图数据是一维，其中0表示自由可通行区域，100表示障碍物。并不是自定义的函数，需要重写。
makePlan：进行路径轨迹的生成，通过函数自带的起点start和目标点goal，以及算法本身的调用，最终生成plan路径。并不是自定义的函数，需要重写。
注意：并且makePlan在生成路径点的时候，一定要生成从起点到终点的路径，不能生成从终点到起点的路径(Astar一般都是逆向寻找路径点，默认生成从终点到起点的)。这点一定要注意，本人在这个地方卡了很长时间，编译不报错，运行不报错，就是不生成路径，特别难受。这也是本文代码中for (int i=astarPath.size()-1;i>-1;i–)的原因。如果你的算法本身生成就是从起点到终点的，那么就可以直接进行for循环了。
#include <stdio.h>
#include <string>
#include "AstarNode.h"
#include "Astar.h"
#include <pluginlib/class_list_macros.h>
#include <ctime>
PLUGINLIB_EXPORT_CLASS(Astar_planner::AstarPlannerRos,nav_core::BaseGlobalPlanner)
int mapSize;
float originX;
float originY;
int width;
int hight;
float resolution;
namespace Astar_planner
{
AstarPlannerRos::AstarPlannerRos()
{
}
AstarPlannerRos::AstarPlannerRos(ros::NodeHandle &nh)
{
ROSNodeHandle=nh;
}
AstarPlannerRos::AstarPlannerRos(std::string name,costmap_2d::Costmap2DROS* costmap_ros)
{
initialize(name,costmap_ros);
}
void AstarPlannerRos::loadRosParamFromNodeHandle(const ros::NodeHandle& nh)
{
nh.param("distance",m_distance,std::string("euclidean"));
nh.param("weight_a",m_weight_a,1.0);
nh.param("weight_b",m_weight_b,1.0);
}
void AstarPlannerRos::initialize(std::string name,costmap_2d::Costmap2DROS* costmap_ros)
{
cout<<"######### hello astar plugins ##########"<<endl;
if (!initialize_)
{
costmap_ros_=costmap_ros;
costmap_=costmap_ros_->getCostmap();
originX = costmap_->getOriginX();
originY = costmap_->getOriginY();
width = costmap_->getSizeInCellsX();
hight = costmap_->getSizeInCellsY();
resolution = costmap_->getResolution();
ros::NodeHandle nh("~/"+name);
loadRosParamFromNodeHandle(nh);
mapSize = width*hight;
cout<<"************msg message**********"<<endl;
cout<<"originX:"<<originX<<endl;
cout<<"originY:"<<originY<<endl;
cout<<"width:"<<width<<endl;
cout<<"height:"<<hight<<endl;
cout<<"resolution:"<<resolution<<endl;
cout<<"*********************************"<<endl;
m_mapData.resize(mapSize);
m_width=width;
m_hight=hight;
for (int iy=0;iy<costmap_->getSizeInCellsY();iy++)
{
for (int ix=0;ix<costmap_->getSizeInCellsX();ix++)
{
unsigned int cost=static_cast<int>(costmap_->getCost(ix,iy));
if (cost==0)
m_mapData[iy*width+ix]=0;
else
m_mapData[iy*width+ix]=100;
}
}
ROS_INFO("Astar planner initialized successfully");
initialize_ = true;
}
else
ROS_WARN("This planner has already been initialized... doing nothing");
}
bool AstarPlannerRos::makePlan(const geometry_msgs::PoseStamped& start,const geometry_msgs::PoseStamped& goal,std::vector<geometry_msgs::PoseStamped>& plan)
{
cout<<"######hello astar plan######"<<endl;
if (!initialize_)
{
ROS_ERROR("The planner has not been initialized, please call initialize() to use the planner");
return false;
}
ROS_DEBUG("Got a start: %.2f, %.2f, and a goal: %.2f, %.2f", start.pose.position.x, start.pose.position.y,goal.pose.position.x, goal.pose.position.y);
plan.clear();
if (goal.header.frame_id != costmap_ros_->getGlobalFrameID())
{
ROS_ERROR("This planner as configured will only accept goals in the %s frame, but a goal was sent in the %s frame.",costmap_ros_->getGlobalFrameID().c_str(), goal.header.frame_id.c_str());
return false;
}
tf::Stamped < tf::Pose > goal_tf;
tf::Stamped < tf::Pose > start_tf;
poseStampedMsgToTF(goal, goal_tf);
poseStampedMsgToTF(start, start_tf);
float startX = start.pose.position.x;
float startY = start.pose.position.y;
float goalX = goal.pose.position.x;
float goalY = goal.pose.position.y;
cout<<"startX:"<<startX<<" "<<"startY:"<<startY<<endl;
cout<<"goalX:"<<goalX<<" "<<"goalY:"<<goalY<<endl;
//定义起点
std::vector<std::vector<int> >start_map_point;
start_map_point.clear();
start_map_point.push_back(WorldTomap(startX,startY));
cout<<"start:"<<"("<<start_map_point[0][0]<<","<<start_map_point[0][1]<<")"<<endl;
if (start_map_point[0][0]==-1&&start_map_point[0][1]==-1)
std::cout<<"\033[0;31m[E] : Please set the valid goal point\033[0m"<<endl;
//定义终点
std::vector<std::vector<int> >goal_map_point;
goal_map_point.clear();
goal_map_point.push_back(WorldTomap(goalX,goalY));
cout<<"goal:"<<"("<<goal_map_point[0][0]<<","<<goal_map_point[0][1]<<")"<<endl;
if (goal_map_point[0][0]==-1&&goal_map_point[0][1]==-1)
std::cout<<"\033[0;30m[Kamerider E] : Please set the valid goal point\033[0m"<<endl;
//开始Astar寻路
int xStart=start_map_point[0][1];
int yStart=start_map_point[0][0];
int xStop=goal_map_point[0][1];
int yStop=goal_map_point[0][0];
// float weight_a=1.0;
// float weight_b=1.0;
// std::string distance="euclidean";
ASTAR::CAstar astar(xStart, yStart, xStop, yStop, m_weight_a, m_weight_b,ASTAR::CAstar::PathType::NOFINDPATHPOINT,m_distance);
astar.InitMap(m_mapData,m_width,m_hight);
std::vector<std::pair<int, int> >astarPath; //Astar算法的路径点
clock_t time_stt=clock();
astarPath=astar.PathPoint();
cout<<"\033[0;32m[W] :Astar Get Time:\033[0m"<<1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC<<"ms"<<endl;
cout<<"astar size:"<<astarPath.size()<<endl;
if (astarPath.size()>0)
{
for (int i=astarPath.size()-1;i>-1;i--)
{
geometry_msgs::PoseStamped current_pose=goal;
float x=0.0,y=0.0;
x=MapToWorld(astarPath[i].first,astarPath[i].second)[0];
y=MapToWorld(astarPath[i].first,astarPath[i].second)[1];
current_pose.pose.position.x=x;
current_pose.pose.position.y=y;
current_pose.pose.position.z = 0.0;
current_pose.pose.orientation.x = 0.0;
current_pose.pose.orientation.y = 0.0;
current_pose.pose.orientation.z = 0.0;
current_pose.pose.orientation.w = 1.0;
plan.push_back(current_pose);
}
return true;
}
else
{
ROS_WARN("Not valid start or goal");
return false;
}
}
std::vector<int> AstarPlannerRos::WorldTomap(double wx,double wy)
{
std::vector<int> v;
v.clear();
if (wx<(1.0*originX) || wy<(1.0*originY))
{
v.push_back(-1);
v.push_back(-1);
return v;
}
int mx=int((1.0*(wx-originX))/resolution);
int my=int((1.0*(wy-originY))/resolution);
if (mx<width && my<hight)
{
v.push_back(mx);
v.push_back(my);
return v;
}
}
std::vector<double> AstarPlannerRos::MapToWorld(int my,int mx)
{
std::vector<double> v;
v.clear();
if(mx>width || my>hight)
{
v.push_back(-1);
v.push_back(-1);
return v;
}
double wx=(mx*resolution+originX);
double wy=(my*resolution+originY);
if (wx>originX&&wy>originY)
{
v.push_back(wx);
v.push_back(wy);
return v;
}
}
};
2.4 插件包进行编译
2.4.1 CMakeLists.txt改写
主要是将AstarNode.cpp编译成libastar_plugin_lib.so动态链接库。
cmake_minimum_required(VERSION 2.8.3)
project(astar_plugin)
# add_compile_options(-std=c++11)
find_package(catkin REQUIRED COMPONENTS
nav_core
roscpp
rospy
std_msgs
)
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
include_directories(include/astar_plugin)
add_library(astar_plugin_lib src/AstarNode.cpp src/Astar.cpp)
2.4.2 astar_plugin.xml改写
其中lib/libastar_plugin_lib表示上述生成动态链接库的位置
class 中name按照命名空间/类名，type按照命名空间::类名，base_class_type表示继承的父类。
<library path="lib/libastar_plugin_lib">
<class name="Astar_planner/AstarPlannerRos" type="Astar_planner::AstarPlannerRos" base_class_type="nav_core::BaseGlobalPlanner">
<description>This is astar global planner plugin by iroboapp project.</description>
</class>
</library>
2.4.3 package.xml 改写
前面都是一些基本的ros包一些依赖的添加，主要重要的是export中对插件说明文件astar_plugin.xml进行注册。
<?xml version="1.0"?>
<package format="2">
<name>astar_plugin</name>
<version>0.0.0</version>
<description>The astar_plugin package</description>
<maintainer email="tgj@todo.todo">tgj</maintainer>
<license>TODO</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>nav_core</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_export_depend>nav_core</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>nav_core</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
<nav_core plugin="${prefix}/astar_plugin.xml" />
</export>
</package>
为了插件包的正常工作，建议将上述三者编译文件路径均放在同一文件中，具体位置见插件功能包的建立。
2.4.4 编译
cd astar_ws/src/astar_plugin
catkin_make
2.5 检验插件包是否建立成功
上述如果编译没问题，那么在终端
rospack plugins --attrib=plugin nav_core
进行astar的插件判断
如果没有发现astar插件，并且编译没有报错，极有可能环境变量没有添加成功。
2.6 插件的使用
全局路径规划插件使用当然在ros中的move_base节点中，添加自定义的全局路径规划器base_global_planner，然后动态加载自定义的该插件算法所用到的一些参数文件astar_plugin_param.yaml。并且注意一点将原先存在的全局路径规划器注释掉。
3. 插件执行情况
再说插件执行情况之前，先简单介绍launch与各个文件夹之间的关系。
启动
roslaunch astar_plugin start_plugin.launch
通过rviz中的2D Nav Goal设置一个目标点，则执行情况如下。
|
|
|
|
|
|
|
|
|
|
4. 注意事项
Ubuntn16.04 turtlebot1代功能包安装直接二进制安装
sudo apt-get install ros-kinetic-turtlebot-*
#其实就是缺啥包安装哪个就行
Ubuntn18.04 由于二进制安装的turtlebot为turtlrbot2，代码并不能通用，因此需要通过源码安装turtlebot，网上有很多方法，但是比较费事，现在推荐一款较方便的方法.ROS Melodic安装、配置和使用turtlebot2（集成众多源代码直接下载
#1.安装依赖包
sudo apt-get install ros-melodic-kobuki-*
sudo apt-get install ros-melodic-ecl-streams
sudo apt-get install libusb-dev
sudo apt-get install libspnav-dev
sudo apt-get install ros-melodic-joystick-drivers
sudo apt-get install bluetooth
sudo apt-get install libbluetooth-dev
sudo apt-get install libcwiid-dev
sudo apt-get install ros-melodic-bfl
#2.编译安装，turtlebot_ws为工作空间
git clone https://gitee.com/massif_li/turtlebot_ws.git
cd turtlebot_ws
catkin_make
可能遇到的问题
问题1：Failed to load nodelet [/navigation_velocity_smoother] of type [yocs_velocity_smoother/VelocitySmootherNodelet] even after refreshing the cache: According to the loaded plugin descriptions the class yocs_velocity_smoother/VelocitySmootherNodelet with base class type nodelet::Nodelet does not exist.
解决方法:
sudo apt-get install ros-melodic-yocs-velocity-smoother
参考:原文链接：https://blog.csdn.net/gloria_littlechi/article/details/107402856 、
https://blog.csdn.net/gloria_littlechi/article/details/107402856
问题2：出现ImportError: No module named scipy
解决方法:
sudo apt-get install python-scipy
参考:https://blog.csdn.net/qq_41204464/article/details/103575669
问题3：若运行出现amcl、map_server、move_base等包的缺失，则直接通过二进制安装即可
注意:上述代码是自己慢慢摸索出来的，可能会存在一定的bug，但是整体的全局路径规划算法框架基本没有问题，转载请注明出处，谢谢。
