diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 526de5c0..6d1530a9 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -18,15 +18,46 @@ ouster_ros(1) * fixed a potential issue of time values within generated point clouds that could result in a value overflow * added a new ``/ouster/metadata`` topic that is consumed by os_cloud and os_image nodelets and - save it to the bag file on record + save it to the bag file on record. * make specifying metadata file optional during record and replay modes as of package version 8.1 * added a no-bond option to the ``sensor.launch`` file * reduce the publish rate of imu tf transforms +* implemented a new node named ``os_driver`` which combines the functionality of ``os_sensor``, + ``os_cloud`` and ``os_image`` into a single node. The new node can be launch via the new + ``driver.launch`` file. +* introduced a new topic ``/ouster/scan`` which publishes ``sensor_msgs::LaserScan`` messages, the + user can pick which beam to be used for the message through the ``scan_ring`` launch argument. +* added ability to pick which messsages to process and through the new ``proc_mask`` launch file + argument. +* introduced a new parameter ``point_cloud_frame`` to allow users to select which frame to use when + publishing the point cloud (choose between ``sensor`` and ``lidar``). The default publishing frame + the sensor one which is in line with the current behavior. +* added the ability to change the names of ``sensor_frame``, ``lidar_frame`` and ``imu_frame`` +* added a placeholder for the ``/ouster/reset`` (not implemented for ROS1). +* breaking: switched back to using static transforms broadcast but with ability to select the frames + to be updated dynamically and at what rate through the two new launch file arguments + ``dynamic_transforms_broadcast`` and ``dynamic_transforms_broadcast_rate``. +* updated RVIZ color scheme for point clouds to match with the ROS2 version of the driver. ouster_ros(2) ------------- -* MVP ouster-ros targeting ros2 distros (beta release) +* MVP ouster-ros targeting ros2 distros * introduced a ``reset`` service to the ``os_sensor`` node +* implemented a new node named ``os_driver`` which combines the functionality of ``os_sensor``, + ``os_cloud`` and ``os_image`` into a single node. +* added support to parse the same parameters provided by the ``ros2_ouster_driver``, the parameters + are ``lidar_ip``, ``computer_ip``, ``proc_mask`` and ``use_system_default_qos``; the parameters + are fully functional and similar to what the ``ros2_ouster_driver`` provides. +* for convenience introduced a new launch file ``driver_launch.py`` that is compatible with the + ``ros2_ouster_driver`` in terms of parameters it accepts and the name of published topics. +* introduced a new parameter ``point_cloud_frame`` to allow users to select which frame to use when + publishing the point cloud (choose between ``sensor`` and ``lidar``). +* breaking: ``lidar`` frame is the default frame used when publishing point clouds. +* added the ability to choose between ``SensorDataQoS`` or ``SystemDefaultQoS`` across all published + topics with ``SensorDataQoS`` selected by default for live sensor mode and ``SystemDefaultQoS`` + enabled for record and replay modes. +* introduced a new topic ``/ouster/scan`` which publishes ``sensor_msgs::msg::LaserScan`` messages + ouster_client ------------- @@ -41,53 +72,34 @@ ouster_client [20230114] ========== -ouster_ros ----------- -* breaking change: renamed ``ouster_ros/ros.h`` to ``ouster_ros/os_ros.h`` and - ``ouster_ros/point.h`` to ``ouster_ros/os_point.h``. -* breaking change: change the type of the ring field within ``ouster::Point`` from ``uint8_t`` to - ``uint16_t`` -* correct LICENSE file installation path. -* update code files copyrights period. -* bug fix: ros driver doesn't use correct udp_dest given by user during launch -* update published TF transforms time with senosr or ros time based on the - active timestamp mode. -* validate lidar and imu port values. warn users when assigning random port numbers. -* switch to using the cartesianT method when populating pcl point cloud for performance and reduced - cpu utilization -* reduce dynamic memory allocation within the driver for performance and driver stability -* add ``pcl_ros`` as a dependency to ``package.xml`` +ouster_ros(2) +------------- +* MVP ouster-ros targeting ros2 distros (beta release) +* introduced a ``reset`` service to the ``os_sensor`` node +* breaking change: updated to ouster sdk release 20230403 +* EOL notice: ouster-ros driver will drop support for ``ROS foxy`` by May 2023. +* bugfix: Address an issue causing the driver to warn about missing non-legacy fields even they exist + in the original metadata file. +* added a new launch file ``sensor_mtp.launch.xml`` for multicast use case (experimental). +* added a technique to estimate the the value of the lidar scan timestamp when it is missing packets + at the beginning +* add frame_id to image topics +* fixed a potential issue of time values within generated point clouds that could result in a value + overflow +* added a new ``/ouster/metadata`` topic that is consumed by os_cloud and os_image nodes and save it + to the bag file on record +* make specifying metadata file optional during record and replay modes as of package version 8.1 +* replace ``tf_prefix`` from ``os_cloud`` with ``sensor_frame``, ``lidar_frame`` and ``imu_frame`` + launch parameters. +* bugfix: fixed an issue that prevents running multiple instances of the sensor and cloud components + in the same process. +* switch to using static transform publisher for the ros2 driver. ouster_client --------------- -* breaking change: signal multiplier type changed to double to support new FW values of signal - multiplier. -* breaking change: make_xyz_lut takes mat4d beam_to_lidar_transform instead of - lidar_origin_to_beam_origin_mm double to accomodate new FWs. Old reference Python implementation - was kept, but new reference was also added. -* address an issue that could cause the processed frame being dropped in favor or the previous - frame when the frame_id wraps-around. -* added a new flag ``CONFIG_FORCE_REINIT`` for ``set_config()`` method, to force the sensor to reinit - even when config params have not changed. -* breaking change: drop defaults parameters from the shortform ``init_client()`` method. -* added a new method ``init_logger()`` to provide control over the logs emitted by ``ouster_client``. -* add parsing for new FW 3.0 thermal features shot_limiting and thermal_shutdown statuses and countdowns -* add frame_status to LidarScan -* introduced a new method ``cartesianT()`` which speeds up the computation of point projecion from range - image, the method also can process the cartesian product with single float precision. A new unit test - ``cartesian_test`` which shows achieved speed up gains by the number of valid returns in lidar scan. -* added ``RAW_HEADERS`` ChanField to LidarScan for packing headers and footer (alpha version, may be - changed/removed without notice in the future) - - -[20221004] -========== - -ouster_ros ----------- -* Moved ouster-ros into separate repo -* Refresh the docker file - -ouster_sdk ----------- -* Removed ouster_ros +------------- +* added a new method ``mtp_init_client`` to init the client with multicast support (experimental). +* the class ``SensorHttp`` which provides easy access to REST APIs of the sensor has been made public + under the ``ouster::sensor::util`` namespace. +* breaking change: get_metadata defaults to outputting non-legacy metadata +* add debug five_word profile which will be removed later +* breaking change: remove deprecations on LidarScan diff --git a/CMakeLists.txt b/CMakeLists.txt index 33c4a73f..f64c0770 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -77,20 +77,27 @@ target_link_libraries(ouster_ros PUBLIC ${catkin_LIBRARIES} ouster_build pcl_com add_dependencies(ouster_ros ${PROJECT_NAME}_gencpp) # ==== Executables ==== -add_library(nodelets_os - src/os_client_base_nodelet.cpp +add_library(${PROJECT_NAME}_nodelets + src/os_sensor_nodelet_base.cpp src/os_sensor_nodelet.cpp src/os_replay_nodelet.cpp src/os_cloud_nodelet.cpp - src/os_image_nodelet.cpp) -target_link_libraries(nodelets_os ouster_ros ${catkin_LIBRARIES}) -add_dependencies(nodelets_os ${PROJECT_NAME}_gencpp) + src/os_image_nodelet.cpp + src/os_driver_nodelet.cpp) +target_link_libraries(${PROJECT_NAME}_nodelets ouster_ros ${catkin_LIBRARIES}) +add_dependencies(${PROJECT_NAME}_nodelets ${PROJECT_NAME}_gencpp) + +# ==== Test ==== +if(CATKIN_ENABLE_TESTING) + catkin_add_gtest(${PROJECT_NAME}_test tests/ring_buffer_test.cpp) + target_link_libraries(${PROJECT_NAME}_test ${catkin_LIBRARIES}) +endif() # ==== Install ==== install( TARGETS - ouster_ros - nodelets_os + ${PROJECT_NAME} + ${PROJECT_NAME}_nodelets ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} @@ -104,7 +111,7 @@ install( install( FILES LICENSE - nodelets_os.xml + ${PROJECT_NAME}_nodelets.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) diff --git a/Dockerfile b/Dockerfile index c703175c..34811a05 100644 --- a/Dockerfile +++ b/Dockerfile @@ -51,10 +51,16 @@ RUN set -xe \ FROM build-env +SHELL ["/bin/bash", "-c"] + ENV CXXFLAGS="-Werror -Wno-deprecated-declarations" -RUN /opt/ros/$ROS_DISTRO/env.sh catkin_make -DCMAKE_BUILD_TYPE=Release \ +RUN /opt/ros/$ROS_DISTRO/env.sh catkin_make \ +-DCMAKE_BUILD_TYPE=Release \ && /opt/ros/$ROS_DISTRO/env.sh catkin_make install +RUN /opt/ros/$ROS_DISTRO/env.sh catkin_make --make-args tests \ +&& source ./devel/setup.bash && rosrun ouster_ros ouster_ros_test + # Entrypoint for running Ouster ros: # # Usage: docker run --rm -it ouster-ros [sensor.launch parameters ..] diff --git a/README.md b/README.md index 73e57d2b..042dd3ef 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ # Official ROS1/ROS2 drivers for Ouster sensors [ROS1 (melodic/noetic)](https://github.com/ouster-lidar/ouster-ros/tree/master) | -[ROS2 (rolling/humble)](https://github.com/ouster-lidar/ouster-ros/tree/ros2) | +[ROS2 (rolling/humble/iron)](https://github.com/ouster-lidar/ouster-ros/tree/ros2) | [ROS2 (foxy)](https://github.com/ouster-lidar/ouster-ros/tree/ros2-foxy)

