Skip to content

Commit

Permalink
Heavily cleanup the CMake for velodyne_pointcloud.
Browse files Browse the repository at this point in the history
Signed-off-by: Chris Lalancette <[email protected]>
  • Loading branch information
clalancette committed Oct 30, 2024
1 parent 92d9bd8 commit 856432e
Show file tree
Hide file tree
Showing 4 changed files with 80 additions and 143 deletions.
107 changes: 68 additions & 39 deletions velodyne_pointcloud/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -23,66 +23,93 @@ find_package(sensor_msgs REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(velodyne_msgs REQUIRED)

# Resolve system dependency on yaml-cpp, which apparently does not
# provide a CMake find_package() module.
find_package(PkgConfig REQUIRED)
pkg_check_modules(YAML_CPP REQUIRED yaml-cpp)
find_path(YAML_CPP_INCLUDE_DIR
NAMES yaml_cpp.h
PATHS ${YAML_CPP_INCLUDE_DIRS})
find_library(YAML_CPP_LIBRARY
NAMES YAML_CPP
PATHS ${YAML_CPP_LIBRARY_DIRS})

link_directories(${YAML_CPP_LIBRARY_DIRS})

if(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5")
add_definitions(-DHAVE_NEW_YAMLCPP)
find_package(yaml-cpp REQUIRED)

# Older versions of yaml-cpp have a target called "yaml-cpp",
# so use that where appropriate.
if(TARGET yaml-cpp::yaml-cpp)
set(YAML_CPP_TARGET "yaml-cpp::yaml-cpp")
else()
set(YAML_CPP_TARGET ${YAML_CPP_LIBRARIES})
endif()

include_directories(include ${PCL_COMMON_INCLUDE_DIRS})
add_library(velodyne_rawdata SHARED
src/lib/rawdata.cpp
src/lib/calibration.cpp)
target_include_directories(velodyne_rawdata PUBLIC
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
target_link_libraries(velodyne_rawdata PUBLIC
angles::angles
${geometry_msgs_TARGETS}
${PCL_LIBRARIES}
rclcpp::rclcpp
${sensor_msgs_TARGETS}
tf2::tf2
tf2_ros::tf2_ros
${velodyne_msgs_TARGETS}
${YAML_CPP_TARGET}
)

add_subdirectory(src/lib)
add_library(velodyne_cloud_types SHARED
src/lib/pointcloudXYZIRT.cpp
src/lib/organized_cloudXYZIRT.cpp
)
target_include_directories(velodyne_cloud_types PUBLIC
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
target_link_libraries(velodyne_cloud_types PUBLIC
${PCL_LIBRARIES}
rclcpp::rclcpp
${sensor_msgs_TARGETS}
tf2_ros::tf2_ros
${velodyne_msgs_TARGETS}
)

add_library(transform SHARED
src/conversions/transform.cpp)
ament_target_dependencies(transform
diagnostic_updater
message_filters
rclcpp
rclcpp_components
tf2
tf2_ros
)
target_link_libraries(transform
target_include_directories(transform PUBLIC
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
target_link_libraries(transform PUBLIC
diagnostic_updater::diagnostic_updater
message_filters::message_filters
rclcpp::rclcpp
tf2::tf2
tf2_ros::tf2_ros
velodyne_cloud_types
velodyne_rawdata
${YAML_CPP_LIBRARIES})
install(TARGETS transform
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
${YAML_CPP_TARGET}
)
target_link_libraries(transform PRIVATE
rclcpp_components::component
)

install(TARGETS velodyne_cloud_types velodyne_rawdata transform
EXPORT ${PROJECT_NAME}
RUNTIME DESTINATION bin
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib)

rclcpp_components_register_nodes(transform
"velodyne_pointcloud::Transform")

add_executable(velodyne_transform_node
src/conversions/transform_node.cpp)
ament_target_dependencies(velodyne_transform_node
rclcpp
target_include_directories(velodyne_transform_node PUBLIC
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
target_link_libraries(velodyne_transform_node PRIVATE
rclcpp::rclcpp
transform
)
target_link_libraries(velodyne_transform_node
transform)
install(TARGETS velodyne_transform_node
DESTINATION lib/${PROJECT_NAME})

install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION include/${PROJECT_NAME})
install(DIRECTORY config launch params
DESTINATION share/${PROJECT_NAME})
# install(PROGRAMS scripts/gen_calibration.py
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

if(BUILD_TESTING)
# Remove empty.xml from lint
Expand Down Expand Up @@ -112,7 +139,7 @@ if(BUILD_TESTING)
add_subdirectory(tests)
endif()

ament_export_include_directories(include)
ament_export_include_directories(include/${PROJECT_NAME})
ament_export_dependencies(
diagnostic_updater
eigen
Expand All @@ -126,4 +153,6 @@ ament_export_dependencies(
velodyne_msgs
)
ament_export_libraries(velodyne_rawdata velodyne_cloud_types)
ament_export_targets(${PROJECT_NAME})

ament_package()
3 changes: 0 additions & 3 deletions velodyne_pointcloud/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,6 @@
</description>
<maintainer email="[email protected]">Josh Whitley</maintainer>
<license>BSD</license>
<url type="website">http://ros.org/wiki/velodyne_pointcloud</url>
<url type="repository">https://github.com/ros-drivers/velodyne</url>
<url type="bugtracker">https://github.com/ros-drivers/velodyne/issues</url>
<author>Jack O'Quin</author>
Expand All @@ -18,8 +17,6 @@

<buildtool_depend>ament_cmake</buildtool_depend>

<build_depend>pkg-config</build_depend>

<depend>angles</depend>
<depend>diagnostic_updater</depend>
<depend>eigen</depend>
Expand Down
48 changes: 0 additions & 48 deletions velodyne_pointcloud/src/lib/calibration.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -44,8 +44,6 @@

#include "velodyne_pointcloud/calibration.hpp"

#ifdef HAVE_NEW_YAMLCPP

namespace YAML
{

Expand All @@ -59,8 +57,6 @@ void operator>>(const YAML::Node & node, T & i)

} // namespace YAML

#endif // HAVE_NEW_YAMLCPP

namespace velodyne_pointcloud
{

Expand Down Expand Up @@ -90,74 +86,38 @@ void operator>>(const YAML::Node & node, std::pair<int, LaserCorrection> & corre
node[VERT_CORRECTION] >> correction.second.vert_correction;
node[DIST_CORRECTION] >> correction.second.dist_correction;

#ifdef HAVE_NEW_YAMLCPP

if (node[TWO_PT_CORRECTION_AVAILABLE]) {
node[TWO_PT_CORRECTION_AVAILABLE] >>
correction.second.two_pt_correction_available;
} else {
#else

if (const YAML::Node * pName = node.FindValue(TWO_PT_CORRECTION_AVAILABLE)) {
*pName >> correction.second.two_pt_correction_available;
} else {
#endif
correction.second.two_pt_correction_available = false;
}

node[DIST_CORRECTION_X] >> correction.second.dist_correction_x;
node[DIST_CORRECTION_Y] >> correction.second.dist_correction_y;
node[VERT_OFFSET_CORRECTION] >> correction.second.vert_offset_correction;

#ifdef HAVE_NEW_YAMLCPP

if (node[HORIZ_OFFSET_CORRECTION]) {
node[HORIZ_OFFSET_CORRECTION] >>
correction.second.horiz_offset_correction;
} else {
#else

if (const YAML::Node * pName = node.FindValue(HORIZ_OFFSET_CORRECTION)) {
*pName >> correction.second.horiz_offset_correction;
} else {
#endif
correction.second.horiz_offset_correction = 0;
}

float max_intensity_float = 255.0;

#ifdef HAVE_NEW_YAMLCPP

if (node[MAX_INTENSITY]) {
node[MAX_INTENSITY] >> max_intensity_float;
}

#else

if (const YAML::Node * pName = node.FindValue(MAX_INTENSITY)) {
*pName >> max_intensity_float;
}

#endif

correction.second.max_intensity = ::floorf(max_intensity_float);

float min_intensity_float = 0.0;

#ifdef HAVE_NEW_YAMLCPP

if (node[MIN_INTENSITY]) {
node[MIN_INTENSITY] >> min_intensity_float;
}

#else

if (const YAML::Node * pName = node.FindValue(MIN_INTENSITY)) {
*pName >> min_intensity_float;
}

#endif

correction.second.min_intensity = ::floorf(min_intensity_float);

node[FOCAL_DISTANCE] >> correction.second.focal_distance;
Expand Down Expand Up @@ -241,17 +201,9 @@ Calibration::Calibration(const std::string & calibration_file)
try {
YAML::Node doc;

#ifdef HAVE_NEW_YAMLCPP

fin.close();
doc = YAML::LoadFile(calibration_file);

#else

YAML::Parser parser(fin);
parser.GetNextDocument(doc);

#endif
doc >> *this;
} catch (YAML::Exception & e) {
fin.close();
Expand Down
65 changes: 12 additions & 53 deletions velodyne_pointcloud/tests/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -3,64 +3,23 @@ find_package(ament_cmake_gtest REQUIRED)
find_package(ament_index_cpp REQUIRED)

ament_add_gtest(test_row_step test_row_step.cpp)
ament_target_dependencies(test_row_step
geometry_msgs
rclcpp
sensor_msgs
tf2_ros
velodyne_msgs
target_include_directories(test_row_step PUBLIC
"$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/include>"
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
target_link_libraries(test_row_step
Eigen3::Eigen
rclcpp::rclcpp
${sensor_msgs_TARGETS}
tf2::tf2
tf2_ros::tf2_ros
${velodyne_msgs_TARGETS}
)

ament_add_gtest(test_calibration
test_calibration.cpp
)

target_link_libraries(test_calibration
ament_index_cpp::ament_index_cpp
rclcpp::rclcpp
velodyne_rawdata
)

ament_target_dependencies(test_calibration
ament_index_cpp
rclcpp
)

target_link_libraries(test_calibration
${YAML_CPP_LIBRARIES}
)

# # Download packet capture (PCAP) files containing test data.
# # Store them in devel-space, so rostest can easily find them.
# catkin_download_test_data(
# ${PROJECT_NAME}_tests_class.pcap
# http://download.ros.org/data/velodyne/class.pcap
# DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/tests
# MD5 65808d25772101358a3719b451b3d015)
# catkin_download_test_data(
# ${PROJECT_NAME}_tests_32e.pcap
# http://download.ros.org/data/velodyne/32e.pcap
# DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/tests
# MD5 e41d02aac34f0967c03a5597e1d554a9)
# catkin_download_test_data(
# ${PROJECT_NAME}_tests_64e_s2.1-300-sztaki.pcap
# http://download.ros.org/data/velodyne/64e_s2.1-300-sztaki.pcap
# DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/tests
# MD5 176c900ffb698f9b948a13e281ffc1a2)
# catkin_download_test_data(
# ${PROJECT_NAME}_tests_vlp16.pcap
# http://download.ros.org/data/velodyne/vlp16.pcap
# DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/tests
# MD5 f45c2bb1d7ee358274e423ea3b66fd73)

# run rostests
# add_rostest(transform_node_hz.test)
# add_rostest(transform_nodelet_hz.test)
# add_rostest(transform_node_32e_hz.test)
# add_rostest(transform_nodelet_32e_hz.test)
# add_rostest(transform_node_64e_s2.1_hz.test)
# add_rostest(transform_nodelet_64e_s2.1_hz.test)
# add_rostest(transform_node_vlp16_hz.test)
# add_rostest(transform_nodelet_vlp16_hz.test)
# add_rostest(two_nodelet_managers.test)

# # parse check all the launch/*.launch files
# roslaunch_add_file_check(../launch)

0 comments on commit 856432e

Please sign in to comment.