当前位置： 首页 > news >正文

ROS平台上使用C++实现A*算法

news 2025/7/9 15:02:36

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<int>> 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<int>());// 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<int>());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           //地图参考的坐标系id
geometry_msgs/PoseWithCovariance posegeometry_msgs/Pose posegeometry_msgs/Point position  //位置float64 xfloat64 yfloat64 zgeometry_msgs/Quaternion orientation  //四元数姿态float64 xfloat64 yfloat64 zfloat64 wfloat64[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 headeruint32 seq                //帧数time stamp                //时间戳string frame_id           //地图参考的坐标系id
geometry_msgs/Pose posegeometry_msgs/Point position   //位置float64 xfloat64 yfloat64 zgeometry_msgs/Quaternion orientation    //四元数姿态float64 xfloat64 yfloat64 zfloat64 w

3.4、A*算法实现

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

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

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

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

astar.h

#pragma once
/*
//A*算法对象类
*/
#include <vector>
#include <list>
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<int>> &_maze);//路径点liststd::list<Point *> GetPath(Point &startPoint, Point &endPoint, bool isIgnoreCorner);
private://寻找路径的函数，返回的是点的指针Point *findPath(Point &startPoint, Point &endPoint, bool isIgnoreCorner);//获取周围点std::vector<Point *> getSurroundPoints(const Point *point, bool isIgnoreCorner) const;//判断某点是否可以用于下一步判断bool isCanreach(const Point *point, const Point *target, bool isIgnoreCorner) const; //判断开启/关闭列表中是否包含某点Point *isInList(const std::list<Point *> &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<int>> maze;std::list<Point *> openList;  //开启列表std::list<Point *> closeList; //关闭列表
public:bool maze_flag = true; //加载地图的标志位，省得多次加载浪费资源
};

astar.cpp

