代码来源:https://github.com/KailinTong/Motion-Planning-for-Mobile-Robots/tree/master/hw_2.

---

前言

1968年发明的A*算法就是把启发式方法（heuristic approaches）如BFS，和常规方法如Dijsktra算法结合在一起的算法。
A-Star算法是一种静态路网中求解最短路径最有效的直接搜索方法，也是解决许多搜索问题的有效算法。
具体原理见朋友写的自动驾驶路径规划——A*（Astar）算法

---

一、获取代码

在开头的网站中下载ros文件夹下的工作空间catkin_ws1，然后将其中的四个功能包复制到自己的工作空间。（直接复制工作空间遇到了本人无法解决的错误，所以做此选择）
编译后发现代码有错误，将字母‘g’删掉就好

编译完成

二、过程演示

1.启动roscore

roscore

2.打开rviz

rviz

3.打开rviz文件

在rviz中打开demo.rviz(路径src/grid_path_searcher/launch/rviz_config)。

4.新建终端加载地图

roslaunch grid_path_searcher demo.launch 

5.进行路径搜索

规划前后对比



画红圈处即为规划的路径
在终端中还有提示信息

三、ROS包

├── grid_path_searcher--------------------------------------------------------------路径搜索包名称
│ ├── CMakeLists.txt
│ ├── include
│ │ ├── Astar_searcher.h------------------------------------------------------------A代码头文件
│ │ ├── backward.hpp
│ │ ├── JPS_searcher.h
│ │ ├── JPS_utils.h
│ │ └── node.h-------------------------------------------------------------------------A代码头文件
│ ├── launch
│ │ ├── demo.launch-----------------------------------------------------------------加载地图的launch文件
│ │ └── rviz_config
│ │ ├── demo.rviz---------------------------------------------------------------------rviz的环境文件
│ │ └── jps_demo.rviz
│ ├── package.xml
│ ├── README.md
│ └── src
│ ├── Astar_searcher.cpp-----------------------------------------------------------A*算法代码文件
│ ├── CMakeLists.txt
│ ├── demo_node.cpp---------------------------------------------------------------主函数文件
│ ├── graph_searcher.cpp
│ ├── random_complex_generator.cpp-----------------------------------------地图生成障碍物
│ └── read_only
│ ├── JPS_searcher.cpp
│ └── JPS_utils.cpp

│ └── waypoint_generator
│ ├── CMakeLists.txt
│ ├── package.xml
│ └── src
│ ├── sample_waypoints.h
│ └── waypoint_generator.cpp-----------------------------------------------------发布目标点信息

以上所有带注释的就是ROS下的A*算法代码实现相关文件.

node.h

#ifndef _NODE_H_
#define _NODE_H_#include <iostream>
#include <ros/ros.h>
#include <ros/console.h>
#include <Eigen/Eigen>
#include "backward.hpp"#define inf 1>>20
struct GridNode;
typedef GridNode* GridNodePtr;struct GridNode
{     int id;        // 1--> open set, -1 --> closed setEigen::Vector3d coord; Eigen::Vector3i dir;   // direction of expandingEigen::Vector3i index;double gScore, fScore;GridNodePtr cameFrom;std::multimap<double, GridNodePtr>::iterator nodeMapIt;GridNode(Eigen::Vector3i _index, Eigen::Vector3d _coord){  id = 0;index = _index;coord = _coord;dir   = Eigen::Vector3i::Zero();gScore = inf;fScore = inf;cameFrom = NULL;}GridNode(){};~GridNode(){};
};#endif

Astar_searcher.h

#ifndef _ASTART_SEARCHER_H
#define _ASTART_SEARCHER_H#include <iostream>
#include <ros/ros.h>
#include <ros/console.h>
#include <Eigen/Eigen>
#include "backward.hpp"
#include "node.h"class AstarPathFinder
{	private:protected:uint8_t * data;GridNodePtr *** GridNodeMap;Eigen::Vector3i goalIdx;int GLX_SIZE, GLY_SIZE, GLZ_SIZE;int GLXYZ_SIZE, GLYZ_SIZE;double resolution, inv_resolution;double gl_xl, gl_yl, gl_zl;double gl_xu, gl_yu, gl_zu;GridNodePtr terminatePtr;std::multimap<double, GridNodePtr> openSet;double getHeu(GridNodePtr node1, GridNodePtr node2);void AstarGetSucc(GridNodePtr currentPtr, std::vector<GridNodePtr> & neighborPtrSets, std::vector<double> & edgeCostSets);		bool isOccupied(const int & idx_x, const int & idx_y, const int & idx_z) const;bool isOccupied(const Eigen::Vector3i & index) const;bool isFree(const int & idx_x, const int & idx_y, const int & idx_z) const;bool isFree(const Eigen::Vector3i & index) const;Eigen::Vector3d gridIndex2coord(const Eigen::Vector3i & index);Eigen::Vector3i coord2gridIndex(const Eigen::Vector3d & pt);public:AstarPathFinder(){};~AstarPathFinder(){};void AstarGraphSearch(Eigen::Vector3d start_pt, Eigen::Vector3d end_pt);void resetGrid(GridNodePtr ptr);void resetUsedGrids();void initGridMap(double _resolution, Eigen::Vector3d global_xyz_l, Eigen::Vector3d global_xyz_u, int max_x_id, int max_y_id, int max_z_id);void setObs(const double coord_x, const double coord_y, const double coord_z);Eigen::Vector3d coordRounding(const Eigen::Vector3d & coord);std::vector<Eigen::Vector3d> getPath();std::vector<Eigen::Vector3d> getVisitedNodes();
};#endif

