Skip to content

Commit

Permalink
Merge pull request #16 from moss-ag/ros2
Browse files Browse the repository at this point in the history 
Add ROS2 support
  • Loading branch information
@kennyjchen
kennyjchen authored Jul 25, 2023
2 parents 4217a4c + 6d804e1 commit 977094b
Show file tree
Hide file tree
Showing 17 changed files with 579 additions and 514 deletions.
132 changes: 61 additions & 71 deletions CMakeLists.txt
100644 → 100755
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,108 +12,98 @@

cmake_minimum_required(VERSION 3.12.4)
project(direct_lidar_inertial_odometry)

add_compile_options(-std=c++14)
set(CMAKE_BUILD_TYPE "Release")

find_package( PCL REQUIRED )
include_directories(${PCL_INCLUDE_DIRS})
add_definitions(${PCL_DEFINITIONS})
link_directories(${PCL_LIBRARY_DIRS})

find_package(rosidl_default_generators REQUIRED)
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(pcl_ros REQUIRED)

find_package(PCL REQUIRED)
find_package( Eigen3 REQUIRED )
include_directories(${EIGEN3_INCLUDE_DIR})
find_package(Threads REQUIRED)

include(FindOpenMP)
if(OPENMP_FOUND)
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
else(OPENMP_FOUND)
message("ERROR: OpenMP could not be found.")
endif(OPENMP_FOUND)
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_STANDARD 17)

set(CMAKE_THREAD_PREFER_PTHREAD TRUE)
set(THREADS_PREFER_PTHREAD_FLAG TRUE)
find_package(Threads REQUIRED)

find_package(catkin REQUIRED COMPONENTS
roscpp
std_msgs
sensor_msgs
geometry_msgs
nav_msgs
pcl_ros
message_generation
)

add_service_files(
DIRECTORY srv
FILES
save_pcd.srv
)

generate_messages(
DEPENDENCIES
sensor_msgs
std_msgs
geometry_msgs
)
# Not all machines have <cpuid.h> available
set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY)

catkin_package(
CATKIN_DEPENDS
roscpp
std_msgs
sensor_msgs
geometry_msgs
pcl_ros
INCLUDE_DIRS
include
LIBRARIES
rosidl_generate_interfaces(
${PROJECT_NAME}
nano_gicp
nanoflann
srv/SavePCD.srv
DEPENDENCIES
)
rosidl_get_typesupport_target(cpp_typesupport_target
${PROJECT_NAME} rosidl_typesupport_cpp)

include_directories(include)
include_directories(SYSTEM ${catkin_INCLUDE_DIRS})
include_directories(${PCL_INCLUDE_DIRS})
include_directories(${EIGEN3_INCLUDE_DIR})

# Not all machines have <cpuid.h> available
set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY)
file(WRITE ${CMAKE_BINARY_DIR}/test_cpuid.cpp "#include <cpuid.h>")
try_compile(HAS_CPUID ${CMAKE_BINARY_DIR} ${CMAKE_BINARY_DIR}/test_cpuid.cpp)
file(REMOVE ${CMAKE_BINARY_DIR}/test_cpuid.cpp)
if(HAS_CPUID)
add_compile_definitions(HAS_CPUID)
endif()
include_directories(include)
include_directories(SYSTEM)

# NanoFLANN
add_library(nanoflann STATIC
src/nano_gicp/nanoflann.cc
)
target_link_libraries(nanoflann ${PCL_LIBRARIES})
ament_target_dependencies(nanoflann)

# NanoGICP
add_library(nano_gicp STATIC
src/nano_gicp/lsq_registration.cc
src/nano_gicp/nano_gicp.cc
)
target_link_libraries(nano_gicp ${PCL_LIBRARIES} ${OpenMP_LIBS} nanoflann)
ament_target_dependencies(nano_gicp)

# Odometry Node
add_executable(dlio_odom_node src/dlio/odom_node.cc src/dlio/odom.cc)
add_dependencies(dlio_odom_node ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
target_compile_options(dlio_odom_node PRIVATE ${OpenMP_FLAGS})
target_link_libraries(dlio_odom_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenMP_LIBS} Threads::Threads nano_gicp)
target_link_libraries(dlio_odom_node ${PCL_LIBRARIES} ${OpenMP_LIBS} Threads::Threads nano_gicp)
ament_target_dependencies(dlio_odom_node "tf2_ros" "pcl_ros" "nav_msgs" "pcl_conversions" "rclcpp" "sensor_msgs" "geometry_msgs")

# Mapping Node
add_executable (dlio_map_node src/dlio/map_node.cc src/dlio/map.cc)
add_dependencies(dlio_map_node ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
target_compile_options(dlio_map_node PRIVATE ${OpenMP_FLAGS})
target_link_libraries(dlio_map_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenMP_LIBS} Threads::Threads)
target_link_libraries(dlio_map_node ${PCL_LIBRARIES} ${cpp_typesupport_target})
ament_target_dependencies(dlio_map_node "tf2_ros" "pcl_ros" "pcl_conversions" "rclcpp")

ament_export_include_directories(include)
ament_export_libraries(${PROJECT_NAME} nano_gicp nanoflann)
ament_export_dependencies(rclcpp std_msgs sensor_msgs geometry_msgs pcl_ros)
ament_package()

# Binaries
install( TARGETS dlio_odom_node dlio_map_node
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} )
DESTINATION lib/${PROJECT_NAME}
)
install( DIRECTORY cfg launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} )
DESTINATION share/${PROJECT_NAME}
)

add_compile_options(-std=c++14)
link_directories(${PCL_LIBRARY_DIRS})

