├── .catkin_tools ├── CATKIN_IGNORE ├── README ├── VERSION └── profiles │ └── default │ ├── build.yaml │ ├── devel_collisions.txt │ └── packages │ └── catkin_tools_prebuild │ ├── devel_manifest.txt │ └── package.xml ├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── config ├── lidar_calibration.rviz └── sensors_suite.urdf ├── doc └── setup.png ├── launch └── lidars_extrinsic_computation.launch ├── package.xml └── src ├── avia_format_converter.cpp ├── lidars_extrinsic_computation.cpp └── mid360_format_converter.cpp /.catkin_tools/CATKIN_IGNORE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TIERS/multi_lidar_multi_uav_dataset/a9c64d91b63f684b74f1ea310455674dafa282bc/.catkin_tools/CATKIN_IGNORE -------------------------------------------------------------------------------- /.catkin_tools/README: -------------------------------------------------------------------------------- 1 | # Catkin Tools Metadata 2 | 3 | This directory was generated by catkin_tools and it contains persistent 4 | configuration information used by the `catkin` command and its sub-commands. 5 | 6 | Each subdirectory of the `profiles` directory contains a set of persistent 7 | configuration options for separate profiles. The default profile is called 8 | `default`. If another profile is desired, it can be described in the 9 | `profiles.yaml` file in this directory. 10 | 11 | Please see the catkin_tools documentation before editing any files in this 12 | directory. Most actions can be performed with the `catkin` command-line 13 | program. 14 | -------------------------------------------------------------------------------- /.catkin_tools/VERSION: -------------------------------------------------------------------------------- 1 | 0.9.2 -------------------------------------------------------------------------------- /.catkin_tools/profiles/default/build.yaml: -------------------------------------------------------------------------------- 1 | authors: [] 2 | blacklist: [] 3 | build_space: build 4 | catkin_make_args: [] 5 | cmake_args: [] 6 | devel_layout: linked 7 | devel_space: devel 8 | extend_path: null 9 | extends: null 10 | install: false 11 | install_space: install 12 | isolate_install: false 13 | jobs_args: [] 14 | licenses: 15 | - TODO 16 | log_space: logs 17 | maintainers: [] 18 | make_args: [] 19 | source_space: src 20 | use_env_cache: false 21 | use_internal_make_jobserver: true 22 | whitelist: [] 23 | -------------------------------------------------------------------------------- /.catkin_tools/profiles/default/devel_collisions.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TIERS/multi_lidar_multi_uav_dataset/a9c64d91b63f684b74f1ea310455674dafa282bc/.catkin_tools/profiles/default/devel_collisions.txt -------------------------------------------------------------------------------- /.catkin_tools/profiles/default/packages/catkin_tools_prebuild/devel_manifest.txt: -------------------------------------------------------------------------------- 1 | /home/iacopo/lidar_dataset/build/catkin_tools_prebuild 2 | /home/iacopo/lidar_dataset/devel/.private/catkin_tools_prebuild/setup.zsh /home/iacopo/lidar_dataset/devel/./setup.zsh 3 | /home/iacopo/lidar_dataset/devel/.private/catkin_tools_prebuild/local_setup.zsh /home/iacopo/lidar_dataset/devel/./local_setup.zsh 4 | /home/iacopo/lidar_dataset/devel/.private/catkin_tools_prebuild/local_setup.bash /home/iacopo/lidar_dataset/devel/./local_setup.bash 5 | /home/iacopo/lidar_dataset/devel/.private/catkin_tools_prebuild/local_setup.sh /home/iacopo/lidar_dataset/devel/./local_setup.sh 6 | /home/iacopo/lidar_dataset/devel/.private/catkin_tools_prebuild/cmake.lock /home/iacopo/lidar_dataset/devel/./cmake.lock 7 | /home/iacopo/lidar_dataset/devel/.private/catkin_tools_prebuild/env.sh /home/iacopo/lidar_dataset/devel/./env.sh 8 | /home/iacopo/lidar_dataset/devel/.private/catkin_tools_prebuild/setup.sh /home/iacopo/lidar_dataset/devel/./setup.sh 9 | /home/iacopo/lidar_dataset/devel/.private/catkin_tools_prebuild/_setup_util.py /home/iacopo/lidar_dataset/devel/./_setup_util.py 10 | /home/iacopo/lidar_dataset/devel/.private/catkin_tools_prebuild/setup.bash /home/iacopo/lidar_dataset/devel/./setup.bash 11 | /home/iacopo/lidar_dataset/devel/.private/catkin_tools_prebuild/lib/pkgconfig/catkin_tools_prebuild.pc /home/iacopo/lidar_dataset/devel/lib/pkgconfig/catkin_tools_prebuild.pc 12 | /home/iacopo/lidar_dataset/devel/.private/catkin_tools_prebuild/share/catkin_tools_prebuild/cmake/catkin_tools_prebuildConfig.cmake /home/iacopo/lidar_dataset/devel/share/catkin_tools_prebuild/cmake/catkin_tools_prebuildConfig.cmake 13 | /home/iacopo/lidar_dataset/devel/.private/catkin_tools_prebuild/share/catkin_tools_prebuild/cmake/catkin_tools_prebuildConfig-version.cmake /home/iacopo/lidar_dataset/devel/share/catkin_tools_prebuild/cmake/catkin_tools_prebuildConfig-version.cmake 14 | -------------------------------------------------------------------------------- /.catkin_tools/profiles/default/packages/catkin_tools_prebuild/package.xml: -------------------------------------------------------------------------------- 1 | 2 | catkin_tools_prebuild 3 | 4 | This package is used to generate catkin setup files. 5 | 6 | 0.0.0 7 | BSD 8 | jbohren 9 | catkin 10 | 11 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(multi_lidar_multi_uav_dataset) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++14) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | geometry_msgs 12 | pcl_conversions 13 | pcl_ros 14 | roscpp 15 | rospy 16 | sensor_msgs 17 | std_msgs 18 | livox_ros_driver 19 | ) 20 | 21 | find_package(OpenCV REQUIRED) 22 | 23 | ################################### 24 | ## catkin specific configuration ## 25 | ################################### 26 | 27 | catkin_package( 28 | DEPENDS 29 | roscpp 30 | std_msgs 31 | sensor_msgs 32 | geometry_msgs 33 | pcl_conversions 34 | pcl_ros 35 | ) 36 | 37 | ########### 38 | ## Build ## 39 | ########### 40 | 41 | ## Specify additional locations of header files 42 | ## Your package locations should be listed before other locations 43 | include_directories( 44 | include 45 | ${catkin_INCLUDE_DIRS} 46 | ) 47 | 48 | add_executable(avia_format_converter src/avia_format_converter.cpp) 49 | target_link_libraries(avia_format_converter ${catkin_LIBRARIES}) 50 | 51 | add_executable(mid360_format_converter src/mid360_format_converter.cpp) 52 | target_link_libraries(mid360_format_converter ${catkin_LIBRARIES}) 53 | 54 | add_executable(lidars_extrinsic_computation src/lidars_extrinsic_computation.cpp) 55 | target_link_libraries(lidars_extrinsic_computation ${catkin_LIBRARIES}) 56 | 57 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 TIERS 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 |
2 |

