Add timestamping_mode parameter

include/ouster/os1.h

@@ -71,6 +71,16 @@ typedef enum {
 } operation_mode_t;
 using OperationMode = operation_mode_t;
 
+/**
+ * Timestamp modes supported by the OS1 LiDAR
+ */
+typedef enum {
+      TIME_FROM_INTERNAL_OSC=0,
+      TIME_FROM_PTP_1588=1,
+      TIME_FROM_SYNC_PULSE_IN=2,
+} timestamp_mode_t;
+using TimestampMode = timestamp_mode_t;
+
 /**
  * Laser pulse modes supported by the OS1 LiDAR
  */
@@ -86,7 +96,8 @@ using PulseMode = pulse_mode_t;
  * @param window_rejection to reject short range data (true), or to accept short range data (false)
  * @note This function was added to configure advanced operation modes for Autoware
  */
-void set_advanced_params(std::string operation_mode_str, std::string pulse_mode_str, bool window_rejection);
+void set_advanced_params(std::string operation_mode_str, std::string timestamp_mode_str,
+                         std::string pulse_mode_str, bool window_rejection);
 
 }
 }

launch/os1.launch

@@ -15,6 +15,7 @@
   <arg name="imu_frame_name" default="imu" doc="Frame name for IMU output message"/>
   <arg name="imu_topic_name" default="/imu_raw" doc="Topic name for IMU output message"/>
   <arg name="operation_mode" default="1024x10" doc="Supported operation modes: 512x10, 1024x10, 2048x10, 512x20, 1024x20 (horizontal resolution x scan rate)"/>
+  <arg name="timestamp_mode" default="TIME_FROM_INTERNAL_OSC" doc="Supported timestamp modes: TIME_FROM_INTERNAL_OSC, TIME_FROM_PTP_1588, TIME_FROM_SYNC_PULSE_IN"/>
   <arg name="pulse_mode" default="STANDARD" doc="Laser pulse width, available modes: STANDARD and NARROW"/>
   <arg name="window_rejection" default="true" doc="Window (short range) rejection enable"/>
 
@@ -31,6 +32,7 @@
     <param name="imu_frame_name" value="$(arg imu_frame_name)"/>
     <param name="pointcloud_mode" value="$(arg pointcloud_mode)"/>
     <param name="operation_mode" value="$(arg operation_mode)"/>
+    <param name="timestamp_mode" value="$(arg timestamp_mode)"/>
     <param name="pulse_mode" value="$(arg pulse_mode)"/>
     <param name="window_rejection" value="$(arg window_rejection)"/>
   </node>

src/os1.cpp

@@ -24,9 +24,11 @@ using ns = std::chrono::nanoseconds;
  * @note Added to support advanced mode parameters configuration for Autoware
  */
 static OperationMode _operation_mode = ouster::OS1::MODE_1024x10;
+static TimestampMode _timestamp_mode = ouster::OS1::TIME_FROM_INTERNAL_OSC;
 static PulseMode _pulse_mode = ouster::OS1::PULSE_STANDARD;
 static bool _window_rejection = true;
 static std::string _operation_mode_str = "";
+static std::string _timestamp_mode_str = "";
 static std::string _pulse_mode_str = "";
 static std::string _window_rejection_str = "";
 //----------------
