Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add ROS2 support #16

Merged
merged 1 commit into from
Jul 25, 2023
Merged

Add ROS2 support #16

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
132 changes: 61 additions & 71 deletions CMakeLists.txt
100644 → 100755
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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