diff --git a/ouster-ros/CMakeLists.txt b/ouster-ros/CMakeLists.txt deleted file mode 100644 index b56cf112..00000000 --- a/ouster-ros/CMakeLists.txt +++ /dev/null @@ -1,231 +0,0 @@ -cmake_minimum_required(VERSION 3.10.0) - -list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/ouster-sdk/cmake) -include(DefaultBuildType) - -# ==== Project Name ==== -project(ouster_ros) - -# ==== Requirements ==== -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(std_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(ouster_sensor_msgs REQUIRED) -find_package(std_srvs REQUIRED) -find_package(rosidl_default_generators REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(PCL REQUIRED COMPONENTS common) -find_package(pcl_conversions REQUIRED) -find_package(tf2_eigen REQUIRED) - -# ==== Options ==== -add_compile_options(-Wall -Wextra) -if(NOT DEFINED CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() -option(CMAKE_POSITION_INDEPENDENT_CODE "Build position independent code." ON) - -set(_ouster_ros_INCLUDE_DIRS - include - ouster-sdk/ouster_client/include - ouster-sdk/ouster_client/include/optional-lite -) - -# ==== Libraries ==== -# Build static libraries and bundle them into ouster_ros using the `--whole-archive` flag. This is -# necessary because catkin doesn't interoperate easily with target-based cmake builds. Object -# libraries are the recommended way to do this, but require >=3.13 to propagate usage requirements. -set(_SAVE_BUILD_SHARED_LIBS ${BUILD_SHARED_LIBS}) -set(BUILD_SHARED_LIBS OFF) - -option(BUILD_VIZ "Turn off building VIZ" OFF) -option(BUILD_PCAP "Turn off building PCAP" OFF) -find_package(OusterSDK REQUIRED) - -set(BUILD_SHARED_LIBS ${_SAVE_BUILD_SHARED_LIBS}) - -# catkin adds all include dirs to a single variable, don't try to use targets -include_directories(${_ouster_ros_INCLUDE_DIRS}) - -# use only MPL-licensed parts of eigen -add_definitions(-DEIGEN_MPL2_ONLY) - -add_library(ouster_ros_library SHARED - src/os_ros.cpp -) - -set(ouster_ros_library_deps - rclcpp - sensor_msgs - geometry_msgs - ouster_sensor_msgs - pcl_conversions - tf2 - tf2_eigen -) - -ament_target_dependencies(ouster_ros_library - ${ouster_ros_library_deps} -) - -target_link_libraries(ouster_ros_library - # PUBLIC (unsupported) - ouster_build - pcl_common - # PRIVATE (unsupported) - -Wl,--whole-archive ouster_client -Wl,--no-whole-archive -) - -# helper method to construct ouster-ros components -function(create_ros2_component - component_lib_name - src_list - additonal_dependencies -) - - add_library(${component_lib_name} SHARED - ${src_list}) - target_compile_definitions(${component_lib_name} - PRIVATE - "OUSTER_ROS_BUILDING_DLL" - ) - - ament_target_dependencies(${component_lib_name} - rclcpp - class_loader - rclcpp_components - rclcpp_lifecycle - std_msgs - sensor_msgs - geometry_msgs - ${additonal_dependencies} - ) - - target_link_libraries(${component_lib_name} - ouster_ros_library - ${cpp_typesupport_target} - ) - -endfunction() - - -# ==== os_sensor_component ==== -create_ros2_component(os_sensor_component - "src/os_sensor_node_base.cpp;src/os_sensor_node.cpp" - "std_srvs" -) -rclcpp_components_register_node(os_sensor_component - PLUGIN "ouster_ros::OusterSensor" - EXECUTABLE os_sensor -) - -# ==== os_replay_component ==== -create_ros2_component(os_replay_component - "src/os_sensor_node_base.cpp;src/os_replay_node.cpp" - "" -) -rclcpp_components_register_node(os_replay_component - PLUGIN "ouster_ros::OusterReplay" - EXECUTABLE os_replay -) - -# ==== os_cloud_component ==== -create_ros2_component(os_cloud_component - "src/os_processing_node_base.cpp;src/os_cloud_node.cpp" - "tf2_ros" -) -rclcpp_components_register_node(os_cloud_component - PLUGIN "ouster_ros::OusterCloud" - EXECUTABLE os_cloud -) - -# ==== os_image_component ==== -create_ros2_component(os_image_component - "src/os_processing_node_base.cpp;src/os_image_node.cpp" - "" -) -rclcpp_components_register_node(os_image_component - PLUGIN "ouster_ros::OusterImage" - EXECUTABLE os_image -) - -# ==== os_driver_component ==== -create_ros2_component(os_driver_component - "src/os_sensor_node_base.cpp;src/os_sensor_node.cpp;src/os_driver_node.cpp" - "std_srvs" -) -rclcpp_components_register_node(os_driver_component - PLUGIN "ouster_ros::OusterDriver" - EXECUTABLE os_driver -) - - -# ==== Test ==== -if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - ament_add_gtest(${PROJECT_NAME}_test - test/ring_buffer_test.cpp - src/os_ros.cpp - test/point_accessor_test.cpp - test/point_transform_test.cpp - test/point_cloud_compose_test.cpp - ) - ament_target_dependencies(${PROJECT_NAME}_test - rclcpp - ouster_sensor_msgs - ) - target_include_directories(${PROJECT_NAME}_test PUBLIC - ${_ouster_ros_INCLUDE_DIRS} - $ - $ - ) - target_link_libraries(${PROJECT_NAME}_test ouster_ros_library) -endif() - - -# ==== Install ==== -install( - TARGETS - ouster_ros_library - os_sensor_component - os_replay_component - os_cloud_component - os_image_component - os_driver_component - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) - -install( - DIRECTORY - ${_ouster_ros_INCLUDE_DIRS} - DESTINATION - include/${PROJECT_NAME} -) - -install( - FILES - ../LICENSE - DESTINATION - share/${PROJECT_NAME} -) - -install( - DIRECTORY - launch - config - DESTINATION - share/${PROJECT_NAME} -) - -ament_export_include_directories(include) -ament_export_dependencies(rosidl_default_runtime) -ament_export_libraries(ouster_ros_library) -ament_export_dependencies(${ouster_ros_library_deps}) -ament_package() diff --git a/ouster-ros/src/os_driver_node.cpp b/ouster-ros/src/os_driver_node.cpp deleted file mode 100644 index 506ac711..00000000 --- a/ouster-ros/src/os_driver_node.cpp +++ /dev/null @@ -1,208 +0,0 @@ -/** - * Copyright (c) 2018-2023, Ouster, Inc. - * All rights reserved. - * - * @file os_driver.cpp - * @brief This node combines the capabilities of os_sensor, os_cloud and - * os_image into a single ROS node/component - */ - -// prevent clang-format from altering the location of "ouster_ros/os_ros.h", the -// header file needs to be the first include due to PCL_NO_PRECOMPILE flag -// clang-format off -#include "ouster_ros/os_ros.h" -// clang-format on - -#include "os_sensor_node.h" - -#include "os_static_transforms_broadcaster.h" -#include "imu_packet_handler.h" -#include "lidar_packet_handler.h" -#include "point_cloud_processor.h" -#include "laser_scan_processor.h" -#include "image_processor.h" -#include "point_cloud_processor_factory.h" - -namespace ouster_ros { - -namespace sensor = ouster::sensor; - -class OusterDriver : public OusterSensor { - public: - OUSTER_ROS_PUBLIC - explicit OusterDriver(const rclcpp::NodeOptions& options) - : OusterSensor("os_driver", options), tf_bcast(this) { - tf_bcast.declare_parameters(); - tf_bcast.parse_parameters(); - declare_parameter("proc_mask", "IMU|IMG|PCL|SCAN"); - declare_parameter("scan_ring", 0); - declare_parameter("ptp_utc_tai_offset", -37.0); - declare_parameter("point_type", "original"); - } - - virtual void on_metadata_updated(const sensor::sensor_info& info) override { - OusterSensor::on_metadata_updated(info); - tf_bcast.broadcast_transforms(info); - } - - virtual void create_publishers() override { - auto proc_mask = get_parameter("proc_mask").as_string(); - auto tokens = impl::parse_tokens(proc_mask, '|'); - - bool use_system_default_qos = - get_parameter("use_system_default_qos").as_bool(); - rclcpp::QoS system_default_qos = rclcpp::SystemDefaultsQoS(); - rclcpp::QoS sensor_data_qos = rclcpp::SensorDataQoS(); - auto selected_qos = - use_system_default_qos ? system_default_qos : sensor_data_qos; - - auto timestamp_mode = get_parameter("timestamp_mode").as_string(); - auto ptp_utc_tai_offset = - get_parameter("ptp_utc_tai_offset").as_double(); - - if (impl::check_token(tokens, "IMU")) { - imu_pub = - create_publisher("imu", selected_qos); - imu_packet_handler = ImuPacketHandler::create_handler( - info, tf_bcast.imu_frame_id(), timestamp_mode, - static_cast(ptp_utc_tai_offset * 1e+9)); - } - - int num_returns = get_n_returns(info); - - std::vector processors; - if (impl::check_token(tokens, "PCL")) { - lidar_pubs.resize(num_returns); - for (int i = 0; i < num_returns; ++i) { - lidar_pubs[i] = create_publisher( - topic_for_return("points", i), selected_qos); - } - - auto point_type = get_parameter("point_type").as_string(); - processors.push_back( - PointCloudProcessorFactory::create_point_cloud_processor(point_type, info, - tf_bcast.point_cloud_frame_id(), tf_bcast.apply_lidar_to_sensor_transform(), - [this](PointCloudProcessor_OutputType msgs) { - for (size_t i = 0; i < msgs.size(); ++i) lidar_pubs[i]->publish(*msgs[i]); - } - ) - ); - - // warn about profile incompatibility - if (PointCloudProcessorFactory::point_type_requires_intensity(point_type) && - info.format.udp_profile_lidar == UDPProfileLidar::PROFILE_RNG15_RFL8_NIR8) { - NODELET_WARN_STREAM( - "selected point type '" << point_type << "' is not compatible with the current udp profile: RNG15_RFL8_NIR8"); - } - } - - if (impl::check_token(tokens, "SCAN")) { - scan_pubs.resize(num_returns); - for (int i = 0; i < num_returns; ++i) { - scan_pubs[i] = create_publisher( - topic_for_return("scan", i), selected_qos); - } - - // TODO: avoid duplication in os_cloud_node - int beams_count = static_cast(get_beams_count(info)); - int scan_ring = get_parameter("scan_ring").as_int(); - scan_ring = std::min(std::max(scan_ring, 0), beams_count - 1); - if (scan_ring != get_parameter("scan_ring").as_int()) { - NODELET_WARN_STREAM( - "scan ring is set to a value that exceeds available range" - "please choose a value between [0, " - << beams_count - << "], " - "ring value clamped to: " - << scan_ring); - } - - processors.push_back(LaserScanProcessor::create( - info, tf_bcast.lidar_frame_id(), scan_ring, - [this](LaserScanProcessor::OutputType msgs) { - for (size_t i = 0; i < msgs.size(); ++i) - scan_pubs[i]->publish(*msgs[i]); - })); - } - - if (impl::check_token(tokens, "IMG")) { - const std::map - channel_field_topic_map_1{ - {sensor::ChanField::RANGE, "range_image"}, - {sensor::ChanField::SIGNAL, "signal_image"}, - {sensor::ChanField::REFLECTIVITY, "reflec_image"}, - {sensor::ChanField::NEAR_IR, "nearir_image"}}; - - const std::map - channel_field_topic_map_2{ - {sensor::ChanField::RANGE, "range_image"}, - {sensor::ChanField::SIGNAL, "signal_image"}, - {sensor::ChanField::REFLECTIVITY, "reflec_image"}, - {sensor::ChanField::NEAR_IR, "nearir_image"}, - {sensor::ChanField::RANGE2, "range_image2"}, - {sensor::ChanField::SIGNAL2, "signal_image2"}, - {sensor::ChanField::REFLECTIVITY2, "reflec_image2"}}; - - auto which_map = num_returns == 1 ? &channel_field_topic_map_1 - : &channel_field_topic_map_2; - for (auto it = which_map->begin(); it != which_map->end(); ++it) { - image_pubs[it->first] = - create_publisher(it->second, - selected_qos); - } - - processors.push_back(ImageProcessor::create( - info, tf_bcast.point_cloud_frame_id(), - [this](ImageProcessor::OutputType msgs) { - for (auto it = msgs.begin(); it != msgs.end(); ++it) { - image_pubs[it->first]->publish(*it->second); - } - })); - } - - if (impl::check_token(tokens, "PCL") || impl::check_token(tokens, "SCAN") || - impl::check_token(tokens, "IMG")) - lidar_packet_handler = LidarPacketHandler::create_handler( - info, processors, timestamp_mode, - static_cast(ptp_utc_tai_offset * 1e+9)); - } - - virtual void on_lidar_packet_msg(const uint8_t* raw_lidar_packet) override { - if (lidar_packet_handler) lidar_packet_handler(raw_lidar_packet); - } - - virtual void on_imu_packet_msg(const uint8_t* raw_imu_packet) override { - if (imu_packet_handler) - imu_pub->publish(imu_packet_handler(raw_imu_packet)); - } - - virtual void cleanup() override { - imu_packet_handler = nullptr; - lidar_packet_handler = nullptr; - imu_pub.reset(); - for (auto p : lidar_pubs) p.reset(); - for (auto p : scan_pubs) p.reset(); - for (auto p : image_pubs) p.second.reset(); - OusterSensor::cleanup(); - } - - private: - OusterStaticTransformsBroadcaster tf_bcast; - - rclcpp::Publisher::SharedPtr imu_pub; - std::vector::SharedPtr> - lidar_pubs; - std::vector::SharedPtr> - scan_pubs; - std::map::SharedPtr> - image_pubs; - ImuPacketHandler::HandlerType imu_packet_handler; - LidarPacketHandler::HandlerType lidar_packet_handler; -}; - -} // namespace ouster_ros - -#include - -RCLCPP_COMPONENTS_REGISTER_NODE(ouster_ros::OusterDriver)