Astar_searcher.cpp

#include "Astar_searcher.h"using namespace std;
using namespace Eigen;bool tie_break = false;void AstarPathFinder::initGridMap(double _resolution, Vector3d global_xyz_l, Vector3d global_xyz_u, int max_x_id, int max_y_id, int max_z_id)
{   gl_xl = global_xyz_l(0);gl_yl = global_xyz_l(1);gl_zl = global_xyz_l(2);gl_xu = global_xyz_u(0);gl_yu = global_xyz_u(1);gl_zu = global_xyz_u(2);GLX_SIZE = max_x_id;GLY_SIZE = max_y_id;GLZ_SIZE = max_z_id;GLYZ_SIZE  = GLY_SIZE * GLZ_SIZE;GLXYZ_SIZE = GLX_SIZE * GLYZ_SIZE;resolution = _resolution;inv_resolution = 1.0 / _resolution;    data = new uint8_t[GLXYZ_SIZE];memset(data, 0, GLXYZ_SIZE * sizeof(uint8_t));GridNodeMap = new GridNodePtr ** [GLX_SIZE];for(int i = 0; i < GLX_SIZE; i++){GridNodeMap[i] = new GridNodePtr * [GLY_SIZE];for(int j = 0; j < GLY_SIZE; j++){GridNodeMap[i][j] = new GridNodePtr [GLZ_SIZE];for( int k = 0; k < GLZ_SIZE;k++){Vector3i tmpIdx(i,j,k);Vector3d pos = gridIndex2coord(tmpIdx);GridNodeMap[i][j][k] = new GridNode(tmpIdx, pos);}}}
}void AstarPathFinder::resetGrid(GridNodePtr ptr)
{ptr->id = 0;ptr->cameFrom = NULL;ptr->gScore = inf;ptr->fScore = inf;
}void AstarPathFinder::resetUsedGrids()
{   for(int i=0; i < GLX_SIZE ; i++)for(int j=0; j < GLY_SIZE ; j++)for(int k=0; k < GLZ_SIZE ; k++)resetGrid(GridNodeMap[i][j][k]);
}void AstarPathFinder::setObs(const double coord_x, const double coord_y, const double coord_z)
{   if( coord_x < gl_xl  || coord_y < gl_yl  || coord_z <  gl_zl || coord_x >= gl_xu || coord_y >= gl_yu || coord_z >= gl_zu )return;int idx_x = static_cast<int>( (coord_x - gl_xl) * inv_resolution);int idx_y = static_cast<int>( (coord_y - gl_yl) * inv_resolution);int idx_z = static_cast<int>( (coord_z - gl_zl) * inv_resolution);      data[idx_x * GLYZ_SIZE + idx_y * GLZ_SIZE + idx_z] = 1;
}vector<Vector3d> AstarPathFinder::getVisitedNodes()
{   vector<Vector3d> visited_nodes;for(int i = 0; i < GLX_SIZE; i++)for(int j = 0; j < GLY_SIZE; j++)for(int k = 0; k < GLZ_SIZE; k++){   if(GridNodeMap[i][j][k]->id != 0) // visualize all nodes in open and close list
//                if(GridNodeMap[i][j][k]->id == -1)  // visualize nodes in close list only TODO: carefulvisited_nodes.push_back(GridNodeMap[i][j][k]->coord);}ROS_WARN("visited_nodes size : %d", visited_nodes.size());return visited_nodes;
}Vector3d AstarPathFinder::gridIndex2coord(const Vector3i & index) 
{Vector3d pt;pt(0) = ((double)index(0) + 0.5) * resolution + gl_xl;pt(1) = ((double)index(1) + 0.5) * resolution + gl_yl;pt(2) = ((double)index(2) + 0.5) * resolution + gl_zl;return pt;
}Vector3i AstarPathFinder::coord2gridIndex(const Vector3d & pt) 
{Vector3i idx;idx <<  min( max( int( (pt(0) - gl_xl) * inv_resolution), 0), GLX_SIZE - 1),min( max( int( (pt(1) - gl_yl) * inv_resolution), 0), GLY_SIZE - 1),min( max( int( (pt(2) - gl_zl) * inv_resolution), 0), GLZ_SIZE - 1);                  return idx;
}Eigen::Vector3d AstarPathFinder::coordRounding(const Eigen::Vector3d & coord)
{return gridIndex2coord(coord2gridIndex(coord));
}inline bool AstarPathFinder::isOccupied(const Eigen::Vector3i & index) const
{return isOccupied(index(0), index(1), index(2));
}inline bool AstarPathFinder::isFree(const Eigen::Vector3i & index) const
{return isFree(index(0), index(1), index(2));
}inline bool AstarPathFinder::isOccupied(const int & idx_x, const int & idx_y, const int & idx_z) const 
{return  (idx_x >= 0 && idx_x < GLX_SIZE && idx_y >= 0 && idx_y < GLY_SIZE && idx_z >= 0 && idx_z < GLZ_SIZE && (data[idx_x * GLYZ_SIZE + idx_y * GLZ_SIZE + idx_z] == 1));
}inline bool AstarPathFinder::isFree(const int & idx_x, const int & idx_y, const int & idx_z) const 
{return (idx_x >= 0 && idx_x < GLX_SIZE && idx_y >= 0 && idx_y < GLY_SIZE && idx_z >= 0 && idx_z < GLZ_SIZE && (data[idx_x * GLYZ_SIZE + idx_y * GLZ_SIZE + idx_z] < 1));
}inline void AstarPathFinder::AstarGetSucc(GridNodePtr currentPtr, vector<GridNodePtr> & neighborPtrSets, vector<double> & edgeCostSets)
{   neighborPtrSets.clear(); // Note: the pointers in this set copy pointers to GridNodeMapedgeCostSets.clear();/**STEP 4: finish AstarPathFinder::AstarGetSucc yourself please write your code below***/// idea index -> coordinate -> edgecostif(currentPtr == nullptr)std::cout << "Error: Current pointer is null!" << endl;Eigen::Vector3i thisNode = currentPtr -> index;int this_x = thisNode[0];int this_y = thisNode[1];int this_z = thisNode[2];auto this_coord = currentPtr -> coord;int  n_x, n_y, n_z;double dist;GridNodePtr temp_ptr = nullptr;Eigen::Vector3d n_coord;for(int i = -1;i <= 1;++i ){for(int j = -1;j <= 1;++j ){for(int k = -1;k <= 1;++k){if( i == 0 && j == 0 && k == 0)continue; // to avoid this noden_x = this_x + i;n_y = this_y + j;n_z = this_z + k;if( (n_x < 0) || (n_x > (GLX_SIZE - 1)) || (n_y < 0) || (n_y > (GLY_SIZE - 1) ) || (n_z < 0) || (n_z > (GLZ_SIZE - 1)))continue; // to avoid index problemif(isOccupied(n_x, n_y, n_z))continue; // to avoid obstacles// put the pointer into neighborPtrSetstemp_ptr = GridNodeMap[n_x][n_y][n_z];if(temp_ptr->id == -1) continue; // todo to check this; why the node can transversing the obstaclesn_coord = temp_ptr->coord;if(temp_ptr == currentPtr){std::cout << "Error: temp_ptr == currentPtr)" << std::endl;}if( (std::abs(n_coord[0] - this_coord[0]) < 1e-6) and (std::abs(n_coord[1] - this_coord[1]) < 1e-6) and (std::abs(n_coord[2] - this_coord[2]) < 1e-6 )){std::cout << "Error: Not expanding correctly!" << std::endl;std::cout << "n_coord:" << n_coord[0] << " "<<n_coord[1]<<" "<<n_coord[2] << std::endl;std::cout << "this_coord:" << this_coord[0] << " "<<this_coord[1]<<" "<<this_coord[2] << std::endl;std::cout << "current node index:" << this_x << " "<< this_y<<" "<< this_z << std::endl;std::cout << "neighbor node index:" << n_x << " "<< n_y<<" "<< n_z << std::endl;}dist = std::sqrt( (n_coord[0] - this_coord[0]) * (n_coord[0] - this_coord[0])+(n_coord[1] - this_coord[1]) * (n_coord[1] - this_coord[1])+(n_coord[2] - this_coord[2]) * (n_coord[2] - this_coord[2]));neighborPtrSets.push_back(temp_ptr); // calculate the cost in edgeCostSets: inf means that is not unexpandededgeCostSets.push_back(dist); // put the cost inot edgeCostSets}}}}double AstarPathFinder::getHeu(GridNodePtr node1, GridNodePtr node2)
{/* choose possible heuristic function you wantManhattan, Euclidean, Diagonal, or 0 (Dijkstra)Remember tie_breaker learned in lecture, add it here ?***STEP 1: finish the AstarPathFinder::getHeu , which is the heuristic functionplease write your code below***/double h;auto node1_coord = node1->coord;auto node2_coord = node2->coord;// Heuristics 1: Manhattan
//    h = std::abs(node1_coord(0) - node2_coord(0) ) +
//            std::abs(node1_coord(1) - node2_coord(1) ) +
//            std::abs(node1_coord(2) - node2_coord(2) );// Heuristics 2: Euclidean
//    h = std::sqrt(std::pow((node1_coord(0) - node2_coord(0)), 2 ) +
//            std::pow((node1_coord(1) - node2_coord(1)), 2 ) +
//            std::pow((node1_coord(2) - node2_coord(2)), 2 ));// Heuristics 3: Diagnol distancedouble dx = std::abs(node1_coord(0) - node2_coord(0) );double dy = std::abs(node1_coord(1) - node2_coord(1) );double dz = std::abs(node1_coord(2) - node2_coord(2) );double min_xyz = std::min({dx, dy, dz});h = dx + dy + dz + (std::sqrt(3.0) -3) * min_xyz; // idea: diagnol is a short-cut, find out how many short-cuts can be realizedif(tie_break){double p = 1.0 / 25.0;h *= (1.0 + p);//std::cout << "Tie Break!" << std::endl;}
//    std::cout <<"h is: "<< h << std::endl;return h;
}void AstarPathFinder::AstarGraphSearch(Vector3d start_pt, Vector3d end_pt)
{   ros::Time time_1 = ros::Time::now();    //index of start_point and end_pointVector3i start_idx = coord2gridIndex(start_pt);Vector3i end_idx   = coord2gridIndex(end_pt);goalIdx = end_idx;//position of start_point and end_pointstart_pt = gridIndex2coord(start_idx);end_pt   = gridIndex2coord(end_idx);//Initialize the pointers of struct GridNode which represent start node and goal nodeGridNodePtr startPtr = new GridNode(start_idx, start_pt);GridNodePtr endPtr   = new GridNode(end_idx,   end_pt);//openSet is the open_list implemented through multimap in STL libraryopenSet.clear();// currentPtr represents the node with lowest f(n) in the open_listGridNodePtr currentPtr  = NULL;GridNodePtr neighborPtr = NULL;//put start node in open setstartPtr -> gScore = 0;startPtr -> fScore = getHeu(startPtr,endPtr);   //STEP 1: finish the AstarPathFinder::getHeu , which is the heuristic functionstartPtr -> id = 1; startPtr -> coord = start_pt;openSet.insert( make_pair(startPtr -> fScore, startPtr) ); // todo Note: modified, insert the pointer GridNodeMap[i][j][k] to the start node in grid map/**STEP 2 :  some else preparatory works which should be done before while loopplease write your code below***/// three dimension pointer GridNodeMap[i][j][k] is pointed to a struct GridNode(Eigen::Vector3i _index, Eigen::Vector3d _coord);// assign g(xs) = 0, g(n) = inf (already done in initialzation of struct)// mark start point as visited(expanded) (id 0: no operation, id: 1 in OPEN, id -1: in CLOSE )GridNodeMap[start_idx[0]][start_idx[1]][start_idx[2]] -> id = 1;vector<GridNodePtr> neighborPtrSets;vector<double> edgeCostSets;Eigen::Vector3i current_idx; // record the current index// this is the main loopwhile ( !openSet.empty() ){/***step 3: Remove the node with lowest cost function from open set to closed setplease write your code belowIMPORTANT NOTE!!!This part you should use the C++ STL: multimap, more details can be find in Homework description***/currentPtr = openSet.begin() -> second; // first T1, second T2openSet.erase(openSet.begin()); // remove the node with minimal f valuecurrent_idx = currentPtr->index;GridNodeMap[current_idx[0]][current_idx[1]][current_idx[2]] -> id = -1;// update the id in grid node map// if the current node is the goal if( currentPtr->index == goalIdx ){ros::Time time_2 = ros::Time::now();terminatePtr = currentPtr;ROS_WARN("[A*]{sucess}  Time in A*  is %f ms, path cost if %f m", (time_2 - time_1).toSec() * 1000.0, currentPtr->gScore * resolution );            return;}//get the succetionAstarGetSucc(currentPtr, neighborPtrSets, edgeCostSets);  //STEP 4: finish AstarPathFinder::AstarGetSucc yourself     /***STEP 5:  For all unexpanded neigbors "m" of node "n", please finish this for loopplease write your code below*        */         for(int i = 0; i < (int)neighborPtrSets.size(); i++){/***Judge if the neigbors have been expandedplease write your code belowIMPORTANT NOTE!!!neighborPtrSets[i]->id = -1 : expanded, equal to this node is in close setneighborPtrSets[i]->id = 1 : unexpanded, equal to this node is in open set*        */neighborPtr = neighborPtrSets[i];if(neighborPtr -> id == 0){ //discover a new node, which is not in the closed set and open set/***STEP 6:  As for a new node, do what you need do ,and then put neighbor in open set and record itplease write your code below*        */// shall update: gScore = inf; fScore = inf; cameFrom = NULL, id, mayby directionneighborPtr->gScore = currentPtr->gScore + edgeCostSets[i];neighborPtr->fScore = neighborPtr->gScore + getHeu(neighborPtr,endPtr);neighborPtr->cameFrom = currentPtr; // todo shallow copy or deep copy// push node "m" into OPENopenSet.insert(make_pair(neighborPtr -> fScore, neighborPtr));neighborPtr -> id = 1;continue;}else if(neighborPtr -> id == 1){ //this node is in open set and need to judge if it needs to update, the "0" should be deleted when you are coding/***STEP 7:  As for a node in open set, update it , maintain the openset ,and then put neighbor in open set and record itplease write your code below*        */// shall update: gScore; fScore; cameFrom, mayby directionif(neighborPtr -> gScore > (currentPtr -> gScore + edgeCostSets[i])){neighborPtr -> gScore = currentPtr -> gScore + edgeCostSets[i];neighborPtr -> fScore = neighborPtr -> gScore + getHeu(neighborPtr,endPtr);neighborPtr -> cameFrom = currentPtr;}continue;}else{//this node is in closed set/**please write your code below*        */// todo nothing to do here?continue;}}      }//if search failsros::Time time_2 = ros::Time::now();if((time_2 - time_1).toSec() > 0.1)ROS_WARN("Time consume in Astar path finding is %f", (time_2 - time_1).toSec() );
}vector<Vector3d> AstarPathFinder::getPath() 
{   vector<Vector3d> path;vector<GridNodePtr> gridPath;/***STEP 8:  trace back from the curretnt nodePtr to get all nodes along the pathplease write your code below*      */auto ptr = terminatePtr;while(ptr -> cameFrom != NULL){gridPath.push_back(ptr);ptr = ptr->cameFrom;}for (auto ptr: gridPath)path.push_back(ptr->coord);reverse(path.begin(),path.end());return path;
}// if the difference of f is trivial, then choose then prefer the path along the straight line from start to goal
// discared!!!
GridNodePtr & TieBreaker(const std::multimap<double, GridNodePtr> &  openSet, const GridNodePtr & endPtr) {// todo do I have to update the f in openSet??std::multimap<double, GridNodePtr> local_set;auto f_min = openSet.begin()->first;auto f_max = f_min + 1e-2;auto itlow = openSet.lower_bound (f_min);auto itup = openSet.upper_bound(f_max);double cross, f_new;for (auto it=itlow; it!=itup; ++it){std::cout << "f value is:" << (*it).first << " pointer is: " << (*it).second << '\n';cross = std::abs(endPtr->coord(0) - (*it).second->coord(0)) +std::abs(endPtr->coord(1) - (*it).second->coord(1)) +std::abs(endPtr->coord(2) - (*it).second->coord(2));f_new = (*it).second->fScore + 0.001 * cross;local_set.insert( make_pair(f_new, (*it).second) ); // todo what is iterator, is this way correct?}return local_set.begin()->second;
}

demo_node.cpp

#include <iostream>
#include <fstream>
#include <math.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <ros/ros.h>
#include <ros/console.h>
#include <sensor_msgs/PointCloud2.h>#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <visualization_msgs/MarkerArray.h>
#include <visualization_msgs/Marker.h>#include "Astar_searcher.h"
#include "JPS_searcher.h"
#include "backward.hpp"using namespace std;
using namespace Eigen;namespace backward {
backward::SignalHandling sh;
}// simulation param from launch file
double _resolution, _inv_resolution, _cloud_margin;
double _x_size, _y_size, _z_size;    // useful global variables
bool _has_map   = false;Vector3d _start_pt;
Vector3d _map_lower, _map_upper;
int _max_x_id, _max_y_id, _max_z_id;// ros related
ros::Subscriber _map_sub, _pts_sub;
ros::Publisher  _grid_path_vis_pub, _visited_nodes_vis_pub, _grid_map_vis_pub;AstarPathFinder * _astar_path_finder     = new AstarPathFinder();
JPSPathFinder   * _jps_path_finder       = new JPSPathFinder();void rcvWaypointsCallback(const nav_msgs::Path & wp);
void rcvPointCloudCallBack(const sensor_msgs::PointCloud2 & pointcloud_map);void visGridPath( vector<Vector3d> nodes, bool is_use_jps );
void visVisitedNode( vector<Vector3d> nodes );
void pathFinding(const Vector3d start_pt, const Vector3d target_pt);void rcvWaypointsCallback(const nav_msgs::Path & wp)
{     if( wp.poses[0].pose.position.z < 0.0 || _has_map == false )return;Vector3d target_pt;target_pt << wp.poses[0].pose.position.x,wp.poses[0].pose.position.y,wp.poses[0].pose.position.z;ROS_INFO("[node] receive the planning target");pathFinding(_start_pt, target_pt); 
}void rcvPointCloudCallBack(const sensor_msgs::PointCloud2 & pointcloud_map)
{   if(_has_map ) return;pcl::PointCloud<pcl::PointXYZ> cloud;pcl::PointCloud<pcl::PointXYZ> cloud_vis;sensor_msgs::PointCloud2 map_vis;pcl::fromROSMsg(pointcloud_map, cloud);if( (int)cloud.points.size() == 0 ) return;pcl::PointXYZ pt;for (int idx = 0; idx < (int)cloud.points.size(); idx++){    pt = cloud.points[idx];        // set obstalces into grid map for path planning_astar_path_finder->setObs(pt.x, pt.y, pt.z);_jps_path_finder->setObs(pt.x, pt.y, pt.z);// for visualize onlyVector3d cor_round = _astar_path_finder->coordRounding(Vector3d(pt.x, pt.y, pt.z));pt.x = cor_round(0);pt.y = cor_round(1);pt.z = cor_round(2);cloud_vis.points.push_back(pt);}cloud_vis.width    = cloud_vis.points.size();cloud_vis.height   = 1;cloud_vis.is_dense = true;pcl::toROSMsg(cloud_vis, map_vis);map_vis.header.frame_id = "/world";_grid_map_vis_pub.publish(map_vis);_has_map = true;
}void pathFinding(const Vector3d start_pt, const Vector3d target_pt)
{//Call A* to search for a path_astar_path_finder->AstarGraphSearch(start_pt, target_pt);//Retrieve the pathauto grid_path     = _astar_path_finder->getPath();auto visited_nodes = _astar_path_finder->getVisitedNodes();//Visualize the resultvisGridPath (grid_path, false);visVisitedNode(visited_nodes);//Reset map for next call_astar_path_finder->resetUsedGrids();//_use_jps = 0 -> Do not use JPS//_use_jps = 1 -> Use JPS//you just need to change the #define value of _use_jps
#define _use_jps 0
#if _use_jps{//Call JPS to search for a path_jps_path_finder -> JPSGraphSearch(start_pt, target_pt);//Retrieve the pathauto grid_path     = _jps_path_finder->getPath();auto visited_nodes = _jps_path_finder->getVisitedNodes();//Visualize the resultvisGridPath   (grid_path, _use_jps);visVisitedNode(visited_nodes);//Reset map for next call_jps_path_finder->resetUsedGrids();}
#endif
}int main(int argc, char** argv)
{ros::init(argc, argv, "demo_node");ros::NodeHandle nh("~");_map_sub  = nh.subscribe( "map",       1, rcvPointCloudCallBack );_pts_sub  = nh.subscribe( "waypoints", 1, rcvWaypointsCallback );_grid_map_vis_pub             = nh.advertise<sensor_msgs::PointCloud2>("grid_map_vis", 1);_grid_path_vis_pub            = nh.advertise<visualization_msgs::Marker>("grid_path_vis", 1);_visited_nodes_vis_pub        = nh.advertise<visualization_msgs::Marker>("visited_nodes_vis",1);nh.param("map/cloud_margin",  _cloud_margin, 0.0);nh.param("map/resolution",    _resolution,   0.2);nh.param("map/x_size",        _x_size, 50.0);nh.param("map/y_size",        _y_size, 50.0);nh.param("map/z_size",        _z_size, 5.0 );nh.param("planning/start_x",  _start_pt(0),  0.0);nh.param("planning/start_y",  _start_pt(1),  0.0);nh.param("planning/start_z",  _start_pt(2),  0.0);_map_lower << - _x_size/2.0, - _y_size/2.0,     0.0;_map_upper << + _x_size/2.0, + _y_size/2.0, _z_size;_inv_resolution = 1.0 / _resolution;_max_x_id = (int)(_x_size * _inv_resolution);_max_y_id = (int)(_y_size * _inv_resolution);_max_z_id = (int)(_z_size * _inv_resolution);_astar_path_finder  = new AstarPathFinder();_astar_path_finder  -> initGridMap(_resolution, _map_lower, _map_upper, _max_x_id, _max_y_id, _max_z_id);//    _jps_path_finder    = new JPSPathFinder();
//    _jps_path_finder    -> initGridMap(_resolution, _map_lower, _map_upper, _max_x_id, _max_y_id, _max_z_id);ros::Rate rate(100);bool status = ros::ok();while(status) {ros::spinOnce();      status = ros::ok();rate.sleep();}delete _astar_path_finder;delete _jps_path_finder;return 0;
}void visGridPath( vector<Vector3d> nodes, bool is_use_jps )
{   visualization_msgs::Marker node_vis; node_vis.header.frame_id = "world";node_vis.header.stamp = ros::Time::now();if(is_use_jps)node_vis.ns = "demo_node/jps_path";elsenode_vis.ns = "demo_node/astar_path";node_vis.type = visualization_msgs::Marker::CUBE_LIST;node_vis.action = visualization_msgs::Marker::ADD;node_vis.id = 0;node_vis.pose.orientation.x = 0.0;node_vis.pose.orientation.y = 0.0;node_vis.pose.orientation.z = 0.0;node_vis.pose.orientation.w = 1.0;if(is_use_jps){node_vis.color.a = 1.0;node_vis.color.r = 1.0;node_vis.color.g = 0.0;node_vis.color.b = 0.0;}else{node_vis.color.a = 1.0;node_vis.color.r = 0.0;node_vis.color.g = 1.0;node_vis.color.b = 0.0;}node_vis.scale.x = _resolution;node_vis.scale.y = _resolution;node_vis.scale.z = _resolution;geometry_msgs::Point pt;for(int i = 0; i < int(nodes.size()); i++){Vector3d coord = nodes[i];pt.x = coord(0);pt.y = coord(1);pt.z = coord(2);node_vis.points.push_back(pt);}_grid_path_vis_pub.publish(node_vis);
}void visVisitedNode( vector<Vector3d> nodes )
{   visualization_msgs::Marker node_vis; node_vis.header.frame_id = "world";node_vis.header.stamp = ros::Time::now();node_vis.ns = "demo_node/expanded_nodes";node_vis.type = visualization_msgs::Marker::CUBE_LIST;node_vis.action = visualization_msgs::Marker::ADD;node_vis.id = 0;node_vis.pose.orientation.x = 0.0;node_vis.pose.orientation.y = 0.0;node_vis.pose.orientation.z = 0.0;node_vis.pose.orientation.w = 1.0;node_vis.color.a = 0.5;node_vis.color.r = 0.0;node_vis.color.g = 0.0;node_vis.color.b = 1.0;node_vis.scale.x = _resolution;node_vis.scale.y = _resolution;node_vis.scale.z = _resolution;geometry_msgs::Point pt;for(int i = 0; i < int(nodes.size()); i++){Vector3d coord = nodes[i];pt.x = coord(0);pt.y = coord(1);pt.z = coord(2);node_vis.points.push_back(pt);}_visited_nodes_vis_pub.publish(node_vis);
}

waypoint_generator.cpp

#include <iostream>
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/Vector3.h>
#include <nav_msgs/Path.h>
#include "sample_waypoints.h"
#include <vector>
#include <deque>
#include <boost/format.hpp>
#include <eigen3/Eigen/Dense>using namespace std;
using bfmt = boost::format;ros::Publisher pub1;
ros::Publisher pub2;
ros::Publisher pub3;
string waypoint_type = string("manual");
bool is_odom_ready;
nav_msgs::Odometry odom;
nav_msgs::Path waypoints;// series waypoint needed
std::deque<nav_msgs::Path> waypointSegments;
ros::Time trigged_time;void load_seg(ros::NodeHandle& nh, int segid, const ros::Time& time_base) {std::string seg_str = boost::str(bfmt("seg%d/") % segid);double yaw;double time_from_start;ROS_INFO("Getting segment %d", segid);ROS_ASSERT(nh.getParam(seg_str + "yaw", yaw));ROS_ASSERT_MSG((yaw > -3.1499999) && (yaw < 3.14999999), "yaw=%.3f", yaw);ROS_ASSERT(nh.getParam(seg_str + "time_from_start", time_from_start));ROS_ASSERT(time_from_start >= 0.0);std::vector<double> ptx;std::vector<double> pty;std::vector<double> ptz;ROS_ASSERT(nh.getParam(seg_str + "x", ptx));ROS_ASSERT(nh.getParam(seg_str + "y", pty));ROS_ASSERT(nh.getParam(seg_str + "z", ptz));ROS_ASSERT(ptx.size());ROS_ASSERT(ptx.size() == pty.size() && ptx.size() == ptz.size());nav_msgs::Path path_msg;path_msg.header.stamp = time_base + ros::Duration(time_from_start);double baseyaw = tf::getYaw(odom.pose.pose.orientation);for (size_t k = 0; k < ptx.size(); ++k) {geometry_msgs::PoseStamped pt;pt.pose.orientation = tf::createQuaternionMsgFromYaw(baseyaw + yaw);Eigen::Vector2d dp(ptx.at(k), pty.at(k));Eigen::Vector2d rdp;rdp.x() = std::cos(-baseyaw-yaw)*dp.x() + std::sin(-baseyaw-yaw)*dp.y();rdp.y() =-std::sin(-baseyaw-yaw)*dp.x() + std::cos(-baseyaw-yaw)*dp.y();pt.pose.position.x = rdp.x() + odom.pose.pose.position.x;pt.pose.position.y = rdp.y() + odom.pose.pose.position.y;pt.pose.position.z = ptz.at(k) + odom.pose.pose.position.z;path_msg.poses.push_back(pt);}waypointSegments.push_back(path_msg);
}void load_waypoints(ros::NodeHandle& nh, const ros::Time& time_base) {int seg_cnt = 0;waypointSegments.clear();ROS_ASSERT(nh.getParam("segment_cnt", seg_cnt));for (int i = 0; i < seg_cnt; ++i) {load_seg(nh, i, time_base);if (i > 0) {ROS_ASSERT(waypointSegments[i - 1].header.stamp < waypointSegments[i].header.stamp);}}ROS_INFO("Overall load %zu segments", waypointSegments.size());
}void publish_waypoints() {waypoints.header.frame_id = std::string("world");waypoints.header.stamp = ros::Time::now();pub1.publish(waypoints);geometry_msgs::PoseStamped init_pose;init_pose.header = odom.header;init_pose.pose = odom.pose.pose;waypoints.poses.insert(waypoints.poses.begin(), init_pose);// pub2.publish(waypoints);waypoints.poses.clear();
}void publish_waypoints_vis() {nav_msgs::Path wp_vis = waypoints;geometry_msgs::PoseArray poseArray;poseArray.header.frame_id = std::string("world");poseArray.header.stamp = ros::Time::now();{geometry_msgs::Pose init_pose;init_pose = odom.pose.pose;poseArray.poses.push_back(init_pose);}for (auto it = waypoints.poses.begin(); it != waypoints.poses.end(); ++it) {geometry_msgs::Pose p;p = it->pose;poseArray.poses.push_back(p);}pub2.publish(poseArray);
}void odom_callback(const nav_msgs::Odometry::ConstPtr& msg) {is_odom_ready = true;odom = *msg;if (waypointSegments.size()) {ros::Time expected_time = waypointSegments.front().header.stamp;if (odom.header.stamp >= expected_time) {waypoints = waypointSegments.front();std::stringstream ss;ss << bfmt("Series send %.3f from start:\n") % trigged_time.toSec();for (auto& pose_stamped : waypoints.poses) {ss << bfmt("P[%.2f, %.2f, %.2f] q(%.2f,%.2f,%.2f,%.2f)") %pose_stamped.pose.position.x % pose_stamped.pose.position.y %pose_stamped.pose.position.z % pose_stamped.pose.orientation.w %pose_stamped.pose.orientation.x % pose_stamped.pose.orientation.y %pose_stamped.pose.orientation.z << std::endl;}ROS_INFO_STREAM(ss.str());publish_waypoints_vis();publish_waypoints();waypointSegments.pop_front();}}
}void goal_callback(const geometry_msgs::PoseStamped::ConstPtr& msg) {
/*    if (!is_odom_ready) {ROS_ERROR("[waypoint_generator] No odom!");return;}*/trigged_time = ros::Time::now(); //odom.header.stamp;//ROS_ASSERT(trigged_time > ros::Time(0));ros::NodeHandle n("~");n.param("waypoint_type", waypoint_type, string("manual"));if (waypoint_type == string("circle")) {waypoints = circle();publish_waypoints_vis();publish_waypoints();} else if (waypoint_type == string("eight")) {waypoints = eight();publish_waypoints_vis();publish_waypoints();} else if (waypoint_type == string("point")) {waypoints = point();publish_waypoints_vis();publish_waypoints();} else if (waypoint_type == string("series")) {load_waypoints(n, trigged_time);} else if (waypoint_type == string("manual-lonely-waypoint")) {if (msg->pose.position.z >= 0) {// if height >= 0, it's a valid goal;geometry_msgs::PoseStamped pt = *msg;waypoints.poses.clear();waypoints.poses.push_back(pt);publish_waypoints_vis();publish_waypoints();} else {ROS_WARN("[waypoint_generator] invalid goal in manual-lonely-waypoint mode.");}} else {if (msg->pose.position.z > 0) {// if height > 0, it's a normal goal;geometry_msgs::PoseStamped pt = *msg;if (waypoint_type == string("noyaw")) {double yaw = tf::getYaw(odom.pose.pose.orientation);pt.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);}waypoints.poses.push_back(pt);publish_waypoints_vis();} else if (msg->pose.position.z > -1.0) {// if 0 > height > -1.0, remove last goal;if (waypoints.poses.size() >= 1) {waypoints.poses.erase(std::prev(waypoints.poses.end()));}publish_waypoints_vis();} else {// if -1.0 > height, end of inputif (waypoints.poses.size() >= 1) {publish_waypoints_vis();publish_waypoints();}}}
}void traj_start_trigger_callback(const geometry_msgs::PoseStamped& msg) {if (!is_odom_ready) {ROS_ERROR("[waypoint_generator] No odom!");return;}ROS_WARN("[waypoint_generator] Trigger!");trigged_time = odom.header.stamp;ROS_ASSERT(trigged_time > ros::Time(0));ros::NodeHandle n("~");n.param("waypoint_type", waypoint_type, string("manual"));ROS_ERROR_STREAM("Pattern " << waypoint_type << " generated!");if (waypoint_type == string("free")) {waypoints = point();publish_waypoints_vis();publish_waypoints();} else if (waypoint_type == string("circle")) {waypoints = circle();publish_waypoints_vis();publish_waypoints();} else if (waypoint_type == string("eight")) {waypoints = eight();publish_waypoints_vis();publish_waypoints();} else if (waypoint_type == string("point")) {waypoints = point();publish_waypoints_vis();publish_waypoints();} else if (waypoint_type == string("series")) {load_waypoints(n, trigged_time);}
}int main(int argc, char** argv) {ros::init(argc, argv, "waypoint_generator");ros::NodeHandle n("~");n.param("waypoint_type", waypoint_type, string("manual"));ros::Subscriber sub1 = n.subscribe("odom", 10, odom_callback);ros::Subscriber sub2 = n.subscribe("goal", 10, goal_callback);ros::Subscriber sub3 = n.subscribe("traj_start_trigger", 10, traj_start_trigger_callback);pub1 = n.advertise<nav_msgs::Path>("waypoints", 50);pub2 = n.advertise<geometry_msgs::PoseArray>("waypoints_vis", 10);trigged_time = ros::Time(0);ros::spin();return 0;
}

---

声明

本人所有文章均为个人学习记录，如有侵权，联系立删。
相关代码可以在个人主页