├── CMakeLists.txt ├── CMakeLists.txt.user ├── cfg ├── localCloudFilter.yaml ├── pointsToMap.rviz ├── pointsToMap_Viz.yaml ├── test.rviz └── traversalCheck_Viz.yaml ├── launch ├── laserToMap.launch ├── pathPlanning.launch ├── pathPlanning_demo.launch ├── pointsToMap.launch ├── realTimeLiDAR.launch ├── realTimeMapping.launch ├── test.launch └── traversalCheck.launch ├── package.xml ├── readMe.md └── src ├── laserToMap.cpp ├── pathPlanning.cpp ├── pathPlanning.hpp ├── pathPlanning_demo.cpp ├── pointsToMap.cpp ├── realTimeLiDAR.cpp ├── realTimeMapping.cpp ├── test_node.cpp └── traversalCheck.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(grid_map_yh) 3 | 4 | set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") 5 | 6 | ## Find catkin macros and libraries 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | grid_map_core 10 | grid_map_ros 11 | grid_map_cv 12 | grid_map_filters 13 | grid_map_loader 14 | grid_map_msgs 15 | grid_map_octomap 16 | grid_map_rviz_plugin 17 | grid_map_visualization 18 | geometry_msgs 19 | sensor_msgs 20 | cv_bridge 21 | octomap_msgs 22 | filters 23 | libpointmatcher_ros 24 | pcl_ros 25 | ) 26 | 27 | find_package(OpenCV REQUIRED 28 | COMPONENTS 29 | opencv_highgui 30 | CONFIG 31 | ) 32 | 33 | find_package(libpointmatcher REQUIRED) 34 | 35 | find_package(octomap REQUIRED) 36 | 37 | ################################### 38 | ## catkin specific configuration ## 39 | ################################### 40 | ## The catkin_package macro generates cmake config files for your package 41 | ## Declare things to be passed to dependent projects 42 | ## INCLUDE_DIRS: uncomment this if your package contains header files 43 | ## LIBRARIES: libraries you create in this project that dependent projects also need 44 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 45 | ## DEPENDS: system dependencies of this project that dependent projects also need 46 | catkin_package( 47 | # INCLUDE_DIRS include 48 | # LIBRARIES ${PROJECT_NAME} 49 | CATKIN_DEPENDS 50 | # DEPENDS system_lib 51 | ) 52 | 53 | ########### 54 | ## Build ## 55 | ########### 56 | 57 | ## Specify additional locations of header files 58 | ## Your package locations should be listed before other locations 59 | include_directories( 60 | include 61 | ${catkin_INCLUDE_DIRS} 62 | ${EIGEN3_INCLUDE_DIR} 63 | ${OCTOMAP_INCLUDE_DIR} 64 | ) 65 | 66 | ## Declare a cpp executable 67 | 68 | add_executable( 69 | test_demo 70 | src/test_node.cpp 71 | ) 72 | 73 | add_executable( 74 | pointsToMap 75 | src/pointsToMap.cpp 76 | ) 77 | 78 | add_executable( 79 | traversalCheck 80 | src/traversalCheck.cpp 81 | ) 82 | 83 | add_executable( 84 | laserToMap 85 | src/laserToMap.cpp 86 | ) 87 | 88 | add_executable( 89 | pathPlanning 90 | src/pathPlanning.cpp 91 | ) 92 | 93 | add_executable( 94 | realTimeMapping 95 | src/realTimeMapping.cpp 96 | ) 97 | 98 | add_executable( 99 | realTimeLiDAR 100 | src/realTimeLiDAR.cpp 101 | ) 102 | 103 | add_executable( 104 | pathPlanning_demo 105 | src/pathPlanning_demo.cpp 106 | ) 107 | 108 | 109 | ## Specify libraries to link a library or executable target against 110 | target_link_libraries( 111 | test_demo 112 | ${catkin_LIBRARIES} 113 | ${libpointmatcher_LIBRARIES} 114 | ) 115 | 116 | target_link_libraries( 117 | pointsToMap 118 | ${catkin_LIBRARIES} 119 | ${libpointmatcher_LIBRARIES} 120 | ) 121 | 122 | target_link_libraries( 123 | traversalCheck 124 | ${catkin_LIBRARIES} 125 | ${libpointmatcher_LIBRARIES} 126 | ) 127 | 128 | target_link_libraries( 129 | laserToMap 130 | ${catkin_LIBRARIES} 131 | ${libpointmatcher_LIBRARIES} 132 | ) 133 | 134 | target_link_libraries( 135 | pathPlanning 136 | ${catkin_LIBRARIES} 137 | ${libpointmatcher_LIBRARIES} 138 | ) 139 | 140 | target_link_libraries( 141 | realTimeMapping 142 | ${catkin_LIBRARIES} 143 | ${libpointmatcher_LIBRARIES} 144 | ) 145 | 146 | target_link_libraries( 147 | pathPlanning_demo 148 | ${catkin_LIBRARIES} 149 | ${libpointmatcher_LIBRARIES} 150 | ) 151 | 152 | target_link_libraries( 153 | realTimeLiDAR 154 | ${catkin_LIBRARIES} 155 | ${libpointmatcher_LIBRARIES} 156 | ) 157 | 158 | 159 | # Mark executables and/or libraries for installation 160 | install( 161 | TARGETS test_demo pointsToMap traversalCheck laserToMap pathPlanning realTimeMapping realTimeLiDAR 162 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 163 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 164 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 165 | ) 166 | 167 | # Mark other files for installation 168 | install( 169 | DIRECTORY cfg launch 170 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 171 | ) 172 | 173 | -------------------------------------------------------------------------------- /CMakeLists.txt.user: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | ProjectExplorer.Project.ActiveTarget 7 | 0 8 | 9 | 10 | ProjectExplorer.Project.EditorSettings 11 | 12 | true 13 | false 14 | true 15 | 16 | Cpp 17 | 18 | CppGlobal 19 | 20 | 21 | 22 | QmlJS 23 | 24 | QmlJSGlobal 25 | 26 | 27 | 2 28 | UTF-8 29 | false 30 | 4 31 | false 32 | true 33 | 1 34 | true 35 | 0 36 | true 37 | 0 38 | 8 39 | true 40 | 1 41 | true 42 | true 43 | true 44 | false 45 | 46 | 47 | 48 | ProjectExplorer.Project.PluginSettings 49 | 50 | 51 | 52 | ProjectExplorer.Project.Target.0 53 | 54 | Desktop 55 | Desktop 56 | {5c6ed0b3-ac21-4b58-8b8b-db3d7fb310fc} 57 | 0 58 | 0 59 | 0 60 | 61 | false 62 | /home/yh/mine_ws/src/grid_map/build-grid_map_yh-Desktop-Default 63 | 64 | 65 | 66 | 67 | false 68 | false 69 | true 70 | Make 71 | 72 | CMakeProjectManager.MakeStep 73 | 74 | 1 75 | Build 76 | 77 | ProjectExplorer.BuildSteps.Build 78 | 79 | 80 | 81 | clean 82 | 83 | true 84 | false 85 | true 86 | Make 87 | 88 | CMakeProjectManager.MakeStep 89 | 90 | 1 91 | Clean 92 | 93 | ProjectExplorer.BuildSteps.Clean 94 | 95 | 2 96 | false 97 | 98 | Default 99 | Default 100 | CMakeProjectManager.CMakeBuildConfiguration 101 | 102 | 1 103 | 104 | 105 | 0 106 | Deploy 107 | 108 | ProjectExplorer.BuildSteps.Deploy 109 | 110 | 1 111 | Deploy locally 112 | 113 | ProjectExplorer.DefaultDeployConfiguration 114 | 115 | 1 116 | 117 | 118 | 119 | false 120 | false 121 | false 122 | false 123 | true 124 | 0.01 125 | 10 126 | true 127 | 1 128 | 25 129 | 130 | 1 131 | true 132 | false 133 | true 134 | valgrind 135 | 136 | 0 137 | 1 138 | 2 139 | 3 140 | 4 141 | 5 142 | 6 143 | 7 144 | 8 145 | 9 146 | 10 147 | 11 148 | 12 149 | 13 150 | 14 151 | 152 | test_demo 153 | 154 | false 155 | 156 | 2 157 | 158 | test_demo 159 | 160 | CMakeProjectManager.CMakeRunConfiguration.test_demo 161 | 3768 162 | true 163 | false 164 | false 165 | false 166 | true 167 | 168 | 169 | 170 | false 171 | false 172 | false 173 | false 174 | true 175 | 0.01 176 | 10 177 | true 178 | 1 179 | 25 180 | 181 | 1 182 | true 183 | false 184 | true 185 | valgrind 186 | 187 | 0 188 | 1 189 | 2 190 | 3 191 | 4 192 | 5 193 | 6 194 | 7 195 | 8 196 | 9 197 | 10 198 | 11 199 | 12 200 | 13 201 | 14 202 | 203 | pointsToMap 204 | 205 | false 206 | 207 | -1 208 | 209 | pointsToMap 210 | 211 | CMakeProjectManager.CMakeRunConfiguration.pointsToMap 212 | 3768 213 | false 214 | true 215 | false 216 | false 217 | true 218 | 219 | 220 | 221 | false 222 | false 223 | false 224 | false 225 | true 226 | 0.01 227 | 10 228 | true 229 | 1 230 | 25 231 | 232 | 1 233 | true 234 | false 235 | true 236 | valgrind 237 | 238 | 0 239 | 1 240 | 2 241 | 3 242 | 4 243 | 5 244 | 6 245 | 7 246 | 8 247 | 9 248 | 10 249 | 11 250 | 12 251 | 13 252 | 14 253 | 254 | traversalCheck 255 | 256 | false 257 | 258 | -1 259 | 260 | traversalCheck 261 | 262 | CMakeProjectManager.CMakeRunConfiguration.traversalCheck 263 | 3768 264 | false 265 | true 266 | false 267 | false 268 | true 269 | 270 | 271 | 272 | false 273 | false 274 | false 275 | false 276 | true 277 | 0.01 278 | 10 279 | true 280 | 1 281 | 25 282 | 283 | 1 284 | true 285 | false 286 | true 287 | valgrind 288 | 289 | 0 290 | 1 291 | 2 292 | 3 293 | 4 294 | 5 295 | 6 296 | 7 297 | 8 298 | 9 299 | 10 300 | 11 301 | 12 302 | 13 303 | 14 304 | 305 | laserToMap 306 | 307 | false 308 | 309 | -1 310 | 311 | laserToMap 312 | 313 | CMakeProjectManager.CMakeRunConfiguration.laserToMap 314 | 3768 315 | false 316 | true 317 | false 318 | false 319 | true 320 | 321 | 322 | 323 | false 324 | false 325 | false 326 | false 327 | true 328 | 0.01 329 | 10 330 | true 331 | 1 332 | 25 333 | 334 | 1 335 | true 336 | false 337 | true 338 | valgrind 339 | 340 | 0 341 | 1 342 | 2 343 | 3 344 | 4 345 | 5 346 | 6 347 | 7 348 | 8 349 | 9 350 | 10 351 | 11 352 | 12 353 | 13 354 | 14 355 | 356 | pathPlanning 357 | 358 | false 359 | 360 | -1 361 | 362 | pathPlanning 363 | 364 | CMakeProjectManager.CMakeRunConfiguration.pathPlanning 365 | 3768 366 | false 367 | true 368 | false 369 | false 370 | true 371 | 372 | 373 | 374 | false 375 | false 376 | false 377 | false 378 | true 379 | 0.01 380 | 10 381 | true 382 | 1 383 | 25 384 | 385 | 1 386 | true 387 | false 388 | true 389 | valgrind 390 | 391 | 0 392 | 1 393 | 2 394 | 3 395 | 4 396 | 5 397 | 6 398 | 7 399 | 8 400 | 9 401 | 10 402 | 11 403 | 12 404 | 13 405 | 14 406 | 407 | realTimeMapping 408 | 409 | false 410 | 411 | -1 412 | 413 | realTimeMapping 414 | 415 | CMakeProjectManager.CMakeRunConfiguration.realTimeMapping 416 | 3768 417 | false 418 | true 419 | false 420 | false 421 | true 422 | 423 | 424 | 425 | false 426 | false 427 | false 428 | false 429 | true 430 | 0.01 431 | 10 432 | true 433 | 1 434 | 25 435 | 436 | 1 437 | true 438 | false 439 | true 440 | valgrind 441 | 442 | 0 443 | 1 444 | 2 445 | 3 446 | 4 447 | 5 448 | 6 449 | 7 450 | 8 451 | 9 452 | 10 453 | 11 454 | 12 455 | 13 456 | 14 457 | 458 | pathPlanning_demo 459 | 460 | false 461 | 462 | -1 463 | 464 | pathPlanning_demo 465 | 466 | CMakeProjectManager.CMakeRunConfiguration.pathPlanning_demo 467 | 3768 468 | false 469 | true 470 | false 471 | false 472 | true 473 | 474 | 475 | 476 | false 477 | false 478 | false 479 | false 480 | true 481 | 0.01 482 | 10 483 | true 484 | 1 485 | 25 486 | 487 | 1 488 | true 489 | false 490 | true 491 | valgrind 492 | 493 | 0 494 | 1 495 | 2 496 | 3 497 | 4 498 | 5 499 | 6 500 | 7 501 | 8 502 | 9 503 | 10 504 | 11 505 | 12 506 | 13 507 | 14 508 | 509 | realTimeLiDAR 510 | 511 | false 512 | 513 | -1 514 | 515 | realTimeLiDAR 516 | 517 | CMakeProjectManager.CMakeRunConfiguration.realTimeLiDAR 518 | 3768 519 | false 520 | true 521 | false 522 | false 523 | true 524 | 525 | 8 526 | 527 | 528 | 529 | ProjectExplorer.Project.TargetCount 530 | 1 531 | 532 | 533 | ProjectExplorer.Project.Updater.EnvironmentId 534 | {284ff1d8-d590-492f-9355-a643515d7c55} 535 | 536 | 537 | ProjectExplorer.Project.Updater.FileVersion 538 | 15 539 | 540 | 541 | -------------------------------------------------------------------------------- /cfg/localCloudFilter.yaml: -------------------------------------------------------------------------------- 1 | - MaxDistDataPointsFilter: 2 | dim: -1 3 | maxDist: 8 4 | - VoxelGridDataPointsFilter: 5 | vSizeX : 0.1 6 | vSizeY : 0.1 7 | vSizeZ : 0.1 8 | useCentroid : 1 9 | #- ObservationDirectionDataPointsFilter: 10 | # x: 0 11 | # y: 0 12 | # z: 0 13 | #- SurfaceNormalDataPointsFilter: 14 | # knn: 5 15 | # epsilon: 0.33 16 | # keepNormals: 1 17 | # keepDensities: 0 18 | #- OrientNormalsDataPointsFilter: 19 | # towardCenter: 0 20 | -------------------------------------------------------------------------------- /cfg/pointsToMap.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Axes1 9 | - /PointCloud21 10 | - /PointCloud22 11 | - /Marker1 12 | - /GridMap1 13 | - /GridMap2 14 | - /Map1 15 | - /Map1/Position1 16 | - /Map1/Orientation1 17 | - /PointCloud23 18 | - /Path1 19 | - /Path1/Offset1 20 | Splitter Ratio: 0.611465 21 | Tree Height: 825 22 | - Class: rviz/Selection 23 | Name: Selection 24 | - Class: rviz/Tool Properties 25 | Expanded: 26 | - /2D Pose Estimate1 27 | - /2D Nav Goal1 28 | - /Publish Point1 29 | Name: Tool Properties 30 | Splitter Ratio: 0.588679 31 | - Class: rviz/Views 32 | Expanded: 33 | - /Current View1 34 | Name: Views 35 | Splitter Ratio: 0.5 36 | - Class: rviz/Time 37 | Experimental: false 38 | Name: Time 39 | SyncMode: 0 40 | SyncSource: PointCloud2 41 | Visualization Manager: 42 | Class: "" 43 | Displays: 44 | - Class: rviz/Axes 45 | Enabled: true 46 | Length: 2 47 | Name: Axes 48 | Radius: 0.4 49 | Reference Frame: robot 50 | Value: true 51 | - Alpha: 1 52 | Autocompute Intensity Bounds: true 53 | Autocompute Value Bounds: 54 | Max Value: 10 55 | Min Value: -10 56 | Value: true 57 | Axis: Z 58 | Channel Name: intensity 59 | Class: rviz/PointCloud2 60 | Color: 0; 255; 0 61 | Color Transformer: FlatColor 62 | Decay Time: 0 63 | Enabled: true 64 | Invert Rainbow: false 65 | Max Color: 255; 255; 255 66 | Max Intensity: 4096 67 | Min Color: 0; 0; 0 68 | Min Intensity: 0 69 | Name: PointCloud2 70 | Position Transformer: XYZ 71 | Queue Size: 10 72 | Selectable: true 73 | Size (Pixels): 3 74 | Size (m): 0.2 75 | Style: Flat Squares 76 | Topic: /velodyne_cloud 77 | Unreliable: false 78 | Use Fixed Frame: true 79 | Use rainbow: true 80 | Value: true 81 | - Alpha: 1 82 | Autocompute Intensity Bounds: true 83 | Autocompute Value Bounds: 84 | Max Value: 0.766162 85 | Min Value: -2.55051 86 | Value: true 87 | Axis: Z 88 | Channel Name: z 89 | Class: rviz/PointCloud2 90 | Color: 255; 255; 255 91 | Color Transformer: Intensity 92 | Decay Time: 0 93 | Enabled: false 94 | Invert Rainbow: true 95 | Max Color: 255; 255; 255 96 | Max Intensity: 0.94424 97 | Min Color: 0; 0; 0 98 | Min Intensity: -1.1 99 | Name: PointCloud2 100 | Position Transformer: XYZ 101 | Queue Size: 10 102 | Selectable: true 103 | Size (Pixels): 3 104 | Size (m): 0.5 105 | Style: Boxes 106 | Topic: /grid_map_visualization/elevation_points 107 | Unreliable: false 108 | Use Fixed Frame: true 109 | Use rainbow: false 110 | Value: false 111 | - Class: rviz/Marker 112 | Enabled: false 113 | Marker Topic: /grid_map_visualization/surface_normals 114 | Name: Marker 115 | Namespaces: 116 | {} 117 | Queue Size: 100 118 | Value: false 119 | - Alpha: 1 120 | Autocompute Intensity Bounds: true 121 | Class: grid_map_rviz_plugin/GridMap 122 | Color: 200; 200; 200 123 | Color Layer: elevation 124 | Color Transformer: IntensityLayer 125 | Enabled: true 126 | Height Layer: elevation 127 | Height Transformer: Layer 128 | History Length: 1 129 | Invert Rainbow: false 130 | Max Color: 255; 255; 255 131 | Max Intensity: 10 132 | Min Color: 0; 0; 0 133 | Min Intensity: 0 134 | Name: GridMap 135 | Show Grid Lines: true 136 | Topic: /grid_map_travelChecked 137 | Unreliable: false 138 | Use Rainbow: true 139 | Value: true 140 | - Alpha: 1 141 | Autocompute Intensity Bounds: true 142 | Class: grid_map_rviz_plugin/GridMap 143 | Color: 200; 200; 200 144 | Color Layer: elevation 145 | Color Transformer: GridMapLayer 146 | Enabled: false 147 | Height Layer: elevation 148 | Height Transformer: "" 149 | History Length: 1 150 | Invert Rainbow: false 151 | Max Color: 255; 255; 255 152 | Max Intensity: 10 153 | Min Color: 0; 0; 0 154 | Min Intensity: 0 155 | Name: GridMap 156 | Show Grid Lines: true 157 | Topic: /grid_map 158 | Unreliable: false 159 | Use Rainbow: true 160 | Value: false 161 | - Alpha: 0.7 162 | Class: rviz/Map 163 | Color Scheme: map 164 | Draw Behind: false 165 | Enabled: false 166 | Name: Map 167 | Topic: /occuMap 168 | Unreliable: false 169 | Value: false 170 | - Alpha: 1 171 | Autocompute Intensity Bounds: true 172 | Autocompute Value Bounds: 173 | Max Value: 10 174 | Min Value: -10 175 | Value: true 176 | Axis: Z 177 | Channel Name: intensity 178 | Class: rviz/PointCloud2 179 | Color: 255; 255; 255 180 | Color Transformer: FlatColor 181 | Decay Time: 0 182 | Enabled: true 183 | Invert Rainbow: false 184 | Max Color: 255; 255; 255 185 | Max Intensity: 4096 186 | Min Color: 0; 0; 0 187 | Min Intensity: 0 188 | Name: PointCloud2 189 | Position Transformer: XYZ 190 | Queue Size: 10 191 | Selectable: true 192 | Size (Pixels): 3 193 | Size (m): 0.1 194 | Style: Flat Squares 195 | Topic: /map_cloud 196 | Unreliable: false 197 | Use Fixed Frame: true 198 | Use rainbow: true 199 | Value: true 200 | - Alpha: 1 201 | Buffer Length: 2 202 | Class: rviz/Path 203 | Color: 255; 255; 0 204 | Enabled: true 205 | Head Diameter: 0.3 206 | Head Length: 0.2 207 | Length: 0.5 208 | Line Style: Billboards 209 | Line Width: 0.2 210 | Name: Path 211 | Offset: 212 | X: 0 213 | Y: 0 214 | Z: 0 215 | Pose Color: 255; 85; 255 216 | Pose Style: None 217 | Radius: 0.2 218 | Shaft Diameter: 0.1 219 | Shaft Length: 0.1 220 | Topic: /pathPoints 221 | Unreliable: false 222 | Value: true 223 | Enabled: true 224 | Global Options: 225 | Background Color: 0; 0; 0 226 | Fixed Frame: global 227 | Frame Rate: 30 228 | Name: root 229 | Tools: 230 | - Class: rviz/Interact 231 | Hide Inactive Objects: true 232 | - Class: rviz/MoveCamera 233 | - Class: rviz/Select 234 | - Class: rviz/FocusCamera 235 | - Class: rviz/Measure 236 | - Class: rviz/SetInitialPose 237 | Topic: /initialpose 238 | - Class: rviz/SetGoal 239 | Topic: /move_base_simple/goal 240 | - Class: rviz/PublishPoint 241 | Single click: true 242 | Topic: /clicked_point 243 | Value: true 244 | Views: 245 | Current: 246 | Class: rviz/Orbit 247 | Distance: 111.405 248 | Enable Stereo Rendering: 249 | Stereo Eye Separation: 0.06 250 | Stereo Focal Distance: 1 251 | Swap Stereo Eyes: false 252 | Value: false 253 | Focal Point: 254 | X: -6.80899 255 | Y: -3.97743 256 | Z: -19.8429 257 | Name: Current View 258 | Near Clip Distance: 0.01 259 | Pitch: 1.5598 260 | Target Frame: 261 | Value: Orbit (rviz) 262 | Yaw: 2.72 263 | Saved: ~ 264 | Window Geometry: 265 | Displays: 266 | collapsed: false 267 | Height: 1028 268 | Hide Left Dock: false 269 | Hide Right Dock: true 270 | QMainWindow State: 000000ff00000000fd00000004000000000000016a0000037afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000037a000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000037afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000037a000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000062b0000003efc0100000002fb0000000800540069006d006501000000000000062b000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000004bb0000037a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 271 | Selection: 272 | collapsed: false 273 | Time: 274 | collapsed: false 275 | Tool Properties: 276 | collapsed: false 277 | Views: 278 | collapsed: true 279 | Width: 1579 280 | X: 224 281 | Y: 14 282 | -------------------------------------------------------------------------------- /cfg/pointsToMap_Viz.yaml: -------------------------------------------------------------------------------- 1 | grid_map_topic: /grid_map 2 | 3 | grid_map_visualizations: 4 | 5 | - name: elevation_points 6 | type: point_cloud 7 | params: 8 | layer: elevation 9 | 10 | - name: surface_normals 11 | type: vectors 12 | params: 13 | layer_prefix: normal_ 14 | position_layer: elevation 15 | scale: 0.06 16 | line_width: 0.05 17 | color: 15600153 # red 18 | -------------------------------------------------------------------------------- /cfg/test.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Elevation1 9 | - /PointCloud21 10 | Splitter Ratio: 0.611465 11 | Tree Height: 437 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.588679 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: Elevation 31 | Visualization Manager: 32 | Class: "" 33 | Displays: 34 | - Class: rviz/Axes 35 | Enabled: false 36 | Length: 0.2 37 | Name: Axes 38 | Radius: 0.01 39 | Reference Frame: 40 | Value: false 41 | - Alpha: 1 42 | Autocompute Intensity Bounds: true 43 | Autocompute Value Bounds: 44 | Max Value: 0.0849175 45 | Min Value: -0.165153 46 | Value: false 47 | Axis: Z 48 | Channel Name: z 49 | Class: rviz/PointCloud2 50 | Color: 255; 255; 255 51 | Color Transformer: Intensity 52 | Decay Time: 0 53 | Enabled: true 54 | Invert Rainbow: false 55 | Max Color: 255; 255; 255 56 | Max Intensity: 0.999303 57 | Min Color: 0; 85; 127 58 | Min Intensity: -3.17157 59 | Name: Elevation 60 | Position Transformer: XYZ 61 | Queue Size: 10 62 | Selectable: false 63 | Size (Pixels): 3 64 | Size (m): 1.5 65 | Style: Boxes 66 | Topic: /grid_map_visualization/elevation_points 67 | Unreliable: false 68 | Use Fixed Frame: true 69 | Use rainbow: false 70 | Value: true 71 | - Alpha: 1 72 | Autocompute Intensity Bounds: true 73 | Autocompute Value Bounds: 74 | Max Value: 20.9609 75 | Min Value: -5.90366 76 | Value: true 77 | Axis: Z 78 | Channel Name: intensity 79 | Class: rviz/PointCloud2 80 | Color: 255; 255; 255 81 | Color Transformer: AxisColor 82 | Decay Time: 0 83 | Enabled: true 84 | Invert Rainbow: false 85 | Max Color: 255; 255; 255 86 | Max Intensity: 255 87 | Min Color: 0; 0; 0 88 | Min Intensity: 0 89 | Name: PointCloud2 90 | Position Transformer: XYZ 91 | Queue Size: 10 92 | Selectable: true 93 | Size (Pixels): 3 94 | Size (m): 0.5 95 | Style: Flat Squares 96 | Topic: /map_cloud 97 | Unreliable: false 98 | Use Fixed Frame: true 99 | Use rainbow: true 100 | Value: true 101 | Enabled: true 102 | Global Options: 103 | Background Color: 0; 0; 0 104 | Fixed Frame: map 105 | Frame Rate: 30 106 | Name: root 107 | Tools: 108 | - Class: rviz/Interact 109 | Hide Inactive Objects: true 110 | - Class: rviz/MoveCamera 111 | - Class: rviz/Select 112 | - Class: rviz/FocusCamera 113 | - Class: rviz/Measure 114 | - Class: rviz/SetInitialPose 115 | Topic: /initialpose 116 | - Class: rviz/SetGoal 117 | Topic: /move_base_simple/goal 118 | - Class: rviz/PublishPoint 119 | Single click: true 120 | Topic: /clicked_point 121 | Value: true 122 | Views: 123 | Current: 124 | Class: rviz/Orbit 125 | Distance: 105.853 126 | Enable Stereo Rendering: 127 | Stereo Eye Separation: 0.06 128 | Stereo Focal Distance: 1 129 | Swap Stereo Eyes: false 130 | Value: false 131 | Focal Point: 132 | X: 9.48552 133 | Y: -7.88273 134 | Z: 7.52759 135 | Name: Current View 136 | Near Clip Distance: 0.01 137 | Pitch: 0.909797 138 | Target Frame: 139 | Value: Orbit (rviz) 140 | Yaw: 3.59042 141 | Saved: ~ 142 | Window Geometry: 143 | Displays: 144 | collapsed: true 145 | Height: 1056 146 | Hide Left Dock: true 147 | Hide Right Dock: true 148 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000002800000396000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000040efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000040e000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007570000003efc0100000002fb0000000800540069006d0065010000000000000757000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000007570000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 149 | Selection: 150 | collapsed: false 151 | Time: 152 | collapsed: false 153 | Tool Properties: 154 | collapsed: false 155 | Views: 156 | collapsed: true 157 | Width: 1879 158 | X: 41 159 | Y: 24 160 | -------------------------------------------------------------------------------- /cfg/traversalCheck_Viz.yaml: -------------------------------------------------------------------------------- 1 | grid_map_topic: /grid_map_travelChecked 2 | 3 | grid_map_visualizations: 4 | 5 | - name: elevation_points 6 | type: point_cloud 7 | params: 8 | layer: elevation 9 | 10 | - name: surface_normals 11 | type: vectors 12 | params: 13 | layer_prefix: normal_ 14 | position_layer: elevation 15 | scale: 0.06 16 | line_width: 0.05 17 | color: 15600153 # red 18 | -------------------------------------------------------------------------------- /launch/laserToMap.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /launch/pathPlanning.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 　　　　 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /launch/pathPlanning_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 　　　　 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /launch/pointsToMap.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /launch/realTimeLiDAR.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /launch/realTimeMapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /launch/test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /launch/traversalCheck.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | s 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | grid_map_yh 4 | 1.6.0 5 | test by yh in　ZJU. 6 | Huan Yin 7 | BSD 8 | https://github.com/ZJUYH/grid_map 9 | http://github.com/ethz-asl/grid_map/issues 10 | Huan Yin 11 | catkin 12 | roscpp 13 | grid_map_core 14 | grid_map_ros 15 | grid_map_cv 16 | grid_map_filters 17 | grid_map_loader 18 | grid_map_msgs 19 | grid_map_octomap 20 | grid_map_rviz_plugin 21 | grid_map_visualization 22 | geometry_msgs 23 | sensor_msgs 24 | cv_bridge 25 | octomap_msgs 26 | 27 | -------------------------------------------------------------------------------- /readMe.md: -------------------------------------------------------------------------------- 1 | Functional deoms used by Robotics Lab in Zhejiang University, China. 2 | 3 | Thanks to ANYbotics: https://github.com/ANYbotics/grid_map 4 | 5 | ```diff 6 | - Parameters Matter! 7 | ``` 8 | 9 | ### test_node: 10 | simple test on lidar submap 11 | ### pointsToMap: 12 | sliding map of laser points -> grid map 13 | ### laserToMap: 14 | laser laser transform -> grid map 15 | ### traversalCheck: 16 | Traversability estimation, slope, step and rough 17 | ### pathPlanning: 18 | A* (& demo to show) 19 | & demo 20 | ### realTimeMapping: 21 | make the 'pointsToMap' be real time 22 | ### realTimeLiDAR: 23 | make the 'laserToMap' be real time 24 | -------------------------------------------------------------------------------- /src/laserToMap.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "pointmatcher/PointMatcher.h" 11 | #include "pointmatcher/Timer.h" 12 | #include "pointmatcher_ros/point_cloud.h" 13 | #include "pointmatcher_ros/transform.h" 14 | #include "nabo/nabo.h" 15 | #include "eigen_conversions/eigen_msg.h" 16 | #include "pointmatcher_ros/get_params_from_server.h" 17 | 18 | #include "tf/transform_broadcaster.h" 19 | 20 | #define checkValue -999 21 | 22 | 23 | /* 24 | * update the laser map frame by frame 25 | * 26 | */ 27 | 28 | 29 | using namespace grid_map; 30 | using namespace std; 31 | 32 | class gridMapping 33 | { 34 | typedef PointMatcher PM; 35 | typedef PM::DataPoints DP; 36 | typedef PM::Matches Matches; 37 | 38 | // not used ? 39 | typedef typename Nabo::NearestNeighbourSearch NNS; 40 | typedef typename NNS::SearchType NNSearchType; 41 | 42 | public: 43 | gridMapping(ros::NodeHandle &n); 44 | ~gridMapping(); 45 | ros::NodeHandle& n; 46 | 47 | ros::Publisher gridPublisher; 48 | ros::Publisher velodynePublisher; 49 | 50 | double size0; 51 | double size1; 52 | double resolution; 53 | double robotHeight; 54 | double velodyneHeight; 55 | string loadVelodyneDirName; 56 | string loadPoseName; 57 | 58 | double fillRadius; 59 | double rangeRadius; 60 | double groundTolarance; 61 | 62 | vector> robotPoses; 63 | tf::TransformBroadcaster tfBroadcaster; 64 | 65 | int startIndex; 66 | int readNum; 67 | 68 | PM::TransformationParameters TrobotToGlobal; 69 | 70 | DP velodyneCloud; 71 | 72 | unique_ptr transformation; 73 | 74 | void update(int index); 75 | 76 | void gridMapper(DP cloudIn); 77 | 78 | bool isInRange(Eigen::Vector2d center); 79 | 80 | private: 81 | 82 | 83 | }; 84 | 85 | gridMapping::~gridMapping() 86 | {} 87 | 88 | gridMapping::gridMapping(ros::NodeHandle& n): 89 | n(n), 90 | size0(getParam("size0", 0)), 91 | size1(getParam("size1", 0)), 92 | resolution(getParam("resolution", 0)), 93 | robotHeight(getParam("robotHeight", 0)), 94 | velodyneHeight(getParam("velodyneHeight", 0)), 95 | loadVelodyneDirName(getParam("loadVelodyneDirName", ".")), 96 | loadPoseName(getParam("loadPoseName", ".")), 97 | transformation(PM::get().REG(Transformation).create("RigidTransformation")), 98 | readNum(getParam("readNum", 0)), 99 | startIndex(getParam("startIndex", 0)), 100 | fillRadius(getParam("fillRadius", 0)), 101 | rangeRadius(getParam("rangeRadius", 0)), 102 | groundTolarance(getParam("groundTolarance", 0)) 103 | { 104 | gridPublisher = n.advertise("grid_map", 1, true); 105 | velodynePublisher = n.advertise("velodyne_cloud", 2, true); 106 | 107 | // read initial transformation 108 | int x, y; 109 | double temp; 110 | vector test; 111 | ifstream in(loadPoseName); 112 | if (!in) { 113 | cout << "Cannot open file.\n"; 114 | } 115 | for (y = 0; y < 9999999; y++) { 116 | test.clear(); 117 | for (x = 0; x < 16; x++) { 118 | in >> temp; 119 | test.push_back(temp); 120 | } 121 | robotPoses.push_back(test); 122 | } 123 | in.close(); 124 | 125 | // one by one frame 126 | for(int index=startIndex; indexupdate(index); 129 | } 130 | 131 | } 132 | 133 | void gridMapping::update(int index) 134 | { 135 | 136 | // loading velodyne 137 | stringstream ss; 138 | ss<>str; 141 | string veloName = loadVelodyneDirName + str + ".vtk"; 142 | { 143 | cout<<"-----------------------------------------------------------------------"<(TrobotToGlobal, "global", "robot", ros::Time::now())); 159 | velodynePublisher.publish(PointMatcher_ros::pointMatcherCloudToRosMsg(velodyneCloud, "robot", ros::Time::now())); 160 | 161 | this->gridMapper(velodyneCloud); 162 | 163 | } 164 | 165 | void gridMapping::gridMapper(DP cloudIn) 166 | { 167 | 168 | // create the grid map 169 | // robot centric 170 | grid_map::GridMap localGridMap({"elevation", "normal_x", "normal_y", "normal_z"}); 171 | localGridMap.setFrameId("robot"); 172 | localGridMap.setGeometry(Length(size0, size1), resolution, Position(0, 0)); 173 | localGridMap.add("elevation", -velodyneHeight); 174 | localGridMap.add("update", 0); 175 | 176 | // transfer the 3D point cloud to the grid map of elevation / normal_z 177 | for(int p=0; probotHeight) 182 | continue; // Skip this point if it does not lie within the elevation map and above the robot Height 183 | 184 | if(cloudIn.features(2, p) < -velodyneHeight) 185 | cloudIn.features(2, p) = -velodyneHeight; 186 | 187 | if (!localGridMap.isValid(index)) 188 | { 189 | // groove or hill 190 | 191 | // need update? 192 | if(localGridMap.at("update", index) == 0) // need 193 | { 194 | localGridMap.at("elevation", index) = cloudIn.features(2, p); //update 195 | localGridMap.at("update", index) = 1; 196 | } 197 | else 198 | { 199 | if(cloudIn.features(2, p) > localGridMap.at("elevation", index)) // a higher value 200 | localGridMap.at("elevation", index) = cloudIn.features(2, p); //update 201 | } 202 | 203 | } 204 | } 205 | 206 | // Those that without update,average filter them 207 | // travel the map 208 | for(GridMapIterator iterator(localGridMap); !iterator.isPastEnd(); ++iterator) 209 | { 210 | 211 | // measurement, continue 212 | if(localGridMap.at("update", *iterator) == 1) 213 | continue; 214 | 215 | Eigen::Vector2d center; 216 | localGridMap.getPosition(*iterator, center); 217 | 218 | // do not operate he nearest ones, for the person 219 | if(this->isInRange(center)) 220 | continue; 221 | 222 | // initial 223 | vector measureEle; 224 | 225 | for (CircleIterator submapIterator(localGridMap, center, fillRadius); 226 | !submapIterator.isPastEnd(); ++submapIterator) 227 | { 228 | if (localGridMap.at("update", *submapIterator) == 1) 229 | measureEle.push_back(localGridMap.at("elevation", *submapIterator)); 230 | } 231 | 232 | if(measureEle.size() > 0) //has measures? 233 | { 234 | double sum = std::accumulate(std::begin(measureEle), std::end(measureEle), 0.0); 235 | double mean = sum / measureEle.size(); 236 | // judge 237 | if(abs(mean-localGridMap.at("elevation", *iterator)) > this->groundTolarance) 238 | localGridMap.at("elevation", *iterator) = mean; 239 | } 240 | 241 | } 242 | 243 | 244 | // Publish grid map & cloud 245 | grid_map_msgs::GridMap message; 246 | GridMapRosConverter::toMessage(localGridMap, message); 247 | gridPublisher.publish(message); 248 | 249 | } 250 | 251 | 252 | bool gridMapping::isInRange(Eigen::Vector2d center) 253 | { 254 | double dis = pow(center(0)-0, 2) + pow(center(1)-0, 2); 255 | 256 | if(dis > this->rangeRadius) 257 | return false; 258 | else 259 | return true; 260 | 261 | } 262 | 263 | 264 | int main(int argc, char** argv) 265 | { 266 | ros::init(argc, argv, "gridmapping"); 267 | 268 | ros::NodeHandle n; 269 | 270 | gridMapping gridmapping(n); 271 | 272 | return 0; 273 | } 274 | -------------------------------------------------------------------------------- /src/pathPlanning.cpp: -------------------------------------------------------------------------------- 1 | #include "pathPlanning.hpp" 2 | 3 | bool pP::Vec2i::operator == (const Vec2i& coordinates_) 4 | { 5 | return (x == coordinates_.x && y == coordinates_.y); 6 | } 7 | 8 | pP::Vec2i operator + (const pP::Vec2i& left_, const pP::Vec2i& right_) 9 | { 10 | return{ left_.x + right_.x, left_.y + right_.y }; 11 | } 12 | 13 | pP::uint pP::Node::getScore() 14 | { 15 | return G + H; 16 | } 17 | 18 | pP::Node::Node(Vec2i coordinates_, Node *parent_) 19 | { 20 | parent = parent_; 21 | coordinates = coordinates_; 22 | G = H = 0; 23 | } 24 | 25 | pP::AStar::~AStar() 26 | {} 27 | 28 | pP::AStar::AStar(ros::NodeHandle& n): 29 | n(n), 30 | size0(getParam("size0", 0)), 31 | resolution(getParam("resolution", 0)), 32 | enableCross(getParam("enableCross", 0)), 33 | robotFrame(getParam("robotFrame", ".")), 34 | targetMsgName(getParam("targetMsgName", ".")) 35 | { 36 | mapSize = size0/resolution; 37 | directionCount = enableCross ? 8 : 4; 38 | source = {mapSize/2, mapSize/2}; // always start at the center of the robot 39 | 40 | targetSub = n.subscribe(targetMsgName, 10, &AStar::pathPlanner, this); 41 | occuMapSub = n.subscribe("occuMap", 10, &AStar::getoccuMap, this); 42 | 43 | pathPointsPub = n.advertise("pathPoints", 2, true); 44 | 45 | } 46 | 47 | void pP::AStar::getoccuMap(const nav_msgs::OccupancyGrid occuMapIn) 48 | { 49 | //haha, don't forget 50 | occus.clear(); 51 | 52 | // build the map: occus & frees 53 | int count = 0; 54 | for(int m=0; m mapSize || abs(target.y) > mapSize) 90 | { 91 | cout<<"The target is OUT of the map!"<getScore() <= currentNode->getScore()) 106 | { 107 | currentNode = node; 108 | } 109 | } 110 | 111 | // break if reaches the goal 112 | if (currentNode->coordinates == target) 113 | { 114 | break; 115 | } 116 | 117 | pathNodes.insert(currentNode); 118 | nextNodes.erase(std::find(nextNodes.begin(), nextNodes.end(), currentNode)); 119 | 120 | for (uint i = 0; i < directionCount; ++i) 121 | { 122 | Vec2i newCoord(currentNode->coordinates + direction[i]); 123 | if (isCollision(newCoord) || 124 | findNodeOnList(pathNodes, newCoord)) 125 | { 126 | continue; 127 | } 128 | 129 | uint totalCost = currentNode->G + ((i < 4) ? 0.8 : 1.2); 130 | 131 | Node *successor = findNodeOnList(nextNodes, newCoord); 132 | if (successor == nullptr) 133 | { 134 | successor = new Node(newCoord, currentNode); 135 | successor->G = totalCost; 136 | successor->H = euclidean(successor->coordinates, target); 137 | nextNodes.insert(successor); 138 | } 139 | else if (totalCost < successor->G) 140 | { 141 | successor->parent = currentNode; 142 | successor->G = totalCost; 143 | } 144 | } 145 | 146 | } 147 | 148 | if(isCollision(target)) 149 | { 150 | // if collision happened, find a closer path in the whites 151 | for (auto node : pathNodes){ 152 | if (node->getScore() <= currentNode->getScore() && node->getScore() != 0) { 153 | currentNode = node;} // 154 | } 155 | } 156 | 157 | std::vector path; 158 | while (currentNode != nullptr) 159 | { 160 | // cout<<"G: "<G<<" H: "<H<coordinates); 162 | currentNode = currentNode->parent; 163 | } 164 | 165 | this->drawPath(path); 166 | 167 | this->releaseNodes(nextNodes); 168 | this->releaseNodes(pathNodes); 169 | 170 | } 171 | 172 | bool pP::AStar::isCollision(Vec2i coord) 173 | { 174 | if( coord.x >= mapSize || coord.x < 0 || 175 | coord.y >= mapSize || coord.y < 0 || 176 | std::find(occus.begin(), occus.end(), coord) != occus.end()) 177 | return true; 178 | else 179 | return false; 180 | } 181 | 182 | pP::Node* pP::AStar::findNodeOnList(std::set& nodes, Vec2i coord) 183 | { 184 | for (auto node : nodes) { 185 | if (node->coordinates == coord) { 186 | return node; 187 | } 188 | } 189 | return nullptr; 190 | } 191 | 192 | pP::Vec2i pP::AStar::getDelta(Vec2i source, Vec2i target) 193 | { 194 | return{ abs(source.x - target.x), abs(source.y - target.y) }; 195 | } 196 | 197 | pP::uint pP::AStar::euclidean(Vec2i source, Vec2i target) 198 | { 199 | auto delta = std::move(getDelta(source, target)); 200 | return static_cast(sqrt(pow(delta.x, 2) + pow(delta.y, 2))); 201 | } 202 | 203 | void pP::AStar::releaseNodes(std::set& nodes) 204 | { 205 | for (auto it = nodes.begin(); it != nodes.end();) 206 | { 207 | delete *it; 208 | it = nodes.erase(it); 209 | } 210 | } 211 | 212 | void pP::AStar::drawPath(std::vector path) 213 | { 214 | nav_msgs::Path pathShow; 215 | pathShow.header.frame_id = robotFrame; 216 | pathShow.header.stamp = ros::Time::now(); 217 | 218 | // Coord-transform, resolution added 219 | // robot coordinate & my coordinate 220 | for(int i=0; i 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "tf/transform_broadcaster.h" 11 | #include "pointmatcher_ros/get_params_from_server.h" 12 | #include 13 | #include 14 | 15 | #define checkValue -999 16 | 17 | /* 18 | * update -> gridMapper -> travelSearch (another node)-> AStar (another node) 19 | * 20 | */ 21 | namespace pP 22 | { 23 | 24 | using namespace grid_map; 25 | using namespace std; 26 | using uint = unsigned int; 27 | 28 | struct Vec2i 29 | { 30 | int x, y; 31 | bool operator == (const Vec2i& coordinates_); 32 | }; 33 | 34 | struct Node 35 | { 36 | uint G, H; 37 | Vec2i coordinates; 38 | Node *parent; 39 | 40 | Node(Vec2i coord_, Node *parent_ = nullptr); 41 | uint getScore(); 42 | }; 43 | 44 | class AStar 45 | { 46 | 47 | public: 48 | AStar(ros::NodeHandle &n); 49 | ~AStar(); 50 | ros::NodeHandle& n; 51 | 52 | ros::Subscriber occuMapSub; 53 | nav_msgs::OccupancyGrid occuMap; 54 | ros::Publisher pathPointsPub; 55 | ros::Subscriber targetSub; 56 | 57 | std::vector occus; 58 | 59 | Vec2i source; 60 | Vec2i target; 61 | vector direction = { 62 | { 0, 1 }, { 1, 0 }, { 0, -1 }, { -1, 0 }, 63 | { -1, -1 }, { 1, 1 }, { -1, 1 }, { 1, -1 } 64 | }; 65 | uint directionCount; 66 | bool enableCross; 67 | 68 | std::set pathNodes; 69 | std::set nextNodes; 70 | 71 | double size0; 72 | double resolution; 73 | int mapSize; 74 | string robotFrame; 75 | string targetMsgName; 76 | 77 | void pathPlanner(const geometry_msgs::PoseStamped targetIn); 78 | void getoccuMap(const nav_msgs::OccupancyGrid occuMapIn); 79 | uint euclidean(Vec2i source, Vec2i target); 80 | bool isCollision(Vec2i coord); 81 | Node* findNodeOnList(std::set& nodes, Vec2i coord); 82 | Vec2i getDelta(Vec2i source, Vec2i target); 83 | void releaseNodes(std::set& nodes); 84 | void drawPath(std::vector path); 85 | 86 | private: 87 | 88 | 89 | }; 90 | 91 | } 92 | -------------------------------------------------------------------------------- /src/pathPlanning_demo.cpp: -------------------------------------------------------------------------------- 1 | #include "pathPlanning.hpp" 2 | 3 | bool pP::Vec2i::operator == (const Vec2i& coordinates_) 4 | { 5 | return (x == coordinates_.x && y == coordinates_.y); 6 | } 7 | 8 | pP::Vec2i operator + (const pP::Vec2i& left_, const pP::Vec2i& right_) 9 | { 10 | return{ left_.x + right_.x, left_.y + right_.y }; 11 | } 12 | 13 | pP::uint pP::Node::getScore() 14 | { 15 | return G + H; 16 | } 17 | 18 | pP::Node::Node(Vec2i coordinates_, Node *parent_) 19 | { 20 | parent = parent_; 21 | coordinates = coordinates_; 22 | G = H = 0; 23 | } 24 | 25 | pP::AStar::~AStar() 26 | {} 27 | 28 | pP::AStar::AStar(ros::NodeHandle& n): 29 | n(n), 30 | size0(getParam("size0", 0)), 31 | resolution(getParam("resolution", 0)), 32 | enableCross(getParam("enableCross", 0)), 33 | robotFrame(getParam("robotFrame", ".")), 34 | targetMsgName(getParam("targetMsgName", ".")) 35 | { 36 | mapSize = size0/resolution; 37 | directionCount = enableCross ? 8 : 4; 38 | source = {mapSize/2, mapSize/2}; // always start at the center of the robot 39 | target = {(int)mapSize/2, mapSize}; 40 | 41 | occuMapSub = n.subscribe("occuMap", 10, &AStar::getoccuMap, this); 42 | 43 | pathPointsPub = n.advertise("pathPoints", 2, true); 44 | 45 | } 46 | 47 | void pP::AStar::getoccuMap(const nav_msgs::OccupancyGrid occuMapIn) 48 | { 49 | //haha, don't forget 50 | occus.clear(); 51 | 52 | // build the map: occus & frees 53 | int count = 0; 54 | for(int m=0; mgetScore() <= currentNode->getScore()) 94 | { 95 | currentNode = node; 96 | } 97 | } 98 | 99 | // break if reaches the goal 100 | if (currentNode->coordinates == target) 101 | { 102 | break; 103 | } 104 | 105 | pathNodes.insert(currentNode); 106 | nextNodes.erase(std::find(nextNodes.begin(), nextNodes.end(), currentNode)); 107 | 108 | for (uint i = 0; i < directionCount; ++i) 109 | { 110 | Vec2i newCoord(currentNode->coordinates + direction[i]); 111 | if (isCollision(newCoord) || 112 | findNodeOnList(pathNodes, newCoord)) 113 | { 114 | continue; 115 | } 116 | 117 | uint totalCost = currentNode->G + ((i < 4) ? 0.8 : 1.2); 118 | 119 | Node *successor = findNodeOnList(nextNodes, newCoord); 120 | if (successor == nullptr) 121 | { 122 | successor = new Node(newCoord, currentNode); 123 | successor->G = totalCost; 124 | successor->H = euclidean(successor->coordinates, target); 125 | nextNodes.insert(successor); 126 | } 127 | else if (totalCost < successor->G) 128 | { 129 | successor->parent = currentNode; 130 | successor->G = totalCost; 131 | } 132 | } 133 | 134 | } 135 | 136 | if(isCollision(target)) 137 | { 138 | // if collision happened, find a closer path in the whites 139 | for (auto node : pathNodes){ 140 | if (node->getScore() <= currentNode->getScore() && node->getScore() != 0) { 141 | currentNode = node;} // 142 | } 143 | } 144 | 145 | std::vector path; 146 | while (currentNode != nullptr) 147 | { 148 | // cout<<"G: "<G<<" H: "<H<coordinates); 150 | currentNode = currentNode->parent; 151 | } 152 | 153 | this->drawPath(path); 154 | 155 | this->releaseNodes(nextNodes); 156 | this->releaseNodes(pathNodes); 157 | 158 | } 159 | 160 | bool pP::AStar::isCollision(Vec2i coord) 161 | { 162 | if( coord.x >= mapSize || coord.x < 0 || 163 | coord.y >= mapSize || coord.y < 0 || 164 | std::find(occus.begin(), occus.end(), coord) != occus.end()) 165 | return true; 166 | else 167 | return false; 168 | } 169 | 170 | pP::Node* pP::AStar::findNodeOnList(std::set& nodes, Vec2i coord) 171 | { 172 | for (auto node : nodes) { 173 | if (node->coordinates == coord) { 174 | return node; 175 | } 176 | } 177 | return nullptr; 178 | } 179 | 180 | pP::Vec2i pP::AStar::getDelta(Vec2i source, Vec2i target) 181 | { 182 | return{ abs(source.x - target.x), abs(source.y - target.y) }; 183 | } 184 | 185 | pP::uint pP::AStar::euclidean(Vec2i source, Vec2i target) 186 | { 187 | auto delta = std::move(getDelta(source, target)); 188 | return static_cast(sqrt(pow(delta.x, 2) + pow(delta.y, 2))); 189 | } 190 | 191 | void pP::AStar::releaseNodes(std::set& nodes) 192 | { 193 | for (auto it = nodes.begin(); it != nodes.end();) 194 | { 195 | delete *it; 196 | it = nodes.erase(it); 197 | } 198 | } 199 | 200 | void pP::AStar::drawPath(std::vector path) 201 | { 202 | nav_msgs::Path pathShow; 203 | pathShow.header.frame_id = robotFrame; 204 | pathShow.header.stamp = ros::Time::now(); 205 | 206 | // Coord-transform, resolution added 207 | // robot coordinate & my coordinate 208 | for(int i=0; i 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | 10 | #include "pointmatcher/PointMatcher.h" 11 | #include "pointmatcher/Timer.h" 12 | #include "pointmatcher_ros/point_cloud.h" 13 | #include "pointmatcher_ros/transform.h" 14 | #include "nabo/nabo.h" 15 | #include "eigen_conversions/eigen_msg.h" 16 | #include "pointmatcher_ros/get_params_from_server.h" 17 | 18 | #include "tf/transform_broadcaster.h" 19 | 20 | #define checkValue -999 21 | 22 | 23 | /* 24 | * cumulation -> gridMapper -> # TODO travelSearch (another node)-> # TODO pathPlanning (another node) 25 | * 26 | */ 27 | 28 | 29 | using namespace grid_map; 30 | using namespace std; 31 | using namespace PointMatcherSupport; 32 | 33 | class gridMapping 34 | { 35 | typedef PointMatcher PM; 36 | typedef PM::DataPoints DP; 37 | typedef PM::Matches Matches; 38 | 39 | // not used ? 40 | typedef typename Nabo::NearestNeighbourSearch NNS; 41 | typedef typename NNS::SearchType NNSearchType; 42 | 43 | public: 44 | gridMapping(ros::NodeHandle &n); 45 | ~gridMapping(); 46 | ros::NodeHandle& n; 47 | 48 | ros::Publisher gridPublisher; 49 | ros::Publisher velodynePublisher; 50 | ros::Publisher mapCloudPublisher; 51 | 52 | double size0; 53 | double size1; 54 | double resolution; 55 | double robotHeight; 56 | string loadVelodyneDirName; 57 | string loadPoseName; 58 | string cloudFilterName; 59 | double velodyneHeight; 60 | 61 | int startIndex; 62 | int readNum; 63 | 64 | PM::DataPointsFilters cloudFilters; 65 | 66 | double matchThreshold; 67 | 68 | vector> robotPoses; 69 | tf::TransformBroadcaster tfBroadcaster; 70 | 71 | PM::TransformationParameters TrobotToGlobal; 72 | PM::TransformationParameters TrobotLastToGlobal; 73 | PM::TransformationParameters TrobotLastToRobot; 74 | PM::TransformationParameters TrobotToRobotLast; 75 | 76 | 77 | DP localMapCloud; 78 | shared_ptr localMapNNS; 79 | 80 | unique_ptr transformation; 81 | 82 | void cumulation(int index); 83 | 84 | void gridMapper(DP cloudIn); 85 | 86 | private: 87 | 88 | 89 | }; 90 | 91 | gridMapping::~gridMapping() 92 | {} 93 | 94 | gridMapping::gridMapping(ros::NodeHandle& n): 95 | n(n), 96 | size0(getParam("size0", 0)), 97 | size1(getParam("size1", 0)), 98 | resolution(getParam("resolution", 0)), 99 | robotHeight(getParam("robotHeight", 0)), 100 | matchThreshold(getParam("matchThreshold", 0)), 101 | loadVelodyneDirName(getParam("loadVelodyneDirName", ".")), 102 | loadPoseName(getParam("loadPoseName", ".")), 103 | cloudFilterName(getParam("cloudFilterName", ".")), 104 | transformation(PM::get().REG(Transformation).create("RigidTransformation")), 105 | velodyneHeight(getParam("velodyneHeight", 0)), 106 | readNum(getParam("readNum", 0)), 107 | startIndex(getParam("startIndex", 0)) 108 | { 109 | gridPublisher = n.advertise("grid_map", 1, true); 110 | velodynePublisher = n.advertise("velodyne_cloud", 2, true); 111 | mapCloudPublisher = n.advertise("map_cloud", 2, true); 112 | 113 | // initilization of filters 114 | ifstream filterStr(cloudFilterName); 115 | cloudFilters = PM::DataPointsFilters(filterStr); 116 | 117 | // read initial transformation 118 | int x, y; 119 | double temp; 120 | vector test; 121 | ifstream in(loadPoseName); 122 | if (!in) { 123 | cout << "Cannot open file.\n"; 124 | } 125 | for (y = 0; y < 9999999; y++) { 126 | test.clear(); 127 | for (x = 0; x < 16; x++) { 128 | in >> temp; 129 | test.push_back(temp); 130 | } 131 | robotPoses.push_back(test); 132 | } 133 | in.close(); 134 | 135 | for(int index=startIndex; indexcumulation(index); 138 | } 139 | 140 | } 141 | 142 | void gridMapping::cumulation(int index) 143 | { 144 | 145 | // loading velodyne 146 | stringstream ss; 147 | ss<>str; 150 | string veloName = loadVelodyneDirName + str + ".vtk"; 151 | { 152 | cout<<"-----------------------------------------------------------------------"<(TrobotToGlobal, "global", "robot", ros::Time::now())); 169 | velodynePublisher.publish(PointMatcher_ros::pointMatcherCloudToRosMsg(velodyneCloud, "robot", ros::Time::now())); 170 | 171 | double t0 = ros::Time::now().toSec(); 172 | 173 | // if needs initiliazation 174 | if(index==startIndex) 175 | { 176 | // copy the velodyne to localMapCloud 177 | localMapCloud = velodyneCloud; 178 | // transformation initilization 179 | TrobotLastToGlobal = TrobotToGlobal; // simple, one word 180 | return; 181 | } 182 | // Sth. wrong? for transformation 183 | // right now 184 | TrobotLastToRobot = TrobotToGlobal.inverse() * TrobotLastToGlobal; 185 | transformation->correctParameters(TrobotLastToRobot); 186 | if(transformation->checkParameters(TrobotLastToRobot)) 187 | { 188 | localMapCloud = transformation->compute(localMapCloud, TrobotLastToRobot); 189 | } 190 | 191 | // accumulation of points, using kd match 192 | PM::Matches matches_velodyne( 193 | Matches::Dists(1, velodyneCloud.features.cols()), 194 | Matches::Ids(1, velodyneCloud.features.cols()) 195 | ); 196 | 197 | // update the kd-tree of local map cloud 198 | localMapNNS.reset(NNS::create(localMapCloud.features, localMapCloud.features.rows() - 1, NNS::KDTREE_LINEAR_HEAP, NNS::TOUCH_STATISTICS)); 199 | // search 200 | localMapNNS->knn(velodyneCloud.features, matches_velodyne.ids, matches_velodyne.dists, 1, 0); 201 | 202 | // search for the update 203 | double dist; 204 | vector addIndexVector; 205 | for(int p=0; pmatchThreshold) 209 | addIndexVector.push_back(p); 210 | } 211 | 212 | // rip the cloud according to the matching distance of velodyne cloud 213 | // faster than concatenate the velodyne directly 214 | DP concatenateCloud = velodyneCloud.createSimilarEmpty(); 215 | int count = 0; 216 | for(int v=0; v(localMapCloud, "robot", ros::Time::now())); 229 | 230 | // one by one pose 231 | TrobotLastToGlobal = TrobotToGlobal; 232 | 233 | double t1 = ros::Time::now().toSec(); 234 | cout<<"Cumulation Time: "<gridMapper(localMapCloud); 238 | 239 | double t2 = ros::Time::now().toSec(); 240 | cout<<"Gridding Time: "<robotHeight) 261 | continue; // Skip this point if it does not lie within the elevation map and above the robot Height 262 | 263 | if(cloudIn.features(2, p) < -velodyneHeight) 264 | cloudIn.features(2, p) = -velodyneHeight; 265 | 266 | if (!localGridMap.isValid(index)) 267 | { 268 | // groove or hill 269 | 270 | // need update? 271 | if(localGridMap.at("update", index) == 0) // need 272 | { 273 | localGridMap.at("elevation", index) = cloudIn.features(2, p); //update 274 | localGridMap.at("update", index) = 1; 275 | } 276 | else 277 | { 278 | if(cloudIn.features(2, p) > localGridMap.at("elevation", index)) // a higher value 279 | localGridMap.at("elevation", index) = cloudIn.features(2, p); //update 280 | } 281 | 282 | } 283 | 284 | } 285 | 286 | // Publish grid map & cloud 287 | grid_map_msgs::GridMap message; 288 | GridMapRosConverter::toMessage(localGridMap, message); 289 | gridPublisher.publish(message); 290 | 291 | } 292 | 293 | 294 | int main(int argc, char** argv) 295 | { 296 | ros::init(argc, argv, "gridmapping"); 297 | 298 | ros::NodeHandle n; 299 | 300 | gridMapping gridmapping(n); 301 | 302 | ros::spin(); 303 | 304 | return 0; 305 | } 306 | -------------------------------------------------------------------------------- /src/realTimeLiDAR.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | 10 | #include "pointmatcher/PointMatcher.h" 11 | #include "pointmatcher/Timer.h" 12 | #include "pointmatcher_ros/point_cloud.h" 13 | #include "pointmatcher_ros/transform.h" 14 | #include "nabo/nabo.h" 15 | #include "eigen_conversions/eigen_msg.h" 16 | #include "pointmatcher_ros/get_params_from_server.h" 17 | 18 | #include "tf/transform_broadcaster.h" 19 | #include "tf/transform_listener.h" 20 | 21 | #define checkValue -999 22 | 23 | 24 | /* 25 | * cumulation -> gridMapper -> # TODO travelSearch (another node)-> # TODO pathPlanning (another node) 26 | * 27 | */ 28 | 29 | 30 | using namespace grid_map; 31 | using namespace std; 32 | using namespace PointMatcherSupport; 33 | 34 | class gridMapping 35 | { 36 | typedef PointMatcher PM; 37 | typedef PM::DataPoints DP; 38 | 39 | public: 40 | gridMapping(ros::NodeHandle &n); 41 | ~gridMapping(); 42 | ros::NodeHandle& n; 43 | 44 | ros::Publisher gridPublisher; 45 | ros::Subscriber velodyneSubscriber; 46 | 47 | double size0; 48 | double size1; 49 | double resolution; 50 | double robotHeight; 51 | string cloudFilterName; 52 | double velodyneHeight; 53 | string robotFrame; 54 | string globalFrame; 55 | 56 | double fillRadius; 57 | double rangeRadius; 58 | double groundTolarance; 59 | 60 | unique_ptr transformation; 61 | 62 | void gridMapper(const sensor_msgs::PointCloud2& cloudMsgIn); 63 | 64 | bool isInRange(Eigen::Vector2d center); 65 | 66 | private: 67 | 68 | 69 | }; 70 | 71 | gridMapping::~gridMapping() 72 | {} 73 | 74 | gridMapping::gridMapping(ros::NodeHandle& n): 75 | n(n), 76 | size0(getParam("size0", 0)), 77 | size1(getParam("size1", 0)), 78 | resolution(getParam("resolution", 0)), 79 | robotHeight(getParam("robotHeight", 0)), 80 | cloudFilterName(getParam("cloudFilterName", ".")), 81 | transformation(PM::get().REG(Transformation).create("RigidTransformation")), 82 | velodyneHeight(getParam("velodyneHeight", 0)), 83 | robotFrame(getParam("robotFrame", ".")), 84 | fillRadius(getParam("fillRadius", 0)), 85 | rangeRadius(getParam("rangeRadius", 0)), 86 | groundTolarance(getParam("groundTolarance", 0)) 87 | { 88 | 89 | gridPublisher = n.advertise("grid_map", 1, true); 90 | 91 | velodyneSubscriber = n.subscribe("velodyne_points", 10, &gridMapping::gridMapper, this); 92 | } 93 | 94 | void gridMapping::gridMapper(const sensor_msgs::PointCloud2& cloudMsgIn) 95 | { 96 | DP velodyneCloud = PointMatcher_ros::rosMsgToPointMatcherCloud(cloudMsgIn); 97 | 98 | // create the grid map 99 | // robot centric 100 | grid_map::GridMap localGridMap({"elevation", "normal_x", "normal_y", "normal_z"}); 101 | localGridMap.setFrameId(robotFrame); 102 | localGridMap.setGeometry(Length(size0, size1), resolution, Position(0, 0)); 103 | localGridMap.add("elevation", -velodyneHeight); 104 | localGridMap.add("update", 0); 105 | 106 | // transfer the 3D point cloud to the grid map of elevation / normal_z 107 | for(int p=0; probotHeight) 112 | continue; // Skip this point if it does not lie within the elevation map and above the robot Height 113 | 114 | if(velodyneCloud.features(2, p) < -velodyneHeight) 115 | velodyneCloud.features(2, p) = -velodyneHeight; 116 | 117 | if (!localGridMap.isValid(index)) 118 | { 119 | // groove or hill 120 | 121 | // need update? 122 | if(localGridMap.at("update", index) == 0) // need 123 | { 124 | localGridMap.at("elevation", index) = velodyneCloud.features(2, p); //update 125 | localGridMap.at("update", index) = 1; 126 | } 127 | else 128 | { 129 | if(velodyneCloud.features(2, p) > localGridMap.at("elevation", index)) // a higher value 130 | localGridMap.at("elevation", index) = velodyneCloud.features(2, p); //update 131 | } 132 | 133 | } 134 | } 135 | 136 | // Those that without update,average filter them 137 | // travel the map 138 | for(GridMapIterator iterator(localGridMap); !iterator.isPastEnd(); ++iterator) 139 | { 140 | 141 | // measurement, continue 142 | if(localGridMap.at("update", *iterator) == 1) 143 | continue; 144 | 145 | Eigen::Vector2d center; 146 | localGridMap.getPosition(*iterator, center); 147 | 148 | // do not operate the nearest ones, for the person following the robot! 149 | if(this->isInRange(center)) 150 | continue; 151 | 152 | // initial 153 | vector measureEle; 154 | 155 | for (CircleIterator submapIterator(localGridMap, center, fillRadius); 156 | !submapIterator.isPastEnd(); ++submapIterator) 157 | { 158 | if (localGridMap.at("update", *submapIterator) == 1) 159 | measureEle.push_back(localGridMap.at("elevation", *submapIterator)); 160 | } 161 | 162 | // calculate the mean value fot those not updated! 163 | if(measureEle.size() > 0) //has measures? 164 | { 165 | double sum = std::accumulate(std::begin(measureEle), std::end(measureEle), 0.0); 166 | double mean = sum / measureEle.size(); 167 | // judge 168 | if(abs(mean-localGridMap.at("elevation", *iterator)) > this->groundTolarance) 169 | localGridMap.at("elevation", *iterator) = mean; 170 | } 171 | 172 | } 173 | 174 | 175 | // Publish grid map & cloud 176 | grid_map_msgs::GridMap message; 177 | GridMapRosConverter::toMessage(localGridMap, message); 178 | gridPublisher.publish(message); 179 | 180 | } 181 | 182 | bool gridMapping::isInRange(Eigen::Vector2d center) 183 | { 184 | double dis = pow(center(0)-0, 2) + pow(center(1)-0, 2); 185 | 186 | if(dis > this->rangeRadius) 187 | return false; 188 | else 189 | return true; 190 | 191 | } 192 | 193 | int main(int argc, char** argv) 194 | { 195 | ros::init(argc, argv, "gridmapping"); 196 | 197 | ros::NodeHandle n; 198 | 199 | gridMapping gridmapping(n); 200 | 201 | ros::spin(); 202 | 203 | return 0; 204 | } 205 | -------------------------------------------------------------------------------- /src/realTimeMapping.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | 10 | #include "pointmatcher/PointMatcher.h" 11 | #include "pointmatcher/Timer.h" 12 | #include "pointmatcher_ros/point_cloud.h" 13 | #include "pointmatcher_ros/transform.h" 14 | #include "nabo/nabo.h" 15 | #include "eigen_conversions/eigen_msg.h" 16 | #include "pointmatcher_ros/get_params_from_server.h" 17 | 18 | #include "tf/transform_broadcaster.h" 19 | #include "tf/transform_listener.h" 20 | 21 | #define checkValue -999 22 | 23 | 24 | /* 25 | * cumulation -> gridMapper -> # TODO travelSearch (another node)-> # TODO pathPlanning (another node) 26 | * 27 | */ 28 | 29 | 30 | using namespace grid_map; 31 | using namespace std; 32 | using namespace PointMatcherSupport; 33 | 34 | class gridMapping 35 | { 36 | typedef PointMatcher PM; 37 | typedef PM::DataPoints DP; 38 | typedef PM::Matches Matches; 39 | 40 | // not used ? 41 | typedef typename Nabo::NearestNeighbourSearch NNS; 42 | typedef typename NNS::SearchType NNSearchType; 43 | 44 | public: 45 | gridMapping(ros::NodeHandle &n); 46 | ~gridMapping(); 47 | ros::NodeHandle& n; 48 | 49 | ros::Publisher gridPublisher; 50 | ros::Publisher mapCloudPublisher; 51 | 52 | ros::Subscriber velodyneSubscriber; 53 | 54 | double size0; 55 | double size1; 56 | double resolution; 57 | double robotHeight; 58 | string cloudFilterName; 59 | double velodyneHeight; 60 | string robotFrame; 61 | string globalFrame; 62 | 63 | PM::DataPointsFilters cloudFilters; 64 | 65 | // first time? 66 | bool firstTime; 67 | 68 | double matchThreshold; 69 | 70 | tf::TransformListener tfListener; 71 | 72 | PM::TransformationParameters TrobotToGlobal; 73 | PM::TransformationParameters TrobotLastToGlobal; 74 | PM::TransformationParameters TrobotLastToRobot; 75 | PM::TransformationParameters TrobotToRobotLast; 76 | 77 | 78 | DP localMapCloud; 79 | shared_ptr localMapNNS; 80 | 81 | unique_ptr transformation; 82 | 83 | void cumulation(const sensor_msgs::PointCloud2& cloudMsgIn); 84 | 85 | void gridMapper(DP cloudIn); 86 | 87 | private: 88 | 89 | 90 | }; 91 | 92 | gridMapping::~gridMapping() 93 | {} 94 | 95 | gridMapping::gridMapping(ros::NodeHandle& n): 96 | n(n), 97 | size0(getParam("size0", 0)), 98 | size1(getParam("size1", 0)), 99 | resolution(getParam("resolution", 0)), 100 | robotHeight(getParam("robotHeight", 0)), 101 | matchThreshold(getParam("matchThreshold", 0)), 102 | cloudFilterName(getParam("cloudFilterName", ".")), 103 | transformation(PM::get().REG(Transformation).create("RigidTransformation")), 104 | velodyneHeight(getParam("velodyneHeight", 0)), 105 | robotFrame(getParam("robotFrame", ".")), 106 | globalFrame(getParam("globalFrame", ".")) 107 | { 108 | firstTime = true; 109 | 110 | // initilization of filters 111 | ifstream filterStr(cloudFilterName); 112 | cloudFilters = PM::DataPointsFilters(filterStr); 113 | 114 | gridPublisher = n.advertise("grid_map", 1, true); 115 | mapCloudPublisher = n.advertise("map_cloud", 2, true); 116 | 117 | velodyneSubscriber = n.subscribe("velodyne_points", 10, &gridMapping::cumulation, this); 118 | } 119 | 120 | void gridMapping::cumulation(const sensor_msgs::PointCloud2& cloudMsgIn) 121 | { 122 | DP velodyneCloud = PointMatcher_ros::rosMsgToPointMatcherCloud(cloudMsgIn); 123 | 124 | // listen to the real time trandformation from robot to world 125 | try 126 | { 127 | TrobotToGlobal = PointMatcher_ros::eigenMatrixToDim( 128 | PointMatcher_ros::transformListenerToEigenMatrix( 129 | tfListener, 130 | globalFrame, 131 | robotFrame, 132 | cloudMsgIn.header.stamp 133 | ), 4); 134 | } 135 | catch (exception& e) 136 | { 137 | cout << e.what() << endl; 138 | return; 139 | } 140 | 141 | // publish 142 | double t0 = ros::Time::now().toSec(); 143 | 144 | // if needs initiliazation 145 | if(firstTime) 146 | { 147 | firstTime = false; 148 | // copy the velodyne to localMapCloud 149 | localMapCloud = velodyneCloud; 150 | // transformation initilization 151 | TrobotLastToGlobal = TrobotToGlobal; // simple, one word 152 | return; 153 | } 154 | // Sth. wrong? for transformation 155 | // right now 156 | TrobotLastToRobot = TrobotToGlobal.inverse() * TrobotLastToGlobal; 157 | transformation->correctParameters(TrobotLastToRobot); 158 | if(transformation->checkParameters(TrobotLastToRobot)) 159 | { 160 | localMapCloud = transformation->compute(localMapCloud, TrobotLastToRobot); 161 | } 162 | 163 | // accumulation of points, using kd match 164 | PM::Matches matches_velodyne( 165 | Matches::Dists(1, velodyneCloud.features.cols()), 166 | Matches::Ids(1, velodyneCloud.features.cols()) 167 | ); 168 | 169 | // update the kd-tree of local map cloud 170 | localMapNNS.reset(NNS::create(localMapCloud.features, localMapCloud.features.rows() - 1, NNS::KDTREE_LINEAR_HEAP, NNS::TOUCH_STATISTICS)); 171 | // search 172 | localMapNNS->knn(velodyneCloud.features, matches_velodyne.ids, matches_velodyne.dists, 1, 0); 173 | 174 | // search for the update 175 | double dist; 176 | vector addIndexVector; 177 | for(int p=0; pmatchThreshold) 181 | addIndexVector.push_back(p); 182 | } 183 | 184 | // rip the cloud according to the matching distance of velodyne cloud 185 | // faster than concatenate the velodyne directly 186 | DP concatenateCloud = velodyneCloud.createSimilarEmpty(); 187 | int count = 0; 188 | for(int v=0; v(localMapCloud, robotFrame, cloudMsgIn.header.stamp)); 201 | 202 | // one by one pose 203 | TrobotLastToGlobal = TrobotToGlobal; 204 | 205 | double t1 = ros::Time::now().toSec(); 206 | cout<<"Cumulation Time: "<gridMapper(localMapCloud); 210 | 211 | double t2 = ros::Time::now().toSec(); 212 | cout<<"Gridding Time: "<robotHeight) 233 | continue; // Skip this point if it does not lie within the elevation map and above the robot Height 234 | 235 | if(cloudIn.features(2, p) < -velodyneHeight) 236 | cloudIn.features(2, p) = -velodyneHeight; 237 | 238 | if (!localGridMap.isValid(index)) 239 | { 240 | // groove or hill 241 | 242 | // need update? 243 | if(localGridMap.at("update", index) == 0) // need 244 | { 245 | localGridMap.at("elevation", index) = cloudIn.features(2, p); //update 246 | localGridMap.at("update", index) = 1; 247 | } 248 | else 249 | { 250 | if(cloudIn.features(2, p) > localGridMap.at("elevation", index)) // a higher value 251 | localGridMap.at("elevation", index) = cloudIn.features(2, p); //update 252 | } 253 | 254 | } 255 | 256 | } 257 | 258 | 259 | // Publish grid map & cloud 260 | grid_map_msgs::GridMap message; 261 | GridMapRosConverter::toMessage(localGridMap, message); 262 | gridPublisher.publish(message); 263 | 264 | } 265 | 266 | 267 | int main(int argc, char** argv) 268 | { 269 | ros::init(argc, argv, "gridmapping"); 270 | 271 | ros::NodeHandle n; 272 | 273 | gridMapping gridmapping(n); 274 | 275 | ros::spin(); 276 | 277 | return 0; 278 | } 279 | -------------------------------------------------------------------------------- /src/test_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "pointmatcher/PointMatcher.h" 9 | #include "pointmatcher/Timer.h" 10 | #include "pointmatcher_ros/point_cloud.h" 11 | #include "pointmatcher_ros/transform.h" 12 | #include "nabo/nabo.h" 13 | #include "eigen_conversions/eigen_msg.h" 14 | #include "pointmatcher_ros/get_params_from_server.h" 15 | 16 | #define checkValue -999 17 | 18 | using namespace grid_map; 19 | using namespace std; 20 | using namespace PointMatcherSupport; 21 | 22 | class gridMapping 23 | { 24 | typedef PointMatcher PM; 25 | typedef PM::DataPoints DP; 26 | typedef PM::Matches Matches; 27 | 28 | // not used ? 29 | typedef typename Nabo::NearestNeighbourSearch NNS; 30 | typedef typename NNS::SearchType NNSearchType; 31 | 32 | public: 33 | gridMapping(ros::NodeHandle &n); 34 | ~gridMapping(); 35 | ros::NodeHandle& n; 36 | 37 | DP mapCloud; 38 | 39 | ros::Publisher gridPublisher; 40 | ros::Publisher cloudPublisher; 41 | double size0; 42 | double size1; 43 | double resolution; 44 | double robotX; 45 | double robotY; 46 | double robotHeight; 47 | string loadMapName; 48 | 49 | void process(); 50 | 51 | private: 52 | 53 | 54 | }; 55 | 56 | gridMapping::~gridMapping() 57 | {} 58 | 59 | gridMapping::gridMapping(ros::NodeHandle& n): 60 | n(n), 61 | size0(getParam("size0", 0)), 62 | size1(getParam("size1", 0)), 63 | resolution(getParam("resolution", 0)), 64 | robotX(getParam("robotX", 0)), 65 | robotY(getParam("robotY", 0)), 66 | robotHeight(getParam("robotHeight", 0)), 67 | loadMapName(getParam("loadMapName", ".")) 68 | { 69 | gridPublisher = n.advertise("grid_map", 1, true); 70 | cloudPublisher = n.advertise("map_cloud", 2, true); 71 | 72 | this->process(); 73 | } 74 | 75 | void gridMapping::process() 76 | { 77 | mapCloud = DP::load(loadMapName); 78 | 79 | // create the grid map 80 | grid_map::GridMap localGridMap({"elevation"}); 81 | localGridMap.setFrameId("map"); 82 | localGridMap.setGeometry(Length(size0, size1), resolution, Position(robotX, robotY)); 83 | 84 | localGridMap.add("traversability", Matrix::Zero(localGridMap.getSize()(0), localGridMap.getSize()(1))); 85 | 86 | ros::Rate rate(3.0); 87 | while (ros::ok()) 88 | { 89 | 90 | // find the points in the grid 91 | for(int p=0; probotHeight) 96 | continue; // Skip this point if it does not lie within the elevation map. 97 | 98 | if (!localGridMap.isValid(index)) 99 | { 100 | if(localGridMap.at("elevation", index) > checkValue) 101 | { 102 | // upper bound 103 | if(mapCloud.features(2, p) > localGridMap.at("elevation", index)) 104 | localGridMap.at("elevation", index) = mapCloud.features(2, p); } 105 | else 106 | localGridMap.at("elevation", index) = mapCloud.features(2, p); 107 | } 108 | } 109 | 110 | // check the traversability area 111 | // for() 112 | // { 113 | 114 | // } 115 | 116 | // Publish grid map & cloud 117 | grid_map_msgs::GridMap message; 118 | GridMapRosConverter::toMessage(localGridMap, message); 119 | gridPublisher.publish(message); 120 | cloudPublisher.publish(PointMatcher_ros::pointMatcherCloudToRosMsg(mapCloud, "map", ros::Time(0))); 121 | 122 | rate.sleep(); 123 | } 124 | 125 | } 126 | 127 | 128 | 129 | int main(int argc, char** argv) 130 | { 131 | ros::init(argc, argv, "test"); 132 | 133 | ros::NodeHandle n; 134 | 135 | gridMapping gridmapping(n); 136 | 137 | return 0; 138 | } 139 | -------------------------------------------------------------------------------- /src/traversalCheck.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "pointmatcher/PointMatcher.h" 9 | #include "pointmatcher/Timer.h" 10 | #include "pointmatcher_ros/point_cloud.h" 11 | #include "pointmatcher_ros/transform.h" 12 | #include "nabo/nabo.h" 13 | #include "eigen_conversions/eigen_msg.h" 14 | #include "pointmatcher_ros/get_params_from_server.h" 15 | 16 | using namespace grid_map; 17 | using namespace std; 18 | using namespace PointMatcherSupport; 19 | 20 | class Traversability 21 | { 22 | typedef PointMatcher PM; 23 | typedef PM::DataPoints DP; 24 | typedef PM::Matches Matches; 25 | 26 | // not used ? 27 | typedef typename Nabo::NearestNeighbourSearch NNS; 28 | typedef typename NNS::SearchType NNSearchType; 29 | 30 | public: 31 | Traversability(ros::NodeHandle &n); 32 | ~Traversability(); 33 | ros::NodeHandle& n; 34 | 35 | ros::Subscriber gridMapSub; 36 | ros::Publisher gridPublisher; 37 | ros::Publisher occuMapPublisher; 38 | 39 | nav_msgs::OccupancyGrid occuMap; 40 | 41 | double slopeCritical; 42 | double stepCritical; 43 | double stepRadiusFirst; 44 | double stepRadiusSecond; 45 | int cellsCritical; 46 | double boundCritical; 47 | int boundDiffNum; 48 | 49 | void check(const grid_map_msgs::GridMap& gridMapIn); 50 | 51 | private: 52 | 53 | 54 | }; 55 | 56 | Traversability::~Traversability() 57 | {} 58 | 59 | Traversability::Traversability(ros::NodeHandle& n): 60 | n(n), 61 | slopeCritical(getParam("slopeCritical", 0)), 62 | stepCritical(getParam("stepCritical", 0)), 63 | stepRadiusFirst(getParam("stepRadiusFirst", 0)), 64 | stepRadiusSecond(getParam("stepRadiusSecond", 0)), 65 | cellsCritical(getParam("cellsCritical", 0)), 66 | boundCritical(getParam("boundCritical", 0)), 67 | boundDiffNum(getParam("boundDiffNum", 0)) 68 | { 69 | gridPublisher = n.advertise("grid_map_travelChecked", 1, true); 70 | occuMapPublisher = n.advertise("occuMap", 1, true); 71 | gridMapSub = n.subscribe("grid_map", 10, &Traversability::check, this); 72 | } 73 | 74 | void Traversability::check(const grid_map_msgs::GridMap& gridMapIn) 75 | { 76 | cout<<"----------------------------"< heightMax) 118 | heightMax = height; 119 | if (height < heightMin) 120 | heightMin = height; 121 | } 122 | 123 | if (init) 124 | localGridMap.at("step_height", *iterator) = heightMax - heightMin; 125 | } 126 | 127 | // Second iteration through the elevation map. 128 | for (GridMapIterator iterator(localGridMap); !iterator.isPastEnd(); ++iterator) 129 | { 130 | int nCells = 0; 131 | double stepMax = 0.0; 132 | bool isValid = false; 133 | 134 | int flatCount = 0; 135 | int hillCount = 0; 136 | 137 | // Requested position (center) of circle in map. 138 | Eigen::Vector2d center; 139 | localGridMap.getPosition(*iterator, center); 140 | 141 | // Compute the traversability and the boundary of slopes, according to the step_height. 142 | for (CircleIterator submapIterator(localGridMap, center, stepRadiusSecond); 143 | !submapIterator.isPastEnd(); ++submapIterator) 144 | { 145 | if (!localGridMap.isValid(*submapIterator, "step_height")) 146 | continue; 147 | isValid = true; 148 | if (localGridMap.at("step_height", *submapIterator) > stepMax) 149 | { 150 | stepMax = localGridMap.at("step_height", *submapIterator); 151 | } 152 | if (localGridMap.at("step_height", *submapIterator) > stepCritical) 153 | nCells++; 154 | 155 | // can be noted 156 | localGridMap.at("step_height", *submapIterator)>boundCritical?hillCount++:flatCount++; 157 | } 158 | 159 | // can be noted, for boundary line 160 | if(flatCount > 0 && hillCount > 0 && abs(flatCount-hillCount) <= boundDiffNum) 161 | localGridMap.at("boundary_line", *iterator) = 1; 162 | else 163 | localGridMap.at("boundary_line", *iterator) = 0; 164 | 165 | if (isValid) 166 | { 167 | step = std::min(stepMax, 168 | (double) nCells / (double) cellsCritical * stepMax); 169 | if (step < stepCritical) 170 | { 171 | // fully = 1 172 | localGridMap.at("traversability_step", *iterator) = 1.0 - step / stepCritical; 173 | } 174 | else 175 | { 176 | // cannot traverse 177 | localGridMap.at("traversability_step", *iterator) = 0.0; 178 | } 179 | } 180 | } 181 | 182 | 183 | 184 | // Publish grid map & cloud 185 | grid_map_msgs::GridMap message; 186 | GridMapRosConverter::toMessage(localGridMap, message); 187 | gridPublisher.publish(message); 188 | 189 | // convert to 2D-costmap and publish 190 | GridMapRosConverter::toOccupancyGrid(localGridMap, "traversability_step", 0.0, 1.0, occuMap); 191 | occuMapPublisher.publish(occuMap); 192 | 193 | } 194 | 195 | 196 | 197 | int main(int argc, char** argv) 198 | { 199 | ros::init(argc, argv, "traversability"); 200 | 201 | ros::NodeHandle n; 202 | 203 | Traversability traversability(n); 204 | 205 | ros::spin(); 206 | 207 | return 0; 208 | } 209 | --------------------------------------------------------------------------------