Astar实现自定义的全局路径规划插件

文章目录

Astar实现自定义的全局路径规划插件
- 1.插件功能包的建立
- 2. 相关步骤
- - 2.1 建立工作空间和环境变量的配置
  - 2.2 建立功能包
  - 2.3 添加源文件与头文件
  - - 2.3.1AstarNode.h头文件
    - 2.3.2AstarNode.cpp源文件
  - 2.4 插件包进行编译
  - - 2.4.1 CMakeLists.txt改写
    - 2.4.2 astar_plugin.xml改写
    - 2.4.3 package.xml 改写
    - 2.4.4 编译
  - 2.5 检验插件包是否建立成功
  - 2.6 插件的使用
- 3. 插件执行情况
- 4. 注意事项

本篇代码： 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插件功能包的文件索引图

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;elsem_mapData[iy*width+ix]=100;}}ROS_INFO("Astar planner initialized successfully");initialize_ = true;}elseROS_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 COMPONENTSnav_coreroscpprospystd_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设置一个目标点，则执行情况如下。

	时刻1	时刻2	时刻3
结果

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