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

@@ -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`|
 
 </center>
 
-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
 

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

ouster_msgs/package.xml

@@ -5,6 +5,7 @@
   <version>0.1.0</version>
   <description>ROS2 messages for ouster lidar driver</description>
   <maintainer email="[email protected]">Steve Macenski</maintainer>
+  <maintainer email="[email protected]">Tom Panzarella</maintainer>
   <license>Apache-2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>

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<std::pair<lidar_mode, std::string>, 5> lidar_mode_strings = {
     {MODE_1024x20, "1024x20"},
     {MODE_2048x10, "2048x10"}}};
 
+const std::array<std::pair<timestamp_mode, std::string>, 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<timestamp_mode, std::string> & 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<timestamp_mode, std::string> & 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<client> 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<client> 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<client> 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();
 

ros2_ouster/include/ros2_ouster/OS1/OS1_util.hpp

@@ -115,7 +115,7 @@ inline std::vector<int> get_px_offset(int lidar_mode)
  * which data is added for every point in the scan.
  */
 template<typename iterator_type, typename F, typename C>
-std::function<void(const uint8_t *, iterator_type it)> batch_to_iter(
+std::function<void(const uint8_t *, iterator_type it, uint64_t)> batch_to_iter(
   const std::vector<double> & xyz_lut, int W, int H,
   const typename iterator_type::value_type & empty, C && c, F && f)
 {
@@ -124,7 +124,8 @@ std::function<void(const uint8_t *, iterator_type it)> 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<void(const uint8_t *, iterator_type it)> 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

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<sensor_msgs::msg::Image>::SharedPtr _intensity_image_pub;
   rclcpp_lifecycle::LifecyclePublisher<sensor_msgs::msg::Image>::SharedPtr _range_image_pub;
   rclcpp_lifecycle::LifecyclePublisher<sensor_msgs::msg::Image>::SharedPtr _noise_image_pub;
-  std::function<void(const uint8_t *, OSImageIt)> _batch_and_publish;
+  std::function<void(const uint8_t *, OSImageIt, uint64_t)> _batch_and_publish;
   rclcpp_lifecycle::LifecycleNode::SharedPtr _node;
   sensor_msgs::msg::Image _reflectivity_image;
   sensor_msgs::msg::Image _intensity_image;

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;
   }

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<point_os::PointOS>::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<void(const uint8_t *, pcl::PointCloud<point_os::PointOS>::iterator)>
+  std::function<void(const uint8_t *,
+    pcl::PointCloud<point_os::PointOS>::iterator, uint64_t)>
   _batch_and_publish;
   rclcpp_lifecycle::LifecyclePublisher<sensor_msgs::msg::PointCloud2>::SharedPtr _pub;
   std::shared_ptr<pcl::PointCloud<point_os::PointOS>> _cloud;

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<sensor_msgs::msg::LaserScan>::SharedPtr _pub;
-  std::function<void(const uint8_t *, OSScanIt)> _batch_and_publish;
+  std::function<void(const uint8_t *, OSScanIt, uint64_t)> _batch_and_publish;
   std::shared_ptr<pcl::PointCloud<scan_os::ScanOS>> _cloud;
   rclcpp_lifecycle::LifecycleNode::SharedPtr _node;
   std::vector<double> _xyz_lut;

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;

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

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