diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 6c0d739d..926c60bb 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -30,6 +30,20 @@ ouster_ros(2) * 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. +* 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 ------------- diff --git a/Dockerfile b/Dockerfile index 1d5e5b38..5cfdb777 100644 --- a/Dockerfile +++ b/Dockerfile @@ -53,10 +53,14 @@ RUN set -xe \ FROM build-env SHELL ["/bin/bash", "-c"] + RUN source /opt/ros/$ROS_DISTRO/setup.bash && colcon build \ --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release \ -DCMAKE_CXX_FLAGS="-Wno-deprecated-declarations" +RUN source /opt/ros/$ROS_DISTRO/setup.bash && colcon test \ + --ctest-args tests ouster_ros --rerun-failed --output-on-failure + # Entrypoint for running Ouster ros: # # Usage: docker run --rm -it ouster-ros [sensor.launch parameters ..] diff --git a/README.md b/README.md index aa4931b7..d838281d 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ # Official ROS driver 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,23 +9,28 @@ | 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) -- [Requirements](#requirements) -- [Getting Started](#getting-started) -- [Usage](#usage) - - [Launching Nodes](#launching-nodes) - - [Sensor Mode](#sensor-mode) - - [Recording Mode](#recording-mode) - - [Replay Mode](#replay-mode) - - [Multicast Mode (experimental)](#multicast-mode-experimental) - - [Invoking Services](#invoking-services) - - [GetMetadata](#getmetadata) - - [GetConfig](#getconfig) - - [SetConfig (experimental)](#setconfig-experimental) -- [License](#license) +- [Official ROS driver for Ouster sensors](#official-ros-driver-for-ouster-sensors) + - [Overview](#overview) + - [Requirements](#requirements) + - [Linux](#linux) + - [Windows](#windows) + - [Mac](#mac) + - [Getting Started](#getting-started) + - [Usage](#usage) + - [Launching Nodes](#launching-nodes) + - [Sensor Mode](#sensor-mode) + - [Recording Mode](#recording-mode) + - [Replay Mode](#replay-mode) + - [Multicast Mode (experimental)](#multicast-mode-experimental) + - [Invoking Services](#invoking-services) + - [GetMetadata](#getmetadata) + - [GetConfig](#getconfig) + - [SetConfig](#setconfig) + - [Reset](#reset) + - [License](#license) ## Overview @@ -38,8 +43,8 @@ dual return and it was configured to use this capability, then another topic wil name `/ouster/points2` which corresponds to the second point cloud. ## Requirements -This driver only supports **Foxy** ROS 2 distro. Please refer to ROS 2 online documentation on how to -setup ros on your machine before proceeding with the remainder of this guide. +This branch is only intended for use with the **Foxy** ROS 2 distro. Please refer to ROS 2 online documentation on how to +setup ROS on your machine before proceeding with the remainder of this guide. > **Note** > If you have _rosdep_ tool installed on your system you can then use the following command to get all @@ -119,14 +124,32 @@ source ros2_ws/install/setup.bash ## Usage ### Launching Nodes -The package supports three modes of interaction, you can connect to a _live sensor_, _replay_ a recorded bag or _record_ a new bag file using the corresponding launch files. Recently, we have -added a new mode that supports multicast. The commands are listed below: +The package supports three modes of interaction, you can connect to a _live sensor_, _replay_ a +recorded bag or _record_ a new bag file using the corresponding launch files. Recently, we have +added a new mode that supports multicast. The commands are listed below, for convenience we do +provide both launch file formats (xml and python) but the python format is preferred method: #### Sensor Mode +To connect to a live sensor you use the following launch file ```bash ros2 launch ouster_ros sensor.launch.xml \ sensor_hostname:= ``` +The equivalent python file is +```bash +ros2 launch ouster_ros driver.launch.py \ + params_files:= +``` +If you don't pass a params_file then the file located at `ouster/config/driver_params.yaml` will be used. Note that in +the params you can start with default options for everything except the `sensor_hostname` param which you should adjust +to match the hostname or ip address of the Ouster sensor you are trying to connect to. + +**comptability mode** +If you are migrating from https://github.com/ros-drivers/ros2_ouster_drivers to the official ouster drivers +we supply you with a file `driver_launch.py` which provides users with same topic name and accepts the same +parameter file `community_driver_config.yaml`. Please note that this is provided for backward compatibilty +it may not be maintained in the future, so it would be better to update to the new format `driver_params.yaml` +which offers the same options and more. #### Recording Mode > Note @@ -197,14 +220,14 @@ To get the current config of a live sensor, invoke the command: ros2 service call /ouster/get_config ouster_srvs/srv/GetConfig ``` -#### SetConfig (experimental) +#### SetConfig To change config via a file while connected to a live sensor, invoke the command: ```bash ros2 service call /ouster/set_config ouster_srvs/srv/SetConfig \ "{config_file: 'some_config.json'}" ``` -#### Reset (experimental) +#### Reset To reset the new reset service, execute the following commnad: ```bash ros2 service call /ouster/reset std_srvs/srv/Empty diff --git a/ouster-ros/CMakeLists.txt b/ouster-ros/CMakeLists.txt index d7f70c4a..d91ea205 100644 --- a/ouster-ros/CMakeLists.txt +++ b/ouster-ros/CMakeLists.txt @@ -156,6 +156,28 @@ rclcpp_components_register_node(os_image_component EXECUTABLE os_image ) +# ==== os_sensor_component ==== +create_ros2_component(os_driver_component + "src/os_sensor_node_base.cpp;src/os_sensor_node.cpp;src/os_driver_node.cpp" + "std_srvs" +) +rclcpp_components_register_node(os_driver_component + PLUGIN "ouster_ros::OusterDriver" + EXECUTABLE os_driver +) + + +# ==== Test ==== +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(${PROJECT_NAME}_test test/ring_buffer_test.cpp) + target_include_directories(${PROJECT_NAME}_test PUBLIC + $ + $ + ) +endif() + + # ==== Install ==== install( TARGETS @@ -164,6 +186,7 @@ install( os_replay_component os_cloud_component os_image_component + os_driver_component ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin diff --git a/ouster-ros/config/community_driver.rviz b/ouster-ros/config/community_driver.rviz new file mode 100644 index 00000000..927c7778 --- /dev/null +++ b/ouster-ros/config/community_driver.rviz @@ -0,0 +1,262 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 138 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PointCloud21 + - /TF1/Tree1 + - /range1 + - /range1/Topic1 + - /nearir1/Topic1 + - /reflec1 + - /signal1 + Splitter Ratio: 0.5 + Tree Height: 1185 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: range + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: true + Max Color: 255; 255; 255 + Max Intensity: 3000 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /points + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + " imu_data_frame": + Value: true + " laser_data_frame": + Value: true + " laser_sensor_frame": + Value: true + All Enabled: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + " laser_sensor_frame": + " imu_data_frame": + {} + " laser_data_frame": + {} + Update Interval: 0 + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: range + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /range_image + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: nearir + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /nearir_image + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: reflec + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /reflec_image + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: signal + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /signal_image + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: "laser_sensor_frame" + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 4.583338260650635 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.9003978371620178 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.277213096618652 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 2272 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001f700000599fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000002b3000005990000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f000007defc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000006e000007de0000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000e7000000239fc0100000002fc0000000000000e700000008d00fffffffa000000010200000004fb0000000c007200650066006c006500630000000000ffffffff0000004a00fffffffb0000000c007300690067006e0061006c0100000000ffffffff0000004a00fffffffb0000000c006e006500610072006900720000000000ffffffff0000004a00fffffffb0000000a00720061006e00670065000000006e000002390000004a00fffffffb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000e700000005afc0100000002fb0000000800540069006d0065010000000000000e700000057100fffffffb0000000800540069006d0065010000000000000450000000000000000000000c6d0000059900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 3696 + X: 144 + Y: 54 + nearir: + collapsed: false + range: + collapsed: false + reflec: + collapsed: false + signal: + collapsed: false diff --git a/ouster-ros/config/community_driver_config.yaml b/ouster-ros/config/community_driver_config.yaml new file mode 100644 index 00000000..f5974dff --- /dev/null +++ b/ouster-ros/config/community_driver_config.yaml @@ -0,0 +1,54 @@ +# [Not recommended] Provided to support existing users of the ROS2 Community +# please migrate to the parameters format as specified in driver_params.yaml +ouster_driver: + ros__parameters: + lidar_ip: 169.254.200.2 + computer_ip: 169.254.219.146 + lidar_mode: "1024x10" + imu_port: 7503 + lidar_port: 7502 + sensor_frame: laser_sensor_frame + laser_frame: laser_data_frame + imu_frame: imu_data_frame + + # 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: False + + # 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 \ No newline at end of file diff --git a/ouster-ros/config/driver_params.yaml b/ouster-ros/config/driver_params.yaml new file mode 100644 index 00000000..040c9e72 --- /dev/null +++ b/ouster-ros/config/driver_params.yaml @@ -0,0 +1,66 @@ +ouster/os_driver: + ros__parameters: + # sensor_hostname[required]: hostname or IP address of the sensor (IP4 or + # IP6). + sensor_hostname: '' + # udp_dest[optional]: hostname or multicast group IP where the sensor will + # send UDP data packets. + udp_dest: '' + # mtp_dest[optional]: hostname IP address for receiving data packets via + # multicast, by default it is INADDR_ANY, so packets will be received on + # first available network interface. + mtp_dest: '' + # mtp_main[optional]: if true, then configure and reinit the sensor, + # otherwise start client with active configuration of sensor + mtp_main: false + # lidar_mode[optional]: resolution and rate; possible values: { 512x10, + # 512x20, 1024x10, 1024x20, 2048x10, 4096x5 }. Leave empty to remain on + # current the lidar mode. + lidar_mode: '' + # timestamp_mode[optional]: method used to timestamp measurements; possible + # values: + # - TIME_FROM_INTERNAL_OSC + # - TIME_FROM_SYNC_PULSE_IN + # - TIME_FROM_PTP_1588 + # - TIME_FROM_ROS_TIME: This option uses the time of reception of first + # packet of a LidarScan as the timestamp of the IMU, + # PointCloud2 and LaserScan messages. + timestamp_mode: '' + # udp_profile_lidar[optional]: lidar packet profile; possible values: + # - LEGACY: not recommended + # - RNG19_RFL8_SIG16_NIR16_DUAL + # - RNG19_RFL8_SIG16_NIR16 + # - RNG15_RFL8_NIR8 + udp_profile_lidar: '' + # metadata[optional]: path to save metadata file to, if left empty a file + # with the sensor hostname or ip will be crearted in ~/.ros folder. + metadata: '' + # lidar_port[optional]: port value should be in the range [0, 65535]. If you + # use 0 as the port value then the first avaliable port number will be + # assigned. + lidar_port: 0 + # imu_port[optional]: port value should be in the range [0, 65535]. If you + # use 0 as the port value then the first avaliable port number will be + # assigned. + imu_port: 0 + # sensor_frame[optional]: name to use when referring to the sensor frame. + sensor_frame: os_sensor + # lidar_frame[optional]: name to use when referring to the lidar frame. + lidar_frame: os_lidar + # imu_frame[optional]: name to use when referring to the imu frame. + imu_frame: os_imu + # point_cloud_frame[optional]: which frame of reference to use when + # generating PointCloud2 or LaserScan messages, select between the values of + # lidar_frame and sensor_frame. + point_cloud_frame: os_lidar + # proc_mask[optional]: use any combination of the 4 flags IMG, PCL, IMU and + # SCAN to enable or disable their respective messages. + proc_mask: IMG|PCL|IMU|SCAN + # scan_ring[optional]: use this parameter in conjunction with the SCAN flag + # to select which beam of the LidarScan to use when producing the LaserScan + # message. Choose a value the range [0, sensor_beams_count). + scan_ring: 0 + # use_system_default_qos[optional]: 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: False diff --git a/ouster-ros/config/os_sensor_cloud_image_params.yaml b/ouster-ros/config/os_sensor_cloud_image_params.yaml new file mode 100644 index 00000000..a2deb92a --- /dev/null +++ b/ouster-ros/config/os_sensor_cloud_image_params.yaml @@ -0,0 +1,30 @@ +# [Not recommended] Use this configuration only when utilizing the disjointed +# os_sensor and os_cloud configuration +# For the future and for better results consider migrating to combined node/component os_driver +# and utilize the driver_params.yaml along with it. +ouster/os_sensor: + ros__parameters: + sensor_hostname: '' + udp_dest: '' + mtp_dest: '' + mtp_main: false + lidar_mode: '' + timestamp_mode: '' + udp_profile_lidar: '' + metadata: '' + lidar_port: 0 + imu_port: 0 + use_system_default_qos: False +ouster/os_cloud: + ros__parameters: + sensor_frame: os_sensor + lidar_frame: os_lidar + imu_frame: os_imu + point_cloud_frame: os_lidar + timestamp_mode: '' # this value needs to match os_sensor/timestamp_mode + proc_mask: IMU|PCL|SCAN # pick IMU, PCL, SCAN or any combination of the three options + use_system_default_qos: False # needs to match the value defined for os_sensor node + scan_ring: 0 # Use this parameter in conjunction with the SCAN flag and choose a + # value the range [0, sensor_beams_count) +ouster/os_image: + use_system_default_qos: False # needs to match the value defined for os_sensor node diff --git a/ouster-ros/config/parameters.yaml b/ouster-ros/config/parameters.yaml deleted file mode 100644 index 7ea15975..00000000 --- a/ouster-ros/config/parameters.yaml +++ /dev/null @@ -1,18 +0,0 @@ -ouster/os_sensor: - ros__parameters: - sensor_hostname: '' - udp_dest: '' - mtp_dest: '' - mtp_main: false - lidar_mode: '' - timestamp_mode: '' - udp_profile_lidar: '' - metadata: '' - lidar_port: 0 - imu_port: 0 -ouster/os_cloud: - ros__parameters: - sensor_frame: 'os_sensor' - lidar_frame: 'os_lidar' - imu_frame: 'os_imu' - timestamp_mode: '' # this value needs to match os_sensor/timestamp_mode diff --git a/ouster-ros/include/ouster_ros/os_processing_node_base.h b/ouster-ros/include/ouster_ros/os_processing_node_base.h index 26c92348..a1410a7e 100644 --- a/ouster-ros/include/ouster_ros/os_processing_node_base.h +++ b/ouster-ros/include/ouster_ros/os_processing_node_base.h @@ -17,21 +17,14 @@ namespace ouster_ros { class OusterProcessingNodeBase : public rclcpp::Node { protected: - explicit OusterProcessingNodeBase(const std::string& name, - const rclcpp::NodeOptions& options) + OusterProcessingNodeBase(const std::string& name, + const rclcpp::NodeOptions& options) : rclcpp::Node(name, options) {} void create_metadata_subscriber( std::function on_sensor_metadata); - int get_n_returns() const; - - static std::string topic_for_return(std::string base, int idx) { - if (idx == 0) return base; - return base + std::to_string(idx + 1); - } - protected: rclcpp::Subscription::SharedPtr metadata_sub; ouster::sensor::sensor_info info; diff --git a/ouster-ros/include/ouster_ros/os_ros.h b/ouster-ros/include/ouster_ros/os_ros.h index 7f17cd0d..b724cd04 100644 --- a/ouster-ros/include/ouster_ros/os_ros.h +++ b/ouster-ros/include/ouster_ros/os_ros.h @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -34,27 +35,47 @@ 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 is_legacy_lidar_profile(const sensor::sensor_info& info); + +/** + * 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 + */ +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 */ -bool read_imu_packet(const sensor::client& cli, ouster_msgs::msg::PacketMsg& pm, - const sensor::packet_format& pf); +std::string topic_for_return(const std::string& topic_base, int return_idx); /** - * 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 + * Parse an imu packet message into a ROS imu message * @param[in] pf the packet format - * @return whether reading was successful + * @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] buf the raw packet message populated by read_imu_packet + * @return ROS sensor message with fields populated from the packet */ -bool read_lidar_packet(const sensor::client& cli, - ouster_msgs::msg::PacketMsg& pm, - const sensor::packet_format& pf); +sensor_msgs::msg::Imu packet_to_imu_msg(const ouster::sensor::packet_format& pf, + const rclcpp::Time& timestamp, + const std::string& frame, + const uint8_t* buf); /** * Parse an imu packet message into a ROS imu message @@ -128,4 +149,47 @@ sensor_msgs::msg::PointCloud2 cloud_to_cloud_msg(const Cloud& cloud, geometry_msgs::msg::TransformStamped transform_to_tf_msg( const ouster::mat4d& mat, const std::string& frame, const std::string& child_frame, rclcpp::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::msg::LaserScan lidar_scan_to_laser_scan_msg( + const ouster::LidarScan& ls, + const rclcpp::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; +} +} // namespace impl + } // namespace ouster_ros diff --git a/ouster-ros/include/ouster_ros/os_sensor_node_base.h b/ouster-ros/include/ouster_ros/os_sensor_node_base.h index b8bdf118..ea2fd8b8 100644 --- a/ouster-ros/include/ouster_ros/os_sensor_node_base.h +++ b/ouster-ros/include/ouster_ros/os_sensor_node_base.h @@ -36,6 +36,10 @@ class OusterSensorNodeBase : public rclcpp_lifecycle::LifecycleNode { void display_lidar_info(const ouster::sensor::sensor_info& info); + static std::string read_text_file(const std::string& text_file); + + static bool write_text_to_file(const std::string& file_path, const std::string& text); + protected: ouster::sensor::sensor_info info; rclcpp::Service::SharedPtr get_metadata_srv; diff --git a/ouster-ros/launch/driver.launch.py b/ouster-ros/launch/driver.launch.py new file mode 100644 index 00000000..17b894a7 --- /dev/null +++ b/ouster-ros/launch/driver.launch.py @@ -0,0 +1,107 @@ +# Copyright 2023 Ouster, Inc. +# + +"""Launch a sensor node along with os_cloud and os_""" + +from pathlib import Path +import launch +import lifecycle_msgs.msg +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node, LifecycleNode +from launch.actions import (DeclareLaunchArgument, IncludeLaunchDescription, + RegisterEventHandler, EmitEvent, LogInfo) +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch.events import matches_action +from launch_ros.events.lifecycle import ChangeState +from launch_ros.event_handlers import OnStateTransition + + +def generate_launch_description(): + """ + Generate launch description for running ouster_ros components separately each + component will run in a separate process). + """ + ouster_ros_pkg_dir = get_package_share_directory('ouster_ros') + default_params_file = \ + Path(ouster_ros_pkg_dir) / 'config' / 'driver_params.yaml' + params_file = LaunchConfiguration('params_file') + params_file_arg = DeclareLaunchArgument('params_file', + default_value=str( + default_params_file), + description='name or path to the parameters file to use.') + + ouster_ns = LaunchConfiguration('ouster_ns') + ouster_ns_arg = DeclareLaunchArgument( + 'ouster_ns', default_value='ouster') + + rviz_enable = LaunchConfiguration('viz') + rviz_enable_arg = DeclareLaunchArgument('viz', default_value='True') + + os_driver_name = LaunchConfiguration('os_driver_name') + os_driver_name_arg = DeclareLaunchArgument('os_driver_name', default_value='os_driver') + + os_driver = LifecycleNode( + package='ouster_ros', + executable='os_driver', + name=os_driver_name, + namespace=ouster_ns, + parameters=[params_file], + output='screen', + ) + + sensor_configure_event = EmitEvent( + event=ChangeState( + lifecycle_node_matcher=matches_action(os_driver), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE, + ) + ) + + sensor_activate_event = RegisterEventHandler( + OnStateTransition( + target_lifecycle_node=os_driver, goal_state='inactive', + entities=[ + LogInfo(msg="os_driver activating..."), + EmitEvent(event=ChangeState( + lifecycle_node_matcher=matches_action(os_driver), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE, + )), + ], + handle_once=True + ) + ) + + # TODO: figure out why registering for on_shutdown event causes an exception + # and error handling + # shutdown_event = RegisterEventHandler( + # OnShutdown( + # on_shutdown=[ + # EmitEvent(event=ChangeState( + # lifecycle_node_matcher=matches_node_name(node_name=F"/ouster/os_sensor"), + # transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVE_SHUTDOWN, + # )), + # LogInfo(msg="os_sensor node exiting..."), + # ], + # ) + # ) + + + rviz_launch_file_path = \ + Path(ouster_ros_pkg_dir) / 'launch' / 'rviz.launch.py' + rviz_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([str(rviz_launch_file_path)]), + condition=IfCondition(rviz_enable) + ) + + return launch.LaunchDescription([ + params_file_arg, + ouster_ns_arg, + rviz_enable_arg, + os_driver_name_arg, + rviz_launch, + os_driver, + sensor_configure_event, + sensor_activate_event, + # shutdown_event + ]) diff --git a/ouster-ros/launch/driver_launch.py b/ouster-ros/launch/driver_launch.py new file mode 100644 index 00000000..6072ba82 --- /dev/null +++ b/ouster-ros/launch/driver_launch.py @@ -0,0 +1,52 @@ +# Copyright 2023 Ouster, Inc. +# + +"""Launch a sensor node along with os_cloud and os_""" + +from pathlib import Path +import launch +import lifecycle_msgs.msg +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node, LifecycleNode +from launch.actions import (DeclareLaunchArgument, IncludeLaunchDescription, + RegisterEventHandler, EmitEvent, LogInfo) +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch.events import matches_action +from launch_ros.events.lifecycle import ChangeState +from launch_ros.event_handlers import OnStateTransition + + +def generate_launch_description(): + """ + Generate launch description for running ouster_ros components separately each + component will run in a separate process). + """ + ouster_ros_pkg_dir = get_package_share_directory('ouster_ros') + # use the community_driver_config.yaml by default + default_params_file = \ + Path(ouster_ros_pkg_dir) / 'config' / 'community_driver_config.yaml' + params_file = LaunchConfiguration('params_file') + params_file_arg = DeclareLaunchArgument('params_file', + default_value=str( + default_params_file), + description='name or path to the parameters file to use.') + + driver_launch_file_path = \ + Path(ouster_ros_pkg_dir) / 'launch' / 'driver.launch.py' + driver_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([str(driver_launch_file_path)]), + launch_arguments={ + 'params_file': params_file, + 'ouster_ns': '', + 'os_driver_name': 'ouster_driver', + 'viz': 'True', + 'rviz_config': './install/ouster_ros/share/ouster_ros/config/community_driver.rviz' + }.items() + ) + + return launch.LaunchDescription([ + params_file_arg, + driver_launch + ]) diff --git a/ouster-ros/launch/replay.independent.launch.xml b/ouster-ros/launch/replay.independent.launch.xml index 787ffac6..d2192c2c 100644 --- a/ouster-ros/launch/replay.independent.launch.xml +++ b/ouster-ros/launch/replay.independent.launch.xml @@ -25,6 +25,14 @@ + + + + + @@ -35,8 +43,12 @@ + + + + + -