From 593dc2bbde252e14658b1ae405f536fab0dd9ffa Mon Sep 17 00:00:00 2001 From: MattSYoung <85848767+MattSYoung@users.noreply.github.com> Date: Thu, 5 Aug 2021 05:32:29 +1200 Subject: [PATCH] Adding a Tins-based driver implementation (#77) * Ported optional-lite source files * Added new functions to types.h/cpp * Function changes lidar.h/cpp * Added and changed functions in client.h/cpp * Bug fix from ouster_example/#246 * Added signal_multiplier parameter to client and types files * Updated changelog * Updated changelog * Updated changelog with a more detailed list of changes * Updated readme with FW version * Created outline of SensorTins class * Completed non-libtins related parts of SensorTins * Added core Libtins elements and dependency * Finished first design of SensorTins * Implemented the ability to switch between default and Tins sensor objects * Added an example metadata file * Tweaks and fixes to get SensorTins working * Improved the documentation around the metadata file, and added code to ensure the Sensor driver implementation writes to it. * Readme and changelog updates * Updated readme with commands for running the driver in Tins mode * Changed the implementation of SensorTins to more closely match Steves vision for how the user changes between driver types * Commented out some erroneously committed debugging code * Changed driver stucture to use different sets of launch and main files for the DefaultDriver and TinsDriver * Updated changelog and readme * Changed metadata filepath to be relative rather than absolute * Added an IP source filter to the Tins sniffer to avoid the sniffer being flooded with irrelevant packets * Updated readme with FW version * Added verbosity settings to launch files * Moved parameter getting to the sensor implementation from the driver implementation * Shifted parameter declaration to sensor::onConfigure functions and made sensor config files separate * Fixed bug caused by a missing parameter declaration * Added debug output to notify the user of how many packets were accumulated into the published point cloud * Added debug code to show the packet retrieval time * Minor tweaks to verbosity and metadata * Updated copyright header to reflect original author * Updated readme and param files * Updated changelog and removed some debugging code * Updated readme, reverted default IP values, removed unneccesary newline * Added missing package.xml dependency and reverted Tins driver verbosity back to INFO by default * Moved common sensor parameters back to Driver onCOnfigure * Renamed default driver class and files names * Changed the getMetadata service to save data to a file when specified * Bug fixes for default driver launch file and metadata filepath printing * Updated changelog * Updated changelog again because nested lists don't seem to work with .rst files * Updated the changelog again to fix formatting * Added example metadata files and removed old ones * Updated readme and GetMetadata service * Moved sniffer to a unique pointer from raw pointer. Also updated metadata filepath processing * Updated CMakeLists file to remove unnecessary libtins include line * Put LiDAR and computer IP getting back inside a try/catch block * First pass from the ROS2 linter * Final changes from ament_uncrustify and ament_cpplint * Added caveats to explain that the ipv6 discovery only works with the default driver * Changelog and readme updates * Shifted INFO outputs that are sensor specific to the configure function for those sensor implementations --- CHANGELOG.rst | 59 ++- README.md | 59 ++- ouster_msgs/srv/GetMetadata.srv | 1 + ros2_ouster/CMakeLists.txt | 22 +- .../include/ros2_ouster/driver_types.hpp | 21 + .../ros2_ouster/full_rotation_accumulator.hpp | 10 + .../ros2_ouster/interfaces/configuration.hpp | 2 + .../ros2_ouster/interfaces/metadata.hpp | 2 +- .../interfaces/sensor_interface.hpp | 10 +- .../include/ros2_ouster/ouster_driver.hpp | 2 + .../processors/image_processor.hpp | 5 +- .../processors/pointcloud_processor.hpp | 5 + .../processors/processor_factories.hpp | 7 +- .../include/ros2_ouster/ros2_utils.hpp | 68 +++ ros2_ouster/include/ros2_ouster/sensor.hpp | 12 +- .../include/ros2_ouster/sensor_tins.hpp | 188 ++++++++ .../{os1_launch.py => driver_launch.py} | 4 +- ros2_ouster/launch/tins_driver_launch.py | 105 ++++ ros2_ouster/package.xml | 1 + .../{sensor.yaml => driver_config.yaml} | 6 +- .../params/ouster_os0128_1024_metadata.json | 451 ++++++++++++++++++ .../params/ouster_os0128_2048_metadata.json | 451 ++++++++++++++++++ ros2_ouster/params/tins_driver_config.yaml | 56 +++ ros2_ouster/src/driver_types.cpp | 5 + ros2_ouster/src/lifecycle_interface.cpp | 1 - ros2_ouster/src/main.cpp | 1 + ros2_ouster/src/main_tins.cpp | 30 ++ ros2_ouster/src/ouster_driver.cpp | 105 ++-- ros2_ouster/src/sensor.cpp | 34 +- ros2_ouster/src/sensor_tins.cpp | 241 ++++++++++ 30 files changed, 1865 insertions(+), 99 deletions(-) create mode 100644 ros2_ouster/include/ros2_ouster/ros2_utils.hpp create mode 100644 ros2_ouster/include/ros2_ouster/sensor_tins.hpp rename ros2_ouster/launch/{os1_launch.py => driver_launch.py} (96%) create mode 100644 ros2_ouster/launch/tins_driver_launch.py rename ros2_ouster/params/{sensor.yaml => driver_config.yaml} (95%) create mode 100644 ros2_ouster/params/ouster_os0128_1024_metadata.json create mode 100644 ros2_ouster/params/ouster_os0128_2048_metadata.json create mode 100644 ros2_ouster/params/tins_driver_config.yaml create mode 100644 ros2_ouster/src/main_tins.cpp create mode 100644 ros2_ouster/src/sensor_tins.cpp diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 18add8c..f61d1f2 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -1,33 +1,48 @@ Changelog ========= -[unreleased] Ported client changes for FW 2.0/2.1 (2021-06-23) +[0.3.0] Added a new SensorTins implementation (2021-06-23) +------------- + +**General Changes** + +* Added example metadata files for OS0-128 in 1024x10 and 2048x10 mode. +* Added/exposed the functions for loading and saving metadata to a json file. +* Added metadatat file saving to ``getMetadata`` service. +* Contibutors: Matthew Young (Trimble Inc). + +**TinsDriver** + +* Added a new SensorImplementation called SensorTins, and a Driver implementation that uses it called TinsDriver. This implementation uses the Tins library to find LiDAR and IMU packets, and is intended to be used with replayed pcap files rather than a real LiDAR. +* Added a new driver called TinsDriver which utilizes the existing OusterDriver class with the SensorTins class. +* Added an new set of launch, param and main files for the TinsDriver. +* Updated readme and parameter file to explain usage of the TinsDriver implementation. +* Added some additional debug code to print the number of packets that have been accumulated into a cloud when it is published. +* Contibutors: Matthew Young (Trimble Inc). + +[0.3.0] Ported client changes for FW 2.0/2.1 (2021-06-23) ------------- **ouster_client** -* From [ouster_example #239](https://github.com/ouster-lidar/ouster_example/commit/e1176f427f68eb0807bb15ccabe34aea47a1c5d3). - * Ported `Optional` code: - * Optional header and license file are now present in their own folder: `include/ros2_ouster/client/optional-lite/` - * Ported some mode-getting functions that use Optional to types.cpp - * Ported the `sensor_config` struct - * Ported sensor_config struct to types.hpp - * Ported the get/set_config functions for sensor_config to client.cpp/.hpp - * Ported some documentation updates, and a != operator for LidarScan objects to lidar_scan.h. - * Ported/expanded data objects added by ouster_example #239 - * Ported several additional enumerated types (e.g. OperatingMode and MultipurposeIOMode) to types.h - * Ported parsing functions for additional enumerated types to types.cpp +* From ouster_example (`#239 `_). + * Optional header and license file are now present in their own folder: ``include/ros2_ouster/client/optional-lite/``. + * Ported some mode-getting functions that use Optional to types.cpp. + * Ported sensor_config struct to types.hpp. + * Ported the get/set_config functions for sensor_config to client.cpp/.hpp. + * Ported some documentation updates, and a != operator for LidarScan objects to lidar_scan.h. + * Ported several additional enumerated types (e.g. OperatingMode and MultipurposeIOMode) to types.h. + * Ported parsing functions for additional enumerated types to types.cpp. * Ported ColumnWindow definition and added it as a parameter to data_format struct, which is the only existing struct changed. - * In lidar_scan.cpp, ported some changes to `make_xyz_lut` and `cartesian` functions -* From [ouster_example/#246](https://github.com/ouster-lidar/ouster_example/commit/2b49e6a2f3dbd0462c557974a3f428915067fd2f) - * Removed a json include incorrectly added to types.h -* From [ouster_example #259](https://github.com/ouster-lidar/ouster_example/commit/b8b23c35d7b719d69341a438d386a801688aa6a4#diff-db793e44a91d87bc6e0d94870833d5d9eeb4420e2fbbec4e2468717a359a651f) - * Increased default timeout time from 30 to 60 seconds for `init_client` and `read_imu_packet` - * In netcompat.cpp, changed non_blocking_mode from off to on (0 to 1) if using Win32. - * Ported signal_multiplier parameter - * Added parameter to `sensor_config` in types.h - * Added signal_multiplier parameter setting to some functions in client.cpp and types.cpp -* Updated changelog + * In lidar_scan.cpp, ported some changes to ``make_xyz_lut`` and ``cartesian`` functions. +* From ouster_example (`#246 `_). + * Removed a json include incorrectly added to types.h. +* From ouster_example (`#259 `_). + * Increased default timeout time from 30 to 60 seconds for ``init_client`` and ``read_imu_packet``. + * In netcompat.cpp, changed non_blocking_mode from off to on (0 to 1) if using Win32. + * Added ``signal_multiplier`` parameter to ``sensor_config`` in types.h. + * Added ``signal_multiplier`` parameter setting to some functions in client.cpp and types.cpp. +* Updated changelog. **General Changes** diff --git a/README.md b/README.md index 51270ac..5c34048 100644 --- a/README.md +++ b/README.md @@ -36,21 +36,22 @@ See design doc in `design/*` directory [here](ros2_ouster/design/design_doc.md). | Service | Type | Description | |-------------------|-------------------------|-----------------------------------| | `reset` | std_srvs/Empty | Reset the sensor's connection | -| `GetMetadata` | ouster_msgs/GetMetadata | Get information about the sensor | +| `GetMetadata` | ouster_msgs/GetMetadata | Get information about the sensor. A optional filepath can be specified to save the metadata to a local file. | | Parameter | Type | Description | |--------------------------|---------|-------------------------------------------------------------------------------------------------------------| | `lidar_ip` | String | IP or hostname of lidar (ex. 10.5.5.87, os1-serialno.local) | -| `computer_ip` | String | IP or hostname of computer to get data (ex. 10.5.5.1) or broadcast (ex. 255.255.255.255) or "" for automatic detection | +| `computer_ip` | String | IP or hostname of computer to get data (ex. 10.5.5.1) or broadcast (ex. 255.255.255.255) or if using the default driver, "" for automatic detection | | `lidar_mode` | String | Mode of data capture, default `512x10` | | `imu_port` | int | Port of IMU data, default 7503 | | `lidar_port` | int | Port of laser data, default 7502 | | `sensor_frame` | String | TF frame of sensor, default `laser_sensor_frame` | | `laser_frame` | String | TF frame of laser data, default `laser_data_frame` | | `imu_frame` | String | TF frame of imu data, default `imu_data_frame` | +| `ethernet_device` | String | An ethernet device (e.g. eth0 or eno1) on which the Tins driver will listen for packets. Note that this is only a parameter for the Tins driver, and is only specified in the config file for that driver. | | | `use_system_default_qos` | bool | Publish data with default QoS for rosbag2 recording, default `False` | | `timestamp_mode` | String | Method used to timestamp measurements, default `TIME_FROM_INTERNAL_OSC` | -| `os1_proc_mask` | String | Mask encoding data processors to activate, default IMG | PCL | IMU | SCAN | +| `os1_proc_mask` | String | Mask encoding data processors to activate, default IMG | PCL | IMU | SCAN | @@ -283,19 +284,61 @@ lidar_ip: "fe80::be0f:a7ff:fe00:2861%eth2" computer_ip: "" ``` -### ROS Connection +Note that this feature is only available with the default driver version, configured by `driver_config.yaml`. When running the Tins-based driver (see the following sections), both the LiDAR and computer IP address must be specified in `tins_driver_config.yaml`. -Now that we have a connection over the network, lets view some data. After building your colcon workspace with this package, source the install space. Run +### Usage with the default driver + +Now that we have a connection over the network, lets view some data. After building your colcon workspace with this package, source the install space, then run: ``` -ros2 launch ros2_ouster os1_launch.py +ros2 launch ros2_ouster driver_launch.py ``` Make sure to update your parameters file if you don't use the default IPs (10.5.5.1, 10.5.5.87). You may also use the `.local` version of your ouster lidar. To find your IPs, see the `dnsmasq` output or check with `nmap -SP 10.5.5.*/24`. -An alternative tool is [avahi-browse](https://linux.die.net/man/1/avahi-browse) ``` +An alternative tool is [avahi-browse](https://linux.die.net/man/1/avahi-browse): + +``` avahi-browse -arlt ``` -Now that your connection is up (hopefully), you can view this information in RViz. Open an RViz session and subscribe to the points, images, and IMU topics in the laser frame. +Now that your connection is up, you can view this information in RViz. Open an RViz session and subscribe to the points, images, and IMU topics in the laser frame. When trying to visualize the point clouds, be sure to change the Fixed Frame under Global Options to "laser_data_frame" as this is the default parent frame of the point cloud headers. + +When the driver configures itself, it will automatically read the metadata parameters from the Ouster. If you wish to save these parameters and use them with captured data (see the next section) then you can save the data to a specific location using the `getMetadata` service that the driver offers. To use it, run the driver with a real Ouster LiDAR and then make the following service call: + +``` +ros2 service call /ouster_driver/get_metadata ouster_msgs/srv/GetMetadata "{metadata_filepath: "/path/to/your/metadata.json"}" +``` + +The driver will then save all the required metadata to the specified file and dump the same metadata as a JSON string to the terminal. Alternatively, the service can be called without specifying a filepath (see below) in which case no file will be saved, and the metadata will still be printed to terminal. Copying this string and manually saving it to a .json file is also a valid way to generate a metadata file. + +``` +ros2 service call /ouster_driver/get_metadata ouster_msgs/srv/GetMetadata +``` Have fun! + +### Usage with Tins-based driver + +If you want to use the driver to read data from a pcap file, you can use the `Tins`-based driver. To do this, open the `tins_driver_config.yaml` file and edit the following parameter: + +* `ethernet_device`: Change this to a working ethernet device on your computer that you plan to replay data through (e.g. "eth1"). + +You can run the Tins driver with the command below. This will use the default `ouster_os0128_1024_metadata.json` file: + +``` +ros2 launch ros2_ouster tins_driver_launch.py +``` + +Alternatively, you can change the metadata being used by specifying the metadata filepath as shown below. You can generate a metadata file using the `getMetadata` service as shown in the previous section. Or you can use one of the example metadata files provided, which come from an Ouster OS0-128 in either 1024x10 or 2048x10 mode. + +``` +ros2 launch ros2_ouster tins_driver_launch.py metadata_filepath:=/path/to/metadata.json +``` + +After launching the driver, in a new terminal, you can replay a pcap file of recorded ouster data using the following command (as an example): + +``` +sudo tcpreplay --intf1=eth1 saved_ouster_data.pcap +``` + +You may need to run this command with `sudo`. Note that this driver version will also work with a live Ouster sensor, provided the data is coming into the correct ethernet device, and the parameters in the metadata file match those of the sensor. However it is recommended that you run the default drive with a real sensor, as this will guarantee that the metadata settings are correct. \ No newline at end of file diff --git a/ouster_msgs/srv/GetMetadata.srv b/ouster_msgs/srv/GetMetadata.srv index 3dd9f82..8e9d1ca 100644 --- a/ouster_msgs/srv/GetMetadata.srv +++ b/ouster_msgs/srv/GetMetadata.srv @@ -1,3 +1,4 @@ +string metadata_filepath --- ouster_msgs/Metadata metadata diff --git a/ros2_ouster/CMakeLists.txt b/ros2_ouster/CMakeLists.txt index 6cda34e..2ef64ea 100644 --- a/ros2_ouster/CMakeLists.txt +++ b/ros2_ouster/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 3.5) project(ros2_ouster) +# Default to compiling in Release mode. If not compiled in Release mode the +# driver WILL fail to hit its desired output publish rate. if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) set(CMAKE_BUILD_TYPE Release) endif() @@ -19,7 +21,6 @@ find_package(tf2_ros REQUIRED) find_package(pcl_conversions REQUIRED) find_package(ouster_msgs REQUIRED) find_package(PCL REQUIRED COMPONENTS common) - find_package(jsoncpp REQUIRED) include_directories( @@ -27,7 +28,11 @@ include_directories( ${PCL_INCLUDE_DIRS} ) +# Create separate executables for the default and Tins drivers. set(executable_name ouster_driver) +set(tins_executable_name ouster_tins_driver) + +# Create a single library target to use for both default and Tins driver. set(library_name ${executable_name}_core) set(dependencies @@ -50,6 +55,7 @@ add_library(${library_name} SHARED src/ouster_driver.cpp src/lifecycle_interface.cpp src/sensor.cpp + src/sensor_tins.cpp src/client/client.cpp src/client/lidar_scan.cpp src/client/netcompat.cpp @@ -62,19 +68,27 @@ ament_target_dependencies(${library_name} target_link_libraries(${library_name} jsoncpp_lib + tins ${PCL_LIBRARIES} ) add_executable(${executable_name} src/main.cpp ) +add_executable(${tins_executable_name} + src/main_tins.cpp +) target_link_libraries(${executable_name} ${library_name}) +target_link_libraries(${tins_executable_name} ${library_name}) + +rclcpp_components_register_nodes(${library_name} "${PROJECT_NAME}::Driver") +rclcpp_components_register_nodes(${library_name} "${PROJECT_NAME}::TinsDriver") -rclcpp_components_register_nodes(ouster_driver_core "${PROJECT_NAME}::Driver") set(node_plugins "${node_plugins}${PROJECT_NAME}::Driver;$\n") +set(node_plugins "${node_plugins}${PROJECT_NAME}::TinsDriver;$\n") -install(TARGETS ${executable_name} ${library_name} +install(TARGETS ${executable_name} ${tins_executable_name} ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME} @@ -100,4 +114,4 @@ endif() ament_export_include_directories(include) ament_export_libraries(${library_name}) ament_export_dependencies(${dependencies}) -ament_package() +ament_package() \ No newline at end of file diff --git a/ros2_ouster/include/ros2_ouster/driver_types.hpp b/ros2_ouster/include/ros2_ouster/driver_types.hpp index 9f6eaaa..7e9da71 100644 --- a/ros2_ouster/include/ros2_ouster/driver_types.hpp +++ b/ros2_ouster/include/ros2_ouster/driver_types.hpp @@ -19,12 +19,33 @@ namespace ros2_ouster { +/** + * @brief The default Ouster driver for connecting to, configuring, and reading + * data from a real Ouster LiDAR. + */ class Driver : public OusterDriver { public: explicit Driver(rclcpp::NodeOptions options); }; + +/** + * @brief An alternative Ouster driver based on the open-source Tins library. + * This driver listens to an ethernet interface for packets with + * matching metadata, so it can be used with data captured in a pcap file + * and replayed after the fact. + * @details This driver differs from the implementation of Driver only in + * that it is initialized with, and uses the SensorTins class instead + * of the Sensor class. The utilization of the data accumulators and + * processor classes is otherwise the same. + */ +class TinsDriver : public OusterDriver +{ +public: + explicit TinsDriver(rclcpp::NodeOptions options); +}; + } // namespace ros2_ouster diff --git a/ros2_ouster/include/ros2_ouster/full_rotation_accumulator.hpp b/ros2_ouster/include/ros2_ouster/full_rotation_accumulator.hpp index a02dd86..9a97bd4 100644 --- a/ros2_ouster/include/ros2_ouster/full_rotation_accumulator.hpp +++ b/ros2_ouster/include/ros2_ouster/full_rotation_accumulator.hpp @@ -84,8 +84,11 @@ class FullRotationAccumulator { if (_batchReady) { _batchReady = false; + _packets_accumulated = 0; } + _packets_accumulated++; + if (_batch->operator()(data, *_ls)) { auto h = std::find_if( _ls->headers.begin(), _ls->headers.end(), [](const auto & h) { @@ -98,12 +101,19 @@ class FullRotationAccumulator } } + uint64_t getPacketsAccumulated() + { + return _packets_accumulated; + } + private: bool _batchReady; std::chrono::nanoseconds _timestamp; std::unique_ptr _batch; std::shared_ptr _ls; ouster::sensor::packet_format _pf; + + uint64_t _packets_accumulated = 0; }; } // namespace sensor diff --git a/ros2_ouster/include/ros2_ouster/interfaces/configuration.hpp b/ros2_ouster/include/ros2_ouster/interfaces/configuration.hpp index 32f7751..d1d3db9 100644 --- a/ros2_ouster/include/ros2_ouster/interfaces/configuration.hpp +++ b/ros2_ouster/include/ros2_ouster/interfaces/configuration.hpp @@ -29,6 +29,8 @@ struct Configuration int lidar_port; std::string lidar_mode; std::string timestamp_mode; + std::string metadata_filepath; + std::string ethernet_device; }; } // namespace ros2_ouster diff --git a/ros2_ouster/include/ros2_ouster/interfaces/metadata.hpp b/ros2_ouster/include/ros2_ouster/interfaces/metadata.hpp index 31a74bb..4343192 100644 --- a/ros2_ouster/include/ros2_ouster/interfaces/metadata.hpp +++ b/ros2_ouster/include/ros2_ouster/interfaces/metadata.hpp @@ -16,10 +16,10 @@ #include #include +#include #include "ros2_ouster/client/types.h" #include "ros2_ouster/client/version.h" #include "ros2_ouster/client/client.h" -#include namespace ros2_ouster { diff --git a/ros2_ouster/include/ros2_ouster/interfaces/sensor_interface.hpp b/ros2_ouster/include/ros2_ouster/interfaces/sensor_interface.hpp index 9ddbe7c..234ecbc 100644 --- a/ros2_ouster/include/ros2_ouster/interfaces/sensor_interface.hpp +++ b/ros2_ouster/include/ros2_ouster/interfaces/sensor_interface.hpp @@ -54,14 +54,20 @@ class SensorInterface /** * @brief Reset lidar sensor * @param configuration file to use + * @param node pointer to the driver node, which provides access to ROS params */ - virtual void reset(const ros2_ouster::Configuration & config) = 0; + virtual void reset( + ros2_ouster::Configuration & config, + rclcpp_lifecycle::LifecycleNode::SharedPtr node) = 0; /** * @brief Configure lidar sensor * @param configuration file to use + * @param node pointer to the driver node, which provides access to ROS params */ - virtual void configure(const ros2_ouster::Configuration & config) = 0; + virtual void configure( + ros2_ouster::Configuration & config, + rclcpp_lifecycle::LifecycleNode::SharedPtr node) = 0; /** * @brief Ask sensor to get its current state for data collection diff --git a/ros2_ouster/include/ros2_ouster/ouster_driver.hpp b/ros2_ouster/include/ros2_ouster/ouster_driver.hpp index 2966adc..aa538cf 100644 --- a/ros2_ouster/include/ros2_ouster/ouster_driver.hpp +++ b/ros2_ouster/include/ros2_ouster/ouster_driver.hpp @@ -17,6 +17,8 @@ #include #include #include +#include +#include #include "ros2_ouster/conversions.hpp" diff --git a/ros2_ouster/include/ros2_ouster/processors/image_processor.hpp b/ros2_ouster/include/ros2_ouster/processors/image_processor.hpp index ab0be8f..3fc0049 100644 --- a/ros2_ouster/include/ros2_ouster/processors/image_processor.hpp +++ b/ros2_ouster/include/ros2_ouster/processors/image_processor.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include "rclcpp/qos.hpp" @@ -128,7 +129,6 @@ class ImageProcessor : public ros2_ouster::DataProcessorInterface for (size_t u = 0; u < _height; u++) { for (size_t v = 0; v < _width; v++) { - const size_t vv = (v + _width - _px_offset[u]) % _width; const size_t index = u * _width + vv; @@ -220,7 +220,8 @@ class ImageProcessor : public ros2_ouster::DataProcessorInterface size_t _bit_depth = 8 * sizeof(uint8_t); const ouster::sensor::packet_format & _pf; const size_t _pixel_value_max = std::numeric_limits::max(); - double _range_multiplier = ouster::sensor::range_unit * (1.0 / 200.0); // assuming 200 m range typical + // assuming 200 m range typical + double _range_multiplier = ouster::sensor::range_unit * (1.0 / 200.0); viz::AutoExposure _ambient_ae, _intensity_ae; viz::BeamUniformityCorrector _ambient_buc; ouster::LidarScan _ls; diff --git a/ros2_ouster/include/ros2_ouster/processors/pointcloud_processor.hpp b/ros2_ouster/include/ros2_ouster/processors/pointcloud_processor.hpp index 75d5d12..0588dd9 100644 --- a/ros2_ouster/include/ros2_ouster/processors/pointcloud_processor.hpp +++ b/ros2_ouster/include/ros2_ouster/processors/pointcloud_processor.hpp @@ -95,6 +95,11 @@ class PointcloudProcessor : public ros2_ouster::DataProcessorInterface _fullRotationAccumulator->getTimestamp(), _frame, override_ts)); + RCLCPP_DEBUG( + _node->get_logger(), + "\n\nCloud published with %s packets\n", + std::to_string(_fullRotationAccumulator->getPacketsAccumulated()).c_str()); + return true; } diff --git a/ros2_ouster/include/ros2_ouster/processors/processor_factories.hpp b/ros2_ouster/include/ros2_ouster/processors/processor_factories.hpp index b69abdb..7c55801 100644 --- a/ros2_ouster/include/ros2_ouster/processors/processor_factories.hpp +++ b/ros2_ouster/include/ros2_ouster/processors/processor_factories.hpp @@ -11,13 +11,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef ROS2_OUSTER__PROCESSOR_FACTORIES_HPP_ -#define ROS2_OUSTER__PROCESSOR_FACTORIES_HPP_ +#ifndef ROS2_OUSTER__PROCESSORS__PROCESSOR_FACTORIES_HPP_ +#define ROS2_OUSTER__PROCESSORS__PROCESSOR_FACTORIES_HPP_ #include #include #include #include +#include #include "image_processor.hpp" #include "imu_processor.hpp" @@ -193,4 +194,4 @@ inline std::multimap +#include "rclcpp/rclcpp.hpp" + +namespace ros2_ouster +{ + +/** + * @brief Declares static ROS2 parameter and sets it to a given value if it was + * not already declared. + * @details This utility function is borrowed from the navigation2 package. + * + * @param[in] node_ptr A node in which given parameter to be declared + * @param[in] param_name The name of parameter + */ +template +void declare_parameter_if_not_declared( + NodeTypePtr node_ptr, + const std::string & param_name) +{ + if (!node_ptr->has_parameter(param_name)) { + node_ptr->declare_parameter(param_name); + } +} + + +/** + * @brief Declares static ROS2 parameter and sets it to a given value if it was + * not already declared. + * @details This utility function is borrowed from the navigation2 package. + * + * @param[in] node_ptr A node in which given parameter to be declared + * @param[in] param_name The name of parameter + * @param[in] default_value Parameter value to initialize with + * @param[in] parameter_descriptor Parameter descriptor (optional) + */ +template +void declare_parameter_if_not_declared( + NodeTypePtr node_ptr, + const std::string & param_name, + const rclcpp::ParameterValue & default_value, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = + rcl_interfaces::msg::ParameterDescriptor()) +{ + if (!node_ptr->has_parameter(param_name)) { + node_ptr->declare_parameter(param_name, default_value, parameter_descriptor); + } +} + +} // namespace ros2_ouster + +#endif // ROS2_OUSTER__ROS2_UTILS_HPP_ diff --git a/ros2_ouster/include/ros2_ouster/sensor.hpp b/ros2_ouster/include/ros2_ouster/sensor.hpp index 8d4601c..d503d07 100644 --- a/ros2_ouster/include/ros2_ouster/sensor.hpp +++ b/ros2_ouster/include/ros2_ouster/sensor.hpp @@ -16,12 +16,14 @@ #include #include +#include #include "ros2_ouster/processors/processor_factories.hpp" #include "ros2_ouster/interfaces/data_processor_interface.hpp" #include "ros2_ouster/interfaces/sensor_interface.hpp" #include "ros2_ouster/client/client.h" +#include "ros2_ouster/ros2_utils.hpp" namespace sensor { @@ -36,14 +38,20 @@ class Sensor : public ros2_ouster::SensorInterface /** * @brief Reset lidar sensor * @param configuration file to use + * @param node pointer to the driver node, which provides access to ROS params */ - void reset(const ros2_ouster::Configuration & config) override; + void reset( + ros2_ouster::Configuration & config, + rclcpp_lifecycle::LifecycleNode::SharedPtr node) override; /** * @brief Configure lidar sensor * @param configuration file to use + * @param node pointer to the driver node, which provides access to ROS params */ - void configure(const ros2_ouster::Configuration & config) override; + void configure( + ros2_ouster::Configuration & config, + rclcpp_lifecycle::LifecycleNode::SharedPtr node) override; /** * @brief Get lidar sensor's metadata diff --git a/ros2_ouster/include/ros2_ouster/sensor_tins.hpp b/ros2_ouster/include/ros2_ouster/sensor_tins.hpp new file mode 100644 index 0000000..815f176 --- /dev/null +++ b/ros2_ouster/include/ros2_ouster/sensor_tins.hpp @@ -0,0 +1,188 @@ +// Copyright 2021, Matthew Young (Trimble Inc) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROS2_OUSTER__SENSOR_TINS_HPP_ +#define ROS2_OUSTER__SENSOR_TINS_HPP_ + +// Libtins includes +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include "ros2_ouster/processors/processor_factories.hpp" +#include "ros2_ouster/interfaces/data_processor_interface.hpp" +#include "ros2_ouster/interfaces/sensor_interface.hpp" +#include "ros2_ouster/client/client.h" +#include "ros2_ouster/ros2_utils.hpp" + +namespace sensor +{ + +class SensorTins : public ros2_ouster::SensorInterface +{ +public: + /** + * @brief Default constructor + */ + SensorTins(); + + /** + * @brief Default destructor + */ + ~SensorTins(); + + /** + * @brief Reset lidar sensor + * @param configuration file to use + * @param node pointer to the driver node, which provides access to ROS params + */ + void reset( + ros2_ouster::Configuration & config, + rclcpp_lifecycle::LifecycleNode::SharedPtr node) override; + + /** + * @brief Configure lidar sensor + * @param configuration file to use + * @param node pointer to the driver node, which provides access to ROS params + */ + void configure( + ros2_ouster::Configuration & config, + rclcpp_lifecycle::LifecycleNode::SharedPtr node) override; + + /** + * @brief Get lidar sensor's metadata + * @return sensor metadata struct + */ + ros2_ouster::Metadata getMetadata() override; + + /** + * @brief Ask sensor to get its current state for data collection + * @return the state enum value + */ + ouster::sensor::client_state get() override; + + /** + * @brief reading a lidar packet + * @param state of the sensor + * @return the packet of data + */ + uint8_t * readLidarPacket( + const ouster::sensor::client_state & state) override; + + /** + * @brief reading an imu packet + * @param state of the sensor + * @return the packet of data + */ + uint8_t * readImuPacket( + const ouster::sensor::client_state & state) override; + + /** + * @brief Sets the metadata class variable + * @param lidar_port + * @param imu_port + */ + void setMetadata( + int lidar_port, + int imu_port, + const std::string & timestamp_mode); + + /** + * @brief Get lidar sensor's packet format + * @return packet format struct + */ + ouster::sensor::packet_format getPacketFormat() override; + + /** + * @brief Load metadata from a file. + * @details Some important notes about this function: This populates an + * ouster::sensor::sensor_info object, but the more commonly used + * ros2_ouster::Metadata object has some additional parameters + * that must be sourced elsewhere. Also note that the underlying + * Ouster library is inconsistent in what parameters must be + * provided in the JSON file vs what will be silently set to a + * default value if not provided. + * @param[in] filepath_to_read A fully qualified filepath to a yaml file + * containing the parameters to load + * @param[out] sensor_info The metadata to load data into. + */ + void loadSensorInfoFromJsonFile( + const std::string filepath_to_read, + ouster::sensor::sensor_info & sensor_info); + + /** + * @brief Initializes the internal Tins::Sniffer object. + * @param[in] eth_device The name of the ethernet device for the sniffer to + * listen to. This is typically a device like "eth0", "eth1" or + * "eno0" + */ + void initializeSniffer(const std::string eth_device); + + /** + * @brief sniff for one packet and one packet only. If a valid LiDAR or IMU + * packet is found, it will set the internal _replay_state to + * client_state::LIDAR_DATA or client_state::IMU_DATA + * @details This function is intended to only be used as a callback for the + * sniffer->sniff_loop function + * @param[in] packet The input packet from the tins sniffer to parse. + * @returns False if the packet is an Ouster LiDAR or IMU packet, in which + * case we don't want to continue sniffing, and want sniff_loop to + * stop. True if the packets aren't good and we need to continue + * looking. + */ + bool sniffOnePacket(Tins::Packet & packet); + +private: + /** + * the client is here because it needs to be, but is not actually used + * in this implementation of the SensorInterface class. + */ + std::shared_ptr _ouster_client; + + /** Internal storage for LiDAR data */ + std::vector _lidar_packet; + + /** Internal storage for IMU data */ + std::vector _imu_packet; + + /** Metadata for the sensor */ + ros2_ouster::Metadata _metadata{}; + + /** Driver configuration */ + ros2_ouster::Configuration _driver_config; + + /** + * The inferred state of the LiDAR, determined by the most recently + * received packet instead of requested from the real LiDAR + */ + ouster::sensor::client_state _inferred_state; + + /** Tins sniffer object for listening to packets */ + std::unique_ptr _tins_sniffer_pointer; + + /** Tins reassembler for reconstructing fragmented packets */ + Tins::IPv4Reassembler _tins_ipv4_reassembler; + + /** Configuration for _tins_sniffer_pointer */ + Tins::SnifferConfiguration _sniffer_config; +}; + +} // namespace sensor + +#endif // ROS2_OUSTER__SENSOR_TINS_HPP_ diff --git a/ros2_ouster/launch/os1_launch.py b/ros2_ouster/launch/driver_launch.py similarity index 96% rename from ros2_ouster/launch/os1_launch.py rename to ros2_ouster/launch/driver_launch.py index f58f57b..82359fe 100755 --- a/ros2_ouster/launch/os1_launch.py +++ b/ros2_ouster/launch/driver_launch.py @@ -36,9 +36,10 @@ def generate_launch_description(): parameter_file = LaunchConfiguration('params_file') node_name = 'ouster_driver' + # Acquire the driver param file params_declare = DeclareLaunchArgument('params_file', default_value=os.path.join( - share_dir, 'params', 'sensor.yaml'), + share_dir, 'params', 'driver_config.yaml'), description='FPath to the ROS2 parameters file to use.') driver_node = LifecycleNode(package='ros2_ouster', @@ -47,6 +48,7 @@ def generate_launch_description(): output='screen', emulate_tty=True, parameters=[parameter_file], + arguments=['--ros-args', '--log-level', 'INFO'], namespace='/', ) diff --git a/ros2_ouster/launch/tins_driver_launch.py b/ros2_ouster/launch/tins_driver_launch.py new file mode 100644 index 0000000..736edd3 --- /dev/null +++ b/ros2_ouster/launch/tins_driver_launch.py @@ -0,0 +1,105 @@ +#!/usr/bin/python3 +# Copyright 2020, Steve Macenski +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import EmitEvent +from launch.actions import RegisterEventHandler +from launch_ros.events.lifecycle import ChangeState +from launch_ros.events.lifecycle import matches_node_name +from launch_ros.event_handlers import OnStateTransition +from launch.actions import LogInfo +from launch.events import matches_action +from launch.event_handlers.on_shutdown import OnShutdown + +import lifecycle_msgs.msg +import os + + +def generate_launch_description(): + share_dir = get_package_share_directory('ros2_ouster') + parameter_file = LaunchConfiguration('params_file') + metadata_filepath = LaunchConfiguration('metadata_filepath') + node_name = 'ouster_driver' + + # Acquire the driver param file + params_declare = DeclareLaunchArgument('params_file', + default_value=os.path.join( + share_dir, 'params', 'tins_driver_config.yaml'), + description='FPath to the ROS2 parameters file to use.') + + # Acquire the metadata param file + metadata_declare = DeclareLaunchArgument('metadata_filepath', + default_value=os.path.join( + share_dir, 'params', 'ouster_os0128_1024_metadata.json'), + description='File for reading/writing sensor metadata to.') + + driver_node = LifecycleNode(package='ros2_ouster', + executable='ouster_tins_driver', + name=node_name, + output='screen', + emulate_tty=True, + parameters=[{'metadata_filepath' : metadata_filepath}, + parameter_file], + arguments=['--ros-args', '--log-level', 'INFO'], + namespace='/', + ) + + configure_event = EmitEvent( + event=ChangeState( + lifecycle_node_matcher=matches_action(driver_node), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE, + ) + ) + + activate_event = RegisterEventHandler( + OnStateTransition( + target_lifecycle_node=driver_node, goal_state='inactive', + entities=[ + LogInfo( + msg="[LifecycleLaunch] Ouster driver node is activating."), + EmitEvent(event=ChangeState( + lifecycle_node_matcher=matches_action(driver_node), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE, + )), + ], + ) + ) + + # TODO make lifecycle transition to shutdown before SIGINT + shutdown_event = RegisterEventHandler( + OnShutdown( + on_shutdown=[ + EmitEvent(event=ChangeState( + lifecycle_node_matcher=matches_node_name(node_name=node_name), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVE_SHUTDOWN, + )), + LogInfo( + msg="[LifecycleLaunch] Ouster driver node is exiting."), + ], + ) + ) + + return LaunchDescription([ + params_declare, + metadata_declare, + driver_node, + activate_event, + configure_event, + shutdown_event, + ]) diff --git a/ros2_ouster/package.xml b/ros2_ouster/package.xml index fdd03a7..ab5bf8a 100644 --- a/ros2_ouster/package.xml +++ b/ros2_ouster/package.xml @@ -25,6 +25,7 @@ launch_ros pcl_conversions libpcl-all + libtins-dev ament_lint_common ament_lint_auto diff --git a/ros2_ouster/params/sensor.yaml b/ros2_ouster/params/driver_config.yaml similarity index 95% rename from ros2_ouster/params/sensor.yaml rename to ros2_ouster/params/driver_config.yaml index 70badfa..cf9345c 100644 --- a/ros2_ouster/params/sensor.yaml +++ b/ros2_ouster/params/driver_config.yaml @@ -1,8 +1,8 @@ ouster_driver: ros__parameters: - lidar_ip: "10.5.5.96" - computer_ip: "10.5.5.1" - lidar_mode: "512x10" + lidar_ip: 10.5.5.96 + computer_ip: 10.5.5.1 + lidar_mode: "1024x10" imu_port: 7503 lidar_port: 7502 sensor_frame: laser_sensor_frame diff --git a/ros2_ouster/params/ouster_os0128_1024_metadata.json b/ros2_ouster/params/ouster_os0128_1024_metadata.json new file mode 100644 index 0000000..49bdc8d --- /dev/null +++ b/ros2_ouster/params/ouster_os0128_1024_metadata.json @@ -0,0 +1,451 @@ +{ + "beam_altitude_angles": + [ + 46.45, + 45.47, + 44.81, + 44.43, + 43.46, + 42.51, + 41.86, + 41.46, + 40.5, + 39.58, + 38.92, + 38.52, + 37.55, + 36.65, + 35.99, + 35.58, + 34.6, + 33.72, + 33.07, + 32.66, + 31.67, + 30.81, + 30.17, + 29.74, + 28.76, + 27.92, + 27.28, + 26.84, + 25.84, + 25.03, + 24.41, + 23.95, + 22.95, + 22.17, + 21.55, + 21.08, + 20.07, + 19.32, + 18.7, + 18.22, + 17.2, + 16.48, + 15.87, + 15.37, + 14.35, + 13.65, + 13.05, + 12.53, + 11.5, + 10.83, + 10.23, + 9.7, + 8.67, + 8.01, + 7.42, + 6.87, + 5.84, + 5.21, + 4.62, + 4.05, + 3.01, + 2.4, + 1.82, + 1.23, + 0.17, + -0.4, + -0.99, + -1.6, + -2.65, + -3.22, + -3.8, + -4.43, + -5.48, + -6.02, + -6.61, + -7.26, + -8.31, + -8.84, + -9.44, + -10.11, + -11.16, + -11.67, + -12.27, + -12.96, + -14.02, + -14.5, + -15.1, + -15.82, + -16.88, + -17.35, + -17.95, + -18.69, + -19.75, + -20.21, + -20.81, + -21.57, + -22.63, + -23.08, + -23.68, + -24.46, + -25.52, + -25.96, + -26.56, + -27.37, + -28.44, + -28.85, + -29.47, + -30.3, + -31.36, + -31.77, + -32.39, + -33.25, + -34.3, + -34.7, + -35.34, + -36.23, + -37.27, + -37.66, + -38.3, + -39.23, + -40.26, + -40.65, + -41.31, + -42.25, + -43.29, + -43.66, + -44.33, + -45.29 + ], + "beam_azimuth_angles": + [ + 12.15, + 4.79, + -2.45, + -9.61, + 11.65, + 4.56, + -2.42, + -9.34, + 11.19, + 4.35, + -2.41, + -9.11, + 10.79, + 4.16, + -2.39, + -8.91, + 10.43, + 3.98, + -2.39, + -8.74, + 10.11, + 3.82, + -2.4, + -8.6, + 9.84, + 3.69, + -2.41, + -8.5, + 9.59, + 3.56, + -2.42, + -8.41, + 9.37, + 3.44, + -2.44, + -8.34, + 9.17, + 3.35, + -2.47, + -8.3, + 9, + 3.24, + -2.5, + -8.27, + 8.86, + 3.16, + -2.55, + -8.26, + 8.73, + 3.07, + -2.58, + -8.26, + 8.62, + 3, + -2.63, + -8.28, + 8.53, + 2.93, + -2.68, + -8.32, + 8.45, + 2.86, + -2.74, + -8.37, + 8.38, + 2.81, + -2.79, + -8.43, + 8.34, + 2.74, + -2.86, + -8.51, + 8.3, + 2.7, + -2.94, + -8.61, + 8.28, + 2.65, + -3.02, + -8.72, + 8.27, + 2.6, + -3.1, + -8.84, + 8.28, + 2.56, + -3.2, + -9, + 8.29, + 2.51, + -3.3, + -9.16, + 8.32, + 2.48, + -3.41, + -9.35, + 8.38, + 2.45, + -3.53, + -9.56, + 8.45, + 2.43, + -3.66, + -9.8, + 8.54, + 2.4, + -3.8, + -10.08, + 8.65, + 2.38, + -3.96, + -10.39, + 8.78, + 2.36, + -4.14, + -10.75, + 8.95, + 2.36, + -4.35, + -11.16, + 9.16, + 2.35, + -4.58, + -11.63, + 9.39, + 2.35, + -4.84, + -12.17 + ], + "build_rev": "v2.0.0", + "data_format": + { + "column_window": + [ + 0, + 0 + ], + "columns_per_frame": 1024, + "columns_per_packet": 16, + "pixel_shift_by_row": + [ + 70, + 49, + 28, + 8, + 68, + 48, + 28, + 8, + 67, + 47, + 28, + 9, + 66, + 47, + 28, + 10, + 65, + 46, + 28, + 10, + 64, + 46, + 28, + 11, + 63, + 45, + 28, + 11, + 62, + 45, + 28, + 11, + 62, + 45, + 28, + 11, + 61, + 45, + 28, + 11, + 61, + 44, + 28, + 11, + 60, + 44, + 28, + 12, + 60, + 44, + 28, + 12, + 60, + 44, + 28, + 11, + 59, + 43, + 27, + 11, + 59, + 43, + 27, + 11, + 59, + 43, + 27, + 11, + 59, + 43, + 27, + 11, + 59, + 43, + 27, + 11, + 59, + 43, + 26, + 10, + 59, + 42, + 26, + 10, + 59, + 42, + 26, + 9, + 59, + 42, + 26, + 9, + 59, + 42, + 25, + 8, + 59, + 42, + 25, + 8, + 59, + 42, + 25, + 7, + 59, + 42, + 24, + 6, + 60, + 42, + 24, + 5, + 60, + 42, + 23, + 4, + 60, + 42, + 23, + 3, + 61, + 42, + 22, + 2, + 62, + 42, + 21, + 0 + ], + "pixels_per_column": 128 + }, + "hostname": "192.168.90.2", + "imu_to_sensor_transform": + [ + 1, + 0, + 0, + 6.253, + 0, + 1, + 0, + -11.775, + 0, + 0, + 1, + 7.645, + 0, + 0, + 0, + 1 + ], + "json_calibration_version": 3, + "lidar_mode": "1024x10", + "lidar_origin_to_beam_origin_mm": 27.67, + "lidar_to_sensor_transform": + [ + -1, + 0, + 0, + 0, + 0, + -1, + 0, + 0, + 0, + 0, + 1, + 36.18, + 0, + 0, + 0, + 1 + ], + "prod_line": "OS-0-128", + "prod_sn": "992109000340" +} \ No newline at end of file diff --git a/ros2_ouster/params/ouster_os0128_2048_metadata.json b/ros2_ouster/params/ouster_os0128_2048_metadata.json new file mode 100644 index 0000000..9ee8e3b --- /dev/null +++ b/ros2_ouster/params/ouster_os0128_2048_metadata.json @@ -0,0 +1,451 @@ +{ + "beam_altitude_angles": + [ + 46.45, + 45.47, + 44.81, + 44.43, + 43.46, + 42.51, + 41.86, + 41.46, + 40.5, + 39.58, + 38.92, + 38.52, + 37.55, + 36.65, + 35.99, + 35.58, + 34.6, + 33.72, + 33.07, + 32.66, + 31.67, + 30.81, + 30.17, + 29.74, + 28.76, + 27.92, + 27.28, + 26.84, + 25.84, + 25.03, + 24.41, + 23.95, + 22.95, + 22.17, + 21.55, + 21.08, + 20.07, + 19.32, + 18.7, + 18.22, + 17.2, + 16.48, + 15.87, + 15.37, + 14.35, + 13.65, + 13.05, + 12.53, + 11.5, + 10.83, + 10.23, + 9.7, + 8.67, + 8.01, + 7.42, + 6.87, + 5.84, + 5.21, + 4.62, + 4.05, + 3.01, + 2.4, + 1.82, + 1.23, + 0.17, + -0.4, + -0.99, + -1.6, + -2.65, + -3.22, + -3.8, + -4.43, + -5.48, + -6.02, + -6.61, + -7.26, + -8.31, + -8.84, + -9.44, + -10.11, + -11.16, + -11.67, + -12.27, + -12.96, + -14.02, + -14.5, + -15.1, + -15.82, + -16.88, + -17.35, + -17.95, + -18.69, + -19.75, + -20.21, + -20.81, + -21.57, + -22.63, + -23.08, + -23.68, + -24.46, + -25.52, + -25.96, + -26.56, + -27.37, + -28.44, + -28.85, + -29.47, + -30.3, + -31.36, + -31.77, + -32.39, + -33.25, + -34.3, + -34.7, + -35.34, + -36.23, + -37.27, + -37.66, + -38.3, + -39.23, + -40.26, + -40.65, + -41.31, + -42.25, + -43.29, + -43.66, + -44.33, + -45.29 + ], + "beam_azimuth_angles": + [ + 12.15, + 4.79, + -2.45, + -9.61, + 11.65, + 4.56, + -2.42, + -9.34, + 11.19, + 4.35, + -2.41, + -9.11, + 10.79, + 4.16, + -2.39, + -8.91, + 10.43, + 3.98, + -2.39, + -8.74, + 10.11, + 3.82, + -2.4, + -8.6, + 9.84, + 3.69, + -2.41, + -8.5, + 9.59, + 3.56, + -2.42, + -8.41, + 9.37, + 3.44, + -2.44, + -8.34, + 9.17, + 3.35, + -2.47, + -8.3, + 9, + 3.24, + -2.5, + -8.27, + 8.86, + 3.16, + -2.55, + -8.26, + 8.73, + 3.07, + -2.58, + -8.26, + 8.62, + 3, + -2.63, + -8.28, + 8.53, + 2.93, + -2.68, + -8.32, + 8.45, + 2.86, + -2.74, + -8.37, + 8.38, + 2.81, + -2.79, + -8.43, + 8.34, + 2.74, + -2.86, + -8.51, + 8.3, + 2.7, + -2.94, + -8.61, + 8.28, + 2.65, + -3.02, + -8.72, + 8.27, + 2.6, + -3.1, + -8.84, + 8.28, + 2.56, + -3.2, + -9, + 8.29, + 2.51, + -3.3, + -9.16, + 8.32, + 2.48, + -3.41, + -9.35, + 8.38, + 2.45, + -3.53, + -9.56, + 8.45, + 2.43, + -3.66, + -9.8, + 8.54, + 2.4, + -3.8, + -10.08, + 8.65, + 2.38, + -3.96, + -10.39, + 8.78, + 2.36, + -4.14, + -10.75, + 8.95, + 2.36, + -4.35, + -11.16, + 9.16, + 2.35, + -4.58, + -11.63, + 9.39, + 2.35, + -4.84, + -12.17 + ], + "build_rev": "v2.0.0", + "data_format": + { + "column_window": + [ + 0, + 0 + ], + "columns_per_frame": 2048, + "columns_per_packet": 16, + "pixel_shift_by_row": + [ + 138, + 96, + 55, + 14, + 135, + 95, + 55, + 16, + 133, + 94, + 55, + 17, + 130, + 93, + 55, + 18, + 128, + 92, + 55, + 19, + 127, + 91, + 55, + 20, + 125, + 90, + 55, + 21, + 124, + 89, + 55, + 21, + 122, + 89, + 55, + 22, + 121, + 88, + 55, + 22, + 120, + 87, + 55, + 22, + 119, + 87, + 54, + 22, + 119, + 86, + 54, + 22, + 118, + 86, + 54, + 22, + 118, + 86, + 54, + 22, + 117, + 85, + 53, + 21, + 117, + 85, + 53, + 21, + 116, + 85, + 53, + 21, + 116, + 84, + 52, + 20, + 116, + 84, + 52, + 19, + 116, + 84, + 51, + 19, + 116, + 84, + 51, + 18, + 116, + 83, + 50, + 17, + 116, + 83, + 50, + 16, + 117, + 83, + 49, + 15, + 117, + 83, + 48, + 13, + 118, + 83, + 47, + 12, + 118, + 83, + 46, + 10, + 119, + 82, + 45, + 8, + 120, + 82, + 44, + 6, + 121, + 82, + 43, + 3, + 122, + 82, + 41, + 0 + ], + "pixels_per_column": 128 + }, + "hostname": "192.168.90.2", + "imu_to_sensor_transform": + [ + 1, + 0, + 0, + 6.253, + 0, + 1, + 0, + -11.775, + 0, + 0, + 1, + 7.645, + 0, + 0, + 0, + 1 + ], + "json_calibration_version": 3, + "lidar_mode": "2048x10", + "lidar_origin_to_beam_origin_mm": 27.67, + "lidar_to_sensor_transform": + [ + -1, + 0, + 0, + 0, + 0, + -1, + 0, + 0, + 0, + 0, + 1, + 36.18, + 0, + 0, + 0, + 1 + ], + "prod_line": "OS-0-128", + "prod_sn": "992109000340" +} \ No newline at end of file diff --git a/ros2_ouster/params/tins_driver_config.yaml b/ros2_ouster/params/tins_driver_config.yaml new file mode 100644 index 0000000..975e929 --- /dev/null +++ b/ros2_ouster/params/tins_driver_config.yaml @@ -0,0 +1,56 @@ +ouster_driver: + ros__parameters: + lidar_ip: 10.5.5.96 + computer_ip: 10.5.5.1 + lidar_mode: "1024x10" + imu_port: 7503 + lidar_port: 7502 + sensor_frame: laser_sensor_frame + laser_frame: laser_data_frame + imu_frame: imu_data_frame + + # The ethernet device the packets are being broadcast through. Only used + # by the "tins" driver type. + ethernet_device: eth1 + + # if False, data are published with sensor data QoS. This is preferrable + # for production but default QoS is needed for rosbag. + # See: https://github.com/ros2/rosbag2/issues/125 + use_system_default_qos: True + + # Set the method used to timestamp measurements. + # Valid modes are: + # + # TIME_FROM_INTERNAL_OSC + # TIME_FROM_SYNC_PULSE_IN + # TIME_FROM_PTP_1588 + # TIME_FROM_ROS_RECEPTION + # + # (See this project's README and/or the Ouster Software Guide for more + # information). + # + timestamp_mode: TIME_FROM_INTERNAL_OSC + + # Mask-like-string used to define the data processors that should be + # activated upon startup of the driver. This will determine the topics + # that are available for client applications to consume. The defacto + # reference for these values are defined in: + # `include/ros2_ouster/processors/processor_factories.hpp` + # + # For convenience, the available data processors are: + # + # IMG - Provides 8-bit image topics suitable for ML applications encoding + # the range, ambient and intensity data from a scan + # PCL - Provides a point cloud encoding of a LiDAR scan + # IMU - Provides a data stream from the LiDARs integral IMU + # SCAN - Provides a synthesized 2D LaserScan from the 3D LiDAR data + # + # To construct a valid string for this parameter join the tokens from above + # (in any combination) with the pipe character. For example, valid strings + # include (but are not limited to): + # + # IMG|PCL + # IMG|PCL|IMU|SCAN + # PCL + # + proc_mask: IMG|PCL|IMU|SCAN diff --git a/ros2_ouster/src/driver_types.cpp b/ros2_ouster/src/driver_types.cpp index f0578c0..3c6b635 100644 --- a/ros2_ouster/src/driver_types.cpp +++ b/ros2_ouster/src/driver_types.cpp @@ -17,11 +17,16 @@ #include "rclcpp_components/register_node_macro.hpp" #include "ros2_ouster/driver_types.hpp" #include "ros2_ouster/sensor.hpp" +#include "ros2_ouster/sensor_tins.hpp" namespace ros2_ouster { ros2_ouster::Driver::Driver(rclcpp::NodeOptions options) : OusterDriver{std::make_unique(), options} {} + +ros2_ouster::TinsDriver::TinsDriver(rclcpp::NodeOptions options) +: OusterDriver{std::make_unique(), options} {} } RCLCPP_COMPONENTS_REGISTER_NODE(ros2_ouster::Driver) +RCLCPP_COMPONENTS_REGISTER_NODE(ros2_ouster::TinsDriver) diff --git a/ros2_ouster/src/lifecycle_interface.cpp b/ros2_ouster/src/lifecycle_interface.cpp index 3d8d1d4..49d32b6 100644 --- a/ros2_ouster/src/lifecycle_interface.cpp +++ b/ros2_ouster/src/lifecycle_interface.cpp @@ -27,7 +27,6 @@ LifecycleInterface::LifecycleInterface( CallbackReturn LifecycleInterface::on_configure(const rclcpp_lifecycle::State & state) { - RCLCPP_INFO(this->get_logger(), "Configuring Ouster driver node."); onConfigure(); return CallbackReturn::SUCCESS; } diff --git a/ros2_ouster/src/main.cpp b/ros2_ouster/src/main.cpp index 747c1ac..d7d0172 100644 --- a/ros2_ouster/src/main.cpp +++ b/ros2_ouster/src/main.cpp @@ -20,6 +20,7 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); auto options = rclcpp::NodeOptions(); + auto node = std::make_shared(options); rclcpp::spin(node->get_node_base_interface()); diff --git a/ros2_ouster/src/main_tins.cpp b/ros2_ouster/src/main_tins.cpp new file mode 100644 index 0000000..f2cfefb --- /dev/null +++ b/ros2_ouster/src/main_tins.cpp @@ -0,0 +1,30 @@ +// Copyright 2021, Steve Macenski +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "rclcpp/rclcpp.hpp" +#include "ros2_ouster/driver_types.hpp" + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto options = rclcpp::NodeOptions(); + + auto node = std::make_shared(options); + + rclcpp::spin(node->get_node_base_interface()); + + rclcpp::shutdown(); + return 0; +} diff --git a/ros2_ouster/src/ouster_driver.cpp b/ros2_ouster/src/ouster_driver.cpp index 75342be..0a5bf74 100644 --- a/ros2_ouster/src/ouster_driver.cpp +++ b/ros2_ouster/src/ouster_driver.cpp @@ -24,6 +24,8 @@ #include "ros2_ouster/ouster_driver.hpp" #include "ros2_ouster/processors/processor_factories.hpp" #include "ros2_ouster/client/types.h" +#include "ros2_ouster/sensor.hpp" +#include "ros2_ouster/sensor_tins.hpp" namespace ros2_ouster { @@ -38,29 +40,44 @@ OusterDriver::OusterDriver( const rclcpp::NodeOptions & options) : LifecycleInterface("OusterDriver", options), _sensor{std::move(sensor)} { - this->declare_parameter("lidar_ip"); - this->declare_parameter("computer_ip"); - this->declare_parameter("imu_port", 7503); - this->declare_parameter("lidar_port", 7502); - this->declare_parameter("lidar_mode", std::string("512x10")); - this->declare_parameter( - "timestamp_mode", std::string("TIME_FROM_INTERNAL_OSC")); + // Declare parameters for configuring the _driver_ this->declare_parameter("sensor_frame", std::string("laser_sensor_frame")); this->declare_parameter("laser_frame", std::string("laser_data_frame")); this->declare_parameter("imu_frame", std::string("imu_data_frame")); this->declare_parameter("use_system_default_qos", false); this->declare_parameter("proc_mask", std::string("IMG|PCL|IMU|SCAN")); + + // Declare parameters used across ALL _sensor_ implementations + this->declare_parameter("lidar_ip"); + this->declare_parameter("computer_ip"); + this->declare_parameter("imu_port", 7503); + this->declare_parameter("lidar_port", 7502); + this->declare_parameter("lidar_mode", std::string("512x10")); + this->declare_parameter("timestamp_mode", std::string("TIME_FROM_INTERNAL_OSC")); } OusterDriver::~OusterDriver() = default; void OusterDriver::onConfigure() { - RCLCPP_INFO( - this->get_logger(), - "This driver is compatible with sensors running fw 2.x."); + // Get parameters for configuring the _driver_ + _laser_sensor_frame = get_parameter("sensor_frame").as_string(); + _laser_data_frame = get_parameter("laser_frame").as_string(); + _imu_data_frame = get_parameter("imu_frame").as_string(); + _use_system_default_qos = get_parameter("use_system_default_qos").as_bool(); + _proc_mask = ros2_ouster::toProcMask(get_parameter("proc_mask").as_string()); + // Get parameters used across ALL _sensor_ implementations. Parameters unique + // a specific Sensor implementation are "getted" in the configure() function + // for that sensor. ros2_ouster::Configuration lidar_config; + lidar_config.imu_port = this->get_parameter("imu_port").as_int(); + lidar_config.lidar_port = this->get_parameter("lidar_port").as_int(); + lidar_config.lidar_mode = this->get_parameter("lidar_mode").as_string(); + lidar_config.timestamp_mode = this->get_parameter("timestamp_mode").as_string(); + + // Deliberately retrieve the IP parameters in a try block without defaults, as + // we cannot estimate a reasonable default IP address for the LiDAR/computer. try { lidar_config.lidar_ip = get_parameter("lidar_ip").as_string(); lidar_config.computer_ip = get_parameter("computer_ip").as_string(); @@ -72,11 +89,6 @@ void OusterDriver::onConfigure() exit(-1); } - lidar_config.imu_port = get_parameter("imu_port").as_int(); - lidar_config.lidar_port = get_parameter("lidar_port").as_int(); - lidar_config.lidar_mode = get_parameter("lidar_mode").as_string(); - lidar_config.timestamp_mode = get_parameter("timestamp_mode").as_string(); - if (lidar_config.timestamp_mode == "TIME_FROM_ROS_RECEPTION") { RCLCPP_WARN( this->get_logger(), @@ -88,39 +100,23 @@ void OusterDriver::onConfigure() _use_ros_time = false; } - _laser_sensor_frame = get_parameter("sensor_frame").as_string(); - _laser_data_frame = get_parameter("laser_frame").as_string(); - _imu_data_frame = get_parameter("imu_frame").as_string(); - _use_system_default_qos = get_parameter("use_system_default_qos").as_bool(); - - _proc_mask = - ros2_ouster::toProcMask(get_parameter("proc_mask").as_string()); + // Configure the driver and sensor + try { + _sensor->configure(lidar_config, shared_from_this()); + } catch (const OusterDriverException & e) { + RCLCPP_FATAL(this->get_logger(), "Exception thrown: (%s)", e.what()); + exit(-1); + } RCLCPP_INFO( this->get_logger(), - "Connecting to sensor at %s.", lidar_config.lidar_ip.c_str()); + "This driver is compatible with sensors running fw 2.x."); - if (lidar_config.computer_ip == "") { - RCLCPP_INFO( - this->get_logger(), - "Sending data from sensor to computer using automatic address detection"); - } else { - RCLCPP_INFO( - this->get_logger(), - "Sending data from sensor to %s.", lidar_config.computer_ip.c_str()); - } _reset_srv = this->create_service( "~/reset", std::bind(&OusterDriver::resetService, this, _1, _2, _3)); _metadata_srv = this->create_service( "~/get_metadata", std::bind(&OusterDriver::getMetadata, this, _1, _2, _3)); - try { - _sensor->configure(lidar_config); - } catch (const OusterDriverException & e) { - RCLCPP_FATAL(this->get_logger(), "Exception thrown: (%s)", e.what()); - exit(-1); - } - _full_rotation_accumulator = std::make_shared( _sensor->getMetadata(), _sensor->getPacketFormat()); @@ -210,14 +206,14 @@ void OusterDriver::processData() { try { ouster::sensor::client_state state = _sensor->get(); + _lidar_packet_data = _sensor->readLidarPacket(state); + _imu_packet_data = _sensor->readImuPacket(state); + RCLCPP_DEBUG( this->get_logger(), - "Packet with state: %s", + "Retrieved packet with state: %s", ros2_ouster::toString(state).c_str()); - _lidar_packet_data = _sensor->readLidarPacket(state); - _imu_packet_data = _sensor->readImuPacket(state); - std::pair key_its; uint64_t override_ts = @@ -240,7 +236,6 @@ void OusterDriver::processData() it->second->process(_imu_packet_data, override_ts); } } - } catch (const OusterDriverException & e) { RCLCPP_WARN( this->get_logger(), @@ -264,7 +259,7 @@ void OusterDriver::resetService( lidar_config.lidar_port = get_parameter("lidar_port").as_int(); lidar_config.lidar_mode = get_parameter("lidar_mode").as_string(); lidar_config.timestamp_mode = get_parameter("timestamp_mode").as_string(); - _sensor->reset(lidar_config); + _sensor->reset(lidar_config, shared_from_this()); } void OusterDriver::getMetadata( @@ -276,6 +271,26 @@ void OusterDriver::getMetadata( return; } response->metadata = toMsg(_sensor->getMetadata()); + + // Save the metadata to file ONLY if the user specifies a filepath + if (request->metadata_filepath != "") { + std::string json_config = ouster::sensor::to_string(_sensor->getMetadata()); + std::ofstream ofs; + ofs.open(request->metadata_filepath); + ofs << json_config << std::endl; + ofs.close(); + if (!ofs) { + RCLCPP_ERROR( + this->get_logger(), + "Failed to save metadata to: %s.", + request->metadata_filepath.c_str()); + } else { + RCLCPP_INFO( + this->get_logger(), + "Saving metadata to a .json file specifed here: %s", + request->metadata_filepath.c_str()); + } + } } } // namespace ros2_ouster diff --git a/ros2_ouster/src/sensor.cpp b/ros2_ouster/src/sensor.cpp index 31beba3..9073b07 100644 --- a/ros2_ouster/src/sensor.cpp +++ b/ros2_ouster/src/sensor.cpp @@ -12,7 +12,8 @@ // limitations under the License. #include - +#include +#include #include "ros2_ouster/client/client.h" #include "ros2_ouster/exception.hpp" #include "ros2_ouster/interfaces/metadata.hpp" @@ -31,14 +32,23 @@ Sensor::~Sensor() _imu_packet.clear(); } -void Sensor::reset(const ros2_ouster::Configuration & config) +void Sensor::reset( + ros2_ouster::Configuration & config, + rclcpp_lifecycle::LifecycleNode::SharedPtr node) { _ouster_client.reset(); - configure(config); + configure(config, node); } -void Sensor::configure(const ros2_ouster::Configuration & config) +void Sensor::configure( + ros2_ouster::Configuration & config, + rclcpp_lifecycle::LifecycleNode::SharedPtr node) { + RCLCPP_INFO( + node->get_logger(), + "Configuring Ouster driver node."); + + // Check the validity of some of the retrieved parameters if (!ouster::sensor::lidar_mode_of_string(config.lidar_mode)) { throw ros2_ouster::OusterDriverException( "Invalid lidar mode: " + config.lidar_mode); @@ -51,6 +61,21 @@ void Sensor::configure(const ros2_ouster::Configuration & config) exit(-1); } + // Report to the user whether automatic address detection is being used, and + // what the source / destination IPs are + RCLCPP_INFO( + node->get_logger(), + "Connecting to sensor at %s.", config.lidar_ip.c_str()); + if (config.computer_ip == "") { + RCLCPP_INFO( + node->get_logger(), + "Sending data from sensor to computer using automatic address detection"); + } else { + RCLCPP_INFO( + node->get_logger(), + "Sending data from sensor to %s.", config.computer_ip.c_str()); + } + _ouster_client = ouster::sensor::init_client( config.lidar_ip, config.computer_ip, @@ -84,7 +109,6 @@ ouster::sensor::client_state Sensor::get() "Failed to get valid sensor data " "information from lidar, returned error!")); } - return state; } diff --git a/ros2_ouster/src/sensor_tins.cpp b/ros2_ouster/src/sensor_tins.cpp new file mode 100644 index 0000000..5430509 --- /dev/null +++ b/ros2_ouster/src/sensor_tins.cpp @@ -0,0 +1,241 @@ +// Copyright 2021, Matthew Young (Trimble Inc) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include "ros2_ouster/client/client.h" +#include "ros2_ouster/exception.hpp" +#include "ros2_ouster/interfaces/metadata.hpp" +#include "ros2_ouster/sensor_tins.hpp" + + +namespace sensor +{ + +SensorTins::SensorTins() +: SensorInterface() {} + +SensorTins::~SensorTins() +{ + _ouster_client.reset(); + _lidar_packet.clear(); + _imu_packet.clear(); +} + +void SensorTins::reset( + ros2_ouster::Configuration & config, + rclcpp_lifecycle::LifecycleNode::SharedPtr node) +{ + _ouster_client.reset(); + configure(config, node); +} + +void SensorTins::configure( + ros2_ouster::Configuration & config, + rclcpp_lifecycle::LifecycleNode::SharedPtr node) +{ + RCLCPP_INFO( + node->get_logger(), + "Configuring Tins-based Ouster driver node."); + + // Declare parameters specific to the SensorTins implementation + ros2_ouster::declare_parameter_if_not_declared( + node, "ethernet_device", rclcpp::ParameterValue("no_ethernet_device_configured")); + ros2_ouster::declare_parameter_if_not_declared( + node, "metadata_filepath", rclcpp::ParameterValue("no_filepath_specified")); + + // Get parameters specific to the SensorTins implementation + try { + config.ethernet_device = node->get_parameter("ethernet_device").as_string(); + config.metadata_filepath = node->get_parameter("metadata_filepath").as_string(); + } catch (...) { + throw ros2_ouster::OusterDriverException( + "TinsDriver failed to retrieve the ethernet_device or metadata_filepath parameter"); + exit(-1); + } + + // Check the validity of some of the retrieved parameters + if (!ouster::sensor::lidar_mode_of_string(config.lidar_mode)) { + throw ros2_ouster::OusterDriverException( + "Invalid lidar mode: " + config.lidar_mode); + exit(-1); + } + + if (!ouster::sensor::timestamp_mode_of_string(config.timestamp_mode)) { + throw ros2_ouster::OusterDriverException( + "Invalid timestamp mode: " + config.timestamp_mode); + exit(-1); + } + + RCLCPP_INFO( + node->get_logger(), + "Looking for packets from sensor IPv4 address %s to destination %s.", + config.lidar_ip.c_str(), + config.computer_ip.c_str()); + + // The driver config is saved internally b/c some parameters are needed for + // the Tins sniffer + _driver_config = config; + + // Read sensor metadata from file + loadSensorInfoFromJsonFile( + _driver_config.metadata_filepath, + _metadata); + + // loadSensorInfoFromJsonFile actually returns a sensor_info object, so + // fill in the params specific to the ros2_ouster::Metadata object, that + // aren't normally supplied in the metadata file. + _metadata.imu_port = _driver_config.imu_port; + _metadata.lidar_port = _driver_config.lidar_port; + _metadata.timestamp_mode = _driver_config.timestamp_mode; + + // Fill anything missing with defaults and resize the packet containers + ros2_ouster::populate_missing_metadata_defaults(_metadata); + _lidar_packet.resize(getPacketFormat().lidar_packet_size + 1); + _imu_packet.resize(getPacketFormat().imu_packet_size + 1); + + // Create and initialize the Tins sniffer object + try { + initializeSniffer(_driver_config.ethernet_device); + } catch (const std::exception & e) { + throw ros2_ouster::OusterDriverException( + "Failed to configure Tins sniffer. Check that the entered ethernet device " + + _driver_config.ethernet_device + " is valid."); + exit(-1); + } +} + +ouster::sensor::client_state SensorTins::get() +{ + // Start a sniff loop to look for a valid LiDAR or IMU packet + _tins_sniffer_pointer->sniff_loop( + std::bind( + &SensorTins::sniffOnePacket, + this, + std::placeholders::_1)); + + // Return the state, as best can be inferred from the data + return _inferred_state; +} + +uint8_t * SensorTins::readLidarPacket(const ouster::sensor::client_state & state) +{ + if (state == ouster::sensor::client_state::LIDAR_DATA) { + return _lidar_packet.data(); + } else { + return nullptr; + } +} + +uint8_t * SensorTins::readImuPacket(const ouster::sensor::client_state & state) +{ + if (state == ouster::sensor::client_state::IMU_DATA) { + return _imu_packet.data(); + } else { + return nullptr; + } +} + +ros2_ouster::Metadata SensorTins::getMetadata() +{ + return _metadata; +} + +ouster::sensor::packet_format SensorTins::getPacketFormat() +{ + return ouster::sensor::get_format(getMetadata()); +} + +void SensorTins::loadSensorInfoFromJsonFile( + std::string filepath_to_read, + ouster::sensor::sensor_info & sensor_info) +{ + if (filepath_to_read == "") { + throw ros2_ouster::OusterDriverException( + "Metadata filepath is empty! The Tins driver needs a valid metadata file!"); + exit(-1); + } + + try { + sensor_info = ouster::sensor::metadata_from_json(filepath_to_read); + } catch (const std::exception & e) { + throw ros2_ouster::OusterDriverException( + "Failed to read metadata from file: " + filepath_to_read + + " with exception " + e.what()); + exit(-1); + } +} + +void SensorTins::initializeSniffer(const std::string eth_device) +{ + _sniffer_config.set_promisc_mode(true); + _sniffer_config.set_immediate_mode(true); + + // Filter out all packets not from the sensor + std::string filter_string = "ip src " + _driver_config.lidar_ip; + _sniffer_config.set_filter(filter_string); + + _tins_sniffer_pointer = std::make_unique(eth_device, _sniffer_config); +} + +bool SensorTins::sniffOnePacket(Tins::Packet & packet) +{ + auto & pdu_to_process = *packet.pdu(); + + // Reassemble the packet if it's fragmented + if (_tins_ipv4_reassembler.process(pdu_to_process) != + Tins::IPv4Reassembler::FRAGMENTED) + { + // Reject the packet if it's not a IP>UDP>RawPDU packet + const Tins::IP * ip = pdu_to_process.find_pdu(); + if (!ip) { + return true; + } + const Tins::UDP * udp = pdu_to_process.find_pdu(); + if (!udp) { + return true; + } + const Tins::RawPDU * raw = pdu_to_process.find_pdu(); + if (!raw) { + return true; + } + + const Tins::RawPDU::payload_type & payload = raw->payload(); + + // If a LiDAR packet... + if ((ip->dst_addr().to_string() == _driver_config.computer_ip) && + (payload.size() == getPacketFormat().lidar_packet_size) && + (udp->dport() == _driver_config.lidar_port)) + { + _inferred_state = ouster::sensor::client_state::LIDAR_DATA; + _lidar_packet = payload; + return false; + } + + // If an IMU packet... + if ((ip->dst_addr().to_string() == _driver_config.computer_ip) && + (payload.size() == getPacketFormat().imu_packet_size) && + (udp->dport() == _driver_config.imu_port)) + { + _inferred_state = ouster::sensor::client_state::IMU_DATA; + _imu_packet = payload; + return false; + } + } + + // The packet is valid but from neither the IMU or LiDAR. Return true to + // keep sniffing. + return true; +} + +} // namespace sensor