@@ -232,6 +234,7 @@ std::shared_ptr<client> init_client(const std::string& hostname,
 
     //read the current settings
     auto curr_operation_mode_str = get_cmd("get_config_param", "active lidar_mode");
+    auto curr_timestamp_mode_str = get_cmd("get_config_param", "active timestamp_mode");
     auto curr_pulse_mode_str = std::string("");
     if (has_pulsemode) {
     	curr_pulse_mode_str = get_cmd("get_config_param", "active pulse_mode");
@@ -245,6 +248,10 @@ std::shared_ptr<client> init_client(const std::string& hostname,
     	success &= do_cmd_chk("set_config_param", "lidar_mode " + _operation_mode_str);
     	do_configure = true;
    	}
+    if (curr_timestamp_mode_str != _timestamp_mode_str) {
+        success &= do_cmd_chk("set_config_param", "timestamp_mode " + _timestamp_mode_str);
+        do_configure = true;
+    }
    	if (has_pulsemode && (curr_pulse_mode_str != _pulse_mode_str)) {
 	    success &= do_cmd_chk("set_config_param", "pulse_mode " + _pulse_mode_str);
 	    do_configure = true;
@@ -320,9 +327,11 @@ bool read_imu_packet(const client& cli, uint8_t* buf) {
  * @note Added to support advanced mode parameters configuration for Autoware
  */
 //----------------
-void set_advanced_params(std::string operation_mode_str, std::string pulse_mode_str, bool window_rejection)
+void set_advanced_params(std::string operation_mode_str, std::string timestamp_mode_str,
+                         std::string pulse_mode_str, bool window_rejection)
 {
    _operation_mode_str = operation_mode_str;
+   _timestamp_mode_str = timestamp_mode_str;
    _pulse_mode_str = pulse_mode_str;
    _window_rejection = window_rejection;
 
@@ -339,15 +348,29 @@ void set_advanced_params(std::string operation_mode_str, std::string pulse_mode_
        _operation_mode = ouster::OS1::MODE_1024x20;
    } else {
    	   std::cout << "Selected operation mode " << _operation_mode_str << " is invalid, using default mode \"1024x10\"" << std::endl;
+       _operation_mode_str = "1024x10";
    }
 
+    _timestamp_mode = ouster::OS1::TIME_FROM_INTERNAL_OSC;
+    if (_timestamp_mode_str == std::string("TIME_FROM_INTERNAL_OSC")) {
+        _timestamp_mode = ouster::OS1::TIME_FROM_INTERNAL_OSC;
+    } else if (_timestamp_mode_str == std::string("TIME_FROM_PTP_1588")) {
+        _timestamp_mode = ouster::OS1::TIME_FROM_PTP_1588;
+    } else if (_timestamp_mode_str == std::string("TIME_FROM_SYNC_PULSE_IN")) {
+        _timestamp_mode = ouster::OS1::TIME_FROM_SYNC_PULSE_IN;
+    } else {
+        std::cout << "Selected timestamp mode " << _timestamp_mode_str << " is invalid, using default mode \"TIME_FROM_INTERNAL_OSC\"" << std::endl;
+        _timestamp_mode_str = "TIME_FROM_INTERNAL_OSC";
+    }
+
    _pulse_mode = ouster::OS1::PULSE_STANDARD;
    if (_pulse_mode_str == std::string("STANDARD")) {
    	   _pulse_mode = ouster::OS1::PULSE_STANDARD;
    } else if (_pulse_mode_str == std::string("NARROW")) {
    	   _pulse_mode = ouster::OS1::PULSE_NARROW;
    } else {
    	   std::cout << "Selected pulse mode " << _pulse_mode_str << " is invalid, using default mode \"STANDARD\"" << std::endl;
+       _pulse_mode_str = "STANDARD";
    }
 
    _window_rejection_str = ((_window_rejection) ? std::string("1") : std::string("0"));

src/os1_node.cpp

@@ -49,6 +49,7 @@ int main(int argc, char** argv) {
     auto imu_frame_name = nh.param("imu_frame_name", std::string("/os1_imu"));
     auto pointcloud_mode = nh.param("pointcloud_mode", std::string("NATIVE"));
     auto operation_mode_str = nh.param("operation_mode", std::string("1024x10"));
+    auto timestamp_mode_str = nh.param("timestamp_mode", std::string("TIME_FROM_INTERNAL_OSC"));
     auto pulse_mode_str = nh.param("pulse_mode", std::string("STANDARD"));
     auto window_rejection = nh.param("window_rejection", true);
 
@@ -62,7 +63,8 @@ int main(int argc, char** argv) {
      * @note Added to support advanced mode parameters configuration for Autoware
      */
     //defines the advanced parameters
-    ouster::OS1::set_advanced_params(operation_mode_str, pulse_mode_str, window_rejection);
+    ouster::OS1::set_advanced_params(operation_mode_str, timestamp_mode_str,
+                                     pulse_mode_str, window_rejection);
     auto queue_size = 10;
     if (operation_mode_str == std::string("512x20") || operation_mode_str == std::string("1024x20")) {
     	queue_size = 20;
@@ -75,6 +77,12 @@ int main(int argc, char** argv) {
 
     auto lidar_handler = ouster_driver::OS1::batch_packets(
         scan_dur, [&](ns scan_ts, const ouster_driver::OS1::CloudOS1& cloud) {
+            /**
+            * @note Changed timestamp from LiDAR to ROS time for Autoware operation,
+            *       unless time is already synchronized using PTP
+            */
+            if (timestamp_mode_str != std::string("TIME_FROM_PTP_1588"))
+                scan_ts = ns(ros::Time::now().toNSec());
             lidar_pub.publish(
                 ouster_driver::OS1::cloud_to_cloud_msg(cloud, scan_ts, lidar_frame_name));
         });

src/os1_ros.cpp

@@ -108,11 +108,7 @@ sensor_msgs::PointCloud2 cloud_to_cloud_msg(const CloudOS1& cloud, ns timestamp,
     }
 
     msg.header.frame_id = frame;
-    /**
-     * @note Changed timestamp from LiDAR to ROS time for Autoware operation
-     */
-    //msg.header.stamp.fromNSec(timestamp.count()); //<-- original code of OS1 driver
-    msg.header.stamp = ros::Time::now();  //<-- prefered time mode in Autoware
+    msg.header.stamp.fromNSec(timestamp.count());
     return msg;
 }
 
@@ -193,7 +189,7 @@ std::function<void(const PacketMsg&)> batch_packets(
 
         auto batch_dur = packet_ts - scan_ts;
         if (batch_dur >= scan_dur || batch_dur < ns(0)) {
-            f(scan_ts, *cloud);
+            f(packet_ts, *cloud);
 
             cloud->clear();
             scan_ts = ns(-1L);