Added support for phase locking and external synchronization.

README.md

@@ -53,6 +53,12 @@ See design doc in `design/*` directory [here](ros2_ouster/design/design_doc.md).
 | `timestamp_mode`         | String  | Method used to timestamp measurements, default `TIME_FROM_INTERNAL_OSC`                                     |
 | `os1_proc_mask`          | String  | Mask encoding data processors to activate, default <code>IMG &#124; PCL &#124; IMU &#124; SCAN</code> |
 | `pointcloud_filter_zero_points` | bool | Reduce pointcloud size by omitting (0, 0, 0) points, default `False`. If used, will make the PC2 unstructured.     |
+| `multipurpose_io_mode`   | String  | Mode of multipurpose IO, default `OFF`                                                                      |
+| `nmea_in_polarity`       | String  | NMEA polarity, default `ACTIVE_HIGH`                                                                        |
+| `sync_pulse_in_polarity` | String  | Polarity of the hardware Sync line, default `ACTIVE_HIGH`                                                   |
+| `nmea_baud_rate`         | String  | NMEA baud rate, default `BAUD_9600`                                                                         |
+| `phase_lock_enable`      | bool    | Phase lock enabled, default `false`                                                                         |
+| `phase_lock_offset`      | int     | Offset between 0 and 360000 to begin the phase lock, default `0`                                            |
 
 
 </center>

ros2_ouster/include/ros2_ouster/client/client.h

@@ -58,7 +58,13 @@ namespace ouster {
       timestamp_mode ts_mode = TIME_FROM_UNSPEC,
       int lidar_port = 0,
       int imu_port = 0,
-      int timeout_sec = 60);
+      int timeout_sec = 60,
+      optional<MultipurposeIOMode> multipurpose_io_mode = MULTIPURPOSE_OFF,
+      optional<Polarity> nmea_polarity = POLARITY_ACTIVE_LOW,
+      optional<Polarity> sync_pulse_polarity = POLARITY_ACTIVE_HIGH,
+      optional<NMEABaudRate> nmea_baud_rate = BAUD_9600,
+      bool phase_lock_enable = false,
+      int phase_lock_offset = 0);
 
     /**
      * Block for up to timeout_sec until either data is ready or an error occurs.

ros2_ouster/include/ros2_ouster/client/types.h

@@ -293,6 +293,14 @@ namespace ouster {
      */
     std::string to_string(AzimuthWindow azimuth_window);
 