#include <math.h>
#include "astar.h"
#include <ros/ros.h>
void Astar::InitAstar(std::vector<std::vector<int>> &_maze){maze = _maze;
}Point *Astar::getLeastFpoint() //返回最小F的点
{if (!openList.empty()){auto resPoint = openList.front();  //返回第一个元素for (auto &point : openList){if (point->F<resPoint->F)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 (tempG<target->G){target->parent = curPoint;target->G = tempG;target->F = calcF(target);}}{   //如果到这一步，就说明已经结束了Point *resPoint = isInList(openList, &endPoint);if (resPoint)return resPoint; //返回列表里的节点指针，不要用原来传入的endpoint指针，因为发生了深拷贝}}}return NULL;
}std::list<Point *> 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<Point *> 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<Point *> &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->x<0 || target->x>maze.size()-1    //x不在地图边界内|| target->y<0 || target->y>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<Point *> Astar::getSurroundPoints(const Point *point, bool isIgnoreCorner) const
{std::vector<Point *> surroundPoints;for (int x = point->x - 1; x <= point->x + 1; x++){for (int y = point->y - 1; y <= point->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 <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/Path.h>
#include <astar.h>
using namespace std;
int main(int argc, char *argv[])
{setlocale(LC_ALL,"");ros::init(argc, argv, "astar_planning_node");MapClass mapclass;//创建地图类Astar astar;bool IsGetPath =false;list<Point *> path_copy; ros::NodeHandle n;ros::Subscriber map_sub = n.subscribe("/map", 10, &MapClass::MapCallback, &mapclass);ros::Subscriber init_pose_sub = n.subscribe("/initialpose",10, &InitPoseCallback);ros::Subscriber goal_pose_sub = n.subscribe("/move_base_simple/goal",10, &GoalPoseCallback);ros::Publisher path_pub = n.advertise<nav_msgs::Path>("path_Astar",10); nav_msgs::Path pathforpub;pathforpub.header.frame_id = "map";pathforpub.header.stamp = ros::Time::now();ros::Rate r(1);while (ros::ok()){if(mapclass.height>0 && mapclass.width> 0 && astar.maze_flag){astar.maze_flag = false;astar.InitAstar(mapclass.map_data);}if(init_pose_point[0]&&goal_pose_point[0]){init_pose_point[0] = 0;init_pose_point[1] = init_pose_point[1]/mapclass.resolution;init_pose_point[2] = init_pose_point[2]/mapclass.resolution;goal_pose_point[0] = 0;goal_pose_point[1] = goal_pose_point[1]/mapclass.resolution;goal_pose_point[2] = goal_pose_point[2]/mapclass.resolution;// ROS_INFO("%f,%f",init_pose_point[1],init_pose_point[2]);// ROS_INFO("%f,%f",goal_pose_point[1],goal_pose_point[2]);Point start((int)init_pose_point[1],(int)init_pose_point[2]);Point end((int)goal_pose_point[1],(int)goal_pose_point[2]);// ROS_INFO("%d,%d",start.x,start.y);// ROS_INFO("%d,%d",end.x,end.y);//A星算法寻找路径list<Point *> path = astar.GetPath(start, end, false); //这个其实是反过来的pathforpub.poses.clear();for (auto p: path)  {geometry_msgs::PoseStamped this_pose_stamped;this_pose_stamped.header.frame_id="map";this_pose_stamped.header.stamp = ros::Time::now();this_pose_stamped.pose.position.x = p->x*mapclass.resolution;this_pose_stamped.pose.position.y = p->y*mapclass.resolution;this_pose_stamped.pose.orientation.x = 0;this_pose_stamped.pose.orientation.y = 0;this_pose_stamped.pose.orientation.z = 0;this_pose_stamped.pose.orientation.w = 0; //直接让四元数全为0了，省得pathforpub.poses.push_back(this_pose_stamped);path_pub.publish(pathforpub);}//path_pub.publish(pathforpub);}path_pub.publish(pathforpub);ros::spinOnce();r.sleep();}return 0;
}

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

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

std_msgs/Header headeruint32 seqtime stampstring frame_id
geometry_msgs/PoseStamped[] poses  
//这里是关键，这里其实是PoseStamped消息的一维数组，所以需要定义路径上每个点的PoseStamped消息，然后用push_back放在末尾std_msgs/Header headeruint32 seqtime stampstring frame_idgeometry_msgs/Pose posegeometry_msgs/Point positionfloat64 xfloat64 yfloat64 zgeometry_msgs/Quaternion orientationfloat64 xfloat64 yfloat64 zfloat64 w

3.6 编写launch文件

<launch><node name="map_server" pkg="map_server" type="map_server" args="$(find astar_planning)/map/map.yaml"/><node pkg="rviz" type="rviz" name="rviz" args="-d $(find astar_planning)/rviz/astar_planning.rviz"/><node pkg="astar_planning" type="astar_planning_node" name="astar_planning_node"/>
</launch>

4、效果图

5.代码地址

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

6.参考

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

http://www.dtcms.com/a/270170.html

相关文章：

  • 3423. 循环数组中相邻元素的最大差值 — day97
  • MySQL 8.0 OCP 1Z0-908 题目解析(23)
  • 服务端渲染
  • VMware克隆虚拟机，模板机已提前设置了固定IP，克隆机需要修改的事项
  • Java项目：基于SSM框架实现的旅游协会管理系统【ssm+B/S架构+源码+数据库+毕业论文】
  • Web前端开发： :where(伪类函数选择器)
  • 软件设计与重构
  • docker部署单机gitlab
  • Pytorch实战——完整的模型训练套路2
  • GitOps实践指南：GitLab CI/CD + ArgoCD 实现 Kubernetes 自动化部署
  • 【AI大模型】深度学习正则化技术：Batch Normalization (BatchNorm) 详解
  • 【GPT深度研究】科研SaaS项目可行性分析报告
  • 【学习笔记】网络设备（华为交换机）基础知识 24 —— 以太网子接口基础知识
  • 【入门级-C++程序设计：3、程序基本语句-多层循环语句】
  • 数据推荐｜标贝科技大规模高质量数据集
  • 25.安卓逆向2-frida hook技术-拦截弹框事件（拦截强制更新弹框和解决jadx加载dex文件不全问题）
  • 从0到1搭建ELK日志收集平台
  • 扣子Coze飞书多维表插件参数获取流程
  • 如何利用机器学习（ML）检测异常登录行为
  • 深深浅浅地理解 Python 中的 `Barrier` 对象
  • emscripten编译cocos2dx项目输入框支持中文
  • MySQL 全库表记录统计与空间估算教程
  • 猿人学js逆向比赛第一届第十五题
  • SpringAI学习笔记-MCP服务器简单示例
  • 软考（软件设计师）数据库原理-SQL
  • HTML+JS+CSS制作一个数独游戏
  • CSS揭秘：9.自适应的椭圆
  • 记一次mount point is busy问题排查
  • 数据结构 —— 栈(stack)在算法思维中的巧妙运用
  • C++进阶—二叉树进阶