├── .gitignore ├── LICENSE ├── README.md ├── catkin_make.sh ├── install_dependence.sh └── src ├── .clang-format ├── dead_reckoning └── odom_imu │ ├── CMakeLists.txt │ ├── config │ ├── filter.yaml │ ├── odom_imu.rviz │ └── odom_imu.yaml │ ├── include │ └── tools │ │ └── odom_imu_flow.h │ ├── launch │ └── odom_imu.launch │ ├── package.xml │ └── src │ ├── app │ └── odom_imu_node.cc │ └── odom_imu_flow.cc ├── doc ├── ground_truth │ ├── tum_ground_truth.txt │ └── tum_vrs_gps.txt ├── img │ ├── 1.png │ ├── 2.png │ ├── lio-sam-result.png │ └── traj.png └── mapping_results │ └── tum_lio_sam_pose.txt ├── kaist_tool ├── file_player │ ├── CMakeLists.txt │ ├── README.md │ ├── cfg │ │ └── file_player.cfg │ ├── depend_pack.rosinstall │ ├── include │ │ └── file_player │ │ │ ├── color.h │ │ │ └── datathread.h │ ├── launch │ │ └── file_player.launch │ ├── msg │ │ └── tmp │ ├── package.xml │ ├── src │ │ ├── CMakeLists.txt │ │ ├── ROSThread.cpp │ │ ├── ROSThread.h │ │ ├── color_code.h │ │ ├── main.cpp │ │ ├── mainwindow.cpp │ │ ├── mainwindow.h │ │ ├── mainwindow.ui │ │ ├── resources.qrc │ │ └── resources │ │ │ ├── fragmentShader.fsh │ │ │ ├── fragmentShaderColor.fsh │ │ │ ├── fragmentShaderOBJ.fsh │ │ │ ├── vertexShader.vsh │ │ │ ├── vertexShaderColor.vsh │ │ │ └── vertexShaderOBJ.vsh │ └── srv │ │ └── tmp ├── irp_sen_msgs │ ├── CMakeLists.txt │ ├── README.md │ ├── msg │ │ ├── LaserScanArray.msg │ │ ├── altimeter.msg │ │ ├── encoder.msg │ │ ├── fog.msg │ │ ├── fog_3axis.msg │ │ ├── imu.msg │ │ └── vrs.msg │ └── package.xml ├── kaist2bag │ ├── CMakeLists.txt │ ├── LICENSE │ ├── README.md │ ├── config │ │ └── config.yaml │ ├── launch │ │ └── kaist2bag.launch │ ├── package.xml │ ├── scripts │ │ └── mergebag.py │ └── src │ │ ├── altimeter_converter.cpp │ │ ├── altimeter_converter.h │ │ ├── converter.cpp │ │ ├── converter.h │ │ ├── encoder_converter.cpp │ │ ├── encoder_converter.h │ │ ├── fog_converter.cpp │ │ ├── fog_converter.h │ │ ├── gps_converter.cpp │ │ ├── gps_converter.h │ │ ├── imu_converter.cpp │ │ ├── imu_converter.h │ │ ├── main.cpp │ │ ├── sick_converter.cpp │ │ ├── sick_converter.h │ │ ├── stereo_converter.cpp │ │ ├── stereo_converter.h │ │ ├── velodyne_converter.cpp │ │ ├── velodyne_converter.h │ │ ├── vrs_converter.cpp │ │ └── vrs_converter.h └── kaist_description │ ├── CMakeLists.txt │ ├── launch │ └── kaist_description.launch │ ├── package.xml │ ├── rviz │ └── kaist.rviz │ └── urdf │ └── urban08.urdf ├── lidar_location ├── CMakeLists.txt ├── README.md ├── cmake │ └── FindGeographicLib.cmake ├── config │ └── front_end │ │ └── config.yaml ├── include │ └── lidar_localization │ │ ├── front_end │ │ ├── front_end.hpp │ │ └── front_end_flow.hpp │ │ ├── models │ │ ├── cloud_filter │ │ │ ├── cloud_filter_interface.hpp │ │ │ └── voxel_filter.hpp │ │ └── registration │ │ │ ├── ndt_registration.hpp │ │ │ └── registration_interface.hpp │ │ ├── publisher │ │ ├── cloud_publisher.hpp │ │ └── odometry_publisher.hpp │ │ ├── sensor_data │ │ ├── cloud_data.hpp │ │ ├── gnss_data.hpp │ │ └── imu_data.hpp │ │ └── subscriber │ │ ├── cloud_subscriber.hpp │ │ ├── gnss_subscriber.hpp │ │ └── imu_subscriber.hpp ├── launch │ └── front_end.launch ├── package.xml ├── rviz │ └── front_end.rviz ├── src │ ├── front_end │ │ ├── front_end.cpp │ │ └── front_end_flow.cpp │ ├── front_end_node.cpp │ ├── models │ │ ├── cloud_filter │ │ │ └── voxel_filter.cpp │ │ └── registration │ │ │ └── ndt_registration.cpp │ ├── publisher │ │ ├── cloud_publisher.cpp │ │ └── odometry_publisher.cpp │ ├── sensor_data │ │ └── gnss_data.cpp │ └── subscriber │ │ ├── cloud_subscriber.cpp │ │ ├── gnss_subscriber.cpp │ │ └── imu_subscriber.cpp └── srv │ └── saveMap.srv ├── lidar_odometry ├── CMakeLists.txt ├── include │ ├── feature_extraction.h │ ├── scan_to_map_odometry.h │ └── scan_to_scan_odometry.h ├── launch │ └── run.launch ├── package.xml ├── rviz_cfg │ └── rviz.rviz └── src │ ├── feature_extraction.cpp │ ├── scan_to_map_odometry.cpp │ └── scan_to_scan_odometry.cpp ├── mapping └── LIO-SAM │ ├── CMakeLists.txt │ ├── Dockerfile │ ├── LICENSE │ ├── README.md │ ├── config │ └── params.yaml │ ├── include │ ├── gpsTools.hpp │ └── utility.h │ ├── launch │ ├── include │ │ ├── config │ │ │ ├── robot.urdf.xacro │ │ │ ├── rviz.rviz │ │ │ └── urban08.urdf.xacro │ │ ├── module_loam.launch │ │ ├── module_navsat.launch │ │ ├── module_robot_state_publisher.launch │ │ ├── module_rviz.launch │ │ └── rosconsole │ │ │ ├── rosconsole_error.conf │ │ │ ├── rosconsole_info.conf │ │ │ └── rosconsole_warn.conf │ └── run.launch │ ├── msg │ └── cloud_info.msg │ ├── package.xml │ ├── src │ ├── featureExtraction.cpp │ ├── imageProjection.cpp │ ├── imuPreintegration.cpp │ ├── mapOptmization.cpp │ └── simpleGpsOdom.cpp │ └── srv │ └── save_map.srv └── scripts ├── evo.sh └── kaist2evo.py /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | devel/ 3 | 4 | *~ 5 | src/CMakeLists.txt 6 | .catkin_workspace 7 | 8 | .vscode 9 | */.vscode 10 | 11 | *.bag 12 | 13 | data 14 | src/data/* 15 | src/lidar_location/slam_data/* 16 | map -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Learning_localization_from_scratch_ws 2 | 3 | ## 项目的目的 4 | 5 | 通过自己手动实现定位相关的功能，来达到学习定位的各个算法的目的。 6 | 7 | 文章将在 **公众号**, **CSDN**, **知乎** 三个途径进行同步更新. 8 | 9 | - **公众号名称**: 从零开始搭SLAM 10 | - **CSDN专栏地址**: [https://blog.csdn.net/tiancailx/category_11916472.html?spm=1001.2014.3001.5482](https://blog.csdn.net/tiancailx/category_11916472.html?spm=1001.2014.3001.5482) 11 | - **知乎专栏地址**: [https://www.zhihu.com/column/c_1530947688660631552](https://www.zhihu.com/column/c_1530947688660631552) 12 | 13 | ## 依赖库 14 | 15 | 代码是处于更新状态的，所有需要安装的依赖库都会写在 install_dependence.sh 脚本中，如果发现编译时由于依赖库报错，按照如下命令安装下依赖库即可. 16 | 17 | ```bash 18 | cd /path_to_workspace/Learning_localization_from_scratch_ws 19 | chmod +x install_dependence.sh 20 | ./install_dependence.sh 21 | ``` 22 | 23 | ## 运行环境 24 | - ubuntu 18.04 25 | - ros melodic 26 | - gtsam 4.0.2 27 | 28 | ## 测试数据 29 | 目前, 所用的数据集是Kaist数据集，其官方链接为 30 | [Complex Urban Dataset](https://sites.google.com/view/complex-urban-dataset/download-lidar#h.sa42osfdnwst) 31 | 32 | 数据集的使用方式见：[kaist数据集体验](https://blog.csdn.net/tiancailx/article/details/125782157?spm=1001.2014.3001.5501) 33 | 34 | 我将所有的数据集汇总到一个在线表格中，地址如下 35 | [公众号数据集汇总](https://docs.qq.com/sheet/DVElRQVNlY0tHU01I?tab=BB08J2) 36 | 37 | ## 提交代码 38 | 39 | - 本仓库已带有clang-format文件，请在IDE中使用格式化之后再进行提交; 如不使用IDE，也可以手动使用clang-format后再提交，具体命令为 40 | `find . -regex '.*\.\(cpp\|hpp\|c\|h\)' -exec clang-format -style=file -i {} \;` 41 | 42 | ## TODO 43 | 44 | - [working] xx 45 | - [done] xx 46 | - [TODO] xx 47 | --- 48 | 49 | 以下为每篇文章对应的节点如何运行，以及对应节点的功能简介 50 | 51 | ## 基于LIO-SAM的建图 52 | 53 | ### 数据集 54 | [Complex Urban Dataset](https://sites.google.com/view/complex-urban-dataset/download-lidar#h.sa42osfdnwst) 55 | 56 | ### 转录bag 57 | Kaist数据集的使用也可以通过工具将数据转成bag从而进行使用。使用的工具是kaist2bag，这个工具已经在工程的kaist_tool文件夹里了，可以直接使用。 58 | 59 | 具体的使用方法为： 60 | - 先修改kaist2bag/config/config.yaml, 将dataset与save_to的文件夹设置好 61 | - 执行命令 `roslaunch kaist2bag kaist2bag.launch`, 即可将每种传感器数据写成独立的bag 62 | - 再执行命令 `rosrun kaist2bag mergebag.py merged.bag ... `, 即可将多个bag合成一个bag 63 | 64 | ### 运行 65 | 数据就使用之前转录好的bag，将bag的地址填写在 LIO-SAM/launch/run.launch 中，通过命令 `roslaunch lio_sam run.launch`, 开始建图。 66 | 67 | ### 建图效果展示 68 | 69 | ![lio-sam-result](src/doc/img/lio-sam-result.png) 70 | 71 | ### 代码改动 72 | - 将左右点云转换到IMU系下并合并点云，配置文件中添加Lidar到IMU的外参 73 | - 替换原始EKF节点，增加原始gps数据转Odometry节点，发布因子图需要的gps数据类型 74 | - 去除原代码中ring和time字段的检查，在数据集转rosbag过程中添加此字段 75 | - 代码中一些参数的改动，IMU频率，是否启用gps，IMU内参 76 | - 增加轮式里程计约束，可以在配置文件中选择是否启用 77 | - 在launch直接播放rosbag，无需手动播放rosbag 78 | - 可选参数配置如下 79 | 80 | ``` 81 | # GPS Settings 82 | useGPS: true 83 | 84 | # LidarOdom Settings 85 | useLidarOdom: false 86 | 87 | # WheelOdom Settings 88 | useWheelOdom: true 89 | ``` 90 | 91 | ## 建图精度评估 evo(kitti/tum) 92 | 93 | ### 第一步 将kaist的真值转成evo能够读取的格式 94 | 95 | 这一步需要安装依赖项 96 | `pip3 install numpy scipy` 97 | 已经填写在 install_dependence.sh 文件中。 98 | 99 | 执行步骤为 100 | ``` 101 | ./src/scripts/kaist2evo.py -p /media/trunk/Trunk/0-LX/Kaist/Urban08 102 | ``` 103 | -p 后边接的是数据集的文件夹，之后可以加 -o 加输出轨迹文件的地址。 104 | 105 | 执行之后会在Urban08文件夹下生成2个txt文件，分别是 tum_ground_truth.txt 与 tum_vrs_gps.txt。 106 | 107 | - tum_ground_truth.txt 是将 global_pose.csv 转成的tum格式的轨迹文件 108 | - tum_vrs_gps.txt 是将 sensor_data/vrs_gps.csv 转成的tum格式的轨迹文件 109 | 110 | 当然，这个我已经转好了，放在了 src/doc/ground_truth 文件夹下，不再需要自己转了。 111 | 112 | #### 参考文章 113 | [KAIST数据集参数](https://blog.csdn.net/Iqun_LAN/article/details/106445884) 114 | 115 | ### 第二步 输出 lio-sam 的轨迹文件 116 | 117 | `roslaunch lio_sam run.launch ` 执行完建图之后，会在 src/doc/mapping_results 文件夹下生成轨迹文件 118 | 119 | 120 | ### 第三步 执行评估 121 | 122 | 然后将 src/doc/ground_truth/tum_ground_truth.txt 与 src/doc/mapping_results/tum_lio_sam_pose.txt 这两个文件，分别填在 src/scripts/evo.sh 的 txt1 与 txt2 中 123 | 124 | 在执行 `src/scripts/evo.sh` 即可绘制轨迹图，ape图，rpe图。 125 | 126 | ![traj](src/doc/img/traj.png) 127 | 128 | ## 基于点面匹配的激光里程计 129 | 130 | #### 基本原理 131 | 132 | 代码文件在./lidar_odometry 133 | 基于点面匹配，借鉴了LIO-SAM中的匹配方式，不同的是LIO-SAM采用的是帧与附近的帧组成的局部地图匹配，本方案借鉴了A-LOAM的思想，提取面特征，先用前后两帧的面特征点构建点面残差，得到一个粗略的帧间里程计，然后将多帧累计，组成地图与当前帧匹配，修正里程计坐标系和世界坐标系之间的转换关系，得到一个更加精确的里程计。 134 | LIO-SAM角点和平面匹配的残差构建和雅克比推导可以参考：https://zhuanlan.zhihu.com/p/548579394 135 | 136 | #### 代码改动 137 | 138 | - 将LIO-SAM中点面匹配部分拆分为三个节点，特征提取、scan_to_scan匹配、scan_to_map匹配 139 | 140 | - 由于角点较少，只提取平面点，简化特征点提取条件，低于阈值即为平面点 141 | - 采用ceres自动求导的方式，代替LIO-SAM中手动求导的方式，只需构建点面残差，省去了求雅可比的过程 142 | - 发布scan_to_scan的里程计和scan_to_map的里程计 143 | 144 | #### 测试效果 145 | 146 | ``` 147 | roslaunch lidar_odometry run.launch 148 | ``` 149 | 150 | ![2](src/doc/img/2.png) -------------------------------------------------------------------------------- /catkin_make.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | catkin_make 4 | -------------------------------------------------------------------------------- /install_dependence.sh: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | 3 | # 安装将kaist的真值转成tum格式轨迹python脚本的依赖 4 | pip3 install numpy scipy -------------------------------------------------------------------------------- /src/.clang-format: -------------------------------------------------------------------------------- 1 | BasedOnStyle: Google 2 | DerivePointerAlignment: false 3 | -------------------------------------------------------------------------------- /src/dead_reckoning/odom_imu/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(odom_imu_node) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 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 | # List C++ dependencies on ros packages 11 | set( ROS_CXX_DEPENDENCIES 12 | nav_msgs 13 | roscpp 14 | sensor_msgs 15 | nav_msgs 16 | tf 17 | irp_sen_msgs 18 | ) 19 | 20 | # Find catkin macros and libraries 21 | find_package(catkin REQUIRED COMPONENTS ${ROS_CXX_DEPENDENCIES} ) 22 | 23 | ## System dependencies are found with CMake's conventions 24 | # find_package(Boost REQUIRED COMPONENTS system) 25 | find_package(PCL REQUIRED QUIET) 26 | 27 | 28 | find_package(PkgConfig REQUIRED) 29 | pkg_check_modules(YAML_CPP REQUIRED yaml-cpp) 30 | 31 | find_package(Ceres REQUIRED) 32 | include_directories(${YAML_CPP_INCLUDEDIR}) 33 | 34 | # Find Eigen3 (from http://wiki.ros.org/jade/Migration) 35 | find_package(Eigen3) 36 | if(NOT EIGEN3_FOUND) 37 | # Fallback to cmake_modules 38 | find_package(cmake_modules REQUIRED) 39 | find_package(Eigen REQUIRED) 40 | set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) 41 | set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES}) # Not strictly necessary as Eigen is head only 42 | # Possibly map additional variables to the EIGEN3_ prefix. 43 | else() 44 | set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) 45 | endif() 46 | 47 | ## Uncomment this if the package has a setup.py. This macro ensures 48 | ## modules and global scripts declared therein get installed 49 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 50 | # catkin_python_setup() 51 | 52 | ################################################ 53 | ## Declare ROS messages, services and actions ## 54 | ################################################ 55 | 56 | ## To declare and build messages, services or actions from within this 57 | ## package, follow these steps: 58 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 59 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 60 | ## * In the file package.xml: 61 | ## * add a build_depend tag for "message_generation" 62 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 63 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 64 | ## but can be declared for certainty nonetheless: 65 | ## * add a exec_depend tag for "message_runtime" 66 | ## * In this file (CMakeLists.txt): 67 | ## * add "message_generation" and every package in MSG_DEP_SET to 68 | ## find_package(catkin REQUIRED COMPONENTS ...) 69 | ## * add "message_runtime" and every package in MSG_DEP_SET to 70 | ## catkin_package(CATKIN_DEPENDS ...) 71 | ## * uncomment the add_*_files sections below as needed 72 | ## and list every .msg/.srv/.action file to be processed 73 | ## * uncomment the generate_messages entry below 74 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 75 | 76 | ## Generate messages in the 'msg' folder 77 | # add_message_files( 78 | # FILES 79 | # Message1.msg 80 | # Message2.msg 81 | # ) 82 | 83 | ## Generate services in the 'srv' folder 84 | # add_service_files( 85 | # FILES 86 | # Service1.srv 87 | # Service2.srv 88 | # ) 89 | 90 | ## Generate actions in the 'action' folder 91 | # add_action_files( 92 | # FILES 93 | # Action1.action 94 | # Action2.action 95 | # ) 96 | 97 | ## Generate added messages and services with any dependencies listed here 98 | # generate_messages( 99 | # DEPENDENCIES 100 | # nav_msgs# sensor_msgs 101 | # ) 102 | 103 | ################################################ 104 | ## Declare ROS dynamic reconfigure parameters ## 105 | ################################################ 106 | 107 | ## To declare and build dynamic reconfigure parameters within this 108 | ## package, follow these steps: 109 | ## * In the file package.xml: 110 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 111 | ## * In this file (CMakeLists.txt): 112 | ## * add "dynamic_reconfigure" to 113 | ## find_package(catkin REQUIRED COMPONENTS ...) 114 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 115 | ## and list every .cfg file to be processed 116 | 117 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 118 | # generate_dynamic_reconfigure_options( 119 | # cfg/DynReconf1.cfg 120 | # cfg/DynReconf2.cfg 121 | # ) 122 | 123 | ################################### 124 | ## catkin specific configuration ## 125 | ################################### 126 | ## The catkin_package macro generates cmake config files for your package 127 | ## Declare things to be passed to dependent projects 128 | ## INCLUDE_DIRS: uncomment this if your package contains header files 129 | ## LIBRARIES: libraries you create in this project that dependent projects also need 130 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 131 | ## DEPENDS: system dependencies of this project that dependent projects also need 132 | catkin_package( 133 | INCLUDE_DIRS include 134 | # LIBRARIES lesson5 135 | CATKIN_DEPENDS ${ROS_CXX_DEPENDENCIES} 136 | # DEPENDS system_lib 137 | ) 138 | 139 | 140 | ########### 141 | ## Build ## 142 | ########### 143 | 144 | ## Specify additional locations of header files 145 | ## Your package locations should be listed before other locations 146 | include_directories( 147 | include 148 | ${catkin_INCLUDE_DIRS} 149 | ${PCL_INCLUDE_DIRS} 150 | ${EIGEN3_INCLUDE_DIRS} 151 | ${CERES_INCLUDE_DIRS} 152 | ) 153 | 154 | ## lidar undistortion 155 | add_executable(${PROJECT_NAME}_odom_imu_node src/app/odom_imu_node.cc src/odom_imu_flow.cc) 156 | 157 | add_dependencies(${PROJECT_NAME}_odom_imu_node ${catkin_EXPORTED_TARGETS}) 158 | target_link_libraries(${PROJECT_NAME}_odom_imu_node 159 | ${catkin_LIBRARIES} 160 | ${PCL_LIBRARY_DIRS} 161 | ${YAML_CPP_LIBRARIES} 162 | ${CERES_LIBRARIES} 163 | 164 | ) 165 | 166 | ############# 167 | ## Install ## 168 | ############# 169 | 170 | # all install targets should use catkin DESTINATION variables 171 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 172 | 173 | ## Mark executable scripts (Python etc.) for installation 174 | ## in contrast to setup.py, you can choose the destination 175 | # catkin_install_python(PROGRAMS 176 | # scripts/my_python_script 177 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 178 | # ) 179 | 180 | ## Mark executables for installation 181 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 182 | # install(TARGETS ${PROJECT_NAME}_node 183 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 184 | # ) 185 | 186 | ## Mark libraries for installation 187 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 188 | # install(TARGETS ${PROJECT_NAME} 189 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 190 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 191 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 192 | # ) 193 | 194 | ## Mark cpp header files for installation 195 | # install(DIRECTORY include/${PROJECT_NAME}/ 196 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 197 | # FILES_MATCHING PATTERN "*.h" 198 | # PATTERN ".svn" EXCLUDE 199 | # ) 200 | 201 | ## Mark other files for installation (e.g. launch and bag files, etc.) 202 | # install(FILES 203 | # # myfile1 204 | # # myfile2 205 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 206 | # ) 207 | 208 | ############# 209 | ## Testing ## 210 | ############# 211 | 212 | ## Add gtest based cpp test target and link libraries 213 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_lesson5.cpp) 214 | # if(TARGET ${PROJECT_NAME}-test) 215 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 216 | # endif() 217 | 218 | ## Add folders to be run by python nosetests 219 | # catkin_add_nosetests(test) 220 | -------------------------------------------------------------------------------- /src/dead_reckoning/odom_imu/config/filter.yaml: -------------------------------------------------------------------------------- 1 | covariance: 2 | prior: 3 | pos: 1.0e-6 4 | vel: 1.0e-6 5 | ori: 1.0e-6 6 | epsilon: 1.0e-6 7 | delta: 1.0e-6 8 | process: 9 | gyro: 2.5e-3 10 | accel: 2.5e-3 11 | bias_accel: 2.5e-3 12 | bias_gyro: 1.0e-4 13 | measurement: 14 | pose: 15 | pos: 1.0e-4 16 | ori: 1.0e-4 17 | pos: 1.0e-4 18 | vel: 2.5e-3 19 | motion_constraint: 20 | activated: true 21 | w_b_thresh: 0.13 22 | 23 | # odom_or_vel: true 24 | odom_or_vel: true 25 | 26 | 27 | # imu_topic: "/kitti/oxts/imu/extract" 28 | imu_topic: "/imu/data_raw" 29 | 30 | 31 | ENCODER_TOPIC: "/encoder_count" 32 | 33 | Encoder_resolution: 4096 34 | Encoder_left_wheel_diameter: 0.623803 35 | Encoder_right_wheel_diameter: 0.623095 36 | Encoder_wheel_base: 1.52683 -------------------------------------------------------------------------------- /src/dead_reckoning/odom_imu/config/odom_imu.yaml: -------------------------------------------------------------------------------- 1 | use_imu: true 2 | use_odometry: true -------------------------------------------------------------------------------- /src/dead_reckoning/odom_imu/launch/odom_imu.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /src/dead_reckoning/odom_imu/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | odom_imu_node 4 | 0.0.0 5 | The odom_imu_node package 6 | 7 | 8 | 9 | 10 | lx 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | nav_msgs 53 | roscpp 54 | sensor_msgs 55 | irp_sen_msgs 56 | 57 | 58 | nav_msgs 59 | tf 60 | nav_msgs 61 | roscpp 62 | sensor_msgs 63 | nav_msgs 64 | tf 65 | irp_sen_msgs 66 | 67 | nav_msgs 68 | roscpp 69 | sensor_msgs 70 | nav_msgs 71 | tf 72 | irp_sen_msgs 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | -------------------------------------------------------------------------------- /src/dead_reckoning/odom_imu/src/app/odom_imu_node.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2021 The Project Author: lixiang 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "tools/odom_imu_flow.h" 18 | 19 | 20 | int main(int argc, char **argv) 21 | { 22 | ros::init(argc, argv, "odom_imu_node"); 23 | 24 | OdomImuFlow odom_imu_flow_; 25 | 26 | // 开启3个线程同时工作 27 | // ros::AsyncSpinner spinner(3); 28 | // spinner.start(); 29 | ros::Rate rate(100); 30 | while(ros::ok()){ 31 | ros::spinOnce(); 32 | 33 | // odom_imu_flow_.fusion2_Run(); 34 | odom_imu_flow_.fusion2_Run(); 35 | 36 | rate.sleep(); 37 | } 38 | 39 | // ros::waitForShutdown(); 40 | return (0); 41 | } -------------------------------------------------------------------------------- /src/doc/img/1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiangli0608/Learning_localization_from_scratch_ws/314f0076853c8f049d9ffd5f294a5e65d7d5f29c/src/doc/img/1.png -------------------------------------------------------------------------------- /src/doc/img/2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiangli0608/Learning_localization_from_scratch_ws/314f0076853c8f049d9ffd5f294a5e65d7d5f29c/src/doc/img/2.png -------------------------------------------------------------------------------- /src/doc/img/lio-sam-result.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiangli0608/Learning_localization_from_scratch_ws/314f0076853c8f049d9ffd5f294a5e65d7d5f29c/src/doc/img/lio-sam-result.png -------------------------------------------------------------------------------- /src/doc/img/traj.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiangli0608/Learning_localization_from_scratch_ws/314f0076853c8f049d9ffd5f294a5e65d7d5f29c/src/doc/img/traj.png -------------------------------------------------------------------------------- /src/kaist_tool/file_player/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(file_player) 3 | 4 | add_definitions(-std=c++11) 5 | 6 | if (NOT CMAKE_BUILD_TYPE) 7 | set (CMAKE_BUILD_TYPE release) 8 | endif () 9 | 10 | find_package(catkin REQUIRED cmake_modules 11 | COMPONENTS 12 | roscpp 13 | rospy 14 | std_msgs 15 | geometry_msgs 16 | message_generation 17 | rosbag 18 | image_transport 19 | cv_bridge 20 | dynamic_reconfigure 21 | pcl_ros 22 | pcl_conversions 23 | pcl_msgs 24 | irp_sen_msgs 25 | camera_info_manager 26 | tf 27 | ) 28 | set(CMAKE_AUTOMOC ON) 29 | 30 | find_package(Eigen REQUIRED) 31 | 32 | #set (CMAKE_PREFIX_PATH /opt/Qt5.6.1/5.6/gcc_64/lib/cmake) 33 | 34 | #find_package(Qt5Core) 35 | find_package(Qt5Widgets) 36 | find_package(Qt5Gui) 37 | find_package(Qt5OpenGL) 38 | 39 | 40 | add_message_files( 41 | DIRECTORY msg 42 | ) 43 | 44 | generate_messages( 45 | DEPENDENCIES 46 | std_msgs 47 | geometry_msgs 48 | ) 49 | 50 | generate_dynamic_reconfigure_options( 51 | cfg/file_player.cfg 52 | #... 53 | ) 54 | 55 | catkin_package( 56 | INCLUDE_DIRS include 57 | LIBRARIES 58 | file_player 59 | CATKIN_DEPENDS 60 | roscpp rospy 61 | std_msgs 62 | geometry_msgs 63 | message_runtime 64 | image_transport 65 | cv_bridge 66 | dynamic_reconfigure 67 | pcl_ros 68 | pcl_conversions 69 | pcl_msgs 70 | irp_sen_msgs 71 | camera_info_manager 72 | tf 73 | 74 | DEPENDS 75 | Eigen 76 | ) 77 | 78 | 79 | set (SRC_DIR ${CMAKE_CURRENT_SOURCE_DIR}/src) 80 | 81 | set (File_Player_QTLib_src ${SRC_DIR}/mainwindow.cpp ${SRC_DIR}/ROSThread.cpp) 82 | set (File_Player_QTLib_hdr ${SRC_DIR}/mainwindow.h ${SRC_DIR}/ROSThread.h) 83 | set (File_Player_QTLib_ui ${SRC_DIR}/mainwindow.ui) 84 | set (File_Player_QTBin_src ${SRC_DIR}/main.cpp) 85 | 86 | 87 | 88 | #find_package(GLEW REQUIRED) 89 | #find_package(GLUT REQUIRED) 90 | 91 | 92 | include_directories( 93 | ${catkin_INCLUDE_DIRS} 94 | ${file_player_INCLUDE_DIRS} 95 | ${SRC_DIR} 96 | ${Qt5Widgets_INCLUDE_DIRS} 97 | ${PROJECT_BINARY_DIR} 98 | include 99 | ${Eigen_INCLUDE_DIRS} 100 | ) 101 | 102 | 103 | #qt5_wrap_cpp(File_Player_QTLib_hdr_moc ${File_Player_QTLib_hdr}) 104 | qt5_wrap_ui (File_Player_QTLib_ui_moc ${File_Player_QTLib_ui}) 105 | qt5_add_resources(SHADER_RSC_ADDED ${SRC_DIR}/resources.qrc) 106 | 107 | 108 | 109 | ########### 110 | ## Build ## 111 | ########### 112 | 113 | 114 | add_executable(file_player ${File_Player_QTLib_src} ${File_Player_QTLib_hdr} ${File_Player_QTBin_src} ${SHADER_RSC_ADDED} ${File_Player_QTLib_ui_moc}) 115 | 116 | add_dependencies(file_player file_player_msgs_generate_messages_cpp ${PROJECT_NAME}_gencfg) 117 | add_dependencies(file_player ${catkin_EXPORTED_TARGETS}) 118 | 119 | target_link_libraries(file_player 120 | 121 | ${catkin_LIBRARIES} 122 | ${QT_LIBRARIES} 123 | ${OPENGL_LIBRARIES} 124 | Qt5::Widgets 125 | Qt5::Gui 126 | Qt5::OpenGL 127 | GL 128 | ${BOOST_CUSTOM_LIBS} 129 | ${Eigen_LIBRARIES} 130 | ${OpenCV_LIBRARIES} 131 | ) 132 | 133 | 134 | 135 | -------------------------------------------------------------------------------- /src/kaist_tool/file_player/README.md: -------------------------------------------------------------------------------- 1 | # File player for complex urban data set 2 | 3 | ## News 4 | - Aug 2021: Our dataset is now available via [this google-site (https://bit.ly/complex-urban-dataset)](https://sites.google.com/view/complex-urban-dataset). 5 | 6 | ## What is File player? 7 | This program is a file player for the complex urban data set. If a user installs the ROS using "Desktop-Full version", there is only one additional dependent package, except for the ROS default package. First, clone this package into the src folder of your desired ROS workspace. 8 | 9 | Maintainer: Jinyong Jeong (jjy0923@kaist.ac.kr) 10 | 11 | ## How to use? 12 | 13 | ### 1. Obtain dependent package (defined msg) 14 | 15 | ``` 16 | $cd ~/catkin_ws/src 17 | $wstool init 18 | $wstool merge file_player/depend_pack.rosinstall 19 | $wstool update 20 | ``` 21 | 22 | ### 2. Build workspace 23 | 24 | ``` 25 | $cd ~/catkin_ws 26 | $catkin_make 27 | ``` 28 | 29 | ### 3. Run file player 30 | 31 | ``` 32 | $source devel/setup.bash 33 | $roslaunch file_player file_player.launch 34 | ``` 35 | 36 | ### 4. Load data files and play 37 | 38 | 1. Click 'Load' button. 39 | 2. Choose data set folder including sensor_data folder and calibration folder. 40 | 3. The player button starts publishing data in the ROS message. 41 | 4. The Stop skip button skips data while the vehicle is stationary for convenience. 42 | 5. The loop button resumes when playback is finished. 43 | -------------------------------------------------------------------------------- /src/kaist_tool/file_player/cfg/file_player.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "dynamic_file_player" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | exit(gen.generate(PACKAGE, "dynamic_file_player", "dynamic_file_player")) 9 | -------------------------------------------------------------------------------- /src/kaist_tool/file_player/depend_pack.rosinstall: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: irp_sen_msgs 3 | uri: https://github.com/irapkaist/irp_sen_msgs.git 4 | 5 | -------------------------------------------------------------------------------- /src/kaist_tool/file_player/include/file_player/color.h: -------------------------------------------------------------------------------- 1 | #ifndef _COLORS_ 2 | #define _COLORS_ 3 | 4 | /* FOREGROUND */ 5 | #define RST "\x1B[0m" 6 | #define KRED "\x1B[31m" 7 | #define KGRN "\x1B[32m" 8 | #define KYEL "\x1B[33m" 9 | #define KBLU "\x1B[34m" 10 | #define KMAG "\x1B[35m" 11 | #define KCYN "\x1B[36m" 12 | #define KWHT "\x1B[37m" 13 | 14 | #define FRED(x) KRED x RST 15 | #define FGRN(x) KGRN x RST 16 | #define FYEL(x) KYEL x RST 17 | #define FBLU(x) KBLU x RST 18 | #define FMAG(x) KMAG x RST 19 | #define FCYN(x) KCYN x RST 20 | #define FWHT(x) KWHT x RST 21 | 22 | #define BOLD(x) "\x1B[1m" x RST 23 | #define UNDL(x) "\x1B[4m" x RST 24 | 25 | #endif /* _COLORS_ */ 26 | -------------------------------------------------------------------------------- /src/kaist_tool/file_player/include/file_player/datathread.h: -------------------------------------------------------------------------------- 1 | #ifndef DATATHREAD_H 2 | #define DATATHREAD_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | template 10 | struct DataThread{ 11 | 12 | std::mutex mutex_; 13 | std::queue data_queue_; 14 | std::condition_variable cv_; 15 | std::thread thread_; 16 | bool active_; 17 | 18 | DataThread() : active_(true){} 19 | 20 | void push(T data){ 21 | mutex_.lock(); 22 | data_queue_.push(data); 23 | mutex_.unlock(); 24 | } 25 | 26 | T pop(){ 27 | T result; 28 | mutex_.lock(); 29 | result = data_queue_.front(); 30 | data_queue_.pop(); 31 | mutex_.unlock(); 32 | return result; 33 | } 34 | 35 | // virtual void DataProcess(T data){ 36 | 37 | // } 38 | 39 | // void DataProcessThread(){ 40 | // while(1){ 41 | // std::unique_lock ul(mutex_); 42 | // cv_.wait(ul); 43 | // if(active_ == false) return; 44 | // ul.unlock(); 45 | 46 | // while(!data_queue_.empty()){ 47 | // auto data = pop(); 48 | // //process 49 | // DataProcess(data); 50 | // } 51 | // } 52 | // } 53 | 54 | }; 55 | 56 | #endif // DATATHREAD_H 57 | -------------------------------------------------------------------------------- /src/kaist_tool/file_player/launch/file_player.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /src/kaist_tool/file_player/msg/tmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiangli0608/Learning_localization_from_scratch_ws/314f0076853c8f049d9ffd5f294a5e65d7d5f29c/src/kaist_tool/file_player/msg/tmp -------------------------------------------------------------------------------- /src/kaist_tool/file_player/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | file_player 4 | 0.0.0 5 | file player 6 | 7 | 8 | 9 | 10 | jjy0923 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | http://irap.kaist.ac.kr --> 22 | 23 | 24 | 25 | 26 | Jinyong Jeong --> 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | catkin 40 | roscpp 41 | rospy 42 | std_msgs 43 | geometry_msgs 44 | message_generation 45 | image_transport 46 | cv_bridge 47 | dynamic_reconfigure 48 | eigen 49 | pcl_ros 50 | pcl_conversions 51 | pcl_msgs 52 | irp_sen_msgs 53 | cmake_modules 54 | camera_info_manager 55 | tf 56 | 57 | roscpp 58 | rospy 59 | std_msgs 60 | geometry_msgs 61 | message_runtime 62 | image_transport 63 | cv_bridge 64 | dynamic_reconfigure 65 | eigen 66 | pcl_ros 67 | pcl_conversions 68 | pcl_msgs 69 | irp_sen_msgs 70 | cmake_modules 71 | camera_info_manager 72 | tf 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | -------------------------------------------------------------------------------- /src/kaist_tool/file_player/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/kinetic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /src/kaist_tool/file_player/src/color_code.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | float hsv[64][3]= 4 | {{1 ,0 ,0 }, 5 | {1 ,0.09375,0 }, 6 | {1 ,0.18750,0 }, 7 | {1 ,0.28125,0 }, 8 | {1 ,0.37500,0 }, 9 | {1 ,0.46875,0 }, 10 | {1 ,0.56250,0 }, 11 | {1 ,0.65625,0 }, 12 | {1 ,0.75000,0 }, 13 | {1 ,0.84375,0 }, 14 | {1 ,0.93750,0 }, 15 | {0.96875,1 ,0 }, 16 | {0.87500,1 ,0 }, 17 | {0.78125,1 ,0 }, 18 | {0.68750,1 ,0 }, 19 | {0.59375,1 ,0 }, 20 | {0.50000,1 ,0 }, 21 | {0.40625,1 ,0 }, 22 | {0.31250,1 ,0 }, 23 | {0.21875,1 ,0 }, 24 | {0.12500,1 ,0 }, 25 | {0.03125,1 ,0 }, 26 | {0 ,1 ,0.06250}, 27 | {0 ,1 ,0.15625}, 28 | {0 ,1 ,0.25000}, 29 | {0 ,1 ,0.34375}, 30 | {0 ,1 ,0.43750}, 31 | {0 ,1 ,0.53125}, 32 | {0 ,1 ,0.62500}, 33 | {0 ,1 ,0.71875}, 34 | {0 ,1 ,0.81250}, 35 | {0 ,1 ,0.90625}, 36 | {0 ,1 ,1 }, 37 | {0 ,0.90625,1 }, 38 | {0 ,0.81250,1 }, 39 | {0 ,0.71875,1 }, 40 | {0 ,0.62500,1 }, 41 | {0 ,0.53125,1 }, 42 | {0 ,0.43750,1 }, 43 | {0 ,0.34375,1 }, 44 | {0 ,0.25000,1 }, 45 | {0 ,0.15625,1 }, 46 | {0 ,0.06250,1 }, 47 | {0.03125,0 ,1 }, 48 | {0.12500,0 ,1 }, 49 | {0.21875,0 ,1 }, 50 | {0.31250,0 ,1 }, 51 | {0.40625,0 ,1 }, 52 | {0.50000,0 ,1 }, 53 | {0.59375,0 ,1 }, 54 | {0.68750,0 ,1 }, 55 | {0.78125,0 ,1 }, 56 | {0.87500,0 ,1 }, 57 | {0.96875,0 ,1 }, 58 | {1 ,0 ,0.93750}, 59 | {1 ,0 ,0.84375}, 60 | {1 ,0 ,0.75000}, 61 | {1 ,0 ,0.65625}, 62 | {1 ,0 ,0.56250}, 63 | {1 ,0 ,0.46875}, 64 | {1 ,0 ,0.37500}, 65 | {1 ,0 ,0.28125}, 66 | {1 ,0 ,0.18750}, 67 | {1 ,0 ,0.09375}}; 68 | 69 | float jet[64][3]= 70 | {{0 ,0 ,0.5625}, 71 | {0 ,0 ,0.6250}, 72 | {0 ,0 ,0.6875}, 73 | {0 ,0 ,0.7500}, 74 | {0 ,0 ,0.8125}, 75 | {0 ,0 ,0.8750}, 76 | {0 ,0 ,0.9375}, 77 | {0 ,0 ,1.0000}, 78 | {0 ,0.0625 ,1.0000}, 79 | {0 ,0.1250 ,1.0000}, 80 | {0 ,0.1875 ,1.0000}, 81 | {0 ,0.2500 ,1.0000}, 82 | {0 ,0.3125 ,1.0000}, 83 | {0 ,0.3750 ,1.0000}, 84 | {0 ,0.4375 ,1.0000}, 85 | {0 ,0.5000 ,1.0000}, 86 | {0 ,0.5625 ,1.0000}, 87 | {0 ,0.6250 ,1.0000}, 88 | {0 ,0.6875 ,1.0000}, 89 | {0 ,0.7500 ,1.0000}, 90 | {0 ,0.8125 ,1.0000}, 91 | {0 ,0.8750 ,1.0000}, 92 | {0 ,0.9375 ,1.0000}, 93 | {0 ,1.0000 ,1.0000}, 94 | {0.0625 ,1.0000 ,0.9375}, 95 | {0.1250 ,1.0000 ,0.8750}, 96 | {0.1875 ,1.0000 ,0.8125}, 97 | {0.2500 ,1.0000 ,0.7500}, 98 | {0.3125 ,1.0000 ,0.6875}, 99 | {0.3750 ,1.0000 ,0.6250}, 100 | {0.4375 ,1.0000 ,0.5625}, 101 | {0.5000 ,1.0000 ,0.5000}, 102 | {0.5625 ,1.0000 ,0.4375}, 103 | {0.6250 ,1.0000 ,0.3750}, 104 | {0.6875 ,1.0000 ,0.3125}, 105 | {0.7500 ,1.0000 ,0.2500}, 106 | {0.8125 ,1.0000 ,0.1875}, 107 | {0.8750 ,1.0000 ,0.1250}, 108 | {0.9375 ,1.0000 ,0.0625}, 109 | {1.0000 ,1.0000 ,0 }, 110 | {1.0000 ,0.9375 ,0 }, 111 | {1.0000 ,0.8750 ,0 }, 112 | {1.0000 ,0.8125 ,0 }, 113 | {1.0000 ,0.7500 ,0 }, 114 | {1.0000 ,0.6875 ,0 }, 115 | {1.0000 ,0.6250 ,0 }, 116 | {1.0000 ,0.5625 ,0 }, 117 | {1.0000 ,0.5000 ,0 }, 118 | {1.0000 ,0.4375 ,0 }, 119 | {1.0000 ,0.3750 ,0 }, 120 | {1.0000 ,0.3125 ,0 }, 121 | {1.0000 ,0.2500 ,0 }, 122 | {1.0000 ,0.1875 ,0 }, 123 | {1.0000 ,0.1250 ,0 }, 124 | {1.0000 ,0.0625 ,0 }, 125 | {1.0000 ,0 ,0 }, 126 | {0.9375 ,0 ,0 }, 127 | {0.8750 ,0 ,0 }, 128 | {0.8125 ,0 ,0 }, 129 | {0.7500 ,0 ,0 }, 130 | {0.6875 ,0 ,0 }, 131 | {0.6250 ,0 ,0 }, 132 | {0.5625 ,0 ,0 }, 133 | {0.5000 ,0 ,0 }}; 134 | 135 | float randCol[64][3]= 136 | {{0 ,0 ,0.5625}, 137 | {0 ,0 ,0.6250}, 138 | {0 ,0.5625 ,1.0000}, 139 | {0.0625 ,1.0000 ,0.9375}, 140 | {0.6250 ,1.0000 ,0.3750}, 141 | {0.5000 ,1.0000 ,0.5000}, 142 | {1.0000 ,0.3750 ,0 }, 143 | {1.0000 ,0.9375 ,0 }, 144 | {0.8125 ,1.0000 ,0.1875}, 145 | {0.6875 ,1.0000 ,0.3125}, 146 | {0.9375 ,1.0000 ,0.0625}, 147 | {0.2500 ,1.0000 ,0.7500}, 148 | {0 ,0.3750 ,1.0000}, 149 | {0 ,0 ,0.6875}, 150 | {0.1250 ,1.0000 ,0.8750}, 151 | {0.5625 ,1.0000 ,0.4375}, 152 | {1.0000 ,0.7500 ,0 }, 153 | {1.0000 ,0 ,0 }, 154 | {0 ,0.4375 ,1.0000}, 155 | {0.8750 ,1.0000 ,0.1250}, 156 | {0.3125 ,1.0000 ,0.6875}, 157 | {0.4375 ,1.0000 ,0.5625}, 158 | {0 ,0 ,0.8750}, 159 | {0.1875 ,1.0000 ,0.8125}, 160 | {0.6250 ,0 ,0 }, 161 | {1.0000 ,0.3125 ,0 }, 162 | {0 ,0 ,0.9375}, 163 | {0 ,0 ,1.0000}, 164 | {1.0000 ,0.8750 ,0 }, 165 | {0 ,0.5000 ,1.0000}, 166 | {0 ,0.6875 ,1.0000}, 167 | {1.0000 ,0.6250 ,0 }, 168 | {1.0000 ,0.5000 ,0 }, 169 | {0 ,0.6250 ,1.0000}, 170 | {1.0000 ,0.6875 ,0 }, 171 | {0 ,0.1250 ,1.0000}, 172 | {1.0000 ,0.5625 ,0 }, 173 | {0.9375 ,0 ,0 }, 174 | {1.0000 ,1.0000 ,0 }, 175 | {0 ,0.1875 ,1.0000}, 176 | {0 ,0.2500 ,1.0000}, 177 | {0 ,0.3125 ,1.0000}, 178 | {1.0000 ,0.4375 ,0 }, 179 | {0.8750 ,0 ,0 }, 180 | {0.8125 ,0 ,0 }, 181 | {0.7500 ,0 ,0 }, 182 | {0.7500 ,1.0000 ,0.2500}, 183 | {0 ,0.0625 ,1.0000}, 184 | {0 ,0.7500 ,1.0000}, 185 | {1.0000 ,0.8125 ,0 }, 186 | {0 ,0.8125 ,1.0000}, 187 | {0 ,0.8750 ,1.0000}, 188 | {0 ,0.9375 ,1.0000}, 189 | {0 ,1.0000 ,1.0000}, 190 | {0.3750 ,1.0000 ,0.6250}, 191 | {1.0000 ,0.1875 ,0 }, 192 | {0.6875 ,0 ,0 }, 193 | {1.0000 ,0.2500 ,0 }, 194 | {0 ,0 ,0.7500}, 195 | {1.0000 ,0.1250 ,0 }, 196 | {0 ,0 ,0.8125}, 197 | {1.0000 ,0.0625 ,0 }, 198 | {0.5625 ,0 ,0 }, 199 | {0.5000 ,0 ,0 }}; 200 | -------------------------------------------------------------------------------- /src/kaist_tool/file_player/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "mainwindow.h" 2 | #include 3 | //#include 4 | #include 5 | #include 6 | #include 7 | //#include 8 | #include 9 | #include 10 | 11 | int main(int argc, char *argv[]) 12 | { 13 | ros::init(argc, argv, "file_player"); 14 | ros::NodeHandle nh; 15 | 16 | QApplication a(argc, argv); 17 | MainWindow w; 18 | w.RosInit(nh); 19 | w.show(); 20 | 21 | return a.exec(); 22 | } 23 | -------------------------------------------------------------------------------- /src/kaist_tool/file_player/src/mainwindow.cpp: -------------------------------------------------------------------------------- 1 | #include "mainwindow.h" 2 | #include "ui_mainwindow.h" 3 | 4 | using namespace std; 5 | 6 | MainWindow::MainWindow(QWidget *parent) : 7 | QMainWindow(parent), 8 | ui_(new Ui::MainWindow) 9 | { 10 | my_ros_ = new ROSThread(this, &mutex); 11 | ui_->setupUi(this); 12 | my_ros_->start(); 13 | 14 | slider_checker_ = false; 15 | play_flag_ = false; 16 | pause_flag_ = false; 17 | loop_flag_ = false; 18 | stop_skip_flag_ = true; 19 | 20 | connect(my_ros_, SIGNAL(StampShow(quint64)), this, SLOT(SetStamp(quint64))); 21 | connect(my_ros_, SIGNAL(StartSignal()), this, SLOT(Play())); 22 | 23 | connect(ui_->quitButton, SIGNAL(pressed()), this, SLOT(TryClose())); 24 | connect(ui_->pushButton, SIGNAL(pressed()), this, SLOT(FilePathSet())); 25 | connect(ui_->pushButton_2, SIGNAL(pressed()), this, SLOT(Play())); 26 | connect(ui_->pushButton_3, SIGNAL(pressed()), this, SLOT(Pause())); 27 | 28 | connect(ui_->doubleSpinBox, SIGNAL(valueChanged(double)), this, SLOT(PlaySpeedChange(double))); 29 | ui_->doubleSpinBox->setRange(0.01,20.0); 30 | ui_->doubleSpinBox->setValue(1.0); 31 | ui_->doubleSpinBox->setSingleStep(0.01); 32 | connect(ui_->checkBox, SIGNAL(stateChanged(int)), this, SLOT (LoopFlagChange(int))); 33 | if(loop_flag_ == true){ 34 | ui_->checkBox->setCheckState(Qt::Checked); 35 | }else{ 36 | ui_->checkBox->setCheckState(Qt::Unchecked); 37 | } 38 | connect(ui_->checkBox_2, SIGNAL(stateChanged(int)), this, SLOT (StopSkipFlagChange(int))); 39 | if(stop_skip_flag_ == true){ 40 | ui_->checkBox_2->setCheckState(Qt::Checked); 41 | }else{ 42 | ui_->checkBox_2->setCheckState(Qt::Unchecked); 43 | } 44 | connect(ui_->checkBox_3, SIGNAL(stateChanged(int)), this, SLOT (AutoStartFlagChange(int))); 45 | if(my_ros_->auto_start_flag_ == true){ 46 | ui_->checkBox_3->setCheckState(Qt::Checked); 47 | }else{ 48 | ui_->checkBox_3->setCheckState(Qt::Unchecked); 49 | } 50 | 51 | connect(ui_->horizontalSlider, SIGNAL(sliderPressed()), this, SLOT(SliderPressed())); 52 | connect(ui_->horizontalSlider, SIGNAL(valueChanged(int)), this, SLOT(SliderValueChange(int))); 53 | connect(ui_->horizontalSlider, SIGNAL(sliderReleased()), this, SLOT(SliderValueApply())); 54 | 55 | ui_->horizontalSlider->setRange(0,10000); 56 | ui_->horizontalSlider->setValue(0); 57 | slider_value_ = 0; 58 | 59 | } 60 | 61 | MainWindow::~MainWindow() 62 | { 63 | emit setThreadFinished(true); //Tell the thread to finish 64 | delete ui_; 65 | my_ros_->quit(); 66 | if(!my_ros_->wait(500)) //Wait until it actually has terminated (max. 3 sec) 67 | { 68 | my_ros_->terminate(); //Thread didn't exit in time, probably deadlocked, terminate it! 69 | my_ros_->wait(); //We have to wait again here! 70 | } 71 | } 72 | 73 | 74 | void MainWindow::RosInit(ros::NodeHandle &n) 75 | { 76 | my_ros_->ros_initialize(n); 77 | } 78 | 79 | 80 | void MainWindow::TryClose() 81 | { 82 | close(); 83 | } 84 | 85 | 86 | void MainWindow::FilePathSet() 87 | { 88 | play_flag_ = false; 89 | my_ros_->play_flag_ = false; 90 | this->ui_->pushButton_2->setText(QString::fromStdString("Play")); 91 | 92 | pause_flag_ = false; 93 | my_ros_->pause_flag_ = false; 94 | this->ui_->pushButton_3->setText(QString::fromStdString("Pause")); 95 | 96 | QFileDialog dialog; 97 | this->ui_->label->setText("Data is beging loaded....."); 98 | data_folder_path_ = dialog.getExistingDirectory(); 99 | my_ros_->data_folder_path_ = data_folder_path_.toUtf8().constData(); 100 | 101 | my_ros_->Ready(); 102 | 103 | this->ui_->label->setText(data_folder_path_); 104 | 105 | } 106 | 107 | void MainWindow::SetStamp(quint64 stamp) 108 | { 109 | this->ui_->label_2->setText(QString::number(stamp)); 110 | 111 | //set slide bar 112 | if(slider_checker_ == false){ 113 | ui_->horizontalSlider->setValue(static_cast(static_cast(stamp - my_ros_->initial_data_stamp_)/static_cast(my_ros_->last_data_stamp_ - my_ros_->initial_data_stamp_)*10000)); 114 | } 115 | } 116 | 117 | void MainWindow::Play() 118 | { 119 | if(my_ros_->play_flag_ == false){ 120 | play_flag_ = true; 121 | my_ros_->play_flag_ = true; 122 | this->ui_->pushButton_2->setText(QString::fromStdString("End")); 123 | 124 | pause_flag_ = false; 125 | my_ros_->pause_flag_ = false; 126 | this->ui_->pushButton_3->setText(QString::fromStdString("Pause")); 127 | 128 | }else{ 129 | play_flag_ = false; 130 | my_ros_->play_flag_ = false; 131 | this->ui_->pushButton_2->setText(QString::fromStdString("Play")); 132 | } 133 | } 134 | 135 | void MainWindow::Pause() 136 | { 137 | if(pause_flag_ == false){ 138 | pause_flag_ = true; 139 | my_ros_->pause_flag_ = true; 140 | this->ui_->pushButton_3->setText(QString::fromStdString("Resume")); 141 | }else{ 142 | pause_flag_ = false; 143 | my_ros_->pause_flag_ = false; 144 | this->ui_->pushButton_3->setText(QString::fromStdString("Pause")); 145 | } 146 | } 147 | 148 | 149 | void MainWindow::PlaySpeedChange(double value) 150 | { 151 | my_ros_->play_rate_ = value; 152 | } 153 | 154 | void MainWindow::LoopFlagChange(int value) 155 | { 156 | if(value == 2){ 157 | loop_flag_ = true; 158 | my_ros_->loop_flag_ = true; 159 | }else if(value == 0){ 160 | loop_flag_ = false; 161 | my_ros_->loop_flag_ = false; 162 | } 163 | } 164 | void MainWindow::StopSkipFlagChange(int value) 165 | { 166 | if(value == 2){ 167 | stop_skip_flag_ = true; 168 | my_ros_->stop_skip_flag_ = true; 169 | }else if(value == 0){ 170 | stop_skip_flag_ = false; 171 | my_ros_->stop_skip_flag_ = false; 172 | } 173 | } 174 | void MainWindow::AutoStartFlagChange(int value) 175 | { 176 | if(value == 2){ 177 | my_ros_->auto_start_flag_ = true; 178 | }else if(value == 0){ 179 | my_ros_->auto_start_flag_ = false; 180 | } 181 | } 182 | void MainWindow::SliderValueChange(int value) 183 | { 184 | slider_value_ = value; 185 | } 186 | 187 | void MainWindow::SliderPressed() 188 | { 189 | slider_checker_ = true; 190 | } 191 | 192 | void MainWindow::SliderValueApply() 193 | { 194 | my_ros_->ResetProcessStamp(slider_value_); 195 | slider_checker_ = false; 196 | } 197 | -------------------------------------------------------------------------------- /src/kaist_tool/file_player/src/mainwindow.h: -------------------------------------------------------------------------------- 1 | #ifndef MAINWINDOW_H 2 | #define MAINWINDOW_H 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include "ROSThread.h" 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #define R2D 180/PI 26 | #define D2R PI/180 27 | #define POWER_CTR_DELAY 200000 28 | #define INTENSITY_MIN 0.0 29 | #define INTENSITY_MAX 100.0 30 | #define INTENSITY_COLOR_MIN 0.0 31 | #define INTENSITY_COLOR_MAX 1.0 32 | 33 | using namespace std; 34 | 35 | extern QMutex mutex; 36 | 37 | namespace Ui { 38 | class MainWindow; 39 | } 40 | 41 | class MainWindow : public QMainWindow 42 | { 43 | Q_OBJECT 44 | 45 | public: 46 | 47 | explicit MainWindow(QWidget *parent = 0); 48 | ~MainWindow(); 49 | void RosInit(ros::NodeHandle &n); 50 | 51 | private slots: 52 | void TryClose(); 53 | void FilePathSet(); 54 | void Play(); 55 | void Pause(); 56 | void PlaySpeedChange(double value); 57 | void LoopFlagChange(int value); 58 | void StopSkipFlagChange(int value); 59 | void AutoStartFlagChange(int value); 60 | void SetStamp(quint64 stamp); 61 | void SliderValueChange(int value); 62 | void SliderPressed(); 63 | void SliderValueApply(); 64 | 65 | signals: 66 | void setThreadFinished(bool); 67 | private: 68 | QMutex mutex; 69 | ROSThread *my_ros_; 70 | Ui::MainWindow *ui_; 71 | QString data_folder_path_; 72 | bool play_flag_; 73 | bool pause_flag_; 74 | bool loop_flag_; 75 | bool stop_skip_flag_; 76 | int slider_value_; 77 | 78 | int slider_checker_; 79 | 80 | }; 81 | 82 | #endif // MAINWINDOW_H 83 | -------------------------------------------------------------------------------- /src/kaist_tool/file_player/src/resources.qrc: -------------------------------------------------------------------------------- 1 | 2 | 3 | resources/fragmentShader.fsh 4 | resources/fragmentShaderColor.fsh 5 | resources/fragmentShaderOBJ.fsh 6 | resources/vertexShader.vsh 7 | resources/vertexShaderColor.vsh 8 | resources/vertexShaderOBJ.vsh 9 | 10 | 11 | -------------------------------------------------------------------------------- /src/kaist_tool/file_player/src/resources/fragmentShader.fsh: -------------------------------------------------------------------------------- 1 | #version 130 2 | 3 | //! [0] 4 | uniform vec4 color; 5 | 6 | out vec4 fragColor; 7 | 8 | void main(void) 9 | { 10 | fragColor = color; 11 | } 12 | //! [0] 13 | -------------------------------------------------------------------------------- /src/kaist_tool/file_player/src/resources/fragmentShaderColor.fsh: -------------------------------------------------------------------------------- 1 | #version 130 2 | 3 | //! [0] 4 | in vec4 varyingColor; 5 | 6 | out vec4 fragColor; 7 | 8 | void main(void) 9 | { 10 | fragColor = varyingColor; 11 | } 12 | //! [0] 13 | -------------------------------------------------------------------------------- /src/kaist_tool/file_player/src/resources/fragmentShaderOBJ.fsh: -------------------------------------------------------------------------------- 1 | #version 130 2 | 3 | //! [0] 4 | uniform sampler2D texture; 5 | 6 | in vec2 varyingTextureCoordinate; 7 | 8 | out vec4 fragColor; 9 | 10 | void main(void) 11 | { 12 | fragColor = texture2D(texture, varyingTextureCoordinate); 13 | } 14 | //! [0] 15 | -------------------------------------------------------------------------------- /src/kaist_tool/file_player/src/resources/vertexShader.vsh: -------------------------------------------------------------------------------- 1 | #version 130 2 | 3 | //! [0] 4 | uniform mat4 mvpMatrix; 5 | 6 | in vec4 vertex; 7 | 8 | void main(void) 9 | { 10 | gl_Position = mvpMatrix * vertex; 11 | 12 | } 13 | //! [0] 14 | -------------------------------------------------------------------------------- /src/kaist_tool/file_player/src/resources/vertexShaderColor.vsh: -------------------------------------------------------------------------------- 1 | #version 130 2 | 3 | //! [0] 4 | uniform mat4 mvpMatrix; 5 | 6 | in vec4 vertex; 7 | in vec4 color; 8 | 9 | out vec4 varyingColor; 10 | 11 | void main(void) 12 | { 13 | varyingColor = color; 14 | gl_Position = mvpMatrix * vertex; 15 | } 16 | //! [0] 17 | -------------------------------------------------------------------------------- /src/kaist_tool/file_player/src/resources/vertexShaderOBJ.vsh: -------------------------------------------------------------------------------- 1 | #version 130 2 | 3 | //! [0] 4 | uniform mat4 mvpMatrix; 5 | 6 | in vec4 vertex; 7 | in vec2 textureCoordinate; 8 | 9 | out vec2 varyingTextureCoordinate; 10 | 11 | void main(void) 12 | { 13 | varyingTextureCoordinate = textureCoordinate; 14 | gl_Position = mvpMatrix * vertex; 15 | } 16 | //! [0] 17 | -------------------------------------------------------------------------------- /src/kaist_tool/file_player/srv/tmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiangli0608/Learning_localization_from_scratch_ws/314f0076853c8f049d9ffd5f294a5e65d7d5f29c/src/kaist_tool/file_player/srv/tmp -------------------------------------------------------------------------------- /src/kaist_tool/irp_sen_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | project(irp_sen_msgs) 4 | if (NOT CMAKE_BUILD_TYPE) 5 | set (CMAKE_BUILD_TYPE RelWithDebInfo) 6 | endif () 7 | find_package(catkin REQUIRED COMPONENTS message_generation std_msgs geometry_msgs sensor_msgs 8 | ) 9 | 10 | add_message_files( 11 | DIRECTORY msg 12 | ) 13 | generate_messages(DEPENDENCIES std_msgs geometry_msgs sensor_msgs) 14 | 15 | catkin_package( 16 | CATKIN_DEPENDS message_runtime std_msgs geometry_msgs sensor_msgs 17 | ) 18 | -------------------------------------------------------------------------------- /src/kaist_tool/irp_sen_msgs/README.md: -------------------------------------------------------------------------------- 1 | # Sensor msg definition for complex urban data set 2 | 3 | Maintainer: Jinyong Jeong (jjy0923@kaist.ac.kr) 4 | 5 | If you use the ROS package provided by the complex urban data set, clone this msg package to the src folder of catkin_ws. 6 | 7 | ## Msg type 8 | 9 | - VRS GPS msg 10 | 11 | - Laser scan array msg 12 | 13 | - IMU msg 14 | 15 | - 3 axis FOG msg 16 | 17 | - 1 axis FOG msg 18 | 19 | - encoder msg 20 | 21 | - altimeter msg 22 | -------------------------------------------------------------------------------- /src/kaist_tool/irp_sen_msgs/msg/LaserScanArray.msg: -------------------------------------------------------------------------------- 1 | Header header # standard ROS message header 2 | 3 | int64 size 4 | sensor_msgs/LaserScan[] LaserScans 5 | 6 | 7 | -------------------------------------------------------------------------------- /src/kaist_tool/irp_sen_msgs/msg/altimeter.msg: -------------------------------------------------------------------------------- 1 | Header header # standard ROS message header 2 | 3 | float64 data 4 | -------------------------------------------------------------------------------- /src/kaist_tool/irp_sen_msgs/msg/encoder.msg: -------------------------------------------------------------------------------- 1 | Header header # standard ROS message header 2 | int64 type 3 | int64 left_count 4 | int64 right_count 5 | -------------------------------------------------------------------------------- /src/kaist_tool/irp_sen_msgs/msg/fog.msg: -------------------------------------------------------------------------------- 1 | Header header # standard ROS message header 2 | 3 | float32 data 4 | -------------------------------------------------------------------------------- /src/kaist_tool/irp_sen_msgs/msg/fog_3axis.msg: -------------------------------------------------------------------------------- 1 | Header header # standard ROS message header 2 | 3 | float32 d_roll 4 | float32 d_pitch 5 | float32 d_yaw 6 | 7 | -------------------------------------------------------------------------------- /src/kaist_tool/irp_sen_msgs/msg/imu.msg: -------------------------------------------------------------------------------- 1 | Header header # standard ROS message header 2 | 3 | geometry_msgs/Quaternion quaternion_data 4 | geometry_msgs/Vector3 eular_data 5 | geometry_msgs/Vector3 gyro_data 6 | geometry_msgs/Vector3 acceleration_data 7 | geometry_msgs/Vector3 magneticfield_data 8 | 9 | -------------------------------------------------------------------------------- /src/kaist_tool/irp_sen_msgs/msg/vrs.msg: -------------------------------------------------------------------------------- 1 | Header header # standard ROS message header 2 | 3 | string GPGGA 4 | string GNGLL 5 | string GNGNS 6 | string GNGST 7 | string GPGSV 8 | string GLGSV 9 | string GNHDT 10 | string GNRMC 11 | string GNVTG 12 | string GNZDA 13 | string GNROT 14 | string GNGMP 15 | 16 | float64 longitude 17 | float64 latitude 18 | 19 | float64 x_coordinate 20 | float64 y_coordinate 21 | 22 | float64 altitude 23 | float64 altitude_orthometric 24 | 25 | int8 fix_state 26 | string fix_state_str 27 | 28 | float64 horizental_precision 29 | float64 lat_std 30 | float64 lon_std 31 | float64 altitude_std 32 | 33 | int8 number_of_sat 34 | 35 | bool heading_valid 36 | float64 heading_true 37 | float64 heading_magnet 38 | float64 speed_knot 39 | float64 speed_km 40 | string GNVTG_mode 41 | 42 | -------------------------------------------------------------------------------- /src/kaist_tool/irp_sen_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | irp_sen_msgs 4 | 1.2.0 5 | 6 | ROS message definitions for IRAP. 7 | 8 | IRAP 9 | Jinyong Jeong 10 | BSD 11 | 12 | http://irap.kaist.ac.kr 13 | https://github.com/irapkaist/irp_msgs 14 | https://github.com/irapkaist/irp_msgs/issues 15 | 16 | catkin 17 | 18 | message_generation 19 | std_msgs 20 | geometry_msgs 21 | sensor_msgs 22 | 23 | message_runtime 24 | std_msgs 25 | geometry_msgs 26 | sensor_msgs 27 | 28 | 29 | -------------------------------------------------------------------------------- /src/kaist_tool/kaist2bag/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 tsyxyz 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 | -------------------------------------------------------------------------------- /src/kaist_tool/kaist2bag/README.md: -------------------------------------------------------------------------------- 1 | # kaist2bag 2 | A tool to convert KAIST urban dataset to rosbag. 3 | 4 | Only tested on Ubuntu 20.04. 5 | 6 | 7 | 8 | ## Guide 9 | 10 | 1. Download desired database files from the [website](https://sites.google.com/view/complex-urban-dataset) 11 | 12 | 2. Extract `.gz.tar` files into their folders 13 | ``` 14 | find . -name 'urban28*.tar.gz' -execdir tar -xzvf '{}' \; 15 | ``` 16 | 17 | 3. Clone and build this repository 18 | ``` 19 | cd src 20 | git clone https://github.com/irapkaist/irp_sen_msgs.git 21 | git clone https://github.com/tsyxyz/kaist2bag.git 22 | cd .. 23 | catkin build 24 | ``` 25 | 26 | 4. Edit the [config file](config/config.yaml) with path and desired topics 27 | 28 | 29 | 5. Create a rosbag file for each sensor type 30 | ``` 31 | source devel/setup.bash 32 | roslaunch kaist2bag kaist2bag.launch 33 | ``` 34 | 35 | 6. Merge all bags into a single one (if desired) 36 | ``` 37 | rosrun kaist2bag mergebag.py merged.bag ... 38 | ``` 39 | 40 | 41 | 42 | 43 | ## Acknowledge 44 | 45 | File parsing code are referenced from [file_player](https://github.com/irapkaist/file_player). 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /src/kaist_tool/kaist2bag/config/config.yaml: -------------------------------------------------------------------------------- 1 | dataset: "/home/touchair/kaist_urban_dataset/urban08" 2 | save_to: "/home/touchair/kaist_urban_dataset/urban08/bag" 3 | 4 | altimeter: true 5 | altimeter_topic: "/altimeter_data" 6 | 7 | encoder: true 8 | wheel_odom_topic: "/wheel_odom" 9 | # encoder_irp_topic: "/encoder_count" 10 | # encoder_raw_topic: "/joint_states" 11 | 12 | fog: false 13 | fog_topic: "/dsp1760_data" 14 | 15 | gps: false 16 | gps_topic: "/gps/fix" 17 | 18 | vrs: true 19 | vrs_topic: "/vrs_gps_data" 20 | 21 | imu: true 22 | # imu_irp_topic: "/xsens_imu_data" 23 | imu_raw_topic: "/imu/data_raw" 24 | # imu_mag_topic: "/imu/mag" 25 | 26 | velodyne: true 27 | velodyne_left_topic: "/ns2/velodyne_points" 28 | velodyne_right_topic: "/ns1/velodyne_points" 29 | 30 | sick: false 31 | sick_back_topic: "/lms511_back/scan" 32 | sick_middle_topic: "/lms511_middle/scan" 33 | 34 | stereo: false 35 | stereo_left_topic: "/stereo/left/image_raw" 36 | stereo_right_topic: "/stereo/right/image_raw" 37 | -------------------------------------------------------------------------------- /src/kaist_tool/kaist2bag/launch/kaist2bag.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /src/kaist_tool/kaist2bag/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | kaist2bag 4 | 0.0.0 5 | The kaist2bag package 6 | 7 | tsyxyz 8 | 9 | MIT 10 | 11 | catkin 12 | roscpp 13 | rospy 14 | std_msgs 15 | geometry_msgs 16 | message_generation 17 | rosbag 18 | image_transport 19 | cv_bridge 20 | pcl_ros 21 | pcl_conversions 22 | pcl_msgs 23 | irp_sen_msgs 24 | tf 25 | roscpp 26 | rospy 27 | std_msgs 28 | roscpp 29 | rospy 30 | std_msgs 31 | geometry_msgs 32 | message_generation 33 | rosbag 34 | image_transport 35 | cv_bridge 36 | pcl_ros 37 | pcl_conversions 38 | pcl_msgs 39 | irp_sen_msgs 40 | tf 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /src/kaist_tool/kaist2bag/scripts/mergebag.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # original source: 4 | # http://www.clearpathrobotics.com/assets/downloads/support/merge_bag.py 5 | 6 | import sys 7 | import argparse 8 | from fnmatch import fnmatchcase 9 | 10 | from rosbag import Bag 11 | 12 | def main(): 13 | 14 | parser = argparse.ArgumentParser(description='Merge one or more bag files with the possibilities of filtering topics.') 15 | parser.add_argument('outputbag', 16 | help='output bag file with topics merged') 17 | parser.add_argument('inputbag', nargs='+', 18 | help='input bag files') 19 | parser.add_argument('-v', '--verbose', action="store_true", default=False, 20 | help='verbose output') 21 | parser.add_argument('-t', '--topics', default="*", 22 | help='string interpreted as a list of topics (wildcards \'*\' and \'?\' allowed) to include in the merged bag file') 23 | 24 | args = parser.parse_args() 25 | 26 | topics = args.topics.split(' ') 27 | 28 | total_included_count = 0 29 | total_skipped_count = 0 30 | 31 | if (args.verbose): 32 | print("Writing bag file: " + args.outputbag) 33 | print("Matching topics against patters: '%s'" % ' '.join(topics)) 34 | 35 | with Bag(args.outputbag, 'w') as o: 36 | for ifile in args.inputbag: 37 | matchedtopics = [] 38 | included_count = 0 39 | skipped_count = 0 40 | if (args.verbose): 41 | print("> Reading bag file: " + ifile) 42 | with Bag(ifile, 'r') as ib: 43 | for topic, msg, t in ib: 44 | if any(fnmatchcase(topic, pattern) for pattern in topics): 45 | if not topic in matchedtopics: 46 | matchedtopics.append(topic) 47 | if (args.verbose): 48 | print("Including matched topic '%s'" % topic) 49 | o.write(topic, msg, t) 50 | included_count += 1 51 | else: 52 | skipped_count += 1 53 | total_included_count += included_count 54 | total_skipped_count += skipped_count 55 | if (args.verbose): 56 | print("< Included %d messages and skipped %d" % (included_count, skipped_count)) 57 | 58 | if (args.verbose): 59 | print("Total: Included %d messages and skipped %d" % (total_included_count, total_skipped_count)) 60 | 61 | if __name__ == "__main__": 62 | main() 63 | 64 | -------------------------------------------------------------------------------- /src/kaist_tool/kaist2bag/src/altimeter_converter.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by tao on 7/11/21. 3 | // 4 | 5 | #include "altimeter_converter.h" 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | namespace kaist2bag { 12 | 13 | AltimeterConverter::AltimeterConverter(const std::string &dataset_dir, const std::string &save_dir, 14 | const std::string &topic) 15 | : Converter(dataset_dir, save_dir), topic_(topic) { 16 | bag_name_ = FilterSlash(topic_) + ".bag"; 17 | } 18 | 19 | int AltimeterConverter::Convert() { 20 | CheckAndCreateSaveDir(); 21 | 22 | boost::filesystem::path bag_file = boost::filesystem::path(save_dir_) / bag_name_; 23 | rosbag::Bag bag(bag_file.string(), rosbag::bagmode::Write); 24 | ROS_INFO("saving %s", bag_file.c_str()); 25 | 26 | const std::string data_file = dataset_dir_ + "/" + default_data_file; 27 | FILE* fp = fopen(data_file.c_str(), "r"); 28 | irp_sen_msgs::altimeter altimeter_data; 29 | int64_t stamp; 30 | double altimeter_value; 31 | while (fscanf(fp, "%ld,%lf\n", &stamp, &altimeter_value) == 2) { 32 | altimeter_data.header.stamp.fromNSec(stamp); 33 | altimeter_data.header.frame_id = "altimeter"; 34 | altimeter_data.data = altimeter_value; 35 | 36 | bag.write(topic_, altimeter_data.header.stamp, altimeter_data); 37 | } 38 | bag.close(); 39 | fclose(fp); 40 | ROS_INFO("done saving %s", bag_file.c_str()); 41 | 42 | return 0; 43 | } 44 | 45 | } // namespace kaist2bag -------------------------------------------------------------------------------- /src/kaist_tool/kaist2bag/src/altimeter_converter.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by tao on 7/11/21. 3 | // 4 | 5 | #ifndef SRC_ALTIMETER_CONVERTER_H 6 | #define SRC_ALTIMETER_CONVERTER_H 7 | 8 | #include 9 | #include "converter.h" 10 | 11 | namespace kaist2bag { 12 | 13 | class AltimeterConverter : public Converter { 14 | public: 15 | AltimeterConverter(const std::string& dataset_dir, const std::string& save_dir, const std::string& topic); 16 | virtual ~AltimeterConverter() = default; 17 | 18 | int Convert() override; 19 | 20 | std::string default_data_file = "sensor_data/altimeter.csv"; 21 | 22 | private: 23 | std::string topic_; 24 | std::string bag_name_; 25 | }; 26 | 27 | 28 | } // namespace kaist2bag 29 | 30 | #endif //SRC_ALTIMETER_CONVERTER_H 31 | -------------------------------------------------------------------------------- /src/kaist_tool/kaist2bag/src/converter.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by tao on 7/11/21. 3 | // 4 | 5 | #include "converter.h" 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | namespace kaist2bag { 12 | 13 | std::string FilterSlash(const std::string& input) { 14 | if (input.empty()) return std::string(); 15 | std::string str = input; 16 | if (*str.begin() == '/') { 17 | str.erase(str.begin()); 18 | if (str.empty()) return std::string(); 19 | } 20 | 21 | if (*(str.end() - 1) == '/') { 22 | str.erase(str.end() - 1); 23 | if (str.empty()) return std::string(); 24 | } 25 | std::replace(str.begin(), str.end(), '/', '_'); 26 | return str; 27 | } 28 | 29 | Converter::Converter(const std::string &dataset_dir, const std::string &save_dir) 30 | : dataset_dir_(dataset_dir), save_dir_(save_dir){ 31 | struct stat buffer; 32 | dir_valid_ = (stat(dataset_dir_.c_str(), &buffer) == 0); 33 | } 34 | 35 | int Converter::Convert() { 36 | return 0; 37 | } 38 | 39 | bool Converter::DirValid() { 40 | return dir_valid_; 41 | } 42 | 43 | void Converter::CheckAndCreateSaveDir() { 44 | if (!boost::filesystem::is_directory(save_dir_) || !boost::filesystem::exists(save_dir_)) { 45 | boost::filesystem::create_directory(save_dir_); 46 | } 47 | } 48 | 49 | } // namespace kaist2bag -------------------------------------------------------------------------------- /src/kaist_tool/kaist2bag/src/converter.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by tao on 7/11/21. 3 | // 4 | 5 | #ifndef SRC_CONVERTER_H 6 | #define SRC_CONVERTER_H 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | namespace kaist2bag { 13 | 14 | std::string FilterSlash(const std::string& input); 15 | 16 | class Converter { 17 | public: 18 | Converter(const std::string& dataset_dir, const std::string& save_dir); 19 | virtual ~Converter() = default; 20 | 21 | virtual int Convert(); 22 | virtual bool DirValid(); 23 | 24 | protected: 25 | void CheckAndCreateSaveDir(); 26 | 27 | protected: 28 | std::string dataset_dir_; 29 | std::string save_dir_; 30 | bool dir_valid_; 31 | }; 32 | 33 | 34 | } // namespace kaist2bag 35 | 36 | #endif //SRC_CONVERTER_H 37 | -------------------------------------------------------------------------------- /src/kaist_tool/kaist2bag/src/encoder_converter.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by tao on 7/21/21. 3 | // 4 | 5 | #ifndef SRC_ENCODER_CONVERTER_H 6 | #define SRC_ENCODER_CONVERTER_H 7 | 8 | #include "converter.h" 9 | 10 | namespace kaist2bag { 11 | 12 | class EncoderConverter : public Converter { 13 | public: 14 | // EncoderConverter(const std::string& dataset_dir, const std::string& save_dir, const std::string &irp_topic, const std::string &raw_topic); 15 | EncoderConverter(const std::string& dataset_dir, const std::string& save_dir, const std::string &wheel_odom_topic); 16 | virtual ~EncoderConverter() = default; 17 | 18 | int Convert() override; 19 | 20 | std::string default_data_file = "sensor_data/encoder.csv"; 21 | std::string default_calib_file = "calibration/EncoderParameter.txt"; 22 | 23 | private: 24 | std::string wheel_odom_topic_; 25 | std::string wheel_odom_bag_name_; 26 | // std::string irp_topic_; 27 | // std::string raw_topic_; 28 | // std::string irp_bag_name_; 29 | // std::string raw_bag_name_; 30 | }; 31 | 32 | 33 | 34 | 35 | } // namespace kaist2bag 36 | 37 | #endif //SRC_ENCODER_CONVERTER_H 38 | -------------------------------------------------------------------------------- /src/kaist_tool/kaist2bag/src/fog_converter.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by tao on 7/21/21. 3 | // 4 | 5 | #include "fog_converter.h" 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | namespace kaist2bag { 12 | 13 | FogConverter::FogConverter(const std::string &dataset_dir, const std::string &save_dir, 14 | const std::string &topic) 15 | : Converter(dataset_dir, save_dir), topic_(topic) { 16 | bag_name_ = FilterSlash(topic_) + ".bag"; 17 | } 18 | 19 | int FogConverter::Convert() { 20 | CheckAndCreateSaveDir(); 21 | 22 | boost::filesystem::path bag_file = boost::filesystem::path(save_dir_) / bag_name_; 23 | rosbag::Bag bag(bag_file.string(), rosbag::bagmode::Write); 24 | ROS_INFO("saving %s", bag_file.c_str()); 25 | 26 | const std::string data_file = dataset_dir_ + "/" + default_data_file; 27 | FILE* fp = fopen(data_file.c_str(), "r"); 28 | irp_sen_msgs::fog_3axis fog_data; 29 | int64_t stamp; 30 | float roll, pitch, yaw; 31 | while (fscanf(fp, "%ld,%f,%f,%f\n", &stamp, &roll, &pitch, &yaw) == 4) { 32 | fog_data.header.stamp.fromNSec(stamp); 33 | fog_data.header.frame_id = "dsp1760"; 34 | fog_data.d_roll = roll; 35 | fog_data.d_pitch = pitch; 36 | fog_data.d_yaw = yaw; 37 | 38 | bag.write(topic_, fog_data.header.stamp, fog_data); 39 | } 40 | bag.close(); 41 | fclose(fp); 42 | ROS_INFO("done saving %s", bag_file.c_str()); 43 | 44 | return 0; 45 | } 46 | 47 | } // namespace kaist2bag -------------------------------------------------------------------------------- /src/kaist_tool/kaist2bag/src/fog_converter.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by tao on 7/21/21. 3 | // 4 | 5 | #ifndef SRC_FOG_CONVERTER_H 6 | #define SRC_FOG_CONVERTER_H 7 | 8 | #include 9 | #include "converter.h" 10 | 11 | namespace kaist2bag { 12 | 13 | class FogConverter : public Converter { 14 | public: 15 | FogConverter(const std::string& dataset_dir, const std::string& save_dir, const std::string& topic); 16 | virtual ~FogConverter() = default; 17 | 18 | int Convert() override; 19 | 20 | std::string default_data_file = "sensor_data/fog.csv"; 21 | 22 | private: 23 | std::string topic_; 24 | std::string bag_name_; 25 | }; 26 | 27 | 28 | } // namespace kaist2bag 29 | 30 | #endif //SRC_FOG_CONVERTER_H 31 | -------------------------------------------------------------------------------- /src/kaist_tool/kaist2bag/src/gps_converter.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by tao on 7/22/21. 3 | // 4 | 5 | #include "gps_converter.h" 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | namespace kaist2bag { 12 | 13 | GpsConverter::GpsConverter(const std::string &dataset_dir, const std::string &save_dir, 14 | const std::string &topic) 15 | : Converter(dataset_dir, save_dir), topic_(topic) { 16 | bag_name_ = FilterSlash(topic_) + ".bag"; 17 | } 18 | 19 | int GpsConverter::Convert() { 20 | CheckAndCreateSaveDir(); 21 | 22 | boost::filesystem::path bag_file = boost::filesystem::path(save_dir_) / bag_name_; 23 | rosbag::Bag bag(bag_file.string(), rosbag::bagmode::Write); 24 | ROS_INFO("saving %s", bag_file.c_str()); 25 | 26 | const std::string data_file = dataset_dir_ + "/" + default_data_file; 27 | FILE* fp = fopen(data_file.c_str(), "r"); 28 | sensor_msgs::NavSatFix gps_data; 29 | int64_t stamp; 30 | double latitude, longitude, altitude; 31 | double cov[9]; 32 | while (fscanf(fp,"%ld,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf\n", 33 | &stamp,&latitude,&longitude,&altitude, 34 | &cov[0],&cov[1],&cov[2],&cov[3],&cov[4], 35 | &cov[5],&cov[6],&cov[7],&cov[8]) == 13) { 36 | gps_data.header.stamp.fromNSec(stamp); 37 | gps_data.header.frame_id = "gps"; 38 | gps_data.latitude = latitude; 39 | gps_data.longitude = longitude; 40 | gps_data.altitude = altitude; 41 | for (int i = 0; i < 9; ++i) { 42 | gps_data.position_covariance[i] = cov[i]; 43 | } 44 | 45 | bag.write(topic_, gps_data.header.stamp, gps_data); 46 | } 47 | bag.close(); 48 | fclose(fp); 49 | ROS_INFO("done saving %s", bag_file.c_str()); 50 | 51 | return 0; 52 | } 53 | 54 | } // namespace kaist2bag -------------------------------------------------------------------------------- /src/kaist_tool/kaist2bag/src/gps_converter.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by tao on 7/22/21. 3 | // 4 | 5 | #ifndef SRC_GPS_CONVERTER_H 6 | #define SRC_GPS_CONVERTER_H 7 | 8 | #include 9 | #include "converter.h" 10 | 11 | namespace kaist2bag { 12 | 13 | class GpsConverter : public Converter { 14 | public: 15 | GpsConverter(const std::string& dataset_dir, const std::string& save_dir, const std::string& topic); 16 | virtual ~GpsConverter() = default; 17 | 18 | int Convert() override; 19 | 20 | std::string default_data_file = "sensor_data/gps.csv"; 21 | 22 | private: 23 | std::string topic_; 24 | std::string bag_name_; 25 | }; 26 | 27 | 28 | } // namespace kaist2bag 29 | 30 | #endif //SRC_GPS_CONVERTER_H 31 | -------------------------------------------------------------------------------- /src/kaist_tool/kaist2bag/src/imu_converter.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by tao on 7/24/21. 3 | // 4 | 5 | #include "imu_converter.h" 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace kaist2bag { 14 | 15 | ImuConverter::ImuConverter(const std::string& dataset_dir, const std::string& save_dir, 16 | const std::string& raw_topic) 17 | : Converter(dataset_dir, save_dir), raw_topic_(raw_topic) 18 | { 19 | // irp_bag_name_ = FilterSlash(irp_topic_) + ".bag"; 20 | raw_bag_name_ = FilterSlash(raw_topic_) + ".bag"; 21 | // mag_bag_name_ = FilterSlash(mag_topic_) + ".bag"; 22 | } 23 | 24 | int ImuConverter::Convert() { 25 | CheckAndCreateSaveDir(); 26 | 27 | // boost::filesystem::path irp_bag_file = boost::filesystem::path(save_dir_) / irp_bag_name_; 28 | boost::filesystem::path raw_bag_file = boost::filesystem::path(save_dir_) / raw_bag_name_; 29 | // boost::filesystem::path mag_bag_file = boost::filesystem::path(save_dir_) / mag_bag_name_; 30 | // rosbag::Bag irp_bag(irp_bag_file.string(), rosbag::bagmode::Write); 31 | rosbag::Bag raw_bag(raw_bag_file.string(), rosbag::bagmode::Write); 32 | // rosbag::Bag mag_bag(mag_bag_file.string(), rosbag::bagmode::Write); 33 | // ROS_INFO("saving %s", irp_bag_file.c_str()); 34 | ROS_INFO("saving %s", raw_bag_file.c_str()); 35 | // ROS_INFO("saving %s", mag_bag_file.c_str()); 36 | 37 | const std::string data_file = dataset_dir_ + "/" + default_data_file; 38 | FILE* fp = fopen(data_file.c_str(), "r"); 39 | // irp_sen_msgs::imu irp_imu; 40 | sensor_msgs::Imu sensor_msgs_imu; 41 | // sensor_msgs::MagneticField sensor_msgs_mag; 42 | 43 | 44 | 45 | int64_t stamp; 46 | double q_x,q_y,q_z,q_w,x,y,z,g_x,g_y,g_z,a_x,a_y,a_z,m_x,m_y,m_z; 47 | while (fscanf(fp,"%ld,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf\n", 48 | &stamp,&q_x,&q_y,&q_z,&q_w,&x,&y,&z,&g_x,&g_y,&g_z,&a_x,&a_y,&a_z,&m_x,&m_y,&m_z) == 17) { 49 | // irp_imu.header.stamp.fromNSec(stamp); 50 | // irp_imu.header.frame_id = "imu"; 51 | // irp_imu.quaternion_data.x = q_x; 52 | // irp_imu.quaternion_data.y = q_y; 53 | // irp_imu.quaternion_data.z = q_z; 54 | // irp_imu.quaternion_data.w = q_w; 55 | // irp_imu.eular_data.x = x; 56 | // irp_imu.eular_data.y = y; 57 | // irp_imu.eular_data.z = z; 58 | // irp_imu.gyro_data.x = g_x; 59 | // irp_imu.gyro_data.y = g_y; 60 | // irp_imu.gyro_data.z = g_z; 61 | // irp_imu.acceleration_data.x = a_x; 62 | // irp_imu.acceleration_data.y = a_y; 63 | // irp_imu.acceleration_data.z = a_z; 64 | // irp_imu.magneticfield_data.x = m_x; 65 | // irp_imu.magneticfield_data.y = m_y; 66 | // irp_imu.magneticfield_data.z = m_z; 67 | 68 | sensor_msgs_imu.header.stamp.fromNSec(stamp); 69 | sensor_msgs_imu.header.frame_id = "imu"; 70 | sensor_msgs_imu.orientation.x = q_x; 71 | sensor_msgs_imu.orientation.y = q_y; 72 | sensor_msgs_imu.orientation.z = q_z; 73 | sensor_msgs_imu.orientation.w = q_w; 74 | sensor_msgs_imu.angular_velocity.x = g_x; 75 | sensor_msgs_imu.angular_velocity.y = g_y; 76 | sensor_msgs_imu.angular_velocity.z = g_z; 77 | sensor_msgs_imu.linear_acceleration.x = a_x; 78 | sensor_msgs_imu.linear_acceleration.y = a_y; 79 | sensor_msgs_imu.linear_acceleration.z = a_z; 80 | sensor_msgs_imu.orientation_covariance[0] = 3; 81 | sensor_msgs_imu.orientation_covariance[4] = 3; 82 | sensor_msgs_imu.orientation_covariance[8] = 3; 83 | sensor_msgs_imu.angular_velocity_covariance[0] = 3; 84 | sensor_msgs_imu.angular_velocity_covariance[4] = 3; 85 | sensor_msgs_imu.angular_velocity_covariance[8] = 3; 86 | sensor_msgs_imu.linear_acceleration_covariance[0] = 3; 87 | sensor_msgs_imu.linear_acceleration_covariance[4] = 3; 88 | sensor_msgs_imu.linear_acceleration_covariance[8] = 3; 89 | 90 | // sensor_msgs_mag.header.stamp.fromNSec(stamp); 91 | // sensor_msgs_mag.header.frame_id = "imu"; 92 | // sensor_msgs_mag.magnetic_field.x = m_x; 93 | // sensor_msgs_mag.magnetic_field.y = m_y; 94 | // sensor_msgs_mag.magnetic_field.z = m_z; 95 | 96 | // irp_bag.write(irp_topic_, irp_imu.header.stamp, irp_imu); 97 | raw_bag.write(raw_topic_, sensor_msgs_imu.header.stamp, sensor_msgs_imu); 98 | // mag_bag.write(mag_topic_, sensor_msgs_mag.header.stamp, sensor_msgs_mag); 99 | } 100 | // irp_bag.close(); 101 | raw_bag.close(); 102 | // mag_bag.close(); 103 | fclose(fp); 104 | // ROS_INFO("done saving %s", irp_bag_file.c_str()); 105 | ROS_INFO("done saving %s", raw_bag_file.c_str()); 106 | // ROS_INFO("done saving %s", mag_bag_file.c_str()); 107 | 108 | return 0; 109 | } 110 | 111 | } // namespace kaist2bag -------------------------------------------------------------------------------- /src/kaist_tool/kaist2bag/src/imu_converter.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by tao on 7/24/21. 3 | // 4 | 5 | #ifndef SRC_IMU_CONVERTER_H 6 | #define SRC_IMU_CONVERTER_H 7 | #include 8 | #include "converter.h" 9 | 10 | namespace kaist2bag { 11 | 12 | class ImuConverter : public Converter { 13 | public: 14 | // ImuConverter(const std::string& dataset_dir, const std::string& save_dir, 15 | // const std::string& irp_topic, const std::string& raw_topic, 16 | // const std::string& mag_topic); 17 | 18 | ImuConverter(const std::string& dataset_dir, const std::string& save_dir, 19 | const std::string& raw_topic); 20 | 21 | virtual ~ImuConverter() = default; 22 | 23 | int Convert() override; 24 | 25 | std::string default_data_file = "sensor_data/xsens_imu.csv"; 26 | 27 | private: 28 | // std::string irp_topic_; 29 | std::string raw_topic_; 30 | // std::string mag_topic_; 31 | // std::string irp_bag_name_; 32 | std::string raw_bag_name_; 33 | // std::string mag_bag_name_; 34 | }; 35 | 36 | 37 | } // namespace kaist2bag 38 | #endif //SRC_IMU_CONVERTER_H 39 | -------------------------------------------------------------------------------- /src/kaist_tool/kaist2bag/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "altimeter_converter.h" 5 | #include "encoder_converter.h" 6 | #include "fog_converter.h" 7 | #include "gps_converter.h" 8 | #include "vrs_converter.h" 9 | #include "imu_converter.h" 10 | #include "velodyne_converter.h" 11 | #include "sick_converter.h" 12 | #include "stereo_converter.h" 13 | 14 | using namespace kaist2bag; 15 | 16 | enum SensorType { 17 | kAltimeter = 0, 18 | kEncoder, 19 | kFog, 20 | kGps, 21 | kVrs, 22 | kImu, 23 | kVelodyne, 24 | kSick, 25 | kStereo, 26 | kSensorTypeCount 27 | }; 28 | 29 | int main(int argc, char** argv) { 30 | ros::init(argc, argv, "kaist2bag"); 31 | 32 | ros::NodeHandle nh; 33 | std::vector sensors(kSensorTypeCount, false); 34 | std::vector sensor_names = { 35 | "altimeter", 36 | "encoder", 37 | "fog", 38 | "gps", 39 | "vrs", 40 | "imu", 41 | "velodyne", 42 | "sick", 43 | "stereo" 44 | }; 45 | 46 | for (size_t i = 0; i < sensors.size(); ++i) { 47 | bool sensor_on = false; 48 | nh.getParam(sensor_names[i], sensor_on); 49 | sensors[i] = sensor_on; 50 | } 51 | 52 | std::string dataset; 53 | nh.getParam("dataset", dataset); 54 | std::string save_to; 55 | nh.getParam("save_to", save_to); 56 | double t0 = ros::Time::now().toSec(); 57 | 58 | 59 | if(sensors[SensorType::kAltimeter]) { 60 | std::string altimeter_topic; 61 | nh.getParam("altimeter_topic", altimeter_topic); 62 | AltimeterConverter altimeter(dataset, save_to, altimeter_topic); 63 | altimeter.Convert(); 64 | } 65 | double t1 = ros::Time::now().toSec(); 66 | double altimeter_cost = t1 - t0; 67 | 68 | 69 | if(sensors[SensorType::kEncoder]) { 70 | std::string wheel_odom_topic; 71 | nh.getParam("wheel_odom_topic", wheel_odom_topic); 72 | // std::string encoder_irp_topic, encoder_raw_topic; 73 | // nh.getParam("encoder_irp_topic", encoder_irp_topic); 74 | // nh.getParam("encoder_raw_topic", encoder_raw_topic); 75 | EncoderConverter encoder(dataset, save_to, wheel_odom_topic); 76 | // EncoderConverter encoder(dataset, save_to, encoder_irp_topic, encoder_raw_topic); 77 | encoder.Convert(); 78 | } 79 | double t2 = ros::Time::now().toSec(); 80 | double encoder_cost = t2 - t1; 81 | 82 | 83 | if(sensors[SensorType::kFog]) { 84 | std::string fog_topic; 85 | nh.getParam("fog_topic", fog_topic); 86 | FogConverter fog(dataset, save_to, fog_topic); 87 | fog.Convert(); 88 | } 89 | double t3 = ros::Time::now().toSec(); 90 | double fog_cost = t3 - t2; 91 | 92 | 93 | if(sensors[SensorType::kGps]) { 94 | std::string gps_topic; 95 | nh.getParam("gps_topic", gps_topic); 96 | GpsConverter gps(dataset, save_to, gps_topic); 97 | gps.Convert(); 98 | } 99 | double t4 = ros::Time::now().toSec(); 100 | double gps_cost = t4 - t3; 101 | 102 | 103 | if(sensors[SensorType::kVrs]) { 104 | std::string vrs_topic; 105 | nh.getParam("vrs_topic", vrs_topic); 106 | VrsConverter vrs(dataset, save_to, vrs_topic); 107 | vrs.Convert(); 108 | } 109 | double t5 = ros::Time::now().toSec(); 110 | double vrs_cost = t5 - t4; 111 | 112 | 113 | if(sensors[SensorType::kImu]) { 114 | std::string irp_topic, raw_topic, mag_topic; 115 | // nh.getParam("imu_irp_topic", irp_topic); 116 | nh.getParam("imu_raw_topic", raw_topic); 117 | // nh.getParam("imu_mag_topic", mag_topic); 118 | ImuConverter imu(dataset, save_to, raw_topic); 119 | // ImuConverter imu(dataset, save_to, irp_topic, raw_topic, mag_topic); 120 | imu.Convert(); 121 | } 122 | double t6 = ros::Time::now().toSec(); 123 | double imu_cost = t6 - t5; 124 | 125 | 126 | if(sensors[SensorType::kVelodyne]) { 127 | std::string vlp_left_topic, vlp_right_topic; 128 | nh.getParam("velodyne_left_topic", vlp_left_topic); 129 | nh.getParam("velodyne_right_topic", vlp_right_topic); 130 | VelodyneConverter vlp(dataset, save_to, vlp_left_topic, vlp_right_topic); 131 | vlp.Convert(); 132 | } 133 | double t7 = ros::Time::now().toSec(); 134 | double velodyne_cost = t7 - t6; 135 | 136 | 137 | if(sensors[SensorType::kSick]) { 138 | std::string sick_back_topic, sick_middle_topic; 139 | nh.getParam("sick_back_topic", sick_back_topic); 140 | nh.getParam("sick_middle_topic", sick_middle_topic); 141 | SickConverter sick(dataset, save_to, sick_back_topic, sick_middle_topic); 142 | sick.Convert(); 143 | } 144 | double t8 = ros::Time::now().toSec(); 145 | double sick_cost = t8 - t7; 146 | 147 | 148 | if(sensors[SensorType::kStereo]) { 149 | std::string stereo_left_topic, stereo_right_topic; 150 | nh.getParam("stereo_left_topic", stereo_left_topic); 151 | nh.getParam("stereo_right_topic", stereo_right_topic); 152 | StereoConverter stereo(dataset, save_to, stereo_left_topic, stereo_right_topic); 153 | stereo.Convert(); 154 | } 155 | double t9 = ros::Time::now().toSec(); 156 | double stereo_cost = t9 - t8; 157 | 158 | 159 | double all_cost = t9 - t0; 160 | ROS_INFO("altimeter_cost %f\n", altimeter_cost); 161 | ROS_INFO("encoder_cost %f\n", encoder_cost); 162 | ROS_INFO("fog_cost %f\n", fog_cost); 163 | ROS_INFO("gps_cost %f\n", gps_cost); 164 | ROS_INFO("vrs_cost %f\n", vrs_cost); 165 | ROS_INFO("imu_cost %f\n", imu_cost); 166 | ROS_INFO("velodyne_cost %f\n", velodyne_cost); 167 | ROS_INFO("sick_cost %f\n", sick_cost); 168 | ROS_INFO("stereo_cost %f\n", stereo_cost); 169 | ROS_INFO("all_cost %f\n", all_cost); 170 | 171 | return 0; 172 | } -------------------------------------------------------------------------------- /src/kaist_tool/kaist2bag/src/sick_converter.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by tao on 7/28/21. 3 | // 4 | 5 | #include "sick_converter.h" 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace kaist2bag { 13 | 14 | SickConverter::SickConverter(const std::string &dataset_dir, const std::string &save_dir, 15 | const std::string &back_topic, const std::string& middle_topic) 16 | : Converter(dataset_dir, save_dir), back_topic_(back_topic), middle_topic_(middle_topic) { 17 | back_bag_name_ = FilterSlash(back_topic_) + ".bag"; 18 | middle_bag_name_ = FilterSlash(middle_topic_) + ".bag"; 19 | } 20 | 21 | int SickConverter::Convert() { 22 | CheckAndCreateSaveDir(); 23 | 24 | boost::filesystem::path back_bag_file = boost::filesystem::path(save_dir_) / back_bag_name_; 25 | boost::filesystem::path middle_bag_file = boost::filesystem::path(save_dir_) / middle_bag_name_; 26 | 27 | const std::string back_stamp_file = dataset_dir_ + "/" + default_back_stamp_file; 28 | const std::string back_data_dir = dataset_dir_ + "/" + default_back_data_dir; 29 | const std::string middle_stamp_file = dataset_dir_ + "/" + default_middle_stamp_file; 30 | const std::string middle_data_dir = dataset_dir_ + "/" + default_middle_data_dir; 31 | 32 | ROS_INFO("saving %s", back_bag_file.c_str()); 33 | Convert(back_stamp_file, back_data_dir, back_bag_file.string(), back_topic_, "back_sick"); 34 | ROS_INFO("done saving %s", back_bag_file.c_str()); 35 | ROS_INFO("saving %s", middle_bag_file.c_str()); 36 | Convert(middle_stamp_file, middle_data_dir, middle_bag_file.string(), middle_topic_, "middle_sick"); 37 | ROS_INFO("done saving %s", middle_bag_file.c_str()); 38 | 39 | return 0; 40 | } 41 | 42 | void SickConverter::Convert(const std::string& stamp_file, const std::string& data_dir, const std::string& bag_file, 43 | const std::string& topic, const std::string& frame_id) { 44 | rosbag::Bag bag(bag_file, rosbag::bagmode::Write); 45 | bag.setChunkThreshold(768*1024); 46 | bag.setCompression(rosbag::compression::BZ2); 47 | 48 | FILE* fp = fopen(stamp_file.c_str(), "r"); 49 | int64_t stamp; 50 | std::vector all_stamps; 51 | while (fscanf(fp, "%ld\n", &stamp) == 1) { 52 | all_stamps.push_back(stamp); 53 | } 54 | fclose(fp); 55 | 56 | size_t total = all_stamps.size(); 57 | for (size_t i = 0; i < all_stamps.size(); ++i) { 58 | std::string st = std::to_string(all_stamps[i]); 59 | std::string frame_file = data_dir + "/" + st + ".bin"; 60 | ROS_INFO("converting %s\n", frame_file.c_str()); 61 | if (!boost::filesystem::exists(frame_file)) { 62 | ROS_WARN("%s not exist\n", frame_file.c_str()); 63 | continue; 64 | } 65 | std::ifstream file; 66 | file.open(frame_file, std::ios::in | std::ios::binary); 67 | irp_sen_msgs::LaserScanArray scan_array; 68 | sensor_msgs::LaserScan scan; 69 | while (!file.eof()) { 70 | float range; 71 | float intensity; 72 | file.read(reinterpret_cast(&range), sizeof(float)); 73 | file.read(reinterpret_cast(&intensity), sizeof(float)); 74 | scan.ranges.push_back(range); 75 | scan.intensities.push_back((intensity)); 76 | } 77 | file.close(); 78 | scan.header.stamp.fromNSec(all_stamps[i]); 79 | scan.header.frame_id = frame_id; 80 | scan.angle_min = -1.65806281567; 81 | scan.angle_max = -1.65806281567; 82 | scan.angle_increment = 0.0116355288774; 83 | scan.time_increment = 0.0; 84 | scan.range_min = 0.0; 85 | scan.range_max = 81.0; 86 | scan_array.LaserScans.push_back(scan); 87 | scan_array.size = scan_array.LaserScans.size(); 88 | 89 | scan_array.header.stamp.fromNSec(all_stamps[i]); 90 | scan_array.header.frame_id = "back_sick"; 91 | bag.write(topic, scan_array.header.stamp, scan_array); 92 | ROS_INFO("done converting %s\n", frame_file.c_str()); 93 | ROS_INFO("total %lu, already convert %lu, remain %lu\n", total, i + 1, total - i - 1); 94 | } 95 | bag.close(); 96 | } 97 | 98 | } // namespace kaist2bag -------------------------------------------------------------------------------- /src/kaist_tool/kaist2bag/src/sick_converter.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by tao on 7/28/21. 3 | // 4 | 5 | #ifndef SRC_SICK_CONVERTER_H 6 | #define SRC_SICK_CONVERTER_H 7 | #include 8 | #include "converter.h" 9 | 10 | namespace kaist2bag { 11 | 12 | class SickConverter : public Converter { 13 | public: 14 | SickConverter(const std::string& dataset_dir, const std::string& save_dir, 15 | const std::string& back_topic, const std::string& middle_topic); 16 | virtual ~SickConverter() = default; 17 | 18 | int Convert() override; 19 | 20 | std::string default_back_stamp_file = "sensor_data/SICK_back_stamp.csv"; 21 | std::string default_back_data_dir = "sensor_data/SICK_back"; 22 | std::string default_middle_stamp_file = "sensor_data/SICK_middle_stamp.csv"; 23 | std::string default_middle_data_dir = "sensor_data/SICK_middle"; 24 | 25 | private: 26 | std::string back_topic_; 27 | std::string back_bag_name_; 28 | std::string middle_topic_; 29 | std::string middle_bag_name_; 30 | 31 | void Convert(const std::string& stamp_file, const std::string& data_dir, 32 | const std::string& bag_file, const std::string& topic, 33 | const std::string& frame_id); 34 | }; 35 | 36 | 37 | } // namespace kaist2bag 38 | #endif //SRC_SICK_CONVERTER_H 39 | -------------------------------------------------------------------------------- /src/kaist_tool/kaist2bag/src/stereo_converter.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by tao on 7/28/21. 3 | // 4 | 5 | #include "stereo_converter.h" 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | // #include 15 | #include 16 | 17 | namespace kaist2bag { 18 | 19 | StereoConverter::StereoConverter(const std::string &dataset_dir, const std::string &save_dir, 20 | const std::string &left_topic, const std::string& right_topic) 21 | : Converter(dataset_dir, save_dir), left_topic_(left_topic), right_topic_(right_topic) { 22 | left_bag_name_ = FilterSlash(left_topic_) + ".bag"; 23 | right_bag_name_ = FilterSlash(right_topic_) + ".bag"; 24 | } 25 | 26 | int StereoConverter::Convert() { 27 | CheckAndCreateSaveDir(); 28 | 29 | boost::filesystem::path left_bag_file = boost::filesystem::path(save_dir_) / left_bag_name_; 30 | boost::filesystem::path right_bag_file = boost::filesystem::path(save_dir_) / right_bag_name_; 31 | 32 | const std::string stamp_file = dataset_dir_ + "/" + default_stamp_file; 33 | const std::string left_data_dir = dataset_dir_ + "/" + default_left_data_dir; 34 | const std::string right_data_dir = dataset_dir_ + "/" + default_right_data_dir; 35 | 36 | ROS_INFO("saving %s", left_bag_file.c_str()); 37 | Convert(stamp_file, left_data_dir, left_bag_file.string(), left_topic_, "/stereo/left"); 38 | ROS_INFO("done saving %s", left_bag_file.c_str()); 39 | ROS_INFO("saving %s", right_bag_file.c_str()); 40 | Convert(stamp_file, right_data_dir, right_bag_file.string(), right_topic_, "/stereo/right"); 41 | ROS_INFO("done saving %s", right_bag_file.c_str()); 42 | 43 | return 0; 44 | } 45 | 46 | void StereoConverter::Convert(const std::string& stamp_file, const std::string& data_dir, const std::string& bag_file, 47 | const std::string& topic, const std::string& frame_id) { 48 | rosbag::Bag bag(bag_file, rosbag::bagmode::Write); 49 | bag.setChunkThreshold(768*1024); 50 | bag.setCompression(rosbag::compression::BZ2); 51 | 52 | FILE* fp = fopen(stamp_file.c_str(), "r"); 53 | int64_t stamp; 54 | std::vector all_stamps; 55 | while (fscanf(fp, "%ld\n", &stamp) == 1) { 56 | all_stamps.push_back(stamp); 57 | } 58 | fclose(fp); 59 | 60 | size_t total = all_stamps.size(); 61 | for (size_t i = 0; i < all_stamps.size(); ++i) { 62 | std::string st = std::to_string(all_stamps[i]); 63 | std::string frame_file = data_dir + "/" + st + ".png"; 64 | ROS_INFO("converting %s\n", frame_file.c_str()); 65 | if (!boost::filesystem::exists(frame_file)) { 66 | ROS_WARN("%s not exist\n", frame_file.c_str()); 67 | continue; 68 | } 69 | cv::Mat img = cv::imread(frame_file, CV_LOAD_IMAGE_ANYDEPTH); 70 | cv_bridge::CvImage img_msg; 71 | img_msg.header.stamp.fromNSec(all_stamps[i]); 72 | img_msg.header.frame_id = frame_id; 73 | img_msg.encoding = sensor_msgs::image_encodings::BAYER_BGGR8; 74 | img_msg.image = img; 75 | sensor_msgs::Image msg; 76 | img_msg.toImageMsg(msg); 77 | bag.write(topic, msg.header.stamp, msg); 78 | 79 | ROS_INFO("done converting %s\n", frame_file.c_str()); 80 | ROS_INFO("total %lu, already convert %lu, remain %lu\n", total, i + 1, total - i - 1); 81 | } 82 | bag.close(); 83 | } 84 | 85 | } // namespace kaist2bag -------------------------------------------------------------------------------- /src/kaist_tool/kaist2bag/src/stereo_converter.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by tao on 7/28/21. 3 | // 4 | 5 | #ifndef SRC_STEREO_CONVERTER_H 6 | #define SRC_STEREO_CONVERTER_H 7 | #include 8 | #include "converter.h" 9 | 10 | namespace kaist2bag { 11 | 12 | class StereoConverter : public Converter { 13 | public: 14 | StereoConverter(const std::string& dataset_dir, const std::string& save_dir, 15 | const std::string& left_topic, const std::string& right_topic); 16 | virtual ~StereoConverter() = default; 17 | 18 | int Convert() override; 19 | 20 | std::string default_stamp_file = "sensor_data/stereo_stamp.csv"; 21 | std::string default_left_data_dir = "image/stereo_left"; 22 | std::string default_right_data_dir = "image/stereo_right"; 23 | 24 | private: 25 | std::string left_topic_; 26 | std::string left_bag_name_; 27 | std::string right_topic_; 28 | std::string right_bag_name_; 29 | 30 | void Convert(const std::string& stamp_file, const std::string& data_dir, 31 | const std::string& bag_file, const std::string& topic, 32 | const std::string& frame_id); 33 | }; 34 | 35 | 36 | } // namespace kaist2bag 37 | #endif //SRC_STEREO_CONVERTER_H 38 | -------------------------------------------------------------------------------- /src/kaist_tool/kaist2bag/src/velodyne_converter.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by tao on 7/25/21. 3 | // 4 | 5 | #include "velodyne_converter.h" 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | namespace kaist2bag { 16 | 17 | VelodyneConverter::VelodyneConverter(const std::string &dataset_dir, const std::string &save_dir, 18 | const std::string &left_topic, const std::string& right_topic) 19 | : Converter(dataset_dir, save_dir), left_topic_(left_topic), right_topic_(right_topic) { 20 | left_bag_name_ = FilterSlash(left_topic_) + ".bag"; 21 | right_bag_name_ = FilterSlash(right_topic_) + ".bag"; 22 | } 23 | 24 | int VelodyneConverter::Convert() { 25 | CheckAndCreateSaveDir(); 26 | 27 | boost::filesystem::path left_bag_file = boost::filesystem::path(save_dir_) / left_bag_name_; 28 | boost::filesystem::path right_bag_file = boost::filesystem::path(save_dir_) / right_bag_name_; 29 | 30 | const std::string left_stamp_file = dataset_dir_ + "/" + default_left_stamp_file; 31 | const std::string left_data_dir = dataset_dir_ + "/" + default_left_data_dir; 32 | const std::string right_stamp_file = dataset_dir_ + "/" + default_right_stamp_file; 33 | const std::string right_data_dir = dataset_dir_ + "/" + default_right_data_dir; 34 | 35 | ROS_INFO("saving %s", left_bag_file.c_str()); 36 | ConvertLeft(left_stamp_file, left_data_dir, left_bag_file.string(), left_topic_, "left_velodyne"); 37 | ROS_INFO("done saving %s", left_bag_file.c_str()); 38 | ROS_INFO("saving %s", right_bag_file.c_str()); 39 | ConvertLeft(right_stamp_file, right_data_dir, right_bag_file.string(), right_topic_, "right_velodyne"); 40 | ROS_INFO("done saving %s", right_bag_file.c_str()); 41 | 42 | return 0; 43 | } 44 | 45 | void VelodyneConverter::ConvertLeft(const std::string& stamp_file, const std::string& data_dir, const std::string& bag_file, 46 | const std::string& topic, const std::string& frame_id) { 47 | rosbag::Bag bag(bag_file, rosbag::bagmode::Write); 48 | bag.setChunkThreshold(768*1024); 49 | bag.setCompression(rosbag::compression::BZ2); 50 | 51 | FILE* fp = fopen(stamp_file.c_str(), "r"); 52 | int64_t stamp; 53 | std::vector all_stamps; 54 | while (fscanf(fp, "%ld\n", &stamp) == 1) { 55 | all_stamps.push_back(stamp); 56 | } 57 | fclose(fp); 58 | 59 | size_t total = all_stamps.size(); 60 | for (size_t i = 0; i < all_stamps.size(); ++i) { 61 | std::string st = std::to_string(all_stamps[i]); 62 | std::string frame_file = data_dir + "/" + st + ".bin"; 63 | ROS_INFO("converting %s\n", frame_file.c_str()); 64 | if (!boost::filesystem::exists(frame_file)) { 65 | ROS_WARN("%s not exist\n", frame_file.c_str()); 66 | continue; 67 | } 68 | std::ifstream file; 69 | file.open(frame_file, std::ios::in | std::ios::binary); 70 | pcl::PointCloud pcl_cloud; 71 | pcl_cloud.clear(); 72 | 73 | float angle; 74 | uint16_t ring; 75 | 76 | while (!file.eof()) { 77 | PointXYZIR point; 78 | file.read(reinterpret_cast(&point.x), sizeof(float)); 79 | file.read(reinterpret_cast(&point.y), sizeof(float)); 80 | file.read(reinterpret_cast(&point.z), sizeof(float)); 81 | file.read(reinterpret_cast(&point.intensity), sizeof(float)); 82 | angle = atan2(point.z, sqrt(point.x * point.x + point.y * point.y)) / M_PI * 180 + 15; 83 | ring = round(angle / 2); 84 | point.ring = ring; 85 | pcl_cloud.points.push_back(point); 86 | } 87 | file.close(); 88 | sensor_msgs::PointCloud2 cloud; 89 | pcl::toROSMsg(pcl_cloud, cloud); 90 | cloud.header.stamp.fromNSec(all_stamps[i]); 91 | cloud.header.frame_id = frame_id; 92 | bag.write(topic, cloud.header.stamp, cloud); 93 | ROS_INFO("bag write %s, %u points\n", topic.c_str(), cloud.height * cloud.width); 94 | ROS_INFO("done converting %s\n", frame_file.c_str()); 95 | ROS_INFO("total %lu, already convert %lu, remain %lu\n", total, i + 1, total - i - 1); 96 | } 97 | bag.close(); 98 | } 99 | 100 | 101 | void VelodyneConverter::ConvertRight(const std::string& stamp_file, const std::string& data_dir, const std::string& bag_file, 102 | const std::string& topic, const std::string& frame_id) { 103 | rosbag::Bag bag(bag_file, rosbag::bagmode::Write); 104 | bag.setChunkThreshold(768*1024); 105 | bag.setCompression(rosbag::compression::BZ2); 106 | 107 | FILE* fp = fopen(stamp_file.c_str(), "r"); 108 | int64_t stamp; 109 | std::vector all_stamps; 110 | while (fscanf(fp, "%ld\n", &stamp) == 1) { 111 | all_stamps.push_back(stamp); 112 | } 113 | fclose(fp); 114 | 115 | size_t total = all_stamps.size(); 116 | for (size_t i = 0; i < all_stamps.size(); ++i) { 117 | std::string st = std::to_string(all_stamps[i]); 118 | std::string frame_file = data_dir + "/" + st + ".bin"; 119 | ROS_INFO("converting %s\n", frame_file.c_str()); 120 | if (!boost::filesystem::exists(frame_file)) { 121 | ROS_WARN("%s not exist\n", frame_file.c_str()); 122 | continue; 123 | } 124 | std::ifstream file; 125 | file.open(frame_file, std::ios::in | std::ios::binary); 126 | pcl::PointCloud pcl_cloud; 127 | pcl_cloud.clear(); 128 | 129 | float angle; 130 | uint16_t ring; 131 | 132 | while (!file.eof()) { 133 | PointXYZIR point; 134 | file.read(reinterpret_cast(&point.x), sizeof(float)); 135 | file.read(reinterpret_cast(&point.y), sizeof(float)); 136 | file.read(reinterpret_cast(&point.z), sizeof(float)); 137 | file.read(reinterpret_cast(&point.intensity), sizeof(float)); 138 | angle = atan2(point.z, sqrt(point.x * point.x + point.y * point.y)) / M_PI * 180 + 15; 139 | ring = round(angle / 2); 140 | point.ring = ring + 16; 141 | pcl_cloud.points.push_back(point); 142 | } 143 | file.close(); 144 | sensor_msgs::PointCloud2 cloud; 145 | pcl::toROSMsg(pcl_cloud, cloud); 146 | cloud.header.stamp.fromNSec(all_stamps[i]); 147 | cloud.header.frame_id = frame_id; 148 | bag.write(topic, cloud.header.stamp, cloud); 149 | ROS_INFO("bag write %s, %u points\n", topic.c_str(), cloud.height * cloud.width); 150 | ROS_INFO("done converting %s\n", frame_file.c_str()); 151 | ROS_INFO("total %lu, already convert %lu, remain %lu\n", total, i + 1, total - i - 1); 152 | } 153 | bag.close(); 154 | } 155 | 156 | } // namespace kaist2bag -------------------------------------------------------------------------------- /src/kaist_tool/kaist2bag/src/velodyne_converter.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by tao on 7/25/21. 3 | // 4 | 5 | #ifndef SRC_VELODYNE_CONVERTER_H 6 | #define SRC_VELODYNE_CONVERTER_H 7 | #include 8 | #include "converter.h" 9 | 10 | //pcl 11 | #include 12 | #include 13 | #include 14 | 15 | 16 | struct VelodynePointXYZIR 17 | { 18 | PCL_ADD_POINT4D 19 | PCL_ADD_INTENSITY; 20 | uint16_t ring; 21 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 22 | } EIGEN_ALIGN16; 23 | POINT_CLOUD_REGISTER_POINT_STRUCT (VelodynePointXYZIR, 24 | (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity) 25 | (uint16_t, ring, ring) 26 | ) 27 | 28 | using PointXYZIR = VelodynePointXYZIR; 29 | 30 | 31 | namespace kaist2bag { 32 | 33 | class VelodyneConverter : public Converter { 34 | public: 35 | VelodyneConverter(const std::string& dataset_dir, const std::string& save_dir, 36 | const std::string& left_topic, const std::string& right_topic); 37 | virtual ~VelodyneConverter() = default; 38 | 39 | int Convert() override; 40 | 41 | std::string default_left_stamp_file = "sensor_data/VLP_left_stamp.csv"; 42 | std::string default_left_data_dir = "sensor_data/VLP_left"; 43 | std::string default_right_stamp_file = "sensor_data/VLP_right_stamp.csv"; 44 | std::string default_right_data_dir = "sensor_data/VLP_right"; 45 | 46 | private: 47 | std::string left_topic_; 48 | std::string left_bag_name_; 49 | std::string right_topic_; 50 | std::string right_bag_name_; 51 | 52 | void ConvertLeft(const std::string& stamp_file, const std::string& data_dir, 53 | const std::string& bag_file, const std::string& topic, 54 | const std::string& frame_id); 55 | 56 | void ConvertRight(const std::string& stamp_file, const std::string& data_dir, 57 | const std::string& bag_file, const std::string& topic, 58 | const std::string& frame_id); 59 | 60 | 61 | }; 62 | 63 | 64 | } // namespace kaist2bag 65 | #endif //SRC_VELODYNE_CONVERTER_H 66 | -------------------------------------------------------------------------------- /src/kaist_tool/kaist2bag/src/vrs_converter.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by tao on 7/22/21. 3 | // 4 | 5 | #include "vrs_converter.h" 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace kaist2bag { 13 | 14 | VrsConverter::VrsConverter(const std::string &dataset_dir, const std::string &save_dir, 15 | const std::string &topic) 16 | : Converter(dataset_dir, save_dir), topic_(topic) { 17 | bag_name_ = FilterSlash(topic_) + ".bag"; 18 | } 19 | 20 | int VrsConverter::Convert() { 21 | CheckAndCreateSaveDir(); 22 | 23 | boost::filesystem::path bag_file = boost::filesystem::path(save_dir_) / bag_name_; 24 | rosbag::Bag bag(bag_file.string(), rosbag::bagmode::Write); 25 | ROS_INFO("saving %s", bag_file.c_str()); 26 | 27 | const std::string data_file = dataset_dir_ + "/" + default_data_file; 28 | FILE* fp = fopen(data_file.c_str(), "r"); 29 | sensor_msgs::NavSatFix gps_data; 30 | double cov[9]; 31 | 32 | // irp_sen_msgs::vrs vrs_data; 33 | int64_t stamp; 34 | double latitude, longitude, altitude, altitude_orthometric; 35 | double x_coordinate, y_coordinate, horizental_precision, lat_std, lon_std, altitude_std, 36 | heading_magnet, speed_knot, speed_km; 37 | int fix_state, number_of_sat, heading_valid; 38 | char GNVTG_mode; 39 | while (fscanf(fp,"%ld,%lf,%lf,%lf,%lf,%lf,%d,%d,%lf,%lf,%lf,%lf,%d,%lf,%lf,%lf,%c,%lf\n", 40 | &stamp,&latitude,&longitude,&x_coordinate,&y_coordinate,&altitude,&fix_state, 41 | &number_of_sat,&horizental_precision,&lat_std,&lon_std,&altitude_std, 42 | &heading_valid,&heading_magnet,&speed_knot,&speed_km,&GNVTG_mode) == 17) { 43 | 44 | 45 | gps_data.header.stamp.fromNSec(stamp); 46 | gps_data.header.frame_id = "vrs_gps"; 47 | gps_data.latitude = latitude; 48 | gps_data.longitude = longitude; 49 | gps_data.altitude = altitude; 50 | gps_data.position_covariance[0] = lat_std; 51 | gps_data.position_covariance[4] = lon_std; 52 | gps_data.position_covariance[8] = altitude_std; 53 | 54 | bag.write(topic_, gps_data.header.stamp, gps_data); 55 | 56 | // vrs_data.header.stamp.fromNSec(stamp); 57 | // vrs_data.header.frame_id = "altimeter"; 58 | // vrs_data.latitude = latitude; 59 | // vrs_data.altitude_orthometric = altitude_orthometric; 60 | // vrs_data.longitude = longitude; 61 | // vrs_data.x_coordinate = x_coordinate; 62 | // vrs_data.y_coordinate = y_coordinate; 63 | // vrs_data.altitude = altitude; 64 | // vrs_data.fix_state = fix_state; 65 | // if (fix_state == 1) vrs_data.fix_state_str = "normal"; 66 | // if (fix_state == 4) vrs_data.fix_state_str = "fix"; 67 | // if (fix_state == 5) vrs_data.fix_state_str = "float"; 68 | // vrs_data.number_of_sat = number_of_sat; 69 | // vrs_data.horizental_precision = horizental_precision; 70 | // vrs_data.lat_std = lat_std; 71 | // vrs_data.lon_std = lon_std; 72 | // vrs_data.altitude_std = altitude_std; 73 | // vrs_data.heading_valid = heading_valid; 74 | // vrs_data.heading_magnet = heading_magnet; 75 | // vrs_data.speed_knot = speed_knot; 76 | // vrs_data.speed_km = speed_km; 77 | // vrs_data.GNVTG_mode = GNVTG_mode; 78 | 79 | // bag.write(topic_, vrs_data.header.stamp, vrs_data); 80 | } 81 | bag.close(); 82 | fclose(fp); 83 | ROS_INFO("done saving %s", bag_file.c_str()); 84 | 85 | return 0; 86 | } 87 | 88 | } // namespace kaist2bag 89 | -------------------------------------------------------------------------------- /src/kaist_tool/kaist2bag/src/vrs_converter.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by tao on 7/22/21. 3 | // 4 | 5 | #ifndef SRC_VRS_CONVERTER_H 6 | #define SRC_VRS_CONVERTER_H 7 | 8 | #include 9 | #include "converter.h" 10 | 11 | namespace kaist2bag { 12 | 13 | class VrsConverter : public Converter { 14 | public: 15 | VrsConverter(const std::string& dataset_dir, const std::string& save_dir, const std::string& topic); 16 | virtual ~VrsConverter() = default; 17 | 18 | int Convert() override; 19 | 20 | std::string default_data_file = "sensor_data/vrs_gps.csv"; 21 | 22 | private: 23 | std::string topic_; 24 | std::string bag_name_; 25 | }; 26 | 27 | 28 | } // namespace kaist2bag 29 | 30 | #endif //SRC_VRS_CONVERTER_H 31 | -------------------------------------------------------------------------------- /src/kaist_tool/kaist_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(kaist_description) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 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) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | # catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 32 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 33 | ## but can be declared for certainty nonetheless: 34 | ## * add a exec_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs # Or other packages containing msgs 70 | # ) 71 | 72 | ################################################ 73 | ## Declare ROS dynamic reconfigure parameters ## 74 | ################################################ 75 | 76 | ## To declare and build dynamic reconfigure parameters within this 77 | ## package, follow these steps: 78 | ## * In the file package.xml: 79 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 80 | ## * In this file (CMakeLists.txt): 81 | ## * add "dynamic_reconfigure" to 82 | ## find_package(catkin REQUIRED COMPONENTS ...) 83 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 84 | ## and list every .cfg file to be processed 85 | 86 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 87 | # generate_dynamic_reconfigure_options( 88 | # cfg/DynReconf1.cfg 89 | # cfg/DynReconf2.cfg 90 | # ) 91 | 92 | ################################### 93 | ## catkin specific configuration ## 94 | ################################### 95 | ## The catkin_package macro generates cmake config files for your package 96 | ## Declare things to be passed to dependent projects 97 | ## INCLUDE_DIRS: uncomment this if your package contains header files 98 | ## LIBRARIES: libraries you create in this project that dependent projects also need 99 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 100 | ## DEPENDS: system dependencies of this project that dependent projects also need 101 | catkin_package( 102 | # INCLUDE_DIRS include 103 | # LIBRARIES nucbot_description 104 | # CATKIN_DEPENDS other_catkin_pkg 105 | # DEPENDS system_lib 106 | ) 107 | 108 | ########### 109 | ## Build ## 110 | ########### 111 | 112 | ## Specify additional locations of header files 113 | ## Your package locations should be listed before other locations 114 | include_directories( 115 | # include 116 | # ${catkin_INCLUDE_DIRS} 117 | ) 118 | 119 | ## Declare a C++ library 120 | # add_library(${PROJECT_NAME} 121 | # src/${PROJECT_NAME}/nucbot_description.cpp 122 | # ) 123 | 124 | ## Add cmake target dependencies of the library 125 | ## as an example, code may need to be generated before libraries 126 | ## either from message generation or dynamic reconfigure 127 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 128 | 129 | ## Declare a C++ executable 130 | ## With catkin_make all packages are built within a single CMake context 131 | ## The recommended prefix ensures that target names across packages don't collide 132 | # add_executable(${PROJECT_NAME}_node src/nucbot_description_node.cpp) 133 | 134 | ## Rename C++ executable without prefix 135 | ## The above recommended prefix causes long target names, the following renames the 136 | ## target back to the shorter version for ease of user use 137 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 138 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 139 | 140 | ## Add cmake target dependencies of the executable 141 | ## same as for the library above 142 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 143 | 144 | ## Specify libraries to link a library or executable target against 145 | # target_link_libraries(${PROJECT_NAME}_node 146 | # ${catkin_LIBRARIES} 147 | # ) 148 | 149 | ############# 150 | ## Install ## 151 | ############# 152 | 153 | # all install targets should use catkin DESTINATION variables 154 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 155 | 156 | ## Mark executable scripts (Python etc.) for installation 157 | ## in contrast to setup.py, you can choose the destination 158 | # install(PROGRAMS 159 | # scripts/my_python_script 160 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark executables and/or libraries for installation 164 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 165 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 166 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 167 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 168 | # ) 169 | 170 | ## Mark cpp header files for installation 171 | # install(DIRECTORY include/${PROJECT_NAME}/ 172 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 173 | # FILES_MATCHING PATTERN "*.h" 174 | # PATTERN ".svn" EXCLUDE 175 | # ) 176 | 177 | ## Mark other files for installation (e.g. launch and bag files, etc.) 178 | # install(FILES 179 | # # myfile1 180 | # # myfile2 181 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 182 | # ) 183 | 184 | ############# 185 | ## Testing ## 186 | ############# 187 | 188 | ## Add gtest based cpp test target and link libraries 189 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_nucbot_description.cpp) 190 | # if(TARGET ${PROJECT_NAME}-test) 191 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 192 | # endif() 193 | 194 | ## Add folders to be run by python nosetests 195 | # catkin_add_nosetests(test) 196 | -------------------------------------------------------------------------------- /src/kaist_tool/kaist_description/launch/kaist_description.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 11 | 12 | -------------------------------------------------------------------------------- /src/kaist_tool/kaist_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | kaist_description 4 | 0.0.1 5 | 6 | URDF description for the kaist 7 | 8 | 9 | xiangli 10 | 11 | 12 | TODO 13 | 14 | catkin 15 | urdf 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /src/kaist_tool/kaist_description/rviz/kaist.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 548 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Preferences: 31 | PromptSaveOnExit: true 32 | Toolbars: 33 | toolButtonStyle: 2 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 1 39 | Class: rviz/Grid 40 | Color: 160; 160; 164 41 | Enabled: true 42 | Line Style: 43 | Line Width: 0.029999999329447746 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 10 53 | Reference Frame: 54 | Value: true 55 | - Class: rviz/TF 56 | Enabled: true 57 | Frame Timeout: 15 58 | Frames: 59 | All Enabled: true 60 | GPS_back_link: 61 | Value: true 62 | GPS_front_link: 63 | Value: true 64 | base_link: 65 | Value: true 66 | front_laser_link: 67 | Value: true 68 | imu_link: 69 | Value: true 70 | Marker Scale: 1 71 | Name: TF 72 | Show Arrows: true 73 | Show Axes: true 74 | Show Names: true 75 | Tree: 76 | base_link: 77 | GPS_back_link: 78 | {} 79 | GPS_front_link: 80 | {} 81 | front_laser_link: 82 | {} 83 | imu_link: 84 | {} 85 | Update Interval: 0 86 | Value: true 87 | - Alpha: 1 88 | Autocompute Intensity Bounds: true 89 | Autocompute Value Bounds: 90 | Max Value: 10 91 | Min Value: -10 92 | Value: true 93 | Axis: Z 94 | Channel Name: intensity 95 | Class: rviz/PointCloud2 96 | Color: 255; 255; 255 97 | Color Transformer: Intensity 98 | Decay Time: 0 99 | Enabled: true 100 | Invert Rainbow: false 101 | Max Color: 255; 255; 255 102 | Min Color: 0; 0; 0 103 | Name: PointCloud2 104 | Position Transformer: XYZ 105 | Queue Size: 10 106 | Selectable: true 107 | Size (Pixels): 3 108 | Size (m): 0.05000000074505806 109 | Style: Flat Squares 110 | Topic: /ns1/velodyne_points 111 | Unreliable: false 112 | Use Fixed Frame: true 113 | Use rainbow: true 114 | Value: true 115 | - Alpha: 1 116 | Autocompute Intensity Bounds: true 117 | Autocompute Value Bounds: 118 | Max Value: 10 119 | Min Value: -10 120 | Value: true 121 | Axis: Z 122 | Channel Name: intensity 123 | Class: rviz/PointCloud2 124 | Color: 255; 255; 255 125 | Color Transformer: Intensity 126 | Decay Time: 0 127 | Enabled: true 128 | Invert Rainbow: false 129 | Max Color: 255; 255; 255 130 | Min Color: 0; 0; 0 131 | Name: PointCloud2 132 | Position Transformer: XYZ 133 | Queue Size: 10 134 | Selectable: true 135 | Size (Pixels): 3 136 | Size (m): 0.05000000074505806 137 | Style: Flat Squares 138 | Topic: /ns2/velodyne_points 139 | Unreliable: false 140 | Use Fixed Frame: true 141 | Use rainbow: true 142 | Value: true 143 | Enabled: true 144 | Global Options: 145 | Background Color: 48; 48; 48 146 | Default Light: true 147 | Fixed Frame: base_link 148 | Frame Rate: 30 149 | Name: root 150 | Tools: 151 | - Class: rviz/Interact 152 | Hide Inactive Objects: true 153 | - Class: rviz/MoveCamera 154 | - Class: rviz/Select 155 | - Class: rviz/FocusCamera 156 | - Class: rviz/Measure 157 | - Class: rviz/SetInitialPose 158 | Theta std deviation: 0.2617993950843811 159 | Topic: /initialpose 160 | X std deviation: 0.5 161 | Y std deviation: 0.5 162 | - Class: rviz/SetGoal 163 | Topic: /move_base_simple/goal 164 | - Class: rviz/PublishPoint 165 | Single click: true 166 | Topic: /clicked_point 167 | Value: true 168 | Views: 169 | Current: 170 | Class: rviz/XYOrbit 171 | Distance: 2.1413605213165283 172 | Enable Stereo Rendering: 173 | Stereo Eye Separation: 0.05999999865889549 174 | Stereo Focal Distance: 1 175 | Swap Stereo Eyes: false 176 | Value: false 177 | Focal Point: 178 | X: 0.11234211921691895 179 | Y: 0.2475755214691162 180 | Z: 3.5762786865234375e-7 181 | Focal Shape Fixed Size: true 182 | Focal Shape Size: 0.05000000074505806 183 | Invert Z Axis: false 184 | Name: Current View 185 | Near Clip Distance: 0.009999999776482582 186 | Pitch: 0.930398166179657 187 | Target Frame: base_link 188 | Value: XYOrbit (rviz) 189 | Yaw: 1.6053974628448486 190 | Saved: ~ 191 | Window Geometry: 192 | Displays: 193 | collapsed: false 194 | Height: 845 195 | Hide Left Dock: false 196 | Hide Right Dock: false 197 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002affc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002af000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002affc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002af000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005fd0000003efc0100000002fb0000000800540069006d00650100000000000005fd000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000038c000002af00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 198 | Selection: 199 | collapsed: false 200 | Time: 201 | collapsed: false 202 | Tool Properties: 203 | collapsed: false 204 | Views: 205 | collapsed: false 206 | Width: 1533 207 | X: 67 208 | Y: 27 209 | -------------------------------------------------------------------------------- /src/kaist_tool/kaist_description/urdf/urban08.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 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | -------------------------------------------------------------------------------- /src/lidar_location/README.md: -------------------------------------------------------------------------------- 1 | # NDT匹配进行定位 2 | ## Reference 3 | 大部分代码参考自[该仓库](https://github.com/Little-Potato-1990/localization_in_auto_driving.git)，在该仓库的基础上对kaist数据集做了适配 4 | ## Dependencies 5 | * ROS 6 | * GeographicLib-1.51 7 | ## Run the package 8 | 1. Run the launch file: 9 | ``` 10 | roslaunch front_end.launch 11 | ``` 12 | 2. Play existing bag files: 13 | ``` 14 | rosbag play urban08.bag -r 0.3 15 | ``` 16 | 3. lidar_location/config/front_end/config.yaml下 可以选择是否使用离线地图进行定位 17 | 4. 不使用离线地图进行定位时，在定位结束后，可以使用以下命令将全局地图保存在lidar_location/slam_data下 18 | ``` 19 | rosservice call /save_map 20 | ``` 21 | -------------------------------------------------------------------------------- /src/lidar_location/cmake/FindGeographicLib.cmake: -------------------------------------------------------------------------------- 1 | # Look for GeographicLib 2 | # 3 | # Set 4 | # GeographicLib_FOUND = GEOGRAPHICLIB_FOUND = TRUE 5 | # GeographicLib_INCLUDE_DIRS = /usr/local/include 6 | # GeographicLib_LIBRARIES = /usr/local/lib/libGeographic.so 7 | # GeographicLib_LIBRARY_DIRS = /usr/local/lib 8 | 9 | find_library (GeographicLib_LIBRARIES Geographic 10 | PATHS "${CMAKE_INSTALL_PREFIX}/../GeographicLib/lib") 11 | 12 | if (GeographicLib_LIBRARIES) 13 | get_filename_component (GeographicLib_LIBRARY_DIRS 14 | "${GeographicLib_LIBRARIES}" PATH) 15 | get_filename_component (_ROOT_DIR "${GeographicLib_LIBRARY_DIRS}" PATH) 16 | set (GeographicLib_INCLUDE_DIRS "${_ROOT_DIR}/include") 17 | set (GeographicLib_BINARY_DIRS "${_ROOT_DIR}/bin") 18 | if (NOT EXISTS "${GeographicLib_INCLUDE_DIRS}/GeographicLib/Config.h") 19 | # On Debian systems the library is in e.g., 20 | # /usr/lib/x86_64-linux-gnu/libGeographic.so 21 | # so try stripping another element off _ROOT_DIR 22 | get_filename_component (_ROOT_DIR "${_ROOT_DIR}" PATH) 23 | set (GeographicLib_INCLUDE_DIRS "${_ROOT_DIR}/include") 24 | set (GeographicLib_BINARY_DIRS "${_ROOT_DIR}/bin") 25 | if (NOT EXISTS "${GeographicLib_INCLUDE_DIRS}/GeographicLib/Config.h") 26 | unset (GeographicLib_INCLUDE_DIRS) 27 | unset (GeographicLib_LIBRARIES) 28 | unset (GeographicLib_LIBRARY_DIRS) 29 | unset (GeographicLib_BINARY_DIRS) 30 | endif () 31 | endif () 32 | unset (_ROOT_DIR) 33 | endif () 34 | 35 | include (FindPackageHandleStandardArgs) 36 | find_package_handle_standard_args (GeographicLib DEFAULT_MSG 37 | GeographicLib_LIBRARY_DIRS GeographicLib_LIBRARIES 38 | GeographicLib_INCLUDE_DIRS) 39 | mark_as_advanced (GeographicLib_LIBRARY_DIRS GeographicLib_LIBRARIES 40 | GeographicLib_INCLUDE_DIRS) 41 | -------------------------------------------------------------------------------- /src/lidar_location/config/front_end/config.yaml: -------------------------------------------------------------------------------- 1 | data_path: ./ # 数据存放路径 2 | # 选择离线地图 3 | map: offline # 目前支持: offline, online 4 | map_path: /home/liwei/Learning_localization_from_scratch_ws/src/data/GlobalMap.pcd 5 | # 匹配 6 | registration_method: NDT # 选择点云匹配方法，目前支持：NDT 7 | 8 | # 局部地图 9 | key_frame_distance: 2.0 # 关键帧距离 10 | local_frame_num: 20 11 | local_map_filter: voxel_filter # 选择滑窗地图点云滤波方法，目前支持：voxel_filter 12 | 13 | # rviz显示 14 | display_filter: voxel_filter # rviz 实时显示点云时滤波方法，目前支持：voxel_filter 15 | 16 | # 当前帧 17 | frame_filter: voxel_filter # 选择当前帧点云滤波方法，目前支持：voxel_filter 18 | 19 | # 各配置选项对应参数 20 | ## 匹配相关参数 21 | NDT: 22 | res : 1.0 23 | step_size : 0.1 24 | trans_eps : 0.01 25 | max_iter : 30 26 | ## 滤波相关参数 27 | voxel_filter: 28 | local_map: 29 | leaf_size: [0.6, 0.6, 0.6] 30 | frame: 31 | leaf_size: [1.3, 1.3, 1.3] 32 | display: 33 | leaf_size: [0.5, 0.5, 0.5] 34 | -------------------------------------------------------------------------------- /src/lidar_location/include/lidar_localization/front_end/front_end.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LIDAR_LOCALIZATION_FRONT_END_FRONT_END_HPP_ 2 | #define LIDAR_LOCALIZATION_FRONT_END_FRONT_END_HPP_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include "lidar_localization/sensor_data/cloud_data.hpp" 12 | #include "lidar_localization/models/registration/ndt_registration.hpp" 13 | #include "lidar_localization/models/cloud_filter/voxel_filter.hpp" 14 | 15 | namespace lidar_localization { 16 | class FrontEnd { 17 | public: 18 | struct Frame { 19 | Eigen::Matrix4f pose = Eigen::Matrix4f::Identity(); 20 | CloudData cloud_data; 21 | }; 22 | 23 | public: 24 | FrontEnd(); 25 | 26 | bool InitWithConfig(); 27 | Eigen::Matrix4f Update(const CloudData& cloud_data); 28 | Eigen::Matrix4f UpdateWithOfflineMap(const CloudData& cloud_data); 29 | bool SetInitPose(const Eigen::Matrix4f& init_pose); 30 | 31 | bool SaveMap(); 32 | bool PublishGlobalMap(); 33 | bool GetNewLocalMap(CloudData::CLOUD_PTR& local_map_ptr); 34 | bool GetNewGlobalMap(CloudData::CLOUD_PTR& global_map_ptr); 35 | bool GetCurrentScan(CloudData::CLOUD_PTR& current_scan_ptr); 36 | bool IfOfflineMap(); 37 | 38 | private: 39 | bool InitParam(const YAML::Node& config_node); 40 | bool InitOfflineMap(const YAML::Node& config_node); 41 | bool InitDataPath(const YAML::Node& config_node); 42 | bool InitRegistration(std::shared_ptr& registration_ptr, const YAML::Node& config_node); 43 | bool InitFilter(std::string filter_user, std::shared_ptr& filter_ptr, const YAML::Node& config_node); 44 | bool UpdateWithNewFrame(const Frame& new_key_frame); 45 | 46 | private: 47 | std::string data_path_ = ""; 48 | 49 | std::shared_ptr frame_filter_ptr_; 50 | std::shared_ptr local_map_filter_ptr_; 51 | std::shared_ptr display_filter_ptr_; 52 | std::shared_ptr registration_ptr_; 53 | 54 | std::deque local_map_frames_; 55 | std::deque global_map_frames_; 56 | 57 | bool has_new_local_map_ = false; 58 | bool has_new_global_map_ = false; 59 | CloudData::CLOUD_PTR local_map_ptr_; 60 | CloudData::CLOUD_PTR global_map_ptr_; 61 | CloudData::CLOUD_PTR result_cloud_ptr_; 62 | Frame current_frame_; 63 | 64 | Eigen::Matrix4f init_pose_ = Eigen::Matrix4f::Identity(); 65 | 66 | float key_frame_distance_ = 2.0; 67 | int local_frame_num_ = 20; 68 | std::string map; 69 | std::string map_path; 70 | }; 71 | } 72 | 73 | #endif -------------------------------------------------------------------------------- /src/lidar_location/include/lidar_localization/front_end/front_end_flow.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LIDAR_LOCALIZATION_FRONT_END_FRONT_END_FLOW_HPP_ 2 | #define LIDAR_LOCALIZATION_FRONT_END_FRONT_END_FLOW_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "lidar_localization/subscriber/cloud_subscriber.hpp" 9 | #include "lidar_localization/subscriber/imu_subscriber.hpp" 10 | #include "lidar_localization/subscriber/gnss_subscriber.hpp" 11 | 12 | #include "lidar_localization/publisher/cloud_publisher.hpp" 13 | #include "lidar_localization/publisher/odometry_publisher.hpp" 14 | #include "lidar_localization/front_end/front_end.hpp" 15 | 16 | namespace lidar_localization { 17 | class FrontEndFlow { 18 | public: 19 | FrontEndFlow(ros::NodeHandle& nh); 20 | bool Run(); 21 | bool SaveMap(); 22 | 23 | private: 24 | bool ReadData(); 25 | bool Workflow(); 26 | bool WorkflowWithMap(); 27 | 28 | private: 29 | std::shared_ptr cloud_sub_ptr; 30 | std::shared_ptr cloud_sub_ptr_ns2; 31 | std::shared_ptr imu_sub_ptr; 32 | std::shared_ptr gnss_sub_ptr; 33 | 34 | std::shared_ptr cloud_pub_ptr; 35 | std::shared_ptr local_map_pub_ptr; 36 | std::shared_ptr global_map_pub_ptr; 37 | std::shared_ptr laser_odom_pub_ptr; 38 | std::shared_ptr front_end_ptr; 39 | 40 | std::deque cloud_data_buff; 41 | std::deque cloud_data_buff_ns2; 42 | std::deque imu_data_buff; 43 | std::deque gnss_data_buff; 44 | 45 | Eigen::Matrix4f vehilcle_to_ns1; 46 | Eigen::Matrix4f vehilcle_to_ns2; 47 | Eigen::Matrix4f vehilcle_to_imu; 48 | Eigen::Matrix4f ns2_to_ns1; 49 | Eigen::Matrix4f ns1_to_imu; 50 | Eigen::Matrix4f ns2_to_imu; 51 | bool front_end_pose_inited; 52 | CloudData::CLOUD_PTR local_map_ptr; 53 | CloudData::CLOUD_PTR global_map_ptr; 54 | CloudData::CLOUD_PTR current_scan_ptr; 55 | double run_time; 56 | double init_time; 57 | bool time_inited; 58 | bool has_global_map_published; 59 | }; 60 | } 61 | 62 | #endif 63 | 64 | 65 | -------------------------------------------------------------------------------- /src/lidar_location/include/lidar_localization/models/cloud_filter/cloud_filter_interface.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LIDAR_LOCALIZATION_MODELS_CLOUD_FILTER_CLOUD_FILTER_INTERFACE_HPP_ 2 | #define LIDAR_LOCALIZATION_MODELS_CLOUD_FILTER_CLOUD_FILTER_INTERFACE_HPP_ 3 | 4 | #include 5 | #include "lidar_localization/sensor_data/cloud_data.hpp" 6 | 7 | namespace lidar_localization { 8 | class CloudFilterInterface { 9 | public: 10 | virtual ~CloudFilterInterface() = default; 11 | 12 | virtual bool Filter(const CloudData::CLOUD_PTR& input_cloud_ptr, CloudData::CLOUD_PTR& filtered_cloud_ptr) = 0; 13 | }; 14 | } 15 | 16 | #endif -------------------------------------------------------------------------------- /src/lidar_location/include/lidar_localization/models/cloud_filter/voxel_filter.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LIDAR_LOCALIZATION_MODELS_CLOUD_FILTER_VOXEL_FILTER_HPP_ 2 | #define LIDAR_LOCALIZATION_MODELS_CLOUD_FILTER_VOXEL_FILTER_HPP_ 3 | 4 | #include 5 | #include 6 | #include "lidar_localization/models/cloud_filter/cloud_filter_interface.hpp" 7 | 8 | namespace lidar_localization { 9 | class VoxelFilter: public CloudFilterInterface { 10 | public: 11 | VoxelFilter(const YAML::Node& node); 12 | VoxelFilter(float leaf_size_x, float leaf_size_y, float leaf_size_z); 13 | 14 | bool Filter(const CloudData::CLOUD_PTR& input_cloud_ptr, CloudData::CLOUD_PTR& filtered_cloud_ptr) override; 15 | 16 | private: 17 | bool SetFilterParam(float leaf_size_x, float leaf_size_y, float leaf_size_z); 18 | 19 | private: 20 | pcl::VoxelGrid voxel_filter_; 21 | }; 22 | } 23 | #endif -------------------------------------------------------------------------------- /src/lidar_location/include/lidar_localization/models/registration/ndt_registration.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LIDAR_LOCALIZATION_MODELS_REGISTRATION_NDT_REGISTRATION_HPP_ 2 | #define LIDAR_LOCALIZATION_MODELS_REGISTRATION_NDT_REGISTRATION_HPP_ 3 | 4 | #include 5 | #include "lidar_localization/models/registration/registration_interface.hpp" 6 | 7 | namespace lidar_localization { 8 | class NDTRegistration: public RegistrationInterface { 9 | public: 10 | NDTRegistration(const YAML::Node& node); 11 | NDTRegistration(float res, float step_size, float trans_eps, int max_iter); 12 | 13 | bool SetInputTarget(const CloudData::CLOUD_PTR& input_target) override; 14 | bool ScanMatch(const CloudData::CLOUD_PTR& input_source, 15 | const Eigen::Matrix4f& predict_pose, 16 | CloudData::CLOUD_PTR& result_cloud_ptr, 17 | Eigen::Matrix4f& result_pose) override; 18 | 19 | private: 20 | bool SetRegistrationParam(float res, float step_size, float trans_eps, int max_iter); 21 | 22 | private: 23 | pcl::NormalDistributionsTransform::Ptr ndt_ptr_; 24 | }; 25 | } 26 | 27 | #endif -------------------------------------------------------------------------------- /src/lidar_location/include/lidar_localization/models/registration/registration_interface.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LIDAR_LOCALIZATION_MODELS_REGISTRATION_INTERFACE_HPP_ 2 | #define LIDAR_LOCALIZATION_MODELS_REGISTRATION_INTERFACE_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include "lidar_localization/sensor_data/cloud_data.hpp" 8 | 9 | namespace lidar_localization { 10 | class RegistrationInterface { 11 | public: 12 | virtual ~RegistrationInterface() = default; 13 | 14 | virtual bool SetInputTarget(const CloudData::CLOUD_PTR& input_target) = 0; 15 | virtual bool ScanMatch(const CloudData::CLOUD_PTR& input_source, 16 | const Eigen::Matrix4f& predict_pose, 17 | CloudData::CLOUD_PTR& result_cloud_ptr, 18 | Eigen::Matrix4f& result_pose) = 0; 19 | }; 20 | } 21 | 22 | #endif -------------------------------------------------------------------------------- /src/lidar_location/include/lidar_localization/publisher/cloud_publisher.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LIDAR_LOCALIZATION_PUBLISHER_CLOUD_PUBLISHER_HPP_ 2 | #define LIDAR_LOCALIZATION_PUBLISHER_CLOUD_PUBLISHER_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "lidar_localization/sensor_data/cloud_data.hpp" 10 | 11 | namespace lidar_localization { 12 | class CloudPublisher { 13 | public: 14 | CloudPublisher(ros::NodeHandle& nh, 15 | std::string topic_name, 16 | size_t buff_size, 17 | std::string frame_id); 18 | CloudPublisher() = default; 19 | void Publish(CloudData::CLOUD_PTR cloud_ptr_input); 20 | 21 | private: 22 | ros::NodeHandle nh_; 23 | ros::Publisher publisher_; 24 | std::string frame_id_; 25 | }; 26 | } 27 | #endif -------------------------------------------------------------------------------- /src/lidar_location/include/lidar_localization/publisher/odometry_publisher.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LIDAR_LOCALIZATION_PUBLISHER_ODOMETRY_PUBLISHER_HPP_ 2 | #define LIDAR_LOCALIZATION_PUBLISHER_ODOMETRY_PUBLISHER_HPP_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | namespace lidar_localization { 11 | class OdometryPublisher { 12 | public: 13 | OdometryPublisher(ros::NodeHandle& nh, 14 | std::string topic_name, 15 | std::string base_frame_id, 16 | std::string child_frame_id, 17 | int buff_size); 18 | OdometryPublisher() = default; 19 | 20 | void Publish(const Eigen::Matrix4f& transform_matrix); 21 | 22 | private: 23 | ros::NodeHandle nh_; 24 | ros::Publisher publisher_; 25 | nav_msgs::Odometry odometry_; 26 | }; 27 | } 28 | #endif -------------------------------------------------------------------------------- /src/lidar_location/include/lidar_localization/sensor_data/cloud_data.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LIDAR_LOCALIZATION_SENSOR_DATA_CLOUD_DATA_HPP_ 2 | #define LIDAR_LOCALIZATION_SENSOR_DATA_CLOUD_DATA_HPP_ 3 | 4 | #include 5 | #include 6 | 7 | namespace lidar_localization { 8 | class CloudData { 9 | public: 10 | using POINT = pcl::PointXYZ; 11 | using CLOUD = pcl::PointCloud; 12 | using CLOUD_PTR = CLOUD::Ptr; 13 | 14 | public: 15 | CloudData() 16 | :cloud_ptr(new CLOUD()) { 17 | } 18 | 19 | public: 20 | double time = 0.0; 21 | CLOUD_PTR cloud_ptr; 22 | }; 23 | } 24 | 25 | #endif -------------------------------------------------------------------------------- /src/lidar_location/include/lidar_localization/sensor_data/gnss_data.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LIDAR_LOCALIZATION_SENSOR_DATA_GNSS_DATA_HPP_ 2 | #define LIDAR_LOCALIZATION_SENSOR_DATA_GNSS_DATA_HPP_ 3 | 4 | #include 5 | #include 6 | 7 | #include "GeographicLib/LocalCartesian.hpp" 8 | 9 | using std::vector; 10 | using std::string; 11 | 12 | namespace lidar_localization { 13 | class GNSSData { 14 | public: 15 | double time = 0.0; 16 | double longitude = 0.0; 17 | double latitude = 0.0; 18 | double altitude = 0.0; 19 | double local_E = 0.0; 20 | double local_N = 0.0; 21 | double local_U = 0.0; 22 | int status = 0; 23 | int service = 0; 24 | 25 | private: 26 | static GeographicLib::LocalCartesian geo_converter; 27 | static bool origin_position_inited; 28 | 29 | public: 30 | void InitOriginPosition(); 31 | void UpdateXYZ(); 32 | }; 33 | } 34 | #endif -------------------------------------------------------------------------------- /src/lidar_location/include/lidar_localization/sensor_data/imu_data.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LIDAR_LOCALIZATION_SENSOR_DATA_IMU_DATA_HPP_ 2 | #define LIDAR_LOCALIZATION_SENSOR_DATA_IMU_DATA_HPP_ 3 | 4 | #include 5 | 6 | namespace lidar_localization { 7 | class IMUData { 8 | public: 9 | struct LinearAcceleration { 10 | double x = 0.0; 11 | double y = 0.0; 12 | double z = 0.0; 13 | }; 14 | 15 | struct AngularVelocity { 16 | double x = 0.0; 17 | double y = 0.0; 18 | double z = 0.0; 19 | }; 20 | 21 | struct Orientation { 22 | double x = 0.0; 23 | double y = 0.0; 24 | double z = 0.0; 25 | double w = 0.0; 26 | }; 27 | 28 | double time = 0.0; 29 | LinearAcceleration linear_acceleration; 30 | AngularVelocity angular_velocity; 31 | Orientation orientation; 32 | 33 | public: 34 | // 把四元数转换成旋转矩阵送出去 35 | Eigen::Matrix3f GetOrientationMatrix() { 36 | Eigen::Quaterniond q(orientation.w, orientation.x, orientation.y, orientation.z); 37 | Eigen::Matrix3f matrix = q.matrix().cast(); 38 | 39 | return matrix; 40 | } 41 | }; 42 | } 43 | #endif -------------------------------------------------------------------------------- /src/lidar_location/include/lidar_localization/subscriber/cloud_subscriber.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LIDAR_LOCALIZATION_SUBSCRIBER_CLOUD_SUBSCRIBER_HPP_ 2 | #define LIDAR_LOCALIZATION_SUBSCRIBER_CLOUD_SUBSCRIBER_HPP_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "lidar_localization/sensor_data/cloud_data.hpp" 13 | 14 | namespace lidar_localization { 15 | class CloudSubscriber { 16 | public: 17 | CloudSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size); 18 | CloudSubscriber() = default; 19 | void ParseData(std::deque& deque_cloud_data); 20 | 21 | private: 22 | void msg_callback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg_ptr); 23 | 24 | private: 25 | ros::NodeHandle nh_; 26 | ros::Subscriber subscriber_; 27 | 28 | std::deque new_cloud_data_; 29 | }; 30 | } 31 | 32 | #endif -------------------------------------------------------------------------------- /src/lidar_location/include/lidar_localization/subscriber/gnss_subscriber.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LIDAR_LOCALIZATION_SUBSCRIBER_GNSS_SUBSCRIBER_HPP_ 2 | #define LIDAR_LOCALIZATION_SUBSCRIBER_GNSS_SUBSCRIBER_HPP_ 3 | 4 | #include 5 | #include 6 | #include "sensor_msgs/NavSatFix.h" 7 | #include "lidar_localization/sensor_data/gnss_data.hpp" 8 | 9 | namespace lidar_localization { 10 | class GNSSSubscriber { 11 | public: 12 | GNSSSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size); 13 | GNSSSubscriber() = default; 14 | void ParseData(std::deque& deque_gnss_data); 15 | 16 | private: 17 | void msg_callback(const sensor_msgs::NavSatFixConstPtr& nav_sat_fix_ptr); 18 | 19 | private: 20 | ros::NodeHandle nh_; 21 | ros::Subscriber subscriber_; 22 | 23 | std::deque new_gnss_data_; 24 | }; 25 | } 26 | #endif 27 | -------------------------------------------------------------------------------- /src/lidar_location/include/lidar_localization/subscriber/imu_subscriber.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LIDAR_LOCALIZATION_SUBSCRIBER_IMU_SUBSCRIBER_HPP_ 2 | #define LIDAR_LOCALIZATION_SUBSCRIBER_IMU_SUBSCRIBER_HPP_ 3 | 4 | #include 5 | #include 6 | #include "sensor_msgs/Imu.h" 7 | #include "lidar_localization/sensor_data/imu_data.hpp" 8 | 9 | namespace lidar_localization { 10 | class IMUSubscriber { 11 | public: 12 | IMUSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size); 13 | IMUSubscriber() = default; 14 | void ParseData(std::deque& deque_imu_data); 15 | 16 | private: 17 | void msg_callback(const sensor_msgs::ImuConstPtr& imu_msg_ptr); 18 | 19 | private: 20 | ros::NodeHandle nh_; 21 | ros::Subscriber subscriber_; 22 | 23 | std::deque new_imu_data_; 24 | }; 25 | } 26 | #endif -------------------------------------------------------------------------------- /src/lidar_location/launch/front_end.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /src/lidar_location/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lidar_location 4 | 0.0.0 5 | The lidar_location package 6 | 7 | 8 | 9 | 10 | liwei 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | eigen_conversions 53 | geometry_msgs 54 | pcl_ros 55 | roscpp 56 | rospy 57 | std_msgs 58 | tf 59 | eigen_conversions 60 | geometry_msgs 61 | pcl_ros 62 | roscpp 63 | rospy 64 | std_msgs 65 | tf 66 | roslib 67 | message_generation 68 | message_runtime 69 | eigen_conversions 70 | geometry_msgs 71 | pcl_ros 72 | roscpp 73 | rospy 74 | std_msgs 75 | tf 76 | roslib 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | -------------------------------------------------------------------------------- /src/lidar_location/src/front_end/front_end_flow.cpp: -------------------------------------------------------------------------------- 1 | #include "lidar_localization/front_end/front_end_flow.hpp" 2 | 3 | namespace lidar_localization { 4 | FrontEndFlow::FrontEndFlow(ros::NodeHandle& nh) { 5 | cloud_sub_ptr = std::make_shared(nh, "/ns1/velodyne_points", 100000); 6 | cloud_sub_ptr_ns2 = std::make_shared(nh, "/ns2/velodyne_points", 100000); 7 | imu_sub_ptr = std::make_shared(nh, "/imu/data_raw", 1000000); 8 | gnss_sub_ptr = std::make_shared(nh, "/vrs_gps_data", 1000000); 9 | 10 | cloud_pub_ptr = std::make_shared(nh, "current_scan", 100, "/map"); 11 | local_map_pub_ptr = std::make_shared(nh, "local_map", 100, "/map"); 12 | global_map_pub_ptr = std::make_shared(nh, "global_map", 100, "/map"); 13 | laser_odom_pub_ptr = std::make_shared(nh, "laser_odom", "map", "lidar", 100); 14 | 15 | front_end_ptr = std::make_shared(); 16 | 17 | // 坐标系之间的转换矩阵 18 | vehilcle_to_ns1 << -0.515105,-0.702383,-0.491249,-0.438343,0.487008,-0.711468,0.506593,0.395882,-0.70533,0.0217062,0.708547,1.94095,0,0,0,1; 19 | vehilcle_to_ns2 << -0.512695,0.700506,-0.496422,-0.436669,-0.497416,-0.713622,-0.493276,-0.411346,-0.6998,-0.00597189,0.714313,1.94785,0,0,0,1; 20 | vehilcle_to_imu << 1,0,0,-0.07,0,1,0,0,0,0,1,1.7,0,0,0,1; 21 | 22 | ns2_to_ns1 = vehilcle_to_ns2.inverse() * vehilcle_to_ns1; 23 | ns1_to_imu = vehilcle_to_ns1.inverse() * vehilcle_to_imu; 24 | ns2_to_imu = vehilcle_to_ns2.inverse() * vehilcle_to_imu; 25 | 26 | front_end_pose_inited = false; 27 | local_map_ptr.reset(new CloudData::CLOUD()); 28 | global_map_ptr.reset(new CloudData::CLOUD()); 29 | current_scan_ptr.reset(new CloudData::CLOUD()); 30 | 31 | run_time = 0.0; 32 | init_time = 0.0; 33 | time_inited = false; 34 | has_global_map_published = false; 35 | } 36 | 37 | bool FrontEndFlow::Run() { 38 | ReadData(); 39 | if(!front_end_ptr->IfOfflineMap()) { 40 | Workflow(); 41 | } 42 | else { 43 | WorkflowWithMap(); 44 | } 45 | return true; 46 | } 47 | 48 | bool FrontEndFlow::SaveMap() { 49 | bool result = front_end_ptr->SaveMap(); 50 | if (front_end_ptr->GetNewGlobalMap(global_map_ptr)) { 51 | global_map_pub_ptr->Publish(global_map_ptr); 52 | has_global_map_published = true; 53 | } 54 | return result; 55 | } 56 | 57 | bool FrontEndFlow::ReadData() { 58 | cloud_sub_ptr->ParseData(cloud_data_buff); 59 | cloud_sub_ptr_ns2->ParseData(cloud_data_buff_ns2); 60 | return true; 61 | } 62 | 63 | bool FrontEndFlow::Workflow(){ 64 | while (cloud_data_buff.size() > 0 && cloud_data_buff_ns2.size() > 0) { 65 | CloudData cloud_data = cloud_data_buff.front(); 66 | CloudData cloud_data_ns2 = cloud_data_buff_ns2.front(); 67 | // 把ns2的点云转换到ns1下 68 | pcl::transformPointCloud (*(cloud_data_ns2.cloud_ptr), *(cloud_data_ns2.cloud_ptr), ns2_to_ns1); 69 | *(cloud_data.cloud_ptr) = *(cloud_data.cloud_ptr) + *(cloud_data_ns2.cloud_ptr); 70 | 71 | if (!time_inited) { 72 | time_inited = true; 73 | init_time = cloud_data.time; 74 | } else { 75 | run_time = cloud_data.time - init_time; 76 | } 77 | 78 | double d_time = cloud_data.time - cloud_data_ns2.time; 79 | if (d_time < -0.05) { 80 | cloud_data_buff.pop_front(); 81 | } else if (d_time > 0.05) { 82 | cloud_data_buff_ns2.pop_front(); 83 | } else { 84 | cloud_data_buff.pop_front(); 85 | cloud_data_buff_ns2.pop_front(); 86 | // 定义初始化的位姿 87 | Eigen::Matrix4f odometry_matrix = Eigen::Matrix4f::Identity(); 88 | odometry_matrix(0,3) = 0; 89 | odometry_matrix(1,3) = 0; 90 | odometry_matrix(2,3) = 0; 91 | Eigen::Vector3f vector(0, 1, 0); 92 | double angle = M_PI / 4; 93 | Eigen::AngleAxisf rotationVector(angle, vector.normalized()); 94 | Eigen::Matrix3f rotationMatrix = Eigen::Matrix3f::Identity(); 95 | rotationMatrix = rotationVector.toRotationMatrix(); 96 | odometry_matrix.block<3,3>(0,0) = rotationMatrix; 97 | 98 | if (!front_end_pose_inited) { 99 | front_end_pose_inited = true; 100 | front_end_ptr->SetInitPose(odometry_matrix); 101 | } 102 | Eigen::Matrix4f laser_matrix = front_end_ptr->Update(cloud_data); 103 | laser_odom_pub_ptr->Publish(laser_matrix); 104 | 105 | front_end_ptr->GetCurrentScan(current_scan_ptr); 106 | cloud_pub_ptr->Publish(current_scan_ptr); 107 | if (front_end_ptr->GetNewLocalMap(local_map_ptr)) 108 | local_map_pub_ptr->Publish(local_map_ptr); 109 | } 110 | } 111 | return true; 112 | } 113 | 114 | bool FrontEndFlow::WorkflowWithMap(){ 115 | // 带有离线地图时 116 | while (cloud_data_buff.size() > 0 && cloud_data_buff_ns2.size() > 0) { 117 | CloudData cloud_data = cloud_data_buff.front(); 118 | CloudData cloud_data_ns2 = cloud_data_buff_ns2.front(); 119 | // 把ns1\ns2的点云转换到imu下 120 | pcl::transformPointCloud (*(cloud_data.cloud_ptr), *(cloud_data.cloud_ptr), ns1_to_imu); 121 | pcl::transformPointCloud (*(cloud_data_ns2.cloud_ptr), *(cloud_data_ns2.cloud_ptr), ns2_to_imu); 122 | *(cloud_data.cloud_ptr) = *(cloud_data.cloud_ptr) + *(cloud_data_ns2.cloud_ptr); 123 | 124 | if (!time_inited) { 125 | time_inited = true; 126 | init_time = cloud_data.time; 127 | } else { 128 | run_time = cloud_data.time - init_time; 129 | } 130 | 131 | double d_time = cloud_data.time - cloud_data_ns2.time; 132 | if (d_time < -0.05) { 133 | cloud_data_buff.pop_front(); 134 | } else if (d_time > 0.05) { 135 | cloud_data_buff_ns2.pop_front(); 136 | } else { 137 | cloud_data_buff.pop_front(); 138 | cloud_data_buff_ns2.pop_front(); 139 | 140 | if (!front_end_pose_inited) { 141 | front_end_pose_inited = true; 142 | // 定义初始化的位姿 143 | imu_sub_ptr->ParseData(imu_data_buff); 144 | gnss_sub_ptr->ParseData(gnss_data_buff); 145 | IMUData imu_data = imu_data_buff.front(); 146 | GNSSData gnss_data = gnss_data_buff.front(); 147 | gnss_data.InitOriginPosition(); 148 | gnss_data.UpdateXYZ(); 149 | 150 | Eigen::Matrix4f odometry_matrix = Eigen::Matrix4f::Identity(); 151 | odometry_matrix(0,3) = gnss_data.local_E; 152 | odometry_matrix(1,3) = gnss_data.local_N; 153 | odometry_matrix(2,3) = gnss_data.local_U; 154 | odometry_matrix.block<3,3>(0,0) = imu_data.GetOrientationMatrix(); 155 | 156 | 157 | front_end_ptr->SetInitPose(odometry_matrix); 158 | } 159 | Eigen::Matrix4f laser_matrix = front_end_ptr->UpdateWithOfflineMap(cloud_data); 160 | if(has_global_map_published == false) { 161 | if (front_end_ptr->GetNewGlobalMap(global_map_ptr)) { 162 | global_map_pub_ptr->Publish(global_map_ptr); 163 | has_global_map_published = true; 164 | } 165 | } 166 | has_global_map_published = true; 167 | laser_odom_pub_ptr->Publish(laser_matrix); 168 | 169 | front_end_ptr->GetCurrentScan(current_scan_ptr); 170 | cloud_pub_ptr->Publish(current_scan_ptr); 171 | } 172 | } 173 | return true; 174 | } 175 | } -------------------------------------------------------------------------------- /src/lidar_location/src/front_end_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "lidar_localization/front_end/front_end_flow.hpp" 4 | 5 | using namespace lidar_localization; 6 | 7 | std::shared_ptr _front_end_flow_ptr; 8 | 9 | bool save_map_callback(lidar_location::saveMap::Request &request, lidar_location::saveMap::Response &response) { 10 | response.succeed = _front_end_flow_ptr->SaveMap(); 11 | return response.succeed; 12 | } 13 | 14 | 15 | 16 | int main(int argc, char *argv[]) { 17 | ros::init(argc, argv, "front_end_node"); 18 | ros::NodeHandle nh; 19 | 20 | ros::ServiceServer service = nh.advertiseService("save_map", save_map_callback); 21 | ros::Rate rate(100); 22 | _front_end_flow_ptr = std::make_shared(nh); 23 | while (ros::ok()) { 24 | ros::spinOnce(); 25 | _front_end_flow_ptr->Run(); 26 | rate.sleep(); 27 | } 28 | return 0; 29 | } 30 | -------------------------------------------------------------------------------- /src/lidar_location/src/models/cloud_filter/voxel_filter.cpp: -------------------------------------------------------------------------------- 1 | #include "lidar_localization/models/cloud_filter/voxel_filter.hpp" 2 | 3 | namespace lidar_localization { 4 | 5 | VoxelFilter::VoxelFilter(const YAML::Node& node) { 6 | float leaf_size_x = node["leaf_size"][0].as(); 7 | float leaf_size_y = node["leaf_size"][1].as(); 8 | float leaf_size_z = node["leaf_size"][2].as(); 9 | 10 | SetFilterParam(leaf_size_x, leaf_size_y, leaf_size_z); 11 | } 12 | 13 | VoxelFilter::VoxelFilter(float leaf_size_x, float leaf_size_y, float leaf_size_z) { 14 | SetFilterParam(leaf_size_x, leaf_size_y, leaf_size_z); 15 | } 16 | 17 | bool VoxelFilter::SetFilterParam(float leaf_size_x, float leaf_size_y, float leaf_size_z) { 18 | voxel_filter_.setLeafSize(leaf_size_x, leaf_size_y, leaf_size_z); 19 | 20 | ROS_INFO_STREAM("Voxel Filter 的参数为："<< std::endl 21 | << leaf_size_x << ", " 22 | << leaf_size_y << ", " 23 | << leaf_size_z 24 | << std::endl << std::endl;); 25 | 26 | return true; 27 | } 28 | 29 | bool VoxelFilter::Filter(const CloudData::CLOUD_PTR& input_cloud_ptr, CloudData::CLOUD_PTR& filtered_cloud_ptr) { 30 | voxel_filter_.setInputCloud(input_cloud_ptr); 31 | voxel_filter_.filter(*filtered_cloud_ptr); 32 | 33 | return true; 34 | } 35 | } -------------------------------------------------------------------------------- /src/lidar_location/src/models/registration/ndt_registration.cpp: -------------------------------------------------------------------------------- 1 | #include "lidar_localization/models/registration/ndt_registration.hpp" 2 | 3 | namespace lidar_localization { 4 | 5 | NDTRegistration::NDTRegistration(const YAML::Node& node) 6 | :ndt_ptr_(new pcl::NormalDistributionsTransform()) { 7 | 8 | float res = node["res"].as(); 9 | float step_size = node["step_size"].as(); 10 | float trans_eps = node["trans_eps"].as(); 11 | int max_iter = node["max_iter"].as(); 12 | 13 | SetRegistrationParam(res, step_size, trans_eps, max_iter); 14 | } 15 | 16 | NDTRegistration::NDTRegistration(float res, float step_size, float trans_eps, int max_iter) 17 | :ndt_ptr_(new pcl::NormalDistributionsTransform()) { 18 | 19 | SetRegistrationParam(res, step_size, trans_eps, max_iter); 20 | } 21 | 22 | bool NDTRegistration::SetRegistrationParam(float res, float step_size, float trans_eps, int max_iter) { 23 | ndt_ptr_->setResolution(res); 24 | ndt_ptr_->setStepSize(step_size); 25 | ndt_ptr_->setTransformationEpsilon(trans_eps); 26 | ndt_ptr_->setMaximumIterations(max_iter); 27 | 28 | ROS_INFO_STREAM("NDT 的匹配参数为：" << std::endl 29 | << "res: " << res << ", " 30 | << "step_size: " << step_size << ", " 31 | << "trans_eps: " << trans_eps << ", " 32 | << "max_iter: " << max_iter 33 | << std::endl << std::endl;); 34 | 35 | return true; 36 | } 37 | 38 | bool NDTRegistration::SetInputTarget(const CloudData::CLOUD_PTR& input_target) { 39 | ndt_ptr_->setInputTarget(input_target); 40 | 41 | return true; 42 | } 43 | 44 | bool NDTRegistration::ScanMatch(const CloudData::CLOUD_PTR& input_source, 45 | const Eigen::Matrix4f& predict_pose, 46 | CloudData::CLOUD_PTR& result_cloud_ptr, 47 | Eigen::Matrix4f& result_pose) { 48 | ndt_ptr_->setInputSource(input_source); 49 | ndt_ptr_->align(*result_cloud_ptr, predict_pose); 50 | result_pose = ndt_ptr_->getFinalTransformation(); 51 | 52 | return true; 53 | } 54 | } -------------------------------------------------------------------------------- /src/lidar_location/src/publisher/cloud_publisher.cpp: -------------------------------------------------------------------------------- 1 | #include "lidar_localization/publisher/cloud_publisher.hpp" 2 | 3 | namespace lidar_localization { 4 | CloudPublisher::CloudPublisher(ros::NodeHandle& nh, 5 | std::string topic_name, 6 | size_t buff_size, 7 | std::string frame_id) 8 | :nh_(nh), frame_id_(frame_id) { 9 | publisher_ = nh_.advertise(topic_name, buff_size); 10 | } 11 | 12 | void CloudPublisher::Publish(CloudData::CLOUD_PTR cloud_ptr_input) { 13 | sensor_msgs::PointCloud2Ptr cloud_ptr_output(new sensor_msgs::PointCloud2()); 14 | pcl::toROSMsg(*cloud_ptr_input, *cloud_ptr_output); 15 | cloud_ptr_output->header.stamp = ros::Time::now(); 16 | cloud_ptr_output->header.frame_id = frame_id_; 17 | publisher_.publish(*cloud_ptr_output); 18 | } 19 | 20 | } -------------------------------------------------------------------------------- /src/lidar_location/src/publisher/odometry_publisher.cpp: -------------------------------------------------------------------------------- 1 | #include "lidar_localization/publisher/odometry_publisher.hpp" 2 | 3 | namespace lidar_localization { 4 | OdometryPublisher::OdometryPublisher(ros::NodeHandle& nh, 5 | std::string topic_name, 6 | std::string base_frame_id, 7 | std::string child_frame_id, 8 | int buff_size) 9 | :nh_(nh) { 10 | 11 | publisher_ = nh_.advertise(topic_name, buff_size); 12 | odometry_.header.frame_id = base_frame_id; 13 | odometry_.child_frame_id = child_frame_id; 14 | } 15 | 16 | void OdometryPublisher::Publish(const Eigen::Matrix4f& transform_matrix) { 17 | odometry_.header.stamp = ros::Time::now(); 18 | 19 | //set the position 20 | odometry_.pose.pose.position.x = transform_matrix(0,3); 21 | odometry_.pose.pose.position.y = transform_matrix(1,3); 22 | odometry_.pose.pose.position.z = transform_matrix(2,3); 23 | 24 | Eigen::Quaternionf q; 25 | q = transform_matrix.block<3,3>(0,0); 26 | odometry_.pose.pose.orientation.x = q.x(); 27 | odometry_.pose.pose.orientation.y = q.y(); 28 | odometry_.pose.pose.orientation.z = q.z(); 29 | odometry_.pose.pose.orientation.w = q.w(); 30 | 31 | publisher_.publish(odometry_); 32 | } 33 | } -------------------------------------------------------------------------------- /src/lidar_location/src/sensor_data/gnss_data.cpp: -------------------------------------------------------------------------------- 1 | #include "lidar_localization/sensor_data/gnss_data.hpp" 2 | 3 | 4 | GeographicLib::LocalCartesian lidar_localization::GNSSData::geo_converter; 5 | 6 | namespace lidar_localization { 7 | 8 | void GNSSData::InitOriginPosition() { 9 | geo_converter.Reset(latitude, longitude, altitude); 10 | } 11 | 12 | void GNSSData::UpdateXYZ() { 13 | geo_converter.Forward(latitude, longitude, altitude, local_E, local_N, local_U); 14 | } 15 | } -------------------------------------------------------------------------------- /src/lidar_location/src/subscriber/cloud_subscriber.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "lidar_localization/subscriber/cloud_subscriber.hpp" 3 | 4 | namespace lidar_localization { 5 | CloudSubscriber::CloudSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size) 6 | :nh_(nh) { 7 | subscriber_ = nh_.subscribe(topic_name, buff_size, &CloudSubscriber::msg_callback, this); 8 | } 9 | 10 | void CloudSubscriber::msg_callback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg_ptr) { 11 | CloudData cloud_data; 12 | cloud_data.time = cloud_msg_ptr->header.stamp.toSec(); 13 | pcl::fromROSMsg(*cloud_msg_ptr, *(cloud_data.cloud_ptr)); 14 | new_cloud_data_.push_back(cloud_data); 15 | } 16 | 17 | void CloudSubscriber::ParseData(std::deque& cloud_data_buff) { 18 | if (new_cloud_data_.size() > 0) { 19 | cloud_data_buff.insert(cloud_data_buff.end(), new_cloud_data_.begin(), new_cloud_data_.end()); 20 | new_cloud_data_.clear(); 21 | } 22 | 23 | } 24 | } 25 | -------------------------------------------------------------------------------- /src/lidar_location/src/subscriber/gnss_subscriber.cpp: -------------------------------------------------------------------------------- 1 | #include "lidar_localization/subscriber/gnss_subscriber.hpp" 2 | 3 | 4 | namespace lidar_localization { 5 | GNSSSubscriber::GNSSSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size) 6 | :nh_(nh) { 7 | subscriber_ = nh_.subscribe(topic_name, buff_size, &GNSSSubscriber::msg_callback, this); 8 | } 9 | 10 | void GNSSSubscriber::msg_callback(const sensor_msgs::NavSatFixConstPtr& nav_sat_fix_ptr) { 11 | GNSSData gnss_data; 12 | gnss_data.time = nav_sat_fix_ptr->header.stamp.toSec(); 13 | gnss_data.latitude = nav_sat_fix_ptr->latitude; 14 | gnss_data.longitude = nav_sat_fix_ptr->longitude; 15 | gnss_data.altitude = nav_sat_fix_ptr->longitude; 16 | gnss_data.status = nav_sat_fix_ptr->status.status; 17 | gnss_data.service = nav_sat_fix_ptr->status.service; 18 | 19 | new_gnss_data_.push_back(gnss_data); 20 | } 21 | 22 | void GNSSSubscriber::ParseData(std::deque& gnss_data_buff) { 23 | if (new_gnss_data_.size() > 0) { 24 | gnss_data_buff.insert(gnss_data_buff.end(), new_gnss_data_.begin(), new_gnss_data_.end()); 25 | new_gnss_data_.clear(); 26 | } 27 | } 28 | } -------------------------------------------------------------------------------- /src/lidar_location/src/subscriber/imu_subscriber.cpp: -------------------------------------------------------------------------------- 1 | #include "lidar_localization/subscriber/imu_subscriber.hpp" 2 | 3 | 4 | namespace lidar_localization{ 5 | IMUSubscriber::IMUSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size) 6 | :nh_(nh) { 7 | subscriber_ = nh_.subscribe(topic_name, buff_size, &IMUSubscriber::msg_callback, this); 8 | } 9 | 10 | void IMUSubscriber::msg_callback(const sensor_msgs::ImuConstPtr& imu_msg_ptr) { 11 | IMUData imu_data; 12 | imu_data.time = imu_msg_ptr->header.stamp.toSec(); 13 | 14 | imu_data.linear_acceleration.x = imu_msg_ptr->linear_acceleration.x; 15 | imu_data.linear_acceleration.y = imu_msg_ptr->linear_acceleration.y; 16 | imu_data.linear_acceleration.z = imu_msg_ptr->linear_acceleration.z; 17 | 18 | imu_data.angular_velocity.x = imu_msg_ptr->angular_velocity.x; 19 | imu_data.angular_velocity.y = imu_msg_ptr->angular_velocity.y; 20 | imu_data.angular_velocity.z = imu_msg_ptr->angular_velocity.z; 21 | 22 | imu_data.orientation.x = imu_msg_ptr->orientation.x; 23 | imu_data.orientation.y = imu_msg_ptr->orientation.y; 24 | imu_data.orientation.z = imu_msg_ptr->orientation.z; 25 | imu_data.orientation.w = imu_msg_ptr->orientation.w; 26 | 27 | new_imu_data_.push_back(imu_data); 28 | } 29 | 30 | void IMUSubscriber::ParseData(std::deque& imu_data_buff) { 31 | if (new_imu_data_.size() > 0) { 32 | imu_data_buff.insert(imu_data_buff.end(), new_imu_data_.begin(), new_imu_data_.end()); 33 | new_imu_data_.clear(); 34 | } 35 | } 36 | } -------------------------------------------------------------------------------- /src/lidar_location/srv/saveMap.srv: -------------------------------------------------------------------------------- 1 | 2 | --- 3 | bool succeed -------------------------------------------------------------------------------- /src/lidar_odometry/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(lidar_odometry) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS "-std=c++11") 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | geometry_msgs 10 | sensor_msgs 11 | nav_msgs 12 | roscpp 13 | rospy 14 | rosbag 15 | std_msgs 16 | image_transport 17 | cv_bridge 18 | tf 19 | ) 20 | 21 | find_package(Eigen3 REQUIRED) 22 | find_package(OpenMP REQUIRED) 23 | find_package(PCL 1.8 REQUIRED) 24 | find_package(OpenCV REQUIRED) 25 | find_package(Ceres REQUIRED) 26 | 27 | 28 | include_directories( 29 | include 30 | ${catkin_INCLUDE_DIRS} 31 | ${PCL_INCLUDE_DIRS} 32 | ${CERES_INCLUDE_DIRS} 33 | ${OpenCV_INCLUDE_DIRS}) 34 | 35 | catkin_package( 36 | CATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs nav_msgs 37 | DEPENDS EIGEN3 PCL 38 | INCLUDE_DIRS include 39 | ) 40 | 41 | add_executable(feature_extraction src/feature_extraction.cpp) 42 | target_link_libraries(feature_extraction ${catkin_LIBRARIES} ${PCL_LIBRARIES}) 43 | 44 | add_executable(scan_to_scan_odometry src/scan_to_scan_odometry.cpp) 45 | target_link_libraries(scan_to_scan_odometry ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES}) 46 | 47 | add_executable(scan_to_map_odometry src/scan_to_map_odometry.cpp) 48 | target_link_libraries(scan_to_map_odometry ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES}) 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /src/lidar_odometry/include/feature_extraction.h: -------------------------------------------------------------------------------- 1 | #ifndef FEATURE_EXTRACTION_H_ 2 | #define FEATURE_EXTRACTION_H_ 3 | 4 | #endif -------------------------------------------------------------------------------- /src/lidar_odometry/include/scan_to_map_odometry.h: -------------------------------------------------------------------------------- 1 | #ifndef SCAN_TO_SCAN_MAP_H_ 2 | #define SCAN_TO_SCAN_MAP_H_ 3 | 4 | #endif -------------------------------------------------------------------------------- /src/lidar_odometry/include/scan_to_scan_odometry.h: -------------------------------------------------------------------------------- 1 | #ifndef SCAN_TO_SCAN_ODOMETRY_H_ 2 | #define SCAN_TO_SCAN_ODOMETRY_H_ 3 | 4 | #endif -------------------------------------------------------------------------------- /src/lidar_odometry/launch/run.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /src/lidar_odometry/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lidar_odometry 4 | 0.0.0 5 | The one_liom package 6 | hcx 7 | TODO 8 | catkin 9 | geometry_msgs 10 | nav_msgs 11 | roscpp 12 | rospy 13 | std_msgs 14 | turtlesim 15 | geometry_msgs 16 | nav_msgs 17 | roscpp 18 | rospy 19 | std_msgs 20 | turtlesim 21 | geometry_msgs 22 | nav_msgs 23 | roscpp 24 | rospy 25 | std_msgs 26 | turtlesim 27 | message_generation 28 | message_runtime 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /src/lidar_odometry/rviz_cfg/rviz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Odometry1/Shape1 9 | - /Odometry2/Shape1 10 | Splitter Ratio: 0.658333361 11 | Tree Height: 767 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.588679016 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: "" 31 | Toolbars: 32 | toolButtonStyle: 2 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.5 37 | Cell Size: 1 38 | Class: rviz/Grid 39 | Color: 160; 160; 164 40 | Enabled: true 41 | Line Style: 42 | Line Width: 0.0299999993 43 | Value: Lines 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 0 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 10 52 | Reference Frame: 53 | Value: true 54 | - Alpha: 1 55 | Buffer Length: 1 56 | Class: rviz/Path 57 | Color: 25; 255; 0 58 | Enabled: true 59 | Head Diameter: 0.300000012 60 | Head Length: 0.200000003 61 | Length: 0.300000012 62 | Line Style: Billboards 63 | Line Width: 0.5 64 | Name: Path 65 | Offset: 66 | X: 0 67 | Y: 0 68 | Z: 0 69 | Pose Color: 255; 85; 255 70 | Pose Style: None 71 | Radius: 0.0299999993 72 | Shaft Diameter: 0.100000001 73 | Shaft Length: 0.100000001 74 | Topic: /laser_odom_path 75 | Unreliable: false 76 | Value: true 77 | - Alpha: 1 78 | Buffer Length: 1 79 | Class: rviz/Path 80 | Color: 0; 0; 255 81 | Enabled: true 82 | Head Diameter: 0.300000012 83 | Head Length: 0.200000003 84 | Length: 0.300000012 85 | Line Style: Billboards 86 | Line Width: 0.5 87 | Name: Path 88 | Offset: 89 | X: 0 90 | Y: 0 91 | Z: 0 92 | Pose Color: 255; 85; 255 93 | Pose Style: None 94 | Radius: 0.0299999993 95 | Shaft Diameter: 0.100000001 96 | Shaft Length: 0.100000001 97 | Topic: /laser_map_path 98 | Unreliable: false 99 | Value: true 100 | - Angle Tolerance: 0.100000001 101 | Class: rviz/Odometry 102 | Covariance: 103 | Orientation: 104 | Alpha: 0.5 105 | Color: 255; 255; 127 106 | Color Style: Unique 107 | Frame: Local 108 | Offset: 1 109 | Scale: 1 110 | Value: true 111 | Position: 112 | Alpha: 0.300000012 113 | Color: 204; 51; 204 114 | Scale: 1 115 | Value: true 116 | Value: false 117 | Enabled: true 118 | Keep: 1 119 | Name: Odometry 120 | Position Tolerance: 0.100000001 121 | Shape: 122 | Alpha: 1 123 | Axes Length: 5 124 | Axes Radius: 1 125 | Color: 255; 25; 0 126 | Head Length: 0.300000012 127 | Head Radius: 0.100000001 128 | Shaft Length: 1 129 | Shaft Radius: 0.0500000007 130 | Value: Axes 131 | Topic: /laser_map_odometry 132 | Unreliable: false 133 | Value: true 134 | - Angle Tolerance: 0.100000001 135 | Class: rviz/Odometry 136 | Covariance: 137 | Orientation: 138 | Alpha: 0.5 139 | Color: 255; 255; 127 140 | Color Style: Unique 141 | Frame: Local 142 | Offset: 1 143 | Scale: 1 144 | Value: true 145 | Position: 146 | Alpha: 0.300000012 147 | Color: 204; 51; 204 148 | Scale: 1 149 | Value: true 150 | Value: true 151 | Enabled: true 152 | Keep: 1 153 | Name: Odometry 154 | Position Tolerance: 0.100000001 155 | Shape: 156 | Alpha: 1 157 | Axes Length: 5 158 | Axes Radius: 1 159 | Color: 255; 25; 0 160 | Head Length: 0.300000012 161 | Head Radius: 0.100000001 162 | Shaft Length: 1 163 | Shaft Radius: 0.0500000007 164 | Value: Axes 165 | Topic: /laser_odom_to_init 166 | Unreliable: false 167 | Value: true 168 | Enabled: true 169 | Global Options: 170 | Background Color: 255; 255; 255 171 | Default Light: true 172 | Fixed Frame: map 173 | Frame Rate: 30 174 | Name: root 175 | Tools: 176 | - Class: rviz/Interact 177 | Hide Inactive Objects: true 178 | - Class: rviz/MoveCamera 179 | - Class: rviz/Select 180 | - Class: rviz/FocusCamera 181 | - Class: rviz/Measure 182 | - Class: rviz/SetInitialPose 183 | Topic: /initialpose 184 | - Class: rviz/SetGoal 185 | Topic: /move_base_simple/goal 186 | - Class: rviz/PublishPoint 187 | Single click: true 188 | Topic: /clicked_point 189 | Value: true 190 | Views: 191 | Current: 192 | Class: rviz/Orbit 193 | Distance: 340.332214 194 | Enable Stereo Rendering: 195 | Stereo Eye Separation: 0.0599999987 196 | Stereo Focal Distance: 1 197 | Swap Stereo Eyes: false 198 | Value: false 199 | Focal Point: 200 | X: 63.1372261 201 | Y: -51.4731789 202 | Z: 21.0256786 203 | Focal Shape Fixed Size: true 204 | Focal Shape Size: 0.0500000007 205 | Invert Z Axis: false 206 | Name: Current View 207 | Near Clip Distance: 0.00999999978 208 | Pitch: 0.679797113 209 | Target Frame: map_child 210 | Value: Orbit (rviz) 211 | Yaw: 2.59543157 212 | Saved: ~ 213 | Window Geometry: 214 | Displays: 215 | collapsed: false 216 | Height: 1056 217 | Hide Left Dock: false 218 | Hide Right Dock: false 219 | QMainWindow State: 000000ff00000000fd0000000400000000000001c30000038efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000038e000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000295fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000295000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f00000046fc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000005760000038e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 220 | Selection: 221 | collapsed: false 222 | Time: 223 | collapsed: false 224 | Tool Properties: 225 | collapsed: false 226 | Views: 227 | collapsed: false 228 | Width: 1855 229 | X: 65 230 | Y: 24 231 | -------------------------------------------------------------------------------- /src/mapping/LIO-SAM/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(lio_sam) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS "-std=c++11") 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -pthread") 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | tf 10 | roscpp 11 | rospy 12 | roslib 13 | cv_bridge 14 | # pcl library 15 | pcl_conversions 16 | # msgs 17 | std_msgs 18 | sensor_msgs 19 | geometry_msgs 20 | nav_msgs 21 | message_generation 22 | visualization_msgs 23 | ) 24 | 25 | find_package(roslib) 26 | find_package(OpenMP REQUIRED) 27 | find_package(PCL REQUIRED QUIET) 28 | find_package(OpenCV REQUIRED QUIET) 29 | find_package(GTSAM REQUIRED QUIET) 30 | find_package(Boost REQUIRED COMPONENTS timer) 31 | 32 | add_message_files( 33 | DIRECTORY msg 34 | FILES 35 | cloud_info.msg 36 | ) 37 | 38 | add_service_files( 39 | DIRECTORY srv 40 | FILES 41 | save_map.srv 42 | ) 43 | 44 | generate_messages( 45 | DEPENDENCIES 46 | geometry_msgs 47 | std_msgs 48 | nav_msgs 49 | sensor_msgs 50 | ) 51 | 52 | catkin_package( 53 | INCLUDE_DIRS include 54 | DEPENDS PCL GTSAM 55 | 56 | CATKIN_DEPENDS 57 | std_msgs 58 | nav_msgs 59 | geometry_msgs 60 | sensor_msgs 61 | message_runtime 62 | message_generation 63 | visualization_msgs 64 | ) 65 | 66 | # include directories 67 | include_directories( 68 | include 69 | ${catkin_INCLUDE_DIRS} 70 | ${PCL_INCLUDE_DIRS} 71 | ${OpenCV_INCLUDE_DIRS} 72 | ${GTSAM_INCLUDE_DIR} 73 | ) 74 | 75 | # link directories 76 | link_directories( 77 | include 78 | ${PCL_LIBRARY_DIRS} 79 | ${OpenCV_LIBRARY_DIRS} 80 | ${GTSAM_LIBRARY_DIRS} 81 | ) 82 | 83 | ########### 84 | ## Build ## 85 | ########### 86 | 87 | # Gps odometry 88 | add_executable(${PROJECT_NAME}_gpsOdometry src/simpleGpsOdom.cpp) 89 | target_link_libraries(${PROJECT_NAME}_gpsOdometry ${catkin_LIBRARIES}) 90 | 91 | # Range Image Projection 92 | add_executable(${PROJECT_NAME}_imageProjection src/imageProjection.cpp) 93 | add_dependencies(${PROJECT_NAME}_imageProjection ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp) 94 | target_link_libraries(${PROJECT_NAME}_imageProjection ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) 95 | 96 | # Feature Association 97 | add_executable(${PROJECT_NAME}_featureExtraction src/featureExtraction.cpp) 98 | add_dependencies(${PROJECT_NAME}_featureExtraction ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp) 99 | target_link_libraries(${PROJECT_NAME}_featureExtraction ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) 100 | 101 | # Mapping Optimization 102 | add_executable(${PROJECT_NAME}_mapOptmization src/mapOptmization.cpp) 103 | add_dependencies(${PROJECT_NAME}_mapOptmization ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp) 104 | target_compile_options(${PROJECT_NAME}_mapOptmization PRIVATE ${OpenMP_CXX_FLAGS}) 105 | target_link_libraries(${PROJECT_NAME}_mapOptmization Boost::timer ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} ${OpenMP_CXX_FLAGS} gtsam) 106 | 107 | # IMU Preintegration 108 | add_executable(${PROJECT_NAME}_imuPreintegration src/imuPreintegration.cpp) 109 | target_link_libraries(${PROJECT_NAME}_imuPreintegration Boost::timer ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} gtsam) 110 | -------------------------------------------------------------------------------- /src/mapping/LIO-SAM/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM osrf/ros:kinetic-desktop-full-xenial 2 | 3 | RUN apt-get update \ 4 | && apt-get install -y curl \ 5 | && curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add - \ 6 | && apt-get update \ 7 | && apt-get install -y ros-kinetic-navigation \ 8 | && apt-get install -y ros-kinetic-robot-localization \ 9 | && apt-get install -y ros-kinetic-robot-state-publisher \ 10 | && rm -rf /var/lib/apt/lists/* 11 | 12 | RUN apt-get update \ 13 | && apt install -y software-properties-common \ 14 | && add-apt-repository -y ppa:borglab/gtsam-release-4.0 \ 15 | && apt-get update \ 16 | && apt install -y libgtsam-dev libgtsam-unstable-dev \ 17 | && rm -rf /var/lib/apt/lists/* 18 | 19 | SHELL ["/bin/bash", "-c"] 20 | 21 | RUN mkdir -p ~/catkin_ws/src \ 22 | && cd ~/catkin_ws/src \ 23 | && git clone https://github.com/TixiaoShan/LIO-SAM.git \ 24 | && cd .. \ 25 | && source /opt/ros/kinetic/setup.bash \ 26 | && catkin_make 27 | 28 | RUN echo "source /opt/ros/kinetic/setup.bash" >> /root/.bashrc \ 29 | && echo "source /root/catkin_ws/devel/setup.bash" >> /root/.bashrc 30 | 31 | WORKDIR /root/catkin_ws 32 | -------------------------------------------------------------------------------- /src/mapping/LIO-SAM/LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2020, Tixiao Shan 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /src/mapping/LIO-SAM/include/gpsTools.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by echo on 2019/11/26. 3 | // 4 | 5 | #ifndef PCD_COMPARE_GPSTOOLS_H 6 | #define PCD_COMPARE_GPSTOOLS_H 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #define DEG_TO_RAD 0.01745329252 13 | #define EARTH_MAJOR 6378137.0 ///< WGS84 MAJOR AXIS 14 | #define EARTH_MINOR 6356752.31424518 ///< WGS84 MINOR AXIS 15 | 16 | class GpsTools { 17 | public: 18 | GpsTools() { lla_origin_.setIdentity(); }; 19 | 20 | 21 | // Eigen::Vector3d LLA2ECEF(const Eigen::Vector3d &lla); 22 | // Eigen::Vector3d ECEF2LLA(const Eigen::Vector3d &ecef); 23 | // Eigen::Vector3d ECEF2ENU(const Eigen::Vector3d &ecef); 24 | // Eigen::Vector3d ENU2ECEF(const Eigen::Vector3d &enu); 25 | //static Eigen::Vector3d GpsMsg2Eigen(const sensor_msgs::NavSatFix &gps_msgs); 26 | // void updateGPSpose(const sensor_msgs::NavSatFix &gps_msgs); 27 | 28 | /** 29 | * ros msg to eigen 30 | * @param gps_msgs 31 | * @return 32 | */ 33 | static Eigen::Vector3d GpsMsg2Eigen(const sensor_msgs::NavSatFix &gps_msgs) { 34 | Eigen::Vector3d 35 | lla(gps_msgs.latitude, gps_msgs.longitude, gps_msgs.altitude); 36 | return lla; 37 | } 38 | 39 | 40 | /** 41 | * //2. LLA经度(longitude),纬度(latitude)和高度(altitude)经纬高坐标系 转(Earth-Centered, Earth-Fixed) 42 | * Z轴指向指向北，但不完全精确地与地球转动轴重合。转动轴有微小“摆动”，称之为“极运动(polar motion)” 43 | * X轴在球面上与格林威治线和赤道的交点 44 | * @param lla 45 | * @return 46 | */ 47 | Eigen::Vector3d LLA2ECEF(const Eigen::Vector3d &lla) { 48 | Eigen::Vector3d ecef; 49 | double lat = deg2rad(lla.x()); 50 | double lon = deg2rad(lla.y()); 51 | double alt = lla.z(); 52 | double earth_r = pow(EARTH_MAJOR, 2) 53 | / sqrt(pow(EARTH_MAJOR * cos(lat), 2) + pow(EARTH_MINOR * sin(lat), 2)); 54 | ecef.x() = (earth_r + alt) * cos(lat) * cos(lon); 55 | ecef.y() = (earth_r + alt) * cos(lat) * sin(lon); 56 | ecef.z() = (pow(EARTH_MINOR / EARTH_MAJOR, 2) * earth_r + alt) * sin(lat); 57 | 58 | return ecef; 59 | } 60 | 61 | Eigen::Vector3d ECEF2LLA(const Eigen::Vector3d &ecef) { 62 | double e = 63 | sqrt((pow(EARTH_MAJOR, 2) - pow(EARTH_MINOR, 2)) / pow(EARTH_MAJOR, 2)); 64 | double e_ = 65 | sqrt((pow(EARTH_MAJOR, 2) - pow(EARTH_MINOR, 2)) / pow(EARTH_MINOR, 2)); 66 | double p = sqrt(pow(ecef.x(), 2) + pow(ecef.y(), 2)); 67 | double theta = atan2(ecef.z() * EARTH_MAJOR, p * EARTH_MINOR); 68 | 69 | double lon = atan2(ecef.y(), ecef.x()); 70 | double lat = atan2((ecef.z() + pow(e_, 2) * EARTH_MINOR * pow(sin(theta), 3)), 71 | p - pow(e, 2) * EARTH_MAJOR * pow(cos(theta), 3)); 72 | double earth_r = pow(EARTH_MAJOR, 2) 73 | / sqrt(pow(EARTH_MAJOR * cos(lat), 2) + pow(EARTH_MINOR * sin(lat), 2)); 74 | double alt = p / cos(lat) - earth_r; 75 | Eigen::Vector3d lla(rad2deg(lat), rad2deg(lon), alt); 76 | return lla; 77 | } 78 | 79 | Eigen::Vector3d ECEF2ENU(const Eigen::Vector3d &ecef) { 80 | double lat = deg2rad(lla_origin_.x()); 81 | double lon = deg2rad(lla_origin_.y()); 82 | 83 | Eigen::Vector3d t = -LLA2ECEF(lla_origin_); 84 | Eigen::Matrix3d r; 85 | r << -sin(lon), cos(lon), 0, 86 | -cos(lon) * sin(lat), -sin(lat) * sin(lon), cos(lat), 87 | cos(lon) * cos(lat), sin(lon) * cos(lat), sin(lat); 88 | 89 | Eigen::Vector3d enu; 90 | enu = ecef + t; 91 | enu = r * enu; 92 | return enu; 93 | } 94 | 95 | Eigen::Vector3d ENU2ECEF(const Eigen::Vector3d &enu) { 96 | double lat = deg2rad(lla_origin_.x()); 97 | double lon = deg2rad(lla_origin_.y()); 98 | 99 | Eigen::Vector3d t = LLA2ECEF(lla_origin_); 100 | Eigen::Matrix3d r; 101 | r << -sin(lon), -cos(lon) * sin(lat), cos(lon) * cos(lat), 102 | cos(lon), -sin(lon) * sin(lat), sin(lon) * cos(lat), 103 | 0, cos(lat), sin(lat); 104 | Eigen::Vector3d ecef; 105 | ecef = r * enu + t; 106 | return ecef; 107 | } 108 | 109 | void updateGPSpose(const sensor_msgs::NavSatFix &gps_msgs) { 110 | //检查状态4 111 | if (gps_msgs.status.status == 4 || gps_msgs.status.status == 5 || gps_msgs.status.status == 1 112 | || gps_msgs.status.status == 2) { 113 | //第一个的时候设置为起点 114 | if (lla_origin_ == Eigen::Vector3d::Identity()) { 115 | Eigen::Vector3d lla = GpsMsg2Eigen(gps_msgs); 116 | lla_origin_ = lla; 117 | std::cout << "GPS origin: " << lla_origin_ << "\n status: " << gps_msgs.status.status << std::endl; 118 | } else { 119 | Eigen::Vector3d lla = GpsMsg2Eigen(gps_msgs); 120 | Eigen::Vector3d ecef = LLA2ECEF(lla); 121 | Eigen::Vector3d enu = ECEF2ENU(ecef); 122 | gps_pos_ = enu; 123 | std::cout << "GPS lla_origin_: " << lla_origin_ << "\n curr" << gps_pos_ << std::endl; 124 | } 125 | } 126 | } 127 | 128 | //变量部分 129 | //1.lla的起点 130 | Eigen::Vector3d lla_origin_; 131 | //2.enu下的坐标 132 | Eigen::Vector3d gps_pos_; 133 | private: 134 | static inline double deg2rad(const double °) { 135 | return deg * DEG_TO_RAD; 136 | }; 137 | static inline double rad2deg(const double &rad) { 138 | return rad / DEG_TO_RAD; 139 | } 140 | 141 | }; 142 | 143 | #endif //PCD_COMPARE_GPSTOOLS_H 144 | -------------------------------------------------------------------------------- /src/mapping/LIO-SAM/launch/include/config/robot.urdf.xacro: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /src/mapping/LIO-SAM/launch/include/config/urban08.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /src/mapping/LIO-SAM/launch/include/module_loam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /src/mapping/LIO-SAM/launch/include/module_navsat.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /src/mapping/LIO-SAM/launch/include/module_robot_state_publisher.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /src/mapping/LIO-SAM/launch/include/module_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /src/mapping/LIO-SAM/launch/include/rosconsole/rosconsole_error.conf: -------------------------------------------------------------------------------- 1 | # Set the default ros output to warning and higher 2 | log4j.logger.ros = ERROR -------------------------------------------------------------------------------- /src/mapping/LIO-SAM/launch/include/rosconsole/rosconsole_info.conf: -------------------------------------------------------------------------------- 1 | # Set the default ros output to warning and higher 2 | log4j.logger.ros = INFO -------------------------------------------------------------------------------- /src/mapping/LIO-SAM/launch/include/rosconsole/rosconsole_warn.conf: -------------------------------------------------------------------------------- 1 | # Set the default ros output to warning and higher 2 | log4j.logger.ros = WARN -------------------------------------------------------------------------------- /src/mapping/LIO-SAM/launch/run.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 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /src/mapping/LIO-SAM/msg/cloud_info.msg: -------------------------------------------------------------------------------- 1 | # Cloud Info 2 | Header header 3 | 4 | int32[] startRingIndex 5 | int32[] endRingIndex 6 | 7 | int32[] pointColInd # point column index in range image 8 | float32[] pointRange # point range 9 | 10 | int64 imuAvailable 11 | int64 odomAvailable 12 | 13 | # Attitude for LOAM initialization 14 | float32 imuRollInit 15 | float32 imuPitchInit 16 | float32 imuYawInit 17 | 18 | # Initial guess from imu pre-integration 19 | float32 initialGuessX 20 | float32 initialGuessY 21 | float32 initialGuessZ 22 | float32 initialGuessRoll 23 | float32 initialGuessPitch 24 | float32 initialGuessYaw 25 | 26 | # Point cloud messages 27 | sensor_msgs/PointCloud2 cloud_deskewed # original cloud deskewed 28 | sensor_msgs/PointCloud2 cloud_corner # extracted corner feature 29 | sensor_msgs/PointCloud2 cloud_surface # extracted surface feature 30 | 31 | # 3rd party messages 32 | sensor_msgs/PointCloud2 key_frame_cloud 33 | sensor_msgs/PointCloud2 key_frame_color 34 | sensor_msgs/PointCloud2 key_frame_poses 35 | sensor_msgs/PointCloud2 key_frame_map 36 | -------------------------------------------------------------------------------- /src/mapping/LIO-SAM/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lio_sam 4 | 1.0.0 5 | Lidar Odometry 6 | 7 | Tixiao Shan 8 | 9 | TODO 10 | 11 | Tixiao Shan 12 | 13 | catkin 14 | 15 | roscpp 16 | roscpp 17 | rospy 18 | rospy 19 | 20 | tf 21 | tf 22 | 23 | cv_bridge 24 | cv_bridge 25 | 26 | pcl_conversions 27 | pcl_conversions 28 | 29 | std_msgs 30 | std_msgs 31 | sensor_msgs 32 | sensor_msgs 33 | geometry_msgs 34 | geometry_msgs 35 | nav_msgs 36 | nav_msgs 37 | visualization_msgs 38 | visualization_msgs 39 | 40 | message_generation 41 | message_generation 42 | message_runtime 43 | message_runtime 44 | 45 | GTSAM 46 | GTSAM 47 | 48 | roslib 49 | roslib 50 | 51 | 52 | -------------------------------------------------------------------------------- /src/mapping/LIO-SAM/src/simpleGpsOdom.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | 6 | #include "ros/ros.h" 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include "utility.h" 16 | #include "gpsTools.hpp" 17 | 18 | class GNSSOdom : public ParamServer 19 | { 20 | public: 21 | GNSSOdom() : nh("~") { 22 | 23 | // nh.param("lio_sam_6axis/gpsFixTopic", gpsFixTopic, "navsat/fix"); 24 | 25 | std::cout << "gpsFixTopic: " << std::endl; 26 | 27 | gpsSub = nh.subscribe(gpsFixTopic, 100, &GNSSOdom::GNSSCB, this, ros::TransportHints().tcpNoDelay()); 28 | gpsOdomSub = nh.subscribe("/navsat/odom", 100, &GNSSOdom::GNSSOdomCB, this, ros::TransportHints().tcpNoDelay()); 29 | gpsOdomPub = nh.advertise("/gps_odom", 100, false); 30 | fusedPathPub = nh.advertise("/fused_gps_path", 100); 31 | gpsOdomPathPub = nh.advertise("/navsat/odom/path", 100); 32 | } 33 | 34 | private: 35 | void GNSSCB(const sensor_msgs::NavSatFixConstPtr &msg) { 36 | if (std::isnan(msg->latitude + msg->longitude + msg->altitude)) { 37 | ROS_ERROR("POS LLA NAN..."); 38 | return; 39 | } 40 | if (!initXyz) { 41 | ROS_INFO("Init Orgin GPS LLA %f, %f, %f", msg->latitude, msg->longitude, msg->altitude); 42 | gtools.lla_origin_ << msg->latitude, msg->longitude, msg->altitude; 43 | initXyz = true; 44 | } 45 | if (!initXyz) { 46 | ROS_ERROR("waiting init origin axis"); 47 | return; 48 | } 49 | double gps_time = msg->header.stamp.toSec(); 50 | // convert LLA to XYZ 51 | Eigen::Vector3d lla = gtools.GpsMsg2Eigen(*msg); 52 | Eigen::Vector3d ecef = gtools.LLA2ECEF(lla); 53 | Eigen::Vector3d enu = gtools.ECEF2ENU(ecef); 54 | ROS_INFO("GPS ENU XYZ : %f, %f, %f", enu(0), enu(1), enu(2)); 55 | 56 | // maybe you need to get the extrinsics between your gnss and imu 57 | // most of the time, they are in the same frame 58 | Eigen::Vector3d calib_enu = enu; 59 | 60 | // caculate yaw 61 | //bool orientation_ready_ = false; 62 | double distance = sqrt(pow(enu(1) - prevPos(1), 2) + 63 | pow(enu(0) - prevPos(0), 2)); 64 | if (distance > 0.1) { 65 | yaw = atan2(enu(1) - prevPos(1), 66 | enu(0) - prevPos(0)); // 返回值是此点与远点连线与x轴正方向的夹角 67 | yawQuat = tf::createQuaternionMsgFromYaw(yaw); 68 | prevPos = enu; 69 | prevYaw = yaw; 70 | orientationReady_ = true; 71 | if (!firstYawInit) { 72 | firstYawInit = true; 73 | gtools.lla_origin_ << msg->latitude, msg->longitude, msg->altitude; 74 | } 75 | ROS_INFO("gps yaw : %f", yaw); 76 | } else 77 | orientationReady_ = false; 78 | 79 | // make sure your initial yaw and origin postion are consistent 80 | if (!firstYawInit) { 81 | ROS_WARN("waiting init origin yaw"); 82 | return; 83 | } 84 | 85 | 86 | // pub gps odometry 87 | nav_msgs::Odometry odom_msg; 88 | odom_msg.header.stamp = msg->header.stamp; 89 | odom_msg.header.frame_id = "map"; 90 | odom_msg.child_frame_id = "gps"; 91 | 92 | // ----------------- 1. use utm ----------------------- 93 | // odom_msg.pose.pose.position.x = utm_x - origin_utm_x; 94 | // odom_msg.pose.pose.position.y = utm_y - origin_utm_y; 95 | // odom_msg.pose.pose.position.z = msg->altitude - origin_al; 96 | 97 | // ----------------- 2. use enu ----------------------- 98 | odom_msg.pose.pose.position.x = calib_enu(0); 99 | odom_msg.pose.pose.position.y = calib_enu(1); 100 | odom_msg.pose.pose.position.z = calib_enu(2); 101 | odom_msg.pose.covariance[0] = msg->position_covariance[0]; 102 | odom_msg.pose.covariance[7] = msg->position_covariance[4]; 103 | odom_msg.pose.covariance[14] = msg->position_covariance[8]; 104 | odom_msg.pose.covariance[1] = lla[0]; 105 | odom_msg.pose.covariance[2] = lla[1]; 106 | odom_msg.pose.covariance[3] = lla[2]; 107 | if (orientationReady_) 108 | odom_msg.pose.pose.orientation = yawQuat; 109 | else { 110 | // when we do not get the proper yaw, we set it NAN 111 | geometry_msgs::Quaternion quaternion = tf::createQuaternionMsgFromYaw(NAN); 112 | } 113 | gpsOdomPub.publish(odom_msg); 114 | 115 | // publish path 116 | rospath.header.frame_id = "map"; 117 | rospath.header.stamp = msg->header.stamp; 118 | geometry_msgs::PoseStamped pose; 119 | pose.header = rospath.header; 120 | pose.pose.position.x = calib_enu(0); 121 | pose.pose.position.y = calib_enu(1); 122 | pose.pose.position.z = calib_enu(2); 123 | pose.pose.orientation.x = yawQuat.x; 124 | pose.pose.orientation.y = yawQuat.y; 125 | pose.pose.orientation.z = yawQuat.z; 126 | pose.pose.orientation.w = yawQuat.w; 127 | rospath.poses.push_back(pose); 128 | fusedPathPub.publish(rospath); 129 | } 130 | 131 | void GNSSOdomCB(const nav_msgs::OdometryConstPtr &msg) 132 | { 133 | gpsOdomPath.header.frame_id = "map"; 134 | gpsOdomPath.header.stamp = msg->header.stamp; 135 | geometry_msgs::PoseStamped pose; 136 | pose.header = gpsOdomPath.header; 137 | pose.pose.position.x = msg->pose.pose.position.x; 138 | pose.pose.position.y = msg->pose.pose.position.y; 139 | pose.pose.position.z = msg->pose.pose.position.z; 140 | pose.pose.orientation.x = msg->pose.pose.orientation.x; 141 | pose.pose.orientation.y = msg->pose.pose.orientation.y; 142 | pose.pose.orientation.z = msg->pose.pose.orientation.z; 143 | pose.pose.orientation.w = msg->pose.pose.orientation.w; 144 | gpsOdomPath.poses.push_back(pose); 145 | gpsOdomPathPub.publish(gpsOdomPath); 146 | } 147 | 148 | 149 | ros::NodeHandle nh; 150 | GpsTools gtools; 151 | 152 | ros::Publisher gpsOdomPub, fusedPathPub, gpsOdomPathPub; 153 | ros::Subscriber gpsSub; 154 | ros::Subscriber gpsOdomSub; 155 | 156 | std::mutex mutexLock; 157 | std::deque gpsBuf; 158 | 159 | bool orientationReady_ = false; 160 | bool initXyz = false; 161 | bool firstYawInit = false; 162 | Eigen::Vector3d prevPos; 163 | double yaw = 0.0, prevYaw = 0.0; 164 | geometry_msgs::Quaternion yawQuat; 165 | nav_msgs::Path rospath; 166 | nav_msgs::Path gpsOdomPath; 167 | }; 168 | 169 | int main(int argc, char **argv) { 170 | ros::init(argc, argv, "lio_sam_6axis"); 171 | GNSSOdom gps; 172 | ROS_INFO("\033[1;32m----> Simple GPS Odmetry Started.\033[0m"); 173 | ros::spin(); 174 | return 0; 175 | } -------------------------------------------------------------------------------- /src/mapping/LIO-SAM/srv/save_map.srv: -------------------------------------------------------------------------------- 1 | float32 resolution 2 | string destination 3 | --- 4 | bool success 5 | -------------------------------------------------------------------------------- /src/scripts/evo.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # 命令 格式 参考轨迹 估计轨迹 [可选项] 4 | # evo_rpe tum 文件1.txt 文件2.txt -r trans_part --plot --plot_mode xy --save_results results/保存的结果文件夹的名字.zip 5 | # evo_rpe tum 文件1.txt 文件2.txt -r trans_part --plot --plot_mode xy --save_results results/保存的结果文件夹的名字.zip 6 | 7 | txt1="/home/trunk/0-ws/Learning_localization_from_scratch_ws/src/doc/ground_truth/tum_ground_truth.txt" 8 | txt2="/home/trunk/0-ws/Learning_localization_from_scratch_ws/src/doc/mapping_results/tum_lio_sam_pose.txt" 9 | 10 | evo_ape tum $txt1 $txt2 -r trans_part --plot --plot_mode xy -a & 11 | evo_rpe tum $txt1 $txt2 -r trans_part --plot --plot_mode xy -a & 12 | evo_traj tum $txt2 --ref=$txt1 -a --plot --plot_mode xy 13 | 14 | -------------------------------------------------------------------------------- /src/scripts/kaist2evo.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import argparse 3 | import os 4 | import csv 5 | import numpy as np 6 | from scipy.spatial.transform import Rotation as R 7 | 8 | if __name__ == "__main__": 9 | # 传入urban_08文件夹的路径 python3 kaist2evo.py -p [the path of urban bag] 10 | parser = argparse.ArgumentParser(description="path") 11 | parser.add_argument("-p", "--path", type=str, help="the path of urban bag") 12 | parser.add_argument("-o", "--output", default=None,type=str, help="the path of result") 13 | args = parser.parse_args() 14 | urbag_path = args.path 15 | output_path = args.output 16 | 17 | # 默认保存在urban_bag路径下 18 | if output_path is None: 19 | output_path = urbag_path 20 | print(f'urban_bag的路径为:{urbag_path}') 21 | print(f'保存结果的路径为:{output_path}') 22 | 23 | gps_file = os.path.join(urbag_path, "sensor_data","vrs_gps.csv") 24 | tum_vrs_gps = os.path.join(output_path, "tum_vrs_gps.txt") 25 | 26 | global_pose_file = os.path.join(urbag_path, "global_pose.csv") 27 | tum_ground_truth = os.path.join(output_path, "tum_ground_truth.txt") 28 | 29 | # 将vrs_gps.csv数据写成tum格式的轨迹 30 | with open(gps_file, 'r') as f1, open(tum_vrs_gps, 'w') as f2: 31 | f1 = list(csv.reader(f1)) 32 | for gps_line in f1: 33 | # print(gps_line) 34 | # sensor_data/vrs_gps.csv保存为tum格式, 姿态写成0.0 35 | tum_var_gps_line = str(float(gps_line[0]) * 0.000000001) + " " \ 36 | + str(float(gps_line[3])) + " " + str(float(gps_line[4])) + " " + str(float(gps_line[5])) + " " \ 37 | + str(0.0) + " " + str(0.0) + " " +str(0.0) + " " + str(0.0) 38 | f2.write(tum_var_gps_line + "\n") 39 | 40 | # 将global_pose.csv写成tum格式的轨迹 41 | with open(global_pose_file, 'r') as f1, open(tum_ground_truth, 'w') as f2: 42 | f1 = list(csv.reader(f1)) 43 | for global_pose_line in f1: 44 | # print(global_pose_line) 45 | # 以下是从/global_pose.csv读取的位姿数据 46 | x = float(global_pose_line[4]) 47 | y = float(global_pose_line[8]) 48 | z = float(global_pose_line[12]) 49 | 50 | rotation_matrix = np.array([float(global_pose_line[1]), float(global_pose_line[2]),float(global_pose_line[3]), 51 | float(global_pose_line[5]), float(global_pose_line[6]), float(global_pose_line[7]), 52 | float(global_pose_line[9]), float(global_pose_line[10]), float(global_pose_line[11])]) 53 | rotation_matrix = rotation_matrix.reshape(3,3) 54 | R_quat = R.from_matrix(rotation_matrix).as_quat() 55 | q_x = R_quat[0] 56 | q_y = R_quat[1] 57 | q_z = R_quat[2] 58 | q_w = R_quat[3] 59 | 60 | # 将global_pose.csv保存为tum格式 61 | tum_ground_truth_line = str(float(global_pose_line[0]) * 0.000000001) + " " \ 62 | + str(float(x)) + " " + str(float(y)) + " " + str(float(z)) + " " \ 63 | + str(q_x) + " " + str(q_y) + " " +str(q_z) + " " + str(q_w) 64 | f2.write(tum_ground_truth_line + "\n") --------------------------------------------------------------------------------