网站导航
> 技术文档 > ROS平台上使用C++实现A*算法_ros2的路径规划算法实现c++

ROS平台上使用C++实现A*算法_ros2的路径规划算法实现c++

网友: 技术文档 2025-07-30 21:07:58

1.概述

在ROS平台上使用C++实现A*算法，并使用map_server在rviz上进行可视化，通过插件获取起点和终点进行路径规划，并通过path显示。

2.平台

ubuntu20.04 +ROS-Noetic

3.具体步骤

假设我们已经有了一张map，将其导入rviz中，我们可以获得地图数据，然后获取起点/initialpose和终点/move_base_simple/goal的坐标，这时我们就可以编写代码进行算法层面上的路径规划，最后将该路径上所有的坐标点通过path话题传入rviz中并在上面进行可视化显示。

3.1载入地图

roscore
rosrun map_server map_server map.yaml
//载入地图的路径，一定要在yaml路径

然后输入代码打开rviz可视化平台：

rviz

左下角Add添加Map，在下拉Topic中选择/map话题，就可以看到相应的地图了。

以上完成了载入地图到rviz中的操作，但是吧，这样只是在rviz上的显示，并没有实际获取地图的数据，所以我们需要订阅/map话题，然后读取其中的数据。/map话题中的数据类型为nav_msgs/OccupancyGrid，消息具体内容如下：

geometry_msgs/Point position

现在数据类型知道了，就需要通过订阅话题/map，然后读取话题内容的地图信息存储到代码中，这里比较重要的有两点，首先是，我们一般会通过如下步骤进行订阅topic：

//主函数中ros::NodeHandle n;ros::Subscriber map_sub = n.subscribe(\"/map\", 10, &MapCallback);//重写MapCallback回调函数void MapCallback(const nav_msgs::OccupancyGrid msg){/*code*/}

 先定义一个类，然后再写订阅代码：

