Add support for rev7 fw v3.0.1 ouster sensors

ouster_msgs/msg/Metadata.msg

@@ -3,9 +3,10 @@ string lidar_mode
 string timestamp_mode
 float64[] beam_azimuth_angles
 float64[] beam_altitude_angles
+float64[] beam_to_lidar_transform
 float64[] imu_to_sensor_transform
 float64[] lidar_to_sensor_transform
 string serial_no
 string firmware_rev
-int8 imu_port
-int8 lidar_port
+uint16 imu_port
+uint16 lidar_port

ros2_ouster/CMakeLists.txt

@@ -22,6 +22,8 @@ find_package(pcl_conversions REQUIRED)
 find_package(ouster_msgs REQUIRED)
 find_package(PCL REQUIRED COMPONENTS common)
 find_package(jsoncpp REQUIRED)
+find_package(CURL REQUIRED)
+find_package(tf2_eigen REQUIRED)
 
 include_directories(
   include
@@ -48,6 +50,7 @@ set(dependencies
   ouster_msgs
   tf2_geometry_msgs
   pcl_conversions
+  tf2_eigen
 )
 
 add_library(${library_name} SHARED
@@ -60,6 +63,7 @@ add_library(${library_name} SHARED
   src/client/lidar_scan.cpp
   src/client/netcompat.cpp
   src/client/types.cpp
+  src/client/parsing.cpp
 )
 
 ament_target_dependencies(${library_name}
@@ -69,6 +73,7 @@ ament_target_dependencies(${library_name}
 target_link_libraries(${library_name}
   jsoncpp_lib
   tins
+  CURL::libcurl
   ${PCL_LIBRARIES}
 )
 

ros2_ouster/include/ros2_ouster/client/client.h

@@ -17,13 +17,14 @@ namespace ouster {
 
     struct client;
 
+    /** Returned by poll_client. */
     enum client_state
     {
-      TIMEOUT = 0,
-      CLIENT_ERROR = 1,
-      LIDAR_DATA = 2,
-      IMU_DATA = 4,
-      EXIT = 8
+      TIMEOUT = 0,     ///< Client has timed out
+      CLIENT_ERROR = 1,///< Client has reported an error
+      LIDAR_DATA = 2,  ///< New lidar data available
+      IMU_DATA = 4,    ///< New IMU data available
+      EXIT = 8         ///< Client has exited
     };
 
     /** Minimum supported version. */
@@ -96,40 +97,69 @@ namespace ouster {
     bool read_imu_packet(const client & cli, uint8_t * buf, const packet_format & pf);
 
     /**
-     * Get metadata text blob from the sensor.
-     *
-     * Will attempt to fetch from the network if not already populated.
-     *
-     * @param cli client returned by init_client associated with the connection
-     * @param timeout_sec how long to wait for the sensor to initialize
-     * @return a text blob of metadata parseable into a sensor_info struct
-     */
-    std::string get_metadata(client & cli, int timeout_sec = 60);
+ * Get metadata text blob from the sensor.
+ *
+ * Will attempt to fetch from the network if not already populated.
+ *
+ * @throw runtime_error if the sensor is in ERROR state, the firmware version
+ * used to initialize the HTTP or TCP client is invalid, the metadata could
+ * not be retrieved from the sensor, or the response could not be parsed.
+ *
+ * @param[in] cli client returned by init_client associated with the connection.
+ * @param[in] timeout_sec how long to wait for the sensor to initialize.
+ * @param[in] legacy_format whether to use legacy format of metadata output.
+ *
+ * @return a text blob of metadata parseable into a sensor_info struct.
+ */
+    std::string get_metadata(client& cli, int timeout_sec = 60,
+                             bool legacy_format = false);
 
     /**
-     * Get sensor config from the sensor
-     *
-     * Populates passed in config with the results of get_config
-     *
-     * @param hostname sensor hostname
-     * @param config sensor config to populate
-     * @param active whether to pull active or passive configs
-     * @return true if sensor config successfully populated
-     */
+ * Get sensor config from the sensor.
+ *
+ * Populates passed in config with the results of get_config.
+ *
+ * @param[in] hostname sensor hostname.
+ * @param[out] config sensor config to populate.
+ * @param[in] active whether to pull active or passive configs.
+ *
+ * @return true if sensor config successfully populated.
+ */
     bool get_config(const std::string& hostname, sensor_config& config,
                     bool active = true);
 
     /**
-     * Set sensor config on sensor
-     *
-     * @param hostname sensor hostname
-     * @param sensor config
-     * @param flags flags to pass in
-     * @return true if config params successfuly set on sensor
-     */
+ * Set sensor config on sensor.
+ *
+ * @throw runtime_error on failure to communcate with the sensor.
+ * @throw invalid_argument when config parameters fail validation.
+ *
+ * @param[in] hostname sensor hostname.
+ * @param[in] config sensor config.
+ * @param[in] config_flags flags to pass in.
+ *
+ * @return true if config params successfuly set on sensor.
+ */
     bool set_config(const std::string& hostname, const sensor_config& config,
                     uint8_t config_flags = 0);
 
+    /**
+ * Return the port used to listen for lidar UDP data.
+ *
+ * @param[in] cli client returned by init_client associated with the connection.
+ *
+ * @return the port number.
+ */
+    int get_lidar_port(client& cli);
+
+    /**
+ * Return the port used to listen for imu UDP data.
+ *
+ * @param[in] cli client returned by init_client associated with the connection.
+ *
+ * @return the port number.
+ */
+    int get_imu_port(client& cli);
 
   } // namespace sensor
 }  // namespace ouster

ros2_ouster/include/ros2_ouster/client/client_factory.hpp

@@ -0,0 +1,52 @@
+//
+// Created by user on 23/05/23.
+//
+
+#ifndef ROS2_OUSTER__OS1__OS1_BASE_FACTORY_HPP_
+#define ROS2_OUSTER__OS1__OS1_BASE_FACTORY_HPP_
+
+#include "ros2_ouster/client/interfaces/client_interface.hpp"
+#include "ros2_ouster/client/impl/http.hpp"
+#include "ros2_ouster/client/impl/tcp.hpp"
+
+namespace ouster
+{
+namespace sensor
+{
+
+/**
+     * Creates an instance of the SensorHttp interface.
+     *
+     * @param[in] hostname hostname of the sensor to communicate with.
+     */
+inline std::unique_ptr<util::ClientInterface>
+util::ClientInterface::create(const std::string &hostname)
+{
+  auto fw = firmware_version(hostname);
+
+  if (fw == ouster::util::invalid_version || fw.major < 2) {
+    throw std::runtime_error(
+            "SensorHttp:: create firmware version information unavailable or "
+            "not fully supported version. Please upgrade your sensor to FW "
+            "2.0 or later.");
+  }
+
+  if (fw.major == 2) {
+    switch (fw.minor) {
+      case 0:
+        // FW 2.0 doesn't work properly with http
+        return std::make_unique<impl::TcpImpl>(hostname);
+        //      case 1:
+        //        return std::make_unique<SensorHttpImp_2_1>(hostname);
+        //      case 2:
+        //        return std::make_unique<SensorHttpImp_2_2>(hostname);
+    }
+  }
+
+  return std::make_unique<impl::HttpImpl>(hostname);
+}
+
+}// namespace sensor
+}// namespace ouster
+
+#endif//ROS2_OUSTER_OS1_BASE_FACTORY_H

ros2_ouster/include/ros2_ouster/client/impl/curl_client.hpp

@@ -0,0 +1,116 @@
+#ifndef ROS2_OUSTER__CLIENT__INTERFACES__CURL_CLIENT_HPP_
+#define ROS2_OUSTER__CLIENT__INTERFACES__CURL_CLIENT_HPP_
+
+#include <curl/curl.h>
+
+#include <cstring>
+#include <utility>
+
+namespace ouster
+{
+namespace util
+{
+
+class CurlClient
+{
+  public:
+  explicit CurlClient(std::string base_url_) : base_url(std::move(base_url_))
+  {
+    curl_global_init(CURL_GLOBAL_ALL);
+    curl_handle = curl_easy_init();
+    curl_easy_setopt(curl_handle, CURLOPT_WRITEFUNCTION,
+                     &CurlClient::write_memory_callback);
+    curl_easy_setopt(curl_handle, CURLOPT_WRITEDATA, this);
+  }
+
+  ~CurlClient()
+  {
+    curl_easy_cleanup(curl_handle);
+    curl_global_cleanup();
+  }
+
+  public:
+  std::string get(const std::string & url) const
+  {
+    auto full_url = url_combine(base_url, url);
+    return execute_get(full_url);
+  }
+
+  std::string encode(const std::string & str) const
+  {
+    auto curl_str_deleter = [](char * s) { curl_free(s); };
+    auto encoded_str = std::unique_ptr<char, decltype(curl_str_deleter)>(
+            curl_easy_escape(curl_handle, str.c_str(),
+                             static_cast<int>(str.length())),
+            curl_str_deleter);
+    return {encoded_str.get()};
+  }
+
+  private:
+  static std::string url_combine(const std::string & url1,
+                                 const std::string & url2)
+  {
+    if (!url1.empty() && !url2.empty()) {
+      if (url1[url1.length() - 1] == '/' && url2[0] == '/') {
+        return url1 + url2.substr(1);
+      }
+      if (url1[url1.length() - 1] != '/' && url2[0] != '/') {
+        return url1 + '/' + url2;
+      }
+    }
+
+    return url1 + url2;
+  }
+
+  std::string execute_get(const std::string & url) const
+  {
+    curl_easy_setopt(curl_handle, CURLOPT_URL, url.c_str());
+    curl_easy_setopt(curl_handle, CURLOPT_HTTPGET, 1L);
+    buffer.clear();
+    auto res = curl_easy_perform(curl_handle);
+    if (res == CURLE_SEND_ERROR) {
+      // Specific versions of curl doesn't play well with the sensor http
+      // server. When CURLE_SEND_ERROR happens for the first time silently
+      // re-attempting the http request resolves the problem.
+      res = curl_easy_perform(curl_handle);
+    }
+    if (res != CURLE_OK) {
+      throw std::runtime_error(
+              "CurlClient::execute_get failed for the url: [" + url +
+              "] with the error message: " + curl_easy_strerror(res));
+    }
+    long http_code = 0;
+    curl_easy_getinfo(curl_handle, CURLINFO_RESPONSE_CODE, &http_code);
+    if (http_code != 200) {
+      throw std::runtime_error(
+              std::string("CurlClient::execute_get failed for url: [" + url +
+                          "] with the code: [") +
+              std::to_string(http_code) + std::string("] - and return: ") +
+              buffer);
+    }
+    return buffer;
+  }
+
+  static size_t write_memory_callback(void * contents, size_t element_size,
+                                      size_t elements_count,
+                                      void * user_pointer)
+  {
+    size_t size_increment = element_size * elements_count;
+    auto cc = static_cast<CurlClient *>(user_pointer);
+    auto size_current = cc->buffer.size();
+    cc->buffer.resize(size_current + size_increment);
+    memcpy((void *) &cc->buffer[size_current], contents, size_increment);
+    size_current += size_increment;
+    return size_increment;
+  }
+
+  private:
+  CURL * curl_handle;
+  mutable std::string buffer;
+  std::string base_url;
+};
+
+}// namespace util
+}// namespace ouster
+
+#endif//ROS2_OUSTER__CLIENT__INTERFACES__CURL_CLIENT_HPP_