diff --git a/ouster_msgs/msg/Metadata.msg b/ouster_msgs/msg/Metadata.msg index 2b933f7..753699d 100644 --- a/ouster_msgs/msg/Metadata.msg +++ b/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 diff --git a/ros2_ouster/CMakeLists.txt b/ros2_ouster/CMakeLists.txt index 2ef64ea..9c60386 100644 --- a/ros2_ouster/CMakeLists.txt +++ b/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} ) diff --git a/ros2_ouster/include/ros2_ouster/client/client.h b/ros2_ouster/include/ros2_ouster/client/client.h index cf7f3ff..8a961c5 100644 --- a/ros2_ouster/include/ros2_ouster/client/client.h +++ b/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 diff --git a/ros2_ouster/include/ros2_ouster/client/client_factory.hpp b/ros2_ouster/include/ros2_ouster/client/client_factory.hpp new file mode 100644 index 0000000..7c3a59e --- /dev/null +++ b/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::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(hostname); + // case 1: + // return std::make_unique(hostname); + // case 2: + // return std::make_unique(hostname); + } + } + + return std::make_unique(hostname); +} + +}// namespace sensor +}// namespace ouster + +#endif//ROS2_OUSTER_OS1_BASE_FACTORY_H diff --git a/ros2_ouster/include/ros2_ouster/client/impl/curl_client.hpp b/ros2_ouster/include/ros2_ouster/client/impl/curl_client.hpp new file mode 100644 index 0000000..c8de1fb --- /dev/null +++ b/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 + +#include +#include + +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( + curl_easy_escape(curl_handle, str.c_str(), + static_cast(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(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_ \ No newline at end of file diff --git a/ros2_ouster/include/ros2_ouster/client/impl/http.hpp b/ros2_ouster/include/ros2_ouster/client/impl/http.hpp new file mode 100644 index 0000000..2099b00 --- /dev/null +++ b/ros2_ouster/include/ros2_ouster/client/impl/http.hpp @@ -0,0 +1,233 @@ +// Copyright 2020, Steve Macenski +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROS2_OUSTER__CLIENT__IMPL_HTTP_HPP_ +#define ROS2_OUSTER__CLIENT__IMPL_HTTP_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include "ros2_ouster/client/interfaces/client_interface.hpp" +#include "ros2_ouster/client/impl/curl_client.hpp" + +namespace ouster +{ +namespace impl +{ + +/** + * An implementation of the sensor http interface + */ +class HttpImpl : public sensor::util::ClientInterface +{ + public: + /** + * Constructs an http interface to communicate with the sensor. + * + * @param[in] hostname hostname of the sensor to communicate with. + */ + explicit HttpImpl(const std::string & hostname) + : http_client(std::make_unique("http://" + hostname)) + {} + + /** + * Deconstruct the sensor http interface. + */ + ~HttpImpl() override = default; + + /** + * Queries the sensor metadata. + * + * @return returns a Json object of the sensor metadata. + */ + [[nodiscard]] inline Json::Value metadata() const override + { + return get_json("api/v1/sensor/metadata"); + } + + /** + * Queries the sensor_info. + * + * @return returns a Json object representing the sensor_info. + */ + [[nodiscard]] inline Json::Value sensor_info() const override + { + return get_json("api/v1/sensor/metadata/sensor_info"); + } + + /** + * Queries active/staged configuration on the sensor + * + * @param[in] active if true retrieve active, otherwise get staged configs. + * + * @return a string represnting the active or staged config + */ + [[nodiscard]] inline std::string get_config_params(bool active) const override + { + auto config_type = active ? "active" : "staged"; + return get(std::string("api/v1/sensor/cmd/get_config_param?args=") + + config_type); + } + + /** + * Set the value of a specfic configuration on the sensor, the changed + * configuration is not active until the sensor is restarted. + * + * @param[in] key name of the config to change. + * @param[in] value the new value to set for the selected configuration. + */ + inline void set_config_param(const std::string & key, + const std::string & value) const override + { + auto encoded_value = http_client->encode(value);// encode config params + auto url = "api/v1/sensor/cmd/set_config_param?args=" + key + "+" + + encoded_value; + execute(url, "\"set_config_param\""); + } + + /** + * Retrieves the active configuration on the sensor + */ + [[nodiscard]] inline Json::Value active_config_params() const override + { + return get_json("api/v1/sensor/cmd/get_config_param?args=active"); + } + + /** + * Retrieves the staged configuration on the sensor + */ + [[nodiscard]] inline Json::Value staged_config_params() const override + { + return get_json("api/v1/sensor/cmd/get_config_param?args=staged"); + } + + /** + * Enables automatic assignment of udp destination ports. + */ + inline void set_udp_dest_auto() const override + { + execute("api/v1/sensor/cmd/set_udp_dest_auto", "{}"); + } + + /** + * Retrieves beam intrinsics of the sensor. + */ + [[nodiscard]] inline Json::Value beam_intrinsics() const override + { + return get_json("api/v1/sensor/metadata/beam_intrinsics"); + } + + /** + * Retrieves imu intrinsics of the sensor. + */ + [[nodiscard]] inline Json::Value imu_intrinsics() const override + { + return get_json("api/v1/sensor/metadata/imu_intrinsics"); + } + + /** + * Retrieves lidar intrinsics of the sensor. + */ + [[nodiscard]] inline Json::Value lidar_intrinsics() const override + { + return get_json("api/v1/sensor/metadata/lidar_intrinsics"); + } + + /** + * Retrieves lidar data format. + */ + [[nodiscard]] inline Json::Value lidar_data_format() const override + { + return get_json("api/v1/sensor/metadata/lidar_data_format"); + } + + /** + * Gets the calibaration status of the sensor. + */ + [[nodiscard]] inline Json::Value calibration_status() const override + { + return get_json("api/v1/sensor/metadata/calibration_status"); + } + + /** + * Restarts the sensor applying all staged configurations. + */ + inline void reinitialize() const override + { + execute("api/v1/sensor/cmd/reinitialize", "{}"); + } + + /** + * Persist active configuration parameters to the sensor. + */ + inline void save_config_params() const override + { + execute("api/v1/sensor/cmd/save_config_params", "{}"); + } + + private: + [[nodiscard]] inline std::string get(const std::string & url) const + { + return http_client->get(url); + } + + [[nodiscard]] Json::Value get_json(const std::string & url) const + { + Json::CharReaderBuilder builder; + auto reader = std::unique_ptr{builder.newCharReader()}; + Json::Value root; + auto result = get(url); + if (!reader->parse(result.c_str(), result.c_str() + result.size(), &root, + nullptr)) + throw std::runtime_error("SensorHttpImp::get_json failed! url: " + url); + return root; + } + + void execute(const std::string & url, const std::string & validation) const + { + auto result = get(url); + if (result != validation) + throw std::runtime_error("SensorHttpImp::execute failed! url: " + url + + " returned [" + result + "], expected [" + + validation + "]"); + } + + std::unique_ptr http_client; +}; + +}// namespace impl +}// namespace ouster + +#endif// ROS2_OUSTER__CLIENT__IMPL_HTTP_HPP_ diff --git a/ros2_ouster/include/ros2_ouster/client/impl/parsing.h b/ros2_ouster/include/ros2_ouster/client/impl/parsing.h deleted file mode 100644 index ee2b52a..0000000 --- a/ros2_ouster/include/ros2_ouster/client/impl/parsing.h +++ /dev/null @@ -1,208 +0,0 @@ -/** - * @file - * @brief Packet parsing internals - */ - -#pragma once - -#include -#include - -#include "ros2_ouster/client/types.h" - -namespace ouster { - namespace sensor { - namespace impl { - - constexpr int pixel_bytes = 12; - constexpr int imu_packet_size = 48; - constexpr int cols_per_packet = 16; - constexpr int64_t encoder_ticks_per_rev = 90112; - - constexpr int column_bytes(int n_pixels) - { - return 16 + (n_pixels * pixel_bytes) + 4; - } - - constexpr int packet_bytes(int n_pixels) - { - return cols_per_packet * column_bytes(n_pixels); - } - - template < int N_PIXELS > - const uint8_t * nth_col(int n, const uint8_t * lidar_buf) - { - return lidar_buf + (n * column_bytes(N_PIXELS)); - } - - template < int N_PIXELS > - inline uint32_t col_status(const uint8_t * col_buf) - { - uint32_t res; - std::memcpy(&res, col_buf + column_bytes(N_PIXELS) - 4, sizeof(uint32_t)); - return res; - } - - inline uint64_t col_timestamp(const uint8_t * col_buf) - { - uint64_t res; - std::memcpy(&res, col_buf, sizeof(uint64_t)); - return res; // nanoseconds - } - - inline uint32_t col_encoder(const uint8_t * col_buf) - { - uint32_t res; - std::memcpy(&res, col_buf + 12, sizeof(uint32_t)); - return res; - } - - inline uint16_t col_measurement_id(const uint8_t * col_buf) - { - uint16_t res; - std::memcpy(&res, col_buf + 8, sizeof(uint16_t)); - return res; - } - - inline uint16_t col_frame_id(const uint8_t * col_buf) - { - uint16_t res; - std::memcpy(&res, col_buf + 10, sizeof(uint16_t)); - return res; - } - - inline const uint8_t * nth_px(int n, const uint8_t * col_buf) - { - return col_buf + 16 + (n * pixel_bytes); - } - - inline uint32_t px_range(const uint8_t * px_buf) - { - uint32_t res; - std::memcpy(&res, px_buf, sizeof(uint32_t)); - res &= 0x000fffff; - return res; - } - - inline uint16_t px_reflectivity(const uint8_t * px_buf) - { - uint16_t res; - std::memcpy(&res, px_buf + 4, sizeof(uint16_t)); - return res; - } - - inline uint16_t px_signal(const uint8_t * px_buf) - { - uint16_t res; - std::memcpy(&res, px_buf + 6, sizeof(uint16_t)); - return res; - } - - inline uint16_t px_ambient(const uint8_t * px_buf) - { - uint16_t res; - std::memcpy(&res, px_buf + 8, sizeof(uint16_t)); - return res; - } - - inline uint64_t imu_sys_ts(const uint8_t * imu_buf) - { - uint64_t res; - std::memcpy(&res, imu_buf, sizeof(uint64_t)); - return res; - } - - inline uint64_t imu_accel_ts(const uint8_t * imu_buf) - { - uint64_t res; - std::memcpy(&res, imu_buf + 8, sizeof(uint64_t)); - return res; - } - - inline uint64_t imu_gyro_ts(const uint8_t * imu_buf) - { - uint64_t res; - std::memcpy(&res, imu_buf + 16, sizeof(uint64_t)); - return res; - } - - inline float imu_la_x(const uint8_t * imu_buf) - { - float res; - std::memcpy(&res, imu_buf + 24, sizeof(float)); - return res; - } - - inline float imu_la_y(const uint8_t * imu_buf) - { - float res; - std::memcpy(&res, imu_buf + 28, sizeof(float)); - return res; - } - - inline float imu_la_z(const uint8_t * imu_buf) - { - float res; - std::memcpy(&res, imu_buf + 32, sizeof(float)); - return res; - } - - inline float imu_av_x(const uint8_t * imu_buf) - { - float res; - std::memcpy(&res, imu_buf + 36, sizeof(float)); - return res; - } - - inline float imu_av_y(const uint8_t * imu_buf) - { - float res; - std::memcpy(&res, imu_buf + 40, sizeof(float)); - return res; - } - - inline float imu_av_z(const uint8_t * imu_buf) - { - float res; - std::memcpy(&res, imu_buf + 44, sizeof(float)); - return res; - } - - template < int N_PIXELS > - constexpr packet_format packet_2_0() - { - return { - impl::packet_bytes(N_PIXELS), - impl::imu_packet_size, - impl::cols_per_packet, - N_PIXELS, - impl::encoder_ticks_per_rev, - - impl::nth_col < N_PIXELS >, - impl::col_timestamp, - impl::col_encoder, - impl::col_measurement_id, - impl::col_frame_id, - impl::col_status < N_PIXELS >, - - impl::nth_px, - impl::px_range, - impl::px_reflectivity, - impl::px_signal, - impl::px_ambient, - - impl::imu_sys_ts, - impl::imu_accel_ts, - impl::imu_gyro_ts, - impl::imu_la_x, - impl::imu_la_y, - impl::imu_la_z, - impl::imu_av_x, - impl::imu_av_y, - impl::imu_av_z, - }; - } - - } // namespace impl - } // namespace sensor -} // namespace ouster diff --git a/ros2_ouster/include/ros2_ouster/client/impl/tcp.hpp b/ros2_ouster/include/ros2_ouster/client/impl/tcp.hpp new file mode 100644 index 0000000..ec4019b --- /dev/null +++ b/ros2_ouster/include/ros2_ouster/client/impl/tcp.hpp @@ -0,0 +1,364 @@ +// Copyright 2020, Steve Macenski +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROS2_OUSTER__CLIENT__IMPL_TCP_HPP_ +#define ROS2_OUSTER__CLIENT__IMPL_TCP_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include "ros2_ouster/client/interfaces/client_interface.hpp" + +namespace ouster +{ +namespace impl +{ + +class TcpImpl : public sensor::util::ClientInterface +{ + public: + // timeout for reading from a TCP socket during config + static constexpr int RCVTIMEOUT_SEC = 10; + // maximum size to handle during recv + static constexpr size_t MAX_RESULT_LENGTH = 16 * 1024; + + /** + * Constructs an tcp interface to communicate with the sensor. + * + * @param[in] hostname hostname of the sensor to communicate with. + */ + explicit TcpImpl(const std::string &hostname) + : socket_handle(cfg_socket(hostname.c_str())), + read_buf(std::unique_ptr{new char[MAX_RESULT_LENGTH + 1]}) + {} + + /** + * Deconstruct the sensor tcp interface. + */ + ~TcpImpl() override { close(socket_handle); } + /** + * Queries the sensor metadata. + * + * @return returns a Json object of the sensor metadata. + */ + [[nodiscard]] inline Json::Value metadata() const override + { + Json::Value root; + root["sensor_info"] = sensor_info(); + root["beam_intrinsics"] = beam_intrinsics(); + root["imu_intrinsics"] = imu_intrinsics(); + root["lidar_intrinsics"] = lidar_intrinsics(); + root["lidar_data_format"] = lidar_data_format(); + root["calibration_status"] = calibration_status(); + Json::CharReaderBuilder builder; + auto reader = std::unique_ptr{builder.newCharReader()}; + auto res = get_config_params(true); + Json::Value node; + auto parse_success = reader->parse(res.c_str(), res.c_str() + res.size(), + &node, nullptr); + root["config_params"] = parse_success ? node : res; + return root; + } + + /** + * Queries the sensor_info. + * + * @return returns a Json object representing the sensor_info. + */ + [[nodiscard]] inline Json::Value sensor_info() const override + { + return tcp_cmd_json({"get_sensor_info"}); + } + + /** + * Queries active/staged configuration on the sensor + * + * @param[in] active if true retrieve active, otherwise get staged configs. + * + * @return a string represnting the active or staged config + */ + [[nodiscard]] inline std::string get_config_params(bool active) const override + { + auto config_type = active ? "active" : "staged"; + return tcp_cmd({"get_config_param", config_type}); + } + + inline static std::string rtrim(const std::string &s) + { + return s.substr( + 0, std::distance( + s.begin(), + std::find_if(s.rbegin(), s.rend(), [](unsigned char ch) { + return !isspace(ch); + }).base())); + } + + /** + * Set the value of a specfic configuration on the sensor, the changed + * configuration is not active until the sensor is restarted. + * + * @param[in] key name of the config to change. + * @param[in] value the new value to set for the selected configuration. + */ + inline void set_config_param(const std::string &key, + const std::string &value) const override + { + tcp_cmd_with_validation({"set_config_param", key, rtrim(value)}, + "set_config_param"); + } + + /** + * Retrieves the active configuration on the sensor + */ + [[nodiscard]] inline Json::Value active_config_params() const override + { + return tcp_cmd_json({"get_config_param", "active"}); + } + + /** + * Retrieves the staged configuration on the sensor + */ + [[nodiscard]] inline Json::Value staged_config_params() const override + { + return tcp_cmd_json({"get_config_param", "staged"}); + } + + /** + * Enables automatic assignment of udp destination ports. + */ + inline void set_udp_dest_auto() const override + { + tcp_cmd_with_validation({"set_udp_dest_auto"}, "set_udp_dest_auto"); + } + + /** + * Retrieves beam intrinsics of the sensor. + */ + [[nodiscard]] inline Json::Value beam_intrinsics() const override + { + return tcp_cmd_json({"get_beam_intrinsics"}); + } + + /** + * Retrieves imu intrinsics of the sensor. + */ + [[nodiscard]] inline Json::Value imu_intrinsics() const override + { + return tcp_cmd_json({"get_imu_intrinsics"}); + } + + /** + * Retrieves lidar intrinsics of the sensor. + */ + [[nodiscard]] inline Json::Value lidar_intrinsics() const override + { + return tcp_cmd_json({"get_lidar_intrinsics"}); + } + + /** + * Retrieves lidar data format. + */ + [[nodiscard]] inline Json::Value lidar_data_format() const override + { + return tcp_cmd_json({"get_lidar_data_format"}, false); + } + + /** + * Gets the calibaration status of the sensor. + */ + [[nodiscard]] inline Json::Value calibration_status() const override + { + return tcp_cmd_json({"get_calibration_status"}, false); + } + + /** + * Restarts the sensor applying all staged configurations. + */ + inline void reinitialize() const override + { + // reinitialize to make all staged parameters effective + tcp_cmd_with_validation({"reinitialize"}, "reinitialize"); + } + + /** + * Persist active configuration parameters to the sensor. + */ + inline void save_config_params() const override + { + tcp_cmd_with_validation({"write_config_txt"}, "write_config_txt"); + } + + private: + /** + * Configure socket + * @param addr address of socket to reconfigure + * @return int The socket ID number + */ + inline static int cfg_socket(const char *addr) + { + struct addrinfo hints { + }, *info_start, *ai; + + memset(&hints, 0, sizeof hints); + hints.ai_family = AF_UNSPEC; + hints.ai_socktype = SOCK_STREAM; + + // try to parse as numeric address first: avoids spurious errors from + // DNS lookup when not using a hostname (and should be faster) + hints.ai_flags = AI_NUMERICHOST; + int ret = getaddrinfo(addr, "7501", &hints, &info_start); + if (ret != 0) { + hints.ai_flags = 0; + ret = getaddrinfo(addr, "7501", &hints, &info_start); + if (ret != 0) { + std::cout << "cfg getaddrinfo(): {}" << gai_strerror(ret) << std::endl; + return -1; + } + } + + if (info_start == nullptr) { + std::cerr << "cfg getaddrinfo(): empty result" << std::endl; + return -1; + } + + int sock_fd; + for (ai = info_start; ai != nullptr; ai = ai->ai_next) { + sock_fd = socket(ai->ai_family, ai->ai_socktype, ai->ai_protocol); + if (sock_fd < 0) { + std::cerr << "cfg socket(): " << std::strerror(errno) << std::endl; + continue; + } + + if (connect(sock_fd, ai->ai_addr, + static_cast(ai->ai_addrlen)) == -1) { + close(sock_fd); + continue; + } + + struct timeval tv {}; + tv.tv_sec = RCVTIMEOUT_SEC; + tv.tv_usec = 0; + if (setsockopt(sock_fd, SOL_SOCKET, SO_RCVTIMEO, (const char *) &tv, + sizeof tv)) { + std::cout << "cfg set_rcvtimeout(): {}" << std::strerror(errno) + << std::endl; + close(sock_fd); + continue; + } + + break; + } + + freeaddrinfo(info_start); + if (ai == nullptr) { return -1; } + + return sock_fd; + } + + /** + * Conduct a specific TCP socket command + * @param cmd_tokens command tokens + * @return result string of packet data + */ + [[nodiscard]] inline std::string + tcp_cmd(const std::vector &cmd_tokens) const + { + std::stringstream ss; + for (const auto &token: cmd_tokens) { ss << token << " "; } + ss << "\n"; + std::string cmd = ss.str(); + + ssize_t len = send(socket_handle, cmd.c_str(), cmd.length(), 0); + if (len != static_cast(cmd.length())) { + throw std::runtime_error("tcp_cmd socket::send failed"); + } + + // need to synchronize with server by reading response + std::stringstream read_ss; + do { + len = recv(socket_handle, read_buf.get(), MAX_RESULT_LENGTH, 0); + if (len < 0) { + throw std::runtime_error{"tcp_cmd recv(): " + + std::string(std::strerror(errno))}; + } + read_buf.get()[len] = '\0'; + read_ss << read_buf.get(); + } while (len > 0 && read_buf.get()[len - 1] != '\n'); + + auto res = read_ss.str(); + res.erase(res.find_last_not_of(" \r\n\t") + 1); + + return res; + } + + [[nodiscard]] inline Json::Value + tcp_cmd_json(const std::vector &cmd_tokens, + bool exception_on_parse_errors = true) const + { + Json::CharReaderBuilder builder; + auto reader = std::unique_ptr{builder.newCharReader()}; + Json::Value root; + auto result = tcp_cmd(cmd_tokens); + auto success = reader->parse(result.c_str(), result.c_str() + result.size(), + &root, nullptr); + if (success) return root; + if (!exception_on_parse_errors) return result; + + throw std::runtime_error("SensorTcp::tcp_cmd_json failed for " + + cmd_tokens[0] + + " command. returned json string [" + result + + "] couldn't be parsed ["); + } + + inline void + tcp_cmd_with_validation(const std::vector &cmd_tokens, + const std::string &validation) const + { + auto result = tcp_cmd(cmd_tokens); + if (result != validation) { + throw std::runtime_error("SensorTcp::tcp_cmd failed: " + cmd_tokens[0] + + " command returned [" + result + + "], expected [" + validation + "]"); + } + } + + int socket_handle; + std::unique_ptr read_buf; +}; + +}// namespace impl +}// namespace ouster + +#endif// ROS2_OUSTER__CLIENT__IMPL_TCP_HPP_ diff --git a/ros2_ouster/include/ros2_ouster/client/interfaces/client_interface.hpp b/ros2_ouster/include/ros2_ouster/client/interfaces/client_interface.hpp new file mode 100644 index 0000000..f41de6a --- /dev/null +++ b/ros2_ouster/include/ros2_ouster/client/interfaces/client_interface.hpp @@ -0,0 +1,176 @@ +/** + * Copyright (c) 2022, Ouster, Inc. + * All rights reserved. + * + * @file sensor_http.h + * @brief A high level HTTP interface for Ouster sensors. + * + */ + +#ifndef ROS2_OUSTER__CLIENT__INTERFACES_CLIENT_INTERFACE_HPP_ +#define ROS2_OUSTER__CLIENT__INTERFACES_CLIENT_INTERFACE_HPP_ + +#include +#include +#include + +#include +#include + +namespace ouster +{ +namespace sensor +{ +namespace util +{ + +/** + * An interface to communicate with Ouster sensors via http requests + */ +class ClientInterface +{ + protected: + /** + * Constructs an http interface to communicate with the sensor. + */ + ClientInterface() = default; + + public: + /** + * Deconstruct the sensor http interface. + */ + virtual ~ClientInterface() = default; + + /** + * Queries the sensor metadata. + * + * @return returns a Json object of the sensor metadata. + */ + [[nodiscard]] virtual Json::Value metadata() const = 0; + + /** + * Queries the sensor_info. + * + * @return returns a Json object representing the sensor_info. + */ + [[nodiscard]] virtual Json::Value sensor_info() const = 0; + + /** + * Queries active/staged configuration on the sensor + * + * @param[in] active if true retrieve active, otherwise get staged configs. + * + * @return a string represnting the active or staged config + */ + [[nodiscard]] virtual std::string get_config_params(bool active) const = 0; + + /** + * Set the value of a specfic configuration on the sensor, the changed + * configuration is not active until the sensor is restarted. + * + * @param[in] key name of the config to change. + * @param[in] value the new value to set for the selected configuration. + */ + virtual void set_config_param(const std::string & key, + const std::string & value) const = 0; + + /** + * Retrieves the active configuration on the sensor + */ + [[nodiscard]] virtual Json::Value active_config_params() const = 0; + + /** + * Retrieves the staged configuration on the sensor + */ + [[nodiscard]] virtual Json::Value staged_config_params() const = 0; + + /** + * Enables automatic assignment of udp destination ports. + */ + virtual void set_udp_dest_auto() const = 0; + + /** + * Retrieves beam intrinsics of the sensor. + */ + [[nodiscard]] virtual Json::Value beam_intrinsics() const = 0; + + /** + * Retrieves imu intrinsics of the sensor. + */ + [[nodiscard]] virtual Json::Value imu_intrinsics() const = 0; + + /** + * Retrieves lidar intrinsics of the sensor. + */ + [[nodiscard]] virtual Json::Value lidar_intrinsics() const = 0; + + /** + * Retrieves lidar data format. + */ + [[nodiscard]] virtual Json::Value lidar_data_format() const = 0; + + /** + * Gets the calibaration status of the sensor. + */ + [[nodiscard]] virtual Json::Value calibration_status() const = 0; + + /** + * Restarts the sensor applying all staged configurations. + */ + virtual void reinitialize() const = 0; + + /** + * Persist active configuration parameters to the sensor. + */ + virtual void save_config_params() const = 0; + + /** + * Retrieves sensor firmware version information as a string. + * + * @param[in] hostname hostname of the sensor to communicate with. + */ + inline static std::string + firmware_version_string(const std::string & hostname) + { + auto net_client = std::make_unique("http://" + hostname); + return net_client->get("api/v1/system/firmware"); + } + + /** + * Retrieves sensor firmware version information. + * + * @param[in] hostname hostname of the sensor to communicate with. + */ + inline static ouster::util::version + firmware_version(const std::string & hostname) + { + auto result = firmware_version_string(hostname); + auto rgx = std::regex(R"(v(\d+).(\d+)\.(\d+))"); + std::smatch matches; + std::regex_search(result, matches, rgx); + + if (matches.size() < 4) return ouster::util::invalid_version; + + try { + return ouster::util::version{static_cast(stoul(matches[1])), + static_cast(stoul(matches[2])), + static_cast(stoul(matches[3]))}; + } + catch (const std::exception &) { + return ouster::util::invalid_version; + } + } + + /** + * Creates an instance of the SensorHttp interface. + * + * @param[in] hostname hostname of the sensor to communicate with. + */ + static std::unique_ptr create(const std::string & hostname); +}; + +}// namespace util +}// namespace sensor +}// namespace ouster + +#endif// ROS2_OUSTER__CLIENT__INTERFACES_CLIENT_INTERFACE_HPP_ \ No newline at end of file diff --git a/ros2_ouster/include/ros2_ouster/client/lidar_scan.h b/ros2_ouster/include/ros2_ouster/client/lidar_scan.h index c6fd168..a2ff7a3 100644 --- a/ros2_ouster/include/ros2_ouster/client/lidar_scan.h +++ b/ros2_ouster/include/ros2_ouster/client/lidar_scan.h @@ -15,6 +15,327 @@ namespace ouster { +namespace impl { +// forward declaration +struct LidarScan; + +using sensor::ChanFieldType; + +template +struct FieldTag; + +template <> +struct FieldTag { + static constexpr ChanFieldType tag = ChanFieldType::UINT8; +}; + +template <> +struct FieldTag { + static constexpr ChanFieldType tag = ChanFieldType::UINT16; +}; + +template <> +struct FieldTag { + static constexpr ChanFieldType tag = ChanFieldType::UINT32; +}; + +template <> +struct FieldTag { + static constexpr ChanFieldType tag = ChanFieldType::UINT64; +}; + +/* + * Tagged union for LidarScan fields + */ +struct FieldSlot { + ChanFieldType tag; + union { + img_t f8; + img_t f16; + img_t f32; + img_t f64; + }; + + FieldSlot(ChanFieldType t, size_t w, size_t h) : tag{t} { + switch (t) { + case ChanFieldType::VOID: + break; + case ChanFieldType::UINT8: + new (&f8) img_t{h, w}; + f8.setZero(); + break; + case ChanFieldType::UINT16: + new (&f16) img_t{h, w}; + f16.setZero(); + break; + case ChanFieldType::UINT32: + new (&f32) img_t{h, w}; + f32.setZero(); + break; + case ChanFieldType::UINT64: + new (&f64) img_t{h, w}; + f64.setZero(); + break; + } + } + + FieldSlot() : FieldSlot{ChanFieldType::VOID, 0, 0} {}; + + ~FieldSlot() { clear(); } + + FieldSlot(const FieldSlot& other) { + switch (other.tag) { + case ChanFieldType::VOID: + break; + case ChanFieldType::UINT8: + new (&f8) img_t{other.f8}; + break; + case ChanFieldType::UINT16: + new (&f16) img_t{other.f16}; + break; + case ChanFieldType::UINT32: + new (&f32) img_t{other.f32}; + break; + case ChanFieldType::UINT64: + new (&f64) img_t{other.f64}; + break; + } + tag = other.tag; + } + + FieldSlot(FieldSlot&& other) noexcept { set_from(other); } + + FieldSlot& operator=(FieldSlot other) { + clear(); + set_from(other); + return *this; + } + + template + Eigen::Ref> get() { + if (tag == FieldTag::tag) + return get_unsafe(); + else + throw std::invalid_argument("Accessed field at wrong type"); + } + + template + Eigen::Ref> get() const { + if (tag == FieldTag::tag) + return get_unsafe(); + else + throw std::invalid_argument("Accessed field at wrong type"); + } + + friend bool operator==(const FieldSlot& l, const FieldSlot& r) { + if (l.tag != r.tag) return false; + switch (l.tag) { + case ChanFieldType::VOID: + return true; + case ChanFieldType::UINT8: + return (l.f8 == r.f8).all(); + case ChanFieldType::UINT16: + return (l.f16 == r.f16).all(); + case ChanFieldType::UINT32: + return (l.f32 == r.f32).all(); + case ChanFieldType::UINT64: + return (l.f64 == r.f64).all(); + default: + assert(false); + } + // unreachable, appease older gcc + return false; + } + + private: + void set_from(FieldSlot& other) { + switch (other.tag) { + case ChanFieldType::VOID: + break; + case ChanFieldType::UINT8: + new (&f8) img_t{std::move(other.f8)}; + break; + case ChanFieldType::UINT16: + new (&f16) img_t{std::move(other.f16)}; + break; + case ChanFieldType::UINT32: + new (&f32) img_t{std::move(other.f32)}; + break; + case ChanFieldType::UINT64: + new (&f64) img_t{std::move(other.f64)}; + break; + } + tag = other.tag; + other.clear(); + } + + void clear() { + switch (tag) { + case ChanFieldType::VOID: + break; + case ChanFieldType::UINT8: + f8.~img_t(); + break; + case ChanFieldType::UINT16: + f16.~img_t(); + break; + case ChanFieldType::UINT32: + f32.~img_t(); + break; + case ChanFieldType::UINT64: + f64.~img_t(); + break; + } + tag = ChanFieldType::VOID; + } + + template + Eigen::Ref> get_unsafe(); + + template + Eigen::Ref> get_unsafe() const; +}; + +template <> +inline Eigen::Ref> FieldSlot::get_unsafe() { + return f8; +} + +template <> +inline Eigen::Ref> FieldSlot::get_unsafe() { + return f16; +} + +template <> +inline Eigen::Ref> FieldSlot::get_unsafe() { + return f32; +} + +template <> +inline Eigen::Ref> FieldSlot::get_unsafe() { + return f64; +} + +template <> +inline Eigen::Ref> FieldSlot::get_unsafe() const { + return f8; +} + +template <> +inline Eigen::Ref> FieldSlot::get_unsafe() const { + return f16; +} + +template <> +inline Eigen::Ref> FieldSlot::get_unsafe() const { + return f32; +} + +template <> +inline Eigen::Ref> FieldSlot::get_unsafe() const { + return f64; +} + +/* + * Call a generic operation op(f, Args..) with the type parameter T having + * the correct (dynamic) field type for the LidarScan channel field f + * Example code for the operation: + * \code + * struct print_field_size { + * template + * void operator()(Eigen::Ref> field) { + * std::cout << "Rows: " + field.rows() << std::endl; + * std::cout << "Cols: " + field.cols() << std::endl; + * } + * }; + * \endcode + */ +template +void visit_field(SCAN&& ls, sensor::ChanField f, OP&& op, Args&&... args) { + switch (ls.field_type(f)) { + case sensor::ChanFieldType::UINT8: + op.template operator()(ls.template field(f), + std::forward(args)...); + break; + case sensor::ChanFieldType::UINT16: + op.template operator()(ls.template field(f), + std::forward(args)...); + break; + case sensor::ChanFieldType::UINT32: + op.template operator()(ls.template field(f), + std::forward(args)...); + break; + case sensor::ChanFieldType::UINT64: + op.template operator()(ls.template field(f), + std::forward(args)...); + break; + default: + throw std::invalid_argument("Invalid field for LidarScan"); + } +} + +/* + * Call a generic operation op(f, Args...) for each field of the lidar scan + * with type parameter T having the correct field type + */ +template +void foreach_field(SCAN&& ls, OP&& op, Args&&... args) { + for (const auto& ft : ls) + visit_field(std::forward(ls), ft.first, std::forward(op), + ft.first, std::forward(args)...); +} + +// Read LidarScan field and cast to the destination +struct read_and_cast { + template + void operator()(Eigen::Ref> src, Eigen::Ref> dest) { + dest = src.template cast(); + } + template + void operator()(Eigen::Ref> src, Eigen::Ref> dest) { + dest = src.template cast(); + } + template + void operator()(Eigen::Ref> src, img_t& dest) { + dest = src.template cast(); + } + template + void operator()(Eigen::Ref> src, img_t& dest) { + dest = src.template cast(); + } +}; + +// Copy fields from `ls_source` LidarScan to `field_dest` img with casting +// to the img_t type of `field_dest`. +struct copy_and_cast { + template + void operator()(Eigen::Ref> field_dest, const LidarScan& ls_source, + sensor::ChanField ls_source_field) { + visit_field(ls_source, ls_source_field, read_and_cast(), + field_dest); + } +}; +} // namespace impl + +/** + * Flags for frame_status + */ +enum frame_status_masks : uint64_t { + FRAME_STATUS_THERMAL_SHUTDOWN_MASK = 0x0f, ///< Mask to get thermal shutdown status + FRAME_STATUS_SHOT_LIMITING_MASK = 0xf0 ///< Mask to get shot limting status +}; + +enum frame_status_shifts: uint64_t { + FRAME_STATUS_THERMAL_SHUTDOWN_SHIFT = 0, ///< No shift for thermal shutdown + FRAME_STATUS_SHOT_LIMITING_SHIFT = 4 /// shift 4 for shot limiting +}; + +/** + * Alias for the lidar scan field types + */ +using LidarScanFieldTypes = + std::vector>; + /** * Datastructure for efficient operations on aggregated lidar data. * @@ -28,127 +349,285 @@ namespace ouster { */ class LidarScan { public: - static constexpr int N_FIELDS = 4; - - using raw_t = uint32_t; - using ts_t = std::chrono::nanoseconds; - using data_t = Eigen::Array < raw_t, Eigen::Dynamic, N_FIELDS >; - - using DynStride = Eigen::Stride < Eigen::Dynamic, Eigen::Dynamic >; + template + using Header = Eigen::Array; ///< Header typedef /** XYZ coordinates with dimensions arranged contiguously in columns */ using Points = Eigen::Array < double, Eigen::Dynamic, 3 >; - /** Data fields reported per channel */ - enum Field { RANGE, INTENSITY, AMBIENT, REFLECTIVITY }; - - /** Measurement block information, other than the channel data */ - struct BlockHeader - { - ts_t timestamp; - uint32_t encoder; - uint32_t status; - }; - - /* Members variables: use with caution, some of these will become private */ - std::ptrdiff_t w {0}; - std::ptrdiff_t h {0}; - data_t data {}; - std::vector < BlockHeader > headers {}; - int32_t frame_id {-1}; - - /** The default constructor creates an invalid 0 x 0 scan */ +private: + Header timestamp_; + Header measurement_id_; + Header status_; + std::map fields_; + LidarScanFieldTypes field_types_; + + LidarScan(size_t w, size_t h, LidarScanFieldTypes field_types); + +public: + /** + * Pointer offsets to deal with strides. + * + * @warning Members variables: use with caution, some of these will become + * private. + */ + std::ptrdiff_t w{0}; + + /** + * Pointer offsets to deal with strides. + * + * @warning Members variables: use with caution, some of these will become + * private. + */ + std::ptrdiff_t h{0}; + + /** + * Frame status - information from the packet header which corresponds to a + * frame + * + * @warning Member variables: use with caution, some of these will become + * private. + */ + uint64_t frame_status{0}; + + /** + * The current frame ID. + * + * @warning Members variables: use with caution, some of these will become + * private. + */ + int32_t frame_id{-1}; + + using FieldIter = + decltype(field_types_)::const_iterator; ///< An STL Iterator of the + ///< field types + + /** The default constructor creates an invalid 0 x 0 scan. */ LidarScan() = default; /** - * Initialize an empty scan with the given horizontal / vertical resolution. + * Initialize a scan with fields configured for the LEGACY udp profile. * - * @param w horizontal resoulution, i.e. the number of measurements per scan - * @param h vertical resolution, i.e. the number of channels + * @param[in] w horizontal resoulution, i.e. the number of measurements per + * scan. + * @param[in] h vertical resolution, i.e. the number of channels. */ LidarScan(size_t w, size_t h) - : w {static_cast < std::ptrdiff_t > (w)}, - h {static_cast < std::ptrdiff_t > (h)}, - data {w * h, N_FIELDS}, - headers {w, BlockHeader {ts_t {0}, 0, 0}} {} + : LidarScan{w, h, sensor::UDPProfileLidar::PROFILE_LIDAR_LEGACY} {} /** - * Access timestamps as a vector. + * Initialize a scan with the default fields for a particular udp profile. * - * @returns copy of the measurement timestamps as a vector + * @param[in] w horizontal resoulution, i.e. the number of measurements per + * scan. + * @param[in] h vertical resolution, i.e. the number of channels. + * @param[in] profile udp profile. */ - std::vector < LidarScan::ts_t > timestamps() const { - std::vector < LidarScan::ts_t > res; - res.reserve(headers.size()); - for (const auto & h : headers) { - res.push_back(h.timestamp); - } - return res; - } + LidarScan(size_t w, size_t h, sensor::UDPProfileLidar profile); + + /** + * Initialize a scan with a custom set of fields. + * + * @tparam Iterator A standard template iterator for the custom fields. + * + * @param[in] w horizontal resoulution, i.e. the number of measurements per + * scan. + * @param[in] h vertical resolution, i.e. the number of channels. + * @param[in] begin begin iterator of pairs of channel fields and types. + * @param[in] end end iterator of pairs of channel fields and types. + */ + template + LidarScan(size_t w, size_t h, Iterator begin, Iterator end) + : LidarScan(w, h, {begin, end}){}; /** - * Access measurement block header fields. + * Initialize a lidar scan from another lidar scan. * - * @return the header values for the specified measurement id + * @param[in] other The other lidar scan to initialize from. */ - BlockHeader & header(size_t m_id) {return headers.at(m_id);} + LidarScan(const LidarScan& other) = default; - /** @copydoc header(size_t m_id) */ - const BlockHeader & header(size_t m_id) const {return headers.at(m_id);} + /** @copydoc LidarScan(const LidarScan& other) */ + LidarScan(LidarScan&& other) = default; /** - * Access measurement block data. + * Copy via Move semantic. * - * @param m_id the measurement id of the desired block - * @return a view of the measurement block data + * @param[in] other The lidar scan to copy from. */ - Eigen::Map < data_t, Eigen::Unaligned, DynStride > block(size_t m_id) { - return Eigen::Map < data_t, Eigen::Unaligned, DynStride > ( - data.row(m_id).data(), h, N_FIELDS, {w * h, w}); + LidarScan& operator=(const LidarScan& other) = default; + + /** @copydoc operator=(const LidarScan& other) */ + LidarScan& operator=(LidarScan&& other) = default; + + /** + * Lidar scan destructor. + */ + ~LidarScan() = default; + + /** + * Get frame shot limiting status + */ + inline sensor::ShotLimitingStatus shot_limiting() const { + return static_cast( + (frame_status & frame_status_masks::FRAME_STATUS_SHOT_LIMITING_MASK) >> + frame_status_shifts::FRAME_STATUS_SHOT_LIMITING_SHIFT); } - /** @copydoc block(size_t m_id) */ - Eigen::Map < const data_t, Eigen::Unaligned, DynStride > block( - size_t m_id) const { - return Eigen::Map < const data_t, Eigen::Unaligned, DynStride > ( - data.row(m_id).data(), h, N_FIELDS, {w * h, w}); + /** + * Get frame thermal shutdown status + */ + inline sensor::ThermalShutdownStatus thermal_shutdown() const { + return static_cast( + (frame_status & + frame_status_masks::FRAME_STATUS_THERMAL_SHUTDOWN_MASK) >> + frame_status_shifts::FRAME_STATUS_THERMAL_SHUTDOWN_SHIFT); } /** * Access a lidar data field. * - * @param f the field to view - * @return a view of the field data + * @throw std::invalid_argument if T does not match the runtime field type. + * + * @tparam T The type parameter T must match the dynamic type of the field. + * See the constructor documentation for expected field types or query + * dynamically for generic operations. + * + * @param[in] f the field to view. + * + * @return a view of the field data. */ - Eigen::Map < img_t < raw_t >> field(Field f) { - return Eigen::Map < img_t < raw_t >> (data.col(f).data(), h, w); + template ::value, T>::type = 0> + inline Eigen::Ref> field(sensor::ChanField f) { + return fields_.at(f).get(); } /** @copydoc field(Field f) */ - Eigen::Map < const img_t < raw_t >> field(Field f) const { - return Eigen::Map < const img_t < raw_t >> (data.col(f).data(), h, w); + template ::value, T>::type = 0> + inline Eigen::Ref> field(sensor::ChanField f) const { + return fields_.at(f).get(); + } + + /** + * Get the type of the specified field. + * + * @param[in] f the field to query. + * + * @return the type tag associated with the field. + */ + inline sensor::ChanFieldType field_type(sensor::ChanField f) const { + return fields_.count(f) ? fields_.at(f).tag : sensor::ChanFieldType::VOID; + } + + /** A const forward iterator over field / type pairs. */ + inline FieldIter begin() const { return field_types_.cbegin(); } + + /** @copydoc begin() */ + inline FieldIter end() const { return field_types_.cend(); } + + /** + * Access the measurement timestamp headers. + * + * @return a view of timestamp as a w-element vector. + */ + inline Eigen::Ref> timestamp() { + return timestamp_; + } + + /** + * @copydoc timestamp() + */ + inline Eigen::Ref> timestamp() const { + return timestamp_; + } + + /** + * Access the measurement id headers. + * + * @return a view of measurement ids as a w-element vector. + */ + inline Eigen::Ref> measurement_id() { + return measurement_id_; + } + + /** @copydoc measurement_id() */ + inline Eigen::Ref> measurement_id() const { + return measurement_id_; + } + + /** + * Access the measurement status headers. + * + * @return a view of measurement statuses as a w-element vector. + */ + inline Eigen::Ref> status() { + return status_; + } + + /** @copydoc status() */ + inline Eigen::Ref> status() const { + return status_; } + + /** + * Assess completeness of scan. + * @param[in] window The column window to use for validity assessment + * @return whether all columns within given column window were valid + */ + bool complete(sensor::ColumnWindow window) const; + + friend inline bool operator==(const LidarScan& a, const LidarScan& b); }; - /** Equality for column headers. */ - inline bool operator == ( - const LidarScan::BlockHeader & a, - const LidarScan::BlockHeader & b) - { - return a.timestamp == b.timestamp && - a.encoder == b.encoder && - a.status == b.status; + /** + * Get string representation of lidar scan field types. + * + * @param[in] field_types The field types to get the string representation of. + * + * @return string representation of the lidar scan field types. + */ + std::string to_string(const LidarScanFieldTypes& field_types); + + /** + * Get the lidar scan field types from a lidar scan + * + * @param[in] ls The lidar scan to get the lidar scan field types from. + * + * @return The lidar scan field types + */ + inline LidarScanFieldTypes get_field_types(const LidarScan& ls) { + return {ls.begin(), ls.end()}; } + /** + * Get the lidar scan field types from sensor info + * + * @param[in] info The sensor info to get the lidar scan field types from. + * + * @return The lidar scan field types + */ + LidarScanFieldTypes get_field_types(const sensor::sensor_info& info); + + /** + * Get string representation of a lidar scan. + * + * @param[in] ls The lidar scan to get the string representation of. + * + * @return string representation of the lidar scan. + */ + std::string to_string(const LidarScan& ls); + /** Equality for scans. */ - inline bool operator == (const LidarScan & a, const LidarScan & b) + inline bool operator == (const LidarScan & a, const LidarScan & b) { - return a.w == b.w && - a.h == b.h && - (a.data == b.data).all() && - a.headers == b.headers && - a.frame_id && - b.frame_id; + return a.frame_id == b.frame_id && a.w == b.w && a.h == b.h && + a.frame_status == b.frame_status && a.fields_ == b.fields_ && + a.field_types_ == b.field_types_ && + (a.timestamp() == b.timestamp()).all() && + (a.measurement_id() == b.measurement_id()).all() && + (a.status() == b.status()).all(); } /** Not Equality for scans. */ @@ -190,7 +669,7 @@ namespace ouster { size_t w, size_t h, double range_unit, - double lidar_origin_to_beam_origin_mm, + const mat4d& beam_to_lidar_transform, const mat4d & transform, const std::vector < double > & azimuth_angles_deg, const std::vector < double > & altitude_angles_deg); @@ -204,24 +683,35 @@ namespace ouster { inline XYZLut make_xyz_lut(const sensor::sensor_info & sensor) { return make_xyz_lut( - sensor.format.columns_per_frame, - sensor.format.pixels_per_column, - sensor::range_unit, - sensor.lidar_origin_to_beam_origin_mm, - sensor.lidar_to_sensor_transform, - sensor.beam_azimuth_angles, - sensor.beam_altitude_angles); + sensor.format.columns_per_frame, sensor.format.pixels_per_column, + sensor::range_unit, sensor.beam_to_lidar_transform, + sensor.lidar_to_sensor_transform, sensor.beam_azimuth_angles, + sensor.beam_altitude_angles); } /** - * Convert LidarScan to cartesian points. - * - * @param scan a LidarScan - * @param xyz_lut lookup tables generated by make_xyz_lut - * @return cartesian points where ith row is a 3D point which corresponds - * to ith pixel in LidarScan where i = row * w + col - */ - LidarScan::Points cartesian(const LidarScan & scan, const XYZLut & lut); + * Convert LidarScan to Cartesian points. + * + * @param[in] scan a LidarScan. + * @param[in] lut lookup tables generated by make_xyz_lut. + * + * @return Cartesian points where ith row is a 3D point which corresponds + * to ith pixel in LidarScan where i = row * w + col. + */ + LidarScan::Points cartesian(const LidarScan& scan, const XYZLut& lut); + + /** + * Convert a staggered range image to Cartesian points. + * + * @param[in] range a range image in the same format as the RANGE field of a + * LidarScan. + * @param[in] lut lookup tables generated by make_xyz_lut. + * + * @return Cartesian points where ith row is a 3D point which corresponds + * to ith pixel in LidarScan where i = row * w + col. + */ + LidarScan::Points cartesian(const Eigen::Ref>& range, + const XYZLut& lut); /** * Generate a destaggered version of a channel field. @@ -292,7 +782,8 @@ namespace ouster { { std::ptrdiff_t w; std::ptrdiff_t h; - uint16_t next_m_id; + uint16_t next_valid_m_id; + uint16_t next_headers_m_id; LidarScan ls_write; public: @@ -304,8 +795,19 @@ namespace ouster { * * @param w number of columns in the lidar scan. One of 512, 1024, or 2048 * @param pf expected format of the incoming packets used for parsing + * @param profile expected profile of the incoming packets used for parsing + */ + ScanBatcher(size_t w, const sensor::packet_format & pf) + : w(w), h(pf.pixels_per_column), next_valid_m_id(0), next_headers_m_id(0), pf(pf), + ls_write(w, h, pf.udp_profile_lidar) {} + + /** + * Create a batcher given information about the scan and packet format. + * + * @param[in] info sensor metadata returned from the client. */ - ScanBatcher(size_t w, const sensor::packet_format & pf); + explicit ScanBatcher(const sensor::sensor_info& info) + : ScanBatcher(info.format.columns_per_frame, sensor::get_format(info)) {} /** * Add a packet to the scan. diff --git a/ros2_ouster/include/ros2_ouster/client/point.h b/ros2_ouster/include/ros2_ouster/client/point.h index 4be979a..b378bb9 100644 --- a/ros2_ouster/include/ros2_ouster/client/point.h +++ b/ros2_ouster/include/ros2_ouster/client/point.h @@ -21,7 +21,7 @@ namespace ouster_ros { float intensity; uint32_t t; uint16_t reflectivity; - uint8_t ring; + uint16_t ring; uint16_t ambient; uint32_t range; EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -34,7 +34,7 @@ POINT_CLOUD_REGISTER_POINT_STRUCT( (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity) // use std::uint32_t to avoid conflicting with pcl::uint32_t (std::uint32_t, t, t)(std::uint16_t, reflectivity, reflectivity)( - std::uint8_t, ring, + std::uint16_t, ring, ring)(std::uint16_t, ambient, ambient)(std::uint32_t, range, range) ) // clang-format on diff --git a/ros2_ouster/include/ros2_ouster/client/types.h b/ros2_ouster/include/ros2_ouster/client/types.h index 961ae37..3aee248 100644 --- a/ros2_ouster/include/ros2_ouster/client/types.h +++ b/ros2_ouster/include/ros2_ouster/client/types.h @@ -5,10 +5,13 @@ #pragma once +#include #include #include #include #include +#include + #include "optional-lite/optional.hpp" // Declare namespaces from optional-lite @@ -40,15 +43,25 @@ namespace ouster { extern const mat4d default_imu_to_sensor_transform; extern const mat4d default_lidar_to_sensor_transform; - /** Resolution modes for the LiDAR */ - enum lidar_mode + enum configuration_version { - MODE_UNSPEC = 0, - MODE_512x10, - MODE_512x20, - MODE_1024x10, - MODE_1024x20, - MODE_2048x10 + FW_2_0 = 3, + FW_2_2 = 4 + }; + + /** + * Constants used for configuration. Refer to the sensor documentation for the + * meaning of each option. + */ + enum lidar_mode { + MODE_UNSPEC = 0, ///< lidar mode: unspecified + MODE_512x10, ///< lidar mode: 10 scans of 512 columns per second + MODE_512x20, ///< lidar mode: 20 scans of 512 columns per second + MODE_1024x10, ///< lidar mode: 10 scans of 1024 columns per second + MODE_1024x20, ///< lidar mode: 20 scans of 1024 columns per second + MODE_2048x10, ///< lidar mode: 10 scans of 2048 columns per second + MODE_4096x5 ///< lidar mode: 5 scans of 4096 columns per second. Only + ///< available on select sensors }; enum timestamp_mode @@ -59,10 +72,15 @@ namespace ouster { TIME_FROM_PTP_1588 }; - enum config_flags : uint8_t - { - CONFIG_UDP_DEST_AUTO = (1 << 0), - CONFIG_PERSIST = (1 << 1) + /** + * Flags for set_config() + */ + enum config_flags : uint8_t { + CONFIG_UDP_DEST_AUTO = (1 << 0), ///< Set udp_dest automatically + CONFIG_PERSIST = (1 << 1), ///< Make configuration persistent + CONFIG_FORCE_REINIT = (1 << 2) ///< Forces the sensor to re-init during + ///< set_config even when config params + ///< have not changed }; enum OperatingMode @@ -93,63 +111,261 @@ namespace ouster { BAUD_115200 }; - enum configuration_version - { - FW_2_0 = 3 + /** Profile indicating packet format of lidar data. */ + enum UDPProfileLidar { + /** Legacy lidar data */ + PROFILE_LIDAR_LEGACY = 1, + /** Dual Returns data */ + PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL, + /** Single Returns data */ + PROFILE_RNG19_RFL8_SIG16_NIR16, + /** Single Returns Low Data Rate */ + PROFILE_RNG15_RFL8_NIR8, + /** Five Word Profile */ + PROFILE_FIVE_WORD_PIXEL, }; - struct data_format - { - uint32_t pixels_per_column; - uint32_t columns_per_packet; - uint32_t columns_per_frame; - std::vector < int > pixel_shift_by_row; - ColumnWindow column_window; + /** Profile indicating packet format of IMU data. */ + enum UDPProfileIMU { + PROFILE_IMU_LEGACY = 1 ///< Legacy IMU data }; - struct sensor_info - { - std::string name; - std::string sn; - std::string fw_rev; - lidar_mode mode; - std::string prod_line; - data_format format; - std::vector < double > beam_azimuth_angles; - std::vector < double > beam_altitude_angles; - double lidar_origin_to_beam_origin_mm; - mat4d imu_to_sensor_transform; - mat4d lidar_to_sensor_transform; - mat4d extrinsic; + /** Thermal Shutdown status. */ + enum ThermalShutdownStatus { + THERMAL_SHUTDOWN_NORMAL = 0x00, ///< Normal operation + THERMAL_SHUTDOWN_IMMINENT = 0x01, ///< Thermal Shutdown imminent }; - struct sensor_config - { + /** Shot Limiting status. */ + enum ShotLimitingStatus { + SHOT_LIMITING_NORMAL = 0x00, ///< Normal operation + SHOT_LIMITING_IMMINENT = 0x01, ///< Shot limiting imminent + SHOT_LIMITING_REDUCTION_0_10 = + 0x02, //< Shot limiting reduction by 0 to 10% + SHOT_LIMITING_REDUCTION_10_20 = + 0x03, ///< Shot limiting reduction by 10 to 20% + SHOT_LIMITING_REDUCTION_20_30 = + 0x04, ///< Shot limiting reduction by 20 to 30% + SHOT_LIMITING_REDUCTION_30_40 = + 0x05, ///< Shot limiting reduction by 30 to 40% + SHOT_LIMITING_REDUCTION_40_50 = + 0x06, ///< Shot limiting reduction by 40 to 50% + SHOT_LIMITING_REDUCTION_50_60 = + 0x07, ///< Shot limiting reduction by 50 to 60% + SHOT_LIMITING_REDUCTION_60_70 = + 0x08, ///< Shot limiting reduction by 60 to 70% + SHOT_LIMITING_REDUCTION_70_75 = + 0x09, ///< Shot limiting reduction by 70 to 80% + }; + + /** Tag to identitify a paricular value reported in the sensor channel data + * block. */ + enum ChanField { + RANGE = 1, ///< 1st return range in mm + RANGE2 = 2, ///< 2nd return range in mm + SIGNAL = 3, ///< 1st return signal in photons + SIGNAL2 = 4, ///< 2nd return signal in photons + REFLECTIVITY = 5, ///< 1st return reflectivity, calibrated by range and sensor + ///< sensitivity in FW 2.1+. See sensor docs for more details + REFLECTIVITY2 = 6, ///< 2nd return reflectivity, calibrated by range and sensor + ///< sensitivity in FW 2.1+. See sensor docs for more details + NEAR_IR = 7, ///< near_ir in photons + FLAGS = 8, ///< 1st return flags + FLAGS2 = 9, ///< 2nd return flags + RAW_HEADERS = 40, ///< raw headers for packet/footer/column for dev use + RAW32_WORD5 = 45, ///< raw word access to packet for dev use + RAW32_WORD6 = 46, ///< raw word access to packet for dev use + RAW32_WORD7 = 47, ///< raw word access to packet for dev use + RAW32_WORD8 = 48, ///< raw word access to packet for dev use + RAW32_WORD9 = 49, ///< raw word access to packet for dev use + CUSTOM0 = 50, ///< custom user field + CUSTOM1 = 51, ///< custom user field + CUSTOM2 = 52, ///< custom user field + CUSTOM3 = 53, ///< custom user field + CUSTOM4 = 54, ///< custom user field + CUSTOM5 = 55, ///< custom user field + CUSTOM6 = 56, ///< custom user field + CUSTOM7 = 57, ///< custom user field + CUSTOM8 = 58, ///< custom user field + CUSTOM9 = 59, ///< custom user field + RAW32_WORD1 = 60, ///< raw word access to packet for dev use + RAW32_WORD2 = 61, ///< raw word access to packet for dev use + RAW32_WORD3 = 62, ///< raw word access to packet for dev use + RAW32_WORD4 = 63, ///< raw word access to packet for dev use + CHAN_FIELD_MAX = 64, ///< max which allows us to introduce future fields + }; + + /** + * Types of channel fields. + */ + enum ChanFieldType { VOID = 0, UINT8, UINT16, UINT32, UINT64 }; + + /** Stores data format information. */ + struct data_format { + uint32_t pixels_per_column; ///< pixels per column + uint32_t columns_per_packet; ///< columns per packet + uint32_t + columns_per_frame; ///< columns per frame, should match with lidar mode + std::vector + pixel_shift_by_row; ///< shift of pixels by row to enable destagger + ColumnWindow column_window; ///< window of columns over which sensor fires + UDPProfileLidar udp_profile_lidar; ///< profile of lidar packet + UDPProfileIMU udp_profile_imu; ///< profile of imu packet + uint16_t fps; ///< frames per second + }; + + /** Stores necessary information from sensor to parse and project sensor data. + */ + struct sensor_info { + std::string + name; ///< @deprecated Will be removed in the next version + std::string sn; ///< sensor serial number + std::string fw_rev; ///< fw revision + lidar_mode mode; ///< lidar mode of sensor + std::string prod_line; ///< prod line + data_format format; ///< data format of sensor + std::vector + beam_azimuth_angles; ///< beam azimuth angles for 3D projection + std::vector + beam_altitude_angles; ///< beam altitude angles for 3D projection + double lidar_origin_to_beam_origin_mm; ///< distance between lidar origin + ///< and beam origin in mm + mat4d beam_to_lidar_transform; ///< transform between beam and lidar frame + mat4d imu_to_sensor_transform; ///< transform between sensor coordinate + ///< frame and imu + mat4d lidar_to_sensor_transform; ///< transform between lidar and sensor + ///< coordinate frames + mat4d extrinsic; ///< extrinsic matrix + uint32_t init_id; ///< initialization ID updated every reinit + uint16_t udp_port_lidar; ///< the lidar destination port + uint16_t udp_port_imu; ///< the imu destination port + }; + + /** + * Struct for sensor configuration parameters. + */ + struct sensor_config { + /** + * The destination address for the + * lidar/imu data to be sent to + */ optional udp_dest; + /** + * The destination port for the lidar + * data to be sent to + */ optional udp_port_lidar; + /** + * The destination port for the imu data + * to be sent to + */ optional udp_port_imu; + /** + * The timestamp mode for the sensor to use. + * Refer to timestamp_mode for more details. + */ optional ts_mode; + /** + * The lidar mode for the sensor to use. + * Refer to lidar_mode for more details. + */ optional ld_mode; + /** + * The operating mode for the sensor to use. + * Refer to OperatingMode for more details. + */ optional operating_mode; + /** + * The multipurpose io mode for the sensor to use. + * Refer to MultipurposeIOMode for more details. + */ optional multipurpose_io_mode; + /** + * The azimuth window for the sensor to use. + * Refer to AzimuthWindow for more details. + */ optional azimuth_window; - optional signal_multiplier; + /** + * Multiplier for signal strength of sensor. See the sensor docs for + * more details on usage. + */ + optional signal_multiplier; + /** + * The nmea polarity for the sensor to use. + * Refer to Polarity for more details. + */ optional nmea_in_polarity; + /** + * Whether NMEA UART input $GPRMC messages should be ignored. + * Refer to the sensor docs for more details. + */ optional nmea_ignore_valid_char; + /** + * The nmea baud rate for the sensor to use. + * Refer to Polarity> for more details. + */ optional nmea_baud_rate; + /** + * Number of leap seconds added to UDP timestamp. + * See the sensor docs for more details. + */ optional nmea_leap_seconds; + /** + * Polarity of SYNC_PULSE_IN input. + * See Polarity for more details. + */ optional sync_pulse_in_polarity; + /** + * Polarity of SYNC_PULSE_OUT output. + * See Polarity for more details. + */ optional sync_pulse_out_polarity; + /** + * Angle in degrees that sensor traverses between each SYNC_PULSE_OUT + * pulse. See senor docs for more details. + */ optional sync_pulse_out_angle; + /** + * Width of SYNC_PULSE_OUT pulse in ms. + * See sensor docs for more details. + */ optional sync_pulse_out_pulse_width; + /** + * Frequency of SYNC_PULSE_OUT pulse in Hz. + * See sensor docs for more details. + */ optional sync_pulse_out_frequency; + /** + * Whether phase locking is enabled. + * See sensor docs for more details. + */ optional phase_lock_enable; + /** + * Angle that sensors are locked to in millidegrees. + * See sensor docs for more details. + */ optional phase_lock_offset; + + /** + * Columns per packet. + * See sensor docs for more details. + */ + optional columns_per_packet; + /** + * The lidar profile for the sensor to use. + * Refer to UDPProfileLidar for more details. + */ + optional udp_profile_lidar; + /** + * The imu profile for the sensor to use. + * Refer to UDPProfileIMU for more details. + */ + optional udp_profile_imu; }; /** Equality/Not-Equality for data_format */ @@ -293,6 +509,69 @@ namespace ouster { */ std::string to_string(AzimuthWindow azimuth_window); + /** + * Get string representation of a lidar profile. + * + * @param[in] profile The profile to get the string representation of. + * + * @return string representation of the lidar profile. + */ + std::string to_string(UDPProfileLidar profile); + + /** + * Get lidar profile from string. + * + * @param[in] s The string to decode into a lidar profile. + * + * @return lidar profile corresponding to the string, or nullopt on error. + */ + optional udp_profile_lidar_of_string(const std::string& s); + + /** + * Get string representation of an IMU profile. + * + * @param[in] profile The profile to get the string representation of. + * + * @return string representation of the lidar profile. + */ + std::string to_string(UDPProfileIMU profile); + + /** + * Get imu profile from string + * + * @param[in] s The string to decode into an imu profile. + * + * @return imu profile corresponding to the string, or nullopt on error. + */ + optional udp_profile_imu_of_string(const std::string& s); + + /** + * Get string representation of a Shot Limiting Status. + * + * @param[in] shot_limiting_status The shot limiting status to get the string + * representation of. + * + * @return string representation of the shot limiting status. + */ + std::string to_string(ShotLimitingStatus shot_limiting_status); + + /** + * Get string representation of Thermal Shutdown Status. + * + * @param[in] thermal_shutdown_status The thermal shutdown status to get the + * string representation of. + * + * @return string representation of thermal shutdown status. + */ + std::string to_string(ThermalShutdownStatus thermal_shutdown_status); + + /** + * Determine validity of provided signal multiplier value + * + * @param[in] signal_multiplier Signal multiplier value. + */ + void check_signal_multiplier(double signal_multiplier); + /** * Parse metadata text blob from the sensor into a sensor_info struct. * @@ -344,51 +623,378 @@ namespace ouster { std::string to_string(const sensor_config& config); /** - * Table of accessors for extracting data from imu and lidar packets. + * Convert non-legacy string representation of metadata to legacy. + * + * @param[in] metadata non-legacy string representation of metadata. + * + * @return legacy string representation of metadata. + */ + std::string convert_to_legacy(const std::string& metadata); + + + Json::Value to_json(const sensor_config &config); + + /** + * Get string representation of a channel field. + * + * @param[in] field The field to get the string representation of. + * + * @return string representation of the channel field. + */ + std::string to_string(ChanField field); + + /** + * Get the size of the ChanFieldType in bytes. + * + * @param[in] ft the field type + * + * @return size of the field type in bytes + */ + size_t field_type_size(ChanFieldType ft); + + /** + * Get string representation of a channel field. + * + * @param[in] ft The field type to get the string representation of. + * + * @return string representation of the channel field type. + */ + std::string to_string(ChanFieldType ft); + + /** + * Table of accessors for extracting data from imu and lidar packets. + * + * In the user guide, refer to section 9 for the lidar packet format and section + * 10 for imu packets. + * + * For 0 <= n < columns_per_packet, nth_col(n, packet_buf) returns a pointer to + * the nth measurement block. For 0 <= m < pixels_per_column, nth_px(m, col_buf) + * returns the mth channel data block. + * + * Use imu_la_{x,y,z} to access the acceleration in the corresponding + * direction. Use imu_av_{x,y,z} to read the angular velocity. + */ + class packet_format final + { + template + T px_field(const uint8_t *px_buf, ChanField i) const; + + struct Impl; + std::shared_ptr impl_{}; + + std::vector> field_types_; + + public: + explicit packet_format( + const sensor_info &info);//< create packet_format from sensor_info + + using FieldIter = + decltype(field_types_)::const_iterator;///< iterator over field types + ///< of packet + + const UDPProfileLidar + udp_profile_lidar; ///< udp lidar profile of packet format + const size_t lidar_packet_size{};///< lidar packet size + const size_t imu_packet_size; ///< imu packet size + const int columns_per_packet; ///< columns per lidar packet + const int pixels_per_column; ///< pixels per column for lidar + + const size_t packet_header_size{}; + const size_t col_header_size{}; + const size_t col_footer_size{}; + const size_t col_size{}; + const size_t packet_footer_size{}; + + /** + * Read the packet type packet header. + * + * @param[in] lidar_buf the lidar buf. * - * In the user guide, refer to section 9 for the lidar packet format and section - * 10 for imu packets. + * @return the packet type. + */ + uint16_t packet_type(const uint8_t *lidar_buf) const; + + /** + * Read the frame_id packet header. * - * For 0 <= n < columns_per_packet, nth_col(n, packet_buf) returns a pointer to - * the nth measurement block. For 0 <= m < pixels_per_column, nth_px(m, col_buf) - * returns the mth channel data block. + * @param[in] lidar_buf the lidar buf. * - * Use imu_la_{x,y,z} to access the acceleration in the corresponding - * direction. Use imu_av_{x,y,z} to read the angular velocity. + * @return the frame id. */ - struct packet_format - { - const size_t lidar_packet_size; - const size_t imu_packet_size; - const int columns_per_packet; - const int pixels_per_column; - const int encoder_ticks_per_rev; + uint16_t frame_id(const uint8_t *lidar_buf) const; + + /** + * Read the initialization id packet header. + * + * @param[in] lidar_buf the lidar buf. + * + * @return the init id. + */ + uint32_t init_id(const uint8_t *lidar_buf) const; + + /** + * Read the packet serial number header. + * + * @param[in] lidar_buf the lidar buf. + * + * @return the serial number. + */ + uint64_t prod_sn(const uint8_t *lidar_buf) const; + + /** + * Read the packet thermal shutdown countdown + * + * @param[in] lidar_buf the lidar buf. + * + * @return the thermal shutdown countdown. + */ + uint16_t countdown_thermal_shutdown(const uint8_t *lidar_buf) const; + + /** + * Read the packet shot limiting countdown + * + * @param[in] lidar_buf the lidar buf. + * + * @return the shot limiting countdown. + */ + uint16_t countdown_shot_limiting(const uint8_t *lidar_buf) const; + + /** + * Read the packet thermal shutdown header. + * + * @param[in] lidar_buf the lidar buf. + * + * @return the thermal shutdown status + */ + uint8_t thermal_shutdown(const uint8_t *lidar_buf) const; + + /** + * Read the packet shot limiting header. + * + * @param[in] lidar_buf the lidar buf. + * + * @return the shot limiting status + */ + uint8_t shot_limiting(const uint8_t *lidar_buf) const; + + /** + * Get the bit width of the specified channel field. + * + * @param[in] f the channel field to query. + * + * @return a type tag specifying the bitwidth of the requested field or + * ChannelFieldType::VOID if it is not supported by the packet format. + */ + ChanFieldType field_type(ChanField f) const; + + /** + * A const forward iterator over field / type pairs. + */ + FieldIter begin() const; + + /** + * A const forward iterator over field / type pairs. + */ + FieldIter end() const; + + /** + * Get pointer to the packet footer of a lidar buffer. + * + * @param[in] lidar_buf the lidar buffer. + * + * @return pointer to packet footer of lidar buffer, can be nullptr if + * packet format doesn't have packet footer. + */ + const uint8_t *footer(const uint8_t *lidar_buf) const; // Measurement block accessors - const uint8_t * (* const nth_col)(int n, const uint8_t * lidar_buf); - uint64_t(*const col_timestamp)(const uint8_t * col_buf); - uint32_t(*const col_encoder)(const uint8_t * col_buf); - uint16_t(*const col_measurement_id)(const uint8_t * col_buf); - uint16_t(*const col_frame_id)(const uint8_t * col_buf); - uint32_t(*const col_status)(const uint8_t * col_buf); - - // Channel data block accessors - const uint8_t * (* const nth_px)(int n, const uint8_t * col_buf); - uint32_t(*const px_range)(const uint8_t * px_buf); - uint16_t(*const px_reflectivity)(const uint8_t * px_buf); - uint16_t(*const px_signal)(const uint8_t * px_buf); - uint16_t(*const px_ambient)(const uint8_t * px_buf); + /** + * Get pointer to nth column of a lidar buffer. + * + * @param[in] n which column. + * @param[in] lidar_buf the lidar buffer. + * + * @return pointer to nth column of lidar buffer. + */ + const uint8_t *nth_col(int n, const uint8_t *lidar_buf) const; + + /** + * Read column timestamp from column buffer. + * + * @param[in] col_buf the column buffer. + * + * @return column timestamp. + */ + uint64_t col_timestamp(const uint8_t *col_buf) const; + + /** + * Read measurement id from column buffer. + * + * @param[in] col_buf the column buffer. + * + * @return column measurement id. + */ + uint16_t col_measurement_id(const uint8_t *col_buf) const; + + /** + * Read column status from column buffer. + * + * @param[in] col_buf the column buffer. + * + * @return column status. + */ + uint32_t col_status(const uint8_t *col_buf) const; + + [[deprecated("Use frame_id instead")]] uint16_t col_frame_id( + const uint8_t *col_buf) const;///< @deprecated Use frame_id instead + + /** + * Copy the specified channel field out of a packet measurement block. + * + * @tparam T T should be an unsigned integer type large enough to store + * values of the specified field. Otherwise, data will be truncated. + * + * @param[in] col_buf a measurement block pointer returned by `nth_col()`. + * @param[in] f the channel field to copy. + * @param[out] dst destination array of size pixels_per_column * dst_stride. + * @param[in] dst_stride stride for writing to the destination array. + */ + template::value, T>::type = 0> + void col_field(const uint8_t *col_buf, ChanField f, T *dst, + int dst_stride = 1) const; + + // Per-pixel channel data block accessors + /** + * Get pointer to nth pixel of a column buffer. + * + * @param[in] n which pixel. + * @param[in] col_buf the column buffer. + * + * @return pointer to nth pixel of a column buffer. + */ + const uint8_t *nth_px(int n, const uint8_t *col_buf) const; + + /** + * Read range from pixel buffer. + * + * @param[in] px_buf the pixel buffer. + * + * @return range from pixel buffer. + */ + uint32_t px_range(const uint8_t *px_buf) const; + + /** + * Read reflectivity from pixel buffer. + * + * @param[in] px_buf the pixel buffer. + * + * @return reflectivity from pixel buffer. + */ + uint16_t px_reflectivity(const uint8_t *px_buf) const; + + /** + * Read signal from pixel buffer. + * + * @param[in] px_buf the pixel buffer. + * + * @return signal from pixel buffer. + */ + uint16_t px_signal(const uint8_t *px_buf) const; + + // TODO switch to px_near_ir + /** + * Read ambient from pixel buffer. + * + * @param[in] px_buf the pixel buffer. + * + * @return ambient from pixel buffer. + */ + uint16_t px_ambient(const uint8_t *px_buf) const; // IMU packet accessors - uint64_t(*const imu_sys_ts)(const uint8_t * imu_buf); - uint64_t(*const imu_accel_ts)(const uint8_t * imu_buf); - uint64_t(*const imu_gyro_ts)(const uint8_t * imu_buf); - float(*const imu_la_x)(const uint8_t * imu_buf); - float(*const imu_la_y)(const uint8_t * imu_buf); - float(*const imu_la_z)(const uint8_t * imu_buf); - float(*const imu_av_x)(const uint8_t * imu_buf); - float(*const imu_av_y)(const uint8_t * imu_buf); - float(*const imu_av_z)(const uint8_t * imu_buf); + /** + * Read sys ts from imu packet buffer. + * @param[in] imu_buf the imu packet buffer. + * + * @return sys ts from imu pacet buffer. + */ + static uint64_t imu_sys_ts(const uint8_t *imu_buf) ; + + /** + * Read acceleration timestamp. + * + * @param[in] imu_buf the imu packet buffer. + * + * @return acceleration ts from imu packet buffer. + */ + static uint64_t imu_accel_ts(const uint8_t *imu_buf) ; + + /** + * Read gyro timestamp. + * + * @param[in] imu_buf the imu packet buffer. + * + * @return gyro ts from imu packet buffer. + */ + static uint64_t imu_gyro_ts(const uint8_t *imu_buf) ; + + /** + * Read acceleration in x. + * + * @param[in] imu_buf the imu packet buffer. + * + * @return acceleration in x. + */ + static float imu_la_x(const uint8_t *imu_buf) ; + + /** + * Read acceleration in y. + * + * @param[in] imu_buf the imu packet buffer. + * + * @return acceleration in y. + */ + static float imu_la_y(const uint8_t *imu_buf) ; + + /** + * Read acceleration in z. + * + * @param[in] imu_buf the imu packet buffer. + * + * @return acceleration in z. + */ + static float imu_la_z(const uint8_t *imu_buf) ; + + /** + * Read angular velocity in x. + * + * @param[in] imu_buf the imu packet buffer. + * + * @return angular velocity in x. + */ + static float imu_av_x(const uint8_t *imu_buf) ; + + /** + * Read angular velocity in y. + * + * @param[in] imu_buf the imu packet buffer. + * + * @return angular velocity in y. + */ + static float imu_av_y(const uint8_t *imu_buf) ; + + /** + * Read angular velocity in z. + * + * @param[in] imu_buf the imu packet buffer. + * + * @return angular velocity in z. + */ + static float imu_av_z(const uint8_t *imu_buf) ; + + /** Declare get_format as friend. */ + friend const packet_format &get_format(const sensor_info &); }; /** diff --git a/ros2_ouster/include/ros2_ouster/conversions.hpp b/ros2_ouster/include/ros2_ouster/conversions.hpp index 365e1ff..62bb16d 100644 --- a/ros2_ouster/include/ros2_ouster/conversions.hpp +++ b/ros2_ouster/include/ros2_ouster/conversions.hpp @@ -19,28 +19,171 @@ #include #include -#include "pcl/point_cloud.h" -#include "pcl/point_types.h" -#include "pcl_conversions/pcl_conversions.h" -#include "ros2_ouster/client/point.h" -#include "ros2_ouster/interfaces/metadata.hpp" +#include +#include +#include -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "sensor_msgs/msg/imu.hpp" -#include "sensor_msgs/msg/laser_scan.hpp" -#include "tf2/LinearMath/Transform.h" +#include +#include +#include +#include #ifdef TF2_CPP_HEADERS + #include #include #else + #include #include #endif #include "ouster_msgs/msg/metadata.hpp" #include "ros2_ouster/client/client.h" +#include "ros2_ouster/client/point.h" +#include "ros2_ouster/interfaces/metadata.hpp" namespace ros2_ouster { +template +using PointsT = Eigen::Array; +using PointsF = PointsT; + +using Cloud = pcl::PointCloud; + +namespace util { + +/** + * Converts a staggered range image to Cartesian points. + * + * @param[in, out] points The resulting point cloud, should be pre-allocated and + * have the same dimensions as the direction array. + * @param[in] range a range image in the same format as the RANGE field of a + * LidarScan. + * @param[in] direction the direction of an xyz lut. + * @param[in] offset the offset of an xyz lut. + * + * @return Cartesian points where ith row is a 3D point which corresponds + * to ith pixel in LidarScan where i = row * w + col. + */ +template +void cartesianT(PointsT& points, + const Eigen::Ref>& range, + const PointsT& direction, const PointsT& offset) { + assert(points.rows() == direction.rows() && + "points & direction row count mismatch"); + assert(points.rows() == offset.rows() && + "points & offset row count mismatch"); + assert(points.rows() == range.size() && + "points and range image size mismatch"); + + const auto pts = points.data(); + const auto* const rng = range.data(); + const auto* const dir = direction.data(); + const auto* const ofs = offset.data(); + + const auto N = range.size(); + const auto col_x = 0 * N; // 1st column of points (x) + const auto col_y = 1 * N; // 2nd column of points (y) + const auto col_z = 2 * N; // 3rd column of points (z) + +#ifdef __OUSTER_UTILIZE_OPENMP__ +#pragma omp parallel for schedule(static) +#endif + for (auto i = 0; i < N; ++i) { + const auto r = rng[i]; + const auto idx_x = col_x + i; + const auto idx_y = col_y + i; + const auto idx_z = col_z + i; + if (r == 0) { + pts[idx_x] = pts[idx_y] = pts[idx_z] = static_cast(0.0); + } else { + pts[idx_x] = r * dir[idx_x] + ofs[idx_x]; + pts[idx_y] = r * dir[idx_y] + ofs[idx_y]; + pts[idx_z] = r * dir[idx_z] + ofs[idx_z]; + } + } +} + +struct read_and_cast { + template + void operator()(Eigen::Ref> field, + ouster::img_t& dest) { + dest = field.template cast(); + } +}; + +inline ouster::sensor::ChanField suitable_return(ouster::sensor::ChanField input_field, bool second) { + switch (input_field) { + case ouster::sensor::ChanField::RANGE: + case ouster::sensor::ChanField::RANGE2: + return second ? ouster::sensor::ChanField::RANGE2 + : ouster::sensor::ChanField::RANGE; + case ouster::sensor::ChanField::SIGNAL: + case ouster::sensor::ChanField::SIGNAL2: + return second ? ouster::sensor::ChanField::SIGNAL2 + : ouster::sensor::ChanField::SIGNAL; + case ouster::sensor::ChanField::REFLECTIVITY: + case ouster::sensor::ChanField::REFLECTIVITY2: + return second ? ouster::sensor::ChanField::REFLECTIVITY2 + : ouster::sensor::ChanField::REFLECTIVITY; + case ouster::sensor::ChanField::NEAR_IR: + return ouster::sensor::ChanField::NEAR_IR; + default: + throw std::runtime_error("Unreachable"); + } +} + +template +inline ouster::img_t get_or_fill_zero(ouster::sensor::ChanField f, + const ouster::LidarScan& ls) { + if (!ls.field_type(f)) { + return ouster::img_t::Zero(ls.h, ls.w); + } + + ouster::img_t result{ls.h, ls.w}; + ouster::impl::visit_field(ls, f, read_and_cast(), result); + return result; +} + +template +void copy_scan_to_cloud(Cloud& cloud, const ouster::LidarScan& ls, + uint64_t scan_ts, const PointT& points, + const ouster::img_t& range, + const ouster::img_t& reflectivity, + const ouster::img_t& near_ir, + const ouster::img_t& signal) { + auto timestamp = ls.timestamp(); + + const auto rg = range.data(); + const auto rf = reflectivity.data(); + const auto nr = near_ir.data(); + const auto sg = signal.data(); + +#ifdef __OUSTER_UTILIZE_OPENMP__ +#pragma omp parallel for collapse(2) +#endif + for (auto u = 0; u < ls.h; u++) { + for (auto v = 0; v < ls.w; v++) { + const auto col_ts = timestamp[v]; + const auto ts = col_ts > scan_ts ? col_ts - scan_ts : 0UL; + const auto idx = u * ls.w + v; + const auto xyz = points.row(idx); + cloud.points[idx] = ouster_ros::Point{ + {static_cast(xyz(0)), static_cast(xyz(1)), + static_cast(xyz(2)), 1.0f}, + static_cast(sg[idx]), + static_cast(ts), + static_cast(rf[idx]), + static_cast(u), + static_cast(nr[idx]), + static_cast(rg[idx]), + }; + } + } +} + +} //namespace util + /** * @brief Convert ClientState to string */ @@ -89,12 +232,13 @@ inline ouster_msgs::msg::Metadata toMsg(const ros2_ouster::Metadata & mdata) msg.timestamp_mode = mdata.timestamp_mode; msg.beam_azimuth_angles = mdata.beam_azimuth_angles; msg.beam_altitude_angles = mdata.beam_altitude_angles; + msg.beam_to_lidar_transform = ros2_ouster::toVector(mdata.beam_to_lidar_transform); msg.imu_to_sensor_transform = ros2_ouster::toVector(mdata.imu_to_sensor_transform); msg.lidar_to_sensor_transform = ros2_ouster::toVector(mdata.lidar_to_sensor_transform); msg.serial_no = mdata.sn; msg.firmware_rev = mdata.fw_rev; - msg.imu_port = mdata.imu_port; - msg.lidar_port = mdata.lidar_port; + msg.imu_port = mdata.udp_port_imu; + msg.lidar_port = mdata.udp_port_lidar; return msg; } @@ -102,26 +246,17 @@ inline ouster_msgs::msg::Metadata toMsg(const ros2_ouster::Metadata & mdata) * @brief Convert transformation to message format */ inline geometry_msgs::msg::TransformStamped toMsg( - const Eigen::Matrix & mat, const std::string & frame, - const std::string & child_frame, const rclcpp::Time & time) + const ouster::mat4d & mat, const std::string & frame, + const std::string & child_frame, const rclcpp::Time & timestamp) { - assert(mat.size() == 16); - - tf2::Transform tf; + Eigen::Affine3d aff; + aff.linear() = mat.block<3, 3>(0, 0); + aff.translation() = mat.block<3, 1>(0, 3) * 1e-3; - tf.setOrigin({mat(3) / 1e3, mat(7) / 1e3, mat(11) / 1e3}); - tf.setBasis( - { - mat(0), mat(1), mat(2), - mat(4), mat(5), mat(6), - mat(8), mat(9), mat(10) - }); - - geometry_msgs::msg::TransformStamped msg; - msg.header.stamp = time; + geometry_msgs::msg::TransformStamped msg = tf2::eigenToTransform(aff); + msg.header.stamp = timestamp; msg.header.frame_id = frame; msg.child_frame_id = child_frame; - msg.transform = tf2::toMsg(tf); return msg; } @@ -143,7 +278,7 @@ inline sensor_msgs::msg::Imu toMsg( m.orientation.x = 0; m.orientation.y = 0; m.orientation.z = 0; - m.orientation.w = 1; + m.orientation.w = 0; m.header.stamp = override_ts == 0 ? rclcpp::Time(pf.imu_gyro_ts(buf)) : rclcpp::Time(override_ts); m.header.frame_id = frame; @@ -192,7 +327,7 @@ inline sensor_msgs::msg::Imu toMsg( * row-major ordered consistent to the shape of the LiDAR array. */ inline sensor_msgs::msg::PointCloud2 toMsg( - const pcl::PointCloud & cloud, + const Cloud & cloud, const std::chrono::nanoseconds timestamp, const std::string & frame, const uint64_t override_ts) @@ -208,13 +343,24 @@ inline sensor_msgs::msg::PointCloud2 toMsg( * @brief Convert cloud to scan message format */ inline sensor_msgs::msg::LaserScan toMsg( - const ouster::LidarScan ls, + const ouster::LidarScan& ls, const std::chrono::nanoseconds timestamp, const std::string & frame, const ouster::sensor::sensor_info & mdata, const uint8_t ring_to_use, - const uint64_t override_ts) + const uint64_t override_ts, + int return_index) { + const bool second = (return_index == 1); + + // across supported lidar profiles range is always 32-bit + auto range_channel_field = + second ? ouster::sensor::ChanField::RANGE2 : ouster::sensor::ChanField::RANGE; + ouster::img_t range = ls.field(range_channel_field); + + ouster::img_t signal = util::get_or_fill_zero( + util::suitable_return(ouster::sensor::ChanField::SIGNAL, second), ls); + sensor_msgs::msg::LaserScan msg; rclcpp::Time t(timestamp.count()); msg.header.stamp = override_ts == 0 ? t : rclcpp::Time(override_ts); @@ -234,10 +380,10 @@ inline sensor_msgs::msg::LaserScan toMsg( // due to the condition being reduced to i >= 0 for (size_t i = ls.w * ring_to_use + ls.w; i-- > ls.w * ring_to_use;) { msg.ranges.push_back( - static_cast((ls.field(ouster::LidarScan::RANGE)(i) * ouster::sensor::range_unit)) + static_cast((range.data()[i] * ouster::sensor::range_unit)) ); msg.intensities.push_back( - static_cast((ls.field(ouster::LidarScan::INTENSITY)(i))) + static_cast((signal.data()[i])) ); } @@ -251,31 +397,36 @@ inline sensor_msgs::msg::LaserScan toMsg( * @param ls input lidar data * @param cloud output pcl pointcloud to populate */ -static void toCloud( - const ouster::XYZLut & xyz_lut, - ouster::LidarScan::ts_t scan_ts, - const ouster::LidarScan & ls, pcl::PointCloud & cloud) -{ - cloud.resize(ls.w * ls.h); - auto points = ouster::cartesian(ls, xyz_lut); +inline void toCloud(const ouster::XYZLut & xyz_lut, uint64_t scan_ts, + const ouster::LidarScan& ls, Cloud& cloud, + int return_index) { + const bool second = (return_index == 1); - for (auto u = 0; u < ls.h; u++) { - for (auto v = 0; v < ls.w; v++) { - const auto xyz = points.row(u * ls.w + v); - const auto pix = ls.data.row(u * ls.w + v); - const auto ts = (ls.header(v).timestamp - scan_ts).count(); - cloud(v, u) = ouster_ros::Point{ - {{static_cast(xyz(0)), - static_cast(xyz(1)), - static_cast(xyz(2)), 1.0f}}, - static_cast(pix(ouster::LidarScan::INTENSITY)), - static_cast(ts), - static_cast(pix(ouster::LidarScan::REFLECTIVITY)), - static_cast(u), - static_cast(pix(ouster::LidarScan::AMBIENT)), - static_cast(pix(ouster::LidarScan::RANGE))}; - } - } + assert(cloud.width == static_cast(ls.w) && + cloud.height == static_cast(ls.h) && + "point cloud and lidar scan size mismatch"); + + // across supported lidar profiles range is always 32-bit + auto range_channel_field = + second ? ouster::sensor::ChanField::RANGE2 : ouster::sensor::ChanField::RANGE; + ouster::img_t range = ls.field(range_channel_field); + + ouster::img_t reflectivity = util::get_or_fill_zero( + util::suitable_return(ouster::sensor::ChanField::REFLECTIVITY, second), ls); + + ouster::img_t signal = util::get_or_fill_zero( + util::suitable_return(ouster::sensor::ChanField::SIGNAL, second), ls); + + ouster::img_t near_ir = util::get_or_fill_zero( + util::suitable_return(ouster::sensor::ChanField::NEAR_IR, second), ls); + + const PointsF lut_direction = xyz_lut.direction.cast(); + const PointsF lut_offset = xyz_lut.offset.cast(); + PointsF points = PointsF(lut_direction.rows(), lut_offset.cols()); + util::cartesianT(points, range, lut_direction, lut_offset); + + util::copy_scan_to_cloud(cloud, ls, scan_ts, points, range, reflectivity, near_ir, + signal); } } // namespace ros2_ouster diff --git a/ros2_ouster/include/ros2_ouster/full_rotation_accumulator.hpp b/ros2_ouster/include/ros2_ouster/full_rotation_accumulator.hpp index 695d99a..cfcefef 100644 --- a/ros2_ouster/include/ros2_ouster/full_rotation_accumulator.hpp +++ b/ros2_ouster/include/ros2_ouster/full_rotation_accumulator.hpp @@ -34,19 +34,23 @@ class FullRotationAccumulator public: FullRotationAccumulator( const ouster::sensor::sensor_info & mdata, - const ouster::sensor::packet_format & pf) - : _batchReady(false), _pf(pf), _packets_accumulated(0) + ouster::sensor::packet_format pf) + : _batchReady(false), _pf(std::move(pf)), _packets_accumulated(0) { - _batch = std::make_unique(mdata.format.columns_per_frame, _pf); + _batch = std::make_unique(mdata); _ls = std::make_shared( ouster::LidarScan{mdata.format.columns_per_frame, - mdata.format.pixels_per_column}); + mdata.format.pixels_per_column, mdata.format.udp_profile_lidar}); + + _compute_scan_ts = [this](const auto& ts_v) { + return compute_scan_ts_0(ts_v); + }; } /** * @brief Returns true if the lidarscan is ready */ - bool isBatchReady() + bool isBatchReady() const { return _batchReady; } @@ -90,30 +94,135 @@ class FullRotationAccumulator _packets_accumulated++; if (_batch->operator()(data, *_ls)) { - auto h = std::find_if( - _ls->headers.begin(), _ls->headers.end(), [](const auto & h) { - return h.timestamp != std::chrono::nanoseconds{0}; - }); - if (h != _ls->headers.end()) { - _timestamp = h->timestamp; - } + _timestamp = std::chrono::nanoseconds(_compute_scan_ts(_ls->timestamp())); _batchReady = true; } } - uint64_t getPacketsAccumulated() + uint64_t getPacketsAccumulated() const { return _packets_accumulated; } private: + template + int find_if_reverse(const Eigen::Array& array, + UnaryPredicate predicate) { + auto p = array.data() + array.size() - 1; + do { + if (predicate(*p)) return p - array.data(); + } while (p-- != array.data()); + return -1; + } + + template + uint64_t ulround(T value) { + T rounded_value = std::round(value); + if (rounded_value < 0) return 0ULL; + if (rounded_value > ULLONG_MAX) return ULLONG_MAX; + return static_cast(rounded_value); + } + + static uint64_t linear_interpolate(int x0, uint64_t y0, int x1, uint64_t y1, int x) { + uint64_t min_v, max_v; + double sign; + if (y1 > y0) { + min_v = y0; + max_v = y1; + sign = +1; + } else { + min_v = y1; + max_v = y0; + sign = -1; + } + return y0 + (x - x0) * sign * (max_v - min_v) / (x1 - x0); + } + + uint64_t impute_value(int last_scan_last_nonzero_idx, + uint64_t last_scan_last_nonzero_value, + int curr_scan_first_nonzero_idx, + uint64_t curr_scan_first_nonzero_value, + int scan_width) { + assert(scan_width + curr_scan_first_nonzero_idx > + last_scan_last_nonzero_idx); + double interpolated_value = linear_interpolate( + last_scan_last_nonzero_idx, last_scan_last_nonzero_value, + scan_width + curr_scan_first_nonzero_idx, + curr_scan_first_nonzero_value, scan_width); + return ulround(interpolated_value); + } + + uint64_t extrapolate_value(int curr_scan_first_nonzero_idx, + uint64_t curr_scan_first_nonzero_value) { + double extrapolated_value = + curr_scan_first_nonzero_value - + _scan_col_ts_spacing_ns * curr_scan_first_nonzero_idx; + return ulround(extrapolated_value); + } + + // compute_scan_ts_0 for first scan + uint64_t compute_scan_ts_0( + const ouster::LidarScan::Header& ts_v) { + auto idx = std::find_if(ts_v.data(), ts_v.data() + ts_v.size(), + [](uint64_t h) { return h != 0; }); + assert(idx != ts_v.data() + ts_v.size()); // should never happen + int curr_scan_first_nonzero_idx = idx - ts_v.data(); + uint64_t curr_scan_first_nonzero_value = *idx; + + uint64_t scan_ns = + curr_scan_first_nonzero_idx == 0 + ? curr_scan_first_nonzero_value + : extrapolate_value(curr_scan_first_nonzero_idx, + curr_scan_first_nonzero_value); + + _last_scan_last_nonzero_idx = + find_if_reverse(ts_v, [](uint64_t h) { return h != 0; }); + assert(_last_scan_last_nonzero_idx >= 0); // should never happen + _last_scan_last_nonzero_value = ts_v(_last_scan_last_nonzero_idx); + _compute_scan_ts = [this](const auto& ts_v) { + return compute_scan_ts_n(ts_v); + }; + return scan_ns; + } + + // compute_scan_ts_n applied to all subsequent scans except first one + uint64_t compute_scan_ts_n( + const ouster::LidarScan::Header& ts_v) { + auto idx = std::find_if(ts_v.data(), ts_v.data() + ts_v.size(), + [](uint64_t h) { return h != 0; }); + assert(idx != ts_v.data() + ts_v.size()); // should never happen + int curr_scan_first_nonzero_idx = idx - ts_v.data(); + uint64_t curr_scan_first_nonzero_value = *idx; + + uint64_t scan_ns = curr_scan_first_nonzero_idx == 0 + ? curr_scan_first_nonzero_value + : impute_value(_last_scan_last_nonzero_idx, + _last_scan_last_nonzero_value, + curr_scan_first_nonzero_idx, + curr_scan_first_nonzero_value, + static_cast(ts_v.size())); + + _last_scan_last_nonzero_idx = + find_if_reverse(ts_v, [](uint64_t h) { return h != 0; }); + assert(_last_scan_last_nonzero_idx >= 0); // should never happen + _last_scan_last_nonzero_value = ts_v(_last_scan_last_nonzero_idx); + return scan_ns; + } + bool _batchReady; - std::chrono::nanoseconds _timestamp; + std::chrono::nanoseconds _timestamp{}; std::unique_ptr _batch; std::shared_ptr _ls; ouster::sensor::packet_format _pf; uint64_t _packets_accumulated = 0; + + std::function&)> + _compute_scan_ts; + int _last_scan_last_nonzero_idx = -1; + uint64_t _last_scan_last_nonzero_value = 0; + double _scan_col_ts_spacing_ns{}; // interval or spacing between columns of a + // scan }; } // namespace sensor diff --git a/ros2_ouster/include/ros2_ouster/interfaces/configuration.hpp b/ros2_ouster/include/ros2_ouster/interfaces/configuration.hpp index d1d3db9..7f58971 100644 --- a/ros2_ouster/include/ros2_ouster/interfaces/configuration.hpp +++ b/ros2_ouster/include/ros2_ouster/interfaces/configuration.hpp @@ -31,6 +31,7 @@ struct Configuration std::string timestamp_mode; std::string metadata_filepath; std::string ethernet_device; + std::string lidar_udp_profile; }; } // namespace ros2_ouster diff --git a/ros2_ouster/include/ros2_ouster/interfaces/metadata.hpp b/ros2_ouster/include/ros2_ouster/interfaces/metadata.hpp index 4343192..361c6b8 100644 --- a/ros2_ouster/include/ros2_ouster/interfaces/metadata.hpp +++ b/ros2_ouster/include/ros2_ouster/interfaces/metadata.hpp @@ -14,6 +14,7 @@ #ifndef ROS2_OUSTER__INTERFACES__METADATA_HPP_ #define ROS2_OUSTER__INTERFACES__METADATA_HPP_ +#include #include #include #include @@ -39,19 +40,19 @@ struct Metadata : ouster::sensor::sensor_info beam_azimuth_angles = {}; beam_altitude_angles = {}; lidar_origin_to_beam_origin_mm = 0.; + beam_to_lidar_transform = {}; imu_to_sensor_transform = {}; lidar_to_sensor_transform = {}; extrinsic = {}; + init_id = 0; + udp_port_lidar = 0; + udp_port_imu = 0; timestamp_mode = "UNKNOWN"; - imu_port = 0; - lidar_port = 0; } Metadata( const ouster::sensor::sensor_info & info, int _imu_port, - int _lidar_port, const std::string & _timestamp_mode) - : imu_port(_imu_port), - lidar_port(_lidar_port), - timestamp_mode(_timestamp_mode) + int _lidar_port, std::string _timestamp_mode) + : timestamp_mode(std::move(_timestamp_mode)) { name = info.name; sn = info.sn; @@ -62,20 +63,23 @@ struct Metadata : ouster::sensor::sensor_info beam_azimuth_angles = info.beam_azimuth_angles; beam_altitude_angles = info.beam_altitude_angles; lidar_origin_to_beam_origin_mm = info.lidar_origin_to_beam_origin_mm; + beam_to_lidar_transform = info.beam_to_lidar_transform; imu_to_sensor_transform = info.imu_to_sensor_transform; lidar_to_sensor_transform = info.lidar_to_sensor_transform; extrinsic = info.extrinsic; + udp_port_lidar = _lidar_port; + udp_port_imu = _imu_port; } + std::string timestamp_mode; - int imu_port; - int lidar_port; }; /** * @brief fill in values that could not be parsed from metadata. */ -inline void populate_missing_metadata_defaults(ouster::sensor::sensor_info & info) +inline void populate_missing_metadata_defaults(ouster::sensor::sensor_info & info, + ouster::sensor::lidar_mode specified_lidar_mode) { if (info.name.empty()) { info.name = "UNKNOWN"; @@ -85,8 +89,20 @@ inline void populate_missing_metadata_defaults(ouster::sensor::sensor_info & inf info.sn = "UNKNOWN"; } + ouster::util::version v = ouster::util::version_of_string(info.fw_rev); + if (v == ouster::util::invalid_version) { + std::cerr << "Unknown sensor firmware version; output may not be reliable" + << std::endl; + } + else if (v < ouster::sensor::min_version) { + std::cerr << "Firmware < " << to_string(ouster::sensor::min_version).c_str() + << " not supported; output may not be reliable" << std::endl; + } + if (!info.mode) { - info.mode = ouster::sensor::MODE_UNSPEC; + std::cerr << "Lidar mode not found in metadata; output may not be reliable" + << std::endl; + info.mode = specified_lidar_mode; } if (info.prod_line.empty()) { @@ -94,6 +110,8 @@ inline void populate_missing_metadata_defaults(ouster::sensor::sensor_info & inf } if (info.beam_azimuth_angles.empty() || info.beam_altitude_angles.empty()) { + std::cerr << "Beam angles not found in metadata; using design values" + << std::endl; info.beam_azimuth_angles = ouster::sensor::gen1_azimuth_angles; info.beam_altitude_angles = ouster::sensor::gen1_altitude_angles; } diff --git a/ros2_ouster/include/ros2_ouster/interfaces/sensor_interface.hpp b/ros2_ouster/include/ros2_ouster/interfaces/sensor_interface.hpp index 8f9620f..66746a4 100644 --- a/ros2_ouster/include/ros2_ouster/interfaces/sensor_interface.hpp +++ b/ros2_ouster/include/ros2_ouster/interfaces/sensor_interface.hpp @@ -102,6 +102,13 @@ class SensorInterface * @return packet format struct */ virtual ouster::sensor::packet_format getPacketFormat() = 0; + + /** + * @brief Indicate whether a reactivation operation is required + * @param packet data to read from + * @return sensor metadata struct + */ + virtual bool shouldReset(const ouster::sensor::client_state & state, const uint8_t * packet) = 0; }; } // namespace ros2_ouster diff --git a/ros2_ouster/include/ros2_ouster/ouster_driver.hpp b/ros2_ouster/include/ros2_ouster/ouster_driver.hpp index b57d404..ed24ea8 100644 --- a/ros2_ouster/include/ros2_ouster/ouster_driver.hpp +++ b/ros2_ouster/include/ros2_ouster/ouster_driver.hpp @@ -142,6 +142,10 @@ class OusterDriver : public lifecycle_interface::LifecycleInterface */ void processData(); + void handlePollError(); + bool handleLidarPacket(const ouster::sensor::client_state & state); + bool handleImuPacket(const ouster::sensor::client_state & state); + rclcpp::Service::SharedPtr _reset_srv; rclcpp::Service::SharedPtr _metadata_srv; @@ -169,6 +173,13 @@ class OusterDriver : public lifecycle_interface::LifecycleInterface std::condition_variable _process_cond; std::mutex _ringbuffer_mutex; bool _processing_active; + + int _poll_error_count = 0; + static constexpr int _max_poll_error_count = 10; + int _lidar_packet_error_count = 0; + static constexpr int _max_lidar_packet_error_count = 60; + int _imu_packet_error_count = 0; + static constexpr int _max_imu_packet_error_count = 60; }; } // namespace ros2_ouster diff --git a/ros2_ouster/include/ros2_ouster/processors/image_processor.hpp b/ros2_ouster/include/ros2_ouster/processors/image_processor.hpp index 38d4d12..0a0677c 100644 --- a/ros2_ouster/include/ros2_ouster/processors/image_processor.hpp +++ b/ros2_ouster/include/ros2_ouster/processors/image_processor.hpp @@ -56,38 +56,55 @@ class ImageProcessor : public ros2_ouster::DataProcessorInterface ImageProcessor( const rclcpp_lifecycle::LifecycleNode::SharedPtr node, const ouster::sensor::sensor_info & mdata, - const std::string & frame, + std::string frame, const rclcpp::QoS & qos, const ouster::sensor::packet_format & pf, std::shared_ptr fullRotationAccumulator) - : DataProcessorInterface(), _node(node), _frame(frame), _pf(pf) + : DataProcessorInterface(), _node(node), _frame(std::move(frame)), _pf(pf) { - _fullRotationAccumulator = fullRotationAccumulator; + _fullRotationAccumulator = std::move(fullRotationAccumulator); _height = mdata.format.pixels_per_column; _width = mdata.format.columns_per_frame; _px_offset = mdata.format.pixel_shift_by_row; - _ls = ouster::LidarScan{_width, _height}; + _ls = ouster::LidarScan{_width, _height, mdata.format.udp_profile_lidar}; + //TODO(images): figure out if the processors are still valid, add reflectivity _range_image_pub = _node->create_publisher( "range_image", qos); _intensity_image_pub = _node->create_publisher( - "intensity_image", qos); + "signal_image", qos); _ambient_image_pub = _node->create_publisher( - "ambient_image", qos); + "nearir_image", qos); } /** * @brief A destructor clearing memory allocated */ - ~ImageProcessor() + ~ImageProcessor() override { _range_image_pub.reset(); _ambient_image_pub.reset(); _intensity_image_pub.reset(); } - void generate_images(const std::chrono::nanoseconds timestamp, const uint64_t override_ts) + void generate_images(const std::chrono::nanoseconds timestamp, const uint64_t override_ts, int return_index) { + const bool second = (return_index == 1); + + // across supported lidar profiles range is always 32-bit + auto range_channel_field = + second ? ouster::sensor::ChanField::RANGE2 : ouster::sensor::ChanField::RANGE; + ouster::img_t range = _ls.field(range_channel_field); + + ouster::img_t reflectivity = ros2_ouster::util::get_or_fill_zero( + ros2_ouster::util::suitable_return(ouster::sensor::ChanField::REFLECTIVITY, second), _ls); + + ouster::img_t signal = ros2_ouster::util::get_or_fill_zero( + ros2_ouster::util::suitable_return(ouster::sensor::ChanField::SIGNAL, second), _ls); + + ouster::img_t near_ir = ros2_ouster::util::get_or_fill_zero( + ros2_ouster::util::suitable_return(ouster::sensor::ChanField::NEAR_IR, second), _ls); + _range_image.width = _width; _range_image.height = _height; _range_image.step = _width; @@ -131,7 +148,7 @@ class ImageProcessor : public ros2_ouster::DataProcessorInterface const size_t vv = (v + _width - _px_offset[u]) % _width; const size_t index = u * _width + vv; - if (_ls.field(ouster::LidarScan::RANGE)(index) == 0) { + if (range.data()[index] == 0) { reinterpret_cast( _range_image.data.data())[u * _width + v] = 0; } else { @@ -139,11 +156,11 @@ class ImageProcessor : public ros2_ouster::DataProcessorInterface _range_image.data.data())[u * _width + v] = _pixel_value_max - std::min( - std::round(_ls.field(ouster::LidarScan::RANGE)(index) * _range_multiplier), + std::round(range.data()[index] * _range_multiplier), static_cast(_pixel_value_max)); } - ambient_image_eigen(u, v) = _ls.field(ouster::LidarScan::AMBIENT)(index); - intensity_image_eigen(u, v) = _ls.field(ouster::LidarScan::INTENSITY)(index); + ambient_image_eigen(u, v) = near_ir.data()[index]; + intensity_image_eigen(u, v) = signal.data()[index]; } } @@ -180,7 +197,8 @@ class ImageProcessor : public ros2_ouster::DataProcessorInterface } _ls = *_fullRotationAccumulator->getLidarScan(); - generate_images(_fullRotationAccumulator->getTimestamp(), override_ts); + //TODO(dual-packet-format): figure out if it makes sense to publish dual images + generate_images(_fullRotationAccumulator->getTimestamp(), override_ts, 0); return true; } diff --git a/ros2_ouster/include/ros2_ouster/processors/imu_processor.hpp b/ros2_ouster/include/ros2_ouster/processors/imu_processor.hpp index add3590..f8ca984 100644 --- a/ros2_ouster/include/ros2_ouster/processors/imu_processor.hpp +++ b/ros2_ouster/include/ros2_ouster/processors/imu_processor.hpp @@ -14,6 +14,7 @@ #ifndef ROS2_OUSTER__PROCESSORS__IMU_PROCESSOR_HPP_ #define ROS2_OUSTER__PROCESSORS__IMU_PROCESSOR_HPP_ +#include #include #include #include @@ -47,10 +48,10 @@ class IMUProcessor : public ros2_ouster::DataProcessorInterface IMUProcessor( const rclcpp_lifecycle::LifecycleNode::SharedPtr node, const ouster::sensor::sensor_info & mdata, - const std::string & frame, + std::string frame, const rclcpp::QoS & qos, - const ouster::sensor::packet_format & pf) - : DataProcessorInterface(), _node(node), _frame(frame), _pf(pf) + ouster::sensor::packet_format pf) + : DataProcessorInterface(), _node(node), _frame(std::move(frame)), _pf(std::move(pf)) { _pub = node->create_publisher("imu", qos); } @@ -58,7 +59,7 @@ class IMUProcessor : public ros2_ouster::DataProcessorInterface /** * @brief A destructor clearing memory allocated */ - ~IMUProcessor() + ~IMUProcessor() override { _pub.reset(); } diff --git a/ros2_ouster/include/ros2_ouster/processors/pointcloud_processor.hpp b/ros2_ouster/include/ros2_ouster/processors/pointcloud_processor.hpp index a76556f..f565cee 100644 --- a/ros2_ouster/include/ros2_ouster/processors/pointcloud_processor.hpp +++ b/ros2_ouster/include/ros2_ouster/processors/pointcloud_processor.hpp @@ -53,16 +53,16 @@ class PointcloudProcessor : public ros2_ouster::DataProcessorInterface PointcloudProcessor( const rclcpp_lifecycle::LifecycleNode::SharedPtr node, const ouster::sensor::sensor_info & mdata, - const std::string & frame, + std::string frame, const rclcpp::QoS & qos, const ouster::sensor::packet_format & pf, std::shared_ptr fullRotationAccumulator) - : DataProcessorInterface(), _node(node), _frame(frame) + : DataProcessorInterface(), _node(node), _frame(std::move(frame)) { _node->declare_parameter("pointcloud_filter_zero_points", false); _node->get_parameter("pointcloud_filter_zero_points", _filter_zero_points); - _fullRotationAccumulator = fullRotationAccumulator; + _fullRotationAccumulator = std::move(fullRotationAccumulator); _height = mdata.format.pixels_per_column; _width = mdata.format.columns_per_frame; _xyz_lut = ouster::make_xyz_lut(mdata); @@ -77,7 +77,7 @@ class PointcloudProcessor : public ros2_ouster::DataProcessorInterface /** * @brief A destructor clearing memory allocated */ - ~PointcloudProcessor() + ~PointcloudProcessor() override { _pub.reset(); } @@ -92,9 +92,10 @@ class PointcloudProcessor : public ros2_ouster::DataProcessorInterface return true; } + //TODO(dual-packet-format): add support for publishing from the dual packet mode ros2_ouster::toCloud( - _xyz_lut, _fullRotationAccumulator->getTimestamp(), - *_fullRotationAccumulator->getLidarScan(), *_cloud); + _xyz_lut, _fullRotationAccumulator->getTimestamp().count(), + *_fullRotationAccumulator->getLidarScan(), *_cloud, 0); if (_filter_zero_points) { _cloud_filtered->points.clear(); @@ -143,7 +144,7 @@ class PointcloudProcessor : public ros2_ouster::DataProcessorInterface private: std::unique_ptr _cloud; std::unique_ptr _cloud_filtered; - bool _filter_zero_points; + bool _filter_zero_points{}; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _pub; rclcpp_lifecycle::LifecycleNode::SharedPtr _node; ouster::XYZLut _xyz_lut; diff --git a/ros2_ouster/include/ros2_ouster/processors/processor_factories.hpp b/ros2_ouster/include/ros2_ouster/processors/processor_factories.hpp index 7c55801..08b01d8 100644 --- a/ros2_ouster/include/ros2_ouster/processors/processor_factories.hpp +++ b/ros2_ouster/include/ros2_ouster/processors/processor_factories.hpp @@ -86,7 +86,7 @@ inline std::unique_ptr createImageProcessor const std::string & frame, const rclcpp::QoS & qos, const ouster::sensor::packet_format & pf, - std::shared_ptr fullRotationAccumulator) + const std::shared_ptr& fullRotationAccumulator) { return std::make_unique( node, mdata, frame, qos, pf, @@ -105,7 +105,7 @@ inline std::unique_ptr createPointcloudProc const std::string & frame, const rclcpp::QoS & qos, const ouster::sensor::packet_format & pf, - std::shared_ptr fullRotationAccumulator) + const std::shared_ptr& fullRotationAccumulator) { return std::make_unique( node, mdata, frame, qos, pf, fullRotationAccumulator); @@ -137,7 +137,7 @@ inline std::unique_ptr createScanProcessor( const std::string & frame, const rclcpp::QoS & qos, const ouster::sensor::packet_format & pf, - std::shared_ptr fullRotationAccumulator) + const std::shared_ptr& fullRotationAccumulator) { return std::make_unique( node, mdata, frame, qos, pf, @@ -152,7 +152,7 @@ inline std::multimap fullRotationAccumulator, + const std::shared_ptr& fullRotationAccumulator, std::uint32_t mask = ros2_ouster::DEFAULT_PROC_MASK) { std::multimap fullRotationAccumulator) - : DataProcessorInterface(), _node(node), _frame(frame), _pf(pf) + : DataProcessorInterface(), _node(node), _frame(std::move(frame)), _pf(std::move(pf)) { - _fullRotationAccumulator = fullRotationAccumulator; + _fullRotationAccumulator = std::move(fullRotationAccumulator); _mdata = mdata; _pub = _node->create_publisher("scan", qos); @@ -71,7 +71,7 @@ class ScanProcessor : public ros2_ouster::DataProcessorInterface /** * @brief A destructor clearing memory allocated */ - ~ScanProcessor() + ~ScanProcessor() override { _pub.reset(); } @@ -86,11 +86,12 @@ class ScanProcessor : public ros2_ouster::DataProcessorInterface return true; } + //TODO(dual-packet-format): add support for publishing from the dual packet mode _pub->publish( ros2_ouster::toMsg( *_fullRotationAccumulator->getLidarScan(), _fullRotationAccumulator->getTimestamp(), - _frame, _mdata, _ring, override_ts)); + _frame, _mdata, _ring, override_ts, 0)); return true; } diff --git a/ros2_ouster/include/ros2_ouster/sensor.hpp b/ros2_ouster/include/ros2_ouster/sensor.hpp index 94c1c35..adefe72 100644 --- a/ros2_ouster/include/ros2_ouster/sensor.hpp +++ b/ros2_ouster/include/ros2_ouster/sensor.hpp @@ -96,9 +96,52 @@ class Sensor : public ros2_ouster::SensorInterface */ ouster::sensor::packet_format getPacketFormat() override; + /** + * @brief Indicate whether a reactivation operation is required + * @return sensor metadata struct + */ + bool shouldReset(const ouster::sensor::client_state & state, const uint8_t * packet) override; + + void reset_sensor(bool force_reinit, bool init_id_reset = false); + void reactivate_sensor(bool init_id_reset = false); + private: + [[nodiscard]] std::shared_ptr + configure_and_initialize_sensor(const ros2_ouster::Configuration & config); + + uint8_t compose_config_flags(const ouster::sensor::sensor_config & config); + + inline bool init_id_changed(const ouster::sensor::packet_format & pf, + const uint8_t * lidar_buf) + { + uint32_t current_init_id = pf.init_id(lidar_buf); + if (!last_init_id_initialized) { + last_init_id = current_init_id + 1; + last_init_id_initialized = true; + } + if (reset_last_init_id && last_init_id != current_init_id) { + last_init_id = current_init_id; + reset_last_init_id = false; + return false; + } + if (last_init_id == current_init_id) return false; + last_init_id = current_init_id; + return true; + } + + inline static bool is_non_legacy_lidar_profile(const ouster::sensor::sensor_info & info) + { + return info.format.udp_profile_lidar != + ouster::sensor::UDPProfileLidar::PROFILE_LIDAR_LEGACY; + } + std::shared_ptr _ouster_client; ros2_ouster::Metadata _metadata{}; + + bool force_sensor_reinit = false; + bool reset_last_init_id = true; + bool last_init_id_initialized = false; + uint32_t last_init_id{}; }; } // namespace sensor diff --git a/ros2_ouster/include/ros2_ouster/sensor_tins.hpp b/ros2_ouster/include/ros2_ouster/sensor_tins.hpp index f5fb6d7..4a0e452 100644 --- a/ros2_ouster/include/ros2_ouster/sensor_tins.hpp +++ b/ros2_ouster/include/ros2_ouster/sensor_tins.hpp @@ -111,6 +111,12 @@ class SensorTins : public ros2_ouster::SensorInterface */ ouster::sensor::packet_format getPacketFormat() override; + /** + * @brief Indicate whether a reactivation operation is required + * @return sensor metadata struct + */ + bool shouldReset(const ouster::sensor::client_state & state, const uint8_t * packet) override; + /** * @brief Load metadata from a file. * @details Some important notes about this function: This populates an diff --git a/ros2_ouster/params/driver_config.yaml b/ros2_ouster/params/driver_config.yaml index cf9345c..0714095 100644 --- a/ros2_ouster/params/driver_config.yaml +++ b/ros2_ouster/params/driver_config.yaml @@ -1,8 +1,8 @@ ouster_driver: ros__parameters: - lidar_ip: 10.5.5.96 - computer_ip: 10.5.5.1 - lidar_mode: "1024x10" + lidar_ip: 10.0.21.219 + computer_ip: 10.0.21.220 + lidar_mode: "4096x5" imu_port: 7503 lidar_port: 7502 sensor_frame: laser_sensor_frame @@ -14,6 +14,19 @@ ouster_driver: # See: https://github.com/ros2/rosbag2/issues/125 use_system_default_qos: False + # Set the lidar packet data format. + # Valid modes are: + # + # LEGACY + # RNG19_RFL8_SIG16_NIR16 + # RNG19_RFL8_SIG16_NIR16_DUAL + # RNG15_RFL8_NIR8 + # + # (See this project's README and/or the Ouster Software Guide for more + # information). + # + lidar_udp_profile: RNG19_RFL8_SIG16_NIR16 + # Set the method used to timestamp measurements. # Valid modes are: # @@ -25,7 +38,7 @@ ouster_driver: # (See this project's README and/or the Ouster Software Guide for more # information). # - timestamp_mode: TIME_FROM_INTERNAL_OSC + timestamp_mode: TIME_FROM_PTP_1588 # Mask-like-string used to define the data processors that should be # activated upon startup of the driver. This will determine the topics diff --git a/ros2_ouster/src/client/client.cpp b/ros2_ouster/src/client/client.cpp index fe93f02..b62e915 100644 --- a/ros2_ouster/src/client/client.cpp +++ b/ros2_ouster/src/client/client.cpp @@ -3,35 +3,29 @@ #include #include -#include #include -#include #include -#include #include -#include -#include #include -#include #include -#include #include //#include "ouster/build.h" #include "ros2_ouster/client/impl/netcompat.h" -#include "ros2_ouster/client/types.h" +#include "ros2_ouster/client/client_factory.hpp" namespace ouster { namespace sensor { +using namespace std::chrono_literals; namespace chrono = std::chrono; struct client { - SOCKET lidar_fd; - SOCKET imu_fd; + SOCKET lidar_fd{}; + SOCKET imu_fd{}; std::string hostname; Json::Value meta; ~client() @@ -49,7 +43,7 @@ const int RCVBUF_SIZE = 256 * 1024; int32_t get_sock_port(SOCKET sock_fd) { - struct sockaddr_storage ss; + struct sockaddr_storage ss{}; socklen_t addrlen = sizeof ss; if (!impl::socket_valid( @@ -69,9 +63,10 @@ int32_t get_sock_port(SOCKET sock_fd) } } +//TODO(udp-socket): port from ouster sdk? SOCKET udp_data_socket(int port) { - struct addrinfo hints, * info_start, * ai; + struct addrinfo hints{}, * info_start, * ai; memset(&hints, 0, sizeof hints); hints.ai_family = AF_INET6; @@ -80,18 +75,18 @@ SOCKET udp_data_socket(int port) auto port_s = std::to_string(port); - int ret = getaddrinfo(NULL, port_s.c_str(), &hints, &info_start); + int ret = getaddrinfo(nullptr, port_s.c_str(), &hints, &info_start); if (ret != 0) { std::cerr << "getaddrinfo(): " << gai_strerror(ret) << std::endl; return SOCKET_ERROR; } - if (info_start == NULL) { + if (info_start == nullptr) { std::cerr << "getaddrinfo: empty result" << std::endl; return SOCKET_ERROR; } SOCKET sock_fd; - for (ai = info_start; ai != NULL; ai = ai->ai_next) { + for (ai = info_start; ai != nullptr; ai = ai->ai_next) { sock_fd = socket(ai->ai_family, ai->ai_socktype, ai->ai_protocol); if (!impl::socket_valid(sock_fd)) { std::cerr << "udp socket(): " << impl::socket_get_error() << @@ -121,7 +116,7 @@ SOCKET udp_data_socket(int port) } freeaddrinfo(info_start); - if (ai == NULL) { + if (ai == nullptr) { impl::socket_close(sock_fd); return SOCKET_ERROR; } @@ -147,389 +142,188 @@ SOCKET udp_data_socket(int port) return sock_fd; } -SOCKET cfg_socket(const char * addr) -{ - struct addrinfo hints, * info_start, * ai; - - memset(&hints, 0, sizeof hints); - hints.ai_family = AF_UNSPEC; - hints.ai_socktype = SOCK_STREAM; +Json::Value collect_metadata(const std::string& hostname, int timeout_sec) { + auto net_client = util::ClientInterface::create(hostname); + auto timeout_time = + chrono::steady_clock::now() + chrono::seconds{timeout_sec}; + std::string status; - int ret = getaddrinfo(addr, "7501", &hints, &info_start); - if (ret != 0) { - std::cerr << "getaddrinfo: " << gai_strerror(ret) << std::endl; - return SOCKET_ERROR; - } - if (info_start == NULL) { - std::cerr << "getaddrinfo: empty result" << std::endl; - return SOCKET_ERROR; - } - - SOCKET sock_fd; - for (ai = info_start; ai != NULL; ai = ai->ai_next) { - sock_fd = socket(ai->ai_family, ai->ai_socktype, ai->ai_protocol); - if (!impl::socket_valid(sock_fd)) { - std::cerr << "socket: " << impl::socket_get_error() << std::endl; - continue; - } - - if (connect(sock_fd, ai->ai_addr, (socklen_t)ai->ai_addrlen) < 0) { - impl::socket_close(sock_fd); - continue; - } - - break; - } - - freeaddrinfo(info_start); - if (ai == NULL) { - return SOCKET_ERROR; - } - - return sock_fd; -} - -bool do_tcp_cmd( - SOCKET sock_fd, const std::vector & cmd_tokens, - std::string & res) -{ - const size_t max_res_len = 16 * 1024; - auto read_buf = std::unique_ptr{new char[max_res_len + 1]}; - - std::stringstream ss; - for (const auto & token : cmd_tokens) { - ss << token << " "; - } - ss << "\n"; - std::string cmd = ss.str(); - - ssize_t len = send(sock_fd, cmd.c_str(), cmd.length(), 0); - if (len != (ssize_t)cmd.length()) { - return false; - } - - // need to synchronize with server by reading response - std::stringstream read_ss; do { - len = recv(sock_fd, read_buf.get(), max_res_len, 0); - if (len < 0) { - return false; - } - read_buf.get()[len] = '\0'; - read_ss << read_buf.get(); - } while (len > 0 && read_buf.get()[len - 1] != '\n'); - - res = read_ss.str(); - res.erase(res.find_last_not_of(" \r\n\t") + 1); - - return true; -} - -void update_json_obj(Json::Value & dst, const Json::Value & src) -{ - const std::vector & members = src.getMemberNames(); - for (const auto & key : members) { - dst[key] = src[key]; + if (chrono::steady_clock::now() >= timeout_time) return false; + std::this_thread::sleep_for(1s); + status = net_client->sensor_info()["status"].asString(); + } while (status == "INITIALIZING"); + + // not all metadata available when sensor isn't RUNNING + if (status != "RUNNING") { + throw std::runtime_error( + "Cannot obtain full metadata with sensor status: " + status + + ". Please ensure that sensor is not in a STANDBY, UNCONFIGURED, " + "WARMUP, or ERROR state"); } -} - -bool collect_metadata(client & cli, SOCKET sock_fd, chrono::seconds timeout) -{ - Json::CharReaderBuilder builder{}; - auto reader = std::unique_ptr{builder.newCharReader()}; - Json::Value root{}; - std::string errors{}; - - std::string res; - bool success = true; - - auto timeout_time = chrono::steady_clock::now() + timeout; - - do { - success &= do_tcp_cmd(sock_fd, {"get_sensor_info"}, res); - success &= - reader->parse(res.c_str(), res.c_str() + res.size(), &root, NULL); - - if (chrono::steady_clock::now() >= timeout_time) {return false;} - std::this_thread::sleep_for(chrono::seconds(1)); - } while (success && root["status"].asString() == "INITIALIZING"); - - update_json_obj(cli.meta, root); - - success &= do_tcp_cmd(sock_fd, {"get_beam_intrinsics"}, res); - success &= - reader->parse(res.c_str(), res.c_str() + res.size(), &root, NULL); - update_json_obj(cli.meta, root); - - success &= do_tcp_cmd(sock_fd, {"get_imu_intrinsics"}, res); - success &= - reader->parse(res.c_str(), res.c_str() + res.size(), &root, NULL); - update_json_obj(cli.meta, root); - - success &= do_tcp_cmd(sock_fd, {"get_lidar_intrinsics"}, res); - success &= - reader->parse(res.c_str(), res.c_str() + res.size(), &root, NULL); - update_json_obj(cli.meta, root); - - // try to query data format - bool got_format = true; - got_format &= do_tcp_cmd(sock_fd, {"get_lidar_data_format"}, res); - got_format &= - reader->parse(res.c_str(), res.c_str() + res.size(), &root, NULL); - if (got_format) {cli.meta["data_format"] = root;} - - // get lidar mode - success &= do_tcp_cmd(sock_fd, {"get_config_param", "active"}, res); - success &= - reader->parse(res.c_str(), res.c_str() + res.size(), &root, NULL); + auto metadata = net_client->metadata(); // merge extra info into metadata - cli.meta["hostname"] = cli.hostname; - cli.meta["lidar_mode"] = root["lidar_mode"]; - cli.meta["json_calibration_version"] = FW_2_0; - - return success; +// metadata["client_version"] = client_version(); + return metadata; } -} // namespace - -// conversion for operating_mode (introduced in fw 2.0) to auto_start -// (deprecated in 1.13) -const std::array, 2> auto_start_strings = - {{ - {OPERATING_NORMAL, "NORMAL"}, - {OPERATING_STANDBY, "STANDBY"} - }}; - -static std::string auto_start_string(OperatingMode mode) -{ - auto end = auto_start_strings.end(); - auto res = std::find_if( - auto_start_strings.begin(), - end, - [&](const std::pair& p) {return p.first == mode;} - ); - - return res == end ? "UNKNOWN" : res->second; -} - -bool set_config_helper( - SOCKET sock_fd, - const sensor_config& config, - uint8_t config_flags) -{ - std::string res; - bool success = true; - - auto set_param = [&sock_fd, &res](std::string param_name, - std::string param_value) { - bool success = true; - success &= do_tcp_cmd( - sock_fd, {"set_config_param", param_name, param_value}, res); - success &= res == "set_config_param"; - return success; - }; - - // set params - if (config.udp_dest && !set_param("udp_dest", config.udp_dest.value())) - return false; - - if (config.udp_port_lidar && - !set_param("udp_port_lidar", - std::to_string(config.udp_port_lidar.value()))) - return false; - - if (config.udp_port_imu && - !set_param("udp_port_imu", std::to_string(config.udp_port_imu.value()))) - return false; - - if (config.ts_mode && - !set_param("timestamp_mode", to_string(config.ts_mode.value()))) - return false; - - if (config.ld_mode && - !set_param("lidar_mode", to_string(config.ld_mode.value()))) - return false; - - // "operating_mode" introduced in fw 2.0. use deprecated 'auto_start_flag' - // to support 1.13 - if (config.operating_mode && - !set_param("operating_mode", - to_string(config.operating_mode.value()))) - return false; - - if (config.multipurpose_io_mode && - !set_param("multipurpose_io_mode", - to_string(config.multipurpose_io_mode.value()))) - return false; - - if (config.azimuth_window && - !set_param("azimuth_window", to_string(config.azimuth_window.value()))) - return false; - - if (config.signal_multiplier && - !set_param("signal_multiplier", - std::to_string(config.signal_multiplier.value()))) - return false; - - if (config.sync_pulse_out_angle && - !set_param("sync_pulse_out_angle", - std::to_string(config.sync_pulse_out_angle.value()))) - return false; - - if (config.sync_pulse_out_pulse_width && - !set_param("sync_pulse_out_pulse_width", - std::to_string(config.sync_pulse_out_pulse_width.value()))) - return false; - - if (config.nmea_in_polarity && - !set_param("nmea_in_polarity", - to_string(config.nmea_in_polarity.value()))) - return false; - - if (config.nmea_baud_rate && - !set_param("nmea_baud_rate", to_string(config.nmea_baud_rate.value()))) - return false; - - if (config.nmea_ignore_valid_char) { - const std::string nmea_ignore_valid_char_string = - config.nmea_ignore_valid_char.value() ? "1" : "0"; - if (!set_param("nmea_ignore_valid_char", nmea_ignore_valid_char_string)) - return false; - } - - if (config.nmea_leap_seconds && - !set_param("nmea_leap_seconds", - std::to_string(config.nmea_leap_seconds.value()))) - return false; - - if (config.sync_pulse_in_polarity && - !set_param("sync_pulse_in_polarity", - to_string(config.sync_pulse_in_polarity.value()))) - return false; - - if (config.sync_pulse_out_polarity && - !set_param("sync_pulse_out_polarity", - to_string(config.sync_pulse_in_polarity.value()))) - return false; - - if (config.sync_pulse_out_frequency && - !set_param("sync_pulse_out_frequency", - std::to_string(config.sync_pulse_out_frequency.value()))) - return false; - - if (config.phase_lock_enable) { - const std::string phase_lock_enable_string = - config.phase_lock_enable.value() ? "true" : "false"; - if (!set_param("phase_lock_enable", phase_lock_enable_string)) - return false; - } - - if (config.phase_lock_offset && - !set_param("phase_lock_offset", - std::to_string(config.phase_lock_offset.value()))) - return false; - // reinitialize - success &= do_tcp_cmd(sock_fd, {"reinitialize"}, res); - success &= res == "reinitialize"; - - // save if indicated - if (config_flags & CONFIG_PERSIST) { - // use deprecated write_config_txt to support 1.13 - success &= do_tcp_cmd(sock_fd, {"write_config_txt"}, res); - success &= res == "write_config_txt"; - } - - return success; -} +} // namespace bool get_config( - const std::string& hostname, + const std::string& hostname, sensor_config& config, - bool active) + bool active) { - Json::CharReaderBuilder builder{}; - auto reader = std::unique_ptr{builder.newCharReader()}; - Json::Value root{}; - std::string errors{}; - - SOCKET sock_fd = cfg_socket(hostname.c_str()); - if (sock_fd < 0) return false; - - std::string res; - bool success = true; - - std::string active_or_staged = active ? "active" : "staged"; - success &= do_tcp_cmd(sock_fd, {"get_config_param", active_or_staged}, res); - success &= - reader->parse(res.c_str(), res.c_str() + res.size(), &root, NULL); - + auto net_client = util::ClientInterface::create(hostname); + auto res = net_client->get_config_params(active); config = parse_config(res); - - impl::socket_close(sock_fd); - - return success; + return true; } bool set_config( - const std::string& hostname, + const std::string& hostname, const sensor_config& config, - uint8_t config_flags) + uint8_t config_flags) { - // open socket - SOCKET sock_fd = cfg_socket(hostname.c_str()); - if (sock_fd < 0) return false; + auto net_client = util::ClientInterface::create(hostname); - std::string res; - bool success = true; + // reset staged config to avoid spurious errors + auto config_params = net_client->active_config_params(); + Json::Value config_params_copy = config_params; - if (config_flags & CONFIG_UDP_DEST_AUTO) - { - if (config.udp_dest) - { - impl::socket_close(sock_fd); - throw std::invalid_argument( - "UDP_DEST_AUTO flag set but provided config has udp_dest"); - } - success &= do_tcp_cmd(sock_fd, {"set_udp_dest_auto"}, res); - success &= res == "set_udp_dest_auto"; + // set all desired config parameters + Json::Value config_json = to_json(config); + for (const auto &key: config_json.getMemberNames()) { + config_params[key] = config_json[key]; } - if (success) - { - success = set_config_helper(sock_fd, config, config_flags); + if (config_json.isMember("operating_mode") && + config_params.isMember("auto_start_flag")) { + // we're setting operating mode and this sensor has a FW with + // auto_start_flag + config_params["auto_start_flag"] = + config_json["operating_mode"] == "NORMAL" ? 1 : 0; } - impl::socket_close(sock_fd); + // Signal multiplier changed from int to double for FW 3.0/2.5+, with + // corresponding change to config.signal_multiplier. + // Change values 1, 2, 3 back to ints to support older FWs + if (config_json.isMember("signal_multiplier")) { + check_signal_multiplier(config_params["signal_multiplier"].asDouble()); + if (config_params["signal_multiplier"].asDouble() != 0.25 && + config_params["signal_multiplier"].asDouble() != 0.5) { + config_params["signal_multiplier"] = + config_params["signal_multiplier"].asInt(); + } + } - return success; -} + // set automatic udp dest, if flag specified + if (config_flags & CONFIG_UDP_DEST_AUTO) { + if (config.udp_dest) + throw std::invalid_argument( + "UDP_DEST_AUTO flag set but provided config has udp_dest"); + net_client->set_udp_dest_auto(); + + auto staged = net_client->staged_config_params(); + + // now we set config_params according to the staged udp_dest from the + // sensor + if (staged.isMember("udp_ip")) {// means the FW version carries udp_ip + config_params["udp_ip"] = staged["udp_ip"]; + config_params["udp_dest"] = staged["udp_ip"]; + } + else {// don't need to worry about udp_ip + config_params["udp_dest"] = staged["udp_dest"]; + } + } -std::string get_metadata(client & cli, int timeout_sec) -{ - if (!cli.meta) { - SOCKET sock_fd = cfg_socket(cli.hostname.c_str()); - if (sock_fd < 0) {return "";} + // if configuration didn't change then skip applying the params + // note: comparison will fail if config_params contains newer config params + // introduced after the verison of FW the sensor is on + if (config_flags & CONFIG_FORCE_REINIT || + config_params_copy != config_params) { + Json::StreamWriterBuilder builder; + builder["indentation"] = ""; + // send full string -- depends on older FWs not rejecting a blob even + // when it contains unknown keys + auto config_params_str = Json::writeString(builder, config_params); + net_client->set_config_param(".", config_params_str); + // reinitialize to make all staged parameters effective + net_client->reinitialize(); + } - bool success = - collect_metadata(cli, sock_fd, chrono::seconds{timeout_sec}); + // save if indicated + if (config_flags & CONFIG_PERSIST) { net_client->save_config_params(); } - impl::socket_close(sock_fd); + return true; +} - if (!success) {return "";} +std::string get_metadata(client& cli, int timeout_sec, bool legacy_format) { + try { + cli.meta = collect_metadata(cli.hostname, timeout_sec); + } catch (const std::exception& e) { +// logger().warn(std::string("Unable to retrieve sensor metadata: ") + e.what()); + std::cerr << "Unable to retrieve sensor metadata: " << e.what() << std::endl; + throw; } Json::StreamWriterBuilder builder; builder["enableYAMLCompatibility"] = true; builder["precision"] = 6; builder["indentation"] = " "; - return Json::writeString(builder, cli.meta); + auto metadata_string = Json::writeString(builder, cli.meta); + if (legacy_format) { +// logger().warn( +// "The SDK will soon output the non-legacy metadata format by " +// "default. If you parse the metadata directly instead of using the " +// "SDK (which will continue to read both legacy and non-legacy " +// "formats), please be advised that on the next release you will " +// "either have to update your parsing or specify legacy_format = " +// "true to the get_metadata function."); + std::cerr << "The SDK will soon output the non-legacy metadata format by " + "default. If you parse the metadata directly instead of using the " + "SDK (which will continue to read both legacy and non-legacy " + "formats), please be advised that on the next release you will " + "either have to update your parsing or specify legacy_format = " + "true to the get_metadata function." << std::endl; + } + + // We can't insert this logic into the light init_client since its advantage + // is that it doesn't make netowrk calls but we need it to run every time + // there is a valid connection to the sensor So we insert it here + // TODO: remove after release of FW 3.2/3.3 (sufficient warning) + sensor_config config; + get_config(cli.hostname, config); + auto fw_version = util::ClientInterface::firmware_version(cli.hostname); + // only warn for people on the latest FW, as people on older FWs may not + // care + if (fw_version.major >= 3 && + config.udp_profile_lidar == UDPProfileLidar::PROFILE_LIDAR_LEGACY) { +// logger().warn( +// "Please note that the Legacy Lidar Profile will be deprecated " +// "in the sensor FW soon. If you plan to upgrade your FW, we " +// "recommend using the Single Return Profile instead. For users " +// "sticking with older FWs, the Ouster SDK will continue to parse " +// "the legacy lidar profile."); + std::cerr << "Please note that the Legacy Lidar Profile will be deprecated " + "in the sensor FW soon. If you plan to upgrade your FW, we " + "recommend using the Single Return Profile instead. For users " + "sticking with older FWs, the Ouster SDK will continue to parse " + "the legacy lidar profile." << std::endl; + } + return legacy_format ? convert_to_legacy(metadata_string) : metadata_string; } std::shared_ptr init_client( const std::string & hostname, int lidar_port, int imu_port) { +// logger().info("initializing sensor: {} with lidar port/imu port: {}/{}", +// hostname, lidar_port, imu_port); + std::cout << "initializing sensor: " << hostname << " with lidar port/imu port: " + << lidar_port << "/" << imu_port << std::endl; + auto cli = std::make_shared(); cli->hostname = hostname; @@ -537,7 +331,7 @@ std::shared_ptr init_client( cli->imu_fd = udp_data_socket(imu_port); if (!impl::socket_valid(cli->lidar_fd) || !impl::socket_valid(cli->imu_fd)) { - return std::shared_ptr(); + return {}; } return cli; @@ -546,84 +340,48 @@ std::shared_ptr init_client( std::shared_ptr init_client( const std::string & hostname, const std::string & udp_dest_host, - lidar_mode mode, timestamp_mode ts_mode, + lidar_mode ld_mode, timestamp_mode ts_mode, int lidar_port, int imu_port, int timeout_sec) { auto cli = init_client(hostname, lidar_port, imu_port); - if (!cli) {return std::shared_ptr();} + if (!cli) {return {};} // update requested ports to actual bound ports lidar_port = get_sock_port(cli->lidar_fd); imu_port = get_sock_port(cli->imu_fd); if (!impl::socket_valid(lidar_port) || !impl::socket_valid(imu_port)) { - return std::shared_ptr(); - } - - SOCKET sock_fd = cfg_socket(hostname.c_str()); - if (!impl::socket_valid(sock_fd)) {return std::shared_ptr();} - - std::string res; - bool success = true; - - // If udp_dest_host is empty string, use automatic addressing with set_udp_dest_auto - if (udp_dest_host != "") - { - success &= - do_tcp_cmd(sock_fd, {"set_config_param", "udp_dest", udp_dest_host}, res); - success &= res == "set_config_param"; - } - else - { - success &= - do_tcp_cmd(sock_fd, {"set_udp_dest_auto"}, res); - success &= res == "set_udp_dest_auto"; - } - - success &= do_tcp_cmd( - sock_fd, - {"set_config_param", "udp_port_lidar", std::to_string(lidar_port)}, - res); - success &= res == "set_config_param"; - - success &= do_tcp_cmd( - sock_fd, {"set_config_param", "udp_port_imu", std::to_string(imu_port)}, - res); - success &= res == "set_config_param"; - - // if specified (not UNSPEC), set the lidar and timestamp modes - if (mode) { - success &= do_tcp_cmd( - sock_fd, {"set_config_param", "lidar_mode", to_string(mode)}, res); - success &= res == "set_config_param"; + return {}; } - if (ts_mode) { - success &= do_tcp_cmd( - sock_fd, {"set_config_param", "timestamp_mode", to_string(ts_mode)}, - res); - success &= res == "set_config_param"; + try { + sensor::sensor_config config; + uint8_t config_flags = 0; + if (udp_dest_host.empty()) + config_flags |= CONFIG_UDP_DEST_AUTO; + else + config.udp_dest = udp_dest_host; + if (ld_mode) config.ld_mode = ld_mode; + if (ts_mode) config.ts_mode = ts_mode; + if (lidar_port) config.udp_port_lidar = lidar_port; + if (imu_port) config.udp_port_imu = imu_port; + config.operating_mode = OPERATING_NORMAL; + set_config(hostname, config, config_flags); + + // will block until no longer INITIALIZING + cli->meta = collect_metadata(hostname, timeout_sec); + // check for sensor error states + auto status = cli->meta["sensor_info"]["status"].asString(); + if (status == "ERROR" || status == "UNCONFIGURED") + return {}; + } catch (const std::runtime_error& e) { + // log error message +// logger().error("init_client(): {}", e.what()); + std::cerr << "init_client(): " << e.what() << std::endl; + return {}; } - // wake up from STANDBY, if necessary - success &= do_tcp_cmd( - sock_fd, {"set_config_param", "operating_mode", "NORMAL"}, res); - success &= res == "set_config_param"; - - // reinitialize to activate new settings - success &= do_tcp_cmd(sock_fd, {"reinitialize"}, res); - success &= res == "reinitialize"; - - // will block until no longer INITIALIZING - success &= collect_metadata(*cli, sock_fd, chrono::seconds{timeout_sec}); - - // check for sensor error states - auto status = cli->meta["status"].asString(); - success &= (status != "ERROR" && status != "UNCONFIGURED"); - - impl::socket_close(sock_fd); - - return success ? cli : std::shared_ptr(); + return cli; } client_state poll_client(const client & c, const int timeout_sec) @@ -633,15 +391,15 @@ client_state poll_client(const client & c, const int timeout_sec) FD_SET(c.lidar_fd, &rfds); FD_SET(c.imu_fd, &rfds); - timeval tv; + timeval tv{}; tv.tv_sec = timeout_sec; tv.tv_usec = 0; SOCKET max_fd = std::max(c.lidar_fd, c.imu_fd); - SOCKET retval = select((int)max_fd + 1, &rfds, NULL, NULL, &tv); + SOCKET retval = select((int)max_fd + 1, &rfds, nullptr, nullptr, &tv); - client_state res = client_state(0); + auto res = client_state(0); if (!impl::socket_valid(retval) && impl::socket_exit()) { res = EXIT; @@ -658,13 +416,19 @@ client_state poll_client(const client & c, const int timeout_sec) static bool recv_fixed(SOCKET fd, void * buf, int64_t len) { - int64_t n = recv(fd, (char *)buf, len + 1, 0); - if (n == len) { + // Have to read longer than len because you need to know if the packet is + // too large + int64_t bytes_read = recv(fd, (char*)buf, len + 1, 0); + + if (bytes_read == len) { return true; - } else if (n == -1) { - std::cerr << "recvfrom: " << impl::socket_get_error() << std::endl; + } else if (bytes_read == -1) { +// logger().error("recvfrom: {}", impl::socket_get_error()); + std::cerr << "recvfrom: " << impl::socket_get_error() << std::endl; } else { - std::cerr << "Unexpected udp packet length: " << n << std::endl; +// logger().warn("Unexpected udp packet length: {}", bytes_read); + std::cerr << "Unexpected udp packet length: " << bytes_read << ", expected: " + << len << std::endl; } return false; } @@ -681,5 +445,27 @@ bool read_imu_packet(const client & cli, uint8_t * buf, const packet_format & pf return recv_fixed(cli.imu_fd, buf, pf.imu_packet_size); } +int get_lidar_port(client& cli) { return get_sock_port(cli.lidar_fd); } + +int get_imu_port(client& cli) { return get_sock_port(cli.imu_fd); } + +/** + * Return the socket file descriptor used to listen for lidar UDP data. + * + * @param[in] cli client returned by init_client associated with the connection. + * + * @return the socket file descriptor. + */ +extern SOCKET get_lidar_socket_fd(client& cli) { return cli.lidar_fd; } + +/** + * Return the socket file descriptor used to listen for imu UDP data. + * + * @param[in] cli client returned by init_client associated with the connection. + * + * @return the socket file descriptor. + */ +extern SOCKET get_imu_socket_fd(client& cli) { return cli.imu_fd; } + } // namespace sensor -} // namespace ouster +} // namespace ouster \ No newline at end of file diff --git a/ros2_ouster/src/client/lidar_scan.cpp b/ros2_ouster/src/client/lidar_scan.cpp index 0e58232..92a188d 100644 --- a/ros2_ouster/src/client/lidar_scan.cpp +++ b/ros2_ouster/src/client/lidar_scan.cpp @@ -3,41 +3,209 @@ #include #include #include +#include namespace ouster { -constexpr int LidarScan::N_FIELDS; +namespace impl { +template +using Table = std::array, N>; + +using namespace sensor; + +static const Table legacy_field_slots{ + {{ChanField::RANGE, ChanFieldType::UINT32}, + {ChanField::SIGNAL, ChanFieldType::UINT32}, + {ChanField::NEAR_IR, ChanFieldType::UINT32}, + {ChanField::REFLECTIVITY, ChanFieldType::UINT32}}}; + +static const Table dual_field_slots{{ + {ChanField::RANGE, ChanFieldType::UINT32}, + {ChanField::RANGE2, ChanFieldType::UINT32}, + {ChanField::SIGNAL, ChanFieldType::UINT16}, + {ChanField::SIGNAL2, ChanFieldType::UINT16}, + {ChanField::REFLECTIVITY, ChanFieldType::UINT8}, + {ChanField::REFLECTIVITY2, ChanFieldType::UINT8}, + {ChanField::NEAR_IR, ChanFieldType::UINT16}, +}}; + +static const Table single_field_slots{{ + {ChanField::RANGE, ChanFieldType::UINT32}, + {ChanField::SIGNAL, ChanFieldType::UINT16}, + {ChanField::REFLECTIVITY, ChanFieldType::UINT16}, + {ChanField::NEAR_IR, ChanFieldType::UINT16}, +}}; + +static const Table lb_field_slots{{ + {ChanField::RANGE, ChanFieldType::UINT32}, + {ChanField::REFLECTIVITY, ChanFieldType::UINT16}, + {ChanField::NEAR_IR, ChanFieldType::UINT16}, +}}; + +static const Table five_word_slots{{ + {ChanField::RAW32_WORD1, ChanFieldType::UINT32}, + {ChanField::RAW32_WORD2, ChanFieldType::UINT32}, + {ChanField::RAW32_WORD3, ChanFieldType::UINT32}, + {ChanField::RAW32_WORD4, ChanFieldType::UINT32}, + {ChanField::RAW32_WORD5, ChanFieldType::UINT32}, +}}; + +struct DefaultFieldsEntry { + const std::pair* fields; + size_t n_fields; +}; + +Table default_scan_fields{ + {{UDPProfileLidar::PROFILE_LIDAR_LEGACY, + {legacy_field_slots.data(), legacy_field_slots.size()}}, + {UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL, + {dual_field_slots.data(), dual_field_slots.size()}}, + {UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16, + {single_field_slots.data(), single_field_slots.size()}}, + {UDPProfileLidar::PROFILE_RNG15_RFL8_NIR8, + {lb_field_slots.data(), lb_field_slots.size()}}, + {UDPProfileLidar::PROFILE_FIVE_WORD_PIXEL, + {five_word_slots.data(), five_word_slots.size()}}}}; + +static std::vector> lookup_scan_fields( + UDPProfileLidar profile) { + auto end = impl::default_scan_fields.end(); + auto it = + std::find_if(impl::default_scan_fields.begin(), end, + [profile](const auto& kv) { return kv.first == profile; }); + + if (it == end || it->first == 0) + throw std::invalid_argument("Unknown lidar udp profile"); + + auto entry = it->second; + return {entry.fields, entry.fields + entry.n_fields}; +} + +} // namespace impl + +// explicitly instantiate for each supported field type +template Eigen::Ref> LidarScan::field(sensor::ChanField f); +template Eigen::Ref> LidarScan::field(sensor::ChanField f); +template Eigen::Ref> LidarScan::field(sensor::ChanField f); +template Eigen::Ref> LidarScan::field(sensor::ChanField f); +template Eigen::Ref> LidarScan::field(sensor::ChanField f) const; +template Eigen::Ref> LidarScan::field(sensor::ChanField f) const; +template Eigen::Ref> LidarScan::field(sensor::ChanField f) const; +template Eigen::Ref> LidarScan::field(sensor::ChanField f) const; + +std::string to_string(const LidarScanFieldTypes& field_types) { + std::stringstream ss; + ss << "("; + for (size_t i = 0; i < field_types.size(); ++i) { + if (i > 0) ss << ", "; + ss << sensor::to_string(field_types[i].first) << ":" + << sensor::to_string(field_types[i].second); + } + ss << ")"; + return ss.str(); +} + +LidarScanFieldTypes get_field_types(const sensor::sensor_info& info) { + // Get typical LidarScan to obtain field types + return impl::lookup_scan_fields(info.format.udp_profile_lidar); +} + +std::string to_string(const LidarScan& ls) { + std::stringstream ss; + LidarScanFieldTypes field_types(ls.begin(), ls.end()); + ss << "LidarScan: {h = " << ls.h << ", w = " << ls.w + << ", fid = " << ls.frame_id << "," << std::endl + << " frame status = " << std::hex << ls.frame_status << std::dec + << ", thermal_shutdown status = " << to_string(ls.thermal_shutdown()) + << ", shot_limiting status = " << to_string(ls.shot_limiting()) << "," + << std::endl + << " field_types = " << to_string(field_types) << "," << std::endl; + + if (!field_types.empty()) { + ss << " fields = [" << std::endl; + img_t key{ls.h, ls.w}; + for (const auto& ft : ls) { + impl::visit_field(ls, ft.first, impl::read_and_cast(), key); + ss << " " << to_string(ft.first) << ":" << to_string(ft.second) + << " = ("; + ss << key.minCoeff() << "; " << key.mean() << "; " + << key.maxCoeff(); + ss << ")," << std::endl; + } + ss << " ]," << std::endl; + } + + auto ts = ls.timestamp().cast(); + ss << " timestamp = (" << ts.minCoeff() << "; " << ts.mean() << "; " + << ts.maxCoeff() << ")," << std::endl; + auto mid = ls.measurement_id().cast(); + ss << " measurement_id = (" << mid.minCoeff() << "; " << mid.mean() << "; " + << mid.maxCoeff() << ")," << std::endl; + auto st = ls.status().cast(); + ss << " status = (" << st.minCoeff() << "; " << st.mean() << "; " + << st.maxCoeff() << ")" << std::endl; + + ss << "}"; + return ss.str(); +} XYZLut make_xyz_lut( size_t w, size_t h, double range_unit, - double lidar_origin_to_beam_origin_mm, + const mat4d& beam_to_lidar_transform, const mat4d & transform, const std::vector & azimuth_angles_deg, const std::vector & altitude_angles_deg) { if (w <= 0 || h <= 0) throw std::invalid_argument("lut dimensions must be greater than zero"); - if (azimuth_angles_deg.size() != h || altitude_angles_deg.size() != h) + + if ((azimuth_angles_deg.size() != h || altitude_angles_deg.size() != h) && + (azimuth_angles_deg.size() != w * h || + altitude_angles_deg.size() != w * h)) { throw std::invalid_argument("unexpected scan dimensions"); + } - Eigen::ArrayXd encoder(w * h); // theta_e - Eigen::ArrayXd azimuth(w * h); // theta_a - Eigen::ArrayXd altitude(w * h); // phi + double beam_to_lidar_euclidean_distance_mm = beam_to_lidar_transform(0, 3); + if (beam_to_lidar_transform(2, 3) != 0) { + beam_to_lidar_euclidean_distance_mm = + std::sqrt(std::pow(beam_to_lidar_transform(0, 3), 2) + + std::pow(beam_to_lidar_transform(2, 3), 2)); + } - const double azimuth_radians = M_PI * 2.0 / w; + XYZLut lut; + + Eigen::ArrayXd encoder(w * h); // theta_e + Eigen::ArrayXd azimuth(w * h); // theta_a + Eigen::ArrayXd altitude(w * h); // phi + + if (azimuth_angles_deg.size() == h && altitude_angles_deg.size() == h) { + // OS sensor + const double azimuth_radians = M_PI * 2.0 / w; - // populate angles for each pixel - for (size_t v = 0; v < w; v++) { - for (size_t u = 0; u < h; u++) { - size_t i = u * w + v; - encoder(i) = 2.0 * M_PI - (v * azimuth_radians); - azimuth(i) = -azimuth_angles_deg[u] * M_PI / 180.0; - altitude(i) = altitude_angles_deg[u] * M_PI / 180.0; + // populate angles for each pixel + for (size_t v = 0; v < w; v++) { + for (size_t u = 0; u < h; u++) { + size_t i = u * w + v; + encoder(i) = 2.0 * M_PI - (v * azimuth_radians); + azimuth(i) = -azimuth_angles_deg[u] * M_PI / 180.0; + altitude(i) = altitude_angles_deg[u] * M_PI / 180.0; + } } - } - XYZLut lut; + } else if (azimuth_angles_deg.size() == w * h && + altitude_angles_deg.size() == w * h) { + // DF sensor + // populate angles for each pixel + for (size_t v = 0; v < w; v++) { + for (size_t u = 0; u < h; u++) { + size_t i = u * w + v; + encoder(i) = 0; + azimuth(i) = azimuth_angles_deg[i] * M_PI / 180.0; + altitude(i) = altitude_angles_deg[i] * M_PI / 180.0; + } + } + } // unit vectors for each pixel lut.direction = LidarScan::Points{w * h, 3}; @@ -47,10 +215,15 @@ XYZLut make_xyz_lut( // offsets due to beam origin lut.offset = LidarScan::Points{w * h, 3}; - lut.offset.col(0) = encoder.cos() - lut.direction.col(0); - lut.offset.col(1) = encoder.sin() - lut.direction.col(1); - lut.offset.col(2) = -lut.direction.col(2); - lut.offset *= lidar_origin_to_beam_origin_mm; + lut.offset.col(0) = + encoder.cos() * beam_to_lidar_transform(0, 3) - + lut.direction.col(0) * beam_to_lidar_euclidean_distance_mm; + lut.offset.col(1) = + encoder.sin() * beam_to_lidar_transform(0, 3) - + lut.direction.col(1) * beam_to_lidar_euclidean_distance_mm; + lut.offset.col(2) = + -lut.direction.col(2) * beam_to_lidar_euclidean_distance_mm + + beam_to_lidar_transform(2, 3); // apply the supplied transform auto rot = transform.topLeftCorner(3, 3).transpose(); @@ -68,81 +241,276 @@ XYZLut make_xyz_lut( LidarScan::Points cartesian(const LidarScan & scan, const XYZLut & lut) { - if (scan.w * scan.h != lut.direction.rows()) - throw std::invalid_argument("unexpected scan dimensions"); - - auto reshaped = Eigen::Map>( - scan.field(LidarScan::RANGE).data(), scan.h * scan.w); + return cartesian(scan.field(sensor::ChanField::RANGE), lut); +} + +LidarScan::Points cartesian(const Eigen::Ref>& range, + const XYZLut& lut) { + if (range.cols() * range.rows() != lut.direction.rows()) + throw std::invalid_argument("unexpected image dimensions"); + auto reshaped = Eigen::Map>( + range.data(), range.cols() * range.rows()); auto nooffset = lut.direction.colwise() * reshaped.cast(); return (nooffset.array() == 0.0).select(nooffset, nooffset + lut.offset); } -ScanBatcher::ScanBatcher(size_t w, const sensor::packet_format & pf) -: w(w), h(pf.pixels_per_column), next_m_id(0), ls_write(w, h), pf(pf) {} +LidarScan::LidarScan(size_t w, size_t h, LidarScanFieldTypes field_types) + : timestamp_{Header::Zero(w)}, + measurement_id_{Header::Zero(w)}, + status_{Header::Zero(w)}, + field_types_{std::move(field_types)}, + w{static_cast(w)}, + h{static_cast(h)} +{ + // TODO: error on duplicate fields + for (const auto & ft: field_types_) { + if (fields_.count(ft.first) > 0) + throw std::invalid_argument("Duplicated fields found"); + fields_[ft.first] = impl::FieldSlot{ft.second, w, h}; + } +} + +LidarScan::LidarScan(size_t w, size_t h, sensor::UDPProfileLidar profile) + : LidarScan{w, h, impl::lookup_scan_fields(profile)} {} + +bool LidarScan::complete(sensor::ColumnWindow window) const { + const auto& status = this->status(); + auto start = window.first; + auto end = window.second; + + if (start <= end) { + return status.segment(start, end - start + 1) + .unaryExpr([](uint32_t s) { return s & 0x01; }) + .isConstant(0x01); + } else { + return status.segment(0, end) + .unaryExpr([](uint32_t s) { return s & 0x01; }) + .isConstant(0x01) && + status.segment(start, this->w - start) + .unaryExpr([](uint32_t s) { return s & 0x01; }) + .isConstant(0x01); + } +} + +namespace { + +/* + * Generic operation to set all columns in the range [start, end) to zero + */ +struct zero_field_cols { + template + void operator()(Eigen::Ref> field, sensor::ChanField, std::ptrdiff_t start, + std::ptrdiff_t end) { + field.block(0, start, field.rows(), end - start).setZero(); + } +}; + +/* + * Zero out all measurement block headers in range [start, end) + */ +void zero_header_cols(LidarScan& ls, std::ptrdiff_t start, std::ptrdiff_t end) { + ls.timestamp().segment(start, end - start).setZero(); + ls.measurement_id().segment(start, end - start).setZero(); + ls.status().segment(start, end - start).setZero(); +} + +/* + * Generic operation to read a channel field from a packet measurement block + * into a scan + */ +struct parse_field_col { + template + void operator()(Eigen::Ref> field, sensor::ChanField f, uint16_t m_id, + const sensor::packet_format& pf, const uint8_t* col_buf) { + // user defined fields that we shouldn't change + if (f >= sensor::ChanField::CUSTOM0 && f <= sensor::ChanField::CUSTOM9) return; + + // RAW_HEADERS field is populated separately because it has + // a different processing scheme and doesn't fit into existing field + // model (i.e. data packed per column rather than per pixel) + if (f == sensor::ChanField::RAW_HEADERS) return; + + pf.col_field(col_buf, f, field.col(m_id).data(), field.cols()); + } +}; + +uint64_t frame_status(const uint8_t thermal_shutdown, + const uint8_t shot_limiting) { + uint64_t res = 0; + + // clang-format off + res |= (thermal_shutdown & 0x0f) + << FRAME_STATUS_THERMAL_SHUTDOWN_SHIFT; // right nibble is thermal + // shutdown status, apply mask + // for safety, then shift + //clang-format on + res |= (shot_limiting & 0x0f) + << FRAME_STATUS_SHOT_LIMITING_SHIFT; // right nibble is shot + // limiting, apply mask for + // safety, then shift + return res; +} + +/** + * Checks whether RAW_HEADERS field is present and can be used to store headers. + * + * @param[in] pf packet format + * @param[in] ls lidar scan to check for RAW_HEADERS field presence. + */ +bool raw_headers_enabled(const sensor::packet_format& pf, const LidarScan& ls) { + sensor::ChanFieldType raw_headers_ft = ls.field_type(sensor::ChanField::RAW_HEADERS); + if (!raw_headers_ft) { + return false; + } + // ensure that we can pack headers into the size of a single RAW_HEADERS + // column + if (pf.pixels_per_column * sensor::field_type_size(raw_headers_ft) < + (pf.packet_header_size + pf.col_header_size + pf.col_footer_size + + pf.packet_footer_size)) { +// logger().debug( +// "WARNING: Can't fit RAW_HEADERS into a column of {} {} " +// "values", +// pf.pixels_per_column, to_string(raw_headers_ft)); + std:: cerr << "WARNING: Can't fit RAW_HEADERS into a column of " << pf.pixels_per_column + << " " << to_string(raw_headers_ft) << " values" << std::endl; + return false; + } + return true; +} + +/** + * Pack the lidar packet and column headers and footer into a RAW_HEADERS field. + */ +struct pack_raw_headers_col { + template + void operator()(Eigen::Ref> rh_field, sensor::ChanField, + const sensor::packet_format& pf, uint16_t col_idx, + const uint8_t* packet_buf) { + const uint8_t* col_buf = pf.nth_col(col_idx, packet_buf); + const uint16_t m_id = pf.col_measurement_id(col_buf); + + using ColMajorView = + Eigen::Map>; + + const ColMajorView col_header_vec(reinterpret_cast(col_buf), + pf.col_header_size / sizeof(T)); + + rh_field.block(0, m_id, col_header_vec.size(), 1) = col_header_vec; + + const ColMajorView col_footer_vec( + reinterpret_cast(col_buf + pf.col_size - + pf.col_footer_size), + pf.col_footer_size / sizeof(T)); + + rh_field.block(col_header_vec.size(), m_id, col_footer_vec.size(), 1) = + col_footer_vec; + + const ColMajorView packet_header_vec( + reinterpret_cast(packet_buf), + pf.packet_header_size / sizeof(T)); + + rh_field.block(col_header_vec.size() + col_footer_vec.size(), m_id, + packet_header_vec.size(), 1) = packet_header_vec; + + const ColMajorView packet_footer_vec( + reinterpret_cast(pf.footer(packet_buf)), + pf.packet_footer_size / sizeof(T)); + + rh_field.block(col_header_vec.size() + col_footer_vec.size() + + packet_header_vec.size(), + m_id, packet_footer_vec.size(), 1) = packet_footer_vec; + } +}; + +} // namespace bool ScanBatcher::operator()(const uint8_t * packet_buf, LidarScan & ls) { - using row_view_t = - Eigen::Map>; - if (ls.w != w || ls.h != h) { throw std::invalid_argument("unexpected scan dimensions"); } bool swapped = false; + const bool raw_headers = raw_headers_enabled(pf, ls); + + const uint16_t f_id = pf.frame_id(packet_buf); + if (ls_write.frame_id != f_id) { + // if not initializing with first packet + if (ls_write.frame_id != -1) { + // got a packet from a new frame + for (const auto& field_type : ls) { + auto end_m_id = next_valid_m_id; + if (raw_headers && field_type.first == sensor::ChanField::RAW_HEADERS) { + end_m_id = next_headers_m_id; + } + impl::visit_field(ls, field_type.first, zero_field_cols(), + field_type.first, end_m_id, w); + } + zero_header_cols(ls, next_valid_m_id, w); + + // finish the scan and notify callback + std::swap(ls, ls_write); + swapped = true; + } + + // drop reordered packets from the previous frame + if (ls.frame_id == static_cast(f_id + 1)) return false; + + // start new frame + next_valid_m_id = 0; + next_headers_m_id = 0; + ls_write.frame_id = f_id; + + const uint8_t f_thermal_shutdown = pf.thermal_shutdown(packet_buf); + const uint8_t f_shot_limiting = pf.shot_limiting(packet_buf); + ls.frame_status = frame_status(f_thermal_shutdown, f_shot_limiting); + } + + // parse measurement blocks for (int icol = 0; icol < pf.columns_per_packet; icol++) { const uint8_t * col_buf = pf.nth_col(icol, packet_buf); const uint16_t m_id = pf.col_measurement_id(col_buf); - const uint16_t f_id = pf.col_frame_id(col_buf); - const std::chrono::nanoseconds ts(pf.col_timestamp(col_buf)); - const uint32_t encoder = pf.col_encoder(col_buf); + const uint64_t ts = pf.col_timestamp(col_buf); const uint32_t status = pf.col_status(col_buf); - const bool valid = (status == 0xffffffff); + const bool valid = (status & 0x01); // drop invalid / out-of-bounds data in case of misconfiguration - if (!valid || m_id >= w || f_id + 1 == ls_write.frame_id) {continue;} - - if (ls_write.frame_id != f_id) { - // if not initializing with first packet - if (ls_write.frame_id != -1) { - // zero out remaining missing columns - auto rows = h * LidarScan::N_FIELDS; - row_view_t{ls_write.data.data(), rows, w} - .block(0, next_m_id, rows, w - next_m_id) - .setZero(); - - // finish the scan and notify callback - std::swap(ls, ls_write); - swapped = true; + if (!valid || m_id >= w) {continue;} + + if (raw_headers) { + // zero out missing columns if we jumped forward + if (m_id >= next_headers_m_id) { + impl::visit_field(ls, sensor::ChanField::RAW_HEADERS, zero_field_cols(), + sensor::ChanField::RAW_HEADERS, next_headers_m_id, + m_id); + next_headers_m_id = m_id + 1; } - // start new frame - next_m_id = 0; - ls_write.frame_id = f_id; + impl::visit_field(ls, sensor::ChanField::RAW_HEADERS, + pack_raw_headers_col(), sensor::ChanField::RAW_HEADERS, + pf, icol, packet_buf); } // zero out missing columns if we jumped forward - if (m_id >= next_m_id) { - auto rows = h * LidarScan::N_FIELDS; - row_view_t{ls_write.data.data(), rows, w} - .block(0, next_m_id, rows, m_id - next_m_id) - .setZero(); - next_m_id = m_id + 1; + if (m_id >= next_valid_m_id) { + for (const auto& field_type : ls) { + if (field_type.first == sensor::ChanField::RAW_HEADERS) continue; + impl::visit_field(ls, field_type.first, zero_field_cols(), + field_type.first, next_valid_m_id, m_id); + } + zero_header_cols(ls, next_valid_m_id, m_id); + next_valid_m_id = m_id + 1; } - ls_write.header(m_id) = {ts, encoder, status}; - for (uint8_t ipx = 0; ipx < h; ipx++) { - const uint8_t * px_buf = pf.nth_px(ipx, col_buf); + // write new header values + ls.timestamp()[m_id] = ts; + ls.measurement_id()[m_id] = m_id; + ls.status()[m_id] = status; - ls_write.block(m_id).row(ipx) << - static_cast(pf.px_range(px_buf)), - static_cast(pf.px_signal(px_buf)), - static_cast(pf.px_ambient(px_buf)), - static_cast(pf.px_reflectivity(px_buf)); - } + impl::foreach_field(ls, parse_field_col(), m_id, pf, col_buf); } + return swapped; } diff --git a/ros2_ouster/src/client/parsing.cpp b/ros2_ouster/src/client/parsing.cpp new file mode 100644 index 0000000..e013b81 --- /dev/null +++ b/ros2_ouster/src/client/parsing.cpp @@ -0,0 +1,511 @@ +/** +* Copyright (c) 2018, Ouster, Inc. +* All rights reserved. +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ros2_ouster/client/types.h" + +namespace ouster { +namespace sensor { + +namespace impl { + +constexpr int imu_packet_size = 48; +constexpr int64_t encoder_ticks_per_rev = 90112; + +template +using Table = std::array, N>; + +struct FieldInfo { + ChanFieldType ty_tag; + size_t offset; + uint64_t mask; + int shift; +}; + +struct ProfileEntry { + const std::pair* fields; + size_t n_fields; + size_t chan_data_size; +}; + +static const Table legacy_field_info{{ + {ChanField::RANGE, {UINT32, 0, 0x000fffff, 0}}, + {ChanField::FLAGS, {UINT8, 3, 0, 4}}, + {ChanField::REFLECTIVITY, {UINT16, 4, 0, 0}}, + {ChanField::SIGNAL, {UINT16, 6, 0, 0}}, + {ChanField::NEAR_IR, {UINT16, 8, 0, 0}}, + {ChanField::RAW32_WORD1, {UINT32, 0, 0, 0}}, + {ChanField::RAW32_WORD2, {UINT32, 4, 0, 0}}, + {ChanField::RAW32_WORD3, {UINT32, 8, 0, 0}}, +}}; + +static const Table lb_field_info{{ + {ChanField::RANGE, {UINT16, 0, 0x7fff, -3}}, + {ChanField::FLAGS, {UINT8, 1, 0b10000000, 7}}, + {ChanField::REFLECTIVITY, {UINT8, 2, 0, 0}}, + {ChanField::NEAR_IR, {UINT8, 3, 0, -4}}, + {ChanField::RAW32_WORD1, {UINT32, 0, 0, 0}}, +}}; + +static const Table dual_field_info{{ + {ChanField::RANGE, {UINT32, 0, 0x0007ffff, 0}}, + {ChanField::FLAGS, {UINT8, 2, 0b11111000, 3}}, + {ChanField::REFLECTIVITY, {UINT8, 3, 0, 0}}, + {ChanField::RANGE2, {UINT32, 4, 0x0007ffff, 0}}, + {ChanField::FLAGS2, {UINT8, 6, 0b11111000, 3}}, + {ChanField::REFLECTIVITY2, {UINT8, 7, 0, 0}}, + {ChanField::SIGNAL, {UINT16, 8, 0, 0}}, + {ChanField::SIGNAL2, {UINT16, 10, 0, 0}}, + {ChanField::NEAR_IR, {UINT16, 12, 0, 0}}, + {ChanField::RAW32_WORD1, {UINT32, 0, 0, 0}}, + {ChanField::RAW32_WORD2, {UINT32, 4, 0, 0}}, + {ChanField::RAW32_WORD3, {UINT32, 8, 0, 0}}, + {ChanField::RAW32_WORD4, {UINT32, 12, 0, 0}}, +}}; + +static const Table single_field_info{{ + {ChanField::RANGE, {UINT32, 0, 0x0007ffff, 0}}, + {ChanField::FLAGS, {UINT8, 2, 0b11111000, 3}}, + {ChanField::REFLECTIVITY, {UINT8, 4, 0, 0}}, + {ChanField::SIGNAL, {UINT16, 6, 0, 0}}, + {ChanField::NEAR_IR, {UINT16, 8, 0, 0}}, + {ChanField::RAW32_WORD1, {UINT32, 0, 0, 0}}, + {ChanField::RAW32_WORD2, {UINT32, 4, 0, 0}}, + {ChanField::RAW32_WORD3, {UINT32, 8, 0, 0}}, +}}; + +static const Table five_word_pixel_info{{ + {ChanField::RANGE, {UINT32, 0, 0x0007ffff, 0}}, + {ChanField::FLAGS, {UINT8, 2, 0b11111000, 3}}, + {ChanField::REFLECTIVITY, {UINT8, 3, 0, 0}}, + {ChanField::RANGE2, {UINT32, 4, 0x0007ffff, 0}}, + {ChanField::FLAGS2, {UINT8, 6, 0b11111000, 3}}, + {ChanField::REFLECTIVITY2, {UINT8, 7, 0, 0}}, + {ChanField::SIGNAL, {UINT16, 8, 0, 0}}, + {ChanField::SIGNAL2, {UINT16, 10, 0, 0}}, + {ChanField::NEAR_IR, {UINT16, 12, 0, 0}}, + {ChanField::RAW32_WORD1, {UINT32, 0, 0, 0}}, + {ChanField::RAW32_WORD2, {UINT32, 4, 0, 0}}, + {ChanField::RAW32_WORD3, {UINT32, 8, 0, 0}}, + {ChanField::RAW32_WORD4, {UINT32, 12, 0, 0}}, + {ChanField::RAW32_WORD5, {UINT32, 16, 0, 0}}, +}}; + +Table profiles{{ + {UDPProfileLidar::PROFILE_LIDAR_LEGACY, + {legacy_field_info.data(), legacy_field_info.size(), 12}}, + {UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL, + {dual_field_info.data(), dual_field_info.size(), 16}}, + {UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16, + {single_field_info.data(), single_field_info.size(), 12}}, + {UDPProfileLidar::PROFILE_RNG15_RFL8_NIR8, + {lb_field_info.data(), lb_field_info.size(), 4}}, + {UDPProfileLidar::PROFILE_FIVE_WORD_PIXEL, + {five_word_pixel_info.data(), five_word_pixel_info.size(), 20}}, +}}; + +static const ProfileEntry& lookup_profile_entry(UDPProfileLidar profile) { + auto end = profiles.end(); + auto it = + std::find_if(impl::profiles.begin(), end, + [profile](const auto& kv) { return kv.first == profile; }); + + if (it == end || it->first == 0) + throw std::invalid_argument("Unknown lidar udp profile"); + + return it->second; +} + +} // namespace impl + +struct packet_format::Impl { + size_t packet_header_size; + size_t col_header_size; + size_t channel_data_size; + size_t col_footer_size; + size_t packet_footer_size; + + size_t col_size; + size_t lidar_packet_size; + + size_t timestamp_offset; + size_t measurement_id_offset; + size_t status_offset; + + std::map fields; + + Impl(UDPProfileLidar profile, int pixels_per_column, + int columns_per_packet) { + bool legacy = (profile == UDPProfileLidar::PROFILE_LIDAR_LEGACY); + + const auto& entry = impl::lookup_profile_entry(profile); + + packet_header_size = legacy ? 0 : 32; + col_header_size = legacy ? 16 : 12; + channel_data_size = entry.chan_data_size; + col_footer_size = legacy ? 4 : 0; + packet_footer_size = legacy ? 0 : 32; + + col_size = col_header_size + pixels_per_column * channel_data_size + + col_footer_size; + lidar_packet_size = packet_header_size + columns_per_packet * col_size + + packet_footer_size; + + fields = {entry.fields, entry.fields + entry.n_fields}; + + timestamp_offset = 0; + measurement_id_offset = 8; + status_offset = legacy ? col_size - col_footer_size : 10; + } +}; + +packet_format::packet_format(const sensor_info& info) + : impl_{std::make_shared(info.format.udp_profile_lidar, + info.format.pixels_per_column, + info.format.columns_per_packet)}, + udp_profile_lidar{info.format.udp_profile_lidar}, + lidar_packet_size{impl_->lidar_packet_size}, + imu_packet_size{impl::imu_packet_size}, + columns_per_packet(info.format.columns_per_packet), + pixels_per_column(info.format.pixels_per_column), + packet_header_size{impl_->packet_header_size}, + col_header_size{impl_->col_header_size}, + col_footer_size{impl_->col_footer_size}, + col_size{impl_->col_size}, + packet_footer_size{impl_->packet_footer_size} { + for (const auto& kv : impl_->fields) { + field_types_.emplace_back(kv.first, kv.second.ty_tag); + } +} + +template +static void col_field_impl(const uint8_t* col_buf, DST* dst, size_t offset, + uint64_t mask, int shift, int pixels_per_column, + int dst_stride, size_t channel_data_size, + size_t col_header_size) { + if (sizeof(DST) < sizeof(SRC)) + throw std::invalid_argument("Dest type too small for specified field"); + + for (int px = 0; px < pixels_per_column; px++) { + auto px_src = + col_buf + col_header_size + offset + (px * channel_data_size); + DST* px_dst = dst + px * dst_stride; + *px_dst = 0; + std::memcpy(px_dst, px_src, sizeof(SRC)); + if (mask) *px_dst &= mask; + if (shift > 0) *px_dst >>= shift; + if (shift < 0) *px_dst <<= std::abs(shift); + } +} + +template ::value, T>::type> +void packet_format::col_field(const uint8_t* col_buf, ChanField i, T* dst, + int dst_stride) const { + const auto& f = impl_->fields.at(i); + + switch (f.ty_tag) { + case UINT8: + col_field_impl( + col_buf, dst, f.offset, f.mask, f.shift, pixels_per_column, + dst_stride, impl_->channel_data_size, impl_->col_header_size); + break; + case UINT16: + col_field_impl( + col_buf, dst, f.offset, f.mask, f.shift, pixels_per_column, + dst_stride, impl_->channel_data_size, impl_->col_header_size); + break; + case UINT32: + col_field_impl( + col_buf, dst, f.offset, f.mask, f.shift, pixels_per_column, + dst_stride, impl_->channel_data_size, impl_->col_header_size); + break; + case UINT64: + col_field_impl( + col_buf, dst, f.offset, f.mask, f.shift, pixels_per_column, + dst_stride, impl_->channel_data_size, impl_->col_header_size); + break; + default: + throw std::invalid_argument("Invalid field for packet format"); + } +} + +// explicitly instantiate for each field type +template void packet_format::col_field(const uint8_t*, ChanField, uint8_t*, + int) const; +template void packet_format::col_field(const uint8_t*, ChanField, uint16_t*, + int) const; +template void packet_format::col_field(const uint8_t*, ChanField, uint32_t*, + int) const; +template void packet_format::col_field(const uint8_t*, ChanField, uint64_t*, + int) const; + +ChanFieldType packet_format::field_type(ChanField f) const { + return impl_->fields.count(f) ? impl_->fields.at(f).ty_tag + : ChanFieldType::VOID; +} + +packet_format::FieldIter packet_format::begin() const { + return field_types_.cbegin(); +} + +packet_format::FieldIter packet_format::end() const { + return field_types_.cend(); +} + +/* Packet headers */ + +uint16_t packet_format::packet_type(const uint8_t* lidar_buf) const { + if (udp_profile_lidar == UDPProfileLidar::PROFILE_LIDAR_LEGACY) { + // LEGACY profile has no packet_type - use 0 to code as 'legacy' + return 0; + } + uint16_t res; + std::memcpy(&res, lidar_buf + 0, sizeof(uint16_t)); + return res; +} + +uint16_t packet_format::frame_id(const uint8_t* lidar_buf) const { + if (udp_profile_lidar == UDPProfileLidar::PROFILE_LIDAR_LEGACY) { + return col_frame_id(nth_col(0, lidar_buf)); + } + uint16_t res; + std::memcpy(&res, lidar_buf + 2, sizeof(uint16_t)); + return res; +} + +uint32_t packet_format::init_id(const uint8_t* lidar_buf) const { + if (udp_profile_lidar == UDPProfileLidar::PROFILE_LIDAR_LEGACY) { + // LEGACY profile has no init_id - use 0 to code as 'legacy' + return 0; + } + uint32_t res; + std::memcpy(&res, lidar_buf + 4, sizeof(uint32_t)); + return res & 0x00ffffff; +} + +uint64_t packet_format::prod_sn(const uint8_t* lidar_buf) const { + if (udp_profile_lidar == UDPProfileLidar::PROFILE_LIDAR_LEGACY) { + // LEGACY profile has no prod_sn (serial number) - use 0 to code as + // 'legacy' + return 0; + } + uint64_t res; + std::memcpy(&res, lidar_buf + 7, sizeof(uint64_t)); + return res & 0x000000ffffffffff; +} + +uint16_t packet_format::countdown_thermal_shutdown( + const uint8_t* lidar_buf) const { + if (udp_profile_lidar == UDPProfileLidar::PROFILE_LIDAR_LEGACY) { + // LEGACY profile has no shutdown counter in packet header - use 0 for + // 'normal operation' + return 0; + } + uint16_t res; + std::memcpy(&res, lidar_buf + 16, sizeof(uint8_t)); + return res; +} + +uint16_t packet_format::countdown_shot_limiting( + const uint8_t* lidar_buf) const { + if (udp_profile_lidar == UDPProfileLidar::PROFILE_LIDAR_LEGACY) { + // LEGACY profile has no shot limiting countdown in packet header - use + // 0 for 'normal operation' + return 0; + } + uint16_t res; + std::memcpy(&res, lidar_buf + 17, sizeof(uint8_t)); + return res; +} + +uint8_t packet_format::thermal_shutdown(const uint8_t* lidar_buf) const { + if (udp_profile_lidar == UDPProfileLidar::PROFILE_LIDAR_LEGACY) { + // LEGACY profile has no shutdown status in packet header - use 0 for + // 'normal operation' + return 0; + } + uint8_t res; + std::memcpy(&res, lidar_buf + 18, sizeof(uint8_t)); + return res & 0x0f; +} + +uint8_t packet_format::shot_limiting(const uint8_t* lidar_buf) const { + if (udp_profile_lidar == UDPProfileLidar::PROFILE_LIDAR_LEGACY) { + // LEGACY profile has no shot limiting in packet header - use 0 for + // 'normal operation' + return 0; + } + uint8_t res; + std::memcpy(&res, lidar_buf + 19, sizeof(uint8_t)); + return res & 0x0f; +} + +const uint8_t* packet_format::footer(const uint8_t* lidar_buf) const { + if (impl_->packet_footer_size == 0) return nullptr; + return lidar_buf + impl_->packet_header_size + + (columns_per_packet * impl_->col_size); +} + +/* Measurement block access */ + +const uint8_t* packet_format::nth_col(int n, const uint8_t* lidar_buf) const { + return lidar_buf + impl_->packet_header_size + (n * impl_->col_size); +} + +uint32_t packet_format::col_status(const uint8_t* col_buf) const { + uint32_t res; + std::memcpy(&res, col_buf + impl_->status_offset, sizeof(uint32_t)); + if (udp_profile_lidar == UDPProfileLidar::PROFILE_LIDAR_LEGACY) { + return res; // LEGACY was 32 bits of all 1s + } else { + return res & 0xffff; // For eUDP packets, we want the last 16 bits + } +} + +uint64_t packet_format::col_timestamp(const uint8_t* col_buf) const { + uint64_t res; + std::memcpy(&res, col_buf + impl_->timestamp_offset, sizeof(uint64_t)); + return res; +} + +uint16_t packet_format::col_measurement_id(const uint8_t* col_buf) const { + uint16_t res; + std::memcpy(&res, col_buf + impl_->measurement_id_offset, sizeof(uint16_t)); + return res; +} + +uint16_t packet_format::col_frame_id(const uint8_t* col_buf) const { + if (udp_profile_lidar == UDPProfileLidar::PROFILE_LIDAR_LEGACY) { + uint16_t res; + std::memcpy(&res, col_buf + 10, sizeof(uint16_t)); + return res; + } else { + return 0; + } +} + +/* Channel data fields */ + +const uint8_t* packet_format::nth_px(int n, const uint8_t* col_buf) const { + return col_buf + impl_->col_header_size + (n * impl_->channel_data_size); +} + +template +T packet_format::px_field(const uint8_t* px_buf, ChanField i) const { + const auto& f = impl_->fields.at(i); + + if (sizeof(T) < field_type_size(f.ty_tag)) + throw std::invalid_argument("Dest type too small for specified field"); + + T res = 0; + std::memcpy(&res, px_buf + f.offset, field_type_size(f.ty_tag)); + if (f.mask) res &= f.mask; + if (f.shift > 0) res >>= f.shift; + if (f.shift < 0) res <<= std::abs(f.shift); + return res; +} + +uint32_t packet_format::px_range(const uint8_t* px_buf) const { + return px_field(px_buf, ChanField::RANGE); +} + +uint16_t packet_format::px_reflectivity(const uint8_t* px_buf) const { + return px_field(px_buf, ChanField::REFLECTIVITY); +} + +uint16_t packet_format::px_signal(const uint8_t* px_buf) const { + return px_field(px_buf, ChanField::SIGNAL); +} + +uint16_t packet_format::px_ambient(const uint8_t* px_buf) const { + return px_field(px_buf, ChanField::NEAR_IR); +} + +/* IMU packet parsing */ + +uint64_t packet_format::imu_sys_ts(const uint8_t* imu_buf) { + uint64_t res; + std::memcpy(&res, imu_buf, sizeof(uint64_t)); + return res; +} + +uint64_t packet_format::imu_accel_ts(const uint8_t* imu_buf) { + uint64_t res; + std::memcpy(&res, imu_buf + 8, sizeof(uint64_t)); + return res; +} + +uint64_t packet_format::imu_gyro_ts(const uint8_t* imu_buf) { + uint64_t res; + std::memcpy(&res, imu_buf + 16, sizeof(uint64_t)); + return res; +} + +float packet_format::imu_la_x(const uint8_t* imu_buf) { + float res; + std::memcpy(&res, imu_buf + 24, sizeof(float)); + return res; +} + +float packet_format::imu_la_y(const uint8_t* imu_buf) { + float res; + std::memcpy(&res, imu_buf + 28, sizeof(float)); + return res; +} + +float packet_format::imu_la_z(const uint8_t* imu_buf) { + float res; + std::memcpy(&res, imu_buf + 32, sizeof(float)); + return res; +} + +float packet_format::imu_av_x(const uint8_t* imu_buf) { + float res; + std::memcpy(&res, imu_buf + 36, sizeof(float)); + return res; +} + +float packet_format::imu_av_y(const uint8_t* imu_buf) { + float res; + std::memcpy(&res, imu_buf + 40, sizeof(float)); + return res; +} + +float packet_format::imu_av_z(const uint8_t* imu_buf) { + float res; + std::memcpy(&res, imu_buf + 44, sizeof(float)); + return res; +} + +const packet_format& get_format(const sensor_info& info) { + using key = std::tuple; + static std::map> cache{}; + static std::mutex cache_mx{}; + + key k{info.format.pixels_per_column, info.format.columns_per_packet, + info.format.udp_profile_lidar}; + + std::lock_guard lk{cache_mx}; + if (!cache.count(k)) { + cache[k] = std::make_unique(info); + } + + return *cache.at(k); +} + +} // namespace sensor +} // namespace ouster diff --git a/ros2_ouster/src/client/types.cpp b/ros2_ouster/src/client/types.cpp index d0a1399..45adb7d 100644 --- a/ros2_ouster/src/client/types.cpp +++ b/ros2_ouster/src/client/types.cpp @@ -3,18 +3,15 @@ * @brief Implementation of Ouster client datatypes and constants. */ -#include -#include #include #include #include #include #include #include -#include -#include +#include + #include "ros2_ouster/client/types.h" -#include "ros2_ouster/client/impl/parsing.h" #include "ros2_ouster/client/version.h" // Declare namespaces from optional-lite @@ -31,109 +28,168 @@ namespace sensor namespace { -const std::array, 5> - lidar_mode_strings = {{ - {MODE_512x10, "512x10"}, - {MODE_512x20, "512x20"}, - {MODE_1024x10, "1024x10"}, - {MODE_1024x20, "1024x20"}, - {MODE_2048x10, "2048x10"} - }}; - -const std::array, 3> - 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"} - }}; - -const std::array, 2> - operating_mode_strings = {{ - {OPERATING_NORMAL, "NORMAL"}, - {OPERATING_STANDBY, "STANDBY"} - }}; - -const std::array, 6> - multipurpose_io_mode_strings = {{ - {MULTIPURPOSE_OFF, "OFF"}, - {MULTIPURPOSE_INPUT_NMEA_UART, "INPUT_NMEA_UART"}, - {MULTIPURPOSE_OUTPUT_FROM_INTERNAL_OSC, "OUTPUT_FROM_INTERNAL_OSC"}, - {MULTIPURPOSE_OUTPUT_FROM_SYNC_PULSE_IN, "OUTPUT_FROM_SYNC_PULSE_IN"}, - {MULTIPURPOSE_OUTPUT_FROM_PTP_1588, "OUTPUT_FROM_PTP_1588"}, - {MULTIPURPOSE_OUTPUT_FROM_ENCODER_ANGLE, "OUTPUT_FROM_ENCODER_ANGLE"} - }}; - -const std::array, 2> - polarity_strings = {{ - {POLARITY_ACTIVE_LOW, "ACTIVE_LOW"}, - {POLARITY_ACTIVE_HIGH, "ACTIVE_HIGH"} - }}; - -const std::array, 2> - nmea_baud_rate_strings = {{ - {BAUD_9600, "BAUD_9600"}, - {BAUD_115200, "BAUD_115200"} - }}; +template +using Table = std::array, N>; + +extern const Table lidar_mode_strings{ + {{MODE_UNSPEC, "UNKNOWN"}, + {MODE_512x10, "512x10"}, + {MODE_512x20, "512x20"}, + {MODE_1024x10, "1024x10"}, + {MODE_1024x20, "1024x20"}, + {MODE_2048x10, "2048x10"}, + {MODE_4096x5, "4096x5"}}}; + +extern const Table timestamp_mode_strings{ + {{TIME_FROM_UNSPEC, "UNKNOWN"}, + {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"}}}; + +extern const Table operating_mode_strings{ + {{OPERATING_NORMAL, "NORMAL"}, {OPERATING_STANDBY, "STANDBY"}}}; + +extern const Table + multipurpose_io_mode_strings{ + {{MULTIPURPOSE_OFF, "OFF"}, + {MULTIPURPOSE_INPUT_NMEA_UART, "INPUT_NMEA_UART"}, + {MULTIPURPOSE_OUTPUT_FROM_INTERNAL_OSC, "OUTPUT_FROM_INTERNAL_OSC"}, + {MULTIPURPOSE_OUTPUT_FROM_SYNC_PULSE_IN, "OUTPUT_FROM_SYNC_PULSE_IN"}, + {MULTIPURPOSE_OUTPUT_FROM_PTP_1588, "OUTPUT_FROM_PTP_1588"}, + {MULTIPURPOSE_OUTPUT_FROM_ENCODER_ANGLE, + "OUTPUT_FROM_ENCODER_ANGLE"}}}; + +extern const Table polarity_strings{ + {{POLARITY_ACTIVE_LOW, "ACTIVE_LOW"}, + {POLARITY_ACTIVE_HIGH, "ACTIVE_HIGH"}}}; + +extern const Table nmea_baud_rate_strings{ + {{BAUD_9600, "BAUD_9600"}, {BAUD_115200, "BAUD_115200"}}}; + +extern const Table chanfield_strings{{ + {ChanField::RANGE, "RANGE"}, + {ChanField::RANGE2, "RANGE2"}, + {ChanField::SIGNAL, "SIGNAL"}, + {ChanField::SIGNAL2, "SIGNAL2"}, + {ChanField::REFLECTIVITY, "REFLECTIVITY"}, + {ChanField::REFLECTIVITY2, "REFLECTIVITY2"}, + {ChanField::NEAR_IR, "NEAR_IR"}, + {ChanField::FLAGS, "FLAGS"}, + {ChanField::FLAGS2, "FLAGS2"}, + {ChanField::RAW_HEADERS, "RAW_HEADERS"}, + {ChanField::CUSTOM0, "CUSTOM0"}, + {ChanField::CUSTOM1, "CUSTOM1"}, + {ChanField::CUSTOM2, "CUSTOM2"}, + {ChanField::CUSTOM3, "CUSTOM3"}, + {ChanField::CUSTOM4, "CUSTOM4"}, + {ChanField::CUSTOM5, "CUSTOM5"}, + {ChanField::CUSTOM6, "CUSTOM6"}, + {ChanField::CUSTOM7, "CUSTOM7"}, + {ChanField::CUSTOM8, "CUSTOM8"}, + {ChanField::CUSTOM9, "CUSTOM9"}, + {ChanField::RAW32_WORD1, "RAW32_WORD1"}, + {ChanField::RAW32_WORD2, "RAW32_WORD2"}, + {ChanField::RAW32_WORD3, "RAW32_WORD3"}, + {ChanField::RAW32_WORD4, "RAW32_WORD4"}, + {ChanField::RAW32_WORD5, "RAW32_WORD5"}, + {ChanField::RAW32_WORD6, "RAW32_WORD6"}, + {ChanField::RAW32_WORD7, "RAW32_WORD7"}, + {ChanField::RAW32_WORD8, "RAW32_WORD8"}, + {ChanField::RAW32_WORD9, "RAW32_WORD9"}, +}}; + +extern const Table udp_profile_lidar_strings{{ + {PROFILE_LIDAR_LEGACY, "LEGACY"}, + {PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL, "RNG19_RFL8_SIG16_NIR16_DUAL"}, + {PROFILE_RNG19_RFL8_SIG16_NIR16, "RNG19_RFL8_SIG16_NIR16"}, + {PROFILE_RNG15_RFL8_NIR8, "RNG15_RFL8_NIR8"}, + {PROFILE_FIVE_WORD_PIXEL, "FIVE_WORD_PIXEL"}, +}}; + +extern const Table udp_profile_imu_strings{{ + {PROFILE_IMU_LEGACY, "LEGACY"}, +}}; + +extern const Table shot_limiting_status_strings{{ + {SHOT_LIMITING_NORMAL, "SHOT_LIMITING_NORMAL"}, + {SHOT_LIMITING_IMMINENT, "SHOT_LIMITING_IMMINENT"}, + {SHOT_LIMITING_REDUCTION_0_10, "SHOT_LIMITING_REDUCTION_0_10"}, + {SHOT_LIMITING_REDUCTION_10_20, "SHOT_LIMITING_REDUCTION_10_20"}, + {SHOT_LIMITING_REDUCTION_20_30, "SHOT_LIMITING_REDUCTION_20_30"}, + {SHOT_LIMITING_REDUCTION_30_40, "SHOT_LIMITING_REDUCTION_30_40"}, + {SHOT_LIMITING_REDUCTION_40_50, "SHOT_LIMITING_REDUCTION_40_50"}, + {SHOT_LIMITING_REDUCTION_50_60, "SHOT_LIMITING_REDUCTION_50_60"}, + {SHOT_LIMITING_REDUCTION_60_70, "SHOT_LIMITING_REDUCTION_60_70"}, + {SHOT_LIMITING_REDUCTION_70_75, "SHOT_LIMITING_REDUCTION_70_75"}, +}}; + +extern const Table thermal_shutdown_status_strings{{ + {THERMAL_SHUTDOWN_NORMAL, "THERMAL_SHUTDOWN_NORMAL"}, + {THERMAL_SHUTDOWN_IMMINENT, "THERMAL_SHUTDOWN_IMMINENT"}, +}}; } // namespace -bool operator==(const data_format & lhs, const data_format & rhs) -{ - return lhs.pixels_per_column == rhs.pixels_per_column && - lhs.columns_per_packet == rhs.columns_per_packet && - lhs.columns_per_frame == rhs.columns_per_frame && - lhs.pixel_shift_by_row == rhs.pixel_shift_by_row && - lhs.column_window == rhs.column_window; +bool operator==(const data_format& lhs, const data_format& rhs) { + return (lhs.pixels_per_column == rhs.pixels_per_column && + lhs.columns_per_packet == rhs.columns_per_packet && + lhs.columns_per_frame == rhs.columns_per_frame && + lhs.pixel_shift_by_row == rhs.pixel_shift_by_row && + lhs.column_window == rhs.column_window && + lhs.udp_profile_lidar == rhs.udp_profile_lidar && + lhs.udp_profile_imu == rhs.udp_profile_imu && lhs.fps == rhs.fps); } -bool operator!=(const data_format & lhs, const data_format & rhs) -{ +bool operator!=(const data_format& lhs, const data_format& rhs) { return !(lhs == rhs); } -bool operator==(const sensor_info& lhs, const sensor_info& rhs) -{ +bool operator==(const sensor_info& lhs, const sensor_info& rhs) { return (lhs.name == rhs.name && lhs.sn == rhs.sn && lhs.fw_rev == rhs.fw_rev && lhs.mode == rhs.mode && lhs.prod_line == rhs.prod_line && lhs.format == rhs.format && lhs.beam_azimuth_angles == rhs.beam_azimuth_angles && lhs.beam_altitude_angles == rhs.beam_altitude_angles && - lhs.lidar_origin_to_beam_origin_mm == rhs.lidar_origin_to_beam_origin_mm && + lhs.lidar_origin_to_beam_origin_mm == + rhs.lidar_origin_to_beam_origin_mm && + lhs.beam_to_lidar_transform == rhs.beam_to_lidar_transform && lhs.imu_to_sensor_transform == rhs.imu_to_sensor_transform && lhs.lidar_to_sensor_transform == rhs.lidar_to_sensor_transform && - lhs.extrinsic == rhs.extrinsic); + lhs.extrinsic == rhs.extrinsic && lhs.init_id == rhs.init_id && + lhs.udp_port_lidar == rhs.udp_port_lidar && + lhs.udp_port_imu == rhs.udp_port_imu); } -bool operator!=(const sensor_info& lhs, const sensor_info& rhs) -{ +bool operator!=(const sensor_info& lhs, const sensor_info& rhs) { return !(lhs == rhs); } -bool operator==(const sensor_config& lhs, const sensor_config& rhs) -{ +bool operator==(const sensor_config& lhs, const sensor_config& rhs) { return (lhs.udp_dest == rhs.udp_dest && lhs.udp_port_lidar == rhs.udp_port_lidar && lhs.udp_port_imu == rhs.udp_port_imu && lhs.ts_mode == rhs.ts_mode && lhs.ld_mode == rhs.ld_mode && lhs.operating_mode == rhs.operating_mode && + lhs.multipurpose_io_mode == rhs.multipurpose_io_mode && lhs.azimuth_window == rhs.azimuth_window && lhs.signal_multiplier == rhs.signal_multiplier && - lhs.sync_pulse_out_angle == rhs.sync_pulse_out_angle && - lhs.sync_pulse_out_pulse_width == rhs.sync_pulse_out_pulse_width && lhs.nmea_in_polarity == rhs.nmea_in_polarity && - lhs.nmea_baud_rate == rhs.nmea_baud_rate && lhs.nmea_ignore_valid_char == rhs.nmea_ignore_valid_char && + lhs.nmea_baud_rate == rhs.nmea_baud_rate && lhs.nmea_leap_seconds == rhs.nmea_leap_seconds && - lhs.multipurpose_io_mode == rhs.multipurpose_io_mode && lhs.sync_pulse_in_polarity == rhs.sync_pulse_in_polarity && lhs.sync_pulse_out_polarity == rhs.sync_pulse_out_polarity && + lhs.sync_pulse_out_angle == rhs.sync_pulse_out_angle && + lhs.sync_pulse_out_pulse_width == rhs.sync_pulse_out_pulse_width && lhs.sync_pulse_out_frequency == rhs.sync_pulse_out_frequency && lhs.phase_lock_enable == rhs.phase_lock_enable && - lhs.phase_lock_offset == rhs.phase_lock_offset); + lhs.phase_lock_offset == rhs.phase_lock_offset && + lhs.columns_per_packet == rhs.columns_per_packet && + lhs.udp_profile_lidar == rhs.udp_profile_lidar && + lhs.udp_profile_imu == rhs.udp_profile_imu); } -bool operator!=(const sensor_config& lhs, const sensor_config& rhs) -{ +bool operator!=(const sensor_config& lhs, const sensor_config& rhs) { return !(lhs == rhs); } @@ -175,16 +231,17 @@ data_format default_data_format(lidar_mode mode) throw std::invalid_argument{"default_data_format"}; } - return { - pixels_per_column, - columns_per_packet, - columns_per_frame, - offset, - column_window - }; + return {pixels_per_column, + columns_per_packet, + columns_per_frame, + offset, + column_window, + UDPProfileLidar::PROFILE_LIDAR_LEGACY, + UDPProfileIMU::PROFILE_IMU_LEGACY, + static_cast(frequency_of_lidar_mode(mode))}; } -static double default_lidar_origin_to_beam_origin(std::string prod_line) +static double default_lidar_origin_to_beam_origin(const std::string& prod_line) { double lidar_origin_to_beam_origin_mm = 12.163; // default for gen 1 if (prod_line.find("OS-0-") == 0) @@ -202,71 +259,65 @@ static double default_lidar_origin_to_beam_origin(std::string prod_line) return lidar_origin_to_beam_origin_mm; } -sensor_info default_sensor_info(lidar_mode mode) -{ - return sensor::sensor_info{ - "UNKNOWN", - "000000000000", - "UNKNOWN", - mode, - "OS-1-64", - default_data_format(mode), - gen1_azimuth_angles, - gen1_altitude_angles, - default_lidar_origin_to_beam_origin("OS-1-64"), - default_imu_to_sensor_transform, - default_lidar_to_sensor_transform, - mat4d::Identity()}; -} - -constexpr packet_format packet_1_13 = impl::packet_2_0<64>(); -constexpr packet_format packet_2_0_16 = impl::packet_2_0<16>(); -constexpr packet_format packet_2_0_32 = impl::packet_2_0<32>(); -constexpr packet_format packet_2_0_64 = impl::packet_2_0<64>(); -constexpr packet_format packet_2_0_128 = impl::packet_2_0<128>(); - -const packet_format & get_format(const sensor_info & info) -{ - switch (info.format.pixels_per_column) { - case 16: - return packet_2_0_16; - case 32: - return packet_2_0_32; - case 64: - return packet_2_0_64; - case 128: - return packet_2_0_128; - default: - return packet_1_13; - } +mat4d default_beam_to_lidar_transform(std::string prod_line) { + mat4d beam_to_lidar_transform = mat4d::Identity(); + beam_to_lidar_transform(0, 3) = + default_lidar_origin_to_beam_origin(std::move(prod_line)); + return beam_to_lidar_transform; } -std::string to_string(lidar_mode mode) -{ - auto end = lidar_mode_strings.end(); - auto res = std::find_if( - lidar_mode_strings.begin(), end, - [&](const std::pair & p) { - return p.first == mode; - }); +sensor_info default_sensor_info(lidar_mode mode) { + return sensor::sensor_info{"UNKNOWN", + "000000000000", + "UNKNOWN", + mode, + "OS-1-64", + default_data_format(mode), + gen1_azimuth_angles, + gen1_altitude_angles, + default_lidar_origin_to_beam_origin("OS-1-64"), + default_beam_to_lidar_transform("OS-1-64"), + default_imu_to_sensor_transform, + default_lidar_to_sensor_transform, + mat4d::Identity(), + 0, + 0, + 0}; +} + +template +static optional lookup(const Table table, const K& k) { + auto end = table.end(); + auto res = std::find_if(table.begin(), end, [&](const std::pair& p) { + return p.first == k; + }); - return res == end ? "UNKNOWN" : res->second; + return res == end ? nullopt : make_optional(res->second); } -lidar_mode lidar_mode_of_string(const std::string & s) -{ - auto end = lidar_mode_strings.end(); - auto res = std::find_if( - lidar_mode_strings.begin(), end, - [&](const std::pair & p) { - return p.second == s; - }); +template +static optional rlookup(const Table table, + const char* v) { + auto end = table.end(); + auto res = std::find_if(table.begin(), end, + [&](const std::pair& p) { + return std::strcmp(p.second, v) == 0; + }); - return res == end ? lidar_mode(0) : res->first; + return res == end ? nullopt : make_optional(res->first); } -uint32_t n_cols_of_lidar_mode(lidar_mode mode) -{ +std::string to_string(lidar_mode mode) { + auto res = lookup(lidar_mode_strings, mode); + return res ? res.value() : "UNKNOWN"; +} + +lidar_mode lidar_mode_of_string(const std::string& s) { + auto res = rlookup(lidar_mode_strings, s.c_str()); + return res ? res.value() : lidar_mode::MODE_UNSPEC; +} + +uint32_t n_cols_of_lidar_mode(lidar_mode mode) { switch (mode) { case MODE_512x10: case MODE_512x20: @@ -276,14 +327,17 @@ uint32_t n_cols_of_lidar_mode(lidar_mode mode) return 1024; case MODE_2048x10: return 2048; + case MODE_4096x5: + return 4096; default: throw std::invalid_argument{"n_cols_of_lidar_mode"}; } } -int frequency_of_lidar_mode(lidar_mode mode) -{ +int frequency_of_lidar_mode(lidar_mode mode) { switch (mode) { + case MODE_4096x5: + return 5; case MODE_512x10: case MODE_1024x10: case MODE_2048x10: @@ -296,287 +350,287 @@ int frequency_of_lidar_mode(lidar_mode mode) } } -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; - }); +std::string to_string(timestamp_mode mode) { + auto res = lookup(timestamp_mode_strings, mode); + return res ? res.value() : "UNKNOWN"; +} - return res == end ? "UNKNOWN" : res->second; +timestamp_mode timestamp_mode_of_string(const std::string& s) { + auto res = rlookup(timestamp_mode_strings, s.c_str()); + return res ? res.value() : timestamp_mode::TIME_FROM_UNSPEC; } -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; - }); +std::string to_string(OperatingMode mode) { + auto res = lookup(operating_mode_strings, mode); + return res ? res.value() : "UNKNOWN"; +} - return res == end ? timestamp_mode(0) : res->first; +optional operating_mode_of_string(const std::string& s) { + return rlookup(operating_mode_strings, s.c_str()); } -std::string to_string(OperatingMode mode) -{ - auto end = operating_mode_strings.end(); - auto res = std::find_if( - operating_mode_strings.begin(), - end, - [&](const std::pair& p) {return p.first == mode;} - ); +std::string to_string(MultipurposeIOMode mode) { + auto res = lookup(multipurpose_io_mode_strings, mode); + return res ? res.value() : "UNKNOWN"; +} - return res == end ? "UNKNOWN" : res->second; +optional multipurpose_io_mode_of_string( + const std::string& s) { + return rlookup(multipurpose_io_mode_strings, s.c_str()); } -optional operating_mode_of_string(const std::string& s) -{ - auto end = operating_mode_strings.end(); - auto res = std::find_if( - operating_mode_strings.begin(), - end, - [&](const std::pair& p) {return p.second == s;} - ); +std::string to_string(Polarity polarity) { + auto res = lookup(polarity_strings, polarity); + return res ? res.value() : "UNKNOWN"; +} - return res == end ? nullopt : make_optional(res->first); +optional polarity_of_string(const std::string& s) { + return rlookup(polarity_strings, s.c_str()); } -std::string to_string(MultipurposeIOMode mode) -{ - auto end = multipurpose_io_mode_strings.end(); - auto res = std::find_if( - multipurpose_io_mode_strings.begin(), - end, - [&](const std::pair& p) {return p.first == mode;} - ); +std::string to_string(NMEABaudRate rate) { + auto res = lookup(nmea_baud_rate_strings, rate); + return res ? res.value() : "UNKNOWN"; +} - return res == end ? "UNKNOWN" : res->second; +optional nmea_baud_rate_of_string(const std::string& s) { + return rlookup(nmea_baud_rate_strings, s.c_str()); } -optional multipurpose_io_mode_of_string( - const std::string& s) -{ - auto end = multipurpose_io_mode_strings.end(); - auto res = std::find_if( - multipurpose_io_mode_strings.begin(), - end, - [&](const std::pair& p) {return p.second == s;} - ); +std::string to_string(AzimuthWindow azimuth_window) { + std::stringstream ss; + ss << "[" << azimuth_window.first << ", " << azimuth_window.second << "]"; + return ss.str(); +} - return res == end ? nullopt : make_optional(res->first); +std::string to_string(ChanField field) { + auto res = lookup(chanfield_strings, field); + return res ? res.value() : "UNKNOWN"; } -std::string to_string(Polarity polarity) +bool operator==(const AzimuthWindow& lhs, const AzimuthWindow& rhs) { - auto end = polarity_strings.end(); - auto res = std::find_if( - polarity_strings.begin(), - end, - [&](const std::pair& p) {return p.first == polarity;} - ); + return (lhs.first == rhs.first && lhs.second == rhs.second); +} - return res == end ? "UNKNOWN" : res->second; +std::string to_string(ChanFieldType ft) { + switch (ft) { + case sensor::ChanFieldType::VOID: + return "VOID"; + case sensor::ChanFieldType::UINT8: + return "UINT8"; + case sensor::ChanFieldType::UINT16: + return "UINT16"; + case sensor::ChanFieldType::UINT32: + return "UINT32"; + case sensor::ChanFieldType::UINT64: + return "UINT64"; + default: + return "UNKNOWN"; + } } -optional polarity_of_string(const std::string& s) -{ - auto end = polarity_strings.end(); - auto res = std::find_if( - polarity_strings.begin(), - end, - [&](const std::pair& p) {return p.second == s;} - ); +size_t field_type_size(ChanFieldType ft) { + switch (ft) { + case sensor::ChanFieldType::UINT8: + return 1; + case sensor::ChanFieldType::UINT16: + return 2; + case sensor::ChanFieldType::UINT32: + return 4; + case sensor::ChanFieldType::UINT64: + return 8; + default: + return 0; + } +} - return res == end ? nullopt : make_optional(res->first); +std::string to_string(UDPProfileLidar profile) { + auto res = lookup(udp_profile_lidar_strings, profile); + return res ? res.value() : "UNKNOWN"; } -std::string to_string(NMEABaudRate rate) -{ - auto end = nmea_baud_rate_strings.end(); - auto res = std::find_if( - nmea_baud_rate_strings.begin(), - end, - [&](const std::pair& p) {return p.first == rate;} - ); +optional udp_profile_lidar_of_string(const std::string& s) { + return rlookup(udp_profile_lidar_strings, s.c_str()); +} - return res == end ? "UNKNOWN" : res->second; +std::string to_string(UDPProfileIMU profile) { + auto res = lookup(udp_profile_imu_strings, profile); + return res ? res.value() : "UNKNOWN"; } -optional nmea_baud_rate_of_string(const std::string& s) -{ - auto end = nmea_baud_rate_strings.end(); - auto res = std::find_if( - nmea_baud_rate_strings.begin(), - end, - [&](const std::pair& p) {return p.second == s;} - ); +optional udp_profile_imu_of_string(const std::string& s) { + return rlookup(udp_profile_imu_strings, s.c_str()); +} - return res == end ? nullopt : make_optional(res->first); +std::string to_string(ShotLimitingStatus shot_limiting_status) { + auto res = lookup(shot_limiting_status_strings, shot_limiting_status); + return res ? res.value() : "UNKNOWN"; } -std::string to_string(AzimuthWindow azimuth_window) -{ - std::stringstream ss; - ss << "[" << azimuth_window.first << ", " << azimuth_window.second << "]"; - return ss.str(); +std::string to_string(ThermalShutdownStatus thermal_shutdown_status) { + auto res = + lookup(thermal_shutdown_status_strings, thermal_shutdown_status); + return res ? res.value() : "UNKNOWN"; } -bool operator==(const AzimuthWindow& lhs, const AzimuthWindow& rhs) -{ - return (lhs.first == rhs.first && lhs.second == rhs.second); +void check_signal_multiplier(const double signal_multiplier) { + std::string signal_multiplier_error = + "Provided signal multiplier is invalid: " + + std::to_string(signal_multiplier) + + " cannot be converted to one of [0.25, 0.5, 1, 2, 3]"; + + std::set valid_values = {0.25, 0.5, 1, 2, 3}; + if (!valid_values.count(signal_multiplier)) { + throw std::runtime_error(signal_multiplier_error); + } } -sensor_config parse_config(const Json::Value& root) -{ +static sensor_config parse_config(const Json::Value& root) { sensor_config config{}; - if (!root["udp_dest"].empty()) + if (!root["udp_dest"].empty()) { config.udp_dest = root["udp_dest"].asString(); + } else if (!root["udp_ip"].empty()) { + // deprecated params from FW 1.13. Set FW 2.0+ configs appropriately + config.udp_dest = root["udp_ip"].asString(); + //TODO(logger): consider using the spdlog library as well +// logger().warn( +// "Please note that udp_ip has been deprecated in favor " +// "of udp_dest. Will set udp_dest appropriately..."); + } + std::cerr << "Please note that udp_ip has been deprecated in favor " + "of udp_dest. Will set udp_dest appropriately..." << std::endl; + if (!root["udp_port_lidar"].empty()) config.udp_port_lidar = root["udp_port_lidar"].asInt(); if (!root["udp_port_imu"].empty()) config.udp_port_imu = root["udp_port_imu"].asInt(); if (!root["timestamp_mode"].empty()) config.ts_mode = - timestamp_mode_of_string(root["timestamp_mode"].asString()); + timestamp_mode_of_string(root["timestamp_mode"].asString()); if (!root["lidar_mode"].empty()) config.ld_mode = lidar_mode_of_string(root["lidar_mode"].asString()); + if (!root["azimuth_window"].empty()) config.azimuth_window = - std::make_pair(root["azimuth_window"][0].asInt(), - root["azimuth_window"][1].asInt()); + std::make_pair(root["azimuth_window"][0].asInt(), + root["azimuth_window"][1].asInt()); - if (!root["signal_multiplier"].empty()) - config.signal_multiplier = root["signal_multiplier"].asInt(); + if (!root["signal_multiplier"].empty()) { + double signal_multiplier = root["signal_multiplier"].asDouble(); + check_signal_multiplier(signal_multiplier); + config.signal_multiplier = signal_multiplier; + } - if (!root["operating_mode"].empty()) - { + if (!root["operating_mode"].empty()) { auto operating_mode = - operating_mode_of_string(root["operating_mode"].asString()); - if (operating_mode) - { + operating_mode_of_string(root["operating_mode"].asString()); + if (operating_mode) { config.operating_mode = operating_mode; - } - else - { - throw std::invalid_argument("Unexpected Operating Mode"); + } else { + throw std::runtime_error{"Unexpected Operating Mode"}; } + } else if (!root["auto_start_flag"].empty()) { +// logger().warn( +// "Please note that auto_start_flag has been deprecated in favor " +// "of operating_mode. Will set operating_mode appropriately..."); + std::cerr << "Please note that auto_start_flag has been deprecated in favor " + "of operating_mode. Will set operating_mode appropriately..." << std::endl; + config.operating_mode = root["auto_start_flag"].asBool() + ? sensor::OPERATING_NORMAL + : sensor::OPERATING_STANDBY; } - if (!root["multipurpose_io_mode"].empty()) - { + if (!root["multipurpose_io_mode"].empty()) { auto multipurpose_io_mode = multipurpose_io_mode_of_string( - root["multipurpose_io_mode"].asString()); - if (multipurpose_io_mode) - { + root["multipurpose_io_mode"].asString()); + if (multipurpose_io_mode) { config.multipurpose_io_mode = multipurpose_io_mode; - } - else - { - throw std::invalid_argument("Unexpected Multipurpose IO Mode"); + } else { + throw std::runtime_error{"Unexpected Multipurpose IO Mode"}; } } - if (!root["sync_pulse_out_angle"].empty()) config.sync_pulse_out_angle = root["sync_pulse_out_angle"].asInt(); if (!root["sync_pulse_out_pulse_width"].empty()) - config.sync_pulse_out_pulse_width = - root["sync_pulse_out_pulse_width"].asInt(); + config.sync_pulse_out_pulse_width = + root["sync_pulse_out_pulse_width"].asInt(); - if (!root["nmea_in_polarity"].empty()) - { + if (!root["nmea_in_polarity"].empty()) { auto nmea_in_polarity = - polarity_of_string(root["nmea_in_polarity"].asString()); - if (nmea_in_polarity) - { + polarity_of_string(root["nmea_in_polarity"].asString()); + if (nmea_in_polarity) { config.nmea_in_polarity = nmea_in_polarity; - } - else - { - throw std::invalid_argument("Unexpected NMEA Input Polarity"); + } else { + throw std::runtime_error{"Unexpected NMEA Input Polarity"}; } } - - if (!root["nmea_baud_rate"].empty()) - { + if (!root["nmea_baud_rate"].empty()) { auto nmea_baud_rate = - nmea_baud_rate_of_string(root["nmea_baud_rate"].asString()); - if (nmea_baud_rate) - { + nmea_baud_rate_of_string(root["nmea_baud_rate"].asString()); + if (nmea_baud_rate) { config.nmea_baud_rate = nmea_baud_rate; - } - else - { - throw std::invalid_argument("Unexpected NMEA Baud Rate"); + } else { + throw std::runtime_error{"Unexpected NMEA Baud Rate"}; } } - if (!root["nmea_ignore_valid_char"].empty()) config.nmea_ignore_valid_char = root["nmea_ignore_valid_char"].asBool(); if (!root["nmea_leap_seconds"].empty()) config.nmea_leap_seconds = root["nmea_leap_seconds"].asInt(); - if (!root["sync_pulse_in_polarity"].empty()) - { + if (!root["sync_pulse_in_polarity"].empty()) { auto sync_pulse_in_polarity = - polarity_of_string(root["sync_pulse_in_polarity"].asString()); - if (sync_pulse_in_polarity) - { + polarity_of_string(root["sync_pulse_in_polarity"].asString()); + if (sync_pulse_in_polarity) { config.sync_pulse_in_polarity = sync_pulse_in_polarity; - } - else - { - throw std::invalid_argument("Unexpected Sync Pulse Input Polarity"); + } else { + throw std::runtime_error{"Unexpected Sync Pulse Input Polarity"}; } } - - if (!root["sync_pulse_out_polarity"].empty()) - { + if (!root["sync_pulse_out_polarity"].empty()) { auto sync_pulse_out_polarity = - polarity_of_string(root["sync_pulse_out_polarity"].asString()); - if (sync_pulse_out_polarity) - { + polarity_of_string(root["sync_pulse_out_polarity"].asString()); + if (sync_pulse_out_polarity) { config.sync_pulse_out_polarity = sync_pulse_out_polarity; - } - else - { - throw std::invalid_argument("Unexpected Sync Pulse Output Polarity"); + } else { + throw std::runtime_error{"Unexpected Sync Pulse Output Polarity"}; } } - if (!root["sync_pulse_out_frequency"].empty()) config.sync_pulse_out_frequency = - root["sync_pulse_out_frequency"].asInt(); + root["sync_pulse_out_frequency"].asInt(); + if (!root["phase_lock_enable"].empty()) - config.phase_lock_enable = - root["phase_lock_enable"].asString() == "true" ? true : false; + config.phase_lock_enable = root["phase_lock_enable"].asString() == "true"; + if (!root["phase_lock_offset"].empty()) config.phase_lock_offset = root["phase_lock_offset"].asInt(); - // deprecated params from 1.13. set 2.0 configs appropriately - if (!root["udp_dest"].empty()) config.udp_dest = root["udp_dest"].asString(); - if (!root["operating_mode"].empty()) - config.operating_mode = root["operating_mode"].asBool() - ? sensor::OPERATING_NORMAL - : sensor::OPERATING_STANDBY; + if (!root["columns_per_packet"].empty()) + config.columns_per_packet = root["columns_per_packet"].asInt(); + // udp_profiles + if (!root["udp_profile_lidar"].empty()) + config.udp_profile_lidar = + udp_profile_lidar_of_string(root["udp_profile_lidar"].asString()); + + if (!root["udp_profile_imu"].empty()) + config.udp_profile_imu = + udp_profile_imu_of_string(root["udp_profile_imu"].asString()); return config; } -sensor_config parse_config(const std::string& meta) +sensor_config parse_config(const std::string& meta) { Json::Value root{}; Json::CharReaderBuilder builder{}; std::string errors{}; std::stringstream ss{meta}; - if (meta.size()) + if (!meta.empty()) { if (!Json::parseFromStream(builder, ss, &root, &errors)) { @@ -587,199 +641,563 @@ sensor_config parse_config(const std::string& meta) return parse_config(root); } -sensor_info parse_metadata(const std::string & meta) -{ +// bool represents whether it is an object (true) or just a member (false) +// NOTE: lidar_data_format and calibration_status should be objects but as they +// were introduced earlier, non-legacy formats for FW version do not include +// them +const std::map nonlegacy_metadata_fields = { + {"sensor_info", true}, {"beam_intrinsics", true}, + {"imu_intrinsics", true}, {"lidar_intrinsics", true}, + {"config_params", true}, {"lidar_data_format", false}, + {"calibration_status", false}}; + +static bool is_new_format(const std::string& metadata) { + Json::Value root{}; + Json::CharReaderBuilder builder{}; + std::string errors{}; + std::stringstream ss{metadata}; + + if (!metadata.empty()) { + if (!Json::parseFromStream(builder, ss, &root, &errors)) + throw std::runtime_error{ + "Error parsing metadata when checking format: " + errors}; + } + + size_t nonlegacy_fields_present = 0; + std::string missing_fields; + for (const auto& field_pair : nonlegacy_metadata_fields) { + auto field = field_pair.first; + auto is_obj = field_pair.second; + if (root.isMember(field)) { + nonlegacy_fields_present++; + if (is_obj && !root[field].isObject()) { + throw std::runtime_error{"Non-legacy metadata field " + field + + " must have child fields"}; + } + } else { + missing_fields += field + " "; + } + } + + if (nonlegacy_fields_present > 0 && + nonlegacy_fields_present < nonlegacy_metadata_fields.size()) { + throw std::runtime_error{"Non-legacy metadata must include fields: " + + missing_fields}; + } + + return nonlegacy_fields_present == nonlegacy_metadata_fields.size(); +} + +static data_format parse_data_format(const Json::Value& root) { + const std::vector data_format_required_fields{ + "pixels_per_column", "columns_per_packet", "columns_per_frame"}; + + for (const auto& field : data_format_required_fields) { + if (!root.isMember(field)) { + throw std::runtime_error{ + "Metadata field data_format must include field: " + field}; + } + } + data_format format; + + format.pixels_per_column = root["pixels_per_column"].asInt(); + format.columns_per_packet = root["columns_per_packet"].asInt(); + format.columns_per_frame = root["columns_per_frame"].asInt(); + + if (root.isMember("pixel_shift_by_row")) { + if (root["pixel_shift_by_row"].size() != format.pixels_per_column) { + throw std::runtime_error{"Unexpected number of pixel_shift_by_row"}; + } + + for (const auto& v : root["pixel_shift_by_row"]) + format.pixel_shift_by_row.push_back(v.asInt()); + } else { + // DF path + format.pixel_shift_by_row.assign(format.pixels_per_column, 0); + } + + if (root.isMember("column_window")) { + if (root["column_window"].size() != 2) { + throw std::runtime_error{"Unexpected size of column_window tuple"}; + } + format.column_window.first = root["column_window"][0].asInt(); + format.column_window.second = root["column_window"][1].asInt(); + } else { +// logger().warn( +// "No column window found. Using default column window (full)"); + std::cerr << "No column window found. Using default column window (full)" << std::endl; + format.column_window = default_column_window(format.columns_per_frame); + } + + if (root.isMember("udp_profile_lidar")) { + // initializing directly triggers -Wmaybe-uninitialized + // GCC 8.3.1 + optional profile{nullopt}; + profile = + udp_profile_lidar_of_string(root["udp_profile_lidar"].asString()); + if (profile) { + format.udp_profile_lidar = profile.value(); + } else { + throw std::runtime_error{"Unexpected udp lidar profile"}; + } + } else { +// logger().warn("No lidar profile found. Using LEGACY lidar profile"); + std::cerr << "No lidar profile found. Using LEGACY lidar profile" << std::endl; + format.udp_profile_lidar = PROFILE_LIDAR_LEGACY; + } + + if (root.isMember("udp_profile_imu")) { + optional profile{nullopt}; + profile = udp_profile_imu_of_string(root["udp_profile_imu"].asString()); + if (profile) { + format.udp_profile_imu = profile.value(); + } else { + throw std::runtime_error{"Unexpected udp imu profile"}; + } + } else { +// logger().warn("No imu profile found. Using LEGACY imu profile"); + std::cerr << "No imu profile found. Using LEGACY imu profile" << std::endl; + format.udp_profile_imu = PROFILE_IMU_LEGACY; + } + + if (root.isMember("fps")) { + format.fps = root["fps"].asInt(); + } else { + // logger().warn("No fps found. Trying to use one from lidar mode (or + // 0)"); + format.fps = 0; + } + + return format; +} // namespace sensor + +static sensor_info parse_legacy(const std::string& meta) { Json::Value root{}; Json::CharReaderBuilder builder{}; std::string errors{}; std::stringstream ss{meta}; - if (meta.size()) - { - if (!Json::parseFromStream(builder, ss, &root, &errors)) - { - throw std::invalid_argument{"Unexpected number of pixel_shift_by_row"}; + if (meta.size()) { + if (!Json::parseFromStream(builder, ss, &root, &errors)) + throw std::runtime_error{errors}; + } + + // NOTE[pb]: DF development metadata.json doesn't have beam_altitude_angles + // and beam_azimuth_angles and instead provides beam_xyz. However + // final implementation should have azimuth/altitude angles and + // we may uncomment the validation back closer to the release. + // const std::vector minimum_legacy_metadata_fields{ + // "beam_altitude_angles", "beam_azimuth_angles", "lidar_mode"}; + + // NOTE[pb]: DF metadata doesn't have lidar_mode, but because it's + // going through convert_to_legacy() function it get's the empty + // string lidar_mode ... hmmm, fine for now... + const std::vector minimum_legacy_metadata_fields{"lidar_mode"}; + + for (auto field : minimum_legacy_metadata_fields) { + if (!root.isMember(field)) { + throw std::runtime_error{"Metadata must contain: " + field}; + } + } + + // nice to have fields which we will use defaults for if they don't + // exist + const std::vector desired_legacy_metadata_fields{ + "imu_to_sensor_transform", "lidar_to_sensor_transform", "prod_line", + "prod_sn", "build_rev"}; + for (auto field : desired_legacy_metadata_fields) { + if (!root.isMember(field)) { +// logger().warn("No " + field + +// " found in metadata. Will be left blank or filled in " +// "with default legacy values"); + std::cerr << "No " << field << + " found in metadata. Will be left blank or filled in " + "with default legacy values" << std::endl; + } + } + + // fields that don't survive round trip through to_string + const std::vector not_parsed_metadata_fields{ + "build_date", "image_rev", "prod_pn", "status"}; + for (auto field : not_parsed_metadata_fields) { + if (!root.isMember(field)) { +// logger().warn( +// "No " + field + +// " found in metadata. Your metadata may be the result " +// "of calling to_string() on the sensor_info object OR " +// "you recorded this data with a very old version of " +// "Ouster Studio. We advise you to record the metadata " +// "directly " +// "with get_metadata and to update your Ouster Studio."); + std::cerr << "No " << field << + " found in metadata. Your metadata may be the result " + "of calling to_string() on the sensor_info object OR " + "you recorded this data with a very old version of " + "Ouster Studio. We advise you to record the metadata " + "directly " + "with get_metadata and to update your Ouster Studio." << std::endl; } } sensor_info info{}; + // info.name is deprecated, will be empty string if not present info.name = root["hostname"].asString(); + + // if these are not present they are also empty strings info.sn = root["prod_sn"].asString(); + info.prod_line = root["prod_line"].asString(); info.fw_rev = root["build_rev"].asString(); + + // checked that lidar_mode is present already - never empty string info.mode = lidar_mode_of_string(root["lidar_mode"].asString()); - info.prod_line = root["prod_line"].asString(); // "data_format" introduced in fw 2.0. Fall back to 1.13 - if (root.isMember("data_format")) - { - info.format.pixels_per_column = - root["data_format"]["pixels_per_column"].asInt(); - info.format.columns_per_packet = - root["data_format"]["columns_per_packet"].asInt(); - info.format.columns_per_frame = - root["data_format"]["columns_per_frame"].asInt(); - - if (root["data_format"]["pixel_shift_by_row"].size() != - info.format.pixels_per_column) - { - throw std::runtime_error{"Unexpected number of pixel_shift_by_row"}; - } - - for (const auto & v : root["data_format"]["pixel_shift_by_row"]) - { - info.format.pixel_shift_by_row.push_back(v.asInt()); + if (root.isMember("data_format")) { + info.format = parse_data_format(root["data_format"]); + // data_format.fps was added for DF sensors, so we are backfilling + // fps value for OS sensors here if it's not present in metadata + if (info.format.fps == 0) { + info.format.fps = frequency_of_lidar_mode(info.mode); } - } - else - { - std::cerr << "WARNING: No data_format found." << std::endl; + } else { +// logger().warn("No data_format found. Using default legacy data format"); + std::cerr << "No data_format found. Using default legacy data format" << std::endl; info.format = default_data_format(info.mode); } - // "lidar_origin_to_beam_origin_mm" introduced in fw 2.0. Fall back to 1.13 - if (root.isMember("lidar_origin_to_beam_origin_mm")) - { + // "lidar_origin_to_beam_origin_mm" introduced in fw 2.0 BUT missing + // on OS-DOME. Handle falling back to FW 1.13 or setting to 0 + // according to prod-line + if (root.isMember("lidar_origin_to_beam_origin_mm")) { info.lidar_origin_to_beam_origin_mm = - root["lidar_origin_to_beam_origin_mm"].asDouble(); - } - else - { - std::cerr << "WARNING: No lidar_origin_to_beam_origin_mm found." << - std::endl; - info.lidar_origin_to_beam_origin_mm = - default_lidar_origin_to_beam_origin(info.prod_line); + root["lidar_origin_to_beam_origin_mm"].asDouble(); + } else { + if (info.prod_line.find("OS-DOME-") == + 0) { // is an OS-DOME - fill with 0 + info.lidar_origin_to_beam_origin_mm = 0; + } else { // not an OS-DOME +// logger().warn( +// "No lidar_origin_to_beam_origin_mm found. Using default " +// "value for the specified prod_line or default gen 1 values" +// "if prod_line is missing"); + std::cerr << "No lidar_origin_to_beam_origin_mm found. Using default " + "value for the specified prod_line or default gen 1 values" + "if prod_line is missing" << std::endl; + info.lidar_origin_to_beam_origin_mm = + default_lidar_origin_to_beam_origin( + info.prod_line); // note it is possible that + // info.prod_line is "" + } } - if (root["beam_altitude_angles"].size() != info.format.pixels_per_column) - { - throw std::invalid_argument{"Unexpected number of beam_altitude_angles"}; + // beam_to_lidar_transform" introduced in fw 2.5/fw 3.0 + if (root.isMember("beam_to_lidar_transform")) { + for (int i = 0; i < 4; i++) { + for (int j = 0; j < 4; j++) { + const Json::Value::ArrayIndex ind = i * 4 + j; + info.beam_to_lidar_transform(i, j) = + root["beam_to_lidar_transform"][ind].asDouble(); + } + } + } else { + // fw is < 2.5/3.0 and we need to manually fill it in + info.beam_to_lidar_transform = mat4d::Identity(); + info.beam_to_lidar_transform(0, 3) = + info.lidar_origin_to_beam_origin_mm; } - if (root["beam_azimuth_angles"].size() != info.format.pixels_per_column) - { - throw std::invalid_argument{"Unexpected number of beam_azimuth_angles"}; + if (root["beam_altitude_angles"].size() != 0 && + root["beam_altitude_angles"].size() != info.format.pixels_per_column) { + throw std::runtime_error{"Unexpected number of beam_altitude_angles"}; } - for (const auto & v : root["beam_altitude_angles"]) - { - info.beam_altitude_angles.push_back(v.asDouble()); + if (root["beam_azimuth_angles"].size() != 0 && + root["beam_azimuth_angles"].size() != info.format.pixels_per_column) { + throw std::runtime_error{"Unexpected number of beam_azimuth_angles"}; } - for (const auto & v : root["beam_azimuth_angles"]) - { - info.beam_azimuth_angles.push_back(v.asDouble()); + if (root["beam_altitude_angles"].size() == info.format.pixels_per_column) { + if (root["beam_altitude_angles"][0].isArray()) { + // DF sensor path + for (const auto& row : root["beam_altitude_angles"]) + for (const auto& v : row) + info.beam_altitude_angles.push_back(v.asDouble()); + + if (info.beam_altitude_angles.size() != + info.format.pixels_per_column * info.format.columns_per_frame) { + throw std::runtime_error{ + "Unexpected number of total beam_altitude_angles"}; + } + } else { + // OS sensor path + for (const auto& v : root["beam_altitude_angles"]) + info.beam_altitude_angles.push_back(v.asDouble()); + } + } + + if (root["beam_azimuth_angles"].size() == info.format.pixels_per_column) { + if (root["beam_azimuth_angles"][0].isArray()) { + // DF sensor path + for (const auto& row : root["beam_azimuth_angles"]) { + for (const auto& v : row) + info.beam_azimuth_angles.push_back(v.asDouble()); + } + + if (info.beam_azimuth_angles.size() != + info.format.pixels_per_column * info.format.columns_per_frame) { + throw std::runtime_error{ + "Unexpected number of total beam_azimuth_angles"}; + } + } else { + // OS sensor path + for (const auto& v : root["beam_azimuth_angles"]) + info.beam_azimuth_angles.push_back(v.asDouble()); + } + } + + // NOTE[pb]: this block that handles beam_xyz shouldn't survive past + // the DF development phase and we need to swith to azimuth/altitude + // angles in the metadata, because they take less space and they + // are less redundant configuration of intrinsics than unit xyz vectors + if (info.beam_altitude_angles.empty() && info.beam_azimuth_angles.empty()) { + if (root["beam_xyz"].size() != + 3 * info.format.pixels_per_column * info.format.columns_per_frame) { + throw std::runtime_error{"Unexpected number of beam_xyz"}; + } + + // DF sensor path + auto& xyz = root["beam_xyz"]; + for (Json::Value::ArrayIndex idx = 0; idx < xyz.size(); idx += 3) { + auto x = xyz[idx + 0].asDouble(); + auto y = xyz[idx + 1].asDouble(); + auto z = xyz[idx + 2].asDouble(); + auto al = std::atan2(z, sqrt(x * x + y * y)) * 180.0 / M_PI; + auto az = std::atan2(y, x) * 180.0 / M_PI; + info.beam_altitude_angles.push_back(al); + info.beam_azimuth_angles.push_back(az); + } } // "imu_to_sensor_transform" may be absent in sensor config // produced by Ouster Studio, so we backfill it with default value - if (root["imu_to_sensor_transform"].size() == 16) - { - for (int i = 0; i < 4; i++) - { - for (int j = 0; j < 4; j++) - { + if (root["imu_to_sensor_transform"].size() == 16) { + for (int i = 0; i < 4; i++) { + for (int j = 0; j < 4; j++) { const Json::Value::ArrayIndex ind = i * 4 + j; info.imu_to_sensor_transform(i, j) = - root["imu_to_sensor_transform"][ind].asDouble(); + root["imu_to_sensor_transform"][ind].asDouble(); } } - } - else - { - std::cerr << "WARNING: No valid imu_to_sensor_transform found." << - std::endl; + } else { +// logger().warn( +// "No valid imu_to_sensor_transform found. Using default for gen " +// "1"); + std::cerr << "No valid imu_to_sensor_transform found. Using default for gen " + "1" << std::endl; info.imu_to_sensor_transform = default_imu_to_sensor_transform; } // "lidar_to_sensor_transform" may be absent in sensor config // produced by Ouster Studio, so we backfill it with default value - if (root["lidar_to_sensor_transform"].size() == 16) - { - for (int i = 0; i < 4; i++) - { - for (int j = 0; j < 4; j++) - { + if (root["lidar_to_sensor_transform"].size() == 16) { + for (int i = 0; i < 4; i++) { + for (int j = 0; j < 4; j++) { const Json::Value::ArrayIndex ind = i * 4 + j; info.lidar_to_sensor_transform(i, j) = - root["lidar_to_sensor_transform"][ind].asDouble(); + root["lidar_to_sensor_transform"][ind].asDouble(); } } - } - else - { - std::cerr << "WARNING: No valid lidar_to_sensor_transform found." << - std::endl; + } else { +// logger().warn( +// "No valid lidar_to_sensor_transform found. Using default for " +// "gen " +// "1"); + std::cerr << "No valid lidar_to_sensor_transform found. Using default for " + "gen " + "1" << std::endl; info.lidar_to_sensor_transform = default_lidar_to_sensor_transform; } + auto zero_check = [](auto el, const std::string &name) { + if (el.size() == 0) return; + bool all_zeros = std::all_of(el.cbegin(), el.cend(), + [](double k) { return k == 0.0; }); + if (all_zeros) { + throw std::runtime_error{"Field " + name + + " in the metadata cannot all be zeros."}; + } + }; + + zero_check(info.beam_altitude_angles, "beam_altitude_angles"); + zero_check(info.beam_azimuth_angles, "beam_azimuth_angles"); + info.extrinsic = mat4d::Identity(); + // default to 0 if keys are not present + info.init_id = root["initialization_id"].asInt(); + info.udp_port_lidar = root["udp_port_lidar"].asInt(); + info.udp_port_imu = root["udp_port_imu"].asInt(); + return info; } -sensor_info metadata_from_json(const std::string & json_file) -{ +static void update_json_obj(Json::Value& dst, const Json::Value& src) { + const std::vector& members = src.getMemberNames(); + for (const auto& key : members) { + dst[key] = src[key]; + } +} + +std::string convert_to_legacy(const std::string& metadata) { + if (!is_new_format(metadata)) + throw std::invalid_argument( + "Invalid non-legacy metadata format provided"); + + Json::Value root{}; + Json::CharReaderBuilder read_builder{}; + std::string errors{}; + std::stringstream ss{metadata}; + + if (!metadata.empty()) { + if (!Json::parseFromStream(read_builder, ss, &root, &errors)) { + throw std::runtime_error{ + "Errors parsing metadata for convert_to_legacy: " + errors}; + } + } + Json::Value result{}; + + if (root.isMember("config_params")) { + result["lidar_mode"] = root["config_params"]["lidar_mode"]; + result["udp_port_lidar"] = root["config_params"]["udp_port_lidar"]; + result["udp_port_imu"] = root["config_params"]["udp_port_imu"]; + } + if (root.isMember("client_version")) { + result["client_version"] = root["client_version"]; + } + result["json_calibration_version"] = FW_2_2; + + result["hostname"] = ""; + + update_json_obj(result, root["sensor_info"]); + update_json_obj(result, root["beam_intrinsics"]); + update_json_obj(result, root["imu_intrinsics"]); + update_json_obj(result, root["lidar_intrinsics"]); + + if (root.isMember("lidar_data_format") && + root["lidar_data_format"].isObject()) { + result["data_format"] = Json::Value{}; + update_json_obj(result["data_format"], root["lidar_data_format"]); + } + + Json::StreamWriterBuilder write_builder; + write_builder["enableYAMLCompatibility"] = true; + write_builder["precision"] = 6; + write_builder["indentation"] = " "; + return Json::writeString(write_builder, result); +} + +sensor_info parse_metadata(const std::string& metadata) { + Json::Value root{}; + Json::CharReaderBuilder builder{}; + std::string errors{}; + std::stringstream ss{metadata}; + + if (!metadata.empty()) { + if (!Json::parseFromStream(builder, ss, &root, &errors)) + throw std::runtime_error{ + "Errors parsing metadata for parse_metadata: " + errors}; + } + + sensor_info info{}; + if (is_new_format(metadata)) { +// logger().debug("parsing non-legacy metadata format"); + std::cerr << "parsing non-legacy metadata format" << std::endl; + info = parse_legacy(convert_to_legacy(metadata)); + } else { +// logger().debug("parsing legacy metadata format"); + std::cerr << "parsing legacy metadata format" << std::endl; + info = parse_legacy(metadata); + } + return info; +} + +sensor_info metadata_from_json(const std::string& json_file) { std::stringstream buf{}; std::ifstream ifs{}; ifs.open(json_file); buf << ifs.rdbuf(); ifs.close(); - if (!ifs) - { + if (!ifs) { std::stringstream ss; ss << "Failed to read metadata file: " << json_file; - throw std::runtime_error(ss.str()); + throw std::runtime_error{ss.str()}; } return parse_metadata(buf.str()); } -std::string to_string(const sensor_info & info) -{ +std::string to_string(const sensor_info& info) { Json::Value root{}; -// root["client_version"] = ouster::CLIENT_VERSION; - root["hostname"] = info.name; + +// root["client_version"] = client_version(); + root["hostname"] = ""; root["prod_sn"] = info.sn; root["build_rev"] = info.fw_rev; root["lidar_mode"] = to_string(info.mode); root["prod_line"] = info.prod_line; + root["data_format"]["pixels_per_column"] = info.format.pixels_per_column; root["data_format"]["columns_per_packet"] = info.format.columns_per_packet; root["data_format"]["columns_per_frame"] = info.format.columns_per_frame; - - for (auto i : info.format.pixel_shift_by_row) { + root["data_format"]["fps"] = info.format.fps; + for (auto i : info.format.pixel_shift_by_row) root["data_format"]["pixel_shift_by_row"].append(i); - } root["data_format"]["column_window"].append( - info.format.column_window.first); + info.format.column_window.first); root["data_format"]["column_window"].append( - info.format.column_window.second); + info.format.column_window.second); + + root["data_format"]["udp_profile_lidar"] = + to_string(info.format.udp_profile_lidar); + root["data_format"]["udp_profile_imu"] = + to_string(info.format.udp_profile_imu); + root["lidar_origin_to_beam_origin_mm"] = - info.lidar_origin_to_beam_origin_mm; + info.lidar_origin_to_beam_origin_mm; - for (auto i : info.beam_azimuth_angles) { - root["beam_azimuth_angles"].append(i); + for (size_t i = 0; i < 4; i++) { + for (size_t j = 0; j < 4; j++) { + root["beam_to_lidar_transform"].append( + info.beam_to_lidar_transform(i, j)); + } } - for (auto i : info.beam_altitude_angles) { + for (auto i : info.beam_azimuth_angles) + root["beam_azimuth_angles"].append(i); + for (auto i : info.beam_altitude_angles) root["beam_altitude_angles"].append(i); - } - for (size_t i = 0; i < 4; i++) - { - for (size_t j = 0; j < 4; j++) - { + for (size_t i = 0; i < 4; i++) { + for (size_t j = 0; j < 4; j++) { root["imu_to_sensor_transform"].append( - info.imu_to_sensor_transform(i, j)); + info.imu_to_sensor_transform(i, j)); + } + } + for (size_t i = 0; i < 4; i++) { + for (size_t j = 0; j < 4; j++) { root["lidar_to_sensor_transform"].append( - info.lidar_to_sensor_transform(i, j)); + info.lidar_to_sensor_transform(i, j)); } } - root["json_calibration_version"] = FW_2_0; + root["initialization_id"] = info.init_id; + root["udp_port_lidar"] = info.udp_port_lidar; + root["udp_port_imu"] = info.udp_port_imu; + + root["json_calibration_version"] = FW_2_2; Json::StreamWriterBuilder builder; builder["enableYAMLCompatibility"] = true; @@ -788,121 +1206,133 @@ std::string to_string(const sensor_info & info) return Json::writeString(builder, root); } -std::string to_string(const sensor_config& config) { - Json::Value root{}; +Json::Value to_json(const sensor_config &config) +{ + Json::Value root{Json::objectValue}; - if (config.udp_dest) - { - root["udp_dest"] = config.udp_dest.value(); - } + if (config.udp_dest) { root["udp_dest"] = config.udp_dest.value(); } - if (config.udp_port_lidar) - { - root["udp_port_lidar"] = config.udp_port_lidar.value(); - } + if (config.udp_port_lidar) { + root["udp_port_lidar"] = config.udp_port_lidar.value(); + } - if (config.udp_port_imu) - { - root["udp_port_imu"] = config.udp_port_imu.value(); - } + if (config.udp_port_imu) { + root["udp_port_imu"] = config.udp_port_imu.value(); + } - if (config.ts_mode) - { - root["timestamp_mode"] = to_string(config.ts_mode.value()); - } + if (config.ts_mode) { + root["timestamp_mode"] = to_string(config.ts_mode.value()); + } - if (config.ld_mode) - { - root["lidar_mode"] = to_string(config.ld_mode.value()); - } + if (config.ld_mode) { + root["lidar_mode"] = to_string(config.ld_mode.value()); + } - if (config.operating_mode) { - root["operating_mode"] = to_string(config.operating_mode.value()); - } + if (config.operating_mode) { + auto mode = config.operating_mode.value(); + root["operating_mode"] = to_string(mode); + } - if (config.multipurpose_io_mode) { - root["multipurpose_io_mode"] = - to_string(config.multipurpose_io_mode.value()); - } + if (config.multipurpose_io_mode) { + root["multipurpose_io_mode"] = + to_string(config.multipurpose_io_mode.value()); + } - if (config.azimuth_window) - { - Json::Value azimuth_window; - azimuth_window.append(config.azimuth_window.value().first); - azimuth_window.append(config.azimuth_window.value().second); - root["azimuth_window"] = azimuth_window; - } + if (config.azimuth_window) { + Json::Value azimuth_window; + azimuth_window.append(config.azimuth_window.value().first); + azimuth_window.append(config.azimuth_window.value().second); + root["azimuth_window"] = azimuth_window; + } - if (config.signal_multiplier) - { + if (config.signal_multiplier) { + check_signal_multiplier(config.signal_multiplier.value()); + if ((config.signal_multiplier == 0.25) || + (config.signal_multiplier == 0.5)) { root["signal_multiplier"] = config.signal_multiplier.value(); } - - if (config.sync_pulse_out_angle) - { - root["sync_pulse_out_angle"] = config.sync_pulse_out_angle.value(); + else { + // jsoncpp < 1.7.7 strips 0s off of exact representation + // so 2.0 becomes 2 + // On ubuntu 18.04, the default jsoncpp is 1.7.4-3 Fix was: + // https://github.com/open-source-parsers/jsoncpp/pull/547 + // Work around by always casting to int before writing out to json + int signal_multiplier_int = int(config.signal_multiplier.value()); + root["signal_multiplier"] = signal_multiplier_int; } + } - if (config.sync_pulse_out_pulse_width) - { - root["sync_pulse_out_pulse_width"] = - config.sync_pulse_out_pulse_width.value(); - } + if (config.sync_pulse_out_angle) { + root["sync_pulse_out_angle"] = config.sync_pulse_out_angle.value(); + } - if (config.nmea_in_polarity) - { - root["nmea_in_polarity"] = to_string(config.nmea_in_polarity.value()); - } + if (config.sync_pulse_out_pulse_width) { + root["sync_pulse_out_pulse_width"] = + config.sync_pulse_out_pulse_width.value(); + } - if (config.nmea_baud_rate) - { - root["nmea_baud_rate"] = to_string(config.nmea_baud_rate.value()); - } + if (config.nmea_in_polarity) { + root["nmea_in_polarity"] = to_string(config.nmea_in_polarity.value()); + } - if (config.nmea_ignore_valid_char) - { - root["nmea_ignore_valid_char"] = - config.nmea_ignore_valid_char.value() ? 1 : 0; - } + if (config.nmea_baud_rate) { + root["nmea_baud_rate"] = to_string(config.nmea_baud_rate.value()); + } - if (config.nmea_leap_seconds) - { - root["nmea_leap_seconds"] = config.nmea_leap_seconds.value(); - } + if (config.nmea_ignore_valid_char) { + root["nmea_ignore_valid_char"] = + config.nmea_ignore_valid_char.value() ? 1 : 0; + } - if (config.sync_pulse_in_polarity) - { - root["sync_pulse_in_polarity"] = - to_string(config.sync_pulse_in_polarity.value()); - } + if (config.nmea_leap_seconds) { + root["nmea_leap_seconds"] = config.nmea_leap_seconds.value(); + } - if (config.sync_pulse_out_polarity) - { - root["sync_pulse_out_polarity"] = - to_string(config.sync_pulse_out_polarity.value()); - } + if (config.sync_pulse_in_polarity) { + root["sync_pulse_in_polarity"] = + to_string(config.sync_pulse_in_polarity.value()); + } - if (config.sync_pulse_out_frequency) - { - root["sync_pulse_out_frequency"] = - config.sync_pulse_out_frequency.value(); - } + if (config.sync_pulse_out_polarity) { + root["sync_pulse_out_polarity"] = + to_string(config.sync_pulse_out_polarity.value()); + } - if (config.phase_lock_enable) - { - root["phase_lock_enable"] = config.phase_lock_enable.value(); - } + if (config.sync_pulse_out_frequency) { + root["sync_pulse_out_frequency"] = config.sync_pulse_out_frequency.value(); + } - if (config.phase_lock_offset) - { - root["phase_lock_offset"] = config.phase_lock_offset.value(); - } + if (config.phase_lock_enable) { + root["phase_lock_enable"] = config.phase_lock_enable.value(); + } + + if (config.phase_lock_offset) { + root["phase_lock_offset"] = config.phase_lock_offset.value(); + } - Json::StreamWriterBuilder builder; - builder["enableYAMLCompatibility"] = true; - builder["precision"] = 6; - builder["indentation"] = " "; - return Json::writeString(builder, root); + if (config.columns_per_packet) { + root["columns_per_packet"] = config.columns_per_packet.value(); + } + + if (config.udp_profile_lidar) { + root["udp_profile_lidar"] = to_string(config.udp_profile_lidar.value()); + } + + if (config.udp_profile_imu) { + root["udp_profile_imu"] = to_string(config.udp_profile_imu.value()); + } + + return root; +} + +std::string to_string(const sensor_config& config) { + Json::Value root = to_json(config); + + Json::StreamWriterBuilder builder; + builder["enableYAMLCompatibility"] = true; + builder["precision"] = 6; + builder["indentation"] = " "; + return Json::writeString(builder, root); } std::ostream& operator<<(std::ostream& os, const sensor_config& config) @@ -912,34 +1342,34 @@ std::ostream& operator<<(std::ostream& os, const sensor_config& config) } extern const std::vector gen1_altitude_angles = { - 16.611, 16.084, 15.557, 15.029, 14.502, 13.975, 13.447, 12.920, - 12.393, 11.865, 11.338, 10.811, 10.283, 9.756, 9.229, 8.701, - 8.174, 7.646, 7.119, 6.592, 6.064, 5.537, 5.010, 4.482, - 3.955, 3.428, 2.900, 2.373, 1.846, 1.318, 0.791, 0.264, - -0.264, -0.791, -1.318, -1.846, -2.373, -2.900, -3.428, -3.955, - -4.482, -5.010, -5.537, -6.064, -6.592, -7.119, -7.646, -8.174, - -8.701, -9.229, -9.756, -10.283, -10.811, -11.338, -11.865, -12.393, - -12.920, -13.447, -13.975, -14.502, -15.029, -15.557, -16.084, -16.611, + 16.611, 16.084, 15.557, 15.029, 14.502, 13.975, 13.447, 12.920, + 12.393, 11.865, 11.338, 10.811, 10.283, 9.756, 9.229, 8.701, + 8.174, 7.646, 7.119, 6.592, 6.064, 5.537, 5.010, 4.482, + 3.955, 3.428, 2.900, 2.373, 1.846, 1.318, 0.791, 0.264, + -0.264, -0.791, -1.318, -1.846, -2.373, -2.900, -3.428, -3.955, + -4.482, -5.010, -5.537, -6.064, -6.592, -7.119, -7.646, -8.174, + -8.701, -9.229, -9.756, -10.283, -10.811, -11.338, -11.865, -12.393, + -12.920, -13.447, -13.975, -14.502, -15.029, -15.557, -16.084, -16.611, }; extern const std::vector gen1_azimuth_angles = { - 3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164, - 3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164, - 3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164, - 3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164, - 3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164, - 3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164, - 3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164, - 3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164, + 3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164, + 3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164, + 3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164, + 3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164, + 3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164, + 3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164, + 3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164, + 3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164, }; extern const mat4d default_imu_to_sensor_transform = - (mat4d() << 1, 0, 0, 6.253, 0, 1, 0, -11.775, 0, 0, 1, 7.645, 0, 0, 0, 1) - .finished(); + (mat4d() << 1, 0, 0, 6.253, 0, 1, 0, -11.775, 0, 0, 1, 7.645, 0, 0, 0, 1) + .finished(); extern const mat4d default_lidar_to_sensor_transform = - (mat4d() << -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 36.18, 0, 0, 0, 1) - .finished(); + (mat4d() << -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 36.18, 0, 0, 0, 1) + .finished(); } // namespace sensor @@ -961,7 +1391,7 @@ version version_of_string(const std::string & s) { std::istringstream is{s}; char c1, c2, c3; - version v; + version v{}; is >> c1 >> v.major >> c2 >> v.minor >> c3 >> v.patch; diff --git a/ros2_ouster/src/driver_types.cpp b/ros2_ouster/src/driver_types.cpp index 3c6b635..2063508 100644 --- a/ros2_ouster/src/driver_types.cpp +++ b/ros2_ouster/src/driver_types.cpp @@ -22,11 +22,13 @@ namespace ros2_ouster { ros2_ouster::Driver::Driver(rclcpp::NodeOptions options) -: OusterDriver{std::make_unique(), options} {} + : OusterDriver{std::make_unique(), options} +{} ros2_ouster::TinsDriver::TinsDriver(rclcpp::NodeOptions options) -: OusterDriver{std::make_unique(), options} {} -} + : OusterDriver{std::make_unique(), options} +{} +}// namespace ros2_ouster RCLCPP_COMPONENTS_REGISTER_NODE(ros2_ouster::Driver) RCLCPP_COMPONENTS_REGISTER_NODE(ros2_ouster::TinsDriver) diff --git a/ros2_ouster/src/ouster_driver.cpp b/ros2_ouster/src/ouster_driver.cpp index 136e12d..e12cae6 100644 --- a/ros2_ouster/src/ouster_driver.cpp +++ b/ros2_ouster/src/ouster_driver.cpp @@ -11,9 +11,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include #include #include @@ -46,6 +43,7 @@ OusterDriver::OusterDriver( this->declare_parameter("imu_frame", std::string("imu_data_frame")); this->declare_parameter("use_system_default_qos", false); this->declare_parameter("proc_mask", std::string("IMG|PCL|IMU|SCAN")); + this->declare_parameter("lidar_udp_profile", std::string("RNG19_RFL8_SIG16_NIR16")); // Declare parameters used across ALL _sensor_ implementations this->declare_parameter("lidar_ip","10.5.5.96"); @@ -75,6 +73,7 @@ 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.lidar_udp_profile = get_parameter("lidar_udp_profile").as_string(); // Deliberately retrieve the IP parameters in a try block without defaults, as // we cannot estimate a reasonable default IP address for the LiDAR/computer. @@ -258,8 +257,14 @@ void OusterDriver::receiveData() // Receive raw sensor data from the network. // This blocks for some time until either data is received or timeout ouster::sensor::client_state state = _sensor->get(); - bool got_lidar = _sensor->readLidarPacket(state, _lidar_packet_buf->tail()); - bool got_imu = _sensor->readImuPacket(state, _imu_packet_buf->tail()); + + if (state == ouster::sensor::client_state::EXIT) { + handlePollError(); + return; + } + + bool got_lidar = handleLidarPacket(state); + bool got_imu = handleImuPacket(state); // If we got some data, push to ringbuffer and signal processing thread if (got_lidar || got_imu) { @@ -294,6 +299,104 @@ void OusterDriver::receiveData() } } +void OusterDriver::handlePollError() { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 100, + "sensor::poll_client()) returned error"); + // in case error continues for a while attempt to recover by + // performing sensor reset + if (++_poll_error_count > _max_poll_error_count) { + RCLCPP_ERROR_STREAM( + get_logger(), + "maximum number of allowed errors from " + "sensor::poll_client() reached, performing self reset..."); + _poll_error_count = 0; + _reset_srv.reset(); + } +} + +bool OusterDriver::handleLidarPacket(const ouster::sensor::client_state & state) +{ + if (!(state & ouster::sensor::client_state::LIDAR_DATA)) { + return false; + } + + if (!_sensor->readLidarPacket(state, _lidar_packet_buf->tail())) { + if (++_lidar_packet_error_count > _max_lidar_packet_error_count) { + RCLCPP_ERROR_STREAM( + get_logger(), + "maximum number of allowed errors from " + "sensor::read_lidar_packet() reached, reactivating..."); + _lidar_packet_error_count = 0; + //TODO(reset): perform only a reactivation instead of a full reset + _reset_srv.reset(); + +// reactivate_sensor(true); + ros2_ouster::Configuration lidar_config; + lidar_config.lidar_ip = get_parameter("lidar_ip").as_string(); + lidar_config.computer_ip = get_parameter("computer_ip").as_string(); + 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, shared_from_this()); + } + return false; + } + + _lidar_packet_error_count = 0; + if (_sensor->shouldReset(state, _lidar_packet_buf->tail())) { + // TODO: short circut reset if no breaking changes occured? + RCLCPP_WARN(get_logger(), + "sensor init_id has changed! reactivating.."); + //TODO(reset): perform only a reactivation instead of a full reset + _reset_srv.reset(); + +// reactivate_sensor(false); + ros2_ouster::Configuration lidar_config; + lidar_config.lidar_ip = get_parameter("lidar_ip").as_string(); + lidar_config.computer_ip = get_parameter("computer_ip").as_string(); + 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, shared_from_this()); + return false; + } + + return true; +} + +bool OusterDriver::handleImuPacket(const ouster::sensor::client_state & state) +{ + if (!(state & ouster::sensor::client_state::IMU_DATA)) return false; + + if (!_sensor->readImuPacket(state, _imu_packet_buf->tail())) { + if (++_imu_packet_error_count > _max_imu_packet_error_count) { + RCLCPP_ERROR_STREAM( + get_logger(), + "maximum number of allowed errors from " + "sensor::read_lidar_packet() reached, reactivating..."); + _imu_packet_error_count = 0; + //TODO(reset): perform only a reactivation instead of a full reset + _reset_srv.reset(); + +// reactivate_sensor(true); + ros2_ouster::Configuration lidar_config; + lidar_config.lidar_ip = get_parameter("lidar_ip").as_string(); + lidar_config.computer_ip = get_parameter("computer_ip").as_string(); + 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, shared_from_this()); + } + return false; + } + + _imu_packet_error_count = 0; + return true; +} + void OusterDriver::resetService( const std::shared_ptr/*request_header*/, const std::shared_ptr request, @@ -324,7 +427,7 @@ void OusterDriver::getMetadata( response->metadata = toMsg(_sensor->getMetadata()); // Save the metadata to file ONLY if the user specifies a filepath - if (request->metadata_filepath != "") { + if (!request->metadata_filepath.empty()) { std::string json_config = ouster::sensor::to_string(_sensor->getMetadata()); std::ofstream ofs; ofs.open(request->metadata_filepath); diff --git a/ros2_ouster/src/sensor.cpp b/ros2_ouster/src/sensor.cpp index c81c2ed..f01d426 100644 --- a/ros2_ouster/src/sensor.cpp +++ b/ros2_ouster/src/sensor.cpp @@ -12,7 +12,6 @@ // limitations under the License. #include -#include #include #include "ros2_ouster/client/client.h" #include "ros2_ouster/exception.hpp" @@ -35,6 +34,10 @@ void Sensor::reset( rclcpp_lifecycle::LifecycleNode::SharedPtr node) { _ouster_client.reset(); + + //TODO(force-reinit): currently always forcing reinit, rethink this later? + reactivate_sensor(true); + configure(config, node); } @@ -64,7 +67,7 @@ void Sensor::configure( RCLCPP_INFO( node->get_logger(), "Connecting to sensor at %s.", config.lidar_ip.c_str()); - if (config.computer_ip == "") { + if (config.computer_ip.empty()) { RCLCPP_INFO( node->get_logger(), "Sending data from sensor to computer using automatic address detection"); @@ -74,13 +77,7 @@ void Sensor::configure( "Sending data from sensor to %s.", config.computer_ip.c_str()); } - _ouster_client = ouster::sensor::init_client( - config.lidar_ip, - config.computer_ip, - ouster::sensor::lidar_mode_of_string(config.lidar_mode), - ouster::sensor::timestamp_mode_of_string(config.timestamp_mode), - config.lidar_port, - config.imu_port); + _ouster_client = configure_and_initialize_sensor(config); if (!_ouster_client) { throw ros2_ouster::OusterDriverException( @@ -90,6 +87,40 @@ void Sensor::configure( setMetadata(config.lidar_port, config.imu_port, config.timestamp_mode); } +bool Sensor::shouldReset(const ouster::sensor::client_state & state, const uint8_t * packet) +{ + return (state & ouster::sensor::client_state::LIDAR_DATA) && + is_non_legacy_lidar_profile(getMetadata()) && + init_id_changed(getPacketFormat(), packet); +} + +std::shared_ptr Sensor::configure_and_initialize_sensor( + const ros2_ouster::Configuration &config) +{ + ouster::sensor::sensor_config sensor_config{}; + sensor_config.udp_dest = config.computer_ip; + sensor_config.udp_port_imu = config.imu_port; + sensor_config.udp_port_lidar = config.lidar_port; + sensor_config.ld_mode = ouster::sensor::lidar_mode_of_string(config.lidar_mode); + sensor_config.ts_mode = ouster::sensor::timestamp_mode_of_string(config.timestamp_mode); + sensor_config.udp_profile_lidar = + ouster::sensor::udp_profile_lidar_of_string(config.lidar_udp_profile); + + uint8_t config_flags = compose_config_flags(sensor_config); + if (!set_config(config.lidar_ip, sensor_config, config_flags)) { + throw std::runtime_error("Error connecting to sensor " + config.lidar_ip); + } + + std::cout << "Sensor " << config.lidar_ip + << " configured successfully, initializing client" << std::endl; + + return ouster::sensor::init_client(config.lidar_ip, sensor_config.udp_dest.value(), + sensor_config.ld_mode.value(), + sensor_config.ts_mode.value(), + sensor_config.udp_port_lidar.value(), + sensor_config.udp_port_imu.value()); +} + ouster::sensor::client_state Sensor::get() { const ouster::sensor::client_state state = ouster::sensor::poll_client(*_ouster_client); @@ -142,7 +173,7 @@ void Sensor::setMetadata( ouster::sensor::get_metadata(*_ouster_client)), imu_port, lidar_port, timestamp_mode); } - ros2_ouster::populate_missing_metadata_defaults(_metadata); + ros2_ouster::populate_missing_metadata_defaults(_metadata, ouster::sensor::MODE_UNSPEC); } ros2_ouster::Metadata Sensor::getMetadata() @@ -155,4 +186,64 @@ ouster::sensor::packet_format Sensor::getPacketFormat() return ouster::sensor::get_format(getMetadata()); } +uint8_t Sensor::compose_config_flags(const ouster::sensor::sensor_config &config) +{ + uint8_t config_flags = 0; + if (config.udp_dest) { + std::cout << "Will send UDP data to " << config.udp_dest.value() + << std::endl; + } + else { + std::cout << "Will use automatic UDP destination" << std::endl; + config_flags |= ouster::sensor::CONFIG_UDP_DEST_AUTO; + } + + if (force_sensor_reinit) { + force_sensor_reinit = false; + std::cout << "Forcing sensor to reinitialize" << std::endl; + config_flags |= ouster::sensor::CONFIG_FORCE_REINIT; + } + + return config_flags; +} + +// param init_id_reset is overriden to true when force_reinit is true +void Sensor::reset_sensor(bool force_reinit, bool init_id_reset) { +// if (!sensor_connection_active) { +// RCLCPP_WARN(get_logger(), +// "sensor reset is invoked but sensor connection is not " +// "active, ignoring call!"); +// return; +// } + + force_sensor_reinit = force_reinit; + reset_last_init_id = force_reinit || init_id_reset; +// auto request_transitions = std::vector{ +// lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE, +// lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP, +// lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE, +// lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE}; +// execute_transitions_sequence(request_transitions, 0); +} + +// TODO: need to notify dependent node(s) of the update +void Sensor::reactivate_sensor(bool init_id_reset) { +// if (!sensor_connection_active) { +// This may indicate that we are in the process of re-activation +// RCLCPP_WARN(get_logger(), +// "sensor reactivate is invoked but sensor connection is " +// "not active, ignoring call!"); +// return; +// } + + reset_last_init_id = init_id_reset; +// update_config_and_metadata(); +// publish_metadata(); +// save_metadata(); +// auto request_transitions = std::vector{ +// lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE, +// lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE}; +// execute_transitions_sequence(request_transitions, 0); +} + } // namespace sensor diff --git a/ros2_ouster/src/sensor_tins.cpp b/ros2_ouster/src/sensor_tins.cpp index 1b31bd2..0eaa7a9 100644 --- a/ros2_ouster/src/sensor_tins.cpp +++ b/ros2_ouster/src/sensor_tins.cpp @@ -95,12 +95,10 @@ void SensorTins::configure( // loadSensorInfoFromJsonFile actually returns a sensor_info object, so // fill in the params specific to the ros2_ouster::Metadata object, that // aren't normally supplied in the metadata file. - _metadata.imu_port = _driver_config.imu_port; - _metadata.lidar_port = _driver_config.lidar_port; _metadata.timestamp_mode = _driver_config.timestamp_mode; // Fill anything missing with defaults and resize the packet containers - ros2_ouster::populate_missing_metadata_defaults(_metadata); + ros2_ouster::populate_missing_metadata_defaults(_metadata, ouster::sensor::MODE_UNSPEC); _lidar_packet.resize(getPacketFormat().lidar_packet_size + 1); _imu_packet.resize(getPacketFormat().imu_packet_size + 1); @@ -115,6 +113,11 @@ void SensorTins::configure( } } +bool SensorTins::shouldReset(const ouster::sensor::client_state & state, const uint8_t * packet) +{ + return false; +} + ouster::sensor::client_state SensorTins::get() { // Start a sniff loop to look for a valid LiDAR or IMU packet