class MapClass{ public: int origin_x = 0; int origin_y = 0; float resolution = 0; int width = 0; int height = 0; vector<vector> map_data; //采用二维容器，可以自适应调整长度，存储栅栏属性 public: void MapCallback(const nav_msgs::OccupancyGrid msg);};void MapClass::MapCallback(const nav_msgs::OccupancyGrid msg){ origin_x = msg.info.origin.position.x; origin_y = msg.info.origin.position.y; resolution = msg.info.resolution; //像素：米/像素 width = msg.info.width; height = msg.info.height; // for(int i=0;i<height;i++) map_data.push_back(vector()); // for(int i=0;i<height;i++){ // for(int j=0;j<width;j++){ // map_data[j].push_back(msg.data[i*width + j]); // } // } //祭奠我傻逼的获取地图数据 for(int i=0;i<width;i++) map_data.push_back(vector()); for(int i=0;i<width;i++){for(int j=0;j<height;j++){map_data[i].push_back(msg.data[j*width+i]);} }}//主函数中MapClass mapclass;ros::NodeHandle n;ros::Subscriber map_sub = n.subscribe(\"/map\", 10, &MapClass::MapCallback, &mapclass);//这是重点，使用MapClass的类里的回调函数就可以把地图数据获取出来

这样我们就把获得了一个含有地图全部数据的类mapclass。

3.2 通过/initialpose在rviz设置起点

这里比较好理解，在rviz上设置起点，实际上就是鼠标在地图上选一个点，然后这个点的信息会通过/initialpose话题发布，这时只需要订阅该话题就可以获取其中信息，选取起点的步骤如下：

这个话题中的信息类型是\"geometry_msgs/PoseWithCovarianceStamped\"，查看消息数据格式为：

rosmsg show geometry_msgs/PoseWithCovarianceStamped std_msgs/Header header uint32 seq //帧数 time stamp //时间戳 string frame_id  //地图参考的坐标系idgeometry_msgs/PoseWithCovariance pose geometry_msgs/Pose pose geometry_msgs/Point position //位置 float64 x float64 y float64 z geometry_msgs/Quaternion orientation //四元数姿态 float64 x float64 y float64 z float64 w float64[36] covariance

这里面暂时四元数姿态用不上，具体需要frame_id和pose就够用了，订阅方式我暂时用了最简单的方法：

float init_pose_point[3]={0};void InitPoseCallback(const geometry_msgs::PoseWithCovarianceStamped msg){ init_pose_point[0] = 1; //标志位，1表示这个起点没被用过，0表示被用完了 init_pose_point[1] = msg.pose.pose.position.x; init_pose_point[2] = msg.pose.pose.position.y;}//main中ros::Subscriber init_pose_sub = n.subscribe(\"/initialpose\",10, &InitPoseCallback);

这样就获得了初始点的坐标。

3.3通过move_base_simple/goal获取终点坐标

消息类型为geometry_msgs::PoseStamped

消息格式为

rosmsg show geometry_msgs/PoseStamped std_msgs/Header header uint32 seq //帧数 time stamp //时间戳 string frame_id  //地图参考的坐标系idgeometry_msgs/Pose pose geometry_msgs/Point position //位置 float64 x float64 y float64 z geometry_msgs/Quaternion orientation //四元数姿态 float64 x float64 y float64 z float64 w

3.4、A*算法实现

通过上述三个步骤，已经获取了地图数据、起点和终点的坐标，那么就可以编写A*算法进行路径规划了。

（1）A*算法理论：A*算法理论视频

（2）算法实现：A*算法理论文章

这篇文章里面的A*算法简单易懂，并且代码对于初学C++的我来说显得很厉害，很简单，包括对类、vector、list的指针的使用。略微修改后如下：

astar.h

#pragma once/*//A*算法对象类*/#include #include const int kCost1 = 10; //直移一格消耗const int kCost2 = 17; //斜移一格消耗struct Point{int x, y; //点坐标，这里为了方便按照C++的数组来计算，x代表横排，y代表竖列int F, G, H; //F=G+HPoint *parent; //parent的坐标，这里没有用指针，从而简化代码Point(int _x, int _y) :x(_x), y(_y), F(0), G(0), H(0), parent(NULL) //变量初始化{}};class Astar{public: //构造函数，输入为_maze的地址void InitAstar(std::vector<std::vector> &_maze); //路径点liststd::list GetPath(Point &startPoint, Point &endPoint, bool isIgnoreCorner);private: //寻找路径的函数，返回的是点的指针Point *findPath(Point &startPoint, Point &endPoint, bool isIgnoreCorner); //获取周围点 std::vector getSurroundPoints(const Point *point, bool isIgnoreCorner) const;//判断某点是否可以用于下一步判断 bool isCanreach(const Point *point, const Point *target, bool isIgnoreCorner) const; //判断开启/关闭列表中是否包含某点 Point *isInList(const std::list &list, const Point *point) const; //从开启列表中返回F值最小的节点Point *getLeastFpoint(); //计算FGH值int calcG(Point *temp_start, Point *point);int calcH(Point *point, Point *end);int calcF(Point *point);private:std::vector<std::vector> maze;std::list openList; //开启列表std::list closeList; //关闭列表public:bool maze_flag = true; //加载地图的标志位，省得多次加载浪费资源};

astar.cpp

#include #include \"astar.h\"#include void Astar::InitAstar(std::vector<std::vector> &_maze){maze = _maze;}Point *Astar::getLeastFpoint() //返回最小F的点{if (!openList.empty()){auto resPoint = openList.front(); //返回第一个元素for (auto &point : openList){if (point->FF)resPoint = point;}return resPoint;}return NULL;}Point *Astar::findPath(Point &startPoint, Point &endPoint, bool isIgnoreCorner){openList.push_back(new Point(startPoint.x, startPoint.y)); //置入起点,拷贝开辟一个节点，内外隔离while (!openList.empty()){auto curPoint = getLeastFpoint(); //找到F值最小的点//ROS_INFO(\"%d,%d\",curPoint->x,curPoint->y);openList.remove(curPoint); //从开启列表中删除closeList.push_back(curPoint); //放到关闭列表//1,找到当前周围八个格中可以通过的格子//ROS_INFO(\"改找周围点了\");auto surroundPoints = getSurroundPoints(curPoint, isIgnoreCorner);for (auto &target : surroundPoints){//ROS_INFO(\"开始判断周围点了\");//2,对某一个格子，如果它不在开启列表中，加入到开启列表，设置当前格为其父节点，计算F G Hif (!isInList(openList, target)){//BROS_INFO(\"判断每个目标点\");target->parent = curPoint; //给子节点放父节点 target->G = calcG(curPoint, target);target->H = calcH(target, &endPoint);target->F = calcF(target); openList.push_back(target);}//3，对某一个格子，它在开启列表中，计算G值, 如果比原来的大, 就什么都不做, 否则设置它的父节点为当前点,并更新G和Felse{int tempG = calcG(curPoint, target);if (tempGG){target->parent = curPoint; target->G = tempG;target->F = calcF(target);}}{ //如果到这一步，就说明已经结束了Point *resPoint = isInList(openList, &endPoint);if (resPoint)return resPoint; //返回列表里的节点指针，不要用原来传入的endpoint指针，因为发生了深拷贝}}}return NULL;} std::list Astar::GetPath(Point &startPoint, Point &endPoint, bool isIgnoreCorner){//ROS_INFO(\"开始坐标:(%d,%d),结束坐标:(%d,%d)\",startPoint.x,startPoint.y,endPoint.x,endPoint.y);Point *result = findPath(startPoint, endPoint, isIgnoreCorner);//ROS_INFO(\"找到路径了\");std::list path;//返回路径，如果没找到路径，返回空链表while (result){path.push_front(result);result = result->parent;} // 清空临时开闭列表，防止重复执行GetPath导致结果异常openList.clear();closeList.clear();ROS_INFO(\"获取路径成功\");return path;} Point *Astar::isInList(const std::list &list, const Point *point) const{//判断某个节点是否在列表中，这里不能比较指针，因为每次加入列表是新开辟的节点，只能比较坐标for (auto p : list){if (p->x == point->x&&p->y == point->y)return p;}return NULL;} bool Astar::isCanreach(const Point *point, const Point *target, bool isIgnoreCorner) const{//ROS_INFO(\"判断是否可以到达\");//判断是否可以到达if (target->xx>maze.size()-1 //x不在地图边界内|| target->yy>maze[0].size() - 1 //y不在地图边界内|| maze[target->x][target->y] == 100//障碍物|| (target->x == point->x&&target->y == point->y)//与当前节点重合|| isInList(closeList, target)) //如果点与当前节点重合、超出地图、是障碍物、或者在关闭列表中，返回false{return false;}else{//ROS_INFO(\"可以到达\");if (abs(point->x - target->x) + abs(point->y - target->y) == 1) //非斜角可以{//ROS_INFO(\"非斜角\");return true;}else{//斜对角要判断是否绊住，意思就是我能不能从直线走到斜对角去if (maze[point->x][target->y] == 0 && maze[target->x][point->y] == 0){//ROS_INFO(\"拌不住\");return true;}else{//ROS_INFO(\"绊住了\");return isIgnoreCorner; //到这就说明是真的被绊住了，那就看用不用考虑会被绊住的条件}}}} std::vector Astar::getSurroundPoints(const Point *point, bool isIgnoreCorner) const{std::vector surroundPoints;for (int x = point->x - 1; x x + 1; x++){for (int y = point->y - 1; y y + 1; y++){if (isCanreach(point, new Point(x, y), isIgnoreCorner)){surroundPoints.push_back(new Point(x, y));//ROS_INFO(\"在放点\");}}}return surroundPoints;}int Astar::calcG(Point *temp_start, Point *point){//如果x，y绝对值加起来是0那么就是一个点，如果是差1，那么就是差一格，如果不是1那就是斜着走的int extraG = (abs(point->x - temp_start->x) + abs(point->y - temp_start->y)) == 1 ? kCost1 : kCost2; //如果是初始节点，则其父节点是空，父节点的G就为0int parentG = point->parent == NULL ? 0 : point->parent->G; return parentG + extraG;}int Astar::calcH(Point *point, Point *end){//return sqrt((double)(end->x - point->x)*(double)(end->x - point->x) + (double)(end->y - point->y)*(double)(end->y - point->y))*kCost1;return (abs(end->x - point->x)+abs(end->y - point->y)) * kCost1;} int Astar::calcF(Point *point){return point->G + point->H;}

这样就就完成了A*算法的修改，主函数具体调用为：

main.cpp

#include #include #include #include

3.5、发布规划的path到rviz上并显示

主函数我其实已经放了publisher一个path_pub，话题名称为/path，消息类型为nav_msgs::Path，具体格式为：

std_msgs/Header header uint32 seq time stamp string frame_idgeometry_msgs/PoseStamped[] poses //这里是关键，这里其实是PoseStamped消息的一维数组，所以需要定义路径上每个点的PoseStamped消息，然后用push_back放在末尾 std_msgs/Header header uint32 seq time stamp string frame_id geometry_msgs/Pose pose geometry_msgs/Point position float64 x float64 y float64 z geometry_msgs/Quaternion orientation float64 x float64 y float64 z float64 w

3.6 编写launch文件

4、效果图

5.代码地址

https://github.com/zhuofsky/ros/tree/master/catkin_ws/src/astar_planning

6.参考

https://zhuanlan.zhihu.com/p/666365243

网络标签：

相关问题