@@ -9,7 +9,7 @@ | ROS Version | Build Status (Linux) | |:-----------:|:------:| | ROS1 (melodic/noetic) | [![melodic/noetic](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml/badge.svg?branch=master)](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml) -| ROS2 (rolling/humble) | [![rolling/humble](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml/badge.svg?branch=ros2)](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml) +| ROS2 (rolling/humble/iron) | [![rolling/humble/iron](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml/badge.svg?branch=ros2)](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml) | ROS2 (foxy) | [![foxy](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml/badge.svg?branch=ros2-foxy)](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml) - [Overview](#overview) @@ -21,6 +21,7 @@ - [Recording Mode](#recording-mode) - [Replay Mode](#replay-mode) - [Multicast Mode (experimental)](#multicast-mode-experimental) + - [Launch Files Arguments](#launch-files-arguments) - [Invoking Services](#invoking-services) - [GetMetadata](#getmetadata) - [GetConfig](#getconfig) @@ -30,12 +31,12 @@ ## Overview -This ROS package provide support for all Ouster sensors with FW v2.0 or later. Upon launch the driver -will configure and connect to the selected sensor device, once connected the driver will handle -incoming IMU and lidar packets, decode lidar frames and publish corresponding ROS messages on the -topics of `/ouster/imu` and `/ouster/points`. In the case the sensor supports dual return and it was -configured to use this capability, then another topic will published named `/ouster/points2` which -corresponds to the second point cloud. +This ROS package provide support for all Ouster sensors with FW v2.0 or later. Upon launch the +driver will configure and connect to the selected sensor device, once connected the driver will +handle incoming IMU and lidar packets, decode lidar frames and publish corresponding ROS messages +on the topics of `/ouster/imu` and `/ouster/points`. In the case the used sensor supports dual +return and it was configured to use this capability, then another topic will published named +`/ouster/points2` which corresponds to the second point cloud. ## Requirements This package only supports **Melodic** and **Noetic** ROS distros. Please refer to ROS online @@ -96,11 +97,20 @@ recorded bag or _record_ a new bag file using the corresponding launch files. Re added a new mode that supports multicast. The commands are listed below: #### Sensor Mode +The driver offers two launch files to connect to an Ouster sensor: `sensor.launch` and +`driver.launch`; they differ in terms of how the processing of incoming packets is performed. +`sensor.launch` spawns three nodelets, one to connect to the sensor and publishes raw packets to +the two other nodelets which handles converting them into **Imu**, **Image** and **PointCloud2** +messages. Meanwhile, `driver.launch` file spawn a single nodelet that handles all of these tasks. +You can invoke the two files in the same way. The following line shows how to run the node using +`driver.launch`: ```bash -roslaunch ouster_ros sensor.launch \ +roslaunch ouster_ros driver.launch \ sensor_hostname:= \ metadata:= # optional ``` +`driver.launch` offers better performance and reduced overhead on the ROS bus, thus it is preferred +over `sensor.launch`. `sensor.launch` is mainly provided for backward compatibilty. #### Recording Mode > Note @@ -154,6 +164,17 @@ roslaunch ouster_ros sensor_mtp.launch \ > In both cases the **mtp_dest** is optional and if left unset the client will utilize the first available interface. +### Launch Files Arguments +Each of the previously mentioned launch files include a variety of launch arguments that helps the +user customize the driver behaivor. To view the arguments that each launch file provides and their +purpose pass `--ros-args` along with the specific launch file that you are interested in. For +example, to view launche arguments of the `driver.launch` use the following command: +```bash +roslaunch ouster_ros driver.launch --ros-args +``` +The command should list all available arguments, whether they are optional or required and the +description and posible values of each argument. + ### Invoking Services To execute any of the following service, first you need to open a new terminal and source the castkin workspace again by running the command: diff --git a/config/viz.rviz b/config/viz.rviz index 5dc13c07..570ec59a 100644 --- a/config/viz.rviz +++ b/config/viz.rviz @@ -59,7 +59,7 @@ Visualization Manager: Min Value: -10 Value: true Axis: Z - Channel Name: z + Channel Name: range Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity @@ -67,9 +67,9 @@ Visualization Manager: Enabled: true Invert Rainbow: true Max Color: 255; 255; 255 - Max Intensity: 15 + Max Intensity: 10000 Min Color: 0; 0; 0 - Min Intensity: -10 + Min Intensity: 0 Name: PointCloud2 Position Transformer: XYZ Queue Size: 10 @@ -89,7 +89,7 @@ Visualization Manager: Min Value: -10 Value: true Axis: Z - Channel Name: z + Channel Name: range Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity @@ -97,9 +97,9 @@ Visualization Manager: Enabled: true Invert Rainbow: true Max Color: 255; 255; 255 - Max Intensity: 15 + Max Intensity: 10000 Min Color: 0; 0; 0 - Min Intensity: -10 + Min Intensity: 0 Name: PointCloud2 Position Transformer: XYZ Queue Size: 10 diff --git a/include/ouster_ros/os_point.h b/include/ouster_ros/os_point.h index 403fc92d..8b745ac6 100644 --- a/include/ouster_ros/os_point.h +++ b/include/ouster_ros/os_point.h @@ -2,7 +2,7 @@ * Copyright (c) 2018-2023, Ouster, Inc. * All rights reserved. * - * @file point.h + * @file os_point.h * @brief PCL point datatype for use with ouster sensors */ diff --git a/include/ouster_ros/os_ros.h b/include/ouster_ros/os_ros.h index 743b52cf..7f888177 100644 --- a/include/ouster_ros/os_ros.h +++ b/include/ouster_ros/os_ros.h @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -32,64 +33,60 @@ using Cloud = pcl::PointCloud; using ns = std::chrono::nanoseconds; /** - * Read an imu packet into a ROS message. Blocks for up to a second if no data - * is available. - * @param[in] cli the sensor client - * @param[out] pm the destination packet message - * @param[in] pf the packet format - * @return whether reading was successful + * Checks sensor_info if it currently represents a legacy udp lidar profile + * @param[in] info sensor_info + * @return whether sensor_info represents the legacy udp lidar profile */ -bool read_imu_packet(const sensor::client& cli, PacketMsg& pm, - const sensor::packet_format& pf); +bool is_legacy_lidar_profile(const sensor::sensor_info& info); /** - * Read a lidar packet into a ROS message. Blocks for up to a second if no data - * is available. - * @param[in] cli the sensor client - * @param[out] pm the destination packet message - * @param[in] pf the packet format - * @return whether reading was successful + * Gets the number of point cloud returns that this sensor_info object + * represents + * @param[in] info sensor_info + * @return number of returns + */ +int get_n_returns(const sensor::sensor_info& info); + +/** + * Gets the number beams based on supplied sensor_info + * @param[in] info sensor_info + * @return number of beams a sensor has */ -bool read_lidar_packet(const sensor::client& cli, PacketMsg& pm, - const sensor::packet_format& pf); +size_t get_beams_count(const sensor::sensor_info& info); + +/** + * Adds a suffix to the topic base name based on the return index + * @param[in] topic_base topic base name + * @param[in] return_idx return index {0, 1, ... n_returns } + * @return number of returns + */ +std::string topic_for_return(const std::string& topic_base, int return_idx); /** * Parse an imu packet message into a ROS imu message - * @param[in] pm packet message populated by read_imu_packet + * @param[in] pf the packet format * @param[in] timestamp the timestamp to give the resulting ROS message * @param[in] frame the frame to set in the resulting ROS message - * @param[in] pf the packet format + * @param[in] buf the raw packet message populated by read_imu_packet * @return ROS sensor message with fields populated from the packet */ -sensor_msgs::Imu packet_to_imu_msg(const PacketMsg& pm, +sensor_msgs::Imu packet_to_imu_msg(const ouster::sensor::packet_format& pf, const ros::Time& timestamp, const std::string& frame, - const sensor::packet_format& pf); + const uint8_t* buf); /** * Parse an imu packet message into a ROS imu message * @param[in] pm packet message populated by read_imu_packet + * @param[in] timestamp the timestamp to give the resulting ROS message * @param[in] frame the frame to set in the resulting ROS message * @param[in] pf the packet format * @return ROS sensor message with fields populated from the packet */ -[[deprecated]] sensor_msgs::Imu packet_to_imu_msg( - const PacketMsg& pm, const std::string& frame, - const sensor::packet_format& pf); - -/** - * Populate a PCL point cloud from a LidarScan - * @param[in] xyz_lut lookup table from sensor beam angles (see lidar_scan.h) - * @param[in] scan_ts scan start used to caluclate relative timestamps for - * points - * @param[in] ls input lidar data - * @param[out] cloud output pcl pointcloud to populate - * @param[in] return_index index of return desired starting at 0 - */ -[[deprecated("use the 2nd version of scan_to_cloud_f")]] void scan_to_cloud( - const ouster::XYZLut& xyz_lut, std::chrono::nanoseconds scan_ts, - const ouster::LidarScan& ls, ouster_ros::Cloud& cloud, - int return_index = 0); +sensor_msgs::Imu packet_to_imu_msg(const PacketMsg& pm, + const ros::Time& timestamp, + const std::string& frame, + const sensor::packet_format& pf); /** * Populate a PCL point cloud from a LidarScan. @@ -99,14 +96,15 @@ sensor_msgs::Imu packet_to_imu_msg(const PacketMsg& pm, * @param[in] lut_offset the offset of the xyz lut (with single precision) * @param[in] scan_ts scan start used to caluclate relative timestamps for * points. - * @param[in] ls input lidar data + * @param[in] lidar_scan input lidar data * @param[out] cloud output pcl pointcloud to populate * @param[in] return_index index of return desired starting at 0 */ [[deprecated("use the 2nd version of scan_to_cloud_f")]] void scan_to_cloud_f( ouster::PointsF& points, const ouster::PointsF& lut_direction, const ouster::PointsF& lut_offset, std::chrono::nanoseconds scan_ts, - const ouster::LidarScan& ls, ouster_ros::Cloud& cloud, int return_index); + const ouster::LidarScan& lidar_scan, ouster_ros::Cloud& cloud, + int return_index); /** * Populate a PCL point cloud from a LidarScan. @@ -116,15 +114,15 @@ sensor_msgs::Imu packet_to_imu_msg(const PacketMsg& pm, * @param[in] lut_offset the offset of the xyz lut (with single precision) * @param[in] scan_ts scan start used to caluclate relative timestamps for * points - * @param[in] ls input lidar data + * @param[in] lidar_scan input lidar data * @param[out] cloud output pcl pointcloud to populate * @param[in] return_index index of return desired starting at 0 */ void scan_to_cloud_f(ouster::PointsF& points, const ouster::PointsF& lut_direction, const ouster::PointsF& lut_offset, uint64_t scan_ts, - const ouster::LidarScan& ls, ouster_ros::Cloud& cloud, - int return_index); + const ouster::LidarScan& lidar_scan, + ouster_ros::Cloud& cloud, int return_index); /** * Serialize a PCL point cloud to a ROS message @@ -137,16 +135,6 @@ sensor_msgs::PointCloud2 cloud_to_cloud_msg(const Cloud& cloud, const ros::Time& timestamp, const std::string& frame); -/** - * Serialize a PCL point cloud to a ROS message - * @param[in] cloud the PCL point cloud to convert - * @param[in] timestamp the timestamp to apply to the resulting ROS message - * @param[in] frame the frame to set in the resulting ROS message - * @return a ROS message containing the point cloud - */ -[[deprecated]] sensor_msgs::PointCloud2 cloud_to_cloud_msg( - const Cloud& cloud, ns timestamp, const std::string& frame); - /** * Convert transformation matrix return by sensor to ROS transform * @param[in] mat transformation matrix return by sensor @@ -158,5 +146,47 @@ sensor_msgs::PointCloud2 cloud_to_cloud_msg(const Cloud& cloud, */ geometry_msgs::TransformStamped transform_to_tf_msg( const ouster::mat4d& mat, const std::string& frame, - const std::string& child_frame, ros::Time timestamp = ros::Time::now()); + const std::string& child_frame, ros::Time timestamp); + +/** + * Convert transformation matrix return by sensor to ROS transform + * @param[in] ls lidar scan object + * @param[in] timestamp value to set as the timestamp of the generated + * @param[in] frame the parent frame of the generated laser scan message + * @param[in] lidar_mode lidar mode (width x frequency) + * @param[in] ring selected ring to be published + * @return ROS message suitable for publishing as a LaserScan + */ +sensor_msgs::LaserScan lidar_scan_to_laser_scan_msg( + const ouster::LidarScan& ls, const ros::Time& timestamp, + const std::string& frame, const ouster::sensor::lidar_mode lidar_mode, + const uint16_t ring, const int return_index); + +namespace impl { +sensor::ChanField suitable_return(sensor::ChanField input_field, bool second); + +struct read_and_cast { + template + void operator()(Eigen::Ref> field, + ouster::img_t& dest) { + dest = field.template cast(); + } +}; + +template +inline ouster::img_t get_or_fill_zero(sensor::ChanField field, + const ouster::LidarScan& ls) { + if (!ls.field_type(field)) { + return ouster::img_t::Zero(ls.h, ls.w); + } + + ouster::img_t result{ls.h, ls.w}; + ouster::impl::visit_field(ls, field, read_and_cast(), result); + return result; +} + +ros::Time ts_to_ros_time(uint64_t ts); + +} // namespace impl + } // namespace ouster_ros diff --git a/include/ouster_ros/os_client_base_nodelet.h b/include/ouster_ros/os_sensor_nodelet_base.h similarity index 64% rename from include/ouster_ros/os_client_base_nodelet.h rename to include/ouster_ros/os_sensor_nodelet_base.h index 0525e451..44925f10 100644 --- a/include/ouster_ros/os_client_base_nodelet.h +++ b/include/ouster_ros/os_sensor_nodelet_base.h @@ -12,22 +12,27 @@ #include -namespace nodelets_os { +namespace ouster_ros { -class OusterClientBase : public nodelet::Nodelet { +class OusterSensorNodeletBase : public nodelet::Nodelet { protected: bool is_arg_set(const std::string& arg) const { return arg.find_first_not_of(' ') != std::string::npos; } - void create_get_metadata_service(ros::NodeHandle& nh); + void create_get_metadata_service(); - void create_metadata_publisher(ros::NodeHandle& nh); + void create_metadata_publisher(); void publish_metadata(); void display_lidar_info(const ouster::sensor::sensor_info& info); + std::string read_text_file(const std::string& text_file); + + bool write_text_to_file(const std::string& file_path, + const std::string& text); + protected: ouster::sensor::sensor_info info; ros::ServiceServer get_metadata_srv; @@ -35,4 +40,4 @@ class OusterClientBase : public nodelet::Nodelet { ros::Publisher metadata_pub; }; -} // namespace nodelets_os \ No newline at end of file +} // namespace ouster_ros \ No newline at end of file diff --git a/launch/common.launch b/launch/common.launch index 8013b123..70c3bc9b 100644 --- a/launch/common.launch +++ b/launch/common.launch @@ -3,24 +3,57 @@ + + + + + + + + + + + + + args="load ouster_ros/OusterCloud os_nodelet_mgr $(arg _no_bond)"> + + + + + + + + + args="load ouster_ros/OusterImage os_nodelet_mgr $(arg _no_bond)"> @@ -31,10 +64,12 @@ launch-prefix="bash -c 'sleep 5; $0 $@' " args="-d $(arg rviz_config)"/> - + + diff --git a/launch/driver.launch b/launch/driver.launch new file mode 100644 index 00000000..c3b4f633 --- /dev/null +++ b/launch/driver.launch @@ -0,0 +1,89 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/record.launch b/launch/record.launch index ee7f590f..df445780 100644 --- a/launch/record.launch +++ b/launch/record.launch @@ -29,7 +29,32 @@ + + + + + + + + + + + + + args="load ouster_ros/OusterSensor os_nodelet_mgr"> @@ -59,7 +84,17 @@ + + + + + + + + diff --git a/launch/replay.launch b/launch/replay.launch index 6120cd6b..7f2080fb 100644 --- a/launch/replay.launch +++ b/launch/replay.launch @@ -9,7 +9,32 @@ }"/> + + + + + + + + + + + + + args="load ouster_ros/OusterReplay os_nodelet_mgr"> @@ -34,7 +59,17 @@ + + + + + + + + diff --git a/launch/sensor.launch b/launch/sensor.launch index 5d086f56..1d252b01 100644 --- a/launch/sensor.launch +++ b/launch/sensor.launch @@ -28,12 +28,37 @@ + + + + + - + + + + + + + + + args="load ouster_ros/OusterSensor os_nodelet_mgr $(arg _no_bond)"> @@ -62,8 +87,18 @@ + + + + + + + + diff --git a/launch/sensor_mtp.launch b/launch/sensor_mtp.launch index 8a0cb711..39545c15 100644 --- a/launch/sensor_mtp.launch +++ b/launch/sensor_mtp.launch @@ -32,7 +32,37 @@ + + + + + + + + + + + + + + + + + args="load ouster_ros/OusterSensor os_nodelet_mgr $(arg _no_bond)"> @@ -63,7 +93,18 @@ + + + + + + + + + diff --git a/nodelets_os.xml b/nodelets_os.xml deleted file mode 100644 index 45c2162a..00000000 --- a/nodelets_os.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - - A nodelet that connects to an Ouster sensor and publish incoming imu and lidar packets to other ros nodes. - - - - - A nodelet that can load up existing Ouster recordings and replay them. - - - - - A nodelet that process incoming Ouster lidar packets and publishes a corresponding point cloud. - - - - - A nodelet that processes Ouster point clouds and publish them as depth images. - - - diff --git a/ouster_ros_nodelets.xml b/ouster_ros_nodelets.xml new file mode 100644 index 00000000..cb7314aa --- /dev/null +++ b/ouster_ros_nodelets.xml @@ -0,0 +1,27 @@ + + + + A nodelet that connects to an Ouster sensor and publish incoming imu and lidar packets to other ros nodes. + + + + + A nodelet that can load up existing Ouster recordings and replay them. + + + + + A nodelet that process incoming Ouster lidar packets and publishes a corresponding point cloud. + + + + + A nodelet that processes Ouster point clouds and publish them as depth images. + + + + + A nodelet that combines the capabilities of OusterSensor, OusterCloud and OusterImage into a single nodelet. + + + diff --git a/package.xml b/package.xml index 55840171..ae550b51 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ ouster_ros - 0.8.3 + 0.9.0 Ouster ROS driver ouster developers BSD @@ -35,6 +35,6 @@ gtest - + diff --git a/src/image_processor.h b/src/image_processor.h new file mode 100644 index 00000000..e3eefd8f --- /dev/null +++ b/src/image_processor.h @@ -0,0 +1,198 @@ +/** + * Copyright (c) 2018-2023, Ouster, Inc. + * All rights reserved. + * + * @file point_cloud_producer.h + * @brief takes in a lidar scan object and produces a PointCloud2 message + */ + +#pragma once + +// 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 + +#include "ouster/image_processing.h" + +namespace sensor = ouster::sensor; +namespace viz = ouster::viz; + +namespace ouster_ros { +class ImageProcessor { + public: + using OutputType = + std::map>; + using PostProcessingFn = std::function; + + public: + ImageProcessor(const ouster::sensor::sensor_info& info, + const std::string& frame_id, PostProcessingFn func) + : frame(frame_id), post_processing_fn(func), info_(info) { + uint32_t H = info.format.pixels_per_column; + uint32_t W = info.format.columns_per_frame; + + image_msgs[sensor::ChanField::RANGE] = + std::make_shared(); + image_msgs[sensor::ChanField::SIGNAL] = + std::make_shared(); + image_msgs[sensor::ChanField::REFLECTIVITY] = + std::make_shared(); + image_msgs[sensor::ChanField::NEAR_IR] = + std::make_shared(); + if (get_n_returns(info) == 2) { + image_msgs[sensor::ChanField::RANGE2] = + std::make_shared(); + image_msgs[sensor::ChanField::SIGNAL2] = + std::make_shared(); + image_msgs[sensor::ChanField::REFLECTIVITY2] = + std::make_shared(); + } + + for (auto it = image_msgs.begin(); it != image_msgs.end(); ++it) { + init_image_msg(*it->second, H, W, frame); + } + } + + private: + using pixel_type = uint16_t; + const size_t pixel_value_max = std::numeric_limits::max(); + + static void init_image_msg(sensor_msgs::Image& msg, size_t H, size_t W, + const std::string& frame) { + msg.width = W; + msg.height = H; + msg.step = W * sizeof(pixel_type); + // TODO: allow choosing higher image encoding representation + msg.encoding = sensor_msgs::image_encodings::MONO16; + msg.data.resize(W * H * sizeof(pixel_type)); + msg.header.frame_id = frame; + } + + private: + void process(const ouster::LidarScan& lidar_scan, uint64_t, + const ros::Time& msg_ts) { + process_return(lidar_scan, 0); + if (get_n_returns(info_) == 2) process_return(lidar_scan, 1); + for (auto it = image_msgs.begin(); it != image_msgs.end(); ++it) { + it->second->header.stamp = msg_ts; + } + if (post_processing_fn) post_processing_fn(image_msgs); + } + + void process_return(const ouster::LidarScan& lidar_scan, int return_index) { + const bool first = return_index == 0; + + // across supported lidar profiles range is always 32-bit + auto range_channel = + first ? sensor::ChanField::RANGE : sensor::ChanField::RANGE2; + ouster::img_t range = + lidar_scan.field(range_channel); + + ouster::img_t reflectivity = impl::get_or_fill_zero( + impl::suitable_return(sensor::ChanField::REFLECTIVITY, !first), + lidar_scan); + + ouster::img_t signal = impl::get_or_fill_zero( + impl::suitable_return(sensor::ChanField::SIGNAL, !first), + lidar_scan); + + // TODO: note that near_ir will be processed twice for DUAL return + // sensor + ouster::img_t near_ir = impl::get_or_fill_zero( + impl::suitable_return(sensor::ChanField::NEAR_IR, !first), + lidar_scan); + + uint32_t H = info_.format.pixels_per_column; + uint32_t W = info_.format.columns_per_frame; + + // views into message data + auto range_image_map = Eigen::Map>( + (pixel_type*)image_msgs[impl::suitable_return( + sensor::ChanField::RANGE, !first)] + ->data.data(), + H, W); + auto signal_image_map = Eigen::Map>( + (pixel_type*)image_msgs[impl::suitable_return( + sensor::ChanField::SIGNAL, !first)] + ->data.data(), + H, W); + auto reflec_image_map = Eigen::Map>( + (pixel_type*) + image_msgs[impl::suitable_return( + sensor::ChanField::REFLECTIVITY, !first)] + ->data.data(), + H, W); + auto nearir_image_map = Eigen::Map>( + (pixel_type*)image_msgs[impl::suitable_return( + sensor::ChanField::NEAR_IR, !first)] + ->data.data(), + H, W); + + const auto& px_offset = info_.format.pixel_shift_by_row; + + ouster::img_t signal_image_eigen(H, W); + ouster::img_t reflec_image_eigen(H, W); + ouster::img_t nearir_image_eigen(H, W); + + const auto rg = range.data(); + const auto sg = signal.data(); + const auto rf = reflectivity.data(); + const auto nr = near_ir.data(); + + // copy data out of Cloud message, with destaggering + for (size_t u = 0; u < H; u++) { + for (size_t v = 0; v < W; v++) { + const size_t vv = (v + W - px_offset[u]) % W; + const size_t idx = u * W + vv; + // TODO: re-examine this truncation later + // 16 bit img: use 4mm resolution and throw out returns > 260m + auto r = (rg[idx] + 0b10) >> 2; + range_image_map(u, v) = r > pixel_value_max ? 0 : r; + signal_image_eigen(u, v) = sg[idx]; + reflec_image_eigen(u, v) = rf[idx]; + nearir_image_eigen(u, v) = nr[idx]; + } + } + + signal_ae(signal_image_eigen, first); + reflec_ae(reflec_image_eigen, first); + nearir_buc(nearir_image_eigen); + nearir_ae(nearir_image_eigen, first); + nearir_image_eigen = nearir_image_eigen.sqrt(); + signal_image_eigen = signal_image_eigen.sqrt(); + + // copy data into image messages + signal_image_map = + (signal_image_eigen * pixel_value_max).cast(); + reflec_image_map = + (reflec_image_eigen * pixel_value_max).cast(); + nearir_image_map = + (nearir_image_eigen * pixel_value_max).cast(); + } + + public: + static LidarScanProcessor create(const ouster::sensor::sensor_info& info, + const std::string& frame, + PostProcessingFn func) { + auto handler = std::make_shared(info, frame, func); + return [handler](const ouster::LidarScan& lidar_scan, uint64_t scan_ts, + const ros::Time& msg_ts) { + handler->process(lidar_scan, scan_ts, msg_ts); + }; + } + + private: + std::string frame; + OutputType image_msgs; + PostProcessingFn post_processing_fn; + sensor::sensor_info info_; + + viz::AutoExposure nearir_ae, signal_ae, reflec_ae; + viz::BeamUniformityCorrector nearir_buc; +}; + +} // namespace ouster_ros \ No newline at end of file diff --git a/src/imu_packet_handler.h b/src/imu_packet_handler.h new file mode 100644 index 00000000..2217ea6e --- /dev/null +++ b/src/imu_packet_handler.h @@ -0,0 +1,43 @@ +/** + * Copyright (c) 2018-2023, Ouster, Inc. + * All rights reserved. + * + * @file imu_packet_handler.h + * @brief ... + */ + +#pragma once + +// 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 + +namespace ouster_ros { + +class ImuPacketHandler { + public: + using HandlerOutput = sensor_msgs::Imu; + using HandlerType = std::function; + + public: + static HandlerType create_handler(const ouster::sensor::sensor_info& info, + const std::string& frame, + bool use_ros_time) { + const auto& pf = ouster::sensor::get_format(info); + using Timestamper = std::function; + // clang-format off + auto timestamper = use_ros_time ? + Timestamper{[](const uint8_t* /*imu_buf*/) { + return ros::Time::now(); }} : + Timestamper{[pf](const uint8_t* imu_buf) { + return impl::ts_to_ros_time(pf.imu_gyro_ts(imu_buf)); }}; + // clang-format on + return [&pf, &frame, timestamper](const uint8_t* imu_buf) { + return packet_to_imu_msg(pf, timestamper(imu_buf), frame, imu_buf); + }; + } +}; + +} // namespace ouster_ros \ No newline at end of file diff --git a/src/laser_scan_processor.h b/src/laser_scan_processor.h new file mode 100644 index 00000000..ccd184e7 --- /dev/null +++ b/src/laser_scan_processor.h @@ -0,0 +1,67 @@ +/** + * Copyright (c) 2018-2023, Ouster, Inc. + * All rights reserved. + * + * @file point_cloud_producer.h + * @brief takes in a lidar scan object and produces a PointCloud2 message + */ + +#pragma once + +// 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 + +namespace ouster_ros { + +class LaserScanProcessor { + public: + using OutputType = std::vector>; + using PostProcessingFn = std::function; + + public: + LaserScanProcessor(const ouster::sensor::sensor_info& info, + const std::string& frame_id, uint16_t ring, + PostProcessingFn func) + : frame(frame_id), + ld_mode(info.mode), + ring_(ring), + scan_msgs(get_n_returns(info), + std::make_shared()), + post_processing_fn(func) {} + + private: + void process(const ouster::LidarScan& lidar_scan, uint64_t, + const ros::Time& msg_ts) { + for (int i = 0; i < static_cast(scan_msgs.size()); ++i) { + *scan_msgs[i] = lidar_scan_to_laser_scan_msg( + lidar_scan, msg_ts, frame, ld_mode, ring_, i); + } + + if (post_processing_fn) post_processing_fn(scan_msgs); + } + + public: + static LidarScanProcessor create(const ouster::sensor::sensor_info& info, + const std::string& frame, uint16_t ring, + PostProcessingFn func) { + auto handler = + std::make_shared(info, frame, ring, func); + + return [handler](const ouster::LidarScan& lidar_scan, uint64_t scan_ts, + const ros::Time& msg_ts) { + handler->process(lidar_scan, scan_ts, msg_ts); + }; + } + + private: + std::string frame; + sensor::lidar_mode ld_mode; + uint16_t ring_; + OutputType scan_msgs; + PostProcessingFn post_processing_fn; +}; + +} // namespace ouster_ros \ No newline at end of file diff --git a/src/lidar_packet_handler.h b/src/lidar_packet_handler.h new file mode 100644 index 00000000..a9c006e0 --- /dev/null +++ b/src/lidar_packet_handler.h @@ -0,0 +1,291 @@ +/** + * Copyright (c) 2018-2023, Ouster, Inc. + * All rights reserved. + * + * @file lidar_packet_handler.h + * @brief ... + */ + +#pragma once + +// 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 + +namespace { + +template +int find_if_reverse(const Eigen::Array& array, + UnaryPredicate predicate) { + auto p = array.data() + array.size() - 1; + do { + if (predicate(*p)) return p - array.data(); + } while (p-- != array.data()); + return -1; +} + +uint64_t linear_interpolate(int x0, uint64_t y0, int x1, uint64_t y1, int x) { + uint64_t min_v, max_v; + double sign; + if (y1 > y0) { + min_v = y0; + max_v = y1; + sign = +1; + } else { + min_v = y1; + max_v = y0; + sign = -1; + } + return y0 + (x - x0) * sign * (max_v - min_v) / (x1 - x0); +} + +template +uint64_t ulround(T value) { + T rounded_value = std::round(value); + if (rounded_value < 0) return 0ULL; + if (rounded_value > ULLONG_MAX) return ULLONG_MAX; + return static_cast(rounded_value); +} + +// TODO: move to a separate file +std::set parse_tokens(const std::string& input, char delim) { + std::set tokens; + std::stringstream ss(input); + std::string token; + + while (getline(ss, token, delim)) { + // Remove leading and trailing whitespaces from the token + size_t start = token.find_first_not_of(" "); + size_t end = token.find_last_not_of(" "); + token = token.substr(start, end - start + 1); + if (!token.empty()) tokens.insert(token); + } + + return tokens; +} + +inline bool check_token(const std::set& tokens, + const std::string& token) { + return tokens.find(token) != tokens.end(); +} + +} // namespace + +namespace sensor = ouster::sensor; + +using LidarScanProcessor = + std::function; + +namespace ouster_ros { + +class LidarPacketHandler { + using LidarPacketAccumlator = std::function; + + public: + using HandlerOutput = ouster::LidarScan; + + using HandlerType = std::function; + + public: + LidarPacketHandler(const ouster::sensor::sensor_info& info, + bool use_ros_time) { + // initialize lidar_scan processor and buffer + scan_batcher = std::make_unique(info); + lidar_scan = std::make_unique( + info.format.columns_per_frame, info.format.pixels_per_column, + info.format.udp_profile_lidar); + + // initalize time handlers + scan_col_ts_spacing_ns = compute_scan_col_ts_spacing_ns(info.mode); + compute_scan_ts = [this](const auto& ts_v) { + return compute_scan_ts_0(ts_v); + }; + const sensor::packet_format& pf = sensor::get_format(info); + + lidar_packet_accumlator = + use_ros_time + ? LidarPacketAccumlator{[this, pf](const uint8_t* lidar_buf) { + return lidar_handler_ros_time(pf, lidar_buf); + }} + : LidarPacketAccumlator{[this, pf](const uint8_t* lidar_buf) { + return lidar_handler_sensor_time(pf, lidar_buf); + }}; + } + + LidarPacketHandler(const LidarPacketHandler&) = delete; + LidarPacketHandler& operator=(const LidarPacketHandler&) = delete; + ~LidarPacketHandler() = default; + + void register_lidar_scan_handler(LidarScanProcessor handler) { + lidar_scan_handlers.push_back(handler); + } + + void clear_registered_lidar_scan_handlers() { lidar_scan_handlers.clear(); } + + public: + static HandlerType create_handler( + const ouster::sensor::sensor_info& info, bool use_ros_time, + const std::vector& handlers) { + auto handler = std::make_shared(info, use_ros_time); + + handler->lidar_scan_handlers = handlers; + + return [handler](const uint8_t* lidar_buf) { + if (handler->lidar_packet_accumlator(lidar_buf)) { + for (auto h : handler->lidar_scan_handlers) { + h(*handler->lidar_scan, handler->lidar_scan_estimated_ts, + handler->lidar_scan_estimated_msg_ts); + } + } + }; + } + + // time interpolation methods + uint64_t impute_value(int last_scan_last_nonzero_idx, + uint64_t last_scan_last_nonzero_value, + int curr_scan_first_nonzero_idx, + uint64_t curr_scan_first_nonzero_value, + int scan_width) { + assert(scan_width + curr_scan_first_nonzero_idx > + last_scan_last_nonzero_idx); + double interpolated_value = linear_interpolate( + last_scan_last_nonzero_idx, last_scan_last_nonzero_value, + scan_width + curr_scan_first_nonzero_idx, + curr_scan_first_nonzero_value, scan_width); + return ulround(interpolated_value); + } + + uint64_t extrapolate_value(int curr_scan_first_nonzero_idx, + uint64_t curr_scan_first_nonzero_value) { + double extrapolated_value = + curr_scan_first_nonzero_value - + scan_col_ts_spacing_ns * curr_scan_first_nonzero_idx; + return ulround(extrapolated_value); + } + + // compute_scan_ts_0 for first scan + uint64_t compute_scan_ts_0( + const ouster::LidarScan::Header& ts_v) { + auto idx = std::find_if(ts_v.data(), ts_v.data() + ts_v.size(), + [](uint64_t h) { return h != 0; }); + assert(idx != ts_v.data() + ts_v.size()); // should never happen + int curr_scan_first_nonzero_idx = idx - ts_v.data(); + uint64_t curr_scan_first_nonzero_value = *idx; + + uint64_t scan_ns = + curr_scan_first_nonzero_idx == 0 + ? curr_scan_first_nonzero_value + : extrapolate_value(curr_scan_first_nonzero_idx, + curr_scan_first_nonzero_value); + + last_scan_last_nonzero_idx = + find_if_reverse(ts_v, [](uint64_t h) { return h != 0; }); + assert(last_scan_last_nonzero_idx >= 0); // should never happen + last_scan_last_nonzero_value = ts_v(last_scan_last_nonzero_idx); + compute_scan_ts = [this](const auto& ts_v) { + return compute_scan_ts_n(ts_v); + }; + + return scan_ns; + } + + // compute_scan_ts_n applied to all subsequent scans except first one + uint64_t compute_scan_ts_n( + const ouster::LidarScan::Header& ts_v) { + auto idx = std::find_if(ts_v.data(), ts_v.data() + ts_v.size(), + [](uint64_t h) { return h != 0; }); + assert(idx != ts_v.data() + ts_v.size()); // should never happen + int curr_scan_first_nonzero_idx = idx - ts_v.data(); + uint64_t curr_scan_first_nonzero_value = *idx; + + uint64_t scan_ns = curr_scan_first_nonzero_idx == 0 + ? curr_scan_first_nonzero_value + : impute_value(last_scan_last_nonzero_idx, + last_scan_last_nonzero_value, + curr_scan_first_nonzero_idx, + curr_scan_first_nonzero_value, + static_cast(ts_v.size())); + + last_scan_last_nonzero_idx = + find_if_reverse(ts_v, [](uint64_t h) { return h != 0; }); + assert(last_scan_last_nonzero_idx >= 0); // should never happen + last_scan_last_nonzero_value = ts_v(last_scan_last_nonzero_idx); + return scan_ns; + } + + uint16_t packet_col_index(const sensor::packet_format& pf, + const uint8_t* lidar_buf) { + return pf.col_measurement_id(pf.nth_col(0, lidar_buf)); + } + + ros::Time extrapolate_frame_ts(const sensor::packet_format& pf, + const uint8_t* lidar_buf, + const ros::Time current_time) { + auto curr_scan_first_arrived_idx = packet_col_index(pf, lidar_buf); + auto delta_time = ros::Duration( + 0, + std::lround(scan_col_ts_spacing_ns * curr_scan_first_arrived_idx)); + return current_time - delta_time; + } + + bool lidar_handler_sensor_time(const sensor::packet_format&, + const uint8_t* lidar_buf) { + if (!(*scan_batcher)(lidar_buf, *lidar_scan)) return false; + lidar_scan_estimated_ts = compute_scan_ts(lidar_scan->timestamp()); + lidar_scan_estimated_msg_ts = + impl::ts_to_ros_time(lidar_scan_estimated_ts); + return true; + } + + bool lidar_handler_ros_time(const sensor::packet_format& pf, + const uint8_t* lidar_buf) { + auto packet_receive_time = ros::Time::now(); + if (!lidar_handler_ros_time_frame_ts_initialized) { + lidar_handler_ros_time_frame_ts = extrapolate_frame_ts( + pf, lidar_buf, packet_receive_time); // first point cloud time + lidar_handler_ros_time_frame_ts_initialized = true; + } + if (!(*scan_batcher)(lidar_buf, *lidar_scan)) return false; + lidar_scan_estimated_ts = compute_scan_ts(lidar_scan->timestamp()); + lidar_scan_estimated_msg_ts = lidar_handler_ros_time_frame_ts; + // set time for next point cloud msg + lidar_handler_ros_time_frame_ts = + extrapolate_frame_ts(pf, lidar_buf, packet_receive_time); + return true; + } + + static double compute_scan_col_ts_spacing_ns(sensor::lidar_mode ld_mode) { + const auto scan_width = sensor::n_cols_of_lidar_mode(ld_mode); + const auto scan_frequency = sensor::frequency_of_lidar_mode(ld_mode); + const double one_sec_in_ns = 1e+9; + return one_sec_in_ns / (scan_width * scan_frequency); + } + + private: + std::unique_ptr scan_batcher; + std::unique_ptr lidar_scan; + uint64_t lidar_scan_estimated_ts; + ros::Time lidar_scan_estimated_msg_ts; + + bool lidar_handler_ros_time_frame_ts_initialized = false; + ros::Time lidar_handler_ros_time_frame_ts; + + int last_scan_last_nonzero_idx = -1; + uint64_t last_scan_last_nonzero_value = 0; + + double scan_col_ts_spacing_ns; // interval or spacing between columns of a + // scan + + std::function&)> + compute_scan_ts; + + std::vector lidar_scan_handlers; + + LidarPacketAccumlator lidar_packet_accumlator; +}; + +} // namespace ouster_ros \ No newline at end of file diff --git a/src/os_cloud_nodelet.cpp b/src/os_cloud_nodelet.cpp index a58576bb..b6ea552d 100644 --- a/src/os_cloud_nodelet.cpp +++ b/src/os_cloud_nodelet.cpp @@ -6,7 +6,7 @@ * @brief A nodelet to publish point clouds and imu topics */ -// prevent clang-format from altering the location of "ouster_ros/ros.h", the +// 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" @@ -19,8 +19,6 @@ #include #include #include -#include -#include #include #include @@ -28,335 +26,173 @@ #include #include "ouster_ros/PacketMsg.h" +#include "os_transforms_broadcaster.h" +#include "imu_packet_handler.h" +#include "lidar_packet_handler.h" +#include "point_cloud_processor.h" +#include "laser_scan_processor.h" namespace sensor = ouster::sensor; -using ouster_ros::PacketMsg; -using sensor::UDPProfileLidar; -using namespace std::chrono_literals; - -namespace { - -template -int find_if_reverse(const Eigen::Array& array, - UnaryPredicate predicate) { - auto p = array.data() + array.size() - 1; - do { - if (predicate(*p)) return p - array.data(); - } while (p-- != array.data()); - return -1; -} - -uint64_t linear_interpolate(int x0, uint64_t y0, int x1, uint64_t y1, int x) { - uint64_t min_v, max_v; - double sign; - if (y1 > y0) { - min_v = y0; - max_v = y1; - sign = +1; - } else { - min_v = y1; - max_v = y0; - sign = -1; - } - return y0 + (x - x0) * sign * (max_v - min_v) / (x1 - x0); -} - -template -uint64_t ulround(T value) { - T rounded_value = std::round(value); - if (rounded_value < 0) return 0ULL; - if (rounded_value > ULLONG_MAX) return ULLONG_MAX; - return static_cast(rounded_value); -} -} // namespace +namespace ouster_ros { -namespace nodelets_os { class OusterCloud : public nodelet::Nodelet { - private: - bool is_arg_set(const std::string& arg) const { - return arg.find_first_not_of(' ') != std::string::npos; - } + public: + OusterCloud() : tf_bcast(getName()) {} + private: virtual void onInit() override { - parse_parameters(); - create_metadata_subscriber(getNodeHandle()); + create_metadata_subscriber(); NODELET_INFO("OusterCloud: nodelet created!"); } - void parse_parameters() { - auto& pnh = getPrivateNodeHandle(); - auto tf_prefix = pnh.param("tf_prefix", std::string{}); - if (is_arg_set(tf_prefix) && tf_prefix.back() != '/') - tf_prefix.append("/"); - sensor_frame = tf_prefix + "os_sensor"; - imu_frame = tf_prefix + "os_imu"; - lidar_frame = tf_prefix + "os_lidar"; - auto timestamp_mode_arg = pnh.param("timestamp_mode", std::string{}); - use_ros_time = timestamp_mode_arg == "TIME_FROM_ROS_TIME"; - } - - void create_metadata_subscriber(ros::NodeHandle& nh) { - metadata_sub = nh.subscribe( + void create_metadata_subscriber() { + metadata_sub = getNodeHandle().subscribe( "metadata", 1, &OusterCloud::metadata_handler, this); } void metadata_handler(const std_msgs::String::ConstPtr& metadata_msg) { // TODO: handle sensor reconfigurtion NODELET_INFO("OusterCloud: retrieved new sensor metadata!"); - auto metadata = metadata_msg->data; - info = sensor::parse_metadata(metadata); - n_returns = compute_n_returns(info.format); - scan_col_ts_spacing_ns = compute_scan_col_ts_spacing_ns(info.mode); - create_lidarscan_objects(); - compute_scan_ts = [this](const auto& ts_v) { - return compute_scan_ts_0(ts_v); - }; - auto& nh = getNodeHandle(); - create_publishers(nh); - create_subscribers(nh); - } - - static int compute_n_returns(const sensor::data_format& format) { - return format.udp_profile_lidar == - UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL - ? 2 - : 1; - } - - static double compute_scan_col_ts_spacing_ns(sensor::lidar_mode ld_mode) { - const auto scan_width = sensor::n_cols_of_lidar_mode(ld_mode); - const auto scan_frequency = sensor::frequency_of_lidar_mode(ld_mode); - const double one_sec_in_ns = 1e+9; - return one_sec_in_ns / (scan_width * scan_frequency); - } - void create_lidarscan_objects() { - // The ouster_ros drive currently only uses single precision when it - // produces the point cloud. So it isn't of a benefit to compute point - // cloud xyz coordinates using double precision (for the time being). - auto xyz_lut = ouster::make_xyz_lut(info); - lut_direction = xyz_lut.direction.cast(); - lut_offset = xyz_lut.offset.cast(); - points = ouster::PointsF(lut_direction.rows(), lut_offset.cols()); - pc_ptr = boost::make_shared(); - - uint32_t H = info.format.pixels_per_column; - uint32_t W = info.format.columns_per_frame; - ls = ouster::LidarScan{W, H, info.format.udp_profile_lidar}; - cloud = ouster_ros::Cloud{W, H}; - - scan_batcher = std::make_unique(info); - } - - void create_publishers(ros::NodeHandle& nh) { - imu_pub = nh.advertise("imu", 100); + auto info = sensor::parse_metadata(metadata_msg->data); - auto img_suffix = [](int ind) { - if (ind == 0) return std::string(); - return std::to_string(ind + 1); // need second return to return 2 - }; + tf_bcast.parse_parameters(getPrivateNodeHandle()); - lidar_pubs.resize(n_returns); - for (int i = 0; i < n_returns; i++) { - auto pub = nh.advertise( - std::string("points") + img_suffix(i), 10); - lidar_pubs[i] = pub; + auto dynamic_transforms = + getPrivateNodeHandle().param("dynamic_transforms_broadcast", false); + auto dynamic_transforms_rate = getPrivateNodeHandle().param( + "dynamic_transforms_broadcast_rate", 1.0); + if (dynamic_transforms && dynamic_transforms_rate < 1.0) { + NODELET_WARN( + "OusterCloud: dynamic transforms enabled but wrong rate is " + "set, clamping to 1 Hz!"); + dynamic_transforms_rate = 1.0; } - } - void create_subscribers(ros::NodeHandle& nh) { - auto lidar_handler = use_ros_time - ? &OusterCloud::lidar_handler_ros_time - : &OusterCloud::lidar_handler_sensor_time; - - lidar_packet_sub = - nh.subscribe("lidar_packets", 2048, lidar_handler, this); - imu_packet_sub = nh.subscribe( - "imu_packets", 100, &OusterCloud::imu_handler, this); - } - - void pcl_toROSMsg(const ouster_ros::Cloud& pcl_cloud, - sensor_msgs::PointCloud2& cloud) { - // TODO: remove the staging step in the future - static pcl::PCLPointCloud2 pcl_pc2; - pcl::toPCLPointCloud2(pcl_cloud, pcl_pc2); - pcl_conversions::moveFromPCL(pcl_pc2, cloud); - } - - void convert_scan_to_pointcloud_publish(uint64_t scan_ts, - const ros::Time& msg_ts) { - for (int i = 0; i < n_returns; ++i) { - scan_to_cloud_f(points, lut_direction, lut_offset, scan_ts, ls, - cloud, i); - pcl_toROSMsg(cloud, *pc_ptr); - pc_ptr->header.stamp = msg_ts; - pc_ptr->header.frame_id = sensor_frame; - lidar_pubs[i].publish(pc_ptr); + if (!dynamic_transforms) { + NODELET_INFO("OusterCloud: using static transforms broadcast"); + tf_bcast.broadcast_transforms(info); + } else { + NODELET_INFO_STREAM( + "OusterCloud: dynamic transforms broadcast enabled wit " + "broadcast rate of: " + << dynamic_transforms_rate << " Hz"); + timer_ = getNodeHandle().createTimer( + ros::Duration(1.0 / dynamic_transforms_rate), + [this, info](const ros::TimerEvent&) { + tf_bcast.broadcast_transforms(info, last_msg_ts); + }); } - tf_bcast.sendTransform(ouster_ros::transform_to_tf_msg( - info.lidar_to_sensor_transform, sensor_frame, lidar_frame, msg_ts)); - - tf_bcast.sendTransform(ouster_ros::transform_to_tf_msg( - info.imu_to_sensor_transform, sensor_frame, imu_frame, msg_ts)); + create_publishers_subscribers(info); } - uint64_t impute_value(int last_scan_last_nonzero_idx, - uint64_t last_scan_last_nonzero_value, - int curr_scan_first_nonzero_idx, - uint64_t curr_scan_first_nonzero_value, - int scan_width) { - assert(scan_width + curr_scan_first_nonzero_idx > - last_scan_last_nonzero_idx); - double interpolated_value = linear_interpolate( - last_scan_last_nonzero_idx, last_scan_last_nonzero_value, - scan_width + curr_scan_first_nonzero_idx, - curr_scan_first_nonzero_value, scan_width); - return ulround(interpolated_value); - } - - uint64_t extrapolate_value(int curr_scan_first_nonzero_idx, - uint64_t curr_scan_first_nonzero_value) { - double extrapolated_value = - curr_scan_first_nonzero_value - - scan_col_ts_spacing_ns * curr_scan_first_nonzero_idx; - return ulround(extrapolated_value); - } - - // compute_scan_ts_0 for first scan - uint64_t compute_scan_ts_0( - const ouster::LidarScan::Header& ts_v) { - auto idx = std::find_if(ts_v.data(), ts_v.data() + ts_v.size(), - [](uint64_t h) { return h != 0; }); - assert(idx != ts_v.data() + ts_v.size()); // should never happen - int curr_scan_first_nonzero_idx = idx - ts_v.data(); - uint64_t curr_scan_first_nonzero_value = *idx; - - uint64_t scan_ns = - curr_scan_first_nonzero_idx == 0 - ? curr_scan_first_nonzero_value - : extrapolate_value(curr_scan_first_nonzero_idx, - curr_scan_first_nonzero_value); - - last_scan_last_nonzero_idx = - find_if_reverse(ts_v, [](uint64_t h) { return h != 0; }); - assert(last_scan_last_nonzero_idx >= 0); // should never happen - last_scan_last_nonzero_value = ts_v(last_scan_last_nonzero_idx); - compute_scan_ts = [this](const auto& ts_v) { - return compute_scan_ts_n(ts_v); - }; - return scan_ns; - } + void create_publishers_subscribers(const sensor::sensor_info& info) { + auto& pnh = getPrivateNodeHandle(); + auto proc_mask = pnh.param("proc_mask", std::string{"IMU|PCL|SCAN"}); + auto tokens = parse_tokens(proc_mask, '|'); - // compute_scan_ts_n applied to all subsequent scans except first one - uint64_t compute_scan_ts_n( - const ouster::LidarScan::Header& ts_v) { - auto idx = std::find_if(ts_v.data(), ts_v.data() + ts_v.size(), - [](uint64_t h) { return h != 0; }); - assert(idx != ts_v.data() + ts_v.size()); // should never happen - int curr_scan_first_nonzero_idx = idx - ts_v.data(); - uint64_t curr_scan_first_nonzero_value = *idx; - - uint64_t scan_ns = curr_scan_first_nonzero_idx == 0 - ? curr_scan_first_nonzero_value - : impute_value(last_scan_last_nonzero_idx, - last_scan_last_nonzero_value, - curr_scan_first_nonzero_idx, - curr_scan_first_nonzero_value, - static_cast(ts_v.size())); - - last_scan_last_nonzero_idx = - find_if_reverse(ts_v, [](uint64_t h) { return h != 0; }); - assert(last_scan_last_nonzero_idx >= 0); // should never happen - last_scan_last_nonzero_value = ts_v(last_scan_last_nonzero_idx); - return scan_ns; - } + auto timestamp_mode_arg = pnh.param("timestamp_mode", std::string{}); + bool use_ros_time = timestamp_mode_arg == "TIME_FROM_ROS_TIME"; - void lidar_handler_sensor_time(const PacketMsg::ConstPtr& packet) { - if (!(*scan_batcher)(packet->buf.data(), ls)) return; - auto scan_ts = compute_scan_ts(ls.timestamp()); - convert_scan_to_pointcloud_publish(scan_ts, to_ros_time(scan_ts)); - } + auto& nh = getNodeHandle(); - uint16_t packet_col_index(const uint8_t* packet_buf) { - const auto& pf = sensor::get_format(info); - return pf.col_measurement_id(pf.nth_col(0, packet_buf)); - } + if (check_token(tokens, "IMU")) { + imu_pub = nh.advertise("imu", 100); + imu_packet_handler = ImuPacketHandler::create_handler( + info, tf_bcast.imu_frame_id(), use_ros_time); + imu_packet_sub = nh.subscribe( + "imu_packets", 100, [this](const PacketMsg::ConstPtr msg) { + auto imu_msg = imu_packet_handler(msg->buf.data()); + if (imu_msg.header.stamp > last_msg_ts) + last_msg_ts = imu_msg.header.stamp; + imu_pub.publish(imu_msg); + }); + } - ros::Time extrapolate_frame_ts(const uint8_t* lidar_buf, - const ros::Time current_time) { - auto curr_scan_first_arrived_idx = packet_col_index(lidar_buf); - auto delta_time = ros::Duration( - 0, - std::lround(scan_col_ts_spacing_ns * curr_scan_first_arrived_idx)); - return current_time - delta_time; - } + int num_returns = get_n_returns(info); + + std::vector processors; + if (check_token(tokens, "PCL")) { + lidar_pubs.resize(num_returns); + for (int i = 0; i < num_returns; ++i) { + lidar_pubs[i] = nh.advertise( + topic_for_return("points", i), 10); + } + + processors.push_back(PointCloudProcessor::create( + 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) { + if (msgs[i]->header.stamp > last_msg_ts) + last_msg_ts = msgs[i]->header.stamp; + lidar_pubs[i].publish(*msgs[i]); + } + })); + } - void lidar_handler_ros_time(const PacketMsg::ConstPtr& packet) { - auto packet_receive_time = ros::Time::now(); - const uint8_t* packet_buf = packet->buf.data(); - static auto frame_ts = extrapolate_frame_ts( - packet_buf, packet_receive_time); // first point cloud time - if (!(*scan_batcher)(packet_buf, ls)) return; - auto scan_ts = compute_scan_ts(ls.timestamp()); - convert_scan_to_pointcloud_publish(scan_ts, frame_ts); - // set time for next point cloud msg - frame_ts = extrapolate_frame_ts(packet_buf, packet_receive_time); - } + if (check_token(tokens, "SCAN")) { + scan_pubs.resize(num_returns); + for (int i = 0; i < num_returns; ++i) { + scan_pubs[i] = nh.advertise( + topic_for_return("scan", i), 10); + } + + // TODO: avoid duplication in os_cloud_node + int beams_count = static_cast(get_beams_count(info)); + int scan_ring = pnh.param("scan_ring", 0); + scan_ring = std::min(std::max(scan_ring, 0), beams_count - 1); + if (scan_ring != pnh.param("scan_ring", 0)) { + 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 + .point_cloud_frame_id(), // TODO: should we allow having a + // different frame for the laser + // scan than point cloud??? + scan_ring, [this](LaserScanProcessor::OutputType msgs) { + for (size_t i = 0; i < msgs.size(); ++i) { + if (msgs[i]->header.stamp > last_msg_ts) + last_msg_ts = msgs[i]->header.stamp; + scan_pubs[i].publish(*msgs[i]); + } + })); + } - void imu_handler(const PacketMsg::ConstPtr& packet) { - const auto& pf = sensor::get_format(info); - ros::Time msg_ts = - use_ros_time ? ros::Time::now() - : to_ros_time(pf.imu_gyro_ts(packet->buf.data())); - sensor_msgs::Imu imu_msg = - ouster_ros::packet_to_imu_msg(*packet, msg_ts, imu_frame, pf); - sensor_msgs::ImuPtr imu_msg_ptr = - boost::make_shared(imu_msg); - imu_pub.publish(imu_msg_ptr); - }; - - inline ros::Time to_ros_time(uint64_t ts) { - ros::Time t; - t.fromNSec(ts); - return t; + if (check_token(tokens, "PCL") || check_token(tokens, "SCAN")) { + lidar_packet_handler = LidarPacketHandler::create_handler( + info, use_ros_time, processors); + lidar_packet_sub = nh.subscribe( + "lidar_packets", 100, [this](const PacketMsg::ConstPtr msg) { + lidar_packet_handler(msg->buf.data()); + }); + } } private: ros::Subscriber metadata_sub; - ros::Subscriber lidar_packet_sub; - std::vector lidar_pubs; ros::Subscriber imu_packet_sub; ros::Publisher imu_pub; - sensor_msgs::PointCloud2::Ptr pc_ptr; - - sensor::sensor_info info; - int n_returns = 0; - - ouster::PointsF lut_direction; - ouster::PointsF lut_offset; - ouster::PointsF points; - ouster::LidarScan ls; - ouster_ros::Cloud cloud; - std::unique_ptr scan_batcher; - - std::string sensor_frame; - std::string imu_frame; - std::string lidar_frame; + ros::Subscriber lidar_packet_sub; + std::vector lidar_pubs; + std::vector scan_pubs; - tf2_ros::TransformBroadcaster tf_bcast; + OusterTransformsBroadcaster tf_bcast; - bool use_ros_time; + ImuPacketHandler::HandlerType imu_packet_handler; + LidarPacketHandler::HandlerType lidar_packet_handler; - int last_scan_last_nonzero_idx = -1; - uint64_t last_scan_last_nonzero_value = 0; - std::function&)> - compute_scan_ts; - double scan_col_ts_spacing_ns; // interval or spacing between columns of a - // scan + ros::Timer timer_; + ros::Time last_msg_ts; }; -} // namespace nodelets_os +} // namespace ouster_ros -PLUGINLIB_EXPORT_CLASS(nodelets_os::OusterCloud, nodelet::Nodelet) +PLUGINLIB_EXPORT_CLASS(ouster_ros::OusterCloud, nodelet::Nodelet) diff --git a/src/os_driver_nodelet.cpp b/src/os_driver_nodelet.cpp new file mode 100644 index 00000000..65d15074 --- /dev/null +++ b/src/os_driver_nodelet.cpp @@ -0,0 +1,180 @@ +/** + * Copyright (c) 2018-2023, Ouster, Inc. + * All rights reserved. + * + * @file os_driver_nodelet.cpp + * @brief This node combines the capabilities of os_sensor, os_cloud and os_img + * into a single ROS nodelet + */ + +// 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 +#include +#include + +#include "os_sensor_nodelet.h" +#include "os_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" + +namespace sensor = ouster::sensor; + +namespace ouster_ros { + +class OusterDriver : public OusterSensor { + public: + OusterDriver() : tf_bcast(getName()) {} + + private: + // virtual void onInit() override { + // } + + virtual void on_metadata_updated(const sensor::sensor_info& info) override { + OusterSensor::on_metadata_updated(info); + + // for OusterDriver we are going to always assume static broadcast + // at least for now + tf_bcast.parse_parameters(getPrivateNodeHandle()); + tf_bcast.broadcast_transforms(info); + } + + virtual void create_publishers() override { + auto& pnh = getPrivateNodeHandle(); + auto proc_mask = + pnh.param("proc_mask", std::string{"IMU|IMG|PCL|SCAN"}); + auto tokens = parse_tokens(proc_mask, '|'); + + auto timestamp_mode_arg = pnh.param("timestamp_mode", std::string{}); + bool use_ros_time = timestamp_mode_arg == "TIME_FROM_ROS_TIME"; + + auto& nh = getNodeHandle(); + + if (check_token(tokens, "IMU")) { + imu_pub = nh.advertise("imu", 100); + imu_packet_handler = ImuPacketHandler::create_handler( + info, tf_bcast.imu_frame_id(), use_ros_time); + } + + int num_returns = get_n_returns(info); + + std::vector processors; + if (check_token(tokens, "PCL")) { + lidar_pubs.resize(num_returns); + for (int i = 0; i < num_returns; ++i) { + lidar_pubs[i] = nh.advertise( + topic_for_return("points", i), 10); + } + + processors.push_back(PointCloudProcessor::create( + 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]); + } + })); + } + + if (check_token(tokens, "SCAN")) { + scan_pubs.resize(num_returns); + for (int i = 0; i < num_returns; ++i) { + scan_pubs[i] = nh.advertise( + topic_for_return("scan", i), 10); + } + + // TODO: avoid duplication in os_cloud_node + int beams_count = static_cast(get_beams_count(info)); + int scan_ring = pnh.param("scan_ring", 0); + scan_ring = std::min(std::max(scan_ring, 0), beams_count - 1); + if (scan_ring != pnh.param("scan_ring", 0)) { + 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 + .point_cloud_frame_id(), // TODO: should we allow having a + // different frame for the laser + // scan than point cloud??? + scan_ring, [this](LaserScanProcessor::OutputType msgs) { + for (size_t i = 0; i < msgs.size(); ++i) { + scan_pubs[i].publish(*msgs[i]); + } + })); + } + + if (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] = + nh.advertise(it->second, 10); + } + + 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 (check_token(tokens, "PCL") || check_token(tokens, "SCAN") || + check_token(tokens, "IMG")) + lidar_packet_handler = LidarPacketHandler::create_handler( + info, use_ros_time, processors); + } + + 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)); + } + + private: + ros::Publisher imu_pub; + std::vector lidar_pubs; + std::vector scan_pubs; + std::map image_pubs; + + OusterTransformsBroadcaster tf_bcast; + + ImuPacketHandler::HandlerType imu_packet_handler; + LidarPacketHandler::HandlerType lidar_packet_handler; +}; + +} // namespace ouster_ros + +PLUGINLIB_EXPORT_CLASS(ouster_ros::OusterDriver, nodelet::Nodelet) diff --git a/src/os_image_nodelet.cpp b/src/os_image_nodelet.cpp index 14cd826c..4b5bd7a3 100644 --- a/src/os_image_nodelet.cpp +++ b/src/os_image_nodelet.cpp @@ -11,7 +11,7 @@ * vision applications, use higher bit depth values in /os_cloud_node/points */ -// prevent clang-format from altering the location of "ouster_ros/ros.h", the +// 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" @@ -21,7 +21,6 @@ #include #include #include -#include #include #include #include @@ -37,17 +36,17 @@ namespace sensor = ouster::sensor; namespace viz = ouster::viz; -using sensor::UDPProfileLidar; using pixel_type = uint16_t; const size_t pixel_value_max = std::numeric_limits::max(); -namespace nodelets_os { +namespace ouster_ros { + class OusterImage : public nodelet::Nodelet { private: virtual void onInit() override { create_metadata_subscriber(getNodeHandle()); - NODELET_INFO("OusterImage: nodelet created!"); + NODELET_INFO("OusterImage: node initialized!"); } void create_metadata_subscriber(ros::NodeHandle& nh) { @@ -56,14 +55,12 @@ class OusterImage : public nodelet::Nodelet { } void metadata_handler(const std_msgs::String::ConstPtr& metadata_msg) { - // TODO: handle sensor reconfigurtion NODELET_INFO("OusterImage: retrieved new sensor metadata!"); - auto& nh = getNodeHandle(); - auto metadata = metadata_msg->data; - info = sensor::parse_metadata(metadata); - auto n_returns = compute_n_returns(); - create_publishers(nh, n_returns); - create_subscribers(nh, n_returns); + info = sensor::parse_metadata(metadata_msg->data); + create_cloud_object(); + const int n_returns = get_n_returns(info); + create_publishers(n_returns); + create_subscriptions(n_returns); } void create_cloud_object() { @@ -72,43 +69,33 @@ class OusterImage : public nodelet::Nodelet { cloud = ouster_ros::Cloud{W, H}; } - int compute_n_returns() { - return info.format.udp_profile_lidar == - UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL - ? 2 - : 1; - } - - std::string topic(std::string base, int idx) { - if (idx == 0) return base; - return base + std::to_string(idx + 1); - } - - void create_publishers(ros::NodeHandle& nh, int n_returns) { + void create_publishers(int n_returns) { + auto& nh = getNodeHandle(); nearir_image_pub = nh.advertise("nearir_image", 100); ros::Publisher a_pub; - for (int i = 0; i < n_returns; i++) { - a_pub = - nh.advertise(topic("range_image", i), 100); + for (int i = 0; i < n_returns; ++i) { + a_pub = nh.advertise( + topic_for_return("range_image", i), 100); range_image_pubs.push_back(a_pub); - a_pub = - nh.advertise(topic("signal_image", i), 100); + a_pub = nh.advertise( + topic_for_return("signal_image", i), 100); signal_image_pubs.push_back(a_pub); - a_pub = - nh.advertise(topic("reflec_image", i), 100); + a_pub = nh.advertise( + topic_for_return("reflec_image", i), 100); reflec_image_pubs.push_back(a_pub); } } - void create_subscribers(ros::NodeHandle& nh, int n_returns) { + void create_subscriptions(int n_returns) { + auto& nh = getNodeHandle(); pc_subs.resize(n_returns); for (int i = 0; i < n_returns; ++i) { pc_subs[i] = nh.subscribe( - topic("points", i), 100, + topic_for_return("points", i), 100, [this, i](const sensor_msgs::PointCloud2::ConstPtr msg) { point_cloud_handler(msg, i); }); @@ -218,6 +205,7 @@ class OusterImage : public nodelet::Nodelet { viz::AutoExposure nearir_ae, signal_ae, reflec_ae; viz::BeamUniformityCorrector nearir_buc; }; -} // namespace nodelets_os -PLUGINLIB_EXPORT_CLASS(nodelets_os::OusterImage, nodelet::Nodelet) +} // namespace ouster_ros + +PLUGINLIB_EXPORT_CLASS(ouster_ros::OusterImage, nodelet::Nodelet) diff --git a/src/os_replay_nodelet.cpp b/src/os_replay_nodelet.cpp index f3c2efca..3cb2e9a3 100644 --- a/src/os_replay_nodelet.cpp +++ b/src/os_replay_nodelet.cpp @@ -9,27 +9,26 @@ #include -#include - -#include "ouster_ros/os_client_base_nodelet.h" +#include "ouster_ros/os_sensor_nodelet_base.h" namespace sensor = ouster::sensor; -namespace nodelets_os { +namespace ouster_ros { -class OusterReplay : public OusterClientBase { +class OusterReplay : public OusterSensorNodeletBase { private: virtual void onInit() override { - NODELET_INFO("Running in replay mode"); auto meta_file = get_meta_file(); - create_metadata_publisher(getNodeHandle()); + create_metadata_publisher(); load_metadata_from_file(meta_file); publish_metadata(); + create_get_metadata_service(); + NODELET_INFO("Running in replay mode"); } std::string get_meta_file() const { - auto& pnh = getPrivateNodeHandle(); - auto meta_file = pnh.param("metadata", std::string{}); + auto meta_file = + getPrivateNodeHandle().param("metadata", std::string{}); if (!is_arg_set(meta_file)) { NODELET_ERROR("Must specify metadata file in replay mode"); throw std::runtime_error("metadata no specificed"); @@ -37,12 +36,9 @@ class OusterReplay : public OusterClientBase { return meta_file; } - void load_metadata_from_file(const std::string meta_file) { + void load_metadata_from_file(const std::string& meta_file) { try { - std::ifstream in_file(meta_file); - std::stringstream buffer; - buffer << in_file.rdbuf(); - cached_metadata = buffer.str(); + cached_metadata = read_text_file(meta_file); info = sensor::parse_metadata(cached_metadata); display_lidar_info(info); } catch (const std::runtime_error& e) { @@ -53,6 +49,6 @@ class OusterReplay : public OusterClientBase { } }; -} // namespace nodelets_os +} // namespace ouster_ros -PLUGINLIB_EXPORT_CLASS(nodelets_os::OusterReplay, nodelet::Nodelet) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(ouster_ros::OusterReplay, nodelet::Nodelet) \ No newline at end of file diff --git a/src/os_ros.cpp b/src/os_ros.cpp index 81f99794..1a8d394e 100644 --- a/src/os_ros.cpp +++ b/src/os_ros.cpp @@ -13,8 +13,8 @@ // clang-format on #include -#include #include + #include #include @@ -25,34 +25,42 @@ namespace sensor = ouster::sensor; namespace ouster_ros { -bool read_imu_packet(const sensor::client& cli, PacketMsg& pm, - const sensor::packet_format& pf) { - pm.buf.resize(pf.imu_packet_size + 1); - return read_imu_packet(cli, pm.buf.data(), pf); +bool is_legacy_lidar_profile(const sensor::sensor_info& info) { + using sensor::UDPProfileLidar; + return info.format.udp_profile_lidar == + UDPProfileLidar::PROFILE_LIDAR_LEGACY; } -bool read_lidar_packet(const sensor::client& cli, PacketMsg& pm, - const sensor::packet_format& pf) { - pm.buf.resize(pf.lidar_packet_size + 1); - return read_lidar_packet(cli, pm.buf.data(), pf); +int get_n_returns(const sensor::sensor_info& info) { + using sensor::UDPProfileLidar; + return info.format.udp_profile_lidar == + UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL + ? 2 + : 1; } -sensor_msgs::Imu packet_to_imu_msg(const PacketMsg& pm, +size_t get_beams_count(const sensor::sensor_info& info) { + return info.beam_azimuth_angles.size(); +} + +std::string topic_for_return(const std::string& base, int idx) { + return idx == 0 ? base : base + std::to_string(idx + 1); +} + +sensor_msgs::Imu packet_to_imu_msg(const ouster::sensor::packet_format& pf, const ros::Time& timestamp, const std::string& frame, - const sensor::packet_format& pf) { - const double standard_g = 9.80665; + const uint8_t* buf) { sensor_msgs::Imu m; - const uint8_t* buf = pm.buf.data(); - m.header.stamp = timestamp; m.header.frame_id = frame; m.orientation.x = 0; m.orientation.y = 0; m.orientation.z = 0; - m.orientation.w = 0; + m.orientation.w = 1; + const double standard_g = 9.80665; m.linear_acceleration.x = pf.imu_la_x(buf) * standard_g; m.linear_acceleration.y = pf.imu_la_y(buf) * standard_g; m.linear_acceleration.z = pf.imu_la_z(buf) * standard_g; @@ -66,6 +74,7 @@ sensor_msgs::Imu packet_to_imu_msg(const PacketMsg& pm, m.angular_velocity_covariance[i] = 0; m.linear_acceleration_covariance[i] = 0; } + for (int i = 0; i < 9; i += 4) { m.linear_acceleration_covariance[i] = 0.01; m.angular_velocity_covariance[i] = 6e-4; @@ -75,21 +84,13 @@ sensor_msgs::Imu packet_to_imu_msg(const PacketMsg& pm, } sensor_msgs::Imu packet_to_imu_msg(const PacketMsg& pm, + const ros::Time& timestamp, const std::string& frame, const sensor::packet_format& pf) { - ros::Time timestamp; - timestamp.fromNSec(pf.imu_gyro_ts(pm.buf.data())); - return packet_to_imu_msg(pm, timestamp, frame, pf); + return packet_to_imu_msg(pf, timestamp, frame, pm.buf.data()); } -struct read_and_cast { - template - void operator()(Eigen::Ref> field, - ouster::img_t& dest) { - dest = field.template cast(); - } -}; - +namespace impl { sensor::ChanField suitable_return(sensor::ChanField input_field, bool second) { switch (input_field) { case sensor::ChanField::RANGE: @@ -111,57 +112,13 @@ sensor::ChanField suitable_return(sensor::ChanField input_field, bool second) { } } -template -inline ouster::img_t get_or_fill_zero(sensor::ChanField f, - const ouster::LidarScan& ls) { - if (!ls.field_type(f)) { - return ouster::img_t::Zero(ls.h, ls.w); - } - - ouster::img_t result{ls.h, ls.w}; - ouster::impl::visit_field(ls, f, read_and_cast(), result); - return result; +ros::Time ts_to_ros_time(uint64_t ts) { + ros::Time t; + t.fromNSec(ts); + return t; } -void scan_to_cloud(const ouster::XYZLut& xyz_lut, - std::chrono::nanoseconds scan_ts, - const ouster::LidarScan& ls, ouster_ros::Cloud& cloud, - int return_index) { - bool second = (return_index == 1); - cloud.resize(ls.w * ls.h); - - ouster::img_t near_ir = get_or_fill_zero( - suitable_return(sensor::ChanField::NEAR_IR, second), ls); - - ouster::img_t range = get_or_fill_zero( - suitable_return(sensor::ChanField::RANGE, second), ls); - - ouster::img_t signal = get_or_fill_zero( - suitable_return(sensor::ChanField::SIGNAL, second), ls); - - ouster::img_t reflectivity = get_or_fill_zero( - suitable_return(sensor::ChanField::REFLECTIVITY, second), ls); - - auto points = ouster::cartesian(range, xyz_lut); - auto timestamp = ls.timestamp(); - - for (auto u = 0; u < ls.h; u++) { - for (auto v = 0; v < ls.w; v++) { - const auto xyz = points.row(u * ls.w + v); - const auto ts = - (std::chrono::nanoseconds(timestamp[v]) - scan_ts).count(); - cloud(v, u) = ouster_ros::Point{ - {{static_cast(xyz(0)), static_cast(xyz(1)), - static_cast(xyz(2)), 1.0f}}, - static_cast(signal(u, v)), - static_cast(ts), - static_cast(reflectivity(u, v)), - static_cast(u), - static_cast(near_ir(u, v)), - static_cast(range(u, v))}; - } - } -} +} // namespace impl template @@ -256,14 +213,14 @@ void scan_to_cloud_f(ouster::PointsF& points, second ? sensor::ChanField::RANGE2 : sensor::ChanField::RANGE; ouster::img_t range = ls.field(range_channel_field); - ouster::img_t reflectivity = get_or_fill_zero( - suitable_return(sensor::ChanField::REFLECTIVITY, second), ls); + ouster::img_t reflectivity = impl::get_or_fill_zero( + impl::suitable_return(sensor::ChanField::REFLECTIVITY, second), ls); - ouster::img_t signal = get_or_fill_zero( - suitable_return(sensor::ChanField::SIGNAL, second), ls); + ouster::img_t signal = impl::get_or_fill_zero( + impl::suitable_return(sensor::ChanField::SIGNAL, second), ls); - ouster::img_t near_ir = get_or_fill_zero( - suitable_return(sensor::ChanField::NEAR_IR, second), ls); + ouster::img_t near_ir = impl::get_or_fill_zero( + impl::suitable_return(sensor::ChanField::NEAR_IR, second), ls); ouster::cartesianT(points, range, lut_direction, lut_offset); @@ -287,14 +244,14 @@ void scan_to_cloud_f(ouster::PointsF& points, second ? sensor::ChanField::RANGE2 : sensor::ChanField::RANGE; ouster::img_t range = ls.field(range_channel_field); - ouster::img_t reflectivity = get_or_fill_zero( - suitable_return(sensor::ChanField::REFLECTIVITY, second), ls); + ouster::img_t reflectivity = impl::get_or_fill_zero( + impl::suitable_return(sensor::ChanField::REFLECTIVITY, second), ls); - ouster::img_t signal = get_or_fill_zero( - suitable_return(sensor::ChanField::SIGNAL, second), ls); + ouster::img_t signal = impl::get_or_fill_zero( + impl::suitable_return(sensor::ChanField::SIGNAL, second), ls); - ouster::img_t near_ir = get_or_fill_zero( - suitable_return(sensor::ChanField::NEAR_IR, second), ls); + ouster::img_t near_ir = impl::get_or_fill_zero( + impl::suitable_return(sensor::ChanField::NEAR_IR, second), ls); ouster::cartesianT(points, range, lut_direction, lut_offset); @@ -312,13 +269,6 @@ sensor_msgs::PointCloud2 cloud_to_cloud_msg(const Cloud& cloud, return msg; } -sensor_msgs::PointCloud2 cloud_to_cloud_msg(const Cloud& cloud, ns ts, - const std::string& frame) { - ros::Time timestamp; - timestamp.fromNSec(ts.count()); - return cloud_to_cloud_msg(cloud, timestamp, frame); -} - geometry_msgs::TransformStamped transform_to_tf_msg( const ouster::mat4d& mat, const std::string& frame, const std::string& child_frame, ros::Time timestamp) { @@ -333,4 +283,44 @@ geometry_msgs::TransformStamped transform_to_tf_msg( return msg; } + +// TODO: provide a method that accepts sensor_msgs::LaserScan object +sensor_msgs::LaserScan lidar_scan_to_laser_scan_msg( + const ouster::LidarScan& ls, const ros::Time& timestamp, + const std::string& frame, const ouster::sensor::lidar_mode ld_mode, + const uint16_t ring, const int return_index) { + sensor_msgs::LaserScan msg; + msg.header.stamp = timestamp; + msg.header.frame_id = frame; + msg.angle_min = -M_PI; // TODO: configure to match the actual scan window + msg.angle_max = M_PI; // TODO: configure to match the actual scan window + msg.range_min = 0.1f; // TODO: fill per product type + msg.range_max = 120.0f; // TODO: fill per product type + + const auto scan_width = sensor::n_cols_of_lidar_mode(ld_mode); + const auto scan_frequency = sensor::frequency_of_lidar_mode(ld_mode); + msg.scan_time = 1.0f / scan_frequency; + msg.time_increment = 1.0f / (scan_width * scan_frequency); + msg.angle_increment = 2 * M_PI / scan_width; + + auto which_range = return_index == 0 ? sensor::ChanField::RANGE + : sensor::ChanField::RANGE2; + ouster::img_t range = ls.field(which_range); + auto which_signal = return_index == 0 ? sensor::ChanField::SIGNAL + : sensor::ChanField::SIGNAL2; + ouster::img_t signal = + impl::get_or_fill_zero(which_signal, ls); + const auto rg = range.data(); + const auto sg = signal.data(); + msg.ranges.resize(ls.w); + msg.intensities.resize(ls.w); + int idx = ls.w * ring + ls.w; + for (int i = 0; idx-- > ls.w * ring; ++i) { + msg.ranges[i] = rg[idx] * ouster::sensor::range_unit; + msg.intensities[i] = static_cast(sg[idx]); + } + + return msg; +} + } // namespace ouster_ros diff --git a/src/os_sensor_nodelet.cpp b/src/os_sensor_nodelet.cpp index efb89d5f..b7233bc9 100644 --- a/src/os_sensor_nodelet.cpp +++ b/src/os_sensor_nodelet.cpp @@ -13,455 +13,608 @@ // clang-format on #include +#include -#include -#include #include +#include -#include "ouster_ros/GetConfig.h" #include "ouster_ros/PacketMsg.h" -#include "ouster_ros/SetConfig.h" -#include "ouster_ros/os_client_base_nodelet.h" +#include "os_sensor_nodelet.h" namespace sensor = ouster::sensor; using nonstd::optional; -using ouster_ros::GetConfig; -using ouster_ros::PacketMsg; -using ouster_ros::SetConfig; - -namespace nodelets_os { - -class OusterSensor : public OusterClientBase { - private: - virtual void onInit() override { - auto& pnh = getPrivateNodeHandle(); - sensor_hostname = get_sensor_hostname(pnh); - sensor::sensor_config config; - uint8_t flags; - std::tie(config, flags) = create_sensor_config_rosparams(pnh); - configure_sensor(sensor_hostname, config, flags); - sensor_client = create_sensor_client(sensor_hostname, config); - auto& nh = getNodeHandle(); - create_metadata_publisher(nh); - update_config_and_metadata(*sensor_client); - publish_metadata(); - save_metadata(pnh); - create_get_metadata_service(nh); - create_get_config_service(nh); - create_set_config_service(nh); - start_connection_loop(nh); - } - - std::string get_sensor_hostname(ros::NodeHandle& nh) { - auto hostname = nh.param("sensor_hostname", std::string{}); - if (!is_arg_set(hostname)) { - auto error_msg = "Must specify a sensor hostname"; - NODELET_ERROR_STREAM(error_msg); - throw std::runtime_error(error_msg); - } - return hostname; - } +using namespace std::chrono_literals; +using namespace std::string_literals; - bool update_config_and_metadata(sensor::client& cli) { - sensor::sensor_config config; - auto success = get_config(sensor_hostname, config); - if (!success) { - NODELET_ERROR("Failed to collect sensor config"); - cached_config.clear(); - cached_metadata.clear(); - return false; - } +namespace ouster_ros { - cached_config = to_string(config); +void OusterSensor::onInit() { + sensor_hostname = get_sensor_hostname(); + sensor::sensor_config config = parse_config_from_ros_parameters(); + configure_sensor(sensor_hostname, config); + sensor_client = create_sensor_client(sensor_hostname, config); + if (!sensor_client) { + auto error_msg = "Failed to initialize client"; + NODELET_ERROR_STREAM(error_msg); + throw std::runtime_error(error_msg); + } - try { - cached_metadata = sensor::get_metadata(cli); - } catch (const std::exception& e) { - NODELET_ERROR_STREAM( - "sensor::get_metadata exception: " << e.what()); - cached_metadata.clear(); - } + create_metadata_publisher(); + update_config_and_metadata(*sensor_client); + create_services(); + + // activate + create_publishers(); + allocate_buffers(); + start_packet_processing_threads(); + start_sensor_connection_thread(); +} + +std::string OusterSensor::get_sensor_hostname() { + auto& nh = getPrivateNodeHandle(); + auto hostname = nh.param("sensor_hostname", std::string{}); + if (!is_arg_set(hostname)) { + auto error_msg = "Must specify a sensor hostname"; + NODELET_ERROR_STREAM(error_msg); + throw std::runtime_error(error_msg); + } - if (cached_metadata.empty()) { - NODELET_ERROR("Failed to collect sensor metadata"); - return false; - } + return hostname; +} + +void OusterSensor::update_config_and_metadata(sensor::client& cli) { + sensor::sensor_config config; + auto success = get_config(sensor_hostname, config); + if (!success) { + active_config.clear(); + cached_metadata.clear(); + auto error_msg = std::string{"Failed to collect sensor config"}; + NODELET_ERROR_STREAM(error_msg); + throw std::runtime_error(error_msg); + } - info = sensor::parse_metadata(cached_metadata); - // TODO: revist when *min_version* is changed - populate_metadata_defaults(info, sensor::MODE_UNSPEC); - display_lidar_info(info); + active_config = sensor::to_string(config); - return cached_config.size() > 0 && cached_metadata.size() > 0; + try { + cached_metadata = sensor::get_metadata(cli, 60, false); + } catch (const std::exception& e) { + NODELET_ERROR_STREAM("sensor::get_metadata exception: " << e.what()); + cached_metadata.clear(); } - void save_metadata(ros::NodeHandle& nh) { - auto meta_file = nh.param("metadata", std::string{}); - if (!is_arg_set(meta_file)) { - meta_file = sensor_hostname.substr(0, sensor_hostname.rfind('.')) + - "-metadata.json"; - NODELET_INFO_STREAM( - "No metadata file was specified, using: " << meta_file); - } + if (cached_metadata.empty()) { + auto error_msg = std::string{"Failed to collect sensor metadata"}; + NODELET_ERROR_STREAM(error_msg); + throw std::runtime_error(error_msg); + } - // write metadata file. If metadata_path is relative, will use cwd - // (usually ~/.ros) - if (!write_metadata(meta_file, cached_metadata)) { - NODELET_ERROR("Exiting because of failure to write metadata path"); - throw std::runtime_error("Failure to write metadata path"); - } + info = sensor::parse_metadata(cached_metadata); + // TODO: revist when *min_version* is changed + populate_metadata_defaults(info, sensor::MODE_UNSPEC); + + publish_metadata(); + save_metadata(); + on_metadata_updated(info); +} + +void OusterSensor::save_metadata() { + auto& nh = getPrivateNodeHandle(); + auto meta_file = nh.param("metadata", std::string{}); + if (!is_arg_set(meta_file)) { + meta_file = sensor_hostname.substr(0, sensor_hostname.rfind('.')) + + "-metadata.json"; + NODELET_INFO_STREAM( + "No metadata file was specified, using: " << meta_file); } - void create_get_config_service(ros::NodeHandle& nh) { - get_config_srv = - nh.advertiseService( + // write metadata file. If metadata_path is relative, will use cwd + // (usually ~/.ros) + if (write_text_to_file(meta_file, cached_metadata)) { + NODELET_INFO_STREAM("Wrote sensor metadata to " << meta_file); + } else { + NODELET_WARN_STREAM( + "Failed to write metadata to " << meta_file << + "; check that the path is valid. If you provided a relative " + "path, please note that the working directory of all ROS " + "nodes is set by default to $ROS_HOME"); + } +} + +void OusterSensor::create_reset_service() { + reset_srv = getNodeHandle() + .advertiseService( + "reset", [this](std_srvs::Empty::Request&, + std_srvs::Empty::Response&) { + NODELET_INFO("reset service invoked"); + // reset_sensor(true); + return true; + }); + + NODELET_INFO("reset service created"); +} + +void OusterSensor::create_get_config_service() { + get_config_srv = + getNodeHandle() + .advertiseService( "get_config", [this](GetConfig::Request&, GetConfig::Response& response) { - response.config = cached_config; - return cached_config.size() > 0; + response.config = active_config; + return active_config.size() > 0; }); - NODELET_INFO("get_config service created"); - } + NODELET_INFO("get_config service created"); +} - void create_set_config_service(ros::NodeHandle& nh) { - set_config_srv = - nh.advertiseService( +void OusterSensor::create_set_config_service() { + set_config_srv = + getNodeHandle() + .advertiseService( "set_config", [this](SetConfig::Request& request, SetConfig::Response& response) { - sensor::sensor_config config; response.config = ""; - auto success = - load_config_file(request.config_file, config); - if (!success) { - NODELET_ERROR_STREAM("Failed to load and parse file: " - << request.config_file); - return false; - } try { - configure_sensor(sensor_hostname, config, 0); + staged_config = read_text_file(request.config_file); + if (staged_config.empty()) { + NODELET_ERROR_STREAM( + "provided config file: " + << request.config_file + << " turned to be empty. set_config ignored!"); + return false; + } } catch (const std::exception& e) { + NODELET_ERROR_STREAM( + "exception thrown while loading config file: " + << request.config_file + << ", exception details: " << e.what()); return false; } - success = update_config_and_metadata(*sensor_client); - response.config = cached_config; - return success; + + response.config = staged_config; + // TODO: this is currently set to force_reinit but it + // doesn't need to be the case if it was possible to know + // that the new config would result in a reinit when a + // reinit is not forced + reset_sensor(true); + return true; }); - NODELET_INFO("set_config service created"); + NODELET_INFO("set_config service created"); +} + +std::shared_ptr OusterSensor::create_sensor_client( + const std::string& hostname, const sensor::sensor_config& config) { + NODELET_INFO_STREAM("Starting sensor " << hostname << " initialization..."); + + int lidar_port = config.udp_port_lidar ? config.udp_port_lidar.value() : 0; + int imu_port = config.udp_port_imu ? config.udp_port_imu.value() : 0; + auto udp_dest = config.udp_dest ? config.udp_dest.value() : ""; + + std::shared_ptr cli; + if (sensor::in_multicast(udp_dest)) { + // use the mtp_init_client to recieve data via multicast + // if mtp_main is true when sensor will be configured + cli = sensor::mtp_init_client(hostname, config, mtp_dest, mtp_main); + } else if (lidar_port != 0 && imu_port != 0) { + // use no-config version of init_client to bind to pre-configured + // ports + cli = sensor::init_client(hostname, lidar_port, imu_port); + } else { + // use the full init_client to generate and assign random ports to + // sensor + cli = + sensor::init_client(hostname, udp_dest, sensor::MODE_UNSPEC, + sensor::TIME_FROM_UNSPEC, lidar_port, imu_port); } - std::shared_ptr create_sensor_client( - const std::string& hostname, const sensor::sensor_config& config) { - NODELET_INFO_STREAM("Starting sensor " << hostname - << " initialization..."); - - int lidar_port = - config.udp_port_lidar ? config.udp_port_lidar.value() : 0; - int imu_port = config.udp_port_imu ? config.udp_port_imu.value() : 0; - auto udp_dest = config.udp_dest ? config.udp_dest.value() : ""; - - std::shared_ptr cli; - if (sensor::in_multicast(udp_dest)) { - // use the mtp_init_client to recieve data via multicast - // if mtp_main is true when sensor will be configured - cli = sensor::mtp_init_client(hostname, config, mtp_dest, mtp_main); - } else if (lidar_port != 0 && imu_port != 0) { - // use no-config version of init_client to bind to pre-configured - // ports - cli = sensor::init_client(hostname, lidar_port, imu_port); - } else { - // use the full init_client to generate and assign random ports to - // sensor - cli = sensor::init_client(hostname, udp_dest, sensor::MODE_UNSPEC, - sensor::TIME_FROM_UNSPEC, lidar_port, - imu_port); - } - - if (!cli) { - auto error_msg = "Failed to initialize client"; - NODELET_ERROR_STREAM(error_msg); - throw std::runtime_error(error_msg); - } - - return cli; + return cli; +} + +sensor::sensor_config OusterSensor::parse_config_from_ros_parameters() { + auto& nh = getPrivateNodeHandle(); + auto udp_dest = nh.param("udp_dest", std::string{}); + auto mtp_dest_arg = nh.param("mtp_dest", std::string{}); + auto mtp_main_arg = nh.param("mtp_main", false); + auto lidar_port = nh.param("lidar_port", 0); + auto imu_port = nh.param("imu_port", 0); + auto lidar_mode_arg = nh.param("lidar_mode", std::string{}); + auto timestamp_mode_arg = nh.param("timestamp_mode", std::string{}); + auto udp_profile_lidar_arg = nh.param("udp_profile_lidar", std::string{}); + + if (lidar_port < 0 || lidar_port > 65535) { + auto error_msg = + "Invalid lidar port number! port value should be in the range " + "[0, 65535]."; + NODELET_ERROR_STREAM(error_msg); + throw std::runtime_error(error_msg); } - std::pair create_sensor_config_rosparams( - ros::NodeHandle& nh) { - auto udp_dest = nh.param("udp_dest", std::string{}); - auto mtp_dest_arg = nh.param("mtp_dest", std::string{}); - auto mtp_main_arg = nh.param("mtp_main", false); - auto lidar_port = nh.param("lidar_port", 0); - auto imu_port = nh.param("imu_port", 0); - auto lidar_mode_arg = nh.param("lidar_mode", std::string{}); - auto timestamp_mode_arg = nh.param("timestamp_mode", std::string{}); - auto udp_profile_lidar_arg = - nh.param("udp_profile_lidar", std::string{}); + if (imu_port < 0 || imu_port > 65535) { + auto error_msg = + "Invalid imu port number! port value should be in the range " + "[0, 65535]."; + NODELET_ERROR_STREAM(error_msg); + throw std::runtime_error(error_msg); + } - if (lidar_port < 0 || lidar_port > 65535) { + nonstd::optional udp_profile_lidar; + if (is_arg_set(udp_profile_lidar_arg)) { + // set lidar profile from param + udp_profile_lidar = + sensor::udp_profile_lidar_of_string(udp_profile_lidar_arg); + if (!udp_profile_lidar) { auto error_msg = - "Invalid lidar port number! port value should be in the range " - "[0, 65535]."; + "Invalid udp profile lidar: " + udp_profile_lidar_arg; NODELET_ERROR_STREAM(error_msg); throw std::runtime_error(error_msg); } + } - if (imu_port < 0 || imu_port > 65535) { - auto error_msg = - "Invalid imu port number! port value should be in the range " - "[0, 65535]."; + // set lidar mode from param + sensor::lidar_mode lidar_mode = sensor::MODE_UNSPEC; + if (is_arg_set(lidar_mode_arg)) { + lidar_mode = sensor::lidar_mode_of_string(lidar_mode_arg); + if (!lidar_mode) { + auto error_msg = "Invalid lidar mode: " + lidar_mode_arg; NODELET_ERROR_STREAM(error_msg); throw std::runtime_error(error_msg); } + } - optional udp_profile_lidar; - if (is_arg_set(udp_profile_lidar_arg)) { - // set lidar profile from param - udp_profile_lidar = - sensor::udp_profile_lidar_of_string(udp_profile_lidar_arg); - if (!udp_profile_lidar) { + // set timestamp mode from param + sensor::timestamp_mode timestamp_mode = sensor::TIME_FROM_UNSPEC; + if (is_arg_set(timestamp_mode_arg)) { + // In case the option TIME_FROM_ROS_TIME is set then leave the + // sensor timestamp_mode unmodified + if (timestamp_mode_arg == "TIME_FROM_ROS_TIME") { + NODELET_INFO( + "TIME_FROM_ROS_TIME timestamp mode specified." + " IMU and pointcloud messages will use ros time"); + } else { + timestamp_mode = + sensor::timestamp_mode_of_string(timestamp_mode_arg); + if (!timestamp_mode) { auto error_msg = - "Invalid udp profile lidar: " + udp_profile_lidar_arg; + "Invalid timestamp mode: " + timestamp_mode_arg; NODELET_ERROR_STREAM(error_msg); throw std::runtime_error(error_msg); } } + } - // set lidar mode from param - sensor::lidar_mode lidar_mode = sensor::MODE_UNSPEC; - if (is_arg_set(lidar_mode_arg)) { - lidar_mode = sensor::lidar_mode_of_string(lidar_mode_arg); - if (!lidar_mode) { - auto error_msg = "Invalid lidar mode: " + lidar_mode_arg; - NODELET_ERROR_STREAM(error_msg); - throw std::runtime_error(error_msg); - } + sensor::sensor_config config; + if (lidar_port == 0) { + NODELET_WARN_COND( + !is_arg_set(mtp_dest_arg), + "lidar port set to zero, the client will assign a random port " + "number!"); + } else { + config.udp_port_lidar = lidar_port; + } + + if (imu_port == 0) { + NODELET_WARN_COND( + !is_arg_set(mtp_dest_arg), + "imu port set to zero, the client will assign a random port " + "number!"); + } else { + config.udp_port_imu = imu_port; + } + + config.udp_profile_lidar = udp_profile_lidar; + config.operating_mode = sensor::OPERATING_NORMAL; + if (lidar_mode) config.ld_mode = lidar_mode; + if (timestamp_mode) config.ts_mode = timestamp_mode; + if (is_arg_set(udp_dest)) { + config.udp_dest = udp_dest; + if (sensor::in_multicast(udp_dest)) { + mtp_dest = is_arg_set(mtp_dest_arg) ? mtp_dest_arg : std::string{}; + mtp_main = mtp_main_arg; } + } - // set timestamp mode from param - sensor::timestamp_mode timestamp_mode = sensor::TIME_FROM_UNSPEC; - if (is_arg_set(timestamp_mode_arg)) { - // In case the option TIME_FROM_ROS_TIME is set then leave the - // sensor timestamp_mode unmodified - if (timestamp_mode_arg == "TIME_FROM_ROS_TIME") { - NODELET_INFO( - "TIME_FROM_ROS_TIME timestamp mode specified." - " IMU and pointcloud messages will use ros time"); + return config; +} + +sensor::sensor_config OusterSensor::parse_config_from_staged_config_string() { + sensor::sensor_config config = sensor::parse_config(staged_config); + staged_config.clear(); + return config; +} + +uint8_t OusterSensor::compose_config_flags( + const sensor::sensor_config& config) { + uint8_t config_flags = 0; + if (config.udp_dest) { + NODELET_INFO_STREAM("Will send UDP data to " + << config.udp_dest.value()); + // TODO: revise multicast setup inference + if (sensor::in_multicast(*config.udp_dest)) { + if (is_arg_set(mtp_dest)) { + NODELET_INFO_STREAM("Will recieve data via multicast on " + << mtp_dest); } else { - timestamp_mode = - sensor::timestamp_mode_of_string(timestamp_mode_arg); - if (!timestamp_mode) { - auto error_msg = - "Invalid timestamp mode: " + timestamp_mode_arg; - NODELET_ERROR_STREAM(error_msg); - throw std::runtime_error(error_msg); - } + NODELET_INFO( + "mtp_dest was not set, will recieve data via multicast " + "on first available interface"); } } + } else { + NODELET_INFO("Will use automatic UDP destination"); + config_flags |= ouster::sensor::CONFIG_UDP_DEST_AUTO; + } - sensor::sensor_config config; - if (lidar_port == 0) { - NODELET_WARN_COND( - !is_arg_set(mtp_dest_arg), - "lidar port set to zero, the client will assign a random port " - "number!"); - } else { - config.udp_port_lidar = lidar_port; - } + if (force_sensor_reinit) { + force_sensor_reinit = false; + NODELET_INFO("Forcing sensor to reinitialize"); + config_flags |= ouster::sensor::CONFIG_FORCE_REINIT; + } - if (imu_port == 0) { - NODELET_WARN_COND( - !is_arg_set(mtp_dest_arg), - "imu port set to zero, the client will assign a random port " - "number!"); - } else { - config.udp_port_imu = imu_port; - } + return config_flags; +} - config.udp_profile_lidar = udp_profile_lidar; - config.operating_mode = sensor::OPERATING_NORMAL; - if (lidar_mode) config.ld_mode = lidar_mode; - if (timestamp_mode) config.ts_mode = timestamp_mode; - - uint8_t config_flags = 0; - - if (is_arg_set(udp_dest)) { - NODELET_INFO_STREAM("Will send UDP data to " << udp_dest); - config.udp_dest = udp_dest; - if (sensor::in_multicast(udp_dest)) { - if (is_arg_set(mtp_dest_arg)) { - NODELET_INFO_STREAM("Will recieve data via multicast on " - << mtp_dest_arg); - mtp_dest = mtp_dest_arg; - } else { - NODELET_INFO( - "mtp_dest was not set, will recieve data via multicast " - "on first available interface"); - mtp_dest = std::string{}; - } - mtp_main = mtp_main_arg; - } +void OusterSensor::configure_sensor(const std::string& hostname, + sensor::sensor_config& config) { + if (config.udp_dest && sensor::in_multicast(config.udp_dest.value()) && + !mtp_main) { + if (!get_config(hostname, config, true)) { + NODELET_ERROR("Error getting active config"); } else { - NODELET_INFO("Will use automatic UDP destination"); - config_flags |= ouster::sensor::CONFIG_UDP_DEST_AUTO; + NODELET_INFO("Retrived active config of sensor"); } + return; + } - return std::make_pair(config, config_flags); + uint8_t config_flags = compose_config_flags(config); + if (!set_config(hostname, config, config_flags)) { + auto error_msg = "Error connecting to sensor " + hostname; + NODELET_ERROR_STREAM(error_msg); + throw std::runtime_error(error_msg); } - void configure_sensor(const std::string& hostname, - sensor::sensor_config& config, int config_flags) { - if (config.udp_dest && sensor::in_multicast(config.udp_dest.value()) && - !mtp_main) { - if (!get_config(hostname, config, true)) { - NODELET_ERROR("Error getting active config"); - } else { - NODELET_INFO("Retrived active config of sensor"); - } - return; - } + NODELET_INFO_STREAM("Sensor " << hostname + << " was configured successfully"); +} + +void OusterSensor::populate_metadata_defaults( + sensor::sensor_info& info, sensor::lidar_mode specified_lidar_mode) { + if (!info.name.size()) info.name = "UNKNOWN"; + if (!info.sn.size()) info.sn = "UNKNOWN"; + + ouster::util::version v = ouster::util::version_of_string(info.fw_rev); + if (v == ouster::util::invalid_version) + NODELET_WARN( + "Unknown sensor firmware version; output may not be reliable"); + else if (v < sensor::min_version) + NODELET_WARN("Firmware < %s not supported; output may not be reliable", + to_string(sensor::min_version).c_str()); + + if (!info.mode) { + NODELET_WARN( + "Lidar mode not found in metadata; output may not be reliable"); + info.mode = specified_lidar_mode; + } - try { - if (!set_config(hostname, config, config_flags)) { - auto err_msg = "Error connecting to sensor " + hostname; - NODELET_ERROR_STREAM(err_msg); - throw std::runtime_error(err_msg); - } - } catch (const std::exception& e) { - NODELET_ERROR("Error setting config: %s", e.what()); - throw; - } + if (!info.prod_line.size()) info.prod_line = "UNKNOWN"; - NODELET_INFO_STREAM("Sensor " << hostname - << " was configured successfully"); - } - - bool load_config_file(const std::string& config_file, - sensor::sensor_config& out_config) { - std::ifstream ifs{}; - ifs.open(config_file); - if (ifs.fail()) return false; - std::stringstream buf; - buf << ifs.rdbuf(); - out_config = sensor::parse_config(buf.str()); - return true; - } - - private: - // fill in values that could not be parsed from metadata - void populate_metadata_defaults(sensor::sensor_info& info, - sensor::lidar_mode specified_lidar_mode) { - if (!info.name.size()) info.name = "UNKNOWN"; - if (!info.sn.size()) info.sn = "UNKNOWN"; - - ouster::util::version v = ouster::util::version_of_string(info.fw_rev); - if (v == ouster::util::invalid_version) - NODELET_WARN( - "Unknown sensor firmware version; output may not be reliable"); - else if (v < sensor::min_version) - NODELET_WARN( - "Firmware < %s not supported; output may not be reliable", - to_string(sensor::min_version).c_str()); - - if (!info.mode) { - NODELET_WARN( - "Lidar mode not found in metadata; output may not be reliable"); - info.mode = specified_lidar_mode; + if (info.beam_azimuth_angles.empty() || info.beam_altitude_angles.empty()) { + NODELET_ERROR("Beam angles not found in metadata; using design values"); + info.beam_azimuth_angles = sensor::gen1_azimuth_angles; + info.beam_altitude_angles = sensor::gen1_altitude_angles; + } +} + +void OusterSensor::on_metadata_updated(const sensor::sensor_info& info) { + display_lidar_info(info); +} + +void OusterSensor::create_services() { + create_reset_service(); + create_get_metadata_service(); + create_get_config_service(); + create_set_config_service(); +} + +void OusterSensor::create_publishers() { + auto& nh = getNodeHandle(); + lidar_packet_pub = nh.advertise("lidar_packets", 1280); + imu_packet_pub = nh.advertise("imu_packets", 100); +} + +void OusterSensor::allocate_buffers() { + auto& pf = sensor::get_format(info); + + lidar_packet.buf.resize(pf.lidar_packet_size); + // TODO: gauge necessary queue size for lidar packets + lidar_packets = + std::make_unique(pf.lidar_packet_size, 1024); + + imu_packet.buf.resize(pf.imu_packet_size); + // TODO: gauge necessary queue size for lidar packets + imu_packets = + std::make_unique(pf.imu_packet_size, 1024); +} + +bool OusterSensor::init_id_changed(const sensor::packet_format& pf, + const uint8_t* lidar_buf) { + uint32_t current_init_id = pf.init_id(lidar_buf); + if (!last_init_id_initialized) { + last_init_id = current_init_id + 1; + last_init_id_initialized = true; + } + if (reset_last_init_id && last_init_id != current_init_id) { + last_init_id = current_init_id; + reset_last_init_id = false; + return false; + } + if (last_init_id == current_init_id) return false; + last_init_id = current_init_id; + return true; +} + +void OusterSensor::handle_poll_client_error() { + NODELET_WARN_THROTTLE(1, "sensor::poll_client()) returned error"); + // in case error continues for a while attempt to recover by + // performing sensor reset + if (++poll_client_error_count > max_poll_client_error_count) { + NODELET_ERROR_STREAM( + "maximum number of allowed errors from " + "sensor::poll_client() reached, performing self reset..."); + poll_client_error_count = 0; + reset_sensor(true); + } +} + +void OusterSensor::handle_lidar_packet(sensor::client& cli, + const sensor::packet_format& pf) { + lidar_packets->write_overwrite([this, &cli, pf](uint8_t* buffer) { + bool success = sensor::read_lidar_packet(cli, buffer, pf); + if (success) { + read_lidar_packet_errors = 0; + if (!is_legacy_lidar_profile(info) && init_id_changed(pf, buffer)) { + // TODO: short circut reset if no breaking changes occured? + NODELET_WARN("sensor init_id has changed! reactivating.."); + reactivate_sensor(); + } + } else { + if (++read_lidar_packet_errors > max_read_lidar_packet_errors) { + NODELET_ERROR( + "maximum number of allowed errors from " + "sensor::read_lidar_packet() reached, reactivating..."); + read_lidar_packet_errors = 0; + reactivate_sensor(true); + } } + }); +} - if (!info.prod_line.size()) info.prod_line = "UNKNOWN"; - - if (info.beam_azimuth_angles.empty() || - info.beam_altitude_angles.empty()) { - NODELET_ERROR( - "Beam angles not found in metadata; using design values"); - info.beam_azimuth_angles = sensor::gen1_azimuth_angles; - info.beam_altitude_angles = sensor::gen1_altitude_angles; +void OusterSensor::handle_imu_packet(sensor::client& cli, + const sensor::packet_format& pf) { + imu_packets->write_overwrite([this, &cli, pf](uint8_t* buffer) { + bool success = sensor::read_imu_packet(cli, buffer, pf); + if (!success) { + if (++read_imu_packet_errors > max_read_imu_packet_errors) { + NODELET_ERROR_STREAM( + "maximum number of allowed errors from " + "sensor::read_imu_packet() reached, reactivating..."); + read_imu_packet_errors = 0; + reactivate_sensor(true); + } } + }); +} + +void OusterSensor::connection_loop(sensor::client& cli, + const sensor::packet_format& pf) { + auto state = sensor::poll_client(cli); + if (state == sensor::EXIT) { + NODELET_INFO("poll_client: caught signal, exiting!"); + return; } - - // try to write metadata file - bool write_metadata(const std::string& meta_file, - const std::string& metadata) { - std::ofstream ofs(meta_file); - if (ofs.is_open()) { - ofs << metadata << std::endl; - ofs.close(); - NODELET_INFO("Wrote metadata to %s", meta_file.c_str()); - } else { - NODELET_WARN( - "Failed to write metadata to %s; check that the path is valid. " - "If you provided a relative path, please note that the working " - "directory of all ROS nodes is set by default to $ROS_HOME", - meta_file.c_str()); - return false; - } - return true; + if (state & sensor::CLIENT_ERROR) { + handle_poll_client_error(); + return; + } + poll_client_error_count = 0; + if (state & sensor::LIDAR_DATA) { + handle_lidar_packet(cli, pf); } + if (state & sensor::IMU_DATA) { + handle_imu_packet(cli, pf); + } +} - void allocate_buffers() { +void OusterSensor::start_sensor_connection_thread() { + sensor_connection_active = true; + sensor_connection_thread = std::make_unique([this]() { auto& pf = sensor::get_format(info); - lidar_packet.buf.resize(pf.lidar_packet_size + 1); - imu_packet.buf.resize(pf.imu_packet_size + 1); - } - - void create_publishers(ros::NodeHandle& nh) { - lidar_packet_pub = nh.advertise("lidar_packets", 1280); - imu_packet_pub = nh.advertise("imu_packets", 100); - } - - void start_connection_loop(ros::NodeHandle& nh) { - allocate_buffers(); - create_publishers(nh); - timer_ = nh.createTimer( - ros::Duration(0), - [this](const ros::TimerEvent&) { - auto& pf = sensor::get_format(info); - connection_loop(*sensor_client, pf); - timer_.stop(); - timer_.start(); - }, - true); - } - - void connection_loop(sensor::client& cli, const sensor::packet_format& pf) { - auto state = sensor::poll_client(cli); - if (state == sensor::EXIT) { - NODELET_INFO("poll_client: caught signal, exiting"); - return; - } - if (state & sensor::CLIENT_ERROR) { - NODELET_ERROR("poll_client: returned error"); - return; + while (sensor_connection_active) { + connection_loop(*sensor_client, pf); } - if (state & sensor::LIDAR_DATA) { - if (sensor::read_lidar_packet(cli, lidar_packet.buf.data(), pf)) - lidar_packet_pub.publish(lidar_packet); + NODELET_DEBUG("sensor_connection_thread done."); + }); +} + +void OusterSensor::stop_sensor_connection_thread() { + NODELET_DEBUG("sensor_connection_thread stopping."); + if (sensor_connection_thread->joinable()) { + sensor_connection_active = false; + sensor_connection_thread->join(); + } +} + +void OusterSensor::start_packet_processing_threads() { + imu_packets_skip = false; + imu_packets_processing_thread_active = true; + imu_packets_processing_thread = std::make_unique([this]() { + auto read_packet = [this](const uint8_t* buffer) { + if (!imu_packets_skip) on_imu_packet_msg(buffer); + }; + while (imu_packets_processing_thread_active) { + imu_packets->read(read_packet); } - if (state & sensor::IMU_DATA) { - if (sensor::read_imu_packet(cli, imu_packet.buf.data(), pf)) - imu_packet_pub.publish(imu_packet); + NODELET_DEBUG("imu_packets_processing_thread done."); + }); + + lidar_packets_skip = false; + lidar_packets_processing_thread_active = true; + lidar_packets_processing_thread = std::make_unique([this]() { + while (lidar_packets_processing_thread_active) { + lidar_packets->read([this](const uint8_t* buffer) { + if (!lidar_packets_skip) on_lidar_packet_msg(buffer); + }); } + + NODELET_DEBUG("lidar_packets_processing_thread done."); + }); +} + +void OusterSensor::stop_packet_processing_threads() { + NODELET_DEBUG("stopping packet processing threads."); + + if (imu_packets_processing_thread->joinable()) { + imu_packets_processing_thread_active = false; + imu_packets_processing_thread->join(); } - private: - PacketMsg lidar_packet; - PacketMsg imu_packet; - ros::Publisher lidar_packet_pub; - ros::Publisher imu_packet_pub; - std::shared_ptr sensor_client; - ros::Timer timer_; - std::string sensor_hostname; - ros::ServiceServer get_config_srv; - ros::ServiceServer set_config_srv; - std::string cached_config; - std::string mtp_dest; - bool mtp_main; -}; - -} // namespace nodelets_os - -PLUGINLIB_EXPORT_CLASS(nodelets_os::OusterSensor, nodelet::Nodelet) + if (lidar_packets_processing_thread->joinable()) { + lidar_packets_processing_thread_active = false; + lidar_packets_processing_thread->join(); + } +} + +void OusterSensor::on_lidar_packet_msg(const uint8_t* raw_lidar_packet) { + // copying the data from queue buffer into the message buffer + // this can be avoided by constructing an abstraction where + // OusterSensor has its own RingBuffer of PacketMsg but for + // now we are focusing on optimizing the code for OusterDriver + std::memcpy(lidar_packet.buf.data(), raw_lidar_packet, + lidar_packet.buf.size()); + lidar_packet_pub.publish(lidar_packet); +} + +void OusterSensor::on_imu_packet_msg(const uint8_t* raw_imu_packet) { + // copying the data from queue buffer into the message buffer + // this can be avoided by constructing an abstraction where + // OusterSensor has its own RingBuffer of PacketMsg but for + // now we are focusing on optimizing the code for OusterDriver + std::memcpy(imu_packet.buf.data(), raw_imu_packet, imu_packet.buf.size()); + imu_packet_pub.publish(imu_packet); +} + +// param init_id_reset is overriden to true when force_reinit is true +void OusterSensor::reset_sensor(bool /*force_reinit*/, bool /*init_id_reset*/) { + NODELET_WARN("sensor reset is invoked but sensor it is not implemented"); +} + +void OusterSensor::reactivate_sensor(bool /*init_id_reset*/) { + NODELET_WARN( + "sensor reactivate is invoked but sensor it is not implemented"); +} + +} // namespace ouster_ros + +PLUGINLIB_EXPORT_CLASS(ouster_ros::OusterSensor, nodelet::Nodelet) diff --git a/src/os_sensor_nodelet.h b/src/os_sensor_nodelet.h new file mode 100644 index 00000000..6e636ca8 --- /dev/null +++ b/src/os_sensor_nodelet.h @@ -0,0 +1,156 @@ +/** + * Copyright (c) 2018-2023, Ouster, Inc. + * All rights reserved. + * + * @file os_sensor_node.h + * @brief A node that connects to a live ouster sensor + */ + +#pragma once + +// prevent clang-format from altering the location of "ouster_ros/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 +#include + +#include "ouster_ros/GetConfig.h" +#include "ouster_ros/SetConfig.h" +#include "ouster_ros/PacketMsg.h" +#include "ouster_ros/os_sensor_nodelet_base.h" + +#include "thread_safe_ring_buffer.h" + +namespace sensor = ouster::sensor; + +namespace ouster_ros { + +class OusterSensor : public OusterSensorNodeletBase { + private: + virtual void onInit() override; + + protected: + virtual void on_metadata_updated(const sensor::sensor_info& info); + + virtual void create_services(); + + virtual void create_publishers(); + + virtual void on_lidar_packet_msg(const uint8_t* raw_lidar_packet); + + virtual void on_imu_packet_msg(const uint8_t* raw_imu_packet); + + private: + std::string get_sensor_hostname(); + + void update_config_and_metadata(sensor::client& client); + + void save_metadata(); + + // param init_id_reset is overriden to true when force_reinit is true + void reset_sensor(bool force_reinit, bool init_id_reset = false); + + void reactivate_sensor(bool init_id_reset = false); + + void create_reset_service(); + + void create_get_config_service(); + + void create_set_config_service(); + + std::shared_ptr create_sensor_client( + const std::string& hostname, const sensor::sensor_config& config); + + sensor::sensor_config parse_config_from_ros_parameters(); + + sensor::sensor_config parse_config_from_staged_config_string(); + + uint8_t compose_config_flags(const sensor::sensor_config& config); + + void configure_sensor(const std::string& hostname, + sensor::sensor_config& config); + + std::string load_config_file(const std::string& config_file); + + private: + // fill in values that could not be parsed from metadata + void populate_metadata_defaults(sensor::sensor_info& info, + sensor::lidar_mode specified_lidar_mode); + + void allocate_buffers(); + + bool init_id_changed(const sensor::packet_format& pf, + const uint8_t* lidar_buf); + + void handle_poll_client_error(); + + void handle_lidar_packet(sensor::client& client, + const sensor::packet_format& pf); + + void handle_imu_packet(sensor::client& client, + const sensor::packet_format& pf); + + void cleanup(); + + void connection_loop(sensor::client& client, + const sensor::packet_format& pf); + + void start_sensor_connection_thread(); + + void stop_sensor_connection_thread(); + + void start_packet_processing_threads(); + + void stop_packet_processing_threads(); + + private: + std::string sensor_hostname; + std::string staged_config; + std::string active_config; + std::string mtp_dest; + bool mtp_main; + std::shared_ptr sensor_client; + PacketMsg lidar_packet; + PacketMsg imu_packet; + ros::Publisher lidar_packet_pub; + ros::Publisher imu_packet_pub; + ros::ServiceServer reset_srv; + ros::ServiceServer get_config_srv; + ros::ServiceServer set_config_srv; + + // TODO: implement & utilize a lock-free ring buffer in future + std::unique_ptr lidar_packets; + std::unique_ptr imu_packets; + + std::atomic sensor_connection_active = {false}; + std::unique_ptr sensor_connection_thread; + + std::atomic imu_packets_processing_thread_active = {false}; + std::atomic imu_packets_skip; + std::unique_ptr imu_packets_processing_thread; + + std::atomic lidar_packets_processing_thread_active = {false}; + std::atomic lidar_packets_skip; + std::unique_ptr lidar_packets_processing_thread; + + bool force_sensor_reinit = false; + bool reset_last_init_id = true; + + bool last_init_id_initialized = false; + uint32_t last_init_id; + + // TODO: add as a ros parameter + const int max_poll_client_error_count = 10; + int poll_client_error_count = 0; + // TODO: add as a ros parameter + const int max_read_lidar_packet_errors = 60; + int read_lidar_packet_errors = 0; + // TODO: add as a ros parameter + const int max_read_imu_packet_errors = 60; + int read_imu_packet_errors = 0; +}; + +} // namespace ouster_ros diff --git a/src/os_client_base_nodelet.cpp b/src/os_sensor_nodelet_base.cpp similarity index 52% rename from src/os_client_base_nodelet.cpp rename to src/os_sensor_nodelet_base.cpp index ebfe4b13..b4681c62 100644 --- a/src/os_client_base_nodelet.cpp +++ b/src/os_sensor_nodelet_base.cpp @@ -2,11 +2,13 @@ * Copyright (c) 2018-2023, Ouster, Inc. * All rights reserved. * - * @file os_client_base_nodelet.cpp - * @brief implementation of OusterClientBase interface + * @file os_sensor_nodelet_base.cpp + * @brief implementation of OusterSensorNodeletBase interface */ -#include "ouster_ros/os_client_base_nodelet.h" +#include + +#include "ouster_ros/os_sensor_nodelet_base.h" #include #include @@ -14,11 +16,11 @@ #include "ouster_ros/GetMetadata.h" namespace sensor = ouster::sensor; -using ouster_ros::GetMetadata; -namespace nodelets_os { +namespace ouster_ros { -void OusterClientBase::create_get_metadata_service(ros::NodeHandle& nh) { +void OusterSensorNodeletBase::create_get_metadata_service() { + auto& nh = getNodeHandle(); get_metadata_srv = nh.advertiseService( "get_metadata", @@ -30,17 +32,19 @@ void OusterClientBase::create_get_metadata_service(ros::NodeHandle& nh) { NODELET_INFO("get_metadata service created"); } -void OusterClientBase::create_metadata_publisher(ros::NodeHandle& nh) { +void OusterSensorNodeletBase::create_metadata_publisher() { + auto& nh = getNodeHandle(); metadata_pub = nh.advertise("metadata", 1, true); } -void OusterClientBase::publish_metadata() { +void OusterSensorNodeletBase::publish_metadata() { std_msgs::String metadata_msg; metadata_msg.data = cached_metadata; metadata_pub.publish(metadata_msg); } -void OusterClientBase::display_lidar_info(const sensor::sensor_info& info) { +void OusterSensorNodeletBase::display_lidar_info( + const sensor::sensor_info& info) { auto lidar_profile = info.format.udp_profile_lidar; NODELET_INFO_STREAM( "ouster client version: " @@ -51,4 +55,23 @@ void OusterClientBase::display_lidar_info(const sensor::sensor_info& info) { << "lidar udp profile: " << sensor::to_string(lidar_profile)); } -} // namespace nodelets_os +std::string OusterSensorNodeletBase::read_text_file( + const std::string& text_file) { + std::ifstream ifs{}; + ifs.open(text_file); + if (ifs.fail()) return {}; + std::stringstream buf; + buf << ifs.rdbuf(); + return buf.str(); +} + +bool OusterSensorNodeletBase::write_text_to_file(const std::string& file_path, + const std::string& text) { + std::ofstream ofs(file_path); + if (!ofs.is_open()) return false; + ofs << text << std::endl; + ofs.close(); + return true; +} + +} // namespace ouster_ros diff --git a/src/os_transforms_broadcaster.h b/src/os_transforms_broadcaster.h new file mode 100644 index 00000000..a3fea44b --- /dev/null +++ b/src/os_transforms_broadcaster.h @@ -0,0 +1,108 @@ +/** + * Copyright (c) 2018-2023, Ouster, Inc. + * All rights reserved. + * + * @file os_tf_publisher.h + * @brief ... + */ + +#pragma once + +#include +#include + +namespace ouster_ros { + +class OusterTransformsBroadcaster { + public: + OusterTransformsBroadcaster(const std::string& parent_name) + : node_name(parent_name) {} + + private: + bool is_arg_set(const std::string& arg) const { + return arg.find_first_not_of(' ') != std::string::npos; + } + + const std::string& getName() const { return node_name; } + + public: + void parse_parameters(ros::NodeHandle& pnh) { + sensor_frame = pnh.param("sensor_frame", std::string{"os_sensor"}); + lidar_frame = pnh.param("lidar_frame", std::string{"os_lidar"}); + imu_frame = pnh.param("imu_frame", std::string{"os_imu"}); + point_cloud_frame = pnh.param("point_cloud_frame", std::string{}); + + if (!is_arg_set(sensor_frame) || !is_arg_set(lidar_frame) || + !is_arg_set(imu_frame)) { + NODELET_ERROR( + "sensor_frame, lidar_frame, imu_frame parameters can not be " + "empty"); + } + + // validate point_cloud_frame + if (!is_arg_set(point_cloud_frame)) { + // for ROS1 we'd still default to sensor_frame + point_cloud_frame = sensor_frame; + } else if (point_cloud_frame != sensor_frame && + point_cloud_frame != lidar_frame) { + NODELET_WARN( + "point_cloud_frame value needs to match the value of either" + "sensor_frame or lidar_frame but a different value was supplied" + ", using lidar_frame's value as the value for " + "point_cloud_frame"); + point_cloud_frame = lidar_frame; + } + + if (point_cloud_frame == sensor_frame) { + NODELET_WARN( + "point_cloud_frame is set to sensor_frame. If you need to " + "reproject or raytrace the points then use the lidar_frame " + "instead. Refer to: https://github.com/ouster-lidar/ouster-ros" + "/issues/33 for more details."); + } + + // Finally prepend tf_prefix if it was set + auto tf_prefix = pnh.param("tf_prefix", std::string{}); + if (is_arg_set(tf_prefix)) { + if (tf_prefix.back() != '/') tf_prefix.append("/"); + sensor_frame = tf_prefix + sensor_frame; + lidar_frame = tf_prefix + lidar_frame; + imu_frame = tf_prefix + imu_frame; + } + } + + void broadcast_transforms(const sensor::sensor_info& info) { + auto now = ros::Time::now(); + static_tf_bcast.sendTransform(transform_to_tf_msg( + info.lidar_to_sensor_transform, sensor_frame, lidar_frame, now)); + static_tf_bcast.sendTransform(transform_to_tf_msg( + info.imu_to_sensor_transform, sensor_frame, imu_frame, now)); + } + + void broadcast_transforms(const sensor::sensor_info& info, + const ros::Time& ts) { + tf_bcast.sendTransform(transform_to_tf_msg( + info.lidar_to_sensor_transform, sensor_frame, lidar_frame, ts)); + tf_bcast.sendTransform(transform_to_tf_msg( + info.imu_to_sensor_transform, sensor_frame, imu_frame, ts)); + } + + const std::string& imu_frame_id() const { return imu_frame; } + const std::string& point_cloud_frame_id() const { + return point_cloud_frame; + } + bool apply_lidar_to_sensor_transform() const { + return point_cloud_frame == sensor_frame; + } + + private: + std::string node_name; + tf2_ros::StaticTransformBroadcaster static_tf_bcast; + tf2_ros::TransformBroadcaster tf_bcast; // non-static + std::string sensor_frame; + std::string imu_frame; + std::string lidar_frame; + std::string point_cloud_frame; +}; + +} // namespace ouster_ros diff --git a/src/point_cloud_processor.h b/src/point_cloud_processor.h new file mode 100644 index 00000000..52d8ff2e --- /dev/null +++ b/src/point_cloud_processor.h @@ -0,0 +1,105 @@ +/** + * Copyright (c) 2018-2023, Ouster, Inc. + * All rights reserved. + * + * @file point_cloud_producer.h + * @brief takes in a lidar scan object and produces a PointCloud2 message + */ + +#pragma once + +// prevent clang-format from altering the location of "ouster_ros/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 "lidar_packet_handler.h" + +namespace ouster_ros { + +class PointCloudProcessor { + public: + using OutputType = std::vector>; + using PostProcessingFn = std::function; + + public: + PointCloudProcessor(const ouster::sensor::sensor_info& info, + const std::string& frame_id, + bool apply_lidar_to_sensor_transform, + PostProcessingFn func) + : frame(frame_id), + cloud{info.format.columns_per_frame, info.format.pixels_per_column}, + pc_msgs(get_n_returns(info)), + post_processing_fn(func) { + for (size_t i = 0; i < pc_msgs.size(); ++i) + pc_msgs[i] = std::make_shared(); + ouster::mat4d additional_transform = + apply_lidar_to_sensor_transform ? info.lidar_to_sensor_transform + : ouster::mat4d::Identity(); + auto xyz_lut = ouster::make_xyz_lut( + info.format.columns_per_frame, info.format.pixels_per_column, + ouster::sensor::range_unit, info.beam_to_lidar_transform, + additional_transform, info.beam_azimuth_angles, + info.beam_altitude_angles); + // The ouster_ros drive currently only uses single precision when it + // produces the point cloud. So it isn't of a benefit to compute point + // cloud xyz coordinates using double precision (for the time being). + lut_direction = xyz_lut.direction.cast(); + lut_offset = xyz_lut.offset.cast(); + points = ouster::PointsF(lut_direction.rows(), lut_offset.cols()); + } + + private: + void pcl_toROSMsg(const ouster_ros::Cloud& pcl_cloud, + sensor_msgs::PointCloud2& cloud) { + // TODO: remove the staging step in the future + pcl::toPCLPointCloud2(pcl_cloud, staging_pcl_pc2); + pcl_conversions::moveFromPCL(staging_pcl_pc2, cloud); + } + + void process(const ouster::LidarScan& lidar_scan, uint64_t scan_ts, + const ros::Time& msg_ts) { + for (int i = 0; i < static_cast(pc_msgs.size()); ++i) { + scan_to_cloud_f(points, lut_direction, lut_offset, scan_ts, + lidar_scan, cloud, i); + pcl_toROSMsg(cloud, *pc_msgs[i]); + pc_msgs[i]->header.stamp = msg_ts; + pc_msgs[i]->header.frame_id = frame; + } + + if (post_processing_fn) post_processing_fn(pc_msgs); + } + + public: + static LidarScanProcessor create(const ouster::sensor::sensor_info& info, + const std::string& frame, + bool apply_lidar_to_sensor_transform, + PostProcessingFn func) { + auto handler = std::make_shared( + info, frame, apply_lidar_to_sensor_transform, func); + + return [handler](const ouster::LidarScan& lidar_scan, uint64_t scan_ts, + const ros::Time& msg_ts) { + handler->process(lidar_scan, scan_ts, msg_ts); + }; + } + + private: + // a buffer used for staging during the conversion + // from a PCL point cloud to a ros point cloud message + pcl::PCLPointCloud2 staging_pcl_pc2; + + std::string frame; + + ouster::PointsF lut_direction; + ouster::PointsF lut_offset; + ouster::PointsF points; + ouster_ros::Cloud cloud; + + OutputType pc_msgs; + + PostProcessingFn post_processing_fn; +}; + +} // namespace ouster_ros \ No newline at end of file diff --git a/src/thread_safe_ring_buffer.h b/src/thread_safe_ring_buffer.h new file mode 100644 index 00000000..26b5c88d --- /dev/null +++ b/src/thread_safe_ring_buffer.h @@ -0,0 +1,146 @@ +/** + * Copyright (c) 2018-2023, Ouster, Inc. + * All rights reserved. + * + * @file thread_safe_ring_buffer.h + * @brief File contains thread safe implementation of a ring buffer + */ + +#pragma once + +#include +#include +#include + +/** + * @class ThreadSafeRingBuffer thread safe ring buffer. + */ +class ThreadSafeRingBuffer { + public: + ThreadSafeRingBuffer(size_t item_size_, size_t items_count_) + : buffer(item_size_ * items_count_), + item_size(item_size_), + max_items_count(items_count_), + active_items_count(0), + write_idx(0), + read_idx(0) {} + + /** + * Gets the maximum number of items that this ring buffer can hold. + */ + size_t capacity() const { return max_items_count; } + + /** + * Gets the number of item that currently occupy the ring buffer. This + * number would vary between 0 and the capacity(). + * + * @remarks + * if returned value was 0 or the value was equal to the buffer capacity(), + * this does not guarantee that a subsequent call to read() or write() + * wouldn't cause the calling thread to be blocked. + */ + size_t size() const { + std::lock_guard lock(mutex); + return active_items_count; + } + + /** + * Checks if the ring buffer is empty. + * + * @remarks + * if empty() returns true this does not guarantee that calling the write() + * operation directly right after wouldn't block the calling thread. + */ + bool empty() const { + std::lock_guard lock(mutex); + return active_items_count == 0; + } + + /** + * Checks if the ring buffer is full. + * + * @remarks + * if full() returns true this does not guarantee that calling the read() + * operation directly right after wouldn't block the calling thread. + */ + bool full() const { + std::lock_guard lock(mutex); + return active_items_count == max_items_count; + } + + /** + * Writes to the buffer safely, the method will keep blocking until the + * there is a space available within the buffer. + */ + template + void write(BufferWriteFn&& buffer_write) { + std::unique_lock lock(mutex); + fullCondition.wait(lock, + [this] { return active_items_count < capacity(); }); + buffer_write(&buffer[write_idx * item_size]); + write_idx = (write_idx + 1) % capacity(); + ++active_items_count; + emptyCondition.notify_one(); + } + + /** + * Writes to the buffer safely, if there is not space left then this method + * will overite the last item. + */ + template + void write_overwrite(BufferWriteFn&& buffer_write) { + std::unique_lock lock(mutex); + buffer_write(&buffer[write_idx * item_size]); + write_idx = (write_idx + 1) % capacity(); + if (active_items_count < capacity()) { + ++active_items_count; + } else { + read_idx = (read_idx + 1) % capacity(); + } + emptyCondition.notify_one(); + } + + /** + * Gives access to read the buffer through a callback, the method will block + * until there is something to read is available. + */ + template + void read(BufferReadFn&& buffer_read) { + std::unique_lock lock(mutex); + emptyCondition.wait(lock, [this] { return active_items_count > 0; }); + buffer_read(&buffer[read_idx * item_size]); + read_idx = (read_idx + 1) % capacity(); + --active_items_count; + fullCondition.notify_one(); + } + + /** + * Gives access to read the buffer through a callback, if buffer is + * inaccessible the method will timeout and buffer_read gets a nullptr. + */ + template + void read_timeout(BufferReadFn&& buffer_read, + std::chrono::seconds timeout) { + std::unique_lock lock(mutex); + if (emptyCondition.wait_for( + lock, timeout, [this] { return active_items_count > 0; })) { + buffer_read(&buffer[read_idx * item_size]); + read_idx = (read_idx + 1) % capacity(); + --active_items_count; + fullCondition.notify_one(); + } else { + buffer_read((uint8_t*)nullptr); + } + } + + private: + std::vector buffer; + size_t item_size; + size_t max_items_count; + size_t active_items_count; + size_t write_idx; + size_t read_idx; + mutable std::mutex mutex; + std::condition_variable fullCondition; + std::condition_variable emptyCondition; +}; \ No newline at end of file diff --git a/tests/ring_buffer_test.cpp b/tests/ring_buffer_test.cpp new file mode 100644 index 00000000..8d8bd7f4 --- /dev/null +++ b/tests/ring_buffer_test.cpp @@ -0,0 +1,181 @@ +#include +#include +#include +#include +#include "../src/thread_safe_ring_buffer.h" + +using namespace std::chrono_literals; + +class ThreadSafeRingBufferTest : public ::testing::Test { + protected: + static const int ITEM_SIZE = 4; // predefined size for all items used in + static const int ITEM_COUNT = 3; // number of item the buffer could hold + + void SetUp() override { + buffer = std::make_unique(ITEM_SIZE, ITEM_COUNT); + } + + void TearDown() override { + buffer.reset(); + } + + std::string rand_str(int size) { + const std::string characters = + "abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ0123456789"; + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_int_distribution dist(0, characters.size() - 1); + + std::string result; + for (int i = 0; i < size; ++i) { + result += characters[dist(gen)]; + } + return result; + } + + std::vector rand_vector_str(int vec_size, int str_size) { + std::vector output(vec_size); + for (auto i = 0; i < vec_size; ++i) + output[i] = rand_str(str_size); + return output; + } + + std::vector known_vector_str(int vec_size, const std::string& known) { + std::vector output(vec_size); + for (auto i = 0; i < vec_size; ++i) + output[i] = known; + return output; + } + + std::unique_ptr buffer; +}; + +TEST_F(ThreadSafeRingBufferTest, ReadWriteToBufferSimple) { + + assert (ITEM_COUNT > 1 && "or this test can't run"); + + const int TOTAL_ITEMS = 10; // total items to process + const std::vector source = rand_vector_str(TOTAL_ITEMS, ITEM_SIZE); + std::vector target = known_vector_str(TOTAL_ITEMS, "0000"); + + EXPECT_TRUE(buffer->empty()); + EXPECT_FALSE(buffer->full()); + + for (int i = 0; i < ITEM_COUNT; ++i) { + buffer->write([i, &source](uint8_t* buffer) { + std::memcpy(buffer, &source[i][0], ITEM_SIZE); + }); + } + + EXPECT_FALSE(buffer->empty()); + EXPECT_TRUE(buffer->full()); + + // remove one item + buffer->read([&target](uint8_t* buffer){ + std::memcpy(&target[0][0], buffer, ITEM_SIZE); + }); + + EXPECT_FALSE(buffer->empty()); + EXPECT_FALSE(buffer->full()); + + for (int i = 1; i < ITEM_COUNT; ++i) { + buffer->read([i, &target](uint8_t* buffer){ + std::memcpy(&target[i][0], buffer, ITEM_SIZE); + }); + } + + EXPECT_TRUE(buffer->empty()); + EXPECT_FALSE(buffer->full()); + + for (int i = 0; i < ITEM_COUNT; ++i) { + std::cout << "source " << source[i] << ", target " << target[i] << std::endl; + EXPECT_EQ(target[i], source[i]); + } +} + +TEST_F(ThreadSafeRingBufferTest, ReadWriteToBuffer) { + + const int TOTAL_ITEMS = 10; // total items to process + const std::vector source = rand_vector_str(TOTAL_ITEMS, ITEM_SIZE); + std::vector target = known_vector_str(TOTAL_ITEMS, "0000"); + + EXPECT_TRUE(buffer->empty()); + EXPECT_FALSE(buffer->full()); + + std::thread producer([this, &source]() { + for (int i = 0; i < TOTAL_ITEMS; ++i) { + buffer->write([i, &source](uint8_t* buffer){ + std::memcpy(buffer, &source[i][0], ITEM_SIZE); + }); + } + }); + + std::thread consumer([this, &target]() { + for (int i = 0; i < TOTAL_ITEMS; ++i) { + buffer->read([i, &target](uint8_t* buffer){ + std::memcpy(&target[i][0], buffer, ITEM_SIZE); + }); + } + }); + + producer.join(); + consumer.join(); + + for (int i = 0; i < TOTAL_ITEMS; ++i) { + std::cout << "source " << source[i] << ", target " << target[i] << std::endl; + EXPECT_EQ(target[i], source[i]); + } +} + +TEST_F(ThreadSafeRingBufferTest, ReadWriteToBufferWithOverwrite) { + + const int TOTAL_ITEMS = 10; // total items to process + const std::vector source = rand_vector_str(TOTAL_ITEMS, ITEM_SIZE); + std::vector target = known_vector_str(TOTAL_ITEMS, "0000"); + + EXPECT_TRUE(buffer->empty()); + EXPECT_FALSE(buffer->full()); + + std::thread producer([this, &source]() { + for (int i = 0; i < TOTAL_ITEMS; ++i) { + buffer->write_overwrite([i, &source](uint8_t* buffer){ + std::memcpy(buffer, &source[i][0], ITEM_SIZE); + }); + } + }); + + // wait for 1 second before starting the consumer thread + // allowing sufficient time for the producer thread to be + // completely done + std::this_thread::sleep_for(1s); + std::thread consumer([this, &target]() { + for (int i = 0; i < TOTAL_ITEMS; ++i) { + buffer->read_timeout([i, &target](uint8_t* buffer){ + if (buffer != nullptr) + std::memcpy(&target[i][0], buffer, ITEM_SIZE); + }, 1s); + } + }); + + producer.join(); + consumer.join(); + + // Since our buffer can host only up to ITEM_COUNT simultanously only the + // last ITEM_COUNT items would have remained in the buffer by the time + // the consumer started processing. + for (int i = 0; i < ITEM_COUNT; ++i) { + std::cout << "source " << source[i] << ", target " << target[i] << std::endl; + EXPECT_EQ(target[i], source[TOTAL_ITEMS-ITEM_COUNT+i]); + } + + for (int i = ITEM_COUNT; i < TOTAL_ITEMS; ++i) { + std::cout << "source " << source[i] << ", target " << target[i] << std::endl; + EXPECT_EQ(target[i], "0000"); + } +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file