代码来源:https://github.com/KailinTong/Motion-Planning-for-Mobile-Robots/tree/master/hw_2.
文章目录
- 前言
- 一、获取代码
- 二、过程演示
- 1.启动roscore
- 2.打开rviz
- 3.打开rviz文件
- 4.新建终端加载地图
- 5.进行路径搜索
- 三、ROS包
- node.h
- Astar_searcher.h
- Astar_searcher.cpp
- demo_node.cpp
- waypoint_generator.cpp
- 声明
前言
1968年发明的A*算法就是把启发式方法（heuristic approaches）如BFS，和常规方法如Dijsktra算法结合在一起的算法。
A-Star算法是一种静态路网中求解最短路径最有效的直接搜索方法，也是解决许多搜索问题的有效算法。
具体原理见朋友写的自动驾驶路径规划——A*（Astar）算法
一、获取代码
在开头的网站中下载ros文件夹下的工作空间catkin_ws1，然后将其中的四个功能包复制到自己的工作空间。（直接复制工作空间遇到了本人无法解决的错误，所以做此选择）
编译后发现代码有错误，将字母‘g’删掉就好
编译完成
二、过程演示
1.启动roscore
复制代码
1
2roscore
2.打开rviz
复制代码
1
2rviz
3.打开rviz文件
在rviz中打开demo.rviz(路径src/grid_path_searcher/launch/rviz_config)。
4.新建终端加载地图
复制代码
1
2roslaunch 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
复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42#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 set Eigen::Vector3d coord; Eigen::Vector3i dir; // direction of expanding Eigen::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
复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56#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
复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456#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: careful visited_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 GridNodeMap edgeCostSets.clear(); /* * STEP 4: finish AstarPathFinder::AstarGetSucc yourself please write your code below * * */ // idea index -> coordinate -> edgecost if(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 node n_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 problem if(isOccupied(n_x, n_y, n_z)) continue; // to avoid obstacles // put the pointer into neighborPtrSets temp_ptr = GridNodeMap[n_x][n_y][n_z]; if(temp_ptr->id == -1) continue; // todo to check this; why the node can transversing the obstacles n_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 unexpanded edgeCostSets.push_back(dist); // put the cost inot edgeCostSets } } } } double AstarPathFinder::getHeu(GridNodePtr node1, GridNodePtr node2) { /* choose possible heuristic function you want Manhattan, Euclidean, Diagonal, or 0 (Dijkstra) Remember tie_breaker learned in lecture, add it here ? * * * STEP 1: finish the AstarPathFinder::getHeu , which is the heuristic function please 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 distance double 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 realized if(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_point Vector3i start_idx = coord2gridIndex(start_pt); Vector3i end_idx = coord2gridIndex(end_pt); goalIdx = end_idx; //position of start_point and end_point start_pt = gridIndex2coord(start_idx); end_pt = gridIndex2coord(end_idx); //Initialize the pointers of struct GridNode which represent start node and goal node GridNodePtr 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 library openSet.clear(); // currentPtr represents the node with lowest f(n) in the open_list GridNodePtr currentPtr = NULL; GridNodePtr neighborPtr = NULL; //put start node in open set startPtr -> gScore = 0; startPtr -> fScore = getHeu(startPtr,endPtr); //STEP 1: finish the AstarPathFinder::getHeu , which is the heuristic function startPtr -> 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 loop please 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 loop while ( !openSet.empty() ){ /* * * step 3: Remove the node with lowest cost function from open set to closed set please write your code below IMPORTANT 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 T2 openSet.erase(openSet.begin()); // remove the node with minimal f value current_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 succetion AstarGetSucc(currentPtr, neighborPtrSets, edgeCostSets); //STEP 4: finish AstarPathFinder::AstarGetSucc yourself /* * * STEP 5: For all unexpanded neigbors "m" of node "n", please finish this for loop please write your code below * */ for(int i = 0; i < (int)neighborPtrSets.size(); i++){ /* * * Judge if the neigbors have been expanded please write your code below IMPORTANT NOTE!!! neighborPtrSets[i]->id = -1 : expanded, equal to this node is in close set neighborPtrSets[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 it please write your code below * */ // shall update: gScore = inf; fScore = inf; cameFrom = NULL, id, mayby direction neighborPtr->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 OPEN openSet.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 it please write your code below * */ // shall update: gScore; fScore; cameFrom, mayby direction if(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 fails ros::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 path please 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
复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287#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 only Vector3d 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 path auto grid_path = _astar_path_finder->getPath(); auto visited_nodes = _astar_path_finder->getVisitedNodes(); //Visualize the result visGridPath (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 path auto grid_path = _jps_path_finder->getPath(); auto visited_nodes = _jps_path_finder->getVisitedNodes(); //Visualize the result visGridPath (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"; else node_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
复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259#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 input if (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; }
声明
本人所有文章均为个人学习记录，如有侵权，联系立删。
相关代码可以在个人主页
最后
以上就是疯狂砖头最近收集整理的关于ROS学习记录（四）基于ROS的A*算法仿真前言一、获取代码二、过程演示三、ROS包声明的全部内容，更多相关ROS学习记录（四）基于ROS内容请搜索靠谱客的其他文章。
本图文内容来源于网友提供，作为学习参考使用，或来自网络收集整理，版权属于原作者所有。
发表评论 取消回复