Towards Robust UAV Tracking in GNSS-Denied Environments: A Multi-LiDAR Multi-UAV Dataset

3 | 4 | 5 | 6 | 7 |
8 |
9 | Project Page 10 |   •   11 | Paper 12 |   •   13 | Contact Us 14 |
15 |
16 |

17 | 18 |

19 | 20 |
21 | 22 | We present a novel multi-LiDAR dataset specifically designed for UAV tracking. Our dataset includes data from a spinning LiDAR, two solid-state LiDARs with different Field of View (FoV) and scan patterns, and an RGB-D camera. This diverse sensor suite allows for research on new challenges in the field, including limited FoV adaptability and multi-modality data processing. For a comprehensive list of sequences refer to the paper [Towards Robust UAV Tracking in GNSS-Denied Environments: A Multi-LiDAR Multi-UAV Dataset](https://arxiv.org/abs/2310.09165) and the [project page](https://tiers.github.io/multi_lidar_multi_uav_dataset ) 23 | 24 |
25 | 26 | ## Calibration 27 | 28 | We provide a ROS package to compute the extrinsic parameters between LiDARs and camera based on GICP. As the OS1 has the largest FOV, it is treated as base reference frame ("base_link") in which all the other point clouds are transformed. For the Avia, Mid-360 and Realsense D435, we integrated the first five frames to increase point cloud density. 29 | 30 | To use this package, play the Calibration rosbag from our dataset: 31 | ~~~ 32 | rosbag play Calibration.bag -l 33 | ~~~ 34 | Then run our calibration launch file: 35 | ~~~ 36 | roslaunch multi_lidar_multi_uav_dataset lidars_extrinsic_computation.launch 37 | ~~~ 38 | 39 | The computed extrinsic parameters will appear in the terminal: 40 | ~~~ 41 | OS -> base_link 0 0 0 0 0 0 /os_sensor /base_link 10 42 | Avia -> base_link 0.149354 0.0423582 -0.0524961 3.13419 -3.13908 -3.13281 /avia_frame /base_link 10 43 | Mid360 -> base_link 0.125546 -0.0554536 -0.20206 0.00467344 0.0270294 0.0494959 /mid360_frame /base_link 10 44 | Camera -> base_link -0.172863 0.11895 -0.101785 1.55222 3.11188 1.60982 /camera_depth_optical_frame /base_link 10 45 | ~~~ 46 | 47 | ## Install 48 | The code has been tested on Ubuntu 20.04 with ROS Noetic 49 | 50 | ### Dependencies 51 | - PCL 52 | 53 | - Eigen 54 | 55 | - Livox_ros_driver, Follow [livox_ros_driver Installation](https://github.com/Livox-SDK/livox_ros_driver). 56 | 57 | ### Build 58 | ``` 59 | cd ~/catkin_ws/src 60 | git clone https://github.com/TIERS/multi_lidar_multi_uav_dataset 61 | cd .. 62 | catkin build 63 | ``` 64 | 65 | ## Citation 66 | If you use this dataset for any academic work, please cite the following publication: 67 | 68 | ``` 69 | @inproceedings{catalano2023towards, 70 | title={Towards robust uav tracking in gnss-denied environments: a multi-lidar multi-uav dataset}, 71 | author={Catalano, Iacopo and Yu, Xianjia and Queralta, Jorge Pe{\~n}a}, 72 | booktitle={2023 IEEE International Conference on Robotics and Biomimetics (ROBIO)}, 73 | pages={1--7}, 74 | year={2023}, 75 | organization={IEEE} 76 | } 77 | ``` 78 | -------------------------------------------------------------------------------- /config/lidar_calibration.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Grid1 9 | - /velo1 10 | - /os01 11 | - /avia1 12 | - /os11 13 | - /TF1 14 | Splitter Ratio: 0.599056601524353 15 | Tree Height: 1106 16 | - Class: rviz/Selection 17 | Name: Selection 18 | - Class: rviz/Tool Properties 19 | Expanded: 20 | - /2D Pose Estimate1 21 | - /2D Nav Goal1 22 | - /Publish Point1 23 | Name: Tool Properties 24 | Splitter Ratio: 0.5886790156364441 25 | - Class: rviz/Views 26 | Expanded: 27 | - /Current View1 28 | Name: Views 29 | Splitter Ratio: 0.5 30 | - Class: rviz/Time 31 | Experimental: false 32 | Name: Time 33 | SyncMode: 0 34 | SyncSource: os0 35 | Preferences: 36 | PromptSaveOnExit: true 37 | Toolbars: 38 | toolButtonStyle: 2 39 | Visualization Manager: 40 | Class: "" 41 | Displays: 42 | - Alpha: 0.5 43 | Cell Size: 1 44 | Class: rviz/Grid 45 | Color: 160; 160; 164 46 | Enabled: true 47 | Line Style: 48 | Line Width: 0.029999999329447746 49 | Value: Lines 50 | Name: Grid 51 | Normal Cell Count: 0 52 | Offset: 53 | X: 0 54 | Y: 0 55 | Z: 0 56 | Plane: XY 57 | Plane Cell Count: 10 58 | Reference Frame: 59 | Value: true 60 | - Alpha: 1 61 | Autocompute Intensity Bounds: true 62 | Autocompute Value Bounds: 63 | Max Value: 22.239625930786133 64 | Min Value: -34.454681396484375 65 | Value: true 66 | Axis: Z 67 | Channel Name: intensity 68 | Class: rviz/PointCloud2 69 | Color: 173; 127; 168 70 | Color Transformer: FlatColor 71 | Decay Time: 0 72 | Enabled: true 73 | Invert Rainbow: false 74 | Max Color: 255; 255; 255 75 | Max Intensity: 255 76 | Min Color: 0; 0; 0 77 | Min Intensity: 0 78 | Name: velo 79 | Position Transformer: XYZ 80 | Queue Size: 10 81 | Selectable: true 82 | Size (Pixels): 3 83 | Size (m): 0.029999999329447746 84 | Style: Flat Squares 85 | Topic: /a_velo 86 | Unreliable: false 87 | Use Fixed Frame: true 88 | Use rainbow: true 89 | Value: true 90 | - Alpha: 1 91 | Autocompute Intensity Bounds: true 92 | Autocompute Value Bounds: 93 | Max Value: 10 94 | Min Value: -10 95 | Value: true 96 | Axis: Z 97 | Channel Name: intensity 98 | Class: rviz/PointCloud2 99 | Color: 233; 185; 110 100 | Color Transformer: FlatColor 101 | Decay Time: 0 102 | Enabled: true 103 | Invert Rainbow: false 104 | Max Color: 255; 255; 255 105 | Max Intensity: 2342 106 | Min Color: 0; 0; 0 107 | Min Intensity: 0 108 | Name: os0 109 | Position Transformer: XYZ 110 | Queue Size: 10 111 | Selectable: true 112 | Size (Pixels): 3 113 | Size (m): 0.019999999552965164 114 | Style: Flat Squares 115 | Topic: /a_os0 116 | Unreliable: false 117 | Use Fixed Frame: true 118 | Use rainbow: true 119 | Value: true 120 | - Alpha: 1 121 | Autocompute Intensity Bounds: true 122 | Autocompute Value Bounds: 123 | Max Value: 10 124 | Min Value: -10 125 | Value: true 126 | Axis: Z 127 | Channel Name: intensity 128 | Class: rviz/PointCloud2 129 | Color: 78; 154; 6 130 | Color Transformer: FlatColor 131 | Decay Time: 1 132 | Enabled: true 133 | Invert Rainbow: false 134 | Max Color: 255; 255; 255 135 | Max Intensity: 205 136 | Min Color: 0; 0; 0 137 | Min Intensity: 0 138 | Name: avia 139 | Position Transformer: XYZ 140 | Queue Size: 10 141 | Selectable: true 142 | Size (Pixels): 3 143 | Size (m): 0.019999999552965164 144 | Style: Flat Squares 145 | Topic: /a_avia 146 | Unreliable: false 147 | Use Fixed Frame: true 148 | Use rainbow: true 149 | Value: true 150 | - Alpha: 1 151 | Autocompute Intensity Bounds: true 152 | Autocompute Value Bounds: 153 | Max Value: 10 154 | Min Value: -10 155 | Value: true 156 | Axis: Z 157 | Channel Name: intensity 158 | Class: rviz/PointCloud2 159 | Color: 239; 41; 41 160 | Color Transformer: FlatColor 161 | Decay Time: 1 162 | Enabled: true 163 | Invert Rainbow: false 164 | Max Color: 255; 255; 255 165 | Max Intensity: 181 166 | Min Color: 0; 0; 0 167 | Min Intensity: 0 168 | Name: hori 169 | Position Transformer: XYZ 170 | Queue Size: 10 171 | Selectable: true 172 | Size (Pixels): 3 173 | Size (m): 0.019999999552965164 174 | Style: Flat Squares 175 | Topic: /a_horizon 176 | Unreliable: false 177 | Use Fixed Frame: true 178 | Use rainbow: true 179 | Value: true 180 | - Alpha: 0 181 | Autocompute Intensity Bounds: true 182 | Autocompute Value Bounds: 183 | Max Value: 10 184 | Min Value: -10 185 | Value: true 186 | Axis: Z 187 | Channel Name: intensity 188 | Class: rviz/PointCloud2 189 | Color: 114; 159; 207 190 | Color Transformer: FlatColor 191 | Decay Time: 0 192 | Enabled: true 193 | Invert Rainbow: false 194 | Max Color: 255; 255; 255 195 | Max Intensity: 2904 196 | Min Color: 0; 0; 0 197 | Min Intensity: 0 198 | Name: os1 199 | Position Transformer: XYZ 200 | Queue Size: 10 201 | Selectable: true 202 | Size (Pixels): 3 203 | Size (m): 0.009999999776482582 204 | Style: Flat Squares 205 | Topic: /a_os1 206 | Unreliable: false 207 | Use Fixed Frame: true 208 | Use rainbow: true 209 | Value: true 210 | - Class: rviz/TF 211 | Enabled: true 212 | Frame Timeout: 15 213 | Frames: 214 | All Enabled: true 215 | Marker Scale: 1 216 | Name: TF 217 | Show Arrows: true 218 | Show Axes: true 219 | Show Names: true 220 | Tree: 221 | {} 222 | Update Interval: 0 223 | Value: true 224 | - Alpha: 1 225 | Axes Length: 1 226 | Axes Radius: 0.10000000149011612 227 | Class: rviz/Pose 228 | Color: 255; 25; 0 229 | Enabled: false 230 | Head Length: 0.30000001192092896 231 | Head Radius: 0.10000000149011612 232 | Name: Pose 233 | Shaft Length: 1 234 | Shaft Radius: 0.05000000074505806 235 | Shape: Arrow 236 | Topic: /vrpn_client_node/UWBTest/pose 237 | Unreliable: false 238 | Value: false 239 | - Class: rviz/Axes 240 | Enabled: false 241 | Length: 1 242 | Name: Axes 243 | Radius: 0.10000000149011612 244 | Reference Frame: 245 | Value: false 246 | - Class: rviz/Image 247 | Enabled: false 248 | Image Topic: /cam_1/depth/image_rect_raw 249 | Max Value: 1 250 | Median window: 5 251 | Min Value: 0 252 | Name: Image 253 | Normalize Range: true 254 | Queue Size: 2 255 | Transport Hint: raw 256 | Unreliable: false 257 | Value: false 258 | - Class: rviz/Image 259 | Enabled: false 260 | Image Topic: /cam_1/color/image_raw 261 | Max Value: 1 262 | Median window: 5 263 | Min Value: 0 264 | Name: Image 265 | Normalize Range: true 266 | Queue Size: 2 267 | Transport Hint: raw 268 | Unreliable: false 269 | Value: false 270 | Enabled: true 271 | Global Options: 272 | Background Color: 48; 48; 48 273 | Default Light: true 274 | Fixed Frame: base_link 275 | Frame Rate: 30 276 | Name: root 277 | Tools: 278 | - Class: rviz/Interact 279 | Hide Inactive Objects: true 280 | - Class: rviz/MoveCamera 281 | - Class: rviz/Select 282 | - Class: rviz/FocusCamera 283 | - Class: rviz/Measure 284 | - Class: rviz/SetInitialPose 285 | Theta std deviation: 0.2617993950843811 286 | Topic: /initialpose 287 | X std deviation: 0.5 288 | Y std deviation: 0.5 289 | - Class: rviz/SetGoal 290 | Topic: /move_base_simple/goal 291 | - Class: rviz/PublishPoint 292 | Single click: true 293 | Topic: /clicked_point 294 | Value: true 295 | Views: 296 | Current: 297 | Class: rviz/Orbit 298 | Distance: 16.33476448059082 299 | Enable Stereo Rendering: 300 | Stereo Eye Separation: 0.05999999865889549 301 | Stereo Focal Distance: 1 302 | Swap Stereo Eyes: false 303 | Value: false 304 | Focal Point: 305 | X: 1.7753968238830566 306 | Y: -0.6641759276390076 307 | Z: 0.014890879392623901 308 | Focal Shape Fixed Size: true 309 | Focal Shape Size: 0.05000000074505806 310 | Invert Z Axis: false 311 | Name: Current View 312 | Near Clip Distance: 0.009999999776482582 313 | Pitch: 0.429798424243927 314 | Target Frame: 315 | Value: Orbit (rviz) 316 | Yaw: 3.2685723304748535 317 | Saved: ~ 318 | Window Geometry: 319 | Displays: 320 | collapsed: false 321 | Height: 1403 322 | Hide Left Dock: false 323 | Hide Right Dock: true 324 | Image: 325 | collapsed: false 326 | QMainWindow State: 000000ff00000000fd0000000400000000000001fd000004ddfc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004dd000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065000000010c000002960000001600fffffffb0000000a0049006d00610067006500000001f3000003300000001600ffffff000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009cf0000003efc0100000002fb0000000800540069006d00650100000000000009cf000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000007cc000004dd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 327 | Selection: 328 | collapsed: false 329 | Time: 330 | collapsed: false 331 | Tool Properties: 332 | collapsed: false 333 | Views: 334 | collapsed: true 335 | Width: 2511 336 | X: 2097 337 | Y: 0 338 | -------------------------------------------------------------------------------- /config/sensors_suite.urdf: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /doc/setup.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TIERS/multi_lidar_multi_uav_dataset/a9c64d91b63f684b74f1ea310455674dafa282bc/doc/setup.png -------------------------------------------------------------------------------- /launch/lidars_extrinsic_computation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | multi_lidar_multi_uav_dataset 4 | 1.0.0 5 | The multi LiDAR multi UAV tracking dataset package 6 | 7 | 8 | Iacopo Catalano 9 | 10 | 11 | MIT 12 | 13 | catkin 14 | 15 | roscpp 16 | rospy 17 | geometry_msgs 18 | sensor_msgs 19 | std_msgs 20 | pcl_conversions 21 | pcl_ros 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /src/avia_format_converter.cpp: -------------------------------------------------------------------------------- 1 | #include "livox_ros_driver/CustomMsg.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | typedef pcl::PointXYZI PointType; 9 | 10 | #define PI 3.1415926535 11 | 12 | using namespace std; 13 | 14 | ros::Publisher pub_ros_points; 15 | string frame_id = "avia_frame"; 16 | 17 | 18 | void livoxLidarHandler(const livox_ros_driver::CustomMsgConstPtr& livox_msg_in) { 19 | pcl::PointCloud pcl_in; 20 | for (unsigned int i = 0; i < livox_msg_in->point_num; ++i) { 21 | PointType pt; 22 | pt.x = livox_msg_in->points[i].x; 23 | pt.y = livox_msg_in->points[i].y; 24 | pt.z = livox_msg_in->points[i].z; 25 | pt.intensity = livox_msg_in->points[i].reflectivity; 26 | pcl_in.push_back(pt); 27 | } 28 | 29 | ros::Time timestamp(livox_msg_in->header.stamp.toSec()); 30 | 31 | sensor_msgs::PointCloud2 pcl_ros_msg; 32 | pcl::toROSMsg(pcl_in, pcl_ros_msg); 33 | pcl_ros_msg.header.stamp = timestamp; 34 | pcl_ros_msg.header.frame_id = frame_id; 35 | 36 | pub_ros_points.publish(pcl_ros_msg); 37 | } 38 | 39 | int main(int argc, char** argv) { 40 | ros::init(argc, argv, "livox_format_converter"); 41 | ros::NodeHandle nh; 42 | 43 | ros::Subscriber sub_livox_lidar = nh.subscribe("/avia/livox/lidar", 100, livoxLidarHandler); 44 | pub_ros_points = nh.advertise("/avia_points", 100); 45 | 46 | ros::spin(); 47 | } 48 | -------------------------------------------------------------------------------- /src/lidars_extrinsic_computation.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | // Include ROS library 4 | #include 5 | 6 | // Include TF for transforms 7 | #include 8 | #include 9 | #include 10 | 11 | // Include point clouds types 12 | #include 13 | 14 | // Include PCL library 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | typedef pcl::PointXYZ PointType; 32 | 33 | class ExtrinsicCalibrator{ 34 | private: 35 | ros::NodeHandle nh; 36 | 37 | ros::Subscriber sub_avia; 38 | ros::Subscriber sub_mid360; 39 | ros::Subscriber sub_os; 40 | ros::Subscriber sub_camera; 41 | 42 | ros::Publisher pub_avia; 43 | ros::Publisher pub_mid360; 44 | ros::Publisher pub_os; 45 | ros::Publisher pub_camera; 46 | 47 | tf::TransformBroadcaster tf_br; 48 | 49 | // Avia tf 50 | int avia_integrate_frames = 5; 51 | pcl::PointCloud avia_igcloud; 52 | Eigen::Matrix4f avia_tf_matrix; 53 | bool avia_tf_initd = false; 54 | 55 | // Mid-360 TF 56 | int mid360_integrate_frames = 5; 57 | pcl::PointCloud mid360_igcloud; 58 | Eigen::Matrix4f mid360_tf_matrix; 59 | bool mid360_tf_initd = false; 60 | 61 | // Camera TF 62 | int camera_integrate_frames = 2; 63 | Eigen::Matrix4f camera_tf_matrix; 64 | bool camera_tf_initd = false; 65 | pcl::PointCloud camera_igcloud; 66 | 67 | // Ouster TF 68 | Eigen::Matrix4f os_tf_matrix; 69 | bool os_tf_initd = false; 70 | pcl::PointCloud os_cloud; 71 | bool os_received = false; 72 | 73 | // Cropbox filter to limit points distance 74 | pcl::CropBox cropBoxFilter; 75 | Eigen::Vector4f min_box_filter; 76 | Eigen::Vector4f max_box_filter; 77 | 78 | // Initial guess point cloud registration 79 | Eigen::AngleAxisf avia_init_rot_x; 80 | Eigen::AngleAxisf avia_init_rot_y; 81 | Eigen::AngleAxisf avia_init_rot_z; 82 | Eigen::Translation3f avia_init_translation; 83 | Eigen::Matrix4f avia_init_tf; 84 | 85 | Eigen::AngleAxisf mid360_init_rot_x; 86 | Eigen::AngleAxisf mid360_init_rot_y; 87 | Eigen::AngleAxisf mid360_init_rot_z; 88 | Eigen::Translation3f mid360_init_translation; 89 | Eigen::Matrix4f mid360_init_tf; 90 | 91 | Eigen::AngleAxisf camera_init_rot_x; 92 | Eigen::AngleAxisf camera_init_rot_y; 93 | Eigen::AngleAxisf camera_init_rot_z; 94 | Eigen::Translation3f camera_init_translation; 95 | Eigen::Matrix4f camera_init_tf; 96 | 97 | 98 | public: 99 | ExtrinsicCalibrator(){ 100 | sub_avia = nh.subscribe("/avia_points", 1000, &ExtrinsicCalibrator::aviaCallback, this); 101 | sub_mid360 = nh.subscribe("/mid360_points", 1000, &ExtrinsicCalibrator::mid360Callback, this); 102 | sub_os = nh.subscribe("/ouster/points", 1000, &ExtrinsicCalibrator::ousterCallback, this); 103 | sub_camera = nh.subscribe("/camera/depth/color/points", 1000, &ExtrinsicCalibrator::cameraCallback, this); 104 | 105 | pub_avia = nh.advertise("/a_avia", 1); 106 | pub_mid360 = nh.advertise("/a_mid360", 1); 107 | pub_os = nh.advertise("/a_os", 1); 108 | pub_camera = nh.advertise("/a_camera", 1); 109 | 110 | // Set min and max box filter 111 | min_box_filter = Eigen::Vector4f(-50.0, -50.0, -50.0, 1.0); 112 | max_box_filter = Eigen::Vector4f(50.0, 50.0, 50.0, 1.0); 113 | 114 | cropBoxFilter.setMin(min_box_filter); 115 | cropBoxFilter.setMax(max_box_filter); 116 | 117 | // Initialize guess estimate matrices 118 | avia_init_rot_x = Eigen::AngleAxisf( 0.0 , Eigen::Vector3f::UnitX()); 119 | avia_init_rot_y = Eigen::AngleAxisf( 0.0 , Eigen::Vector3f::UnitX()); 120 | avia_init_rot_z = Eigen::AngleAxisf( 0.0 , Eigen::Vector3f::UnitX()); 121 | avia_init_translation = Eigen::Translation3f(0.0,0.0,0.0); 122 | avia_init_tf = (avia_init_translation * avia_init_rot_z * avia_init_rot_y * avia_init_rot_x).matrix(); 123 | 124 | mid360_init_rot_x = Eigen::AngleAxisf( 0.0 , Eigen::Vector3f::UnitX()); 125 | mid360_init_rot_y = Eigen::AngleAxisf( 0.0 , Eigen::Vector3f::UnitY()); 126 | mid360_init_rot_z = Eigen::AngleAxisf( 0.0 , Eigen::Vector3f::UnitZ()); 127 | mid360_init_translation = Eigen::Translation3f(0.0, 0.0, 0.0); 128 | mid360_init_tf = (mid360_init_translation * mid360_init_rot_z * mid360_init_rot_y * mid360_init_rot_x).matrix(); 129 | 130 | camera_init_rot_x = Eigen::AngleAxisf( -1.57295 , Eigen::Vector3f::UnitX()); 131 | camera_init_rot_y = Eigen::AngleAxisf( 0.0 , Eigen::Vector3f::UnitY()); 132 | camera_init_rot_z = Eigen::AngleAxisf( -1.57295 , Eigen::Vector3f::UnitZ()); 133 | camera_init_translation = Eigen::Translation3f(0.0, 0.0, 0.0); 134 | camera_init_tf = (camera_init_translation * camera_init_rot_z * camera_init_rot_y * camera_init_rot_x).matrix(); 135 | }; 136 | 137 | 138 | ~ExtrinsicCalibrator(){}; 139 | 140 | void aviaCallback(const sensor_msgs::PointCloud2ConstPtr& pointCloudIn) 141 | { 142 | if(!os_received) return; 143 | pcl::PointCloud full_cloud_in; 144 | pcl::fromROSMsg(*pointCloudIn, full_cloud_in); 145 | pcl_conversions::toPCL(pointCloudIn->header, full_cloud_in.header); 146 | 147 | if(avia_integrate_frames > 0) 148 | { 149 | avia_igcloud += full_cloud_in; 150 | avia_integrate_frames--; 151 | return; 152 | }else 153 | { 154 | if(!avia_tf_initd) 155 | { 156 | 157 | ROS_INFO("\n\n\n Calibrate Avia ..."); 158 | calibratePointCloud(avia_igcloud.makeShared(), os_cloud.makeShared(), avia_tf_matrix, avia_init_tf); 159 | Eigen::Matrix3f rot_matrix = avia_tf_matrix.block(0,0,3,3); 160 | Eigen::Vector3f trans_vector = avia_tf_matrix.block(0,3,3,1); 161 | 162 | std::cout << "Avia -> base_link " << trans_vector.transpose() 163 | << " " << rot_matrix.eulerAngles(2,1,0).transpose() << " /" << "avia_frame" 164 | << " /" << "base_link" << " 10" << std::endl; 165 | 166 | // publish result 167 | pcl::PointCloud out_cloud; 168 | pcl::transformPointCloud (avia_igcloud , out_cloud, avia_tf_matrix); 169 | 170 | sensor_msgs::PointCloud2 avia_msg; 171 | pcl::toROSMsg(out_cloud, avia_msg); 172 | avia_msg.header.stamp = ros::Time::now(); 173 | avia_msg.header.frame_id = "base_link"; 174 | pub_avia.publish(avia_msg); 175 | 176 | avia_tf_initd = true; 177 | }else 178 | { 179 | pcl::PointCloud out_cloud; 180 | pcl::transformPointCloud (full_cloud_in , out_cloud, avia_tf_matrix); 181 | 182 | sensor_msgs::PointCloud2 avia_msg; 183 | pcl::toROSMsg(out_cloud, avia_msg); 184 | avia_msg.header.stamp = ros::Time::now(); 185 | avia_msg.header.frame_id = "base_link"; 186 | pub_avia.publish(avia_msg); 187 | } 188 | } 189 | } 190 | 191 | void mid360Callback(const sensor_msgs::PointCloud2ConstPtr& pointCloudIn) 192 | { 193 | if(!os_received) return; 194 | pcl::PointCloud full_cloud_in; 195 | pcl::fromROSMsg(*pointCloudIn, full_cloud_in); 196 | pcl_conversions::toPCL(pointCloudIn->header, full_cloud_in.header); 197 | 198 | if(mid360_integrate_frames > 0) 199 | { 200 | mid360_igcloud += full_cloud_in; 201 | mid360_integrate_frames--; 202 | return; 203 | }else 204 | { 205 | if(!mid360_tf_initd) 206 | { 207 | 208 | ROS_INFO("\n\n\n Calibrate Mid-360 ..."); 209 | calibratePointCloud(mid360_igcloud.makeShared(), os_cloud.makeShared(), mid360_tf_matrix, mid360_init_tf); 210 | Eigen::Matrix3f rot_matrix = mid360_tf_matrix.block(0,0,3,3); 211 | Eigen::Vector3f trans_vector = mid360_tf_matrix.block(0,3,3,1); 212 | 213 | std::cout << "Mid360 -> base_link " << trans_vector.transpose() 214 | << " " << rot_matrix.eulerAngles(2,1,0).transpose() << " /" << "mid360_frame" 215 | << " /" << "base_link" << " 10" << std::endl; 216 | 217 | // publish result 218 | pcl::PointCloud out_cloud; 219 | pcl::transformPointCloud (mid360_igcloud , out_cloud, mid360_tf_matrix); 220 | 221 | sensor_msgs::PointCloud2 mid360_msg; 222 | pcl::toROSMsg(out_cloud, mid360_msg); 223 | mid360_msg.header.stamp = ros::Time::now(); 224 | mid360_msg.header.frame_id = "base_link"; 225 | pub_mid360.publish(mid360_msg); 226 | 227 | mid360_tf_initd = true; 228 | }else 229 | { 230 | pcl::PointCloud out_cloud; 231 | pcl::transformPointCloud (full_cloud_in , out_cloud, mid360_tf_matrix); 232 | 233 | sensor_msgs::PointCloud2 mid360_msg; 234 | pcl::toROSMsg(out_cloud, mid360_msg); 235 | mid360_msg.header.stamp = ros::Time::now(); 236 | mid360_msg.header.frame_id = "base_link"; 237 | pub_mid360.publish(mid360_msg); 238 | } 239 | } 240 | } 241 | 242 | void cameraCallback(const sensor_msgs::PointCloud2ConstPtr& pointCloudIn) 243 | { 244 | pcl::PointCloud full_cloud_in; 245 | pcl::fromROSMsg(*pointCloudIn, full_cloud_in); 246 | pcl_conversions::toPCL(pointCloudIn->header, full_cloud_in.header); 247 | 248 | if(camera_integrate_frames > 0) 249 | { 250 | camera_igcloud += full_cloud_in; 251 | camera_integrate_frames--; 252 | return; 253 | }else 254 | { 255 | if(!camera_tf_initd) 256 | { 257 | 258 | ROS_INFO("\n\n\n Calibrate Camera ..."); 259 | calibratePointCloud(camera_igcloud.makeShared(), os_cloud.makeShared(), camera_tf_matrix, camera_init_tf); 260 | Eigen::Matrix3f rot_matrix = camera_tf_matrix.block(0,0,3,3); 261 | Eigen::Vector3f trans_vector = camera_tf_matrix.block(0,3,3,1); 262 | 263 | std::cout << "Camera -> base_link " << trans_vector.transpose() 264 | << " " << rot_matrix.eulerAngles(2,1,0).transpose() << " /" << "camera_depth_optical_frame" 265 | << " /" << "base_link" << " 10" << std::endl; 266 | 267 | // publish result 268 | pcl::PointCloud out_cloud; 269 | pcl::transformPointCloud (camera_igcloud , out_cloud, camera_tf_matrix); 270 | 271 | sensor_msgs::PointCloud2 camera_msg; 272 | pcl::toROSMsg(out_cloud, camera_msg); 273 | camera_msg.header.stamp = ros::Time::now(); 274 | camera_msg.header.frame_id = "base_link"; 275 | pub_camera.publish(camera_msg); 276 | 277 | camera_tf_initd = true; 278 | }else 279 | { 280 | pcl::PointCloud out_cloud; 281 | pcl::transformPointCloud (full_cloud_in , out_cloud, camera_tf_matrix); 282 | 283 | sensor_msgs::PointCloud2 camera_msg; 284 | pcl::toROSMsg(out_cloud, camera_msg); 285 | camera_msg.header.stamp = ros::Time::now(); 286 | camera_msg.header.frame_id = "base_link"; 287 | pub_camera.publish(camera_msg); 288 | } 289 | } 290 | 291 | } 292 | 293 | void ousterCallback(const sensor_msgs::PointCloud2ConstPtr& pointCloudIn) 294 | { 295 | pcl::PointCloud full_cloud_in; 296 | pcl::fromROSMsg(*pointCloudIn, full_cloud_in); 297 | pcl_conversions::toPCL(pointCloudIn->header, full_cloud_in.header); 298 | 299 | Eigen::AngleAxisf init_rot_x( 0.0 , Eigen::Vector3f::UnitX()); 300 | Eigen::AngleAxisf init_rot_y( 0.0 , Eigen::Vector3f::UnitY()); 301 | Eigen::AngleAxisf init_rot_z( 0.0 , Eigen::Vector3f::UnitZ()); 302 | Eigen::Translation3f init_trans(0.0,0.0,0.0); 303 | Eigen::Matrix4f init_tf = (init_trans * init_rot_z * init_rot_y * init_rot_x).matrix(); 304 | 305 | Eigen::Matrix3f rot_matrix = init_tf.block(0,0,3,3); 306 | Eigen::Vector3f trans_vector = init_tf.block(0,3,3,1); 307 | 308 | if(!os_tf_initd){ 309 | std::cout << "OS0 -> base_link " << trans_vector.transpose() 310 | << " " << rot_matrix.eulerAngles(2,1,0).transpose() << " /" << "os0_sensor" 311 | << " /" << "base_link" << " 10" << std::endl; 312 | 313 | os_tf_initd = true; 314 | } 315 | 316 | pcl::PointCloud out_cloud; 317 | pcl::transformPointCloud (full_cloud_in , full_cloud_in, init_tf); 318 | 319 | os_cloud.clear(); 320 | os_cloud += full_cloud_in; 321 | os_received = true; 322 | 323 | sensor_msgs::PointCloud2 os_msg; 324 | pcl::toROSMsg(os_cloud, os_msg); 325 | os_msg.header.stamp = ros::Time::now(); 326 | os_msg.header.frame_id = "base_link"; 327 | pub_os.publish(os_msg); 328 | 329 | } 330 | 331 | void calibratePointCloud(pcl::PointCloud::Ptr source_cloud, 332 | pcl::PointCloud::Ptr target_cloud, Eigen::Matrix4f &tf_matrix, Eigen::Matrix4f &tf_init_guess_matrix) 333 | { 334 | std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now(); 335 | std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now(); 336 | std::chrono::duration> time_span = 337 | std::chrono::duration_cast>>(t2 - t1); 338 | 339 | std::cout << "------------checking PCL GICP---------------- "<< std::endl; 340 | int pCount = source_cloud->size(); 341 | 342 | pcl::PointCloud::Ptr source_cloud_downsampled (new pcl::PointCloud ); 343 | pcl::PointCloud::Ptr target_cloud_downsampled (new pcl::PointCloud ); 344 | 345 | // Remove far points 346 | pcl::PointCloud::Ptr source_cloud_filtered (new pcl::PointCloud); 347 | 348 | // Crop point cloud to a max distance of 50 meters 349 | cropBoxFilter.setInputCloud(source_cloud); 350 | cropBoxFilter.filter(*source_cloud_filtered); 351 | 352 | ROS_INFO_STREAM("Source-> Removing far points "<< source_cloud->size() << " "<< source_cloud_filtered->size() ); 353 | 354 | // Remove far points 355 | pcl::PointCloud::Ptr target_cloud_filtered (new pcl::PointCloud); 356 | 357 | cropBoxFilter.setInputCloud(target_cloud); 358 | cropBoxFilter.filter(*target_cloud_filtered); 359 | ROS_INFO_STREAM("Target-> Removing far points "<< target_cloud->size() << " "<< target_cloud_filtered->size() ); 360 | 361 | // Apply voxel grid filter to downsample cloud 362 | pcl::VoxelGrid< PointType> vox; 363 | vox.setLeafSize (0.05f, 0.05f, 0.05f); 364 | 365 | vox.setInputCloud (source_cloud_filtered); 366 | vox.filter(*source_cloud_downsampled); 367 | 368 | vox.setInputCloud (target_cloud_filtered); 369 | vox.filter(*target_cloud_downsampled); 370 | 371 | std::cout << "GICP start .... " << source_cloud_downsampled->size() << " to "<< target_cloud_downsampled->size()<< std::endl; 372 | pcl::GeneralizedIterativeClosestPoint gicp; 373 | gicp.setTransformationEpsilon(0.01); 374 | gicp.setMaxCorrespondenceDistance(10.0); 375 | gicp.setMaximumIterations(100); 376 | gicp.setRANSACIterations(100); 377 | gicp.setInputSource(source_cloud_downsampled); 378 | gicp.setInputTarget(target_cloud_downsampled); 379 | 380 | pcl::PointCloud::Ptr aligned_cloud (new pcl::PointCloud); 381 | 382 | gicp.align(*aligned_cloud, tf_init_guess_matrix); 383 | 384 | std::cout << "has converged: " << gicp.hasConverged() << " score: " << 385 | gicp.getFitnessScore() << std::endl; 386 | 387 | tf_matrix = gicp.getFinalTransformation (); 388 | } 389 | }; 390 | 391 | int 392 | main(int argc, char **argv) 393 | { 394 | ros::init(argc, argv, "Extrinsic Calibration"); 395 | 396 | ExtrinsicCalibrator swo; 397 | ROS_INFO("\033[1;32m---->\033[0m Extrinsic Calibration Started."); 398 | 399 | ros::Rate r(10); 400 | while(ros::ok()){ 401 | 402 | ros::spinOnce(); 403 | r.sleep(); 404 | 405 | } 406 | 407 | 408 | return 0; 409 | } 410 | -------------------------------------------------------------------------------- /src/mid360_format_converter.cpp: -------------------------------------------------------------------------------- 1 | #include "livox_ros_driver/CustomMsg.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | typedef pcl::PointXYZI PointType; 9 | 10 | #define PI 3.1415926535 11 | 12 | using namespace std; 13 | 14 | ros::Publisher pub_ros_points; 15 | string frame_id = "mid360_frame"; 16 | 17 | 18 | void livoxLidarHandler(const livox_ros_driver::CustomMsgConstPtr& livox_msg_in) { 19 | pcl::PointCloud pcl_in; 20 | for (unsigned int i = 0; i < livox_msg_in->point_num; ++i) { 21 | PointType pt; 22 | pt.x = livox_msg_in->points[i].x; 23 | pt.y = livox_msg_in->points[i].y; 24 | pt.z = livox_msg_in->points[i].z; 25 | pt.intensity = livox_msg_in->points[i].reflectivity; 26 | pcl_in.push_back(pt); 27 | } 28 | 29 | ros::Time timestamp(livox_msg_in->header.stamp.toSec()); 30 | 31 | sensor_msgs::PointCloud2 pcl_ros_msg; 32 | pcl::toROSMsg(pcl_in, pcl_ros_msg); 33 | pcl_ros_msg.header.stamp = timestamp; 34 | pcl_ros_msg.header.frame_id = frame_id; 35 | 36 | pub_ros_points.publish(pcl_ros_msg); 37 | } 38 | 39 | int main(int argc, char** argv) { 40 | ros::init(argc, argv, "livox_format_converter"); 41 | ros::NodeHandle nh; 42 | 43 | ros::Subscriber sub_livox_lidar = nh.subscribe("/mid360/livox/lidar", 100, livoxLidarHandler); 44 | pub_ros_points = nh.advertise("/mid360_points", 100); 45 | 46 | ros::spin(); 47 | } 48 | --------------------------------------------------------------------------------