From bcd032a49957beca5217ee10d53306a1d7b11df3 Mon Sep 17 00:00:00 2001 From: Tom Panzarella Date: Mon, 13 Apr 2020 15:56:26 -0400 Subject: [PATCH] Support for accessing and mutating the timestamp mode on the LiDAR (#36) * Support for accessing and mutating the timestamp mode on the LiDAR The on-LiDAR `timestamp_mode` can now be set in the config file. Additionally, its state is fedback in the `GetMetadata` service. I HIL-tested these changes via visual inspection and everything seems to be working correctly. I see that in #10 there is a plan add some automated testing. I think this is a *very good* idea. I'll add some comments to that issue shortly. Closes: #34 * Adds TIME_FROM_ROS_RECEPTION as valid timestamp_mode * When using TIME_FROM_ROS_RECEPTION all data processors that process a given LiDAR scan will get the same ROS timestamp so they can be time correlated. * Made log message less alarming --- README.md | 72 +++++++++++++++---- ouster_msgs/msg/Metadata.msg | 1 + ouster_msgs/package.xml | 1 + ros2_ouster/include/ros2_ouster/OS1/OS1.hpp | 70 ++++++++++++++++-- .../include/ros2_ouster/OS1/OS1_util.hpp | 7 +- .../OS1/processors/image_processor.hpp | 6 +- .../OS1/processors/imu_processor.hpp | 4 +- .../OS1/processors/pointcloud_processor.hpp | 7 +- .../OS1/processors/scan_processor.hpp | 6 +- .../include/ros2_ouster/conversions.hpp | 8 ++- .../ros2_ouster/interfaces/configuration.hpp | 1 + .../interfaces/data_processor_interface.hpp | 4 +- .../ros2_ouster/interfaces/metadata.hpp | 1 + .../include/ros2_ouster/ouster_driver.hpp | 1 + ros2_ouster/package.xml | 1 + ros2_ouster/params/os1.yaml | 13 ++++ ros2_ouster/src/OS1/OS1_sensor.cpp | 10 ++- ros2_ouster/src/ouster_driver.cpp | 17 ++++- 18 files changed, 194 insertions(+), 36 deletions(-) diff --git a/README.md b/README.md index af29734..6cb5af0 100644 --- a/README.md +++ b/README.md @@ -38,21 +38,69 @@ See design doc in `design/*` directory [here](ros2_ouster/design/design_doc.md). | `reset` | std_srvs/Empty | Reset the sensor's connection | | `GetMetadata` | ouster_msgs/GetMetadata | Get information about the sensor | -| Parameter | Type | Description | -|--------------------------|---------|----------------------------------------------------------------------| -| `lidar_ip` | String | IP of lidar (ex. 10.5.5.87) | -| `computer_ip` | String | IP of computer to get data (ex. 10.5.5.1) | -| `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` | -| `use_system_default_qos` | bool | Publish data with default QoS for rosbag2 recording, default `False` | +| Parameter | Type | Description | +|--------------------------|---------|------------------------------------------------------------------------| +| `lidar_ip` | String | IP of lidar (ex. 10.5.5.87) | +| `computer_ip` | String | IP of computer to get data (ex. 10.5.5.1) | +| `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` | +| `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`| -Note: TF will provide you the transformations from the sensor frame to each of the data frames. +Note: TF will provide you the transformations from the sensor frame to each of +the data frames. + +### Timestamp Modes + +Referring to the parameter table above, the `timestamp_mode` parameter has four +allowable options (as of this writing). They are: `TIME_FROM_INTERNAL_OSC`, +`TIME_FROM_SYNC_PULSE_IN`, `TIME_FROM_PTP_1588`, `TIME_FROM_ROS_RECEPTION`. A +description of each now follows. + +#### `TIME_FROM_INTERNAL_OSC` + +Use the LiDAR internal clock. Measurements are time stamped with ns since +power-on. Free running counter based on the OS1’s internal oscillator. Counts +seconds and nanoseconds since OS1 turn on, reported at ns resolution (both a +second and nanosecond register in every UDP packet), but min increment is on +the order of 10 ns. Accuracy is +/- 90 ppm. + +#### `TIME_FROM_SYNC_PULSE_IN` + +A free running counter synced to the `SYNC_PULSE_IN` input counts seconds (# of +pulses) and nanoseconds since OS1 turn on. If `multipurpose_io_mode` is set to +`INPUT_NMEA_UART` then the seconds register jumps to time extracted from a NMEA +`$GPRMC` message read on the `multipurpose_io` port. Reported at ns resolution +(both a second and nanosecond register in every UDP packet), but min increment +is on the order of 10 ns. Accuracy is +/- 1 s from a perfect `SYNC_PULSE_IN` +source. + +#### `TIME_FROM_PTP_1588` + +Synchronize with an external PTP master. A monotonically increasing counter +that will begin counting seconds and nanoseconds since startup. As soon as a +1588 sync event happens, the time will be updated to seconds and nanoseconds +since 1970. The counter must always count forward in time. If another 1588 sync +event happens the counter will either jump forward to match the new time, or +slow itself down. It is reported at ns resolution (there is both a second and +nanosecond register in every UDP packet), but the minimum increment +varies. Accuracy is +/- <50 us from the 1588 master. + +#### `TIME_FROM_ROS_RECEPTION` + +Data are stamped with the ROS time when they are received. The inherent latency +between when the data were sampled by the LiDAR and when the data were received +by this ROS node is not modelled. This approach may be acceptable to get up and +running quickly or for static applications. However, for mobile robots, +particularly those traveling at higher speeds, it is not recommended to use +this `timestamp_mode`. When running in this mode, the on-LiDAR `timestamp_mode` +will not be set by this driver. ## Extensions diff --git a/ouster_msgs/msg/Metadata.msg b/ouster_msgs/msg/Metadata.msg index 0857faa..2b933f7 100644 --- a/ouster_msgs/msg/Metadata.msg +++ b/ouster_msgs/msg/Metadata.msg @@ -1,5 +1,6 @@ string hostname string lidar_mode +string timestamp_mode float64[] beam_azimuth_angles float64[] beam_altitude_angles float64[] imu_to_sensor_transform diff --git a/ouster_msgs/package.xml b/ouster_msgs/package.xml index fc86e08..9ba5d2c 100644 --- a/ouster_msgs/package.xml +++ b/ouster_msgs/package.xml @@ -5,6 +5,7 @@ 0.1.0 ROS2 messages for ouster lidar driver Steve Macenski + Tom Panzarella Apache-2.0 ament_cmake diff --git a/ros2_ouster/include/ros2_ouster/OS1/OS1.hpp b/ros2_ouster/include/ros2_ouster/OS1/OS1.hpp index 063de1e..0607650 100644 --- a/ros2_ouster/include/ros2_ouster/OS1/OS1.hpp +++ b/ros2_ouster/include/ros2_ouster/OS1/OS1.hpp @@ -68,6 +68,14 @@ enum lidar_mode MODE_2048x10 }; +enum timestamp_mode +{ + TIME_FROM_INTERNAL_OSC = 1, + TIME_FROM_SYNC_PULSE_IN, + TIME_FROM_PTP_1588, + TIME_FROM_ROS_RECEPTION = 99 +}; + struct version { int16_t major; @@ -82,6 +90,13 @@ const std::array, 5> lidar_mode_strings = { {MODE_1024x20, "1024x20"}, {MODE_2048x10, "2048x10"}}}; +const std::array, 4> +timestamp_mode_strings = { + {{TIME_FROM_INTERNAL_OSC, "TIME_FROM_INTERNAL_OSC"}, + {TIME_FROM_SYNC_PULSE_IN, "TIME_FROM_SYNC_PULSE_IN"}, + {TIME_FROM_PTP_1588, "TIME_FROM_PTP_1588"}, + {TIME_FROM_ROS_RECEPTION, "TIME_FROM_ROS_RECEPTION"}}}; + const size_t lidar_packet_bytes = 12608; const size_t imu_packet_bytes = 48; @@ -125,9 +140,9 @@ inline std::string to_string(version v) } /** - * Get lidar mode from string + * Get version from string * @param string - * @return lidar mode corresponding to the string, or invalid_version on error + * @return version corresponding to the string, or invalid_version on error */ inline version version_of_string(const std::string & s) { @@ -178,6 +193,38 @@ inline lidar_mode lidar_mode_of_string(const std::string & s) return res == end ? lidar_mode(0) : res->first; } +/** + * Get a string representation of a timestamp_mode + * @param[in] mode The timestamp_mode to stringify + * @return A string representation of the timestamp_mode (or "UNKNOWN") + */ +inline std::string to_string(timestamp_mode mode) +{ + auto end = timestamp_mode_strings.end(); + auto res = std::find_if(timestamp_mode_strings.begin(), end, + [&](const std::pair & p) { + return p.first == mode; + }); + + return res == end ? "UNKNOWN" : res->second; +} + +/** + * Get a timestamp_mode from a string + * @param[in] s The string to convert to a timestamp_mode + * @return timestamp_mode corresponding to the string, or 0 on error + */ +inline timestamp_mode timestamp_mode_of_string(const std::string & s) +{ + auto end = timestamp_mode_strings.end(); + auto res = std::find_if(timestamp_mode_strings.begin(), end, + [&](const std::pair & p) { + return p.second == s; + }); + + return res == end ? timestamp_mode(0) : res->first; +} + /** * Get number of columns in a scan for a lidar mode * @param lidar_mode @@ -377,13 +424,17 @@ inline int cfg_socket(const char * addr) * Connect to and configure the sensor and start listening for data * @param hostname hostname or ip of the sensor * @param udp_dest_host hostname or ip where the sensor should send data + * @param mode lidar_mode defining azimuth resolution and rotation rate + * @param ts_mode method used to timestamp lidar measurements * @param lidar_port port on which the sensor will send lidar data * @param imu_port port on which the sensor will send imu data * @return pointer owning the resources associated with the connection */ inline std::shared_ptr init_client( const std::string & hostname, - const std::string & udp_dest_host, lidar_mode mode = MODE_1024x10, + const std::string & udp_dest_host, + lidar_mode mode = MODE_1024x10, + timestamp_mode ts_mode = TIME_FROM_INTERNAL_OSC, int lidar_port = 7502, int imu_port = 7503) { @@ -420,6 +471,14 @@ inline std::shared_ptr init_client( sock_fd, {"set_config_param", "lidar_mode", to_string(mode)}, res); success &= res == "set_config_param"; + if (ts_mode != TIME_FROM_ROS_RECEPTION) { + success &= do_tcp_cmd( + sock_fd, + {"set_config_param", "timestamp_mode", to_string(ts_mode)}, + res); + success &= res == "set_config_param"; + } + success &= do_tcp_cmd(sock_fd, {"get_sensor_info"}, res); success &= reader->parse(res.c_str(), res.c_str() + res.size(), &cli->meta, &errors); @@ -447,6 +506,7 @@ inline std::shared_ptr init_client( // merge extra info into metadata cli->meta["hostname"] = hostname; cli->meta["lidar_mode"] = to_string(mode); + cli->meta["timestamp_mode"] = to_string(ts_mode); cli->meta["imu_port"] = imu_port; cli->meta["lidar_port"] = lidar_port; @@ -574,13 +634,15 @@ inline ros2_ouster::Metadata parse_metadata(const std::string & meta) } } - ros2_ouster::Metadata info = {"UNKNOWN", "UNKNOWN", "UNNKOWN", "UNNKOWN", + ros2_ouster::Metadata info = { + "UNKNOWN", "UNKNOWN", "UNNKOWN", "UNNKOWN", "UNKNOWN", {}, {}, {}, {}, 7503, 7502}; info.hostname = root["hostname"].asString(); info.sn = root["prod_sn"].asString(); info.fw_rev = root["build_rev"].asString(); info.mode = root["lidar_mode"].asString(); + info.timestamp_mode = root["timestamp_mode"].asString(); info.lidar_port = root["lidar_port"].asInt(); info.imu_port = root["lidar_port"].asInt(); diff --git a/ros2_ouster/include/ros2_ouster/OS1/OS1_util.hpp b/ros2_ouster/include/ros2_ouster/OS1/OS1_util.hpp index 0945f1d..f4391a6 100644 --- a/ros2_ouster/include/ros2_ouster/OS1/OS1_util.hpp +++ b/ros2_ouster/include/ros2_ouster/OS1/OS1_util.hpp @@ -115,7 +115,7 @@ inline std::vector get_px_offset(int lidar_mode) * which data is added for every point in the scan. */ template -std::function batch_to_iter( +std::function batch_to_iter( const std::vector & xyz_lut, int W, int H, const typename iterator_type::value_type & empty, C && c, F && f) { @@ -124,7 +124,8 @@ std::function batch_to_iter( int64_t scan_ts{-1L}; - return [ = ](const uint8_t * packet_buf, iterator_type it) mutable { + return [ = ](const uint8_t * packet_buf, iterator_type it, + uint64_t override_ts) mutable { for (int icol = 0; icol < OS1::columns_per_buffer; icol++) { const uint8_t * col_buf = OS1::nth_col(icol, packet_buf); const uint16_t m_id = OS1::col_measurement_id(col_buf); @@ -142,7 +143,7 @@ std::function batch_to_iter( if (scan_ts != -1) { // zero out remaining missing columns std::fill(it + (H * next_m_id), it + (H * W), empty); - f(scan_ts); + f(override_ts == 0 ? scan_ts : override_ts); } // start new frame diff --git a/ros2_ouster/include/ros2_ouster/OS1/processors/image_processor.hpp b/ros2_ouster/include/ros2_ouster/OS1/processors/image_processor.hpp index af75bb2..afcdd53 100644 --- a/ros2_ouster/include/ros2_ouster/OS1/processors/image_processor.hpp +++ b/ros2_ouster/include/ros2_ouster/OS1/processors/image_processor.hpp @@ -173,10 +173,10 @@ class ImageProcessor : public ros2_ouster::DataProcessorInterface * @brief Process method to create images * @param data the packet data */ - bool process(uint8_t * data) override + bool process(uint8_t * data, uint64_t override_ts) override { OSImageIt it = _information_image.begin(); - _batch_and_publish(data, it); + _batch_and_publish(data, it, override_ts); return true; } @@ -207,7 +207,7 @@ class ImageProcessor : public ros2_ouster::DataProcessorInterface rclcpp_lifecycle::LifecyclePublisher::SharedPtr _intensity_image_pub; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _range_image_pub; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _noise_image_pub; - std::function _batch_and_publish; + std::function _batch_and_publish; rclcpp_lifecycle::LifecycleNode::SharedPtr _node; sensor_msgs::msg::Image _reflectivity_image; sensor_msgs::msg::Image _intensity_image; diff --git a/ros2_ouster/include/ros2_ouster/OS1/processors/imu_processor.hpp b/ros2_ouster/include/ros2_ouster/OS1/processors/imu_processor.hpp index d0a0fa1..af580de 100644 --- a/ros2_ouster/include/ros2_ouster/OS1/processors/imu_processor.hpp +++ b/ros2_ouster/include/ros2_ouster/OS1/processors/imu_processor.hpp @@ -65,10 +65,10 @@ class IMUProcessor : public ros2_ouster::DataProcessorInterface * @brief Process method to create imu * @param data the packet data */ - bool process(uint8_t * data) override + bool process(uint8_t * data, uint64_t override_ts) override { if (_pub->get_subscription_count() > 0 && _pub->is_activated()) { - _pub->publish(ros2_ouster::toMsg(data, _frame)); + _pub->publish(ros2_ouster::toMsg(data, _frame, override_ts)); } return true; } diff --git a/ros2_ouster/include/ros2_ouster/OS1/processors/pointcloud_processor.hpp b/ros2_ouster/include/ros2_ouster/OS1/processors/pointcloud_processor.hpp index d555204..9cbd610 100644 --- a/ros2_ouster/include/ros2_ouster/OS1/processors/pointcloud_processor.hpp +++ b/ros2_ouster/include/ros2_ouster/OS1/processors/pointcloud_processor.hpp @@ -87,10 +87,10 @@ class PointcloudProcessor : public ros2_ouster::DataProcessorInterface * @brief Process method to create pointcloud * @param data the packet data */ - bool process(uint8_t * data) override + bool process(uint8_t * data, uint64_t override_ts) override { pcl::PointCloud::iterator it = _cloud->begin(); - _batch_and_publish(data, it); + _batch_and_publish(data, it, override_ts); return true; } @@ -111,7 +111,8 @@ class PointcloudProcessor : public ros2_ouster::DataProcessorInterface } private: - std::function::iterator)> + std::function::iterator, uint64_t)> _batch_and_publish; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _pub; std::shared_ptr> _cloud; diff --git a/ros2_ouster/include/ros2_ouster/OS1/processors/scan_processor.hpp b/ros2_ouster/include/ros2_ouster/OS1/processors/scan_processor.hpp index dc23227..c179848 100644 --- a/ros2_ouster/include/ros2_ouster/OS1/processors/scan_processor.hpp +++ b/ros2_ouster/include/ros2_ouster/OS1/processors/scan_processor.hpp @@ -98,10 +98,10 @@ class ScanProcessor : public ros2_ouster::DataProcessorInterface * @brief Process method to create scan * @param data the packet data */ - bool process(uint8_t * data) override + bool process(uint8_t * data, uint64_t override_ts) override { OSScanIt it = _aggregated_scans.begin(); - _batch_and_publish(data, it); + _batch_and_publish(data, it, override_ts); return true; } @@ -123,7 +123,7 @@ class ScanProcessor : public ros2_ouster::DataProcessorInterface private: rclcpp_lifecycle::LifecyclePublisher::SharedPtr _pub; - std::function _batch_and_publish; + std::function _batch_and_publish; std::shared_ptr> _cloud; rclcpp_lifecycle::LifecycleNode::SharedPtr _node; std::vector _xyz_lut; diff --git a/ros2_ouster/include/ros2_ouster/conversions.hpp b/ros2_ouster/include/ros2_ouster/conversions.hpp index 850d92d..9aab719 100644 --- a/ros2_ouster/include/ros2_ouster/conversions.hpp +++ b/ros2_ouster/include/ros2_ouster/conversions.hpp @@ -67,6 +67,7 @@ inline ouster_msgs::msg::Metadata toMsg(const ros2_ouster::Metadata & mdata) ouster_msgs::msg::Metadata msg; msg.hostname = mdata.hostname; msg.lidar_mode = mdata.mode; + msg.timestamp_mode = mdata.timestamp_mode; msg.beam_azimuth_angles = mdata.beam_azimuth_angles; msg.beam_altitude_angles = mdata.beam_altitude_angles; msg.imu_to_sensor_transform = mdata.imu_to_sensor_transform; @@ -107,7 +108,8 @@ inline geometry_msgs::msg::TransformStamped toMsg( */ inline sensor_msgs::msg::Imu toMsg( const uint8_t * buf, - const std::string & frame) + const std::string & frame, + uint64_t override_ts = 0) { const double standard_g = 9.80665; sensor_msgs::msg::Imu m; @@ -116,8 +118,8 @@ inline sensor_msgs::msg::Imu toMsg( m.orientation.z = 0; m.orientation.w = 1; - rclcpp::Time t(OS1::imu_gyro_ts(buf)); - m.header.stamp = t; + m.header.stamp = override_ts == 0 ? + rclcpp::Time(OS1::imu_gyro_ts(buf)) : rclcpp::Time(override_ts); m.header.frame_id = frame; m.linear_acceleration.x = OS1::imu_la_x(buf) * standard_g; diff --git a/ros2_ouster/include/ros2_ouster/interfaces/configuration.hpp b/ros2_ouster/include/ros2_ouster/interfaces/configuration.hpp index 595b0e3..e85dddf 100644 --- a/ros2_ouster/include/ros2_ouster/interfaces/configuration.hpp +++ b/ros2_ouster/include/ros2_ouster/interfaces/configuration.hpp @@ -28,6 +28,7 @@ struct Configuration int imu_port; int lidar_port; std::string lidar_mode; + std::string timestamp_mode; }; } // namespace ros2_ouster diff --git a/ros2_ouster/include/ros2_ouster/interfaces/data_processor_interface.hpp b/ros2_ouster/include/ros2_ouster/interfaces/data_processor_interface.hpp index 9d0f93c..d6f8fef 100644 --- a/ros2_ouster/include/ros2_ouster/interfaces/data_processor_interface.hpp +++ b/ros2_ouster/include/ros2_ouster/interfaces/data_processor_interface.hpp @@ -46,8 +46,10 @@ class DataProcessorInterface /** * @brief Process a packet with the lidar-specific APIs * @param data packet input + * @param override_ts Timestamp in nanos to use to override the ts in the + * packet data. To use the packet data, pass as 0. */ - virtual bool process(uint8_t * data) = 0; + virtual bool process(uint8_t * data, uint64_t override_ts = 0) = 0; /** * @brief Activating processor from lifecycle state transitions diff --git a/ros2_ouster/include/ros2_ouster/interfaces/metadata.hpp b/ros2_ouster/include/ros2_ouster/interfaces/metadata.hpp index 560041c..a5d6987 100644 --- a/ros2_ouster/include/ros2_ouster/interfaces/metadata.hpp +++ b/ros2_ouster/include/ros2_ouster/interfaces/metadata.hpp @@ -41,6 +41,7 @@ struct Metadata std::string sn; std::string fw_rev; std::string mode; + std::string timestamp_mode; std::vector beam_azimuth_angles; std::vector beam_altitude_angles; std::vector imu_to_sensor_transform; diff --git a/ros2_ouster/include/ros2_ouster/ouster_driver.hpp b/ros2_ouster/include/ros2_ouster/ouster_driver.hpp index f7cd697..4d2b5ff 100644 --- a/ros2_ouster/include/ros2_ouster/ouster_driver.hpp +++ b/ros2_ouster/include/ros2_ouster/ouster_driver.hpp @@ -138,6 +138,7 @@ class OusterDriver : public lifecycle_interface::LifecycleInterface std::unique_ptr _tf_b; bool _use_system_default_qos; + bool _use_ros_time; }; } // namespace ros2_ouster diff --git a/ros2_ouster/package.xml b/ros2_ouster/package.xml index b7c5fa3..c95a9be 100644 --- a/ros2_ouster/package.xml +++ b/ros2_ouster/package.xml @@ -5,6 +5,7 @@ 0.1.0 ROS2 Drivers for the Ouster OS-1 Lidar Steve Macenski + Tom Panzarella Apache-2.0 ament_cmake diff --git a/ros2_ouster/params/os1.yaml b/ros2_ouster/params/os1.yaml index b0828c0..f660ba0 100644 --- a/ros2_ouster/params/os1.yaml +++ b/ros2_ouster/params/os1.yaml @@ -13,3 +13,16 @@ ouster_driver: # 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 diff --git a/ros2_ouster/src/OS1/OS1_sensor.cpp b/ros2_ouster/src/OS1/OS1_sensor.cpp index f28b2e5..4aef05c 100644 --- a/ros2_ouster/src/OS1/OS1_sensor.cpp +++ b/ros2_ouster/src/OS1/OS1_sensor.cpp @@ -49,9 +49,17 @@ void OS1Sensor::configure(const ros2_ouster::Configuration & config) exit(-1); } + if (!OS1::timestamp_mode_of_string(config.timestamp_mode)) { + throw ros2_ouster::OusterDriverException( + std::string( + "Invalid timestamp mode %s!", config.timestamp_mode.c_str())); + exit(-1); + } + _ouster_client = OS1::init_client( config.lidar_ip, config.computer_ip, OS1::lidar_mode_of_string(config.lidar_mode), + OS1::timestamp_mode_of_string(config.timestamp_mode), config.lidar_port, config.imu_port); if (!_ouster_client) { @@ -102,7 +110,7 @@ ros2_ouster::Metadata OS1Sensor::getMetadata() if (_ouster_client) { return OS1::parse_metadata(OS1::get_metadata(*_ouster_client)); } else { - return {"UNKNOWN", "UNKNOWN", "UNNKOWN", "UNNKOWN", + return {"UNKNOWN", "UNKNOWN", "UNNKOWN", "UNNKOWN", "UNKNOWN", {}, {}, {}, {}, 7503, 7502}; } } diff --git a/ros2_ouster/src/ouster_driver.cpp b/ros2_ouster/src/ouster_driver.cpp index c1bebfa..26d6bd5 100644 --- a/ros2_ouster/src/ouster_driver.cpp +++ b/ros2_ouster/src/ouster_driver.cpp @@ -38,6 +38,8 @@ OusterDriver::OusterDriver(const rclcpp::NodeOptions & options) 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")); 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")); @@ -66,6 +68,15 @@ void OusterDriver::onConfigure() 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(), + "Using TIME_FROM_ROS_RECEPTION to stamp data with ROS time on " + "reception. This has unmodelled latency!"); + _use_ros_time = true; + } else { + _use_ros_time = false; + } _laser_sensor_frame = get_parameter("sensor_frame").as_string(); _laser_data_frame = get_parameter("laser_frame").as_string(); @@ -191,8 +202,11 @@ void OusterDriver::processData() if (packet_data) { std::pair key_its; key_its = _data_processors.equal_range(state); + uint64_t override_ts = + this->_use_ros_time ? this->now().nanoseconds() : 0; + for (DataProcessorMapIt it = key_its.first; it != key_its.second; it++) { - it->second->process(packet_data); + it->second->process(packet_data, override_ts); } } } catch (const OusterDriverException & e) { @@ -217,6 +231,7 @@ void OusterDriver::resetService( 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(); _sensor->reset(lidar_config); }