include(FindOpenMP)
if(OPENMP_FOUND)
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
else(OPENMP_FOUND)
message("ERROR: OpenMP could not be found.")
endif(OPENMP_FOUND)
file(WRITE ${CMAKE_BINARY_DIR}/test_cpuid.cpp "#include <cpuid.h>")
try_compile(HAS_CPUID ${CMAKE_BINARY_DIR} ${CMAKE_BINARY_DIR}/test_cpuid.cpp)
file(REMOVE ${CMAKE_BINARY_DIR}/test_cpuid.cpp)
if(HAS_CPUID)
add_compile_definitions(HAS_CPUID)
endif()

target_compile_options(dlio_odom_node PRIVATE ${OpenMP_FLAGS})

target_compile_options(dlio_map_node PRIVATE ${OpenMP_FLAGS})
54 changes: 27 additions & 27 deletions cfg/dlio.yaml
100644 → 100755
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,35 +11,35 @@
###########################################################

dlio:
ros__parameters:

version: 1.1.1
version: 1.1.1

adaptive: true
adaptive: true

pointcloud:
deskew: true
voxelize: true
pointcloud:
deskew: true
voxelize: true

imu:
calibration: true
intrinsics:
accel:
bias: [ 0.0, 0.0, 0.0 ]
sm: [ 1., 0., 0.,
0., 1., 0.,
0., 0., 1. ]
gyro:
bias: [ 0.0, 0.0, 0.0 ]

extrinsics:
baselink2imu:
t: [ 0.006253, -0.011775, 0.007645 ]
R: [ 1., 0., 0.,
0., 1., 0.,
0., 0., 1. ]
baselink2lidar:
t: [ 0., 0., 0. ]
R: [ 1., 0., 0.,
0., 1., 0.,
0., 0., 1. ]
imu:
calibration: true
intrinsics:
accel:
bias: [ 0.0, 0.0, 0.0 ]
sm: [ 1., 0., 0.,
0., 1., 0.,
0., 0., 1. ]
gyro:
bias: [ 0.0, 0.0, 0.0 ]

extrinsics:
baselink2imu:
t: [ 0.006253, -0.011775, 0.007645 ]
R: [ 1., 0., 0.,
0., 1., 0.,
0., 0., 1. ]
baselink2lidar:
t: [ 0., 0., 0. ]
R: [ 1., 0., 0.,
0., 1., 0.,
0., 0., 1. ]
101 changes: 51 additions & 50 deletions cfg/params.yaml
100644 → 100755
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,62 +11,63 @@
###########################################################

dlio:
ros__parameters:

frames:
odom: odom
baselink: base_link
lidar: lidar
imu: imu
frames:
odom: odom
baselink: base_link
lidar: lidar
imu: imu

map:
waitUntilMove: true
dense:
filtered: false
sparse:
frequency: 1.0
leafSize: 0.25
map:
waitUntilMove: true
dense:
filtered: false
sparse:
frequency: 1.0
leafSize: 0.25

odom:
odom:

gravity: 9.80665
gravity: 9.80665

imu:
approximateGravity: false
calibration:
gyro: true
accel: true
time: 3
bufferSize: 5000
imu:
approximateGravity: false
calibration:
gyro: true
accel: true
time: 3
bufferSize: 5000

preprocessing:
cropBoxFilter:
size: 1.0
voxelFilter:
res: 0.25
preprocessing:
cropBoxFilter:
size: 1.0
voxelFilter:
res: 0.25

keyframe:
threshD: 1.0
threshR: 45.0

submap:
keyframe:
knn: 10
kcv: 10
kcc: 10
gicp:
minNumPoints: 64
kCorrespondences: 16
maxCorrespondenceDistance: 0.5
maxIterations: 32
transformationEpsilon: 0.01
rotationEpsilon: 0.01
initLambdaFactor: 1e-9
threshD: 1.0
threshR: 45.0

submap:
keyframe:
knn: 10
kcv: 10
kcc: 10
gicp:
minNumPoints: 64
kCorrespondences: 16
maxCorrespondenceDistance: 0.5
maxIterations: 32
transformationEpsilon: 0.01
rotationEpsilon: 0.01
initLambdaFactor: 1e-9

geo:
Kp: 4.5
Kv: 11.25
Kq: 4.0
Kab: 2.25
Kgb: 1.0
abias_max: 5.0
gbias_max: 0.5
geo:
Kp: 4.5
Kv: 11.25
Kq: 4.0
Kab: 2.25
Kgb: 1.0
abias_max: 5.0
gbias_max: 0.5
26 changes: 0 additions & 26 deletions include/dlio/dlio.h
100644 → 100755
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,6 @@
#include <stdlib.h>
#include <string>
#include <sys/times.h>
#include <sys/vtimes.h>
#include <thread>

template <typename T>
Expand All @@ -42,39 +41,14 @@ std::string to_string_with_precision(const T a_value, const int n = 6)
return out.str();
}

// ROS
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseArray.h>
#include <nav_msgs/Path.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf2_ros/transform_broadcaster.h>

// BOOST
#include <boost/format.hpp>
#include <boost/circular_buffer.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/range/adaptor/indexed.hpp>
#include <boost/range/adaptor/adjacent_filtered.hpp>

// PCL
#define PCL_NO_PRECOMPILE
#include <pcl/filters/crop_box.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/io/pcd_io.h>
#include <pcl/surface/concave_hull.h>
#include <pcl/surface/convex_hull.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/impl/transforms.hpp>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>

// DLIO
#include <nano_gicp/nano_gicp.h>
#include <direct_lidar_inertial_odometry/save_pcd.h>

namespace dlio {
enum class SensorType { OUSTER, VELODYNE, HESAI, UNKNOWN };
Expand Down
Loading

0 comments on commit 977094b

Please sign in to comment.