+    /**
+     * Get string representation of a bool.
+     *
+     * @param value
+     * @return string representation of bool.
+     */
+    std::string to_string(bool value);
+
     /**
      * Parse metadata text blob from the sensor into a sensor_info struct.
      *

ros2_ouster/include/ros2_ouster/interfaces/configuration.hpp

@@ -31,6 +31,12 @@ struct Configuration
   std::string timestamp_mode;
   std::string metadata_filepath;
   std::string ethernet_device;
+  std::string multipurpose_io_mode;
+  std::string nmea_in_polarity;
+  std::string nmea_baud_rate;
+  std::string sync_pulse_in_polarity;
+  bool phase_lock_enable;
+  int phase_lock_offset;
 };
 
 }  // namespace ros2_ouster

ros2_ouster/src/client/client.cpp

@@ -546,9 +546,17 @@ std::shared_ptr<client> init_client(
 std::shared_ptr<client> init_client(
   const std::string & hostname,
   const std::string & udp_dest_host,
-  lidar_mode mode, timestamp_mode ts_mode,
-  int lidar_port, int imu_port,
-  int timeout_sec)
+  lidar_mode mode,
+  timestamp_mode ts_mode,
+  int lidar_port,
+  int imu_port,
+  int timeout_sec,
+  optional<MultipurposeIOMode> multipurpose_io_mode,
+  optional<Polarity> nmea_polarity,
+  optional<Polarity> sync_pulse_polarity,
+  optional<NMEABaudRate> nmea_baud_rate,
+  bool phase_lock_enable,
+  int phase_lock_offset)
 {
   auto cli = init_client(hostname, lidar_port, imu_port);
   if (!cli) {return std::shared_ptr<client>();}
@@ -605,6 +613,44 @@ std::shared_ptr<client> init_client(
     success &= res == "set_config_param";
   }
 
+  if (multipurpose_io_mode.has_value()) {
+    success &= do_tcp_cmd(
+      sock_fd, {"set_config_param", "multipurpose_io_mode", to_string(multipurpose_io_mode.value())},
+      res);
+    success &= res == "set_config_param";
+  }
+
+  if (nmea_polarity.has_value()) {
+    success &= do_tcp_cmd(
+      sock_fd, {"set_config_param", "nmea_in_polarity", to_string(nmea_polarity.value())},
+      res);
+    success &= res == "set_config_param";
+  }
+
+  if (sync_pulse_polarity.has_value()) {
+    success &= do_tcp_cmd(
+      sock_fd, {"set_config_param", "sync_pulse_in_polarity", to_string(sync_pulse_polarity.value())},
+      res);
+    success &= res == "set_config_param";
+  }
+
+  if (nmea_baud_rate.has_value()) {
+    success &= do_tcp_cmd(
+      sock_fd, {"set_config_param", "nmea_baud_rate", to_string(nmea_baud_rate.value())},
+      res);
+    success &= res == "set_config_param";
+  }
+
+  success &= do_tcp_cmd(
+    sock_fd, {"set_config_param", "phase_lock_enable", to_string(phase_lock_enable)},
+    res);
+  success &= res == "set_config_param";
+
+  success &= do_tcp_cmd(
+  sock_fd, {"set_config_param", "phase_lock_offset", std::to_string(phase_lock_offset)},
+  res);
+  success &= res == "set_config_param";
+
   // wake up from STANDBY, if necessary
   success &= do_tcp_cmd(
     sock_fd, {"set_config_param", "operating_mode", "NORMAL"}, res);

ros2_ouster/src/client/types.cpp

@@ -426,6 +426,11 @@ std::string to_string(AzimuthWindow azimuth_window)
   return ss.str();
 }
 
+std::string to_string(bool value) 
+{
+  return value ? "true" : "false";
+}
+
 bool operator==(const AzimuthWindow& lhs, const AzimuthWindow& rhs) 
 {
   return (lhs.first == rhs.first && lhs.second == rhs.second);

ros2_ouster/src/ouster_driver.cpp

@@ -54,6 +54,12 @@ OusterDriver::OusterDriver(
   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("multipurpose_io_mode", std::string("OFF"));
+  this->declare_parameter("nmea_in_polarity", std::string("ACTIVE_HIGH"));
+  this->declare_parameter("sync_pulse_in_polarity", std::string("ACTIVE_HIGH"));
+  this->declare_parameter("nmea_baud_rate", std::string("BAUD_9600"));
+  this->declare_parameter("phase_lock_enable", false);
+  this->declare_parameter<int>("phase_lock_offset", 0);
 }
 
 OusterDriver::~OusterDriver() = default;
@@ -75,6 +81,12 @@ void OusterDriver::onConfigure()
   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();
+  lidar_config.multipurpose_io_mode = this->get_parameter("multipurpose_io_mode").as_string();
+  lidar_config.nmea_in_polarity = this->get_parameter("nmea_in_polarity").as_string();
+  lidar_config.nmea_baud_rate = this->get_parameter("nmea_baud_rate").as_string();
+  lidar_config.sync_pulse_in_polarity = this->get_parameter("sync_pulse_in_polarity").as_string();
+  lidar_config.phase_lock_enable = this->get_parameter("phase_lock_enable").as_bool();
+  lidar_config.phase_lock_offset = this->get_parameter("phase_lock_offset").as_int();
 
   // Deliberately retrieve the IP parameters in a try block without defaults, as
   // we cannot estimate a reasonable default IP address for the LiDAR/computer.

ros2_ouster/src/sensor.cpp

@@ -59,6 +59,45 @@ void Sensor::configure(
     exit(-1);
   }
 
+  if (!ouster::sensor::multipurpose_io_mode_of_string(config.multipurpose_io_mode)) {
+    throw ros2_ouster::OusterDriverException(
+            "Invaild multipurpose io mode: " + config.multipurpose_io_mode);
+    exit(-1);
+  }
+
+  if (!ouster::sensor::polarity_of_string(config.nmea_in_polarity)) {
+    throw ros2_ouster::OusterDriverException(
+            "Invaild nmea in polarity: " + config.nmea_in_polarity);
+    exit(-1);
+  }
+
+  if (!ouster::sensor::polarity_of_string(config.sync_pulse_in_polarity)) {
+    throw ros2_ouster::OusterDriverException(
+            "Invaild sync pulse in polarity: " + config.sync_pulse_in_polarity);
+    exit(-1);
+  }
+
+  if (!ouster::sensor::nmea_baud_rate_of_string(config.nmea_baud_rate)) {
+    throw ros2_ouster::OusterDriverException(
+            "Invaild nmea baud rate: " + config.nmea_baud_rate);
+    exit(-1);
+  }
+
+  // Phase locking is only supported when time-synchronized from an external source (page 28. Ouster Software User Manual)
+  if(config.phase_lock_enable){
+    if (!(config.timestamp_mode == "TIME_FROM_PTP_1588" || config.timestamp_mode == "TIME_FROM_SYNC_PULSE_IN")) {
+      throw ros2_ouster::OusterDriverException(
+            "Phase Locking is only available when using external time-synchronization (TIME_FROM_PTP_1588 or TIME_FROM_SYNC_PULSE_IN):" + config.timestamp_mode);
+      exit(-1);
+    }
+  }
+
+  if(config.phase_lock_offset < 0 || config.phase_lock_enable > 360000) {
+    throw ros2_ouster::OusterDriverException(
+          "Phase lock offset must be between 0 and 360000 (0 to 360 degrees)" + std::to_string(config.phase_lock_offset));
+      exit(-1);
+  }
+
   // Report to the user whether automatic address detection is being used, and 
   // what the source / destination IPs are
   RCLCPP_INFO(
@@ -80,7 +119,14 @@ void Sensor::configure(
     ouster::sensor::lidar_mode_of_string(config.lidar_mode),
     ouster::sensor::timestamp_mode_of_string(config.timestamp_mode),
     config.lidar_port,
-    config.imu_port);
+    config.imu_port,
+    60,
+    ouster::sensor::multipurpose_io_mode_of_string(config.multipurpose_io_mode),
+    ouster::sensor::polarity_of_string(config.nmea_in_polarity),
+    ouster::sensor::polarity_of_string(config.sync_pulse_in_polarity),
+    ouster::sensor::nmea_baud_rate_of_string(config.nmea_baud_rate),
+    config.phase_lock_enable,
+    config.phase_lock_offset);
 
   if (!_ouster_client) {
     throw ros2_ouster::OusterDriverException(