diff --git a/.gitmodules b/.gitmodules index 024568e3..294cdba6 100644 --- a/.gitmodules +++ b/.gitmodules @@ -2,10 +2,6 @@ path = src/third-party/ublox url = https://github.com/KumarRobotics/ublox.git branch = ros2 -[submodule "src/third-party/ouster-ros"] - path = src/third-party/ouster-ros - url = https://github.com/ouster-lidar/ouster-ros.git - branch = ros2 [submodule "src/third-party/zed-ros2-wrapper"] path = src/third-party/zed-ros2-wrapper url = https://github.com/stereolabs/zed-ros2-wrapper.git diff --git a/Dockerfile b/Dockerfile index 5c8e7a20..61ac7cce 100644 --- a/Dockerfile +++ b/Dockerfile @@ -117,9 +117,9 @@ RUN git clone https://github.com/ANYbotics/kindr.git \ RUN apt-get update && apt-get install -y --no-install-recommends \ libsrt1.4-openssl \ lsb-release \ - gnupg2 libssl-dev libpng16-16 \ + gnupg2 libpng16-16 \ zlib1g-dev libffi-dev \ - libglib2.0-dev libmount-dev \ + libmount-dev \ python3-mrcal libsnmp-dev snmp \ && rm -rf /var/lib/apt/lists/* @@ -195,10 +195,12 @@ ENTRYPOINT ["/bin/bash", "-c", "source /ros.sh && exec \"$@\"", "--"] CMD ["/bin/bash"] + +## Helpers for development and CI pipelines ############################ # Stage 9: Linter (Optional) ############################ -FROM builder as linter +FROM builder AS linter SHELL ["/bin/bash", "-o", "pipefail", "-c"] WORKDIR /rover COPY ros.sh . @@ -207,4 +209,23 @@ RUN black . --exclude "src/third-party/|build|install|\.tox|dist" --check RUN find ./src -path ./src/third-party -prune -o \ \( -name "*.h" -o -name "*.hpp" -o -name "*.cpp" \) -print \ | xargs clang-format --dry-run --Werror -RUN source ros.sh && pylint -E src \ No newline at end of file +RUN source ros.sh && pylint -E src + +############################ +# Stage 10: Rosdep collector (Optional) +############################ +FROM ros2_humble-base AS rosdep-collector +SHELL ["/bin/bash", "-o", "pipefail", "-c"] +WORKDIR /rover +COPY src/ ./src +RUN source /opt/ros/humble/setup.bash && \ + rosdep init && rosdep update && \ + rosdep keys --from-paths src --ignore-src > /rosdep-keys.txt +RUN sort /rosdep-keys.txt -o /rosdep-keys.txt + + +############################ +# Stage 11: Rosdep exporter (Optional) +############################ +FROM scratch AS rosdep-exporter +COPY --from=rosdep-collector /rosdep-keys.txt . \ No newline at end of file diff --git a/rosdep-keys.txt b/rosdep-keys.txt index 019dcdd8..cb2f6298 100644 --- a/rosdep-keys.txt +++ b/rosdep-keys.txt @@ -1,90 +1,81 @@ -libtins-dev -rosbridge_server +asio +boost +compressed_depth_image_transport compressed_image_transport -joy +controller_manager +cv_bridge +diagnostic_updater +diff_drive_controller draco_point_cloud_transport eigen -joint_state_broadcaster -robot_localization +ffmpeg_encoder_decoder ffmpeg_image_transport -image_transport_plugins -moveit_planners +filters +forward_command_controller +geographic_msgs +glib-2.0 +grid_map_cmake_helpers +grid_map_core +grid_map_filters +grid_map_msgs +grid_map_ros gstreamer-1.0 +hardware_interface +image_transport +image_transport_plugins +joint_state_broadcaster +joint_state_publisher +joint_state_publisher_gui +joint_trajectory_controller +joy +joy_linux +libglib-dev libpcl-all-dev -moveit_simple_controller_manager -point_cloud_transport -pcl_conversions -xacro -nav2_bringup -grid_map_msgs +libssl-dev +liburdfdom-tools +moveit_configs_utils moveit_kinematics -asio -grid_map_filters -tf_transformations -rtabmap_ros -diagnostic_updater -cv_bridge -rviz_common -rviz_default_plugins -compressed_depth_image_transport +moveit_planners +moveit_ros_move_group moveit_ros_visualization -grid_map_cmake_helpers -gtest -catkin -ros2controlcli -hardware_interface moveit_servo -qtbase5-dev -pcl_ros -theora_image_transport -grid_map_core -zed_msgs -liburdfdom-tools -forward_command_controller -nmea_msgs +moveit_simple_controller_manager +nav2_bringup nav2_core -curl -rviz2 nav2_costmap_2d -nav2_util nav2_msgs -filters -tbb -python3-pytest -navigation2 -libjsoncpp-dev -rtcm_msgs nav2_regulated_pure_pursuit_controller -spdlog nav2_smac_planner -moveit_ros_move_group -controller_manager -rtabmap -point_cloud_transport_plugins -zstd_point_cloud_transport -joint_state_publisher -grid_map_ros +nav2_util +navigation2 +nmea_msgs pcl +pcl_conversions +pcl_ros pkg-config -ffmpeg_encoder_decoder -image_transport -zlib_point_cloud_transport +point_cloud_transport +point_cloud_transport_plugins +position_controllers +python3-numpy +python3-opencv +python3-packaging +python3-pytest +python3-scipy +python3-yaml +robot_localization +ros2controlcli ros2_controllers_test_nodes -moveit_configs_utils -geographic_msgs -rviz -boost -joint_trajectory_controller -diff_drive_controller -joint_state_publisher_gui -libjsoncpp -controller_manager_msgs -flex -bison -libncurses-dev -usbutils -tinyxml2 -clang-tidy -python3-vcstool -joy_linux -position_controllers \ No newline at end of file +rosbridge_server +rtabmap +rtabmap_ros +rtcm_msgs +rviz2 +rviz_common +rviz_default_plugins +tbb +tf_transformations +theora_image_transport +xacro +zed_msgs +zlib_point_cloud_transport +zstd_point_cloud_transport diff --git a/setup/generate_rosdeps.sh b/setup/generate_rosdeps.sh new file mode 100755 index 00000000..6157bc82 --- /dev/null +++ b/setup/generate_rosdeps.sh @@ -0,0 +1,17 @@ +#!/bin/bash + +SCRIPT_DIR="$(dirname "$0")" +DOCKERFILE_DIR="$SCRIPT_DIR/.." + +# check if docker is installed +if ! command -v docker &> /dev/null +then + echo "Docker could not be found. Are you in the Docker environment?" + echo "If so please exit back to the host environment." + echo "If not, please install Docker and try again. " + exit 1 +fi + +echo "Generating rosdeps inside the docker container..." +docker build --target rosdep-exporter --output $DOCKERFILE_DIR $DOCKERFILE_DIR +echo "Rosdeps generated successfully." \ No newline at end of file diff --git a/src/Cameras/camera_streaming/CMakeLists.txt b/src/Cameras/camera_streaming/CMakeLists.txt deleted file mode 100644 index 601423ab..00000000 --- a/src/Cameras/camera_streaming/CMakeLists.txt +++ /dev/null @@ -1,77 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(camera_streaming) - -# Set the C++ standard to C++17 (or higher, if needed) -set(CMAKE_CXX_STANDARD 20) - -# Find dependencies (ROS2, GStreamer, and PkgConfig) -find_package(rclcpp REQUIRED) -find_package(interfaces REQUIRED) -find_package(std_srvs REQUIRED) -find_package(ament_cmake REQUIRED) - -# Use pkg-config to find the necessary GStreamer libraries -find_package(PkgConfig REQUIRED) -pkg_check_modules(GSTREAMER REQUIRED gstreamer-1.0) -pkg_check_modules(GST_APP REQUIRED gstreamer-app-1.0) -pkg_check_modules(GST_VIDEO REQUIRED gstreamer-video-1.0) - - -# Include the necessary directories for GStreamer -include_directories( - include/${PROJECT_NAME} - ${GSTREAMER_INCLUDE_DIRS} - ${GST_APP_INCLUDE_DIRS} - ${GST_VIDEO_INCLUDE_DIRS} -) - - -# Link directories for the necessary libraries -link_directories( - ${GSTREAMER_LIBRARY_DIRS} - ${GST_APP_LIBRARY_DIRS} - ${GST_VIDEO_LIBRARY_DIRS} -) - -# Add the executable target -add_executable(webrtc_node src/webrtc_node.cpp) - -# Link with the required libraries -target_link_libraries(webrtc_node - rclcpp::rclcpp - ${GSTREAMER_LIBRARIES} - ${GST_APP_LIBRARIES} - ${GST_VIDEO_LIBRARIES} -) - -# Ensure that the pkg-config variables are used -pkg_check_modules(GLIB REQUIRED glib-2.0) - -# Set up any necessary ROS2 specific configurations -ament_target_dependencies(webrtc_node - rclcpp - interfaces - std_srvs -) - -# Install header files -install(DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} -) - -# Install the executable (optional, depending on your needs) -install(TARGETS webrtc_node DESTINATION lib/${PROJECT_NAME}) - -# Install launch files (if any) -install(DIRECTORY launch - DESTINATION share/${PROJECT_NAME} -) - -# Install configuration files (if any) -install(DIRECTORY config - DESTINATION share/${PROJECT_NAME} -) - -ament_export_include_directories("include/${PROJECT_NAME}") - -ament_package() \ No newline at end of file diff --git a/src/Cameras/camera_streaming/config/webrtc.yaml b/src/Cameras/camera_streaming/config/webrtc.yaml deleted file mode 100644 index 5f09006e..00000000 --- a/src/Cameras/camera_streaming/config/webrtc.yaml +++ /dev/null @@ -1,33 +0,0 @@ -#Types: 0 for v4l2, 1 for test source, 2 for rtp jpeg source, Other types are not supported yet - -webrtc_node: - ros__parameters: - camera_name: - - "Drive" - - "EndEffector" - - "Bottom" - - "Microscope" - - "Panoramic" - Drive: - path: "/dev/drive_camera" - type: 0 - aruco: true - aruco_detect_interval: 30 - EndEffector: - path: "/dev/v4l/by-id/usb-Sonix_Technology_Co.__Ltd._USB_2.0_Camera_SN5100-video-index0" - type: 0 - encoded: true - Bottom: - path: "/dev/v4l/by-id/usb-HD_Camera_Manufacturer_USB_2.0_Camera-video-index0" - type: 0 - encoded: true - Microscope: - type: 2 - path: "udp://0.0.0.0:9000" - encoded: true - # Hack: - # Tricks the clients into displaying this as a option which the panoramic camera will recieve the request instead - Panoramic: - type: 1 - web_server: false - web_server_path: "/home/cprt/gstreamer/webrtc/gstwebrtc-api/dist" diff --git a/src/Cameras/camera_streaming/include/camera_streaming/webrtc_node.h b/src/Cameras/camera_streaming/include/camera_streaming/webrtc_node.h deleted file mode 100644 index 86f8a49a..00000000 --- a/src/Cameras/camera_streaming/include/camera_streaming/webrtc_node.h +++ /dev/null @@ -1,231 +0,0 @@ -/** - * @file webrtc_node.h - * @brief Header file for the WebRTCStreamer class. - * @author Connor Needham - * - * This file contains the declaration of the WebRTCStreamer class, which is - * responsible for handling video streaming using WebRTC and GStreamer. - */ - -#ifndef WEBRTC_STREAMER_HPP -#define WEBRTC_STREAMER_HPP - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -template struct GstDeleter { - void operator()(T *object) const { - if (object) { - gst_object_unref(object); - } - } -}; - -// Type alias for unique_ptr managing a GStreamer object -template using GstUniquePtr = std::unique_ptr>; - -/** - * @class WebRTCStreamer - * @brief A class for streaming video using WebRTC and GStreamer. - * - * The WebRTCStreamer class provides functionality to start video streaming - * from different camera sources, manage the GStreamer pipeline, and handle - * video output requests. - */ -class WebRTCStreamer : public rclcpp::Node { -public: - /** - * @brief Constructor for WebRTCStreamer. - */ - WebRTCStreamer(); - - /** - * @brief Destructor for WebRTCStreamer. - */ - ~WebRTCStreamer(); - - /** - * @enum CameraType - * @brief Enum representing the type of camera source. - */ - enum class CameraType { - V4l2Src = 0, /**< V4L2 source */ - TestSrc, /**< Test source */ - NetworkSrc, /**< Network source */ - }; - - /** - * @struct CameraSource - * @brief Struct representing a camera source. - */ - struct CameraSource { - std::string name; - std::string path; - CameraType type; - bool encoded; - bool aruco; - }; - -private: - /** - * @brief Initializes the GStreamer pipeline - */ - bool start(); - /** - * @brief Shuts down and cleans up the gstreamer pipeline resources - */ - void stop(); - - /** - * @brief Declares parameters for the WebRTCStreamer node. - * - * This function declares parameters such as web server settings, camera - * names, and camera properties. - */ - void declare_parameters(); - - /** - * @brief Callback function to start video streaming. - * - * @param request The request object containing video output parameters. - * @param response The response object to be populated with the result. - */ - void start_video_cb( - const std::shared_ptr request, - std::shared_ptr response); - - /** - * @brief Callback function to handle video capture requests. - * - * @param request The request object containing video capture parameters. - * @param response The response object to be populated with the result. - */ - void capture_frame( - const std::shared_ptr request, - std::shared_ptr response); - - /** - * @brief Callback function to get available camera sources. - * - * @param request The request object (not used). - * @param response The response object to be populated with the camera - * sources. - */ - void get_cameras( - const std::shared_ptr request, - std::shared_ptr response); - - /** - * @brief Callback function to restart the video pipeline. - * - * @param request The request object (not used). - * @param response The response object to be populated with the result. - */ - void restart_pipeline( - const std::shared_ptr request, - std::shared_ptr response); - - /** - * @brief Creates a GStreamer source element for the given camera source. - * - * @param src The camera source information. - * @return A pointer to the created GStreamer element. - */ - GstElement *create_source(const CameraSource &src); - - /** - * @brief Updates the GStreamer pipeline based on the video output request. - * - * @param request The request object containing video output parameters. - * @return True if the pipeline was successfully updated, false otherwise. - */ - bool update_pipeline( - const std::shared_ptr request); - - /** - * @brief Initializes the GStreamer pipeline. - * - * @return A pointer to the initialized GStreamer pipeline element. - */ - GstElement *initialize_pipeline(); - - /** - * @brief Unlinks current sources from the compositor element. - */ - void unlink_sources_from_compositor(); - - /** - * @brief Unlinks a specific pad from its peer. - * - * @param pad The pad to be unlinked. - * @return True if the pad was successfully unlinked, false otherwise. - */ - static bool unlink_pad(GstPad *pad); - - /** - * @brief Creates a GStreamer video converter element. - * - * @return A pointer to the created video converter element. - */ - GstElement *create_vid_conv(); - /** - * @brief Adds a chain of GStreamer elements to the pipeline. - * - * @param chain The vector of GStreamer elements to be added. - * @return A pointer to the last element in the chain. - */ - GstElement *add_element_chain(const std::vector &chain); - /** - * @brief Creates a GStreamer element of the specified type. - * - * @param element_type The type of the GStreamer element to create. - * @param element_name The name of the GStreamer element (optional). - * @return A pointer to the created GStreamer element. - */ - GstElement *create_element(std::string element_type, - std::string element_name = ""); - - bool web_server_; /**< Flag indicating if the web server is enabled */ - std::string web_server_path_; /**< Path to the web server */ - std::vector elements_; /**< List of GStreamer elements */ - GstUniquePtr pipeline_; /**< GStreamer pipeline element */ - GstElement *compositor_; /**< GStreamer compositor element */ - rclcpp::Service::SharedPtr - start_video_service_; /**< ROS2 service for starting video output */ - rclcpp::Service::SharedPtr - capture_service_; /**< ROS2 service for capturing frames */ - rclcpp::Service::SharedPtr - get_cams_service_; /**< ROS2 service for getting camera list */ - rclcpp::Service::SharedPtr - restart_service_; /**< ROS2 service for restarting the video pipeline */ - rclcpp::Publisher::SharedPtr - marker_pub_; /**< ROS2 publisher for marker detection messages */ - GstUniquePtr bus_; /**< GStreamer bus for message handling */ - std::map> - source_pads_; /**< Maps camera names to compositor pads*/ - int height_; /**< Max height of the video */ - int width_; /**< Max width of the video */ - int framerate_; /**< Max framerate of the video */ - - /** - * @brief Callback function for handling GStreamer bus messages. - * - * @param bus The GStreamer bus. - * @param message The GStreamer message. - * @param user_data User data passed to the callback. - * @return True if the message was successfully handled, false otherwise. - */ - static gboolean on_bus_message(GstBus *bus, GstMessage *message, - gpointer user_data); -}; - -#endif // WEBRTC_STREAMER_HPP diff --git a/src/Cameras/camera_streaming/launch/webRTC.launch.py b/src/Cameras/camera_streaming/launch/webRTC.launch.py deleted file mode 100644 index f9d1a81d..00000000 --- a/src/Cameras/camera_streaming/launch/webRTC.launch.py +++ /dev/null @@ -1,20 +0,0 @@ -import os -import launch_ros.actions -from ament_index_python.packages import get_package_share_directory -import launch - - -def generate_launch_description(): - config_dir = os.path.join(get_package_share_directory("camera_streaming"), "config") - - params_file = os.path.join(config_dir, "webrtc.yaml") - - webrtc_node = launch_ros.actions.Node( - package="camera_streaming", - executable="webrtc_node", - output="log", - parameters=[params_file], - arguments=["--ros-args", "--log-level", "Info"], - ) - - return launch.LaunchDescription([webrtc_node]) diff --git a/src/Cameras/camera_streaming/package.xml b/src/Cameras/camera_streaming/package.xml deleted file mode 100644 index b010c558..00000000 --- a/src/Cameras/camera_streaming/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - camera_streaming - 0.0.0 - TODO: Package description - connor - TODO: License declaration - - ament_cmake - rclcpp - interfaces - std_srvs - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/src/Cameras/camera_streaming/src/webrtc_node.cpp b/src/Cameras/camera_streaming/src/webrtc_node.cpp deleted file mode 100644 index 9730d6fb..00000000 --- a/src/Cameras/camera_streaming/src/webrtc_node.cpp +++ /dev/null @@ -1,611 +0,0 @@ -#include "webrtc_node.h" - -#include - -#include -#include - -#include "rclcpp/executors.hpp" - -WebRTCStreamer::WebRTCStreamer() - : Node("webrtc_node"), pipeline_(nullptr), compositor_(nullptr) { - gst_init(nullptr, nullptr); - - // Declare parameters - declare_parameters(); - - this->get_parameter("web_server", web_server_); - this->get_parameter("web_server_path", web_server_path_); - this->get_parameter("max_width", width_); - this->get_parameter("max_height", height_); - this->get_parameter("max_framerate", framerate_); - - // Set up the service for starting video - start_video_service_ = this->create_service( - "start_video", std::bind(&WebRTCStreamer::start_video_cb, this, - std::placeholders::_1, std::placeholders::_2)); - capture_service_ = this->create_service( - "capture_frame", std::bind(&WebRTCStreamer::capture_frame, this, - std::placeholders::_1, std::placeholders::_2)); - get_cams_service_ = this->create_service( - "get_cameras", std::bind(&WebRTCStreamer::get_cameras, this, - std::placeholders::_1, std::placeholders::_2)); - restart_service_ = this->create_service( - "reset_video", std::bind(&WebRTCStreamer::restart_pipeline, this, - std::placeholders::_1, std::placeholders::_2)); - marker_pub_ = this->create_publisher( - "marker_detected", rclcpp::QoS(rclcpp::KeepLast(10)).reliable()); - - start(); - GST_DEBUG_BIN_TO_DOT_FILE(GST_BIN(pipeline_.get()), GST_DEBUG_GRAPH_SHOW_ALL, - "start_pipeline"); -} - -WebRTCStreamer::~WebRTCStreamer() { stop(); } - -bool WebRTCStreamer::start() { - // Fetch camera parameters - std::vector camera_names; - this->get_parameter("camera_name", camera_names); - initialize_pipeline(); - - for (const auto &name : camera_names) { - std::string camera_path; - this->get_parameter(name + ".path", camera_path); - int camera_type; - this->get_parameter(name + ".type", camera_type); - bool encoded; - this->get_parameter(name + ".encoded", encoded); - bool aruco; - this->get_parameter(name + ".aruco", aruco); - CameraSource source; - source.name = name; - source.path = camera_path; - source.type = static_cast(camera_type); - source.encoded = encoded; - source.aruco = aruco; - - if (source.type != CameraType::TestSrc && camera_path.empty()) { - RCLCPP_ERROR(this->get_logger(), "Camera path not set for %s", - name.c_str()); - continue; - } - if (source.type == CameraType::V4l2Src && - !std::filesystem::exists(source.path)) { - RCLCPP_ERROR(this->get_logger(), "Camera path does not exist: %s", - camera_path.c_str()); - continue; - } - GstElement *src = create_source(source); - if (src) { - auto src_pad = - GstUniquePtr(gst_element_get_static_pad(src, "src")); - auto sink_pad = GstUniquePtr( - gst_element_request_pad_simple(compositor_, "sink_%u")); - g_object_set(G_OBJECT(sink_pad.get()), "height", height_, "width", width_, - "alpha", 0.0, NULL); - auto rc = gst_pad_link(src_pad.get(), sink_pad.get()); - if (rc != GST_PAD_LINK_OK) { - RCLCPP_ERROR(this->get_logger(), "Could not link camera %s", - name.c_str()); - gst_element_release_request_pad(compositor_, sink_pad.get()); - continue; - } - source_pads_.emplace(name, std::move(sink_pad)); - RCLCPP_INFO(this->get_logger(), "Camera source %s created", name.c_str()); - } - } - const auto ret = gst_element_set_state(pipeline_.get(), GST_STATE_PLAYING); - if (ret == GST_STATE_CHANGE_FAILURE) { - RCLCPP_ERROR(this->get_logger(), "Failed to start pipeline"); - return false; - } - RCLCPP_INFO(this->get_logger(), "Pipeline started successfully"); - return true; -} - -void WebRTCStreamer::stop() { - for (auto &pair : source_pads_) { - gst_element_release_request_pad(compositor_, pair.second.get()); - } - source_pads_.clear(); - - for (const auto &element : elements_) { - if (element) { - gst_element_set_state(element, GST_STATE_NULL); - gst_bin_remove(GST_BIN(pipeline_.get()), element); - gst_object_unref(element); - } - } - elements_.clear(); - - if (pipeline_) { - gst_element_set_state(pipeline_.get(), GST_STATE_NULL); - } -} - -void WebRTCStreamer::declare_parameters() { - this->declare_parameter("web_server", true); - this->declare_parameter("web_server_path", "."); - this->declare_parameter("max_width", 1280); - this->declare_parameter("max_height", 720); - this->declare_parameter("max_framerate", 30); - this->declare_parameter("camera_name", std::vector()); - std::vector camera_name; - this->get_parameter("camera_name", camera_name); - for (const auto &name : camera_name) { - this->declare_parameter(name + ".path", std::string()); - this->declare_parameter(name + ".type", - static_cast(CameraType::V4l2Src)); - this->declare_parameter(name + ".encoded", false); - this->declare_parameter(name + ".aruco", false); - this->declare_parameter(name + ".aruco_detect_interval", 1); - } -} - -void WebRTCStreamer::start_video_cb( - const std::shared_ptr request, - std::shared_ptr response) { - if (!pipeline_) { - RCLCPP_ERROR(this->get_logger(), "Pipeline not initialized"); - response->success = false; - return; - } - gst_element_set_state(pipeline_.get(), GST_STATE_PAUSED); - if (update_pipeline(request) != true) { - RCLCPP_ERROR(this->get_logger(), "Failed to update pipeline"); - response->success = false; - GST_DEBUG_BIN_TO_DOT_FILE(GST_BIN(pipeline_.get()), - GST_DEBUG_GRAPH_SHOW_ALL, "error_pipeline"); - return; - } - const auto ret = gst_element_set_state(pipeline_.get(), GST_STATE_PLAYING); - if (ret != GST_STATE_CHANGE_FAILURE) { - RCLCPP_INFO(this->get_logger(), "Pipeline started successfully"); - response->success = true; - } else { - RCLCPP_ERROR(this->get_logger(), "Failed to start pipeline"); - response->success = false; - } - GST_DEBUG_BIN_TO_DOT_FILE(GST_BIN(pipeline_.get()), GST_DEBUG_GRAPH_SHOW_ALL, - "pipeline_update"); -} - -void WebRTCStreamer::capture_frame( - const std::shared_ptr request, - std::shared_ptr response) { - response->success = true; - const std::string &name = request->source; - auto source_tee = GstUniquePtr( - gst_bin_get_by_name(GST_BIN(pipeline_.get()), name.c_str())); - if (!source_tee) { - RCLCPP_ERROR(this->get_logger(), "Failed to get source: %s", name.c_str()); - return; - } - std::vector elements; - elements.emplace_back(create_element("queue")); - if (!elements.back()) { - return; - } - g_object_set(G_OBJECT(elements.back()), "max-size-buffers", 1, nullptr); - g_object_set(G_OBJECT(elements.back()), "leaky", 2, nullptr); - - elements.emplace_back(create_vid_conv()); - elements.emplace_back(create_element("jpegenc")); - - GstElement *sink = create_element("appsink"); - elements.push_back(sink); - if (!add_element_chain(elements)) { - response->success = false; - return; - } - - if (!gst_element_link(source_tee.get(), elements.front())) { - RCLCPP_ERROR(this->get_logger(), - "%s: Failed to link capture pipe section to %s", __FUNCTION__, - name.c_str()); - response->success = false; - for (auto element : elements) { - gst_bin_remove(GST_BIN(pipeline_.get()), element); - gst_object_unref(element); - } - return; - } - constexpr auto timeout = 1000000000; // 1 second - GstSample *sample = gst_app_sink_try_pull_sample(GST_APP_SINK(sink), timeout); - - auto pad = GstUniquePtr( - gst_element_get_static_pad(elements.front(), "sink")); - unlink_pad(pad.get()); - for (auto element : elements) { - gst_bin_remove(GST_BIN(pipeline_.get()), element); - } - - if (!sample) { - RCLCPP_ERROR(this->get_logger(), "Failed to pull sample from appsink"); - response->success = false; - return; - } - GstBuffer *buffer = gst_sample_get_buffer(sample); - if (!buffer) { - RCLCPP_ERROR(this->get_logger(), "Failed to get buffer from sample"); - gst_sample_unref(sample); - response->success = false; - return; - } - GstMapInfo map; - if (!gst_buffer_map(buffer, &map, GST_MAP_READ)) { - RCLCPP_ERROR(this->get_logger(), "Failed to map buffer"); - gst_sample_unref(sample); - response->success = false; - return; - } - std::vector &img_vec = response->image.data; - std::string &filename = request->filename; - img_vec.resize(map.size); - std::memcpy(img_vec.data(), map.data, map.size); - response->image.format = "jpeg"; - if (!filename.empty()) { - std::ofstream file(filename, std::ios::binary); - if (!file) { - RCLCPP_ERROR(this->get_logger(), "Failed to open file %s for writing", - filename.c_str()); - response->success = false; - } else { - file.write(reinterpret_cast(map.data), map.size); - if (!file) { - RCLCPP_ERROR(this->get_logger(), "Failed to write to file %s", - filename.c_str()); - response->success = false; - } - file.close(); - } - } - gst_buffer_unmap(buffer, &map); - gst_sample_unref(sample); -} - -void WebRTCStreamer::get_cameras( - const std::shared_ptr request, - std::shared_ptr response) { - response->sources = {}; - for (auto it = source_pads_.cbegin(); it != source_pads_.end(); ++it) { - response->sources.push_back(it->first); - } -} - -void WebRTCStreamer::restart_pipeline( - const std::shared_ptr request, - std::shared_ptr response) { - stop(); - response->success = start(); - if (response->success) { - response->message = "Pipeline restarted successfully"; - } else { - response->message = "Failed to restart pipeline"; - } - GST_DEBUG_BIN_TO_DOT_FILE(GST_BIN(pipeline_.get()), GST_DEBUG_GRAPH_SHOW_ALL, - "restart_pipeline"); - RCLCPP_INFO(this->get_logger(), "%s", response->message.c_str()); -} - -GstElement *WebRTCStreamer::create_vid_conv() { - GstElement *videoconvert = create_element("nvvidconv"); - if (videoconvert) { - return videoconvert; - } - RCLCPP_INFO(this->get_logger(), - "Failed to create nvvidconv, using videoconvert instead"); - return create_element("videoconvert"); -} -GstElement *WebRTCStreamer::create_element(std::string element_type, - std::string element_name) { - GstElement *element = nullptr; - if (element_name.empty()) { - element = gst_element_factory_make(element_type.c_str(), nullptr); - } else { - element = - gst_element_factory_make(element_type.c_str(), element_name.c_str()); - } - if (!element) { - RCLCPP_ERROR(this->get_logger(), "Failed to create %s", - element_type.c_str()); - } - return element; -} - -GstElement * -WebRTCStreamer::add_element_chain(const std::vector &chain) { - GstElement *lastElement = nullptr; - bool success = true; - for (auto element : chain) { - if (!element) { - RCLCPP_ERROR(this->get_logger(), "%s: Bad chain", __FUNCTION__); - success = false; - break; - } - gst_bin_add(GST_BIN(pipeline_.get()), element); - gst_element_sync_state_with_parent(element); - if (!lastElement) { - lastElement = element; - continue; - } - if (!gst_element_link(lastElement, element)) { - char *name1 = gst_element_get_name(lastElement); - char *name2 = gst_element_get_name(element); - RCLCPP_ERROR(this->get_logger(), "Could not link %s with %s", name1, - name2); - g_free(name1); - g_free(name2); - success = false; - break; - } - lastElement = element; - } - if (success) { - elements_.insert(elements_.end(), chain.begin(), chain.end()); - return lastElement; - } - for (auto element : chain) { - if (element) { - gst_bin_remove(GST_BIN(pipeline_.get()), element); - gst_object_unref(element); - } - if (element == lastElement) { - break; - } - } - return nullptr; -} - -static void on_marker_detected(GstElement *element, gint marker_id, - gpointer user_data) { - auto *marker_pub = - static_cast *>(user_data); - std_msgs::msg::Int32 msg; - msg.data = marker_id; - RCLCPP_INFO(rclcpp::get_logger("webrtc_node"), "Marker detected with ID: %d", - marker_id); - if (!marker_pub) { - RCLCPP_ERROR( - rclcpp::get_logger("webrtc_node"), - "Marker publisher is not initialized, cannot publish marker ID"); - return; - } - marker_pub->publish(msg); -} - -GstElement *WebRTCStreamer::create_source(const CameraSource &src) { - std::vector elements; - - const std::string &name = src.name; - const CameraType &type = src.type; - const std::map source_map = { - {CameraType::TestSrc, "videotestsrc"}, - {CameraType::V4l2Src, "v4l2src"}, - {CameraType::NetworkSrc, "udpsrc"}, - }; - - // Add src element(s) - const auto iter = source_map.find(type); - if (iter == source_map.end()) { - RCLCPP_WARN(this->get_logger(), "Unimplemented Type for camera: %s", - name.c_str()); - return nullptr; - } - GstElement *source_element = create_element(iter->second); - if (!source_element) { - return nullptr; - } - elements.push_back(source_element); - if (src.type == CameraType::TestSrc) { - g_object_set(G_OBJECT(source_element), "pattern", 0, nullptr); - g_object_set(G_OBJECT(source_element), "is-live", TRUE, nullptr); - } else if (src.type == CameraType::V4l2Src) { - g_object_set(G_OBJECT(source_element), "device", src.path.c_str(), nullptr); - } else if (src.type == CameraType::NetworkSrc) { - g_object_set(G_OBJECT(source_element), "uri", src.path.c_str(), nullptr); - auto caps = GstUniquePtr(gst_caps_from_string( - "application/x-rtp, media=video, encoding-name=JPEG, payload=26, " - "clock-rate=90000")); - g_object_set(G_OBJECT(source_element), "caps", caps.get(), nullptr); - elements.push_back(create_element("rtpjpegdepay")); - } - // Add jpeg decoders (if necessary) - if (src.encoded) { - elements.emplace_back(create_element("jpegparse")); - elements.emplace_back(create_element("jpegdec")); - } - - // Add small queue that drops oldest buffers - elements.emplace_back(create_element("queue")); - if (!elements.back()) { - for (auto element : elements) { - if (element) - gst_object_unref(element); - } - return nullptr; - } - g_object_set(G_OBJECT(elements.back()), "max-size-buffers", 1, nullptr); - g_object_set(G_OBJECT(elements.back()), "leaky", 2, nullptr); - - if (src.aruco) { - elements.emplace_back(create_element("videoconvert")); - elements.emplace_back(create_element("arucomarker")); - if (!elements.back()) { - elements.pop_back(); - RCLCPP_WARN(this->get_logger(), - "Failed to create aruco marker element, skipping aruco " - "processing"); - } else { - int aruco_detect_interval; - this->get_parameter(name + ".aruco_detect_interval", - aruco_detect_interval); - g_object_set(G_OBJECT(elements.back()), "detect-every", - aruco_detect_interval, nullptr); - g_signal_connect(elements.back(), "marker-detected", - G_CALLBACK(on_marker_detected), marker_pub_.get()); - } - elements.emplace_back(create_element("videoconvert")); - } - - // Add video converter - elements.emplace_back(create_vid_conv()); - // Add tee to connect screen capture to - elements.emplace_back(create_element("tee", name)); - - // Add small queue for dataflow seperations - elements.emplace_back(create_element("queue")); - g_object_set(G_OBJECT(elements.back()), "max-size-buffers", 1, nullptr); - g_object_set(G_OBJECT(elements.back()), "leaky", 2, nullptr); - - elements.emplace_back(create_vid_conv()); - - return add_element_chain(elements); -} - -GstElement *WebRTCStreamer::initialize_pipeline() { - pipeline_ = GstUniquePtr(gst_pipeline_new("webrtc-pipeline")); - std::vector elements; - compositor_ = create_element("nvcompositor"); - if (!compositor_) { - RCLCPP_INFO(this->get_logger(), - "Could not create nvcompositor, using compositor instead"); - compositor_ = create_element("compositor"); - } - elements.push_back(compositor_); - - elements.push_back(create_element("queue")); - g_object_set(G_OBJECT(elements.back()), "max-size-buffers", 1, nullptr); - g_object_set(G_OBJECT(elements.back()), "leaky", 2, nullptr); - - elements.emplace_back(create_vid_conv()); - - elements.emplace_back(create_element("capsfilter")); - GstElement *capsfilter = elements.back(); - assert(capsfilter); - std::stringstream ss; - ss << "video/x-raw(memory:NVMM), height=" << height_ << ", width=" << width_ - << ", framerate=" << framerate_ << "/1"; - auto caps = GstUniquePtr(gst_caps_from_string(ss.str().c_str())); - g_object_set(G_OBJECT(capsfilter), "caps", caps.get(), nullptr); - - elements.emplace_back(create_element("webrtcsink", "webrtcsink")); - GstElement *webrtcsink = elements.back(); - assert(webrtcsink != nullptr); - g_object_set(G_OBJECT(webrtcsink), "run-signalling-server", TRUE, nullptr); - auto video_caps = - GstUniquePtr(gst_caps_from_string("video/x-h265; video/x-h264")); - g_object_set(G_OBJECT(webrtcsink), "video-caps", video_caps.get(), nullptr); - if (web_server_) { - g_object_set(G_OBJECT(webrtcsink), "run-web-server", TRUE, nullptr); - g_object_set(G_OBJECT(webrtcsink), "web-server-host-addr", - "http://0.0.0.0:8080/", nullptr); - g_object_set(G_OBJECT(webrtcsink), "web-server-directory", - web_server_path_.c_str(), nullptr); - } - - if (!add_element_chain(elements)) { - RCLCPP_ERROR(this->get_logger(), "Failed to add elements to pipeline"); - return nullptr; - } - - bus_ = - GstUniquePtr(gst_pipeline_get_bus(GST_PIPELINE(pipeline_.get()))); - if (gst_bus_add_watch(bus_.get(), &WebRTCStreamer::on_bus_message, this)) { - RCLCPP_INFO(this->get_logger(), "Bus watch added successfully."); - } else { - RCLCPP_ERROR(this->get_logger(), "Failed to add bus watch."); - } - return pipeline_.get(); -} -bool WebRTCStreamer::unlink_pad(GstPad *pad) { - if (!pad) { - return false; - } - auto peer = GstUniquePtr(gst_pad_get_peer(pad)); - if (peer) { - if (GST_PAD_IS_SRC(peer.get())) { - gst_pad_unlink(peer.get(), pad); - } else { - gst_pad_unlink(pad, peer.get()); - } - auto parent = gst_pad_get_parent_element(peer.get()); - gst_element_release_request_pad(parent, peer.get()); - gst_object_unref(parent); - } - return true; -} - -bool WebRTCStreamer::update_pipeline( - const std::shared_ptr request) { - if (!pipeline_) { - RCLCPP_ERROR(this->get_logger(), "Pipeline not initialized"); - return false; - } - const auto &total_height = height_; - const auto &total_width = width_; - - int i = 1; - for (auto it = source_pads_.cbegin(); it != source_pads_.end(); ++it) { - auto &pad = it->second; - g_object_set(G_OBJECT(pad.get()), "alpha", 0, NULL); - } - for (const auto &source : request->sources) { - const std::string &name = source.name; - const int height = source.height * total_height / 100; - const int width = source.width * total_width / 100; - const int origin_x = source.origin_x * total_width / 100; - const int origin_y = source.origin_y * total_height / 100; - - auto iter = source_pads_.find(name); - if (iter == source_pads_.end()) { - RCLCPP_WARN(this->get_logger(), "%s: Could not find camera %s", - __FUNCTION__, name.c_str()); - continue; - } - auto &pad = iter->second; - g_object_set(G_OBJECT(pad.get()), "xpos", origin_x, "ypos", origin_y, - "height", height, "width", width, "zorder", i, "alpha", 1.0, - NULL); - ++i; - } - return true; -} - -gboolean WebRTCStreamer::on_bus_message(GstBus *bus, GstMessage *message, - gpointer user_data) { - WebRTCStreamer *streamer = static_cast(user_data); - RCLCPP_INFO(streamer->get_logger(), "Received bus message: %s", - GST_MESSAGE_TYPE_NAME(message)); - - switch (GST_MESSAGE_TYPE(message)) { - case GST_MESSAGE_ERROR: - case GST_MESSAGE_WARNING: - gchar *debug; - GError *err; - gst_message_parse_error(message, &err, &debug); - RCLCPP_ERROR(streamer->get_logger(), "GStreamer Error: %s", err->message); - g_error_free(err); - g_free(debug); - break; - case GST_MESSAGE_STATE_CHANGED: { - GstState old_state, new_state, pending_state; - gst_message_parse_state_changed(message, &old_state, &new_state, - &pending_state); - RCLCPP_INFO(streamer->get_logger(), "State change: %s -> %s", - gst_element_state_get_name(old_state), - gst_element_state_get_name(new_state)); - } break; - default: - break; - } - return TRUE; -} - -int main(int argc, char **argv) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/src/Cameras/camera_streaming_qt/.gitignore b/src/Cameras/camera_streaming_qt/.gitignore deleted file mode 100644 index aa3808c5..00000000 --- a/src/Cameras/camera_streaming_qt/.gitignore +++ /dev/null @@ -1,82 +0,0 @@ -# This file is used to ignore files which are generated -# ---------------------------------------------------------------------------- - -*~ -*.autosave -*.a -*.core -*.moc -*.o -*.obj -*.orig -*.rej -*.so -*.so.* -*_pch.h.cpp -*_resource.rc -*.qm -.#* -*.*# -core -!core/ -tags -.DS_Store -.directory -*.debug -Makefile* -*.prl -*.app -moc_*.cpp -ui_*.h -qrc_*.cpp -Thumbs.db -*.res -*.rc -/.qmake.cache -/.qmake.stash - -# qtcreator generated files -*.pro.user* -*.qbs.user* -CMakeLists.txt.user* - -# xemacs temporary files -*.flc - -# Vim temporary files -.*.swp - -# Visual Studio generated files -*.ib_pdb_index -*.idb -*.ilk -*.pdb -*.sln -*.suo -*.vcproj -*vcproj.*.*.user -*.ncb -*.sdf -*.opensdf -*.vcxproj -*vcxproj.* - -# MinGW generated files -*.Debug -*.Release - -# Python byte code -*.pyc - -# Binaries -# -------- -*.dll -*.exe - -# Directories with generated files -.moc/ -.obj/ -.pch/ -.rcc/ -.uic/ -/build*/ diff --git a/src/Cameras/camera_streaming_qt/CMakeLists.txt b/src/Cameras/camera_streaming_qt/CMakeLists.txt deleted file mode 100644 index ed2475c9..00000000 --- a/src/Cameras/camera_streaming_qt/CMakeLists.txt +++ /dev/null @@ -1,103 +0,0 @@ -cmake_minimum_required(VERSION 3.16) - -project(camera_streaming_qt VERSION 0.1 LANGUAGES CXX) - -set(CMAKE_AUTOUIC ON) -set(CMAKE_AUTOMOC ON) -set(CMAKE_AUTORCC ON) - -set(CMAKE_CXX_STANDARD 17) -set(CMAKE_CXX_STANDARD_REQUIRED ON) - -find_package(rclcpp REQUIRED) -find_package(std_msgs REQUIRED) -find_package(interfaces REQUIRED) -find_package(std_srvs REQUIRED) -find_package(QT NAMES Qt6 Qt5 REQUIRED COMPONENTS Widgets) -find_package(Qt${QT_VERSION_MAJOR} REQUIRED COMPONENTS Widgets) - -pkg_check_modules(GSTREAMER REQUIRED gstreamer-1.0) -pkg_check_modules(GST_APP REQUIRED gstreamer-app-1.0) -pkg_check_modules(GST_VIDEO REQUIRED gstreamer-video-1.0) - -# Include directories required for GStreamer -include_directories( - ${CMAKE_SOURCE_DIR}/include/${PROJECT_NAME} - ${GSTREAMER_INCLUDE_DIRS} - ${GST_APP_INCLUDE_DIRS} - ${GST_VIDEO_INCLUDE_DIRS} -) - -# Link directories for the necessary libraries -link_directories( - ${GSTREAMER_LIBRARY_DIRS} - ${GST_APP_LIBRARY_DIRS} - ${GST_VIDEO_LIBRARY_DIRS} -) - -set(SOURCES - src/mainwindow.cpp - src/widgets/capture_image_widget.cpp - src/widgets/main_widget.cpp - src/widgets/preset_widget.cpp - src/widgets/source_widget.cpp -) - -set(HEADERS - include/${PROJECT_NAME}/camera_client.h - include/${PROJECT_NAME}/mainwindow.h - include/${PROJECT_NAME}/tests/test_camera_node.h - include/${PROJECT_NAME}/widgets/capture_image_widget.h - include/${PROJECT_NAME}/widgets/main_widget.h - include/${PROJECT_NAME}/widgets/preset_widget.h - include/${PROJECT_NAME}/widgets/source_widget.h -) - -# Camera client node -add_executable(camera_client_node src/camera_client.cpp ${SOURCES} ${HEADERS}) - -# Test nodes -add_executable(test_camera_node src/tests/test_camera_node.cpp) - -# Link with the required libraries -target_link_libraries( - camera_client_node - rclcpp::rclcpp - Qt${QT_VERSION_MAJOR}::Widgets - ${GSTREAMER_LIBRARIES} - ${GST_APP_LIBRARIES} - ${GST_VIDEO_LIBRARIES} -) - -target_link_libraries( - test_camera_node - rclcpp::rclcpp -) - -ament_target_dependencies(camera_client_node - rclcpp - std_msgs - interfaces - std_srvs -) - -ament_target_dependencies(test_camera_node - rclcpp - std_msgs - interfaces - std_srvs -) - -install(DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} -) - -install(TARGETS - camera_client_node - test_camera_node - DESTINATION lib/${PROJECT_NAME} -) - -ament_export_include_directories("include/${PROJECT_NAME}") - -ament_package() diff --git a/src/Cameras/camera_streaming_qt/include/camera_streaming_qt/camera_client.h b/src/Cameras/camera_streaming_qt/include/camera_streaming_qt/camera_client.h deleted file mode 100644 index 3bff2815..00000000 --- a/src/Cameras/camera_streaming_qt/include/camera_streaming_qt/camera_client.h +++ /dev/null @@ -1,80 +0,0 @@ -/** - * @file camera_client.h - * @brief Header file for the CameraClient class - * @author Aria Wong - * - * This file contains the declaration of the CameraClient class. It also - * contains the main function which is used to initialize the ROS2 node, Qt and - * gstreamer. The service name is camera_client_node. - */ - -#ifndef CAMERA_CLIENT_H -#define CAMERA_CLIENT_H - -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -/** - * @class CameraClient - * @brief A class that contains functions to get camera source names and start - * the video using ROS2 services. It also contains the gstreamer code which is - * used to display the video feeds. - */ -class CameraClient : public rclcpp::Node { -public: - typedef struct GstData { - GstElement *pipeline; - GstElement *source; - GstElement *convert; - GstElement *sink; - WId winId; - } GstData; - - CameraClient(); - ~CameraClient(); - - /** - * @brief Gets camera source names from the get_cameras service - */ - std::vector get_cameras(); - - /** - * @brief Submits a preset to the ROS2 service and then begins video - */ - void start_video(int num_sources, - std::vector sources); - -private: - /** - * @brief Creates gstreamer pipeline and widget to hold the gstreamer sink - * data. - */ - void open_gst_widget(); - - /** - * @brief Links the gstreamer sink to the widget. - */ - GstBusSyncReply bus_sync_handler(GstBus *bus, GstMessage *message, - GstData *data); - - /** - * @brief Waits for the result from the service. If no result after - * service_result_timeout_ seconds, then stop. - */ - template - bool wait_for_service_result(typename rclcpp::Client::SharedFuture future); - - // How long to wait before timing out when waiting for a service result (in - // seconds) - const int service_result_timeout_ = 1; -}; - -#endif diff --git a/src/Cameras/camera_streaming_qt/include/camera_streaming_qt/mainwindow.h b/src/Cameras/camera_streaming_qt/include/camera_streaming_qt/mainwindow.h deleted file mode 100644 index dff5d4da..00000000 --- a/src/Cameras/camera_streaming_qt/include/camera_streaming_qt/mainwindow.h +++ /dev/null @@ -1,53 +0,0 @@ -/** - * @file mainwindow.h - * @brief Header file for the MainWindow class - * @author Aria Wong - * - * This file contains the declaration of the MainWindow class. - */ -#ifndef MAINWINDOW_H -#define MAINWINDOW_H - -#include -#include -#include -#include - -#include "camera_client.h" -#include "widgets/main_widget.h" -#include "widgets/source_widget.h" - -/** - * @class MainWindow - * @brief This class contains the window that holds all of the widgets in the - * camera client GUI. - */ -class MainWindow : public QMainWindow { - Q_OBJECT - -public: - MainWindow(CameraClient *camera_client = nullptr, QWidget *parent = nullptr); - ~MainWindow(); - -signals: - /** - * @brief Gets the source names of all the cameras from CameraClient - */ - void request_source_names(); - -private slots: - /** - * @brief Gets source names from the CameraClient - */ - void get_source_names(); - - /** - * @brief Gives the preset to the camera client, so it can start the video - */ - void receive_preset(std::vector preset); - -private: - CameraClient *camera_client_; - MainWidget *main_widget_; -}; -#endif // MAINWINDOW_H diff --git a/src/Cameras/camera_streaming_qt/include/camera_streaming_qt/tests/test_camera_node.h b/src/Cameras/camera_streaming_qt/include/camera_streaming_qt/tests/test_camera_node.h deleted file mode 100644 index e31bbb01..00000000 --- a/src/Cameras/camera_streaming_qt/include/camera_streaming_qt/tests/test_camera_node.h +++ /dev/null @@ -1,44 +0,0 @@ -/** - * @file test_camera_node.h - * @brief Header file for the TestCameraNode class. - * @author Aria Wong - * - * This file contains the declaration of the TestCameraNode which is used as a - * test class for the get_cameras and start_video services. - */ -#ifndef TEST_CAMERA_NODE_H -#define TEST_CAMERA_NODE_H - -#include -#include -#include - -/** - * @class TestCameraNode - * @brief A class that contain test functions to simulate the get_cameras and - * start_video services. - */ -class TestCameraNode : public rclcpp::Node { -public: - TestCameraNode(); - ~TestCameraNode(); - - /** - * @brief Returns mock camera source names. - */ - void get_cameras( - const std::shared_ptr request, - std::shared_ptr response); - - rclcpp::Service::SharedPtr get_cameras_service_; - - /** - * @brief Prints the preset from the request and returns true. - */ - void - start_video(const std::shared_ptr request, - std::shared_ptr response); - rclcpp::Service::SharedPtr start_video_service_; -}; - -#endif \ No newline at end of file diff --git a/src/Cameras/camera_streaming_qt/include/camera_streaming_qt/widgets/capture_image_widget.h b/src/Cameras/camera_streaming_qt/include/camera_streaming_qt/widgets/capture_image_widget.h deleted file mode 100644 index 27580a04..00000000 --- a/src/Cameras/camera_streaming_qt/include/camera_streaming_qt/widgets/capture_image_widget.h +++ /dev/null @@ -1,60 +0,0 @@ -/** - * @file capture_image_widget.h - * @brief Header file for the CaptureImageWidget class - * @author Aria Wong - * - * This file contains the declaration of the CaptureImageWidget - * class, which holds the widgets that allow the user to take - * an image of one of the rover sources. - */ - -#ifndef CAPTURE_IMAGE_WIDGET_H -#define CAPTURE_IMAGE_WIDGET_H - -#include -#include -#include -#include -#include -#include - -/** - * @class CaptureImageWidget - * @brief A class that holds the widgets, allowing the user to capture an image. - * - * The CaptureImageWidget class provides the widgets that allow the user to take - * an image of the video sources on the rover. - */ -class CaptureImageWidget : public QWidget { - Q_OBJECT - -public: - CaptureImageWidget(QWidget *parent = nullptr); - ~CaptureImageWidget(); - -public slots: - void set_source_name(QString name); - void set_filename(QString filename); - -private: - QVBoxLayout *main_layout_; - - QLabel *widget_header_; - - // Source name and refresh button - QString source_name_; - QHBoxLayout *source_layout_; - QLabel *source_name_label_; - QComboBox *source_name_combo_box_; - QPushButton *refresh_button_; - - // Filename - QString filename_; - QHBoxLayout *filename_layout_; - QLabel *filename_label_; - QLineEdit *filename_line_edit_; - - QPushButton *capture_image_button_; -}; - -#endif \ No newline at end of file diff --git a/src/Cameras/camera_streaming_qt/include/camera_streaming_qt/widgets/main_widget.h b/src/Cameras/camera_streaming_qt/include/camera_streaming_qt/widgets/main_widget.h deleted file mode 100644 index 235cbe14..00000000 --- a/src/Cameras/camera_streaming_qt/include/camera_streaming_qt/widgets/main_widget.h +++ /dev/null @@ -1,107 +0,0 @@ -/** - * @file main_widget.h - * @brief Header file for the MainWidget class - * @author Aria Wong - * - * This file contains the declaration of the MainWidget class, which is - * responsible for displaying all of the widgets in the camera streaming - * application. - */ - -#ifndef MAIN_WIDGET_H -#define MAIN_WIDGET_H - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "widgets/capture_image_widget.h" -#include "widgets/preset_widget.h" -#include "widgets/source_widget.h" - -/** - * @class MainWidget - * @brief A class for displaying all of the widgets in the camera streaming - * application. - * - * The MainWidget class combines all of the smaller widgets required to use the - * camera streaming application and displays them. - */ -class MainWidget : public QWidget { - Q_OBJECT - -public: - MainWidget(QWidget *parent = nullptr); - ~MainWidget(); - - void receive_source_names(std::vector names); - -signals: - void request_source_names(); - void send_preset(std::vector preset); - -public slots: - /** - * @brief Slot that sets the signal server IP that gets called when the - * server_ip_line_edit_ gets modified. - * - * @param ip IP address for the signal server - */ - void set_signal_server_ip(QString ip); - - /** - * @brief Receives preset to send to CameraClient - */ - void receive_preset(std::vector preset); - -private slots: - /** - * @brief Called when clicking the refresh button in SourceWidget to get the - * source names - */ - void get_source_names(); - -private: - QVBoxLayout *main_layout_; - - // Presets UI - QHBoxLayout *preset_layout_; - - QPushButton *drive_preset_button_; - QPushButton *eef_preset_button_; - QPushButton *microscope_preset_button_; - QPushButton *belly_preset_button_; - QPushButton *drive_eef_preset_button_; - QPushButton *eef_drive_preset_button_; - - // Functions for sending hardcoded presets to CameraClient - void send_drive_preset(); - void send_eef_preset(); - void send_microscope_preset(); - void send_belly_preset(); - void send_drive_eef_preset(); - void send_eef_drive_preset(); - - // Signal server UI - QHBoxLayout *signal_server_layout_; - QLineEdit *server_ip_line_edit_; - QPushButton *server_connect_button_; - - QString signal_server_ip_; - - // Layout for horizontally displaying capture image and preset UIs - QHBoxLayout *controls_layout_; - - // Capture Image UI - CaptureImageWidget *capture_image_widget_; - - // Preset UI - PresetWidget *preset_widget_; -}; - -#endif \ No newline at end of file diff --git a/src/Cameras/camera_streaming_qt/include/camera_streaming_qt/widgets/preset_widget.h b/src/Cameras/camera_streaming_qt/include/camera_streaming_qt/widgets/preset_widget.h deleted file mode 100644 index 43ab1c48..00000000 --- a/src/Cameras/camera_streaming_qt/include/camera_streaming_qt/widgets/preset_widget.h +++ /dev/null @@ -1,92 +0,0 @@ -/** - * @file preset_widget.h - * @brief Header file for the PresetWidget class. - * @author Aria Wong - * - * This file contains the declaration of the PresetWidget class, which - * is holds all of the widgets that lets a user customize a preset. - */ - -#ifndef PRESET_WIDGET_H -#define PRESET_WIDGET_H - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "widgets/source_widget.h" - -/** - * @class PresetWidget - * @brief A class that holds the widgets to allow a user to customize a preset. - * - * The PresetWidget class holds the widgets that let the user customize a - * preset, mainly the list of sources. - */ -class PresetWidget : public QWidget { - Q_OBJECT - -public: - PresetWidget(QWidget *parent = nullptr); - ~PresetWidget(); - -signals: - /** - * @brief Gets the source names from CameraClient - */ - void request_source_names(); - - /** - * @brief Sends this preset to the CameraClient to start video - */ - void send_preset(std::vector preset); - -public slots: - /** - * @brief Adds a new SourceWidget to the preset - */ - void add_source(); - - /** - * @brief Called when clicking on the submit preset button to send this preset - * to the CameraClient. - */ - void submit_preset(); - - /** - * @brief Removes given SourceWidget from this preset and deletes it - */ - void remove_source(SourceWidget *src); - - /** - * @brief Gets source names from CameraClient - */ - void receive_source_names(std::vector sources); - -private slots: - /** - * @brief Called when clicking the refresh button in SourceWidget to get the - * source names - */ - void get_source_names(); - -private: - std::vector sources_; - - QLabel *widget_header_; - - QWidget *sources_container_; - QVBoxLayout *sources_layout_; - QScrollArea *sources_scroll_area_; - QVBoxLayout *main_layout_; - - QPushButton *add_source_button_; - QPushButton *submit_preset_button_; -}; - -#endif \ No newline at end of file diff --git a/src/Cameras/camera_streaming_qt/include/camera_streaming_qt/widgets/source_widget.h b/src/Cameras/camera_streaming_qt/include/camera_streaming_qt/widgets/source_widget.h deleted file mode 100644 index 0671b3e6..00000000 --- a/src/Cameras/camera_streaming_qt/include/camera_streaming_qt/widgets/source_widget.h +++ /dev/null @@ -1,120 +0,0 @@ -/** - * @file source_widget.h - * @brief Header file for the SourceWidget class - * @author Aria Wong - * - * This file contains the declaration of the SourceWidget class, which is - * responsible for containing all of the widgets that allows the user to - * customize the properties of a source. - */ - -#ifndef SOURCE_WIDGET_H -#define SOURCE_WIDGET_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/** - * @class SourceWidget - * @brief A class for displaying and customizing the properties of a source. - * - * The SourceWidget class provides functionality for displaying and customizing - * the properties of a source like the name, width, height, origin X and origin - * Y. - */ -class SourceWidget : public QWidget { - Q_OBJECT - -public: - SourceWidget(QWidget *parent = nullptr); - ~SourceWidget(); - - interfaces::msg::VideoSource *get_source() const { return source_; } - - /** - * @brief Returns get_requesting_source_names - */ - bool get_requesting_source_names() const; - - /** - * @brief Uses names to fill name_combo_box - * - * @param names List of source names - */ - void receive_source_names(std::vector names); - -signals: - /** - * @brief Removes this widget from the preset and deletes itself - * - * @param widget The SourceWidget to remove - */ - void request_remove(SourceWidget *widget); - - /** - * @brief Gets the source names of all the cameras from CameraClient - */ - void request_source_names(); - -public slots: - void set_source_name(QString name); - void set_width(QString width); - void set_height(QString height); - void set_origin_x(QString x); - void set_origin_y(QString y); - - /** - * @brief Called when clicking on the remove source button to remove this - * source from the preset - */ - void remove_source(); - - /** - * @brief Called when clicking the refresh button to get the source names - */ - void get_source_names(); - -private: - // Source model - interfaces::msg::VideoSource *source_; - - QVBoxLayout *main_layout_; - QLabel *source_name_label_; - - // Name UI - QHBoxLayout *name_layout_; - QLabel *name_label_; - QComboBox *name_combo_box_; - QPushButton *refresh_sources_button_; - - // Size UI - QHBoxLayout *size_layout_; - QLabel *width_label_; - QLineEdit *width_line_edit_; - QLabel *height_label_; - QLineEdit *height_line_edit_; - - QIntValidator *size_validator_; - - // Origin UI - QHBoxLayout *origin_layout_; - QLabel *origin_x_label_; - QLineEdit *origin_x_line_edit_; - QLabel *origin_y_label_; - QLineEdit *origin_y_line_edit_; - - QIntValidator *origin_validator_; - - QPushButton *remove_button_; - - bool requesting_source_names_; -}; - -#endif \ No newline at end of file diff --git a/src/Cameras/camera_streaming_qt/package.xml b/src/Cameras/camera_streaming_qt/package.xml deleted file mode 100644 index 8551516a..00000000 --- a/src/Cameras/camera_streaming_qt/package.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - - camera_streaming_qt - 0.0.0 - TODO: Package description - Aria Wong - TODO: License declaration - - ament_cmake - rclcpp - interfaces - - ament_lint_auto - ament_lint_common - - - ament_cmake - - \ No newline at end of file diff --git a/src/Cameras/camera_streaming_qt/src/camera_client.cpp b/src/Cameras/camera_streaming_qt/src/camera_client.cpp deleted file mode 100644 index ea3a904b..00000000 --- a/src/Cameras/camera_streaming_qt/src/camera_client.cpp +++ /dev/null @@ -1,140 +0,0 @@ -#include "../include/camera_streaming_qt/camera_client.h" - -#include -#include -#include - -#include "mainwindow.h" - -CameraClient::CameraClient() : Node("camera_client_node") {} - -CameraClient::~CameraClient() {} - -std::vector CameraClient::get_cameras() { - rclcpp::Client::SharedPtr client = - this->create_client("get_cameras"); - - auto request = std::make_shared(); - - rclcpp::Client::SharedFuture future = - client->async_send_request(request).future.share(); - - // Wait for result, if failed, then return - if (!wait_for_service_result(future)) - return std::vector(); - - std::vector sources = future.get()->sources; - - RCLCPP_INFO(this->get_logger(), "Received sources: "); - - for (int i = 0; i < sources.size(); i++) { - RCLCPP_INFO(this->get_logger(), sources[i].c_str()); - } - - return sources; -} - -void CameraClient::start_video( - int num_sources, std::vector sources) { - rclcpp::Client::SharedPtr client = - this->create_client("start_video"); - - auto request = std::make_shared(); - request->num_sources = num_sources; - request->sources = sources; - - rclcpp::Client::SharedFuture future = - client->async_send_request(request).future.share(); - - // Wait for result, if failed, then return - if (!wait_for_service_result(future)) - return; - - RCLCPP_INFO(get_logger(), "Start video service call succeeded."); - - open_gst_widget(); -} - -void CameraClient::open_gst_widget() { - GstData gst_data; - - // Prepare the pipeline - gst_data.pipeline = gst_pipeline_new("xvoverlay"); - gst_data.source = gst_element_factory_make("videotestsrc", NULL); - gst_data.sink = gst_element_factory_make("ximagesink", NULL); - gst_bin_add_many(GST_BIN(gst_data.pipeline), gst_data.source, gst_data.sink, - NULL); - gst_element_link(gst_data.source, gst_data.sink); - gst_debug_set_active(true); - gst_debug_set_default_threshold(GST_LEVEL_WARNING); - - // Create window where the gstreamer sink output will go to - QWidget window; - window.resize(320, 240); - window.show(); - gst_data.winId = window.winId(); - - // For some reason X11 sink needs time before we can assign to a window (even - // though others don't) So need to use a bus to receive message when we can - // assign the sink to the window - // This code was found from: - // https://stackoverflow.com/questions/68461714/gstreamer-ximagesink-doesnt-work-when-embedded-in-gtk-window - GstBus *bus; - bus = gst_element_get_bus(gst_data.pipeline); - gst_bus_set_sync_handler( - bus, (GstBusSyncHandler)&CameraClient::bus_sync_handler, &gst_data, NULL); - gst_object_unref(bus); - gst_element_set_state(gst_data.pipeline, GST_STATE_PLAYING); -} - -GstBusSyncReply CameraClient::bus_sync_handler(GstBus *bus, GstMessage *msg, - GstData *data) { - if (!data) - return GST_BUS_PASS; - - if (!gst_is_video_overlay_prepare_window_handle_message(msg)) { - return GST_BUS_PASS; - } - - // Assign sink to window - gst_video_overlay_set_window_handle(GST_VIDEO_OVERLAY(GST_MESSAGE_SRC(msg)), - data->winId); - - return GST_BUS_PASS; -} - -template -bool CameraClient::wait_for_service_result( - typename rclcpp::Client::SharedFuture future) { - if (rclcpp::spin_until_future_complete( - this->get_node_base_interface(), future, - std::chrono::seconds(service_result_timeout_)) != - rclcpp::FutureReturnCode::SUCCESS) { - RCLCPP_ERROR(get_logger(), "Service call failed."); - return false; - } - - return true; -} - -int main(int argc, char **argv) { - rclcpp::init(argc, argv); - - auto camera_client = std::make_shared(); - - QApplication a(argc, argv); - - gst_init(&argc, &argv); - - // Main window with all of the settings - MainWindow *w = new MainWindow(camera_client.get(), nullptr); - w->resize(1280, 720); - w->show(); - - a.exec(); - - rclcpp::spin(camera_client); - rclcpp::shutdown(); - - return 0; -} \ No newline at end of file diff --git a/src/Cameras/camera_streaming_qt/src/mainwindow.cpp b/src/Cameras/camera_streaming_qt/src/mainwindow.cpp deleted file mode 100644 index 1db0dd91..00000000 --- a/src/Cameras/camera_streaming_qt/src/mainwindow.cpp +++ /dev/null @@ -1,38 +0,0 @@ -#include "mainwindow.h" - -#include -#include - -#include "widgets/main_widget.h" - -MainWindow::MainWindow(CameraClient *camera_client, QWidget *parent) - : QMainWindow(parent) { - camera_client_ = camera_client; - - // Setup main widget - main_widget_ = new MainWidget(this); - - connect(main_widget_, &MainWidget::request_source_names, this, - &MainWindow::get_source_names); - - connect(main_widget_, &MainWidget::send_preset, this, - &MainWindow::receive_preset); - - setCentralWidget(main_widget_); -} - -MainWindow::~MainWindow() {} - -void MainWindow::get_source_names() { - if (!camera_client_ || !main_widget_) - return; - std::vector names = camera_client_->get_cameras(); - main_widget_->receive_source_names(names); -} - -void MainWindow::receive_preset( - std::vector preset) { - if (!camera_client_) - return; - camera_client_->start_video(preset.size(), preset); -} diff --git a/src/Cameras/camera_streaming_qt/src/tests/test_camera_node.cpp b/src/Cameras/camera_streaming_qt/src/tests/test_camera_node.cpp deleted file mode 100644 index 1b2cb7a7..00000000 --- a/src/Cameras/camera_streaming_qt/src/tests/test_camera_node.cpp +++ /dev/null @@ -1,65 +0,0 @@ -#include "tests/test_camera_node.h" - -#include -#include -#include - -TestCameraNode::TestCameraNode() : Node("test_camera_node") { - get_cameras_service_ = this->create_service( - "get_cameras", std::bind(&TestCameraNode::get_cameras, this, - std::placeholders::_1, std::placeholders::_2)); - RCLCPP_INFO(this->get_logger(), "Service ready: get_cameras"); - - start_video_service_ = this->create_service( - "start_video", std::bind(&TestCameraNode::start_video, this, - std::placeholders::_1, std::placeholders::_2)); - RCLCPP_INFO(this->get_logger(), "Service ready: start_video"); -} - -TestCameraNode::~TestCameraNode() {} - -void TestCameraNode::get_cameras( - const std::shared_ptr request, - std::shared_ptr response) { - std::vector names{"test source 1", "test source 2", - "test source 3"}; - - for (int i = 0; i < names.size(); i++) { - response->sources.push_back(names[i]); - } - - RCLCPP_INFO(this->get_logger(), "Sending response with sources: "); - - for (int i = 0; i < response->sources.size(); i++) { - RCLCPP_INFO(this->get_logger(), response->sources[i].c_str()); - } -} - -void TestCameraNode::start_video( - const std::shared_ptr request, - std::shared_ptr response) { - RCLCPP_INFO(this->get_logger(), - "Received request with the following video sources: "); - - for (int i = 0; i < request->sources.size(); i++) { - interfaces::msg::VideoSource src = request->sources[i]; - RCLCPP_INFO(this->get_logger(), "Name: %s", src.name.c_str()); - RCLCPP_INFO(this->get_logger(), "Width: %d", src.width); - RCLCPP_INFO(this->get_logger(), "Height: %d", src.height); - RCLCPP_INFO(this->get_logger(), "X: %d", src.origin_x); - RCLCPP_INFO(this->get_logger(), "Y: %d", src.origin_y); - } - - response->success = true; -} - -int main(int argc, char **argv) { - rclcpp::init(argc, argv); - - TestCameraNode::SharedPtr get_camera_node = - std::make_shared(); - - rclcpp::spin(get_camera_node); - rclcpp::shutdown(); - return 0; -} diff --git a/src/Cameras/camera_streaming_qt/src/widgets/capture_image_widget.cpp b/src/Cameras/camera_streaming_qt/src/widgets/capture_image_widget.cpp deleted file mode 100644 index 8aab6cbd..00000000 --- a/src/Cameras/camera_streaming_qt/src/widgets/capture_image_widget.cpp +++ /dev/null @@ -1,56 +0,0 @@ -#include "widgets/capture_image_widget.h" - -#include - -CaptureImageWidget::CaptureImageWidget(QWidget *parent) : QWidget(parent) { - main_layout_ = new QVBoxLayout(this); - - widget_header_ = new QLabel("Capture Image"); - main_layout_->addWidget(widget_header_); - - source_layout_ = new QHBoxLayout(); - - // Setup source name and refresh button - source_name_label_ = new QLabel("Name: "); - source_layout_->addWidget(source_name_label_); - - source_name_combo_box_ = new QComboBox(); - connect(source_name_combo_box_, &QComboBox::currentTextChanged, this, - &CaptureImageWidget::set_source_name); - source_layout_->addWidget(source_name_combo_box_); - - refresh_button_ = new QPushButton("Refresh"); - source_layout_->addWidget(refresh_button_); - - main_layout_->addLayout(source_layout_); - - // Setup filename - filename_layout_ = new QHBoxLayout(); - - filename_label_ = new QLabel("Filename (Optional): "); - filename_layout_->addWidget(filename_label_); - - filename_line_edit_ = new QLineEdit(); - connect(filename_line_edit_, &QLineEdit::textChanged, this, - &CaptureImageWidget::set_filename); - filename_layout_->addWidget(filename_line_edit_); - - main_layout_->addLayout(filename_layout_); - - capture_image_button_ = new QPushButton("Capture Image"); - main_layout_->addWidget(capture_image_button_); - - main_layout_->setAlignment(Qt::AlignTop); -} - -CaptureImageWidget::~CaptureImageWidget() {} - -void CaptureImageWidget::set_source_name(QString name) { - source_name_ = name; - qDebug() << "Changed capture image source to: " << name; -} - -void CaptureImageWidget::set_filename(QString filename) { - filename_ = filename; - qDebug() << "Changed capture image filename to: " << filename; -} \ No newline at end of file diff --git a/src/Cameras/camera_streaming_qt/src/widgets/main_widget.cpp b/src/Cameras/camera_streaming_qt/src/widgets/main_widget.cpp deleted file mode 100644 index 54b29722..00000000 --- a/src/Cameras/camera_streaming_qt/src/widgets/main_widget.cpp +++ /dev/null @@ -1,185 +0,0 @@ -#include "widgets/main_widget.h" - -#include - -MainWidget::MainWidget(QWidget *parent) : QWidget(parent) { - main_layout_ = new QVBoxLayout(this); - - // Setup presets UI - preset_layout_ = new QHBoxLayout(); - - drive_preset_button_ = new QPushButton("Drive"); - preset_layout_->addWidget(drive_preset_button_); - connect(drive_preset_button_, &QPushButton::clicked, this, - &MainWidget::send_drive_preset); - - eef_preset_button_ = new QPushButton("EEF"); - preset_layout_->addWidget(eef_preset_button_); - connect(eef_preset_button_, &QPushButton::clicked, this, - &MainWidget::send_eef_preset); - - microscope_preset_button_ = new QPushButton("Microscope"); - preset_layout_->addWidget(microscope_preset_button_); - connect(microscope_preset_button_, &QPushButton::clicked, this, - &MainWidget::send_microscope_preset); - - belly_preset_button_ = new QPushButton("Belly"); - preset_layout_->addWidget(belly_preset_button_); - connect(belly_preset_button_, &QPushButton::clicked, this, - &MainWidget::send_belly_preset); - - drive_eef_preset_button_ = new QPushButton("Drive + EEF"); - preset_layout_->addWidget(drive_eef_preset_button_); - connect(drive_eef_preset_button_, &QPushButton::clicked, this, - &MainWidget::send_drive_eef_preset); - - eef_drive_preset_button_ = new QPushButton("EEF + Drive"); - preset_layout_->addWidget(eef_drive_preset_button_); - connect(eef_drive_preset_button_, &QPushButton::clicked, this, - &MainWidget::send_eef_drive_preset); - - main_layout_->addLayout(preset_layout_); - - // Setup signal server UI - signal_server_layout_ = new QHBoxLayout(); - - server_ip_line_edit_ = new QLineEdit("ws://localhost:8443"); - signal_server_layout_->addWidget(server_ip_line_edit_); - - server_connect_button_ = new QPushButton("Connect"); - signal_server_layout_->addWidget(server_connect_button_); - - connect(server_ip_line_edit_, &QLineEdit::textChanged, this, - &MainWidget::set_signal_server_ip); - - main_layout_->addLayout(signal_server_layout_); - - controls_layout_ = new QHBoxLayout(); - - // Setup capture image UI - capture_image_widget_ = new CaptureImageWidget(); - controls_layout_->addWidget(capture_image_widget_, 1); - - // Setup preset UI - preset_widget_ = new PresetWidget(); - - connect(preset_widget_, &PresetWidget::request_source_names, this, - &MainWidget::get_source_names); - - connect(preset_widget_, &PresetWidget::send_preset, this, - &MainWidget::receive_preset); - - controls_layout_->addWidget(preset_widget_, 1); - - main_layout_->addLayout(controls_layout_); - - // Make widgets start at top - main_layout_->setAlignment(Qt::AlignTop); -} - -MainWidget::~MainWidget() {} - -void MainWidget::receive_source_names(std::vector names) { - if (!preset_widget_) - return; - - preset_widget_->receive_source_names(names); -} - -void MainWidget::receive_preset( - std::vector preset) { - emit send_preset(preset); -} - -void MainWidget::set_signal_server_ip(QString ip) { - signal_server_ip_ = ip; - qDebug() << "IP: " << signal_server_ip_; -} - -void MainWidget::get_source_names() { emit request_source_names(); } - -void MainWidget::send_drive_preset() { - std::vector preset; - interfaces::msg::VideoSource drive; - drive.name = "Drive"; - drive.width = 100; - drive.height = 100; - preset.push_back(drive); - - emit send_preset(preset); -} - -void MainWidget::send_eef_preset() { - std::vector preset; - interfaces::msg::VideoSource eef; - eef.name = "EndEffector"; - eef.width = 100; - eef.height = 100; - preset.push_back(eef); - - emit send_preset(preset); -} - -void MainWidget::send_microscope_preset() { - std::vector preset; - interfaces::msg::VideoSource microscope; - microscope.name = "Microscope"; - microscope.width = 100; - microscope.height = 100; - preset.push_back(microscope); - - emit send_preset(preset); -} - -void MainWidget::send_belly_preset() { - std::vector preset; - interfaces::msg::VideoSource belly; - belly.name = "Bottom"; - belly.width = 100; - belly.height = 100; - preset.push_back(belly); - - emit send_preset(preset); -} - -void MainWidget::send_drive_eef_preset() { - std::vector preset; - - interfaces::msg::VideoSource drive; - interfaces::msg::VideoSource eef; - - drive.name = "Drive"; - drive.width = 100; - drive.height = 100; - preset.push_back(drive); - - eef.name = "EndEffector"; - eef.width = 30; - eef.height = 30; - eef.origin_x = 70; - eef.origin_y = 0; - preset.push_back(eef); - - emit send_preset(preset); -} - -void MainWidget::send_eef_drive_preset() { - std::vector preset; - - interfaces::msg::VideoSource eef; - interfaces::msg::VideoSource drive; - - eef.name = "EndEffector"; - eef.width = 100; - eef.height = 100; - preset.push_back(eef); - - drive.name = "Drive"; - drive.width = 30; - drive.height = 30; - drive.origin_x = 70; - drive.origin_y = 0; - preset.push_back(drive); - - emit send_preset(preset); -} diff --git a/src/Cameras/camera_streaming_qt/src/widgets/preset_widget.cpp b/src/Cameras/camera_streaming_qt/src/widgets/preset_widget.cpp deleted file mode 100644 index db8099c5..00000000 --- a/src/Cameras/camera_streaming_qt/src/widgets/preset_widget.cpp +++ /dev/null @@ -1,92 +0,0 @@ -#include "widgets/preset_widget.h" - -#include - -PresetWidget::PresetWidget(QWidget *parent) : QWidget(parent) { - main_layout_ = new QVBoxLayout(this); - - widget_header_ = new QLabel("Custom Preset"); - main_layout_->addWidget(widget_header_); - - // Create the layout and scroll area that will hold all the sources - sources_container_ = new QWidget(); - sources_layout_ = new QVBoxLayout(sources_container_); - sources_layout_->setAlignment(Qt::AlignTop); - sources_container_->setLayout(sources_layout_); - - sources_scroll_area_ = new QScrollArea(); - sources_scroll_area_->setWidgetResizable(true); - sources_scroll_area_->setWidget(sources_container_); - - main_layout_->addWidget(sources_scroll_area_); - - // Create default source - add_source(); - - add_source_button_ = new QPushButton("Add Source"); - - connect(add_source_button_, &QPushButton::clicked, this, - &PresetWidget::add_source); - - main_layout_->addWidget(add_source_button_); - - submit_preset_button_ = new QPushButton("Submit Preset"); - - connect(submit_preset_button_, &QPushButton::clicked, this, - &PresetWidget::submit_preset); - - main_layout_->addWidget(submit_preset_button_); -} - -PresetWidget::~PresetWidget() {} - -void PresetWidget::add_source() { - if (!sources_layout_) - return; - - SourceWidget *src = new SourceWidget(); - - connect(src, &SourceWidget::request_remove, this, - &PresetWidget::remove_source); - connect(src, &SourceWidget::request_source_names, this, - &PresetWidget::get_source_names); - - sources_.push_back(src); - sources_layout_->addWidget(src); -} - -void PresetWidget::submit_preset() { - std::vector sources; - - // Put all Sources from sources_ in sources - for (int i = 0; i < sources_.size(); i++) { - sources.push_back(*sources_[i]->get_source()); - } - - emit send_preset(sources); -} - -void PresetWidget::remove_source(SourceWidget *src) { - if (!src) - return; - - // Remove src from sources - for (int i = 0; i < sources_.size(); i++) { - if (sources_[i] == src) { - sources_.erase(sources_.begin() + i); - } - } - - src->deleteLater(); -} - -void PresetWidget::receive_source_names(std::vector names) { - // Send source names to all SourceWidgets with requesting_source_names_ on - for (int i = 0; i < sources_.size(); i++) { - if (sources_[i]->get_requesting_source_names()) { - sources_[i]->receive_source_names(names); - } - } -} - -void PresetWidget::get_source_names() { emit request_source_names(); } \ No newline at end of file diff --git a/src/Cameras/camera_streaming_qt/src/widgets/source_widget.cpp b/src/Cameras/camera_streaming_qt/src/widgets/source_widget.cpp deleted file mode 100644 index 8e6c3148..00000000 --- a/src/Cameras/camera_streaming_qt/src/widgets/source_widget.cpp +++ /dev/null @@ -1,162 +0,0 @@ -#include "widgets/source_widget.h" - -#include - -SourceWidget::SourceWidget(QWidget *parent) : QWidget(parent) { - source_ = new interfaces::msg::VideoSource(); - - main_layout_ = new QVBoxLayout(this); - - source_name_label_ = new QLabel("Source"); - main_layout_->addWidget(source_name_label_); - - // Setup name UI - name_layout_ = new QHBoxLayout(); - - name_label_ = new QLabel("Name: "); - name_layout_->addWidget(name_label_); - - name_combo_box_ = new QComboBox(); - - connect(name_combo_box_, &QComboBox::currentTextChanged, this, - &SourceWidget::set_source_name); - - name_layout_->addWidget(name_combo_box_); - - refresh_sources_button_ = new QPushButton("Refresh"); - - connect(refresh_sources_button_, &QPushButton::clicked, this, - &SourceWidget::get_source_names); - - name_layout_->addWidget(refresh_sources_button_); - - main_layout_->addLayout(name_layout_); - - // Setup size UI - size_layout_ = new QHBoxLayout(); - - size_validator_ = new QIntValidator(); - size_validator_->setBottom(0); - - width_label_ = new QLabel("Width: "); - size_layout_->addWidget(width_label_); - - width_line_edit_ = new QLineEdit(QString::number(100)); - width_line_edit_->setValidator(size_validator_); - - connect(width_line_edit_, &QLineEdit::textChanged, this, - &SourceWidget::set_width); - - // TODO: Add default value to config file - set_width("100"); - - size_layout_->addWidget(width_line_edit_); - - height_label_ = new QLabel("Height: "); - size_layout_->addWidget(height_label_); - - height_line_edit_ = new QLineEdit(QString::number(100)); - height_line_edit_->setValidator(size_validator_); - - connect(height_line_edit_, &QLineEdit::textChanged, this, - &SourceWidget::set_height); - - set_height("100"); - - size_layout_->addWidget(height_line_edit_); - - main_layout_->addLayout(size_layout_); - - // Setup origin UI - origin_layout_ = new QHBoxLayout(); - - origin_validator_ = new QIntValidator(); - - origin_x_label_ = new QLabel("Origin X: "); - origin_layout_->addWidget(origin_x_label_); - - origin_x_line_edit_ = new QLineEdit(QString::number(0)); - origin_x_line_edit_->setValidator(origin_validator_); - - connect(origin_x_line_edit_, &QLineEdit::textChanged, this, - &SourceWidget::set_origin_x); - - origin_layout_->addWidget(origin_x_line_edit_); - - origin_y_label_ = new QLabel("Origin Y: "); - origin_layout_->addWidget(origin_y_label_); - - origin_y_line_edit_ = new QLineEdit(QString::number(0)); - origin_y_line_edit_->setValidator(origin_validator_); - - connect(origin_y_line_edit_, &QLineEdit::textChanged, this, - &SourceWidget::set_origin_y); - - origin_layout_->addWidget(origin_y_line_edit_); - - main_layout_->addLayout(origin_layout_); - - // Setup remove button - remove_button_ = new QPushButton("Remove Source"); - - connect(remove_button_, &QPushButton::clicked, this, - &SourceWidget::remove_source); - - main_layout_->addWidget(remove_button_); -} - -SourceWidget::~SourceWidget() {} - -bool SourceWidget::get_requesting_source_names() const { - return requesting_source_names_; -} - -void SourceWidget::receive_source_names(std::vector names) { - name_combo_box_->clear(); - for (int i = 0; i < names.size(); i++) { - name_combo_box_->addItem(QString::fromStdString(names[i])); - } -} - -void SourceWidget::set_source_name(QString name) { - if (!source_) - return; - source_->name = name.toStdString(); - qDebug() << "Changed source name to: " - << QString::fromStdString(source_->name); -} - -void SourceWidget::set_width(QString width) { - if (!source_ || !width.toInt()) - return; - source_->width = width.toInt(); - qDebug() << "Changed source width to: " << source_->width; -} - -void SourceWidget::set_height(QString height) { - if (!source_ || !height.toInt()) - return; - source_->height = height.toInt(); - qDebug() << "Changed source height to: " << source_->height; -} - -void SourceWidget::set_origin_x(QString x) { - if (!source_ || !x.toInt()) - return; - source_->origin_x = x.toInt(); - qDebug() << "Changed source origin x to: " << source_->origin_x; -} - -void SourceWidget::set_origin_y(QString y) { - if (!source_ || !y.toInt()) - return; - source_->origin_y = y.toInt(); - qDebug() << "Changed source origin y to: " << source_->origin_y; -} - -void SourceWidget::remove_source() { emit request_remove(this); } - -void SourceWidget::get_source_names() { - requesting_source_names_ = true; - emit request_source_names(); -} \ No newline at end of file diff --git a/src/Cameras/video_streaming/package.xml b/src/Cameras/video_streaming/package.xml index 3e036343..84955559 100644 --- a/src/Cameras/video_streaming/package.xml +++ b/src/Cameras/video_streaming/package.xml @@ -15,6 +15,8 @@ std_msgs gstreamer-1.0 glib-2.0 + libssl-dev + libglib-dev interfaces diff --git a/src/HW-Devices/science_sensors/science_sensors/can_module_sensor.py b/src/HW-Devices/science_sensors/science_sensors/can_module_sensor.py deleted file mode 100644 index a796d074..00000000 --- a/src/HW-Devices/science_sensors/science_sensors/can_module_sensor.py +++ /dev/null @@ -1,98 +0,0 @@ -#!/usr/bin/env python3 -import rclpy -from rclpy.node import Node -from std_msgs.msg import Float64 -import can -import struct - - -class CANListener(can.Listener): - def __init__(self, process_callback): - super().__init__() - self.process_callback = process_callback - - def on_message_received(self, msg): - self.process_callback(msg) - - -class ScienceReader(Node): - """ - ROS 2 Node that reads sensor data from CAN bus messages. - Assumes each CAN message encodes a float64 (8 bytes). - """ - - def __init__(self): - super().__init__("science_reader") - - self.declare_parameter("sensors", ["ozone", "hydrogen"]) - self.sensors = self.get_parameter("sensors").value - - self.sensor_dict = {} - filters = [] - default_id = 100 - - for sensor in self.sensors: - self.declare_parameter(f"{sensor}.msg_id", default_id) - msg_id = ( - self.get_parameter(f"{sensor}.msg_id") - .get_parameter_value() - .integer_value - ) - if msg_id in self.sensor_dict: - self.get_logger().warn("Duplicate msg ID") - filters.append({"can_id": msg_id, "can_mask": 0x7FF}) - self.sensor_dict[msg_id] = self.create_publisher(Float64, sensor, 10) - default_id += 1 - - # Set up CAN bus with filters - self.bus = can.interface.Bus( - channel="can0", bustype="socketcan", can_filters=filters - ) - - # Set up a listener with a notifier - self.listener = CANListener(self.process_message) - self.notifier = can.Notifier(self.bus, [self.listener]) - - def process_message(self, msg): - msg_id = msg.arbitration_id - if msg_id not in self.sensor_dict: - self.get_logger().warn(f"Unknown CAN ID received: 0x{msg_id:X}") - return - - # Expecting 8 bytes representing a float64 - if len(msg.data) != 8: - self.get_logger().error( - f"Invalid data length from CAN ID 0x{msg_id:X}: {len(msg.data)}" - ) - return - - try: - value = struct.unpack(" MoveServo: - req = MoveServo.Request() - req.port = port - req.pos = pos - req.min = min - req.max = max - self.cli.call_async(req) - - -def main(args=None): - rclpy.init(args=args) - servo_Client = microscope_control() - rclpy.spin(servo_Client) - servo_Client.destroy_node() - - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/src/HW-Devices/science_sensors/science_sensors/panoramic.py b/src/HW-Devices/science_sensors/science_sensors/panoramic.py index 800815c6..db9f1a7a 100644 --- a/src/HW-Devices/science_sensors/science_sensors/panoramic.py +++ b/src/HW-Devices/science_sensors/science_sensors/panoramic.py @@ -6,9 +6,9 @@ from rclpy.node import Node from rclpy.callback_groups import MutuallyExclusiveCallbackGroup from rclpy.executors import MultiThreadedExecutor -from interfaces.srv import MoveServo from interfaces.srv import VideoCapture from threading import Event +from std_msgs.msg import Float32 class PanoramicNode(Node): @@ -18,10 +18,8 @@ def __init__(self): self.load_params() self.callback_group = MutuallyExclusiveCallbackGroup() self.video_callback_group = MutuallyExclusiveCallbackGroup() - self.servo_cli = self.create_client( - MoveServo, - "/servo_service", - ) + self.servo_pan_pub = self.create_publisher(Float32, "/pan", 10) + self.servo_tilt_pub = self.create_publisher(Float32, "/tilt", 10) self.video_cli = self.create_client( VideoCapture, "/capture_frame", callback_group=self.video_callback_group ) @@ -33,17 +31,9 @@ def __init__(self): ) def load_params(self): - self.declare_parameter("tilt_port", 0) - self.declare_parameter("pan_port", 1) self.declare_parameter("camera_name", "Drive") self.declare_parameter("images", 10) self.declare_parameter("sleep", 2.0) - self.pan_port = ( - self.get_parameter("pan_port").get_parameter_value().integer_value - ) - self.tilt_port = ( - self.get_parameter("tilt_port").get_parameter_value().integer_value - ) self.camera_name = ( self.get_parameter("camera_name").get_parameter_value().string_value ) @@ -51,23 +41,16 @@ def load_params(self): self.get_parameter("images").get_parameter_value().integer_value ) self.sleep_time = self.get_parameter("sleep").get_parameter_value().double_value - self.get_logger().info( - f"Parameters - pan_port: {self.pan_port}, tilt_port: {self.tilt_port}, " - f"camera_name: {self.camera_name}, images: {self.num_images}, sleep: {self.sleep_time}" - ) - def move_servo(self, port, angle): - if not self.servo_cli.wait_for_service(timeout_sec=1.0): - self.get_logger().error("Servo service not available, exiting...") - return - request = MoveServo.Request() - request.port = port - request.pos = angle - self.servo_cli.call_async(request) + @staticmethod + def move_servo(publisher, angle): + msg = Float32() + msg.data = angle + publisher.publish(msg) def move_panoramic(self, pan_angle, tilt_angle): - self.move_servo(self.pan_port, pan_angle) - self.move_servo(self.tilt_port, tilt_angle) + self.move_servo(self.servo_pan_pub, pan_angle) + self.move_servo(self.servo_tilt_pub, tilt_angle) def capture_image(self) -> npt.NDArray[np.uint8] | None: if not self.video_cli.wait_for_service(timeout_sec=1.0): diff --git a/src/HW-Devices/science_sensors/science_sensors/pi_gpio_controller.py b/src/HW-Devices/science_sensors/science_sensors/pi_gpio_controller.py deleted file mode 100644 index cedeec3a..00000000 --- a/src/HW-Devices/science_sensors/science_sensors/pi_gpio_controller.py +++ /dev/null @@ -1,62 +0,0 @@ -import rclpy -from rclpy.node import Node -from std_srvs.srv import SetBool -from RPi import GPIO -from rcl_interfaces.msg import ParameterDescriptor, ParameterType - - -class GPIO_Controller(Node): - def __init__(self): - super().__init__("gpio_service_node") - GPIO.setmode(GPIO.BCM) - self.load_parameters() - for pin in self.gpio_pins: - GPIO.setup(pin, GPIO.OUT) - self.srv = self.create_service( - SetBool, self.service_name, self.set_gpio_callback - ) - - def load_parameters(self): - self.declare_parameter( - "gpio_pins", - [0], - ParameterDescriptor(type=ParameterType.PARAMETER_INTEGER_ARRAY), - ) - self.gpio_pins = ( - self.get_parameter("gpio_pins").get_parameter_value().integer_array_value - ) - if not self.gpio_pins: - self.get_logger().error("No GPIO pins provided") - raise ValueError("No GPIO pins provided") - self.declare_parameter("service_name", "set_gpio") - self.service_name = ( - self.get_parameter("service_name").get_parameter_value().string_value - ) - - def set_gpio_callback(self, request, response): - value = GPIO.HIGH if request.data else GPIO.LOW - for pin in self.gpio_pins: - GPIO.output(pin, value) - response.success = True - response.message = ( - f"GPIO pin(s) ({self.gpio_pins}) {'ON' if request.data else 'OFF'}" - ) - self.get_logger().info(response.message) - return response - - -def main(args=None): - rclpy.init(args=args) - node = GPIO_Controller() - try: - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - GPIO.cleanup() - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/src/HW-Devices/science_sensors/science_sensors/pi_gpio_reader.py b/src/HW-Devices/science_sensors/science_sensors/pi_gpio_reader.py deleted file mode 100644 index c49935ba..00000000 --- a/src/HW-Devices/science_sensors/science_sensors/pi_gpio_reader.py +++ /dev/null @@ -1,62 +0,0 @@ -import rclpy -from rclpy.node import Node -from std_msgs.msg import Bool -from RPi import GPIO - - -class GPIOReader(Node): - def __init__(self): - super().__init__("gpio_reader_node") - self.declare_parameter("gpio_pins", []) - self.declare_parameter("interval", 1.0) - - self.gpio_pins = ( - self.get_parameter("gpio_pins").get_parameter_value().integer_array_value - ) - self.interval = ( - self.get_parameter("interval").get_parameter_value().double_value - ) - - if not self.gpio_pins: - self.get_logger().error("No GPIO pins provided") - raise ValueError("No GPIO pins provided") - - GPIO.setmode(GPIO.BCM) - for pin in self.gpio_pins: - GPIO.setup(pin, GPIO.IN) - - self.publishers = { - pin: self.create_publisher(Bool, f"/gpio/{pin}", 10) - for pin in self.gpio_pins - } - - self.timer = self.create_timer(self.interval, self.read_gpio) - - self.get_logger().info( - f"Reading GPIO pins {self.gpio_pins} every {self.interval} seconds" - ) - - def read_gpio(self): - for pin in self.gpio_pins: - value = GPIO.input(pin) - msg = Bool() - msg.data = bool(value) - self.publishers[pin].publish(msg) - self.get_logger().debug(f"Published GPIO {pin}: {msg.data}") - - -def main(args=None): - rclpy.init(args=args) - node = GPIOReader() - try: - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - GPIO.cleanup() - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/src/HW-Devices/science_sensors/science_sensors/plot.py b/src/HW-Devices/science_sensors/science_sensors/plot.py deleted file mode 100644 index c013d59b..00000000 --- a/src/HW-Devices/science_sensors/science_sensors/plot.py +++ /dev/null @@ -1,32 +0,0 @@ -import numpy as np -import matplotlib.pyplot as plt - -# Load the CSV data. -# Adjust the filename if necessary. -data = np.loadtxt("spectrum_data.csv", delimiter=",") - -# Split the data into columns. -wavelength = data[:, 0] -intensity = data[:, 1] - -# Apply the transformation to the wavelength array -wavelength_transformed = ((1 / 785) - (1 / wavelength)) * 1e7 - -# Create a new figure. -plt.figure(figsize=(10, 6)) - -# Plot transformed wavelength vs intensity. -plt.plot(wavelength_transformed, intensity, marker="o", linestyle="-", label="Spectrum") - - -# Add title and labels. -plt.title("StellarNet Spectrum (Transformed Wavelength)") -plt.xlabel("Transformed Wavelength") -plt.ylabel("Intensity") - -# Add a grid and legend. -plt.grid(True) -plt.legend() - -# Display the plot. -plt.show() diff --git a/src/HW-Devices/science_sensors/science_sensors/raman.py b/src/HW-Devices/science_sensors/science_sensors/raman.py deleted file mode 100644 index 5c1f7d0c..00000000 --- a/src/HW-Devices/science_sensors/science_sensors/raman.py +++ /dev/null @@ -1,140 +0,0 @@ -#!/usr/bin/env python3 -import io -import os -import pwd -import time -import numpy as np -import rclpy -from rclpy.node import Node -from science_sensors.stellarnet_driverLibs import stellarnet_driver3 as sn -from interfaces.srv import Raman -from pathlib import Path -from std_srvs.srv import SetBool - -WORKSPACE_ROOT = Path(__file__).resolve().parents[3] - -CSV_DIR = WORKSPACE_ROOT / "raman" -CSV_DIR.mkdir(exist_ok=True) - -# This saves the CSV files to the raman directory in the workspace and returns a csv in the response -# here is how to call this stupid ass service since it runs as root -""" -sudo -E bash -c ' - source /opt/ros/humble/setup.bash - source ~/cprt_rover_24/install/setup.bash - ros2 service call /get_raman_spectrum interfaces/srv/Raman \ - "{inittime: 5000, scansavg: 1, smoothing: 1}" -' -sudo -E bash -c 'source /opt/ros/humble/setup.bash && source ~/cprt_rover_24/install/setup.bash && ros2 run science_sensors raman' - -""" - - -class RamanServ(Node): - def __init__(self) -> None: - super().__init__("raman") - - version = sn.version() - self.get_logger().info(f"StellarNet driver version: {version}") - - self.spectrometer, self.wav = sn.array_get_spec(0) - self.get_logger().info(f"Device ID: {sn.getDeviceId(self.spectrometer)}") - sn.ext_trig(self.spectrometer, True) - self.laser_client = self.create_client(SetBool, "/raman_light") - - self.srv = self.create_service( - Raman, - "get_raman_spectrum", - self.handle_raman_request, - ) - self.get_logger().info("Raman service ready") - - def handle_raman_request( - self, request: Raman.Request, response: Raman.Response - ) -> Raman.Response: - self.get_logger().info( - f"Request int={request.inittime} ms " - f"avg={request.scansavg} smooth={request.smoothing}" - ) - self._set_laser(False) - time.sleep(0.5) # Allow time for the laser to turn off - self.get_logger().info("Capturing spectrum...") - - data_off = self._get_spectrum( - request.inittime, request.scansavg, request.smoothing - ) - - self._set_laser(True) - time.sleep(0.5) # Allow time for the laser to turn on - - data_on = self._get_spectrum( - request.inittime, request.scansavg, request.smoothing - ) - data = np.column_stack((data_on[:, 0], data_on[:, 1] - data_off[:, 1])) - - ts = time.strftime("%Y%m%d_%H%M%S") - runParams = ( - "_t_" - + str(request.inittime) - + "_savg_" - + str(request.scansavg) - + "_s_" - + str(request.smoothing) - ) - filename = f"{CSV_DIR}/raman_{ts}{runParams}.csv" - np.savetxt(filename, data, delimiter=",", fmt="%.6f") - _chown_to_invoking_user(filename) - - buf = io.StringIO() - np.savetxt(buf, data, delimiter=",", fmt="%.6f") - response.csv = buf.getvalue() - - self.get_logger().info("Spectrum captured and sent") - self.get_logger().info(f"Saved to {filename}") - self._set_laser(False) - return response - - def _get_spectrum(self, inttime: int, scansavg: int, smooth: int): - """Grab one spectrum (private helper).""" - self.spectrometer["device"].set_config( - int_time=inttime, - scans_to_avg=scansavg, - x_smooth=smooth, - ) - return sn.array_spectrum(self.spectrometer, self.wav) - - def _set_laser(self, state: bool) -> None: - """Turn the laser on or off.""" - if not self.laser_client.wait_for_service(timeout_sec=1.0): - self.get_logger().error("Laser service not available, exiting...") - return - request = SetBool.Request() - # we use a relay of low being on - request.data = not state - self.laser_client.call_async(request) - - -def _chown_to_invoking_user(path: str) -> None: - """Make the output file belong to the original user (not root).""" - user = os.getenv("SUDO_USER") or os.getenv("USER") - if not user: - return - try: - pw = pwd.getpwnam(user) - os.chown(path, pw.pw_uid, pw.pw_gid) - except (KeyError, PermissionError): - pass - - -def main(args=None) -> None: - rclpy.init(args=args) - node = RamanServ() - rclpy.spin(node) - sn.reset(node.spectrometer) - - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_demo.py b/src/HW-Devices/science_sensors/science_sensors/stellarnet_demo.py deleted file mode 100644 index 820769ee..00000000 --- a/src/HW-Devices/science_sensors/science_sensors/stellarnet_demo.py +++ /dev/null @@ -1,64 +0,0 @@ -from science_sensors.stellarnet_driverLibs import stellarnet_driver3 as sn - -# For Windows ONLY: Must be run in administrator mode -# Only need to run it one time after switch back from the SpectraWiz. Or manually run InstallDriver.exe located in stellarnet_driverLis/windows_only -# sn.installDeviceDriver() - -# This resturn a Version number of compilation date of driver -version = sn.version() -print(version) - -# Device parameters to set -inttime = 50 -scansavg = 1 -smooth = 0 -xtiming = 3 - -# init Spectrometer - Get BOTH spectrometer and wavelength -spectrometer, wav = sn.array_get_spec( - 0 -) # 0 for first channel and 1 for second channel , up to 127 spectrometers -""" -# Equivalent to get spectrometer and wav separately: -spectrometer = sn.array_get_spec_only(0) -wav = sn.getSpectrum_X(spectrometer) -""" - -print(spectrometer) -sn.ext_trig(spectrometer, True) - -# Get device ID -deviceID = sn.getDeviceId(spectrometer) -print("\nMy device ID: ", deviceID) - -# Get current device parameter -currentParam = sn.getDeviceParam(spectrometer) - -# Call to Enable or Disable External Trigger to by default is Disbale=False -> with timeout -# Enable or Disable Ext Trigger by Passing True or False, If pass True than Timeout function will be disable, so user can also use this function as timeout enable/disbale -sn.ext_trig(spectrometer, True) - - -# Only call this function on first call to get spectrum or when you want to change device setting. -# -- Set last parameter to 'True' throw away the first spectrum data because the data may not be true for its inttime after the update. -# -- Set to 'False' if you don't want to do another capture to throw away the first data, however your next spectrum data might not be valid. -sn.setParam(spectrometer, inttime, scansavg, smooth, xtiming, True) - -# Get spectrometer data - Get BOTH X and Y in single return -first_data = sn.array_spectrum(spectrometer, wav) # get specturm for the first time -""" -# Get Y value ONLY : -first_data = sn.getSpectrum_Y(spectrometer) -""" -print("First data:", first_data) - -# ============================================== -# Burst FIFO mode: Not recommended with high integration time. -# burst_data_2 = sn.getBurstFifo_Y(spectrometer) -# ============================================== - -# Release the spectrometer before ends the program -sn.reset(spectrometer) - -# For Windows ONLY: Must be run in administrator mode -# sn.uninstallDeviceDriver() diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/__init__.py b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp27-win32.pyd b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp27-win32.pyd deleted file mode 100644 index 42f8322a..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp27-win32.pyd and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp27-win_amd64.pyd b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp27-win_amd64.pyd deleted file mode 100644 index aeae74b3..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp27-win_amd64.pyd and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp310-win32.pyd b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp310-win32.pyd deleted file mode 100644 index 4656aca4..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp310-win32.pyd and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp310-win_amd64.pyd b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp310-win_amd64.pyd deleted file mode 100644 index 6d32a42e..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp310-win_amd64.pyd and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp311-win_amd64.pyd b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp311-win_amd64.pyd deleted file mode 100644 index 00f03da7..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp311-win_amd64.pyd and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp312-win_amd64.pyd b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp312-win_amd64.pyd deleted file mode 100644 index 8e3fe859..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp312-win_amd64.pyd and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp35-win32.pyd b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp35-win32.pyd deleted file mode 100644 index 2d49a7c2..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp35-win32.pyd and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp35-win_amd64.pyd b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp35-win_amd64.pyd deleted file mode 100644 index d9c86637..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp35-win_amd64.pyd and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp36-win32.pyd b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp36-win32.pyd deleted file mode 100644 index 692ce50f..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp36-win32.pyd and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp36-win_amd64.pyd b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp36-win_amd64.pyd deleted file mode 100644 index 32c76d86..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp36-win_amd64.pyd and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp37-win32.pyd b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp37-win32.pyd deleted file mode 100644 index 8e476818..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp37-win32.pyd and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp37-win_amd64.pyd b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp37-win_amd64.pyd deleted file mode 100644 index 8f4ebbcc..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp37-win_amd64.pyd and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp38-win32.pyd b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp38-win32.pyd deleted file mode 100644 index 068d2c9a..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp38-win32.pyd and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp38-win_amd64.pyd b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp38-win_amd64.pyd deleted file mode 100644 index 87354054..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp38-win_amd64.pyd and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp39-win32.pyd b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp39-win32.pyd deleted file mode 100644 index 41656970..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp39-win32.pyd and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp39-win_amd64.pyd b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp39-win_amd64.pyd deleted file mode 100644 index 381fc29d..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp39-win_amd64.pyd and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-310-aarch64-linux-gnu.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-310-aarch64-linux-gnu.so deleted file mode 100644 index 6830fdcd..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-310-aarch64-linux-gnu.so and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-310-arm-linux-gnueabihf.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-310-arm-linux-gnueabihf.so deleted file mode 100644 index aa50cb9d..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-310-arm-linux-gnueabihf.so and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-310-darwin.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-310-darwin.so deleted file mode 100644 index 3ca08a14..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-310-darwin.so and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-310-x86_64-linux-gnu.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-310-x86_64-linux-gnu.so deleted file mode 100644 index 6acdeffa..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-310-x86_64-linux-gnu.so and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-311-aarch64-linux-gnu.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-311-aarch64-linux-gnu.so deleted file mode 100644 index c1af8bda..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-311-aarch64-linux-gnu.so and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-311-arm-linux-gnueabihf.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-311-arm-linux-gnueabihf.so deleted file mode 100644 index 3295ca9c..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-311-arm-linux-gnueabihf.so and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-311-darwin.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-311-darwin.so deleted file mode 100644 index 46589c15..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-311-darwin.so and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-311-x86_64-linux-gnu.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-311-x86_64-linux-gnu.so deleted file mode 100644 index 665d1acf..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-311-x86_64-linux-gnu.so and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-312-aarch64-linux-gnu.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-312-aarch64-linux-gnu.so deleted file mode 100644 index 531bef4a..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-312-aarch64-linux-gnu.so and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-312-darwin.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-312-darwin.so deleted file mode 100644 index 815c382a..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-312-darwin.so and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-312-x86_64-linux-gnu.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-312-x86_64-linux-gnu.so deleted file mode 100644 index ff62094e..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-312-x86_64-linux-gnu.so and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-35m-x86_64-linux-gnu.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-35m-x86_64-linux-gnu.so deleted file mode 100644 index 8a4f739e..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-35m-x86_64-linux-gnu.so and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-36m-aarch64-linux-gnu.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-36m-aarch64-linux-gnu.so deleted file mode 100644 index dadf2c6a..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-36m-aarch64-linux-gnu.so and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-36m-x86_64-linux-gnu.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-36m-x86_64-linux-gnu.so deleted file mode 100644 index 8376eaf0..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-36m-x86_64-linux-gnu.so and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-37m-aarch64-linux-gnu.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-37m-aarch64-linux-gnu.so deleted file mode 100644 index 616d8ccd..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-37m-aarch64-linux-gnu.so and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-37m-arm-linux-gnueabihf.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-37m-arm-linux-gnueabihf.so deleted file mode 100644 index 6d6b6e25..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-37m-arm-linux-gnueabihf.so and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-37m-darwin.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-37m-darwin.so deleted file mode 100644 index c14ab481..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-37m-darwin.so and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-37m-x86_64-linux-gnu.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-37m-x86_64-linux-gnu.so deleted file mode 100644 index 70293b43..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-37m-x86_64-linux-gnu.so and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-38-aarch64-linux-gnu.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-38-aarch64-linux-gnu.so deleted file mode 100644 index dd496ff2..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-38-aarch64-linux-gnu.so and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-38-arm-linux-gnueabihf.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-38-arm-linux-gnueabihf.so deleted file mode 100644 index b90e8024..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-38-arm-linux-gnueabihf.so and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-38-darwin.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-38-darwin.so deleted file mode 100644 index 380e8414..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-38-darwin.so and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-38-x86_64-linux-gnu.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-38-x86_64-linux-gnu.so deleted file mode 100644 index cc5529da..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-38-x86_64-linux-gnu.so and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-39-aarch64-linux-gnu.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-39-aarch64-linux-gnu.so deleted file mode 100644 index d483c923..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-39-aarch64-linux-gnu.so and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-39-darwin.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-39-darwin.so deleted file mode 100644 index fead0027..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-39-darwin.so and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-39-x86_64-linux-gnu.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-39-x86_64-linux-gnu.so deleted file mode 100644 index e97d66d3..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-39-x86_64-linux-gnu.so and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/InstallDriver.exe b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/InstallDriver.exe deleted file mode 100644 index e553cc96..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/InstallDriver.exe and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/StellarNet_Python_USB_Driver.cat b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/StellarNet_Python_USB_Driver.cat deleted file mode 100644 index 045603f2..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/StellarNet_Python_USB_Driver.cat and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/StellarNet_Python_USB_Driver.inf b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/StellarNet_Python_USB_Driver.inf deleted file mode 100644 index bd5c63a3..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/StellarNet_Python_USB_Driver.inf and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/dpinst32.exe b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/dpinst32.exe deleted file mode 100644 index 410a135a..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/dpinst32.exe and /dev/null differ diff --git a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/dpinst64.exe b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/dpinst64.exe deleted file mode 100644 index 00964418..00000000 Binary files a/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/dpinst64.exe and /dev/null differ diff --git a/src/HW-Devices/science_sensors/setup.py b/src/HW-Devices/science_sensors/setup.py index ebdb362a..f948852c 100644 --- a/src/HW-Devices/science_sensors/setup.py +++ b/src/HW-Devices/science_sensors/setup.py @@ -32,12 +32,7 @@ entry_points={ "console_scripts": [ "gas_sensor = science_sensors.gas_sensor:main", - "microscope_control = science_sensors.microscope_control:main", - "raman = science_sensors.raman:main", - "pi_gpio_controller = science_sensors.pi_gpio_controller:main", - "pi_gpio_reader = science_sensors.pi_gpio_reader:main", "panoramic = science_sensors.panoramic:main", - "can_module_sensor = science_sensors.can_module_sensor:main", ], }, ) diff --git a/src/HW-Devices/servo_pkg/config/pi_controller.yaml b/src/HW-Devices/servo_pkg/config/pi_controller.yaml deleted file mode 100644 index 06c158cc..00000000 --- a/src/HW-Devices/servo_pkg/config/pi_controller.yaml +++ /dev/null @@ -1,15 +0,0 @@ -pi_Servo_node: - ros__parameters: - servos_used: 2 - # Microscope Servo - servo0: - name: "microscope" - min: 2.5 # Min PWM value - max: 12.5 # Max PWM value - rom: 3.1415 # 180 degrees - # Collection Servo - servo1: - name: "collection" - min: 4.5 # Min PWM value - max: 10.5 # Max PWM value - rom: 3.1415 # 180 degrees \ No newline at end of file diff --git a/src/HW-Devices/servo_pkg/config/servo_client.yaml b/src/HW-Devices/servo_pkg/config/servo_client.yaml deleted file mode 100644 index 3998a0cf..00000000 --- a/src/HW-Devices/servo_pkg/config/servo_client.yaml +++ /dev/null @@ -1,9 +0,0 @@ -Servo_Client_node: - ros__parameters: - servo_num: 1 # the servo currently being used - servo0: - name: "tilt" - servo1: - name: "pan" - servo3: - name: "gripper" \ No newline at end of file diff --git a/src/HW-Devices/servo_pkg/launch/i2c_servo_launch.launch.py b/src/HW-Devices/servo_pkg/launch/i2c_servo_launch.launch.py deleted file mode 100644 index 6a86d1e1..00000000 --- a/src/HW-Devices/servo_pkg/launch/i2c_servo_launch.launch.py +++ /dev/null @@ -1,23 +0,0 @@ -import launch -import launch_ros.actions -import os -from ament_index_python.packages import get_package_share_directory - - -def generate_launch_description(): - pkg_servo = get_package_share_directory("servo_pkg") - parent_params = os.path.join(pkg_servo, "config", "parent_config.yaml") - - return launch.LaunchDescription( - [ - launch_ros.actions.Node( - package="servo_pkg", - executable="i2c_Servo", - name="USB_Servo_node", - parameters=[parent_params], - ), - launch_ros.actions.Node( - package="servo_pkg", executable="servo_client", name="servo_client_node" - ), - ] - ) diff --git a/src/HW-Devices/servo_pkg/launch/pi_servo_launch.launch.py b/src/HW-Devices/servo_pkg/launch/pi_servo_launch.launch.py deleted file mode 100644 index ae00fedd..00000000 --- a/src/HW-Devices/servo_pkg/launch/pi_servo_launch.launch.py +++ /dev/null @@ -1,22 +0,0 @@ -import launch -import launch_ros.actions -import os -from ament_index_python.packages import get_package_share_directory - - -def generate_launch_description(): - pkg_servo = get_package_share_directory("servo_pkg") - child_params = os.path.join(pkg_servo, "config", "pi_controller.yaml") - parent_params = os.path.join(pkg_servo, "config", "parent_config.yaml") - - return launch.LaunchDescription( - [ - launch_ros.actions.Node( - package="servo_pkg", - executable="pi_Servo", - name="pi_Servo_node", - parameters=[parent_params, child_params], - remappings=[("servo_service", "science_servo_service")], - ) - ] - ) diff --git a/src/Nav/elevation_mapping/package.xml b/src/Nav/elevation_mapping/package.xml index 98bb8c4a..10284c03 100644 --- a/src/Nav/elevation_mapping/package.xml +++ b/src/Nav/elevation_mapping/package.xml @@ -20,7 +20,6 @@ grid_map_ros grid_map_msgs grid_map_filters - kindr kindr_ros message_filters pcl_ros diff --git a/src/Nav/kindr_ros/kindr_ros/package.xml b/src/Nav/kindr_ros/kindr_ros/package.xml index 161670c1..58463898 100644 --- a/src/Nav/kindr_ros/kindr_ros/package.xml +++ b/src/Nav/kindr_ros/kindr_ros/package.xml @@ -15,7 +15,6 @@ ament_cmake - kindr geometry_msgs tf2 diff --git a/src/Teleop-Control/drive_cpp/CMakeLists.txt b/src/Teleop-Control/drive_cpp/CMakeLists.txt deleted file mode 100644 index f13a6ff5..00000000 --- a/src/Teleop-Control/drive_cpp/CMakeLists.txt +++ /dev/null @@ -1,83 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(drive_cpp) - -# Find the required packages -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(pluginlib REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(ros_phoenix REQUIRED) - -# Specify the C++ standard -set(CMAKE_CXX_STANDARD 14) - -# Include directories -include_directories(include/${PROJECT_NAME}) - -# ======================================================== -# Build shared library for TalonDriveController component -add_library(talon_drive_controller SHARED - src/TalonDriveController.cpp - src/WheelControl.cpp -) - -# Link component with dependencies -ament_target_dependencies(talon_drive_controller - rclcpp - rclcpp_components - pluginlib - geometry_msgs - nav_msgs - ros_phoenix -) - -# Register the node as a component -rclcpp_components_register_nodes(talon_drive_controller "TalonDriveController") - -# Install the shared library -install(TARGETS - talon_drive_controller - DESTINATION lib/ -) - -# ======================================================== -add_executable(drive_cpp_node src/main.cpp) - -target_link_libraries(drive_cpp_node - talon_drive_controller -) - -ament_target_dependencies(drive_cpp_node - rclcpp - geometry_msgs - nav_msgs - ros_phoenix -) - -install(TARGETS - drive_cpp_node - DESTINATION lib/${PROJECT_NAME} -) -install( - FILES talon_drive_controller_plugin.xml - DESTINATION share/${PROJECT_NAME} -) - - -# ======================================================== -# Install headers -install(DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} -) -install(DIRECTORY launch - DESTINATION share/${PROJECT_NAME} -) -install(DIRECTORY config - DESTINATION share/${PROJECT_NAME} -) -ament_export_include_directories("include/${PROJECT_NAME}") - -ament_package() -pluginlib_export_plugin_description_file(rclcpp_components talon_drive_controller_plugin.xml) diff --git a/src/Teleop-Control/drive_cpp/config/pxn.yaml b/src/Teleop-Control/drive_cpp/config/pxn.yaml deleted file mode 100644 index 647efbc6..00000000 --- a/src/Teleop-Control/drive_cpp/config/pxn.yaml +++ /dev/null @@ -1,79 +0,0 @@ -flightstick_control: - ros__parameters: - drive_mode_button: 11 - arm_ik_mode_button: 9 - arm_manual_mode_button: 10 - nav_mode_button: 6 - science_mode_button: 7 - teleop_light_mode: 2 - arm_dummy_mode_button: 8 - - drive_mode: - throttle: - axis: 2 - min: -1.0 - max: 1.0 - forward_axis: 1 - yaw_axis: 0 - cam_tilt_axis: 5 - cam_pan_axis: 4 - cam_reset: 1 - lights_up: 5 - lights_down: 3 - camera_speed: 3.0 - cruise_control: 0 - max_linear: 2.0 - max_angular: 1.0 - max_increment: 0.1 - min_speed: 0.001 - default_pan: 3.14159 - default_tilt: 0.785398 - - arm_manual_mode: - throttle: - axis: 2 - min: -1.0 - max: 1.0 - base_axis: 0 - wrist_roll: 4 - wrist_yaw_positive: 3 - wrist_yaw_negative: 5 - act1_axis: 1 - act2_axis: 5 - elbow_yaw: 3 - claw_open: 1 - claw_close: 0 - simple_forward: 2 - simple_backward: 4 - servo_name: "arm" - - arm_ik_mode: - x_axis: 1 - y_axis: 0 - up_button: 5 - down_button: 3 - rotate_around_y: 5 - rotate_around_x: 4 - rotate_around_z: 3 - open_claw: 1 - close_claw: 0 - base_frame: 4 - eef_frame: 2 - servo_name: "arm" - - science_mode: - platform_axis: 1 - drill_button: 0 - microscope_axis: 5 - soil_collection_button: 5 - cancel_collection_button: 3 - soil_test_button: 2 - lock_sample_button: 4 - microscope_light_button: 4 - microscope_servo_name: "microscope" - collection_servo_name: "collection" - collection_open: 2.967060 - collection_dump: 1.396263 - collection_test: 0.0 - collection_lock: 3.141593 - throttle_axis: 2 diff --git a/src/Teleop-Control/drive_cpp/config/talon_drive_common.yaml b/src/Teleop-Control/drive_cpp/config/talon_drive_common.yaml deleted file mode 100644 index f94a609c..00000000 --- a/src/Teleop-Control/drive_cpp/config/talon_drive_common.yaml +++ /dev/null @@ -1,10 +0,0 @@ -/**: - ros__parameters: - interface: "can0" - watchdog_ms: 300 - P: 2.00 - I: 0.0 - D: 0.0 - max_voltage: 24.0 - brake_mode: true - sensor_multiplier: 0.0003623188 # 1/2760 diff --git a/src/Teleop-Control/drive_cpp/config/talon_drive_unique.yaml b/src/Teleop-Control/drive_cpp/config/talon_drive_unique.yaml deleted file mode 100644 index 85d17249..00000000 --- a/src/Teleop-Control/drive_cpp/config/talon_drive_unique.yaml +++ /dev/null @@ -1,28 +0,0 @@ -frontLeft: - ros__parameters: - id: 3 - -backLeft: - ros__parameters: - id: 4 - -frontRight: - ros__parameters: - id: 1 - -backRight: - ros__parameters: - id: 2 - -talon_drive_controller: - ros__parameters: - wheels: ["frontRight", "frontLeft", "backRight", "backLeft"] - max_speed: 2.0 - base_width: 0.9 - pub_odom: true - pub_elec: true - wheel_rad: 0.10 - angular_slip_ratio: 0.6 - low_latency_mode: true - is_open_loop: true - open_loop_scalar: 0.5 # Converts m/s of commanded velocity to a percent output diff --git a/src/Teleop-Control/drive_cpp/config/thrustmaster.yaml b/src/Teleop-Control/drive_cpp/config/thrustmaster.yaml deleted file mode 100644 index af12f1a4..00000000 --- a/src/Teleop-Control/drive_cpp/config/thrustmaster.yaml +++ /dev/null @@ -1,27 +0,0 @@ -flightstick_control: - ros__parameters: - drive_mode_button: 12 - arm_ik_mode_button: 11 - arm_manual_mode_button: 10 - nav_mode_button: 13 - science_mode_button: 14 - - drive_mode: - throttle: - axis: 3 - min: -1.0 - max: 1.0 - forward_axis: 1 - yaw_axis: 0 - cam_tilt_axis: 5 - cam_pan_axis: 4 - cam_reset: 1 - cam_next: 3 - cam_prev: 2 - cruise_control: 0 - max_linear: 2.0 - max_angular: 1.0 - max_increment: 0.1 - min_speed: 0.001 - - \ No newline at end of file diff --git a/src/Teleop-Control/drive_cpp/include/drive_cpp/TalonDriveController.hpp b/src/Teleop-Control/drive_cpp/include/drive_cpp/TalonDriveController.hpp deleted file mode 100644 index 67cd4729..00000000 --- a/src/Teleop-Control/drive_cpp/include/drive_cpp/TalonDriveController.hpp +++ /dev/null @@ -1,137 +0,0 @@ -#ifndef TALON_DRIVE_CONTROLLER_HPP -#define TALON_DRIVE_CONTROLLER_HPP - -#include "WheelControl.hpp" -#include "geometry_msgs/msg/twist.hpp" -#include "nav_msgs/msg/odometry.hpp" -#include "rclcpp/rclcpp.hpp" - -using namespace geometry_msgs::msg; -using namespace nav_msgs::msg; - -/** - * @brief The TalonDriveController class is responsible for controlling a - * robot's drive system using Talon motors. It subscribes to a twist message to - * control the robot's motion and publishes odometry information. Assumes - * diff drive and equal number of left and right wheels - */ -class TalonDriveController : public rclcpp::Node { -public: - /** - * @brief Construct a new TalonDriveController object. - */ - explicit TalonDriveController(const rclcpp::NodeOptions &options); - -private: - /** - * @brief Callback to publish odometry information. - */ - void odom_pub_callback(); - - /** - * @brief Callback to control the motor velocities based on the twist command. - */ - void control_timer_callback(); - - /** - * @brief Callback to process incoming Twist messages. - * - * @param msg The Twist message containing linear and angular velocity - * commands. - */ - void twist_callback(const Twist::SharedPtr msg); - - /** - * @brief The maximum speed the robot can travel at, in meters per second. - */ - double maxSpeed_; - - /** - * @brief The width of the robot's base, used for computing turn radius and - * angular velocity. - */ - double baseWidth_; - - /** - * @brief Flag indicating whether to publish odometry information. - */ - bool pubOdom_; - - /** - * @brief Frame id used for odometry output - */ - std::string frameId_; - - /** - * @brief Covariance value for angular odometry - */ - double angularCov_; - - /** - * @brief Covariance value for linear odometry - */ - double linearCov_; - - /** - * @brief Circumference of the wheels, used for computing velocity. - */ - double wheelCircumference_; - - /** - * @brief Ratio of expected slip on angular movement (must be > 0 and <=1) - * ~0 means can not turn, 1 means no slip - */ - double angularSlipRatio_; - - /** - * @brief Flag indicating whether to publish electrical status - * information. (Not implemented) - */ - bool pubElec_; - - /** - * @brief Timeout before shutting down motors if no control messages are - * received. - */ - double timeout_; - - /** - * @brief The timestamp of the last control message. - */ - double lastTimestamp_; - - /** - * @brief The list of wheels controlled by this drive controller. - */ - std::vector wheels_; - - bool low_latency_mode_; - - /** - * @brief The list of motor status subscriptions, one for each wheel. - */ - std::vector::SharedPtr> statusSubs_; - - /** - * @brief The subscription to the Twist messages for controlling the robot's - * motion. - */ - rclcpp::Subscription::SharedPtr twistSub_; - - /** - * @brief The publisher for sending odometry information. - */ - rclcpp::Publisher::SharedPtr odomPub_; - - /** - * @brief Timer to periodically send control commands to the wheels. - */ - rclcpp::TimerBase::SharedPtr timer_; - - /** - * @brief Timer to periodically publish odometry data. - */ - rclcpp::TimerBase::SharedPtr odomTimer_; -}; - -#endif // TALON_DRIVE_CONTROLLER_HPP diff --git a/src/Teleop-Control/drive_cpp/include/drive_cpp/WheelControl.hpp b/src/Teleop-Control/drive_cpp/include/drive_cpp/WheelControl.hpp deleted file mode 100644 index dc604615..00000000 --- a/src/Teleop-Control/drive_cpp/include/drive_cpp/WheelControl.hpp +++ /dev/null @@ -1,124 +0,0 @@ -#ifndef WHEEL_CONTROL_HPP -#define WHEEL_CONTROL_HPP - -#include "rclcpp/rclcpp.hpp" -#include "ros_phoenix/msg/motor_control.hpp" -#include "ros_phoenix/msg/motor_status.hpp" - -using namespace ros_phoenix::msg; - -/** - * @brief Enum representing the two possible sides of a wheel: LEFT and RIGHT. - */ -enum class WheelSide { LEFT, RIGHT }; - -/** - * @brief The WheelControl class is responsible for controlling a single wheel - * of the robot. It manages the wheel's velocity and publishes motor control - * messages to control the wheel. - */ -class WheelControl { -public: - /** - * @brief Struct to hold the status of a wheel, including velocity, - * temperature, current, and voltages. - */ - struct Status { - double velocity; ///< The current velocity of the wheel. - double temperature; ///< The temperature of the motor. - double current; ///< The output current to the motor. - double voltageIn; ///< The input voltage to the motor. - double voltageOut; ///< The output voltage from the motor. - }; - - /** - * @brief Construct a new WheelControl object. - * @param wheel_name The name of the wheel (e.g., "frontLeft", "backRight"). - * @param node A pointer to the ROS node for creating publishers. - * @param is_open_loop Whether to use open loop (PERCENT_OUTPUT) or closed - * loop (VELOCITY) control. - * @param open_loop_scalar Scalar multiplier for velocity commands when in - * open loop mode. - * @throws std::invalid_argument if the wheel name doesn't include "Left" or - * "Right". - */ - WheelControl(std::string wheel_name, rclcpp::Node *node, bool is_open_loop, - double open_loop_scalar); - - /** - * @brief Set the velocity of the wheel. - * @param value The velocity to set (range depends on the motor control mode). - */ - void setVelocity(double value); - - /** - * @brief Publish the current motor control command to the wheel's motor. - * This sends the velocity command to the motor. - */ - void send() const; - - /** - * @brief Get the side of the wheel (left or right). - * @return WheelSide The side of the wheel. - */ - WheelSide getWheelSide() const { return side_; } - - /** - * @brief Get the current velocity of the wheel. - * @return double The current velocity of the wheel. - */ - double getVelocity() const { return status_.velocity; } - - /** - * @brief Set the status of the wheel based on the motor status message. - * This updates the wheel's internal status with data such as velocity, - * temperature, etc. - * @param msg The motor status message received from the motor. - */ - void setStatus(const MotorStatus::SharedPtr msg); - -private: - /** - * @brief The name of the wheel (e.g., "frontLeft"). - */ - std::string name_; - - /** - * @brief The side of the wheel (LEFT or RIGHT). - */ - WheelSide side_; - - /** - * @brief The motor control message used to set the motor parameters. - */ - MotorControl control_; - - /** - * @brief The current status of the wheel (e.g., velocity, temperature). - */ - Status status_; - - /** - * @brief Pointer to the ROS node for creating publishers. - */ - rclcpp::Node *node_; - - /** - * @brief Publisher for sending motor control messages. - */ - rclcpp::Publisher::SharedPtr pub_; - - /** - * @brief Whether the wheel is operating in open loop mode (true) or closed - * loop mode (false). - */ - bool is_open_loop_; - - /** - * @brief Scalar multiplier applied to velocity commands when in open loop - * mode. - */ - double open_loop_scalar_; -}; - -#endif // WHEEL_CONTROL_HPP diff --git a/src/Teleop-Control/drive_cpp/launch/talon.launch.py b/src/Teleop-Control/drive_cpp/launch/talon.launch.py deleted file mode 100644 index 0f596e3d..00000000 --- a/src/Teleop-Control/drive_cpp/launch/talon.launch.py +++ /dev/null @@ -1,77 +0,0 @@ -import launch -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode -from ament_index_python.packages import get_package_share_directory -import os - - -def generate_launch_description(): - """Generate launch description with multiple components.""" - - common_config_path = os.path.join( - get_package_share_directory("drive_cpp"), "config", "talon_drive_common.yaml" - ) - unique_config_path = os.path.join( - get_package_share_directory("drive_cpp"), "config", "talon_drive_unique.yaml" - ) - config_paths_lst = [common_config_path, unique_config_path] - - container_name = LaunchConfiguration("container_name") - container_arg = DeclareLaunchArgument( - "container_name", - default_value="PhoenixContainer", - description="Name of the target container", - ) - - container = ComposableNodeContainer( - name=container_name, - namespace="", - package="ros_phoenix", - executable="phoenix_container", - composable_node_descriptions=[ - ComposableNode( - package="ros_phoenix", - plugin="ros_phoenix::TalonSRX", - name="frontLeft", - parameters=config_paths_lst, - extra_arguments=[{"use_intra_process_comms": True}], - ), - ComposableNode( - package="ros_phoenix", - plugin="ros_phoenix::TalonSRX", - name="backLeft", - parameters=config_paths_lst, - extra_arguments=[{"use_intra_process_comms": True}], - ), - ComposableNode( - package="ros_phoenix", - plugin="ros_phoenix::TalonSRX", - name="frontRight", - parameters=config_paths_lst, - extra_arguments=[{"use_intra_process_comms": True}], - ), - ComposableNode( - package="ros_phoenix", - plugin="ros_phoenix::TalonSRX", - name="backRight", - parameters=config_paths_lst, - extra_arguments=[{"use_intra_process_comms": True}], - ), - ComposableNode( - package="drive_cpp", - plugin="TalonDriveController", - name="talon_drive_controller", - parameters=config_paths_lst, - extra_arguments=[{"use_intra_process_comms": True}], - ), - ], - ) - - return launch.LaunchDescription( - [ - container_arg, - container, - ] - ) diff --git a/src/Teleop-Control/drive_cpp/package.xml b/src/Teleop-Control/drive_cpp/package.xml deleted file mode 100644 index 4f155539..00000000 --- a/src/Teleop-Control/drive_cpp/package.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - - drive_cpp - 0.0.0 - TODO: Package description - connor - TODO: License declaration - - ament_cmake - rclcpp - std_msgs - geometry_msgs - sensor_msgs - ros_phoenix - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/src/Teleop-Control/drive_cpp/src/TalonDriveController.cpp b/src/Teleop-Control/drive_cpp/src/TalonDriveController.cpp deleted file mode 100644 index 3068d16e..00000000 --- a/src/Teleop-Control/drive_cpp/src/TalonDriveController.cpp +++ /dev/null @@ -1,151 +0,0 @@ -#include "TalonDriveController.hpp" - -#include -#include - -TalonDriveController::TalonDriveController(const rclcpp::NodeOptions &options) - : rclcpp::Node("talon_drive_controller", options) { - lastTimestamp_ = 0; - this->declare_parameter("max_speed", 2.0); - maxSpeed_ = this->get_parameter("max_speed").as_double(); - this->declare_parameter("base_width", 0.9144); - baseWidth_ = this->get_parameter("base_width").as_double(); - this->declare_parameter("pub_odom", true); - pubOdom_ = this->get_parameter("pub_odom").as_bool(); - this->declare_parameter("pub_elec", true); - pubElec_ = this->get_parameter("pub_elec").as_bool(); - this->declare_parameter("angular_cov", 0.3); - angularCov_ = this->get_parameter("angular_cov").as_double(); - this->declare_parameter("linear_cov", 0.3); - linearCov_ = this->get_parameter("linear_cov").as_double(); - this->declare_parameter("wheel_rad", 0.2); - wheelCircumference_ = this->get_parameter("wheel_rad").as_double() * 2 * M_PI; - this->declare_parameter("frame_id", "base_link"); - frameId_ = this->get_parameter("frame_id").as_string(); - this->declare_parameter("frequency", 10.0); - const double frequency = - std::max(this->get_parameter("frequency").as_double(), 1.0); - this->declare_parameter("timeout", 2.0); - timeout_ = this->get_parameter("timeout").as_double(); - this->declare_parameter("angular_slip_ratio", 1.0); - angularSlipRatio_ = this->get_parameter("angular_slip_ratio").as_double(); - if (angularSlipRatio_ <= 0.0 || angularSlipRatio_ > 1.0) { - RCLCPP_WARN( - this->get_logger(), - "Angular slip ratio should be in range (0.0, 1.0], setting to 1.0"); - angularSlipRatio_ = 1.0; - } - this->declare_parameter("low_latency_mode", true); - low_latency_mode_ = this->get_parameter("low_latency_mode").as_bool(); - - this->declare_parameter("is_open_loop", true); - bool is_open_loop = this->get_parameter("is_open_loop").as_bool(); - this->declare_parameter("open_loop_scalar", 0.5); - double open_loop_scalar = this->get_parameter("open_loop_scalar").as_double(); - - this->declare_parameter("wheels", - std::vector{"frontRight", "frontLeft", - "backRight", "backLeft"}); - std::vector wheel_names = - this->get_parameter("wheels").as_string_array(); - - for (const auto &wheel_name : wheel_names) { - wheels_.emplace_back( - WheelControl(wheel_name, this, is_open_loop, open_loop_scalar)); - if (pubOdom_) { - statusSubs_.emplace_back(this->create_subscription( - wheel_name + "/status", 10, - std::bind(&WheelControl::setStatus, &wheels_.back(), - std::placeholders::_1))); - } - } - - RCLCPP_INFO(this->get_logger(), "There are %ld wheels in the drive", - wheels_.size()); - - twistSub_ = this->create_subscription( - "/cmd_vel", 10, - std::bind(&TalonDriveController::twist_callback, this, - std::placeholders::_1)); - const int periodMs = 1000 / frequency; - timer_ = this->create_wall_timer( - std::chrono::milliseconds(periodMs), - std::bind(&TalonDriveController::control_timer_callback, this)); - - if (pubOdom_) { - odomPub_ = this->create_publisher("/drive/odom", 10); - odomTimer_ = this->create_wall_timer( - std::chrono::milliseconds(periodMs), - std::bind(&TalonDriveController::odom_pub_callback, this)); - } -} - -void TalonDriveController::odom_pub_callback() { - double rotationsLeft = 0.0; - double rotationsRight = 0.0; - - for (const auto &wheel : wheels_) { - if (wheel.getWheelSide() == WheelSide::LEFT) { - rotationsLeft += wheel.getVelocity(); - } else { - rotationsRight -= wheel.getVelocity(); - } - } - const int wheelsPerSide = wheels_.size() / 2; - const double vl = rotationsLeft * wheelCircumference_ / wheelsPerSide; - const double vr = rotationsRight * wheelCircumference_ / wheelsPerSide; - - Odometry odom; - odom.header.stamp = this->get_clock()->now(); - odom.header.frame_id = "odom"; - odom.child_frame_id = frameId_; - odom.twist.twist.linear.x = (vr + vl) / 2; - odom.twist.twist.angular.z = angularSlipRatio_ * (vl - vr) / baseWidth_; - odom.twist.covariance = {0}; - odom.twist.covariance[0] = linearCov_; - odom.twist.covariance[35] = angularCov_; - odomPub_->publish(odom); -} - -void TalonDriveController::control_timer_callback() { - if (this->get_clock()->now().seconds() - lastTimestamp_ > timeout_) { - for (auto &wheel : wheels_) { - wheel.setVelocity(0.0); - } - } - for (const auto &wheel : wheels_) { - wheel.send(); - } -} - -void TalonDriveController::twist_callback(const Twist::SharedPtr msg) { - lastTimestamp_ = this->get_clock()->now().seconds(); - - double linearX = msg->linear.x; - if (linearX > maxSpeed_) { - linearX = maxSpeed_; - } - if (linearX < -maxSpeed_) { - linearX = -maxSpeed_; - } - const double angular_portion = - msg->angular.z * baseWidth_ / angularSlipRatio_; - - double vr = linearX + angular_portion / 2; - double vl = linearX - angular_portion / 2; - - for (auto &wheel : wheels_) { - if (wheel.getWheelSide() == WheelSide::LEFT) { - wheel.setVelocity(vl / wheelCircumference_); - } else { - wheel.setVelocity(-vr / wheelCircumference_); - } - if (low_latency_mode_) { - wheel.send(); - } - } -} - -#include "rclcpp_components/register_node_macro.hpp" - -RCLCPP_COMPONENTS_REGISTER_NODE(TalonDriveController) \ No newline at end of file diff --git a/src/Teleop-Control/drive_cpp/src/WheelControl.cpp b/src/Teleop-Control/drive_cpp/src/WheelControl.cpp deleted file mode 100644 index a944115e..00000000 --- a/src/Teleop-Control/drive_cpp/src/WheelControl.cpp +++ /dev/null @@ -1,52 +0,0 @@ -#include "WheelControl.hpp" - -#include - -WheelControl::WheelControl(std::string wheel_name, rclcpp::Node *node, - bool is_open_loop, double open_loop_scalar) - : node_(node), name_(wheel_name), is_open_loop_(is_open_loop), - open_loop_scalar_(open_loop_scalar) { - if (wheel_name.find("Left") != std::string::npos || - wheel_name.find("left") != std::string::npos) { - side_ = WheelSide::LEFT; - } else if (wheel_name.find("Right") != std::string::npos || - wheel_name.find("right") != std::string::npos) { - side_ = WheelSide::RIGHT; - } else { - RCLCPP_FATAL(node->get_logger(), - "Wheel name must contain either 'Left' or 'Right'"); - throw std::invalid_argument( - "Wheel name must contain either 'Left' or 'Right'"); - } - pub_ = node->create_publisher(wheel_name + "/set", 10); - - if (is_open_loop_) { - control_.mode = MotorControl::PERCENT_OUTPUT; - RCLCPP_INFO(node->get_logger(), - "Wheel %s configured for open loop control with scalar %.3f", - wheel_name.c_str(), open_loop_scalar_); - } else { - control_.mode = MotorControl::VELOCITY; - RCLCPP_INFO(node->get_logger(), - "Wheel %s configured for closed loop velocity control", - wheel_name.c_str()); - } -} - -void WheelControl::setVelocity(double value) { - if (is_open_loop_) { - control_.value = value * open_loop_scalar_; - } else { - control_.value = value; - } -} - -void WheelControl::send() const { pub_->publish(control_); } - -void WheelControl::setStatus(const MotorStatus::SharedPtr msg) { - status_.velocity = msg->velocity; - status_.temperature = msg->temperature; - status_.current = msg->output_current; - status_.voltageIn = msg->bus_voltage; - status_.voltageOut = msg->output_voltage; -} diff --git a/src/Teleop-Control/drive_cpp/src/main.cpp b/src/Teleop-Control/drive_cpp/src/main.cpp deleted file mode 100644 index 67ce7ec3..00000000 --- a/src/Teleop-Control/drive_cpp/src/main.cpp +++ /dev/null @@ -1,10 +0,0 @@ -#include "TalonDriveController.hpp" -#include "rclcpp/rclcpp.hpp" - -int main(int argc, char **argv) { - rclcpp::init(argc, argv); - auto node = std::make_shared(rclcpp::NodeOptions{}); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} diff --git a/src/Teleop-Control/drive_cpp/talon_drive_controller_plugin.xml b/src/Teleop-Control/drive_cpp/talon_drive_controller_plugin.xml deleted file mode 100644 index ed6acc60..00000000 --- a/src/Teleop-Control/drive_cpp/talon_drive_controller_plugin.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - Composable Talon Drive Controller - - \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/include/arm.hpp b/src/Teleop-Control/joystick_control/include/arm.hpp index 88087e50..0d0df25f 100644 --- a/src/Teleop-Control/joystick_control/include/arm.hpp +++ b/src/Teleop-Control/joystick_control/include/arm.hpp @@ -3,7 +3,6 @@ #include "control_msgs/msg/joint_jog.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" -#include "interfaces/srv/move_servo.hpp" #include "rclcpp/rclcpp.hpp" #include "ros_phoenix/msg/motor_control.hpp" #include "sensor_msgs/msg/joy.hpp" diff --git a/src/Teleop-Control/joystick_control/package.xml b/src/Teleop-Control/joystick_control/package.xml index d25514fa..767ac9e5 100644 --- a/src/Teleop-Control/joystick_control/package.xml +++ b/src/Teleop-Control/joystick_control/package.xml @@ -18,6 +18,7 @@ ros_phoenix interfaces joy + joy_linux ament_cmake diff --git a/src/interfaces/CMakeLists.txt b/src/interfaces/CMakeLists.txt index 4f117aed..a8b04d90 100644 --- a/src/interfaces/CMakeLists.txt +++ b/src/interfaces/CMakeLists.txt @@ -29,10 +29,8 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/RtpStats.msg" "srv/NavToGPSGeopose.srv" "srv/VideoOut.srv" - "srv/MoveServo.srv" "srv/VideoCapture.srv" "srv/GetCameras.srv" - "srv/Raman.srv" "srv/GraphDot.srv" DEPENDENCIES builtin_interfaces diff --git a/src/interfaces/srv/Cam_Reset.srv b/src/interfaces/srv/Cam_Reset.srv deleted file mode 100644 index de5fec9e..00000000 --- a/src/interfaces/srv/Cam_Reset.srv +++ /dev/null @@ -1,7 +0,0 @@ -# VideoOut.srv - -bool reset - ---- -float32 yaw -float32 pitch \ No newline at end of file diff --git a/src/interfaces/srv/MoveServo.srv b/src/interfaces/srv/MoveServo.srv deleted file mode 100644 index dd49b23c..00000000 --- a/src/interfaces/srv/MoveServo.srv +++ /dev/null @@ -1,9 +0,0 @@ -# Request: Four integers, port, position, min, and max respectively -int64 port -int64 pos -int64 min -int64 max ---- -# Response: status and status message -bool status -string status_msg diff --git a/src/interfaces/srv/Raman.srv b/src/interfaces/srv/Raman.srv deleted file mode 100644 index 1bbe93b4..00000000 --- a/src/interfaces/srv/Raman.srv +++ /dev/null @@ -1,7 +0,0 @@ -# --- request --- -int32 inittime -int32 scansavg -int32 smoothing ---- -# --- response --- -string csv # whole CSV as text \ No newline at end of file diff --git a/src/third-party/ouster-ros/.github/pull_request_template.md b/src/third-party/ouster-ros/.github/pull_request_template.md deleted file mode 100644 index 62551453..00000000 --- a/src/third-party/ouster-ros/.github/pull_request_template.md +++ /dev/null @@ -1,5 +0,0 @@ -## Related Issues & PRs - -## Summary of Changes - -## Validation diff --git a/src/third-party/ouster-ros/.github/workflows/docker-image.yml b/src/third-party/ouster-ros/.github/workflows/docker-image.yml deleted file mode 100644 index 1f65d607..00000000 --- a/src/third-party/ouster-ros/.github/workflows/docker-image.yml +++ /dev/null @@ -1,39 +0,0 @@ -name: ouster-ros - -on: - push: - branches: - - ros2 - pull_request: - branches: - - ros2 - -jobs: - build: - runs-on: ubuntu-latest - strategy: - fail-fast: false - matrix: - exclude: - - ros_distro: iron - ros_distro: - - rolling - - humble - - iron - - jazzy - - kilted - rmw_imp: - - rmw_fastrtps_cpp - - rmw_cyclonedds_cpp - - rmw_zenoh_cpp - steps: - - uses: actions/checkout@v4 - with: - submodules: true - - name: Build the Docker image - run: | - docker build . \ - --build-arg ROS_DISTRO=${{ matrix.ros_distro }} \ - --build-arg RMW_IMPLEMENTATION=${{ matrix.rmw_imp }} \ - --file Dockerfile \ - --tag ouster-ros-${{ matrix.ros_distro }}:$(date +%s) diff --git a/src/third-party/ouster-ros/.gitmodules b/src/third-party/ouster-ros/.gitmodules deleted file mode 100644 index c5fc0a32..00000000 --- a/src/third-party/ouster-ros/.gitmodules +++ /dev/null @@ -1,3 +0,0 @@ -[submodule "ouster-sdk"] - path = ouster-ros/ouster-sdk - url = https://github.com/ouster-lidar/ouster_example.git diff --git a/src/third-party/ouster-ros/CHANGELOG.rst b/src/third-party/ouster-ros/CHANGELOG.rst deleted file mode 100644 index 6220dd8e..00000000 --- a/src/third-party/ouster-ros/CHANGELOG.rst +++ /dev/null @@ -1,173 +0,0 @@ -========= -Changelog -========= - -[unreleased] -============ -* [BUGFIX]: correctly align timestamps to the generated point cloud. -* [BUGFIX]: NEAR_IR data is not populated with data for organized point clouds that have no range. -* Add support to enable **loop** for pcap replay + other replay config. -* Add a new launch file parameter ``pub_static_tf`` that allows users to turn off the braodcast - of sensor TF transforms. -* Introduce a new topic ``/ouster/telemetry`` that publishes ``ouster_ros::Telemetry`` messages, - the topic can be turned on/off by including the token ``TLM`` in the flag ``proc_mask`` launch arg. -* Add a new launch file parameter ``min_scan_valid_columns_ratio`` to allow users to set the minimum - ratio of valid columns in a scan for it to be processed. Default value is ``0.0``. -* Update where ouster-ros and ouster_client include directories get installed so that those headers - can be included externally. -* Add ``storage`` launch parameter to ``record.launch.xml`` -* Add a padding-free point type of ``PointXYZI`` under ``ouster_ros`` namespace contrary to the pcl - version ``pcl::PointXYZI`` for bandwith sensitive applications. -* [BUGFIX]: Use the node clock to ensure messages report sim time in replay mode. -* Introduce a new param ``v_reduction`` that allows reducing the number of beams count of the published - point cloud -* Allow users to use ``Zenoh`` with the supplied Dockerfile and add it to the CI pipeline. -* Introduce a new capability to suppress certain range measurements of the point cloud by providing - a mask image to the driver through the ``mask_path`` launch file argument. -* [BUGFIX]: Correct the computation of ``pointcloud.is_dense`` flag. - - -ouster_ros v0.13.2 -================== -* [BUGFIX]: Make sure to initialize the sensor with launch file parameters. -* [BUGFIX]: ``os_driver`` failed when RAW option is used. - - -ouster_ros v0.13.0 -================== -* [BUGFIX]: LaserScan is not properly aligned with generated point cloud - * address an issue where LaserScan appeared different on FW prior to 2.4 -* [BUGFIX]: LaserScan does not work when using dual mode -* [BUGFIX]: ROS2 crashes when standby mode is set and then set to normal -* [BUGFIX]: Implement lock free ring buffer with throttling to reduce partial frames -* add support for FUSA udp profile ``FUSA_RNG15_RFL8_NIR8_DUAL``. -* [BREAKING]: Set xyz values of individual points in the PointCloud to NaNs when range is zero. -* Added support to replay pcap format direclty from ouster-ros. The feature needs to be enabled - explicitly by turning on the ``BUILD_PCAP`` cmake option and having ``libpcap-dev`` installed. -* [BREAKING] Added new launch files args ``azimuth_window_start`` and ``azimuth_window_end`` to - allow users to set LIDAR FOV on startup. The new options will reset the current azimuth window - to the default [0, 360] azimuth if not configured. -* Added a new launch ``persist_config`` option to request the sensor persist the current config -* Added a new ``loop`` option to the ``replay.launch.xml`` file. -* Added support for automatic sensor reconnection. Consult ``attempt_reconnect`` launch file arg - documentation and the associated params to enable. Known Issues: - - Doesn't handle detect and handle invalid configurations -* Added an automatic start mode to make it easier to start the node without using time actions. - - To disable set ``auto_start`` to ``false`` during launch -* Added a new parameter ``organized`` to request publishing unorganized point cloud -* Added a new parameter ``destagger`` to request publishing staggered point cloud -* Added two parameters ``min_range``, ``max_range`` to limit the lidar effective range -* Updated ouster_client to the release of ``20240425`` [v0.11.1]; changes listed below. - -ouster_client -------------- -* Added a new buffered UDP source implementation BufferedUDPSource. -* The method version_of_string is marked as deprecated, use version_from_string -instead. -* Added a new method firmware_version_from_metadata which works across firmwares. -* Added support for return order configuration parameter. -* Added support for gyro and accelerometer FSR configuration parameters. -* [BUGFIX] mtp_init_client throws a bad optional access. -* [BUGFIX] properly handle 32-bit frame IDs from the -* FUSA_RNG15_RFL8_NIR8_DUAL sensor UDP profile. - - -ouster_ros v0.12.0 -================== -* [BREAKING]: updated ouster_client to the release of ``20231031`` [v0.10.0]; changes listed below. -* [BREAKING]: publish PCL point clouds destaggered. -* introduced a new launch file parameter ``ptp_utc_tai_offset`` which represent offset in seconds - to be applied to all ROS messages the driver generates when ``TIME_FROM_PTP_1588`` timestamp mode - is used. - * [BREAKING]: the default value of ``ptp_utc_tai_offset`` is set to ``-37.0``. To retain the same - time offset for an existing system, users need to set ``ptp_utc_tai_offset`` to ``0.0``. -* fix: destagger columns timestamp when generating destaggered point clouds. -* shutdown the driver when unable to connect to the sensor on startup -* breaking: rename ouster_msgs to ouster_sensor_msgs -* added the ability to customize the published point clouds(s) to velodyne point cloud format and - other common pcl point types. -* ouster_image_compoenent can operate separately from ouster_cloud_component. -* fix: gracefully stop the driver when shutdown is requested. - -ouster_client -------------- -* [BREAKING] Updates to ``sensor_info`` include: - * new fields added: ``build_date``, ``image_rev``, ``prod_pn``, ``status``, ``cal`` (representing - the value stored in the ``calibration_status`` metadata JSON key), ``config`` (representing the - value of the ``sensor_config`` metadata JSON key) - * the original JSON string is accessible via the ``original_string()`` method - * The ``updated_metadata_string()`` now returns a JSON string reflecting any modifications to - ``sensor_info`` - * ``to_string`` is now marked as deprecated -* [BREAKING] The RANGE field defined in `parsing.cpp`, for the low data rate profile, is now 32 bits - wide (originally 16 bits). - * Please note this fixes a SDK bug. The underlying UDP format is unchanged. -* [BREAKING] The NEAR_IR field defined in `parsing.cpp`, for the low data rate profile, is now 16 - bits wide (originally 8 bits). - * Plase note this fixes a SDK bug. The underlying UDP format is unchanged. -* [BREAKING] changed frame_id return size to 32 bits from 16 bits -* An array of per-packet timestamps (called ``packet_timestamp``) is added to ``LidarScan`` -* The client now retries failed requests to an Ouster sensor's HTTP API -* Increased the default timeout for HTTP requests to 40s -* Added FuSA UDP profile to support Ouster FW 3.1+ -* Improved ``ScanBatcher`` performance by roughly 3x (depending on hardware) -* Receive buffer size increased from 256KB to 1MB -* [bugfix] Fixed an issue that caused incorrect Cartesian point computation in the ``viz.Cloud`` - Python class -* [bugfix] Fixed an issue that resulted in some ``packet_format`` methods returning an uninitialized - value -* [bugfix] Fixed a libpcap-related linking issue -* [bugfix] Fixed an eigen 3.3-related linking issue -* [bugfix] Fixed a zero beam angle calculation issue -* [bugfix] Fixed dropped columns issue with 4096x5 and 2048x10 - -ouster_ros v0.10.0 -================== - -ouster_ros(2) -------------- -* MVP ouster-ros targeting ros2 distros (beta release) -* introduced a ``reset`` service to the ``os_sensor`` node -* breaking change: updated to ouster sdk release 20230403 -* EOL notice: ouster-ros driver will drop support for ``ROS foxy`` by May 2023. -* bugfix: Address an issue causing the driver to warn about missing non-legacy fields even they exist - in the original metadata file. -* added a new launch file ``sensor_mtp.launch.xml`` for multicast use case (experimental). -* added a technique to estimate the the value of the lidar scan timestamp when it is missing packets - at the beginning -* add frame_id to image topics -* fixed a potential issue of time values within generated point clouds that could result in a value - overflow -* added a new ``/ouster/metadata`` topic that is consumed by os_cloud and os_image nodes and save it - to the bag file on record -* make specifying metadata file optional during record and replay modes as of package version 8.1 -* replace ``tf_prefix`` from ``os_cloud`` with ``sensor_frame``, ``lidar_frame`` and ``imu_frame`` - launch parameters. -* bugfix: fixed an issue that prevents running multiple instances of the sensor and cloud components - in the same process. -* switch to using static transform publisher for the ros2 driver. -* implemented a new node named ``os_driver`` which combines the functionality of ``os_sensor``, - ``os_cloud`` and ``os_image`` into a single node. -* added support to parse the same parameters provided by the ``ros2_ouster_driver``, the parameters - are ``lidar_ip``, ``computer_ip``, ``proc_mask`` and ``use_system_default_qos``; the parameters - are fully functional and similar to what the ``ros2_ouster_driver`` provides. -* for convenience introduced a new launch file ``driver_launch.py`` that is compatible with the - ``ros2_ouster_driver`` in terms of parameters it accepts and the name of published topics. -* introduced a new parameter ``point_cloud_frame`` to allow users to select which frame to use when - publishing the point cloud (choose between ``sensor`` and ``lidar``). -* breaking: ``lidar`` frame is the default frame used when publishing point clouds. -* added the ability to choose between ``SensorDataQoS`` or ``SystemDefaultQoS`` across all published - topics with ``SensorDataQoS`` selected by default for live sensor mode and ``SystemDefaultQoS`` - enabled for record and replay modes. -* introduced a new topic ``/ouster/scan`` which publishes ``sensor_msgs::msg::LaserScan`` messages -* fix: on dual returns the 2nd point cloud replaces the 1st one. -* breaking: merge ``ouster-srvs`` package into ``ouster-msgs``. - -ouster_client -------------- -* added a new method ``mtp_init_client`` to init the client with multicast support (experimental). -* the class ``SensorHttp`` which provides easy access to REST APIs of the sensor has been made public - under the ``ouster::sensor::util`` namespace. -* breaking change: get_metadata defaults to outputting non-legacy metadata -* add debug five_word profile which will be removed later -* breaking change: remove deprecations on LidarScan diff --git a/src/third-party/ouster-ros/Dockerfile b/src/third-party/ouster-ros/Dockerfile deleted file mode 100644 index 87837893..00000000 --- a/src/third-party/ouster-ros/Dockerfile +++ /dev/null @@ -1,73 +0,0 @@ -ARG ROS_DISTRO=rolling -ARG RMW_IMPLEMENTATION=rmw_fastrtps_cpp - -FROM ros:${ROS_DISTRO}-ros-core AS build-env -ARG RMW_IMPLEMENTATION -ARG ROS_DISTRO -ENV DEBIAN_FRONTEND=noninteractive \ - RMW_IMPLEMENTATION=${RMW_IMPLEMENTATION} \ - BUILD_HOME=/var/lib/build \ - OUSTER_ROS_PATH=/opt/ros2_ws/src/ouster-ros - -RUN set -xue && \ - apt-get update && \ - apt-get install -y \ - build-essential \ - cmake \ - fakeroot \ - dpkg-dev \ - python3-rosdep \ - python3-rospkg \ - python3-bloom \ - python3-colcon-common-extensions - -RUN if [ "$RMW_IMPLEMENTATION" = "rmw_cyclonedds_cpp" ]; then \ - apt-get install -y ros-${ROS_DISTRO}-rmw-cyclonedds-cpp; \ - elif [ "$RMW_IMPLEMENTATION" = "rmw_zenoh_cpp" ]; then \ - apt-get install -y ros-${ROS_DISTRO}-rmw-zenoh-cpp; \ - fi - -# Set up non-root build user -ARG BUILD_UID=1000 -ARG BUILD_GID=${BUILD_UID} - -RUN set -xe \ -&& groupadd -o -g ${BUILD_GID} build \ -&& useradd -o -u ${BUILD_UID} -d ${BUILD_HOME} -rm -s /bin/bash -g build build - -# Set up build environment -COPY --chown=build:build . $OUSTER_ROS_PATH - -RUN set -xe \ -&& apt-get update \ -&& rosdep init \ -&& rosdep update --rosdistro=$ROS_DISTRO \ -&& rosdep install --from-paths $OUSTER_ROS_PATH -y --ignore-src - - -USER build:build -WORKDIR ${BUILD_HOME} - -RUN set -xe \ -&& mkdir src \ -&& cp -R $OUSTER_ROS_PATH ./src -FROM build-env -ARG ROS_DISTRO - -SHELL ["/bin/bash", "-c"] - -RUN source /opt/ros/$ROS_DISTRO/setup.bash && colcon build \ - --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release \ - -DCMAKE_CXX_FLAGS="-Wno-deprecated-declarations" - -RUN source /opt/ros/$ROS_DISTRO/setup.bash && colcon test \ - --ctest-args tests ouster_ros --rerun-failed --output-on-failure - -# Entrypoint for running Ouster ros: -# -# Usage: docker run --rm -it ouster-ros [sensor.launch parameters ..] -# -ENTRYPOINT ["bash", "-c", "set -e \ -&& source ./install/setup.bash \ -&& ros2 launch ouster_ros sensor.launch.xml \"$@\" \ -", "ros-entrypoint"] diff --git a/src/third-party/ouster-ros/LICENSE b/src/third-party/ouster-ros/LICENSE deleted file mode 100644 index 50fd48e4..00000000 --- a/src/third-party/ouster-ros/LICENSE +++ /dev/null @@ -1,63 +0,0 @@ -Format: https://www.debian.org/doc/packaging-manuals/copyright-format/1.0/ -Upstream-Name: OusterSDK -Upstream-Contact: Ouster Sensor SDK Developers -Source: https://github.com/ouster-lidar/ouster_example - -Files: * -Copyright: 2018-2023 Ouster, Inc -License: BSD-3-Clause - -Files: include/optional-lite/* -Copyright: 2014-2021 Martin Moene -License: BSL-1.0 - -License: BSD-3-Clause - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -License: BSL-1.0 - Boost Software License - Version 1.0 - August 17th, 2003 - - Permission is hereby granted, free of charge, to any person or organization - obtaining a copy of the software and accompanying documentation covered by - this license (the "Software") to use, reproduce, display, distribute, - execute, and transmit the Software, and to prepare derivative works of the - Software, and to permit third-parties to whom the Software is furnished to - do so, all subject to the following: - - The copyright notices in the Software and this entire statement, including - the above license grant, this restriction and the following disclaimer, - must be included in all copies of the Software, in whole or in part, and - all derivative works of the Software, unless such copies or derivative - works are solely in the form of machine-executable object code generated by - a source language processor. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT - SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE - FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, - ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER - DEALINGS IN THE SOFTWARE. \ No newline at end of file diff --git a/src/third-party/ouster-ros/README.md b/src/third-party/ouster-ros/README.md deleted file mode 100644 index 8a544af1..00000000 --- a/src/third-party/ouster-ros/README.md +++ /dev/null @@ -1,309 +0,0 @@ -# Official ROS driver for Ouster sensors - -[ROS1 (melodic/noetic)](https://github.com/ouster-lidar/ouster-ros/tree/master) | -[ROS2 (rolling/humble/iron/jazzy/kilted)](https://github.com/ouster-lidar/ouster-ros/tree/ros2) | -[ROS2 (galactic/foxy)](https://github.com/ouster-lidar/ouster-ros/tree/ros2-foxy) - -

- -| ROS Version | Build Status (Linux) | -|:-----------:|:------:| -| ROS1 (melodic/noetic) | [![melodic/noetic](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml/badge.svg?branch=master)](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml) -| ROS2 (rolling/humble/iron/jazzy/kilted) | [![rolling/humble/iron/jazzy/kilted](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml/badge.svg?branch=ros2)](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml) -| ROS2 (galactic/foxy) | [![galactic/foxy](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml/badge.svg?branch=ros2-foxy)](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml) - -- [Official ROS driver for Ouster sensors](#official-ros-driver-for-ouster-sensors) - - [Overview](#overview) - - [Supported Devices](#supported-devices) - - [Requirements](#requirements) - - [Linux](#linux) - - [Windows](#windows) - - [Mac](#mac) - - [Getting Started](#getting-started) - - [Usage](#usage) - - [Launching Nodes](#launching-nodes) - - [Sensor Mode](#sensor-mode) - - [Recording Mode](#recording-mode) - - [Replay Mode](#replay-mode) - - [PCAP Replay Mode](#pcap-replay-mode) - - [Multicast Mode (experimental)](#multicast-mode-experimental) - - [Invoking Services](#invoking-services) - - [GetMetadata](#getmetadata) - - [GetConfig](#getconfig) - - [SetConfig](#setconfig) - - [Reset](#reset) - - [Driver Parameters](#driver-parameters) - - [License](#license) - - -## Overview - -This ROS package provide support for all Ouster sensors with FW v2.0 or later targeting ros2 distros. -Upon launch the driver will configure and connect to the selected sensor device, once connected the -driver will handle incoming IMU and lidar packets, decode lidar frames and publish corresponding ROS -messages on the topics of `/ouster/imu` and `/ouster/points`. In the case the used sensor supports -dual return and it was configured to use this capability, then another topic will published under the -name `/ouster/points2` which corresponds to the second point cloud. - - -## Supported Devices -The driver supports the following list of Ouster sensors: -- [OS0](https://ouster.com/products/hardware/os0-lidar-sensor) -- [OS1](https://ouster.com/products/hardware/os1-lidar-sensor) -- [OS2](https://ouster.com/products/hardware/os2-lidar-sensor) -- [OSDome](https://ouster.com/products/hardware/osdome-lidar-sensor) - -You can obtain detailed specs sheet about the sensors and obtain updated FW through the website -[downloads](https://ouster.com/downloads) section. - -## Requirements -This branch is only intended for use with **Rolling**, **Humble**, **Iron**, **Jazzy** and **Kilted** -ROS 2 distros. Please refer to ROS 2 online documentation on how to setup ROS on your machine before -proceeding with the remainder of this guide. - -> **Note** -> If you have _rosdep_ tool installed on your system you can then use the following command to get all - required dependencies: - ``` - rosdep install --from-paths $OUSTER_ROS_PATH -y --ignore-src - ``` - -### Linux - -In addition to the base ROS installation, the following ROS packages are required: -```bash -sudo apt install -y \ - ros-$ROS_DISTRO-pcl-ros \ - ros-$ROS_DISTRO-tf2-eigen \ - ros-$ROS_DISTRO-rviz2 -``` -where `$ROS_DISTRO` can be either ``rolling``, ``humble``, ``iron``, ``jazzy`` or ``kilted``. - -> **Note** -> Installing `ros-$ROS_DISTRO-rviz` package is optional in case you didn't need to visualize the -> point cloud using rviz but remember to always set `viz` launch arg to `false`. - -The following packages are also required -```bash -sudo apt install -y \ - build-essential \ - libeigen3-dev \ - libjsoncpp-dev \ - libspdlog-dev \ - libcurl4-openssl-dev \ - cmake \ - python3-colcon-common-extensions -``` -> **Note** -> You may choose a different _ssl_ backend for the _curl_ library such as `libcurl4-gnutls-dev` or -> `libcurl4-nss-dev` - -> **Note** -> To use the PCAP replay mode you need to have `libpcap-dev` installed - -### Windows -TBD - - -### Mac -TBD - - -## Getting Started -To build the driver using ROS2 you need to clone the project into the `src` folder of a ros2 -workspace as shown below: - -```bash -mkdir -p ros2_ws/src && cd ros2_ws/src -git clone -b ros2 --recurse-submodules https://github.com/ouster-lidar/ouster-ros.git -``` - -Next to compile the driver you need to source the ROS environemt into the active termainl: -```bash -source /opt/ros//setup.bash # replace ros-distro with 'rolling', 'humble', 'iron', 'jazzy' or `kilted` -``` - -Finally, invoke `colcon build` command from within the catkin workspace as shown below: -```bash -cd ros2_ws -colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release -``` -> **Note** -> Specifying `Release` as the build type is important to have a reasonable performance of the driver. - -> **Note** -> For ROS2 we recommend using **CycloneDDS** over **FastDDS**, through out Galactic, Foxy, Humble distros. -> **FastDDS** is usually the default ros middleware on most platforms, please follow the -[Guide](https://docs.ros.org/en/humble/Installation/DDS-Implementations/Working-with-Eclipse-CycloneDDS.html) -to learn how to enable **CycloneDDS** on your platform. -> The **Zeonh** ROS middleware is now available for use with ouster-ros driver from Humble and afterwards -> (exlcuding Iron). Zenoh have received great feedback from the ROS community but that's in general, we don't -> have a comparative analysis against the other middlewares, to enable Zenoh: -> ```bash -> sudo apt install ros-${ROS_DISTRO}-rmw-zenoh-cpp -> export RMW_IMPLEMENTATION=rmw_zenoh_cpp -> ``` -> then follow the instructions here and rebuild the ouster-ros driver (clean build). - - -Once the build succeeds, you must source the _install_ folder of your ros2 workspace to add launch -commands to your environment: -```bash -source ros2_ws/install/setup.bash -``` - -## Usage - -### Launching Nodes -The package supports three modes of interaction, you can connect to a _live sensor_, _replay_ a recorded -bag or _record_ a new bag file using the corresponding launch files. Recently, we have added a new mode -that supports multicast. The commands are listed below, for convenience we do provide both launch file -formats (xml and python) but the python format is the preferred method: - -#### Sensor Mode -To connect to a live sensor you use the following launch file -```bash -ros2 launch ouster_ros sensor.launch.xml \ - sensor_hostname:= -``` -The equivalent python launch file is: -```bash -ros2 launch ouster_ros driver.launch.py \ - params_file:= -``` -If you don't pass a `params_file` then the file located at `ouster/config/driver_params.yaml` will be used. Note that in -the params you can start with default options for everything except the `sensor_hostname` param which you should adjust -to match the hostname or ip address of the Ouster sensor you are trying to connect to. - -**comptability mode** -If you are migrating from https://github.com/ros-drivers/ros2_ouster_drivers to the official ouster drivers -we supply you with a file `driver_launch.py` which provides users with same topic name and accepts the same -parameter file `community_driver_config.yaml`. Please note that this is provided for backward compatibilty -it may not be maintained in the future, so it would be better to update to the new format `driver_params.yaml` -which offers the same options and more. - -#### Recording Mode -> Note -> As of package version 8.1, specifiying metadata file is optional since the introduction of the -> metadata topic -```bash -ros2 launch ouster_ros record.launch.xml \ - sensor_hostname:= \ - bag_file:= \ - metadata:= # optional -``` - -#### Replay Mode -> Note -> As of package version 8.1, specifiying metadata file is optional if the bag file being replayed -> already contains the metadata topic - -```bash -ros2 launch ouster_ros replay.launch.xml \ - bag_file:= \ - metadata:= # optional if bag file has /metadata topic -``` - -##### PCAP Replay Mode -> Note -> To use this feature you need to compile the driver with `BUILD_PCAP` option enabled -```bash -ros2 launch ouster_ros replay_pcap.launch.xml \ - pcap_file:= \ - metadata:= # required -``` - -#### Multicast Mode (experimental) -The multicast launch mode supports configuring the sensor to broadcast lidar packets from the same -sensor (live) to multiple active clients. You initiate this mode by using `sensor_mtp.launch.xml` -file to start the node. You will need to specify a valid multicast group for the **udp_dest** -argument which the sensor is going to broadcast data to it. You will also need to set **mtp_main** -argument to **true**, this is need to configure the sensor with the specified **udp_dest** and any -other sensor settings. You can control on which ip (IP4 only) you wish to receive the data on this -machine from the multicast group using the **mtp_dest** argument as follows: -```bash -roslaunch ouster_ros sensor_mtp.launch.xml \ - sensor_hostname:= \ - udp_dest:= \ - mtp_main:=true \ - mtp_dest:= # mtp_dest is optional -``` -Using a different machine that belongs to the same netwok subnet, you can start another instance of -the client to start receiving sensor messages through the multicast group as shown below (note that -**mtp_main** is set to **false**): -```bash -roslaunch ouster_ros sensor_mtp.launch.xml \ - sensor_hostname:= \ - udp_dest:= \ - mtp_main:=false \ - mtp_dest:= # mtp_dest is optional -``` - -> **Note:** -> In both cases the **mtp_dest** is optional and if left unset the client will utilize the first -available interface. - -### Invoking Services -To execute any of the following service, first you need to open a new terminal -and source the ros2 workspace again by running the command -`source ros2_ws/install/setup.bash` -#### GetMetadata -To get metadata while connected to a live sensor or during a replay session invoke -the following command: -```bash -ros2 service call /ouster/get_metadata ouster_srvs/srv/GetMetadata -``` - -#### GetConfig -To get the current config of a live sensor, invoke the command: -```bash -ros2 service call /ouster/get_config ouster_srvs/srv/GetConfig -``` - -#### SetConfig -To change config via a file while connected to a live sensor, invoke the command: -```bash -ros2 service call /ouster/set_config ouster_srvs/srv/SetConfig \ - "{config_file: 'some_config.json'}" -``` - -#### Reset -To reset the new reset service, execute the following commnad: -```bash -ros2 service call /ouster/reset std_srvs/srv/Empty -``` -When this service is invoked the client should stop streaming, dispose current -connection, reset the sensor and reconnect again. - -> **Note** -> Changing settings is not yet fully support during a reset operation (more on this) - -### Driver Parameters -The driver has several parameters that allow you to customize its behavior, all of -these parameters are defined with the `driver_params.yaml` file found under `config` -folder. The only required parameter is `sensor_hostname` which sets the sensor -hostname or ip that you want to connect to through ouster-ros driver. - -Other notable parameters include: -* **point_type**: This parameter allows to customize the point cloud that the - driver produces through its `/ouster/points` topics. Choose one of the following - values: - - `original`: This uses the original point representation `ouster_ros::Point` - of the ouster-ros driver. - - `native`: directly maps all fields as published by the sensor to an - equivalent point cloud representation with the additon of ring - and timestamp fields. - - `xyz`: the simplest point type, only has {x, y, z} - - `xyzi`: same as xyz point type but adds intensity (signal) field. this - type is not compatible with the low data profile. - - `xyzir`: same as xyzi type but adds ring (channel) field. - this type is same as Velodyne point cloud type - this type is not compatible with the low data profile. - -This is not a comprehenisve list of all the parameters that the driver supports -for more detailed list please refer to the `config/driver_params.yaml` file. - -For further detailed instructions about the driver refer to the [main guide](./docs/index.rst) - -## License -[License File](./LICENSE) diff --git a/src/third-party/ouster-ros/docs/images/logo.png b/src/third-party/ouster-ros/docs/images/logo.png deleted file mode 100644 index edc0ba68..00000000 Binary files a/src/third-party/ouster-ros/docs/images/logo.png and /dev/null differ diff --git a/src/third-party/ouster-ros/docs/index.rst b/src/third-party/ouster-ros/docs/index.rst deleted file mode 100644 index fd58e456..00000000 --- a/src/third-party/ouster-ros/docs/index.rst +++ /dev/null @@ -1,172 +0,0 @@ -.. title:: ROS Guide - -================= -Ouster ROS driver -================= - -The driver publishes Ouster sensor data as standard ROS topics. The driver supports ROS Melodic on -Ubuntu 18, and ROS Noetic for Ubuntu 20. Follow `ROS installation guide -`_ to get started using ROS on your platform. - -.. note:: - If you have been using a release prior to 20220826, then check `our migration guide - <./doc/migration-guide.rst>`_ which covers all the breaking changes and how to mitigate them. - - -Building ROS Driver -==================== - -The build dependencies include those of the sample code:: - - sudo apt install build-essential cmake libeigen3-dev libjsoncpp-dev - -Additionally, you should install the ros dependencies:: - - sudo apt install \ - ros--pcl-ros \ - ros--tf2-geometry-msgs \ - ros--rviz - -where ```` is ``melodic`` or ``noetic``. - -Alternatively, if you would like to install dependencies with `rosdep`:: - - rosdep install --from-paths - -To build:: - - mkdir -p catkin_ws/src && cd catkin_ws/src - git clone --recurse-submodules https://github.com/ouster-lidar/ouster-ros.git - source /opt/ros//setup.bash - catkin_make -DCMAKE_BUILD_TYPE=Release - -.. warning:: - Do not create your workspace directory inside the cloned ouster_example repository, - as this will confuse the ROS build system. - -For each command in the following sections, make sure to first set up the ROS environment in each -new terminal by running:: - - source catkin_ws/devel/setup.bash - -Running ROS Nodes with a Sensor -================================ - -Make sure the sensor is connected to the network. See "Connecting to the Sensor" in the `Software -User Manual `_ for instructions and different options for network -configuration. - -To connect to a sensor and publish its data as ROS topics, execute the command:: - - roslaunch ouster_ros sensor.launch sensor_hostname:= - -where: -- ``sensor_hostname:=`` can be the hostname (os-99xxxxxxxxxx.local) or IP of the - sensor - -Additionally, the launch file has following list of arguments that you can use: -- ``metadata:=`` to set the name where sensor metadata configuration will be - saved to. Note that by default the working directory of all ROS nodes is set to ``${ROS_HOME}``, - which is generally ``$HOME/.ros``. If you provide a relative path to ``metadata``, i.e., - ``metadata:=meta.json`` it will write to ``${ROS_HOME}/meta.json``. If you wish the file be saved - in the current directory you may use the absolute path instead, such as ``metadata:=$PWD/meta.json`` -- ``udp_dest:=`` to specify the hostname or IP to which the sensor should send data -- ``lidar_mode:=`` where mode is one of ``512x10``, ``512x20``, ``1024x10``, ``1024x20``, or - ``2048x10`` -- ``timestamp_mode`` choose any value of the following timestamp modes: ``TIME_FROM_INTERNAL_OSC``, - ``TIME_FROM_SYNC_PULSE_IN``, ``TIME_FROM_PTP_1588``, or ``TIME_FROM_ROS_TIME``. You can read about - the first three modes in `Ouster Sensor Guide `_. The last - mode ``TIME_FROM_ROS_TIME`` is specific to the ouster_ros driver; when this mode is set, the - driver uses ROS time as the timestamp for published IMU and Lidar messages. -- ``viz:=true/false`` to visualize the sensor output, if you have the rviz ROS package installed - - -Recording Data -=============== - -To record raw sensor output you may use the provided ``record.launch`` file as follows:: - - roslaunch ouster_ros record.launch \ - sensor_hostname:= \ - metadata:= \ - bag_file:= - -This will connect to the specified sensor, write the sensor metadata to a file and start -recording imu and lidar packets to the specified bag_file once the sensor is connected. - -It is necessary that you provide a name for the metadata file and maintain this file along -with the recorded bag_file otherwise you won't be able to play the file correctly. - -If no bag_file is specified then a name will be generated based on the current date/time. - -By default ROS saves all files to $ROS_HOME, if you want to have these files saved in the -current directory, simply give the absolute path to each file. For example:: - - roslaunch ouster_ros record.launch \ - sensor_hostname:= \ - metadata:=$PWD/ \ - bag_file:=$PWD/ - -Alternatively, you may connect to the sensor using the ``roslaunch ouster_ros sensor.launch ..`` -command and then use the rosbag command in a separate terminal to start recording lidar packets -at any time using the following command:: - - rosbag record /ouster/imu_packets /ouster/lidar_packets - -For more information on rosbag functionality refer to `rosbag record`_. - -.. _rosbag record: https://wiki.ros.org/rosbag/Commandline#rosbag_record - -.. warning:: - When recording a bag file directly via the ``rosbag record``, you need to - save the metadata information of the sensor you are connected to. This can be - achieved by supplying a path to the ``metadata`` argument of the ``sensor.launch``. - You will need the metadata file information to properly replay the recorded bag - file. - -Playing Back Recorded Data -========================== - -You may use the ``replay.launch`` file to repalay previously captured sensor data. -Simply invoke the launch file with the following parameters:: - - roslaunch ouster_ros replay.launch \ - metadata:= \ - bag_file:= - -A metadata file is mandatory for replay of data. See `Recording Data`_ for how -to obtain the metadata file when recording your data. - -Ouster ROS Services -=================== - -The ROS driver currently advertises three services ``/ouster/get_metadata``, -``/ouster/get_config``, and ``/ouster/set_config``. The first one is available -in all three modes of operation: ``Sensor``, ``Replay``, and ``Recording``. -The latter two, however, are only available in ``Sensor`` and ``Recording`` -modes. i.e. when connected to a sensor. - -The usage of the three services is described below: - -- ``/ouster/get_metadata``: This service takes no parameters and returns the - current sensor metadata, you may use as follows:: - - rosservice call /ouster/get_metadata - - This will return a json string that contains the sensor metadata - -- ``/ouster/get_config``: This service takes no parameters and returns the - current sensor configuration, you may use as follows:: - - rosservice call /ouster/get_config - - This will return a json string represting the current configuration - -- ``/ouster/set_config``: Takes a single parameter and also returns the updated - sensor configuration. You may use as follows:: - - rosservice call /ouster/set_config "config_file: ''" - - It is not guranteed that all requested configuration are applied to the sensor, - thus it is the caller responsibilty to examine the returned json object and - check which of the sensor configuration parameters were successfully applied. diff --git a/src/third-party/ouster-ros/docs/migration-guide.rst b/src/third-party/ouster-ros/docs/migration-guide.rst deleted file mode 100644 index f2986d00..00000000 --- a/src/third-party/ouster-ros/docs/migration-guide.rst +++ /dev/null @@ -1,73 +0,0 @@ -.. title:: Ouster-ROS Driver Migration Guide - - -=================================== -Migrating from 20220608 to 20220826 -=================================== -The ``20220826`` release brought several improvements to the **ouster-ros** driver, however, these -improvements brought also several breaking changes. This guide summarizes these change and how to -mitigate each one. - -Launch Files: -============= -The ``ouster.launch`` has been split into three functionally different launch files: -``sensor.launch``, ``replay.launch``, and ``record.launch``. Each of these launch files only accepts -parameters that are relevant to their functional use. For example, the ``sensor_hostname`` argument -is only valid and required when using the ``sensor.launch`` and ``record.launch`` launch files. -The ``metadata`` argument however is no longer required when connecting to the sensor in live mode. -It is still required when using ``record.launch`` and ``replay.launch``. Refer to the `main -documentation <./doc/index.rst>`_ for more details on the different cases of usage. - -Topics -====== - -Topics Renaming ---------------- - -Rather than having each topic published by **ouster-ros** be prefixed with the name of the ros node -that publishes it, the topic names of all ros nodes that compose the **ouster-ros** driver have been -combined under a single namespace. Thus all topics would appear prefixed by the new namespace. - -If a user wishes to maintain the old topic names then they can achieve that by utilizing the -```` tag in ros launch files. For example, let's say we want to remap the three topics -published by ``os_cloud_node`` to their old names when connecting to a sensor through the -``sensor.launch`` file. To do so, we need to edit the ``sensor.launch`` file and add the three -following remap tags right before any node is instantiated (i.e. the ```` tag defintions -should precede any ```` tags in the launch file):: - - - - - -Same thing needs to happen if we want to remap the topic names that are published by ``img_node``. -This is shown below as well:: - - - - - - -Topics Dropped --------------- - -Additionally, the following two topic have been dropped:: - - /os1_node/imu_packets - /os1_node/lidar_packets - -These two topics are a duplicates of the ``/os_node/imu_packets`` and ``/os_node/lidar_packets``, -which both have been renamed to ``/ouster/imu_packets`` and ``/ouster/lidar_packets`` respectively. -If you rely on these specific topics names in your integration then you could easily follow same -steps described in the `Topics Renaming`_ section. - -Services -======== -This update brought two additional ros services and changed the name of the only ros service that -existed prior to this release. The service name was ``/os_config`` and when it is invoked it would -retrive the sensor metadata. This service was also combined under the unifed namespace ``ouster`` -and was renamed to ``get_metadata`` to match its actual functionality. In case the user wish to -maintain the old name they can achieve so in similar manner that was described in the previous -section. In short edit the launch files and add the following ````` tag prior to the -instantiation of any of the three nodes:: - - diff --git a/src/third-party/ouster-ros/ouster-ros/CMakeLists.txt b/src/third-party/ouster-ros/ouster-ros/CMakeLists.txt deleted file mode 100644 index 868a7add..00000000 --- a/src/third-party/ouster-ros/ouster-ros/CMakeLists.txt +++ /dev/null @@ -1,269 +0,0 @@ -cmake_minimum_required(VERSION 3.10.0) - -list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/ouster-sdk/cmake) -include(DefaultBuildType) - -# ==== Project Name ==== -project(ouster_ros) - -# ==== Requirements ==== -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(std_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(ouster_sensor_msgs REQUIRED) -find_package(std_srvs REQUIRED) -find_package(rosidl_default_generators REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(PCL REQUIRED COMPONENTS common) -find_package(pcl_conversions REQUIRED) -find_package(tf2_eigen REQUIRED) -find_package(OpenCV REQUIRED) - -# ==== Options ==== -add_compile_options(-Wall -Wextra) -if(NOT DEFINED CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() -option(CMAKE_POSITION_INDEPENDENT_CODE "Build position independent code." ON) - -set(_ouster_ros_INCLUDE_DIRS - "include;" - "ouster-sdk/ouster_client/include;" - "ouster-sdk/ouster_client/include/optional-lite;" - "ouster-sdk/ouster_pcap/include" -) - -# ==== Libraries ==== -# Build static libraries and bundle them into ouster_ros using the `--whole-archive` flag. This is -# necessary because catkin doesn't interoperate easily with target-based cmake builds. Object -# libraries are the recommended way to do this, but require >=3.13 to propagate usage requirements. -set(_SAVE_BUILD_SHARED_LIBS ${BUILD_SHARED_LIBS}) -set(BUILD_SHARED_LIBS OFF) - -option(BUILD_VIZ "Turn off building VIZ" OFF) -option(BUILD_PCAP "Turn off building PCAP" OFF) -option(BUILD_OSF "Turn off building OSF" OFF) -find_package(OusterSDK REQUIRED) - -set(BUILD_SHARED_LIBS ${_SAVE_BUILD_SHARED_LIBS}) - -# catkin adds all include dirs to a single variable, don't try to use targets -include_directories( - ${_ouster_ros_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS}) - -# use only MPL-licensed parts of eigen -add_definitions(-DEIGEN_MPL2_ONLY) - -set(OUSTER_TARGET_LINKS ouster_client) -if (BUILD_PCAP) - list(APPEND OUSTER_TARGET_LINKS ouster_pcap) -endif() - -add_library(ouster_ros_library SHARED - src/os_ros.cpp -) - -set(ouster_ros_library_deps - rclcpp - sensor_msgs - geometry_msgs - ouster_sensor_msgs - pcl_conversions - tf2 - tf2_eigen -) - -ament_target_dependencies(ouster_ros_library - ${ouster_ros_library_deps} -) - -target_link_libraries(ouster_ros_library - # PUBLIC (unsupported) - ouster_build - pcl_common - # PRIVATE (unsupported) - -Wl,--whole-archive ${OUSTER_TARGET_LINKS} -Wl,--no-whole-archive - ${OpenCV_LIBRARIES} -) - -# helper method to construct ouster-ros components -function(create_ros2_component - component_lib_name - src_list - additonal_dependencies -) - - add_library(${component_lib_name} SHARED - ${src_list}) - target_compile_definitions(${component_lib_name} - PRIVATE - "OUSTER_ROS_BUILDING_DLL" - ) - - ament_target_dependencies(${component_lib_name} - rclcpp - class_loader - rclcpp_components - rclcpp_lifecycle - std_msgs - sensor_msgs - geometry_msgs - ${additonal_dependencies} - ) - - target_link_libraries(${component_lib_name} - ouster_ros_library - ${cpp_typesupport_target} - ) - -endfunction() - - -# ==== os_sensor_component ==== -create_ros2_component(os_sensor_component - "src/os_sensor_node_base.cpp;src/os_sensor_node.cpp" - "std_srvs" -) -rclcpp_components_register_node(os_sensor_component - PLUGIN "ouster_ros::OusterSensor" - EXECUTABLE os_sensor -) - -# ==== os_replay_component ==== -create_ros2_component(os_replay_component - "src/os_sensor_node_base.cpp;src/os_replay_node.cpp" - "" -) -rclcpp_components_register_node(os_replay_component - PLUGIN "ouster_ros::OusterReplay" - EXECUTABLE os_replay -) - -# ==== os_cloud_component ==== -create_ros2_component(os_cloud_component - "src/os_processing_node_base.cpp;src/os_cloud_node.cpp" - "tf2_ros" -) -rclcpp_components_register_node(os_cloud_component - PLUGIN "ouster_ros::OusterCloud" - EXECUTABLE os_cloud -) - -# ==== os_image_component ==== -create_ros2_component(os_image_component - "src/os_processing_node_base.cpp;src/os_image_node.cpp" - "" -) -rclcpp_components_register_node(os_image_component - PLUGIN "ouster_ros::OusterImage" - EXECUTABLE os_image -) - -# ==== os_driver_component ==== -create_ros2_component(os_driver_component - "src/os_sensor_node_base.cpp;src/os_sensor_node.cpp;src/os_driver_node.cpp" - "std_srvs" -) -rclcpp_components_register_node(os_driver_component - PLUGIN "ouster_ros::OusterDriver" - EXECUTABLE os_driver -) - - -if (BUILD_PCAP) -# ==== os_replay_component ==== -create_ros2_component(os_pcap_component - "src/os_sensor_node_base.cpp;src/os_pcap_node.cpp" - "" -) -rclcpp_components_register_node(os_pcap_component - PLUGIN "ouster_ros::OusterPcap" - EXECUTABLE os_pcap -) -endif() - -# ==== Test ==== -if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - ament_add_gtest(${PROJECT_NAME}_test - src/os_ros.cpp - test/test_main.cpp - test/lock_free_ring_buffer_test.cpp - test/point_accessor_test.cpp - test/point_transform_test.cpp - test/point_cloud_compose_test.cpp - ) - ament_target_dependencies(${PROJECT_NAME}_test - rclcpp - ouster_sensor_msgs - ) - target_include_directories(${PROJECT_NAME}_test PUBLIC - ${_ouster_ros_INCLUDE_DIRS} - $ - $ - ) - target_link_libraries(${PROJECT_NAME}_test ouster_ros_library) -endif() - - -# ==== Install ==== -set(OUSTER_INSTALL_TARGETS - ouster_ros_library - os_sensor_component - os_replay_component - os_cloud_component - os_image_component - os_driver_component -) -if (BUILD_PCAP) - list(APPEND OUSTER_INSTALL_TARGETS os_pcap_component) -endif() - -install( - TARGETS - ${OUSTER_INSTALL_TARGETS} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) - -install( - DIRECTORY - include/${PROJECT_NAME}/ - DESTINATION - include/${PROJECT_NAME}/ -) - -install( - DIRECTORY - ouster-sdk/ouster_client/include/ouster - ouster-sdk/ouster_client/include/optional-lite/nonstd - DESTINATION include -) - -install( - FILES - ../LICENSE - DESTINATION - share/${PROJECT_NAME} -) - -install( - DIRECTORY - launch - config - DESTINATION - share/${PROJECT_NAME} -) - -ament_export_include_directories(include) -ament_export_dependencies(rosidl_default_runtime) -ament_export_libraries(ouster_ros_library) -ament_export_dependencies(${ouster_ros_library_deps}) -ament_package() diff --git a/src/third-party/ouster-ros/ouster-ros/config/community_driver.rviz b/src/third-party/ouster-ros/ouster-ros/config/community_driver.rviz deleted file mode 100644 index 927c7778..00000000 --- a/src/third-party/ouster-ros/ouster-ros/config/community_driver.rviz +++ /dev/null @@ -1,262 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 138 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /PointCloud21 - - /TF1/Tree1 - - /range1 - - /range1/Topic1 - - /nearir1/Topic1 - - /reflec1 - - /signal1 - Splitter Ratio: 0.5 - Tree Height: 1185 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: - - /2D Goal Pose1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz_common/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: PointCloud2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: false - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: range - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: true - Max Color: 255; 255; 255 - Max Intensity: 3000 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /points - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz_default_plugins/TF - Enabled: true - Frame Timeout: 15 - Frames: - " imu_data_frame": - Value: true - " laser_data_frame": - Value: true - " laser_sensor_frame": - Value: true - All Enabled: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: false - Tree: - " laser_sensor_frame": - " imu_data_frame": - {} - " laser_data_frame": - {} - Update Interval: 0 - Value: true - - Class: rviz_default_plugins/Image - Enabled: true - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: range - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /range_image - Value: true - - Class: rviz_default_plugins/Image - Enabled: true - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: nearir - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /nearir_image - Value: true - - Class: rviz_default_plugins/Image - Enabled: true - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: reflec - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /reflec_image - Value: true - - Class: rviz_default_plugins/Image - Enabled: true - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: signal - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /signal_image - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: "laser_sensor_frame" - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Covariance x: 0.25 - Covariance y: 0.25 - Covariance yaw: 0.06853891909122467 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /goal_pose - - Class: rviz_default_plugins/PublishPoint - Single click: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /clicked_point - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Class: rviz_default_plugins/Orbit - Distance: 4.583338260650635 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.9003978371620178 - Target Frame: - Value: Orbit (rviz) - Yaw: 4.277213096618652 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 2272 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001f700000599fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000002b3000005990000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f000007defc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000006e000007de0000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000e7000000239fc0100000002fc0000000000000e700000008d00fffffffa000000010200000004fb0000000c007200650066006c006500630000000000ffffffff0000004a00fffffffb0000000c007300690067006e0061006c0100000000ffffffff0000004a00fffffffb0000000c006e006500610072006900720000000000ffffffff0000004a00fffffffb0000000a00720061006e00670065000000006e000002390000004a00fffffffb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000e700000005afc0100000002fb0000000800540069006d0065010000000000000e700000057100fffffffb0000000800540069006d0065010000000000000450000000000000000000000c6d0000059900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 3696 - X: 144 - Y: 54 - nearir: - collapsed: false - range: - collapsed: false - reflec: - collapsed: false - signal: - collapsed: false diff --git a/src/third-party/ouster-ros/ouster-ros/config/community_driver_config.yaml b/src/third-party/ouster-ros/ouster-ros/config/community_driver_config.yaml deleted file mode 100644 index 0c65579d..00000000 --- a/src/third-party/ouster-ros/ouster-ros/config/community_driver_config.yaml +++ /dev/null @@ -1,54 +0,0 @@ -# [Not recommended] Provided to support existing users of the ROS2 Community -# please migrate to the parameters format as specified in driver_params.yaml -ouster_driver: - ros__parameters: - lidar_ip: 169.254.200.2 - computer_ip: 169.254.219.146 - lidar_mode: "1024x10" - imu_port: 7503 - lidar_port: 7502 - sensor_frame: laser_sensor_frame - laser_frame: laser_data_frame - imu_frame: imu_data_frame - - # if False, data are published with sensor data QoS. This is preferrable - # for production but default QoS is needed for rosbag. - # See: https://github.com/ros2/rosbag2/issues/125 - use_system_default_qos: False - - # Set the method used to timestamp measurements. - # Valid modes are: - # - # TIME_FROM_INTERNAL_OSC - # TIME_FROM_SYNC_PULSE_IN - # TIME_FROM_PTP_1588 - # TIME_FROM_ROS_RECEPTION - # - # (See this project's README and/or the Ouster Software Guide for more - # information). - # - timestamp_mode: TIME_FROM_INTERNAL_OSC - - # Mask-like-string used to define the data processors that should be - # activated upon startup of the driver. This will determine the topics - # that are available for client applications to consume. The defacto - # reference for these values are defined in: - # `include/ros2_ouster/processors/processor_factories.hpp` - # - # For convenience, the available data processors are: - # - # IMG - Provides 8-bit image topics suitable for ML applications encoding - # the range, ambient and intensity data from a scan - # PCL - Provides a point cloud encoding of a LiDAR scan - # IMU - Provides a data stream from the LiDARs integral IMU - # SCAN - Provides a synthesized 2D LaserScan from the 3D LiDAR data - # - # To construct a valid string for this parameter join the tokens from above - # (in any combination) with the pipe character. For example, valid strings - # include (but are not limited to): - # - # IMG|PCL - # IMG|PCL|IMU|SCAN - # PCL - # - proc_mask: IMG|PCL|IMU|SCAN|TLM \ No newline at end of file diff --git a/src/third-party/ouster-ros/ouster-ros/config/driver_params.yaml b/src/third-party/ouster-ros/ouster-ros/config/driver_params.yaml deleted file mode 100644 index f4cf64da..00000000 --- a/src/third-party/ouster-ros/ouster-ros/config/driver_params.yaml +++ /dev/null @@ -1,127 +0,0 @@ -ouster/os_driver: - ros__parameters: - # sensor_hostname[required]: hostname or IP address of the sensor (IP4 or - # IP6). - sensor_hostname: '' - # udp_dest[optional]: hostname or multicast group IP where the sensor will - # send UDP data packets. - udp_dest: '' - # mtp_dest[optional]: hostname IP address for receiving data packets via - # multicast, by default it is INADDR_ANY, so packets will be received on - # first available network interface. - mtp_dest: '' - # mtp_main[optional]: if true, then configure and reinit the sensor, - # otherwise start client with active configuration of sensor - mtp_main: false - # lidar_mode[optional]: resolution and rate; possible values: { 512x10, - # 512x20, 1024x10, 1024x20, 2048x10, 4096x5 }. Leave empty to remain on - # current the lidar mode. - lidar_mode: '' - # timestamp_mode[optional]: method used to timestamp measurements; possible - # values: - # - TIME_FROM_INTERNAL_OSC - # - TIME_FROM_SYNC_PULSE_IN - # - TIME_FROM_PTP_1588 - # - TIME_FROM_ROS_TIME: This option uses the time of reception of first - # packet of a LidarScan as the timestamp of the IMU, - # PointCloud2 and LaserScan messages. - timestamp_mode: '' - # ptp_utc_tai_offset[optional]: UTC/TAI offset in seconds to apply when - # TIME_FROM_PTP_1588 timestamp mode is used. - ptp_utc_tai_offset: -37.0 - # udp_profile_lidar[optional]: lidar packet profile; possible values: - # - LEGACY: not recommended - # - RNG19_RFL8_SIG16_NIR16 - # - RNG15_RFL8_NIR8 - # - RNG19_RFL8_SIG16_NIR16_DUAL - # - FUSA_RNG15_RFL8_NIR8_DUAL - udp_profile_lidar: '' - # metadata[optional]: path to save metadata file to, if left empty a file - # with the sensor hostname or ip will be crearted in ~/.ros folder. - metadata: '' - # lidar_port[optional]: port value should be in the range [0, 65535]. If you - # use 0 as the port value then the first avaliable port number will be - # assigned. - lidar_port: 0 - # imu_port[optional]: port value should be in the range [0, 65535]. If you - # use 0 as the port value then the first avaliable port number will be - # assigned. - imu_port: 0 - # sensor_frame[optional]: name to use when referring to the sensor frame. - sensor_frame: os_sensor - # lidar_frame[optional]: name to use when referring to the lidar frame. - lidar_frame: os_lidar - # imu_frame[optional]: name to use when referring to the imu frame. - imu_frame: os_imu - # point_cloud_frame[optional]: which frame of reference to use when - # generating PointCloud2 or LaserScan messages, select between the values of - # lidar_frame and sensor_frame. - point_cloud_frame: os_lidar - # pub_static_tf[optional]: when this flag is set to True, the driver will - # broadcast the TF transforms for the imu/sensor/lidar frames. Prevent the - # driver from broadcasting TF transforms by setting this parameter to False. - pub_static_tf: true - # proc_mask[optional]: use any combination of the 6 flags IMG, PCL, IMU, SCAN - # RAW and TLM to enable or disable their respective messages. - proc_mask: IMU|PCL|SCAN|IMG|RAW|TLM - # scan_ring[optional]: use this parameter in conjunction with the SCAN flag - # to select which beam of the LidarScan to use when producing the LaserScan - # message. Choose a value the range [0, sensor_beams_count). - scan_ring: 0 - # use_system_default_qos[optional]: if false, data are published with sensor - # data QoS. This is preferrable for production but default QoS is needed for - # rosbag. See: https://github.com/ros2/rosbag2/issues/125 - use_system_default_qos: false - # point_type[optional]: choose from: {original, native, xyz, xyzi, o_xyzi, - # yzir} - # Here is a breif description of each option: - # - original: This uses the original point representation ouster_ros::Point - # of the ouster-ros driver. - # - native: directly maps all fields as published by the sensor to an - # equivalent point cloud representation with the additon of ring - # and timestamp fields. - # - xyz: the simplest point type, only has {x, y, z} - # - xyzi: same as xyz point type but adds intensity (signal) field. this - # type is not compatible with the low data profile. - # - o_xyzi: same as xyzi point type but doesn't add the extra 4 padding bytes. - # - xyzir: same as xyzi type but adds ring (channel) field. - # this type is same as Velodyne point cloud type - # this type is not compatible with the low data profile. - # for more details about the fields of each point type and their data refer - # to the following header files: - # - ouster_ros/os_point.h - # - ouster_ros/sensor_point_types.h - # - ouster_ros/common_point_types.h. - point_type: original - # azimuth window start[optional]: values range [0, 360000] millidegrees - azimuth_window_start: 0 - # azimuth_window_end[optional]: values range [0, 360000] millidegrees - azimuth_window_end: 360000 - # persist_config[optional]: request the sensor to persist settings - persist_config: false - # attempt_config[optional]: attempting to reconnect to the sensor after - # connection loss or sensor powered down - attempt_reconnect: false - # dormant_period_between_reconnects[optional]: wait time in seconds between - # reconnection attempts - dormant_period_between_reconnects: 1.0 - # max_failed_reconnect_attempts[optional]: maximum number of attempts trying - # to communicate with the sensor. Counter resets upon successful connection. - max_failed_reconnect_attempts: 2147483647 - # organized[optional]: whether to generate an organized point cloud. default - # is organized. - organized: true - # destagger[optional]: enable or disable point cloud destaggering, default enabled. - destagger: true - # min_range[optional]: minimum lidar range to consider (meters). - min_range: 0.0 - # max_range[optional]: maximum lidar range to consider (meters). - max_range: 1000.0 - # v_reduction[optional]: vertical beam reduction; available options: {1, 2, 4, 8, 16}. - v_reduction: 1 - # min_scan_valid_columns_ratio[optional]: The minimum ratio of valid columns for - # processing the LidarScan [0, 1]. default is 0% - min_scan_valid_columns_ratio: 0.0 - # mask_path[optional]: path to an image file that will be used to mask parts of the - # pointcloud. - mask_path: '' diff --git a/src/third-party/ouster-ros/ouster-ros/config/metadata-qos-override.yaml b/src/third-party/ouster-ros/ouster-ros/config/metadata-qos-override.yaml deleted file mode 100644 index ffbf1b6c..00000000 --- a/src/third-party/ouster-ros/ouster-ros/config/metadata-qos-override.yaml +++ /dev/null @@ -1,11 +0,0 @@ -/ouster/metadata: - history: keep_last - depth: 1 - reliability: reliable - durability: transient_local - -/os_node/metadata: - history: keep_last - depth: 1 - reliability: reliable - durability: transient_local diff --git a/src/third-party/ouster-ros/ouster-ros/config/os_sensor_cloud_image_params.yaml b/src/third-party/ouster-ros/ouster-ros/config/os_sensor_cloud_image_params.yaml deleted file mode 100644 index e6826858..00000000 --- a/src/third-party/ouster-ros/ouster-ros/config/os_sensor_cloud_image_params.yaml +++ /dev/null @@ -1,39 +0,0 @@ -# [Not recommended] Use this configuration only when utilizing the disjointed -# os_sensor and os_cloud configuration -# For the future and for better results consider migrating to combined node/component os_driver -# and utilize the driver_params.yaml along with it. -ouster/os_sensor: - ros__parameters: - sensor_hostname: '' - udp_dest: '' - mtp_dest: '' - mtp_main: false - lidar_mode: '' - timestamp_mode: '' - udp_profile_lidar: '' - metadata: '' - lidar_port: 0 - imu_port: 0 - use_system_default_qos: false - azimuth_window_start: 0 - azimuth_window_end: 360000 - persist_config: false - attempt_reconnect: false - dormant_period_between_reconnects: 1.0 - max_failed_reconnect_attempts: 2147483647 -ouster/os_cloud: - ros__parameters: - sensor_frame: os_sensor - lidar_frame: os_lidar - imu_frame: os_imu - point_cloud_frame: os_lidar - pub_static_tf: true - timestamp_mode: '' # this value needs to match os_sensor/timestamp_mode - ptp_utc_tai_offset: -37.0 # UTC/TAI offset in seconds to apply when using TIME_FROM_PTP_1588 - proc_mask: IMU|PCL|SCAN|TLM # pick IMU, PCL, SCAN, TLM or any combination of the 4 options - use_system_default_qos: false # needs to match the value defined for os_sensor node - scan_ring: 0 # Use this parameter in conjunction with the SCAN flag and choose a - # value the range [0, sensor_beams_count) - point_type: original # choose from: {original, native, xyz, xyzi, o_xyzi, xyzir} -ouster/os_image: - use_system_default_qos: false # needs to match the value defined for os_sensor node diff --git a/src/third-party/ouster-ros/ouster-ros/config/viz-reliable.rviz b/src/third-party/ouster-ros/ouster-ros/config/viz-reliable.rviz deleted file mode 100644 index 42a2301f..00000000 --- a/src/third-party/ouster-ros/ouster-ros/config/viz-reliable.rviz +++ /dev/null @@ -1,262 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 138 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /PointCloud21 - - /TF1/Tree1 - - /range1 - - /range1/Topic1 - - /nearir1/Topic1 - - /reflec1 - - /signal1 - Splitter Ratio: 0.5 - Tree Height: 1185 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: - - /2D Goal Pose1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz_common/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: PointCloud2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: false - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: range - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: true - Max Color: 255; 255; 255 - Max Intensity: 10000 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /ouster/points - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz_default_plugins/TF - Enabled: true - Frame Timeout: 15 - Frames: - " os_imu": - Value: true - " os_lidar": - Value: true - " os_sensor": - Value: true - All Enabled: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: false - Tree: - " os_sensor": - " os_imu": - {} - " os_lidar": - {} - Update Interval: 0 - Value: true - - Class: rviz_default_plugins/Image - Enabled: true - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: range - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /ouster/range_image - Value: true - - Class: rviz_default_plugins/Image - Enabled: true - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: nearir - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /ouster/nearir_image - Value: true - - Class: rviz_default_plugins/Image - Enabled: true - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: reflec - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /ouster/reflec_image - Value: true - - Class: rviz_default_plugins/Image - Enabled: true - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: signal - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /ouster/signal_image - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: "os_sensor" - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Covariance x: 0.25 - Covariance y: 0.25 - Covariance yaw: 0.06853891909122467 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /goal_pose - - Class: rviz_default_plugins/PublishPoint - Single click: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /clicked_point - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Class: rviz_default_plugins/Orbit - Distance: 4.583338260650635 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.9003978371620178 - Target Frame: - Value: Orbit (rviz) - Yaw: 4.277213096618652 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 2272 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001f700000599fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000002b3000005990000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f000007defc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000006e000007de0000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000e7000000239fc0100000002fc0000000000000e700000008d00fffffffa000000010200000004fb0000000c007200650066006c006500630000000000ffffffff0000004a00fffffffb0000000c007300690067006e0061006c0100000000ffffffff0000004a00fffffffb0000000c006e006500610072006900720000000000ffffffff0000004a00fffffffb0000000a00720061006e00670065000000006e000002390000004a00fffffffb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000e700000005afc0100000002fb0000000800540069006d0065010000000000000e700000057100fffffffb0000000800540069006d0065010000000000000450000000000000000000000c6d0000059900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 3696 - X: 144 - Y: 54 - nearir: - collapsed: false - range: - collapsed: false - reflec: - collapsed: false - signal: - collapsed: false diff --git a/src/third-party/ouster-ros/ouster-ros/config/viz.rviz b/src/third-party/ouster-ros/ouster-ros/config/viz.rviz deleted file mode 100644 index c7f1642b..00000000 --- a/src/third-party/ouster-ros/ouster-ros/config/viz.rviz +++ /dev/null @@ -1,298 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 138 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /PointCloud21 - - /TF1/Tree1 - - /range1 - - /range1/Topic1 - - /nearir1/Topic1 - - /reflec1 - - /signal1 - - /LaserScan1 - - /LaserScan1/Topic1 - Splitter Ratio: 0.626074492931366 - Tree Height: 1185 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: - - /2D Goal Pose1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz_common/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: PointCloud2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: false - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: range - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: true - Max Color: 255; 255; 255 - Max Intensity: 10000 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /ouster/points - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz_default_plugins/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: true - os_imu: - Value: true - os_lidar: - Value: true - os_sensor: - Value: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: false - Tree: - os_sensor: - os_imu: - {} - os_lidar: - {} - Update Interval: 0 - Value: true - - Class: rviz_default_plugins/Image - Enabled: true - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: range - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /ouster/range_image - Value: true - - Class: rviz_default_plugins/Image - Enabled: true - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: nearir - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /ouster/nearir_image - Value: true - - Class: rviz_default_plugins/Image - Enabled: true - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: reflec - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /ouster/reflec_image - Value: true - - Class: rviz_default_plugins/Image - Enabled: true - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: signal - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /ouster/signal_image - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: false - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/LaserScan - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 1000 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: LaserScan - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.02 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /ouster/scan - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: os_sensor - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Covariance x: 0.25 - Covariance y: 0.25 - Covariance yaw: 0.06853891909122467 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /goal_pose - - Class: rviz_default_plugins/PublishPoint - Single click: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /clicked_point - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Class: rviz_default_plugins/Orbit - Distance: 4.583338260650635 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.9003978371620178 - Target Frame: - Value: Orbit (rviz) - Yaw: 4.277213096618652 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 2272 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001f700000599fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000002b3000005990000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f000007defc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000006e000007de0000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000e7000000239fc0100000002fc0000000000000e700000008d00fffffffa000000010200000004fb0000000c007200650066006c006500630000000000ffffffff0000004a00fffffffb0000000c007300690067006e0061006c0100000000ffffffff0000004a00fffffffb0000000c006e006500610072006900720000000000ffffffff0000004a00fffffffb0000000a00720061006e00670065000000006e000002390000004a00fffffffb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000e700000005afc0100000002fb0000000800540069006d0065010000000000000e700000057100fffffffb0000000800540069006d0065010000000000000450000000000000000000000c6d0000059900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 3696 - X: 144 - Y: 54 - nearir: - collapsed: false - range: - collapsed: false - reflec: - collapsed: false - signal: - collapsed: false diff --git a/src/third-party/ouster-ros/ouster-ros/include/ouster_ros/common_point_types.h b/src/third-party/ouster-ros/ouster-ros/include/ouster_ros/common_point_types.h deleted file mode 100644 index 7fc4d7cc..00000000 --- a/src/third-party/ouster-ros/ouster-ros/include/ouster_ros/common_point_types.h +++ /dev/null @@ -1,125 +0,0 @@ -/** - * Copyright (c) 2018-2023, Ouster, Inc. - * All rights reserved. - * - * @file common_point_types.h - * @brief common PCL point datatype for use with ouster sensors - */ - -#pragma once - -#include - -namespace ouster_ros { - -/* The following are pcl point representations that are common/standard point - representation that we readily support. - */ - -/* - * Same as Apollo point cloud type - * @remark XYZIT point type is not compatible with RNG15_RFL8_NIR8/LOW_DATA - * udp lidar profile. - */ -struct EIGEN_ALIGN16 _Ouster_PointXYZI { - union EIGEN_ALIGN16 { - float data[4]; - struct { - float x; - float y; - float z; - float intensity; - }; - }; - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -struct PointXYZI : public _Ouster_PointXYZI { - - inline PointXYZI(const _Ouster_PointXYZI& pt) - { - x = pt.x; y = pt.y; z = pt.z; - intensity = pt.intensity; - } - - inline PointXYZI() - { - x = y = z = 0.0f; data[3] = 1.0f; - intensity = 0.0f; - } - - inline const auto as_tuple() const { - return std::tie(x, y, z, intensity); - } - - inline auto as_tuple() { - return std::tie(x, y, z, intensity); - } - - template - inline auto& get() { - return std::get(as_tuple()); - } -}; - -/* - * Same as Velodyne point cloud type - * @remark XYZIR point type is not compatible with RNG15_RFL8_NIR8/LOW_DATA - * udp lidar profile. - */ -struct EIGEN_ALIGN16 _PointXYZIR { - PCL_ADD_POINT4D; - float intensity; - uint16_t ring; - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -struct PointXYZIR : public _PointXYZIR { - - inline PointXYZIR(const _PointXYZIR& pt) - { - x = pt.x; y = pt.y; z = pt.z; data[3] = 1.0f; - intensity = pt.intensity; ring = pt.ring; - } - - inline PointXYZIR() - { - x = y = z = 0.0f; data[3] = 1.0f; - intensity = 0.0f; ring = 0; - } - - inline const auto as_tuple() const { - return std::tie(x, y, z, intensity, ring); - } - - inline auto as_tuple() { - return std::tie(x, y, z, intensity, ring); - } - - template - inline auto& get() { - return std::get(as_tuple()); - } -}; - -} // namespace ouster_ros - -// clang-format off - -POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::PointXYZI, - (float, x, x) - (float, y, y) - (float, z, z) - (float, intensity, intensity) -) - -/* common point types */ -POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::PointXYZIR, - (float, x, x) - (float, y, y) - (float, z, z) - (float, intensity, intensity) - (std::uint16_t, ring, ring) -) - -// clang-format on diff --git a/src/third-party/ouster-ros/ouster-ros/include/ouster_ros/os_point.h b/src/third-party/ouster-ros/ouster-ros/include/ouster_ros/os_point.h deleted file mode 100644 index 0a174f3f..00000000 --- a/src/third-party/ouster-ros/ouster-ros/include/ouster_ros/os_point.h +++ /dev/null @@ -1,88 +0,0 @@ -/** - * Copyright (c) 2018-2023, Ouster, Inc. - * All rights reserved. - * - * @file os_point.h - * @brief PCL point datatype for use with ouster sensors - */ - -#pragma once - -#include - -namespace ouster_ros { - -// The default/original representation of the point cloud since the driver -// inception. This shouldn't be confused with Point_LEGACY which provides exact -// mapping of the fields of Ouster LidarScan of the Legacy Profile, copying -// the same order and using the same bit representation. For example, this Point -// struct uses float data type to represent intensity (aka signal); however, the -// sensor sends the signal channel either as UINT16 or UINT32 depending on the -// active udp lidar profile. -struct EIGEN_ALIGN16 _Point { - PCL_ADD_POINT4D; - float intensity; // equivalent to signal - uint32_t t; - uint16_t reflectivity; - uint16_t ring; // equivalent to channel - uint16_t ambient; // equivalent to near_ir - uint32_t range; - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -struct Point : public _Point { - - inline Point(const _Point& pt) - { - x = pt.x; y = pt.y; z = pt.z; data[3] = 1.0f; - intensity = pt.intensity; - t = pt.t; - reflectivity = pt.reflectivity; - ring = pt.ring; - ambient = pt.ambient; - range = pt.range; - } - - inline Point() - { - x = y = z = 0.0f; data[3] = 1.0f; - intensity = 0.0f; - t = 0; - reflectivity = 0; - ring = 0; - ambient = 0; - range = 0; - } - - inline const auto as_tuple() const { - return std::tie(x, y, z, intensity, t, reflectivity, ring, ambient, range); - } - - inline auto as_tuple() { - return std::tie(x, y, z, intensity, t, reflectivity, ring, ambient, range); - } - - template - inline auto& get() { - return std::get(as_tuple()); - } -}; - -} // namespace ouster_ros - -// clang-format off - -// DEFAULT/ORIGINAL -POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point, - (float, x, x) - (float, y, y) - (float, z, z) - (float, intensity, intensity) - (std::uint32_t, t, t) - (std::uint16_t, reflectivity, reflectivity) - (std::uint16_t, ring, ring) - (std::uint16_t, ambient, ambient) - (std::uint32_t, range, range) -) - -// clang-format on diff --git a/src/third-party/ouster-ros/ouster-ros/include/ouster_ros/os_processing_node_base.h b/src/third-party/ouster-ros/ouster-ros/include/ouster_ros/os_processing_node_base.h deleted file mode 100644 index 1b6a9771..00000000 --- a/src/third-party/ouster-ros/ouster-ros/include/ouster_ros/os_processing_node_base.h +++ /dev/null @@ -1,33 +0,0 @@ -/** - * Copyright (c) 2018-2023, Ouster, Inc. - * All rights reserved. - * - * @file os_processing_node_base.h - * @brief Base class for ouster_ros os_cloud and os_image nodes - * - */ - -#include - -#include -#include -#include - -namespace ouster_ros { - -class OusterProcessingNodeBase : public rclcpp::Node { - protected: - OusterProcessingNodeBase(const std::string& name, - const rclcpp::NodeOptions& options) - : rclcpp::Node(name, options) {} - - void create_metadata_subscriber( - std::function - on_sensor_metadata); - - protected: - rclcpp::Subscription::SharedPtr metadata_sub; - ouster::sensor::sensor_info info; -}; - -} // namespace ouster_ros \ No newline at end of file diff --git a/src/third-party/ouster-ros/ouster-ros/include/ouster_ros/os_ros.h b/src/third-party/ouster-ros/ouster-ros/include/ouster_ros/os_ros.h deleted file mode 100644 index 4d7301f7..00000000 --- a/src/third-party/ouster-ros/ouster-ros/include/ouster_ros/os_ros.h +++ /dev/null @@ -1,212 +0,0 @@ -/** - * Copyright (c) 2018-2023, Ouster, Inc. - * All rights reserved. - * - * @file os_ros.h - * @brief Higher-level functions to read data from the ouster sensors as ROS - * messages - */ - -#pragma once - -#define PCL_NO_PRECOMPILE -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "ouster_sensor_msgs/msg/packet_msg.hpp" -#include "ouster_sensor_msgs/msg/telemetry.hpp" -#include "ouster_ros/os_point.h" - -namespace ouster_ros { - -namespace sensor = ouster::sensor; - -/** - * Checks sensor_info if it currently represents a legacy udp lidar profile - * @param[in] info sensor_info - * @return whether sensor_info represents the legacy udp lidar profile - */ -bool is_legacy_lidar_profile(const sensor::sensor_info& info); - -/** - * Gets the number of point cloud returns that this sensor_info object represents - * @param[in] info sensor_info - * @return number of returns - */ -int get_n_returns(const sensor::sensor_info& info); - - -/** - * Gets the number beams based on supplied sensor_info - * @param[in] info sensor_info - * @return number of beams a sensor has - */ -size_t get_beams_count(const sensor::sensor_info& info); - -/** - * Adds a suffix to the topic base name based on the return index - * @param[in] topic_base topic base name - * @param[in] return_idx return index {0, 1, ... n_returns } - * @return number of returns - */ -std::string topic_for_return(const std::string& topic_base, int return_idx); - -/** - * Parse an imu packet message into a ROS imu message - * @param[in] pf the packet format - * @param[in] timestamp the timestamp to give the resulting ROS message - * @param[in] frame the frame to set in the resulting ROS message - * @param[in] buf the raw packet message populated by read_imu_packet - * @return ROS sensor message with fields populated from the packet - */ -sensor_msgs::msg::Imu packet_to_imu_msg(const ouster::sensor::packet_format& pf, - const rclcpp::Time& timestamp, - const std::string& frame, - const uint8_t* buf); - -/** - * Parse an imu packet message into a ROS imu message - * @param[in] pm packet message populated by read_imu_packet - * @param[in] timestamp the timestamp to give the resulting ROS message - * @param[in] frame the frame to set in the resulting ROS message - * @param[in] pf the packet format - * @return ROS sensor message with fields populated from the packet - */ -sensor_msgs::msg::Imu packet_to_imu_msg(const ouster_sensor_msgs::msg::PacketMsg& pm, - const rclcpp::Time& timestamp, - const std::string& frame, - const sensor::packet_format& pf); - -/** - * Convert transformation matrix return by sensor to ROS transform - * @param[in] mat transformation matrix return by sensor - * @param[in] frame the parent frame of the published transform - * @param[in] child_frame the child frame of the published transform - * @param[in] timestamp value to set as the timestamp of the generated - * TransformStamped message - * @return ROS message suitable for publishing as a transform - */ -geometry_msgs::msg::TransformStamped transform_to_tf_msg( - const ouster::mat4d& mat, const std::string& frame, - const std::string& child_frame, rclcpp::Time timestamp); - - -/** - * Convert transformation matrix return by sensor to ROS transform - * @param[in] ls lidar scan object - * @param[in] timestamp value to set as the timestamp of the generated - * @param[in] frame the parent frame of the generated laser scan message - * @param[in] lidar_mode lidar mode (width x frequency) - * @param[in] ring selected ring to be published - * @param[in] pixel_shift_by_row pixel shifts by row - * @param[in] return_index index of return desired starting at 0 - * @return ROS message suitable for publishing as a LaserScan - */ -sensor_msgs::msg::LaserScan lidar_scan_to_laser_scan_msg( - const ouster::LidarScan& ls, - const rclcpp::Time& timestamp, - const std::string &frame, - const ouster::sensor::lidar_mode lidar_mode, - const uint16_t ring, const std::vector& pixel_shift_by_row, - const int return_index); - -/** - * Parse a LidarPacket and generate the Telemetry message - * @param[in] lidar_packet lidar packet to parse telemetry data from - * @param[in] timestamp the timestamp to give the resulting ROS message - * @param[in] pf the packet format - * @return ROS sensor message with fields populated from the packet - */ -ouster_sensor_msgs::msg::Telemetry lidar_packet_to_telemetry_msg( - const ouster::sensor::LidarPacket& lidar_packet, - const rclcpp::Time& timestamp, - const ouster::sensor::packet_format& pf); - -namespace impl { - -sensor::ChanField scan_return(sensor::ChanField input_field, bool second); - -struct read_and_cast { - template - void operator()(Eigen::Ref> field, - ouster::img_t& dest) { - dest = field.template cast(); - } -}; - -template -inline ouster::img_t get_or_fill_zero(sensor::ChanField field, - const ouster::LidarScan& ls) { - if (!ls.field_type(field)) return ouster::img_t::Zero(ls.h, ls.w); - ouster::img_t result{ls.h, ls.w}; - ouster::impl::visit_field(ls, field, read_and_cast(), result); - return result; -} - -/** - * simple utility function that ensures we don't wrap around uint64_t due - * to a negative value being bigger than ts value in absolute terms. - * @remark method does not check upper boundary - */ -inline uint64_t ts_safe_offset_add(uint64_t ts, int64_t offset) { - return offset < 0 && ts < static_cast(std::abs(offset)) ? 0 : ts + offset; -} - -std::set parse_tokens(const std::string& input, char delim); - -inline bool check_token(const std::set& tokens, - const std::string& token) { - return tokens.find(token) != tokens.end(); -} - -ouster::util::version parse_version(const std::string& fw_rev); - -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); -} - -void warn_mask_resized(int image_cols, int image_rows, - int scan_height, int scan_width); - -template -ouster::img_t load_mask(const std::string& mask_path, - size_t height, size_t width) { - if (mask_path.empty()) return ouster::img_t(); - - cv::Mat image = cv::imread(mask_path, cv::IMREAD_GRAYSCALE); - if (image.empty()) { - throw std::runtime_error("Failed to load mask image from path: " + mask_path); - } - - if (image.rows != static_cast(height) || image.cols != static_cast(width)) { - warn_mask_resized(image.cols, image.rows, static_cast(height), static_cast(width)); - cv::Mat resized; - cv::resize(image, resized, cv::Size(width, height), 0, 0, cv::INTER_NEAREST); - image = resized; - } - Eigen::MatrixXi eigen_img(image.rows, image.cols); - cv::cv2eigen(image, eigen_img); - Eigen::MatrixXi zero_image = Eigen::MatrixXi::Zero(eigen_img.rows(), eigen_img.cols()); - Eigen::MatrixXi ones_image = Eigen::MatrixXi::Ones(eigen_img.rows(), eigen_img.cols()); - return (eigen_img.array() == 0.0).select(zero_image, ones_image).cast(); -} - -} // namespace impl - -} // namespace ouster_ros diff --git a/src/third-party/ouster-ros/ouster-ros/include/ouster_ros/os_sensor_node_base.h b/src/third-party/ouster-ros/ouster-ros/include/ouster_ros/os_sensor_node_base.h deleted file mode 100644 index 2bafe696..00000000 --- a/src/third-party/ouster-ros/ouster-ros/include/ouster_ros/os_sensor_node_base.h +++ /dev/null @@ -1,66 +0,0 @@ -/** - * Copyright (c) 2018-2023, Ouster, Inc. - * All rights reserved. - * - * @file os_sensor_node_base.h - * @brief Base class for ouster_ros sensor and replay nodes - * - */ - -#include - -#include -#include -#include -#include - -#include "ouster_sensor_msgs/srv/get_metadata.hpp" - -#include - -namespace ouster_ros { - -class OusterSensorNodeBase : public rclcpp_lifecycle::LifecycleNode { - protected: - explicit OusterSensorNodeBase(const std::string& name, - const rclcpp::NodeOptions& options); - - protected: - bool is_arg_set(const std::string& arg) const { - return arg.find_first_not_of(' ') != std::string::npos; - } - - void create_get_metadata_service(); - - void create_metadata_pub(); - - void publish_metadata(); - - void display_lidar_info(const ouster::sensor::sensor_info& info); - - static std::string read_text_file(const std::string& text_file); - - static bool write_text_to_file(const std::string& file_path, - const std::string& text); - - static std::string transition_id_to_string(uint8_t transition_id); - - template - bool change_state(std::uint8_t transition_id, CallbackT callback, - CallbackT_Args... callback_args, - std::chrono::seconds time_out = std::chrono::seconds{3}); - - void execute_transitions_sequence(std::vector transitions_sequence, - size_t at); - - - protected: - std::shared_ptr> change_state_client; - - ouster::sensor::sensor_info info; - rclcpp::Service::SharedPtr get_metadata_srv; - std::string cached_metadata; - rclcpp::Publisher::SharedPtr metadata_pub; -}; - -} // namespace ouster_ros \ No newline at end of file diff --git a/src/third-party/ouster-ros/ouster-ros/include/ouster_ros/sensor_point_types.h b/src/third-party/ouster-ros/ouster-ros/include/ouster_ros/sensor_point_types.h deleted file mode 100644 index fa2a9a13..00000000 --- a/src/third-party/ouster-ros/ouster-ros/include/ouster_ros/sensor_point_types.h +++ /dev/null @@ -1,424 +0,0 @@ -/** - * Copyright (c) 2018-2023, Ouster, Inc. - * All rights reserved. - * - * @file sensor_point_types.h - * @brief native sensor point types - * The following are one-to-one mapping of pcl point representatios that could - * fit the data sent by the sensor (with the addition of t and ring fields), - * All of these representation follow the same order of LaserScan fields: - * 1. range - * 2. signal - * 3. refelctivity - * 4. near_ir - * With the exception Point_RNG15_RFL8_NIR8 aka LOW_DATA which does not include - * the signal field -**/ - -#pragma once - -#include -#include - -namespace ouster_ros { - -template -using Table = std::array, N>; - -namespace sensor = ouster::sensor; - -template -using ChanFieldTable = Table; - -} - - -namespace ouster_ros { - -// Profile_LEGACY -static constexpr ChanFieldTable<4> Profile_LEGACY{{ - {sensor::ChanField::RANGE, sensor::ChanFieldType::UINT32}, - {sensor::ChanField::SIGNAL, sensor::ChanFieldType::UINT32}, - {sensor::ChanField::NEAR_IR, sensor::ChanFieldType::UINT32}, - {sensor::ChanField::REFLECTIVITY, sensor::ChanFieldType::UINT32}} -}; - -// auto=LEGACY -struct EIGEN_ALIGN16 _Point_LEGACY { - PCL_ADD_POINT4D; - uint32_t t; // timestamp in nanoseconds relative to frame start - uint16_t ring; // equivalent to channel - uint32_t range; - uint32_t signal; // equivalent to intensity - uint32_t reflectivity; - uint32_t near_ir; // equivalent to ambient - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -struct Point_LEGACY : public _Point_LEGACY { - - inline Point_LEGACY(const _Point_LEGACY& pt) - { - x = pt.x; y = pt.y; z = pt.z; data[3] = 1.0f; - t = pt.t; ring = pt.ring; - range = pt.range; signal = pt.signal; - reflectivity = pt.reflectivity; near_ir = pt.near_ir; - } - - inline Point_LEGACY() - { - x = y = z = 0.0f; data[3] = 1.0f; - t = 0; ring = 0; - range = 0; signal = 0; - reflectivity = 0; near_ir = 0; - } - - inline const auto as_tuple() const { - return std::tie(x, y, z, t, ring, range, signal, reflectivity, near_ir); - } - - inline auto as_tuple() { - return std::tie(x, y, z, t, ring, range, signal, reflectivity, near_ir); - } - - template - inline auto& get() { - return std::get(as_tuple()); - } -}; - -} // namespace ouster_ros - -// clang-format off - -POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point_LEGACY, - (float, x, x) - (float, y, y) - (float, z, z) - (std::uint32_t, t, t) - (std::uint16_t, ring, ring) - (std::uint32_t, range, range) - (std::uint32_t, signal, signal) - (std::uint32_t, reflectivity, reflectivity) - (std::uint32_t, near_ir, near_ir) -) - -namespace ouster_ros { - -// Profile_RNG19_RFL8_SIG16_NIR16_DUAL: aka dual returns -// This profile is definied differently from RNG19_RFL8_SIG16_NIR16_DUAL of how -// the sensor actually sends the data. The actual RNG19_RFL8_SIG16_NIR16_DUAL -// has 7 fields not 4, but this profile is defined differently in ROS because -// we build and publish a point cloud for each return separately. However, it -// might be desireable to some of the users to choose a point cloud -// representation which combines parts of the the two or more returns. This isn't -// something that the current framework could deal with as of now. -static constexpr ChanFieldTable<4> Profile_RNG19_RFL8_SIG16_NIR16_DUAL {{ - {sensor::ChanField::RANGE, sensor::ChanFieldType::UINT32}, - {sensor::ChanField::SIGNAL, sensor::ChanFieldType::UINT16}, - {sensor::ChanField::REFLECTIVITY, sensor::ChanFieldType::UINT8}, - {sensor::ChanField::NEAR_IR, sensor::ChanFieldType::UINT16}, -}}; - -// Note: this is one way to implement the processing of 2nd return -// This should be an exact copy of Profile_RNG19_RFL8_SIG16_NIR16_DUAL with the -// exception of ChanField values for the first three fields. NEAR_IR is same for both -static constexpr ChanFieldTable<4> Profile_RNG19_RFL8_SIG16_NIR16_DUAL_2ND_RETURN {{ - {sensor::ChanField::RANGE2, sensor::ChanFieldType::UINT32}, - {sensor::ChanField::SIGNAL2, sensor::ChanFieldType::UINT16}, - {sensor::ChanField::REFLECTIVITY2, sensor::ChanFieldType::UINT8}, - {sensor::ChanField::NEAR_IR, sensor::ChanFieldType::UINT16}, -}}; - -// auto=RNG19_RFL8_SIG16_NIR16_DUAL -struct EIGEN_ALIGN16 _Point_RNG19_RFL8_SIG16_NIR16_DUAL { - PCL_ADD_POINT4D; - uint32_t t; // timestamp in nanoseconds relative to frame start - uint16_t ring; // equivalent channel - uint32_t range; - uint16_t signal; // equivalent to intensity - uint8_t reflectivity; - uint16_t near_ir; // equivalent to ambient - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -struct Point_RNG19_RFL8_SIG16_NIR16_DUAL : public _Point_RNG19_RFL8_SIG16_NIR16_DUAL { - - inline Point_RNG19_RFL8_SIG16_NIR16_DUAL(const _Point_RNG19_RFL8_SIG16_NIR16_DUAL& pt) - { - x = pt.x; y = pt.y; z = pt.z; data[3] = 1.0f; - t = pt.t; ring = pt.ring; - range = pt.range; signal = pt.signal; - reflectivity = pt.reflectivity; near_ir = pt.near_ir; - } - - inline Point_RNG19_RFL8_SIG16_NIR16_DUAL() - { - x = y = z = 0.0f; data[3] = 1.0f; - t = 0; ring = 0; - range = 0; signal = 0; - reflectivity = 0; near_ir = 0; - } - - inline const auto as_tuple() const { - return std::tie(x, y, z, t, ring, range, signal, reflectivity, near_ir); - } - - inline auto as_tuple() { - return std::tie(x, y, z, t, ring, range, signal, reflectivity, near_ir); - } - - template - inline auto& get() { - return std::get(as_tuple()); - } -}; - -} // namespce ouster_ros - -// clang-format off - -POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point_RNG19_RFL8_SIG16_NIR16_DUAL, - (float, x, x) - (float, y, y) - (float, z, z) - (std::uint32_t, t, t) - (std::uint16_t, ring, ring) - (std::uint32_t, range, range) - (std::uint16_t, signal, signal) - (std::uint8_t, reflectivity, reflectivity) - (std::uint16_t, near_ir, near_ir) -) - -// clang-format on - -namespace ouster_ros { - -// Profile_RNG19_RFL8_SIG16_NIR16 aka single return -static constexpr ChanFieldTable<4> Profile_RNG19_RFL8_SIG16_NIR16{{ - {sensor::ChanField::RANGE, sensor::ChanFieldType::UINT32}, - {sensor::ChanField::SIGNAL, sensor::ChanFieldType::UINT16}, - {sensor::ChanField::REFLECTIVITY, sensor::ChanFieldType::UINT16}, - {sensor::ChanField::NEAR_IR, sensor::ChanFieldType::UINT16}, -}}; - -// auto=RNG19_RFL8_SIG16_NIR16 -struct EIGEN_ALIGN16 _Point_RNG19_RFL8_SIG16_NIR16 { - PCL_ADD_POINT4D; - uint32_t t; // timestamp in nanoseconds relative to frame start - uint16_t ring; // equivalent channel - uint32_t range; - uint16_t signal; // equivalent to intensity - uint16_t reflectivity; - uint16_t near_ir; // equivalent to ambient - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -struct Point_RNG19_RFL8_SIG16_NIR16 : public _Point_RNG19_RFL8_SIG16_NIR16 { - - inline Point_RNG19_RFL8_SIG16_NIR16(const _Point_RNG19_RFL8_SIG16_NIR16& pt) - { - x = pt.x; y = pt.y; z = pt.z; data[3] = 1.0f; - t = pt.t; ring = pt.ring; - range = pt.range; signal = pt.signal; - reflectivity = pt.reflectivity; near_ir = pt.near_ir; - } - - inline Point_RNG19_RFL8_SIG16_NIR16() - { - x = y = z = 0.0f; data[3] = 1.0f; - t = 0; ring = 0; - range = 0; signal = 0; - reflectivity = 0; near_ir = 0; - } - - inline const auto as_tuple() const { - return std::tie(x, y, z, t, ring, range, signal, reflectivity, near_ir); - } - - inline auto as_tuple() { - return std::tie(x, y, z, t, ring, range, signal, reflectivity, near_ir); - } - - template - inline auto& get() { - return std::get(as_tuple()); - } -}; - -} // namespace ouster_ros - -// clang-format off - -POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point_RNG19_RFL8_SIG16_NIR16, - (float, x, x) - (float, y, y) - (float, z, z) - (std::uint32_t, t, t) - (std::uint16_t, ring, ring) - (std::uint32_t, range, range) - (std::uint16_t, signal, signal) - (std::uint16_t, reflectivity, reflectivity) - (std::uint16_t, near_ir, near_ir) -) - -// clang-format on - -namespace ouster_ros { - -// Profile_RNG15_RFL8_NIR8 aka LOW_DATA -static constexpr ChanFieldTable<3> Profile_RNG15_RFL8_NIR8{{ - {sensor::ChanField::RANGE, sensor::ChanFieldType::UINT32}, - {sensor::ChanField::REFLECTIVITY, sensor::ChanFieldType::UINT16}, - {sensor::ChanField::NEAR_IR, sensor::ChanFieldType::UINT16}, -}}; - -// auto=RNG15_RFL8_NIR8 aka LOW_DATA profile -struct EIGEN_ALIGN16 _Point_RNG15_RFL8_NIR8 { - PCL_ADD_POINT4D; - // No signal/intensity in low data mode - uint32_t t; // timestamp in nanoseconds relative to frame start - uint16_t ring; // equivalent to channel - uint32_t range; - uint16_t reflectivity; - uint16_t near_ir; // equivalent to ambient - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - - -struct Point_RNG15_RFL8_NIR8 : public _Point_RNG15_RFL8_NIR8 { - - inline Point_RNG15_RFL8_NIR8(const _Point_RNG15_RFL8_NIR8& pt) { - x = pt.x; y = pt.y; z = pt.z; data[3] = 1.0f; - t = pt.t; ring = pt.ring; - range = pt.range; - reflectivity = pt.reflectivity; near_ir = pt.near_ir; - } - - inline Point_RNG15_RFL8_NIR8() - { - x = y = z = 0.0f; data[3] = 1.0f; - t = 0; ring = 0; - range = 0; - reflectivity = 0; near_ir = 0; - } - - inline const auto as_tuple() const { - return std::tie(x, y, z, t, ring, range, reflectivity, near_ir); - } - - inline auto as_tuple() { - return std::tie(x, y, z, t, ring, range, reflectivity, near_ir); - } - - template - inline auto& get() { - return std::get(as_tuple()); - } -}; - -} // namespace ouster_ros - -// clang-format off - -// Default=RNG15_RFL8_NIR8 aka LOW_DATA profile -POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point_RNG15_RFL8_NIR8, - (float, x, x) - (float, y, y) - (float, z, z) - (std::uint32_t, t, t) - (std::uint16_t, ring, ring) - (std::uint32_t, range, range) - (std::uint16_t, reflectivity, reflectivity) - (std::uint16_t, near_ir, near_ir) -) - -// clang-format on - - -namespace ouster_ros { - -// Profile_FUSA_RNG15_RFL8_NIR8_DUAL: aka fusa dual returns -// This profile is definied differently from PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL of how -// the sensor actually sends the data. The actual PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL -// has 5 fields not 3, but this profile is defined differently in ROS because -// we build and publish a point cloud for each return separately. However, it -// might be desireable to some of the users to choose a point cloud -// representation which combines parts of the the two or more returns. This isn't -// something that the current framework could deal with as of now. -static constexpr ChanFieldTable<3> Profile_FUSA_RNG15_RFL8_NIR8_DUAL {{ - {sensor::ChanField::RANGE, sensor::ChanFieldType::UINT32}, - {sensor::ChanField::REFLECTIVITY, sensor::ChanFieldType::UINT8}, - {sensor::ChanField::NEAR_IR, sensor::ChanFieldType::UINT16}, -}}; - -// Note: this is one way to implement the processing of 2nd return -// This should be an exact copy of Profile_FUSA_RNG15_RFL8_NIR8_DUAL with the -// exception of ChanField values for the first three fields. NEAR_IR is same for both -static constexpr ChanFieldTable<3> Profile_FUSA_RNG15_RFL8_NIR8_DUAL_2ND_RETURN {{ - {sensor::ChanField::RANGE2, sensor::ChanFieldType::UINT32}, - {sensor::ChanField::REFLECTIVITY2, sensor::ChanFieldType::UINT8}, - {sensor::ChanField::NEAR_IR, sensor::ChanFieldType::UINT16}, -}}; - -// auto=RNG19_RFL8_SIG16_NIR16_DUAL -struct EIGEN_ALIGN16 _Point_FUSA_RNG15_RFL8_NIR8_DUAL { - PCL_ADD_POINT4D; - uint32_t t; // timestamp in nanoseconds relative to frame start - uint16_t ring; // equivalent to channel - uint32_t range; - uint8_t reflectivity; - uint16_t near_ir; // equivalent to ambient - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -struct Point_FUSA_RNG15_RFL8_NIR8_DUAL : public _Point_FUSA_RNG15_RFL8_NIR8_DUAL { - - inline Point_FUSA_RNG15_RFL8_NIR8_DUAL(const _Point_FUSA_RNG15_RFL8_NIR8_DUAL& pt) - { - x = pt.x; y = pt.y; z = pt.z; data[3] = 1.0f; - t = pt.t; ring = pt.ring; - range = pt.range; - reflectivity = pt.reflectivity; - near_ir = pt.near_ir; - } - - inline Point_FUSA_RNG15_RFL8_NIR8_DUAL() - { - x = y = z = 0.0f; data[3] = 1.0f; - t = 0; ring = 0; - range = 0; - reflectivity = 0; - near_ir = 0; - } - - inline const auto as_tuple() const { - return std::tie(x, y, z, t, ring, range, reflectivity, near_ir); - } - - inline auto as_tuple() { - return std::tie(x, y, z, t, ring, range, reflectivity, near_ir); - } - - template - inline auto& get() { - return std::get(as_tuple()); - } -}; - -} // namespce ouster_ros - -// clang-format off - -POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point_FUSA_RNG15_RFL8_NIR8_DUAL, - (float, x, x) - (float, y, y) - (float, z, z) - (std::uint32_t, t, t) - (std::uint16_t, ring, ring) - (std::uint32_t, range, range) - (std::uint8_t, reflectivity, reflectivity) - (std::uint16_t, near_ir, near_ir) -) - -// clang-format on \ No newline at end of file diff --git a/src/third-party/ouster-ros/ouster-ros/include/ouster_ros/visibility_control.h b/src/third-party/ouster-ros/ouster-ros/include/ouster_ros/visibility_control.h deleted file mode 100644 index d8afc38d..00000000 --- a/src/third-party/ouster-ros/ouster-ros/include/ouster_ros/visibility_control.h +++ /dev/null @@ -1,44 +0,0 @@ -#ifndef OUSTER_ROS_VISIBILITY_CONTROL_H_ -#define OUSTER_ROS_VISIBILITY_CONTROL_H_ - -#ifdef __cplusplus -extern "C" -{ -#endif - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ - #ifdef __GNUC__ - #define OUSTER_ROS_EXPORT __attribute__ ((dllexport)) - #define OUSTER_ROS_IMPORT __attribute__ ((dllimport)) - #else - #define OUSTER_ROS_EXPORT __declspec(dllexport) - #define OUSTER_ROS_IMPORT __declspec(dllimport) - #endif - #ifdef OUSTER_ROS_BUILDING_DLL - #define OUSTER_ROS_PUBLIC OUSTER_ROS_EXPORT - #else - #define OUSTER_ROS_PUBLIC OUSTER_ROS_IMPORT - #endif - #define OUSTER_ROS_PUBLIC_TYPE OUSTER_ROS_PUBLIC - #define OUSTER_ROS_LOCAL -#else - #define OUSTER_ROS_EXPORT __attribute__ ((visibility("default"))) - #define OUSTER_ROS_IMPORT - #if __GNUC__ >= 4 - #define OUSTER_ROS_PUBLIC __attribute__ ((visibility("default"))) - #define OUSTER_ROS_LOCAL __attribute__ ((visibility("hidden"))) - #else - #define OUSTER_ROS_PUBLIC - #define OUSTER_ROS_LOCAL - #endif - #define OUSTER_ROS_PUBLIC_TYPE -#endif - -#ifdef __cplusplus -} -#endif - -#endif // OUSTER_ROS_VISIBILITY_CONTROL_H_ diff --git a/src/third-party/ouster-ros/ouster-ros/launch/driver.launch.py b/src/third-party/ouster-ros/ouster-ros/launch/driver.launch.py deleted file mode 100644 index 9da3b1b4..00000000 --- a/src/third-party/ouster-ros/ouster-ros/launch/driver.launch.py +++ /dev/null @@ -1,105 +0,0 @@ -# Copyright 2023 Ouster, Inc. -# - -"""Launch a sensor node along with os_cloud and os_""" - -from pathlib import Path -import launch -import lifecycle_msgs.msg -from ament_index_python.packages import get_package_share_directory -from launch_ros.actions import LifecycleNode -from launch.actions import (DeclareLaunchArgument, IncludeLaunchDescription, - RegisterEventHandler, EmitEvent, LogInfo) -from launch.conditions import IfCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration -from launch.events import matches_action -from launch_ros.events.lifecycle import ChangeState -from launch_ros.event_handlers import OnStateTransition - - -def generate_launch_description(): - """ - Generate launch description for running ouster_ros components separately each - component will run in a separate process). - """ - ouster_ros_pkg_dir = get_package_share_directory('ouster_ros') - default_params_file = \ - Path(ouster_ros_pkg_dir) / 'config' / 'driver_params.yaml' - params_file = LaunchConfiguration('params_file') - params_file_arg = DeclareLaunchArgument('params_file', - default_value=str( - default_params_file), - description='name or path to the parameters file to use.') - - ouster_ns = LaunchConfiguration('ouster_ns') - ouster_ns_arg = DeclareLaunchArgument( - 'ouster_ns', default_value='ouster') - - rviz_enable = LaunchConfiguration('viz') - rviz_enable_arg = DeclareLaunchArgument('viz', default_value='True') - - os_driver_name = LaunchConfiguration('os_driver_name') - os_driver_name_arg = DeclareLaunchArgument( - 'os_driver_name', default_value='os_driver') - - os_driver = LifecycleNode( - package='ouster_ros', - executable='os_driver', - name=os_driver_name, - namespace=ouster_ns, - parameters=[params_file], - output='screen', - ) - - sensor_configure_event = EmitEvent( - event=ChangeState( - lifecycle_node_matcher=matches_action(os_driver), - transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE, - ) - ) - - sensor_activate_event = RegisterEventHandler( - OnStateTransition( - target_lifecycle_node=os_driver, goal_state='inactive', - entities=[ - LogInfo(msg="os_driver activating..."), - EmitEvent(event=ChangeState( - lifecycle_node_matcher=matches_action(os_driver), - transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE, - )), - ], - handle_once=True - ) - ) - - sensor_finalized_event = RegisterEventHandler( - OnStateTransition( - target_lifecycle_node=os_driver, goal_state='finalized', - entities=[ - LogInfo( - msg="Failed to communicate with the sensor in a timely manner."), - EmitEvent(event=launch.events.Shutdown( - reason="Couldn't communicate with sensor")) - ], - ) - ) - - rviz_launch_file_path = \ - Path(ouster_ros_pkg_dir) / 'launch' / 'rviz.launch.py' - rviz_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource([str(rviz_launch_file_path)]), - condition=IfCondition(rviz_enable) - ) - - return launch.LaunchDescription([ - params_file_arg, - ouster_ns_arg, - rviz_enable_arg, - os_driver_name_arg, - rviz_launch, - os_driver, - sensor_configure_event, - sensor_activate_event, - sensor_finalized_event - ]) diff --git a/src/third-party/ouster-ros/ouster-ros/launch/driver_launch.py b/src/third-party/ouster-ros/ouster-ros/launch/driver_launch.py deleted file mode 100644 index 6072ba82..00000000 --- a/src/third-party/ouster-ros/ouster-ros/launch/driver_launch.py +++ /dev/null @@ -1,52 +0,0 @@ -# Copyright 2023 Ouster, Inc. -# - -"""Launch a sensor node along with os_cloud and os_""" - -from pathlib import Path -import launch -import lifecycle_msgs.msg -from ament_index_python.packages import get_package_share_directory -from launch_ros.actions import Node, LifecycleNode -from launch.actions import (DeclareLaunchArgument, IncludeLaunchDescription, - RegisterEventHandler, EmitEvent, LogInfo) -from launch.conditions import IfCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration -from launch.events import matches_action -from launch_ros.events.lifecycle import ChangeState -from launch_ros.event_handlers import OnStateTransition - - -def generate_launch_description(): - """ - Generate launch description for running ouster_ros components separately each - component will run in a separate process). - """ - ouster_ros_pkg_dir = get_package_share_directory('ouster_ros') - # use the community_driver_config.yaml by default - default_params_file = \ - Path(ouster_ros_pkg_dir) / 'config' / 'community_driver_config.yaml' - params_file = LaunchConfiguration('params_file') - params_file_arg = DeclareLaunchArgument('params_file', - default_value=str( - default_params_file), - description='name or path to the parameters file to use.') - - driver_launch_file_path = \ - Path(ouster_ros_pkg_dir) / 'launch' / 'driver.launch.py' - driver_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource([str(driver_launch_file_path)]), - launch_arguments={ - 'params_file': params_file, - 'ouster_ns': '', - 'os_driver_name': 'ouster_driver', - 'viz': 'True', - 'rviz_config': './install/ouster_ros/share/ouster_ros/config/community_driver.rviz' - }.items() - ) - - return launch.LaunchDescription([ - params_file_arg, - driver_launch - ]) diff --git a/src/third-party/ouster-ros/ouster-ros/launch/record.composite.launch.xml b/src/third-party/ouster-ros/ouster-ros/launch/record.composite.launch.xml deleted file mode 100644 index e0ca5ded..00000000 --- a/src/third-party/ouster-ros/ouster-ros/launch/record.composite.launch.xml +++ /dev/null @@ -1,201 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/third-party/ouster-ros/ouster-ros/launch/record.launch.xml b/src/third-party/ouster-ros/ouster-ros/launch/record.launch.xml deleted file mode 120000 index 7454aa44..00000000 --- a/src/third-party/ouster-ros/ouster-ros/launch/record.launch.xml +++ /dev/null @@ -1 +0,0 @@ -record.composite.launch.xml \ No newline at end of file diff --git a/src/third-party/ouster-ros/ouster-ros/launch/replay.composite.launch.xml b/src/third-party/ouster-ros/ouster-ros/launch/replay.composite.launch.xml deleted file mode 100644 index 81014828..00000000 --- a/src/third-party/ouster-ros/ouster-ros/launch/replay.composite.launch.xml +++ /dev/null @@ -1,143 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/third-party/ouster-ros/ouster-ros/launch/replay.launch.xml b/src/third-party/ouster-ros/ouster-ros/launch/replay.launch.xml deleted file mode 120000 index bb2a354b..00000000 --- a/src/third-party/ouster-ros/ouster-ros/launch/replay.launch.xml +++ /dev/null @@ -1 +0,0 @@ -replay.composite.launch.xml \ No newline at end of file diff --git a/src/third-party/ouster-ros/ouster-ros/launch/replay_pcap.launch.xml b/src/third-party/ouster-ros/ouster-ros/launch/replay_pcap.launch.xml deleted file mode 100644 index 7a965287..00000000 --- a/src/third-party/ouster-ros/ouster-ros/launch/replay_pcap.launch.xml +++ /dev/null @@ -1,129 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/third-party/ouster-ros/ouster-ros/launch/rviz.launch.py b/src/third-party/ouster-ros/ouster-ros/launch/rviz.launch.py deleted file mode 100644 index bf8ca655..00000000 --- a/src/third-party/ouster-ros/ouster-ros/launch/rviz.launch.py +++ /dev/null @@ -1,39 +0,0 @@ -# Copyright 2023 Ouster, Inc. -# - -from pathlib import Path -from launch import LaunchDescription -from launch_ros.actions import Node -from launch.conditions import IfCondition -from ament_index_python.packages import get_package_share_directory -from launch.substitutions import LaunchConfiguration -from launch.actions import DeclareLaunchArgument - - -def generate_launch_description(): - """Generate launch description for rviz""" - ouster_ros_pkg_dir = get_package_share_directory('ouster_ros') - print(ouster_ros_pkg_dir) - default_rviz_config = Path(ouster_ros_pkg_dir) / 'config' / 'viz.rviz' - print(default_rviz_config) - rviz_config = LaunchConfiguration('rviz_config') - rviz_config_arg = DeclareLaunchArgument( - 'rviz_config', default_value=str(default_rviz_config)) - - ouster_ns = LaunchConfiguration('ouster_ns') - ouster_ns_arg = DeclareLaunchArgument( - 'ouster_ns', default_value='ouster') - - rviz_node = Node( - package='rviz2', - namespace=ouster_ns, - executable='rviz2', - name='rviz2', - arguments=['-d', rviz_config] - ) - - return LaunchDescription([ - ouster_ns_arg, - rviz_config_arg, - rviz_node - ]) diff --git a/src/third-party/ouster-ros/ouster-ros/launch/rviz.launch.xml b/src/third-party/ouster-ros/ouster-ros/launch/rviz.launch.xml deleted file mode 100644 index 0c9beb17..00000000 --- a/src/third-party/ouster-ros/ouster-ros/launch/rviz.launch.xml +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - \ No newline at end of file diff --git a/src/third-party/ouster-ros/ouster-ros/launch/sensor.composite.launch.py b/src/third-party/ouster-ros/ouster-ros/launch/sensor.composite.launch.py deleted file mode 100644 index 7be2e150..00000000 --- a/src/third-party/ouster-ros/ouster-ros/launch/sensor.composite.launch.py +++ /dev/null @@ -1,94 +0,0 @@ -# Copyright 2023 Ouster, Inc. -# - -"""Launch ouster nodes using a composite container""" - -from pathlib import Path -import launch -from ament_index_python.packages import get_package_share_directory -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode -from launch.actions import (DeclareLaunchArgument, IncludeLaunchDescription, - ExecuteProcess, TimerAction) -from launch.conditions import IfCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, FindExecutable - - -def generate_launch_description(): - """ - Generate launch description for running ouster_ros components in a single - process/container. - """ - ouster_ros_pkg_dir = get_package_share_directory('ouster_ros') - default_params_file = \ - Path(ouster_ros_pkg_dir) / 'config' / 'os_sensor_cloud_image_params.yaml' - params_file = LaunchConfiguration('params_file') - params_file_arg = DeclareLaunchArgument('params_file', - default_value=str( - default_params_file), - description='name or path to the parameters file to use.') - - ouster_ns = LaunchConfiguration('ouster_ns') - ouster_ns_arg = DeclareLaunchArgument( - 'ouster_ns', default_value='ouster') - - rviz_enable = LaunchConfiguration('viz') - rviz_enable_arg = DeclareLaunchArgument('viz', default_value='True') - - auto_start = LaunchConfiguration('auto_start') - auto_start_arg = DeclareLaunchArgument('auto_start', default_value='True') - - os_sensor = ComposableNode( - package='ouster_ros', - plugin='ouster_ros::OusterSensor', - name='os_sensor', - namespace=ouster_ns, - parameters=[params_file, - {'auto_start': auto_start}] - ) - - os_cloud = ComposableNode( - package='ouster_ros', - plugin='ouster_ros::OusterCloud', - name='os_cloud', - namespace=ouster_ns, - parameters=[params_file] - ) - - os_image = ComposableNode( - package='ouster_ros', - plugin='ouster_ros::OusterImage', - name='os_image', - namespace=ouster_ns, - parameters=[params_file] - ) - - os_container = ComposableNodeContainer( - name='os_container', - namespace=ouster_ns, - package='rclcpp_components', - executable='component_container_mt', - composable_node_descriptions=[ - os_sensor, - os_cloud, - os_image - ], - output='screen', - ) - - rviz_launch_file_path = \ - Path(ouster_ros_pkg_dir) / 'launch' / 'rviz.launch.py' - rviz_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource([str(rviz_launch_file_path)]), - condition=IfCondition(rviz_enable) - ) - - return launch.LaunchDescription([ - params_file_arg, - ouster_ns_arg, - rviz_enable_arg, - auto_start_arg, - rviz_launch, - os_container - ]) diff --git a/src/third-party/ouster-ros/ouster-ros/launch/sensor.composite.launch.xml b/src/third-party/ouster-ros/ouster-ros/launch/sensor.composite.launch.xml deleted file mode 100644 index 7bbfc2b9..00000000 --- a/src/third-party/ouster-ros/ouster-ros/launch/sensor.composite.launch.xml +++ /dev/null @@ -1,168 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/third-party/ouster-ros/ouster-ros/launch/sensor.independent.launch.py b/src/third-party/ouster-ros/ouster-ros/launch/sensor.independent.launch.py deleted file mode 100644 index 9618ee14..00000000 --- a/src/third-party/ouster-ros/ouster-ros/launch/sensor.independent.launch.py +++ /dev/null @@ -1,121 +0,0 @@ -# Copyright 2023 Ouster, Inc. -# - -"""Launch a sensor node along with os_cloud and os_""" - -from pathlib import Path -import launch -import lifecycle_msgs.msg -from ament_index_python.packages import get_package_share_directory -from launch_ros.actions import Node, LifecycleNode -from launch.actions import (DeclareLaunchArgument, IncludeLaunchDescription, - RegisterEventHandler, EmitEvent, LogInfo) -from launch.conditions import IfCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration -from launch.events import matches_action -from launch_ros.events.lifecycle import ChangeState -from launch_ros.event_handlers import OnStateTransition - - -def generate_launch_description(): - """ - Generate launch description for running ouster_ros components separately each - component will run in a separate process). - """ - ouster_ros_pkg_dir = get_package_share_directory('ouster_ros') - default_params_file = \ - Path(ouster_ros_pkg_dir) / 'config' / \ - 'os_sensor_cloud_image_params.yaml' - params_file = LaunchConfiguration('params_file') - params_file_arg = DeclareLaunchArgument('params_file', - default_value=str( - default_params_file), - description='name or path to the parameters file to use.') - - ouster_ns = LaunchConfiguration('ouster_ns') - ouster_ns_arg = DeclareLaunchArgument( - 'ouster_ns', default_value='ouster') - - rviz_enable = LaunchConfiguration('viz') - rviz_enable_arg = DeclareLaunchArgument('viz', default_value='True') - - os_sensor = LifecycleNode( - package='ouster_ros', - executable='os_sensor', - name='os_sensor', - namespace=ouster_ns, - parameters=[params_file], - output='screen', - ) - - sensor_configure_event = EmitEvent( - event=ChangeState( - lifecycle_node_matcher=matches_action(os_sensor), - transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE, - ) - ) - - sensor_activate_event = RegisterEventHandler( - OnStateTransition( - target_lifecycle_node=os_sensor, goal_state='inactive', - entities=[ - LogInfo(msg="os_sensor activating..."), - EmitEvent(event=ChangeState( - lifecycle_node_matcher=matches_action(os_sensor), - transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE, - )), - ], - handle_once=True - ) - ) - - sensor_finalized_event = RegisterEventHandler( - OnStateTransition( - target_lifecycle_node=os_sensor, goal_state='finalized', - entities=[ - LogInfo( - msg="Failed to communicate with the sensor in a timely manner."), - EmitEvent(event=launch.events.Shutdown( - reason="Couldn't communicate with sensor")) - ], - ) - ) - - os_cloud = Node( - package='ouster_ros', - executable='os_cloud', - name='os_cloud', - namespace=ouster_ns, - parameters=[params_file], - output='screen', - ) - - os_image = Node( - package='ouster_ros', - executable='os_image', - name='os_image', - namespace=ouster_ns, - parameters=[params_file], - output='screen', - ) - - rviz_launch_file_path = \ - Path(ouster_ros_pkg_dir) / 'launch' / 'rviz.launch.py' - rviz_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource([str(rviz_launch_file_path)]), - condition=IfCondition(rviz_enable) - ) - - return launch.LaunchDescription([ - params_file_arg, - ouster_ns_arg, - rviz_enable_arg, - rviz_launch, - os_sensor, - os_cloud, - os_image, - sensor_configure_event, - sensor_activate_event, - sensor_finalized_event - ]) diff --git a/src/third-party/ouster-ros/ouster-ros/launch/sensor.independent.launch.xml b/src/third-party/ouster-ros/ouster-ros/launch/sensor.independent.launch.xml deleted file mode 100644 index 63277bdd..00000000 --- a/src/third-party/ouster-ros/ouster-ros/launch/sensor.independent.launch.xml +++ /dev/null @@ -1,153 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/third-party/ouster-ros/ouster-ros/launch/sensor.launch.xml b/src/third-party/ouster-ros/ouster-ros/launch/sensor.launch.xml deleted file mode 120000 index 5ff3040f..00000000 --- a/src/third-party/ouster-ros/ouster-ros/launch/sensor.launch.xml +++ /dev/null @@ -1 +0,0 @@ -sensor.composite.launch.xml \ No newline at end of file diff --git a/src/third-party/ouster-ros/ouster-ros/launch/sensor_mtp.launch.xml b/src/third-party/ouster-ros/ouster-ros/launch/sensor_mtp.launch.xml deleted file mode 100644 index d61da022..00000000 --- a/src/third-party/ouster-ros/ouster-ros/launch/sensor_mtp.launch.xml +++ /dev/null @@ -1,149 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/.clang-format b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/.clang-format deleted file mode 100644 index 7b42f0da..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/.clang-format +++ /dev/null @@ -1,168 +0,0 @@ -# generated with: clang-format -style=Google -dump-config > .clang-format ---- -Language: Cpp -# BasedOnStyle: Google -AccessModifierOffset: -1 -AlignAfterOpenBracket: Align -AlignConsecutiveMacros: false -AlignConsecutiveAssignments: false -AlignConsecutiveDeclarations: false -AlignEscapedNewlines: Left -AlignOperands: true -AlignTrailingComments: true -AllowAllArgumentsOnNextLine: true -AllowAllConstructorInitializersOnNextLine: true -AllowAllParametersOfDeclarationOnNextLine: true -AllowShortBlocksOnASingleLine: Never -AllowShortCaseLabelsOnASingleLine: false -AllowShortFunctionsOnASingleLine: All -AllowShortLambdasOnASingleLine: All -AllowShortIfStatementsOnASingleLine: WithoutElse -AllowShortLoopsOnASingleLine: true -AlwaysBreakAfterDefinitionReturnType: None -AlwaysBreakAfterReturnType: None -AlwaysBreakBeforeMultilineStrings: true -AlwaysBreakTemplateDeclarations: Yes -BinPackArguments: true -BinPackParameters: true -BraceWrapping: - AfterCaseLabel: false - AfterClass: false - AfterControlStatement: false - AfterEnum: false - AfterFunction: false - AfterNamespace: false - AfterObjCDeclaration: false - AfterStruct: false - AfterUnion: false - AfterExternBlock: false - BeforeCatch: false - BeforeElse: false - IndentBraces: false - SplitEmptyFunction: true - SplitEmptyRecord: true - SplitEmptyNamespace: true -BreakBeforeBinaryOperators: None -BreakBeforeBraces: Attach -BreakBeforeInheritanceComma: false -BreakInheritanceList: BeforeColon -BreakBeforeTernaryOperators: true -BreakConstructorInitializersBeforeComma: false -BreakConstructorInitializers: BeforeColon -BreakAfterJavaFieldAnnotations: false -BreakStringLiterals: true -ColumnLimit: 80 -CommentPragmas: '^ IWYU pragma:' -CompactNamespaces: false -ConstructorInitializerAllOnOneLineOrOnePerLine: true -ConstructorInitializerIndentWidth: 4 -ContinuationIndentWidth: 4 -Cpp11BracedListStyle: true -DeriveLineEnding: true -DerivePointerAlignment: false -DisableFormat: false -ExperimentalAutoDetectBinPacking: false -FixNamespaceComments: true -ForEachMacros: - - foreach - - Q_FOREACH - - BOOST_FOREACH -IncludeBlocks: Regroup -IncludeCategories: - - Regex: '^' - Priority: 2 - SortPriority: 0 - - Regex: '^<.*\.h>' - Priority: 1 - SortPriority: 0 - - Regex: '^<.*' - Priority: 2 - SortPriority: 0 - - Regex: '.*' - Priority: 3 - SortPriority: 0 -IncludeIsMainRegex: '([-_](test|unittest))?$' -IncludeIsMainSourceRegex: '' -IndentCaseLabels: true -IndentGotoLabels: true -IndentPPDirectives: None -IndentWidth: 4 -IndentWrappedFunctionNames: false -JavaScriptQuotes: Leave -JavaScriptWrapImports: true -KeepEmptyLinesAtTheStartOfBlocks: false -MacroBlockBegin: '' -MacroBlockEnd: '' -MaxEmptyLinesToKeep: 1 -NamespaceIndentation: None -ObjCBinPackProtocolList: Never -ObjCBlockIndentWidth: 2 -ObjCSpaceAfterProperty: false -ObjCSpaceBeforeProtocolList: true -PenaltyBreakAssignment: 2 -PenaltyBreakBeforeFirstCallParameter: 1 -PenaltyBreakComment: 300 -PenaltyBreakFirstLessLess: 120 -PenaltyBreakString: 1000 -PenaltyBreakTemplateDeclaration: 10 -PenaltyExcessCharacter: 1000000 -PenaltyReturnTypeOnItsOwnLine: 200 -PointerAlignment: Left -RawStringFormats: - - Language: Cpp - Delimiters: - - cc - - CC - - cpp - - Cpp - - CPP - - 'c++' - - 'C++' - CanonicalDelimiter: '' - BasedOnStyle: google - - Language: TextProto - Delimiters: - - pb - - PB - - proto - - PROTO - EnclosingFunctions: - - EqualsProto - - EquivToProto - - PARSE_PARTIAL_TEXT_PROTO - - PARSE_TEST_PROTO - - PARSE_TEXT_PROTO - - ParseTextOrDie - - ParseTextProtoOrDie - CanonicalDelimiter: '' - BasedOnStyle: google -ReflowComments: true -SortIncludes: true -SortUsingDeclarations: true -SpaceAfterCStyleCast: false -SpaceAfterLogicalNot: false -SpaceAfterTemplateKeyword: true -SpaceBeforeAssignmentOperators: true -SpaceBeforeCpp11BracedList: false -SpaceBeforeCtorInitializerColon: true -SpaceBeforeInheritanceColon: true -SpaceBeforeParens: ControlStatements -SpaceBeforeRangeBasedForLoopColon: true -SpaceInEmptyBlock: false -SpaceInEmptyParentheses: false -SpacesBeforeTrailingComments: 2 -SpacesInAngles: false -SpacesInConditionalStatement: false -SpacesInContainerLiterals: true -SpacesInCStyleCastParentheses: false -SpacesInParentheses: false -SpacesInSquareBrackets: false -SpaceBeforeSquareBrackets: false -Standard: Auto -StatementMacros: - - Q_UNUSED - - QT_REQUIRE_VERSION -TabWidth: 8 -UseCRLF: false -UseTab: Never -... diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/.clangd b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/.clangd deleted file mode 100644 index ef516ada..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/.clangd +++ /dev/null @@ -1,2 +0,0 @@ -CompileFlags: - Add: -ferror-limit=0 diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/.dockerignore b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/.dockerignore deleted file mode 100644 index 29b9ef04..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/.dockerignore +++ /dev/null @@ -1,2 +0,0 @@ -python/Dockerfile -ouster_ros/Dockerfile diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/.github/ISSUE_TEMPLATE/bug_report.md b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/.github/ISSUE_TEMPLATE/bug_report.md deleted file mode 100644 index aa86c670..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/.github/ISSUE_TEMPLATE/bug_report.md +++ /dev/null @@ -1,30 +0,0 @@ ---- -name: Bug report -about: Create a report to help us improve -title: '' -labels: bug -assignees: '' - ---- - -**Describe the bug** -A clear and concise description of what the bug is. - -**To Reproduce** -Steps to reproduce the behavior (steps below are just an example): -1. compile the project -2. configure the sensor -3. run the code as ... -4. observe the issue ... - -**Screenshots** -If applicable, add screenshots to help explain your problem. - -**Platform (please complete the following information):** -- Ouster Sensor? \[e.g. OS-0, OS-1, ..\] -- Ouster Firmware Version? \[e.g. 2.3, 2.4, ..\] -- Programming Language? \[e.g. C++, Python, ..\] - - For Python include the targeted Python version -- Operating System? \[e.g. Linux, Windows, MacOS\] -- Machine Architecture? \[e.g. x64, arm\] -- git commit hash (if not the latest). diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/.github/ISSUE_TEMPLATE/feature_request.md b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/.github/ISSUE_TEMPLATE/feature_request.md deleted file mode 100644 index 46718378..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/.github/ISSUE_TEMPLATE/feature_request.md +++ /dev/null @@ -1,25 +0,0 @@ ---- -name: Feature request -about: Suggest an idea for this project -title: '' -labels: enhancement -assignees: '' - ---- - -**Is your feature request related to a problem? Please describe.** -A clear and concise description of what the problem is. Ex. I'm always frustrated when [...] - -**Describe the solution you'd like** -A clear and concise description of what you want to happen. - -**Describe alternatives you've considered** -A clear and concise description of any alternative solutions or features you've considered. - -**Targeted Platform (please complete the following information only if applicable, otherwise dot N/A):** -- Ouster Sensor? \[feature only applicable to specific ouster sensor\] -- Ouster Firmware Version? \[feature only applicable to specific fw version\] -- Programming Language? \[feature applicable to specific programming language\] - - For Python include the targeted Python version [if applicable] -- Operating System? \[feature applicable to specific operating system?\] -- Machine Architecture? \[feature only applicable to specific architecture?\] diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/.github/ISSUE_TEMPLATE/question.md b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/.github/ISSUE_TEMPLATE/question.md deleted file mode 100644 index b441b456..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/.github/ISSUE_TEMPLATE/question.md +++ /dev/null @@ -1,23 +0,0 @@ ---- -name: Question -about: Do you have a specific question -title: '' -labels: question -assignees: '' - ---- - -**Describe your question** -A clear and concise description of your question. - -**Screenshots** -If applicable, add screenshots to help explain your problem. - -**Platform (please complete the following information):** -- Ouster Sensor? \[e.g. OS-0, OS-1, ..\] -- Ouster Firmware Version? \[e.g. 2.3, 2.4, ..\] -- Programming Language? \[e.g. C++, Python, ..\] - - For Python include the targeted Python version -- Operating System? \[e.g. Linux, Windows, MacOS\] -- Machine Architecture? \[e.g. x64, arm\] -- git commit hash (if not the latest). diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/.github/check_title_and_description.py b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/.github/check_title_and_description.py deleted file mode 100644 index 9669f987..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/.github/check_title_and_description.py +++ /dev/null @@ -1,34 +0,0 @@ -import os -import sys -import re - -title_re = r'\A[0-9A-Z][\x20-\x7E]{0,88}([a-zA-Z0-9]|[\)])' -description_re = r'(((\r)?\n[\20-\x7E]{0,100})+)?(\n)?' - -title = os.environ.get("TITLE") -pr_number_string = os.environ.get("PR_NUMBER") -pr_description = "\n" + os.environ.get("PR_BODY") - -# check title length with (PR number) added at end -new_title = title + " (#" + pr_number_string + ")" - -error = False -print(f"Checking PR title {repr(new_title)}...") -if not re.fullmatch(title_re, new_title): - print(f"Error: Please revise the PR title {new_title} " - "to match the regex {title_re} where:\n" - "* the first letter is a capital letter or numeral 0-9\n" - "* title is more than 2 characters long\n" - "* the title is no longer than 90 characters including " - "the addition of (###) for the PR number") - error = True - -print(f"Checking PR description {repr(pr_description)}") -if not re.fullmatch(description_re, pr_description): - print(f"Error: The PR description {repr(pr_description)}, " - "which will be treated as your commit message, does " - "not match the regex {description_re}") - error = True - -if error: - sys.exit(1) diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/.github/pull_request_template.md b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/.github/pull_request_template.md deleted file mode 100644 index 62551453..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/.github/pull_request_template.md +++ /dev/null @@ -1,5 +0,0 @@ -## Related Issues & PRs - -## Summary of Changes - -## Validation diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/.github/workflows/check-commit-message.yml b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/.github/workflows/check-commit-message.yml deleted file mode 100644 index 9ad0fdad..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/.github/workflows/check-commit-message.yml +++ /dev/null @@ -1,19 +0,0 @@ -name: check-merge-commit-message-on-PR - -run-name: Check merge commit messages by ${{ github.actor }} on ${{ github.ref }} -on: [pull_request] - -jobs: - check_title_and_description: - if: github.event_name == 'pull_request' && ! github.event.pull_request.draft - name: check PR title and description - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v3 - - name: check PR title and description - run: python .github/check_title_and_description.py - shell: bash - env: - TITLE: ${{ github.event.pull_request.title }} - PR_NUMBER: ${{ github.event.pull_request.number }} - PR_BODY: ${{ github.event.pull_request.body }} diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/.gitignore b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/.gitignore deleted file mode 100644 index 1d30d7a6..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/.gitignore +++ /dev/null @@ -1,30 +0,0 @@ -__pycache__/ -python/build/ -python/dist/ -python/.tox/ -python/**/*.egg-info/ -python/**/*.so -python/**/*.dll -python/**/*.pyd - -/build -python/artifacts/ - -# visual studio -.vs/ -out/ - -# vim -*.swp - -# CMake, Ninja -.ninja_deps -.ninja_log -CMakeFiles -CTestTestfile.cmake - -# Built artifacts -generated -rules.ninja -*.a -**/_build \ No newline at end of file diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/CHANGELOG.rst b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/CHANGELOG.rst deleted file mode 100644 index f1df3424..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/CHANGELOG.rst +++ /dev/null @@ -1,874 +0,0 @@ -========= -Changelog -========= - -[20240510] [0.11.1] -=================== - -Important notes ---------------- - -* [BREAKING] the ``open_source`` method now returns a ``ScanSource`` by default, not a ``MultiScanSource``. - -Python SDK ----------- - -* Updated the ``open_source`` documentation. -* Fixed an issue that caused the viz to redraw when the mouse cursor is moved. - -ouster_client -------------- - -* Improved the client initialization latency. - -mapping -------- - -* Fixed several issues with the documentation. - - -[20240425] [0.11.0] -=================== - -Important notes ---------------- - -* Dropped support for python3.7 -* Dropped support macOS 10.15 -* This will be the last release that supports Ubuntu 18.04. -* Moved all library level modules under ``ouster.sdk``, this includes ``ouster.client``, ``ouster.pcap`` - ``ouster.osf``. So the new access name will be ``ouster.sdk.client``, ``ouster.sdk.pcap`` and so on -* [BREAKING] many of the ``ouster-cli`` commands and arguments have changed (see below.) -* [BREAKING] moved ``configure_sensor`` method to ``ouster.sdk.sensor.util`` module -* [BREAKING] removed the ``pcap_to_osf`` method. - - -examples --------- - -* Added a new ``async_client_example.cpp`` C++ example. - - -Python SDK ----------- - -* Add support for python 3.12, including wheels on pypi -* Updated VCPKG libraries to 2023.10.19 -* New ``ScanSource`` API: - * Added new ``MultiScanSource`` that supports streaming and manipulating LidarScan frames - from multiple concurrent LidarScan sources - * For non-live sources the ``MultiScanSource`` have the option to choose LidarScan(s) by index - or choose a subset of scans using slicing operation - * The ``MultiScanSource`` interface has the ability to fallback to ``ScanSource`` using the - ``single_source(sensor_idx)``, ``ScanSource`` interface yield a single LidarScan on iteration - rather than a List - * The ``ScanSource`` interface obtained via ``single_source`` method supports same indexing and - and slicing operations as the ``MultiScanSource`` - * Added a generic ``open_source`` that accepts sensor urls, or a path to a pcap recording - or an osf file - * Add explicit flag ``index`` to index unindexed osf files, if flag is set to ``True`` the osf file - will be indexed and the index will be saved to the file on first attempt - * Display a progress bar during index of pcap file or osf (if unindexed) -* Improved the robustness of the ``resolve_metadata`` method used to - automatically identify the sensor metadata associated with a PCAP source. -* [bugfix] SimpleViz complains about missing fields -* [bugfix] Gracefully handle failed sensor connection attempts with proper error reporting -* [bugfix] Fix assertion error when using viz stepping on a live sensor -* [bugfix] Scope MultiLidarViz imports to viz commands -* [bugfix] LidarScan yielded with improper header/status -* [bugfix] OSF ScanSource fields property doesn't report the actual fields -* Removed ``ouster.sdkx``, the ``open_source`` command is now part of ``ouster.sdk`` module -* The ``FLAGS`` field is always added to the list fields of any source type by default. In case of a - dual return lidar profile then a second ``FLAGS2`` will also be added. - - -mapping -------- - -* Updated SLAM API and examples. -* Added real time frame dropping capability to SLAM API. -* The ``ouster-mapping`` package now uses ``point-cloud-utils`` instead of ``open3d``. -* improved per-column pose accuracy, which is now based on the actual column timestamps - - -ouster-cli ----------- - -* Many commands can now be chained together, e.g. ``ouster-cli source slam viz``. -* New ``save`` command can output the result in a variety of formats. -* Added ``--ts`` option for specifying the timestamps to use when saving an OSF - file. Host packet receive time is the default, but not all scan sources have - this info. Lidar packet timestamps can be used as an alternative. -* Changed the output format of ``ouster-cli discover`` to include more information. -* Added JSON format output option to ``ouster-cli discover``. -* Added a flag to output sensor user data to ``ouster-cli discover``. -* Update the minimum required version of ``zeroconf``. -* Removed ``python-magic`` package from required dependencies. -* Made the output of ``ouster-cli source info`` much more - user-friendly. (``ouster-cli source dump`` gives old output.) -* [breaking] changed the argument format of the ``slice`` command. -* [breaking] removed the ``--legacy`` and ``--non-legacy`` flags. -* [breaking] removed the ``ouster-cli mapping``, ``ouster-cli osf``, - ``ouster-cli pcap``, and ``ouster-cli sensor`` commands. -* [bugfix] return a nonzero exit code on error. -* [bugfix] fix an error that occurred when setting the IMU port using the - ``-i`` option. - - -ouster_client -------------- - -* Added a new buffered UDP source implementation ``BufferedUDPSource``. -* The method ``version_of_string`` is marked as deprecated, use ``version_from_string`` - instead. -* Added a new method ``firmware_version_from_metadata`` which works across firmwares. -* Added support for return order configuration parameter. -* Added support for gyro and accelerometer FSR configuration parameters. -* [bugfix] ``mtp_init_client`` throws a bad optional access. -* [bugfix] properly handle 32-bit frame IDs from the - ``FUSA_RNG15_RFL8_NIR8_DUAL`` sensor UDP profile. - - -ouster_osf ----------- - -* [breaking] Greatly simplified OSF writer API with examples. -* [breaking] removed the ``to_native`` and ``from_native`` methods. -* Updated Doxygen API documentation for OSF C++ API. -* Removed support for the deprecated "standard" OSF file format. (The streaming - OSF format is still supported.) -* Added ``osf_file_modify_metadata`` that allows updating the sensor info - associated with each lidar stream in an OSF file. -* Warn the user if reading an empty or improperly indexed file. - - -ouster_viz ----------- -* Added scaled palettes for calibrated reflectivity. -* Distance rings can now be hidden by setting their thickness to zero. -* [bugfix] Fix some rendering issues with the distance rings. -* [bugfix] Fix potential flickering in Viz - - -Known issues ------------- - -* ouster-cli discover may not provide info for sensors using IPv6 link-local - networks on Python 3.8 or with older versions of zeroconf. -* ouster-cli when combining ``slice`` command with ``viz`` the program will - exit once iterate over the selected range of scans even when - the ``--on-eof`` option is set to ``loop``. - - workaround: to have ``viz`` loop over the selected range, first perform a - ``slice`` with ``save``, then playback the generated file. - - -[20231031] [0.10.0] -=================== - -Important notes ---------------- - -* This will be the last release that supports Python 3.7. -* This will be the last release that supports macOS 10.15. - -ouster_viz ----------- - -* Added point cloud accumulation support -* Added an ``PointViz::fps()`` method to return the operating frame rate as a ``double`` - -ouster_client -------------- - -* [BREAKING] Updates to ``sensor_info`` include: - * new fields added: ``build_date``, ``image_rev``, ``prod_pn``, ``status``, ``cal`` (representing the value stored in the ``calibration_status`` metadata JSON key), ``config`` (representing the value of the ``sensor_config`` metadata JSON key) - * the original JSON string is accessible via the ``original_string()`` method - * The ``updated_metadata_string()`` now returns a JSON string reflecting any modifications to ``sensor_info`` - * ``to_string`` is now marked as deprecated -* [BREAKING] The RANGE field defined in `parsing.cpp`, for the low data rate profile, is now 32 bits wide (originally 16 bits.) - * Please note this fixes a SDK bug. The underlying UDP format is unchanged. -* [BREAKING] The NEAR_IR field defined in `parsing.cpp`, for the low data rate profile, is now 16 bits wide (originally 8 bits.) - * Plase note this fixes a SDK bug. The underlying UDP format is unchanged. -* [BREAKING] changed frame_id return size to 32 bits from 16 bits -* An array of per-packet timestamps (called ``packet_timestamp``) is added to ``LidarScan`` -* The client now retries failed requests to an Ouster sensor's HTTP API -* Increased the default timeout for HTTP requests to 40s -* Added FuSA UDP profile to support Ouster FW 3.1+ -* Improved ``ScanBatcher`` performance by roughly 3x (depending on hardware) -* Receive buffer size increased from 256KB to 1MB -* [bugfix] Fixed an issue that caused incorrect Cartesian point computation in the ``viz.Cloud`` Python class -* [bugfix] Fixed an issue that resulted in some ``packet_format`` methods returning an uninitialized value -* [bugfix] Fixed a libpcap-related linking issue -* [bugfix] Fixed an eigen 3.3-related linking issue -* [bugfix] Fixed a zero beam angle calculation issue -* [bugfix] Fixed dropped columns issue with 4096x5 and 2048x10 - -ouster-cli ----------- - -* Added ``source slam`` and ``source slam viz`` commands -* All metadata CLI options are changed to ``-m/--metadata`` -* Added discovery for FW 3.1+ sensors -* Set signal multiplier by default in sensor/SOURCE sensor config -* use ``PYBIND11_MODULE`` instead of deprecated module constructor -* remove deprecated == in pybind for ``.is()`` -* [bugfix] Fix report of fragmentation for ouster-cli pcap/SOURCE pcap info -* [bugfix] Fixed issue regarding windows mDNS in discovery -* [bugfix] Fixed cli pcap recording timestamp issue -* [BREAKING] CSV output ordering switched - -ouster.sdk ----------- - -* ``ouster-mapping`` is now a required dependency -* [BREAKING] change the ``ouster.sdk.viz`` location to the ``ouster.viz`` - package, please update the references if you used ``ouster.sdk.viz`` module -* [bugfix] Fixed Windows pcap support for files larger than 2GB -* [bugfix] Fixed the order of ``LidarScan``'s ``w`` and ``h`` keyword arguments -* [bugfix] Fixed an issue with ``LidarPacket`` when using data recorded with older versions of Ouster Studio - -Known issues ------------- - -* The dependency specifier for ``scipy`` is invalid per PEP-440 -* ``get_config`` always returns true -* Repeated CTRL-C can cause a segmentation fault while visualizing a point cloud - -20230710 -======== - -* Update vcpkg ref of build to 2023-02-24 - -ouster_osf ----------- - -* Add Ouster OSF C++/Python library to save stream of LidarScans with metadata - -ouster_client -------------- - -* Add ``LidarScan.pose`` with poses per column -* Add ``_client.IndexedPcapReader`` and ``_client.PcapIndex`` to enable random - pcap file access by frame number - -* [BREAKING] remove ``ouster::Imu`` object -* [BREAKING] change the return type of ``ouster::packet_format::frame_id`` from ``uint16_t`` to ``uint32_t`` -* [BREAKING] remove methods ``px_range``, ``px_reflectivity``, ``px_signal``, and ``px_ambient`` from ``ouster::packet_format`` -* Add ``get_field_types`` function for ``LidarScan``, from ``sensor_info`` -* bugfix: return metadata regardless of ``sensor_info`` status field -* Make timeout on curl more configurable -* [BREAKING] remove encoder_ticks_per_rev (deprecated) - -ouster_viz ----------- - -* [BREAKING] Changed Python binding for ``Cloud.set_column_poses()`` to accept ``[Wx4x4]`` array - of poses, column-storage order -* bugfix: fix label re-use -* Add ``LidarScan.pose`` handling to ``viz.LidarScanViz``, and new ``T`` keyboard - binding to toggle column poses usage - -ouster_pcap ------------ -* bugfix: Use unordered map to store stream_keys to avoid comparison operators on map - -Python SDK ----------- -* Add Python 3.11 wheels -* Retire simple-viz for ouster-cli utility -* Add default ? key binding to LidarScanViz and consolidate bindings into stored definition -* Remove pcap-to-csv for ouster-cli utility -* Add validator class for LidarPacket - -ouster-cli ----------- -This release also marks the introduction of the ouster-cli utility which includes, among many features: -* Visualization from a connected sensor with automatic configuration -* Recording from a connected sensor -* Simultaneous record and viz from a connected sensor -* Obtaining metadata from a connected sensor -* Visualization from a specified PCAP -* Slice, info, and conversion for a specificed PCAP -* Utilities for benchmarking system, printing system-info -* Discovery which indicates all connected sensors on network -* Automatic logging to .ouster-cli -* Mapping utilities - - -[20230403] -========== - -* Default metadata output across all functionality has been switched to the non-legacy format - -ouster_client -------------- -* Added a new method ``mtp_init_client`` to init the client with multicast support (experimental). -* the class ``SensorHttp`` which provides easy access to REST APIs of the sensor has been made public - under the ``ouster::sensor::util`` namespace. -* breaking change: get_metadata defaults to outputting non-legacy metadata -* add debug five_word profile which will be removed later -* breaking change: remove deprecations on LidarScan - -ouster_viz ----------- -* update viz camera with other objects in draw - -ouster_pcap ------------ -* add seek method to PcapReader -* add port guessing logic - -python ------- -* introduce utility to convert nonlegacy metadata to legacy -* use resolve_metadata to find unspecified metadata for simple-viz -* remove port guessing logic in favor of using new C++ ouster_pcap port guessing functionality -* add soft-id-check to skip the init_id/sn check for lidar_packets with metadata - -Numerous changes to SimpleViz and LidarScanViz including: -* expose visible in viz to Python -* introduce ImageMode and CloudMode -* bugfix: remove spurious sqrt application to autoleveled images - - -[20230114] -========== - -ouster_client --------------- -* breaking change: signal multiplier type changed to double to support new FW values of signal - multiplier. -* breaking change: make_xyz_lut takes mat4d beam_to_lidar_transform instead of - lidar_origin_to_beam_origin_mm double to accomodate new FWs. Old reference Python implementation - was kept, but new reference was also added. -* address an issue that could cause the processed frame being dropped in favor or the previous - frame when the frame_id wraps-around. -* added a new flag ``CONFIG_FORCE_REINIT`` for ``set_config()`` method, to force the sensor to reinit - even when config params have not changed. -* breaking change: drop defaults parameters from the shortform ``init_client()`` method. -* added a new method ``init_logger()`` to provide control over the logs emitted by ``ouster_client``. -* add parsing for new FW 3.0 thermal features shot_limiting and thermal_shutdown statuses and countdowns -* add frame_status to LidarScan -* introduce a new method ``cartesianT()`` which speeds up the computation of point projecion from range - image, the method also can process the cartesian product with single float precision. A new unit test - ``cartesian_test`` which shows achieved speed up gains by the number of valid returns in lidar scan. -* add ``RAW_HEADERS`` ChanField to LidarScan for packing headers and footer (alpha version, may be - changed/removed without notice in the future) - -python ------- -* breaking change: drop defaults parameters of ``client.Sensor`` constructor. -* breaking change: change Scans interface Timeout to default to 1 second instead of None -* added a new method ``init_logger()`` to provide control over the logs emitted by ``client.Sensor``. -* add ``client.LidarScan`` methods ``__repr__()`` and ``__str__()``. -* changed default timeout from 1 seconds to 2 seconds - -ouster_viz ----------- -* add ``SimpleViz.screenshot()`` function and a key handler ``SHIFT-Z`` to - save a screenshot. Can be called from a client thread, and executes - asyncronously (i.e. returns immediately and pushes a one off callback - to frame buffer handlers) -* add ``PointViz.viewport_width()`` and ``PointViz.viewport_height()`` functions -* add ``PointViz.push/pop_frame_buffer_handler()`` to attach a callbacks on - every frame draw update that calls from the main rendering loop. -* add ``SHIFT-X`` key to SimpleViz to start continuous saving of screenshots - on every draw operation. (good for making videos) -* expose ``Camera.set_target`` function through pybind - -ouster-sdk ----------- -* Moved ouster_ros to its own repo -* pin ``openssl`` Conan package dependency to ``openssl/1.1.1s`` due to - ``libtins`` and ``libcurl`` conflicting ``openssl`` versions - - -[20220927] -========== - -ouster_client --------------- -* fix a bug in longform ``init_client()`` which was not setting timestamp_mode and lidar_mode correctly - - -[20220826] -========== - -* drop support for buliding C++ libraries and Python bindings on Ubuntu 16.04 -* drop support for buliding C++ libraries and Python bindings on Mac 10.13, Mac 10.14 -* Python 3.6 wheels are no longer built and published -* drop support for sensors running FW < 2.0 -* require C++ 14 to build - -ouster_client --------------- -* add ```CUSTOM0-9`` ChanFields to LidarScan object -* fix parsing measurement status from packets: previously, with some UDP profiles, higher order bits - could be randomly set -* add option for EIGEN_MAX_ALIGN_BYTES, ON by default -* use of sensor http interface for comms with sensors for FW 2.1+ -* propogate C++ 17 usage requirement in cmake for C++ libraries built as C++17 -* allow vcpkg configuration via environment variables -* fix a bug in sensor_config struct equality comparison operator - -ouster_viz ----------- -* clean up GL context logic to avoid errors on window/intel UHD graphics - -python ------- -* windows extension modules are now statically linked to avoid potential issues with vendored dlls - -ouster_ros ----------- -* drop ROS kinetic support -* switch from nodes to nodelets -* update topic names, group under single ros namespace -* separate launch files for play, replay, and recording -* drop FW 1.13 compatibility for sensors and recorded bags -* remove setting of EIGEN_MAX_ALIGN_BYTES -* add two new ros services /ouster/get_config and /ouster/set_config (experimental) -* add new timestamp_mode TIME_FROM_ROS_TIME -* declare PCL_NO_PRECOMPILE ahead of all PCL library includes - - -[20220608] -========== - -ouster_client -------------- -* change single return parsing for FW 2.3.1 - -python ------- -* single return parsing for FW 2.3.1 reflects change from ouster_client - - -[20220504] -========== - -* update supported vcpkg tag to 2022.02.23 -* update to manylinux2014 for x64 linux ``ouster-sdk`` wheels -* Ouster SDK documentation overhaul with C++/Python APIs in one place -* sample data updated to firmware 2.3 - -ouster_client -------------- -* fix the behavior of ``BeamUniformityCorrector`` on azimuth-windowed data by ignoring zeroed out - columns -* add overloads in ``image_processing.h`` to work with single-precision floats -* add support for new ``RNG19_RFL8_SIG16_NIR16`` single-return and ``RNG15_RFL8_NIR8`` low-bandwidth - lidar UDP profiles introduced in firmware 2.3 - -ouster_viz ----------- -* switch to glad for OpenGL loading. GLEW is still supported for developer builds -* breaking change: significant API update of the ``PointViz`` library. See documentation for details -* the ``simple_viz`` example app and ``LidarScanViz`` utility have been removed. Equivalent - functionality is now provided via Python -* add basic support for drawing 2d and 3d text labels -* update to OpenGL 3.3 - -python ------- -* fix a bug where incorrectly sized packets read from the network could cause the client thread to - silently exit, resulting in a timeout -* fix ``client.Scans`` not raising a timeout when using the ``complete`` flag and receiving only - incomplete scans. This could cause readings scans to hang in rare situations -* added bindings for the new ``PointViz`` API and a new module for higher-level visualizer utilities - in ``ouster.sdk.viz``. See API documentation for details -* the ``ouster-sdk`` package now includes an example visualizer, ``simple-viz``, which will be - installed on that path for the Python environment - -ouster_ros ------------ -* support new fw 2.3 profiles by checking for inclusion of fields when creating point cloud. Missing - fields are filled with zeroes - -[20220107] -========== - -* add support for arm64 macos and linux. Releases are now built and tested on these platforms -* add support for Python 3.10 -* update supported vcpkg tag to 2021.05.12 -* add preliminary cpack and install support. It should be possible to use a pre-built SDK package - instead of including the SDK in the build tree of your project - -ouster_client -------------- -* update cmake package version to 0.3.0 -* avoid unnecessary DNS lookup when using numeric addresses with ``init_client()`` -* disable collecting metadata when sensor is in STANDBY mode -* breaking change: ``set_config()`` will now produce more informative errors by throwing - ``std::invalid_argument`` with an error message when config parameters fail validation -* use ``SO_REUSEPORT`` for UDP sockets on non-windows platforms -* the set of fields available on ``LidarScan`` is now configurable. See the new ``LidarScan`` - constructors for details -* added ``RANGE2``, ``SIGNAL2`` and ``REFLECTIVITY2`` channel fields to support handling data from - the second return -* ``ScanBatcher`` will now parse and populate only the channel fields configured on the - ``LidarScan`` passed to ``operator()()`` -* add support for new configuration parameters: ``udp_profile_lidar``, ``udp_profile_imu`` and - ``columns_per_packet`` -* add udp ports, the new initialization id field, and udp profiles to the metadata stored in - the ``sensor_info`` struct -* ``sensor_info::name`` is now deprecated and will stop being populated in the future -* add methods to query and iterate over available ``LidarScan`` fields and field types -* breaking change: removed ``LidarScan::block`` and ``LidarScan::data`` members. These can't be - supported for different packet profiles -* the ``LidarScan::Field`` defniition has been moved to ``sensor::ChanField`` and enumerators have - been renamed to match the sensor user manual. The old names are still available, but deprecated -* deprecate accessing encoder values and frame ids from measurement blocks using ``packet_format`` - as these will not be reported by the sensor in some future configurations -* add ``packet_frame_id`` member function to ``packet_format`` -* add ``col_field`` member function to ``packet_format`` for parsing channel field values for an - entire measurement block -* add new accessors for measurement headers to ``LidarScan``, deprecating the existing ``header`` - member function -* represent empty sensor config with an empty object instead of null in json representation of the - ``sensor_config`` datatype -* update cmake package version to 0.2.1 -* add a conservative socket read timeout so ``init_client()`` will fail with an error message when - another client fails to close a TCP connection (addresses #258) -* when passed an empty string for the ``udp_dest_host`` parameter, ``init_client()`` will now - configure the sensor using ``set_udp_dest_auto``. Previously, this would turn off UDP output on - the sensor, so any attempt to read data would time out (PR #255) -* fall back to binding ipv4 UDP sockets when ipv6 is not available (addresses #261) - -ouster_pcap ------------ -* report additional information in the ``packet_info`` struct and remove separate ``stream_info`` - API -* switch the default pcap encapsulation to ethernet for Ouster Studio compatibility (addresses #265) - -ouster_ros ----------- -* update ROS package version to 0.3.0 -* allow setting the packet profile in ouster.launch with the ``udp_profile_lidar`` parameter -* publish additional cloud and image topics for the second return when running in dual returns mode -* fix ``os_node`` crash on shutdown due to Eigen alignment flag not being propogated by catkin -* update ROS package version to 0.2.1 -* the ``udp_dest`` parameter to ouster.launch is now optional when connecting to a sensor - -ouster_viz ----------- -* the second CLI argument of simple_viz specifying the UDP data destination is now optional -* fixed bug in AutoExposure causing more points to be mapped to near-zero values -* add functionality to display text over cuboids - -python ------- -* update ouster-sdk version to 0.3.0 -* improve heuristics for identifying sensor data in pcaps, including new packet formats -* release builds for wheels on Windows now use the VS 2017 toolchain and runtime (previously 2019) -* fix potential use-after-free in ``LidarScan.fields`` -* update ouster-sdk version to 0.3.0b1 -* return an error when attempting to initialize ``client.Sensor`` in STANDBY mode -* check for errors while reading from a ``Sensor`` packet source and waiting for a timeout. This - should make stopping a process with ``SIGINT`` more reliable -* add PoC bindings for the ``ouster_viz`` library with a simple example driver. See the - ``ouster.sdk.examples.viz`` module -* add bindings for new configuration and metadata supported by the client library -* breaking change: the ``ChanField`` enum is now implemented as a native binding for easier interop - with C++. Unlike Python enums, the bound class itself is no longer sized or iterable. Use - ``ChanField.values`` to iterate over all ``ChanField`` values or ``LidarScan.fields`` for fields - available on a particular scan instance -* breaking change: arrays returned by ``LidarPacket.field`` and ``LidarPacket.header`` are now - immutable. Modifying the underlying packet buffer through these views was never fully supported -* deprecate ``ColHeader``, ``LidarPacket.header``, and ``LidarScan.header`` in favor of new - properties: ``timestamp``, ``measurement_id``, ``status``, and ``frame_id`` -* replace ``LidarScan`` with native bindings implementing the same API -* ``xyzlut`` can now accept a range image as an ndarray, not just a ``LidarScan`` -* update ouster-sdk version to 0.2.2 -* fix open3d example crash on exit when replaying pcaps on macos (addresses #267) -* change open3d normalization to use bound AutoExposure - - -[20210608] -========== - -ouster_client -------------- -* update cmake package version to 0.2.0 -* add support for new signal multiplier config parameter -* add early version of a C++ API covering the full sensor configuration interface -* increase default initialization timeout to 60 seconds to account for the worst case: waking up - from STANDBY mode - -ouster_pcap ------------ -* ``record_packet()`` now requires passing in a capture timestamp instead of using current time -* work around libtins issue where capture timestamps for pcaps recorded on Windows are always zero -* add preliminary C++ API for working with pcap files containing a single sensor packet capture - -ouster_ros ----------- -* update ROS package version to 0.2.0 -* add Dockerfile to easily set up a build environment or run nodes -* ``img_node`` now outputs 16-bit images, which should be more useful. Range image output is now in - units of 4mm instead of arbitrary scaling (addresses #249) -* ``img_node`` now outputs reflectivity images as well on the ``reflec_image`` topic -* change ``img_node`` topics to match terminology in sensor documentation: ``ambient_image`` is now - ``nearir_image`` and ``intensity_image`` is now ``signal_image`` -* update rviz config to use flat squares by default to work around `a bug on intel systems - `_ -* remove viz_node and all graphics stack dependencies from the package. The ``viz`` flag on the - launch file now runs rviz (addresses #236) -* clean up package.xml and ensure that dependencies are installable with rosdep (PR #219) -* the ``metadata`` argument to ouster_ros launch file is now required. No longer defaults to a name - based on the hostname of the sensor - -ouster_viz ----------- -* update reflectivity visualization for changes in the upcoming 2.1 firmware. Add new colormap and - handle 8-bit reflectivity values -* move most of the visualizer code out of public headers and hide some implementation details -* fix visualizer bug causing a small viewport when resizing the window on macos with a retina - display - -python ------- -* update ouster-sdk version to 0.2.1 -* fix bug in determining if a scan is complete with single-column azimuth windows -* closed PacketSource iterators will now raise an exception on read -* add examples for visualization using open3d (see: ``ouster.sdk.examples.open3d``) -* add support for the new signal multiplier config parameter -* preserve capture timestamps on packets read from pcaps -* first release: version 0.2.0 of ouster-sdk. See the README under the ``python`` directory for - details and links to documentation - - -[20201209] -========== - -Changed -------- - -* switched to date-based version scheme. No longer tracking firmware versions -* added a top-level ``CMakeLists.txt``. Client and visualizer should no longer be built - separately. See the README for updated build instructions -* cmake cleanup, including using custom "find modules" to provide better compatibility between - different versions of cmake -* respect standard cmake ``BUILD_SHARED_LIBS`` and ``CMAKE_POSITION_INDEPENDENT_CODE`` flags -* make ``ouster_ros`` easier to use as a dependency by bundling the client and viz libraries - together into a single library that can be used through catkin -* updated client example code. Now uses more of the client APIs to capture data and write to a - CSV. See ``ouster_client/src/example.cpp`` -* replace callback-based ``batch_to_scan`` function with ``ScanBatcher``. See ``lidar_scan.h`` for - API docs and the new client example code -* update ``LidarScan`` API. Now includes accessors for measurement blocks as well as channel data - fields. See ``lidar_scan.h`` for API docs -* add client version field to metadata json, logs, and help text -* client API renaming to better reflect the Sensor Software Manual - - -[1.14.0-beta.14] - 2020-08-27 -============================= - -Added ------ - -* support for ROS noetic in ``ouster_ros``. Note: this may break building on very old platforms - without a C++14-capable compiler -* an extra extrinsics field in ``sensor_info`` for conveniently passing around an extra user-supplied - transform -* a utility function to convert ``lidar_scan`` data between the "staggered" representation where each - column has the same timestamp and "de-staggered" representation where each column has the same - azimuth angle -* mask support in the visualizer library in ``ouster_viz`` - -Changed -------- - -* ``ouster_ros`` now requires C++14 to support building against noetic libraries -* replaced ``batch_to_iter`` with ``batch_to_scan``, a simplified function that writes directly to a - ``lidar_scan`` instead of arbitrary iterator - -Fixed ------ - -* ipv6 support using dual-stack sockets on all supported platforms. This was broken since the - beta.10 release -* projection to Cartesian coordinates now takes into account the vertical offset the sensor and - lidar frames -* the reference frame of point cloud topics in ``ouster_ros`` is now correctly reported as the "sensor - frame" defined in the user guide - - -[1.14.0-beta.12] - 2020-07-10 -============================= - -*no changes* - - -[1.14.0-beta.11] - 2020-06-17 -============================= - -*no changes* - - -[1.14.0-beta.10] - 2020-05-21 -============================= - -Added ------ - -* preliminary support for Windows and macOS for ``ouster_viz`` and ``ouster_client`` - -Changed -------- - -* replaced VTK visualizer library with one based on GLFW -* renamed all instances of "OS1" including namespaces, headers, node and topic names, to reflect - support for other product lines -* updated all xyz point cloud calculations to take into account new ``lidar_origin_to_beam_origin`` - parameter reported by sensors -* client and ``os_node`` and ``simple_viz`` now avoid setting the lidar and timestamp modes when - connecting to a client unless values are explicitly specicified - -Fixed ------ - -* increase the UDP receive buffer size in the client to reduce chances of dropping packets on - platforms with low defaults -* ``os_cloud_node`` output now uses the updated point cloud calculation, taking into account the lidar - origin offset -* minor regression with destaggering in img_node output in previous beta - - -[1.14.0-beta.4] - 2020-03-17 -============================ - -Added ------ - -* support for gen2 hardware in client, visualizer, and ROS sample code -* support for updated "packed" lidar UDP data format for 16 and 32-beam devices with firmware 1.14 -* range markers in ``simple_viz`` and ``viz_node``. Toggle display using ``g`` key. Distances can be - configured from ``os1.launch``. -* post-processing to improve ambient image uniformity in visualizer - -Changed -------- - -* use random ports for lidar and imu data by default when unspecified - - -[1.13.0] - 2020-03-16 -===================== - -Added ------ - -* post-processing to improve ambient image uniformity in visualizer -* make timestamp mode configurable via the client (PR #97) - -Changed -------- - -* turn on position-independent code by default to make using code in libraries easier (PR #65) -* use random ports for lidar and imu data by default when unspecified - -Fixed ------ - -* prevent legacy tf prefix from making invalid frame names (PR #56) -* use ``iterator_traits`` to make ``batch_to_iter`` work with more types (PR #70) -* use correct name for json dependency in ``package.xml`` (PR #116) -* handle udp socket creation error gracefully in client - - -[1.12.0] - 2019-05-02 -===================== - -Added ------ - -* install directives for ``ouster_ros`` build (addresses #50) - -Changed -------- - -* flip the sign on IMU acceleration output to follow usual conventions -* increase the update rate in the visualizer to ~60hz - -Fixed ------ - -* visualizer issue where the point cloud would occasionally occasionally not be displayed using - newer versions of Eigen - - -[1.11.0] - 2019-03-26 -===================== - -Added ------ - -* allow renaming tf ids using the ``tf_prefix`` parameter - -Changed -------- - -* use frame id to batch packets so client code deals with reordered lidar packets without splitting - frames -* use a uint32_t for PointOS1 timestamps to avoid unnecessary loss of precision - -Fixed ------ - -* bug causing ring and reflectivity to be corrupted in os1_cloud_node output -* misplaced sine in azimuth angle calculation (addresses #42) -* populate timestamps on image node output (addresses #39) - - -[1.10.0] - 2019-01-27 -===================== - -Added ------ - -* ``os1_node`` now queries and uses calibrated beam angles from the sensor -* ``os1_node`` now queries and uses imu / lidar frames from the sensor -* ``os1_node`` reads and writes metadata to ``${ROS_HOME}`` to support replaying data with calibration -* ROS example code now publishes tf2 transforms for imu / lidar frames (addresses #12) -* added ``metadata`` parameter to ``os1.launch`` to override location of metadata -* added ``viz`` parameter to ``os1.launch`` to run the example visualizer with ROS -* added ``image`` parameter to ``os1.launch`` to publish image topics to rviz (addresses #21) -* added range field to ``PointOS1`` - -Changed -------- - -* split point-cloud publishing out of ``os1_node`` into ``os1_cloud_node`` -* example visualizer controls: - - - press ``m`` to cycle through color modes instead of ``i``, ``z``, ``Z``, ``r`` - - ``r`` now resets the camera position - - range/signal images automatically resized to fit window height - -* updated OS-1 client to use newer TCP configuration commands -* updated OS-1 client to set the requested lidar mode, reinitialize on connection -* changed point cloud batching to be based on angle rather than scan duration -* ``ouster_client`` now depends on the ``jsoncpp`` library -* switched order of fields in ``PointOS1`` to be compatible with ``PointXYZI`` (addresses #16) -* moved example visualizer VTK rendering into the main thread (merged #23) -* the timestamp field of PointOS1 now represents time since the start of the scan (the timestamp of - the PointCloud2 message) in nanoseconds - -Removed -------- - -* removed keyboard camera controls in example visualizer -* removed panning and rotating of the image panel in example visualizer - -Fixed ------ - -* no longer dropping UDP packets in 2048 mode on tested hardware -* example visualizer: - - - point cloud display focus no longer snaps back on rotation - - fixed clipping issues with parallel projection - - fixed point coloring issues in z-color mode - - improved visualizer performance diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/CMakeLists.txt b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/CMakeLists.txt deleted file mode 100644 index 5ebbdd0b..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/CMakeLists.txt +++ /dev/null @@ -1,130 +0,0 @@ -cmake_minimum_required(VERSION 3.10...3.22) -if(DEFINED CMAKE_CXX_COMPILER_LAUNCHER ) - message("Using CXX Launcher: ${CMAKE_CXX_COMPILER_LAUNCHER}") - if( DEFINED ENV{CCACHE_DIR} ) - message("Using CCACHE_DIR: $ENV{CCACHE_DIR}") - endif() -endif() - -list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) -include(DefaultBuildType) - -# configure vcpkg from environment variables, if present -include(VcpkgEnv) - -# ==== Project Name ==== -project(ouster_example VERSION 20231031) - -# generate version header -set(OusterSDK_VERSION_STRING 0.11.1) -include(VersionGen) - -# ==== Options ==== -option(CMAKE_POSITION_INDEPENDENT_CODE "Build position independent code." ON) -option(BUILD_SHARED_LIBS "Build shared libraries." OFF) -option(BUILD_PCAP "Build pcap utils." ON) -option(BUILD_OSF "Build Ouster OSF library." ON) -option(BUILD_VIZ "Build Ouster visualizer." ON) -option(BUILD_TESTING "Build tests" OFF) -option(BUILD_EXAMPLES "Build C++ examples" OFF) -option(OUSTER_USE_EIGEN_MAX_ALIGN_BYTES_32 "Eigen max aligned bytes." ON) - -# when building as a top-level project -if(CMAKE_PROJECT_NAME STREQUAL PROJECT_NAME) - set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - - if(NOT DEFINED CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) - set(CMAKE_CXX_STANDARD_REQUIRED ON) - endif() - - if(NOT DEFINED CMAKE_CXX_EXTENSIONS) - set(CMAKE_CXX_EXTENSIONS OFF) - endif() - - if(MSVC) - add_compile_options(/W2) - add_compile_definitions(NOMINMAX _USE_MATH_DEFINES WIN32_LEAN_AND_MEAN) - else() - add_compile_options(-Wall -Wextra) - endif() - - include(CTest) -endif() - -# === Subdirectories === -add_subdirectory(ouster_client) -if(CMAKE_PROJECT_NAME STREQUAL PROJECT_NAME AND OUSTER_USE_EIGEN_MAX_ALIGN_BYTES_32) - message(STATUS "Ouster SDK client: Using EIGEN_MAX_ALIGN_BYTES = 32") - target_compile_definitions(ouster_client PUBLIC EIGEN_MAX_ALIGN_BYTES=32) -endif() - -if(BUILD_PCAP) - add_subdirectory(ouster_pcap) -endif() - -if(BUILD_OSF) - add_subdirectory(ouster_osf) -endif() - -if(BUILD_VIZ) - add_subdirectory(ouster_viz) -endif() - -if(BUILD_EXAMPLES) - add_subdirectory(examples) -endif() - -if(CMAKE_PROJECT_NAME STREQUAL PROJECT_NAME AND BUILD_TESTING) - add_subdirectory(tests) -endif() - -# ==== Packaging ==== -set(CPACK_PACKAGE_CONTACT "oss@ouster.io") -set(CPACK_PACKAGE_VENDOR "Ouster") -set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "Ouster sensor SDK") -set(CPACK_PACKAGE_VERSION ${PROJECT_VERSION}) -set(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/LICENSE") -set(CPACK_RESOURCE_FILE_README "${CMAKE_CURRENT_SOURCE_DIR}/README.rst") -set(CPACK_PACKAGING_INSTALL_PREFIX "/opt/ouster") -set(CPACK_SYSTEM_NAME ${CMAKE_SYSTEM_PROCESSOR}) -set(CPACK_PACKAGE_NAME ouster_sdk) -set(CPACK_GENERATOR "DEB;TGZ") - -# source packages -set(CPACK_SOURCE_GENERATOR "TGZ;ZIP") -set(CPACK_SOURCE_IGNORE_FILES /.git /dist) - -# deb options -set(CPACK_DEBIAN_PACKAGE_NAME ouster-sdk) -set(CPACK_DEBIAN_FILE_NAME DEB-DEFAULT) -set(CPACK_DEBIAN_PACKAGE_DEPENDS - "libjsoncpp-dev, libeigen3-dev, libtins-dev, libglfw3-dev, libglew-dev, libspdlog-dev, libpng-dev, libflatbuffers-dev") -include(CPack) - -# ==== Install ==== -include(CMakePackageConfigHelpers) -write_basic_package_version_file( - OusterSDKConfigVersion.cmake - VERSION ${PACKAGE_VERSION} - COMPATIBILITY AnyNewerVersion) - -install(EXPORT ouster-sdk-targets - FILE OusterSDKTargets.cmake - NAMESPACE OusterSDK:: - DESTINATION lib/cmake/OusterSDK) - -configure_file(cmake/OusterSDKConfig.cmake.in OusterSDKConfig.cmake @ONLY) -install(FILES "${CMAKE_CURRENT_BINARY_DIR}/OusterSDKConfig.cmake" - "${CMAKE_CURRENT_BINARY_DIR}/OusterSDKConfigVersion.cmake" - DESTINATION lib/cmake/OusterSDK) -if(BUILD_PCAP) - # Install the findpcap cmake file for install targets. - # For some reason it only works if you rename it to PcapConfig.cmake - install(FILES "cmake/FindPcap.cmake" - DESTINATION lib/cmake/OusterSDK - RENAME PcapConfig.cmake) -endif() - -install(FILES LICENSE LICENSE-bin - DESTINATION share) diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/CMakeSettings.json b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/CMakeSettings.json deleted file mode 100644 index dcf61699..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/CMakeSettings.json +++ /dev/null @@ -1,40 +0,0 @@ -{ - "configurations": [ - { - "name": "x64-Release", - "generator": "Ninja", - "configurationType": "Release", - "buildRoot": "${projectDir}\\out\\build\\${name}", - "installRoot": "${projectDir}\\out\\install\\${name}", - "cmakeCommandArgs": "", - "buildCommandArgs": "", - "ctestCommandArgs": "", - "inheritEnvironments": [ "msvc_x64_x64" ], - "variables": [ - { - "name": "BUILD_EXAMPLES", - "value": "True", - "type": "BOOL" - } - ] - }, - { - "name": "x64-Debug", - "generator": "Ninja", - "configurationType": "Debug", - "inheritEnvironments": [ "msvc_x64_x64" ], - "buildRoot": "${projectDir}\\out\\build\\${name}", - "installRoot": "${projectDir}\\out\\install\\${name}", - "cmakeCommandArgs": "", - "buildCommandArgs": "", - "ctestCommandArgs": "", - "variables": [ - { - "name": "BUILD_EXAMPLES", - "value": "True", - "type": "BOOL" - } - ] - } - ] -} \ No newline at end of file diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/LICENSE b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/LICENSE deleted file mode 100644 index 2d09cf12..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/LICENSE +++ /dev/null @@ -1,63 +0,0 @@ -Format: https://www.debian.org/doc/packaging-manuals/copyright-format/1.0/ -Upstream-Name: OusterSDK -Upstream-Contact: Ouster Sensor SDK Developers -Source: https://github.com/ouster-lidar/ouster_example - -Files: * -Copyright: 2018-2022 Ouster, Inc -License: BSD-3-Clause - -Files: include/optional-lite/* -Copyright: 2014-2021 Martin Moene -License: BSL-1.0 - -License: BSD-3-Clause - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -License: BSL-1.0 - Boost Software License - Version 1.0 - August 17th, 2003 - - Permission is hereby granted, free of charge, to any person or organization - obtaining a copy of the software and accompanying documentation covered by - this license (the "Software") to use, reproduce, display, distribute, - execute, and transmit the Software, and to prepare derivative works of the - Software, and to permit third-parties to whom the Software is furnished to - do so, all subject to the following: - - The copyright notices in the Software and this entire statement, including - the above license grant, this restriction and the following disclaimer, - must be included in all copies of the Software, in whole or in part, and - all derivative works of the Software, unless such copies or derivative - works are solely in the form of machine-executable object code generated by - a source language processor. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT - SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE - FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, - ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER - DEALINGS IN THE SOFTWARE. \ No newline at end of file diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/LICENSE-bin b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/LICENSE-bin deleted file mode 100644 index a64b4b6f..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/LICENSE-bin +++ /dev/null @@ -1,502 +0,0 @@ -Binary distributions of the Ouster SDK may also include the following software: - ----- - -Name: eigen -Description: compiled-in header-only library -Availability: https://gitlab.com/libeigen/eigen -License: MPL2 - -Name: glew -Description: statically linked -Availability: https://github.com/nigels-com/glew -License: BSD 3-clause - The OpenGL Extension Wrangler Library - Copyright (C) 2002-2007, Milan Ikits - Copyright (C) 2002-2007, Marcelo E. Magallon - Copyright (C) 2002, Lev Povalahev - All rights reserved. - - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - * The name of the author may be used to endorse or promote products - derived from this software without specific prior written permission. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF - THE POSSIBILITY OF SUCH DAMAGE. - -Name: glfw -Description: statically linked -Availability: https://github.com/glfw/glfw -License: zlib - -Name: jsoncpp -Description: statically linked -Availability: https://github.com/open-source-parsers/jsoncpp -License: MIT - Copyright (c) 2007-2010 Baptiste Lepilleur and The JsonCpp Authors - - Permission is hereby granted, free of charge, to any person - obtaining a copy of this software and associated documentation - files (the "Software"), to deal in the Software without - restriction, including without limitation the rights to use, copy, - modify, merge, publish, distribute, sublicense, and/or sell copies - of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be - included in all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, - EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF - MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND - NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS - BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN - ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN - CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - SOFTWARE. - -Name: libpcap -Description: statically linked -Availability: https://github.com/the-tcpdump-group/libpcap -License: BSD 3-clause - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions - are met: - - 1. Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - 2. Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in - the documentation and/or other materials provided with the - distribution. - 3. The names of the authors may not be used to endorse or promote - products derived from this software without specific prior - written permission. - - THIS SOFTWARE IS PROVIDED ``AS IS'' AND WITHOUT ANY EXPRESS OR - IMPLIED WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE IMPLIED - WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - -Name: libtins -Description: statically linked -Availability: https://github.com/mfontanini/libtins -License: BSD 2-clause - Copyright (c) 2012-2017, Matias Fontanini - All rights reserved. - - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are - met: - - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above - copyright notice, this list of conditions and the following disclaimer - in the documentation and/or other materials provided with the - distribution. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR - A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, - DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY - THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -Name: optional-lite -Description: compiled-in header-only library -Availability: https://github.com/martinmoene/optional-lite -License: BSL-1.0 - Permission is hereby granted, free of charge, to any person or organization - obtaining a copy of the software and accompanying documentation covered by - this license (the "Software") to use, reproduce, display, distribute, - execute, and transmit the Software, and to prepare derivative works of the - Software, and to permit third-parties to whom the Software is furnished to - do so, all subject to the following: - - The copyright notices in the Software and this entire statement, including - the above license grant, this restriction and the following disclaimer, - must be included in all copies of the Software, in whole or in part, and - all derivative works of the Software, unless such copies or derivative - works are solely in the form of machine-executable object code generated by - a source language processor. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT - SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE - FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, - ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER - DEALINGS IN THE SOFTWARE. - -Name: pybind11 -Description: compiled-in header-only library -Availability: https://github.com/pybind/pybind11 -License: BSD 3-clause - Copyright (c) 2016 Wenzel Jakob , All rights reserved. - - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: - - 1. Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - - 2. Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - 3. Neither the name of the copyright holder nor the names of its contributors - may be used to endorse or promote products derived from this software - without specific prior written permission. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -Name: gltext -Description: compiled-in header-only library -Availability: https://github.com/vallentin/glText -License: Zlib License - Copyright (c) 2013-2018 Christian Vallentin - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would - be appreciated but is not required. - - 2. Altered source versions must be plainly marked as such, and must not - be misrepresented as being the original software. - - 3. This notice may not be removed or altered from any source - distribution. - -Name: glad -Description: statically linked -Availability: https://github.com/Dav1dde/glad/ -License: MIT - Copyright (c) 2013-2021 David Herberth - - Permission is hereby granted, free of charge, to any person obtaining a copy of - this software and associated documentation files (the "Software"), to deal in - the Software without restriction, including without limitation the rights to - use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of - the Software, and to permit persons to whom the Software is furnished to do so, - subject to the following conditions: - - The above copyright notice and this permission notice shall be included in all - copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS - FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR - COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER - IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN - CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - - -Name: curl -Description: statically linked -Availability: https://github.com/curl/curl -License: Curl License - Copyright (c) 1996 - 2022, Daniel Stenberg, , and many - contributors, see the THANKS file. - - All rights reserved. - - Permission to use, copy, modify, and distribute this software for any purpose - with or without fee is hereby granted, provided that the above copyright - notice and this permission notice appear in all copies. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. IN - NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, - DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR - OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE - OR OTHER DEALINGS IN THE SOFTWARE. - - Except as contained in this notice, the name of a copyright holder shall not - be used in advertising or otherwise to promote the sale, use or other dealings - in this Software without prior written authorization of the copyright holder. - -Name: spdlog -Description: statically linked -Availability: https://github.com/gabime/spdlog -License: MIT - Copyright (c) 2016 Gabi Melman. - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. - - -- NOTE: Third party dependency used by this software -- - This software depends on the fmt lib (MIT License), - and users must comply to its license: https://github.com/fmtlib/fmt/blob/master/LICENSE.rst - - -Name: flatbuffers -Description: statically linked -Availability: https://github.com/google/flatbuffers -License: Apache - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - -Name: libpng -Description: statically linked -Availability: http://www.libpng.org/pub/png/libpng.html -License: PNG Reference Library License - -PNG Reference Library License version 2 ---------------------------------------- - - * Copyright (c) 1995-2023 The PNG Reference Library Authors. - * Copyright (c) 2018-2023 Cosmin Truta. - * Copyright (c) 2000-2002, 2004, 2006-2018 Glenn Randers-Pehrson. - * Copyright (c) 1996-1997 Andreas Dilger. - * Copyright (c) 1995-1996 Guy Eric Schalnat, Group 42, Inc. - -The software is supplied "as is", without warranty of any kind, -express or implied, including, without limitation, the warranties -of merchantability, fitness for a particular purpose, title, and -non-infringement. In no event shall the Copyright owners, or -anyone distributing the software, be liable for any damages or -other liability, whether in contract, tort or otherwise, arising -from, out of, or in connection with the software, or the use or -other dealings in the software, even if advised of the possibility -of such damage. - -Permission is hereby granted to use, copy, modify, and distribute -this software, or portions hereof, for any purpose, without fee, -subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you - must not claim that you wrote the original software. If you - use this software in a product, an acknowledgment in the product - documentation would be appreciated, but is not required. - - 2. Altered source versions must be plainly marked as such, and must - not be misrepresented as being the original software. - - 3. This Copyright notice may not be removed or altered from any - source or altered source distribution. \ No newline at end of file diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/README.rst b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/README.rst deleted file mode 100644 index b8c30032..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/README.rst +++ /dev/null @@ -1,44 +0,0 @@ -.. figure:: https://github.com/ouster-lidar/ouster_example/raw/master/docs/images/Ouster_Logo_TM_Horiz_Black_RGB_600px.png - ------------------------------------------------------- - -========================================================= -Ouster SDK - libraries and tools for Ouster Lidar Sensors -========================================================= - -Cross-platform C++/Python Ouster Sensor Development Toolkit - -To get started with our sensors, client, and visualizer, please see our SDK and sensor documentation: -ouster-sdk/index.html - -- `Ouster SDK Documentation `_ -- `Ouster Sensor Public Documentaion `_ - -This repository contains Ouster SDK source code for connecting to and configuring ouster sensors, -reading and visualizing data. - -* `ouster_client `_ contains an example C++ client for ouster sensors -* `ouster_pcap `_ contains C++ pcap functions for ouster sensors -* `ouster_osf `_ contains C++ OSF library to store ouster sensors data -* `ouster_viz `_ contains a customizable point cloud visualizer -* `python `_ contains the code for the ouster sensor python SDK (``ouster-sdk`` Python package) - -.. note:: - Ouster ROS driver code has been moved out to a separate GitHub repository. To get started using the - driver follow the instructions provided on the repository's main page: https://github.com/ouster-lidar/ouster-ros - - -Contact -======= - -For support of the Ouster SDK, please use `Github Issues `_ in this repo. - -For support of Ouster products outside of the SDK, please use `Ouster customer support `_. - -For suspected security problems, please contact us at security@ouster.io. - - -License -======= - -BSD 3-Clause License, `details `_ diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/clang-linting.sh b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/clang-linting.sh deleted file mode 100755 index 1aed0e97..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/clang-linting.sh +++ /dev/null @@ -1,42 +0,0 @@ -#!/bin/bash - -help='false' -apply='false' -check='true' -SUBCOMMAND='--Werror --dry-run' -#Input arguments -while getopts 'ha' flag -do - case "${flag}" in - h) help='true';; - a) apply='true';; - esac -done - -if [[ $help == 'true' ]]; then - echo """ - How to run: - cd ouster-sdk; then run ./clang-linting.sh - Default: performs a clang format check and prints errors if they exist - ./clang-linting.sh -a - applies the clang format to the files - """ -fi - -if [[ $apply == 'true' ]]; then - check='false' - SUBCOMMAND='' -fi - - -# Ignore the ./ouster_client/include/optional-lite/nonstd/optional.hpp file while formatting files -CMD="find . -regex '.*\.\(cpp\|hpp\|cu\|c\|h\)' -not -path ./ouster_client/include/optional-lite/nonstd/optional.hpp \ --exec clang-format ${SUBCOMMAND} -style=file -i {} \;" -OUTPUT=$(eval "$CMD" 2>&1 | grep error) -if [ -z "$OUTPUT" ]; then - echo "clang-format check passed! No errors" - exit 0; -else - eval "$CMD" - exit 1; -fi diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/Coverage.cmake b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/Coverage.cmake deleted file mode 100644 index 332ab7d6..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/Coverage.cmake +++ /dev/null @@ -1,9 +0,0 @@ -# Cmake Functions For Code Coverage - -FUNCTION(CodeCoverageFunctionality target) - if(DEFINED ENV{CMAKE_COVERAGE_TESTS} AND "$ENV{CMAKE_COVERAGE_TESTS}" MATCHES "true") - message(STATUS "Code Coverage Enabled: Target: ${target}") - target_link_libraries(${target} PRIVATE gcov) - target_compile_options(${target} PRIVATE -O0 --coverage -g) - endif() -ENDFUNCTION() diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/DefaultBuildType.cmake b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/DefaultBuildType.cmake deleted file mode 100644 index 0e290439..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/DefaultBuildType.cmake +++ /dev/null @@ -1,6 +0,0 @@ -# default to "Release" if build type is not set -if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) - message(STATUS "Build type not specified, using: Release") - set(CMAKE_BUILD_TYPE "Release" CACHE STRING "Choose the type of build." FORCE) - set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug" "MinSizeRel" "Release" "RelWithDebInfo") -endif() diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/FindCURL.cmake b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/FindCURL.cmake deleted file mode 100644 index 826c27c5..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/FindCURL.cmake +++ /dev/null @@ -1,45 +0,0 @@ -# Define forwards-compatible imported target for old platforms (Ubuntu 18.04) -# Note: curl from VCPKG appears to completely ignore curl find modules despite -# CMAKE_MODULE_PATH settings - -include(FindPackageHandleStandardArgs) - -# Prefer package config if it exists -find_package(CURL CONFIG QUIET) -if(CURL_FOUND) - find_package_handle_standard_args(CURL CONFIG_MODE) - return() -endif() - -# Trying to find with pkg-config -find_package(PkgConfig QUIET) -if(PKG_CONFIG_FOUND) - pkg_check_modules(pkg_curl QUIET libcurl) - if(pkg_curl_FOUND) - set(CURL_VERSION_STRING ${pkg_curl_VERSION}) - endif() -endif() - -find_path(CURL_INCLUDE_DIRS - NAMES curl/curl.h - HINTS ${pkg_curl_INCLUDE_DIRS}) -mark_as_advanced(CURL_INCLUDE_DIRS) - -# Linux/macos only -find_library(CURL_LIBRARIES NAMES - curl - HINTS ${pkg_curl_LIBRARY_DIRS}) -mark_as_advanced(CURL_LIBRARIES) - -# Check that we have everything that we need -find_package_handle_standard_args(CURL - REQUIRED_VARS CURL_INCLUDE_DIRS CURL_LIBRARIES - VERSION_VAR CURL_VERSION_STRING) - -if(NOT TARGET CURL::libcurl) - add_library(CURL::libcurl UNKNOWN IMPORTED) - set_target_properties(CURL::libcurl PROPERTIES - INTERFACE_INCLUDE_DIRECTORIES "${CURL_INCLUDE_DIRS}" - IMPORTED_LINK_INTERFACE_LANGUAGES "C" - IMPORTED_LOCATION "${CURL_LIBRARIES}") -endif() diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/FindEigen3.cmake b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/FindEigen3.cmake deleted file mode 100644 index 8ba8cbba..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/FindEigen3.cmake +++ /dev/null @@ -1,14 +0,0 @@ -# Define forwards-compatible imported target for old platforms (Ubuntu 16.04) - -include(FindPackageHandleStandardArgs) - -find_package(Eigen3 CONFIG QUIET) - -find_package_handle_standard_args(Eigen3 CONFIG_MODE) - -if(NOT TARGET Eigen3::Eigen) - add_library(Eigen3::Eigen INTERFACE IMPORTED) - - set_target_properties(Eigen3::Eigen PROPERTIES - INTERFACE_INCLUDE_DIRECTORIES ${EIGEN3_INCLUDE_DIR}) -endif() diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/FindGTest.cmake b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/FindGTest.cmake deleted file mode 100644 index 7929834f..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/FindGTest.cmake +++ /dev/null @@ -1,50 +0,0 @@ -# Attempt to deal with gtest cmake differences across platforms - -function(find_gtest) - # use system find module in this scope - set(CMAKE_MODULE_PATH "") - - # using the cmake-provided find module succeeds, but the resulting GTest::GTest and - # GTest::Main targets cause link errors with vcpkg 2021.05.12 on macos. Try CONFIG-only first - find_package(GTest CONFIG QUIET) - - if (GTest_CONFIG AND TARGET GTest::gtest AND TARGET GTest::gtest_main) - find_package_handle_standard_args(GTest DEFAULT_MSG GTest_CONFIG) - return() - endif() - - # next, try using a find module. Requires temporarily turning off REQUIRED to avoid failing at the - # call to find_package() - set(GTest_FIND_REQUIRED_SAVED ${GTest_FIND_REQUIRED}) - set(GTest_FIND_REQUIRED 0) - find_package(GTest MODULE QUIET) - set(GTest_FIND_REQUIRED ${GTest_FIND_REQUIRED_SAVED}) - - if (GTEST_FOUND) - find_package_handle_standard_args(GTest DEFAULT_MSG GTEST_LIBRARY GTEST_MAIN_LIBRARY) - - # aliases to cmake >= 3.20 targets. More portable than IMPORTED_GLOBAL - # https://cmake.org/cmake/help/v3.22/module/FindGTest.html#imported-targets - if(NOT TARGET GTest::gtest) - add_library(GTest::gtest INTERFACE IMPORTED) - target_link_libraries(GTest::gtest INTERFACE GTest::GTest) - endif() - if(NOT TARGET GTest::gtest_main) - add_library(GTest::gtest_main INTERFACE IMPORTED) - target_link_libraries(GTest::gtest_main INTERFACE GTest::Main) - endif() - - return() - endif() - - # finally, try src location for libgtest-dev for debian-based distros where - # the find module appears to be broken (xenial, bionic) - find_path(GTEST_ROOT CMakeLists.txt "/usr/src/gtest") - find_package_handle_standard_args(GTest DEFAULT_MSG GTEST_ROOT) - add_subdirectory(${GTEST_ROOT} gtest) - add_library(GTest::gtest ALIAS gtest) - add_library(GTest::gtest_main ALIAS gtest_main) - -endfunction() - -find_gtest() diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/FindOusterSDK.cmake b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/FindOusterSDK.cmake deleted file mode 100644 index 10257caa..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/FindOusterSDK.cmake +++ /dev/null @@ -1,5 +0,0 @@ -# Allow downstream code to depend on source transparently -if(NOT TARGET EXAMPLE_INCLUDED) - add_custom_target(EXAMPLE_INCLUDED) - add_subdirectory(${CMAKE_CURRENT_LIST_DIR}/.. ouster_example EXCLUDE_FROM_ALL) -endif() diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/FindPcap.cmake b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/FindPcap.cmake deleted file mode 100644 index b7c91f66..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/FindPcap.cmake +++ /dev/null @@ -1,32 +0,0 @@ -find_package(PkgConfig QUIET) -if(PKG_CONFIG_FOUND) - pkg_check_modules(PC_PCAP QUIET libpcap) - if(PC_PCAP_FOUND) - set(PCAP_VERSION_STRING ${PC_PCAP_VERSION}) - endif() -endif() - -find_path(PCAP_INCLUDE_DIR - NAMES pcap.h - HINTS ${PC_PCAP_INCLUDE_DIRS}) - -find_library(PCAP_LIBRARY - NAMES pcap pcap_static wpcap - HINTS ${PC_PCAP_LIBRARY_DIRS}) - -if(NOT TARGET libpcap::libpcap) - add_library(libpcap::libpcap UNKNOWN IMPORTED) - set_target_properties(libpcap::libpcap PROPERTIES - INTERFACE_INCLUDE_DIRECTORIES "${PCAP_INCLUDE_DIR}" - IMPORTED_LINK_INTERFACE_LANGUAGES "C" - IMPORTED_LOCATION "${PCAP_LIBRARY}") -endif() - -include(FindPackageHandleStandardArgs) -find_package_handle_standard_args(Pcap - REQUIRED_VARS PCAP_LIBRARY PCAP_INCLUDE_DIR - VERSION_VAR PCAP_VERSION_STRING) - -mark_as_advanced( - PCAP_INCLUDE_DIR - PCAP_LIBRARY) diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/FindPybind11Internal.cmake b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/FindPybind11Internal.cmake deleted file mode 100644 index 5a2dd59e..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/FindPybind11Internal.cmake +++ /dev/null @@ -1,54 +0,0 @@ -include(FindPackageHandleStandardArgs) - -if(DEFINED PYTHON_EXECUTABLE) - execute_process( - COMMAND - "${PYTHON_EXECUTABLE}" "-c" - "import sys; print(f'{str(sys.version_info.major)}.{str(sys.version_info.minor)}.{str(sys.version_info.micro)}')" - OUTPUT_VARIABLE _version_full - OUTPUT_STRIP_TRAILING_WHITESPACE) - - execute_process( - COMMAND - "${PYTHON_EXECUTABLE}" "-c" - "import pybind11;print(pybind11.get_cmake_dir())" - OUTPUT_VARIABLE _pybind_dir - RESULT_VARIABLE _pybind_result - ERROR_VARIABLE _pybind_error - OUTPUT_STRIP_TRAILING_WHITESPACE) - if(${_pybind_result} EQUAL 0) - LIST(APPEND CMAKE_PREFIX_PATH "${_pybind_dir}") - if("${_version_full}" VERSION_GREATER_EQUAL "3.11.0") - find_package(pybind11 2.10 REQUIRED) - else() - find_package(pybind11 2.0 REQUIRED) - endif() - - set(_PYBIND11_INTERNAL_PYTHON_VERSION "") - if(NOT PYTHON_VERSION STREQUAL "") - message("Found Python Version VIA: PYTHON_VERSION") - set(_PYBIND11_INTERNAL_PYTHON_VERSION "${PYTHON_VERSION}") - elseif(NOT PYTHONLIBS_VERSION_STRING STREQUAL "") - message("Found Python Version VIA: PYTHONLIBS_VERSION_STRING") - set(_PYBIND11_INTERNAL_PYTHON_VERSION "${PYTHONLIBS_VERSION_STRING}") - elseif(NOT PYTHON_VERSION_STRING STREQUAL "") - message("Found Python Version VIA: PYTHON_VERSION_STRING") - set(_PYBIND11_INTERNAL_PYTHON_VERSION "${PYTHON_VERSION_STRING}") - endif() - if(VCPKG_TOOLCHAIN AND NOT "${_version_full}" VERSION_EQUAL "${_PYBIND11_INTERNAL_PYTHON_VERSION}") - message(FATAL_ERROR "Python Versions Do Not Match -\tRequested Version: -\t\t${_version_full} -\tVersions Found: -\t\tPYTHON_VERSION: \"${PYTHON_VERSION}\" -\t\tPYTHONLIBS_VERSION_STRING: \"${PYTHONLIBS_VERSION_STRING}\" -\t\tPYTHON_VERSION_STRING: \"${PYTHON_VERSION_STRING}\" -\tInternal Cache: \"${_PYBIND11_INTERNAL_PYTHON_VERSION}\"") - endif() - else() - message(FATAL_ERROR "ERROR In Setting Pybind11 CMAKE Prefix Path: ${_pybind_error}") - endif() -else() - message(FATAL_ERROR "PYTHON_EXECUTABLE NOT SET") -endif() - diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/Findglfw3.cmake b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/Findglfw3.cmake deleted file mode 100644 index da49773b..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/Findglfw3.cmake +++ /dev/null @@ -1,14 +0,0 @@ -# For ROS/catkin compatibility only. Set variables expected by catkin to allow exporting GLFW3 in -# the DEPENDS clause of the catkin_package() macro. - -include(FindPackageHandleStandardArgs) - -find_package(glfw3 CONFIG REQUIRED) - -# Package on >=18.04 sets a target -if(TARGET glfw) - get_target_property(GLFW3_LIBRARIES glfw LOCATION) - get_target_property(GLFW3_INCLUDE_DIRS glfw INTERFACE_INCLUDE_DIRECTORIES) -endif() - -find_package_handle_standard_args(glfw3 CONFIG_MODE) diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/Findjsoncpp.cmake b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/Findjsoncpp.cmake deleted file mode 100644 index 967e7326..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/Findjsoncpp.cmake +++ /dev/null @@ -1,57 +0,0 @@ -# Targets defined for jsoncpp differ a good amount between distributions -include(FindPackageHandleStandardArgs) - -# Recent jsoncpp cmake config fails if called twice -if(NOT jsoncpp_FOUND) - find_package(jsoncpp CONFIG QUIET) -endif() - -# This target exists on ubuntu / debian. For some reason debian bullseye doesn't -# ship a static lib / define the jsoncpp_lib_static target. -if(jsoncpp_FOUND AND TARGET jsoncpp_lib) - find_package_handle_standard_args(jsoncpp CONFIG_MODE) - return() -endif() - -# With vcpkg, only a static lib is available so create a target for compatibility -if(jsoncpp_FOUND AND TARGET jsoncpp_static) - add_library(jsoncpp_lib INTERFACE) - target_link_libraries(jsoncpp_lib INTERFACE jsoncpp_static) - install(TARGETS jsoncpp_lib EXPORT ouster-sdk-targets) - find_package_handle_standard_args(jsoncpp CONFIG_MODE) - return() -endif() - -# Fall back to find_library with hints from pkgconfig -find_package(PkgConfig QUIET) -if(PKG_CONFIG_FOUND) - pkg_check_modules(PC_JSONCPP QUIET jsoncpp) - if(PC_JSONCPP_FOUND) - set(jsoncpp_VERSION_STRING ${PC_JSONCPP_VERSION}) - endif() -endif() - -find_library(jsoncpp_LIBRARY NAMES libjsoncpp.so libjsoncpp.dylib - PATHS ${PC_JSONCPP_LIBDIR} ${PC_JSONCPP_LIBRARY_DIRS}) -find_library(jsoncpp_STATIC_LIBRARY NAMES libjsoncpp.a - PATHS ${PC_JSONCPP_LIBDIR} ${PC_JSONCPP_LIBRARY_DIRS}) - -find_path(jsoncpp_INCLUDE_DIR - NAMES json/json.h - PATHS ${PC_JSONCPP_INCLUDE_DIRS}) - -find_package_handle_standard_args(jsoncpp - REQUIRED_VARS jsoncpp_LIBRARY jsoncpp_INCLUDE_DIR - VERSION_VAR jsoncpp_VERSION_STRING) - -if(jsoncpp_FOUND) - add_library(jsoncpp_lib_static STATIC IMPORTED) - set_target_properties(jsoncpp_lib_static PROPERTIES - INTERFACE_INCLUDE_DIRECTORIES ${jsoncpp_INCLUDE_DIR} - IMPORTED_LOCATION ${jsoncpp_STATIC_LIBRARY}) - - add_library(jsoncpp_lib SHARED IMPORTED) - set_target_properties(jsoncpp_lib PROPERTIES - INTERFACE_INCLUDE_DIRECTORIES ${jsoncpp_INCLUDE_DIR} - IMPORTED_LOCATION ${jsoncpp_LIBRARY}) -endif() diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/Findlibtins.cmake b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/Findlibtins.cmake deleted file mode 100644 index 55c87d38..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/Findlibtins.cmake +++ /dev/null @@ -1,59 +0,0 @@ -# Package configuration on supported platforms: -# debian: only pkgconfig provided -# vcpkg: config.cmake, `tins` target and LIBTINS_INCLUDE_DIRS, LIBTINS_LIBRARIES for compat -# brew: same as vcpkg, but LIBTINS_INCLUDE_DIRS is wrong -# conan: `libtins`, `libtins::libtins` targets and libtins_INCLUDE_DIRS, libtins_LIBRARIES -# libtins_LIBRARIES can't be used because it contains paths that differ between build and -# install when building with conan - -# Try to find a cmake config compatible with vcpkg and fall back to defining -# targets using pkgconfig. -include(FindPackageHandleStandardArgs) - -# Prefer config, if found -find_package(libtins CONFIG QUIET) -if(libtins_FOUND AND TARGET tins) - # The config in vcpkg and homebrew doesn't set interface include directories - # on the target (since it's usually installed and included relative to the - # system include path). That assumption fails if brew is installed under /opt. - # The config does, however, set LIBTINS_INCLUDE_DIRS but bizarrely, this - # appears to be set to a path under /tmp on homebrew, so we find the actual - # include dir here and create our own target below. - unset(LIBTINS_INCLUDE_DIRS) - find_path(LIBTINS_INCLUDE_DIRS NAMES tins/tins.h) - set_target_properties(tins PROPERTIES - INTERFACE_INCLUDE_DIRECTORIES ${LIBTINS_INCLUDE_DIRS}) - - find_package_handle_standard_args(libtins CONFIG_MODE) -else() - # Fall back to find_library with hints from pkgconfig - find_package(PkgConfig QUIET) - if(PKG_CONFIG_FOUND) - pkg_check_modules(PC_TINS QUIET libtins) - - if(PC_TINS_FOUND) - set(LIBTINS_VERSION ${PC_TINS_VERSION}) - endif() - endif() - - find_library(LIBTINS_LIBRARIES NAMES libtins.so libtins.dylib - PATHS ${PC_TINS_LIBDIR} ${PC_TINS_LIBRARY_DIRS}) - - find_path(LIBTINS_INCLUDE_DIRS - NAMES tins/tins.h - PATHS ${PC_TINS_INCLUDE_DIRS}) - - find_package_handle_standard_args(libtins - REQUIRED_VARS LIBTINS_LIBRARIES LIBTINS_INCLUDE_DIRS - VERSION_VAR LIBTINS_VERSION) -endif() - -# LIBTINS_LIBRARIES is set, add target with a name compatible with the conan -# package -if(NOT TARGET libtins) - add_library(libtins INTERFACE) - set_target_properties(libtins PROPERTIES - INTERFACE_LINK_LIBRARIES ${LIBTINS_LIBRARIES}) - add_library(libtins::libtins ALIAS libtins) - install(TARGETS libtins EXPORT ouster-sdk-targets) -endif() diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/OusterSDKConfig.cmake.in b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/OusterSDKConfig.cmake.in deleted file mode 100644 index 136243bf..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/OusterSDKConfig.cmake.in +++ /dev/null @@ -1,38 +0,0 @@ -message(STATUS "Found OusterSDK: ${CMAKE_CURRENT_LIST_FILE}") - -include(CMakeFindDependencyMacro) - -# ouster_client dependencies -find_dependency(Eigen3) -find_dependency(jsoncpp) -find_dependency(CURL) -find_dependency(spdlog) - -# ouster_osf dependencies -if(@BUILD_OSF@) - find_package(ZLIB REQUIRED) - find_package(PNG REQUIRED) - find_package(Flatbuffers NAMES Flatbuffers FlatBuffers) -endif() - -# viz dependencies -if(@BUILD_VIZ@) - set(OpenGL_GL_PREFERENCE GLVND) - find_dependency(OpenGL) - find_dependency(glfw3) - - if(@OUSTER_VIZ_USE_GLAD@) - find_dependency(glad) - else() - find_dependency(GLEW) - endif() -endif() - -if(@BUILD_PCAP@) - # make libtins dependency optional; on debian distros, libtins doesn't include - # a config module and sdk targets will just include paths in that case - find_package(libtins QUIET) - find_package(Pcap REQUIRED HINTS ${CMAKE_CURRENT_LIST_DIR}) -endif() - -include("${CMAKE_CURRENT_LIST_DIR}/OusterSDKTargets.cmake") diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/VcpkgEnv.cmake b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/VcpkgEnv.cmake deleted file mode 100644 index c46f0f08..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/VcpkgEnv.cmake +++ /dev/null @@ -1,17 +0,0 @@ -# Make it possible to configure builds against vcpkg using env variables -# - must include() this before project() in CMakeLists.txt -# https://vcpkg.readthedocs.io/en/latest/users/integration/ - -# set toolchain file if VCPKG_ROOT is defind -if(DEFINED ENV{VCPKG_ROOT} AND NOT DEFINED CMAKE_TOOLCHAIN_FILE) - message(STATUS "Using CMAKE_TOOLCHAIN_FILE from env VCPKG_ROOT: $ENV{VCPKG_ROOT}") - set(CMAKE_TOOLCHAIN_FILE "$ENV{VCPKG_ROOT}/scripts/buildsystems/vcpkg.cmake" - CACHE STRING "The CMake toolchain file") -endif() - -# set VCPKG_TARGET_TRIPLET from corresponding env variable -if(DEFINED ENV{VCPKG_TARGET_TRIPLET} AND NOT DEFINED VCPKG_TARGET_TRIPLET) - message(STATUS "Using VCPKG_TARGET_TRIPLET from env: $ENV{VCPKG_TARGET_TRIPLET}") - set(VCPKG_TARGET_TRIPLET "$ENV{VCPKG_TARGET_TRIPLET}" - CACHE STRING "Triplet used for vcpkg build") -endif() diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/VersionGen.cmake b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/VersionGen.cmake deleted file mode 100644 index 0b986166..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/VersionGen.cmake +++ /dev/null @@ -1,54 +0,0 @@ -if(CMAKE_SCRIPT_MODE_FILE) - # in build stage - set(GIT_HASH "unknown") - - find_package(Git QUIET) - if(GIT_FOUND) - execute_process( - COMMAND ${GIT_EXECUTABLE} describe --always --match none --dirty - OUTPUT_VARIABLE GIT_OUTPUT - RESULT_VARIABLE GIT_RESULT - ERROR_QUIET - WORKING_DIRECTORY ${VERSION_GEN_SOURCE_DIR}) - - if(${GIT_RESULT} EQUAL 0) - set(GIT_HASH "${GIT_OUTPUT}") - endif() - endif() - - string(STRIP "${GIT_HASH}" GIT_HASH) - string(TOLOWER "${BUILD_TYPE}" BUILD_TYPE) - - configure_file( - ${VERSION_GEN_SOURCE_DIR}/build.h.in - ${VERSION_GEN_OUT_DIR}/build.h @ONLY) -elseif(NOT TARGET ouster_build) - # in configuration stage: expects OusterSDK_VERSION_STRING to be set - if(OusterSDK_VERSION_STRING MATCHES "^([0-9]+\.[0-9]+\.[0-9]+)(.([.0-9A-z]+))?$") - set(OusterSDK_VERSION "${CMAKE_MATCH_1}") - set(OusterSDK_VERSION_SUFFIX "${CMAKE_MATCH_3}") - else() - message(FATAL_ERROR "Failed to parse version string: ${OusterSDK_VERSION_STRING}") - endif() - - add_custom_target(ouster_generate_header) - add_custom_command(TARGET ouster_generate_header PRE_BUILD - WORKING_DIRECTORY ${CMAKE_BINARY_DIR} - COMMENT "Generating build info header" - COMMAND ${CMAKE_COMMAND} - -DVERSION_GEN_OUT_DIR="${CMAKE_CURRENT_BINARY_DIR}/generated/ouster/impl" - -DVERSION_GEN_SOURCE_DIR="${CMAKE_CURRENT_LIST_DIR}" - -DBUILD_TYPE="${CMAKE_BUILD_TYPE}" - -DBUILD_SYSTEM="${CMAKE_SYSTEM_NAME}" - -DOusterSDK_VERSION="${OusterSDK_VERSION}" - -DOusterSDK_VERSION_SUFFIX="${OusterSDK_VERSION_SUFFIX}" - -P ${CMAKE_CURRENT_LIST_FILE}) - - add_library(ouster_build INTERFACE) - target_include_directories(ouster_build INTERFACE - $) - add_dependencies(ouster_build ouster_generate_header) - install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/generated/ouster - DESTINATION include) - -endif() diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/build.h.in b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/build.h.in deleted file mode 100644 index 0ad0901f..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/cmake/build.h.in +++ /dev/null @@ -1,17 +0,0 @@ -#pragma once - -namespace ouster { - -const char* const BUILD_HASH = "@GIT_HASH@"; - -const char* const BUILD_TYPE = "@BUILD_TYPE@"; - -const char* const BUILD_SYSTEM = "@BUILD_SYSTEM@"; - -const char* const SDK_VERSION = - "@OusterSDK_VERSION@@OusterSDK_VERSION_SUFFIX@"; - -const char* const SDK_VERSION_FULL = - "@OusterSDK_VERSION@@OusterSDK_VERSION_SUFFIX@+@GIT_HASH@-@BUILD_TYPE@"; - -} diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/conan/test_package/CMakeLists.txt b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/conan/test_package/CMakeLists.txt deleted file mode 100644 index dacc3d41..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/conan/test_package/CMakeLists.txt +++ /dev/null @@ -1,7 +0,0 @@ -cmake_minimum_required(VERSION 3.10.0) - -project(PackageTest CXX) - -find_package(OusterSDK REQUIRED) - -add_subdirectory(../../examples examples) diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/conan/test_package/conanfile.py b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/conan/test_package/conanfile.py deleted file mode 100644 index 34f29785..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/conan/test_package/conanfile.py +++ /dev/null @@ -1,39 +0,0 @@ -import os - -from conans import ConanFile, CMake, tools - - -class OusterSDKTestConan(ConanFile): - settings = "os", "compiler", "build_type", "arch" - generators = "cmake_paths", "cmake_find_package" - - def build(self): - cmake = CMake(self) - # Current dir is "test_package/build/" and CMakeLists.txt is - # in "test_package" - cmake.definitions["BUILD_OSF"] = True - cmake.definitions[ - "CMAKE_PROJECT_PackageTest_INCLUDE"] = os.path.join( - self.build_folder, "conan_paths.cmake") - cmake.configure() - cmake.build() - - def imports(self): - self.copy("*.dll", dst="bin", src="bin") - self.copy("*.dylib*", dst="bin", src="lib") - self.copy('*.so*', dst='bin', src='lib') - - def test(self): - if not tools.cross_building(self): - os.chdir("examples") - # on Windows VS puts execulables under `./Release` or `./Debug` folders - exec_path = f"{os.sep}{self.settings.build_type}" if self.settings.os == "Windows" else "" - self.run(f".{exec_path}{os.sep}client_example") - - # List files + ldd for verbosity - if self.settings.os == "Linux": - self.run(f"ls -al .{exec_path}{os.sep}") - self.run(f"ldd .{exec_path}{os.sep}osf_reader_example") - - # Smoke test OSF lib - self.run(f".{exec_path}{os.sep}osf_reader_example") diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/conanfile.py b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/conanfile.py deleted file mode 100644 index 2db6364e..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/conanfile.py +++ /dev/null @@ -1,137 +0,0 @@ -import os -import re -from conans import ConanFile, CMake, tools - -from pprint import pformat - - -class OusterSDKConan(ConanFile): - name = "ouster_sdk" - license = "BSD 3-Clause License" - author = "Ouster, Inc." - url = "https://github.com/ouster-lidar/ouster_example" - description = "Ouster SDK - tools for working with Ouster Lidars" - topics = ("lidar", "driver", "hardware", "point cloud", "3d", "robotics", "automotive") - settings = "os", "compiler", "build_type", "arch" - - options = { - "build_viz": [True, False], - "build_pcap": [True, False], - "build_osf": [True, False], - "shared": [True, False], - "fPIC": [True, False], - "ensure_cpp17": [True, False], - "eigen_max_align_bytes": [True, False], - } - default_options = { - "build_viz": False, - "build_pcap": False, - "build_osf": False, - "shared": False, - "fPIC": True, - "ensure_cpp17": False, - "eigen_max_align_bytes": False, - } - - generators = "cmake_paths", "cmake_find_package" - exports_sources = [ - "cmake/*", - "conan/*", - "ouster_client/*", - "ouster_pcap/*", - "ouster_osf/*", - "ouster_viz/*", - "tests/*", - "CMakeLists.txt", - "CMakeSettings.json", - "LICENSE", - "LICENSE-bin", - "README.rst" - ] - - # https://docs.conan.io/en/1.51/howtos/capture_version.html#how-to-capture-package-version-from-text-or-build-files - def set_version(self): - content = tools.load(os.path.join(self.recipe_folder, "CMakeLists.txt")) - version = re.search(r"set\(OusterSDK_VERSION_STRING (.*)\)", content).group(1) - self.version = version.strip() - - def config_options(self): - if self.settings.os == "Windows": - del self.options.fPIC - - def requirements(self): - # not required directly here but because boost and openssl pulling - # slightly different versions of zlib we need to set it - # here explicitly - self.requires("zlib/1.3") - - self.requires("eigen/3.4.0") - self.requires("jsoncpp/1.9.5") - self.requires("spdlog/1.11.0") - self.requires("fmt/9.1.0") - self.requires("libcurl/7.84.0") - - if self.options.build_pcap: - self.requires("libtins/4.3") - - if self.options.build_osf: - self.requires("flatbuffers/23.5.26") - self.requires("libpng/1.6.39") - - if self.options.build_viz: - self.requires("glad/0.1.34") - if self.settings.os != "Windows": - self.requires("xorg/system") - self.requires("glfw/3.3.6") - # maybe needed for cpp examples, but not for the lib - # self.requires("tclap/1.2.4") - - def configure_cmake(self): - cmake = CMake(self) - cmake.definitions["BUILD_VIZ"] = self.options.build_viz - cmake.definitions["BUILD_PCAP"] = self.options.build_pcap - cmake.definitions["BUILD_OSF"] = self.options.build_osf - cmake.definitions["OUSTER_USE_EIGEN_MAX_ALIGN_BYTES_32"] = self.options.eigen_max_align_bytes - # alt way, but we use CMAKE_TOOLCHAIN_FILE in other pipeline so avoid overwrite - # cmake.definitions["CMAKE_TOOLCHAIN_FILE"] = os.path.join(self.build_folder, "conan_paths.cmake") - cmake.definitions[ - "CMAKE_PROJECT_ouster_example_INCLUDE"] = os.path.join( - self.build_folder, "conan_paths.cmake") - cmake.definitions["BUILD_SHARED_LIBS"] = True if self.options.shared else False - cmake.definitions["CMAKE_POSITION_INDEPENDENT_CODE"] = ( - True if "fPIC" in self.options and self.options.fPIC else False - ) - - # we use this option until we remove nonstd::optional from SDK codebase (soon) - if self.options.ensure_cpp17: - cmake.definitions["CMAKE_CXX_STANDARD"] = 17 - - self.output.info("Cmake definitions: ") - self.output.info(pformat(cmake.definitions)) - cmake.configure() - return cmake - - def build(self): - cmake = self.configure_cmake() - cmake.build() - - def package(self): - cmake = self.configure_cmake() - cmake.install() - - def package_info(self): - self.cpp_info.libs = tools.collect_libs(self) - self.cpp_info.includedirs = [ - "include", - "include/optional-lite" - ] - self.cpp_info.build_modules["cmake_find_package"].append( - "lib/cmake/OusterSDK/OusterSDKConfig.cmake" - ) - - self.cpp_info.set_property("cmake_file_name", "OusterSDK") - - self.cpp_info.filenames["cmake_find_package"] = "OusterSDK" - self.cpp_info.filenames["cmake_find_package_multi"] = "OusterSDK" - self.cpp_info.names["cmake_find_package"] = "OusterSDK" - self.cpp_info.names["cmake_find_package_multi"] = "OusterSDK" diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/Doxyfile b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/Doxyfile deleted file mode 100644 index d279d7c6..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/Doxyfile +++ /dev/null @@ -1,2603 +0,0 @@ -# Doxyfile 1.9.2 - -# This file describes the settings to be used by the documentation system -# doxygen (www.doxygen.org) for a project. -# -# All text after a double hash (##) is considered a comment and is placed in -# front of the TAG it is preceding. -# -# All text after a single hash (#) is considered a comment and will be ignored. -# The format is: -# TAG = value [value, ...] -# For lists, items can also be appended using: -# TAG += value [value, ...] -# Values that contain spaces should be placed between quotes (\" \"). - -#--------------------------------------------------------------------------- -# Project related configuration options -#--------------------------------------------------------------------------- - -# This tag specifies the encoding used for all characters in the configuration -# file that follow. The default is UTF-8 which is also the encoding used for all -# text before the first occurrence of this tag. Doxygen uses libiconv (or the -# iconv built into libc) for the transcoding. See -# https://www.gnu.org/software/libiconv/ for the list of possible encodings. -# The default value is: UTF-8. - -DOXYFILE_ENCODING = UTF-8 - -# The PROJECT_NAME tag is a single word (or a sequence of words surrounded by -# double-quotes, unless you are using Doxywizard) that should identify the -# project for which the documentation is generated. This name is used in the -# title of most generated pages and in a few other places. -# The default value is: My Project. - -PROJECT_NAME = "$project" - -# The PROJECT_NUMBER tag can be used to enter a project or revision number. This -# could be handy for archiving the generated documentation or if some version -# control system is used. - -PROJECT_NUMBER = "$version" - -# Using the PROJECT_BRIEF tag one can provide an optional one line description -# for a project that appears at the top of each page and should give viewer a -# quick idea about the purpose of the project. Keep the description short. - -PROJECT_BRIEF = - -# With the PROJECT_LOGO tag one can specify a logo or an icon that is included -# in the documentation. The maximum height of the logo should not exceed 55 -# pixels and the maximum width should not exceed 200 pixels. Doxygen will copy -# the logo to the output directory. - -PROJECT_LOGO = - -# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path -# into which the generated documentation will be written. If a relative path is -# entered, it will be relative to the location where doxygen was started. If -# left blank the current directory will be used. - -OUTPUT_DIRECTORY = $output_dir - -# If the CREATE_SUBDIRS tag is set to YES then doxygen will create 4096 sub- -# directories (in 2 levels) under the output directory of each output format and -# will distribute the generated files over these directories. Enabling this -# option can be useful when feeding doxygen a huge amount of source files, where -# putting all generated files in the same directory would otherwise causes -# performance problems for the file system. -# The default value is: NO. - -CREATE_SUBDIRS = NO - -# If the ALLOW_UNICODE_NAMES tag is set to YES, doxygen will allow non-ASCII -# characters to appear in the names of generated files. If set to NO, non-ASCII -# characters will be escaped, for example _xE3_x81_x84 will be used for Unicode -# U+3044. -# The default value is: NO. - -ALLOW_UNICODE_NAMES = NO - -# The OUTPUT_LANGUAGE tag is used to specify the language in which all -# documentation generated by doxygen is written. Doxygen will use this -# information to generate all constant output in the proper language. -# Possible values are: Afrikaans, Arabic, Armenian, Brazilian, Catalan, Chinese, -# Chinese-Traditional, Croatian, Czech, Danish, Dutch, English (United States), -# Esperanto, Farsi (Persian), Finnish, French, German, Greek, Hungarian, -# Indonesian, Italian, Japanese, Japanese-en (Japanese with English messages), -# Korean, Korean-en (Korean with English messages), Latvian, Lithuanian, -# Macedonian, Norwegian, Persian (Farsi), Polish, Portuguese, Romanian, Russian, -# Serbian, Serbian-Cyrillic, Slovak, Slovene, Spanish, Swedish, Turkish, -# Ukrainian and Vietnamese. -# The default value is: English. - -OUTPUT_LANGUAGE = English - -# If the BRIEF_MEMBER_DESC tag is set to YES, doxygen will include brief member -# descriptions after the members that are listed in the file and class -# documentation (similar to Javadoc). Set to NO to disable this. -# The default value is: YES. - -BRIEF_MEMBER_DESC = YES - -# If the REPEAT_BRIEF tag is set to YES, doxygen will prepend the brief -# description of a member or function before the detailed description -# -# Note: If both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the -# brief descriptions will be completely suppressed. -# The default value is: YES. - -REPEAT_BRIEF = YES - - -# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then -# doxygen will generate a detailed section even if there is only a brief -# description. -# The default value is: NO. - -ALWAYS_DETAILED_SEC = NO - -# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all -# inherited members of a class in the documentation of that class as if those -# members were ordinary class members. Constructors, destructors and assignment -# operators of the base classes will not be shown. -# The default value is: NO. - -INLINE_INHERITED_MEMB = NO - -# If the FULL_PATH_NAMES tag is set to YES, doxygen will prepend the full path -# before files name in the file list and in the header files. If set to NO the -# shortest path that makes the file name unique will be used -# The default value is: YES. - -FULL_PATH_NAMES = YES - -# The STRIP_FROM_PATH tag can be used to strip a user-defined part of the path. -# Stripping is only done if one of the specified strings matches the left-hand -# part of the path. The tag can be used to show relative paths in the file list. -# If left blank the directory from which doxygen is run is used as the path to -# strip. -# -# Note that you can specify absolute paths here, but also relative paths, which -# will be relative from the directory where doxygen is started. -# This tag requires that the tag FULL_PATH_NAMES is set to YES. - -STRIP_FROM_PATH = - -# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of the -# path mentioned in the documentation of a class, which tells the reader which -# header file to include in order to use a class. If left blank only the name of -# the header file containing the class definition is used. Otherwise one should -# specify the list of include paths that are normally passed to the compiler -# using the -I flag. - -STRIP_FROM_INC_PATH = - -# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter (but -# less readable) file names. This can be useful is your file systems doesn't -# support long names like on DOS, Mac, or CD-ROM. -# The default value is: NO. - -SHORT_NAMES = NO - -# If the JAVADOC_AUTOBRIEF tag is set to YES then doxygen will interpret the -# first line (until the first dot) of a Javadoc-style comment as the brief -# description. If set to NO, the Javadoc-style will behave just like regular Qt- -# style comments (thus requiring an explicit @brief command for a brief -# description.) -# The default value is: NO. - -JAVADOC_AUTOBRIEF = NO - -# If the JAVADOC_BANNER tag is set to YES then doxygen will interpret a line -# such as -# /*************** -# as being the beginning of a Javadoc-style comment "banner". If set to NO, the -# Javadoc-style will behave just like regular comments and it will not be -# interpreted by doxygen. -# The default value is: NO. - -JAVADOC_BANNER = NO - -# If the QT_AUTOBRIEF tag is set to YES then doxygen will interpret the first -# line (until the first dot) of a Qt-style comment as the brief description. If -# set to NO, the Qt-style will behave just like regular Qt-style comments (thus -# requiring an explicit \brief command for a brief description.) -# The default value is: NO. - -QT_AUTOBRIEF = NO - -# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make doxygen treat a -# multi-line C++ special comment block (i.e. a block of //! or /// comments) as -# a brief description. This used to be the default behavior. The new default is -# to treat a multi-line C++ comment block as a detailed description. Set this -# tag to YES if you prefer the old behavior instead. -# -# Note that setting this tag to YES also means that rational rose comments are -# not recognized any more. -# The default value is: NO. - -MULTILINE_CPP_IS_BRIEF = NO - -# By default Python docstrings are displayed as preformatted text and doxygen's -# special commands cannot be used. By setting PYTHON_DOCSTRING to NO the -# doxygen's special commands can be used and the contents of the docstring -# documentation blocks is shown as doxygen documentation. -# The default value is: YES. - -PYTHON_DOCSTRING = YES - -# If the INHERIT_DOCS tag is set to YES then an undocumented member inherits the -# documentation from any documented member that it re-implements. -# The default value is: YES. - -INHERIT_DOCS = YES - -# If the SEPARATE_MEMBER_PAGES tag is set to YES then doxygen will produce a new -# page for each member. If set to NO, the documentation of a member will be part -# of the file/class/namespace that contains it. -# The default value is: NO. - -SEPARATE_MEMBER_PAGES = NO - -# The TAB_SIZE tag can be used to set the number of spaces in a tab. Doxygen -# uses this value to replace tabs by spaces in code fragments. -# Minimum value: 1, maximum value: 16, default value: 4. - -TAB_SIZE = 4 - -# This tag can be used to specify a number of aliases that act as commands in -# the documentation. An alias has the form: -# name=value -# For example adding -# "sideeffect=@par Side Effects:^^" -# will allow you to put the command \sideeffect (or @sideeffect) in the -# documentation, which will result in a user-defined paragraph with heading -# "Side Effects:". Note that you cannot put \n's in the value part of an alias -# to insert newlines (in the resulting output). You can put ^^ in the value part -# of an alias to insert a newline as if a physical newline was in the original -# file. When you need a literal { or } or , in the value part of an alias you -# have to escape them by means of a backslash (\), this can lead to conflicts -# with the commands \{ and \} for these it is advised to use the version @{ and -# @} or use a double escape (\\{ and \\}) - -ALIASES = - -# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources -# only. Doxygen will then generate output that is more tailored for C. For -# instance, some of the names that are used will be different. The list of all -# members will be omitted, etc. -# The default value is: NO. - -OPTIMIZE_OUTPUT_FOR_C = NO - -# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java or -# Python sources only. Doxygen will then generate output that is more tailored -# for that language. For instance, namespaces will be presented as packages, -# qualified scopes will look different, etc. -# The default value is: NO. - -OPTIMIZE_OUTPUT_JAVA = NO - -# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran -# sources. Doxygen will then generate output that is tailored for Fortran. -# The default value is: NO. - -OPTIMIZE_FOR_FORTRAN = NO - -# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL -# sources. Doxygen will then generate output that is tailored for VHDL. -# The default value is: NO. - -OPTIMIZE_OUTPUT_VHDL = NO - -# Set the OPTIMIZE_OUTPUT_SLICE tag to YES if your project consists of Slice -# sources only. Doxygen will then generate output that is more tailored for that -# language. For instance, namespaces will be presented as modules, types will be -# separated into more groups, etc. -# The default value is: NO. - -OPTIMIZE_OUTPUT_SLICE = NO - -# Doxygen selects the parser to use depending on the extension of the files it -# parses. With this tag you can assign which parser to use for a given -# extension. Doxygen has a built-in mapping, but you can override or extend it -# using this tag. The format is ext=language, where ext is a file extension, and -# language is one of the parsers supported by doxygen: IDL, Java, JavaScript, -# Csharp (C#), C, C++, Lex, D, PHP, md (Markdown), Objective-C, Python, Slice, -# VHDL, Fortran (fixed format Fortran: FortranFixed, free formatted Fortran: -# FortranFree, unknown formatted Fortran: Fortran. In the later case the parser -# tries to guess whether the code is fixed or free formatted code, this is the -# default for Fortran type files). For instance to make doxygen treat .inc files -# as Fortran files (default is PHP), and .f files as C (default is Fortran), -# use: inc=Fortran f=C. -# -# Note: For files without extension you can use no_extension as a placeholder. -# -# Note that for custom extensions you also need to set FILE_PATTERNS otherwise -# the files are not read by doxygen. When specifying no_extension you should add -# * to the FILE_PATTERNS. -# -# Note see also the list of default file extension mappings. - -EXTENSION_MAPPING = - -# If the MARKDOWN_SUPPORT tag is enabled then doxygen pre-processes all comments -# according to the Markdown format, which allows for more readable -# documentation. See https://daringfireball.net/projects/markdown/ for details. -# The output of markdown processing is further processed by doxygen, so you can -# mix doxygen, HTML, and XML commands with Markdown formatting. Disable only in -# case of backward compatibilities issues. -# The default value is: YES. - -MARKDOWN_SUPPORT = YES - -# When the TOC_INCLUDE_HEADINGS tag is set to a non-zero value, all headings up -# to that level are automatically included in the table of contents, even if -# they do not have an id attribute. -# Note: This feature currently applies only to Markdown headings. -# Minimum value: 0, maximum value: 99, default value: 5. -# This tag requires that the tag MARKDOWN_SUPPORT is set to YES. - -TOC_INCLUDE_HEADINGS = 5 - -# When enabled doxygen tries to link words that correspond to documented -# classes, or namespaces to their corresponding documentation. Such a link can -# be prevented in individual cases by putting a % sign in front of the word or -# globally by setting AUTOLINK_SUPPORT to NO. -# The default value is: YES. - -AUTOLINK_SUPPORT = YES - -# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want -# to include (a tag file for) the STL sources as input, then you should set this -# tag to YES in order to let doxygen match functions declarations and -# definitions whose arguments contain STL classes (e.g. func(std::string); -# versus func(std::string) {}). This also make the inheritance and collaboration -# diagrams that involve STL classes more complete and accurate. -# The default value is: NO. - -BUILTIN_STL_SUPPORT = NO - -# If you use Microsoft's C++/CLI language, you should set this option to YES to -# enable parsing support. -# The default value is: NO. - -CPP_CLI_SUPPORT = NO - -# Set the SIP_SUPPORT tag to YES if your project consists of sip (see: -# https://www.riverbankcomputing.com/software/sip/intro) sources only. Doxygen -# will parse them like normal C++ but will assume all classes use public instead -# of private inheritance when no explicit protection keyword is present. -# The default value is: NO. - -SIP_SUPPORT = NO - -# For Microsoft's IDL there are propget and propput attributes to indicate -# getter and setter methods for a property. Setting this option to YES will make -# doxygen to replace the get and set methods by a property in the documentation. -# This will only work if the methods are indeed getting or setting a simple -# type. If this is not the case, or you want to show the methods anyway, you -# should set this option to NO. -# The default value is: YES. - -IDL_PROPERTY_SUPPORT = YES - -# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC -# tag is set to YES then doxygen will reuse the documentation of the first -# member in the group (if any) for the other members of the group. By default -# all members of a group must be documented explicitly. -# The default value is: NO. - -DISTRIBUTE_GROUP_DOC = NO - -# If one adds a struct or class to a group and this option is enabled, then also -# any nested class or struct is added to the same group. By default this option -# is disabled and one has to add nested compounds explicitly via \ingroup. -# The default value is: NO. - -GROUP_NESTED_COMPOUNDS = NO - -# Set the SUBGROUPING tag to YES to allow class member groups of the same type -# (for instance a group of public functions) to be put as a subgroup of that -# type (e.g. under the Public Functions section). Set it to NO to prevent -# subgrouping. Alternatively, this can be done per class using the -# \nosubgrouping command. -# The default value is: YES. - -SUBGROUPING = YES - -# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and unions -# are shown inside the group in which they are included (e.g. using \ingroup) -# instead of on a separate page (for HTML and Man pages) or section (for LaTeX -# and RTF). -# -# Note that this feature does not work in combination with -# SEPARATE_MEMBER_PAGES. -# The default value is: NO. - -INLINE_GROUPED_CLASSES = NO - -# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and unions -# with only public data fields or simple typedef fields will be shown inline in -# the documentation of the scope in which they are defined (i.e. file, -# namespace, or group documentation), provided this scope is documented. If set -# to NO, structs, classes, and unions are shown on a separate page (for HTML and -# Man pages) or section (for LaTeX and RTF). -# The default value is: NO. - -INLINE_SIMPLE_STRUCTS = NO - -# When TYPEDEF_HIDES_STRUCT tag is enabled, a typedef of a struct, union, or -# enum is documented as struct, union, or enum with the name of the typedef. So -# typedef struct TypeS {} TypeT, will appear in the documentation as a struct -# with name TypeT. When disabled the typedef will appear as a member of a file, -# namespace, or class. And the struct will be named TypeS. This can typically be -# useful for C code in case the coding convention dictates that all compound -# types are typedef'ed and only the typedef is referenced, never the tag name. -# The default value is: NO. - -TYPEDEF_HIDES_STRUCT = NO - -# The size of the symbol lookup cache can be set using LOOKUP_CACHE_SIZE. This -# cache is used to resolve symbols given their name and scope. Since this can be -# an expensive process and often the same symbol appears multiple times in the -# code, doxygen keeps a cache of pre-resolved symbols. If the cache is too small -# doxygen will become slower. If the cache is too large, memory is wasted. The -# cache size is given by this formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range -# is 0..9, the default is 0, corresponding to a cache size of 2^16=65536 -# symbols. At the end of a run doxygen will report the cache usage and suggest -# the optimal cache size from a speed point of view. -# Minimum value: 0, maximum value: 9, default value: 0. - -LOOKUP_CACHE_SIZE = 0 - -# The NUM_PROC_THREADS specifies the number threads doxygen is allowed to use -# during processing. When set to 0 doxygen will based this on the number of -# cores available in the system. You can set it explicitly to a value larger -# than 0 to get more control over the balance between CPU load and processing -# speed. At this moment only the input processing can be done using multiple -# threads. Since this is still an experimental feature the default is set to 1, -# which effectively disables parallel processing. Please report any issues you -# encounter. Generating dot graphs in parallel is controlled by the -# DOT_NUM_THREADS setting. -# Minimum value: 0, maximum value: 32, default value: 1. - -NUM_PROC_THREADS = 1 - -#--------------------------------------------------------------------------- -# Build related configuration options -#--------------------------------------------------------------------------- - -# If the EXTRACT_ALL tag is set to YES, doxygen will assume all entities in -# documentation are documented, even if no documentation was available. Private -# class members and static file members will be hidden unless the -# EXTRACT_PRIVATE respectively EXTRACT_STATIC tags are set to YES. -# Note: This will also disable the warnings about undocumented members that are -# normally produced when WARNINGS is set to YES. -# The default value is: NO. - -EXTRACT_ALL = NO - -# If the EXTRACT_PRIVATE tag is set to YES, all private members of a class will -# be included in the documentation. -# The default value is: NO. - -EXTRACT_PRIVATE = NO - -# If the EXTRACT_PRIV_VIRTUAL tag is set to YES, documented private virtual -# methods of a class will be included in the documentation. -# The default value is: NO. - -EXTRACT_PRIV_VIRTUAL = NO - -# If the EXTRACT_PACKAGE tag is set to YES, all members with package or internal -# scope will be included in the documentation. -# The default value is: NO. - -EXTRACT_PACKAGE = NO - -# If the EXTRACT_STATIC tag is set to YES, all static members of a file will be -# included in the documentation. -# The default value is: NO. - -EXTRACT_STATIC = NO - -# If the EXTRACT_LOCAL_CLASSES tag is set to YES, classes (and structs) defined -# locally in source files will be included in the documentation. If set to NO, -# only classes defined in header files are included. Does not have any effect -# for Java sources. -# The default value is: YES. - -EXTRACT_LOCAL_CLASSES = YES - -# This flag is only useful for Objective-C code. If set to YES, local methods, -# which are defined in the implementation section but not in the interface are -# included in the documentation. If set to NO, only methods in the interface are -# included. -# The default value is: NO. - -EXTRACT_LOCAL_METHODS = NO - -# If this flag is set to YES, the members of anonymous namespaces will be -# extracted and appear in the documentation as a namespace called -# 'anonymous_namespace{file}', where file will be replaced with the base name of -# the file that contains the anonymous namespace. By default anonymous namespace -# are hidden. -# The default value is: NO. - -EXTRACT_ANON_NSPACES = NO - -# If this flag is set to YES, the name of an unnamed parameter in a declaration -# will be determined by the corresponding definition. By default unnamed -# parameters remain unnamed in the output. -# The default value is: YES. - -RESOLVE_UNNAMED_PARAMS = YES - -# If the HIDE_UNDOC_MEMBERS tag is set to YES, doxygen will hide all -# undocumented members inside documented classes or files. If set to NO these -# members will be included in the various overviews, but no documentation -# section is generated. This option has no effect if EXTRACT_ALL is enabled. -# The default value is: NO. - -HIDE_UNDOC_MEMBERS = YES - -# If the HIDE_UNDOC_CLASSES tag is set to YES, doxygen will hide all -# undocumented classes that are normally visible in the class hierarchy. If set -# to NO, these classes will be included in the various overviews. This option -# has no effect if EXTRACT_ALL is enabled. -# The default value is: NO. - -HIDE_UNDOC_CLASSES = YES - -# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, doxygen will hide all friend -# declarations. If set to NO, these declarations will be included in the -# documentation. -# The default value is: NO. - -HIDE_FRIEND_COMPOUNDS = NO - -# If the HIDE_IN_BODY_DOCS tag is set to YES, doxygen will hide any -# documentation blocks found inside the body of a function. If set to NO, these -# blocks will be appended to the function's detailed documentation block. -# The default value is: NO. - -HIDE_IN_BODY_DOCS = NO - -# The INTERNAL_DOCS tag determines if documentation that is typed after a -# \internal command is included. If the tag is set to NO then the documentation -# will be excluded. Set it to YES to include the internal documentation. -# The default value is: NO. - -INTERNAL_DOCS = NO - -# With the correct setting of option CASE_SENSE_NAMES doxygen will better be -# able to match the capabilities of the underlying filesystem. In case the -# filesystem is case sensitive (i.e. it supports files in the same directory -# whose names only differ in casing), the option must be set to YES to properly -# deal with such files in case they appear in the input. For filesystems that -# are not case sensitive the option should be be set to NO to properly deal with -# output files written for symbols that only differ in casing, such as for two -# classes, one named CLASS and the other named Class, and to also support -# references to files without having to specify the exact matching casing. On -# Windows (including Cygwin) and MacOS, users should typically set this option -# to NO, whereas on Linux or other Unix flavors it should typically be set to -# YES. -# The default value is: system dependent. - -CASE_SENSE_NAMES = YES - -# If the HIDE_SCOPE_NAMES tag is set to NO then doxygen will show members with -# their full class and namespace scopes in the documentation. If set to YES, the -# scope will be hidden. -# The default value is: NO. - -HIDE_SCOPE_NAMES = NO - -# If the HIDE_COMPOUND_REFERENCE tag is set to NO (default) then doxygen will -# append additional text to a page's title, such as Class Reference. If set to -# YES the compound reference will be hidden. -# The default value is: NO. - -HIDE_COMPOUND_REFERENCE= NO - -# If the SHOW_HEADERFILE tag is set to YES then the documentation for a class -# will show which file needs to be included to use the class. -# The default value is: YES. - -#SHOW_HEADERFILE = YES - -# If the SHOW_INCLUDE_FILES tag is set to YES then doxygen will put a list of -# the files that are included by a file in the documentation of that file. -# The default value is: YES. - -#SHOW_INCLUDE_FILES = YES - -# If the SHOW_GROUPED_MEMB_INC tag is set to YES then Doxygen will add for each -# grouped member an include statement to the documentation, telling the reader -# which file to include in order to use the member. -# The default value is: NO. - -SHOW_GROUPED_MEMB_INC = NO - -# If the FORCE_LOCAL_INCLUDES tag is set to YES then doxygen will list include -# files with double quotes in the documentation rather than with sharp brackets. -# The default value is: NO. - -FORCE_LOCAL_INCLUDES = NO - -# If the INLINE_INFO tag is set to YES then a tag [inline] is inserted in the -# documentation for inline members. -# The default value is: YES. - -INLINE_INFO = YES - -# If the SORT_MEMBER_DOCS tag is set to YES then doxygen will sort the -# (detailed) documentation of file and class members alphabetically by member -# name. If set to NO, the members will appear in declaration order. -# The default value is: YES. - -SORT_MEMBER_DOCS = YES - -# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the brief -# descriptions of file, namespace and class members alphabetically by member -# name. If set to NO, the members will appear in declaration order. Note that -# this will also influence the order of the classes in the class list. -# The default value is: NO. - -SORT_BRIEF_DOCS = NO - -# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen will sort the -# (brief and detailed) documentation of class members so that constructors and -# destructors are listed first. If set to NO the constructors will appear in the -# respective orders defined by SORT_BRIEF_DOCS and SORT_MEMBER_DOCS. -# Note: If SORT_BRIEF_DOCS is set to NO this option is ignored for sorting brief -# member documentation. -# Note: If SORT_MEMBER_DOCS is set to NO this option is ignored for sorting -# detailed member documentation. -# The default value is: NO. - -SORT_MEMBERS_CTORS_1ST = NO - -# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the hierarchy -# of group names into alphabetical order. If set to NO the group names will -# appear in their defined order. -# The default value is: NO. - -SORT_GROUP_NAMES = NO - -# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be sorted by -# fully-qualified names, including namespaces. If set to NO, the class list will -# be sorted only by class name, not including the namespace part. -# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES. -# Note: This option applies only to the class list, not to the alphabetical -# list. -# The default value is: NO. - -SORT_BY_SCOPE_NAME = NO - -# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to do proper -# type resolution of all parameters of a function it will reject a match between -# the prototype and the implementation of a member function even if there is -# only one candidate or it is obvious which candidate to choose by doing a -# simple string match. By disabling STRICT_PROTO_MATCHING doxygen will still -# accept a match between prototype and implementation in such cases. -# The default value is: NO. - -STRICT_PROTO_MATCHING = NO - -# The GENERATE_TODOLIST tag can be used to enable (YES) or disable (NO) the todo -# list. This list is created by putting \todo commands in the documentation. -# The default value is: YES. - -GENERATE_TODOLIST = YES - -# The GENERATE_TESTLIST tag can be used to enable (YES) or disable (NO) the test -# list. This list is created by putting \test commands in the documentation. -# The default value is: YES. - -GENERATE_TESTLIST = YES - -# The GENERATE_BUGLIST tag can be used to enable (YES) or disable (NO) the bug -# list. This list is created by putting \bug commands in the documentation. -# The default value is: YES. - -GENERATE_BUGLIST = YES - -# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or disable (NO) -# the deprecated list. This list is created by putting \deprecated commands in -# the documentation. -# The default value is: YES. - -GENERATE_DEPRECATEDLIST= YES - -# The ENABLED_SECTIONS tag can be used to enable conditional documentation -# sections, marked by \if ... \endif and \cond -# ... \endcond blocks. - -ENABLED_SECTIONS = - -# The MAX_INITIALIZER_LINES tag determines the maximum number of lines that the -# initial value of a variable or macro / define can have for it to appear in the -# documentation. If the initializer consists of more lines than specified here -# it will be hidden. Use a value of 0 to hide initializers completely. The -# appearance of the value of individual variables and macros / defines can be -# controlled using \showinitializer or \hideinitializer command in the -# documentation regardless of this setting. -# Minimum value: 0, maximum value: 10000, default value: 30. - -MAX_INITIALIZER_LINES = 30 - -# Set the SHOW_USED_FILES tag to NO to disable the list of files generated at -# the bottom of the documentation of classes and structs. If set to YES, the -# list will mention the files that were used to generate the documentation. -# The default value is: YES. - -SHOW_USED_FILES = YES - -# Set the SHOW_FILES tag to NO to disable the generation of the Files page. This -# will remove the Files entry from the Quick Index and from the Folder Tree View -# (if specified). -# The default value is: YES. - -SHOW_FILES = YES - -# Set the SHOW_NAMESPACES tag to NO to disable the generation of the Namespaces -# page. This will remove the Namespaces entry from the Quick Index and from the -# Folder Tree View (if specified). -# The default value is: YES. - -SHOW_NAMESPACES = YES - -# The FILE_VERSION_FILTER tag can be used to specify a program or script that -# doxygen should invoke to get the current version for each file (typically from -# the version control system). Doxygen will invoke the program by executing (via -# popen()) the command command input-file, where command is the value of the -# FILE_VERSION_FILTER tag, and input-file is the name of an input file provided -# by doxygen. Whatever the program writes to standard output is used as the file -# version. For an example see the documentation. - -FILE_VERSION_FILTER = - -# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed -# by doxygen. The layout file controls the global structure of the generated -# output files in an output format independent way. To create the layout file -# that represents doxygen's defaults, run doxygen with the -l option. You can -# optionally specify a file name after the option, if omitted DoxygenLayout.xml -# will be used as the name of the layout file. See also section "Changing the -# layout of pages" for information. -# -# Note that if you run doxygen from a directory containing a file called -# DoxygenLayout.xml, doxygen will parse it automatically even if the LAYOUT_FILE -# tag is left empty. - -LAYOUT_FILE = - -# The CITE_BIB_FILES tag can be used to specify one or more bib files containing -# the reference definitions. This must be a list of .bib files. The .bib -# extension is automatically appended if omitted. This requires the bibtex tool -# to be installed. See also https://en.wikipedia.org/wiki/BibTeX for more info. -# For LaTeX the style of the bibliography can be controlled using -# LATEX_BIB_STYLE. To use this feature you need bibtex and perl available in the -# search path. See also \cite for info how to create references. - -CITE_BIB_FILES = - -#--------------------------------------------------------------------------- -# Configuration options related to warning and progress messages -#--------------------------------------------------------------------------- - -# The QUIET tag can be used to turn on/off the messages that are generated to -# standard output by doxygen. If QUIET is set to YES this implies that the -# messages are off. -# The default value is: NO. - -QUIET = NO - -# The WARNINGS tag can be used to turn on/off the warning messages that are -# generated to standard error (stderr) by doxygen. If WARNINGS is set to YES -# this implies that the warnings are on. -# -# Tip: Turn warnings on while writing the documentation. -# The default value is: YES. - -WARNINGS = YES - -# If the WARN_IF_UNDOCUMENTED tag is set to YES then doxygen will generate -# warnings for undocumented members. If EXTRACT_ALL is set to YES then this flag -# will automatically be disabled. -# The default value is: YES. - -WARN_IF_UNDOCUMENTED = YES - -# If the WARN_IF_DOC_ERROR tag is set to YES, doxygen will generate warnings for -# potential errors in the documentation, such as documenting some parameters in -# a documented function twice, or documenting parameters that don't exist or -# using markup commands wrongly. -# The default value is: YES. - -WARN_IF_DOC_ERROR = YES - -# If WARN_IF_INCOMPLETE_DOC is set to YES, doxygen will warn about incomplete -# function parameter documentation. If set to NO, doxygen will accept that some -# parameters have no documentation without warning. -# The default value is: YES. - -#WARN_IF_INCOMPLETE_DOC = YES - -# This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that -# are documented, but have no documentation for their parameters or return -# value. If set to NO, doxygen will only warn about wrong parameter -# documentation, but not about the absence of documentation. If EXTRACT_ALL is -# set to YES then this flag will automatically be disabled. See also -# WARN_IF_INCOMPLETE_DOC -# The default value is: NO. - -WARN_NO_PARAMDOC = NO - -# If the WARN_AS_ERROR tag is set to YES then doxygen will immediately stop when -# a warning is encountered. If the WARN_AS_ERROR tag is set to FAIL_ON_WARNINGS -# then doxygen will continue running as if WARN_AS_ERROR tag is set to NO, but -# at the end of the doxygen process doxygen will return with a non-zero status. -# Possible values are: NO, YES and FAIL_ON_WARNINGS. -# The default value is: NO. - -WARN_AS_ERROR = NO - - -# The WARN_LOGFILE tag can be used to specify a file to which warning and error -# messages should be written. If left blank the output is written to standard -# error (stderr). - -WARN_LOGFILE = "$warn_log_file" - -#--------------------------------------------------------------------------- -# Configuration options related to the input files -#--------------------------------------------------------------------------- - -# The INPUT tag is used to specify the files and/or directories that contain -# documented source files. You may enter file names like myfile.cpp or -# directories like /usr/src/myproject. Separate the files or directories with -# spaces. See also FILE_PATTERNS and EXTENSION_MAPPING -# Note: If this tag is empty the current directory is searched. - -INPUT = ../ouster_client \ - ../ouster_pcap \ - ../ouster_viz \ - ../ouster_osf \ - -# This tag can be used to specify the character encoding of the source files -# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses -# libiconv (or the iconv built into libc) for the transcoding. See the libiconv -# documentation (see: -# https://www.gnu.org/software/libiconv/) for the list of possible encodings. -# The default value is: UTF-8. - -INPUT_ENCODING = UTF-8 - -# If the value of the INPUT tag contains directories, you can use the -# FILE_PATTERNS tag to specify one or more wildcard patterns (like *.cpp and -# *.h) to filter out the source-files in the directories. -# -# Note that for custom extensions or not directly supported extensions you also -# need to set EXTENSION_MAPPING for the extension otherwise the files are not -# read by doxygen. -# -# Note the list of default checked file patterns might differ from the list of -# default file extension mappings. -# -# If left blank the following patterns are tested:*.c, *.cc, *.cxx, *.cpp, -# *.c++, *.java, *.ii, *.ixx, *.ipp, *.i++, *.inl, *.idl, *.ddl, *.odl, *.h, -# *.hh, *.hxx, *.hpp, *.h++, *.l, *.cs, *.d, *.php, *.php4, *.php5, *.phtml, -# *.inc, *.m, *.markdown, *.md, *.mm, *.dox (to be provided as doxygen C -# comment), *.py, *.pyw, *.f90, *.f95, *.f03, *.f08, *.f18, *.f, *.for, *.vhd, -# *.vhdl, *.ucf, *.qsf and *.ice. - -FILE_PATTERNS = *.c \ - *.cc \ - *.cxx \ - *.cpp \ - *.c++ \ - *.java \ - *.ii \ - *.ixx \ - *.ipp \ - *.i++ \ - *.inl \ - *.idl \ - *.ddl \ - *.odl \ - *.h \ - *.hh \ - *.hxx \ - *.hpp \ - *.h++ \ - *.l \ - *.cs \ - *.d \ - *.php \ - *.php4 \ - *.php5 \ - *.phtml \ - *.inc \ - *.m \ - *.markdown \ - *.md \ - *.mm \ - *.dox \ - *.py \ - *.pyw \ - *.f90 \ - *.f95 \ - *.f03 \ - *.f08 \ - *.f18 \ - *.f \ - *.for \ - *.vhd \ - *.vhdl \ - *.ucf \ - *.qsf \ - *.ice - -# The RECURSIVE tag can be used to specify whether or not subdirectories should -# be searched for input files as well. -# The default value is: NO. - -RECURSIVE = YES - -# The EXCLUDE tag can be used to specify files and/or directories that should be -# excluded from the INPUT source files. This way you can easily exclude a -# subdirectory from a directory tree whose root is specified with the INPUT tag. -# -# Note that relative paths are relative to the directory from which doxygen is -# run. - -EXCLUDE = ../ouster_client/include/optional-lite - -# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or -# directories that are symbolic links (a Unix file system feature) are excluded -# from the input. -# The default value is: NO. - -EXCLUDE_SYMLINKS = NO - -# If the value of the INPUT tag contains directories, you can use the -# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude -# certain files from those directories. -# -# Note that the wildcards are matched against the file with absolute path, so to -# exclude all test directories for example use the pattern */test/* - -EXCLUDE_PATTERNS = - -# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names -# (namespaces, classes, functions, etc.) that should be excluded from the -# output. The symbol name can be a fully qualified name, a word, or if the -# wildcard * is used, a substring. Examples: ANamespace, AClass, -# AClass::ANamespace, ANamespace::*Test -# -# Note that the wildcards are matched against the file with absolute path, so to -# exclude all test directories use the pattern */test/* - -EXCLUDE_SYMBOLS = - -# The EXAMPLE_PATH tag can be used to specify one or more files or directories -# that contain example code fragments that are included (see the \include -# command). - -EXAMPLE_PATH = - -# If the value of the EXAMPLE_PATH tag contains directories, you can use the -# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp and -# *.h) to filter out the source-files in the directories. If left blank all -# files are included. - -EXAMPLE_PATTERNS = * - -# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be -# searched for input files to be used with the \include or \dontinclude commands -# irrespective of the value of the RECURSIVE tag. -# The default value is: NO. - -EXAMPLE_RECURSIVE = NO - -# The IMAGE_PATH tag can be used to specify one or more files or directories -# that contain images that are to be included in the documentation (see the -# \image command). - -IMAGE_PATH = - -# The INPUT_FILTER tag can be used to specify a program that doxygen should -# invoke to filter for each input file. Doxygen will invoke the filter program -# by executing (via popen()) the command: -# -# -# -# where is the value of the INPUT_FILTER tag, and is the -# name of an input file. Doxygen will then use the output that the filter -# program writes to standard output. If FILTER_PATTERNS is specified, this tag -# will be ignored. -# -# Note that the filter must not add or remove lines; it is applied before the -# code is scanned, but not when the output code is generated. If lines are added -# or removed, the anchors will not be placed correctly. -# -# Note that for custom extensions or not directly supported extensions you also -# need to set EXTENSION_MAPPING for the extension otherwise the files are not -# properly processed by doxygen. - -INPUT_FILTER = - -# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern -# basis. Doxygen will compare the file name with each pattern and apply the -# filter if there is a match. The filters are a list of the form: pattern=filter -# (like *.cpp=my_cpp_filter). See INPUT_FILTER for further information on how -# filters are used. If the FILTER_PATTERNS tag is empty or if none of the -# patterns match the file name, INPUT_FILTER is applied. -# -# Note that for custom extensions or not directly supported extensions you also -# need to set EXTENSION_MAPPING for the extension otherwise the files are not -# properly processed by doxygen. - -FILTER_PATTERNS = - -# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using -# INPUT_FILTER) will also be used to filter the input files that are used for -# producing the source files to browse (i.e. when SOURCE_BROWSER is set to YES). -# The default value is: NO. - -FILTER_SOURCE_FILES = NO - -# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file -# pattern. A pattern will override the setting for FILTER_PATTERN (if any) and -# it is also possible to disable source filtering for a specific pattern using -# *.ext= (so without naming a filter). -# This tag requires that the tag FILTER_SOURCE_FILES is set to YES. - -FILTER_SOURCE_PATTERNS = - -# If the USE_MDFILE_AS_MAINPAGE tag refers to the name of a markdown file that -# is part of the input, its contents will be placed on the main page -# (index.html). This can be useful if you have a project on for instance GitHub -# and want to reuse the introduction page also for the doxygen output. - -USE_MDFILE_AS_MAINPAGE = - -#--------------------------------------------------------------------------- -# Configuration options related to source browsing -#--------------------------------------------------------------------------- - -# If the SOURCE_BROWSER tag is set to YES then a list of source files will be -# generated. Documented entities will be cross-referenced with these sources. -# -# Note: To get rid of all source code in the generated output, make sure that -# also VERBATIM_HEADERS is set to NO. -# The default value is: NO. - -SOURCE_BROWSER = NO - -# Setting the INLINE_SOURCES tag to YES will include the body of functions, -# classes and enums directly into the documentation. -# The default value is: NO. - -INLINE_SOURCES = NO - -# Setting the STRIP_CODE_COMMENTS tag to YES will instruct doxygen to hide any -# special comment blocks from generated source code fragments. Normal C, C++ and -# Fortran comments will always remain visible. -# The default value is: YES. - -STRIP_CODE_COMMENTS = YES - -# If the REFERENCED_BY_RELATION tag is set to YES then for each documented -# entity all documented functions referencing it will be listed. -# The default value is: NO. - -REFERENCED_BY_RELATION = NO - -# If the REFERENCES_RELATION tag is set to YES then for each documented function -# all documented entities called/used by that function will be listed. -# The default value is: NO. - -REFERENCES_RELATION = NO - -# If the REFERENCES_LINK_SOURCE tag is set to YES and SOURCE_BROWSER tag is set -# to YES then the hyperlinks from functions in REFERENCES_RELATION and -# REFERENCED_BY_RELATION lists will link to the source code. Otherwise they will -# link to the documentation. -# The default value is: YES. - -REFERENCES_LINK_SOURCE = YES - -# If SOURCE_TOOLTIPS is enabled (the default) then hovering a hyperlink in the -# source code will show a tooltip with additional information such as prototype, -# brief description and links to the definition and documentation. Since this -# will make the HTML file larger and loading of large files a bit slower, you -# can opt to disable this feature. -# The default value is: YES. -# This tag requires that the tag SOURCE_BROWSER is set to YES. - -SOURCE_TOOLTIPS = YES - -# If the USE_HTAGS tag is set to YES then the references to source code will -# point to the HTML generated by the htags(1) tool instead of doxygen built-in -# source browser. The htags tool is part of GNU's global source tagging system -# (see https://www.gnu.org/software/global/global.html). You will need version -# 4.8.6 or higher. -# -# To use it do the following: -# - Install the latest version of global -# - Enable SOURCE_BROWSER and USE_HTAGS in the configuration file -# - Make sure the INPUT points to the root of the source tree -# - Run doxygen as normal -# -# Doxygen will invoke htags (and that will in turn invoke gtags), so these -# tools must be available from the command line (i.e. in the search path). -# -# The result: instead of the source browser generated by doxygen, the links to -# source code will now point to the output of htags. -# The default value is: NO. -# This tag requires that the tag SOURCE_BROWSER is set to YES. - -USE_HTAGS = NO - -# If the VERBATIM_HEADERS tag is set the YES then doxygen will generate a -# verbatim copy of the header file for each class for which an include is -# specified. Set to NO to disable this. -# See also: Section \class. -# The default value is: YES. - -VERBATIM_HEADERS = YES - -#--------------------------------------------------------------------------- -# Configuration options related to the alphabetical class index -#--------------------------------------------------------------------------- - -# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index of all -# compounds will be generated. Enable this if the project contains a lot of -# classes, structs, unions or interfaces. -# The default value is: YES. - -ALPHABETICAL_INDEX = YES - -# In case all classes in a project start with a common prefix, all classes will -# be put under the same header in the alphabetical index. The IGNORE_PREFIX tag -# can be used to specify a prefix (or a list of prefixes) that should be ignored -# while generating the index headers. -# This tag requires that the tag ALPHABETICAL_INDEX is set to YES. - -IGNORE_PREFIX = - -#--------------------------------------------------------------------------- -# Configuration options related to the HTML output -#--------------------------------------------------------------------------- - -# If the GENERATE_HTML tag is set to YES, doxygen will generate HTML output -# The default value is: YES. - -GENERATE_HTML = NO - -# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. If a -# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of -# it. -# The default directory is: html. -# This tag requires that the tag GENERATE_HTML is set to YES. - -HTML_OUTPUT = html - -# The HTML_FILE_EXTENSION tag can be used to specify the file extension for each -# generated HTML page (for example: .htm, .php, .asp). -# The default value is: .html. -# This tag requires that the tag GENERATE_HTML is set to YES. - -HTML_FILE_EXTENSION = .html - -# The HTML_HEADER tag can be used to specify a user-defined HTML header file for -# each generated HTML page. If the tag is left blank doxygen will generate a -# standard header. -# -# To get valid HTML the header file that includes any scripts and style sheets -# that doxygen needs, which is dependent on the configuration options used (e.g. -# the setting GENERATE_TREEVIEW). It is highly recommended to start with a -# default header using -# doxygen -w html new_header.html new_footer.html new_stylesheet.css -# YourConfigFile -# and then modify the file new_header.html. See also section "Doxygen usage" -# for information on how to generate the default header that doxygen normally -# uses. -# Note: The header is subject to change so you typically have to regenerate the -# default header when upgrading to a newer version of doxygen. For a description -# of the possible markers and block names see the documentation. -# This tag requires that the tag GENERATE_HTML is set to YES. - -HTML_HEADER = - -# The HTML_FOOTER tag can be used to specify a user-defined HTML footer for each -# generated HTML page. If the tag is left blank doxygen will generate a standard -# footer. See HTML_HEADER for more information on how to generate a default -# footer and what special commands can be used inside the footer. See also -# section "Doxygen usage" for information on how to generate the default footer -# that doxygen normally uses. -# This tag requires that the tag GENERATE_HTML is set to YES. - -HTML_FOOTER = - -# The HTML_STYLESHEET tag can be used to specify a user-defined cascading style -# sheet that is used by each HTML page. It can be used to fine-tune the look of -# the HTML output. If left blank doxygen will generate a default style sheet. -# See also section "Doxygen usage" for information on how to generate the style -# sheet that doxygen normally uses. -# Note: It is recommended to use HTML_EXTRA_STYLESHEET instead of this tag, as -# it is more robust and this tag (HTML_STYLESHEET) will in the future become -# obsolete. -# This tag requires that the tag GENERATE_HTML is set to YES. - -HTML_STYLESHEET = - -# The HTML_EXTRA_STYLESHEET tag can be used to specify additional user-defined -# cascading style sheets that are included after the standard style sheets -# created by doxygen. Using this option one can overrule certain style aspects. -# This is preferred over using HTML_STYLESHEET since it does not replace the -# standard style sheet and is therefore more robust against future updates. -# Doxygen will copy the style sheet files to the output directory. -# Note: The order of the extra style sheet files is of importance (e.g. the last -# style sheet in the list overrules the setting of the previous ones in the -# list). For an example see the documentation. -# This tag requires that the tag GENERATE_HTML is set to YES. - -HTML_EXTRA_STYLESHEET = - - -# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen -# will adjust the colors in the style sheet and background images according to -# this color. Hue is specified as an angle on a color-wheel, see -# https://en.wikipedia.org/wiki/Hue for more information. For instance the value -# 0 represents red, 60 is yellow, 120 is green, 180 is cyan, 240 is blue, 300 -# purple, and 360 is red again. -# Minimum value: 0, maximum value: 359, default value: 220. -# This tag requires that the tag GENERATE_HTML is set to YES. - -HTML_COLORSTYLE_HUE = 220 - -# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of the colors -# in the HTML output. For a value of 0 the output will use gray-scales only. A -# value of 255 will produce the most vivid colors. -# Minimum value: 0, maximum value: 255, default value: 100. -# This tag requires that the tag GENERATE_HTML is set to YES. - -HTML_COLORSTYLE_SAT = 100 - -# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to the -# luminance component of the colors in the HTML output. Values below 100 -# gradually make the output lighter, whereas values above 100 make the output -# darker. The value divided by 100 is the actual gamma applied, so 80 represents -# a gamma of 0.8, The value 220 represents a gamma of 2.2, and 100 does not -# change the gamma. -# Minimum value: 40, maximum value: 240, default value: 80. -# This tag requires that the tag GENERATE_HTML is set to YES. - -HTML_COLORSTYLE_GAMMA = 80 - -# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML -# page will contain the date and time when the page was generated. Setting this -# to YES can help to show when doxygen was last run and thus if the -# documentation is up to date. -# The default value is: NO. -# This tag requires that the tag GENERATE_HTML is set to YES. - -HTML_TIMESTAMP = NO - -# If the HTML_DYNAMIC_MENUS tag is set to YES then the generated HTML -# documentation will contain a main index with vertical navigation menus that -# are dynamically created via JavaScript. If disabled, the navigation index will -# consists of multiple levels of tabs that are statically embedded in every HTML -# page. Disable this option to support browsers that do not have JavaScript, -# like the Qt help browser. -# The default value is: YES. -# This tag requires that the tag GENERATE_HTML is set to YES. - -HTML_DYNAMIC_MENUS = YES - -# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML -# documentation will contain sections that can be hidden and shown after the -# page has loaded. -# The default value is: NO. -# This tag requires that the tag GENERATE_HTML is set to YES. - -HTML_DYNAMIC_SECTIONS = NO - -# With HTML_INDEX_NUM_ENTRIES one can control the preferred number of entries -# shown in the various tree structured indices initially; the user can expand -# and collapse entries dynamically later on. Doxygen will expand the tree to -# such a level that at most the specified number of entries are visible (unless -# a fully collapsed tree already exceeds this amount). So setting the number of -# entries 1 will produce a full collapsed tree by default. 0 is a special value -# representing an infinite number of entries and will result in a full expanded -# tree by default. -# Minimum value: 0, maximum value: 9999, default value: 100. -# This tag requires that the tag GENERATE_HTML is set to YES. - -HTML_INDEX_NUM_ENTRIES = 100 - -# If the GENERATE_DOCSET tag is set to YES, additional index files will be -# generated that can be used as input for Apple's Xcode 3 integrated development -# environment (see: -# https://developer.apple.com/xcode/), introduced with OSX 10.5 (Leopard). To -# create a documentation set, doxygen will generate a Makefile in the HTML -# output directory. Running make will produce the docset in that directory and -# running make install will install the docset in -# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find it at -# startup. See https://developer.apple.com/library/archive/featuredarticles/Doxy -# genXcode/_index.html for more information. -# The default value is: NO. -# This tag requires that the tag GENERATE_HTML is set to YES. - -GENERATE_DOCSET = NO - -# This tag determines the name of the docset feed. A documentation feed provides -# an umbrella under which multiple documentation sets from a single provider -# (such as a company or product suite) can be grouped. -# The default value is: Doxygen generated docs. -# This tag requires that the tag GENERATE_DOCSET is set to YES. - -DOCSET_FEEDNAME = "Doxygen generated docs" - -# This tag specifies a string that should uniquely identify the documentation -# set bundle. This should be a reverse domain-name style string, e.g. -# com.mycompany.MyDocSet. Doxygen will append .docset to the name. -# The default value is: org.doxygen.Project. -# This tag requires that the tag GENERATE_DOCSET is set to YES. - -DOCSET_BUNDLE_ID = org.doxygen.Project - -# The DOCSET_PUBLISHER_ID tag specifies a string that should uniquely identify -# the documentation publisher. This should be a reverse domain-name style -# string, e.g. com.mycompany.MyDocSet.documentation. -# The default value is: org.doxygen.Publisher. -# This tag requires that the tag GENERATE_DOCSET is set to YES. - -DOCSET_PUBLISHER_ID = org.doxygen.Publisher - -# The DOCSET_PUBLISHER_NAME tag identifies the documentation publisher. -# The default value is: Publisher. -# This tag requires that the tag GENERATE_DOCSET is set to YES. - -DOCSET_PUBLISHER_NAME = Publisher - -# If the GENERATE_HTMLHELP tag is set to YES then doxygen generates three -# additional HTML index files: index.hhp, index.hhc, and index.hhk. The -# index.hhp is a project file that can be read by Microsoft's HTML Help Workshop -# on Windows. In the beginning of 2021 Microsoft took the original page, with -# a.o. the download links, offline the HTML help workshop was already many years -# in maintenance mode). You can download the HTML help workshop from the web -# archives at Installation executable (see: -# http://web.archive.org/web/20160201063255/http://download.microsoft.com/downlo -# ad/0/A/9/0A939EF6-E31C-430F-A3DF-DFAE7960D564/htmlhelp.exe). -# -# The HTML Help Workshop contains a compiler that can convert all HTML output -# generated by doxygen into a single compiled HTML file (.chm). Compiled HTML -# files are now used as the Windows 98 help format, and will replace the old -# Windows help format (.hlp) on all Windows platforms in the future. Compressed -# HTML files also contain an index, a table of contents, and you can search for -# words in the documentation. The HTML workshop also contains a viewer for -# compressed HTML files. -# The default value is: NO. -# This tag requires that the tag GENERATE_HTML is set to YES. - -GENERATE_HTMLHELP = NO - -# The CHM_FILE tag can be used to specify the file name of the resulting .chm -# file. You can add a path in front of the file if the result should not be -# written to the html output directory. -# This tag requires that the tag GENERATE_HTMLHELP is set to YES. - -CHM_FILE = - -# The HHC_LOCATION tag can be used to specify the location (absolute path -# including file name) of the HTML help compiler (hhc.exe). If non-empty, -# doxygen will try to run the HTML help compiler on the generated index.hhp. -# The file has to be specified with full path. -# This tag requires that the tag GENERATE_HTMLHELP is set to YES. - -HHC_LOCATION = - -# The GENERATE_CHI flag controls if a separate .chi index file is generated -# (YES) or that it should be included in the main .chm file (NO). -# The default value is: NO. -# This tag requires that the tag GENERATE_HTMLHELP is set to YES. - -GENERATE_CHI = NO - -# The CHM_INDEX_ENCODING is used to encode HtmlHelp index (hhk), content (hhc) -# and project file content. -# This tag requires that the tag GENERATE_HTMLHELP is set to YES. - -CHM_INDEX_ENCODING = - -# The BINARY_TOC flag controls whether a binary table of contents is generated -# (YES) or a normal table of contents (NO) in the .chm file. Furthermore it -# enables the Previous and Next buttons. -# The default value is: NO. -# This tag requires that the tag GENERATE_HTMLHELP is set to YES. - -BINARY_TOC = NO - -# The TOC_EXPAND flag can be set to YES to add extra items for group members to -# the table of contents of the HTML help documentation and to the tree view. -# The default value is: NO. -# This tag requires that the tag GENERATE_HTMLHELP is set to YES. - -TOC_EXPAND = NO - -# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and -# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated that -# can be used as input for Qt's qhelpgenerator to generate a Qt Compressed Help -# (.qch) of the generated HTML documentation. -# The default value is: NO. -# This tag requires that the tag GENERATE_HTML is set to YES. - -GENERATE_QHP = NO - -# If the QHG_LOCATION tag is specified, the QCH_FILE tag can be used to specify -# the file name of the resulting .qch file. The path specified is relative to -# the HTML output folder. -# This tag requires that the tag GENERATE_QHP is set to YES. - -QCH_FILE = - -# The QHP_NAMESPACE tag specifies the namespace to use when generating Qt Help -# Project output. For more information please see Qt Help Project / Namespace -# (see: -# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#namespace). -# The default value is: org.doxygen.Project. -# This tag requires that the tag GENERATE_QHP is set to YES. - -QHP_NAMESPACE = org.doxygen.Project - -# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating Qt -# Help Project output. For more information please see Qt Help Project / Virtual -# Folders (see: -# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#virtual-folders). -# The default value is: doc. -# This tag requires that the tag GENERATE_QHP is set to YES. - -QHP_VIRTUAL_FOLDER = doc - -# If the QHP_CUST_FILTER_NAME tag is set, it specifies the name of a custom -# filter to add. For more information please see Qt Help Project / Custom -# Filters (see: -# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#custom-filters). -# This tag requires that the tag GENERATE_QHP is set to YES. - -QHP_CUST_FILTER_NAME = - -# The QHP_CUST_FILTER_ATTRS tag specifies the list of the attributes of the -# custom filter to add. For more information please see Qt Help Project / Custom -# Filters (see: -# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#custom-filters). -# This tag requires that the tag GENERATE_QHP is set to YES. - -QHP_CUST_FILTER_ATTRS = - -# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this -# project's filter section matches. Qt Help Project / Filter Attributes (see: -# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#filter-attributes). -# This tag requires that the tag GENERATE_QHP is set to YES. - -QHP_SECT_FILTER_ATTRS = - -# The QHG_LOCATION tag can be used to specify the location (absolute path -# including file name) of Qt's qhelpgenerator. If non-empty doxygen will try to -# run qhelpgenerator on the generated .qhp file. -# This tag requires that the tag GENERATE_QHP is set to YES. - -QHG_LOCATION = - -# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files will be -# generated, together with the HTML files, they form an Eclipse help plugin. To -# install this plugin and make it available under the help contents menu in -# Eclipse, the contents of the directory containing the HTML and XML files needs -# to be copied into the plugins directory of eclipse. The name of the directory -# within the plugins directory should be the same as the ECLIPSE_DOC_ID value. -# After copying Eclipse needs to be restarted before the help appears. -# The default value is: NO. -# This tag requires that the tag GENERATE_HTML is set to YES. - -GENERATE_ECLIPSEHELP = NO - -# A unique identifier for the Eclipse help plugin. When installing the plugin -# the directory name containing the HTML and XML files should also have this -# name. Each documentation set should have its own identifier. -# The default value is: org.doxygen.Project. -# This tag requires that the tag GENERATE_ECLIPSEHELP is set to YES. - -ECLIPSE_DOC_ID = org.doxygen.Project - -# If you want full control over the layout of the generated HTML pages it might -# be necessary to disable the index and replace it with your own. The -# DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) at top -# of each HTML page. A value of NO enables the index and the value YES disables -# it. Since the tabs in the index contain the same information as the navigation -# tree, you can set this option to YES if you also set GENERATE_TREEVIEW to YES. -# The default value is: NO. -# This tag requires that the tag GENERATE_HTML is set to YES. - -DISABLE_INDEX = NO - -# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index -# structure should be generated to display hierarchical information. If the tag -# value is set to YES, a side panel will be generated containing a tree-like -# index structure (just like the one that is generated for HTML Help). For this -# to work a browser that supports JavaScript, DHTML, CSS and frames is required -# (i.e. any modern browser). Windows users are probably better off using the -# HTML help feature. Via custom style sheets (see HTML_EXTRA_STYLESHEET) one can -# further fine tune the look of the index (see "Fine-tuning the output"). As an -# example, the default style sheet generated by doxygen has an example that -# shows how to put an image at the root of the tree instead of the PROJECT_NAME. -# Since the tree basically has the same information as the tab index, you could -# consider setting DISABLE_INDEX to YES when enabling this option. -# The default value is: NO. -# This tag requires that the tag GENERATE_HTML is set to YES. - -GENERATE_TREEVIEW = NO - -# When both GENERATE_TREEVIEW and DISABLE_INDEX are set to YES, then the -# FULL_SIDEBAR option determines if the side bar is limited to only the treeview -# area (value NO) or if it should extend to the full height of the window (value -# YES). Setting this to YES gives a layout similar to -# https://docs.readthedocs.io with more room for contents, but less room for the -# project logo, title, and description. If either GENERATOR_TREEVIEW or -# DISABLE_INDEX is set to NO, this option has no effect. -# The default value is: NO. -# This tag requires that the tag GENERATE_HTML is set to YES. - -#FULL_SIDEBAR = NO - -# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values that -# doxygen will group on one line in the generated HTML documentation. -# -# Note that a value of 0 will completely suppress the enum values from appearing -# in the overview section. -# Minimum value: 0, maximum value: 20, default value: 4. -# This tag requires that the tag GENERATE_HTML is set to YES. - -ENUM_VALUES_PER_LINE = 4 - -# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be used -# to set the initial width (in pixels) of the frame in which the tree is shown. -# Minimum value: 0, maximum value: 1500, default value: 250. -# This tag requires that the tag GENERATE_HTML is set to YES. - -TREEVIEW_WIDTH = 250 - -# If the EXT_LINKS_IN_WINDOW option is set to YES, doxygen will open links to -# external symbols imported via tag files in a separate window. -# The default value is: NO. -# This tag requires that the tag GENERATE_HTML is set to YES. - -EXT_LINKS_IN_WINDOW = NO - -# If the HTML_FORMULA_FORMAT option is set to svg, doxygen will use the pdf2svg -# tool (see https://github.com/dawbarton/pdf2svg) or inkscape (see -# https://inkscape.org) to generate formulas as SVG images instead of PNGs for -# the HTML output. These images will generally look nicer at scaled resolutions. -# Possible values are: png (the default) and svg (looks nicer but requires the -# pdf2svg or inkscape tool). -# The default value is: png. -# This tag requires that the tag GENERATE_HTML is set to YES. - -HTML_FORMULA_FORMAT = png - -# Use this tag to change the font size of LaTeX formulas included as images in -# the HTML documentation. When you change the font size after a successful -# doxygen run you need to manually remove any form_*.png images from the HTML -# output directory to force them to be regenerated. -# Minimum value: 8, maximum value: 50, default value: 10. -# This tag requires that the tag GENERATE_HTML is set to YES. - -FORMULA_FONTSIZE = 10 - -# Use the FORMULA_TRANSPARENT tag to determine whether or not the images -# generated for formulas are transparent PNGs. Transparent PNGs are not -# supported properly for IE 6.0, but are supported on all modern browsers. -# -# Note that when changing this option you need to delete any form_*.png files in -# the HTML output directory before the changes have effect. -# The default value is: YES. -# This tag requires that the tag GENERATE_HTML is set to YES. - -FORMULA_TRANSPARENT = YES - -# The FORMULA_MACROFILE can contain LaTeX \newcommand and \renewcommand commands -# to create new LaTeX commands to be used in formulas as building blocks. See -# the section "Including formulas" for details. - -FORMULA_MACROFILE = - -# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax (see -# https://www.mathjax.org) which uses client side JavaScript for the rendering -# instead of using pre-rendered bitmaps. Use this if you do not have LaTeX -# installed or if you want to formulas look prettier in the HTML output. When -# enabled you may also need to install MathJax separately and configure the path -# to it using the MATHJAX_RELPATH option. -# The default value is: NO. -# This tag requires that the tag GENERATE_HTML is set to YES. - -USE_MATHJAX = NO - -# With MATHJAX_VERSION it is possible to specify the MathJax version to be used. -# Note that the different versions of MathJax have different requirements with -# regards to the different settings, so it is possible that also other MathJax -# settings have to be changed when switching between the different MathJax -# versions. -# Possible values are: MathJax_2 and MathJax_3. -# The default value is: MathJax_2. -# This tag requires that the tag USE_MATHJAX is set to YES. - -#MATHJAX_VERSION = MathJax_2 - -# When MathJax is enabled you can set the default output format to be used for -# the MathJax output. For more details about the output format see MathJax -# version 2 (see: -# http://docs.mathjax.org/en/v2.7-latest/output.html) and MathJax version 3 -# (see: -# http://docs.mathjax.org/en/latest/web/components/output.html). -# Possible values are: HTML-CSS (which is slower, but has the best -# compatibility. This is the name for Mathjax version 2, for MathJax version 3 -# this will be translated into chtml), NativeMML (i.e. MathML. Only supported -# for NathJax 2. For MathJax version 3 chtml will be used instead.), chtml (This -# is the name for Mathjax version 3, for MathJax version 2 this will be -# translated into HTML-CSS) and SVG. -# The default value is: HTML-CSS. -# This tag requires that the tag USE_MATHJAX is set to YES. - -MATHJAX_FORMAT = HTML-CSS - -# When MathJax is enabled you need to specify the location relative to the HTML -# output directory using the MATHJAX_RELPATH option. The destination directory -# should contain the MathJax.js script. For instance, if the mathjax directory -# is located at the same level as the HTML output directory, then -# MATHJAX_RELPATH should be ../mathjax. The default value points to the MathJax -# Content Delivery Network so you can quickly see the result without installing -# MathJax. However, it is strongly recommended to install a local copy of -# MathJax from https://www.mathjax.org before deployment. The default value is: -# - in case of MathJax version 2: https://cdn.jsdelivr.net/npm/mathjax@2 -# - in case of MathJax version 3: https://cdn.jsdelivr.net/npm/mathjax@3 -# This tag requires that the tag USE_MATHJAX is set to YES. - -MATHJAX_RELPATH = - -# The MATHJAX_EXTENSIONS tag can be used to specify one or more MathJax -# extension names that should be enabled during MathJax rendering. For example -# for MathJax version 2 (see -# https://docs.mathjax.org/en/v2.7-latest/tex.html#tex-and-latex-extensions): -# MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols -# For example for MathJax version 3 (see -# http://docs.mathjax.org/en/latest/input/tex/extensions/index.html): -# MATHJAX_EXTENSIONS = ams -# This tag requires that the tag USE_MATHJAX is set to YES. - -MATHJAX_EXTENSIONS = - -# The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces -# of code that will be used on startup of the MathJax code. See the MathJax site -# (see: -# http://docs.mathjax.org/en/v2.7-latest/output.html) for more details. For an -# example see the documentation. -# This tag requires that the tag USE_MATHJAX is set to YES. - -MATHJAX_CODEFILE = - -# When the SEARCHENGINE tag is enabled doxygen will generate a search box for -# the HTML output. The underlying search engine uses javascript and DHTML and -# should work on any modern browser. Note that when using HTML help -# (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets (GENERATE_DOCSET) -# there is already a search function so this one should typically be disabled. -# For large projects the javascript based search engine can be slow, then -# enabling SERVER_BASED_SEARCH may provide a better solution. It is possible to -# search using the keyboard; to jump to the search box use + S -# (what the is depends on the OS and browser, but it is typically -# , /` section, we will use -``$SAMPLE_DATA_PCAP_PATH`` and ``$SAMPLE_DATA_JSON_PATH`` for the locations of the sample data pcap -and json. For Python code, ``pcap_path`` and ``json_path`` are taken to be set to those values, -respectively: - -.. code:: python - - pcap_path = '' - metadata_path = '' - -Similarly, ``$SENSOR_HOSTNAME`` is used for your sensor's hostname. - -.. toctree:: - - basics-sensor - udp-packets - lidar-scan - record-stream - visualizations - conversion - osf-examples - - diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/python/examples/lidar-scan.rst b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/python/examples/lidar-scan.rst deleted file mode 100644 index 7b7c25ae..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/python/examples/lidar-scan.rst +++ /dev/null @@ -1,162 +0,0 @@ -.. _ex-python-lidarscan: - -============================ -The LidarScan Representation -============================ - -.. contents:: - :local: - :depth: 3 - -The :py:class:`.LidarScan` class is explained in depth in the :doc:`/reference/lidar-scan`, which we recommend reading. - -We provide example code to aid in understanding. - -To run the sample code which queries fields from a scan, use the :py:func:`.pcap.pcap_query_scan` example: - -.. tabs:: - - .. code-tab:: console Linux/macOS - - $ python3 -m ouster.sdk.examples.pcap $SAMPLE_DATA_PCAP_PATH $SAMPLE_DATA_JSON_PATH query-scan - - .. code-tab:: powershell Windows x64 - - PS > py -3 -m ouster.sdk.examples.pcap $SAMPLE_DATA_PCAP_PATH $SAMPLE_DATA_JSON_PATH query-scan - -You can run the above on pcaps containing packets of any type of :py:class:`.UDPProfileLidar`. - -You might notice that on a pcap containing dual returns data, you should note -that your ``dtypes`` will not be consistently ``uint32_t``, as you can tell -from the result of running ``query-scan`` on dual returns data: - -.. - [start-query-scan-result] - -.. code:: none - - Available fields and corresponding dtype in LidarScan - RANGE uint32 - RANGE2 uint32 - SIGNAL uint16 - SIGNAL2 uint16 - REFLECTIVITY uint8 - REFLECTIVITY2 uint8 - NEAR_IR uint16 - -.. - [end-query-scan-result] - -.. _ex-staggered-and-destaggered: - -Staggered vs Destaggered 2D Representations -=========================================== - -To generate staggered and destaggered images yourself, you can try the following sample code: - -.. code:: python - - import matplotlib.pyplot as plt - from more_itertools import nth - - # ... `metadata` and `source` variables created as in the previous examples - - scans = client.Scans(source) - - # iterate `scans` and get the 84th LidarScan (it can be different with your data) - scan = nth(scans, 84) - ranges = scan.field(client.ChanField.RANGE) - - # destagger ranges, notice `metadata` use, that is needed to get - # sensor intrinsics and correctly data transforms - ranges_destaggered = client.destagger(source.metadata, ranges) - - plt.imshow(ranges_destaggered, cmap='gray', resample=False) - -.. todo:: - (Kai) Might be nice to cover either here or in the reference how to - duplicate timestamps into an 'img', "destagger" it, and then use for for - association of XYZ points with their timestamps - - -.. _ex-xyzlut: - - -Projecting into Cartesian Coordinates -====================================== - -Let's plot some points! - -If you have a :ref:`configured` sensor, you can plot some points in XYZ using the :py:func:`.client.plot_xyz_points` example: - -.. tabs:: - - .. code-tab:: console Linux/macOS - - $ python3 -m ouster.sdk.examples.client $SENSOR_HOSTNAME plot-xyz-points - - .. code-tab:: powershell Windows x64 - - PS > py -3 -m ouster.sdk.examples.client $SENSOR_HOSTNAME plot-xyz-points - -That should open a 3D plot of a single scan of your location taken just now by -your sensor. You should be able to recognize the contours of the scene around -you. - -If you don’t have a sensor, you can run the same code with our -:py:func:`.pcap.pcap_display_xyz_points` pcap example: - -.. tabs:: - - .. code-tab:: console Linux/macOS - - $ python3 -m ouster.sdk.examples.pcap $SAMPLE_DATA_PCAP_PATH $SAMPLE_DATA_JSON_PATH plot-xyz-points --scan-num 84 - - .. code-tab:: powershell Windows x64 - - PS > py -3 -m ouster.sdk.examples.pcap $SAMPLE_DATA_PCAP_PATH $SAMPLE_DATA_JSON_PATH plot-xyz-points --scan-num 84 - -For visualizers which will stream consecutive frames from sensors or pcaps, -check out our utilities in :doc:`visualizations`. - -.. _ex-correlating-2d-and-3d: - - -Working with 2D and 3D Representations Simultaneously -====================================================== - -The direct correlation between 2D and 3D representations in an Ouster sensor provides a powerful -framework for working with the data. As an easy example, you might decide you want to look at only -the 3D points within a certain range and from certain azimuth angles. - -.. literalinclude:: /../python/src/ouster/sdk/examples/client.py - :start-after: [doc-stag-filter-3d] - :end-before: [doc-etag-filter-3d] - :emphasize-lines: 10-11, 15 - :linenos: - :dedent: - -Since we'd like to filter on azimuth angles, first we first destagger both the 2D and 3D points, so -that our columns in the ``HxW`` representation correspond to azimuth angle, not timestamp. (See -:ref:`ex-staggered-and-destaggered` for an explanation on destaggering.) - -Then we filter the 3D points ``xyz_destaggered`` by comparing the range measurement to -``range_min``, which we can do because there is a 1:1 correspondence between the columns and rows of -the destaggered representations of ``xyz_destaggered`` and ``range_destaggered``. (Similarly, there -would be a 1:1 correspondence between the staggered representations ``xyz`` and ``range``, where the -columns correspond with timestamp). - -Finally, we select only the azimuth columns we're interested in. In this case, we've arbitrarily -chosen the first 270 degrees of rotation. - -If you have a :ref:`configured` sensor, you can run this code with an example: - -.. tabs:: - - .. code-tab:: console Linux/macOS - - $ python3 -m ouster.sdk.examples.client $SENSOR_HOSTNAME filter-3d-by-range-and-azimuth - - .. code-tab:: powershell Windows x64 - - PS > py -3 -m ouster.sdk.examples.client $SENSOR_HOSTNAME filter-3d-by-range-and-azimuth diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/python/examples/osf-examples.rst b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/python/examples/osf-examples.rst deleted file mode 100644 index 1f0087cf..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/python/examples/osf-examples.rst +++ /dev/null @@ -1,160 +0,0 @@ -=================== -OSF Python Examples -=================== - -.. contents:: - :local: - :depth: 3 - -Ouster Python API for OSF -^^^^^^^^^^^^^^^^^^^^^^^^^ - -Python OSF Reader/Writer API is a Python binding to the ``C++`` OSF Reader/Writer implementation -which means that all reading and writing operations works at native speeds. - - -All examples below assume that a user has an ``osf_file`` variable with a path to an OSF file and -``ouster.osf`` package is imported: - -.. code:: - - import ouster.osf as osf - - osf_file = 'path/to/osf_file.osf' - -You can use ``ouster-cli source .... save`` commands to generate a test OSF file to test any of the examples. - -Every example is wrapped into a CLI and available for quick tests by running -``python3 -m ouster.sdk.examples.osf ``: - -.. code:: bash - - $ python3 -m ouster.sdk.examples.osf --help - - usage: osf.py [-h] [--scan-num SCAN_NUM] OSF EXAMPLE - - Ouster Python SDK OSF examples. The EXAMPLE must be one of: - read-scans - read-messages - split-scans - slice-scans - get-lidar-streams - get-sensors-info - check-layout - -For example to execute the ``get-lidar-streams`` example you can run: - -.. code:: bash - - $ python3 -m ouster.sdk.examples.osf get-lidar-streams - - -Read Lidar Scans with ``osf.Scans`` -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -``osf.Scans()`` interface is the simplest way to get all ``LidarScan`` objects for the first sensor -that was found in an OSF (majority of our test data uses only a single sensor recordings): - -.. literalinclude:: /../python/src/ouster/sdk/examples/osf.py - :start-after: [doc-stag-osf-read-scans] - :end-before: [doc-etag-osf-read-scans] - :dedent: - -Underneath it looks for available sensor streams, peeks first, creates the ``osf.Reader``, reads the -**messages** and decodes them to ``LidarScan`` objects. - -.. admonition:: Note about timestamp ``ts`` - - All messages in an OSF are stored with a timestamp so it's an essential part of the stream - during the read operation. If later you will decide to store the post-processed ``LidarScan`` - back into another OSF it's better to preserve the original ``ts`` which usually came from - NIC/PCAP/BAG headers. To get ``ts`` along with ``LidarScan`` use ``osf.Scans().withTs()`` - iterator. - -Get Sensors Info with ``osf.Reader`` -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -``osf.Reader`` is the base ``Reader`` interface that get info about ``start/end_ts``, reads and -decodes all **metadata entries**, get access to **chunks** and **messages** of the OSF file. - -Sensors information is stored as ``osf.LidarSensor`` metadata entry and can be read with the -``reader.meta_store.find()`` function that returns all metadata entry of the specified type (in our -case it's of type ``osf.LidarSensor``): - -.. literalinclude:: /../python/src/ouster/sdk/examples/osf.py - :start-after: [doc-stag-osf-get-sensors-info] - :end-before: [doc-etag-osf-get-sensors-info] - :dedent: - - -Read All Messages with ``osf.Reader`` -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -With ``osf.Reader``, you can use ``reader.messages()`` iterator to read messages in ``timestamp`` -order. - -.. literalinclude:: /../python/src/ouster/sdk/examples/osf.py - :start-after: [doc-stag-osf-read-all-messages] - :end-before: [doc-etag-osf-read-all-messages] - :dedent: - - -Checking Chunks Layout via ``osf.StreamingInfo`` -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -Building on top of an example from above we can check for stream -statistics information from ``osf.StreamingInfo``: - -.. literalinclude:: /../python/src/ouster/sdk/examples/osf.py - :start-after: [doc-stag-osf-check-layout] - :end-before: [doc-etag-osf-check-layout] - :dedent: - -For more information about ``osf.StreamingInfo`` metadata entry please refer to [RFC 0018]_. - - -Get Lidar Scan streams info via ``osf.LidarScanStream`` -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -Every message in an OSF belongs to a stream of a particular type (i.e. ``osf.LidarScanStream``, -``osf.LidarImuStream``, etc.). Streams information stored as **metadata entry** within -``osf.Reader.meta_store`` object that can be read and decoded in various ways. Below is an example -of how we can check parameters of an available LidarScan streams (``osf.LidarScanStream``) by -checking the metadata entries: - -.. literalinclude:: /../python/src/ouster/sdk/examples/osf.py - :start-after: [doc-stag-osf-get-lidar-streams] - :end-before: [doc-etag-osf-get-lidar-streams] - :dedent: - - -Write Lidar Scan with sliced fields with ``osf.Writer`` -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -We will look into the ``osf.Writer`` example on the task of re-coding the available OSF file into Lidar -Scans with a reduced fields. By reduce fields we mean here that if LidarScan has ``7`` channel -fields, we can keep only ``3`` and save the disk space and bandwidth during replay. - -A general scheme of writing scans to the OSF with Writer: - -0. Create ``osf.Writer`` with the output file name, lidar metadata(s) (``ouster.sdk.client.SensorInfo``) and optionally the desired output scan fields. -1. Use the writers's ``save`` function ``writer.save(index, scan)`` to encode the LidarScan ``scan`` into the - underlying message buffer for lidar ``index`` and finally push it to disk. If you have multiple lidars you can - save the scans simultaneously by providing them in an array to ``writer.save``. - -.. literalinclude:: /../python/src/ouster/sdk/examples/osf.py - :start-after: [doc-stag-osf-slice-scans] - :end-before: [doc-etag-osf-slice-scans] - :dedent: - - -Split Lidar Scan stream into multiple files -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -Another example of using ``osf.Writer`` that we will see is the splitting of Lidar Scan stream from -one OSF file into 2 files. - -.. literalinclude:: /../python/src/ouster/sdk/examples/osf.py - :start-after: [doc-stag-osf-split-scans] - :end-before: [doc-etag-osf-split-scans] - :dedent: diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/python/examples/record-stream.rst b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/python/examples/record-stream.rst deleted file mode 100644 index 444897e8..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/python/examples/record-stream.rst +++ /dev/null @@ -1,78 +0,0 @@ -.. _ex-record-stream-viz: - -===================================== -Recording, Streaming, and Conversion -===================================== - -.. contents:: - :local: - :depth: 3 - - -Recording Sensor Data -====================== - -It's easy to record data to a pcap file from a sensor programatically. Let's try it on a -:ref:`configured` sensor: - -.. tabs:: - - .. code-tab:: console Linux/macOS - - $ python3 -m ouster.sdk.examples.client $SENSOR_HOSTNAME record-pcap - - .. code-tab:: powershell Windows x64 - - PS > py -3 -m ouster.sdk.examples.client $SENSOR_HOSTNAME record-pcap - - -This will capture the :class:`.client.LidarPacket`'s and :class:`.client.ImuPacket`'s data for 10 -seconds and store the pcap file along with the metadata json file into the current directory. - -The source code of an example below: - -.. literalinclude:: /../python/src/ouster/sdk/examples/client.py - :start-after: [doc-stag-pcap-record] - :end-before: [doc-etag-pcap-record] - :emphasize-lines: 15 - :linenos: - :dedent: - -Good! The resulting pcap and json files can be used with any examples in the :mod:`.examples.pcap` -module. - -.. _ex-stream: - -Streaming Live Data -==================== - -Instead of working with a recorded dataset or a few captured frames of data, let's see if we can get -a live feed from your :ref:`configured` sensor: - -.. tabs:: - - .. code-tab:: console Linux/macOS - - $ python3 -m ouster.sdk.examples.client $SENSOR_HOSTNAME live-plot-reflectivity - - .. code-tab:: powershell Windows x64 - - PS > py -3 -m ouster.sdk.examples.client $SENSOR_HOSTNAME live-plot-reflectivity - -This should give you a live feed from your sensor that looks like a black and white moving image. -Try waving your hand or moving around to find yourself within the image! - -So how did we do that? - -.. literalinclude:: /../python/src/ouster/sdk/examples/client.py - :start-after: [doc-stag-live-plot-reflectivity] - :end-before: [doc-etag-live-plot-reflectivity] - :emphasize-lines: 2-3 - :linenos: - :dedent: - -Notice that instead of taking a ``sample``, we used :py:meth:`.Scans.stream`, which allows for a -continuous live data stream. We close the ``stream`` when we are finished, hence the use of -:py:func:`.closing` in the highlighted line. - -To exit the visualization, you can use ``ESC``. diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/python/examples/udp-packets.rst b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/python/examples/udp-packets.rst deleted file mode 100644 index 9a489a41..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/python/examples/udp-packets.rst +++ /dev/null @@ -1,39 +0,0 @@ -.. _ex-packets: - -===================== -Lidar and IMU Packets -===================== - - -The :py:class:`.PacketSource` is the basic interface for sensor packets. It can be advantageous to -work with packets directly when latency is a concern, or when you wish to examine the data packet by -packet, e.g., if you wish to examine timestamps of packets. - -Let's make a :py:class:`.PacketSource` from our sample data using :py:class:`.pcap.Pcap`: - -.. code:: python - - with open(metadata_path, 'r') as f: - metadata = client.SensorInfo(f.read()) - - source = pcap.Pcap(pcap_path, metadata) - -Now we can read packets from ``source`` with the following code: - -.. literalinclude:: /../python/src/ouster/sdk/examples/pcap.py - :start-after: [doc-stag-pcap-read-packets] - :end-before: [doc-etag-pcap-read-packets] - :dedent: - -You can try the above code with the :py:func:`~.pcap.pcap_read_packets`: - -.. tabs:: - - .. code-tab:: console Linux/macOS - - $ python3 -m ouster.sdk.examples.pcap $SAMPLE_DATA_PCAP_PATH $SAMPLE_DATA_JSON_PATH read-packets - - .. code-tab:: powershell Windows x64 - - PS > py -3 -m ouster.sdk.examples.pcap $SAMPLE_DATA_PCAP_PATH $SAMPLE_DATA_JSON_PATH read-packets - diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/python/examples/visualizations.rst b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/python/examples/visualizations.rst deleted file mode 100644 index d5e353df..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/python/examples/visualizations.rst +++ /dev/null @@ -1,239 +0,0 @@ -===================== -Visualizations in 3D -===================== - -The Ouster Python SDK provides two visualization utilities for user convenience. These are -introduced briefly below. - -.. contents:: - :local: - :depth: 3 - -.. _ex-ouster-viz: - - -Visualization with Ouster's SDK CLI ``ouster-cli`` -================================================== - -Ouster's OpenGL-based visualizer allows for easy visualization from pcaps and sensors on all -platforms the Ouster SDK supports. - -The default Ouster SDK CLI ``ouster-cli source viz`` visualizer view includes -two 2D range images atop which can be cycled through the available fields, and a 3D point cloud on -the bottom. For dual returns sensors, both returns are displayed by default. - -.. figure:: /images/ouster-viz.png - :align: center - - Ouster SDK CLI ``ouster-cli source viz`` visualization of OS1 128 Rev 7 sample data - -The visualizer can be controlled with mouse and keyboard: - -.. include:: /python/viz/viz-run.rst - :start-after: [start-simple-viz-keymap] - :end-before: [end-simple-viz-keymap] - -To run the visualizer with a sensor:: - - $ ouster-cli source $SENSOR_HOSTNAME viz - -This will auto-configure the udp destination of the sensor while leaving the lidar port as -previously set on the sensor. - -To run the visualizer with a pcap:: - - $ ouster-cli source $SAMPLE_DATA_PCAP_PATH [--meta $SAMPLE_DATA_JSON_PATH] viz - - -Visualization with Ouster's :class:`.viz.PointViz` -=================================================== - -Please refer to :doc:`/python/viz/viz-api-tutorial` for details on extending and customizing -:class:`.viz.PointViz`. - - -.. _ex-open3d: - - -Visualization with Open3d -========================== - -The `Open3d library`_ contains Python bindings for a variety of tools for working with point cloud -data. Loading data into Open3d is just a matter of reshaping the numpy representation of a point -cloud, as demonstrated in the :func:`.examples.pcap.pcap_3d_one_scan` example: - -.. literalinclude:: /../python/src/ouster/sdk/examples/pcap.py - :start-after: [doc-stag-open3d-one-scan] - :end-before: [doc-etag-open3d-one-scan] - :emphasize-lines: 1-6 - :linenos: - :dedent: - -The :mod:`.examples.open3d` module contains a more fully-featured visualizer built using the Open3d -library, which can be used to replay pcap files or visualize a running sensor. The bulk of the -visualizer is implemented in the :func:`.examples.open3d.viewer_3d` function. - -.. note:: - - You'll have to install the `Open3d package`_ from PyPI to run this example. - - -As an example, you can view frame ``84`` from the sample data by running the following command: - -.. tabs:: - - .. code-tab:: console Linux/macOS - - $ python3 -m ouster.sdk.examples.open3d \ - --pcap $SAMPLE_DATA_PCAP_PATH --meta $SAMPLE_DATA_JSON_PATH --start 84 --pause - - .. code-tab:: powershell Windows x64 - - PS > py -3 -m ouster.sdk.examples.open3d ^ - --pcap $SAMPLE_DATA_PCAP_PATH --meta $SAMPLE_DATA_JSON_PATH --start 84 --pause - -You may also want to try the ``--sensor`` option to display the output of a running sensor. Use the -``-h`` flag to see a full list of command line options and flags. - -Running the example above should open a window displaying a scene from a city intersection, -reproduced below: - -.. figure:: /images/lidar_scan_xyz_84_3d.png - :align: center - - Open3D visualization of OS1 sample data (frame 84). Points colored by ``SIGNAL`` field. - -You should be able to click and drag the mouse to look around. You can zoom in and out using the -mouse wheel, and hold control or shift while dragging to pan and roll, respectively. - -Hitting the spacebar will start playing back the rest of the pcap in real time. Note that reasonable -performance for realtime playback requires relatively fast hardware, since Open3d runs all rendering -and processing in a single thread. - -All of the visualizer controls are listed in the table below: - -.. list-table:: Open3d Visualizer Controls - :widths: 15 30 - :header-rows: 1 - :align: center - - * - Key - - What it does - * - **Mouse wheel** - - Zoom in and out - * - **Left click + drag** - - Tilt and rotate the camera - * - **Ctrl + left click + drag** - - Pan the camera laterally - * - **Shift + left click + drag** - - Roll the camera - * - **"+" / "-"** - - Increase or decrease point sizes - * - **Spacebar** - - Pause or resume playback - * - **"M"** - - Cycle through channel fields used for visualization - * - **Right arrow key** - - When reading a pcap, jump 10 frames forward - -.. _Open3d library: http://www.open3d.org/ -.. _Open3d package: https://pypi.org/project/open3d/ - -.. _ex-visualization-with-matplotlib: - - -Visualization with Matplotlib -============================== - -You should have defined ``source`` using either a pcap file or UDP data streaming directly from a -sensor, please refer to :doc:`/python/quickstart` for introduction. - -.. note:: - - Below pictures were rendered using :doc:`OS2 128 Rev 05 Bridge ` sample data. - -Let's read from ``source`` until we get to the 50th frame of data: - -.. code:: python - - from contextlib import closing - from more_itertools import nth - with closing(client.Scans(source)) as scans: - scan = nth(scans, 50) - -.. note:: - - If you're using a sensor and it takes a few seconds, don't be alarmed! It has to get to the 50th - frame of data, which would be 5.0 seconds for a sensor running in 1024x10 mode. - -We can extract the range measurements from the frame of data stored in the :py:class:`.LidarScan` -datatype and plot a range image where each column corresponds to a single azimuth angle: - -.. code:: python - - range_field = scan.field(client.ChanField.RANGE) - range_img = client.destagger(info, range_field) - -We can plot the results using standard Python tools that work with numpy datatypes. Here, we extract -a column segment of the range data and display the result: - -.. code:: python - - import matplotlib.pyplot as plt - plt.imshow(range_img[:, 640:1024], resample=False) - plt.axis('off') - plt.show() - -.. note:: - - If running ``plt.show`` gives you an error about your Matplotlib backend, you will need a `GUI - backend`_ such as TkAgg or Qt5Agg in order to visualize your data with matplotlib. - - -.. figure:: /images/brooklyn_bridge_ls_50_range_image.png - :align: center - :figwidth: 100% - - Range image of OS2 bridge data. Data taken at Brooklyn Bridge, NYC. - - -In addition to viewing the data in 2D, we can also plot the results in 3D by projecting the range -measurements into Cartesian coordinates. To do this, we first create a lookup table, then use it to -produce X, Y, Z coordinates from our scan data with shape (H x W x 3): - -.. code:: python - - xyzlut = client.XYZLut(info) - xyz = xyzlut(scan) - -Now we rearrange the resulting numpy array into a shape that's suitable for plotting: - -.. code:: python - - import numpy as np - [x, y, z] = [c.flatten() for c in np.dsplit(xyz, 3)] - ax = plt.axes(projection='3d') - r = 10 - ax.set_xlim3d([-r, r]) - ax.set_ylim3d([-r, r]) - ax.set_zlim3d([-r/2, r/2]) - plt.axis('off') - z_col = np.minimum(np.absolute(z), 5) - ax.scatter(x, y, z, c=z_col, s=0.2) - plt.show() - -.. figure:: /images/brooklyn_bridge_ls_50_xyz_cut.png - :align: center - - Point cloud from OS2 bridge data with colormap on z. Data taken at Brooklyn Bridge, NYC. - -You should be able to rotate the resulting scene to view it from different angles. - -To learn more about manipulating lidar data, see: - -- :ref:`ex-staggered-and-destaggered` -- :ref:`ex-xyzlut` -- :ref:`ex-correlating-2d-and-3d` - - -.. _GUI backend: https://matplotlib.org/stable/tutorials/introductory/usage.html#the-builtin-backends diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/python/quickstart.rst b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/python/quickstart.rst deleted file mode 100644 index a52ecf1a..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/python/quickstart.rst +++ /dev/null @@ -1,170 +0,0 @@ -.. _quickstart: - -================================================== -Developer's Quick Start with the Ouster Python SDK -================================================== - -This quickstart guide will walk you through visualizing Ouster sensor data quickly with Python code -you write yourself. It assumes that you have followed the steps in :ref:`Python Installation -` to install the Ouster Python SDK. - -Using this Guide -================ - -You'll want to start an interactive Python session and keep it open through the sections, as we'll -be reusing variables created in earlier parts while explaining what we're doing as we go. - -To get started, open a new console/Powershell window and start a python interpreter: - -.. tabs:: - - .. code-tab:: console Linux/macOS - - $ python3 - - .. code-tab:: powershell Windows x64 - - PS > py -3 - - -Throughout this guide we will indicate console commands with ``$`` and python interpreter commands -with ``>>>``, just as we have above. - -If you'd like to start by working with sample data, continue to the next section. If you'd prefer to -start by capturing data from a sensor, you can skip to `Using an Ouster Sensor`_ below. - - -Using Sample Data -================= - -.. include:: /sample-data.rst - :start-after: [start-download-instructions] - :end-before: [end-download-instructions] - -The downloaded pcap file contains lidar and imu packets captured from the network. You can read -more about the `IMU Data Format`_ and `Lidar Data Format`_ in the Ouster Sensor Documentation. The -JSON file contains metadata queried from the sensor TCP interface necessary for interpreting -the packet data. - -In your open python session, save the two paths to variables: - -.. code:: python - - >>> pcap_path = '' - >>> metadata_path = '' - -Because our pcap file contains the UDP packet stream but not the sensor metadata, we load the sensor -information from ``metadata_path`` first, using the client module: - -.. note:: - Starting with ouster-sdk v0.11.0, most of core sdk objects have been moved from the ``ouster`` - namespace into the ``ouster.sdk`` namespace. - -.. note:: - Starting with ouster-sdk v0.11.0 we introduce a unified ``ScanSource`` interface that easier to utilize - and is more capable than original objects demonstrated in this quick guide. Please refer to - :ref:`Using ScanSource API ` for details on how to use the new interface. - - - -.. code:: python - - >>> from ouster.sdk import client - >>> with open(metadata_path, 'r') as f: - ... info = client.SensorInfo(f.read()) - -Now that we've parsed the metadata file into a :py:class:`.SensorInfo`, we can use it to read our -captured UDP data by instantiating :py:class:`.pcap.Pcap`. This class acts as a -:py:class:`.PacketSource` and can be used in many of the same contexts as a real sensor. - -.. code:: python - - >>> from ouster.sdk import pcap - >>> source = pcap.Pcap(pcap_path, info) - -To visualize data from this pcap file, proceed to :doc:`/python/examples/visualizations` examples. - -.. _Lidar Data Format: https://static.ouster.dev/sensor-docs/sw_manual/sensor_data/sensor-data.html#lidar-data -.. _IMU Data Format: https://static.ouster.dev/sensor-docs/sw_manual/sensor_data/sensor-data.html#imu-data -.. _OS2 bridge sample data: https://data.ouster.io/sdk-samples/OS2/OS2_128_bridge_sample.zip - - -Using an Ouster Sensor -====================== - -If you have access to sensor hardware, you can start reading data by -instantiating a :py:class:`.PacketSource` that listens for a UDP data stream on -a local socket. - -.. note:: - - Connecting to an Ouster sensor is covered in the `Networking Guide`_ section of the Ouster - Sensor Documentation. - -In the following, ```` should be substituted for the actual hostname or IP of your -sensor. - -To make sure everything is connected, open a separate console window and try pinging the sensor. You -should see some output like: - -.. tabs:: - - .. code-tab:: console Linux/macOS - - $ ping -c1 - PING (192.0.2.42) 56(84) bytes of data. - 64 bytes from (192.0.2.42): icmp_seq=1 ttl=64 time=0.217 ms - - .. code-tab:: powershell Windows x64 - - PS > ping /n 10 - Pinging (192.0.2.42) with 32 bytes of data: - Reply from 192.0.2.42: bytes=32 time=101ms TTL=124 - - -Next, you'll need to configure the sensor with the config parameters using the client module. - -In your open python session, set ``hostname`` as ````: - -.. code:: python - - >>> hostname = '' - -Now configure the client: - -.. code:: python - - >>> from ouster.sdk import client - >>> config = client.SensorConfig() - >>> config.udp_port_lidar = 7502 - >>> config.udp_port_imu = 7503 - >>> config.operating_mode = client.OperatingMode.OPERATING_NORMAL - >>> client.set_config(hostname, config, persist=True, udp_dest_auto = True) - -Just like with the sample data, you can create a :py:class:`.PacketSource` from the sensor: - -.. code:: python - - >>> source = client.Sensor(hostname, 7502, 7503) - >>> info = source.metadata - -Now we have a ``source`` from our sensor! You're ready to record, visualize to visualize data from your sensor, proceed to -:doc:`/python/examples/visualizations` examples. Or you can check out other things you can do with a -``source`` in the Python :doc:`/python/examples/index`. - - -.. _Networking Guide: https://static.ouster.dev/sensor-docs/image_route1/image_route3/networking_guide/networking_guide.html - - -Next Steps -========== - -Now that you know the basics, you can check out our annotated examples for a more detailed look at -how to work with our data. - -Here are a few things you might be interested in: - - * :ref:`ex-basic-sensor` - * :ref:`ex-packets` - * :doc:`/reference/lidar-scan` - * :ref:`ex-record-stream-viz` diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/python/slam-api-example.rst b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/python/slam-api-example.rst deleted file mode 100644 index a9eaeb3e..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/python/slam-api-example.rst +++ /dev/null @@ -1,91 +0,0 @@ -=============== -SLAM Quickstart -=============== - -.. contents:: - :local: - :depth: 3 - -.. _slam-api-example: - -This guide provides examples of using the SLAM API for development purposes. -Users can run SLAM with an OS sensor's hostname or IP for real-time processing, or with a recorded PCAP/OSF file for offline processing. - -.. warning:: - Due to missing upstream dependency support on python3.12, slam does not work on python3.12. - - -Obtain Lidar Pose and Calculate Pose Difference -=============================================== -The SLAM API outputs the sensor's pose for each lidar scan, which you can use to determine the -sensor's orientation in your system. From the Lidar Poses, we can calculate the Pose difference -between consecutive scans - -.. code:: python - - from ouster.sdk import open_source - from ouster.mapping.slam import KissBackend - import numpy as np - scans = open_source(pcap_path) - slam = KissBackend(scans.metadata, max_range=75, min_range=1, voxel_size=1.0) - last_scan_pose = np.eye(4) - - for idx, scan in enumerate(scans): - scan_w_poses = slam.update(scan) - col = client.first_valid_column(scan_w_poses) - # scan_w_poses.pose is a list where each pose represents a column points' pose. - # use the first valid scan's column pose as the scan pose - scan_pose = scan_w_poses.pose[col] - print(f"idx = {idx} and Scan Pose {scan_pose}") - - # calculate the inverse transformation of the last scan pose - inverse_last = np.linalg.inv(last_scan_pose) - # calculate the pose difference by matrix multiplication - pose_diff = np.dot(inverse_last, scan_pose) - # extract rotation and translation - rotation_diff = pose_diff[:3, :3] - translation_diff = pose_diff[:3, 3] - print(f"idx = {idx} and Rotation Difference: {rotation_diff}, " - f"Translation Difference: {translation_diff}") - - -SLAM with Visulizer and Accumulated Scans -========================================= -Visualizers and Accumulated Scans are also available for monitoring the performance of the algorithm, -as well as for demonstration and feedback purposes. - -.. code:: python - - from functools import partial - from ouster.viz import SimpleViz, ScansAccumulator - from ouster.mapping.slam import KissBackend - scans = open_source(pcap_path) - slam = KissBackend(scans.metadata, max_range=75, min_range=1, voxel_size=1.0) - - scans_w_poses = map(partial(slam.update), scans) - scans_acc = ScansAccumulator(info, - accum_max_num=10, - accum_min_dist_num=1, - map_enabled=True, - map_select_ratio=0.01) - - SimpleViz(info, scans_accum=scans_acc, rate=0.0).run(scans_w_poses) - -More details about the visualizer and accumulated scans can be found at the -:ref:`Ouster Visualizer ` and :ref:`Scans Accumulator ` - - -.. note:: - - The performance of the SLAM algorithm depends on your CPU's processing power and the 'voxel_size' - parameter. - Below is a suggestion for selecting an appropriate voxel size: - - | Outdoor: 1.4 - 2.2 - | Large indoor: 1.0 - 1.8 - | Small indoor: 0.4 - 0.8 - - -Intro SLAM in Ouster-CLI -======================== -We also offer a simpler method to run SLAM using the ``ouster-cli``. For additional details, please refer to :ref:`Ouster-CLI Mapping `. diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/python/using-scan-source.rst b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/python/using-scan-source.rst deleted file mode 100644 index 459b881b..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/python/using-scan-source.rst +++ /dev/null @@ -1,197 +0,0 @@ -================================== -Using the new ScanSource interface -================================== - -.. contents:: - :local: - :depth: 3 - -.. _scan-source-example: - -In this example we are going to demonstrate the use of the new ScanSource API. - - -Using the open_source method -============================ - -The new API introduces a new method name ``open_source`` which allows users to handle different source types -using the same API. Current supported source types are live sensor, pcap file, or osf file. For example, to -open the same pcap file referenced in the main :ref:`Quick Start ` using the simplified API -can be accomplished as follows: - -.. code:: python - - >>> pcap_path = '' - >>> metadata_path = '' - >>> from ouster.sdk import open_source - >>> source = open_source(pcap_path, meta=[metadata_path]) - - -The ``source`` handle here acts the same as the handle returned by the ``pcap.Pcap`` constructor, with some -extra capabilities that we will cover later. - -Notice here that rather than we try to load and parse the metadata ourselves we only need to pass to metadata -to the method through ``meta`` parameter and the method will take care of loading it and associating it with the -source object. The ``meta`` parameter however is optional and can be omitted. When the ``meta`` parameter is not -set explicity the ``open_source`` method will attempt to locate the metadata automatically for us and we can reduce -the call to: - -.. code:: python - >>> source = open_source(pcap_path) - -However if metadata file is not in the same folder as the pcap and don't have a shared name prefix the method will -fail. - - -.. note:: - Another optional but important parameter for the ``open_source`` method is ``sensor_idx``. This paramter is to zero - by default, which should always be the case unless the pcap file that you are using (or osf or any LidarScan storage) - contains scans from more than one sensor, in this case, users can set the ``sensor_idx`` to a any value between zero - and ``sensors_count -1`` to access and manipulate scans from a specific sensor by the order they appear in the file. - Alternatively, if users set the value of ``sensor_idx`` to ``-1`` then ``open_source`` will return a slightly differnt - interface from ``ScanSource`` which is the ``MultiScanSource`` this interface and as the name suggests allows users to - work with sensor data collected from multiple sensors at the same time. - - The main different between the ``MultiScanSource`` and the ``ScanSource`` is the expected return of some of the object - methods. For example, when creating an iterator for a ``ScanSource`` object, the user would get a single ``LidarScan`` - object per iteration. Iterating over the contents of a ``MultiScanSource`` object always yields a **list** of - ``LidarScan(s)`` per iteration corresponding to the number of sensors stored in the pcap file or whatever source type - is being used. This is true even when the pcap file contains data for a single sensor. - - -On the other hand, if the user wants to open an osf file or access the a live sensor, all that changes is url -of the source. For example, to interact with a live sensor the user can execute the following snippet: - -.. code:: python - - >>> sensor_url = '' - >>> from ouster.sdk import open_source - >>> source = open_source(sensor_url) - - -Obtaining sensor metadata -========================= -Every ScanSource holds a reference to the sensor metadata, which has crucial information that is important when -when processing the invidivual scans. Continuing the example, a user this can access the metadata through the -``metadata`` property of a ``ScanSource`` object: - -.. code:: python - - >>> print(source.metadata) - - -Iterating over Scans -==================== - -Once we have successfully obtain a handle to the ScanSource we can iterate over ``LidarScan`` objects stored in the -pcap file and manipulate each one individually. For example, let's say we want to print the frame id of the first 10 -scans. We can achieve that using: - -.. code:: python - - >>> ctr = 0 - >>> source_iter = iter(source) - >>> for scan in source_iter: - ... print(scan.frame_id) - ... ctr += 1 - ... if ctr == 10: - ... break - -As we noted earlier, if we set ``sensor_idx=-1`` when invoking ``open_source`` method, the method will construct a -``MultiScanSource``, which always addresses a group of sensors. Thus, when iterating over the ``source`` the user -receives a collated set of scans from the addressed sensors per iteration. The ``MultiScanSource`` examines the -timestamp of every scan from every sensor and returns a list of scans that fit within the same time window as single -batch. The size of the batch is fixed corresponding to how many sensors contained in the pcap or osf file. However, -the collation could yield a null value if one or more of the sensors didn't produce a ``LidarScan`` object that fits -within the time frame of current batch or iteration. Thus, depending on the operation at hand it is crticial to check -if we got a valid ``LidarScan`` object when examining the iteration output of a ``MultiScanSource``. If we are to -perform the same example as above when ``source`` is a handle to ``MultiScanSource`` and display the frame_id of -``LidarScan`` objects the belongs to the same batch on the same line the code needs to updated to the following: - -.. code:: python - - >>> ctr = 0 - >>> source_iter = iter(source) - >>> for scans in source_iter: - ... for scan in scans: # source_iter here returns a list of scans - ... if scan: # check if invidiual scan object is valid - ... print(scan.frame_id, end=', ') - ... print() # new line for next batch - ... ctr += 1 - ... if ctr == 10: - ... break - - -Note that when iterating over a ``MultiScanSource`` object, it always a list of scans, even when the underlying scan -source has only a single sensor. In this case, the iterator will yield a list with a single element per iteration. - - -Using indexing and slicing capabilities of a ScanSource -======================================================== - -One of the most prominent new features of the ScanSource API, (besides being able to address multi sensors), is the -ability to use indexing and slicing when accessing the stored scans within the ``LidarScan`` source. Currently, this -capability is only supported for non-live sources. That is to say, the functionality we are discussing can only be used -when accessing a pcap or an osf file. To enable this functionality we need to indicate that we want to manipulate the -source as an indexed one upon opening. Revisitng the previous pcap open example, that would be achieved as follows: -: - - -.. code:: python - - >>> pcap_path = '' - >>> from ouster.sdk import open_source - >>> source = open_source(pcap_path, index=True) - -First note that we omitted the ``meta`` parameter since it can be populated automatically as we explained earlier. -Second you will noticed that we introduced a new parameter ``index`` with its value set to ``True`` (default is false), -The same parameter can be applied to when dealing with an osf file but not a live sensor. - -Depending on the file size and the underlying file format there can be some delay before the file is fully indexed (OSF -file take much less time than pcap file to index). A progress bar will appear to indicate progress of the indexing. - -Once the index is built up, then we can start using utilizing and interact with the ``ScanSource`` object to access scans -in the same manner we are dealing with a python list that holds reference to LidarScan objects. - -For example to access the 10th LidarScan and print its frame id, we can do the following: - -.. code:: python - - >>> print(source[10].frame_id) - -Similarly we can access the last LidarScan object and print its frame_id using: - -.. code:: python - - >>> print(source[-1].frame_id) - - -Alternatively we can instead request a range of scans using the python slice operator. For example, to request the first 10 -scans from a ScanSource and print their frame ids, we can do the following: - -.. code:: python - - >>> for scan in source[0:10]: - ... print(scan.frame_id) - - -Note we don't need to add any break here since the operation `source[0:10]` will only yield the first 10 ``LidarScan(s)``. - -To print frame_id of the last 10 LidarScans we do: - -.. code:: python - - >>> for scan in source[-11:-1]: - ... print(scan.frame_id) - - -Finally, as you would expect from a typical slice operation, you can also using negative step and also use a reversed -iteration as shown in the following example: - -.. code:: python - - >>> for scan in source[0:10:2]: # prints the frame_id of every second scan of the first 10 scans - ... print(scan.frame_id) - - >>> for scan in source[10:0:-1]: # prints the frame_id of every scan of the first 10 scans in reverse - ... print(scan.frame_id) diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/python/viz/index.rst b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/python/viz/index.rst deleted file mode 100644 index 63f12bb6..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/python/viz/index.rst +++ /dev/null @@ -1,46 +0,0 @@ -====================== -Point Cloud Visualizer -====================== - -The Ouster visualization toolkit is written in C++ with Python bindings for Python functionality. It -consists of the following: - -- ``simple-viz`` (:class:`.viz.SimpleViz`): the default Python application visualizer, which can - also be used as an entrypoint for more sophisticated custom point cloud visualizations -- ``ouster_viz``: the core C++ library -- :mod:`ouster.viz`: the Python module for the bindings - -.. todo:: - - Update all ``simple-viz`` CLI command mentions to a proper new CLI command + update the - screenshots to the current looking version - -Using ``ouster-cli`` is a fastest way to visualize data from a connected sensor, recorded ``pcap`` -or OSF files with SLAM poses: - -.. figure:: /images/ouster-viz.png - :align: center - - Ouster SDK CLI ``ouster-cli source OS-1-128.pcap viz`` visualization of OS1 128 sample data - -How to use ``ouster-cli`` for visualizations you can learn in :doc:`viz-run` - -.. toctree:: - :hidden: - - viz-run - - -For custom visualizations you can explore a programmatically accessible :class:`.viz.PointViz` API -below: - -.. figure:: /images/viz-tutorial/lidar_scan_viz_checkers.png - :align: center - - ``PointViz`` with ``Image``, ``Label`` and masks applied - - -.. toctree:: - - viz-api-tutorial - diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/python/viz/viz-api-tutorial.rst b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/python/viz/viz-api-tutorial.rst deleted file mode 100644 index 5dccc26d..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/python/viz/viz-api-tutorial.rst +++ /dev/null @@ -1,397 +0,0 @@ -=============================== -PointViz Tutorial & API Usage -=============================== - -.. contents:: - :local: - - -Environment and General Setup -============================= - -In this interactive tutorial we explore the Python bindings of the :class:`.viz.PointViz` and how we -can use it programmatically to visualize various 2D and 3D scenes. - -.. note:: - - We assume you have downloaded the :doc:`sample data ` and set - ``SAMPLE_DATA_PCAP_PATH`` and ``SAMPLE_DATA_JSON_PATH`` to the locations of the OS1 128 sample - data pcap and json files correspondingly. - -Before proceeding, be sure to install ``ouster-sdk`` Python package, see :ref:`OusterSDK Python -Installation` for details. - -This interactive tutorial will open a series of visualizer windows on your screen. Each example can -be exited by pressing ``ESC`` or the exit button on the window; doing so will open the next -visualizer window. - -Let's start the tutorial:: - - python3 -m ouster.sdk.examples.viz $SAMPLE_DATA_PCAP_PATH $SAMPLE_DATA_JSON_PATH - -Now you can proceed through each example, labeled by numbers to match your screen output. - -Creating an empty ``PointViz`` window -===================================== - -:class:`.viz.PointViz` is the main entry point to the Ouster visualizer, and it keeps track of the -window state, runs the main visualizations loop, and handles mouse and keyboard events. - -To create the PointViz window (and see a black screen as nothing has been added), you can try the -following: - -.. literalinclude:: /../python/src/ouster/sdk/examples/viz.py - :start-after: [doc-stag-empty-pointviz] - :end-before: [doc-etag-empty-pointviz] - :emphasize-lines: 2 - :linenos: - :dedent: - -As expected we see a black empty window: - -.. figure:: /images/viz-tutorial/empty_point_viz.png - :align: center - - Empty ``PointViz`` window - -Now let's try to add some visual objects to the viz. Hit ``ESC`` or click the exit button on the -window to move to the next example. - - -Images and Labels -================= - -Some of the basic 2D objects we can add include the 2D :class:`.viz.Label` and the -:class:`.viz.Image`. To add these, we need an understanding of the 2D Coordinate System for :class:`.viz.PointViz`. - -The ``Image`` object --------------------- - -To create a 2D Image you need to use the :class:`.viz.Image` object. Currently only normalized -grayscale images are supported, though you can use :meth:`.viz.Image.set_mask`, which accepts RGBA -masks, to get color. - -The :class:`.viz.Image` screen coordinate system is *height-normalized* and goes from bottom to top -(``[-1, +1]``) for the ``y`` cordinate, and from left to right (``[-aspect, +aspect]``) for the -``x`` coordidate, where:: - - aspect = viewport width in Pixels / viewport height in Pixels - -Here is how we create a square image and place it in the center: - -.. literalinclude:: /../python/src/ouster/sdk/examples/viz.py - :start-after: [doc-stag-image-pos-center] - :end-before: [doc-etag-image-pos-center] - :emphasize-lines: 2-3 - :linenos: - :dedent: - -Expected result: (without top and bottom labels) - -.. figure:: /images/viz-tutorial/image_set_pos_center.png - :align: center - - :meth:`.viz.Image.set_position` to center image - -Window-aligned Images ---------------------- - -To align an image to the left or right screen edge, use :meth:`.viz.Image.set_hshift`, which accepts -a *width-normalized* (``[-1, 1]``) horizontal shift which is applied to the image position after the -:meth:`.viz.Image.set_position` values are first applied. - -Here is how we place an image to the left edge of the screen: - -.. literalinclude:: /../python/src/ouster/sdk/examples/viz.py - :start-after: [doc-stag-image-pos-left] - :end-before: [doc-etag-image-pos-left] - :linenos: - :dedent: - -Expected result: - -.. figure:: /images/viz-tutorial/image_set_pos_left.png - :align: center - - :meth:`.viz.Image.set_position` with :meth:`.viz.Image.set_hshift` to align image to the left - -And here's how we place an image to the right edge of the screen: - -.. literalinclude:: /../python/src/ouster/sdk/examples/viz.py - :start-after: [doc-stag-image-pos-right] - :end-before: [doc-etag-image-pos-right] - :linenos: - :dedent: - -Expected result: - -.. figure:: /images/viz-tutorial/image_set_pos_right.png - :align: center - - :func:`.viz.Image.set_position` to align image to the right - -Finally, here's how we place an image to the right edge and bottom of the screen: - -.. literalinclude:: /../python/src/ouster/sdk/examples/viz.py - :start-after: [doc-stag-image-pos-right-bottom] - :end-before: [doc-etag-image-pos-right-bottom] - :linenos: - :dedent: - -Expected result: - -.. figure:: /images/viz-tutorial/image_set_pos_bot_right.png - :align: center - - :func:`.viz.Image.set_position` to align image to the bottom and right - -Cool! Let's move on to more sophisticated images! - - -LidarScan Fields as Images --------------------------- - -Now we can use :class:`.viz.Image` to visualize ``LidarScan`` fields data as images. In this example -we show the ``RANGE`` and ``SIGNAL`` fields images positioned on the top and bottom of the screen -correspondingly. - -.. note:: - - If you aren't familiar with the ``LidarScan``, please see :doc:`/reference/lidar-scan`. A single - scan contains a full frame of lidar data. - -So how do we do it? - -.. literalinclude:: /../python/src/ouster/sdk/examples/viz.py - :start-after: [doc-stag-scan-fields-images] - :end-before: [doc-etag-scan-fields-images] - :emphasize-lines: 17-34 - :linenos: - :dedent: - -In the highlighted lines, you can see that we're simply using :func:`.viz.Image()`, -:func:`.viz.set_image()`, and :func:`.viz.set_position()` like we did before, but this time with -more interesting data! - -Expected result: - -.. figure:: /images/viz-tutorial/lidar_scan_fields_images.png - :align: center - - ``LidarScan`` fields ``RANGE`` and ``SIGNAL`` visualized with :class:`.viz.Image` - - -2D ``Label`` object --------------------- - -2D labels are represented with :class:`.viz.Label` object and use a slightly different coordinate -system that goes from left to right ``[0, 1]`` for the ``x`` coordinate, and from top to bottom -``[0, 1]`` the for ``y`` coordinate. - -Let's apply the labels to our already visualized ``LidarScan`` field images: - -.. literalinclude:: /../python/src/ouster/sdk/examples/viz.py - :start-after: [doc-stag-scan-fields-images-labels] - :end-before: [doc-etag-scan-fields-images-labels] - :linenos: - :dedent: - -Expected result: - -.. figure:: /images/viz-tutorial/lidar_scan_fields_images_labels.png - :align: center - - ``LidarScan`` fields images captioned with 2D :class:`.viz.Label` - -.. note:: - - Currently the :class:`.viz.Image` has a different 2D coordinate system than the - :class:`.viz.Label`. This is likely to change in the future. - - -Point Clouds: the ``Cloud`` object -================================== - -Point Cloud visualization implemented via the :class:`.viz.Cloud` object can be used in two ways: - - - :ref:`Structured Point Clouds `, where 3D points are defined with 2D - field images (i.e. ``LidarScan`` fields images) - - :ref:`Unstructured Point Clouds `, where 3D points are defined - directly as a set of XYZ 3D points - -Let's take a closer look at each of these. - -.. _structured-point-cloud: - -Structured Point Cloud ----------------------- - -The Ouster sensor produces a structured point cloud as a 2D range image which can be projected into -3D Cartesian coordinates with a pre-generated lookup table. For this reason the internal -implementation of :class:`.viz.Cloud` applies the lookup table transform automatically generated -from :class:`.client.SensorInfo` (metadata object) and 2D ``RANGE`` image as an input. - -.. note:: - - Please refer to :doc:`/reference/lidar-scan` for details about interal ``LidarScan`` - representations and its basic operations. - - -To visualize :class:`.client.LidarScan` object we use the following code: - -.. literalinclude:: /../python/src/ouster/sdk/examples/viz.py - :start-after: [doc-stag-scan-structured] - :end-before: [doc-etag-scan-structured] - :linenos: - :dedent: - -Expected result: - -.. figure:: /images/viz-tutorial/lidar_scan_structured.png - :align: center - - ``LidarScan`` Point Cloud with :class:`.viz.Cloud` as structured - - -.. _unstructured-point-cloud: - -Unstructured Point Clouds -------------------------- - -Point Clouds that are represented as a set of XYZ 3D points are called *unstructured* for our -purposes. - -It's possible to set the :class:`.viz.Cloud` object by setting the 3D points directly, which is -useful when you have unstructured point clouds. - -.. literalinclude:: /../python/src/ouster/sdk/examples/viz.py - :start-after: [doc-stag-scan-unstructured] - :end-before: [doc-etag-scan-unstructured] - :emphasize-lines: 5-6 - :linenos: - :dedent: - -You should see the same visualization as for the structured point clouds. - - -Example: 3D Axes Helper as Unstructured Points -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -The :class:`.viz.Cloud` object also supports color RGBA masks. Below is an example how we can use -unstructured point clouds to draw 3D axes at the origin: - -.. literalinclude:: /../python/src/ouster/sdk/examples/viz.py - :start-after: [doc-stag-axes-helper] - :end-before: [doc-etag-axes-helper] - :emphasize-lines: 17-22 - :linenos: - :dedent: - -Expected result: - -.. figure:: /images/viz-tutorial/axes_helper_unstructured.png - :align: center - - 3D Axes Helper with :class:`.viz.Cloud` unstructured and color RGBA masks - - -``LidarScanViz`` for point cloud with fields images ---------------------------------------------------- - -To make it even more easier to explore the data of :class:`.client.LidarScan` objects, we provide a -higher-order visual component :class:`.viz.LidarScanViz` that enables: - -- 3D point cloud and two 2D fields images in one view -- color palettes for 3D point cloud coloration -- point cloud point size increase/decrease -- toggles for view of different fields images and increasing/decreaseing their size -- dual return point clouds and fields images support -- key handlers for all of the above - -The majority of keyboard operations that you can see in :ref:`simple-viz keymaps -` implemented by :class:`.viz.LidarScanViz`. - -Let's look at how we can explore a :class:`.client.LidarScan` object with a couple of lines: - -.. literalinclude:: /../python/src/ouster/sdk/examples/viz.py - :start-after: [doc-stag-lidar-scan-viz] - :end-before: [doc-etag-lidar-scan-viz] - :emphasize-lines: 2,5,8 - :linenos: - :dedent: - -Not a lot of lines of code needed to visualize all the information desired! - -Expected result: - -.. figure:: /images/viz-tutorial/lidar_scan_viz.png - :align: center - - ``LidarScan`` Point Cloud with :class:`.viz.LidarScanViz` - - -3D ``Label`` object --------------------- - -The same :class:`.viz.Label` object that we used for 2D labels can be used for putting text labels -in 3D space. - -Here's an example of using 3D Labels: - -.. literalinclude:: /../python/src/ouster/sdk/examples/viz.py - :start-after: [doc-stag-lidar-scan-viz-labels] - :end-before: [doc-etag-lidar-scan-viz-labels] - :linenos: - :dedent: - -Expected result: - -.. figure:: /images/viz-tutorial/lidar_scan_viz_labels.png - :align: center - - 3D :class:`.viz.Label` superimposed onto ``LidarScanViz`` - - -Example: Overlay 2D Images and 2D Labels -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -Here's another example to show how one could add 2D Images, RGBA masks and 2D Labels added to :class:`.viz.LidarScanViz`: - -.. literalinclude:: /../python/src/ouster/sdk/examples/viz.py - :start-after: [doc-stag-overlay-images-labels] - :end-before: [doc-etag-overlay-images-labels] - :linenos: - :dedent: - -Expected result: - -.. figure:: /images/viz-tutorial/lidar_scan_viz_checkers.png - :align: center - - 2D :class:`.viz.Label` and :class:`.viz.Image` superimposed onto ``LidarScanViz`` - - -Event Handlers -=============== - -Custom keyboard handlers can be added to handle key presses in :class:`.viz.PointViz`. - -Keyboard handlers ------------------- - -We haven't yet covered the :class:`.viz.Camera` object and ways to control it. So here's a quick -example of how we can map ``R`` key to move the camera closer or farther from the target. - -Random :meth:`.viz.Camera.dolly` change on keypress: - -.. literalinclude:: /../python/src/ouster/sdk/examples/viz.py - :start-after: [doc-stag-key-handlers] - :end-before: [doc-etag-key-handlers] - :emphasize-lines: 1,9 - :linenos: - :dedent: - -Result: Press ``R`` and see a random camera dolly walk. - -We've reached the end of our :class:`.viz.PointViz` tutorial! Thanks and Happy Hacking with the -Ouster Viz library! diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/python/viz/viz-run.rst b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/python/viz/viz-run.rst deleted file mode 100644 index bacc607f..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/python/viz/viz-run.rst +++ /dev/null @@ -1,99 +0,0 @@ -.. _viz-run: - -============================== -Running the Ouster visualizer -============================== - - -After :ref:`installing ` the ``ouster-sdk`` package, you can run:: - - $ ouster-cli source viz - -where ```` is the hostname (os-99xxxxxxxxxx) or IP of the sensor. - -Alternately, to replay the existing data from ``pcap`` and ``json`` files call the visualizer as:: - - $ ouster-cli source [--meta ] viz - -.. figure:: /images/ouster-viz.png - :align: center - - Ouster SDK CLI ``ouster-cli source viz`` visualization of OS1 128 sample data - -The Ouster SDK CLI visualizer does not include a GUI, but can be controlled with the mouse and -keyboard: - -* Click and drag rotates the view -* Middle click and drag moves the view -* Scroll adjusts how far away the camera is from the vehicle - - -.. _simple-viz-keymap: - -.. - [start-simple-viz-keymap] - -Keyboard controls: - ================ =============================================== - Key What it does - ================ =============================================== - ``o`` Toggle on-screen display - ``p / P`` Increase/decrease point size - ``m / M`` Cycle point cloud coloring mode - ``f / F`` Cycle point cloud color palette - ``b / B`` Cycle top 2D image - ``n / N`` Cycle bottom 2D image - ``R`` Reset camera - ``ctr-r`` Set camera to the birds-eye view - ``u`` Toggle camera mode FOLLOW/FIXED - ``e / E`` Increase/decrease size of displayed 2D images - ``' / "`` Increase/decrease spacing in range markers - ``w`` Camera pitch up - ``s`` Camera pitch down - ``a`` Camera yaw left - ``d`` Camera yaw right - ``1`` Toggle first return point cloud visibility - ``2`` Toggle second return point cloud visibility - ``0`` Toggle orthographic camera - ``= / -`` Dolly in/out - ``?`` Prints key bindings - ``space`` Toggle pause - ``esc`` Exit visualization - ``. / ,`` Step one frame forward/back - ``ctrl + . / ,`` Step 10 frames forward/back - ``> / <`` Increase/decrease playback rate (during replay) - ``shift`` Camera Translation with mouse drag - ``shift+z`` Save a screenshot of the current view - ``shift+x`` Toggle a continuous saving of screenshots - ``?`` Print keys to standard out - ================ =============================================== - -.. - [end-simple-viz-keymap] - -The visualizer also includes an option to control the orientation of the point cloud in space when -loaded. If you possess, say, an OS-DOME mounted an upside down, you can start the visualizer with -the option ``--extrinsics``:: - - $ ouster-cli source 10.0.0.13 viz --extrinsics -1 0 0 0 0 1 0 0 0 0 -1 0 0 0 0 1 - -The input is a row-major homogenous matrix. - -For other options, run ``ouster-cli source viz -h`` - -.. note:: - - All basic primitives that you see as part of ``ouster-cli`` visualizer are exposed through - :class:`.viz.PointViz` bindings. Please see :doc:`viz-api-tutorial` for how to use it - programmatically in Python. - - -Advanced usage with sensor --------------------------- - -The Ouster visualizer automatically configures connected sensors to send data to the appropriate udp -destination address. If your sensor is already configured appropriately, you may find it useful to -use the argument ``--no-auto-dest`` to save time by skipping the roundtrip to reconfigure the -sensor. - - diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/python/viz/viz-scans-accum.rst b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/python/viz/viz-scans-accum.rst deleted file mode 100644 index 9509168e..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/python/viz/viz-scans-accum.rst +++ /dev/null @@ -1,196 +0,0 @@ -.. _viz-scans-accum: - -Visualize SLAM Poses using ``ScansAccumulator`` - accumulates track, point clouds and map views ------------------------------------------------------------------------------------------------ - -.. contents:: - :local: - :depth: 3 - - -Overview -^^^^^^^^^ - -**ScansAccumulator** is the continuation of the efforts to view the lidar data with poses that may -come within the ``LidarScan.pose`` property. When poses are not present in the ``LidarScan`` -**ScansAccumulator** may still be useful to view the accumulated *N* scans from the live -sensor/recording and reveal the accuracy/repeatability of the data. - - -Available view modes -~~~~~~~~~~~~~~~~~~~~~ - -There are three view modes of **ScansAccumulator**, that may be enabled/disabled depending on -it's params and the data that is passed throught it: - - * **poses** (or **TRACK**), key ``8`` - all scan poses in a trajectory/path view (available only - if poses data is present in scans) - * **scan map** (or **MAP**), key ``7`` - overall map view with select ratio of random points - from every scan (available for scans with/without poses) - * **scan accum** (or **ACCUM**), key ``6`` - accumulated *N* scans (key frames) that is picked - according to params (available for scans with/without poses) - - -Key bindings -~~~~~~~~~~~~~ - -Keyboard controls available with **ScansAccumulator**: - - ============== ============================================================= - Key What it does - ============== ============================================================= - ``6`` Toggle scans accumulation view mode (ACCUM) - ``7`` Toggle overall map view mode (MAP) - ``8`` Toggle poses/trajectory view mode (TRACK) - ``k / K`` Cycle point cloud coloring mode of accumulated clouds or map - ``g / G`` Cycle point cloud color palette of accumulated clouds or map - ``j / J`` Increase/decrease point size of accumulated clouds or map - ============== ============================================================= - - - -Use in Ouster SDK CLI -^^^^^^^^^^^^^^^^^^^^^^ - -**ScansAccumulator** is present in every CLI viz call, no matter the source of data or way of -calling it.It's in live ``sensor viz``, ``pcap record --viz``, ``osf viz``, ``mapping viz`` and -``pcap viz`` as well as all ``source SOURCE`` versions of those commands. Everywhere the same set of -CLI parameters control the behavior of the **ScansAccumulator**. - - -Works for data sources with/without poses: - - * ``--accum-num N`` - accumulate *N* scans (default: ``0``) - * ``--accum-every K`` - accumulate every *Kth* scan (default: ``1``) - * ``--accum-every-m M`` - accumulate a scan every *Mth* meters traveled (default: ``None``) - * ``--accum-map`` - enable the overall map accumulation, select some percentage of points from - every scan (default: disabled) - * ``--accum-map-ratio R`` - set *R* as a ratio of points to randomly select from every scan - (default: ``0.001`` (*0.1%*)) - - -Examples of the CLI commands: - -Dense accumulated clouds view (with every point of a scan) -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -To obtain the densest view use the ``--accum-num N --accum-every 1`` params where ``N`` is the -number of clouds to accumulate (``N`` up to 100 is generally small enough to avoid slowing down the -viz interface):: - - ouster-cli source OS-1-128_v3.0.1_1024x10_20230216_142857-000.pcap slam viz --accum-num 20 - -and the dense accumulated clouds result: - -.. figure:: /images/scans_accum_dense_every.png - - Dense view of 20 accumulated scans during the ``slam viz`` run - - -Overall map view (with poses) -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -One of the main task that we often needed was a preview of the overall map from the OSF with poses -for example:: - - ouster-cli osf viz OS-0-128_v3.0.1_1024x10_20230415_152307-000.osf --accum-num 20 \ - --accum-every 0 --accum-every-m 10.5 --accum-map -r 0 -e stop - - -And here is the final result when viz is done and stopped (``-e stop``) after playing the whole file: - -.. figure:: /images/scans_accum_map_all_scan.png - - Data fully replayed with map and accum enabled (last current scan is displayed here in grey - palette) - - -.. figure:: /images/scans_accum_accum_scan.png - - Data fully replayed with view only last 20 scans accumulated every 10.5 meters - - -.. figure:: /images/scans_accum_track_all.png - - Data fully replayed with view of only trajectory (yellow knobs is 20 accumulated key frames - positions) - - -Programmatic use with (and without) PointViz -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -With ``point_viz: PointViz`` object the ``ScansAccumulator`` can be used as a regular -``LidarScanViz`` and passed directly to ``SimpleViz``:: - - from ouster.viz import PointViz, add_default_controls, ScansAccumulator - - point_viz = PointViz("SimpleViz usecase") - add_default_controls(point_viz) - - # ... get scans_w_poses Scans source ... - - scans_acc = ScansAccumulator(meta, - point_viz=point_viz, - accum_max_num=10, - accum_min_dist_num=1, - map_enabled=True, - map_select_ratio=0.5) - - SimpleViz(scans_acc, rate=1.0).run(scans_w_poses) - - -Alternatively with a ``PointViz`` it can be used as a canvas to draw the final state only:: - - from ouster.viz import ScansAccumulator, add_default_controls, PointViz - - point_viz = PointViz("Overall map case") - add_default_controls(point_viz) - - # ... get scans_w_poses Scans source ... - - scans_acc = ScansAccumulator(meta, - point_viz=point_viz, - accum_max_num=10, - accum_min_dist_num=1, - map_enabled=True, - map_select_ratio=0.5) - - for scan in scans_w_poses: - scans_acc.update(scan) - - scans_acc.draw(update=True) - point_viz.upadte() - point_viz.run() - - -Without ``PointViz`` it can be used as in the following snippet to accumulate all data and use the -data later to draw anywhere (here we still use the ``PointViz`` and ``viz.Cloud()`` as a main -graphing tool, but it can be ``matplotlib`` instead):: - - from ouster.viz import grey_palette, ScansAccumulator, Cloud, add_default_controls, PointViz - - # ... get scans_w_poses Scans source ... - - # create scans accum without PointViz - scans_acc = ScansAccumulator(meta, - map_enabled=True, - map_select_ratio=0.5) - - # processing doesn't require viz presence in scans accum - for scan in scans_w_poses: - scans_acc.update(scan) - - point_viz = PointViz("Standalone case") - add_default_controls(point_viz) - - # draw the cloud manually to the viz using ScansAccumulator MAP data - cloud_map = Cloud(scans_acc._map_xyz.shape[0]) - cloud_map.set_xyz(scans_acc._map_xyz) - cloud_map.set_key(scans_acc._map_keys["NEAR_IR"]) - cloud_map.set_palette(grey_palette) - cloud_map.set_point_size(1) - point_viz.add(cloud_map) - -In the example above one might use ``matplotlib`` with some modifications to use palette for picking -the key color. - diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/reference/changelog.rst b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/reference/changelog.rst deleted file mode 100644 index 75a2963e..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/reference/changelog.rst +++ /dev/null @@ -1,3 +0,0 @@ -.. title:: Ouster SDK Changelog - -.. include:: ../../CHANGELOG.rst diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/reference/lidar-scan.rst b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/reference/lidar-scan.rst deleted file mode 100644 index 3fa38633..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/reference/lidar-scan.rst +++ /dev/null @@ -1,236 +0,0 @@ -.. _lidar-scan: - -============== -Lidar Scan API -============== - -In this reference document, we explain a core concept in both C++ and Python, and will often link -function names in the order (``C++ class/function``, ``Python class/function``). For -langauge-specific usage and running example code, please see the :ref:`Python LidarScan examples` -or the :ref:`C++ LidarScan examples`. - -The ``LidarScan`` class (:cpp:class:`ouster::LidarScan`, :py:class:`.LidarScan`) batches lidar -packets by full rotations into accessible fields of the appropriate type. The ``LidarScan`` also -allows for easy projection of the batched data into Cartesian coordinates, producing point clouds. - -.. todo:: - Update so that all code-tabs are actually literalincludes from examples somewhere - Currently group tabs are broken since they do not match between tabs and code-tabs - - -Creating the LidarScan -====================== - -A ``LidarScan`` (:cpp:class:`ouster::LidarScan`, :py:class:`.LidarScan`) contains fields of data -specified at its initialization either through a lidar profile or a specific list of fields: - -.. tabs:: - - .. code-tab:: python - - scan = LidarScan(h, w, info.format.udp_profile_lidar) - - .. tab:: C++ - - .. literalinclude:: /../examples/lidar_scan_example.cpp - :language: cpp - :start-after: [doc-stag-lidarscan-profile-constructor] - :end-before: [doc-etag-lidarscan-profile-constructor] - :dedent: - -Since each ``LidarScan`` corresponds to a single frame (and is batched accordingly), you can access -the ``frame_id`` with simply: - -.. tabs:: - - .. code-tab:: python - - frame_id = scan.frame_id # frame_id is an int - - .. code-tab:: c++ - - int32_t frame_id = scan.frame_id - -In addition to ``frame_id`` and the fields specified at initialization, a ``LidarScan`` also -contains the column header information: ``timestamp``, ``status``, ``measurement_id``. These are -aggregated from each measurement block into W-element arrays, which are represented as an -``Eigen::Array`` and a ``numpy.ndarray`` in C++ and Python respectively. Note that if you use set -the sensor configuration parameter ``azimuth_window`` to something less than the full width, the -values in the header outside the azimuth window will be 0'd out accordingly. - -.. tabs:: - - .. code-tab:: python - - # each of these has as many entries as there are columns in the scan - ts_0 = scan.timestamp[0] - measurement_id_0 = scan.measurement_id[0] - status_0 = status[0] - - - .. tab:: C++ - - .. literalinclude:: /../examples/lidar_scan_example.cpp - :language: cpp - :start-after: [doc-stag-lidarscan-cpp-headers] - :end-before: [doc-etag-lidarscan-cpp-headers] - :dedent: - -For any field contained by a ``LidarScan`` scan, you can access that field in the following way: - -.. tabs:: - - .. code-tab:: python - - ranges = scan.field(client.ChanField.RANGE) - ranges2 = scan.field(client.ChanField.RANGE2) - reflectivity = scan.field(client.ChanField.REFLECTIVITY) - reflectivity2 = scan.field(client.ChanField.REFLECTIVITY2) - - .. tab:: C++ - - .. literalinclude:: /../examples/lidar_scan_example.cpp - :language: cpp - :start-after: [doc-stag-lidarscan-cpp-fields] - :end-before: [doc-etag-lidarscan-cpp-fields] - :dedent: - -Finally, the fields of an existing ``LidarScan`` can be found by accessing the ``fields`` of the -scan through an iterator: - -.. tabs:: - - .. tab:: Python - - .. literalinclude:: /../python/src/ouster/sdk/examples/pcap.py - :language: python - :start-after: [doc-stag-pcap-query-scan] - :end-before: [doc-etag-pcap-query-scan] - :dedent: - - .. tab:: C++ - - .. literalinclude:: /../examples/lidar_scan_example.cpp - :language: cpp - :start-after: [doc-stag-cpp-scan-iter] - :end-before: [doc-etag-cpp-scan-iter] - :dedent: - -.. note:: - - The units of a particular field from a ``LidarScan`` are consistent even - when you use lidar profiles which scale the returned data from the sensor. - This is because the LidarScan will reverse the scaling for you when - parsing. For example, the RANGE field on a LidarScan constructed with the - low data rate profile will be in millimeters even though the return from - the sensor is given in 8mm increments. - -Running the above code on a sample ``LidarScan`` will give you output that looks like: - -.. include:: /python/examples/lidar-scan.rst - :start-after: [start-query-scan-result] - :end-before: [end-query-scan-result] - -Now that we know how to create the ``LidarScan`` and access its contents, let's see what -we can do with it! - - -Projecting into Cartesian Coordinates -===================================== - -To facilitate working with 3D points, you can create a pre-computed XYZ look-up table ``XYZLut`` -(:cpp:struct:`ouster::XYZLut`, :py:func:`.client.XYZLut`) on the appropriate metadata, which you can -then use to project the ``RANGE`` and ``RANGE2`` fields into Cartesian coordinates efficiently. The -result of calling this function will be a point cloud represented as an array (either an -``Eigen::Array`` or a numpy array). See the respective API documentations for -:cpp:func:`ouster::cartesian` and :py:func:`.client.XYZLut` for more details. - -.. tabs:: - - .. tab:: Python - - .. literalinclude:: /../python/src/ouster/sdk/examples/client.py - :language: python - :start-after: [doc-stag-plot-xyz-points] - :end-before: [doc-etag-plot-xyz-points] - :dedent: - - .. tab:: C++ - - .. literalinclude:: /../examples/representations_example.cpp - :language: cpp - :start-after: [doc-stag-cpp-xyz] - :end-before: [doc-etag-cpp-xyz] - :dedent: - -Once you have your x, y, and z values, you can plot them easily to get something like: - -.. figure:: /images/lidar_scan_xyz_84.png - :align: center - - Point cloud from OS1 sample data (scan 84). Points colored by ``SIGNAL`` value. - -To see how to generate this image, see the :ref:`Python LidarScan examples`. - -Staggering and Destaggering -=========================== - -The default representation of ``LidarScan`` stores data in **staggered** columns, meaning -that each column contains measurements taken at a single timestamp. As the lasers flashing at each -timestamp are arranged over several different azimuths, the resulting 2D image if directly -visualized is not a natural image. - -Let's take a look at a typical **staggered** representation: - -.. figure:: /images/lidar_scan_staggered.png - :align: center - - LidarScan ``RANGE`` field visualized with :py:func:`matplotlib.pyplot.imshow()` and simple gray - color mapping for better look. - -It would be convenient to obtain a representation where the columns represent azimuth angle instead -of timestamp. For this natural 2D image, we *destagger* the relevant field of the ``LidarScan`` with -``destagger`` (:cpp:func:`ouster::destagger`, :py:func:`.client.destagger` function): - -.. todo:: fix destagger for cpp link - -.. tabs:: - - .. code-tab:: python - - ranges = scan.field(client.ChanField.REFLECTIVITY) - ranges_destaggered = client.destagger(source.metadata, range) - - .. tab:: C++ - - .. literalinclude:: /../examples/representations_example.cpp - :language: cpp - :start-after: [doc-stag-cpp-destagger] - :end-before: [doc-etag-cpp-destagger] - :dedent: - -The above code gives the scene below (see the long strip at the bottom). We've magnified two patches -for better visiblity atop. - -.. figure:: /images/lidar_scan_destaggered.png - :align: center - - **destaggered** LidarScan ``RANGE`` field - -After *destaggering*, we can see the scene contains a man on a bicycle, a few cars, and many trees. -This image now makes visual sense, and we can easily use this data in common visual task pipelines. - - -Populating LidarScans -===================== - -This reference has covered how to a ``LidarScan``, and how to access, project, and destagger its -contents. But in order for ``LidarScan`` s to be useful, we need a way to populate them with packet -data! For convenience, the Ouster Python SDK provides the :py:class:`.Scans` interface, which allows -both sampling, used in :ref:`ex-visualization-with-matplotlib`, and streaming, used in -:ref:`ex-stream`. - -Under the hood, this class batches packets into ``LidarScans``. C++ users must batch packets -themselves using the :cpp:class:`ouster::ScanBatcher` class. To get a feel for how to use it, we recommend -reading `this example on Github -`_. diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/reference/osf.rst b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/reference/osf.rst deleted file mode 100644 index 85aa8f57..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/reference/osf.rst +++ /dev/null @@ -1,60 +0,0 @@ -.. _osf: - -================== -Open Sensor Format -================== - -The *Open Sensor Format* (OSF) is an extensible file format for storing -time-series data, based on FlatBuffers_. - -Compared to pcap, it offers the following advantages: - -- Messages can more easily be randomly-accessed because an index from message - timestamp to the file offset of the chunk containing the message is contained - within the file. -- Full frames of lidar data are meant to be compressed as individual channels - (using lossless PNG-based compression by default,) resulting in smaller file - sizes for the same data. -- Sensor configuration is contained within the file. - -The Ouster SDK provides both Python and C++ methods for working with OSF files. - -Reading and writing OSF files ------------------------------ - -Reading, writing, and recording new OSF files are all possible with the Ouster -SDK and the Ouster SDK CLI. - -* :doc:`Python API examples <../python/examples/osf-examples>` -* :doc:`C++ API examples <../cpp/examples/simple_examples>` -* :doc:`CLI examples <../cli/sample-sessions>` - -Getting example OSF files -------------------------- - -The :doc:`sample data page <../sample-data>` has instructions for obtaining sample datasets. Sample -datasets are availble in OSF format in addition to pcap. - -OSF format details ------------------- - -Typically, users of the Ouster SDK won't have to worry about the implementation -details of OSF. The following is a very basic overview of the structure of an -OSF file. - -An OSF file generally contains the following: - -#. A header, which contains the location of the OSF metadata. -#. A series of "chunks", each of which contain one or more messages, typically containing lidar scans - see :ref:`LidarScan reference `. - -#. Metadata, a collection of generic buffers usually containing the following: - - a. An index of chunks (meant to provide the file offset, in bytes, for a chunk given a timestamp.) - b. An ``ouster/v1/streaming/StreamingInfo``, which contains the start, end timestamps and number of messages for each chunk as well as some statistics. - c. An ``ouster/v1/os_sensor/LidarSensor``, which contains the configuration of the sensor used to collect the data contained in a stream of lidar scans. - d. An ``ouster/v1/os_sensor/LidarScanStream``, which indicates which fields are present in each lidar scan. - -For more details of the structure of an OSF file, consult the definition files -found in the ``ouster_osf/fb`` directory of the Ouster SDK repository. - -.. _FlatBuffers: https://flatbuffers.dev/ diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/sample-data.rst b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/sample-data.rst deleted file mode 100644 index 4caf64e4..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/sample-data.rst +++ /dev/null @@ -1,59 +0,0 @@ -============================= -Download Recorded Sample Data -============================= - -If you haven't downloaded the ``ouster-sdk``, please follow the :ref:`Python Installation -` instructions. - - -.. _sample-data-download: - -Download Data -============= - -.. - [start-download-instructions] - -You can download sample data from the `sensor documentation`_ by clicking through the Ouster Data -App links and using the Download button. After download, you should have two files, a ``.pcap`` file -and a ``.json`` file. - -We will use ``SAMPLE_DATA_PCAP_PATH`` to refer to this pcap and ``SAMPLE_DATA_JSON_PATH`` to this -json in the following. You may find it convenient to assign the paths appropriately in your -console. - -.. _sensor documentation: https://static.ouster.dev/sensor-docs/#sample-data - -.. - [end-download-instructions] - -.. note:: - - All Ouster sample data is provided under the `CC BY-NC-SA license - `_, whether obtained - through the above links or from the Ouster website. - -Visualize It! -============= - -If you've installed the ``ouster-sdk`` (see :ref:`Python Installation `) then -you're all set to visualize with:: - - $ ouster-cli source $SAMPLE_DATA_PCAP_PATH [--meta $SAMPLE_DATA_JSON_PATH] viz - -You should get a view similar to: - -.. figure:: /images/ouster-viz.png - :align: center - - Ouster SDK CLI ``ouster-cli source viz`` visualization of OS1 128 sample data - -You can control your visualizer with mouse and keyboard: - -.. include:: /python/viz/viz-run.rst - :start-after: [start-simple-viz-keymap] - :end-before: [end-simple-viz-keymap] - -Congratulations! You've installed and visualized with the Ouster Python SDK! - -For more on Ouster SDK CLI viz, please see :doc:`python/viz/viz-run` diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/versions.json b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/versions.json deleted file mode 100644 index 152af2c2..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/docs/versions.json +++ /dev/null @@ -1,52 +0,0 @@ -[ - { - "version": "0.11.0", - "tags": { - "sdkx": "sdk/0.11.0", - "sdk": "sdk/0.11.0" - } - }, - { - "version": "0.10.0", - "tags": { - "sdkx": "sdk/0.10.0", - "sdk": "20231031" - } - }, - { - "version": "0.9.0", - "tags": { - "sdkx": "sdk/0.9.0", - "sdk": "sdk/0.9.0" - } - }, - { - "version": "0.8.1", - "tags": { - "sdkx": "sdk/20230403", - "sdk": "20230403" - } - }, - { - "version": "0.7.1", - "tags": { - "sdkx": "sdk/20230114", - "sdk": "20230114" - } - }, - { - "version": "0.5.1", - "tags": { - "sdkx": "sdk/20220826", - "sdk": "20220826" - } - }, - { - "version": "0.4.0", - "tags": { - "sdkx": "sdk/20220608", - "sdk": "20220608" - } - } -] - diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/CMakeLists.txt b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/CMakeLists.txt deleted file mode 100644 index 925019bb..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/CMakeLists.txt +++ /dev/null @@ -1,38 +0,0 @@ -add_executable(client_example client_example.cpp) -target_link_libraries(client_example PRIVATE OusterSDK::ouster_client) - -add_executable(async_client_example async_client_example.cpp) -target_link_libraries(async_client_example PRIVATE OusterSDK::ouster_client) - -add_executable(mtp_client_example mtp_client_example.cpp) -target_link_libraries(mtp_client_example PRIVATE OusterSDK::ouster_client) - -add_executable(config_example config_example.cpp) -target_link_libraries(config_example PRIVATE OusterSDK::ouster_client) - -if(TARGET OusterSDK::ouster_pcap) - add_executable(lidar_scan_example lidar_scan_example.cpp helpers.cpp) - target_link_libraries(lidar_scan_example PRIVATE OusterSDK::ouster_client OusterSDK::ouster_pcap) - - add_executable(representations_example representations_example.cpp helpers.cpp) - target_link_libraries(representations_example PRIVATE OusterSDK::ouster_client OusterSDK::ouster_pcap) -else() - message(STATUS "No ouster_pcap library available; skipping examples") -endif() - -if(TARGET OusterSDK::ouster_osf) - add_executable(osf_reader_example osf_reader_example.cpp) - target_link_libraries(osf_reader_example PRIVATE OusterSDK::ouster_osf) - - add_executable(osf_writer_example osf_writer_example.cpp) - target_link_libraries(osf_writer_example PRIVATE OusterSDK::ouster_osf) -else() - message(STATUS "No ouster_osf library available; skipping examples") -endif() - -if(TARGET OusterSDK::ouster_viz) - add_executable(viz_example viz_example.cpp) - target_link_libraries(viz_example PRIVATE OusterSDK::ouster_client OusterSDK::ouster_viz) -else() - message(STATUS "No ouster_viz library available; skipping examples") -endif() diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/async_client_example.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/async_client_example.cpp deleted file mode 100644 index 3ca55054..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/async_client_example.cpp +++ /dev/null @@ -1,250 +0,0 @@ -/** - * Copyright (c) 2023, Ouster, Inc. - * All rights reserved. - */ - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "ouster/client.h" -#include "ouster/impl/build.h" -#include "ouster/lidar_scan.h" -#include "ouster/types.h" - -using namespace ouster; - -const size_t N_SCANS = 5; -const size_t UDP_BUF_SIZE = 65536; - -void FATAL(const char* msg) { - std::cerr << msg << std::endl; - std::exit(EXIT_FAILURE); -} - -/* - * Display some stats about the captured Lidar Scan - */ -void display_scan_summary(const LidarScan& scan); - -/* - * Write output to CSV files. The output can be viewed in a point cloud - * viewer like CloudCompare: - * - * [0] https://github.com/cloudcompare/cloudcompare - */ -void write_cloud(const std::string& file_path, const LidarScan::Points& cloud); - -int main(int argc, char* argv[]) { - if (argc != 2 && argc != 3) { - std::cerr << "Version: " << ouster::SDK_VERSION_FULL << " (" - << ouster::BUILD_SYSTEM << ")" - << "\n\nUsage: async_client_example " - "[]" - "\n\n is optional: leave blank for " - "automatic destination detection" - << std::endl; - - return argc == 1 ? EXIT_SUCCESS : EXIT_FAILURE; - } - - // Limit ouster_client log statements to "info" and direct the output to log - // file rather than the console (default). - sensor::init_logger("info", "ouster.log"); - - std::cerr << "Ouster client example " << ouster::SDK_VERSION << std::endl; - /* - * The sensor client consists of the network client and a library for - * reading and working with data. - * - * The network client supports reading and writing a limited number of - * configuration parameters and receiving data without working directly with - * the socket APIs. See the `client.h` for more details. The minimum - * required parameters are the sensor hostname/ip and the data destination - * hostname/ip. - */ - const std::string sensor_hostname = argv[1]; - const std::string data_destination = (argc == 3) ? argv[2] : ""; - - std::cerr << "Connecting to \"" << sensor_hostname << "\"...\n"; - - auto handle = sensor::init_client(sensor_hostname, data_destination); - if (!handle) FATAL("Failed to connect"); - std::cerr << "Connection to sensor succeeded" << std::endl; - - /* - * Configuration and calibration parameters can be queried directly from the - * sensor. These are required for parsing the packet stream and calculating - * accurate point clouds. - */ - std::cerr << "Gathering metadata..." << std::endl; - auto metadata = sensor::get_metadata(*handle); - - // Raw metadata can be parsed into a `sensor_info` struct - sensor::sensor_info info = sensor::parse_metadata(metadata); - - size_t w = info.format.columns_per_frame; - size_t h = info.format.pixels_per_column; - - ouster::sensor::ColumnWindow column_window = info.format.column_window; - - std::cerr << " Firmware version: " << info.fw_rev - << "\n Serial number: " << info.sn - << "\n Product line: " << info.prod_line - << "\n Scan dimensions: " << w << " x " << h - << "\n Column window: [" << column_window.first << ", " - << column_window.second << "]" << std::endl; - - // A LidarScan holds lidar data for an entire rotation of the device - LidarScan scan{w, h, info.format.udp_profile_lidar}; - - // pre-compute a table for efficiently calculating point clouds from - // range - XYZLut lut = ouster::make_xyz_lut(info); - // A an array of points to hold the projected representation of the scan - LidarScan::Points cloud; - - // A ScanBatcher can be used to batch packets into scans - sensor::packet_format pf = sensor::get_format(info); - ScanBatcher batch_to_scan(info.format.columns_per_frame, pf); - - /* - * The network client provides some convenience wrappers around socket APIs - * to facilitate reading lidar and IMU data from the network. It is also - * possible to configure the sensor offline and read data directly from a - * UDP socket. - */ - - // buffer to store raw packet data - auto packet_buf = std::make_unique(UDP_BUF_SIZE); - - /* - In this example we spin two threads one to receive lidar packets while the - other thread accumlates lidar packets of the same frame into a LidarScan - object, computes the xyz coordinates and then writes these coordiantes into - a file. The example is a show case of utilizing threads to decouple - reception of packets from processing the point cloud. For a more complete - examples on how to efficient stream and process lidar packets please refer - to the async_udp_source_example.cpp or the ouster_ros driver implementation - */ - size_t n_scans = 0; // counter to track the number of complete scans that - // we have successfully captured and processed. - std::mutex mtx; - std::condition_variable receiving_cv; - std::condition_variable processing_cv; - bool packet_processed = true; - - std::thread packet_receiving_thread([&]() { - while (n_scans < N_SCANS) { - // wait until sensor data is available - sensor::client_state st = sensor::poll_client(*handle); - - // check for timeout - if (st == sensor::TIMEOUT) FATAL("Client has timed out"); - - if (st & sensor::EXIT) FATAL("Exit was requested"); - - // check for error status - if (st & sensor::CLIENT_ERROR) - FATAL("Sensor client returned error state!"); - - // check for lidar data, read a packet and add it to the current - // batch - if (st & sensor::LIDAR_DATA) { - std::unique_lock lock(mtx); - receiving_cv.wait( - lock, [&packet_processed] { return packet_processed; }); - if (!sensor::read_lidar_packet(*handle, packet_buf.get(), pf)) { - FATAL("Failed to read a packet of the expected size!"); - } - packet_processed = false; - processing_cv.notify_one(); - } - - // check if IMU data is available (but don't do anything with it) - if (st & sensor::IMU_DATA) { - std::unique_lock lock(mtx); - receiving_cv.wait( - lock, [&packet_processed] { return packet_processed; }); - sensor::read_imu_packet(*handle, packet_buf.get(), pf); - // we are not going to processor imu data - // so we will keep packet_processed set to true - } - } - }); - - std::thread packet_processing_thread([&]() { - while (n_scans < N_SCANS) { - std::unique_lock lock(mtx); - processing_cv.wait( - lock, [&packet_processed] { return !packet_processed; }); - // batcher will return "true" when the current scan is complete - if (batch_to_scan(packet_buf.get(), scan)) { - // retry until we receive a full set of valid measurements - // (accounting for azimuth_window settings if any) - if (scan.complete(info.format.column_window)) { - display_scan_summary(scan); - std::cerr << "Computing point cloud... " << std::endl; - cloud = ouster::cartesian(scan, lut); - std::string file_name = - "cloud_" + std::to_string(n_scans) + ".csv"; - write_cloud(file_name, cloud); - ++n_scans; - } - } - - packet_processed = true; - receiving_cv.notify_one(); - } - }); - - packet_receiving_thread.join(); - packet_processing_thread.join(); - - std::cerr << "done" << std::endl; - - return EXIT_SUCCESS; -} - -void display_scan_summary(const LidarScan& scan) { - // channel fields can be queried as well - auto n_valid_first_returns = (scan.field(sensor::RANGE) != 0).count(); - - // LidarScan also provides access to header information such as - // status and timestamp - auto status = scan.status(); - auto it = std::find_if(status.data(), status.data() + status.size(), - [](const uint32_t s) { - return (s & 0x01); - }); // find first valid status - if (it != status.data() + status.size()) { - auto ts_ms = std::chrono::duration_cast( - std::chrono::nanoseconds(scan.timestamp()( - it - status.data()))); // get corresponding timestamp - - std::cerr << " Frame no. " << scan.frame_id << " with " - << n_valid_first_returns << " valid first returns at " - << ts_ms.count() << " ms" << std::endl; - } -} - -void write_cloud(const std::string& file_path, const LidarScan::Points& cloud) { - std::ofstream out; - out.open(file_path); - out << std::fixed << std::setprecision(4); - - // write each point, filtering out points without returns - for (int i = 0; i < cloud.rows(); i++) { - auto xyz = cloud.row(i); - if (!xyz.isApproxToConstant(0.0)) - out << xyz(0) << ", " << xyz(1) << ", " << xyz(2) << std::endl; - } - - out.close(); - std::cerr << " Wrote " << file_path << std::endl; -} \ No newline at end of file diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/client_example.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/client_example.cpp deleted file mode 100644 index 65284602..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/client_example.cpp +++ /dev/null @@ -1,204 +0,0 @@ -/** - * Copyright (c) 2022, Ouster, Inc. - * All rights reserved. - */ - -#include -#include -#include -#include -#include - -#include "ouster/client.h" -#include "ouster/impl/build.h" -#include "ouster/lidar_scan.h" -#include "ouster/types.h" - -using namespace ouster; - -const size_t N_SCANS = 5; - -void FATAL(const char* msg) { - std::cerr << msg << std::endl; - std::exit(EXIT_FAILURE); -} - -int main(int argc, char* argv[]) { - if (argc != 2 && argc != 3) { - std::cerr - << "Version: " << ouster::SDK_VERSION_FULL << " (" - << ouster::BUILD_SYSTEM << ")" - << "\n\nUsage: client_example []" - "\n\n is optional: leave blank for " - "automatic destination detection" - << std::endl; - - return argc == 1 ? EXIT_SUCCESS : EXIT_FAILURE; - } - - // Limit ouster_client log statements to "info" - sensor::init_logger("info"); - - std::cerr << "Ouster client example " << ouster::SDK_VERSION << std::endl; - /* - * The sensor client consists of the network client and a library for - * reading and working with data. - * - * The network client supports reading and writing a limited number of - * configuration parameters and receiving data without working directly with - * the socket APIs. See the `client.h` for more details. The minimum - * required parameters are the sensor hostname/ip and the data destination - * hostname/ip. - */ - const std::string sensor_hostname = argv[1]; - const std::string data_destination = (argc == 3) ? argv[2] : ""; - - std::cerr << "Connecting to \"" << sensor_hostname << "\"...\n"; - - auto handle = sensor::init_client(sensor_hostname, data_destination); - if (!handle) FATAL("Failed to connect"); - std::cerr << "Connection to sensor succeeded" << std::endl; - - /* - * Configuration and calibration parameters can be queried directly from the - * sensor. These are required for parsing the packet stream and calculating - * accurate point clouds. - */ - std::cerr << "Gathering metadata..." << std::endl; - auto metadata = sensor::get_metadata(*handle, 10, false); - - // Raw metadata can be parsed into a `sensor_info` struct - sensor::sensor_info info = sensor::parse_metadata(metadata); - - size_t w = info.format.columns_per_frame; - size_t h = info.format.pixels_per_column; - - ouster::sensor::ColumnWindow column_window = info.format.column_window; - - // The dedicated firmware_version_from_metadata API works across firmwares - auto fw_ver = sensor::firmware_version_from_metadata(metadata); - - std::cerr << " Firmware version: " << to_string(fw_ver) - << "\n Serial number: " << info.sn - << "\n Product line: " << info.prod_line - << "\n Scan dimensions: " << w << " x " << h - << "\n Column window: [" << column_window.first << ", " - << column_window.second << "]" << std::endl; - - // A LidarScan holds lidar data for an entire rotation of the device - std::vector scans{ - N_SCANS, LidarScan{w, h, info.format.udp_profile_lidar}}; - - // A ScanBatcher can be used to batch packets into scans - sensor::packet_format pf = sensor::get_format(info); - ScanBatcher batch_to_scan(info.format.columns_per_frame, pf); - - /* - * The network client provides some convenience wrappers around socket APIs - * to facilitate reading lidar and IMU data from the network. It is also - * possible to configure the sensor offline and read data directly from a - * UDP socket. - */ - std::cerr << "Capturing points... "; - - // buffer to store raw packet data - auto lidar_packet = sensor::LidarPacket(pf.lidar_packet_size); - auto imu_packet = sensor::ImuPacket(pf.imu_packet_size); - - for (size_t i = 0; i < N_SCANS;) { - // wait until sensor data is available - sensor::client_state st = sensor::poll_client(*handle); - - // check for error status - if (st & sensor::CLIENT_ERROR) - FATAL("Sensor client returned error state!"); - - // check for lidar data, read a packet and add it to the current batch - if (st & sensor::LIDAR_DATA) { - if (!sensor::read_lidar_packet(*handle, lidar_packet)) { - FATAL("Failed to read a packet of the expected size!"); - } - - // batcher will return "true" when the current scan is complete - if (batch_to_scan(lidar_packet, scans[i])) { - // retry until we receive a full set of valid measurements - // (accounting for azimuth_window settings if any) - if (scans[i].complete(info.format.column_window)) i++; - } - } - - // check if IMU data is available (but don't do anything with it) - if (st & sensor::IMU_DATA) { - sensor::read_imu_packet(*handle, imu_packet); - } - } - std::cerr << "ok" << std::endl; - - /* - * The example code includes functions for efficiently and accurately - * computing point clouds from range measurements. LidarScan data can - * also be accessed directly using the Eigen[0] linear algebra library. - * - * [0] http://eigen.tuxfamily.org - */ - std::cerr << "Computing point clouds... " << std::endl; - - // pre-compute a table for efficiently calculating point clouds from - // range - XYZLut lut = ouster::make_xyz_lut(info); - std::vector clouds; - - for (const LidarScan& scan : scans) { - // compute a point cloud using the lookup table - clouds.push_back(ouster::cartesian(scan, lut)); - - // channel fields can be queried as well - auto n_valid_first_returns = (scan.field(sensor::RANGE) != 0).count(); - - // LidarScan also provides access to header information such as - // status and timestamp - auto status = scan.status(); - auto it = std::find_if(status.data(), status.data() + status.size(), - [](const uint32_t s) { - return (s & 0x01); - }); // find first valid status - if (it != status.data() + status.size()) { - auto ts_ms = std::chrono::duration_cast( - std::chrono::nanoseconds(scan.timestamp()( - it - status.data()))); // get corresponding timestamp - - std::cerr << " Frame no. " << scan.frame_id << " with " - << n_valid_first_returns << " valid first returns at " - << ts_ms.count() << " ms" << std::endl; - } - } - - /* - * Write output to CSV files. The output can be viewed in a point cloud - * viewer like CloudCompare: - * - * [0] https://github.com/cloudcompare/cloudcompare - */ - std::cerr << "Writing files... " << std::endl; - - int file_ind = 0; - std::string file_base{"cloud_"}; - for (const LidarScan::Points& cloud : clouds) { - std::string filename = file_base + std::to_string(file_ind++) + ".csv"; - std::ofstream out; - out.open(filename); - out << std::fixed << std::setprecision(4); - - // write each point, filtering out points without returns - for (int i = 0; i < cloud.rows(); i++) { - auto xyz = cloud.row(i); - if (!xyz.isApproxToConstant(0.0)) - out << xyz(0) << ", " << xyz(1) << ", " << xyz(2) << std::endl; - } - - out.close(); - std::cerr << " Wrote " << filename << std::endl; - } - - return EXIT_SUCCESS; -} diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/config_example.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/config_example.cpp deleted file mode 100644 index 92e5b778..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/config_example.cpp +++ /dev/null @@ -1,117 +0,0 @@ -/** - * Copyright (c) 2022, Ouster, Inc. - * All rights reserved. - * - * For more comprehensive explanation, see the Ouster SDK Docs - * - * Note: This is an example meant to demonstrate use of the CPP Sensor - * Configuration API. Users who merely need to set parameters without doing so - * programmatically may find it easier to do so using the sensor homepage at - * .local or using our Python SDK, available as ouster-sdk on PyPi. - */ -#include -#include - -#include "ouster/client.h" -#include "ouster/impl/build.h" - -using namespace ouster; - -int main(int argc, char* argv[]) { - if (argc != 2) { - std::cerr << "Version: " << ouster::SDK_VERSION_FULL << " (" - << ouster::BUILD_SYSTEM << ")" - << "\n\nUsage: config_example " - << std::endl; - - return argc == 1 ? EXIT_SUCCESS : EXIT_FAILURE; - } - - const std::string sensor_hostname = argv[1]; - - // 1. Get the current config on the sensor - std::cerr << "1. Get original config of sensor... "; - - //! [doc-stag-cpp-get-config] - sensor::sensor_config original_config; - if (!sensor::get_config(sensor_hostname, original_config)) { - std::cerr << "..error: could not connect to sensor!" << std::endl; - return EXIT_FAILURE; - } - //! [doc-etag-cpp-get-config] - std::cerr << "success! Got original config\nOriginal config of sensor:\n" - << to_string(original_config) << std::endl; - - // 2. Make an empty sensor config and set a few config parameters - std::cerr << "\n2. Make new config and set sensor to it... "; - //! [doc-stag-cpp-make-config] - sensor::sensor_config config; - config.azimuth_window = std::make_pair(90000, 270000); - config.ld_mode = sensor::lidar_mode::MODE_512x10; - - // If relevant, use config_flag to set udp dest automatically - uint8_t config_flags = 0; - const bool udp_dest_auto = true; // whether or not to use auto destination - const bool persist = - false; // whether or not we will persist the settings on the sensor - - if (udp_dest_auto) config_flags |= ouster::sensor::CONFIG_UDP_DEST_AUTO; - if (persist) config_flags |= ouster::sensor::CONFIG_PERSIST; - //! [doc-etag-cpp-make-config] - - if (!sensor::set_config(sensor_hostname, config, config_flags)) { - std::cerr << "..error: could not connect to sensor" << std::endl; - return EXIT_FAILURE; - } - std::cerr << "..success! Updated sensor to new config" << std::endl; - - // 3. Get the config from sensor after update - std::cerr << "\n3. Get back updated sensor config... "; - sensor::sensor_config new_config; - if (!sensor::get_config(sensor_hostname, new_config)) { - std::cerr << "..error: could not connect to sensor" << std::endl; - return EXIT_FAILURE; - } else { - std::cerr << "..success! Got updated config" << std::endl; - } - - // Confirm that only what we wanted to change changed - assert(original_config != new_config); - assert(new_config.azimuth_window == config.azimuth_window); - assert(new_config.ld_mode == config.ld_mode); - - std::cerr << "Updated config: \n" << to_string(new_config) << std::endl; - - // 4. You cannot set the udp_dest flag while simultaneously setting - // config.udp_dest Will throw an invalid_argument if you do - std::cerr << "\n4. Test setting udp_dest and config.udp_dest " - "simultaneously... "; - config.udp_dest = "100.100.100.100"; - try { - if (sensor::set_config(sensor_hostname, config, config_flags)) { - // should not happen - std::cerr << "..error: unexpected failure of set_config example" - << std::endl; - return EXIT_FAILURE; - } - } catch (const std::invalid_argument&) { - // expected result - std::cerr << "..success! Got expected failure to set udp_dest while " - "auto flag is set." - << std::endl; - - } catch (const std::runtime_error& e) { - std::cerr << e.what() << std::endl; - return EXIT_FAILURE; - } - - // 5. Set the sensor back to how it started - std::cerr << "\n5. Setting sensor back to original state... "; - if (!sensor::set_config(sensor_hostname, original_config)) { - std::cerr << "..error: could not connect to sensor" << std::endl; - return EXIT_FAILURE; - } - std::cerr << "..success! Returned sensor to original state." << std::endl; - - return EXIT_SUCCESS; -} diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/helpers.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/helpers.cpp deleted file mode 100644 index e7156922..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/helpers.cpp +++ /dev/null @@ -1,48 +0,0 @@ -/** - * Copyright (c) 2022, Ouster, Inc. - * All rights reserved. - */ - -#include "helpers.h" - -#include "ouster/lidar_scan.h" -#include "ouster/os_pcap.h" -#include "ouster/types.h" - -using namespace ouster::sensor; - -constexpr std::size_t BUF_SIZE = 65536; - -void get_complete_scan( - std::shared_ptr handle, - ouster::LidarScan& scan, sensor_info& info) { - // Make sure we start at beginning - ouster::sensor_utils::replay_reset(*handle); - - // Helper variable to help us identify first full frame - int first_frame_id = 0; - - auto pf = get_format(info); - ouster::ScanBatcher batch_to_scan(info.format.columns_per_frame, pf); - - // Buffer to store raw packet data - auto packet_buf = std::make_unique(BUF_SIZE); - - ouster::sensor_utils::packet_info packet_info; - - while (ouster::sensor_utils::next_packet_info(*handle, packet_info)) { - auto packet_size = ouster::sensor_utils::read_packet( - *handle, packet_buf.get(), BUF_SIZE); - - if (packet_size == pf.lidar_packet_size && - packet_info.dst_port == info.udp_port_lidar) { - if (batch_to_scan(packet_buf.get(), scan)) { - if (first_frame_id == 0) { - // end of first frame -- assume it is incomplete and skip - first_frame_id = scan.frame_id; - } else if (first_frame_id != scan.frame_id) - break; - } - } - } -} diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/helpers.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/helpers.h deleted file mode 100644 index 4e15349e..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/helpers.h +++ /dev/null @@ -1,14 +0,0 @@ -/** - * Copyright (c) 2022, Ouster, Inc. - * All rights reserved. - */ - -#include "ouster/lidar_scan.h" -#include "ouster/os_pcap.h" -#include "ouster/types.h" - -// Fill scan with data from the 2nd scan in the pcap -// If there is no 2nd scan, scan will remain unchanged -void get_complete_scan( - std::shared_ptr handle, - ouster::LidarScan& scan, ouster::sensor::sensor_info& info); diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/lidar_scan_example.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/lidar_scan_example.cpp deleted file mode 100644 index c70ed3ff..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/lidar_scan_example.cpp +++ /dev/null @@ -1,150 +0,0 @@ -/** - * Copyright (c) 2022, Ouster, Inc. - * All rights reserved. - * - * This file contains example code for working with the LidarScan class of the - * C++ Ouster SDK. Please see the sdk docs at static.ouster.dev for clearer - * explanations. - */ -#include -#include -#include -#include - -#include "helpers.h" -#include "ouster/client.h" -#include "ouster/impl/build.h" -#include "ouster/lidar_scan.h" -#include "ouster/os_pcap.h" -#include "ouster/types.h" - -using namespace ouster::sensor; - -int main(int argc, char* argv[]) { - if (argc != 3) { - std::cerr << "Version: " << ouster::SDK_VERSION_FULL << " (" - << ouster::BUILD_SYSTEM << ")" - << "\n\nUsage: lidar_scan_example " - << std::endl; - - return argc == 1 ? EXIT_SUCCESS : EXIT_FAILURE; - } - - const std::string pcap_file = argv[1]; - const std::string json_file = argv[2]; - - auto handle = ouster::sensor_utils::replay_initialize(pcap_file); - auto info = metadata_from_json(json_file); - - size_t w = info.format.columns_per_frame; - size_t h = info.format.pixels_per_column; - - // Specifiying only w and h for lidar scan creates one using the LEGACY udp - // profile - //! [doc-stag-lidarscan-default-constructor] - auto legacy_scan = ouster::LidarScan(w, h); - //! [doc-etag-lidarscan-default-constructor] - - // You can also create a LidarScan by providing a lidar profile, avilable - // through the sensor_info - //! [doc-stag-lidarscan-profile-constructor] - auto profile_scan = ouster::LidarScan(w, h, info.format.udp_profile_lidar); - //! [doc-etag-lidarscan-profile-constructor] - - // You might have a dual returns sensor, in which case your profile will - // reflect that it is a dual return profile: - //! [doc-stag-lidarscan-dual-profile-constructor] - auto dual_returns_scan = ouster::LidarScan( - w, h, UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL); - //! [doc-etag-lidarscan-dual-profile-constructor] - - //! [doc-stag-lidarscan-reduced-slots] - // Finally, you can construct by specifying fields directly - static const std::array, 2> - reduced_slots{{{ChanField::RANGE, ChanFieldType::UINT32}, - {ChanField::NEAR_IR, ChanFieldType::UINT16}}}; - auto reduced_fields_scan = - ouster::LidarScan(w, h, reduced_slots.begin(), reduced_slots.end()); - //! [doc-etag-lidarscan-reduced-slots] - - std::cerr << "Creating scans from pcap... "; - - get_complete_scan(handle, profile_scan, info); - get_complete_scan(handle, reduced_fields_scan, info); - - if (info.format.udp_profile_lidar != - UDPProfileLidar::PROFILE_RNG15_RFL8_NIR8) { - get_complete_scan(handle, legacy_scan, info); - } - - if (info.format.udp_profile_lidar == - UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL) { - get_complete_scan(handle, dual_returns_scan, info); - } - std::cerr << "Scans created!" << std::endl; - - ouster::sensor_utils::replay_uninitialize(*handle); - - // Headers - auto frame_id = profile_scan.frame_id; - //! [doc-stag-lidarscan-cpp-headers] - auto ts = profile_scan.timestamp(); - auto status = profile_scan.status(); - auto measurement_id = profile_scan.measurement_id(); - //! [doc-etag-lidarscan-cpp-headers] - - // to access a field: - //! [doc-stag-lidarscan-cpp-fields] - auto range = profile_scan.field(ChanField::RANGE); - //! [doc-etag-lidarscan-cpp-fields] - - // On dual returns, second returns are often the same field name with 2 - // appended: - auto range2 = dual_returns_scan.field(ChanField::RANGE2); - - std::cerr - << "\nPrinting first element of received scan headers\n\tframe_id : " - << frame_id << "\n\tts : " << ts(0) << "\n\tstatus : " << status(0) - << "\n\tmeasurement_id : " << measurement_id(0) << "\n " << std::endl; - - std::cerr << "\nPrinting range of pixel at 15th row and 498th " - "column...\n\trange(15, 498): " - << range(15, 498) << std::endl; - - if (info.format.udp_profile_lidar == - ouster::sensor::UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL) { - std::cerr << "\nPrinting range of second return at 15th row and 498th " - "column...\n\trange(15, 498): " - << range2(15, 498) << std::endl; - } - // Let's see what happens if you try to access a field that isn't in a - // LidarScan - std::cerr << "Accessing field that isn't available..."; - try { - auto signal_field = reduced_fields_scan.field(ChanField::SIGNAL); - std::cerr << signal_field(0, 0) << std::endl; - } catch (const std::out_of_range&) { - std::cerr << " ..received expected out of range error. Continuing..." - << std::endl; - } - - std::cerr << "\nLet's see what's in each of these scans!" << std::endl; - // If you want to iterate through the available fields, you can use an - // iterator - auto print_el = [](ouster::LidarScan& scan, std::string label) { - std::cerr << "Available fields in " << label << "...\n"; - //! [doc-stag-cpp-scan-iter] - for (auto it = scan.begin(); it != scan.end(); it++) { - auto field = it->first; - // auto field_type = it->second; - std::cerr << "\t" << to_string(field) << "\n "; - } - //! [doc-etag-cpp-scan-iter] - std::cerr << std::endl; - }; - - print_el(legacy_scan, std::string("Legacy Scan")); - print_el(profile_scan, std::string("Profile Scan")); - print_el(dual_returns_scan, std::string("Dual Returns Scan")); - print_el(reduced_fields_scan, std::string("Reduced fields Scan")); -} diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/linking_example/CMakeLists.txt b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/linking_example/CMakeLists.txt deleted file mode 100644 index e65254f5..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/linking_example/CMakeLists.txt +++ /dev/null @@ -1,11 +0,0 @@ -cmake_minimum_required(VERSION 3.21) -project(pcap_test) - -find_package(OusterSDK REQUIRED) - -add_executable(pcap_test main.cpp) - -target_link_libraries(pcap_test - OusterSDK::ouster_client - OusterSDK::ouster_pcap -) diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/linking_example/Dockerfile b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/linking_example/Dockerfile deleted file mode 100644 index 19a6809a..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/linking_example/Dockerfile +++ /dev/null @@ -1,46 +0,0 @@ -FROM ubuntu:22.04 - -RUN apt-get update && apt-get install -y \ - libeigen3-dev \ - libjsoncpp-dev \ - libtins-dev \ - libpcap-dev \ - libcurl4-openssl-dev \ - git \ - build-essential \ - cmake \ - libspdlog-dev - -ENV WORKSPACE=/root -ENV INSTALL_DIR="/usr/local" -COPY . $WORKSPACE/ -RUN cd $WORKSPACE && \ - cmake -DCMAKE_INSTALL_PREFIX=$INSTALL_DIR -DBUILD_PCAP=ON -DBUILD_OSF=OFF -DBUILD_VIZ=OFF -DBUILD_EXAMPLES=OFF -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTING=OFF . && \ - cmake --build . --target install - -FROM ubuntu:22.04 - -RUN apt-get update && apt-get install -y \ - libeigen3-dev \ - libjsoncpp-dev \ - libtins-dev \ - libpcap-dev \ - libcurl4-openssl-dev \ - git \ - build-essential \ - cmake \ - libspdlog-dev - -ENV WORKSPACE=/root -ENV INSTALL_DIR="/usr/local" - -COPY --from=0 $INSTALL_DIR $INSTALL_DIR -COPY tests/pcaps/OS-2-32-U0_v2.0.0_1024x10.pcap examples/linking_example/CMakeLists.txt \ - examples/linking_example/main.cpp $WORKSPACE/ - -RUN export CMAKE_PREFIX_PATH="$INSTALL_DIR" &&\ - mkdir -p $WORKSPACE/build &&\ - cd $WORKSPACE/build &&\ - cmake $WORKSPACE && cmake --build . - -CMD $WORKSPACE/build/pcap_test /root/OS-2-32-U0_v2.0.0_1024x10.pcap diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/linking_example/example.bash b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/linking_example/example.bash deleted file mode 100755 index 2f9492ce..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/linking_example/example.bash +++ /dev/null @@ -1,15 +0,0 @@ -#! /bin/bash - -set -e -currentDir="$(cd $(dirname $0) && pwd)" -baseDir=$currentDir/../.. -tempDir="$(mktemp -d)" - -trap 'rm -rf $tempDir' EXIT -trap 'echo \*\*\* ERROR on line: $LINENO exit_code: $?' ERR - -cd $baseDir - -docker build -f $currentDir/Dockerfile --iidfile=$tempDir/iid . - -docker run $(cat $tempDir/iid) diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/linking_example/main.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/linking_example/main.cpp deleted file mode 100644 index 2243ee3b..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/linking_example/main.cpp +++ /dev/null @@ -1,17 +0,0 @@ -#include -#include -#include - -#include -#include - -int main(int argc, char** argv) { - if (argc != 2) { - std::cerr << "\n\nUsage: pcap_test " << std::endl; - - return argc == 1 ? EXIT_SUCCESS : EXIT_FAILURE; - } - const std::string pcap_file = argv[1]; - auto stream_info = ouster::sensor_utils::get_stream_info(pcap_file); - std::cout << *stream_info << std::endl; -} diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/linking_example/readme.rst b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/linking_example/readme.rst deleted file mode 100644 index 6d57f6c8..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/linking_example/readme.rst +++ /dev/null @@ -1,10 +0,0 @@ -============================ -Simple ouster_pcap Example -============================ - -This is a simple example on how to build and link a cpp project with the ouster_pcap -library. - -.. code-block:: bash - ./example.bash - diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/mtp_client_example.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/mtp_client_example.cpp deleted file mode 100644 index 90287ace..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/mtp_client_example.cpp +++ /dev/null @@ -1,84 +0,0 @@ -/** - * Copyright (c) 2023, Ouster, Inc. - * All rights reserved. - */ - -#include - -#include "ouster/client.h" - -using namespace ouster; - -const size_t UDP_BUF_SIZE = 65536; - -int main(int argc, char* argv[]) { - if (argc != 3) { - std::cerr << "\n\nUsage: " - << std::endl; - - return argc == 1 ? EXIT_SUCCESS : EXIT_FAILURE; - } - - const std::string sensor_hostname = argv[1]; - - bool main = false; - - if (std::string(argv[2]) == "main") { - main = true; - } else if (std::string(argv[2]) == "secondary") { - main = false; - } else { - std::cerr << "Invalid second argument: " << argv[2] - << " only values of main or secondary are valid" << std::endl; - return EXIT_FAILURE; - } - - sensor::sensor_config config; - if (!sensor::get_config(sensor_hostname, config)) { - std::cerr << "Failed to get sensor config" << std::endl; - return EXIT_FAILURE; - } - config.udp_dest = "239.201.201.201"; - if (sensor::in_multicast(config.udp_dest.value())) { - std::cerr << "In multicast" << std::endl; - } else { - std::cerr << "Not a multicast address" << std::endl; - return -1; - } - - std::shared_ptr cli = - sensor::mtp_init_client(sensor_hostname, config, "", main); - - if (!cli) { - std::cerr << "Failed to initialize sensor" << std::endl; - return EXIT_FAILURE; - } - - auto metadata = sensor::get_metadata(*cli); - sensor::sensor_info info = sensor::parse_metadata(metadata); - sensor::packet_format pf = sensor::get_format(info); - auto packet_buf = std::make_unique(UDP_BUF_SIZE); - - bool done = false; - while (!done) { - sensor::client_state state = sensor::poll_client(*cli); - if (state == sensor::EXIT) { - std::cerr << "caught signal, exiting" << std::endl; - done = true; - } - if (state == sensor::TIMEOUT) { - std::cerr << "Timed out" << std::endl; - continue; - } - if (state & sensor::LIDAR_DATA) { - if (sensor::read_lidar_packet(*cli, packet_buf.get(), pf)) { - std::cerr << "Read Lidar Packet" << std::endl; - } - } - if (state & sensor::IMU_DATA) { - if (sensor::read_imu_packet(*cli, packet_buf.get(), pf)) { - std::cerr << "Read IMU packet" << std::endl; - } - } - } -} \ No newline at end of file diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/osf_reader_example.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/osf_reader_example.cpp deleted file mode 100644 index 934137ac..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/osf_reader_example.cpp +++ /dev/null @@ -1,51 +0,0 @@ -/** - * Copyright (c) 2023, Ouster, Inc. - * All rights reserved. - * - * This file contains example code for working with the LidarScan class of the - * C++ Ouster SDK. Please see the sdk docs at static.ouster.dev for clearer - * explanations. - */ - -//! [doc-stag-osf-read-cpp] -#include - -#include "ouster/impl/build.h" -#include "ouster/osf/reader.h" -#include "ouster/osf/stream_lidar_scan.h" - -using namespace ouster; - -int main(int argc, char* argv[]) { - if (argc != 2) { - std::cerr << "Version: " << ouster::SDK_VERSION_FULL << " (" - << ouster::BUILD_SYSTEM << ")" - << "\n\nUsage: osf_reader_example " << std::endl; - - return (argc == 1) ? EXIT_SUCCESS : EXIT_FAILURE; - } - - const std::string osf_file = argv[1]; - - // open OSF file - osf::Reader reader(osf_file); - - // read all messages from OSF in timestamp order - for (const auto& m : reader.messages()) { - std::cout << "m.ts: " << m.ts().count() << ", m.id: " << m.id() - << std::endl; - - // In OSF file there maybe different type of messages stored, so here we - // only interested in LidarScan messages - if (m.is()) { - // Decoding LidarScan messages - auto ls = m.decode_msg(); - - // if decoded successfully just print on the screen LidarScan - if (ls) { - std::cout << "ls = " << to_string(*ls) << std::endl; - } - } - } -} -//! [doc-etag-osf-read-cpp] diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/osf_writer_example.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/osf_writer_example.cpp deleted file mode 100644 index d5f62c50..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/osf_writer_example.cpp +++ /dev/null @@ -1,40 +0,0 @@ -/** - * Copyright (c) 2024, Ouster, Inc. - * All rights reserved. - * - * This file contains example code for working with the osf::Writer class of - * the C++ Ouster SDK. Please see the sdk docs at static.ouster.dev for clearer - * explanations. - */ - -//! [doc-stag-osf-write-cpp] -#include - -#include "ouster/impl/build.h" -#include "ouster/osf/writer.h" - -using namespace ouster; - -int main(int argc, char* argv[]) { - if (argc != 2) { - std::cerr << "Version: " << ouster::SDK_VERSION_FULL << " (" - << ouster::BUILD_SYSTEM << ")" - << "\n\nUsage: osf_writer_example " << std::endl; - - return (argc == 1) ? EXIT_SUCCESS : EXIT_FAILURE; - } - - const std::string osf_file = argv[1]; - - // Start writing a 1 stream OSF file with a default initialized sensor info - osf::Writer writer( - osf_file, sensor::default_sensor_info(ouster::sensor::MODE_512x10)); - - // Instantiate a lidar scan with the expected width and height - // default_sensor_info assumes a 64 plane sensor - LidarScan scan(512, 64); - // Manipulate the scan as desired here - // Write it to file on stream 0 - writer.save(0, scan); -} -//! [doc-2tag-osf-write-cpp] diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/representations_example.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/representations_example.cpp deleted file mode 100644 index f4189153..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/representations_example.cpp +++ /dev/null @@ -1,166 +0,0 @@ -/** - * Copyright (c) 2022, Ouster, Inc. - * All rights reserved. - * - * This file contains example code for working with 2D and 3D representations of - * lidar data with the C++ Ouster SDK. Please see the sdk docs at - * static.ouster.dev for clearer explanations. - */ - -#include -#include -#include -#include - -#include "helpers.h" -#include "ouster/client.h" -#include "ouster/impl/build.h" -#include "ouster/lidar_scan.h" -#include "ouster/types.h" - -using namespace ouster; - -//! [docs-stag-x-image-form] -img_t get_x_in_image_form(const LidarScan& scan, bool destaggered, - const sensor::sensor_info& info) { - // For convenience, save w and h to variables - const size_t w = info.format.columns_per_frame; - const size_t h = info.format.pixels_per_column; - - // Get the XYZ in ouster::Points (n x 3 Eigen array) form - XYZLut lut = make_xyz_lut(info); - auto cloud = cartesian(scan.field(sensor::ChanField::RANGE), lut); - - // Access x and reshape as needed - // Note that the values in cloud.col(0) are ordered - auto x = Eigen::Map>(cloud.col(0).data(), h, w); - auto x_destaggered = destagger(x, info.format.pixel_shift_by_row); - - // Apply destagger if desired - if (!destaggered) return x; - return x_destaggered; -} -//! [docs-etag-x-image-form] -// - -int main(int argc, char* argv[]) { - if (argc != 3) { - std::cerr << "Version: " << ouster::SDK_VERSION_FULL << " (" - << ouster::BUILD_SYSTEM << ")" - << "\n\nUsage: representation_example " - << std::endl; - return argc == 1 ? EXIT_SUCCESS : EXIT_FAILURE; - } - - const std::string pcap_file = argv[1]; - const std::string json_file = argv[2]; - - auto handle = sensor_utils::replay_initialize(pcap_file); - auto info = sensor::metadata_from_json(json_file); - - size_t w = info.format.columns_per_frame; - size_t h = info.format.pixels_per_column; - - auto scan = LidarScan(w, h, info.format.udp_profile_lidar); - - std::cerr << "Reading in scan from pcap..." << std::endl; - get_complete_scan(handle, scan, info); - - // 1. Getting XYZ - std::cerr << "1. Calculating 3d Points... " << std::endl; - //! [doc-stag-cpp-xyz] - XYZLut lut = make_xyz_lut(info); - auto range = scan.field(sensor::ChanField::RANGE); - auto cloud = cartesian(range, lut); - //! [doc-etag-cpp-xyz] - // - std::cerr << "\nLet's see what the 2000th point in this cloud is... (" - << cloud(2000, 0) << ", " << cloud(2000, 1) << ", " - << cloud(2000, 2) << ")" << std::endl; - - // 2. Providing a transfomration to XYZ - // You can also make an XYZLut by specifying a special transform if you - // have a different frame you would like to be in, say if you have an - // extrinsics matrix: - Eigen::Matrix transformation = - mat4d::Identity(); - - // Let's turn it upside down and put it on a very tall pole and shift x - transformation(2, 2) = -1; - transformation(1, 1) = -1; - transformation(2, 3) = 20000; // unit is mm, so taht's 20 meters - transformation(0, 3) = 1500; // unit is mm - - transformation = transformation * info.lidar_to_sensor_transform; - std::cerr - << "2. Now we will apply this transformation to the look-up table:\n" - << transformation << std::endl; - - // Remember to apply the lidar_to_sensor_transform if your extrinsics - // matrix was between sensor coordinate system and some stable point, - // say a vehicle center - //! [doc-stag-extrinsics-to-xyzlut] - auto lut_extrinsics = make_xyz_lut( - w, h, sensor::range_unit, info.beam_to_lidar_transform, transformation, - info.beam_azimuth_angles, info.beam_altitude_angles); - - std::cerr << "Calculating 3d Points of with special transform provided.." - << std::endl; - auto cloud_adjusted = cartesian(range, lut_extrinsics); - //! [doc-etag-extrinsics-to-xyzlut] - - std::cerr << "And now the 2000th point in the transformed point cloud... (" - << cloud_adjusted(2000, 0) << ", " << cloud_adjusted(2000, 1) - << ", " << cloud_adjusted(2000, 2) << ")" << std::endl; - - // 3. Destaggering - // Fields come in w x h arrays, but they are staggered, so that a column - // reflects the timestamp. To get each column to make visual sense, - // destagger the image - std::cerr - << "\n3. Getting staggered and destaggered images of Reflectivity..." - << std::endl; - - Eigen::Array reflectivity; - - if (info.format.udp_profile_lidar == - sensor::UDPProfileLidar::PROFILE_LIDAR_LEGACY) { - reflectivity = scan.field(sensor::ChanField::REFLECTIVITY); - } else if (info.format.udp_profile_lidar == - sensor::UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL) { - reflectivity = scan.field(sensor::ChanField::REFLECTIVITY) - .cast(); - } else { // legacy or single return profile - reflectivity = scan.field(sensor::ChanField::REFLECTIVITY) - .cast(); - } - - //! [doc-stag-cpp-destagger] - auto reflectivity_destaggered = - destagger(reflectivity, info.format.pixel_shift_by_row); - //! [doc-etag-cpp-destagger] - - // 4. You can get XYZ in w x h arrays too - std::cerr - << "4. Getting staggered and destaggered images of X Coordinate..." - << std::endl; - auto x_image_staggered = get_x_in_image_form(scan, false, info); - auto x_image_destaggered = get_x_in_image_form(scan, true, info); - - const auto print_row = std::min(123, h - 3); - const auto print_column = std::min(1507, w / 2 + 5); - - const std::string point_string = "(" + std::to_string(print_row) + ", " + - std::to_string(print_column) + ")"; - - std::cerr << "In the staggered image, the point at " << point_string - << " has reflectivity " << reflectivity(print_row, print_column) - << " and an x coordinate of " - << x_image_staggered(print_row, print_column) << "." << std::endl; - std::cerr << "In the destagged image, the point at " << point_string - << " has reflectivity " - << reflectivity_destaggered(print_row, print_column) - << " and an x coordinate of " - << x_image_destaggered(print_row, print_column) << "." - << std::endl; -} diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/viz_example.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/viz_example.cpp deleted file mode 100644 index ef8333f6..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/examples/viz_example.cpp +++ /dev/null @@ -1,63 +0,0 @@ -/** - * Copyright (c) 2022, Ouster, Inc. - * All rights reserved. - * - * Minimal static point viz library example. - */ -#include -#include -#include -#include - -#include "ouster/impl/build.h" -#include "ouster/point_viz.h" - -using namespace ouster; - -int main(int argc, char*[]) { - if (argc != 1) { - std::cerr << "Version: " << ouster::SDK_VERSION_FULL << " (" - << ouster::BUILD_SYSTEM << ")" - << "\n\nUsage: viz_example" << std::endl; - - return EXIT_FAILURE; - } - - // std::random boilerplate - std::random_device rd; - std::default_random_engine re(rd()); - std::uniform_real_distribution dis(-20.0, 20.0); - std::uniform_real_distribution dis2(0.0, 1.0); - - // number of points to display - const size_t cloud_size = 1024; - - // populate random coordinates and color indices - std::vector points(3 * cloud_size); - std::generate(points.begin(), points.end(), [&]() { return dis(re); }); - - std::vector colors(cloud_size); - std::generate(colors.begin(), colors.end(), [&]() { return dis2(re); }); - - // initialize visualizer and add keyboard/mouse callbacks - ouster::viz::PointViz viz("Viz example"); - ouster::viz::add_default_controls(viz); - - // create a point cloud and register it with the visualizer - auto cloud = std::make_shared(cloud_size); - viz.add(cloud); - - // update visualizer cloud object - cloud->set_xyz(points.data()); - cloud->set_key(colors.data()); - - // send updates to be rendered. This method is thread-safe - viz.update(); - - // run rendering loop. Will return when the window is closed - std::cout << "Running rendering loop: press ESC to exit" << std::endl; - viz.run(); - std::cout << "Window closed, exiting" << std::endl; - - return EXIT_SUCCESS; -} diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/CMakeLists.txt b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/CMakeLists.txt deleted file mode 100644 index 6f21a128..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/CMakeLists.txt +++ /dev/null @@ -1,53 +0,0 @@ -# ==== Requirements ==== -find_package(Eigen3 REQUIRED) -find_package(jsoncpp REQUIRED) -find_package(CURL REQUIRED) -find_package(spdlog REQUIRED) -include(Coverage) - -# ==== Libraries ==== -add_library(ouster_client src/client.cpp src/types.cpp src/sensor_info.cpp src/netcompat.cpp src/lidar_scan.cpp - src/image_processing.cpp src/udp_packet_source.cpp src/parsing.cpp - src/sensor_http.cpp src/sensor_http_imp.cpp src/sensor_tcp_imp.cpp src/logging.cpp - src/profile_extension.cpp - src/util.cpp) -target_link_libraries(ouster_client - PUBLIC - Eigen3::Eigen - $ - PRIVATE - CURL::libcurl - jsoncpp_lib - spdlog::spdlog) -target_compile_definitions(ouster_client PRIVATE EIGEN_MPL2_ONLY) -CodeCoverageFunctionality(ouster_client) - -add_library(OusterSDK::ouster_client ALIAS ouster_client) - -# If ouster_client is built as >=c++17, the nonstd::optional backport -# will just be an alias for std::optional. In that case, client code -# must also build as c++17 to use the same implementation of optional -get_target_property(OUSTER_CLIENT_CXX_STANDARD ouster_client CXX_STANDARD) -if(OUSTER_CLIENT_CXX_STANDARD GREATER_EQUAL 17) - target_compile_features(ouster_client INTERFACE cxx_std_17) -endif() - -if(WIN32) - target_link_libraries(ouster_client PUBLIC ws2_32) -endif() -target_include_directories(ouster_client PUBLIC - $ - $) -target_include_directories(ouster_client SYSTEM PUBLIC - $ - $) - -# ==== Install ==== -install(TARGETS ouster_client - EXPORT ouster-sdk-targets - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include) - -install(DIRECTORY include/ouster include/optional-lite DESTINATION include) diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/optional-lite/nonstd/optional.hpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/optional-lite/nonstd/optional.hpp deleted file mode 100644 index aa54e936..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/optional-lite/nonstd/optional.hpp +++ /dev/null @@ -1,1791 +0,0 @@ -// -// Copyright (c) 2014-2018 Martin Moene -// -// https://github.com/martinmoene/optional-lite -// -// Distributed under the Boost Software License, Version 1.0. -// (See accompanying file LICENSE.txt or copy at http://www.boost.org/LICENSE_1_0.txt) - -#pragma once - -#ifndef NONSTD_OPTIONAL_LITE_HPP -#define NONSTD_OPTIONAL_LITE_HPP - -#define optional_lite_MAJOR 3 -#define optional_lite_MINOR 4 -#define optional_lite_PATCH 0 - -#define optional_lite_VERSION optional_STRINGIFY(optional_lite_MAJOR) "." optional_STRINGIFY(optional_lite_MINOR) "." optional_STRINGIFY(optional_lite_PATCH) - -#define optional_STRINGIFY( x ) optional_STRINGIFY_( x ) -#define optional_STRINGIFY_( x ) #x - -// optional-lite configuration: - -#define optional_OPTIONAL_DEFAULT 0 -#define optional_OPTIONAL_NONSTD 1 -#define optional_OPTIONAL_STD 2 - -// tweak header support: - -#ifdef __has_include -# if __has_include() -# include -# endif -#define optional_HAVE_TWEAK_HEADER 1 -#else -#define optional_HAVE_TWEAK_HEADER 0 -//# pragma message("optional.hpp: Note: Tweak header not supported.") -#endif - -// optional selection and configuration: - -#if !defined( optional_CONFIG_SELECT_OPTIONAL ) -# define optional_CONFIG_SELECT_OPTIONAL ( optional_HAVE_STD_OPTIONAL ? optional_OPTIONAL_STD : optional_OPTIONAL_NONSTD ) -#endif - -// Control presence of exception handling (try and auto discover): - -#ifndef optional_CONFIG_NO_EXCEPTIONS -# if _MSC_VER -# include // for _HAS_EXCEPTIONS -# endif -# if defined(__cpp_exceptions) || defined(__EXCEPTIONS) || (_HAS_EXCEPTIONS) -# define optional_CONFIG_NO_EXCEPTIONS 0 -# else -# define optional_CONFIG_NO_EXCEPTIONS 1 -# endif -#endif - -// C++ language version detection (C++20 is speculative): -// Note: VC14.0/1900 (VS2015) lacks too much from C++14. - -#ifndef optional_CPLUSPLUS -# if defined(_MSVC_LANG ) && !defined(__clang__) -# define optional_CPLUSPLUS (_MSC_VER == 1900 ? 201103L : _MSVC_LANG ) -# else -# define optional_CPLUSPLUS __cplusplus -# endif -#endif - -#define optional_CPP98_OR_GREATER ( optional_CPLUSPLUS >= 199711L ) -#define optional_CPP11_OR_GREATER ( optional_CPLUSPLUS >= 201103L ) -#define optional_CPP11_OR_GREATER_ ( optional_CPLUSPLUS >= 201103L ) -#define optional_CPP14_OR_GREATER ( optional_CPLUSPLUS >= 201402L ) -#define optional_CPP17_OR_GREATER ( optional_CPLUSPLUS >= 201703L ) -#define optional_CPP20_OR_GREATER ( optional_CPLUSPLUS >= 202000L ) - -// C++ language version (represent 98 as 3): - -#define optional_CPLUSPLUS_V ( optional_CPLUSPLUS / 100 - (optional_CPLUSPLUS > 200000 ? 2000 : 1994) ) - -// Use C++17 std::optional if available and requested: - -#if optional_CPP17_OR_GREATER && defined(__has_include ) -# if __has_include( ) -# define optional_HAVE_STD_OPTIONAL 1 -# else -# define optional_HAVE_STD_OPTIONAL 0 -# endif -#else -# define optional_HAVE_STD_OPTIONAL 0 -#endif - -#define optional_USES_STD_OPTIONAL ( (optional_CONFIG_SELECT_OPTIONAL == optional_OPTIONAL_STD) || ((optional_CONFIG_SELECT_OPTIONAL == optional_OPTIONAL_DEFAULT) && optional_HAVE_STD_OPTIONAL) ) - -// -// in_place: code duplicated in any-lite, expected-lite, optional-lite, value-ptr-lite, variant-lite: -// - -#ifndef nonstd_lite_HAVE_IN_PLACE_TYPES -#define nonstd_lite_HAVE_IN_PLACE_TYPES 1 - -// C++17 std::in_place in : - -#if optional_CPP17_OR_GREATER - -#include - -namespace nonstd { - -using std::in_place; -using std::in_place_type; -using std::in_place_index; -using std::in_place_t; -using std::in_place_type_t; -using std::in_place_index_t; - -#define nonstd_lite_in_place_t( T) std::in_place_t -#define nonstd_lite_in_place_type_t( T) std::in_place_type_t -#define nonstd_lite_in_place_index_t(K) std::in_place_index_t - -#define nonstd_lite_in_place( T) std::in_place_t{} -#define nonstd_lite_in_place_type( T) std::in_place_type_t{} -#define nonstd_lite_in_place_index(K) std::in_place_index_t{} - -} // namespace nonstd - -#else // optional_CPP17_OR_GREATER - -#include - -namespace nonstd { -namespace detail { - -template< class T > -struct in_place_type_tag {}; - -template< std::size_t K > -struct in_place_index_tag {}; - -} // namespace detail - -struct in_place_t {}; - -template< class T > -inline in_place_t in_place( detail::in_place_type_tag /*unused*/ = detail::in_place_type_tag() ) -{ - return in_place_t(); -} - -template< std::size_t K > -inline in_place_t in_place( detail::in_place_index_tag /*unused*/ = detail::in_place_index_tag() ) -{ - return in_place_t(); -} - -template< class T > -inline in_place_t in_place_type( detail::in_place_type_tag /*unused*/ = detail::in_place_type_tag() ) -{ - return in_place_t(); -} - -template< std::size_t K > -inline in_place_t in_place_index( detail::in_place_index_tag /*unused*/ = detail::in_place_index_tag() ) -{ - return in_place_t(); -} - -// mimic templated typedef: - -#define nonstd_lite_in_place_t( T) nonstd::in_place_t(&)( nonstd::detail::in_place_type_tag ) -#define nonstd_lite_in_place_type_t( T) nonstd::in_place_t(&)( nonstd::detail::in_place_type_tag ) -#define nonstd_lite_in_place_index_t(K) nonstd::in_place_t(&)( nonstd::detail::in_place_index_tag ) - -#define nonstd_lite_in_place( T) nonstd::in_place_type -#define nonstd_lite_in_place_type( T) nonstd::in_place_type -#define nonstd_lite_in_place_index(K) nonstd::in_place_index - -} // namespace nonstd - -#endif // optional_CPP17_OR_GREATER -#endif // nonstd_lite_HAVE_IN_PLACE_TYPES - -// -// Using std::optional: -// - -#if optional_USES_STD_OPTIONAL - -#include - -namespace nonstd { - - using std::optional; - using std::bad_optional_access; - using std::hash; - - using std::nullopt; - using std::nullopt_t; - - using std::operator==; - using std::operator!=; - using std::operator<; - using std::operator<=; - using std::operator>; - using std::operator>=; - using std::make_optional; - using std::swap; -} - -#else // optional_USES_STD_OPTIONAL - -#include -#include - -// optional-lite alignment configuration: - -#ifndef optional_CONFIG_MAX_ALIGN_HACK -# define optional_CONFIG_MAX_ALIGN_HACK 0 -#endif - -#ifndef optional_CONFIG_ALIGN_AS -// no default, used in #if defined() -#endif - -#ifndef optional_CONFIG_ALIGN_AS_FALLBACK -# define optional_CONFIG_ALIGN_AS_FALLBACK double -#endif - -// Compiler warning suppression: - -#if defined(__clang__) -# pragma clang diagnostic push -# pragma clang diagnostic ignored "-Wundef" -#elif defined(__GNUC__) -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wundef" -#elif defined(_MSC_VER ) -# pragma warning( push ) -#endif - -// half-open range [lo..hi): -#define optional_BETWEEN( v, lo, hi ) ( (lo) <= (v) && (v) < (hi) ) - -// Compiler versions: -// -// MSVC++ 6.0 _MSC_VER == 1200 optional_COMPILER_MSVC_VERSION == 60 (Visual Studio 6.0) -// MSVC++ 7.0 _MSC_VER == 1300 optional_COMPILER_MSVC_VERSION == 70 (Visual Studio .NET 2002) -// MSVC++ 7.1 _MSC_VER == 1310 optional_COMPILER_MSVC_VERSION == 71 (Visual Studio .NET 2003) -// MSVC++ 8.0 _MSC_VER == 1400 optional_COMPILER_MSVC_VERSION == 80 (Visual Studio 2005) -// MSVC++ 9.0 _MSC_VER == 1500 optional_COMPILER_MSVC_VERSION == 90 (Visual Studio 2008) -// MSVC++ 10.0 _MSC_VER == 1600 optional_COMPILER_MSVC_VERSION == 100 (Visual Studio 2010) -// MSVC++ 11.0 _MSC_VER == 1700 optional_COMPILER_MSVC_VERSION == 110 (Visual Studio 2012) -// MSVC++ 12.0 _MSC_VER == 1800 optional_COMPILER_MSVC_VERSION == 120 (Visual Studio 2013) -// MSVC++ 14.0 _MSC_VER == 1900 optional_COMPILER_MSVC_VERSION == 140 (Visual Studio 2015) -// MSVC++ 14.1 _MSC_VER >= 1910 optional_COMPILER_MSVC_VERSION == 141 (Visual Studio 2017) -// MSVC++ 14.2 _MSC_VER >= 1920 optional_COMPILER_MSVC_VERSION == 142 (Visual Studio 2019) - -#if defined(_MSC_VER ) && !defined(__clang__) -# define optional_COMPILER_MSVC_VER (_MSC_VER ) -# define optional_COMPILER_MSVC_VERSION (_MSC_VER / 10 - 10 * ( 5 + (_MSC_VER < 1900 ) ) ) -#else -# define optional_COMPILER_MSVC_VER 0 -# define optional_COMPILER_MSVC_VERSION 0 -#endif - -#define optional_COMPILER_VERSION( major, minor, patch ) ( 10 * (10 * (major) + (minor) ) + (patch) ) - -#if defined(__GNUC__) && !defined(__clang__) -# define optional_COMPILER_GNUC_VERSION optional_COMPILER_VERSION(__GNUC__, __GNUC_MINOR__, __GNUC_PATCHLEVEL__) -#else -# define optional_COMPILER_GNUC_VERSION 0 -#endif - -#if defined(__clang__) -# define optional_COMPILER_CLANG_VERSION optional_COMPILER_VERSION(__clang_major__, __clang_minor__, __clang_patchlevel__) -#else -# define optional_COMPILER_CLANG_VERSION 0 -#endif - -#if optional_BETWEEN(optional_COMPILER_MSVC_VERSION, 70, 140 ) -# pragma warning( disable: 4345 ) // initialization behavior changed -#endif - -#if optional_BETWEEN(optional_COMPILER_MSVC_VERSION, 70, 150 ) -# pragma warning( disable: 4814 ) // in C++14 'constexpr' will not imply 'const' -#endif - -// Presence of language and library features: - -#define optional_HAVE(FEATURE) ( optional_HAVE_##FEATURE ) - -#ifdef _HAS_CPP0X -# define optional_HAS_CPP0X _HAS_CPP0X -#else -# define optional_HAS_CPP0X 0 -#endif - -// Unless defined otherwise below, consider VC14 as C++11 for optional-lite: - -#if optional_COMPILER_MSVC_VER >= 1900 -# undef optional_CPP11_OR_GREATER -# define optional_CPP11_OR_GREATER 1 -#endif - -#define optional_CPP11_90 (optional_CPP11_OR_GREATER_ || optional_COMPILER_MSVC_VER >= 1500) -#define optional_CPP11_100 (optional_CPP11_OR_GREATER_ || optional_COMPILER_MSVC_VER >= 1600) -#define optional_CPP11_110 (optional_CPP11_OR_GREATER_ || optional_COMPILER_MSVC_VER >= 1700) -#define optional_CPP11_120 (optional_CPP11_OR_GREATER_ || optional_COMPILER_MSVC_VER >= 1800) -#define optional_CPP11_140 (optional_CPP11_OR_GREATER_ || optional_COMPILER_MSVC_VER >= 1900) -#define optional_CPP11_141 (optional_CPP11_OR_GREATER_ || optional_COMPILER_MSVC_VER >= 1910) - -#define optional_CPP14_000 (optional_CPP14_OR_GREATER) -#define optional_CPP17_000 (optional_CPP17_OR_GREATER) - -// clang >= 2.9, gcc >= 4.9, msvc >= vc14.0/1900 (vs15): -#define optional_CPP11_140_C290_G490 ((optional_CPP11_OR_GREATER_ && (optional_COMPILER_CLANG_VERSION >= 290 || optional_COMPILER_GNUC_VERSION >= 490)) || (optional_COMPILER_MSVC_VER >= 1900)) - -// clang >= 3.5, msvc >= vc11 (vs12): -#define optional_CPP11_110_C350 ( optional_CPP11_110 && !optional_BETWEEN( optional_COMPILER_CLANG_VERSION, 1, 350 ) ) - -// clang >= 3.5, gcc >= 5.0, msvc >= vc11 (vs12): -#define optional_CPP11_110_C350_G500 \ - ( optional_CPP11_110 && \ - !( optional_BETWEEN( optional_COMPILER_CLANG_VERSION, 1, 350 ) \ - || optional_BETWEEN( optional_COMPILER_GNUC_VERSION , 1, 500 ) ) ) - -// Presence of C++11 language features: - -#define optional_HAVE_CONSTEXPR_11 optional_CPP11_140 -#define optional_HAVE_IS_DEFAULT optional_CPP11_140 -#define optional_HAVE_NOEXCEPT optional_CPP11_140 -#define optional_HAVE_NULLPTR optional_CPP11_100 -#define optional_HAVE_REF_QUALIFIER optional_CPP11_140_C290_G490 -#define optional_HAVE_INITIALIZER_LIST optional_CPP11_140 - -// Presence of C++14 language features: - -#define optional_HAVE_CONSTEXPR_14 optional_CPP14_000 - -// Presence of C++17 language features: - -#define optional_HAVE_NODISCARD optional_CPP17_000 - -// Presence of C++ library features: - -#define optional_HAVE_CONDITIONAL optional_CPP11_120 -#define optional_HAVE_REMOVE_CV optional_CPP11_120 -#define optional_HAVE_TYPE_TRAITS optional_CPP11_90 - -#define optional_HAVE_TR1_TYPE_TRAITS (!! optional_COMPILER_GNUC_VERSION ) -#define optional_HAVE_TR1_ADD_POINTER (!! optional_COMPILER_GNUC_VERSION ) - -#define optional_HAVE_IS_ASSIGNABLE optional_CPP11_110_C350 -#define optional_HAVE_IS_MOVE_CONSTRUCTIBLE optional_CPP11_110_C350 -#define optional_HAVE_IS_NOTHROW_MOVE_ASSIGNABLE optional_CPP11_110_C350 -#define optional_HAVE_IS_NOTHROW_MOVE_CONSTRUCTIBLE optional_CPP11_110_C350 -#define optional_HAVE_IS_TRIVIALLY_COPY_CONSTRUCTIBLE optional_CPP11_110_C350_G500 -#define optional_HAVE_IS_TRIVIALLY_MOVE_CONSTRUCTIBLE optional_CPP11_110_C350_G500 - -// C++ feature usage: - -#if optional_HAVE( CONSTEXPR_11 ) -# define optional_constexpr constexpr -#else -# define optional_constexpr /*constexpr*/ -#endif - -#if optional_HAVE( IS_DEFAULT ) -# define optional_is_default = default; -#else -# define optional_is_default {} -#endif - -#if optional_HAVE( CONSTEXPR_14 ) -# define optional_constexpr14 constexpr -#else -# define optional_constexpr14 /*constexpr*/ -#endif - -#if optional_HAVE( NODISCARD ) -# define optional_nodiscard [[nodiscard]] -#else -# define optional_nodiscard /*[[nodiscard]]*/ -#endif - -#if optional_HAVE( NOEXCEPT ) -# define optional_noexcept noexcept -#else -# define optional_noexcept /*noexcept*/ -#endif - -#if optional_HAVE( NULLPTR ) -# define optional_nullptr nullptr -#else -# define optional_nullptr NULL -#endif - -#if optional_HAVE( REF_QUALIFIER ) -// NOLINTNEXTLINE( bugprone-macro-parentheses ) -# define optional_ref_qual & -# define optional_refref_qual && -#else -# define optional_ref_qual /*&*/ -# define optional_refref_qual /*&&*/ -#endif - -// additional includes: - -#if optional_CONFIG_NO_EXCEPTIONS -// already included: -#else -# include -#endif - -#if optional_CPP11_OR_GREATER -# include -#endif - -#if optional_HAVE( INITIALIZER_LIST ) -# include -#endif - -#if optional_HAVE( TYPE_TRAITS ) -# include -#elif optional_HAVE( TR1_TYPE_TRAITS ) -# include -#endif - -// Method enabling - -#if optional_CPP11_OR_GREATER - -#define optional_REQUIRES_0(...) \ - template< bool B = (__VA_ARGS__), typename std::enable_if::type = 0 > - -#define optional_REQUIRES_T(...) \ - , typename std::enable_if< (__VA_ARGS__), int >::type = 0 - -#define optional_REQUIRES_R(R, ...) \ - typename std::enable_if< (__VA_ARGS__), R>::type - -#define optional_REQUIRES_A(...) \ - , typename std::enable_if< (__VA_ARGS__), void*>::type = nullptr - -#endif - -// -// optional: -// - -namespace nonstd { namespace optional_lite { - -namespace std11 { - -template< class T, T v > struct integral_constant { enum { value = v }; }; -template< bool B > struct bool_constant : integral_constant{}; - -typedef bool_constant< true > true_type; -typedef bool_constant< false > false_type; - -#if optional_CPP11_OR_GREATER - using std::move; -#else - template< typename T > T & move( T & t ) { return t; } -#endif - -#if optional_HAVE( CONDITIONAL ) - using std::conditional; -#else - template< bool B, typename T, typename F > struct conditional { typedef T type; }; - template< typename T, typename F > struct conditional { typedef F type; }; -#endif // optional_HAVE_CONDITIONAL - -#if optional_HAVE( IS_ASSIGNABLE ) - using std::is_assignable; -#else - template< class T, class U > struct is_assignable : std11::true_type{}; -#endif - -#if optional_HAVE( IS_MOVE_CONSTRUCTIBLE ) - using std::is_move_constructible; -#else - template< class T > struct is_move_constructible : std11::true_type{}; -#endif - -#if optional_HAVE( IS_NOTHROW_MOVE_ASSIGNABLE ) - using std::is_nothrow_move_assignable; -#else - template< class T > struct is_nothrow_move_assignable : std11::true_type{}; -#endif - -#if optional_HAVE( IS_NOTHROW_MOVE_CONSTRUCTIBLE ) - using std::is_nothrow_move_constructible; -#else - template< class T > struct is_nothrow_move_constructible : std11::true_type{}; -#endif - -#if optional_HAVE( IS_TRIVIALLY_COPY_CONSTRUCTIBLE ) - using std::is_trivially_copy_constructible; -#else - template< class T > struct is_trivially_copy_constructible : std11::true_type{}; -#endif - -#if optional_HAVE( IS_TRIVIALLY_MOVE_CONSTRUCTIBLE ) - using std::is_trivially_move_constructible; -#else - template< class T > struct is_trivially_move_constructible : std11::true_type{}; -#endif - -} // namespace std11 - -#if optional_CPP11_OR_GREATER - -/// type traits C++17: - -namespace std17 { - -#if optional_CPP17_OR_GREATER - -using std::is_swappable; -using std::is_nothrow_swappable; - -#elif optional_CPP11_OR_GREATER - -namespace detail { - -using std::swap; - -struct is_swappable -{ - template< typename T, typename = decltype( swap( std::declval(), std::declval() ) ) > - static std11::true_type test( int /*unused*/ ); - - template< typename > - static std11::false_type test(...); -}; - -struct is_nothrow_swappable -{ - // wrap noexcept(expr) in separate function as work-around for VC140 (VS2015): - - template< typename T > - static constexpr bool satisfies() - { - return noexcept( swap( std::declval(), std::declval() ) ); - } - - template< typename T > - static auto test( int /*unused*/ ) -> std11::integral_constant()>{} - - template< typename > - static auto test(...) -> std11::false_type; -}; - -} // namespace detail - -// is [nothow] swappable: - -template< typename T > -struct is_swappable : decltype( detail::is_swappable::test(0) ){}; - -template< typename T > -struct is_nothrow_swappable : decltype( detail::is_nothrow_swappable::test(0) ){}; - -#endif // optional_CPP17_OR_GREATER - -} // namespace std17 - -/// type traits C++20: - -namespace std20 { - -template< typename T > -struct remove_cvref -{ - typedef typename std::remove_cv< typename std::remove_reference::type >::type type; -}; - -} // namespace std20 - -#endif // optional_CPP11_OR_GREATER - -/// class optional - -template< typename T > -class optional; - -namespace detail { - -// C++11 emulation: - -struct nulltype{}; - -template< typename Head, typename Tail > -struct typelist -{ - typedef Head head; - typedef Tail tail; -}; - -#if optional_CONFIG_MAX_ALIGN_HACK - -// Max align, use most restricted type for alignment: - -#define optional_UNIQUE( name ) optional_UNIQUE2( name, __LINE__ ) -#define optional_UNIQUE2( name, line ) optional_UNIQUE3( name, line ) -#define optional_UNIQUE3( name, line ) name ## line - -#define optional_ALIGN_TYPE( type ) \ - type optional_UNIQUE( _t ); struct_t< type > optional_UNIQUE( _st ) - -template< typename T > -struct struct_t { T _; }; - -union max_align_t -{ - optional_ALIGN_TYPE( char ); - optional_ALIGN_TYPE( short int ); - optional_ALIGN_TYPE( int ); - optional_ALIGN_TYPE( long int ); - optional_ALIGN_TYPE( float ); - optional_ALIGN_TYPE( double ); - optional_ALIGN_TYPE( long double ); - optional_ALIGN_TYPE( char * ); - optional_ALIGN_TYPE( short int * ); - optional_ALIGN_TYPE( int * ); - optional_ALIGN_TYPE( long int * ); - optional_ALIGN_TYPE( float * ); - optional_ALIGN_TYPE( double * ); - optional_ALIGN_TYPE( long double * ); - optional_ALIGN_TYPE( void * ); - -#ifdef HAVE_LONG_LONG - optional_ALIGN_TYPE( long long ); -#endif - - struct Unknown; - - Unknown ( * optional_UNIQUE(_) )( Unknown ); - Unknown * Unknown::* optional_UNIQUE(_); - Unknown ( Unknown::* optional_UNIQUE(_) )( Unknown ); - - struct_t< Unknown ( * )( Unknown) > optional_UNIQUE(_); - struct_t< Unknown * Unknown::* > optional_UNIQUE(_); - struct_t< Unknown ( Unknown::* )(Unknown) > optional_UNIQUE(_); -}; - -#undef optional_UNIQUE -#undef optional_UNIQUE2 -#undef optional_UNIQUE3 - -#undef optional_ALIGN_TYPE - -#elif defined( optional_CONFIG_ALIGN_AS ) // optional_CONFIG_MAX_ALIGN_HACK - -// Use user-specified type for alignment: - -#define optional_ALIGN_AS( unused ) \ - optional_CONFIG_ALIGN_AS - -#else // optional_CONFIG_MAX_ALIGN_HACK - -// Determine POD type to use for alignment: - -#define optional_ALIGN_AS( to_align ) \ - typename type_of_size< alignment_types, alignment_of< to_align >::value >::type - -template< typename T > -struct alignment_of; - -template< typename T > -struct alignment_of_hack -{ - char c; - T t; - alignment_of_hack(); -}; - -template< size_t A, size_t S > -struct alignment_logic -{ - enum { value = A < S ? A : S }; -}; - -template< typename T > -struct alignment_of -{ - enum { value = alignment_logic< - sizeof( alignment_of_hack ) - sizeof(T), sizeof(T) >::value }; -}; - -template< typename List, size_t N > -struct type_of_size -{ - typedef typename std11::conditional< - N == sizeof( typename List::head ), - typename List::head, - typename type_of_size::type >::type type; -}; - -template< size_t N > -struct type_of_size< nulltype, N > -{ - typedef optional_CONFIG_ALIGN_AS_FALLBACK type; -}; - -template< typename T> -struct struct_t { T _; }; - -#define optional_ALIGN_TYPE( type ) \ - typelist< type , typelist< struct_t< type > - -struct Unknown; - -typedef - optional_ALIGN_TYPE( char ), - optional_ALIGN_TYPE( short ), - optional_ALIGN_TYPE( int ), - optional_ALIGN_TYPE( long ), - optional_ALIGN_TYPE( float ), - optional_ALIGN_TYPE( double ), - optional_ALIGN_TYPE( long double ), - - optional_ALIGN_TYPE( char *), - optional_ALIGN_TYPE( short * ), - optional_ALIGN_TYPE( int * ), - optional_ALIGN_TYPE( long * ), - optional_ALIGN_TYPE( float * ), - optional_ALIGN_TYPE( double * ), - optional_ALIGN_TYPE( long double * ), - - optional_ALIGN_TYPE( Unknown ( * )( Unknown ) ), - optional_ALIGN_TYPE( Unknown * Unknown::* ), - optional_ALIGN_TYPE( Unknown ( Unknown::* )( Unknown ) ), - - nulltype - > > > > > > > > > > > > > > - > > > > > > > > > > > > > > - > > > > > > - alignment_types; - -#undef optional_ALIGN_TYPE - -#endif // optional_CONFIG_MAX_ALIGN_HACK - -/// C++03 constructed union to hold value. - -template< typename T > -union storage_t -{ -//private: -// template< typename > friend class optional; - - typedef T value_type; - - storage_t() optional_is_default - - explicit storage_t( value_type const & v ) - { - construct_value( v ); - } - - void construct_value( value_type const & v ) - { - ::new( value_ptr() ) value_type( v ); - } - -#if optional_CPP11_OR_GREATER - - explicit storage_t( value_type && v ) - { - construct_value( std::move( v ) ); - } - - void construct_value( value_type && v ) - { - ::new( value_ptr() ) value_type( std::move( v ) ); - } - - template< class... Args > - storage_t( nonstd_lite_in_place_t(T), Args&&... args ) - { - emplace( std::forward(args)... ); - } - - template< class... Args > - void emplace( Args&&... args ) - { - ::new( value_ptr() ) value_type( std::forward(args)... ); - } - - template< class U, class... Args > - void emplace( std::initializer_list il, Args&&... args ) - { - ::new( value_ptr() ) value_type( il, std::forward(args)... ); - } - -#endif - - void destruct_value() - { - value_ptr()->~T(); - } - - optional_nodiscard value_type const * value_ptr() const - { - return as(); - } - - value_type * value_ptr() - { - return as(); - } - - optional_nodiscard value_type const & value() const optional_ref_qual - { - return * value_ptr(); - } - - value_type & value() optional_ref_qual - { - return * value_ptr(); - } - -#if optional_HAVE( REF_QUALIFIER ) - - optional_nodiscard value_type const && value() const optional_refref_qual - { - return std::move( value() ); - } - - value_type && value() optional_refref_qual - { - return std::move( value() ); - } - -#endif - -#if optional_CPP11_OR_GREATER - - using aligned_storage_t = typename std::aligned_storage< sizeof(value_type), alignof(value_type) >::type; - aligned_storage_t data; - -#elif optional_CONFIG_MAX_ALIGN_HACK - - typedef struct { unsigned char data[ sizeof(value_type) ]; } aligned_storage_t; - - max_align_t hack; - aligned_storage_t data; - -#else - typedef optional_ALIGN_AS(value_type) align_as_type; - - typedef struct { align_as_type data[ 1 + ( sizeof(value_type) - 1 ) / sizeof(align_as_type) ]; } aligned_storage_t; - aligned_storage_t data; - -# undef optional_ALIGN_AS - -#endif // optional_CONFIG_MAX_ALIGN_HACK - - optional_nodiscard void * ptr() optional_noexcept - { - return &data; - } - - optional_nodiscard void const * ptr() const optional_noexcept - { - return &data; - } - - template - optional_nodiscard U * as() - { - return reinterpret_cast( ptr() ); - } - - template - optional_nodiscard U const * as() const - { - return reinterpret_cast( ptr() ); - } -}; - -} // namespace detail - -/// disengaged state tag - -struct nullopt_t -{ - struct init{}; - explicit optional_constexpr nullopt_t( init /*unused*/ ) optional_noexcept {} -}; - -#if optional_HAVE( CONSTEXPR_11 ) -constexpr nullopt_t nullopt{ nullopt_t::init{} }; -#else -// extra parenthesis to prevent the most vexing parse: -const nullopt_t nullopt(( nullopt_t::init() )); -#endif - -/// optional access error - -#if ! optional_CONFIG_NO_EXCEPTIONS - -class bad_optional_access : public std::logic_error -{ -public: - explicit bad_optional_access() - : logic_error( "bad optional access" ) {} -}; - -#endif //optional_CONFIG_NO_EXCEPTIONS - -/// optional - -template< typename T> -class optional -{ -private: - template< typename > friend class optional; - - typedef void (optional::*safe_bool)() const; - -public: - typedef T value_type; - - // x.x.3.1, constructors - - // 1a - default construct - optional_constexpr optional() optional_noexcept - : has_value_( false ) - , contained() - {} - - // 1b - construct explicitly empty - // NOLINTNEXTLINE( google-explicit-constructor, hicpp-explicit-conversions ) - optional_constexpr optional( nullopt_t /*unused*/ ) optional_noexcept - : has_value_( false ) - , contained() - {} - - // 2 - copy-construct -#if optional_CPP11_OR_GREATER - // template< typename U = T - // optional_REQUIRES_T( - // std::is_copy_constructible::value - // || std11::is_trivially_copy_constructible::value - // ) - // > -#endif - optional_constexpr14 optional( optional const & other ) - : has_value_( other.has_value() ) - { - if ( other.has_value() ) - { - contained.construct_value( other.contained.value() ); - } - } - -#if optional_CPP11_OR_GREATER - - // 3 (C++11) - move-construct from optional - template< typename U = T - optional_REQUIRES_T( - std11::is_move_constructible::value - || std11::is_trivially_move_constructible::value - ) - > - optional_constexpr14 optional( optional && other ) - // NOLINTNEXTLINE( performance-noexcept-move-constructor ) - noexcept( std11::is_nothrow_move_constructible::value ) - : has_value_( other.has_value() ) - { - if ( other.has_value() ) - { - contained.construct_value( std::move( other.contained.value() ) ); - } - } - - // 4a (C++11) - explicit converting copy-construct from optional - template< typename U - optional_REQUIRES_T( - std::is_constructible::value - && !std::is_constructible & >::value - && !std::is_constructible && >::value - && !std::is_constructible const & >::value - && !std::is_constructible const && >::value - && !std::is_convertible< optional & , T>::value - && !std::is_convertible< optional && , T>::value - && !std::is_convertible< optional const & , T>::value - && !std::is_convertible< optional const &&, T>::value - && !std::is_convertible< U const & , T>::value /*=> explicit */ - ) - > - explicit optional( optional const & other ) - : has_value_( other.has_value() ) - { - if ( other.has_value() ) - { - contained.construct_value( T{ other.contained.value() } ); - } - } -#endif // optional_CPP11_OR_GREATER - - // 4b (C++98 and later) - non-explicit converting copy-construct from optional - template< typename U -#if optional_CPP11_OR_GREATER - optional_REQUIRES_T( - std::is_constructible::value - && !std::is_constructible & >::value - && !std::is_constructible && >::value - && !std::is_constructible const & >::value - && !std::is_constructible const && >::value - && !std::is_convertible< optional & , T>::value - && !std::is_convertible< optional && , T>::value - && !std::is_convertible< optional const & , T>::value - && !std::is_convertible< optional const &&, T>::value - && std::is_convertible< U const & , T>::value /*=> non-explicit */ - ) -#endif // optional_CPP11_OR_GREATER - > - // NOLINTNEXTLINE( google-explicit-constructor, hicpp-explicit-conversions ) - /*non-explicit*/ optional( optional const & other ) - : has_value_( other.has_value() ) - { - if ( other.has_value() ) - { - contained.construct_value( other.contained.value() ); - } - } - -#if optional_CPP11_OR_GREATER - - // 5a (C++11) - explicit converting move-construct from optional - template< typename U - optional_REQUIRES_T( - std::is_constructible::value - && !std::is_constructible & >::value - && !std::is_constructible && >::value - && !std::is_constructible const & >::value - && !std::is_constructible const && >::value - && !std::is_convertible< optional & , T>::value - && !std::is_convertible< optional && , T>::value - && !std::is_convertible< optional const & , T>::value - && !std::is_convertible< optional const &&, T>::value - && !std::is_convertible< U &&, T>::value /*=> explicit */ - ) - > - explicit optional( optional && other - ) - : has_value_( other.has_value() ) - { - if ( other.has_value() ) - { - contained.construct_value( T{ std::move( other.contained.value() ) } ); - } - } - - // 5a (C++11) - non-explicit converting move-construct from optional - template< typename U - optional_REQUIRES_T( - std::is_constructible::value - && !std::is_constructible & >::value - && !std::is_constructible && >::value - && !std::is_constructible const & >::value - && !std::is_constructible const && >::value - && !std::is_convertible< optional & , T>::value - && !std::is_convertible< optional && , T>::value - && !std::is_convertible< optional const & , T>::value - && !std::is_convertible< optional const &&, T>::value - && std::is_convertible< U &&, T>::value /*=> non-explicit */ - ) - > - // NOLINTNEXTLINE( google-explicit-constructor, hicpp-explicit-conversions ) - /*non-explicit*/ optional( optional && other ) - : has_value_( other.has_value() ) - { - if ( other.has_value() ) - { - contained.construct_value( std::move( other.contained.value() ) ); - } - } - - // 6 (C++11) - in-place construct - template< typename... Args - optional_REQUIRES_T( - std::is_constructible::value - ) - > - optional_constexpr explicit optional( nonstd_lite_in_place_t(T), Args&&... args ) - : has_value_( true ) - , contained( T( std::forward(args)...) ) - {} - - // 7 (C++11) - in-place construct, initializer-list - template< typename U, typename... Args - optional_REQUIRES_T( - std::is_constructible&, Args&&...>::value - ) - > - optional_constexpr explicit optional( nonstd_lite_in_place_t(T), std::initializer_list il, Args&&... args ) - : has_value_( true ) - , contained( T( il, std::forward(args)...) ) - {} - - // 8a (C++11) - explicit move construct from value - template< typename U = T - optional_REQUIRES_T( - std::is_constructible::value - && !std::is_same::type, nonstd_lite_in_place_t(U)>::value - && !std::is_same::type, optional>::value - && !std::is_convertible::value /*=> explicit */ - ) - > - optional_constexpr explicit optional( U && value ) - : has_value_( true ) - , contained( nonstd_lite_in_place(T), std::forward( value ) ) - {} - - // 8b (C++11) - non-explicit move construct from value - template< typename U = T - optional_REQUIRES_T( - std::is_constructible::value - && !std::is_same::type, nonstd_lite_in_place_t(U)>::value - && !std::is_same::type, optional>::value - && std::is_convertible::value /*=> non-explicit */ - ) - > - // NOLINTNEXTLINE( google-explicit-constructor, hicpp-explicit-conversions ) - optional_constexpr /*non-explicit*/ optional( U && value ) - : has_value_( true ) - , contained( nonstd_lite_in_place(T), std::forward( value ) ) - {} - -#else // optional_CPP11_OR_GREATER - - // 8 (C++98) - optional( value_type const & value ) - : has_value_( true ) - , contained( value ) - {} - -#endif // optional_CPP11_OR_GREATER - - // x.x.3.2, destructor - - ~optional() - { - if ( has_value() ) - { - contained.destruct_value(); - } - } - - // x.x.3.3, assignment - - // 1 (C++98and later) - assign explicitly empty - optional & operator=( nullopt_t /*unused*/) optional_noexcept - { - reset(); - return *this; - } - - // 2 (C++98and later) - copy-assign from optional -#if optional_CPP11_OR_GREATER - // NOLINTNEXTLINE( cppcoreguidelines-c-copy-assignment-signature, misc-unconventional-assign-operator ) - optional_REQUIRES_R( - optional &, - true -// std::is_copy_constructible::value -// && std::is_copy_assignable::value - ) - operator=( optional const & other ) - noexcept( - std11::is_nothrow_move_assignable::value - && std11::is_nothrow_move_constructible::value - ) -#else - optional & operator=( optional const & other ) -#endif - { - if ( (has_value() == true ) && (other.has_value() == false) ) { reset(); } - else if ( (has_value() == false) && (other.has_value() == true ) ) { initialize( *other ); } - else if ( (has_value() == true ) && (other.has_value() == true ) ) { contained.value() = *other; } - return *this; - } - -#if optional_CPP11_OR_GREATER - - // 3 (C++11) - move-assign from optional - // NOLINTNEXTLINE( cppcoreguidelines-c-copy-assignment-signature, misc-unconventional-assign-operator ) - optional_REQUIRES_R( - optional &, - true -// std11::is_move_constructible::value -// && std::is_move_assignable::value - ) - operator=( optional && other ) noexcept - { - if ( (has_value() == true ) && (other.has_value() == false) ) { reset(); } - else if ( (has_value() == false) && (other.has_value() == true ) ) { initialize( std::move( *other ) ); } - else if ( (has_value() == true ) && (other.has_value() == true ) ) { contained.value() = std::move( *other ); } - return *this; - } - - // 4 (C++11) - move-assign from value - template< typename U = T > - // NOLINTNEXTLINE( cppcoreguidelines-c-copy-assignment-signature, misc-unconventional-assign-operator ) - optional_REQUIRES_R( - optional &, - std::is_constructible::value - && std11::is_assignable::value - && !std::is_same::type, nonstd_lite_in_place_t(U)>::value - && !std::is_same::type, optional>::value - && !(std::is_scalar::value && std::is_same::type>::value) - ) - operator=( U && value ) - { - if ( has_value() ) - { - contained.value() = std::forward( value ); - } - else - { - initialize( T( std::forward( value ) ) ); - } - return *this; - } - -#else // optional_CPP11_OR_GREATER - - // 4 (C++98) - copy-assign from value - template< typename U /*= T*/ > - optional & operator=( U const & value ) - { - if ( has_value() ) contained.value() = value; - else initialize( T( value ) ); - return *this; - } - -#endif // optional_CPP11_OR_GREATER - - // 5 (C++98 and later) - converting copy-assign from optional - template< typename U > -#if optional_CPP11_OR_GREATER - // NOLINTNEXTLINE( cppcoreguidelines-c-copy-assignment-signature, misc-unconventional-assign-operator ) - optional_REQUIRES_R( - optional&, - std::is_constructible< T , U const &>::value - && std11::is_assignable< T&, U const &>::value - && !std::is_constructible & >::value - && !std::is_constructible && >::value - && !std::is_constructible const & >::value - && !std::is_constructible const && >::value - && !std::is_convertible< optional & , T>::value - && !std::is_convertible< optional && , T>::value - && !std::is_convertible< optional const & , T>::value - && !std::is_convertible< optional const &&, T>::value - && !std11::is_assignable< T&, optional & >::value - && !std11::is_assignable< T&, optional && >::value - && !std11::is_assignable< T&, optional const & >::value - && !std11::is_assignable< T&, optional const && >::value - ) -#else - optional& -#endif // optional_CPP11_OR_GREATER - operator=( optional const & other ) - { - return *this = optional( other ); - } - -#if optional_CPP11_OR_GREATER - - // 6 (C++11) - converting move-assign from optional - template< typename U > - // NOLINTNEXTLINE( cppcoreguidelines-c-copy-assignment-signature, misc-unconventional-assign-operator ) - optional_REQUIRES_R( - optional&, - std::is_constructible< T , U>::value - && std11::is_assignable< T&, U>::value - && !std::is_constructible & >::value - && !std::is_constructible && >::value - && !std::is_constructible const & >::value - && !std::is_constructible const && >::value - && !std::is_convertible< optional & , T>::value - && !std::is_convertible< optional && , T>::value - && !std::is_convertible< optional const & , T>::value - && !std::is_convertible< optional const &&, T>::value - && !std11::is_assignable< T&, optional & >::value - && !std11::is_assignable< T&, optional && >::value - && !std11::is_assignable< T&, optional const & >::value - && !std11::is_assignable< T&, optional const && >::value - ) - operator=( optional && other ) - { - return *this = optional( std::move( other ) ); - } - - // 7 (C++11) - emplace - template< typename... Args - optional_REQUIRES_T( - std::is_constructible::value - ) - > - T& emplace( Args&&... args ) - { - *this = nullopt; - contained.emplace( std::forward(args)... ); - has_value_ = true; - return contained.value(); - } - - // 8 (C++11) - emplace, initializer-list - template< typename U, typename... Args - optional_REQUIRES_T( - std::is_constructible&, Args&&...>::value - ) - > - T& emplace( std::initializer_list il, Args&&... args ) - { - *this = nullopt; - contained.emplace( il, std::forward(args)... ); - has_value_ = true; - return contained.value(); - } - -#endif // optional_CPP11_OR_GREATER - - // x.x.3.4, swap - - void swap( optional & other ) -#if optional_CPP11_OR_GREATER - noexcept( - std11::is_nothrow_move_constructible::value - && std17::is_nothrow_swappable::value - ) -#endif - { - using std::swap; - if ( (has_value() == true ) && (other.has_value() == true ) ) { swap( **this, *other ); } - else if ( (has_value() == false) && (other.has_value() == true ) ) { initialize( std11::move(*other) ); other.reset(); } - else if ( (has_value() == true ) && (other.has_value() == false) ) { other.initialize( std11::move(**this) ); reset(); } - } - - // x.x.3.5, observers - - optional_constexpr value_type const * operator ->() const - { - return assert( has_value() ), - contained.value_ptr(); - } - - optional_constexpr14 value_type * operator ->() - { - return assert( has_value() ), - contained.value_ptr(); - } - - optional_constexpr value_type const & operator *() const optional_ref_qual - { - return assert( has_value() ), - contained.value(); - } - - optional_constexpr14 value_type & operator *() optional_ref_qual - { - return assert( has_value() ), - contained.value(); - } - -#if optional_HAVE( REF_QUALIFIER ) - - optional_constexpr value_type const && operator *() const optional_refref_qual - { - return std::move( **this ); - } - - optional_constexpr14 value_type && operator *() optional_refref_qual - { - return std::move( **this ); - } - -#endif - -#if optional_CPP11_OR_GREATER - optional_constexpr explicit operator bool() const optional_noexcept - { - return has_value(); - } -#else - optional_constexpr operator safe_bool() const optional_noexcept - { - return has_value() ? &optional::this_type_does_not_support_comparisons : 0; - } -#endif - - // NOLINTNEXTLINE( modernize-use-nodiscard ) - /*optional_nodiscard*/ optional_constexpr bool has_value() const optional_noexcept - { - return has_value_; - } - - // NOLINTNEXTLINE( modernize-use-nodiscard ) - /*optional_nodiscard*/ optional_constexpr14 value_type const & value() const optional_ref_qual - { -#if optional_CONFIG_NO_EXCEPTIONS - assert( has_value() ); -#else - if ( ! has_value() ) - { - throw bad_optional_access(); - } -#endif - return contained.value(); - } - - optional_constexpr14 value_type & value() optional_ref_qual - { -#if optional_CONFIG_NO_EXCEPTIONS - assert( has_value() ); -#else - if ( ! has_value() ) - { - throw bad_optional_access(); - } -#endif - return contained.value(); - } - -#if optional_HAVE( REF_QUALIFIER ) && ( !optional_COMPILER_GNUC_VERSION || optional_COMPILER_GNUC_VERSION >= 490 ) - - // NOLINTNEXTLINE( modernize-use-nodiscard ) - /*optional_nodiscard*/ optional_constexpr value_type const && value() const optional_refref_qual - { - return std::move( value() ); - } - - optional_constexpr14 value_type && value() optional_refref_qual - { - return std::move( value() ); - } - -#endif - -#if optional_HAVE( REF_QUALIFIER ) - - template< typename U > - optional_constexpr value_type value_or( U && v ) const optional_ref_qual - { - return has_value() ? contained.value() : static_cast(std::forward( v ) ); - } - - template< typename U > - optional_constexpr14 value_type value_or( U && v ) optional_refref_qual - { -#if optional_COMPILER_CLANG_VERSION - return has_value() ? /*std::move*/( contained.value() ) : static_cast(std::forward( v ) ); -#else - return has_value() ? std::move( contained.value() ) : static_cast(std::forward( v ) ); -#endif - } - -#else - - template< typename U > - optional_constexpr value_type value_or( U const & v ) const - { - return has_value() ? contained.value() : static_cast( v ); - } - -#endif // optional_CPP11_OR_GREATER - - // x.x.3.6, modifiers - - void reset() optional_noexcept - { - if ( has_value() ) - { - contained.destruct_value(); - } - - has_value_ = false; - } - -private: - void this_type_does_not_support_comparisons() const {} - - template< typename V > - void initialize( V const & value ) - { - assert( ! has_value() ); - contained.construct_value( value ); - has_value_ = true; - } - -#if optional_CPP11_OR_GREATER - template< typename V > - void initialize( V && value ) - { - assert( ! has_value() ); - contained.construct_value( std::move( value ) ); - has_value_ = true; - } - -#endif - -private: - bool has_value_; - detail::storage_t< value_type > contained; - -}; - -// Relational operators - -template< typename T, typename U > -inline optional_constexpr bool operator==( optional const & x, optional const & y ) -{ - return bool(x) != bool(y) ? false : !bool( x ) ? true : *x == *y; -} - -template< typename T, typename U > -inline optional_constexpr bool operator!=( optional const & x, optional const & y ) -{ - return !(x == y); -} - -template< typename T, typename U > -inline optional_constexpr bool operator<( optional const & x, optional const & y ) -{ - return (!y) ? false : (!x) ? true : *x < *y; -} - -template< typename T, typename U > -inline optional_constexpr bool operator>( optional const & x, optional const & y ) -{ - return (y < x); -} - -template< typename T, typename U > -inline optional_constexpr bool operator<=( optional const & x, optional const & y ) -{ - return !(y < x); -} - -template< typename T, typename U > -inline optional_constexpr bool operator>=( optional const & x, optional const & y ) -{ - return !(x < y); -} - -// Comparison with nullopt - -template< typename T > -inline optional_constexpr bool operator==( optional const & x, nullopt_t /*unused*/ ) optional_noexcept -{ - return (!x); -} - -template< typename T > -inline optional_constexpr bool operator==( nullopt_t /*unused*/, optional const & x ) optional_noexcept -{ - return (!x); -} - -template< typename T > -inline optional_constexpr bool operator!=( optional const & x, nullopt_t /*unused*/ ) optional_noexcept -{ - return bool(x); -} - -template< typename T > -inline optional_constexpr bool operator!=( nullopt_t /*unused*/, optional const & x ) optional_noexcept -{ - return bool(x); -} - -template< typename T > -inline optional_constexpr bool operator<( optional const & /*unused*/, nullopt_t /*unused*/ ) optional_noexcept -{ - return false; -} - -template< typename T > -inline optional_constexpr bool operator<( nullopt_t /*unused*/, optional const & x ) optional_noexcept -{ - return bool(x); -} - -template< typename T > -inline optional_constexpr bool operator<=( optional const & x, nullopt_t /*unused*/ ) optional_noexcept -{ - return (!x); -} - -template< typename T > -inline optional_constexpr bool operator<=( nullopt_t /*unused*/, optional const & /*unused*/ ) optional_noexcept -{ - return true; -} - -template< typename T > -inline optional_constexpr bool operator>( optional const & x, nullopt_t /*unused*/ ) optional_noexcept -{ - return bool(x); -} - -template< typename T > -inline optional_constexpr bool operator>( nullopt_t /*unused*/, optional const & /*unused*/ ) optional_noexcept -{ - return false; -} - -template< typename T > -inline optional_constexpr bool operator>=( optional const & /*unused*/, nullopt_t /*unused*/ ) optional_noexcept -{ - return true; -} - -template< typename T > -inline optional_constexpr bool operator>=( nullopt_t /*unused*/, optional const & x ) optional_noexcept -{ - return (!x); -} - -// Comparison with T - -template< typename T, typename U > -inline optional_constexpr bool operator==( optional const & x, U const & v ) -{ - return bool(x) ? *x == v : false; -} - -template< typename T, typename U > -inline optional_constexpr bool operator==( U const & v, optional const & x ) -{ - return bool(x) ? v == *x : false; -} - -template< typename T, typename U > -inline optional_constexpr bool operator!=( optional const & x, U const & v ) -{ - return bool(x) ? *x != v : true; -} - -template< typename T, typename U > -inline optional_constexpr bool operator!=( U const & v, optional const & x ) -{ - return bool(x) ? v != *x : true; -} - -template< typename T, typename U > -inline optional_constexpr bool operator<( optional const & x, U const & v ) -{ - return bool(x) ? *x < v : true; -} - -template< typename T, typename U > -inline optional_constexpr bool operator<( U const & v, optional const & x ) -{ - return bool(x) ? v < *x : false; -} - -template< typename T, typename U > -inline optional_constexpr bool operator<=( optional const & x, U const & v ) -{ - return bool(x) ? *x <= v : true; -} - -template< typename T, typename U > -inline optional_constexpr bool operator<=( U const & v, optional const & x ) -{ - return bool(x) ? v <= *x : false; -} - -template< typename T, typename U > -inline optional_constexpr bool operator>( optional const & x, U const & v ) -{ - return bool(x) ? *x > v : false; -} - -template< typename T, typename U > -inline optional_constexpr bool operator>( U const & v, optional const & x ) -{ - return bool(x) ? v > *x : true; -} - -template< typename T, typename U > -inline optional_constexpr bool operator>=( optional const & x, U const & v ) -{ - return bool(x) ? *x >= v : false; -} - -template< typename T, typename U > -inline optional_constexpr bool operator>=( U const & v, optional const & x ) -{ - return bool(x) ? v >= *x : true; -} - -// Specialized algorithms - -template< typename T -#if optional_CPP11_OR_GREATER - optional_REQUIRES_T( - std11::is_move_constructible::value - && std17::is_swappable::value ) -#endif -> -void swap( optional & x, optional & y ) -#if optional_CPP11_OR_GREATER - noexcept( noexcept( x.swap(y) ) ) -#endif -{ - x.swap( y ); -} - -#if optional_CPP11_OR_GREATER - -template< typename T > -optional_constexpr optional< typename std::decay::type > make_optional( T && value ) -{ - return optional< typename std::decay::type >( std::forward( value ) ); -} - -template< typename T, typename...Args > -optional_constexpr optional make_optional( Args&&... args ) -{ - return optional( nonstd_lite_in_place(T), std::forward(args)...); -} - -template< typename T, typename U, typename... Args > -optional_constexpr optional make_optional( std::initializer_list il, Args&&... args ) -{ - return optional( nonstd_lite_in_place(T), il, std::forward(args)...); -} - -#else - -template< typename T > -optional make_optional( T const & value ) -{ - return optional( value ); -} - -#endif // optional_CPP11_OR_GREATER - -} // namespace optional_lite - -using optional_lite::optional; -using optional_lite::nullopt_t; -using optional_lite::nullopt; - -#if ! optional_CONFIG_NO_EXCEPTIONS -using optional_lite::bad_optional_access; -#endif - -using optional_lite::make_optional; - -} // namespace nonstd - -#if optional_CPP11_OR_GREATER - -// specialize the std::hash algorithm: - -namespace std { - -template< class T > -struct hash< nonstd::optional > -{ -public: - std::size_t operator()( nonstd::optional const & v ) const optional_noexcept - { - return bool( v ) ? std::hash{}( *v ) : 0; - } -}; - -} //namespace std - -#endif // optional_CPP11_OR_GREATER - -#if defined(__clang__) -# pragma clang diagnostic pop -#elif defined(__GNUC__) -# pragma GCC diagnostic pop -#elif defined(_MSC_VER ) -# pragma warning( pop ) -#endif - -#endif // optional_USES_STD_OPTIONAL - -#endif // NONSTD_OPTIONAL_LITE_HPP diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/client.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/client.h deleted file mode 100644 index d4c61cb4..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/client.h +++ /dev/null @@ -1,315 +0,0 @@ -/** - * Copyright (c) 2018, Ouster, Inc. - * All rights reserved. - * - * @file - * @brief sample sensor client - */ - -#pragma once - -#include -#include -#include -#include - -#include "ouster/defaults.h" -#include "ouster/types.h" -#include "ouster/version.h" - -namespace ouster { -namespace sensor { - -struct client; - -/** Returned by poll_client. */ -enum client_state { - 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. */ -const util::version min_version = {1, 12, 0}; - -/** - * Initializes and configures ouster_client logs. This method should be invoked - * only once before calling any other method from the library if the user wants - * to direct the library log statements to a different medium (other than - * console which is the default). - * - * @param[in] log_level Control the level of log messages outputed by the - * client. Valid options are (case-sensitive): "trace", "debug", "info", - * "warning", "error", "critical" and "off". - * @param[in] log_file_path Path to location where log files are stored. The - * path must be in a location that the process has write access to. If an empty - * string is provided then the logs will be directed to the console. When an - * empty string is passed then the rest of parameters are ignored. - * @param[in] rotating Configure the log file with rotation, rotation rules are - * specified through the two following parameters max_size_in_bytes and - * max_files. If rotating is set to false the following parameters are ignored. - * @param[in] max_size_in_bytes Maximum number of bytes to write to a rotating - * log file before starting a new file. ignored if rotating is set to false. - * @param[in] max_files Maximum number of rotating files to accumlate before - * re-using the first file. ignored if rotating is set to false. - * - * @return true on success, otherwise false. - */ -bool init_logger(const std::string& log_level, - const std::string& log_file_path = "", bool rotating = false, - int max_size_in_bytes = 0, int max_files = 0); - -/** \defgroup ouster_client_init Ouster Client Client Initialization - * @{ - */ - -/** - * Listen for sensor data on the specified ports; do not configure the sensor. - * - * @param[in] hostname The hostname to connect to. - * @param[in] lidar_port port on which the sensor will send lidar data. - * @param[in] imu_port port on which the sensor will send imu data. - * - * @return pointer owning the resources associated with the connection. - */ -std::shared_ptr init_client(const std::string& hostname, int lidar_port, - int imu_port); - -/** - * Connect to and configure the sensor and start listening for data. - * - * @param[in] hostname hostname or ip of the sensor. - * @param[in] udp_dest_host hostname or ip where the sensor should send data - * or "" for automatic detection of destination. - * @param[in] ld_mode The lidar mode to use. - * @param[in] ts_mode The timestamp mode to use. - * @param[in] lidar_port port on which the sensor will send lidar data. When - * using zero the method will automatically acquire and assign any free port. - * @param[in] imu_port port on which the sensor will send imu data. When - * using zero the method will automatically acquire and assign any free port. - * @param[in] timeout_sec how long to wait for the sensor to initialize. - * @param[in] persist_config if true, persists sensor settings between restarts - * - * @return pointer owning the resources associated with the connection. - */ -std::shared_ptr init_client( - const std::string& hostname, const std::string& udp_dest_host, - lidar_mode ld_mode = MODE_UNSPEC, timestamp_mode ts_mode = TIME_FROM_UNSPEC, - int lidar_port = 0, int imu_port = 0, - int timeout_sec = DEFAULT_HTTP_REQUEST_TIMEOUT_SECONDS, - bool persist_config = false); - -/** - * [BETA] Connect to and configure the sensor and start listening for data via - * multicast. - * - * @param[in] hostname hostname or ip of the sensor. - * @param[in] config sensor config to set on sensor. - * @param[in] mtp_dest_host the address of the host network interface that - * should join the multicast group; if empty, use any appropriate interface. - * @param[in] main a flag that indicates this is the main connection to the - * sensor in an multicast setup. - * @param[in] timeout_sec how long to wait for the sensor to initialize. - * @param[in] persist_config if true, persists sensor settings between restarts - * - * @return pointer owning the resources associated with the connection. - * - * @remarks when main flag is set the config object will be used to configure - * the sensor, otherwise only the port values within the config object will be - * used and the rest will be ignored. - */ -std::shared_ptr mtp_init_client( - const std::string& hostname, const sensor_config& config, - const std::string& mtp_dest_host, bool main, - int timeout_sec = DEFAULT_HTTP_REQUEST_TIMEOUT_SECONDS, - bool persist_config = false); - -/** @}*/ - -/** - * Block for up to timeout_sec until either data is ready or an error occurs. - * - * NOTE: will return immediately if LIDAR_DATA or IMU_DATA are set and not - * cleared by read_lidar_packet() and read_imu_packet() before the next call. - * - * @param[in] cli client returned by init_client associated with the connection. - * @param[in] timeout_sec seconds to block while waiting for data. - * - * @return client_state s where (s & ERROR) is true if an error occured, (s & - * LIDAR_DATA) is true if lidar data is ready to read, and (s & IMU_DATA) is - * true if imu data is ready to read. - */ -client_state poll_client(const client& cli, int timeout_sec = 1); - -/** - * Read lidar data from the sensor. Will not block. - * - * @param[in] cli client returned by init_client associated with the connection. - * @param[out] buf buffer to which to write lidar data. Must be at least - * lidar_packet_bytes + 1 bytes. - * @param[in] pf The packet format. - * - * @return true if a lidar packet was successfully read. - */ -bool read_lidar_packet(const client& cli, uint8_t* buf, - const packet_format& pf); - -/** - * Read lidar data from the sensor. Will not block. - * - * @param[in] cli client returned by init_client associated with the connection. - * @param[out] buf buffer to which to write lidar data. Must be at least - * `bytes + 1` bytes. - * @param[in] bytes expected number of bytes in the packet - * - * @return true if a lidar packet was successfully read. - */ -bool read_lidar_packet(const client& cli, uint8_t* buf, size_t bytes); - -/** - * Read lidar data from the sensor. Will not block. - * - * @param[in] cli client returned by init_client associated with the connection. - * @param[out] packet A LidarPacket to store lidar data read from a sensor. - * Expects the packet to have *correct* number of bytes allocated for the - * packet. In addition, the LidarPacket's host_timestamp attribute is also set. - * - * @return true if a lidar packet was successfully read. - */ -bool read_lidar_packet(const client& cli, LidarPacket& packet); - -/** - * Read imu data from the sensor. Will not block. - * - * @param[in] cli client returned by init_client associated with the connection. - * @param[out] buf buffer to which to write lidar data. Must be at least - * `bytes + 1` bytes. - * @param[in] bytes expected number of bytes in the packet - * - * @return true if a lidar packet was successfully read. - */ -bool read_imu_packet(const client& cli, uint8_t* buf, size_t bytes); - -/** - * Read imu data from the sensor. Will not block. - * - * @param[in] cli client returned by init_client associated with the connection. - * @param[out] buf buffer to which to write imu data. Must be at least - * imu_packet_bytes + 1 bytes. - * @param[in] pf The packet format. - * - * @return true if an imu packet was successfully read. - */ -bool read_imu_packet(const client& cli, uint8_t* buf, const packet_format& pf); - -/** - * Read imu data from the sensor. Will not block. - * - * @param[in] cli client returned by init_client associated with the connection. - * @param[out] packet An ImuPacket to store imu data read from a sensor. Expects - * the packet to have *correct* number of bytes allocated for the packet. In - * addition, the ImuPacket's host_timestamp attribute is also set. - * - * @return true if an imu packet was successfully read. - */ -bool read_imu_packet(const client& cli, ImuPacket& packet); - -/** - * 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 within the timeout period, - * a timeout occured while waiting for the sensor to finish initializing, - * 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 = DEFAULT_HTTP_REQUEST_TIMEOUT_SECONDS, - bool legacy_format = false); - -/** - * 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. - * @param[in] timeout_sec set the timeout for the request, - * this argument is optional. - * - * @return true if sensor config successfully populated. - */ -bool get_config(const std::string& hostname, sensor_config& config, - bool active = true, - int timeout_sec = DEFAULT_HTTP_REQUEST_TIMEOUT_SECONDS); - -// clang-format off -/** - * 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 -}; -// clang-format on - -/** - * 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. - * @param[in] timeout_sec timeout in seconds for http requests - * - * @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, - int timeout_sec = DEFAULT_HTTP_REQUEST_TIMEOUT_SECONDS); - -/** - * 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(const 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(const client& cli); - -/** - * Check if ip address in multicast range. - * - * @param[in] addr ip address to test. - * - * @return true if addr is in multicast range. - */ -bool in_multicast(const std::string& addr); - -} // namespace sensor -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/defaults.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/defaults.h deleted file mode 100644 index d8d95fde..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/defaults.h +++ /dev/null @@ -1,4 +0,0 @@ -#pragma once - -constexpr int DEFAULT_HTTP_REQUEST_TIMEOUT_SECONDS = 40; -constexpr int DEFAULT_COLUMNS_PER_PACKET = 16; diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/image_processing.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/image_processing.h deleted file mode 100644 index a222e556..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/image_processing.h +++ /dev/null @@ -1,110 +0,0 @@ -/** - * Copyright (c) 2018, Ouster, Inc. - * All rights reserved. - * - * @file - * @brief Utilities for post-processing image data - */ - -#pragma once - -#include - -#include "ouster/types.h" - -namespace ouster { -namespace viz { - -/** Adjusts brightness to between 0 and 1. */ -class AutoExposure { - const double lo_percentile, hi_percentile; // percentiles used for scaling - const int ae_update_every; - - double lo_state = -1.0; - double hi_state = -1.0; - double lo = -1.0; - double hi = -1.0; - - bool initialized = false; - int counter = 0; - - template - void update(Eigen::Ref> image, bool update_state); - - public: - /** Default constructor using default percentile and update values. */ - AutoExposure(); - - /** - * Constructor specifying update modulo, and using default percentiles. - * - * @param[in] update_every update every this number of frames. - */ - AutoExposure(int update_every); - - /** - * Constructor specifying low and high percentiles, and update modulo. - * - * @param[in] lo_percentile low percentile to use for adjustment. - * @param[in] hi_percentile high percentile to use for adjustment. - * @param[in] update_every update every this number of frames. - */ - AutoExposure(double lo_percentile, double hi_percentile, int update_every); - - /** - * Scales the image so that contrast is stretched between 0 and 1. - * - * The top percentile is 1 - hi_percentile and the bottom percentile is - * lo_percentile. Similar to linear 'contrast-stretch', i.e. normalization. - * - * @param[in] image Reference to the image, modified in place. - * @param[in] update_state Update lo/hi percentiles if true. - */ - void operator()(Eigen::Ref> image, bool update_state = true); - - /** - * Scales the image so that contrast is stretched between 0 and 1. - * - * The top percentile is 1 - hi_percentile and the bottom percentile is - * lo_percentile. Similar to linear 'contrast-stretch', i.e. normalization. - * - * @param[in] image Reference to the image, modified in place. - * @param[in] update_state Update lo/hi percentiles if true. - */ - void operator()(Eigen::Ref> image, bool update_state = true); -}; - -/** - * Corrects beam uniformity by minimizing median difference between rows, - * thereby correcting subtle horizontal line artifacts in images, especially the - * ambient image. - */ -class BeamUniformityCorrector { - private: - int counter = 0; - Eigen::ArrayXd dark_count; - - template - void update(Eigen::Ref> image, bool update_state); - - public: - /** - * Applies dark count correction to an image, modifying it in-place to have - * reduced horizontal line artifacts. - * - * @param[in] image Reference to the image, modified in-place. - * @param[in] update_state Update dark counts if true. - */ - void operator()(Eigen::Ref> image, bool update_state = true); - - /** - * Applies dark count correction to an image, modifying it in-place to have - * reduced horizontal line artifacts. - * - * @param[in] image Reference to the image, modified in-place. - * @param[in] update_state Update dark counts if true. - */ - void operator()(Eigen::Ref> image, bool update_state = true); -}; -} // namespace viz -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/impl/cartesian.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/impl/cartesian.h deleted file mode 100644 index 24b73982..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/impl/cartesian.h +++ /dev/null @@ -1,74 +0,0 @@ -#pragma once - -#include - -namespace ouster { - -// Users can enable OpenMP within ouster_client by first enabling omp through -// the compiler and then adding '-DOUSTER_OMP' option to the DCMAKE_CXX_FLAGS -#if defined(OUSTER_OMP) -#if defined(_OPENMP) -#define __OUSTER_UTILIZE_OPENMP__ -#else -#pragma message("OUSTER_OMP was defined but openmp is not detected!") -#endif -#endif - -template -using PointsT = Eigen::Array; -using PointsD = PointsT; -using PointsF = PointsT; - -/** - * 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]; - } - } -} - -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/impl/client_poller.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/impl/client_poller.h deleted file mode 100644 index 28a9221e..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/impl/client_poller.h +++ /dev/null @@ -1,70 +0,0 @@ -/** - * Copyright (c) 2023, Ouster, Inc. - * All rights reserved. - */ - -#pragma once - -namespace ouster { -namespace sensor { -namespace impl { - -/** - * Poller used in multiclient scenarios - */ -struct client_poller; - -/** - * produces uninitialized poller - */ -std::shared_ptr make_poller(); - -/** - * Reset poller. Must be called prior to any other operations - * - * @param[in] poller client_poller to reset - */ -void reset_poll(client_poller& poller); - -/** - * Set poller to watch client on the next poll call - * - * @param[in] poller client_poller - * @param[in] cli client to watch - */ -void set_poll(client_poller& poller, const client& cli); - -/** - * Polls clients previously set with `set_poll` - * - * @param[in] poller client_poller - * @param[in] timeout_sec timeout in seconds - * - * @return -1 for error, 0 for timeout, otherwise number of messages received - */ -int poll(client_poller& poller, int timeout_sec = 1); - -/** - * Retrieves error state of the poller - * - * @param[in] poller client_poller - * - * @return client_state which is one of CLIENT_ERROR or EXIT on error, - * otherwise returning TIMEOUT if no error occurred - */ -client_state get_error(const client_poller& poller); - -/** - * Retrieve poll results for particular client - * - * @param[in] poller client_poller - * @param[in] cli client to retrieve results for - * - * @return client_state comprising of either LIDAR_DATA or IMU_DATA, or TIMEOUT - * if no data was received - */ -client_state get_poll(const client_poller& poller, const client& cli); - -} // namespace impl -} // namespace sensor -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/impl/lidar_scan_impl.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/impl/lidar_scan_impl.h deleted file mode 100644 index 9224b882..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/impl/lidar_scan_impl.h +++ /dev/null @@ -1,439 +0,0 @@ -/** - * Copyright (c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#pragma once - -#include -#include -#include - -#include "ouster/impl/packet_writer.h" -#include "ouster/lidar_scan.h" -#include "ouster/types.h" - -namespace ouster { -namespace impl { - -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) { 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); - } -}; - -/** - * Zeros fields in LidarScans - */ -struct zero_field { - /** - * Zeros the field dest. - * - * @tparam T The type of data inside of the eigen array. - * @param[in,out] field_dest The field to zero. - */ - template - void operator()(Eigen::Ref> field_dest) { - field_dest.setZero(); - } -}; - -/** - * 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); - -/** - * OutputItT - STL compatible output iterator over LidarPacket value type - */ -template -void scan_to_packets(const LidarScan& ls, - const ouster::sensor::impl::packet_writer& pw, - OutputItT iter, uint32_t init_id, uint64_t prod_sn) { - int total_packets = ls.packet_timestamp().size(); - auto columns_per_packet = pw.columns_per_packet; - - if (ls.w / columns_per_packet != total_packets) { - std::string err = - "Mismatch between expected number of packets and " - "packet_writer.columns_per_frame"; - throw std::invalid_argument(err); - } - - using ouster::sensor::ChanField; - using ouster::sensor::LidarPacket; - - auto pack_field = [&pw](auto ref_field, ChanField i, LidarPacket& packet) { - // skip over RAW_HEADERS, RAW32_WORD* and CUSTOM* fields - if (i >= ChanField::RAW_HEADERS && i <= ChanField::CHAN_FIELD_MAX) - return; - - pw.set_block(ref_field, i, packet.buf.data()); - }; - - auto unpack_raw_headers = [&pw](auto ref_field, LidarPacket& packet) { - pw.unpack_raw_headers(ref_field, packet.buf.data()); - }; - - auto frame_id = ls.frame_id; - LidarPacket packet(pw.lidar_packet_size); - - for (int p_id = 0; p_id < total_packets; ++p_id) { - uint8_t* lidar_buf = packet.buf.data(); - std::memset(packet.buf.data(), 0, packet.buf.size()); - packet.host_timestamp = ls.packet_timestamp()[p_id]; - - pw.set_frame_id(lidar_buf, frame_id); - pw.set_init_id(lidar_buf, init_id); - pw.set_prod_sn(lidar_buf, prod_sn); - - bool any_valid = false; - for (int icol = 0; icol < columns_per_packet; ++icol) { - uint8_t* col_buf = pw.nth_col(icol, lidar_buf); - - auto id = p_id * columns_per_packet + icol; - - pw.set_col_status(col_buf, ls.status()[id]); - pw.set_col_measurement_id(col_buf, id); - pw.set_col_timestamp(col_buf, ls.timestamp()[id]); - - any_valid |= (ls.status()[id] & 0x01); - } - - // do not emit packet if ts == 0 and none of the columns are valid - if (!any_valid && !packet.host_timestamp) continue; - - foreach_field(ls, pack_field, packet); - - if (raw_headers_enabled(pw, ls)) { - visit_field(ls, ChanField::RAW_HEADERS, unpack_raw_headers, packet); - } else if (pw.udp_profile_lidar != - ouster::sensor::UDPProfileLidar::PROFILE_LIDAR_LEGACY) { - uint32_t* ptr = reinterpret_cast(packet.buf.data() + - packet.buf.size() - 4); - *ptr = 0xdeadbeef; // eUDP packets end in 0xdeadbeef - } - - *iter++ = packet; - } -} - -} // namespace impl - -template -inline img_t destagger(const Eigen::Ref>& img, - const std::vector& pixel_shift_by_row, - bool inverse) { - const size_t h = img.rows(); - const size_t w = img.cols(); - - if (pixel_shift_by_row.size() != h) - throw std::invalid_argument{"image height does not match shifts size"}; - - img_t destaggered{h, w}; - for (size_t u = 0; u < h; u++) { - const std::ptrdiff_t offset = - ((inverse ? -1 : 1) * pixel_shift_by_row[u] + w) % w; - - destaggered.row(u).segment(offset, w - offset) = - img.row(u).segment(0, w - offset); - destaggered.row(u).segment(0, offset) = - img.row(u).segment(w - offset, offset); - } - return destaggered; -} - -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/impl/netcompat.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/impl/netcompat.h deleted file mode 100644 index 3f676f6d..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/impl/netcompat.h +++ /dev/null @@ -1,101 +0,0 @@ -/** - * Copyright (c) 2020, Ouster, Inc. - * All rights reserved. - * - * @file - * @brief Compatibility with windows (unsupported) - */ - -#pragma once - -#include - -#if defined _WIN32 // --------- On Windows --------- - -// Try and limit the stuff windows brings in -#include -#include - -// ssize_t is available in vs -#ifdef _MSC_VER -#include -#define ssize_t SSIZE_T -#endif - -#else // --------- Compiling on *nix --------- - -#include -#include -#include -#include -#include -#include - -// Define windows types -#ifndef SOCKET -#define SOCKET int -#endif - -#ifndef FDSET -#define FDSET fd_set -#endif - -#define SOCKET_ERROR -1 - -#endif // --------- End Platform Differentiation Block --------- - -namespace ouster { -namespace sensor { -namespace impl { - -/** - * Close a specified socket - * @param[in] sock The socket file descriptor to close - * @return success - */ -int socket_close(SOCKET sock); - -/** - * Get the error message for socket errors - * @return The socket error message - */ -std::string socket_get_error(); - -/** - * Check if a socket file descriptor is valid - * @param[in] value The socket file descriptor to check - * @return The validity of the socket file descriptor - */ -bool socket_valid(SOCKET value); - -/** - * Check if the last error was a socket exit event - * @return If the socket has exited - */ -bool socket_exit(); - -/** - * Set a specified socket to non-blocking - * @param[in] value The socket file descriptor to set non-blocking - * @return success - */ -int socket_set_non_blocking(SOCKET value); - -/** - * Set a specified socket to reuse - * @param[in] value The socket file descriptor to set reuse - * @return success - */ -int socket_set_reuse(SOCKET value); - -/** - * Set SO_RCVTIMEO on the specified socket - * @param[in] sock The socket file descriptor - * @param[in] timeout_sec Timeout esconds - * @return success - */ -int socket_set_rcvtimeout(SOCKET sock, int timeout_sec); - -} // namespace impl -} // namespace sensor -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/impl/packet_writer.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/impl/packet_writer.h deleted file mode 100644 index 63652fd4..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/impl/packet_writer.h +++ /dev/null @@ -1,56 +0,0 @@ -/** - * Copyright (c) 2023, Ouster, Inc. - * All rights reserved. - */ - -#pragma once - -#include "ouster/types.h" - -namespace ouster { -namespace sensor { -namespace impl { - -/** - * Writing counterpart to packet_format, used for packet generation - */ -class packet_writer : public packet_format { - template - void set_block_impl(Eigen::Ref> field, ChanField i, - uint8_t* lidar_buf) const; - - public: - using packet_format::packet_format; - // construct from packet format - packet_writer(const packet_format& pf) : packet_format(pf) {} - - uint8_t* nth_col(int n, uint8_t* lidar_buf) const; - uint8_t* nth_px(int n, uint8_t* col_buf) const; - uint8_t* footer(uint8_t* lidar_buf) const; - - void set_col_status(uint8_t* col_buf, uint32_t status) const; - void set_col_timestamp(uint8_t* col_buf, uint64_t ts) const; - void set_col_measurement_id(uint8_t* col_buf, uint16_t m_id) const; - void set_frame_id(uint8_t* lidar_buf, uint32_t frame_id) const; - void set_init_id(uint8_t* lidar_buf, uint32_t init_id) const; - void set_prod_sn(uint8_t* lidar_buf, uint64_t sn) const; - - template - void set_px(uint8_t* px_buf, ChanField i, T value) const; - - template - void set_block(Eigen::Ref> field, ChanField i, - uint8_t* lidar_buf) const; - - /** - * Note: measurement_id indices will be taken from lidar_buf itself, - * so this method expects those to be pre-filled - */ - template - void unpack_raw_headers(Eigen::Ref> field, - uint8_t* lidar_buf) const; -}; - -} // namespace impl -} // namespace sensor -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/impl/profile_extension.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/impl/profile_extension.h deleted file mode 100644 index 804e88c0..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/impl/profile_extension.h +++ /dev/null @@ -1,33 +0,0 @@ -/** - * Copyright (c) 2023, Ouster, Inc. - * All rights reserved. - */ - -#pragma once - -#include -#include -#include - -#include "ouster/types.h" - -namespace ouster { -namespace sensor { -namespace impl { - -struct FieldInfo { - ChanFieldType ty_tag; - size_t offset; - uint64_t mask; - int shift; -}; - -} // namespace impl - -void add_custom_profile( - int profile_nr, const std::string& name, - const std::vector>& fields, - size_t chan_data_size); - -} // namespace sensor -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/impl/ring_buffer.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/impl/ring_buffer.h deleted file mode 100644 index 7a35b4e3..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/impl/ring_buffer.h +++ /dev/null @@ -1,282 +0,0 @@ -/** - * Copyright (c) 2023, Ouster, Inc. - * All rights reserved. - */ - -#pragma once - -#include -#include -#include -#include -#include - -namespace ouster { -namespace sensor { -namespace impl { - -/** - * Ring buffer class for internal use. - * - * This is NOT thread safe, thread safety is delegated to the user. - * Correct read/write procedure is: - * \code - * auto rb = RingBuffer{size, T{...}}; - * - * // write - * if (!rb.full()) { - * T& element = rb.back(); - * do_write(element); - * rb.push(); - * } - * - * // read - * if (!rb.empty()) { - * T& element = rb.front(); - * do_read(element); - * rb.pop(); - * } - * - * \endcode - */ -template -class RingBuffer { - static_assert(std::is_copy_constructible::value, - "must be copy constructible"); - - std::atomic r_idx_, w_idx_; - std::vector bufs_; - - size_t _capacity() const { return bufs_.size(); } - - public: - RingBuffer(size_t size, T value = {}) - : r_idx_(0), w_idx_(0), bufs_(size + 1, value) {} - - RingBuffer(RingBuffer&& other) { - std::swap(bufs_, other.bufs_); - r_idx_ = other.r_idx_.load(); - w_idx_ = other.w_idx_.load(); - } - - /** - * Report the total capacity of allocated elements. - */ - size_t capacity() const { return _capacity() - 1; } - - /** - * Report the size of currently used elements. - */ - size_t size() const { - return (_capacity() + w_idx_ - r_idx_) % _capacity(); - } - - /** - * Check whether ring buffer is empty. - * - * NOTE: - * \code - * if (!rb.empty()) { - * // inside the block, rb.empty() is not guaranteed to stay the same - * // *unless* it is called by the *only* thread advancing read - * // indices to this buffer - * } - * \endcode - */ - bool empty() const { return w_idx_ == r_idx_; } - - /** - * Check whether ring buffer is empty. - * - * NOTE: - * \code - * if (!rb.full()) { - * // inside the block, rb.full() is not guaranteed to stay the same - * // *unless* it is called by the *only* thread advancing write - * // indices to this buffer - * } - * \endcode - */ - bool full() const { return r_idx_ == ((w_idx_ + 1) % _capacity()); } - - /** - * Get element at the front of the ring buffer. - */ - T& front() { return bufs_[r_idx_]; } - const T& front() const { return bufs_[r_idx_]; } - - /** - * Get element at the back of the ring buffer. - */ - T& back() { return bufs_[w_idx_]; } - const T& back() const { return bufs_[w_idx_]; } - - /** - * Flush the ring buffer, making it empty. - */ - void flush() { r_idx_ = w_idx_.load(); }; - - /** - * Atomically increment read index. - * - * Throws if ring buffer is empty. - */ - void pop() { - if (empty()) throw std::underflow_error("popped an empty ring buffer"); - size_t read_idx = r_idx_.load(); - while (!r_idx_.compare_exchange_strong(read_idx, - (read_idx + 1) % _capacity())) { - } - } - - /** - * Atomically increment write index. - * - * Throws if ring buffer is full. - */ - void push() { - if (full()) throw std::overflow_error("pushed a full ring buffer"); - size_t write_idx = r_idx_.load(); - // atomic increment modulo - while (!w_idx_.compare_exchange_strong(write_idx, - (write_idx + 1) % _capacity())) { - } - } -}; - -/** - * Convenience class for working with multiple ring buffers. - */ -template -class RingBufferMap { - using MapBuffers = std::unordered_map>; - MapBuffers rb_map_; - - public: - using MapInputs = std::unordered_map>; - - RingBufferMap() {} - - RingBufferMap(const MapInputs& inputs) : rb_map_{} { - for (const auto& pair : inputs) { - allocate(pair.first, pair.second.first, pair.second.second); - } - } - - /** - * Allocate a new ring buffer. - * - * @param[in] key key to allocate with - * @param[in] size size, in elements, to allocate - * @param[in] value default value of elements in newly allocated ring buffer - */ - void allocate(K key, size_t size, V value) { - if (rb_map_.find(key) != rb_map_.end()) { - throw std::invalid_argument( - "RingBufferMap: failed allocating a ring buffer, key already " - "exists"); - } - - rb_map_.emplace(key, RingBuffer{size, value}); - } - - /** - * Retrieve value at the front of the ring buffer at specified key. - */ - V& front(const K& key) { return rb_map_.at(key).front(); } - const V& front(const K& key) const { return rb_map_.at(key).front(); } - - /** - * Retrieve value at the back of the ring buffer at specified key. - */ - V& back(const K& key) { return rb_map_.at(key).back(); } - const V& back(const K& key) const { return rb_map_.at(key).back(); } - - /** - * Advance read index of the ring buffer at specified key. - */ - void pop(const K& key) { rb_map_.at(key).pop(); } - - /** - * Advance write index of the ring buffer at specified key. - */ - void push(const K& key) { rb_map_.at(key).push(); } - - /** - * Check if the ring buffer at specified key is empty. - */ - bool empty(const K& key) const { return rb_map_.at(key).empty(); } - - /** - * Check if the ring buffer at specified key is full. - */ - bool full(const K& key) const { return rb_map_.at(key).full(); } - - /** - * Report the capacity of the ring buffer at specified key. - */ - size_t capacity(const K& key) const { return rb_map_.at(key).capacity(); } - - /** - * Report the current size of the ring buffer at specified key. - */ - size_t size(const K& key) const { return rb_map_.at(key).size(); } - - /** - * Flush an internal buffer at specified key. - */ - void flush(const K& key) { return rb_map_.at(key).flush(); } - - /** - * Flush all internal buffers. - */ - void flush() { - for (auto& kv : rb_map_) kv.second.flush(); - } - - /** - * Check if any one of the internal buffers is full. - */ - bool any_full() const { - return std::any_of(rb_map_.begin(), rb_map_.end(), - [](const auto& kv) { return kv.second.full(); }); - } - - /** - * Check if any one of the internal buffers is empty. - */ - bool any_empty() const { - return std::any_of(rb_map_.begin(), rb_map_.end(), - [](const auto& kv) { return kv.second.empty(); }); - } - - /** - * Reports total amount of currently stored packets in internal buffers. - * - * NOTE: this is not a great metric since it does not report specific - * buffers, but the total amount instead. - */ - size_t size() const { - return std::accumulate(rb_map_.begin(), rb_map_.end(), size_t{0}, - [](size_t total, const auto& kv) { - return total + kv.second.size(); - }); - } - - /** - * Reports total allocated capacity of packets stored in internal buffers. - * - * NOTE: this is not a great metric since it does not report specific - * buffers, but the total amount instead. - */ - size_t capacity() const { - return std::accumulate(rb_map_.begin(), rb_map_.end(), size_t{0}, - [](size_t total, const auto& kv) { - return total + kv.second.capacity(); - }); - } -}; - -} // namespace impl -} // namespace sensor -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/lidar_scan.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/lidar_scan.h deleted file mode 100644 index a2c95e11..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/lidar_scan.h +++ /dev/null @@ -1,592 +0,0 @@ -/** - * Copyright (c) 2018, Ouster, Inc. All rights reserved. @file - * @brief Holds lidar data by field in row-major order - */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "ouster/defaults.h" -#include "ouster/types.h" - -namespace ouster { - -// forward declarations -namespace impl { -struct FieldSlot; -} - -/** - * Alias for the lidar scan field types - */ -using LidarScanFieldTypes = - std::vector>; - -/** - * Data structure for efficient operations on aggregated lidar data. - * - * Stores each field (range, intensity, etc.) contiguously as a H x W block of - * 4-byte unsigned integers, where H is the number of beams and W is the - * horizontal resolution (e.g. 512, 1024, 2048). - * - * Note: this is the "staggered" representation where each column corresponds - * to a single measurement in time. Use the destagger() function to create an - * image where columns correspond to a single azimuth angle. - */ -class LidarScan { - public: - template - using Header = Eigen::Array; ///< Header typedef - - /** XYZ coordinates with dimensions arranged contiguously in columns. */ - using Points = Eigen::Array; - - private: - Header timestamp_; - Header packet_timestamp_; - Header measurement_id_; - Header status_; - std::vector pose_; - std::map fields_; - LidarScanFieldTypes field_types_; - - LidarScan(size_t w, size_t h, LidarScanFieldTypes field_types, - size_t columns_per_packet); - - 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. - */ - int64_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(); - - /** - * Initialize a scan with fields configured for the LEGACY udp profile. - * - * @param[in] w horizontal resolution, i.e. the number of measurements per - * scan. - * @param[in] h vertical resolution, i.e. the number of channels. - * - * Note, the number of columns per packet is set to the default - * (DEFAULT_COLUMNS_PER_PACKET). - */ - LidarScan(size_t w, size_t h); - - /** - * Initialize a scan with the default fields for a particular udp profile. - * - * @param[in] w horizontal resolution, i.e. the number of measurements per - * scan. - * @param[in] h vertical resolution, i.e. the number of channels. - * @param[in] profile udp profile. - * @param[in] columns_per_packet The number of columns per packet, - * this argument is optional. - */ - LidarScan(size_t w, size_t h, sensor::UDPProfileLidar profile, - size_t columns_per_packet = DEFAULT_COLUMNS_PER_PACKET); - - /** - * 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. - * @param[in] columns_per_packet The number of columns per packet, - * this argument is optional. - */ - template - LidarScan(size_t w, size_t h, Iterator begin, Iterator end, - size_t columns_per_packet = DEFAULT_COLUMNS_PER_PACKET) - : LidarScan(w, h, {begin, end}, columns_per_packet){}; - - /** - * Initialize a lidar scan from another lidar scan. - * - * @param[in] other The other lidar scan to initialize from. - */ - LidarScan(const LidarScan& other); - - /** - * Initialize a lidar scan from another with only the indicated fields. - * Casts, zero pads or removes fields from the original scan if necessary. - * - * @param[in] other The other lidar scan to initialize from. - * @param[in] fields Fields to have in new lidar scan. - */ - LidarScan(const LidarScan& other, const LidarScanFieldTypes& fields); - - /** @copydoc LidarScan(const LidarScan& other) */ - LidarScan(LidarScan&& other); - - /** - * Copy. - * - * @param[in] other The lidar scan to copy from. - */ - LidarScan& operator=(const LidarScan& other); - - /** - * Copy via Move semantic. - * - * @param[in] other The lidar scan to copy from. - */ - LidarScan& operator=(LidarScan&& other); - - /** - * Lidar scan destructor. - */ - ~LidarScan(); - - /** - * Get frame shot limiting status - */ - sensor::ShotLimitingStatus shot_limiting() const; - - /** - * Get frame thermal shutdown status - */ - sensor::ThermalShutdownStatus thermal_shutdown() const; - - /** - * @defgroup ClientLidarScanField Access fields in a lidar scan - * Access a lidar data field. - * - * @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. - */ - - /** - * @copydoc ClientLidarScanField - */ - template ::value, T>::type = 0> - Eigen::Ref> field(sensor::ChanField f); - - /** - * @copydoc ClientLidarScanField - */ - template ::value, T>::type = 0> - Eigen::Ref> field(sensor::ChanField f) const; - - /** - * Get the type of the specified field. - * - * @param[in] f the field to query. - * - * @return the type tag associated with the field. - */ - sensor::ChanFieldType field_type(sensor::ChanField f) const; - - /** A const forward iterator over field / type pairs. */ - FieldIter begin() const; - - /** @copydoc begin() */ - FieldIter end() const; - - /** - * Access the measurement timestamp headers. - * - * @return a view of timestamp as a w-element vector. - */ - Eigen::Ref> timestamp(); - - /** - * @copydoc timestamp() - */ - Eigen::Ref> timestamp() const; - - /** - * Access the packet timestamp headers (usually host time). - * - * @return a view of timestamp as a w-element vector. - */ - Eigen::Ref> packet_timestamp(); - - /** - * Access the host timestamp headers (usually host time). - * - * @return a view of timestamp as a w-element vector. - */ - Eigen::Ref> packet_timestamp() const; - - /** - * Return the first valid packet timestamp - * - * @return the first valid packet timestamp, 0 if none available - */ - uint64_t get_first_valid_packet_timestamp() const; - - /** - * Access the measurement id headers. - * - * @return a view of measurement ids as a w-element vector. - */ - Eigen::Ref> measurement_id(); - - /** @copydoc measurement_id() */ - Eigen::Ref> measurement_id() const; - - /** - * Access the measurement status headers. - * - * @return a view of measurement statuses as a w-element vector. - */ - Eigen::Ref> status(); - - /** @copydoc status() */ - Eigen::Ref> status() const; - - /** - * Access the vector of poses (per each timestamp). - * - * @return a reference to vector with poses (4x4) homogeneous. - */ - std::vector& pose(); - /** @copydoc pose() */ - const std::vector& pose() const; - - /** - * 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 bool operator==(const LidarScan& a, const LidarScan& b); -}; - -/** - * 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 - */ -LidarScanFieldTypes get_field_types(const LidarScan& ls); - -/** - * Get the lidar scan field types from lidar profile - * - * @param[in] udp_profile_lidar lidar profile - * - * @return The lidar scan field types - */ -LidarScanFieldTypes get_field_types(sensor::UDPProfileLidar udp_profile_lidar); - -/** - * 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); - -/** \defgroup ouster_client_lidar_scan_operators Ouster Client lidar_scan.h - * Operators - * @{ - */ - -/** - * Equality for scans. - * - * @param[in] a The first scan to compare. - * @param[in] b The second scan to compare. - * - * @return if a == b. - */ -bool operator==(const LidarScan& a, const LidarScan& b); - -/** - * NOT Equality for scans. - * - * @param[in] a The first scan to compare. - * @param[in] b The second scan to compare. - * - * @return if a != b. - */ -inline bool operator!=(const LidarScan& a, const LidarScan& b) { - return !(a == b); -} -/** @}*/ - -/** Lookup table of beam directions and offsets. */ -struct XYZLut { - LidarScan::Points direction; ///< Lookup table of beam directions - LidarScan::Points offset; ///< Lookup table of beam offsets -}; - -/** - * Generate a set of lookup tables useful for computing Cartesian coordinates - * from ranges. - * - * The lookup tables are: - * - direction: a matrix of unit vectors pointing radially outwards. - * - offset: a matrix of offsets dependent on beam origin distance from lidar - * origin. - * - * Each table is an n x 3 array of doubles stored in column-major order where - * each row corresponds to the nth point in a lidar scan, with 0 <= n < h*w. - * - * Projections to XYZ made with this XYZLut will be in the coordinate frame - * defined by transform*beam_to_lidar_transform. - * - * @param[in] w number of columns in the lidar scan. e.g. 512, 1024, or 2048. - * @param[in] h number of rows in the lidar scan. - * @param[in] range_unit the unit, in meters, of the range, e.g. - * sensor::range_unit. - * @param[in] beam_to_lidar_transform transform between beams and - * lidar origin. Translation portion is in millimeters. - * @param[in] transform additional transformation to apply to resulting points. - * @param[in] azimuth_angles_deg azimuth offsets in degrees for each of h beams. - * @param[in] altitude_angles_deg altitude in degrees for each of h beams. - * - * @return xyz direction and offset vectors for each point in the lidar scan. - */ -XYZLut make_xyz_lut(size_t w, size_t h, double range_unit, - const mat4d& beam_to_lidar_transform, - const mat4d& transform, - const std::vector& azimuth_angles_deg, - const std::vector& altitude_angles_deg); - -/** - * Convenient overload that uses parameters from the supplied sensor_info. - * Projections to XYZ made with this XYZLut will be in the sensor coordinate - * frame defined in the sensor documentation. - * - * @param[in] sensor metadata returned from the client. - * - * @return xyz direction and offset vectors for each point in the lidar scan. - */ -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.beam_to_lidar_transform, - sensor.lidar_to_sensor_transform, sensor.beam_azimuth_angles, - sensor.beam_altitude_angles); -} - -/** \defgroup ouster_client_lidar_scan_cartesian Ouster Client lidar_scan.h - * XYZLut related items. - * @{ - */ -/** - * 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); -/** @}*/ - -/** \defgroup ouster_client_destagger Ouster Client lidar_scan.h - * @{ - */ -/** - * Generate a destaggered version of a channel field. - * - * In the default staggered representation, each column corresponds to a single - * timestamp. In the destaggered representation, each column corresponds to a - * single azimuth angle, compensating for the azimuth offset of each beam. - * - * Destaggering is used for visualizing lidar data as an image or for algorithms - * that exploit the structure of the lidar data, such as beam_uniformity in - * ouster_viz, or computer vision algorithms. - * - * @tparam T the datatype of the channel field. - * - * @param[in] img the channel field. - * @param[in] pixel_shift_by_row offsets, usually queried from the sensor. - * @param[in] inverse perform the inverse operation. - * - * @return destaggered version of the image. - */ -template -inline img_t destagger(const Eigen::Ref>& img, - const std::vector& pixel_shift_by_row, - bool inverse = false); - -/** - * Generate a staggered version of a channel field. - * - * @tparam T the datatype of the channel field. - * - * @param[in] img the channel field. - * @param[in] pixel_shift_by_row offsets, usually queried from the sensor. - * - * @return staggered version of the image. - */ -template -inline img_t stagger(const Eigen::Ref>& img, - const std::vector& pixel_shift_by_row) { - return destagger(img, pixel_shift_by_row, true); -} -/** @}*/ -/** - * Parse lidar packets into a LidarScan. - * - * Make a function that batches a single scan (revolution) of data to a - * LidarScan. - */ -class ScanBatcher { - std::ptrdiff_t w; - std::ptrdiff_t h; - uint16_t next_valid_m_id; - uint16_t next_headers_m_id; - uint16_t next_valid_packet_id; - std::vector cache; - uint64_t cache_packet_ts; - bool cached_packet = false; - - void _parse_by_col(const uint8_t* packet_buf, LidarScan& ls); - void _parse_by_block(const uint8_t* packet_buf, LidarScan& ls); - - public: - sensor::packet_format pf; ///< The packet format object used for decoding - - /** - * Create a batcher given information about the scan and packet format. - * - * @param[in] w number of columns in the lidar scan. One of 512, 1024, or - * 2048. - * @param[in] pf expected format of the incoming packets used for parsing. - */ - ScanBatcher(size_t w, const sensor::packet_format& pf); - - /** - * Create a batcher given information about the scan and packet format. - * - * @param[in] info sensor metadata returned from the client. - */ - ScanBatcher(const sensor::sensor_info& info); - - /** - * Add a packet to the scan. - * @deprecated this method is deprecated in favor of one that accepts a - * reference to a LidarPacket. - * - * @param[in] packet_buf a buffer containing raw bytes from a lidar packet. - * @param[in] ls lidar scan to populate. - * - * @return true when the provided lidar scan is ready to use. - */ - [[deprecated( - "Use ScanBatcher::operator(LidarPacket&, LidarScan&) instead.")]] bool - operator()(const uint8_t* packet_buf, LidarScan& ls); - - /** - * Add a packet to the scan. - * - * @param[in] packet a LidarPacket. - * @param[in] ls lidar scan to populate. - * - * @return true when the provided lidar scan is ready to use. - */ - bool operator()(const ouster::sensor::LidarPacket& packet, LidarScan& ls); - - /** - * Add a packet to the scan. - * - * @param[in] packet_buf a buffer containing raw bytes from a lidar packet. - * @param[in] packet_ts timestamp of the packet (usually HOST time on - * receive). - * @param[in] ls lidar scan to populate. - * - * @return true when the provided lidar scan is ready to use. - */ - bool operator()(const uint8_t* packet_buf, uint64_t packet_ts, - LidarScan& ls); -}; - -} // namespace ouster - -#include "ouster/impl/cartesian.h" -#include "ouster/impl/lidar_scan_impl.h" diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/sensor_http.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/sensor_http.h deleted file mode 100644 index 1430f623..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/sensor_http.h +++ /dev/null @@ -1,157 +0,0 @@ -/** - * Copyright (c) 2022, Ouster, Inc. - * All rights reserved. - * - * @file sensor_http.h - * @brief A high level HTTP interface for Ouster sensors. - * - */ - -#pragma once - -#include -#include -#include - -#include - -namespace ouster { -namespace sensor { -namespace util { - -/** - * An interface to communicate with Ouster sensors via http requests - */ -class SensorHttp { - protected: - /** - * Constructs an http interface to communicate with the sensor. - */ - SensorHttp() = default; - - public: - /** - * Deconstruct the sensor http interface. - */ - virtual ~SensorHttp() = default; - - /** - * Queries the sensor metadata. - * - * @return returns a Json object of the sensor metadata. - */ - virtual Json::Value metadata() const = 0; - - /** - * Queries the sensor_info. - * - * @return returns a Json object representing the sensor_info. - */ - 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 - */ - 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 - */ - virtual Json::Value active_config_params() const = 0; - - /** - * Retrieves the staged configuration on the sensor - */ - 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. - */ - virtual Json::Value beam_intrinsics() const = 0; - - /** - * Retrieves imu intrinsics of the sensor. - */ - virtual Json::Value imu_intrinsics() const = 0; - - /** - * Retrieves lidar intrinsics of the sensor. - */ - virtual Json::Value lidar_intrinsics() const = 0; - - /** - * Retrieves lidar data format. - */ - virtual Json::Value lidar_data_format() const = 0; - - /** - * Gets the calibaration status of the sensor. - */ - 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. - * @param[in] timeout_sec The timeout to use in seconds, this argument - * is optional. - */ - static std::string firmware_version_string( - const std::string& hostname, - int timeout_sec = DEFAULT_HTTP_REQUEST_TIMEOUT_SECONDS); - - /** - * Retrieves sensor firmware version information. - * - * @param[in] hostname hostname of the sensor to communicate with. - * @param[in] timeout_sec The timeout to use in seconds, this argument - * is optional. - */ - static ouster::util::version firmware_version( - const std::string& hostname, - int timeout_sec = DEFAULT_HTTP_REQUEST_TIMEOUT_SECONDS); - - /** - * Creates an instance of the SensorHttp interface. - * - * @param[in] hostname hostname of the sensor to communicate with. - * @param[in] timeout_sec The timeout to use in seconds, this argument - * is optional. - */ - static std::unique_ptr create( - const std::string& hostname, - int timeout_sec = DEFAULT_HTTP_REQUEST_TIMEOUT_SECONDS); -}; - -} // namespace util -} // namespace sensor -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/types.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/types.h deleted file mode 100644 index a9bf6065..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/types.h +++ /dev/null @@ -1,1431 +0,0 @@ -/** - * Copyright (c) 2018, Ouster, Inc. - * All rights reserved. - * - * @file - * @brief Ouster client datatypes and constants - */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "nonstd/optional.hpp" -#include "version.h" - -namespace ouster { - -using nonstd::optional; - -/** - * For image operations. - * - * @tparam T The data type for the array. - */ -template -using img_t = Eigen::Array; - -/** Used for transformations. */ -using mat4d = Eigen::Matrix; - -namespace sensor { - -/** Unit of range from sensor packet, in meters. */ -constexpr double range_unit = 0.001; - -/** Design values for altitude and azimuth offset angles for gen1 sensors. */ -extern const std::vector gen1_altitude_angles; -/** Design values for altitude and azimuth offset angles for gen1 sensors. */ -extern const std::vector gen1_azimuth_angles; - -/** Design values for imu and lidar to sensor-frame transforms. */ -extern const mat4d default_imu_to_sensor_transform; - -/** Design values for imu and lidar to sensor-frame transforms. */ -extern const mat4d default_lidar_to_sensor_transform; - -/** - * 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 columsn 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 - -}; - -/** - * Mode controlling timestamp method. Refer to the sensor documentation for the - * meaning of each option. - */ -enum timestamp_mode { - /** - * Timestamp mode unspecified. - */ - TIME_FROM_UNSPEC = 0, - - /** - * Use the internal clock. - */ - TIME_FROM_INTERNAL_OSC, - - /** - * A free running counter synced to the SYNC_PULSE_IN input - * counts seconds (# of pulses) and nanoseconds since sensor turn - * on. - */ - TIME_FROM_SYNC_PULSE_IN, - - /** Synchronize with an external PTP master. */ - TIME_FROM_PTP_1588 -}; - -/** - * Mode controlling sensor operation. Refer to the sensor documentation for the - * meaning of each option. - */ -enum OperatingMode { - OPERATING_NORMAL = 1, ///< Normal sensor operation - OPERATING_STANDBY ///< Standby -}; - -/** - * Mode controlling ways to input timesync information. Refer to the sensor - * documentation for the meaning of each option. - */ -enum MultipurposeIOMode { - - MULTIPURPOSE_OFF = 1, ///< Multipurpose IO is turned off (default) - - /** - * Used in conjunction with timestamp_mode::TIME_FROM_SYNC_PULSE_IN - * to enable time pulses in on the multipurpose io input. - */ - MULTIPURPOSE_INPUT_NMEA_UART, - - /** - * Output a SYNC_PULSE_OUT signal synchronized with - * the internal clock. - */ - MULTIPURPOSE_OUTPUT_FROM_INTERNAL_OSC, - - /** - * Output a SYNC_PULSE_OUT signal synchronized with - * a SYNC_PULSE_IN provided to the unit. - */ - MULTIPURPOSE_OUTPUT_FROM_SYNC_PULSE_IN, - - /** - * Output a SYNC_PULSE_OUT signal synchronized with - * an external PTP IEEE 1588 master. - */ - MULTIPURPOSE_OUTPUT_FROM_PTP_1588, - - /** - * Output a SYNC_PULSE_OUT signal with a user defined - * rate in an integer number of degrees. - */ - MULTIPURPOSE_OUTPUT_FROM_ENCODER_ANGLE -}; - -/** - * Polarity represents polarity of NMEA UART and SYNC_PULSE inputs and outputs. - * See sensor docs for more details. - */ -enum Polarity { - POLARITY_ACTIVE_LOW = 1, ///< ACTIVE_LOW - POLARITY_ACTIVE_HIGH ///< ACTIVE_HIGH -}; - -/** - * Baud rate the sensor attempts for NMEA UART input $GPRMC messages - * See sensor docs for more details. - */ -enum NMEABaudRate { - BAUD_9600 = 1, ///< 9600 bits per second UART baud rate - BAUD_115200 ///< 115200 bits per second UART baud rate -}; - -/** Profile indicating packet format of lidar data. */ -enum UDPProfileLidar { - PROFILE_LIDAR_UNKNOWN = 0, - - /** Legacy lidar data */ - PROFILE_LIDAR_LEGACY, - - /** 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, - - /** FuSa two-word pixel */ - PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL, -}; - -/** Profile indicating packet format of IMU data. */ -enum UDPProfileIMU { - PROFILE_IMU_LEGACY = 1, ///< Legacy IMU data -}; - -/** Full scale range for IMU data. */ -enum FullScaleRange { - /** Higher precision lower range measurement mode */ - FSR_NORMAL = 0, - - /** Lower precision higher range measurement mode */ - FSR_EXTENDED -}; - -/** Priority of returns for the lidar to output. - * Lidar can have more than 1 or 2 detected "returns". - * This indicates to the lidar which ones it should output. - * See sensor docs for more details. - */ -enum ReturnOrder { - /** Lidar returns the strongest returns first */ - ORDER_STRONGEST_TO_WEAKEST = 0, - - /** Lidar returns the furthest returns first */ - ORDER_FARTHEST_TO_NEAREST, - - /** Lidar returns the nearest returns first */ - ORDER_NEAREST_TO_FARTHEST, - - /** DEPRECATED: Only Present In Old Test Firmware */ - ORDER_DEPRECATED_STRONGEST_RETURN_FIRST, - ORDER_DEPRECATED_LAST_RETURN_FIRST -}; - -/** Thermal Shutdown status. */ -enum ThermalShutdownStatus { - THERMAL_SHUTDOWN_NORMAL = 0x00, ///< Normal operation - THERMAL_SHUTDOWN_IMMINENT = 0x01, ///< Thermal Shutdown imminent -}; - -/** 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% -}; - -/** - * Convenience type alias for azimuth windows, the window over which the - * sensor fires in millidegrees. - */ -using AzimuthWindow = std::pair; -/** - * Convenience type alias for column windows, the window over which the - * sensor fires in columns. - */ -using ColumnWindow = std::pair; - -/** - * Struct for sensor configuration parameters. - */ -struct sensor_config { - optional udp_dest; ///< The destination address for the - ///< lidar/imu data to be sent to - optional udp_port_lidar; ///< The destination port for the lidar - ///< data to be sent to - optional udp_port_imu; ///< The destination port for the imu data - ///< to be sent to - - // TODO: replace ts_mode and ld_mode when timestamp_mode and - // lidar_mode get changed to CapsCase - /** - * 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; - - /** - * 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; - - /** - * The gyro full scale measurement range to use. - * Refer to FullScaleRange for more details. - */ - optional gyro_fsr; - - /** - * The accelerometer full scale measurement range to use. - * Refer to FullScaleRange for more details. - */ - optional accel_fsr; - - /** - * The priority of returns for the lidar to output. - * Refer to ReturnOrder for more details. - */ - optional return_order; - - /** - * The minimum detection range of the lidar in cm. - */ - optional min_range_threshold_cm; -}; - -/** 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 from-sensor calibration information */ -struct calibration_status { - optional reflectivity_status; - optional reflectivity_timestamp; -}; - -/** Stores parsed information from metadata and */ -struct sensor_info { - // clang-format off - std::string - name{}; ///< user-convenience client-side assignable name, corresponds - ///< to hostname in metadata.json if present - std::string sn{}; ///< sensor serial number corresponding to prod_sn in - ///< metadata.json - std::string - fw_rev{}; ///< fw revision corresponding to build_rev in metadata.json - 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 = - mat4d::Zero(); ///< transform between beam and lidar frame - mat4d imu_to_sensor_transform = - mat4d::Zero(); ///< transform between sensor coordinate - ///< frame and imu - mat4d lidar_to_sensor_transform = - mat4d::Zero(); ///< transform between lidar and sensor - ///< coordinate frames - // TODO read extrinsic from metadata.json in the future - mat4d extrinsic = - mat4d::Zero(); ///< user-convenience client-side assignable extrinsic - ///< matrix, currently is not read from metadata.json - 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 - - std::string build_date{}; ///< build date from FW sensor_info - std::string image_rev{}; ///< image rev from FW sensor_info - std::string prod_pn{}; ///< prod pn - std::string status{}; ///< sensor status at time of pulling metadata - - calibration_status cal{}; ///< sensor calibration - sensor_config config{}; ///< parsed sensor config if available from metadata - - /* Constructor from metadata */ - sensor_info(const std::string& metadata, bool skip_beam_validation = false); - - /* Empty constructor -- keep for */ - sensor_info(); - - /* Return original metadata string should sensor_info have been - * constructed from one -- this string will be **unchanged** and - * will not reflect the changes to fields made to sensor_info*/ - std::string original_string() const; - - /* Return an updated version of the metadata string reflecting any - * changes to the sensor_info. - * Errors out if changes are incompatible but does not check for validity */ - std::string updated_metadata_string() const; - - bool has_fields_equal(const sensor_info& other) const; - - private: - std::string - original_metadata_string{}; ///< string from which values were parsed - // clang-format on -}; - -/** - * Equality for data_format. - * - * @param[in] lhs The first object to compare. - * @param[in] rhs The second object to compare. - * - * @return lhs == rhs - */ -bool operator==(const data_format& lhs, const data_format& rhs); - -/** - * Not-Equality for data_format. - * - * @param[in] lhs The first object to compare. - * @param[in] rhs The second object to compare. - * - * @return lhs != rhs - */ -bool operator!=(const data_format& lhs, const data_format& rhs); - -/** - * Equality for sensor_info. - * - * @param[in] lhs The first object to compare. - * @param[in] rhs The second object to compare. - * - * @return lhs == rhs - */ -bool operator==(const sensor_info& lhs, const sensor_info& rhs); - -/** - * Not-Equality for sensor_info. - * - * @param[in] lhs The first object to compare. - * @param[in] rhs The second object to compare. - * - * @return lhs != rhs - */ -bool operator!=(const sensor_info& lhs, const sensor_info& rhs); - -/** - * Equality for sensor config. - * - * @param[in] lhs The first object to compare. - * @param[in] rhs The second object to compare. - * - * @return lhs == rhs - */ -bool operator==(const sensor_config& lhs, const sensor_config& rhs); - -/** - * Not-Equality for sensor config. - * - * @param[in] lhs The first object to compare. - * @param[in] rhs The second object to compare. - * - * @return lhs != rhs - */ -bool operator!=(const sensor_config& lhs, const sensor_config& rhs); - -/** - * Equality of sensor calibration. - * - * @param[in] lhs The first object to compare. - * @param[out] rhs The second object to compare. - */ -bool operator==(const calibration_status& lhs, const calibration_status& rhs); - -/** - * Not-Equality of sensor calibration. - * - * @param[in] lhs The first object to compare. - * @param[out] rhs The second object to compare. - */ -bool operator!=(const calibration_status& lhs, const calibration_status& rhs); - -/** - * Get a default sensor_info for the given lidar mode. - * - * @param[in] mode lidar mode to generate default sensor_info for. - * - * @return default sensor_info for the OS1-64. - */ -sensor_info default_sensor_info(lidar_mode mode); - -/** - * Get string representation of a lidar mode. - * - * @param[in] mode lidar_mode to get the string representation for. - * - * @return string representation of the lidar mode, or "UNKNOWN". - */ -std::string to_string(lidar_mode mode); - -/** - * Get lidar mode from string. - * - * @param[in] s String to decode. - * - * @return lidar mode corresponding to the string, or 0 on error. - */ -lidar_mode lidar_mode_of_string(const std::string& s); - -/** - * Get number of columns in a scan for a lidar mode. - * - * @param[in] mode lidar_mode to get the number of columns for. - * - * @return number of columns per rotation for the mode. - */ -uint32_t n_cols_of_lidar_mode(lidar_mode mode); - -/** - * Get the lidar rotation frequency from lidar mode. - * - * @param[in] mode Lidar mode to get the rotation frequency from. - * - * @return lidar rotation frequency in Hz. - */ -int frequency_of_lidar_mode(lidar_mode mode); - -/** - * Get string representation of a timestamp mode. - * - * @param[in] mode timestamp_mode to get the string representation for. - * - * @return string representation of the timestamp mode, or "UNKNOWN". - */ -std::string to_string(timestamp_mode mode); - -/** - * Get timestamp mode from string. - * - * @param[in] s String to decode into a timestamp mode. - * - * @return timestamp mode corresponding to the string, or 0 on error. - */ -timestamp_mode timestamp_mode_of_string(const std::string& s); - -/** - * Get string representation of an operating mode. - * - * @param[in] mode Operating mode to get the string representation from. - * - * @return string representation of the operating mode, or "UNKNOWN". - */ -std::string to_string(OperatingMode mode); - -/** - * Get operating mode from string. - * - * @param[in] s String to get the operating mode from. - * - * @return operating mode corresponding to the string, or 0 on error. - */ -optional operating_mode_of_string(const std::string& s); - -/** - * Get string representation of a multipurpose io mode. - * - * @param[in] mode Multipurpose io mode to get a string representation from. - * - * @return string representation of the multipurpose io mode, or "UNKNOWN". - */ -std::string to_string(MultipurposeIOMode mode); - -/** - * Get multipurpose io mode from string. - * - * @param[in] s String to decode into a multipurpose io mode. - * - * @return multipurpose io mode corresponding to the string, or 0 on error. - */ -optional multipurpose_io_mode_of_string( - const std::string& s); - -/** - * Get string representation of a polarity. - * - * @param[in] polarity The polarity to get the string representation of. - * - * @return string representation of the polarity, or "UNKNOWN". - */ -std::string to_string(Polarity polarity); - -/** - * Get polarity from string. - * - * @param[in] s The string to decode into a polarity. - * - * @return polarity corresponding to the string, or 0 on error. - */ -optional polarity_of_string(const std::string& s); - -/** - * Get string representation of a NMEA Baud Rate. - * - * @param[in] rate The NNEABaudRate to get the string representation of. - * - * @return string representation of the NMEA baud rate, or "UNKNOWN". - */ -std::string to_string(NMEABaudRate rate); - -/** - * Get nmea baud rate from string. - * - * @param[in] s The string to decode into a NMEA baud rate. - * - * @return nmea baud rate corresponding to the string, or 0 on error. - */ -optional nmea_baud_rate_of_string(const std::string& s); - -/** - * Get string representation of an Azimuth Window. - * - * @param[in] azimuth_window The azimuth window to get the string -representation. of - * - * @return string representation of the azimuth window. - */ -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 full scale range setting from string - * - * @param[in] s The string to decode into a full scale range. - * - * @return full scale range corresponding to the string, or nullopt on error. - */ -optional full_scale_range_of_string(const std::string& s); - -/** - * Get return order setting from string - * - * @param[in] s The string to decode into a return order. - * - * @return return order corresponding to the string, or nullopt on error. - */ -optional return_order_of_string(const std::string& s); - -/** - * Get string representation of a Return Order. - * - * @param[in] return_order The return order to get the string - * representation of. - * - * @return string representation of the return order. - */ -std::string to_string(ReturnOrder return_order); - -/** - * Get string representation of a Full Scale Range. - * - * @param[in] full_scale_range The shot limiting status to get the string - * representation of. - * - * @return string representation of the full scale range. - */ -std::string to_string(FullScaleRange full_scale_range); - -/** - * 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(const double signal_multiplier); - -/** - * Parse metadata text blob from the sensor into a sensor_info struct. - * - * String and vector fields will have size 0 if the parameter cannot - * be found or parsed, while lidar_mode will be set to 0 (invalid). - * - * @throw runtime_error if the text is not valid json - * - * @param[in] metadata a text blob returned by get_metadata from client.h. - * @param[in] skip_beam_validation whether to skip validation on metdata - not - * for use on recorded data or metadata - * from sensors - * - * @return a sensor_info struct populated with a subset of the metadata. - */ -sensor_info parse_metadata(const std::string& metadata, - bool skip_beam_validation = false); - -/** - * Parse metadata given path to a json file. - * - * @throw runtime_error if json file does not exist or is malformed. - * - * @param[in] json_file path to a json file containing sensor metadata. - * @param[in] skip_beam_validation whether to skip validation on metadata - not - * for use on recorded data or metadata - * from sensors - * - * @return a sensor_info struct populated with a subset of the metadata. - */ -sensor_info metadata_from_json(const std::string& json_file, - bool skip_beam_validation = false); - -/** - * String representation of the sensor_info. All fields included. NOT equivalent - * or interchangeable with metadata from sensor. - * - * @param[in] info sensor_info struct - * - * @return a debug string in json format - */ -// clang-format off -[[deprecated("This is a debug function. Use original_string() or " - "updated_metadata_string()")]] std::string -to_string(const sensor_info& info); -// clang-format on - -/** - * Parse config text blob from the sensor into a sensor_config struct. - * - * All fields are optional, and will only be set if found. - * - * @throw runtime_error if the text is not valid json. - * - * @param[in] config a text blob given by get_config from client.h. - * - * @return a sensor_config struct populated with the sensor config. - * parameters. - */ -sensor_config parse_config(const std::string& config); - -/** - * Get a string representation of sensor config. Only set fields will be - * represented. - * - * @param[in] config a struct of sensor config. - * - * @return a json sensor config string. - */ -std::string to_string(const sensor_config& config); - -/** - * 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); - -/** - * Get a string representation of sensor calibration. Only set fields will be - * represented. - * - * @param[in] cal a struct of calibration. - * - * @return string representation of sensor calibration. - */ - -std::string to_string(const calibration_status& cal); - -/** - * Get client version. - * - * @return client version string - */ -std::string client_version(); - -/** - * Get version information from the metadata. - * - * @param[in] metadata string. - * - * @return version corresponding to the string, or invalid_version on error. - */ -ouster::util::version firmware_version_from_metadata( - const std::string& metadata); - -// clang-format off -/** 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 - INTENSITY = 3, ///< @deprecated Use SIGNAL instead - 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 - AMBIENT = 7, ///< @deprecated Use NEAR_IR instead - 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 -}; -// clang-format on - -/** - * 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); - -/** - * Types of channel fields. - */ -enum ChanFieldType { VOID = 0, UINT8, UINT16, UINT32, UINT64 }; - -/** - * 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 { - protected: - template - T px_field(const uint8_t* px_buf, ChanField i) const; - - template - void block_field_impl(Eigen::Ref> field, ChanField i, - const uint8_t* packet_buf) const; - - struct Impl; - std::shared_ptr impl_; - - std::vector> - field_types_; - - public: - packet_format(UDPProfileLidar udp_profile_lidar, size_t pixels_per_column, - size_t columns_per_packet); - - 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; - - const uint64_t max_frame_id; - - /** - * Read the packet type packet header. - * - * @param[in] lidar_buf the lidar buf. - * - * @return the packet type. - */ - uint16_t packet_type(const uint8_t* lidar_buf) const; - - /** - * Read the frame_id packet header. - * - * @param[in] lidar_buf the lidar buf. - * - * @return the frame id. - */ - uint32_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 - /** - * 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 col_measurement_id instead")]] uint32_t col_encoder( - const uint8_t* col_buf) - const; ///< @deprecated Encoder count is deprecated as it is redundant - ///< with measurement id, barring a multiplication factor which - ///< varies by lidar mode. Use col_measurement_id instead - [[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; - - /** - * Returns maximum available size of parsing block usable with block_field - * - * if packet format does not allow for block parsing, returns 0 - */ - int block_parsable() const; - - /** - * Copy the specified channel field out of a packet measurement block. - * Faster traversal than col_field, but has to copy the entire packet all at - * once. - * - * @tparam T T should be an unsigned integer type large enough to store - * values of the specified field. Otherwise, data will be truncated. - * - * @param[out] field destination eigen array - * @param[in] f the channel field to copy. - * @param[in] lidar_buf the lidar buffer. - */ - template ::value, T>::type = 0> - void block_field(Eigen::Ref> field, ChanField f, - const uint8_t* lidar_buf) 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; - - // IMU packet accessors - /** - * Read sys ts from imu packet buffer. - * @param[in] imu_buf the imu packet buffer. - * - * @return sys ts from imu pacet buffer. - */ - uint64_t imu_sys_ts(const uint8_t* imu_buf) const; - - /** - * Read acceleration timestamp. - * - * @param[in] imu_buf the imu packet buffer. - * - * @return acceleration ts from imu packet buffer. - */ - uint64_t imu_accel_ts(const uint8_t* imu_buf) const; - - /** - * Read gyro timestamp. - * - * @param[in] imu_buf the imu packet buffer. - * - * @return gyro ts from imu packet buffer. - */ - uint64_t imu_gyro_ts(const uint8_t* imu_buf) const; - - /** - * Read acceleration in x. - * - * @param[in] imu_buf the imu packet buffer. - * - * @return acceleration in x. - */ - float imu_la_x(const uint8_t* imu_buf) const; - - /** - * Read acceleration in y. - * - * @param[in] imu_buf the imu packet buffer. - * - * @return acceleration in y. - */ - float imu_la_y(const uint8_t* imu_buf) const; - - /** - * Read acceleration in z. - * - * @param[in] imu_buf the imu packet buffer. - * - * @return acceleration in z. - */ - float imu_la_z(const uint8_t* imu_buf) const; - - /** - * Read angular velocity in x. - * - * @param[in] imu_buf the imu packet buffer. - * - * @return angular velocity in x. - */ - float imu_av_x(const uint8_t* imu_buf) const; - - /** - * Read angular velocity in y. - * - * @param[in] imu_buf the imu packet buffer. - * - * @return angular velocity in y. - */ - float imu_av_y(const uint8_t* imu_buf) const; - - /** - * Read angular velocity in z. - * - * @param[in] imu_buf the imu packet buffer. - * - * @return angular velocity in z. - */ - float imu_av_z(const uint8_t* imu_buf) const; - - /** - * Get the mask of possible values that can be parsed by the channel field - * - * @param[in] f the channel field - * - * @return mask of possible values - */ - uint64_t field_value_mask(ChanField f) const; - - /** - * Get number of bits in the channel field - * - * @param[in] f the channel field - * - * @return number of bits - */ - int field_bitness(ChanField f) const; -}; - -/** @defgroup OusterClientTypeGetFormat Get Packet Format functions */ - -/** - * Get a packet parser for a particular data format. - * - * @ingroup OusterClientTypeGetFormat - * - * @param[in] info parameters provided by the sensor. - * - * @return a packet_format suitable for parsing UDP packets sent by the sensor. - */ -const packet_format& get_format(const sensor_info& info); - -/** - * Get a packet parser for a particular data format. - * - * @ingroup OusterClientTypeGetFormat - * - * @param[in] udp_profile_lidar lidar profile - * @param[in] pixels_per_column pixels per column - * @param[in] columns_per_packet columns per packet - * - * @return a packet_format suitable for parsing UDP packets sent by the sensor. - */ -const packet_format& get_format(UDPProfileLidar udp_profile_lidar, - size_t pixels_per_column, - size_t columns_per_packet); - -/** - * Encapsulate a packet buffer and attributes associated with it. - */ -struct Packet { - uint64_t host_timestamp; - std::vector buf; - - Packet(int size = 65536) : host_timestamp{0} { - // this is necessary due to how client works - it may read size() + 1 - // bytes into the packet in case of rogue packet coming through - buf.reserve(size + 1); - buf.resize(size, 0); - } - - template - PacketType& as() { - return static_cast(*this); - } -}; - -/** - * Encapsulate a lidar packet buffer and attributes associated with it. - */ -struct LidarPacket : public Packet { - using Packet::Packet; -}; - -/** - * Encapsulate an imu packet buffer and attributes associated with it. - */ -struct ImuPacket : public Packet { - using Packet::Packet; -}; - -namespace impl { - -/** Maximum number of allowed lidar profiles */ -constexpr int MAX_NUM_PROFILES = 32; - -} // namespace impl - -} // namespace sensor -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/udp_packet_source.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/udp_packet_source.h deleted file mode 100644 index 15e2a17a..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/udp_packet_source.h +++ /dev/null @@ -1,622 +0,0 @@ -/** - * Copyright (c) 2023, Ouster, Inc. - * All rights reserved. - * - * @file - * @brief Wrapper around sensor::client to provide buffering - * - * *Not* a public API. Currently part of the Python bindings implementation. - * - * Maintains a single-producer / single-consumer circular buffer that can be - * populated by a thread without holding the GIL to deal the relatively small - * default OS buffer size and high sensor UDP data rate. Must be thread-safe to - * allow reading data without holding the GIL while other references to the - * client exist. - */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "ouster/client.h" -#include "ouster/impl/ring_buffer.h" -#include "ouster/types.h" - -namespace ouster { -namespace sensor { -namespace impl { - -struct Event { - int source; - client_state state; - - bool operator==(const Event& other) const { - return source == other.source && state == other.state; - } -}; - -} // namespace impl -} // namespace sensor -} // namespace ouster - -namespace std { - -template <> -struct hash { - std::size_t operator()(const ouster::sensor::impl::Event& e) const { - auto h = std::hash{}; - return h(e.source) ^ h(static_cast(e.state)); - } -}; - -} // namespace std - -namespace ouster { -namespace sensor { -namespace impl { - -using EventSet = std::unordered_set; - -/** - * Thread safe event queue. - * - * Safe to use with many-to-many producers/consumers, although some - * considerations apply. - * - * Two main ways of usage are: one queue per multiple consumers, using - * next(events) to have consumers wait on specific events, or using - * multiple queues, one per consumer, as implemented in publisher/subscriber. - */ -class EventQueue { - mutable std::mutex m; - mutable std::condition_variable cv; - std::deque q; - - template - Event _next(Predicate&& p) { - Event e; - { - std::unique_lock lock{m}; - cv.wait(lock, p); - - e = q.front(); - q.pop_front(); - } - cv.notify_all(); - return e; - } - - template - Event _next_timeout(float sec, Predicate&& p) { - Event e; - { - std::unique_lock lock{m}; - using fsec = std::chrono::duration; - bool timeout = !cv.wait_for(lock, fsec{sec}, p); - - if (timeout) return {-1, client_state::TIMEOUT}; - - e = q.front(); - q.pop_front(); - } - cv.notify_all(); - return e; - } - - public: - /** - * Push an event to the back of the queue. - * - * Notifies all threads waiting on the queue. - * - * @param[in] e event - */ - void push(Event e) { - { - std::lock_guard lock{m}; - q.push_back(e); - } - cv.notify_all(); - } - - /** - * Push a [first,last) range of events to the back of the queue. - * - * Notifies all threads waiting on the queue. - * - * @param[in] first iterator to the first event - * @param[in] last past-the-end iterator - */ - template - void push(EventIterT first, EventIterT last) { - { - std::lock_guard lock{m}; - q.insert(q.end(), first, last); - } - cv.notify_all(); - } - - /** - * Push an event to the front of the queue. - * - * Notifies all threads waiting on the queue. - * - * @param[in] e event - */ - void push_priority(Event e) { - { - std::lock_guard lock{m}; - q.push_front(e); - } - cv.notify_all(); - } - - /** - * Pop an event from the front of the queue. - * If the queue is empty, blocks until an event is pushed. - * - * Notifies all other threads waiting on the queue. - * - * @return event - */ - Event pop() { - return _next([this] { return !q.empty(); }); - } - - /** - * Pop an event from the front of the queue. - * If the queue is empty, blocks until an event is pushed or timeout_sec - * seconds have passed - * - * Notifies all other threads waiting on the queue. - * - * @param[in] timeout_sec timeout time in seconds - * @return event or Event{-1, client_state::TIMEOUT} in case of timeout - */ - Event pop(float timeout_sec) { - return _next_timeout(timeout_sec, [this] { return !q.empty(); }); - } - - /** - * Pop an event of the set of events from the front of the queue. - * Blocks until a suitable event is at the front of the queue. - * - * Notifies all other threads waiting on the queue. - * - * @param[in] events subset of events to wait for - * @return event - */ - Event next(const EventSet& events) { - return _next([this, &events] { - return !q.empty() && events.count(q.front()) == 1; - }); - } - - /** - * Pop an event of the set of events from the front of the queue. - * Blocks until a suitable event is at the front of the queue, or - * timeout_sec seconds have passed. - * - * Notifies all other threads waiting on the queue. - * - * @param[in] timeout_sec timeout time in seconds - * @param[in] events subset of events to wait for - * @return event or Event{-1, client_state::TIMEOUT} in case of timeout - */ - Event next(float timeout_sec, const EventSet& events) { - return _next_timeout(timeout_sec, [this, &events] { - return !q.empty() && events.count(q.front()) == 1; - }); - } - - /** - * Flush the queue, immediately returning all elements. - * - * @return queue with all remaining events - */ - // TODO: [[nodiscard]] once we move to cpp17 -- Tim T. - std::deque flush() { - std::lock_guard lock{m}; - std::deque out; - out.swap(q); - return out; - } -}; - -class Publisher { - protected: - EventSet events_; - std::shared_ptr q_; - - public: - /** - * Construct publisher accepting a corresponding set of events - * - * @param[in] events set of events to accept - */ - Publisher(EventSet events) - : events_(std::move(events)), q_(std::make_shared()) {} - - /** - * Construct empty publisher with events to be set later - */ - Publisher() : Publisher(EventSet{}) {} - - /** - * Sets publisher to accept events of type `e` - * - * @param[in] e Event type to accept - */ - void set_accept(Event e) { events_.insert(e); } - - /** - * Checks whether publisher accepts event type. - * - * @param[in] e Event type - * @return true if publisher accepts events of type e - */ - bool accepts(Event e) const { return events_.count(e); } - - /** - * Publish event to the publisher queue. - * - * @param[in] e event - * @param[in] to_front if true, publishes event to the front of the queue - */ - void publish(Event e, bool to_front = false) { - if (accepts(e)) { - if (to_front) { - q_->push_priority(e); - } else { - q_->push(e); - } - } - } - - /** - * Retrieve internal event queue. - * - * Mainly used for constructing subscribers. - * - * @return shared pointer to EventQueue - */ - std::shared_ptr queue() { return q_; } -}; - -class Subscriber { - protected: - std::shared_ptr q_; - std::shared_ptr> rb_; - - static bool _has_packet(Event e) { - return e.state & (client_state::LIDAR_DATA | client_state::IMU_DATA); - } - - public: - Subscriber(std::shared_ptr q, - std::shared_ptr> rb) - : q_(q), rb_(rb) {} - - Subscriber(Subscriber&& other) : q_(), rb_() { - std::swap(q_, other.q_); - std::swap(rb_, other.rb_); - } - - /** - * Pop the next event from the queue. - * - * Blocks thread until event is available. - * - * @return event - */ - Event pop() { return q_->pop(); } - - /** - * Pop the next event from the queue. - * - * Blocks thread until event is available, or timeout is reached. - * - * @param[in] timeout_sec timeout in seconds - * @return event or {-1, client_state::TIMEOUT} - */ - Event pop(float timeout_sec) { return q_->pop(timeout_sec); } - - /** - * Retrieve the packet to the corresponding event. - * - * Packet is guaranteed to stay valid until advance() is called. - * Will throw if the event does not correspond to any packets. - * - * @param[in] e event - * @return packet corresponding to the event - */ - Packet& packet(Event e) { return rb_->front(e); } - const Packet& packet(Event e) const { return rb_->front(e); } - - /** - * Advance the ring buffer read index for the corresponding event. - */ - void advance(Event e) { - if (_has_packet(e)) rb_->pop(e); - } - - /** - * Flush the queue, releasing all corresponding packets from the ring buffer - */ - void flush() { - auto events = q_->flush(); - for (const auto& e : events) { - if (e.state == client_state::EXIT) { - // return exit event back to the queue for later processing - q_->push_priority(e); - } else { - advance(e); - } - } - } -}; - -class Producer { - protected: - std::vector> pubs_; - std::vector> clients_; - std::shared_ptr> rb_; - - std::mutex mtx_; - std::atomic stop_; - - bool _verify() const; - - public: - Producer() - : rb_(std::make_shared>()), stop_(false) {} - - // TODO: move out to client_state extensions of some sort - /* Extra bit flag compatible with client_state to signal buffer overflow. */ - static constexpr int CLIENT_OVERFLOW = 0x10; - - /** - * Add client and allocate buffers for it. - * - * @param[in] cli shared_ptr with initialized client - * @param[in] lidar_buf_size size of the lidar buffer, in packets - * @param[in] lidar_packet_size size of the lidar packet, in bytes - * @param[in] imu_buf_size size of the imu buffer, in packets - * @param[in] imu_packet_size size of the imu packet, in bytes - * @return id of the client used in produced events e.g. - * Event{id, client_state} - */ - int add_client(std::shared_ptr cli, size_t lidar_buf_size, - size_t lidar_packet_size, size_t imu_buf_size, - size_t imu_packet_size); - - /** - * Add client and allocate buffers for it. - * - * Calculates the buffer sizes for the client based on hz rate and provided - * seconds_to_buffer parameter. - * - * @param[in] cli shared_ptr with initialized client - * @param[in] info sensor_info corresponding to the client - * @param[in] seconds_to_buffer amount of seconds worth of buffer allocation - * @return id of the client used in produced events e.g. - * Event{id, client_state} - */ - int add_client(std::shared_ptr cli, const sensor_info& info, - float seconds_to_buffer); - - /** - * Subscribe to a preassembled publisher. - * - * @param[in] pub shared_ptr containing preassembled publisher - * @return shared_ptr with subscriber corresponding to the publisher - */ - std::shared_ptr subscribe(std::shared_ptr pub); - - /** - * Subscribe to a specific set of events. - * - * @param[in] events set of events to subscribe to - * @return shared_ptr with subscriber waiting on the events - */ - std::shared_ptr subscribe(EventSet events); - - /** - * Write data from the network into the circular buffer, reporting events to - * publishers. - * - * Internally verifies that at least some publishers are subscribed to all - * of the events that could be reported by the producer, otherwise returns - * early. - * - * Will return when either shutdown() is called by one of the threads, or - * when CLIENT_ERROR or EXIT are reported by clients. - */ - void run(); - - /** - * Signal the producer to exit and reports EXIT event to all listening - * subscribers, then waits for producer thread to exit before returning. - * - * Additionally, clears all internal publishers and buffers. - */ - void shutdown(); - - /** - * Reports total amount of currently stored packets in internal buffers. - * - * NOTE: this is not a great metric since it does not report specific - * buffers, but the total amount instead. - */ - size_t size() const { return rb_->size(); } - - /** - * Reports total allocated capacity of packets stored in internal buffers. - * - * NOTE: this is not a great metric since it does not report specific - * buffers, but the total amount instead. - */ - size_t capacity() const { return rb_->capacity(); } -}; - -class UDPPacketSource : protected Producer, protected Subscriber { - void _accept_client_events(int id); - - public: - UDPPacketSource(); - - /** - * Add client and allocate buffers for it. - * - * @param[in] cli shared_ptr with initialized client - * @param[in] lidar_buf_size size of the lidar buffer, in packets - * @param[in] lidar_packet_size size of the lidar packet, in bytes - * @param[in] imu_buf_size size of the imu buffer, in packets - * @param[in] imu_packet_size size of the imu packet, in bytes - * @return id of the client used in produced events e.g. - * Event{id, client_state} - */ - void add_client(std::shared_ptr cli, size_t lidar_buf_size, - size_t lidar_packet_size, size_t imu_buf_size, - size_t imu_packet_size); - - /** - * Add client and allocate buffers for it. - * - * Calculates the buffer sizes for the client based on hz rate and provided - * seconds_to_buffer parameter. - * - * @param[in] cli shared_ptr with initialized client - * @param[in] info sensor_info corresponding to the client - * @param[in] seconds_to_buffer amount of seconds worth of buffer allocation - * @return id of the client used in produced events e.g. - * Event{id, client_state} - */ - void add_client(std::shared_ptr cli, const sensor_info& info, - float seconds_to_buffer); - - using Producer::capacity; - using Producer::shutdown; - using Producer::size; - void produce() { Producer::run(); } - - using Subscriber::advance; - using Subscriber::flush; - using Subscriber::packet; - using Subscriber::pop; -}; - -class BufferedUDPSource : protected Producer, protected Subscriber { - BufferedUDPSource(); - - public: - /** - * Listen for sensor data using client - * - * @param[in] client externally created client - * @param[in] lidar_buf_size size of the lidar buffer, in packets - * @param[in] lidar_packet_size size of the lidar packet, in bytes - * @param[in] imu_buf_size size of the imu buffer, in packets - * @param[in] imu_packet_size size of the imu packet, in bytes - */ - BufferedUDPSource(std::shared_ptr client, size_t lidar_buf_size, - size_t lidar_packet_size, size_t imu_buf_size, - size_t imu_packet_size); - - /** - * Listen for sensor data using client - * - * Calculates the buffer sizes for the client based on hz rate and provided - * seconds_to_buffer parameter. - * - * @param[in] client externally created client - * @param[in] info sensor_info corresponding to the client - * @param[in] seconds_to_buffer amount of seconds worth of buffer allocation - */ - BufferedUDPSource(std::shared_ptr client, const sensor_info& info, - float seconds_to_buffer); - - using Producer::capacity; - using Producer::shutdown; - using Producer::size; - void produce() { Producer::run(); } - - using Subscriber::flush; - - /** - * Pop the next client_state from the queue. - * - * Blocks thread until client_state is available. - * - * @return client_state - */ - client_state pop() { return Subscriber::pop().state; } - - /** - * Pop the next client_state from the queue. - * - * Blocks thread until client_state is available, or timeout is reached. - * - * @param[in] timeout_sec timeout in seconds - * @return client_state or client_state::TIMEOUT - */ - client_state pop(float timeout_sec) { - return Subscriber::pop(timeout_sec).state; - } - - /** - * Retrieve the packet to the corresponding client_state. - * - * Packet is guaranteed to stay valid until advance() is called. - * Will throw if the event does not correspond to any packets. - * - * @param[in] st client_state - * @return packet corresponding to st - */ - Packet& packet(client_state st) { return Subscriber::packet({0, st}); } - const Packet& packet(client_state st) const { - return Subscriber::packet({0, st}); - } - - /** - * Advances read in internal buffers - * - * @param[in] st client_state to advance. Does nothing if st is not one of - * LIDAR_DATA or IMU_DATA - */ - void advance(client_state st) { Subscriber::advance({0, st}); } - - /** - * Read next available packet in the buffer. - * - * If client_state returns LIDAR_DATA, submitted lidar packet will be - * populated, similarly if client_state returns IMU_DATA, submitted - * imu packet will be populated instead. - * - * Blocks if the queue is empty for up to `timeout_sec` (zero means wait - * forever). Should only be called by the consumer thread. If reading from - * the network was blocked because the buffer was full, the the - * CLIENT_OVERFLOW flag will be set on the next returned status. - * - * @param[in] lidarp lidar packet to read into - * @param[in] imup imu packet to read into - * @param[in] timeout_sec maximum time to wait for data. - * @return client status, see sensor::poll_client(). - */ - client_state consume(LidarPacket& lidarp, ImuPacket& imup, - float timeout_sec); -}; - -} // namespace impl -} // namespace sensor -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/util.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/util.h deleted file mode 100644 index b31117f0..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/util.h +++ /dev/null @@ -1,19 +0,0 @@ -/** - * Copyright (c) 2023, Ouster, Inc. - * All rights reserved. - */ - -#include - -/* Helper function which produces a combination of the two roots, with - * any value in root_orig that exists in root_new replaced with the root_new - * value. The vector changed indicates each field changed in the result - * from the original - */ - -namespace ouster { - -Json::Value combined(const Json::Value& root_orig, const Json::Value& root_new, - std::vector& changed); - -} diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/version.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/version.h deleted file mode 100644 index fe162cab..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/include/ouster/version.h +++ /dev/null @@ -1,125 +0,0 @@ -/** - * Copyright (c) 2018, Ouster, Inc. - * All rights reserved. - * - * @file - * @brief Simple version struct - */ - -#include -#include - -#pragma once - -namespace ouster { -namespace util { - -struct version { - uint16_t major; ///< Major version number - uint16_t minor; ///< Minor version number - uint16_t patch; ///< Patch(or revision) version number -}; - -const version invalid_version = {0, 0, 0}; - -/** \defgroup ouster_client_version_operators Ouster Client version.h Operators - * @{ - */ -/** - * Equality operation for version structs. - * - * @param[in] u The first version to compare. - * @param[in] v The second version to compare. - * - * @return If the versions are the same. - */ -inline bool operator==(const version& u, const version& v) { - return u.major == v.major && u.minor == v.minor && u.patch == v.patch; -} - -/** - * Less than operation for version structs. - * - * @param[in] u The first version to compare. - * @param[in] v The second version to compare. - * - * @return If the first version is less than the second version. - */ -inline bool operator<(const version& u, const version& v) { - return (u.major < v.major) || (u.major == v.major && u.minor < v.minor) || - (u.major == v.major && u.minor == v.minor && u.patch < v.patch); -} - -/** - * Less than or equal to operation for version structs. - * - * @param[in] u The first version to compare. - * @param[in] v The second version to compare. - * - * @return If the first version is less than or equal to the second version. - */ -inline bool operator<=(const version& u, const version& v) { - return u < v || u == v; -} - -/** - * In-equality operation for version structs. - * - * @param[in] u The first version to compare. - * @param[in] v The second version to compare. - * - * @return If the versions are not the same. - */ -inline bool operator!=(const version& u, const version& v) { return !(u == v); } - -/** - * Greater than or equal to operation for version structs. - * - * @param[in] u The first version to compare. - * @param[in] v The second version to compare. - * - * @return If the first version is greater than or equal to the second version. - */ -inline bool operator>=(const version& u, const version& v) { return !(u < v); } - -/** - * Greater than operation for version structs. - * - * @param[in] u The first version to compare. - * @param[in] v The second version to compare. - * - * @return If the first version is greater than the second version. - */ -inline bool operator>(const version& u, const version& v) { return !(u <= v); } -/** @}*/ - -/** - * Get string representation of a version. - * - * @param[in] v version. - * - * @return string representation of the version. - */ -std::string to_string(const version& v); - -/** - * Get version from string. - * - * @param[in] s string. - * - * @return version corresponding to the string, or invalid_version on error. - */ -[[deprecated("Use version_from_string instead")]] version version_of_string( - const std::string& s); - -/** - * Get version from string. - * - * @param[in] ver string. - * - * @return version corresponding to the string, or invalid_version on error. - */ -version version_from_string(const std::string& ver); - -} // namespace util -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/client.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/client.cpp deleted file mode 100644 index af3d2cd2..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/client.cpp +++ /dev/null @@ -1,696 +0,0 @@ -/** - * Copyright(c) 2018, Ouster, Inc. - * All rights reserved. - */ - -#include "ouster/client.h" - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "logging.h" -#include "ouster/impl/client_poller.h" -#include "ouster/impl/netcompat.h" -#include "ouster/sensor_http.h" -#include "ouster/types.h" - -using namespace std::chrono_literals; -namespace chrono = std::chrono; -using ouster::sensor::impl::Logger; -using ouster::sensor::util::SensorHttp; - -namespace ouster { -namespace sensor { - -struct client { - SOCKET lidar_fd; - SOCKET imu_fd; - std::string hostname; - Json::Value meta; - ~client() { - impl::socket_close(lidar_fd); - impl::socket_close(imu_fd); - } -}; - -// defined in types.cpp -Json::Value config_to_json(const sensor_config& config); - -namespace { - -// default udp receive buffer size on windows is very low -- use 256K -const int RCVBUF_SIZE = 1024 * 1024; - -int32_t get_sock_port(SOCKET sock_fd) { - struct sockaddr_storage ss; - socklen_t addrlen = sizeof ss; - - if (!impl::socket_valid( - getsockname(sock_fd, (struct sockaddr*)&ss, &addrlen))) { - logger().error("udp getsockname(): {}", impl::socket_get_error()); - return SOCKET_ERROR; - } - - if (ss.ss_family == AF_INET) - return ntohs(((struct sockaddr_in*)&ss)->sin_port); - else if (ss.ss_family == AF_INET6) - return ntohs(((struct sockaddr_in6*)&ss)->sin6_port); - else - return SOCKET_ERROR; -} - -SOCKET udp_data_socket(int port) { - struct addrinfo hints, *info_start, *ai; - - memset(&hints, 0, sizeof hints); - hints.ai_family = AF_UNSPEC; - hints.ai_socktype = SOCK_DGRAM; - hints.ai_flags = AI_PASSIVE; - - auto port_s = std::to_string(port); - - int ret = getaddrinfo(NULL, port_s.c_str(), &hints, &info_start); - if (ret != 0) { - logger().error("udp getaddrinfo(): {}", gai_strerror(ret)); - return SOCKET_ERROR; - } - if (info_start == NULL) { - logger().error("udp getaddrinfo(): empty result"); - return SOCKET_ERROR; - } - - // try to bind a dual-stack ipv6 socket, but fall back to ipv4 only if that - // fails (when ipv6 is disabled via kernel parameters). Use two passes to - // deal with glibc addrinfo ordering: - // https://sourceware.org/bugzilla/show_bug.cgi?id=9981 - for (auto preferred_af : {AF_INET6, AF_INET}) { - for (ai = info_start; ai != NULL; ai = ai->ai_next) { - if (ai->ai_family != preferred_af) continue; - - // choose first addrinfo where bind() succeeds - SOCKET sock_fd = - socket(ai->ai_family, ai->ai_socktype, ai->ai_protocol); - if (!impl::socket_valid(sock_fd)) { - logger().warn("udp socket(): {}", impl::socket_get_error()); - continue; - } - - int off = 0; - if (ai->ai_family == AF_INET6 && - setsockopt(sock_fd, IPPROTO_IPV6, IPV6_V6ONLY, (char*)&off, - sizeof(off))) { - logger().warn("udp setsockopt(): {}", impl::socket_get_error()); - impl::socket_close(sock_fd); - continue; - } - - if (impl::socket_set_reuse(sock_fd)) { - logger().warn("udp socket_set_reuse(): {}", - impl::socket_get_error()); - } - - if (::bind(sock_fd, ai->ai_addr, (socklen_t)ai->ai_addrlen)) { - logger().warn("udp bind(): {}", impl::socket_get_error()); - impl::socket_close(sock_fd); - continue; - } - - // bind() succeeded; set some options and return - if (impl::socket_set_non_blocking(sock_fd)) { - logger().warn("udp fcntl(): {}", impl::socket_get_error()); - impl::socket_close(sock_fd); - continue; - } - - if (setsockopt(sock_fd, SOL_SOCKET, SO_RCVBUF, (char*)&RCVBUF_SIZE, - sizeof(RCVBUF_SIZE))) { - logger().warn("udp setsockopt(): {}", impl::socket_get_error()); - impl::socket_close(sock_fd); - continue; - } - - freeaddrinfo(info_start); - return sock_fd; - } - } - - // could not bind() a UDP server socket - freeaddrinfo(info_start); - logger().error("failed to bind udp socket"); - return SOCKET_ERROR; -} - -SOCKET mtp_data_socket(int port, const std::string& udp_dest_host = "", - const std::string& mtp_dest_host = "") { - struct addrinfo hints, *info_start, *ai; - - memset(&hints, 0, sizeof hints); - hints.ai_family = AF_UNSPEC; - hints.ai_socktype = SOCK_DGRAM; - hints.ai_flags = AI_PASSIVE; - - auto port_s = std::to_string(port); - - int ret = getaddrinfo(NULL, port_s.c_str(), &hints, &info_start); - if (ret != 0) { - logger().error("mtp getaddrinfo(): {}", gai_strerror(ret)); - return SOCKET_ERROR; - } - if (info_start == NULL) { - logger().error("mtp getaddrinfo(): empty result"); - return SOCKET_ERROR; - } - - for (auto preferred_af : {AF_INET}) { // TODO test with AF_INET6 - for (ai = info_start; ai != NULL; ai = ai->ai_next) { - if (ai->ai_family != preferred_af) continue; - - // choose first addrinfo where bind() succeeds - SOCKET sock_fd = - socket(ai->ai_family, ai->ai_socktype, ai->ai_protocol); - if (!impl::socket_valid(sock_fd)) { - logger().warn("mtp socket(): {}", impl::socket_get_error()); - continue; - } - - if (impl::socket_set_reuse(sock_fd)) { - logger().warn("mtp socket_set_reuse(): {}", - impl::socket_get_error()); - } - - if (::bind(sock_fd, ai->ai_addr, (socklen_t)ai->ai_addrlen)) { - logger().warn("mtp bind(): {}", impl::socket_get_error()); - impl::socket_close(sock_fd); - continue; - } - - // bind() succeeded; join to multicast group on with preferred - // address connect only if addresses are not empty - if (!udp_dest_host.empty()) { - ip_mreq mreq; - mreq.imr_multiaddr.s_addr = inet_addr(udp_dest_host.c_str()); - if (!mtp_dest_host.empty()) { - mreq.imr_interface.s_addr = - inet_addr(mtp_dest_host.c_str()); - } else { - mreq.imr_interface.s_addr = htonl(INADDR_ANY); - } - - if (setsockopt(sock_fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, - (char*)&mreq, sizeof(mreq))) { - logger().warn("mtp setsockopt(): {}", - impl::socket_get_error()); - impl::socket_close(sock_fd); - continue; - } - } - - // join to multicast group succeeded; set some options and return - if (impl::socket_set_non_blocking(sock_fd)) { - logger().warn("mtp fcntl(): {}", impl::socket_get_error()); - impl::socket_close(sock_fd); - continue; - } - - if (setsockopt(sock_fd, SOL_SOCKET, SO_RCVBUF, (char*)&RCVBUF_SIZE, - sizeof(RCVBUF_SIZE))) { - logger().warn("mtp setsockopt(): {}", impl::socket_get_error()); - impl::socket_close(sock_fd); - continue; - } - - freeaddrinfo(info_start); - return sock_fd; - } - } - - // could not bind() a MTP server socket - freeaddrinfo(info_start); - logger().error("failed to bind mtp socket"); - return SOCKET_ERROR; -} - -Json::Value collect_metadata(const std::string& hostname, int timeout_sec) { - // Note, this function throws std::runtime_error if - // 1. the metadata couldn't be retrieved - // 2. the sensor is in the INITIALIZING state when timeout is reached - auto sensor_http = SensorHttp::create(hostname, timeout_sec); - auto timeout_time = - chrono::steady_clock::now() + chrono::seconds{timeout_sec}; - - std::string status; - // TODO: can remove this loop when we drop support for FW 2.4 - while (true) { - if (chrono::steady_clock::now() >= timeout_time) { - throw std::runtime_error( - "A timeout occurred while waiting for the sensor to " - "initialize."); - } - status = sensor_http->sensor_info()["status"].asString(); - if (status != "INITIALIZING") { - break; - } - std::this_thread::sleep_for(1s); - } - - try { - auto metadata = sensor_http->metadata(); - - metadata["ouster-sdk"]["client_version"] = client_version(); - metadata["ouster-sdk"]["output_source"] = "collect_metadata"; - - return metadata; - } catch (const std::runtime_error& e) { - 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"); - } -} - -} // namespace - -bool get_config(const std::string& hostname, sensor_config& config, bool active, - int timeout_sec) { - auto sensor_http = SensorHttp::create(hostname, timeout_sec); - auto res = sensor_http->get_config_params(active); - config = parse_config(res); - return true; -} - -bool set_config(const std::string& hostname, const sensor_config& config, - uint8_t config_flags, int timeout_sec) { - auto sensor_http = SensorHttp::create(hostname, timeout_sec); - - // reset staged config to avoid spurious errors - auto config_params = sensor_http->active_config_params(); - Json::Value config_params_copy = config_params; - - // set all desired config parameters - Json::Value config_json = config_to_json(config); - for (const auto& key : config_json.getMemberNames()) { - config_params[key] = config_json[key]; - } - - 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; - } - - // 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(); - } - } - - // 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"); - sensor_http->set_udp_dest_auto(); - - auto staged = sensor_http->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"]; - } - } - - // 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); - sensor_http->set_config_param(".", config_params_str); - // reinitialize to make all staged parameters effective - sensor_http->reinitialize(); - } - - // save if indicated - if (config_flags & CONFIG_PERSIST) { - sensor_http->save_config_params(); - } - - return true; -} - -std::string get_metadata(client& cli, int timeout_sec, bool legacy_format) { - // Note, this function calls functions that throw std::runtime_error - // on timeout. - 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()); - throw; - } - - Json::StreamWriterBuilder builder; - builder["enableYAMLCompatibility"] = true; - builder["precision"] = 6; - builder["indentation"] = " "; - 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."); - } - - // 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 = SensorHttp::firmware_version(cli.hostname, timeout_sec); - // 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."); - } - return legacy_format ? convert_to_legacy(metadata_string) : metadata_string; -} - -bool init_logger(const std::string& log_level, const std::string& log_file_path, - bool rotating, int max_size_in_bytes, int max_files) { - if (log_file_path.empty()) { - return Logger::instance().configure_stdout_sink(log_level); - } else { - return Logger::instance().configure_file_sink( - log_level, log_file_path, rotating, max_size_in_bytes, max_files); - } -} - -std::shared_ptr init_client(const std::string& hostname, int lidar_port, - int imu_port) { - logger().info( - "initializing sensor client: {} expecting lidar port/imu port: {}/{}", - hostname, lidar_port, imu_port); - - auto cli = std::make_shared(); - cli->hostname = hostname; - - cli->lidar_fd = udp_data_socket(lidar_port); - 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 cli; -} - -std::shared_ptr init_client(const std::string& hostname, - const std::string& udp_dest_host, - lidar_mode ld_mode, timestamp_mode ts_mode, - int lidar_port, int imu_port, - int timeout_sec, bool persist_config) { - auto cli = init_client(hostname, lidar_port, imu_port); - if (!cli) return std::shared_ptr(); - logger().info("(0 means a random port will be chosen)"); - - // 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(); - - 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; - if (persist_config) config_flags |= CONFIG_PERSIST; - 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 std::shared_ptr(); - } catch (const std::runtime_error& e) { - // log error message - logger().error("init_client(): {}", e.what()); - return std::shared_ptr(); - } - - return cli; -} - -std::shared_ptr mtp_init_client(const std::string& hostname, - const sensor_config& config, - const std::string& mtp_dest_host, - bool main, int timeout_sec, - bool persist_config) { - int lidar_port = config.udp_port_lidar ? config.udp_port_lidar.value() : 0; - int imu_port = config.udp_port_imu ? config.udp_port_imu.value() : 0; - auto udp_dest = config.udp_dest ? config.udp_dest.value() : ""; - - logger().info( - "initializing sensor client: {} expecting ports: {}/{}, multicast " - "group: {} (0 means a random port will be chosen)", - hostname, lidar_port, imu_port, udp_dest); - - auto cli = std::make_shared(); - cli->hostname = hostname; - - cli->lidar_fd = mtp_data_socket(lidar_port, udp_dest, mtp_dest_host); - cli->imu_fd = mtp_data_socket( - imu_port); // no need to join multicast group second time - - if (!impl::socket_valid(cli->lidar_fd) || !impl::socket_valid(cli->imu_fd)) - return std::shared_ptr(); - - if (main) { - auto lidar_port = get_sock_port(cli->lidar_fd); - auto imu_port = get_sock_port(cli->imu_fd); - - sensor_config config_copy{config}; - try { - uint8_t config_flags = 0; - if (lidar_port) config_copy.udp_port_lidar = lidar_port; - if (imu_port) config_copy.udp_port_imu = imu_port; - if (persist_config) config_flags |= CONFIG_PERSIST; - config_copy.operating_mode = OPERATING_NORMAL; - set_config(hostname, config_copy, 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 std::shared_ptr(); - } catch (const std::runtime_error& e) { - // log error message - logger().error("init_client(): {}", e.what()); - return std::shared_ptr(); - } - } - - return cli; -} - -namespace impl { - -struct client_poller { - fd_set rfds; - SOCKET max_fd; - client_state err; -}; - -std::shared_ptr make_poller() { - return std::make_unique(); -} - -void reset_poll(client_poller& poller) { - FD_ZERO(&poller.rfds); - poller.max_fd = 0; - poller.err = client_state::TIMEOUT; -} - -void set_poll(client_poller& poller, const client& c) { - FD_SET(c.lidar_fd, &poller.rfds); - FD_SET(c.imu_fd, &poller.rfds); - poller.max_fd = std::max({poller.max_fd, c.lidar_fd, c.imu_fd}); -} - -int poll(client_poller& poller, int timeout_sec) { - timeval tv; - tv.tv_sec = timeout_sec; - tv.tv_usec = 0; - - SOCKET retval = - select((int)poller.max_fd + 1, &poller.rfds, NULL, NULL, &tv); - - if (!impl::socket_valid(retval)) { - if (impl::socket_exit()) { - poller.err = client_state::EXIT; - } else { - logger().error("select: {}", impl::socket_get_error()); - poller.err = client_state::CLIENT_ERROR; - } - - return -1; - } - - return (int)retval; -} - -client_state get_error(const client_poller& poller) { return poller.err; } - -client_state get_poll(const client_poller& poller, const client& c) { - client_state s = client_state(0); - - if (FD_ISSET(c.lidar_fd, &poller.rfds)) s = client_state(s | LIDAR_DATA); - if (FD_ISSET(c.imu_fd, &poller.rfds)) s = client_state(s | IMU_DATA); - - return s; -} - -} // namespace impl - -client_state poll_client(const client& c, const int timeout_sec) { - impl::client_poller poller; - impl::reset_poll(poller); - impl::set_poll(poller, c); - int res = impl::poll(poller, timeout_sec); - if (res <= 0) { - // covers TIMEOUT and error states - return impl::get_error(poller); - } else { - return impl::get_poll(poller, c); - } -} - -static bool recv_fixed(SOCKET fd, void* buf, int64_t 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 (bytes_read == -1) { - logger().error("recvfrom: {}", impl::socket_get_error()); - } else { - logger().warn("Unexpected udp packet length: {}", bytes_read); - } - return false; -} - -bool read_lidar_packet(const client& cli, uint8_t* buf, size_t bytes) { - return recv_fixed(cli.lidar_fd, buf, bytes); -} - -bool read_lidar_packet(const client& cli, uint8_t* buf, - const packet_format& pf) { - return read_lidar_packet(cli, buf, pf.lidar_packet_size); -} - -bool read_lidar_packet(const client& cli, LidarPacket& packet) { - auto now = std::chrono::high_resolution_clock::now(); - packet.host_timestamp = - std::chrono::duration_cast( - now.time_since_epoch()) - .count(); - return read_lidar_packet(cli, packet.buf.data(), packet.buf.size()); -} - -bool read_imu_packet(const client& cli, uint8_t* buf, size_t bytes) { - return recv_fixed(cli.imu_fd, buf, bytes); -} - -bool read_imu_packet(const client& cli, uint8_t* buf, const packet_format& pf) { - return read_imu_packet(cli, buf, pf.imu_packet_size); -} - -bool read_imu_packet(const client& cli, ImuPacket& packet) { - auto now = std::chrono::high_resolution_clock::now(); - packet.host_timestamp = - std::chrono::duration_cast( - now.time_since_epoch()) - .count(); - return read_imu_packet(cli, packet.buf.data(), packet.buf.size()); -} - -int get_lidar_port(const client& cli) { return get_sock_port(cli.lidar_fd); } - -int get_imu_port(const client& cli) { return get_sock_port(cli.imu_fd); } - -bool in_multicast(const std::string& addr) { - return IN_MULTICAST(ntohl(inet_addr(addr.c_str()))); -} - -/** - * 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 diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/curl_client.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/curl_client.h deleted file mode 100644 index 83e196d3..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/curl_client.h +++ /dev/null @@ -1,122 +0,0 @@ -#include - -#include -#include -#include - -#include "http_client.h" -#include "logging.h" - -class CurlClient : public ouster::util::HttpClient { - public: - CurlClient(const std::string& base_url_, int timeout_seconds) - : HttpClient(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); - curl_easy_setopt(curl_handle, CURLOPT_TIMEOUT, timeout_seconds); - } - - virtual ~CurlClient() override { - curl_easy_cleanup(curl_handle); - curl_global_cleanup(); - } - - public: - std::string get(const std::string& url) const override { - auto full_url = url_combine(base_url, url); - return execute_get(full_url); - } - - std::string encode(const std::string& str) const override { - auto curl_str_deleter = [](char* s) { curl_free(s); }; - auto encoded_str = std::unique_ptr( - curl_easy_escape(curl_handle, str.c_str(), str.length()), - curl_str_deleter); - return std::string(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, int attempts = 3, - int retry_delay_ms = 500) const { - long http_code = 0; - if (attempts < 1) { - throw std::invalid_argument( - "CurlClient::execute_get: invalid number of retries"); - } - if (retry_delay_ms < 0) { - throw std::invalid_argument( - "CurlClient::execute_get: invalid retry delay"); - } - curl_easy_setopt(curl_handle, CURLOPT_URL, url.c_str()); - curl_easy_setopt(curl_handle, CURLOPT_HTTPGET, 1L); - while (attempts--) { - buffer.clear(); - auto res = curl_easy_perform(curl_handle); - if (res == CURLE_SEND_ERROR) { - // Specific versions of curl does'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)); - } - curl_easy_getinfo(curl_handle, CURLINFO_RESPONSE_CODE, &http_code); - if (200 <= http_code && http_code < 300) { - // HTTP 2XX means a successful response - return buffer; - } - if (attempts && 500 <= http_code && http_code < 600) { - // HTTP 5XX means a server error, so we should re-attempt. - // log a warning and sleep before re-attempting - ouster::sensor::logger().warn( - "Re-attempting CurlClient::execute_get after failure for url: [{}] with the code: [{}] - and return: {}", - url, http_code, buffer); - std::this_thread::sleep_for( - std::chrono::milliseconds(retry_delay_ms)); - } - } - - // the request failed after repeated attempts with a response code other - // than HTTP 2XX - 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)); - } - - 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; -}; diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/http_client.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/http_client.h deleted file mode 100644 index 7f900711..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/http_client.h +++ /dev/null @@ -1,56 +0,0 @@ -/** - * Copyright (c) 2022, Ouster, Inc. - * All rights reserved. - * - * @file http_client.h - * @brief This file holds a minimal abstract interface for an http client. - * - * - */ - -#pragma once - -#include - -namespace ouster { -namespace util { - -/** - * An abstraction of http client to handle http requests - */ -class HttpClient { - public: - /** - * Constructs an HttpClient object to communicate with an http server. - * - * @param[in] base_url_ url to the http server. - */ - HttpClient(const std::string& base_url_) : base_url(base_url_) {} - - virtual ~HttpClient() {} - - /** - * Executes a GET request towards the provided url. - * - * @param[in] url http request url. - * - * @return the result of the execution. If request fails it returns an empty - * string. - */ - virtual std::string get(const std::string& url) const = 0; - - /** - * Encodes the given string as a url. - * - * @param[in] str the string to be encoded as a url. - * - * @return Returns the string as encoded url. - */ - virtual std::string encode(const std::string& str) const = 0; - - protected: - std::string base_url; -}; - -} // namespace util -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/image_processing.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/image_processing.cpp deleted file mode 100644 index c9b020ad..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/image_processing.cpp +++ /dev/null @@ -1,262 +0,0 @@ -/** - * Copyright (c) 2018, Ouster, Inc. - * All rights reserved. - */ - -#include "ouster/image_processing.h" - -#include - -#include -#include -#include -#include - -namespace ouster { -namespace viz { - -namespace { - -/* - * damping makes the autoexposure smooth and avoids flickering however, it - * becomes slower to update. - * 1.0 --> slowest, smoothest - * 0.0 --> fastest, prone to flickering - */ -const double ae_damping = 0.90; - -/* - * for performance reasons, we may not want to update every frame but rather - * every few frames - */ -const int ae_default_update_every = 3; - -/* for performance reasons, only consider a subset of points */ -const size_t ae_stride = 4; - -/* if there are too few points, do nothing */ -const size_t ae_min_nonzero_points = 100; - -/* default percentile for scaling in autoexposure */ -const double ae_default_percentile = 0.1; - -} // namespace - -AutoExposure::AutoExposure() - : lo_percentile(ae_default_percentile), - hi_percentile(ae_default_percentile), - ae_update_every(ae_default_update_every) {} - -AutoExposure::AutoExposure(int update_every) - : lo_percentile(ae_default_percentile), - hi_percentile(ae_default_percentile), - ae_update_every(update_every) {} - -AutoExposure::AutoExposure(double lo_percentile, double hi_percentile, - int update_every) - : lo_percentile(lo_percentile), - hi_percentile(hi_percentile), - ae_update_every(update_every) {} - -template -void AutoExposure::update(Eigen::Ref> image, bool update_state) { - Eigen::Map> key_eigen(image.data(), image.size()); - - // int a; - if (counter == 0 && update_state) { - const size_t n = key_eigen.rows(); - std::vector indices; - indices.reserve(n); - for (size_t i = 0; i < n; i += ae_stride) { - // ignore 0 values, which are often due to dropped packets etc - if (key_eigen[i] > 0) { - indices.push_back(i); - } - } - if (indices.size() < ae_min_nonzero_points) { - // too few nonzero values, nothing to do - return; - } - auto cmp = [&](const size_t a, const size_t b) { - return key_eigen(a) < key_eigen(b); - }; - - const size_t lo_kth_extreme = - static_cast(indices.size() * lo_percentile); - std::nth_element(indices.begin(), indices.begin() + lo_kth_extreme, - indices.end(), cmp); - lo = key_eigen[*(indices.begin() + lo_kth_extreme)]; - - const size_t hi_kth_extreme = - static_cast(indices.size() * hi_percentile); - std::nth_element(indices.begin() + lo_kth_extreme, - indices.end() - hi_kth_extreme - 1, indices.end(), - cmp); - hi = key_eigen[*(indices.end() - hi_kth_extreme - 1)]; - - if (!initialized) { - initialized = true; - lo_state = lo; - hi_state = hi; - } - } - if (!initialized) { - return; - } - - // we use the simplest form of exponential smoothing - if (update_state) { - lo_state = ae_damping * lo_state + (1.0 - ae_damping) * lo; - hi_state = ae_damping * hi_state + (1.0 - ae_damping) * hi; - } - - // Apply affine transformation mapping lo_state to lo_percentile and - // hi_state to 1 - hi_percentile. If it would map 0 to positive number, - // instead map using only hi_state - double lo_hi_scale = - (1.0 - (lo_percentile + hi_percentile)) / (hi_state - lo_state); - - if (std::isinf(lo_hi_scale) || std::isnan(lo_hi_scale)) { - // map everything relative to hi_state being 0.5 due to small spread or - // nan - key_eigen *= 0.5 / hi_state; - } else if (lo_hi_scale * (0.0 - lo_state) + lo_percentile <= 0.00) { - // apply affine transformation - key_eigen -= lo_state; - key_eigen *= lo_hi_scale; - key_eigen += lo_percentile; - } else { - // lo_hi_state transformation would map 0 to positive number - // instead, map using only hi_state - key_eigen *= (1.0 - hi_percentile) / (hi_state); - } - - // clamp - key_eigen = key_eigen.max(0.0).min(1.0); - - if (update_state) { - counter = (counter + 1) % ae_update_every; - } -} - -// use overloads vs templates so implicit conversion to Eigen::Ref still works -void AutoExposure::operator()(Eigen::Ref> image, - bool update_state) { - update(image, update_state); -} - -void AutoExposure::operator()(Eigen::Ref> image, - bool update_state) { - update(image, update_state); -} - -namespace { - -/* - * damping makes the correction smooth and avoids flickering. - * however, it becomes slower to update. - * 1.0 --> slowest, smoothest - * 0.0 --> fastest, prone to flickering - */ -const double buc_damping = 0.92; - -/* - * for performance reasons, we may not want to update every frame - * but rather every 8 or so frames. - */ -const int buc_update_every = 8; - -} // namespace - -/* - * computes the dark count, i.e. an additive offset in the brightness of the - * image, to smoothe the difference between rows - */ -template -static Eigen::Array compute_dark_count( - const Eigen::Ref>& image) { - const size_t image_h = image.rows(); - const size_t image_w = image.cols(); - - Eigen::Array tmp = Eigen::Array::Zero(image_w); - Eigen::Array new_dark_count = - Eigen::Array::Zero(image_h); - - // probably computed lazily when used below? - auto row_diffs = image.bottomRows(image_h - 1) - image.topRows(image_h - 1); - - // to handle azimuth-masked data, only consider columns with nonzero values - Eigen::Array col_mask = - image.template cast().colwise().any(); - const size_t n_cols = col_mask.count(); - - if (n_cols == 0) { - return new_dark_count; - } - - img_t row_diffs_nonzero{image_h - 1, n_cols}; - for (size_t i = 0, j = 0; i < image_w && j < n_cols; i++) { - if (col_mask[i]) { - row_diffs_nonzero.col(j) = row_diffs.col(i); - j++; - } - } - - // compute the median of differences between rows - for (size_t i = 1; i < image_h; i++) { - tmp = row_diffs_nonzero.row(i - 1); - std::nth_element(tmp.data(), tmp.data() + n_cols / 2, - tmp.data() + n_cols); - new_dark_count[i] = new_dark_count[i - 1] + tmp[n_cols / 2]; - } - - // remove gradients in the entire height of image by doing linear fit - Eigen::Matrix A(image_h, 2); - for (size_t i = 0; i < image_h; i++) { - A(i, 0) = 1; - A(i, 1) = static_cast(i); - } - Eigen::Matrix x = A.fullPivLu().solve(new_dark_count.matrix()); - new_dark_count -= (A * x).array(); - - // subtract minimum value - new_dark_count -= new_dark_count.minCoeff(); - return new_dark_count; -} - -template -void BeamUniformityCorrector::update(Eigen::Ref> image, - bool update_state) { - const auto image_h = image.rows(); - - // compute dark counts, if necessary - if (dark_count.size() != image_h) { - dark_count = compute_dark_count(image).template cast(); - } else if (update_state && counter == 0) { - // if previous state exists, update using exponential smoothing: - Eigen::ArrayXd new_dark_count = - compute_dark_count(image).template cast(); - dark_count *= buc_damping; - dark_count += new_dark_count * (1.0 - buc_damping); - } - counter = (counter + 1) % buc_update_every; - - // apply the dark count correction - image.colwise() -= dark_count.cast(); - - // clamp any negative values - image = image.cwiseMax(0.0); -} - -void BeamUniformityCorrector::operator()(Eigen::Ref> image, - bool update_state) { - update(image, update_state); -} - -void BeamUniformityCorrector::operator()(Eigen::Ref> image, - bool update_state) { - update(image, update_state); -} - -} // namespace viz -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/lidar_scan.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/lidar_scan.cpp deleted file mode 100644 index bf66eae4..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/lidar_scan.cpp +++ /dev/null @@ -1,837 +0,0 @@ -/** - * Copyright (c) 2018, Ouster, Inc. - * All rights reserved. - */ - -#include "ouster/lidar_scan.h" - -#include -#include -#include -#include -#include -#include - -#include "logging.h" -#include "ouster/impl/lidar_scan_impl.h" -#include "ouster/types.h" - -namespace ouster { - -// clang-format off -/** - * 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 -}; - -//! @cond Doxygen_Suppress -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 -}; -//! @endcond - -// clang-format on -using sensor::ChanField; -using sensor::ChanFieldType; -using sensor::UDPProfileLidar; - -LidarScan::LidarScan() = default; -LidarScan::LidarScan(const LidarScan&) = default; -LidarScan::LidarScan(LidarScan&&) = default; -LidarScan& LidarScan::operator=(const LidarScan&) = default; -LidarScan& LidarScan::operator=(LidarScan&&) = default; -LidarScan::~LidarScan() = default; -namespace impl { - -template -using Table = std::array, N>; - -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}, -}}; - -static const Table fusa_two_word_slots{{ - {ChanField::RANGE, ChanFieldType::UINT32}, - {ChanField::REFLECTIVITY, ChanFieldType::UINT8}, - {ChanField::NEAR_IR, ChanFieldType::UINT16}, - {ChanField::RANGE2, ChanFieldType::UINT32}, - {ChanField::REFLECTIVITY2, ChanFieldType::UINT8}, -}}; - -struct DefaultFieldsEntry { - const std::pair* fields; - size_t n_fields; -}; - -using ouster::sensor::impl::MAX_NUM_PROFILES; -// clang-format off -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()}}, - {UDPProfileLidar::PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL, - {fusa_two_word_slots.data(), fusa_two_word_slots.size()}}, -}}; -// clang-format on - -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}; -} - -bool raw_headers_enabled(const sensor::packet_format& pf, const LidarScan& ls) { - using ouster::sensor::logger; - ChanFieldType raw_headers_ft = ls.field_type(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)); - return false; - } - return true; -} - -} // namespace impl - -// specify sensor:: namespace for doxygen matching -LidarScan::LidarScan(size_t w, size_t h, LidarScanFieldTypes field_types, - size_t columns_per_packet) - : timestamp_{Header::Zero(w)}, - packet_timestamp_{Header::Zero(w / columns_per_packet)}, - measurement_id_{Header::Zero(w)}, - status_{Header::Zero(w)}, - pose_(w, mat4d::Identity()), - field_types_{std::move(field_types)}, - w{static_cast(w)}, - h{static_cast(h)} { - 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(const LidarScan& ls_src, - const LidarScanFieldTypes& field_types) - : timestamp_(ls_src.timestamp_), - packet_timestamp_(ls_src.packet_timestamp_), - measurement_id_(ls_src.measurement_id_), - status_(ls_src.status_), - pose_(ls_src.pose_), - field_types_(field_types), - w(ls_src.w), - h(ls_src.h), - frame_status(ls_src.frame_status), - frame_id(ls_src.frame_id) { - // Initialize 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, static_cast(w), - static_cast(h)}; - } - - // Copy fields - for (const auto& ft : field_types) { - if (ls_src.field_type(ft.first)) { - ouster::impl::visit_field(*this, ft.first, - ouster::impl::copy_and_cast(), ls_src, - ft.first); - } else { - ouster::impl::visit_field(*this, ft.first, - ouster::impl::zero_field()); - } - } -} - -LidarScan::LidarScan(size_t w, size_t h, sensor::UDPProfileLidar profile, - size_t columns_per_packet) - : LidarScan{w, h, impl::lookup_scan_fields(profile), columns_per_packet} {} - -LidarScan::LidarScan(size_t w, size_t h) - : LidarScan{w, h, UDPProfileLidar::PROFILE_LIDAR_LEGACY, - DEFAULT_COLUMNS_PER_PACKET} {} - -sensor::ShotLimitingStatus LidarScan::shot_limiting() const { - return static_cast( - (frame_status & frame_status_masks::FRAME_STATUS_SHOT_LIMITING_MASK) >> - frame_status_shifts::FRAME_STATUS_SHOT_LIMITING_SHIFT); -} - -sensor::ThermalShutdownStatus LidarScan::thermal_shutdown() const { - return static_cast( - (frame_status & - frame_status_masks::FRAME_STATUS_THERMAL_SHUTDOWN_MASK) >> - frame_status_shifts::FRAME_STATUS_THERMAL_SHUTDOWN_SHIFT); -} - -template ::value, T>::type> -Eigen::Ref> LidarScan::field(ChanField f) { - return fields_.at(f).get(); -} - -template ::value, T>::type> -Eigen::Ref> LidarScan::field(ChanField f) const { - return fields_.at(f).get(); -} - -// explicitly instantiate for each supported field type -template Eigen::Ref> LidarScan::field(ChanField f); -template Eigen::Ref> LidarScan::field(ChanField f); -template Eigen::Ref> LidarScan::field(ChanField f); -template Eigen::Ref> LidarScan::field(ChanField f); -template Eigen::Ref> LidarScan::field(ChanField f) const; -template Eigen::Ref> LidarScan::field(ChanField f) const; -template Eigen::Ref> LidarScan::field(ChanField f) const; -template Eigen::Ref> LidarScan::field(ChanField f) const; - -ChanFieldType LidarScan::field_type(ChanField f) const { - return fields_.count(f) ? fields_.at(f).tag : ChanFieldType::VOID; -} - -LidarScan::FieldIter LidarScan::begin() const { return field_types_.cbegin(); } - -LidarScan::FieldIter LidarScan::end() const { return field_types_.cend(); } - -Eigen::Ref> LidarScan::timestamp() { - return timestamp_; -} - -Eigen::Ref> LidarScan::timestamp() const { - return timestamp_; -} - -Eigen::Ref> LidarScan::packet_timestamp() { - return packet_timestamp_; -} -Eigen::Ref> LidarScan::packet_timestamp() - const { - return packet_timestamp_; -} - -uint64_t LidarScan::get_first_valid_packet_timestamp() const { - int total_packets = packet_timestamp().size(); - if (total_packets == 0) { - return 0; // prevent a divide by zero - } - int columns_per_packet = w / total_packets; - - for (int i = 0; i < total_packets; ++i) { - if (status() - .middleRows(i * columns_per_packet, columns_per_packet) - .unaryExpr([](uint32_t s) { return s & 1; }) - .any()) - return packet_timestamp()[i]; - } - - return 0; -} - -Eigen::Ref> LidarScan::measurement_id() { - return measurement_id_; -} -Eigen::Ref> LidarScan::measurement_id() - const { - return measurement_id_; -} - -Eigen::Ref> LidarScan::status() { return status_; } -Eigen::Ref> LidarScan::status() const { - return status_; -} - -std::vector& LidarScan::pose() { return pose_; } - -const std::vector& LidarScan::pose() const { return pose_; } - -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); - } -} - -bool operator==(const LidarScan& a, const LidarScan& b) { - 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() && a.pose() == b.pose(); -} - -LidarScanFieldTypes get_field_types(const LidarScan& ls) { - return {ls.begin(), ls.end()}; -} - -LidarScanFieldTypes get_field_types(UDPProfileLidar udp_profile_lidar) { - // Get typical LidarScan to obtain field types - return impl::lookup_scan_fields(udp_profile_lidar); -} - -LidarScanFieldTypes get_field_types(const sensor::sensor_info& info) { - // Get typical LidarScan to obtain field types - return get_field_types(info.format.udp_profile_lidar); -} - -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(); -} - -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 - << ", packets_per_frame = " << ls.packet_timestamp().size() - << ", 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; - } - - const auto& ts = ls.timestamp(); - ss << " timestamp = (" << ts.minCoeff() << "; " << ts.mean() << "; " - << ts.maxCoeff() << ")," << std::endl; - const auto& packet_ts = ls.packet_timestamp(); - ss << " packet_timestamp = (" << packet_ts.minCoeff() << "; " - << packet_ts.mean() << "; " << packet_ts.maxCoeff() << ")," << std::endl; - const auto& mid = ls.measurement_id().cast(); - ss << " measurement_id = (" << mid.minCoeff() << "; " << mid.mean() << "; " - << mid.maxCoeff() << ")," << std::endl; - const auto& st = ls.status().cast(); - ss << " status = (" << st.minCoeff() << "; " << st.mean() << "; " - << st.maxCoeff() << ")" << std::endl; - ss << " poses = (size: " << ls.pose().size() << ")" << std::endl; - ss << "}"; - return ss.str(); -} - -XYZLut make_xyz_lut(size_t w, size_t h, double range_unit, - 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) && - (azimuth_angles_deg.size() != w * h || - altitude_angles_deg.size() != w * h)) { - throw std::invalid_argument("unexpected scan dimensions"); - } - - 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)); - } - - 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; - } - } - - } 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}; - lut.direction.col(0) = (encoder + azimuth).cos() * altitude.cos(); - lut.direction.col(1) = (encoder + azimuth).sin() * altitude.cos(); - lut.direction.col(2) = altitude.sin(); - - // offsets due to beam origin - lut.offset = LidarScan::Points{w * h, 3}; - 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(); - auto trans = transform.topRightCorner(3, 1).transpose(); - lut.direction.matrix() *= rot; - lut.offset.matrix() *= rot; - lut.offset.matrix() += trans.replicate(w * h, 1); - - // apply scaling factor - lut.direction *= range_unit; - lut.offset *= range_unit; - - return lut; -} - -LidarScan::Points cartesian(const LidarScan& scan, const XYZLut& lut) { - return cartesian(scan.field(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 (reshaped == 0) - .replicate<1, 3>() - .select(nooffset, nooffset + lut.offset); -} - -ScanBatcher::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), - cache(pf.lidar_packet_size), - cache_packet_ts(0), - pf(pf) { - if (pf.columns_per_packet == 0) - throw std::invalid_argument("unexpected columns_per_packet: 0"); -} - -ScanBatcher::ScanBatcher(const sensor::sensor_info& info) - : ScanBatcher(info.format.columns_per_frame, sensor::get_format(info)) {} - -namespace { - -/* - * Generic operation to set all columns in the range [start, end) to zero - */ -struct zero_field_cols { - template - void operator()(Eigen::Ref> field, ChanField, std::ptrdiff_t start, - std::ptrdiff_t end) const { - 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, ChanField f, uint16_t m_id, - const sensor::packet_format& pf, - const uint8_t* col_buf) const { - // user defined fields that we shouldn't change - if (f >= ChanField::CUSTOM0 && f <= 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 == 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; -} - -/** - * 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, ChanField, - const sensor::packet_format& pf, uint16_t col_idx, - const uint8_t* packet_buf) const { - 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 - -void ScanBatcher::_parse_by_col(const uint8_t* packet_buf, LidarScan& ls) { - const bool raw_headers = impl::raw_headers_enabled(pf, ls); - 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 uint64_t ts = pf.col_timestamp(col_buf); - const uint32_t status = pf.col_status(col_buf); - const bool valid = (status & 0x01); - - // drop out-of-bounds data in case of misconfiguration - if (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, ChanField::RAW_HEADERS, zero_field_cols(), - ChanField::RAW_HEADERS, next_headers_m_id, - m_id); - next_headers_m_id = m_id + 1; - } - - impl::visit_field(ls, ChanField::RAW_HEADERS, - pack_raw_headers_col(), ChanField::RAW_HEADERS, - pf, icol, packet_buf); - } - - // drop invalid - if (!valid) continue; - - // zero out missing columns if we jumped forward - if (m_id >= next_valid_m_id) { - for (const auto& field_type : ls) { - if (field_type.first == ChanField::RAW_HEADERS) continue; - if (field_type.first >= ChanField::CUSTOM0 && - field_type.first <= ChanField::CUSTOM9) - 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; - } - - // write new header values - ls.timestamp()[m_id] = ts; - ls.measurement_id()[m_id] = m_id; - ls.status()[m_id] = status; - - impl::foreach_field(ls, parse_field_col(), m_id, pf, col_buf); - } -} - -/* - * Faster version of parse_field_col that works by blocks instead and skips - * extra checks - */ -template -struct parse_field_block { - template - void operator()(Eigen::Ref> field, ChanField f, - const sensor::packet_format& pf, - const uint8_t* packet_buf) const { - // user defined fields that we shouldn't change - if (f >= ChanField::CUSTOM0 && f <= ChanField::CUSTOM9) return; - - pf.block_field(field, f, packet_buf); - } -}; - -void ScanBatcher::_parse_by_block(const uint8_t* packet_buf, LidarScan& ls) { - // zero out missing columns if we jumped forward - const uint16_t first_m_id = - pf.col_measurement_id(pf.nth_col(0, packet_buf)); - if (first_m_id >= next_valid_m_id) { - for (const auto& field_type : ls) { - if (field_type.first >= ChanField::CUSTOM0 && - field_type.first <= ChanField::CUSTOM9) - continue; - impl::visit_field(ls, field_type.first, zero_field_cols(), - field_type.first, next_valid_m_id, first_m_id); - } - zero_header_cols(ls, next_valid_m_id, first_m_id); - next_valid_m_id = first_m_id + pf.columns_per_packet; - } - - // write new header values - 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 uint64_t ts = pf.col_timestamp(col_buf); - const uint32_t status = pf.col_status(col_buf); - - ls.timestamp()[m_id] = ts; - ls.measurement_id()[m_id] = m_id; - ls.status()[m_id] = status; - } - - switch (pf.block_parsable()) { - case 16: - impl::foreach_field(ls, parse_field_block<16>{}, pf, packet_buf); - break; - case 8: - impl::foreach_field(ls, parse_field_block<8>{}, pf, packet_buf); - break; - case 4: - impl::foreach_field(ls, parse_field_block<4>{}, pf, packet_buf); - break; - default: - throw std::invalid_argument("Invalid block dim for packet format"); - } -} - -bool ScanBatcher::operator()(const uint8_t* packet_buf, LidarScan& ls) { - return this->operator()(packet_buf, 0, ls); -} - -bool ScanBatcher::operator()(const ouster::sensor::LidarPacket& packet, - LidarScan& ls) { - return (*this)(packet.buf.data(), packet.host_timestamp, ls); -} - -bool ScanBatcher::operator()(const uint8_t* packet_buf, uint64_t packet_ts, - LidarScan& ls) { - if (ls.w != w || ls.h != h) - throw std::invalid_argument("unexpected scan dimensions"); - if (ls.packet_timestamp().rows() != ls.w / pf.columns_per_packet) - throw std::invalid_argument("unexpected scan columns_per_packet: " + - std::to_string(pf.columns_per_packet)); - - // process cached packet and packet ts - if (cached_packet) { - cached_packet = false; - ls.frame_id = -1; - this->operator()(cache.data(), cache_packet_ts, ls); - } - - const uint64_t f_id = pf.frame_id(packet_buf); - - const bool raw_headers = impl::raw_headers_enabled(pf, ls); - - if (ls.frame_id == -1) { - // expecting to start batching a new scan - next_valid_m_id = 0; - next_headers_m_id = 0; - ls.frame_id = f_id; - zero_header_cols(ls, 0, w); - ls.packet_timestamp().setZero(); - 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); - } else if (ls.frame_id == ((f_id + 1) % (pf.max_frame_id + 1))) { - // drop reordered packets from the previous frame - return false; - } else if (ls.frame_id != f_id) { - // 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 == ChanField::RAW_HEADERS) { - end_m_id = next_headers_m_id; - } - if (field_type.first >= ChanField::CUSTOM0 && - field_type.first <= ChanField::CUSTOM9) - continue; - impl::visit_field(ls, field_type.first, zero_field_cols(), - field_type.first, end_m_id, w); - } - - // store packet buf and ts data to the cache for later processing - std::memcpy(cache.data(), packet_buf, cache.size()); - cache_packet_ts = packet_ts; - cached_packet = true; - - return true; - } - - // handling packet level data: packet_timestamp - const uint8_t* col0_buf = pf.nth_col(0, packet_buf); - const uint16_t packet_id = - pf.col_measurement_id(col0_buf) / pf.columns_per_packet; - if (packet_id < ls.packet_timestamp().rows()) { - ls.packet_timestamp()[packet_id] = packet_ts; - } - - // handling column and pixel level data - bool happy_packet = true; - 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 uint32_t status = pf.col_status(col_buf); - const bool valid = (status & 0x01); - - if (!valid || m_id >= w) { - happy_packet = false; - break; - } - } - - if (pf.block_parsable() && happy_packet && !raw_headers) { - _parse_by_block(packet_buf, ls); - } else { - _parse_by_col(packet_buf, ls); - } - - return false; -} - -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/logging.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/logging.cpp deleted file mode 100644 index e62f6cb4..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/logging.cpp +++ /dev/null @@ -1,103 +0,0 @@ -#include "logging.h" - -#include - -#if (SPDLOG_VER_MAJOR < 1) -namespace spdlog { -namespace sinks { -// rename simple_file_sink_st to basic_file_sink_st -using basic_file_sink_st = simple_file_sink_st; -} // namespace sinks -} // namespace spdlog -#else -#include -#include -#endif - -#include - -#include - -using namespace ouster::sensor::impl; - -const std::string Logger::logger_name{"ouster::sensor"}; - -Logger& Logger::instance() { - static Logger logger_singleton; - return logger_singleton; -} - -spdlog::logger& Logger::get_logger() { return *logger_; } - -Logger::Logger() - : logger_(std::make_unique( - logger_name, std::make_shared())) { - logger_->set_level(spdlog::level::info); - logger_->flush_on(spdlog::level::info); -} - -void Logger::update_sink_and_log_level(spdlog::sink_ptr sink, - const std::string& log_level) { -#if (SPDLOG_VER_MAJOR < 1) - // recreate the logger with the new sink - logger_ = std::make_unique(logger_name, sink); - - using namespace spdlog::level; - auto idx = std::find(std::begin(level_names), std::end(level_names), - log_level.c_str()); - auto ll = idx == std::end(level_names) - ? spdlog::level::off - : static_cast( - std::distance(std::begin(level_names), idx)); -#else - // replace the logger sink with the new sink - logger_->sinks() = {sink}; - - auto ll = spdlog::level::from_str(log_level); -#endif - logger_->set_level(ll); - logger_->flush_on(ll); -} - -bool Logger::configure_stdout_sink(const std::string& log_level) { - try { - update_sink_and_log_level( - std::make_shared(), log_level); - } catch (const spdlog::spdlog_ex& ex) { - std::cerr << logger_name << " init_logger failed: " << ex.what() - << std::endl; - return false; - } - - return true; -} - -bool Logger::configure_file_sink(const std::string& log_level, - const std::string& log_file_path, - bool rotating, int max_size_in_bytes, - int max_files) { - try { - std::shared_ptr> - file_sink; - if (rotating) { - file_sink = std::make_shared( - log_file_path, max_size_in_bytes, max_files); - } else { - file_sink = std::make_shared( - log_file_path, true); - } - update_sink_and_log_level(file_sink, log_level); - } catch (const spdlog::spdlog_ex& ex) { - std::cerr << logger_name << " init_logger failed: " << ex.what() - << std::endl; - return false; - } - - return true; -} - -namespace ouster { -namespace sensor { -spdlog::logger& logger() { return Logger::instance().get_logger(); } -} // namespace sensor -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/logging.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/logging.h deleted file mode 100644 index 01d6b6b6..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/logging.h +++ /dev/null @@ -1,37 +0,0 @@ -#pragma once - -#include - -namespace ouster { -namespace sensor { -namespace impl { - -class Logger { - public: - static Logger& instance(); - - spdlog::logger& get_logger(); - - bool configure_stdout_sink(const std::string& log_level); - - bool configure_file_sink(const std::string& log_level, - const std::string& log_file_path, bool rotating, - int max_size_in_bytes, int max_files); - - private: - Logger(); - - void update_sink_and_log_level(spdlog::sink_ptr sink, - const std::string& log_level); - - private: - static const std::string logger_name; - std::unique_ptr logger_; -}; - -} // namespace impl - -spdlog::logger& logger(); - -} // namespace sensor -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/netcompat.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/netcompat.cpp deleted file mode 100644 index 4c43430f..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/netcompat.cpp +++ /dev/null @@ -1,118 +0,0 @@ -/** - * Copyright (c) 2018, Ouster, Inc. - * All rights reserved. - */ - -#include "ouster/impl/netcompat.h" - -#include - -#if defined _WIN32 - -#include - -#else - -#include -#include -#include - -#include -#include - -#endif - -namespace ouster { -namespace sensor { -namespace impl { - -#ifdef _WIN32 -struct StaticWrapper { - WSADATA wsa_data; - - StaticWrapper() { WSAStartup(MAKEWORD(1, 1), &wsa_data); } - - ~StaticWrapper() { WSACleanup(); } -}; - -static StaticWrapper resources = {}; -#endif - -int socket_close(SOCKET sock) { -#ifdef _WIN32 - return closesocket(sock); -#else - return close(sock); -#endif -} - -std::string socket_get_error() { -#ifdef _WIN32 - int errnum = WSAGetLastError(); - char buf[256] = {0}; - if (FormatMessage(FORMAT_MESSAGE_FROM_SYSTEM, NULL, errnum, 0, buf, - sizeof(buf), NULL) != 0) { - return std::string(buf); - } else { - return std::string{"Unknown WSA error "} + std::to_string(errnum); - } -#else - return std::strerror(errno); -#endif -} - -bool socket_valid(SOCKET sock) { -#ifdef _WIN32 - return sock != SOCKET_ERROR; -#else - return sock >= 0; -#endif -} - -bool socket_exit() { -#ifdef _WIN32 - auto result = WSAGetLastError(); - return result == WSAECONNRESET || result == WSAECONNABORTED || - result == WSAESHUTDOWN; -#else - return errno == EINTR; -#endif -} - -int socket_set_non_blocking(SOCKET value) { -#ifdef _WIN32 - u_long non_blocking_mode = 1; - return ioctlsocket(value, FIONBIO, &non_blocking_mode); -#else - return fcntl(value, F_SETFL, fcntl(value, F_GETFL, 0) | O_NONBLOCK); -#endif -} - -int socket_set_reuse(SOCKET value) { - int option = 1; -#ifndef _WIN32 - int res = - setsockopt(value, SOL_SOCKET, SO_REUSEPORT, &option, sizeof(option)); - if (res != 0) return res; -#endif - return setsockopt(value, SOL_SOCKET, SO_REUSEADDR, (char*)&option, - sizeof(option)); -} - -int socket_set_rcvtimeout(SOCKET sock, int timeout_sec) { -#ifdef _WIN32 - DWORD timeout_ms = timeout_sec * 1000; - return setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, (const char*)&timeout_ms, - sizeof timeout_ms); -#else - struct timeval tv; - tv.tv_sec = timeout_sec; - tv.tv_usec = 0; - return setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, (const char*)&tv, - sizeof tv); -#endif -} - -} // namespace impl -} // namespace sensor -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/parsing.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/parsing.cpp deleted file mode 100644 index a88180f1..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/parsing.cpp +++ /dev/null @@ -1,1038 +0,0 @@ -/** - * Copyright (c) 2018, Ouster, Inc. - * All rights reserved. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "ouster/impl/packet_writer.h" -#include "ouster/types.h" - -namespace ouster { -namespace sensor { - -namespace impl { - -constexpr int imu_packet_size = 48; - -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, {UINT32, 0, 0x7fff, -3}}, - {ChanField::FLAGS, {UINT8, 1, 0b10000000, 7}}, - {ChanField::REFLECTIVITY, {UINT8, 2, 0, 0}}, - {ChanField::NEAR_IR, {UINT16, 2, 0xff00, 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}}, -}}; - -static const Table fusa_two_word_pixel_info{{ - {ChanField::RANGE, {UINT32, 0, 0x7fff, -3}}, - {ChanField::FLAGS, {UINT8, 1, 0b10000000, 7}}, - {ChanField::REFLECTIVITY, {UINT8, 2, 0xff, 0}}, - {ChanField::NEAR_IR, {UINT16, 3, 0xff, -4}}, - {ChanField::RANGE2, {UINT32, 4, 0x7fff, -3}}, - {ChanField::FLAGS2, {UINT8, 5, 0b10000000, 7}}, - {ChanField::REFLECTIVITY2, {UINT8, 6, 0xff, 0}}, - {ChanField::RAW32_WORD1, {UINT32, 0, 0, 0}}, - {ChanField::RAW32_WORD2, {UINT32, 4, 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}}, - {UDPProfileLidar::PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL, - {fusa_two_word_pixel_info.data(), fusa_two_word_pixel_info.size(), 8}}, -}}; - -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; -} - -static int count_set_bits(uint64_t value) { - int count = 0; - while (value) { - count += value & 1; - value >>= 1; - } - return count; -}; - -// TODO: move this out to some generalised FieldInfo utils -uint64_t get_value_mask(const FieldInfo& f) { - // first get type mask - uint64_t type_mask = (uint64_t{1} << (field_type_size(f.ty_tag) * 8)) - 1; - - uint64_t mask = f.mask; - if (mask == 0) mask = type_mask; - if (f.shift > 0) mask >>= f.shift; - if (f.shift < 0) mask <<= std::abs(f.shift); - // final type *may* cut the resultant mask still - mask &= type_mask; - - return mask; -} - -// TODO: move this out to some generalised FieldInfo utils -int get_bitness(const FieldInfo& f) { - return count_set_bits(get_value_mask(f)); -} - -} // 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; - - uint64_t max_frame_id; - - 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, size_t pixels_per_column, - size_t 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; - - if (profile == UDPProfileLidar::PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL) { - max_frame_id = std::numeric_limits::max(); - } else { - max_frame_id = std::numeric_limits::max(); - } - - 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; - - if (lidar_packet_size > 65535) - throw std::invalid_argument( - "lidar_packet_size cannot exceed 65535"); - - 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(UDPProfileLidar udp_profile_lidar, - size_t pixels_per_column, - size_t columns_per_packet) - : impl_{std::make_shared(udp_profile_lidar, pixels_per_column, - columns_per_packet)}, - udp_profile_lidar{udp_profile_lidar}, - lidar_packet_size{impl_->lidar_packet_size}, - imu_packet_size{impl::imu_packet_size}, - columns_per_packet(columns_per_packet), - pixels_per_column(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}, - max_frame_id{impl_->max_frame_id} { - for (const auto& kv : impl_->fields) { - field_types_.push_back({kv.first, kv.second.ty_tag}); - } -} - -packet_format::packet_format(const sensor_info& info) - : packet_format(info.format.udp_profile_lidar, - info.format.pixels_per_column, - info.format.columns_per_packet) {} - -template -void packet_format::block_field_impl(Eigen::Ref> field, ChanField chan, - const uint8_t* packet_buf) const { - if (sizeof(T) < sizeof(SRC)) - throw std::invalid_argument("Dest type too small for specified field"); - - const auto& f = impl_->fields.at(chan); - - size_t offset = f.offset; - uint64_t mask = f.mask; - int shift = f.shift; - size_t channel_data_size = impl_->channel_data_size; - - int cols = field.cols(); - T* data = field.data(); - std::array col_buf; - - for (int icol = 0; icol < columns_per_packet; icol += N) { - for (int i = 0; i < N; ++i) { - col_buf[i] = nth_col(icol + i, packet_buf); - } - - uint16_t m_id = col_measurement_id(col_buf[0]); - - for (int px = 0; px < pixels_per_column; ++px) { - std::ptrdiff_t f_offset = cols * px + m_id; - for (int x = 0; x < N; ++x) { - auto px_src = - col_buf[x] + col_header_size + (px * channel_data_size); - T dst = *reinterpret_cast(px_src + offset); - if (mask) dst &= mask; - if (shift > 0) dst >>= shift; - if (shift < 0) dst <<= std::abs(shift); - *(data + f_offset + x) = dst; - } - } - } -} - -template ::value, T>::type> -void packet_format::block_field(Eigen::Ref> field, ChanField chan, - const uint8_t* packet_buf) const { - const auto& f = impl_->fields.at(chan); - - switch (f.ty_tag) { - case UINT8: - block_field_impl(field, chan, packet_buf); - break; - case UINT16: - block_field_impl(field, chan, packet_buf); - break; - case UINT32: - block_field_impl(field, chan, packet_buf); - break; - case UINT64: - block_field_impl(field, chan, packet_buf); - break; - default: - throw std::invalid_argument("Invalid field for packet format"); - } -} - -// explicitly instantiate for each field type / block dim -template void packet_format::block_field( - Eigen::Ref> field, ChanField chan, - const uint8_t* packet_buf) const; -template void packet_format::block_field( - Eigen::Ref> field, ChanField chan, - const uint8_t* packet_buf) const; -template void packet_format::block_field( - Eigen::Ref> field, ChanField chan, - const uint8_t* packet_buf) const; -template void packet_format::block_field( - Eigen::Ref> field, ChanField chan, - const uint8_t* packet_buf) const; -template void packet_format::block_field( - Eigen::Ref> field, ChanField chan, - const uint8_t* packet_buf) const; -template void packet_format::block_field( - Eigen::Ref> field, ChanField chan, - const uint8_t* packet_buf) const; -template void packet_format::block_field( - Eigen::Ref> field, ChanField chan, - const uint8_t* packet_buf) const; -template void packet_format::block_field( - Eigen::Ref> field, ChanField chan, - const uint8_t* packet_buf) const; -template void packet_format::block_field( - Eigen::Ref> field, ChanField chan, - const uint8_t* packet_buf) const; -template void packet_format::block_field( - Eigen::Ref> field, ChanField chan, - const uint8_t* packet_buf) const; -template void packet_format::block_field( - Eigen::Ref> field, ChanField chan, - const uint8_t* packet_buf) const; -template void packet_format::block_field( - Eigen::Ref> field, ChanField chan, - const uint8_t* packet_buf) const; - -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; - DST dst = *reinterpret_cast(px_src); - if (mask) dst &= mask; - if (shift > 0) dst >>= shift; - if (shift < 0) dst <<= std::abs(shift); - *px_dst = dst; - } -} - -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 = 0; - if (udp_profile_lidar == - UDPProfileLidar::PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL) { - // FuSa profile has 8-bit packet_type - std::memcpy(&res, lidar_buf + 0, sizeof(uint8_t)); - } else { - std::memcpy(&res, lidar_buf + 0, sizeof(uint16_t)); - } - return res; -} - -uint32_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)); - } - - // eUDP frame id is 16 bits, but FUSA frame id is 32 bits - if (udp_profile_lidar == - UDPProfileLidar::PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL) { - uint32_t res = 0; - std::memcpy(&res, lidar_buf + 4, sizeof(res)); - return res; - } else { - uint16_t res = 0; - std::memcpy(&res, lidar_buf + 2, sizeof(res)); - 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 = 0; - if (udp_profile_lidar == - UDPProfileLidar::PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL) { - std::memcpy(&res, lidar_buf + 1, sizeof(uint32_t)); - } else { - 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 = 0; - if (udp_profile_lidar == - UDPProfileLidar::PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL) { - std::memcpy(&res, lidar_buf + 11, sizeof(uint64_t)); - } else { - 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 = 0; - 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 = 0; - 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 = 0; - 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 = 0; - 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 = 0; - 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 = 0; - 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 = 0; - std::memcpy(&res, col_buf + impl_->measurement_id_offset, sizeof(uint16_t)); - return res; -} - -uint32_t packet_format::col_encoder(const uint8_t* col_buf) const { - if (udp_profile_lidar == UDPProfileLidar::PROFILE_LIDAR_LEGACY) { - uint32_t res = 0; - std::memcpy(&res, col_buf + 12, sizeof(uint32_t)); - return res; - } else { - return 0; - } -} - -uint16_t packet_format::col_frame_id(const uint8_t* col_buf) const { - if (udp_profile_lidar == UDPProfileLidar::PROFILE_LIDAR_LEGACY) { - uint16_t res = 0; - 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; -} - -/* IMU packet parsing */ - -uint64_t packet_format::imu_sys_ts(const uint8_t* imu_buf) const { - uint64_t res = 0; - std::memcpy(&res, imu_buf, sizeof(uint64_t)); - return res; -} - -uint64_t packet_format::imu_accel_ts(const uint8_t* imu_buf) const { - uint64_t res = 0; - std::memcpy(&res, imu_buf + 8, sizeof(uint64_t)); - return res; -} - -uint64_t packet_format::imu_gyro_ts(const uint8_t* imu_buf) const { - uint64_t res = 0; - std::memcpy(&res, imu_buf + 16, sizeof(uint64_t)); - return res; -} - -float packet_format::imu_la_x(const uint8_t* imu_buf) const { - float res = 0; - std::memcpy(&res, imu_buf + 24, sizeof(float)); - return res; -} - -float packet_format::imu_la_y(const uint8_t* imu_buf) const { - float res = 0; - std::memcpy(&res, imu_buf + 28, sizeof(float)); - return res; -} - -float packet_format::imu_la_z(const uint8_t* imu_buf) const { - float res = 0; - std::memcpy(&res, imu_buf + 32, sizeof(float)); - return res; -} - -float packet_format::imu_av_x(const uint8_t* imu_buf) const { - float res = 0; - std::memcpy(&res, imu_buf + 36, sizeof(float)); - return res; -} - -float packet_format::imu_av_y(const uint8_t* imu_buf) const { - float res = 0; - std::memcpy(&res, imu_buf + 40, sizeof(float)); - return res; -} - -float packet_format::imu_av_z(const uint8_t* imu_buf) const { - float res = 0; - std::memcpy(&res, imu_buf + 44, sizeof(float)); - return res; -} - -int packet_format::block_parsable() const { - std::array dims = {16, 8, 4}; - for (const auto& d : dims) { - if ((pixels_per_column % d == 0) && (columns_per_packet % d == 0)) - return d; - } - return 0; -} - -const packet_format& get_format(const sensor_info& info) { - return get_format(info.format.udp_profile_lidar, - info.format.pixels_per_column, - info.format.columns_per_packet); -} - -const packet_format& get_format(UDPProfileLidar udp_profile_lidar, - size_t pixels_per_column, - size_t columns_per_packet) { - using key = std::tuple; - static std::map> cache{}; - static std::mutex cache_mx{}; - - key k{pixels_per_column, columns_per_packet, udp_profile_lidar}; - - std::lock_guard lk{cache_mx}; - if (!cache.count(k)) { - cache[k] = std::make_unique( - udp_profile_lidar, pixels_per_column, columns_per_packet); - } - - return *cache.at(k); -} - -uint64_t packet_format::field_value_mask(ChanField i) const { - const auto& f = impl_->fields.at(i); - return get_value_mask(f); -} - -int packet_format::field_bitness(ChanField i) const { - const auto& f = impl_->fields.at(i); - return get_bitness(f); -} - -/* packet_writer implementation */ -namespace impl { - -uint8_t* packet_writer::nth_col(int n, uint8_t* lidar_buf) const { - return const_cast(packet_format::nth_col(n, lidar_buf)); -} - -uint8_t* packet_writer::nth_px(int n, uint8_t* col_buf) const { - return const_cast(packet_format::nth_px(n, col_buf)); -} - -uint8_t* packet_writer::footer(uint8_t* lidar_buf) const { - return const_cast(packet_format::footer(lidar_buf)); -} - -void packet_writer::set_col_status(uint8_t* col_buf, uint32_t status) const { - if (udp_profile_lidar == UDPProfileLidar::PROFILE_LIDAR_LEGACY) { - std::memcpy(col_buf + impl_->status_offset, &status, sizeof(uint32_t)); - } else { - uint16_t s = status & 0xffff; - std::memcpy(col_buf + impl_->status_offset, &s, sizeof(uint16_t)); - } -} - -void packet_writer::set_col_timestamp(uint8_t* col_buf, uint64_t ts) const { - std::memcpy(col_buf + impl_->timestamp_offset, &ts, sizeof(ts)); -} - -void packet_writer::set_col_measurement_id(uint8_t* col_buf, - uint16_t m_id) const { - std::memcpy(col_buf + impl_->measurement_id_offset, &m_id, sizeof(m_id)); -} - -void packet_writer::set_frame_id(uint8_t* lidar_buf, uint32_t frame_id) const { - // eUDP frame id is 16 bits, but FUSA frame id is 32 bits - if (udp_profile_lidar == - UDPProfileLidar::PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL) { - std::memcpy(lidar_buf + 4, &frame_id, sizeof(frame_id)); - return; - } - - uint16_t f_id = static_cast(frame_id); - if (udp_profile_lidar == UDPProfileLidar::PROFILE_LIDAR_LEGACY) { - std::memcpy(nth_col(0, lidar_buf) + 10, &f_id, sizeof(f_id)); - return; - } - - std::memcpy(lidar_buf + 2, &f_id, sizeof(f_id)); -} - -// Helpers for weird sized ints -// TODO: generalise when/if we need other uintXX_t fractionals -class uint24_t { - protected: - uint8_t _internal[3]; - - public: - uint24_t() {} - - uint24_t(const uint32_t val) { *this = val; } - - uint24_t(const uint24_t& val) { *this = val; } - - operator uint32_t() const { - return (_internal[2] << 16) | (_internal[1] << 8) | (_internal[0] << 0); - } - - uint24_t& operator=(const uint24_t& input) { - _internal[0] = input._internal[0]; - _internal[1] = input._internal[1]; - _internal[2] = input._internal[2]; - - return *this; - } - - uint24_t& operator=(const uint32_t input) { - _internal[0] = ((unsigned char*)&input)[0]; - _internal[1] = ((unsigned char*)&input)[1]; - _internal[2] = ((unsigned char*)&input)[2]; - - return *this; - } -}; - -class uint40_t { - protected: - uint8_t _internal[5]; - - public: - uint40_t() {} - - uint40_t(const uint64_t val) { *this = val; } - - uint40_t(const uint40_t& val) { *this = val; } - - operator uint64_t() const { - return (((uint64_t)_internal[4]) << 32) | - (((uint64_t)_internal[3]) << 24) | - (((uint64_t)_internal[2]) << 16) | - (((uint64_t)_internal[1]) << 8) | - (((uint64_t)_internal[0]) << 0); - } - - uint40_t& operator=(const uint40_t& input) { - _internal[0] = input._internal[0]; - _internal[1] = input._internal[1]; - _internal[2] = input._internal[2]; - _internal[3] = input._internal[3]; - _internal[4] = input._internal[4]; - - return *this; - } - - uint40_t& operator=(const uint64_t input) { - _internal[0] = ((unsigned char*)&input)[0]; - _internal[1] = ((unsigned char*)&input)[1]; - _internal[2] = ((unsigned char*)&input)[2]; - _internal[3] = ((unsigned char*)&input)[3]; - _internal[4] = ((unsigned char*)&input)[4]; - - return *this; - } -}; - -#pragma pack(push, 1) -// Relevant parts of packet headers as described/named in sensor documentation -struct FUSAHeader { - uint8_t packet_type; - uint24_t init_id; - uint32_t frame_id; - uint24_t padding; - uint40_t serial_no; -}; - -struct ConfigurableHeader { - uint16_t packet_type; - uint16_t frame_id; - uint24_t init_id; - uint40_t serial_no; -}; -#pragma pack(pop) - -void packet_writer::set_init_id(uint8_t* lidar_buf, uint32_t init_id) const { - if (udp_profile_lidar == UDPProfileLidar::PROFILE_LIDAR_LEGACY) { - // LEGACY profile has no init_id - return; - } - if (udp_profile_lidar == - UDPProfileLidar::PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL) { - auto hdr = (FUSAHeader*)lidar_buf; - hdr->init_id = init_id; - } else { - auto hdr = (ConfigurableHeader*)lidar_buf; - hdr->init_id = init_id; - } -} - -void packet_writer::set_prod_sn(uint8_t* lidar_buf, uint64_t sn) const { - if (udp_profile_lidar == UDPProfileLidar::PROFILE_LIDAR_LEGACY) { - // LEGACY profile has no prod_sn - return; - } - if (udp_profile_lidar == - UDPProfileLidar::PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL) { - auto hdr = (FUSAHeader*)lidar_buf; - hdr->serial_no = sn; - } else { - auto hdr = (ConfigurableHeader*)lidar_buf; - hdr->serial_no = sn; - } -} - -template -void packet_writer::set_px(uint8_t* px_buf, ChanField i, T value) const { - const auto& f = impl_->fields.at(i); - - if (f.shift > 0) value <<= f.shift; - if (f.shift < 0) value >>= std::abs(f.shift); - if (f.mask) value &= f.mask; - T* ptr = reinterpret_cast(px_buf + f.offset); - *ptr &= ~f.mask; - *ptr |= value; -} - -template void packet_writer::set_px(uint8_t*, ChanField, uint8_t) const; -template void packet_writer::set_px(uint8_t*, ChanField, uint16_t) const; -template void packet_writer::set_px(uint8_t*, ChanField, uint32_t) const; -template void packet_writer::set_px(uint8_t*, ChanField, uint64_t) const; - -template -void packet_writer::set_block_impl(Eigen::Ref> field, - ChanField chan, uint8_t* lidar_buf) const { - constexpr int N = 32; - if (columns_per_packet > N) - throw std::runtime_error("Recompile set_block_impl with larger N"); - - const auto& f = impl_->fields.at(chan); - - size_t offset = f.offset; - uint64_t mask = f.mask; - int shift = f.shift; - size_t channel_data_size = impl_->channel_data_size; - - int cols = field.cols(); - const T* data = field.data(); - std::array col_buf; - std::array valid; - for (int i = 0; i < columns_per_packet; ++i) { - col_buf[i] = nth_col(i, lidar_buf); - valid[i] = col_status(col_buf[i]) & 0x01; - } - uint16_t m_id = col_measurement_id(col_buf[0]); - - for (int px = 0; px < pixels_per_column; ++px) { - std::ptrdiff_t f_offset = cols * px + m_id; - for (int x = 0; x < columns_per_packet; ++x) { - if (!valid[x]) continue; - - auto px_dst = - col_buf[x] + col_header_size + (px * channel_data_size); - - uint64_t value = *(data + f_offset + x); - if (shift > 0) value <<= shift; - if (shift < 0) value >>= std::abs(shift); - if (mask) value &= mask; - DST* ptr = reinterpret_cast(px_dst + offset); - *ptr &= ~mask; - *ptr |= value; - } - } -} - -template -void packet_writer::set_block(Eigen::Ref> field, ChanField i, - uint8_t* lidar_buf) const { - const auto& f = impl_->fields.at(i); - - switch (f.ty_tag) { - case UINT8: - set_block_impl(field, i, lidar_buf); - break; - case UINT16: - set_block_impl(field, i, lidar_buf); - break; - case UINT32: - set_block_impl(field, i, lidar_buf); - break; - case UINT64: - set_block_impl(field, i, lidar_buf); - break; - default: - throw std::invalid_argument("Invalid field for packet format"); - } -} - -template void packet_writer::set_block(Eigen::Ref> field, - ChanField i, uint8_t* lidar_buf) const; -template void packet_writer::set_block(Eigen::Ref> field, - ChanField i, uint8_t* lidar_buf) const; -template void packet_writer::set_block(Eigen::Ref> field, - ChanField i, uint8_t* lidar_buf) const; -template void packet_writer::set_block(Eigen::Ref> field, - ChanField i, uint8_t* lidar_buf) const; - -template -void packet_writer::unpack_raw_headers(Eigen::Ref> field, - uint8_t* lidar_buf) const { - using ColMajorView = Eigen::Map>; - - if (sizeof(T) > 4) - throw std::invalid_argument( - "RAW_HEADERS field should be of type" - "uint32_t or smaller to work correctly"); - - uint8_t* col_zero = nth_col(0, lidar_buf); - uint16_t m_id = col_measurement_id(col_zero); - - size_t ch_size = col_header_size / sizeof(T); - size_t cf_size = col_footer_size / sizeof(T); - size_t ph_size = packet_header_size / sizeof(T); - size_t pf_size = packet_footer_size / sizeof(T); - - size_t ch_offset = 0; - size_t cf_offset = ch_offset + ch_size; - size_t ph_offset = cf_offset + cf_size; - size_t pf_offset = ph_offset + ph_size; - - // fill in header and footer, col0 is sufficient for that - ColMajorView ph_view(reinterpret_cast(lidar_buf), ph_size); - ColMajorView pf_view(reinterpret_cast(footer(lidar_buf)), pf_size); - ph_view = field.block(ph_offset, m_id, ph_size, 1); - pf_view = field.block(pf_offset, m_id, pf_size, 1); - - for (int icol = 0; icol < columns_per_packet; ++icol) { - uint8_t* col_buf = nth_col(icol, lidar_buf); - uint8_t* colf_ptr = col_buf + col_size - col_footer_size; - - ColMajorView colh_view(reinterpret_cast(col_buf), ch_size); - ColMajorView colf_view(reinterpret_cast(colf_ptr), cf_size); - - m_id = col_measurement_id(col_buf); - - colh_view = field.block(ch_offset, m_id, ch_size, 1); - colf_view = field.block(cf_offset, m_id, cf_size, 1); - } -} - -template void packet_writer::unpack_raw_headers( - Eigen::Ref> field, uint8_t* lidar_buf) const; -template void packet_writer::unpack_raw_headers( - Eigen::Ref> field, uint8_t* lidar_buf) const; -template void packet_writer::unpack_raw_headers( - Eigen::Ref> field, uint8_t* lidar_buf) const; -template void packet_writer::unpack_raw_headers( - Eigen::Ref> field, uint8_t* lidar_buf) const; - -} // namespace impl -} // namespace sensor -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/profile_extension.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/profile_extension.cpp deleted file mode 100644 index 86199ca3..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/profile_extension.cpp +++ /dev/null @@ -1,161 +0,0 @@ -/** - * Copyright (c) 2023, Ouster, Inc. - * All rights reserved. - */ - -#include "ouster/impl/profile_extension.h" - -#include -#include -#include -#include - -#include "ouster/types.h" - -template -using Table = std::array, N>; -using ouster::sensor::ChanField; -using ouster::sensor::ChanFieldType; -using ouster::sensor::UDPProfileLidar; -using ouster::sensor::impl::MAX_NUM_PROFILES; - -namespace ouster { - -namespace impl { - -// definition copied from lidar_scan.cpp -struct DefaultFieldsEntry { - const std::pair* fields; - size_t n_fields; -}; - -// lidar_scan.cpp -extern Table - default_scan_fields; - -static void extend_default_scan_fields( - UDPProfileLidar profile, - const std::vector>& scan_fields) { - auto end = impl::default_scan_fields.end(); - auto it = std::find_if(impl::default_scan_fields.begin(), end, - [](const auto& kv) { return kv.first == 0; }); - - if (it == end) - throw std::runtime_error("Limit of scan fields has been reached"); - - *it = {profile, {scan_fields.data(), scan_fields.size()}}; -} - -} // namespace impl - -namespace sensor { -namespace impl { - -struct ExtendedProfile { - UDPProfileLidar profile; - std::string name; - std::vector> slots; - std::vector> fields; - size_t chan_data_size; -}; - -/** - * Storage container for dynamically added profiles - * - * used with std::list because we need pointers to elements inside to stay valid - * when new elements are added - */ -std::list extended_profiles_data{}; - -// definition copied from parsing.cpp -struct ProfileEntry { - const std::pair* fields; - size_t n_fields; - size_t chan_data_size; -}; - -// types.cpp -extern Table - udp_profile_lidar_strings; -// parsing.cpp -extern Table profiles; - -static void extend_profile_entries( - UDPProfileLidar profile, - const std::vector>& fields, - size_t chan_data_size) { - auto end = impl::profiles.end(); - auto it = std::find_if(impl::profiles.begin(), end, - [](const auto& kv) { return kv.first == 0; }); - - if (it == end) - throw std::runtime_error("Limit of parsing profiles has been reached"); - - *it = {profile, {fields.data(), fields.size(), chan_data_size}}; -} - -} // namespace impl - -void extend_udp_profile_lidar_strings(UDPProfileLidar profile, - const char* name) { - auto begin = impl::udp_profile_lidar_strings.begin(); - auto end = impl::udp_profile_lidar_strings.end(); - - if (end != std::find_if(begin, end, [profile](const auto& p) { - return p.first == profile; - })) - throw std::invalid_argument( - "Lidar profile of given number already exists"); - if (end != std::find_if(begin, end, [name](const auto& p) { - return p.second && std::strcmp(p.second, name) == 0; - })) - throw std::invalid_argument( - "Lidar profile of given name already exists"); - - // Make sure to only check the string for zero intialization. - // The profile enum can be 0 normally (and is for UNKNOWN). - auto it = - std::find_if(begin, end, [](const auto& kv) { return kv.second == 0; }); - - if (it == end) - throw std::runtime_error("Limit of lidar profiles has been reached"); - - *it = std::make_pair(profile, name); -} - -void add_custom_profile(int profile_nr, // int as UDPProfileLidar - const std::string& name, - const std::vector>& - fields, // int as ChanField - size_t chan_data_size) { - if (profile_nr == 0) - throw std::invalid_argument("profile_nr of 0 are not allowed"); - - auto udp_profile = static_cast(profile_nr); - - { - // fill in profile - impl::ExtendedProfile profile{ - udp_profile, name, {}, {}, chan_data_size}; - for (auto&& pair : fields) { - ChanField chan = static_cast(pair.first); - - profile.slots.emplace_back(chan, pair.second.ty_tag); - profile.fields.emplace_back(chan, pair.second); - } - - impl::extended_profiles_data.push_back(std::move(profile)); - } - - // retake reference to stored data, profile.name.c_str() can get invalidated - // after move - auto&& profile = impl::extended_profiles_data.back(); - - extend_udp_profile_lidar_strings(profile.profile, profile.name.c_str()); - impl::extend_profile_entries(profile.profile, profile.fields, - profile.chan_data_size); - ouster::impl::extend_default_scan_fields(profile.profile, profile.slots); -} - -} // namespace sensor -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/sensor_http.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/sensor_http.cpp deleted file mode 100644 index 9691e798..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/sensor_http.cpp +++ /dev/null @@ -1,52 +0,0 @@ -#include "ouster/sensor_http.h" - -#include "curl_client.h" -#include "sensor_http_imp.h" -#include "sensor_tcp_imp.h" - -using std::string; - -using namespace ouster::util; -using namespace ouster::sensor; -using namespace ouster::sensor::util; -using namespace ouster::sensor::impl; - -string SensorHttp::firmware_version_string(const string& hostname, - int timeout_sec) { - auto http_client = - std::make_unique("http://" + hostname, timeout_sec); - return http_client->get("api/v1/system/firmware"); -} - -version SensorHttp::firmware_version(const string& hostname, int timeout_sec) { - auto result = firmware_version_string(hostname, timeout_sec); - return ouster::util::version_from_string(result); -} - -std::unique_ptr SensorHttp::create(const string& hostname, - int timeout_sec) { - auto fw = firmware_version(hostname, timeout_sec); - - if (fw == 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, - timeout_sec); - case 2: - return std::make_unique(hostname, - timeout_sec); - } - } - - return std::make_unique(hostname, timeout_sec); -} diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/sensor_http_imp.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/sensor_http_imp.cpp deleted file mode 100644 index 46ee7a67..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/sensor_http_imp.cpp +++ /dev/null @@ -1,152 +0,0 @@ -#include "sensor_http_imp.h" - -#include "curl_client.h" - -using std::string; -using namespace ouster::sensor::impl; - -SensorHttpImp::SensorHttpImp(const string& hostname, int timeout_sec) - : http_client( - std::make_unique("http://" + hostname, timeout_sec)) {} - -SensorHttpImp::~SensorHttpImp() = default; - -Json::Value SensorHttpImp::metadata() const { - return get_json("api/v1/sensor/metadata"); -} - -Json::Value SensorHttpImp::sensor_info() const { - return get_json("api/v1/sensor/metadata/sensor_info"); -} - -string SensorHttpImp::get_config_params(bool active) const { - auto config_type = active ? "active" : "staged"; - return get(string("api/v1/sensor/cmd/get_config_param?args=") + - config_type); -} - -void SensorHttpImp::set_config_param(const string& key, - const string& value) const { - 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\""); -} - -Json::Value SensorHttpImp::active_config_params() const { - return get_json("api/v1/sensor/cmd/get_config_param?args=active"); -} - -Json::Value SensorHttpImp::staged_config_params() const { - return get_json("api/v1/sensor/cmd/get_config_param?args=staged"); -} - -void SensorHttpImp::set_udp_dest_auto() const { - execute("api/v1/sensor/cmd/set_udp_dest_auto", "{}"); -} - -Json::Value SensorHttpImp::beam_intrinsics() const { - return get_json("api/v1/sensor/metadata/beam_intrinsics"); -} - -Json::Value SensorHttpImp::imu_intrinsics() const { - return get_json("api/v1/sensor/metadata/imu_intrinsics"); -} - -Json::Value SensorHttpImp::lidar_intrinsics() const { - return get_json("api/v1/sensor/metadata/lidar_intrinsics"); -} - -Json::Value SensorHttpImp::lidar_data_format() const { - return get_json("api/v1/sensor/metadata/lidar_data_format"); -} - -Json::Value SensorHttpImp::calibration_status() const { - return get_json("api/v1/sensor/metadata/calibration_status"); -} - -// reinitialize to activate new settings -void SensorHttpImp::reinitialize() const { - execute("api/v1/sensor/cmd/reinitialize", "{}"); -} - -void SensorHttpImp::save_config_params() const { - execute("api/v1/sensor/cmd/save_config_params", "{}"); -} - -string SensorHttpImp::get(const string& url) const { - return http_client->get(url); -} - -Json::Value SensorHttpImp::get_json(const 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 SensorHttpImp::execute(const string& url, const string& validation) const { - auto result = get(url); - if (result != validation) - throw std::runtime_error("SensorHttpImp::execute failed! url: " + url + - " returned [" + result + "], expected [" + - validation + "]"); -} - -SensorHttpImp_2_2::SensorHttpImp_2_2(const string& hostname, int timeout_sec) - : SensorHttpImp(hostname, timeout_sec) {} - -void SensorHttpImp_2_2::set_udp_dest_auto() const { - return execute("api/v1/sensor/cmd/set_udp_dest_auto", - "\"set_config_param\""); -} - -SensorHttpImp_2_1::SensorHttpImp_2_1(const string& hostname, int timeout_sec) - : SensorHttpImp_2_2(hostname, timeout_sec) {} - -Json::Value SensorHttpImp_2_1::metadata() const { - 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()}; - Json::Value node; - auto res = get_config_params(true); - auto parse_success = - reader->parse(res.c_str(), res.c_str() + res.size(), &node, nullptr); - root["config_params"] = parse_success ? node : res; - return root; -} - -Json::Value SensorHttpImp_2_1::sensor_info() const { - return get_json("api/v1/sensor/cmd/get_sensor_info"); -} - -Json::Value SensorHttpImp_2_1::beam_intrinsics() const { - return get_json("api/v1/sensor/cmd/get_beam_intrinsics"); -} - -Json::Value SensorHttpImp_2_1::imu_intrinsics() const { - return get_json("api/v1/sensor/cmd/get_imu_intrinsics"); -} - -Json::Value SensorHttpImp_2_1::lidar_intrinsics() const { - return get_json("api/v1/sensor/cmd/get_lidar_intrinsics"); -} - -Json::Value SensorHttpImp_2_1::lidar_data_format() const { - return get_json("api/v1/sensor/cmd/get_lidar_data_format"); -} - -Json::Value SensorHttpImp_2_1::calibration_status() const { - return get_json("api/v1/sensor/cmd/get_calibration_status"); -} diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/sensor_http_imp.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/sensor_http_imp.h deleted file mode 100644 index 578ef70d..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/sensor_http_imp.h +++ /dev/null @@ -1,194 +0,0 @@ -/** - * Copyright (c) 2022, Ouster, Inc. - * All rights reserved. - * - * @file sensor_http_imp.h - * @brief An implementation of the HTTP interface for Ouster sensors. - * - */ - -#pragma once - -#include "http_client.h" -#include "ouster/sensor_http.h" - -namespace ouster { -namespace sensor { -namespace impl { - -/** - * An implementation of the sensor http interface - */ -class SensorHttpImp : public util::SensorHttp { - public: - /** - * Constructs an http interface to communicate with the sensor. - * - * @param[in] hostname Hostname of the sensor to communicate with. - * @param[in] timeout_sec The timeout to use in seconds. - */ - SensorHttpImp(const std::string& hostname, int timeout_sec); - - /** - * Deconstruct the sensor http interface. - */ - ~SensorHttpImp() override; - - /** - * Queries the sensor metadata. - * - * @return returns a Json object of the sensor metadata. - */ - Json::Value metadata() const override; - - /** - * Queries the sensor_info. - * - * @return returns a Json object representing the sensor_info. - */ - Json::Value sensor_info() const override; - - /** - * 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 - */ - std::string get_config_params(bool active) const override; - - /** - * 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. - */ - void set_config_param(const std::string& key, - const std::string& value) const override; - - /** - * Retrieves the active configuration on the sensor - */ - Json::Value active_config_params() const override; - - /** - * Retrieves the staged configuration on the sensor - */ - Json::Value staged_config_params() const override; - - /** - * Enables automatic assignment of udp destination ports. - */ - void set_udp_dest_auto() const override; - - /** - * Retrieves beam intrinsics of the sensor. - */ - Json::Value beam_intrinsics() const override; - - /** - * Retrieves imu intrinsics of the sensor. - */ - Json::Value imu_intrinsics() const override; - - /** - * Retrieves lidar intrinsics of the sensor. - */ - Json::Value lidar_intrinsics() const override; - - /** - * Retrieves lidar data format. - */ - Json::Value lidar_data_format() const override; - - /** - * Gets the calibaration status of the sensor. - */ - Json::Value calibration_status() const override; - - /** - * Restarts the sensor applying all staged configurations. - */ - void reinitialize() const override; - - /** - * Persist active configuration parameters to the sensor. - */ - void save_config_params() const override; - - protected: - std::string get(const std::string& url) const; - - Json::Value get_json(const std::string& url) const; - - void execute(const std::string& url, const std::string& validation) const; - - protected: - std::unique_ptr http_client; -}; - -// TODO: remove when firmware 2.2 has been fully phased out -class SensorHttpImp_2_2 : public SensorHttpImp { - public: - SensorHttpImp_2_2(const std::string& hostname, int timeout_sec); - - void set_udp_dest_auto() const override; -}; - -/** - * An implementation of the sensor http interface - */ -class SensorHttpImp_2_1 : public SensorHttpImp_2_2 { - public: - /** - * Constructs an http interface to communicate with the sensor. - * - * @param[in] hostname hostname of the sensor to communicate with. - * @param[in] timeout_sec The timeout to use in seconds. - */ - SensorHttpImp_2_1(const std::string& hostname, int timeout_sec); - - /** - * Queries the sensor metadata. - * - * @return returns a Json object of the sensor metadata. - */ - Json::Value metadata() const override; - - /** - * Queries the sensor_info. - * - * @return returns a Json object representing the sensor_info. - */ - Json::Value sensor_info() const override; - - /** - * Retrieves beam intrinsics of the sensor. - */ - Json::Value beam_intrinsics() const override; - - /** - * Retrieves imu intrinsics of the sensor. - */ - Json::Value imu_intrinsics() const override; - - /** - * Retrieves lidar intrinsics of the sensor. - */ - Json::Value lidar_intrinsics() const override; - - /** - * Retrieves lidar data format. - */ - Json::Value lidar_data_format() const override; - - /** - * Gets the calibaration status of the sensor. - */ - Json::Value calibration_status() const override; -}; - -} // namespace impl -} // namespace sensor -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/sensor_info.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/sensor_info.cpp deleted file mode 100644 index 8d3b8eff..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/sensor_info.cpp +++ /dev/null @@ -1,1041 +0,0 @@ -/** - * Copyright (c) 2023, Ouster, Inc. - * All rights reserved. - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "logging.h" -#include "ouster/impl/build.h" -#include "ouster/types.h" -#include "ouster/util.h" -#include "ouster/version.h" - -namespace ouster { - -using nonstd::make_optional; -using nonstd::nullopt; -using nonstd::optional; - -/* version field required by ouster studio */ -enum configuration_version { FW_2_0 = 3, FW_2_2 = 4 }; - -namespace sensor { - -extern ColumnWindow default_column_window(uint32_t columns_per_frame); -extern data_format default_data_format(lidar_mode mode); -extern double default_lidar_origin_to_beam_origin(std::string prod_line); -extern mat4d default_beam_to_lidar_transform(std::string prod_line); -extern calibration_status default_calibration_status(); -extern sensor_config parse_config(const Json::Value& root); -extern data_format parse_data_format(const Json::Value& root); -extern Json::Value cal_to_json(const calibration_status& cal); -extern Json::Value config_to_json(const sensor_config& config); - -/* Equality operators and functions */ - -bool operator==(const sensor_info& lhs, const sensor_info& rhs) { - return (lhs.has_fields_equal(rhs) && - lhs.original_string() == rhs.original_string()); -} - -bool operator!=(const sensor_info& lhs, const sensor_info& rhs) { - return !(lhs == rhs); -} - -bool sensor_info::has_fields_equal(const sensor_info& other) const { - return ( - this->name == other.name && this->sn == other.sn && - this->fw_rev == other.fw_rev && this->mode == other.mode && - this->prod_line == other.prod_line && this->format == other.format && - this->beam_azimuth_angles == other.beam_azimuth_angles && - this->beam_altitude_angles == other.beam_altitude_angles && - this->lidar_origin_to_beam_origin_mm == - other.lidar_origin_to_beam_origin_mm && - this->beam_to_lidar_transform == other.beam_to_lidar_transform && - this->imu_to_sensor_transform == other.imu_to_sensor_transform && - this->lidar_to_sensor_transform == other.lidar_to_sensor_transform && - this->extrinsic == other.extrinsic && this->init_id == other.init_id && - this->udp_port_lidar == other.udp_port_lidar && - this->udp_port_imu == other.udp_port_imu && - this->build_date == other.build_date && - this->image_rev == other.image_rev && this->prod_pn == other.prod_pn && - this->status == other.status && this->cal == other.cal && - this->config == other.config); -} - -/* Default values */ - -sensor_info default_sensor_info(lidar_mode mode) { - auto info = sensor_info(); - - info.name = "UNKNOWN"; - info.sn = "000000000000"; - info.fw_rev = "UNKNOWN"; - info.mode = mode; - info.prod_line = "OS-1-64"; - info.format = default_data_format(mode); - info.beam_azimuth_angles = gen1_azimuth_angles; - info.beam_altitude_angles = gen1_altitude_angles; - info.lidar_origin_to_beam_origin_mm = - default_lidar_origin_to_beam_origin(info.prod_line); - info.beam_to_lidar_transform = default_imu_to_sensor_transform; - info.imu_to_sensor_transform = - default_beam_to_lidar_transform(info.prod_line); - info.lidar_to_sensor_transform = default_lidar_to_sensor_transform; - info.extrinsic = mat4d::Identity(); - info.init_id = 0; - info.udp_port_lidar = 0; - info.udp_port_imu = 0; - info.build_date = ""; - info.image_rev = ""; - info.prod_pn = ""; - info.status = ""; - info.cal = default_calibration_status(); - info.config = sensor_config{}; - - return info; -} - -// clang-format off -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, -}; - -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, -}; -// clang-format on - -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(); - -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(); - -/* String conversion */ - -// 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 -// TODO parse metadata by FW version specified ? -// clang-format off -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} -}; - -// clang-format on - -static bool is_new_format(const std::string& metadata) { - Json::Value root{}; - Json::CharReaderBuilder builder{}; - std::string errors{}; - std::stringstream ss{metadata}; - - if (metadata.size()) { - 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(); -} - -void parse_legacy(sensor_info& info, const std::string& metadata, - bool skip_beam_validation, bool suppress_legacy_warnings) { - Json::Value root{}; - Json::CharReaderBuilder builder{}; - std::string errors{}; - std::stringstream ss{metadata}; - - if (metadata.size()) { - if (!Json::parseFromStream(builder, ss, &root, &errors)) - throw std::runtime_error{ - "Errors parsing metadata for parse_metadata: " + 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", - "config_params"}; - for (auto field : desired_legacy_metadata_fields) { - if (!root.isMember(field)) { - if (suppress_legacy_warnings && field != "config_params") { - logger().warn( - "No " + field + - " found in metadata. Will be left blank or filled in " - "with default legacy values"); - } - } - } - - // will be empty string if not present - info.name = root["hostname"].asString(); - - // if these are not present they are also empty strings - info.build_date = root["build_date"].asString(); - info.fw_rev = root["build_rev"].asString(); - info.image_rev = root["image_rev"].asString(); - info.prod_line = root["prod_line"].asString(); - info.prod_pn = root["prod_pn"].asString(); - info.sn = root["prod_sn"].asString(); - info.status = root["status"].asString(); - - // default to 0 if init_id key not present - info.init_id = root["initialization_id"].asInt(); - - // checked that lidar_mode is present already - never empty string - info.mode = lidar_mode_of_string(root["lidar_mode"].asString()); - - // "data_format" introduced in fw 2.0. Fall back to 1.13 - 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 { - logger().warn("No data_format found. Using default legacy data format"); - info.format = default_data_format(info.mode); - } - - // "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 { - 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"); - 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 "" - } - } - - // 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_altitude_angles"].size() != 0 && - root["beam_altitude_angles"].size() != info.format.pixels_per_column) - throw std::runtime_error{"Unexpected number of beam_altitude_angles"}; - - 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"}; - - 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++) { - const Json::Value::ArrayIndex ind = i * 4 + j; - info.imu_to_sensor_transform(i, j) = - root["imu_to_sensor_transform"][ind].asDouble(); - } - } - } else { - logger().warn( - "No valid imu_to_sensor_transform found. Using default for gen " - "1"); - 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++) { - const Json::Value::ArrayIndex ind = i * 4 + j; - info.lidar_to_sensor_transform(i, j) = - root["lidar_to_sensor_transform"][ind].asDouble(); - } - } - } else { - logger().warn( - "No valid lidar_to_sensor_transform found. Using default for " - "gen " - "1"); - info.lidar_to_sensor_transform = default_lidar_to_sensor_transform; - } - - auto zero_check = [](auto el, 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."}; - } - }; - - if (!skip_beam_validation) { - zero_check(info.beam_altitude_angles, "beam_altitude_angles"); - zero_check(info.beam_azimuth_angles, "beam_azimuth_angles"); - } else { - logger().warn("Skipping all 0 beam angle check"); - } - - info.extrinsic = mat4d::Identity(); - - // default to 0 if keys are not present - info.udp_port_lidar = root["udp_port_lidar"].asInt(); - info.udp_port_imu = root["udp_port_imu"].asInt(); -} - -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]; - } -} - -sensor_info::sensor_info() { - // TODO - understand why this seg faults in CI when uncommented - // logger().warn("Initializing sensor_info without original metadata - // string"); - original_metadata_string = ""; -} - -sensor_info::sensor_info(const std::string& metadata, - bool skip_beam_validation) { - original_metadata_string = metadata; - - Json::Value root{}; - Json::CharReaderBuilder builder{}; - std::string errors{}; - std::stringstream ss{metadata}; - - if (metadata.size()) { - if (!Json::parseFromStream(builder, ss, &root, &errors)) - throw std::runtime_error{"Errors parsing metadata string: " + - errors}; - } - - if (is_new_format(metadata)) { - logger().info("parsing non-legacy metadata format"); - parse_legacy(*this, convert_to_legacy(metadata), skip_beam_validation, - true); - // also parse the sensor_config - - // we are guaranteed calibration_status as a key exists so don't need to - // check again - if (root["calibration_status"].isObject()) { - if (root["calibration_status"]["reflectivity"]["valid"].isBool()) { - this->cal.reflectivity_status = - root["calibration_status"]["reflectivity"]["valid"] - .asBool(); - } else { - logger().warn( - "metadata field calibration_status.reflectivity.valid is " - "not Bool value, but: {}. Using False instead.", - root["calibration_status"]["reflectivity"]["valid"] - .asString()); - } - - if (this->cal.reflectivity_status) { - this->cal.reflectivity_timestamp = - root["calibration_status"]["reflectivity"]["timestamp"] - .asString(); - } - } - - this->config = parse_config(root["config_params"]); - - } else { - logger().info("parsing legacy metadata format"); - parse_legacy(*this, metadata, skip_beam_validation, false); - } -} - -std::string sensor_info::original_string() const { - return original_metadata_string; -} - -void mat4d_to_json(Json::Value& val, mat4d mat) { - for (size_t i = 0; i < 4; i++) { - for (size_t j = 0; j < 4; j++) { - val.append(mat(i, j)); - } - } -} - -/* DO NOT make public - internal logic use only - * Powers outputting a sensor_info to a flat JSON resembling legacy metadata - */ -Json::Value info_to_flat_json(const sensor_info& info) { - Json::Value result{}; - - result["hostname"] = info.name; - result["prod_sn"] = info.sn; - result["build_rev"] = info.fw_rev; - result["lidar_mode"] = to_string(info.mode); - result["prod_line"] = info.prod_line; - - // data_format - result["data_format"]["pixels_per_column"] = info.format.pixels_per_column; - result["data_format"]["columns_per_packet"] = - info.format.columns_per_packet; - result["data_format"]["columns_per_frame"] = info.format.columns_per_frame; - result["data_format"]["fps"] = info.format.fps; - result["data_format"]["column_window"].append( - info.format.column_window.first); - result["data_format"]["column_window"].append( - info.format.column_window.second); - result["data_format"]["udp_profile_lidar"] = - to_string(info.format.udp_profile_lidar); - result["data_format"]["udp_profile_imu"] = - to_string(info.format.udp_profile_imu); - for (auto i : info.format.pixel_shift_by_row) - result["data_format"]["pixel_shift_by_row"].append(i); - - result["lidar_origin_to_beam_origin_mm"] = - info.lidar_origin_to_beam_origin_mm; - - if (info.beam_azimuth_angles.size() == - info.format.pixels_per_column * info.format.columns_per_frame) { - // Don't output for DF for now - ; - } else { - for (auto i : info.beam_azimuth_angles) - result["beam_azimuth_angles"].append(i); - - for (auto i : info.beam_altitude_angles) - result["beam_altitude_angles"].append(i); - } - - mat4d_to_json(result["beam_to_lidar_transform"], - info.beam_to_lidar_transform); - mat4d_to_json(result["imu_to_sensor_transform"], - info.imu_to_sensor_transform); - mat4d_to_json(result["lidar_to_sensor_transform"], - info.lidar_to_sensor_transform); - mat4d_to_json(result["extrinsic"], info.extrinsic); - - result["initialization_id"] = info.init_id; - result["udp_port_lidar"] = info.udp_port_lidar; - result["udp_port_imu"] = info.udp_port_imu; - - result["build_date"] = info.build_date; - result["image_rev"] = info.image_rev; - result["prod_pn"] = info.prod_pn; - result["status"] = info.status; - - result["calibration_status"] = cal_to_json(info.cal); - result["config_params"] = config_to_json(info.config); - - return result; -} - -/* DO NOT make public - internal logic use only - * Powers outputting a sensor_info to a nested json resembling non-legacy - * metadata - */ -Json::Value info_to_nested_json(const sensor_info& info) { - Json::Value result{}; - - result["sensor_info"]["build_date"] = info.build_date; - result["sensor_info"]["build_rev"] = info.fw_rev; - result["sensor_info"]["image_rev"] = info.image_rev; - result["sensor_info"]["initialization_id"] = info.init_id; - result["sensor_info"]["prod_line"] = info.prod_line; - result["sensor_info"]["prod_pn"] = info.prod_pn; - result["sensor_info"]["prod_sn"] = info.sn; - result["sensor_info"]["status"] = info.status; - - // data_format - result["lidar_data_format"]["pixels_per_column"] = - info.format.pixels_per_column; - result["lidar_data_format"]["columns_per_packet"] = - info.format.columns_per_packet; - result["lidar_data_format"]["columns_per_frame"] = - info.format.columns_per_frame; - result["lidar_data_format"]["fps"] = info.format.fps; - result["lidar_data_format"]["column_window"].append( - info.format.column_window.first); - result["lidar_data_format"]["column_window"].append( - info.format.column_window.second); - result["lidar_data_format"]["udp_profile_lidar"] = - to_string(info.format.udp_profile_lidar); - result["lidar_data_format"]["udp_profile_imu"] = - to_string(info.format.udp_profile_imu); - - for (auto i : info.format.pixel_shift_by_row) - result["lidar_data_format"]["pixel_shift_by_row"].append(i); - - // beam intrinsics - // - mat4d_to_json(result["beam_intrinsics"]["beam_to_lidar_transform"], - info.beam_to_lidar_transform); - result["beam_intrinsics"]["lidar_origin_to_beam_origin_mm"] = - info.lidar_origin_to_beam_origin_mm; - - if (info.beam_azimuth_angles.size() == info.format.pixels_per_column) { - // OS sensor path - for (auto angle : info.beam_azimuth_angles) - result["beam_intrinsics"]["beam_azimuth_angles"].append(angle); - for (auto angle : info.beam_altitude_angles) - result["beam_intrinsics"]["beam_altitude_angles"].append(angle); - } else { - ; - - // TODO: debug DF beam_altitude/beam_azimuthoutpu - /* - int j = 0; - for (size_t i=0; iformat == orig_info.format) - root_new.removeMember("lidar_data_format"); - } else { - // format was not auto-populated so now check fps, - // pixel_shift_by_row, column_window, udp_profile_lidar, - // udp_profile_imu - if (!root_orig["lidar_data_format"].isMember("fps") && - this->format.fps == orig_info.format.fps) - root_new["lidar_data_format"].removeMember("fps"); - if (!root_orig["lidar_data_format"].isMember("column_window") && - this->format.column_window == - orig_info.format.column_window) - root_new["lidar_data_format"].removeMember("column_window"); - if (!root_orig["lidar_data_format"].isMember( - "pixel_shift_by_row") && - this->format.pixel_shift_by_row == - orig_info.format.pixel_shift_by_row) - root_new["lidar_data_format"].removeMember( - "pixel_shift_by_row"); - if (!root_orig["lidar_data_format"].isMember( - "udp_profile_lidar") && - this->format.udp_profile_lidar == - orig_info.format.udp_profile_lidar) - root_new["lidar_data_format"].removeMember( - "udp_profile_lidar"); - if (!root_orig["lidar_data_format"].isMember( - "udp_profile_imu") && - this->format.udp_profile_imu == - orig_info.format.udp_profile_imu) - root_new["lidar_data_format"].removeMember( - "udp_profile_imu"); - } - - // check beam_intrinsics.imu_to_sensor_transform - - // NO NEED - FW 1.12 already had this - skip check - // check lidar_intrinsics.lidar_to_sensor_transform - - // NO NEED - FW 1.12 already had this - skip check - - // check lidar_origin_to_beam_origin_mm - if (!root_orig["beam_intrinsics"].isMember( - "lidar_origin_to_beam_origin_mm") && - this->lidar_origin_to_beam_origin_mm == - orig_info.lidar_origin_to_beam_origin_mm) - root_new["beam_intrinsics"].removeMember( - "lidar_origin_to_beam_origin_mm"); - - // check beam_intinrics.beam_to_lidar_transform - if (!root_orig["beam_intrinsics"].isMember( - "beam_to_lidar_transform") && - this->beam_to_lidar_transform == - orig_info.beam_to_lidar_transform) - root_new["beam_intrinsics"].removeMember( - "beam_to_lidar_transform"); - - if (!root_orig["sensor_info"].isMember("initialization_id") && - this->init_id == orig_info.init_id) - root_new["sensor_info"].removeMember("initialization_id"); - - if (root_orig["calibration_status"] == - "error: Command not recognized." && - this->cal == orig_info.cal) { - root_new["calibration_status"] = - "error: Command not recognized."; - } - - if (root_orig["lidar_data_format"] == - "error: Command not recognized." && - this->format == orig_info.format) { - root_new["lidar_data_format"] = - "error: Command not recognized."; - } - - } else { - // have original metadata string that is legacy format - warn users - // what they will lose Users who initialize with legacy metadata but - // want to change these values ... - // ... should upgrade to non-legacy format - if (this->config != sensor_config() || - this->cal != calibration_status()) { - logger().warn( - "Your sensor_info has set sensor_config and/or " - "calibration_status " - "items despite starting with legacy metadata. These will " - "be " - "disregarded in your output (which will be of legacy " - "format."); - } - root_new = info_to_flat_json(*this); - - root_new.removeMember("config_params"); - root_new.removeMember("calibration_status"); - if (this->udp_port_imu == orig_info.udp_port_imu) { - root_new.removeMember("udp_port_imu"); - } - if (this->udp_port_lidar == orig_info.udp_port_lidar) { - root_new.removeMember("udp_port_lidar"); - } - if (this->extrinsic == orig_info.extrinsic) { - root_new.removeMember("extrinsic"); - } - - // check format - if (!root_orig["data_format"].isObject()) { - if (this->format == orig_info.format) - root_new.removeMember("data_format"); - } else { - // format was not auto-populated so now check fps, - // pixel_shift_by_row, column_window, udp_profile_lidar, - // udp_profile_imu - if (!root_orig["data_format"].isMember("fps") && - this->format.fps == orig_info.format.fps) - root_new["data_format"].removeMember("fps"); - if (!root_orig["data_format"].isMember("pixel_shift_by_row") && - this->format.pixel_shift_by_row == - orig_info.format.pixel_shift_by_row) - root_new["data_format"].removeMember("pixel_shift_by_row"); - if (!root_orig["data_format"].isMember("column_window") && - this->format.column_window == - orig_info.format.column_window) - root_new["data_format"].removeMember("column_window"); - if (!root_orig["data_format"].isMember("udp_profile_lidar") && - this->format.udp_profile_lidar == - orig_info.format.udp_profile_lidar) - root_new["data_format"].removeMember("udp_profile_lidar"); - if (!root_orig["data_format"].isMember("udp_profile_imu") && - this->format.udp_profile_imu == - orig_info.format.udp_profile_imu) - root_new["data_format"].removeMember("udp_profile_imu"); - } - // check imu_to_sensor_transform - if (!root_orig.isMember("imu_to_sensor_transform") && - this->imu_to_sensor_transform == - orig_info.imu_to_sensor_transform) - root_new.removeMember("imu_to_sensor_transform"); - - // check lidar_to_sensor_transform - if (!root_orig.isMember("lidar_to_sensor_transform") && - this->lidar_to_sensor_transform == - orig_info.lidar_to_sensor_transform) - root_new.removeMember("lidar_to_sensor_transform"); - // check lidar_origin_to_beam_origin_mm - if (!root_orig.isMember("lidar_origin_to_beam_origin_mm") && - this->lidar_origin_to_beam_origin_mm == - orig_info.lidar_origin_to_beam_origin_mm) - root_new.removeMember("lidar_origin_to_beam_origin_mm"); - // check beam_to_lidar_transform - if (!root_orig.isMember("beam_to_lidar_transform") && - this->beam_to_lidar_transform == - orig_info.beam_to_lidar_transform) - root_new.removeMember("beam_to_lidar_transform"); - } - } - - std::vector changed; - result = ouster::combined(root_orig, root_new, changed); - - // Relevant for both non-legacy and legacy - result["ouster-sdk"]["output_source"] = "updated_metadata_string"; - result["ouster-sdk"]["client_version"] = client_version(); - for (auto& changed_str : changed) - result["ouster-sdk"]["changed_fields"].append(changed_str); - - Json::StreamWriterBuilder write_builder; - write_builder["enableYAMLCompatibility"] = true; - write_builder["precision"] = 6; - write_builder["indentation"] = " "; - return Json::writeString(write_builder, result); -} - -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.size()) { - 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"]; - - if (root.isMember("ouster-sdk")) result["ouster-sdk"] = root["ouster-sdk"]; - - // TODO eventually remove - // NOTE: DO NOT REMOVE until mid 2024 - // json-calibration-version powers any legacy conversion being done for - // users still on Kitware Ouster Studio probably best to announce removal - // "breakage" by Beginning 2024 - result["json_calibration_version"] = FW_2_2; - - result["hostname"] = root["hostname"].asString(); - - 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 metadata_from_json(const std::string& json_file, - bool skip_beam_validation) { - std::stringstream buf{}; - std::ifstream ifs{}; - ifs.open(json_file); - buf << ifs.rdbuf(); - ifs.close(); - - if (!ifs) { - std::stringstream ss; - ss << "Failed to read metadata file: " << json_file; - throw std::runtime_error{ss.str()}; - } - - return parse_metadata(buf.str(), skip_beam_validation); -} - -// TODO - fix up according to debug output desires -std::string to_string(const sensor_info& info) { - logger().warn( - "Calling debug to_string on sensor_info. Does NOT produce valid " - "metadata.json"); - Json::StreamWriterBuilder builder; - builder["enableYAMLCompatibility"] = true; - builder["precision"] = 6; - builder["indentation"] = " "; - - auto root = info_to_flat_json(info); - root["ouster-sdk"]["output_source"] = "DEBUG:to_string"; - - return Json::writeString(builder, root); -} - -sensor_info parse_metadata(const std::string& metadata, - bool skip_beam_validation) { - return sensor_info(metadata, skip_beam_validation); -} - -// TODO: do we need to expose this method? -std::string get_firmware_version(const Json::Value& metadata_root) { - auto fw_ver = std::string{}; - if (metadata_root["sensor_info"].isObject()) { - if (metadata_root["sensor_info"].isMember("semver")) { - // This is only true for 3.2 and later - fw_ver = metadata_root["sensor_info"]["semver"].asString(); - } else if (metadata_root["sensor_info"].isMember("build_rev")) { - // fall back to build_rev - fw_ver = metadata_root["sensor_info"]["build_rev"].asString(); - } - } - return fw_ver; -} - -ouster::util::version firmware_version_from_metadata( - const std::string& metadata) { - if (metadata.empty()) { - throw std::invalid_argument( - "firmware_version_from_metadata metadata empty!"); - } - - Json::Value root{}; - Json::CharReaderBuilder builder{}; - std::string errors{}; - std::stringstream ss{metadata}; - - if (!Json::parseFromStream(builder, ss, &root, &errors)) - throw std::runtime_error{ - "Errors parsing metadata for parse_metadata: " + errors}; - - auto fw_ver = get_firmware_version(root); - if (fw_ver.empty()) { - throw std::runtime_error( - "firmware_version_from_metadata failed to deduce version info from " - "metadata!"); - } - - return ouster::util::version_from_string(fw_ver); -} - -} // namespace sensor -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/sensor_tcp_imp.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/sensor_tcp_imp.cpp deleted file mode 100644 index a2f95d96..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/sensor_tcp_imp.cpp +++ /dev/null @@ -1,215 +0,0 @@ -/** - * Copyright (c) 2022, Ouster, Inc. - * All rights reserved. - */ - -#include "sensor_tcp_imp.h" - -#include -#include -#include - -#include "logging.h" - -using std::string; -using namespace ouster::sensor::impl; - -SensorTcpImp::SensorTcpImp(const string& hostname) - : socket_handle(cfg_socket(hostname.c_str())), - read_buf(std::unique_ptr{new char[MAX_RESULT_LENGTH + 1]}) {} - -SensorTcpImp::~SensorTcpImp() { socket_close(socket_handle); } - -Json::Value SensorTcpImp::metadata() const { - 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; -} - -Json::Value SensorTcpImp::sensor_info() const { - return tcp_cmd_json({"get_sensor_info"}); -} - -string SensorTcpImp::get_config_params(bool active) const { - auto config_type = active ? "active" : "staged"; - return tcp_cmd({"get_config_param", config_type}); -} - -namespace { -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())); -} -} // namespace - -void SensorTcpImp::set_config_param(const string& key, - const string& value) const { - tcp_cmd_with_validation({"set_config_param", key, rtrim(value)}, - "set_config_param"); -} - -Json::Value SensorTcpImp::active_config_params() const { - return tcp_cmd_json({"get_config_param", "active"}); -} - -Json::Value SensorTcpImp::staged_config_params() const { - return tcp_cmd_json({"get_config_param", "staged"}); -} - -void SensorTcpImp::set_udp_dest_auto() const { - tcp_cmd_with_validation({"set_udp_dest_auto"}, "set_udp_dest_auto"); -} - -Json::Value SensorTcpImp::beam_intrinsics() const { - return tcp_cmd_json({"get_beam_intrinsics"}); -} - -Json::Value SensorTcpImp::imu_intrinsics() const { - return tcp_cmd_json({"get_imu_intrinsics"}); -} - -Json::Value SensorTcpImp::lidar_intrinsics() const { - return tcp_cmd_json({"get_lidar_intrinsics"}); -} - -Json::Value SensorTcpImp::lidar_data_format() const { - return tcp_cmd_json({"get_lidar_data_format"}, false); -} - -Json::Value SensorTcpImp::calibration_status() const { - return tcp_cmd_json({"get_calibration_status"}, false); -} - -void SensorTcpImp::reinitialize() const { - // reinitialize to make all staged parameters effective - tcp_cmd_with_validation({"reinitialize"}, "reinitialize"); -} - -void SensorTcpImp::save_config_params() const { - tcp_cmd_with_validation({"write_config_txt"}, "write_config_txt"); -} - -SOCKET SensorTcpImp::cfg_socket(const char* addr) { - struct addrinfo hints, *info_start, *ai; - - std::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) { - logger().error("cfg getaddrinfo(): {}", gai_strerror(ret)); - return SOCKET_ERROR; - } - } - - if (info_start == nullptr) { - logger().error("cfg getaddrinfo(): empty result"); - return SOCKET_ERROR; - } - - SOCKET 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 (!socket_valid(sock_fd)) { - logger().error("cfg socket(): {}", socket_get_error()); - continue; - } - - if (connect(sock_fd, ai->ai_addr, (socklen_t)ai->ai_addrlen) < 0) { - socket_close(sock_fd); - continue; - } - - if (socket_set_rcvtimeout(sock_fd, RCVTIMEOUT_SEC)) { - logger().error("cfg set_rcvtimeout(): {}", socket_get_error()); - socket_close(sock_fd); - continue; - } - - break; - } - - freeaddrinfo(info_start); - if (ai == nullptr) { - return SOCKET_ERROR; - } - - return sock_fd; -} - -string SensorTcpImp::tcp_cmd(const std::vector& cmd_tokens) const { - std::stringstream ss; - for (const auto& token : cmd_tokens) ss << token << " "; - ss << "\n"; - string cmd = ss.str(); - - ssize_t len = send(socket_handle, cmd.c_str(), cmd.length(), 0); - if (len != (ssize_t)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(): " + socket_get_error()); - } - - 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; -} - -Json::Value SensorTcpImp::tcp_cmd_json(const std::vector& cmd_tokens, - bool exception_on_parse_errors) 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 ["); -} - -void SensorTcpImp::tcp_cmd_with_validation( - const std::vector& cmd_tokens, const 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 + "]"); - } -} diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/sensor_tcp_imp.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/sensor_tcp_imp.h deleted file mode 100644 index c70d375f..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/sensor_tcp_imp.h +++ /dev/null @@ -1,141 +0,0 @@ -/** - * Copyright (c) 2022, Ouster, Inc. - * All rights reserved. - * - * @file sensor_tcp_imp.h - * @brief A high level TCP interface for Ouster sensors. - * - */ - -#pragma once - -#include "ouster/impl/netcompat.h" -#include "ouster/sensor_http.h" - -namespace ouster { -namespace sensor { -namespace impl { - -/** - * A TCP implementation of the SensorHTTP interface - */ -class SensorTcpImp : public util::SensorHttp { - // timeout for reading from a TCP socket during config - const int RCVTIMEOUT_SEC = 10; - // maximum size to to handle during recv - const size_t MAX_RESULT_LENGTH = 16 * 1024; - - public: - /** - * Constructs an tcp interface to communicate with the sensor. - * - * @param[in] hostname hostname of the sensor to communicate with. - */ - SensorTcpImp(const std::string& hostname); - - /** - * Deconstruct the sensor tcp interface. - */ - ~SensorTcpImp() override; - /** - * Queries the sensor metadata. - * - * @return returns a Json object of the sensor metadata. - */ - Json::Value metadata() const override; - - /** - * Queries the sensor_info. - * - * @return returns a Json object representing the sensor_info. - */ - Json::Value sensor_info() const override; - - /** - * 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 - */ - std::string get_config_params(bool active) const override; - - /** - * 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. - */ - void set_config_param(const std::string& key, - const std::string& value) const override; - - /** - * Retrieves the active configuration on the sensor - */ - Json::Value active_config_params() const override; - - /** - * Retrieves the staged configuration on the sensor - */ - Json::Value staged_config_params() const override; - - /** - * Enables automatic assignment of udp destination ports. - */ - void set_udp_dest_auto() const override; - - /** - * Retrieves beam intrinsics of the sensor. - */ - Json::Value beam_intrinsics() const override; - - /** - * Retrieves imu intrinsics of the sensor. - */ - Json::Value imu_intrinsics() const override; - - /** - * Retrieves lidar intrinsics of the sensor. - */ - Json::Value lidar_intrinsics() const override; - - /** - * Retrieves lidar data format. - */ - Json::Value lidar_data_format() const override; - - /** - * Gets the calibaration status of the sensor. - */ - Json::Value calibration_status() const override; - - /** - * Restarts the sensor applying all staged configurations. - */ - void reinitialize() const override; - - /** - * Persist active configuration parameters to the sensor. - */ - void save_config_params() const override; - - private: - SOCKET cfg_socket(const char* addr); - - std::string tcp_cmd(const std::vector& cmd_tokens) const; - - void tcp_cmd_with_validation(const std::vector& cmd_tokens, - const std::string& validation) const; - - Json::Value tcp_cmd_json(const std::vector& cmd_tokens, - bool exception_on_parse_errors = true) const; - - private: - SOCKET socket_handle; - std::unique_ptr read_buf; -}; - -} // namespace impl -} // namespace sensor -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/types.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/types.cpp deleted file mode 100644 index 2aa5a5c7..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/types.cpp +++ /dev/null @@ -1,958 +0,0 @@ -/** - * Copyright (c) 2018, Ouster, Inc. - * All rights reserved. - */ - -#include "ouster/types.h" - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "logging.h" -#include "ouster/defaults.h" -#include "ouster/impl/build.h" -#include "ouster/version.h" - -namespace ouster { - -using nonstd::make_optional; -using nonstd::nullopt; -using nonstd::optional; -using std::stoul; - -namespace sensor { - -namespace impl { - -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"}}}; - -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"}, -}}; - -// clang-format off -Table udp_profile_lidar_strings{{ - {PROFILE_LIDAR_UNKNOWN, "UNKNOWN"}, - {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"}, - {PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL, "FUSA_RNG15_RFL8_NIR8_DUAL"}, -}}; - -Table udp_profile_imu_strings{{ - {PROFILE_IMU_LEGACY, "LEGACY"}, -}}; - -Table full_scale_range_strings{{ - {FSR_NORMAL, "NORMAL"}, - {FSR_EXTENDED, "EXTENDED"}, -}}; - -Table return_order_strings{{ - {ORDER_STRONGEST_TO_WEAKEST, "STRONGEST_TO_WEAKEST"}, - {ORDER_FARTHEST_TO_NEAREST, "FARTHEST_TO_NEAREST"}, - {ORDER_NEAREST_TO_FARTHEST, "NEAREST_TO_FARTHEST"}, - {ORDER_DEPRECATED_STRONGEST_RETURN_FIRST, "STRONGEST_RETURN_FIRST"}, - {ORDER_DEPRECATED_LAST_RETURN_FIRST, "LAST_RETURN_FIRST"}, -}}; - -// TODO: should we name them something better? feel like the most important is -// SHOT_LIMITING_NORMAL -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"}, -}}; - -// TODO: do we want these? do we like the names? -Table thermal_shutdown_status_strings{{ - {THERMAL_SHUTDOWN_NORMAL, "THERMAL_SHUTDOWN_NORMAL"}, - {THERMAL_SHUTDOWN_IMMINENT, "THERMAL_SHUTDOWN_IMMINENT"}, -}}; - -} // namespace impl - -/* Equality operators */ - -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) { - return !(lhs == rhs); -} - -bool operator ==(const calibration_status& lhs, const calibration_status& rhs) { - return (lhs.reflectivity_status == rhs.reflectivity_status && - lhs.reflectivity_timestamp == rhs.reflectivity_timestamp); -} - -bool operator !=(const calibration_status& lhs, const calibration_status& rhs) { - return !(lhs == 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.nmea_in_polarity == rhs.nmea_in_polarity && - 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.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.columns_per_packet == rhs.columns_per_packet && - lhs.udp_profile_lidar == rhs.udp_profile_lidar && - lhs.udp_profile_imu == rhs.udp_profile_imu && - lhs.gyro_fsr == rhs.gyro_fsr && - lhs.accel_fsr == rhs.accel_fsr && - lhs.return_order == rhs.return_order && - lhs.min_range_threshold_cm == rhs.min_range_threshold_cm); -} - -bool operator!=(const sensor_config& lhs, const sensor_config& rhs) { - return !(lhs == rhs); -} - -/* Default values */ - -static ColumnWindow default_column_window(uint32_t columns_per_frame) { - return {0, columns_per_frame - 1}; -} - -data_format default_data_format(lidar_mode mode) { - auto repeat = [](int n, const std::vector& v) { - std::vector res{}; - for (int i = 0; i < n; i++) res.insert(res.end(), v.begin(), v.end()); - return res; - }; - - uint32_t pixels_per_column = 64; - uint32_t columns_per_packet = DEFAULT_COLUMNS_PER_PACKET; - uint32_t columns_per_frame = n_cols_of_lidar_mode(mode); - ColumnWindow column_window = default_column_window(columns_per_frame); - - std::vector offset; - switch (columns_per_frame) { - case 512: - offset = repeat(16, {9, 6, 3, 0}); - break; - case 1024: - offset = repeat(16, {18, 12, 6, 0}); - break; - case 2048: - offset = repeat(16, {36, 24, 12, 0}); - break; - default: - throw std::invalid_argument{"default_data_format"}; - } - - 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))}; -} - -double default_lidar_origin_to_beam_origin(std::string prod_line) { - double lidar_origin_to_beam_origin_mm = 12.163; // default for gen 1 - if (prod_line.find("OS-0-") == 0) - lidar_origin_to_beam_origin_mm = 27.67; - else if (prod_line.find("OS-1-") == 0) - lidar_origin_to_beam_origin_mm = 15.806; - else if (prod_line.find("OS-2-") == 0) - lidar_origin_to_beam_origin_mm = 13.762; - return lidar_origin_to_beam_origin_mm; -} - -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(prod_line); - return beam_to_lidar_transform; -} - -calibration_status default_calibration_status() { - return calibration_status{}; -} - - -/* Misc operations */ - -uint32_t n_cols_of_lidar_mode(lidar_mode mode) { - switch (mode) { - case MODE_512x10: - case MODE_512x20: - return 512; - case MODE_1024x10: - case MODE_1024x20: - 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) { - switch (mode) { - case MODE_4096x5: - return 5; - case MODE_512x10: - case MODE_1024x10: - case MODE_2048x10: - return 10; - case MODE_512x20: - case MODE_1024x20: - return 20; - default: - throw std::invalid_argument{"frequency_of_lidar_mode"}; - } -} - -std::string client_version() { - return std::string("ouster_client ").append(ouster::SDK_VERSION); -} - -/* String conversion */ - -template -static optional lookup(const impl::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 ? nullopt : make_optional(res->second); -} - -template -static optional rlookup(const impl::Table table, - const char* v) { - auto end = table.end(); - auto res = std::find_if(table.begin(), end, - [&](const std::pair& p) { - return p.second && std::strcmp(p.second, v) == 0; - }); - - return res == end ? nullopt : make_optional(res->first); -} - -std::string to_string(lidar_mode mode) { - auto res = lookup(impl::lidar_mode_strings, mode); - return res ? res.value() : "UNKNOWN"; -} - -lidar_mode lidar_mode_of_string(const std::string& s) { - auto res = rlookup(impl::lidar_mode_strings, s.c_str()); - return res ? res.value() : lidar_mode::MODE_UNSPEC; -} - -std::string to_string(timestamp_mode mode) { - auto res = lookup(impl::timestamp_mode_strings, mode); - return res ? res.value() : "UNKNOWN"; -} - -timestamp_mode timestamp_mode_of_string(const std::string& s) { - auto res = rlookup(impl::timestamp_mode_strings, s.c_str()); - return res ? res.value() : timestamp_mode::TIME_FROM_UNSPEC; -} - -std::string to_string(OperatingMode mode) { - auto res = lookup(impl::operating_mode_strings, mode); - return res ? res.value() : "UNKNOWN"; -} - -optional operating_mode_of_string(const std::string& s) { - return rlookup(impl::operating_mode_strings, s.c_str()); -} - -std::string to_string(MultipurposeIOMode mode) { - auto res = lookup(impl::multipurpose_io_mode_strings, mode); - return res ? res.value() : "UNKNOWN"; -} - -optional multipurpose_io_mode_of_string( - const std::string& s) { - return rlookup(impl::multipurpose_io_mode_strings, s.c_str()); -} - -std::string to_string(Polarity polarity) { - auto res = lookup(impl::polarity_strings, polarity); - return res ? res.value() : "UNKNOWN"; -} - -optional polarity_of_string(const std::string& s) { - return rlookup(impl::polarity_strings, s.c_str()); -} - -optional full_scale_range_of_string(const std::string& s) { - return rlookup(impl::full_scale_range_strings, s.c_str()); -} - -optional return_order_of_string(const std::string& s) { - return rlookup(impl::return_order_strings, s.c_str()); -} - -std::string to_string(NMEABaudRate rate) { - auto res = lookup(impl::nmea_baud_rate_strings, rate); - return res ? res.value() : "UNKNOWN"; -} - -optional nmea_baud_rate_of_string(const std::string& s) { - return rlookup(impl::nmea_baud_rate_strings, s.c_str()); -} - -std::string to_string(AzimuthWindow azimuth_window) { - std::stringstream ss; - ss << "[" << azimuth_window.first << ", " << azimuth_window.second << "]"; - return ss.str(); -} - -std::string to_string(ChanField field) { - auto res = lookup(impl::chanfield_strings, field); - return res ? res.value() : "UNKNOWN"; -} - -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"; - } -} - -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; - } -} - -std::string to_string(UDPProfileLidar profile) { - auto res = lookup(impl::udp_profile_lidar_strings, profile); - return res ? res.value() : "UNKNOWN"; -} - -optional udp_profile_lidar_of_string(const std::string& s) { - return rlookup(impl::udp_profile_lidar_strings, s.c_str()); -} - -std::string to_string(UDPProfileIMU profile) { - auto res = lookup(impl::udp_profile_imu_strings, profile); - return res ? res.value() : "UNKNOWN"; -} - -optional udp_profile_imu_of_string(const std::string& s) { - return rlookup(impl::udp_profile_imu_strings, s.c_str()); -} - -std::string to_string(ShotLimitingStatus shot_limiting_status) { - auto res = lookup(impl::shot_limiting_status_strings, shot_limiting_status); - return res ? res.value() : "UNKNOWN"; -} - -std::string to_string(ThermalShutdownStatus thermal_shutdown_status) { - auto res = - lookup(impl::thermal_shutdown_status_strings, thermal_shutdown_status); - return res ? res.value() : "UNKNOWN"; -} - -std::string to_string(ReturnOrder return_order) { - auto res = - lookup(impl::return_order_strings, return_order); - return res ? res.value() : "UNKNOWN"; -} - -std::string to_string(FullScaleRange full_scale_range) { - auto res = - lookup(impl::full_scale_range_strings, full_scale_range); - return res ? res.value() : "UNKNOWN"; -} - -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); - } -} - -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)"); - 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: " + - root["udp_profile_lidar"].asString()}; - } - } else { - logger().warn("No lidar profile found. Using LEGACY lidar profile"); - 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"); - 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; -} - -Json::Value cal_to_json(const calibration_status& cal) { - Json::Value root{Json::objectValue}; - - if (cal.reflectivity_status) { - root["reflectivity"]["valid"] = cal.reflectivity_status.value(); - root["reflectivity"]["timestamp"] = cal.reflectivity_timestamp.value(); - } - - return root; -} - -std::string to_string(const calibration_status& cal) { - Json::Value root = cal_to_json(cal); - - Json::StreamWriterBuilder builder; - builder["enableYAMLCompatibility"] = true; - builder["precision"] = 6; - builder["indentation"] = " "; - return Json::writeString(builder, root); -} - -sensor_config parse_config(const Json::Value& root) { - sensor_config config{}; - - 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(); - logger().warn( - "Please note that udp_ip has been deprecated in favor " - "of udp_dest. Will set udp_dest appropriately..."); - } - - 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()); - 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()); - - 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()) { - auto operating_mode = - operating_mode_of_string(root["operating_mode"].asString()); - if (operating_mode) { - config.operating_mode = 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..."); - config.operating_mode = root["auto_start_flag"].asBool() - ? sensor::OPERATING_NORMAL - : sensor::OPERATING_STANDBY; - } - - if (!root["multipurpose_io_mode"].empty()) { - auto multipurpose_io_mode = multipurpose_io_mode_of_string( - root["multipurpose_io_mode"].asString()); - if (multipurpose_io_mode) { - config.multipurpose_io_mode = 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(); - - if (!root["nmea_in_polarity"].empty()) { - auto 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::runtime_error{"Unexpected NMEA Input Polarity"}; - } - } - if (!root["nmea_baud_rate"].empty()) { - auto 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::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()) { - auto 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::runtime_error{"Unexpected Sync Pulse Input Polarity"}; - } - } - 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) { - config.sync_pulse_out_polarity = sync_pulse_out_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(); - - if (!root["phase_lock_enable"].empty()) - config.phase_lock_enable = - root["phase_lock_enable"].asString() == "true" ? true : false; - - if (!root["phase_lock_offset"].empty()) - config.phase_lock_offset = root["phase_lock_offset"].asInt(); - - 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()); - - // Firmware 3.1 and higher options - if (!root["gyro_fsr"].empty()) { - auto gyro_fsr = - full_scale_range_of_string(root["gyro_fsr"].asString()); - if (gyro_fsr) { - config.gyro_fsr = gyro_fsr; - } else { - throw std::runtime_error{"Unexpected Gyro FSR"}; - } - } - - if (!root["accel_fsr"].empty()) { - auto accel_fsr = - full_scale_range_of_string(root["accel_fsr"].asString()); - if (accel_fsr) { - config.accel_fsr = accel_fsr; - } else { - throw std::runtime_error{"Unexpected Accel FSR"}; - } - } - - if (!root["return_order"].empty()) { - auto return_order = - return_order_of_string(root["return_order"].asString()); - if (return_order) { - config.return_order = return_order; - } else { - throw std::runtime_error{"Unexpected Return Order"}; - } - } - - if (!root["min_range_threshold_cm"].empty()) - config.min_range_threshold_cm = root["min_range_threshold_cm"].asInt(); - - return config; -} - -sensor_config parse_config(const std::string& config) { - Json::Value root{}; - Json::CharReaderBuilder builder{}; - std::string errors{}; - std::stringstream ss{config}; - - if (config.size()) { - if (!Json::parseFromStream(builder, ss, &root, &errors)) { - throw std::runtime_error{errors}; - } - } - - return parse_config(root); -} - -Json::Value config_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_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.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.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.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) { - 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(); - } 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_angle) - root["sync_pulse_out_angle"] = config.sync_pulse_out_angle.value(); - - if (config.sync_pulse_out_pulse_width) - root["sync_pulse_out_pulse_width"] = - config.sync_pulse_out_pulse_width.value(); - - if (config.nmea_in_polarity) - root["nmea_in_polarity"] = to_string(config.nmea_in_polarity.value()); - - if (config.nmea_baud_rate) - root["nmea_baud_rate"] = to_string(config.nmea_baud_rate.value()); - - if (config.nmea_ignore_valid_char) - root["nmea_ignore_valid_char"] = - config.nmea_ignore_valid_char.value() ? 1 : 0; - - if (config.nmea_leap_seconds) - root["nmea_leap_seconds"] = config.nmea_leap_seconds.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_polarity) - root["sync_pulse_out_polarity"] = - to_string(config.sync_pulse_out_polarity.value()); - - if (config.sync_pulse_out_frequency) - root["sync_pulse_out_frequency"] = - config.sync_pulse_out_frequency.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(); - - 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()); - - // Firmware 3.1 and higher options - if (config.gyro_fsr) - root["gyro_fsr"] = to_string(config.gyro_fsr.value()); - - if (config.accel_fsr) - root["accel_fsr"] = to_string(config.accel_fsr.value()); - - if (config.min_range_threshold_cm) - root["min_range_threshold_cm"] = config.min_range_threshold_cm.value(); - - if (config.return_order) - root["return_order"] = to_string(config.return_order.value()); - - return root; -} - -std::string to_string(const sensor_config& config) { - Json::Value root = config_to_json(config); - - Json::StreamWriterBuilder builder; - builder["enableYAMLCompatibility"] = true; - builder["precision"] = 6; - builder["indentation"] = " "; - return Json::writeString(builder, root); -} - -} // namespace sensor - -namespace util { - -std::string to_string(const version& v) { - if (v == invalid_version) { - return "UNKNOWN"; - } - - std::stringstream ss{}; - ss << "v" << v.major << "." << v.minor << "." << v.patch; - return ss.str(); -} - -version version_of_string(const std::string& s) { - std::istringstream is{s}; - char c1, c2, c3; - version v; - - is >> c1 >> v.major >> c2 >> v.minor >> c3 >> v.patch; - - if (is && c1 == 'v' && c2 == '.' && c3 == '.') - return v; - else - return invalid_version; -} - -version version_from_string(const std::string& v) { - auto rgx = std::regex(R"(v?(\d+).(\d+)\.(\d+))"); - std::smatch matches; - std::regex_search(v, matches, rgx); - - if (matches.size() < 4) return invalid_version; - - try { - return version{static_cast(stoul(matches[1])), - static_cast(stoul(matches[2])), - static_cast(stoul(matches[3]))}; - } catch (const std::exception&) { - return invalid_version; - } -} - -} // namespace util -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/udp_packet_source.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/udp_packet_source.cpp deleted file mode 100644 index f07d6178..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/udp_packet_source.cpp +++ /dev/null @@ -1,326 +0,0 @@ -/** - * Copyright (c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#include "ouster/udp_packet_source.h" - -#include -#include -#include -#include -#include - -#include "logging.h" -#include "ouster/client.h" -#include "ouster/impl/client_poller.h" -#include "ouster/types.h" - -namespace ouster { -namespace sensor { -namespace impl { - -std::string to_string(client_state st) { - switch (static_cast(st)) { - case client_state::TIMEOUT: - return "TIMEOUT"; - case client_state::CLIENT_ERROR: - return "CLIENT_ERROR"; - case client_state::LIDAR_DATA: - return "LIDAR_DATA"; - case client_state::IMU_DATA: - return "IMU_DATA"; - case client_state::EXIT: - return "EXIT"; - case Producer::CLIENT_OVERFLOW: - return "OVERFLOW"; - default: - return "UNKNOWN_EVENT"; - } -} - -std::string to_string(Event e) { - return std::string("{") + std::to_string(e.source) + ", " + - to_string(e.state) + "}"; -} - -int Producer::add_client(std::shared_ptr cli, size_t lidar_buf_size, - size_t lidar_packet_size, size_t imu_buf_size, - size_t imu_packet_size) { - std::unique_lock lock{mtx_, std::defer_lock}; - if (!lock.try_lock()) - throw std::runtime_error("add_client called on a running producer"); - - if (!cli) throw std::runtime_error("add_client called with nullptr"); - - int id = clients_.size(); - clients_.push_back(cli); - rb_->allocate({id, client_state::LIDAR_DATA}, lidar_buf_size, - Packet(lidar_packet_size)); - rb_->allocate({id, client_state::IMU_DATA}, imu_buf_size, - Packet(imu_packet_size)); - return id; -} - -int Producer::add_client(std::shared_ptr cli, const sensor_info& info, - float seconds_to_buffer) { - const data_format& df = info.format; - uint32_t packets_per_frame = df.columns_per_frame / df.columns_per_packet; - float lidar_hz = static_cast(packets_per_frame) * df.fps; - float imu_hz = 100.f; - const packet_format& pf = get_format(info); - return add_client(cli, static_cast(lidar_hz * seconds_to_buffer), - pf.lidar_packet_size, - static_cast(imu_hz * seconds_to_buffer), - pf.imu_packet_size); -} - -std::shared_ptr Producer::subscribe( - std::shared_ptr pub) { - std::unique_lock lock{mtx_, std::defer_lock}; - if (!lock.try_lock()) - throw std::runtime_error("subscribe called on a running producer"); - - pubs_.push_back(pub); - return std::make_shared(pub->queue(), rb_); -} - -std::shared_ptr Producer::subscribe(EventSet events) { - auto pub = std::make_shared(events); - return subscribe(pub); -} - -bool Producer::_verify() const { - if (clients_.size() == 0) { - logger().error("Producer started with no clients"); - return false; - } - - if (pubs_.size() == 0) { - logger().error("Producer started with no publishers"); - return false; - } - - bool out = true; - - Event last_chk; - - auto n_pubs_accept = [this, &last_chk](Event e) { - last_chk = e; - auto l = [e](int total, auto& pub) { - return total + static_cast(pub->accepts(e)); - }; - return std::accumulate(pubs_.begin(), pubs_.end(), 0, l); - }; - - if (n_pubs_accept({-1, client_state::CLIENT_ERROR}) == 0) { - logger().error("Producer: none of the publishers accept {}", - to_string(last_chk)); - out = false; - } - - if (n_pubs_accept({-1, client_state::EXIT}) == 0) { - logger().error("Producer: none of the publishers accept {}", - to_string(last_chk)); - out = false; - } - - for (int i = 0, end = clients_.size(); i < end; ++i) { - if (n_pubs_accept({i, client_state::LIDAR_DATA}) != 1) { - logger().error( - "Producer: {} publishers accept {}, needs to be exactly one", - n_pubs_accept(last_chk), to_string(last_chk)); - out = false; - } - - if (n_pubs_accept({i, client_state::IMU_DATA}) != 1) { - logger().error( - "Producer: {} publishers accept {}, needs to be exactly one", - n_pubs_accept(last_chk), to_string(last_chk)); - out = false; - } - - if (n_pubs_accept({i, client_state(Producer::CLIENT_OVERFLOW)}) == 0) { - logger().error("Producer: no publishers accept {}", - to_string(last_chk)); - } - } - return out; -} - -static bool read_packet(const client& cli, Packet& packet, client_state st) { - switch (st) { - case client_state::LIDAR_DATA: - return read_lidar_packet(cli, packet.as()); - case client_state::IMU_DATA: - return read_imu_packet(cli, packet.as()); - default: - return false; - } -} - -static client_state operator&(client_state a, client_state b) { - int a_i = static_cast(a); - int b_i = static_cast(b); - return static_cast(a_i & b_i); -} - -void Producer::run() { - // check publisher/client consistency - if (!_verify()) return; - - std::vector overflows(clients_.size(), false); - - // this could be a private virtual instead - auto handle_event = [this, &overflows](Event e) { - const client_state overflow = client_state(Producer::CLIENT_OVERFLOW); - switch (e.state) { - case 0: - break; - case client_state::CLIENT_ERROR: - case client_state::EXIT: - for (auto& pub : pubs_) pub->publish(e); - break; - case client_state::LIDAR_DATA: - case client_state::IMU_DATA: - if (rb_->full(e)) { - if (!overflows[e.source]) { - overflows[e.source] = true; - for (auto& pub : pubs_) { - // publish with priority - pub->publish({e.source, overflow}, true); - } - } - } else if (read_packet(*clients_[e.source], rb_->back(e), - e.state)) { - rb_->push(e); - for (auto& pub : pubs_) pub->publish(e); - overflows[e.source] = false; - } - break; - default: - break; - } - }; - - std::lock_guard lock{mtx_}; - - std::shared_ptr poller = make_poller(); - while (!stop_) { - reset_poll(*poller); - - for (auto& cli : clients_) set_poll(*poller, *cli); - - int res = poll(*poller); - - if (res == 0) { // TIMEOUT - continue; - } else if (res < 0) { // CLIENT_ERROR / EXIT - client_state st = get_error(*poller); - handle_event({-1, st & client_state::CLIENT_ERROR}); - handle_event({-1, st & client_state::EXIT}); - break; - } else { - for (int i = 0, end = clients_.size(); i < end; ++i) { - client_state st = get_poll(*poller, *clients_[i]); - handle_event({i, st & client_state::LIDAR_DATA}); - handle_event({i, st & client_state::IMU_DATA}); - } - } - } -} - -/* - * Producer will release mtx_ only when it exits the loop. - */ -void Producer::shutdown() { - stop_ = true; - for (auto& pub : pubs_) pub->publish({-1, client_state::EXIT}); - - std::lock_guard lock{mtx_}; - // close UDP sockets when any producer has exited - clients_.clear(); - pubs_.clear(); - rb_.reset(new RingBufferMap()); - stop_ = false; -} - -UDPPacketSource::UDPPacketSource() - : Producer(), - Subscriber(std::move(*Producer::subscribe( - {{-1, client_state::CLIENT_ERROR}, {-1, client_state::EXIT}}))) {} - -void UDPPacketSource::_accept_client_events(int id) { - pubs_[0]->set_accept({id, client_state::LIDAR_DATA}); - pubs_[0]->set_accept({id, client_state::IMU_DATA}); - pubs_[0]->set_accept({id, client_state(Producer::CLIENT_OVERFLOW)}); -} - -void UDPPacketSource::add_client(std::shared_ptr cli, - size_t lidar_buf_size, - size_t lidar_packet_size, size_t imu_buf_size, - size_t imu_packet_size) { - _accept_client_events(Producer::add_client( - cli, lidar_buf_size, lidar_packet_size, imu_buf_size, imu_packet_size)); -} -void UDPPacketSource::add_client(std::shared_ptr cli, - const sensor_info& info, - float seconds_to_buffer) { - _accept_client_events(Producer::add_client(cli, info, seconds_to_buffer)); -} - -BufferedUDPSource::BufferedUDPSource() - : Producer(), - Subscriber(std::move(*Producer::subscribe( - {{-1, client_state::CLIENT_ERROR}, - {-1, client_state::EXIT}, - {0, client_state::LIDAR_DATA}, - {0, client_state::IMU_DATA}, - {0, client_state(Producer::CLIENT_OVERFLOW)}}))) {} - -BufferedUDPSource::BufferedUDPSource(std::shared_ptr client, - size_t lidar_buf_size, - size_t lidar_packet_size, - size_t imu_buf_size, - size_t imu_packet_size) - : BufferedUDPSource() { - Producer::add_client(client, lidar_buf_size, lidar_packet_size, - imu_buf_size, imu_packet_size); -} - -BufferedUDPSource::BufferedUDPSource(std::shared_ptr client, - const sensor_info& info, - float seconds_to_buffer) - : BufferedUDPSource() { - Producer::add_client(client, info, seconds_to_buffer); -} - -client_state BufferedUDPSource::consume(LidarPacket& lidarp, ImuPacket& imup, - float timeout_sec) { - Event e = Subscriber::pop(timeout_sec); - client_state st = e.state; - - // return early without advancing queue - if (!Subscriber::_has_packet(e)) return st; - - Packet& p = Subscriber::packet(e); - - auto write_packet = [&p](auto& packet) { - auto sz = std::min(packet.buf.size(), p.buf.size()); - std::memcpy(packet.buf.data(), p.buf.data(), sz); - packet.host_timestamp = p.host_timestamp; - }; - - if (st & client_state::LIDAR_DATA) { - write_packet(lidarp); - } else if (st & client_state::IMU_DATA) { - write_packet(imup); - } - - Subscriber::advance(e); - return st; -} - -} // namespace impl -} // namespace sensor -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/util.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/util.cpp deleted file mode 100644 index d7a6aff8..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_client/src/util.cpp +++ /dev/null @@ -1,81 +0,0 @@ -/** - * Copyright (c) 2023, Ouster, Inc. - * All rights reserved. - */ - -#include "ouster/util.h" - -#include - -namespace ouster { - -void combined_helper(const Json::Value& root_orig, const Json::Value& root_new, - Json::Value& result, std::string changed_str, - std::vector& changed) { - // Iterate over new root - any - for (Json::Value::const_iterator itr = root_new.begin(); - itr != root_new.end(); itr++) { - std::string key = itr.key().asString(); - std::string new_changed_str = - (changed_str == "") ? key : changed_str + "." + key; - if (root_orig.isMember(key) && root_new[key].isObject() && - root_orig[key].isObject()) { - // need to crawl to collect changed_str output - combined_helper(root_orig[key], root_new[key], result[key], - new_changed_str, changed); - } else { - // CASE: root_orig does not have key OR - // CASE: key is leaf OR - // CASE: key is not matched as leaf/object in root_new vs root_orig - - bool change = false; - - // Unfortunately we have to handle the Ints and Doubles because 0 is - // different from 0.0 in output - // TODO - find differnt way so we don't have to do this - if (!root_orig.isMember(key) || root_new[key].isObject()) { - change = true; - } else if ((root_new[key].isString() || root_new[key].isBool()) && - (root_orig[key] != root_new[key])) { - change = true; - } else if (root_new[key].isIntegral() && - (root_orig[key].asInt() != root_new[key].asInt())) { - change = true; - } else if (root_new[key].isDouble() && (root_orig[key].asDouble() != - root_new[key].asDouble())) { - change = true; - } else if (root_new[key].isArray()) { - // NOTE: this does not handle the array of arrays that DF - // currently has - for (size_t i = 0; i < root_new[key].size(); i++) { - if (root_new[key][(int)i].asDouble() != - root_orig[key][(int)i].asDouble()) { - change = true; - continue; - } - } - } - - if (change) { - result[key] = root_new[key]; - changed.push_back(new_changed_str); - } - } - } -} // namespace ouster - -Json::Value combined(const Json::Value& root_orig, const Json::Value& root_new, - std::vector& changed) { - Json::Value result{}; - - // make sure changed is empty to start - changed.clear(); - - // set result equal to root_orig to capture all keys in root_orig - result = root_orig; - - combined_helper(root_orig, root_new, result, "", changed); - return result; -} - -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/CHANGELOG.rst b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/CHANGELOG.rst deleted file mode 100644 index ef9fdd82..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/CHANGELOG.rst +++ /dev/null @@ -1,6 +0,0 @@ -2023-07-01 -=========== - -Initial public Ouster OSF soft release - -* C++ OSF lib with Python binding \ No newline at end of file diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/CMakeLists.txt b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/CMakeLists.txt deleted file mode 100644 index ae07a49d..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/CMakeLists.txt +++ /dev/null @@ -1,160 +0,0 @@ -# ==== Debug params ============ -# == NOTE(pb): Left intentionally -# set(CMAKE_VERBOSE_MAKEFILE OFF) -# set(CMAKE_FIND_DEBUG_MODE OFF) - -option(OUSTER_OSF_NO_MMAP "Don't use mmap(), useful for WASM targets" OFF) -option(OUSTER_OSF_NO_THREADING "Don't use threads, useful for WASM targets" OFF) - -# ==== Requirements ==== -find_package(ZLIB REQUIRED) -find_package(PNG REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(jsoncpp REQUIRED) -find_package(spdlog REQUIRED) -include(Coverage) - -# TODO: Extract to a separate FindFlatbuffers cmake file -# Flatbuffers flatc resolution and different search name 'flatbuffers` with Conan -# NOTE2[pb]: 200221007: We changed Conan cmake package to look to `flatbuffers` -# because it started failing out of blue :idk:scream: will see. -if(NOT CONAN_EXPORTED) - find_package(Flatbuffers REQUIRED) - if(NOT DEFINED FLATBUFFERS_FLATC_EXECUTABLE) - set(FLATBUFFERS_FLATC_EXECUTABLE flatbuffers::flatc) - endif() - message(STATUS "Flatbuffers found: ${Flatbuffers_DIR}" ) -else() - find_package(flatbuffers REQUIRED) - if(WIN32) - set(FLATBUFFERS_FLATC_EXECUTABLE flatc.exe) - else() - set(FLATBUFFERS_FLATC_EXECUTABLE flatc) - endif() - message(STATUS "flatbuffers found: ${Flatbuffers_DIR}" ) -endif() - -# TODO[pb]: Move to flatbuffers 2.0 and check do we still need this??? -# Using this link lib search method so to get shared .so library and not -# static in Debian systems. But it correctly find static lib in vcpkg/manylinux -# builds. -# STORY: We need to make it static (but with -fPIC) for Python bindings. -# However in Debian packages we can only use shared libs because static -# are not compiled with PIC. Though in vcpkg it uses static lib, -# which we've confirmed to be the case and what we need for manylinux. -# find_library(flatbuffers_lib NAMES flatbuffers REQUIRED) -# set(flatbuffers_lib flatbuffers::flatbuffers) - -# === Flatbuffer builder functions ==== -include(cmake/osf_fb_utils.cmake) - -set(OSF_FB_DIR ${CMAKE_CURRENT_SOURCE_DIR}/fb) - -set(FB_SOURCE_GEN_DIR ${CMAKE_CURRENT_BINARY_DIR}/fb_source_generated) -set(FB_BINARY_SCHEMA_DIR ${CMAKE_CURRENT_BINARY_DIR}/fb_binary_schemas) - -set(FB_MODULES_TO_BUILD os_sensor streaming ml) - -# ======= Typescript Flatbuffer Generators ===== -set(FB_TS_GENERATED_DIR ${FB_SOURCE_GEN_DIR}/ts) -build_ts_fb_modules( - TARGET ts_gen - FB_DIR "${OSF_FB_DIR}" - FB_MODULES "${FB_MODULES_TO_BUILD}" - SOURCE_GEN_DIR "${FB_TS_GENERATED_DIR}" - ) - -# ======= Python Flatbuffer Generators ===== -set(FB_PYTHON_GENERATED_DIR ${FB_SOURCE_GEN_DIR}/python) -build_py_fb_modules( - TARGET py_gen - FB_DIR "${OSF_FB_DIR}" - SOURCE_GEN_DIR "${FB_PYTHON_GENERATED_DIR}" - ) - -# ======= C++ Flatbuffer Generators ===== -set(FB_CPP_GENERATED_DIR ${FB_SOURCE_GEN_DIR}/cpp) -build_cpp_fb_modules( - TARGET cpp_gen - FB_DIR "${OSF_FB_DIR}" - FB_MODULES "${FB_MODULES_TO_BUILD}" - SOURCE_GEN_DIR "${FB_CPP_GENERATED_DIR}" - BINARY_SCHEMA_DIR "${FB_BINARY_SCHEMA_DIR}" - ) - -# === Always generate C++ stubs ============== -# and skip Typescript and Python code from FB specs generation -# since they not needed during a regular OSF lib builds -add_custom_target(all_fb_gen ALL DEPENDS cpp_gen) # ts_gen py_gen - -add_library(ouster_osf STATIC src/compat_ops.cpp - src/png_tools.cpp - src/basics.cpp - src/crc32.cpp - src/metadata.cpp - src/meta_lidar_sensor.cpp - src/meta_extrinsics.cpp - src/meta_streaming_info.cpp - src/stream_lidar_scan.cpp - src/layout_streaming.cpp - src/file.cpp - src/reader.cpp - src/operations.cpp - src/json_utils.cpp - src/fb_utils.cpp - src/pcap_source.cpp - src/writer.cpp -) - -CodeCoverageFunctionality(ouster_osf) - -if (OUSTER_OSF_NO_MMAP) - target_compile_definitions(ouster_osf PRIVATE OUSTER_OSF_NO_MMAP) -endif() - -if (OUSTER_OSF_NO_THREADING) - target_compile_definitions(ouster_osf PRIVATE OUSTER_OSF_NO_THREADING) -endif() - -# Include Flatbuffers generated C++ headers -target_include_directories(ouster_osf PUBLIC - $ - $ -) -target_link_libraries(ouster_osf - PUBLIC - OusterSDK::ouster_client OusterSDK::ouster_pcap PNG::PNG - flatbuffers::flatbuffers ZLIB::ZLIB jsoncpp_lib -) -target_include_directories(ouster_osf PUBLIC - $ - $ - ${EIGEN3_INCLUDE_DIR} -) -add_dependencies(ouster_osf cpp_gen) -add_library(OusterSDK::ouster_osf ALIAS ouster_osf) - - -# Check if ouster_client compiled with -mavx2 option and add those to ouster_osf -# If we are not matching -mavx2 compile flag Eigen lib functions might crash with -# SegFault and double free/memory corruption errors... -get_target_property(CLIENT_OPTIONS OusterSDK::ouster_client COMPILE_OPTIONS) -if(CLIENT_OPTIONS MATCHES ".*-mavx2.*") - target_compile_options(ouster_osf PUBLIC -mavx2) -endif() - -if(CMAKE_PROJECT_NAME STREQUAL PROJECT_NAME AND BUILD_TESTING) - enable_testing() - add_subdirectory(tests) -endif() - -# ==== Install ========================================================= -install(TARGETS ouster_osf - EXPORT ouster-sdk-targets - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include) - -install(DIRECTORY include/ouster DESTINATION include) -install(DIRECTORY ${FB_CPP_GENERATED_DIR}/ DESTINATION include/fb_generated) diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/README.rst b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/README.rst deleted file mode 100644 index a9b54725..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/README.rst +++ /dev/null @@ -1,31 +0,0 @@ -=================== -Ouster OSF library -=================== - -:Maintainers: Pavlo Bashmakov -:Description: OSF File format and tools to store/process lidar sensor data -:Project-type: lib - -Summary -------- - -Ouster OSF is an extendable file format to efficiently store the lidar information -with sensor intrinsics and streaming capabilities. - -This is the beginning of the public OSF to be able to read and process OSF files -generated by Ouster tools (i.e. mapping or Data App) - - -Build & Test -------------- - -See the main Ouser SDK Building docs for details. - - -TODOs ------ - -- [ ] OSF API Documentaion -- [ ] OSF Usage Documentaion -- [ ] OSF examples -- [ ] more OSF related tools ... \ No newline at end of file diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/cmake/osf_fb_utils.cmake b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/cmake/osf_fb_utils.cmake deleted file mode 100644 index 4f909d4b..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/cmake/osf_fb_utils.cmake +++ /dev/null @@ -1,395 +0,0 @@ -# ======================================================================= -# ======== Flatbuffers Generators for C++/Python/Javasctipt ============= - -# This file provides three generator functions that makes rules for convertion -# of Flatbuffer's specs (*.fbs) to the corresponding language stubs: -# - 'build_cpp_fb_modules' - for C++ include headers -# - 'build_ts_fb_modules' - for Typescript modules -# - 'build_py_fb_modules' - for Python modules - -# All functions have the common structure and use the common parameters -# (with binary schemas generators available only in C++): - -# TARGET - (REQUIRED) Name of the target that will combine all custom -# generator commands. It can later be used as dependency for other -# targets (including ALL - to always generate code if needed) - -# FB_DIR - (OPTIONAL) Root of Flatbuffers definitions, where specs organized -# by modules. (see FB_MODULES param) -# Example layout of FB_DIR ('project-luna/fb') for imaginary -# 'project-luna': -# - project-luna -# - fb -# - background_rad.fbs -# - rover_base -# - rover_state.fbs -# - sars_stream.fbs -# - laser_eye -# - photons_stream.fbs -# - neutrino_extrinsics.fbs -# default: if ommited defaults to 'current-project/fb' -# (i.e. "${CMAKE_CURRENT_SOURCE_DIR}/fb") - -# FB_MODULES - (OPTIONAL) List of modules (i.e. subdirectories of FB_DIR) -# to use for code generation. The resulting subdirectories -# structure is preserved (Typescript/C++ only). -# For example (C++) file 'FB_DIR/rover_base/sars_stream.fbs' will -# be converted to 'SOURCE_GEN_DIR/reover_base/sars_stream_generated.h' -# For the 'project-luna' above we have two modules: -# 'rover_base' and 'laser_eye'. -# default: empty, i.e. no modules used for generation and only -# root *.fbs files (FB_DIR/*.fbs) used. (except Python, -# where FB_MODULES is not used and all files recursively -# is generated) - -# SOURCE_GEN_DIR - (OPTIONAL) Location of the generated source files. -# It's also available as TARGET property by SOURCE_GEN_DIR -# name which is usefull if using default SOURCE_GEN_DIR -# location. -# default: "${CMAKE_CURRENT_BINARY_DIR}/fb_source_generated" - -# BINARY_SCHEMA_DIR - (OPTIONAL) Location of the generated binary schemas -# files (*.bfbs). -# It's also available as property BINARY_SCHEMA_DIR -# for generated TARGET. -# default: "${CMAKE_CURRENT_BINARY_DIR}/fb_binary_schemas" - - -# ======= Common Initializer ============================================= - -macro(initialize_osf_fb_utils_defaults) - # Use flatc reolve from flatbuffers/CMake/BuildFlatBuffers.cmake source - # Test if including from FindFlatBuffers - if(FLATBUFFERS_FLATC_EXECUTABLE) - set(FLATC ${FLATBUFFERS_FLATC_EXECUTABLE}) - else() - set(FLATC flatc) - endif() - set(FB_DEFAULT_ROOT_DIR "${CMAKE_CURRENT_SOURCE_DIR}/fb") - set(FB_DEFAULT_SOURCE_GEN_DIR "${CMAKE_CURRENT_BINARY_DIR}/fb_source_generated") - set(FB_DEFAULT_BINARY_SCHEMA_DIR "${CMAKE_CURRENT_BINARY_DIR}/fb_binary_schemas") -endmacro() - -# --- Log/debug helper function -# logs messages if FB_VERBOSE is true (in cmake sense, e.g. 1, Y, TRUE, YES, ON) -macro(fb_log MSG) - if(FB_VERBOSE) - message(STATUS ${MSG}) - endif() -endmacro() - - -# ======= C++ Generation ============================================= - -# --- Helper macro: -# Generates Flatbuffers binary schema generation target for FB_FILE in FB_MODULE -# Result placed in RESULT_DIR -# Side effect: Variable ALL_GEN_FILE appended with generated file. -macro(make_binary_schemas_command) - set(PARSE_ONE_VALUE_ARGS FB_FILE FB_MODULE RESULT_DIR) - cmake_parse_arguments(MOD_ARG "" "${PARSE_ONE_VALUE_ARGS}" "" ${ARGN}) - fb_log("BIN MOD: FB_FILE = ${MOD_ARG_FB_FILE}") - fb_log("BIN MOD: FB_MODULE = ${MOD_ARG_FB_MODULE}") - fb_log("BIN MOD: RESULT_DIR = ${MOD_ARG_RESULT_DIR}") - if (NOT ${MOD_ARG_FB_MODULE} STREQUAL "") - set(BIN_DIR ${MOD_ARG_RESULT_DIR}/${MOD_ARG_FB_MODULE}) - else() - set(BIN_DIR ${MOD_ARG_RESULT_DIR}) - endif() - get_filename_component(FB_NAME ${MOD_ARG_FB_FILE} NAME_WE) - set(BIN_FILE ${BIN_DIR}/${FB_NAME}.bfbs) - add_custom_command( - OUTPUT ${BIN_FILE} - DEPENDS ${MOD_ARG_FB_FILE} - COMMAND - ${FLATC} -b --schema - -o ${BIN_DIR} - ${MOD_ARG_FB_FILE} - ) - list(APPEND ALL_GEN_FILES ${BIN_FILE}) -endmacro() - -# --- Helper macro: -# Generates Flatbuffers C++ schema generation target for FB_FILE in FB_MODULE -# Result placed in RESULT_DIR -# Side effect: Variable ALL_GEN_FILE appended with generated file. -macro(make_cpp_gen_command) - set(PARSE_ONE_VALUE_ARGS FB_FILE FB_MODULE RESULT_DIR) - cmake_parse_arguments(MOD_ARG "" "${PARSE_ONE_VALUE_ARGS}" "" ${ARGN}) - fb_log("CPP MOD: FB_FILE = ${MOD_ARG_FB_FILE}") - fb_log("CPP MOD: FB_MODULE = ${MOD_ARG_FB_MODULE}") - fb_log("CPP MOD: RESULT_DIR = ${MOD_ARG_RESULT_DIR}") - if (NOT ${MOD_ARG_FB_MODULE} STREQUAL "") - set(GEN_DIR ${MOD_ARG_RESULT_DIR}/${MOD_ARG_FB_MODULE}) - else() - set(GEN_DIR ${MOD_ARG_RESULT_DIR}) - endif() - get_filename_component(FB_NAME ${MOD_ARG_FB_FILE} NAME_WE) - set(GEN_FILE ${GEN_DIR}/${FB_NAME}_generated.h) - add_custom_command( - OUTPUT ${GEN_FILE} - DEPENDS ${MOD_ARG_FB_FILE} - COMMAND - ${FLATC} --cpp --no-includes --scoped-enums - -o ${GEN_DIR} - ${MOD_ARG_FB_FILE} - ) - list(APPEND ALL_GEN_FILES ${GEN_FILE}) -endmacro() - -# C++ Flatbuffer generators -# Params: TARGET (REQUIRED), FB_DIR, FB_MODULES, SOURCE_GEN_DIR, BINARY_SCHEMA_DIR -# Properties available on generated TARGET: SOURCE_GEN_DIR, BINARY_SCHEMA_DIR -function(build_cpp_fb_modules) - - initialize_osf_fb_utils_defaults() - - set(PARSE_OPTIONS "") - set(PARSE_ONE_VALUE_ARGS - TARGET - FB_DIR - SOURCE_GEN_DIR - BINARY_SCHEMA_DIR - ) - set(PARSE_MULTI_VALUE_ARGS FB_MODULES) - cmake_parse_arguments(ARGS "${PARSE_OPTIONS}" "${PARSE_ONE_VALUE_ARGS}" - "${PARSE_MULTI_VALUE_ARGS}" ${ARGN} ) - - fb_log("... BUILDING CPP FB MODULES ............ ") - fb_log("ARGS_TARGET = ${ARGS_TARGET}") - fb_log("ARGS_FB_DIR = ${ARGS_FB_DIR}") - fb_log("ARGS_FB_MODULES = ${ARGS_FB_MODULES}") - fb_log("ARG_SOURCE_GEN_DIR = ${ARGS_SOURCE_GEN_DIR}") - fb_log("ARG_BINARY_GEN_DIR = ${ARGS_BINARY_SCHEMA_DIR}") - - if (NOT DEFINED ARGS_FB_DIR) - set(ARGS_FB_DIR "${FB_DEFAULT_ROOT_DIR}") - fb_log("using default FB_DIR = ${ARGS_FB_DIR}") - endif() - - if (NOT DEFINED ARGS_SOURCE_GEN_DIR) - set(ARGS_SOURCE_GEN_DIR "${FB_DEFAULT_SOURCE_GEN_DIR}/cpp") - fb_log("using default SOURCE_GEN_DIR = ${ARGS_SOURCE_GEN_DIR}") - endif() - - if (NOT DEFINED ARGS_BINARY_SCHEMA_DIR) - set(ARGS_BINARY_SCHEMA_DIR "${FB_DEFAULT_BINARY_SCHEMA_DIR}") - fb_log("using default BINARY_SCHEMA_DIR = ${ARGS_BINARY_SCHEMA_DIR}") - endif() - - # List accumulated in macros 'make_*_command' calls - set(ALL_GEN_FILES "") - - file(GLOB MOD_FB_CORE_FILES LIST_DIRECTORIES false ${ARGS_FB_DIR}/*.fbs) - foreach(FB_FILE ${MOD_FB_CORE_FILES}) - - make_cpp_gen_command( - FB_FILE ${FB_FILE} - RESULT_DIR ${ARGS_SOURCE_GEN_DIR} - ) - - if (NOT ${ARGS_BINARY_SCHEMA_DIR} STREQUAL "") - make_binary_schemas_command( - FB_FILE ${FB_FILE} - RESULT_DIR ${ARGS_BINARY_SCHEMA_DIR} - ) - endif() - - endforeach() - - foreach(FB_MODULE ${ARGS_FB_MODULES}) - file(GLOB FB_MODULE_FILES LIST_DIRECTORIES false ${ARGS_FB_DIR}/${FB_MODULE}/*.fbs) - - foreach(FB_FILE ${FB_MODULE_FILES}) - - make_cpp_gen_command( - FB_FILE ${FB_FILE} - FB_MODULE ${FB_MODULE} - RESULT_DIR ${ARGS_SOURCE_GEN_DIR} - ) - - if (NOT ${ARGS_BINARY_SCHEMA_DIR} STREQUAL "") - make_binary_schemas_command( - FB_FILE ${FB_FILE} - FB_MODULE ${FB_MODULE} - RESULT_DIR ${ARGS_BINARY_SCHEMA_DIR} - ) - endif() - - endforeach() - - endforeach() - - add_custom_target(${ARGS_TARGET} DEPENDS ${ALL_GEN_FILES} - COMMENT "Generating C++ code from Flatbuffers spec") - - set_property(TARGET ${ARGS_TARGET} - PROPERTY SOURCE_GEN_DIR - ${ARGS_SOURCE_GEN_DIR} - ) - - set_property(TARGET ${ARGS_TARGET} - PROPERTY BINARY_SCHEMA_DIR - ${ARGS_BINARY_SCHEMA_DIR} - ) - -endfunction() - - -# ======= Typescript Generation ======================================== - -# --- Helper macro: -# Generates Flatbuffers Typescript generation command for FB_FILE in FB_MODULE -# Result placed in RESULT_DIR -# Side effect: Variable ALL_GEN_FILE appended with generated file. -macro(make_ts_gen_command) - set(PARSE_ONE_VALUE_ARGS FB_FILE FB_MODULE RESULT_DIR) - cmake_parse_arguments(MOD_ARG "" "${PARSE_ONE_VALUE_ARGS}" "" ${ARGN}) - fb_log("TS MOD: FB_FILE = ${MOD_ARG_FB_FILE}") - fb_log("TS MOD: FB_MODULE = ${MOD_ARG_FB_MODULE}") - fb_log("TS MOD: RESULT_DIR = ${MOD_ARG_RESULT_DIR}") - if (NOT ${MOD_ARG_FB_MODULE} STREQUAL "") - set(GEN_DIR ${MOD_ARG_RESULT_DIR}/${MOD_ARG_FB_MODULE}) - else() - set(GEN_DIR ${MOD_ARG_RESULT_DIR}) - endif() - get_filename_component(FB_NAME ${MOD_ARG_FB_FILE} NAME_WE) - set(GEN_FILE ${GEN_DIR}/${FB_NAME}_generated.ts) - add_custom_command( - OUTPUT ${GEN_FILE} - DEPENDS ${MOD_ARG_FB_FILE} - COMMAND - ${FLATC} --ts - -o ${GEN_DIR} - ${MOD_ARG_FB_FILE} - ) - list(APPEND ALL_GEN_FILES ${GEN_FILE}) -endmacro() - -# Typescript Flatbuffer generators -# Params: TARGET (REQUIRED), FB_DIR, FB_MODULES, SOURCE_GEN_DIR -# Properties available on generated TARGET: SOURCE_GEN_DIR -function(build_ts_fb_modules) - - initialize_osf_fb_utils_defaults() - - set(PARSE_OPTIONS "") - set(PARSE_ONE_VALUE_ARGS - TARGET - FB_DIR - SOURCE_GEN_DIR - ) - set(PARSE_MULTI_VALUE_ARGS FB_MODULES) - cmake_parse_arguments(ARGS "${PARSE_OPTIONS}" "${PARSE_ONE_VALUE_ARGS}" - "${PARSE_MULTI_VALUE_ARGS}" ${ARGN} ) - - fb_log("... BUILDING TS FB MODULES ............ ") - fb_log("ARGS_TARGET = ${ARGS_TARGET}") - fb_log("ARGS_FB_DIR = ${ARGS_FB_DIR}") - fb_log("ARGS_FB_MODULES = ${ARGS_FB_MODULES}") - fb_log("ARG_SOURCE_GEN_DIR = ${ARGS_SOURCE_GEN_DIR}") - - if (NOT DEFINED ARGS_FB_DIR) - set(ARGS_FB_DIR "${FB_DEFAULT_ROOT_DIR}") - fb_log("using default FB_DIR = ${ARGS_FB_DIR}") - endif() - - if (NOT DEFINED ARGS_SOURCE_GEN_DIR) - set(ARGS_SOURCE_GEN_DIR "${FB_DEFAULT_SOURCE_GEN_DIR}/ts") - fb_log("using default SOURCE_GEN_DIR = ${ARGS_SOURCE_GEN_DIR}") - endif() - - # List accumulated in macros 'make_*_command' calls - set(ALL_GEN_FILES "") - - file(GLOB MOD_FB_CORE_FILES LIST_DIRECTORIES false ${ARGS_FB_DIR}/*.fbs) - foreach(FB_FILE ${MOD_FB_CORE_FILES}) - - make_ts_gen_command( - FB_FILE ${FB_FILE} - FB_MODULE ${FB_MODULE} - RESULT_DIR ${ARGS_SOURCE_GEN_DIR} - ) - - endforeach() - - foreach(FB_MODULE ${ARGS_FB_MODULES}) - - file(GLOB FB_MODULE_FILES LIST_DIRECTORIES false ${ARGS_FB_DIR}/${FB_MODULE}/*.fbs) - - foreach(FB_FILE ${FB_MODULE_FILES}) - - make_ts_gen_command( - FB_FILE ${FB_FILE} - FB_MODULE ${FB_MODULE} - RESULT_DIR ${ARGS_SOURCE_GEN_DIR} - ) - - endforeach() - - endforeach() - - add_custom_target(${ARGS_TARGET} DEPENDS ${ALL_GEN_FILES} - COMMENT "Generating Typescript code from Flatbuffers spec") - - set_property(TARGET ${ARGS_TARGET} - PROPERTY SOURCE_GEN_DIR - ${ARGS_SOURCE_GEN_DIR} - ) - -endfunction() - -# ======= Python Generation ============================================= - -# Python Flatbuffer generators -# Params: TARGET (REQUIRED), FB_DIR, SOURCE_GEN_DIR -# Properties available on generated TARGET: SOURCE_GEN_DIR -function(build_py_fb_modules) - - initialize_osf_fb_utils_defaults() - - set(PARSE_OPTIONS "") - set(PARSE_ONE_VALUE_ARGS - TARGET - FB_DIR - SOURCE_GEN_DIR - ) - set(PARSE_MULTI_VALUE_ARGS "") - cmake_parse_arguments(ARGS "${PARSE_OPTIONS}" "${PARSE_ONE_VALUE_ARGS}" - "${PARSE_MULTI_VALUE_ARGS}" ${ARGN} ) - fb_log("... BUILDING PYTHON FB MODULES ............ ") - fb_log("ARGS_TARGET = ${ARGS_TARGET}") - fb_log("ARGS_FB_DIR = ${ARGS_FB_DIR}") - fb_log("ARG_SOURCE_GEN_DIR = ${ARGS_SOURCE_GEN_DIR}") - - if (NOT DEFINED ARGS_FB_DIR) - set(ARGS_FB_DIR "${FB_DEFAULT_ROOT_DIR}") - fb_log("using default FB_DIR = ${ARGS_FB_DIR}") - endif() - - if (NOT DEFINED ARGS_SOURCE_GEN_DIR) - set(ARGS_SOURCE_GEN_DIR "${FB_DEFAULT_SOURCE_GEN_DIR}/cpp") - fb_log("using default SOURCE_GEN_DIR = ${ARGS_SOURCE_GEN_DIR}") - endif() - - file(GLOB_RECURSE ALL_FBS_FILES LIST_DIRECTORIES false ${ARGS_FB_DIR}/*.fbs) - - # Using random file names to allow multiproject use of this function - string(RANDOM LENGTH 4 PY_GEN_RANDOM) - set(PY_GEN_TIMESTAMP_FILE ${CMAKE_CURRENT_BINARY_DIR}/py_gen_${PY_GEN_RANDOM}.timestamp) - add_custom_command( - OUTPUT ${PY_GEN_TIMESTAMP_FILE} - DEPENDS ${ALL_FBS_FILES} - COMMAND ${FLATC} --python -o ${ARGS_SOURCE_GEN_DIR} ${ALL_FBS_FILES} - COMMAND ${CMAKE_COMMAND} -E touch ${PY_GEN_TIMESTAMP_FILE} - ) - - add_custom_target(${ARGS_TARGET} DEPENDS ${PY_GEN_TIMESTAMP_FILE} - COMMENT "Generating Python code from Flatbuffers spec") - - set_property(TARGET ${ARGS_TARGET} - PROPERTY SOURCE_GEN_DIR - ${ARGS_SOURCE_GEN_DIR} - ) - -endfunction() diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/fb/chunk.fbs b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/fb/chunk.fbs deleted file mode 100644 index 3d6a301d..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/fb/chunk.fbs +++ /dev/null @@ -1,27 +0,0 @@ -namespace ouster.osf.v2; - -// See 'msgs/*.fb' files for messages and corresponding stream specs - -table StampedMessage { - - // timestamp in nanoseconds - ts:uint64 (key); - - // 'id' should always refer to the existing Metadata.entries[] field - // that is then used to reconstruct the 'StampedMessage.buffer' field to - // the corresponding msg Flatbuffer object. - // (e.g metadata.entries[].id == StampedMessage.id) - id:uint32; - - // Flatbuffer encoded object that described in Metadata entry (by 'id') - buffer:[uint8]; -} - -table Chunk { - // List of messages in chunk (sorting by ts is not guaranteed) - messages:[StampedMessage]; -} - -file_identifier "OSF!"; -file_extension "osfc"; -root_type Chunk; diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/fb/header.fbs b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/fb/header.fbs deleted file mode 100644 index 3fca9871..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/fb/header.fbs +++ /dev/null @@ -1,37 +0,0 @@ -namespace ouster.osf.v2; - - -enum HEADER_STATUS:uint8 { - UNKNOWN = 0, - - // INVALID is used during writing and generally means that - // file wasn't finished properly - INVALID, - - // VALID means that the OSF file is properly finished and - // reader might expect the `metadata_offset` pointing to valid metadata - // object - VALID -} - -table Header { - version:uint64; - status:HEADER_STATUS; - - // Offset to the Metadata object from the beginning of the file - metadata_offset:uint64 = 1; - - // Length of the file in bytes, should be used for validation - // that file was constructed and finished correctly - file_length:uint64 = 1; - - // metadata_offset and file_length defaults to 1 (and not 0), so - // we can write actual zero without forcing Flatbuffer to do so. - // Because when we use the default value the value is actually - // not inserted in the buffer and so it is smaller but we want - // it to have a stable byte size. -} - -file_identifier "OSF$"; -file_extension "osfh"; -root_type Header; \ No newline at end of file diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/fb/metadata.fbs b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/fb/metadata.fbs deleted file mode 100644 index acd44a20..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/fb/metadata.fbs +++ /dev/null @@ -1,38 +0,0 @@ -namespace ouster.osf.v2; - -struct ChunkOffset { - // lowest timestamp of a StampedMessage in a Chunk - start_ts:uint64; - // highest timestamp of a StampedMessage in a Chunk - end_ts:uint64; - // offset from the beginning of Chunks (the address following Header's CRC32) - offset:uint64; -} - -// Contains file information (streams metadata, file/session data, etc) -table MetadataEntry { - // used in references from other metadata entries - // and from StampedMessage.id - id:uint32 (key); - - // type identifier of the stream or metas, - // (e.g. 'ouster/os_sensor/LidarSensor') used to dispatch to the fb spec - // for parsing 'buffer' field. - type:string; - - // Flatbuffer encoded object of 'type' (see 'type' field description) - buffer:[uint8]; -} - -table Metadata { - id:string; - start_ts:uint64; - end_ts:uint64; - chunks:[ChunkOffset]; - entries:[MetadataEntry]; // required to be sorted and unique - // by MetadataEntry.id for fast lookups -} - -file_identifier "OSF#"; -file_extension "osfs"; -root_type Metadata; \ No newline at end of file diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/fb/os_sensor/extrinsics.fbs b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/fb/os_sensor/extrinsics.fbs deleted file mode 100644 index 5b17c894..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/fb/os_sensor/extrinsics.fbs +++ /dev/null @@ -1,14 +0,0 @@ -namespace ouster.osf.v2; - -// ============ Extrinsics ======================================== - -table Extrinsics { - extrinsics:[double]; // vector of 16, row-major representaton of [4x4] - // matrix - ref_id:uint32; // reference metadata id of the object it's related to - name:string; // optional name of the extrinsics source or originator -} - -// MetadataEntry.type: ouster/v1/os_sensor/Extrinsics -root_type Extrinsics; -file_identifier "oExt"; \ No newline at end of file diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/fb/os_sensor/lidar_scan_stream.fbs b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/fb/os_sensor/lidar_scan_stream.fbs deleted file mode 100644 index d24125cb..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/fb/os_sensor/lidar_scan_stream.fbs +++ /dev/null @@ -1,89 +0,0 @@ -namespace ouster.osf.v2; - -// sensor::ChanField enum mapping -enum CHAN_FIELD:uint8 { - UNKNOWN = 0, - RANGE = 1, - RANGE2 = 2, - SIGNAL = 3, - SIGNAL2 = 4, - REFLECTIVITY = 5, - REFLECTIVITY2 = 6, - NEAR_IR = 7, - FLAGS = 8, - FLAGS2 = 9, - RAW_HEADERS = 40, - RAW32_WORD5 = 45, - RAW32_WORD6 = 46, - RAW32_WORD7 = 47, - RAW32_WORD8 = 48, - RAW32_WORD9 = 49, - CUSTOM0 = 50, - CUSTOM1 = 51, - CUSTOM2 = 52, - CUSTOM3 = 53, - CUSTOM4 = 54, - CUSTOM5 = 55, - CUSTOM6 = 56, - CUSTOM7 = 57, - CUSTOM8 = 58, - CUSTOM9 = 59, - RAW32_WORD1 = 60, - RAW32_WORD2 = 61, - RAW32_WORD3 = 62, - RAW32_WORD4 = 63 -} - -// sensor::ChanFieldType enum mapping -enum CHAN_FIELD_TYPE:uint8 { - VOID = 0, - UINT8 = 1, - UINT16 = 2, - UINT32 = 3, - UINT64 = 4 -} - -// PNG encoded channel fields of LidarScan -table ChannelData { - buffer:[uint8]; -} - -// Single lidar field spec -struct ChannelField { - chan_field:CHAN_FIELD; - chan_field_type:CHAN_FIELD_TYPE; -} - -table LidarScanMsg { - // ====== LidarScan Channels ======================= - // encoded ChanField data - channels:[ChannelData]; - // corresponding ChanField description to what is contained in channels[] above - field_types:[ChannelField]; - - // ===== LidarScan Headers ========================= - header_timestamp:[uint64]; - header_measurement_id:[uint16]; - header_status:[uint32]; - frame_id:int32; - // pose vector of 4x4 matrices per every timestamp element (i.e. number of - // elements in a vector should be 16 x timestamp.size()), every 4x4 matrix - // has a col-major storage - pose:[double]; - packet_timestamp:[uint64]; -} - -// Scan data from a lidar sensor. One scan is a sweep of a sensor (360 degree). -table LidarScanStream { - sensor_id:uint32; // referenced to metadata.entry[].id with LidarScan - - // LidarScan field_types spec, used only as a HINT about what type - // of messages to expect from LidarScanMsg in a stream. - // NOTE: For LidarScanMsg decoding field types from - // LidarScanMsg.field_types[] should be used. - field_types:[ChannelField]; -} - -// MetadataEntry.type: ouster/v1/os_sensor/LidarScanStream -root_type LidarScanStream; -file_identifier "oLSS"; \ No newline at end of file diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/fb/os_sensor/lidar_sensor.fbs b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/fb/os_sensor/lidar_sensor.fbs deleted file mode 100644 index 353c4e8f..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/fb/os_sensor/lidar_sensor.fbs +++ /dev/null @@ -1,11 +0,0 @@ -namespace ouster.osf.v2; - -// ============ Lidar Sensor ======================================== - -table LidarSensor { - metadata:string; -} - -// MetadataEntry.type: ouster/v1/os_sensor/LidarSensor -root_type LidarSensor; -file_identifier "oLS_"; \ No newline at end of file diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/fb/streaming/streaming_info.fbs b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/fb/streaming/streaming_info.fbs deleted file mode 100644 index 5d1c0798..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/fb/streaming/streaming_info.fbs +++ /dev/null @@ -1,41 +0,0 @@ -namespace ouster.osf.v2; - -table StreamStats { - // refers to metadata.entries[id] that describes a stream - stream_id:uint32; - // host timestamp of the first message in `stream_id` stream - // in the whole OSF file - start_ts:uint64; - // host timestamp of the last message in `stream_id` stream - // in the whole OSF file - end_ts:uint64; - // total number of messages in a `stream_id` in the whole OSF file - message_count:uint64; - // avg size of the messages in bytes for a `stream_id` in the whole - // OSF file - message_avg_size:uint32; -} - -table ChunkInfo { - // offset of the chunk, matches the offset of `metadata.chunks[].offset` and - // serves as a key to address specific Chunk. (offsets always unique per OSF file) - offset:uint64; - // type of messages present in a chunk - stream_id:uint32; - // number of messages in a chunk - message_count:uint32; -} - -// If StreamingInfo is present in metadata it marks that chunks were stored in a -// particular way and the file can be readily used for streaming messages data for -// visualization (web-viz/Data-App etc.) with a guaranteed order by timestamp. - -table StreamingInfo { - // chunk information that describes the message/streams per chunk - chunks:[ChunkInfo]; - // stream statistics per stream for the whole OSF file - stream_stats:[StreamStats]; -} - -// MetadataEntry.type: ouster/v1/streaming/StreamingInfo -root_type StreamingInfo; \ No newline at end of file diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/basics.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/basics.h deleted file mode 100644 index 5899f37d..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/basics.h +++ /dev/null @@ -1,183 +0,0 @@ -/** - * Copyright (c) 2021, Ouster, Inc. - * All rights reserved. - * - * @file basics.h - * @brief basic functions for OSF - * - */ -#pragma once - -#include "chunk_generated.h" -#include "header_generated.h" -#include "metadata_generated.h" -#include "ouster/lidar_scan.h" -#include "ouster/types.h" - -// OSF basic types for LidarSensor and LidarScan/Imu Streams -#include "os_sensor/lidar_scan_stream_generated.h" -#include "os_sensor/lidar_sensor_generated.h" - -namespace ouster { - -/** - * %OSF v2 space - */ -namespace osf { - -// current fb generated code in ouster::osf::gen -namespace gen { -using namespace v2; -} - -/** - * Enumerator for the OSF Version. This will change whenever the underlying - * flatbuffer structures change. - */ -enum OSF_VERSION { - V_INVALID = 0, ///< Invalid OSF Version - V_1_0, ///< Original version of the OSF (2019/9/16) - V_1_1, ///< Add gps/imu/car trajectory to the OSF (2019/11/14) - V_1_2, ///< Change gps_waypoint type to Table in order to - ///< support Python language generator - V_1_3, ///< Add extension for Message in osfChunk - ///< and for Session in osfSession (2020/03/18) - V_1_4, ///< Gen2/128 support (2020/08/11) - - V_2_0 = 20 ///< Second Generation OSF v2 -}; - -/** - * Chunking strategies. Refer to RFC0018 for more details. - */ -enum ChunksLayout { - LAYOUT_STANDARD = 0, ///< not used currently - LAYOUT_STREAMING = 1 ///< default layout (the only one for a user) -}; - -/** - * To String Functionality For ChunksLayout - * - * @param[in] chunks_layout The data to get the string representation format - * @return The string representation - */ -std::string to_string(ChunksLayout chunks_layout); - -/** - * From String Conversion Functionality To ChunksLayout - * - * @param[in] s The String Representation of ChunksLayout - * @return The corrosponding ChunksLayout object - */ -ChunksLayout chunks_layout_of_string(const std::string& s); - -// stable common types mapped to ouster::osf -using v2::HEADER_STATUS; - -/** - * Common timestamp for all time in ouster::osf. - * Nanoseconds were chosen due to the data coming off of the sensor. - */ -using ts_t = std::chrono::nanoseconds; - -/** - * Standard Flatbuffers prefix size - * @todo [pb]: Rename this beast? - */ -static constexpr uint32_t FLATBUFFERS_PREFIX_LENGTH = 4; - -/** - * To String Functionality For HEADER_STATUS - * - * @param[in] status The data to get the string representation format - * @return The string representation - */ -std::string to_string(const HEADER_STATUS status); - -/** - * Debug method to get hex buf values in string - * - * @param[in] buf The buffer to dump to string. - * @param[in] count The size of the buffer. - * @param[in] max_show_count The number of bytes to dump. This arg is optional. - * @return The string representation - */ -std::string to_string(const uint8_t* buf, const size_t count, - const size_t max_show_count = 0); - -/** - * Internal method for reading a file and returning the text - * data. - * - * @param[in] filename The file to read. - * @return The text of the file specified. - */ -std::string read_text_file(const std::string& filename); - -/** - * Reads the prefix size of the Flatbuffers buffer. First 4 bytes. - * - * @param[in] buf Pointer to Flatbuffers buffer stared with prefixed size - * @return the size recovered from the stored prefix size - */ -uint32_t get_prefixed_size(const uint8_t* buf); - -/** - * Calculates the full size of the block (prefixed_size + size + CRC32). - * - * @param[in] buf Pointer to Flatbuffers buffer stared with prefixed size - * @return the calculated size of the block - */ -uint32_t get_block_size(const uint8_t* buf); - -/** - * Check the prefixed size buffer CRC32 fields. - * - * @param[in] buf Structured as size prefixed Flatbuffer buffer, i.e. first - * 4 bytes is the size of the buffer (excluding 4 bytes of the - * size), and the 4 bytes that follows right after the - * 4 + [prefixed_size] is the CRC32 bytes. - * @param[in] max_size Total number of bytes that can be accessed in the buffer, - * as a safety precaution if buffer is not well formed, or - * if first prefixed size bytes are broken. - * @return true if CRC field is correct, false otherwise - */ -bool check_prefixed_size_block_crc( - const uint8_t* buf, - const uint32_t max_size = std::numeric_limits::max()); - -/** @defgroup OsfBatchingFunctions Osf Batching Functions. */ - -/** - * Makes the closure to batch lidar_packets and emit LidarScan object. - * Result returned through callback handler(ts, LidarScan). - * LidarScan uses user modified field types - * - * @ingroup OsfBatchingFunctions - * - * @param[in] info The sensor metadata to use. - * @param[in] ls_field_types The field types to use. - * @param[in] handler The callback to use on the results. - * @return Closure to batch and emit LidarScan objects. - */ -std::function make_build_ls( - const ouster::sensor::sensor_info& info, - const LidarScanFieldTypes& ls_field_types, - std::function handler); - -/** - * The above make_build_ls() function overload. In this function, LidarScan - * uses default field types by the profile - * - * @ingroup OsfBatchingFunctions - * - * @param[in] info The sensor metadata to use. - * @param[in] handler The callback to use on the results. - * @return Closure to batch and emit LidarScan objects. - */ -std::function make_build_ls( - const ouster::sensor::sensor_info& info, - std::function handler); - -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/crc32.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/crc32.h deleted file mode 100644 index 45367b03..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/crc32.h +++ /dev/null @@ -1,50 +0,0 @@ -/** - * Copyright (c) 2021, Ouster, Inc. - * All rights reserved. - * - * @file crc32.h - * @brief crc32 utility - * - */ -#pragma once - -#include - -#include - -namespace ouster { -namespace osf { - -/** - * Size of the CRC field in a buffer - */ -const uint32_t CRC_BYTES_SIZE = 4; - -/** @defgroup OsfCRCFunctions Osf CRC Functions. */ - -/** - * Caclulate CRC value for the buffer of given size. (ZLIB version) - * - * @ingroup OsfCRCFunctions - * - * @param[in] buf Pointer to the data buffer. - * @param[in] size Size of the buffer in bytes. - * @return CRC32 value - */ -uint32_t crc32(const uint8_t* buf, uint32_t size); - -/** - * Caclulate and append CRC value for the buffer of given size and append - * it to the initial crc value. (ZLIB version) - * - * @ingroup OsfCRCFunctions - * - * @param[in] initial_crc Initial crc value to append to. - * @param[in] buf Pointer to the data buffer. - * @param[in] size Size of the buffer in bytes. - * @return CRC32 value - */ -uint32_t crc32(uint32_t initial_crc, const uint8_t* buf, uint32_t size); - -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/file.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/file.h deleted file mode 100644 index 67b112e7..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/file.h +++ /dev/null @@ -1,326 +0,0 @@ -/** - * Copyright (c) 2021, Ouster, Inc. - * All rights reserved. - * - * @file file.h - * @brief common OSF file resource for Reader and Writer operations - * - */ -#pragma once - -#include -#include - -#include "ouster/osf/basics.h" - -namespace ouster { -namespace osf { - -/** - * Enum representing the available file opening modes. - */ -enum class OpenMode : uint8_t { - READ = 0, ///< Open the file in read-only mode. - WRITE = 1 ///< Open the file in write-only mode. (CURRENTLY NOT SUPPORTED) -}; - -/** - * Enum representing the state of the %OSF file. - */ -enum class FileState : uint8_t { - GOOD = 0, ///< The file is good. - BAD = 1 ///< There is something wrong with the file. -}; - -/** - * Chunk buffer type to store raw byte buffers of data. - */ -using ChunkBuffer = std::vector; - -/** - * Interface to abstract the way of how we handle file system read/write - * operations. - */ -class OsfFile { - public: - /** - * Default constructor, sets most data to nullptr and 0. - */ - explicit OsfFile(); - - /** - * Opens the OSF file. - * @note Only OpenMode::READ is supported - * - * @param[in] filename The OSF file to open - * @param[in] mode The mode to open the file in, this argument is optional. - */ - explicit OsfFile(const std::string& filename, - OpenMode mode = OpenMode::READ); - - /** - * Cleans up any filebuffers/memory mapping. - */ - ~OsfFile(); - - // Header Info - - /** - * Returns the size of the OSF file. - * - * @return The size of the OSF file in bytes. - */ - uint64_t size() const; - - /** - * Returns the filename of the open OSF file. - * - * @return The filename of the open OSF file. - */ - std::string filename() const; - - /** - * Returns the version of the OSF file. - * - * @return The version of the OSF file. - */ - OSF_VERSION version(); - - /** - * Returns the offset in the OSF file where the - * metadata section is located. - * - * @throws std::logic_error Exception on bad osf file. - * - * @return Offset to the metadata in bytes - */ - uint64_t metadata_offset(); - - /** - * Returns the offset in the OSF file where the - * chunk section is located. - * - * @throws std::logic_error Exception on bad osf file. - * - * @return Offset to the chunks in bytes - */ - uint64_t chunks_offset(); - - /** - * Checks the validity of header and session/file_info blocks. - * - * @return If the header, session, and file_info blocks are valid. - */ - bool valid(); - - /** - * Return the status of the OSF file. - * @todo Need to have more states here (eod, valid, error, etc) - * - * @return If the OSF file is good or not. - */ - bool good() const; - - // Convenience operators - /** - * Return the negated status of the OSF file. - * - * @relates good - * - * @return If the OSF file is good or not, negated. - */ - bool operator!() const; - - /** - * Return the status of the OSF file. - * - * @relates good - * - * @return If the OSF file is good or not. - */ - explicit operator bool() const; - - /** - * Get the current offset in the OSF file. - * - * @return The current offset in the OSF file. - */ - uint64_t offset() const; - - /** - * File seek (in mmap mode it's just moving the offset_ pointer - * without any file system opeations.) - * - * @throws std::logic_error Exception on bad osf file. - * @throws std::out_of_range Exception on out of range seek. - * - * @param[in] pos position in the file - * @return A reference to `this` object. - */ - OsfFile& seek(uint64_t pos); - - /** - * Read from file (in current mmap mode it's copying data from - * mmap address to the 'buf' address). - * - * @todo Handle errors in future and get the way to read them back - * with FileState etc. - * - * @throws std::logic_error Exception on bad osf file. - * @throws std::out_of_range Exception on out of range read. - * - * @param[out] buf The buffer to write to. - * @param[in] count The number of bytes to write to buf. - * @return A reference to `this` object. - */ - OsfFile& read(uint8_t* buf, const uint64_t count); - - /** - * Returns whether the OSF file is memory mapped or not. - * - * @return Is the OSF file memory mapped or not. - */ - bool is_memory_mapped() const; - - /** - * Mmap access to the file content with the specified offset from the - * beginning of the file. - * - * @throws std::logic_error Exception on bad osf file. - * @throws std::logic_error Exception not being memory mapped. - * @throws std::out_of_range Exception on out of range read. - * - * @param[in] offset The specified offset to access into the OSF file, this - * argument is optional. - * @return The pointer to the OSF file. - */ - const uint8_t* buf(const uint64_t offset = 0) const; - - /** - * Clears file handle and allocated resources. In current mmap - * implementation it's unmapping memory and essentially invalidates the - * memory addresses that might be captured within MessageRefs - * and Reader. - * @sa ouster::osf::MessageRef, ouster::osf::Reader - */ - void close(); - - /** - * Debug helper method to dump OsfFile state to a string. - * - * @return The string representation of OsfFile - */ - std::string to_string(); - - /** - * Copy policy: - * Don't allow the copying of the file handler - */ - OsfFile(const OsfFile&) = delete; - - /** - * @copydoc OsfFile::OsfFile(const OsfFile&) - */ - OsfFile& operator=(const OsfFile&) = delete; - - /** - * Move policy: - * Allow transferring ownership of the underlying file - * handler (mmap). - */ - OsfFile(OsfFile&& other); - - /** - * @copydoc OsfFile::OsfFile(OsfFile&& other) - */ - OsfFile& operator=(OsfFile&& other); - - /** - * Read chunk specified at offset. - * - * @throws std::out_of_range Exception on out of range read. - * - * @param[in] offset The offset to read the chunk from. - * @return Shared pointer to the chunk. nullptr if osf file is bad - */ - std::shared_ptr read_chunk(uint64_t offset); - - /** - * Get a pointer to the start of the header chunk. - * - * @return Pointer to the header chunk. nullptr if filestream is bad. - */ - uint8_t* get_header_chunk_ptr(); - - /** - * Get a pointer to the start of the header chunk. - * - * @return Pointer to the metadata chunk. nullptr if filestream is bad. - */ - uint8_t* get_metadata_chunk_ptr(); - - private: - /** - * Convenience method to set error and print it's content. - * - * @todo [pb] Adding more error states will probably extend the set of this - * function. - * - * @param[in] msg Message to print - */ - void error(const std::string& msg = std::string()); - - /** - * Opened filename as it was passed in contructor. - */ - std::string filename_; - - /** - * Current offset to the file. (not used in mmaped implementation) but used - * for copying(reading) blocks of memory from the file to the specified - * memory. - */ - uint64_t offset_; - - /** - * Size of the opened file in bytes. - */ - uint64_t size_; - - /** - * Mmaped memory address pointed to the beginning of the file (byte 0) - */ - uint8_t* file_buf_; - - /** - * File stream for reading. - */ - std::ifstream file_stream_; - - /** - * Pointer for the osf file header chunk. - */ - std::shared_ptr header_chunk_; - - /** - * Pointer for the metadata chunk - */ - std::shared_ptr metadata_chunk_; - - /** - * Last read chunk cached, to save the double read on the sequence of verify - * and then read iterator access (used only when compiled with - * OUSTER_OSF_NO_MMAP, and in mmap version we rely on the OS/kernel caching) - */ - std::shared_ptr chunk_cache_; - - uint64_t chunk_cache_offset_; - - /** - * Internal state of the OSF file. - */ - FileState state_; -}; - -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/layout_streaming.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/layout_streaming.h deleted file mode 100644 index 904f6506..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/layout_streaming.h +++ /dev/null @@ -1,121 +0,0 @@ -/** - * Copyright (c) 2021, Ouster, Inc. - * All rights reserved. - * - * @file layout_streaming.h - * @brief OSF Streaming Layout - * - */ -#pragma once - -#include "ouster/osf/meta_streaming_info.h" -#include "ouster/osf/writer.h" - -namespace ouster { -namespace osf { - -/** @defgroup OSFStreamingDefaultSize OSF Streaming Default Size. */ - -/** - * Default Streaming Chunk Size. - * This is used in StreamingLayoutCW - * - * @ingroup OSFStreamingDefaultSize - * @relates StreamingLayoutCW - */ -constexpr uint32_t STREAMING_DEFAULT_CHUNK_SIZE = - 2 * 1024 * 1024; // not strict ... - -/** - * Sreaming Layout chunking strategy - * - * TODO[pb]: sorting TBD as in RFC0018 but first pass should be good enough - * because usually messages from the same source and of the same type comes - * sorted from pcap/bag sources. - * - * When messages laid out into chunks in an ordered as they come (not full - * RFC0018 compliant, see TODO below), with every chunk holding messages - * exclusively of a single stream_id. Tries not to exceede `chunk_size` (if - * possible). However if a single message size is bigger than specified - * `chunk_size` it's still recorded. - */ -class StreamingLayoutCW : public ChunksWriter { - public: - /** - * @param[in] writer Writer object for use when writing messages - * @param[in] chunk_size The chunk size to use, this arg is optional. - */ - StreamingLayoutCW(Writer& writer, - uint32_t chunk_size = STREAMING_DEFAULT_CHUNK_SIZE); - - /** - * @copydoc ChunksWriter::save_message - * - * @throws std::logic_error Exception on inconsistent timestamps. - */ - void save_message(const uint32_t stream_id, const ts_t ts, - const std::vector& buf) override; - - /** - * @copydoc ChunksWriter::finish - */ - void finish() override; - - /** - * @copydoc ChunksWriter::chunk_size - */ - uint32_t chunk_size() const override; - - private: - /** - * Internal method to calculate and append the stats - * for a specific set of new messages. - * - * @param[in] stream_id The stream id to associate with the message. - * @param[in] ts The timestamp for the messages. - * @param[in] msg_buf A vector of message buffers to gather stats about. - */ - void stats_message(const uint32_t stream_id, const ts_t ts, - const std::vector& msg_buf); - - /** - * Finish out a chunk and write the chunk to the writer. - * - * @param[in] stream_id The stream id finish up. - * @param[in] chunk_builder The chunk builder to use for formulating the - * chunk. - */ - void finish_chunk(uint32_t stream_id, - const std::shared_ptr& chunk_builder); - - /** - * Chunk size to use for writing. - */ - const uint32_t chunk_size_; - - /** - * Per stream_id chunk builders. - * Map Format: - */ - std::map> chunk_builders_{}; - - /** - * Vector pairs for chunk info/stream_id - * Pair Format: - */ - std::vector> chunk_stream_id_{}; - - /** - * Per stream_id stats. - * Map Format: - */ - std::map stream_stats_{}; - - /** - * Internal writer object to use for writing. - */ - Writer& writer_; -}; - -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/meta_extrinsics.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/meta_extrinsics.h deleted file mode 100644 index 8f896064..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/meta_extrinsics.h +++ /dev/null @@ -1,136 +0,0 @@ -/** - * Copyright (c) 2021, Ouster, Inc. - * All rights reserved. - * - * @file meta_extrinsics.h - * @brief Metadata entry Extrinsics - * - */ -#pragma once - -#include -#include - -#include "ouster/osf/metadata.h" -#include "ouster/types.h" - -namespace ouster { -namespace osf { - -/** - * Metadata entry to store sensor Extrinsics. - * - * OSF type: - * ouster/v1/os_sensor/Extrinsics - * - * Flat Buffer Reference: - * fb/os_sensor/extrinsics.fbs - */ -class Extrinsics : public MetadataEntryHelper { - public: - /** - * @param[in] extrinsics ///< The extrinsic matrix to store - * ///< mat4d - 4x4 homogeneous transform - * @param[in] ref_meta_id The flat buffer metadata(not sensor_info) - * reference id - * @param[in] name ///< Named id if needed, to support multiple extrinsics - * ///< perobject (i.e. LidarSensor, or Gps) with name - * ///< maybe usedto associate extrinsics to some external - * ///< system of records or just name the source - * ///< originator of the extrinsics information. - */ - explicit Extrinsics(const mat4d& extrinsics, uint32_t ref_meta_id = 0, - const std::string& name = ""); - - /** - * Get the extrinsics matrix. - * - * @return The eigen extrinsics matrix. - */ - const mat4d& extrinsics() const; - - /** - * Get the extrinsics name. - * - * @return The extrinsics name. - */ - const std::string& name() const; - - /** - * Get the reference metadata id. - * - * @return The reference metadata id. - */ - uint32_t ref_meta_id() const; - - /** - * @copydoc MetadataEntry::buffer - */ - std::vector buffer() const final; - - /** - * Create an Extrinsics object from a byte array. - * - * @todo Figure out why this wasnt just done as a constructor overload. - * - * @relates MetadataEntry::from_buffer - * - * @param[in] buf The byte vector to construct an Extrinsics object from. - * @return The new Extrinsics cast as a MetadataEntry - */ - static std::unique_ptr from_buffer( - const std::vector& buf); - - /** - * Get the string representation for the Extrinsics object. - * - * @relates MetadataEntry::repr - * - * @return The string representation for the Extrinsics object. - */ - std::string repr() const override; - - private: - /** - * The internal extrinsics array. - * - * Flat Buffer Reference: - * fb/os_sensor/extrinsics.fbs :: Extrinsics :: extrinsics - */ - mat4d extrinsics_; - - /** - * The internal flatbuffer metadata reference id. - * - * Flat Buffer Reference: - * fb/os_sensor/extrinsics.fbs :: Extrinsics :: ref_id - */ - uint32_t ref_meta_id_; - - /** - * The internal name for the extrinsics array. - * - * Flat Buffer Reference: fb/os_sensor/extrinsics.fbs :: Extrinsics :: name - */ - std::string name_; -}; - -/** @defgroup OSFTraitsExtrinsics OSF Templated traits struct. */ - -/** - * Templated struct for returning the OSF type string. - * - * @ingroup OSFTraitsExtrinsics - */ -template <> -struct MetadataTraits { - /** - * Return the OSF type string. - * - * @return The OSF type string "ouster/v1/os_sensor/Extrinsics". - */ - static const std::string type() { return "ouster/v1/os_sensor/Extrinsics"; } -}; - -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/meta_lidar_sensor.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/meta_lidar_sensor.h deleted file mode 100644 index 698929d1..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/meta_lidar_sensor.h +++ /dev/null @@ -1,128 +0,0 @@ -/** - * Copyright (c) 2021, Ouster, Inc. - * All rights reserved. - * - * @file meta_lidar_sensor.h - * @brief Metadata entry LidarSensor - * - */ -#pragma once - -#include -#include - -#include "ouster/osf/metadata.h" -#include "ouster/types.h" - -namespace ouster { -namespace osf { - -/** - * Metadata entry to store lidar sensor_info, i.e. Ouster sensor configuration. - * - * OSF type: - * ouster/v1/os_sensor/LidarSensor - * - * Flat Buffer Reference: - * fb/os_sensor/lidar_sensor.fbs - */ -class LidarSensor : public MetadataEntryHelper { - using sensor_info = ouster::sensor::sensor_info; - - public: - /** - * @param[in] si Initialize the LidarSensor with a sensor_info object. - */ - explicit LidarSensor(const sensor_info& si); - - /** - * @param[in] sensor_metadata Initialize the LidarSensor with a json string - * representation of the sensor_info object. - */ - explicit LidarSensor(const std::string& sensor_metadata); - - /** - * Returns the sensor_info associated with the LidarSensor. - * - * @return The sensor_info associated with the LidarSensor. - */ - const sensor_info& info() const; - - /** - * Returns the json string representation sensor_info associated - * with the LidarSensor. - * - * @return ///< The json string representation of the - * ///< sensor_info object. - */ - const std::string& metadata() const; - - /** - * @copydoc MetadataEntry::buffer - */ - std::vector buffer() const final; - - /** - * Create a LidarSensor object from a byte array. - * - * @todo Figure out why this wasnt just done as a constructor overload. - * - * @relates MetadataEntry::from_buffer - * - * @param[in] buf The raw flatbuffer byte vector to initialize from. - * @return The new LidarSensor cast as a MetadataEntry - */ - static std::unique_ptr from_buffer( - const std::vector& buf); - - /** - * Get the string representation for the LidarSensor object. - * - * @relates MetadataEntry::repr - * - * @return The string representation for the LidarSensor object. - */ - std::string repr() const override; - - /** - * @todo Figure out why we have both repr and to_string - * - * @relates MetadataEntry::to_string - * - * @copydoc LidarSensor::repr - */ - std::string to_string() const override; - - private: - /** - * The internal sensor_info object. - */ - sensor_info sensor_info_; - - /** - * The internal json string representation of the sensor_info object. - */ - const std::string metadata_; -}; - -/** @defgroup OSFTraitsLidarSensor Templated struct for traits */ - -/** - * Templated struct for returning the OSF type string. - * - * @ingroup OSFTraitsLidarSensor - */ -template <> -struct MetadataTraits { - /** - * Return the OSF type string. - * - * @return The OSF type string "ouster/v1/os_sensor/LidarSensor". - */ - static const std::string type() { - return "ouster/v1/os_sensor/LidarSensor"; - } -}; - -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/meta_streaming_info.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/meta_streaming_info.h deleted file mode 100644 index cb92cd84..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/meta_streaming_info.h +++ /dev/null @@ -1,250 +0,0 @@ -/** - * Copyright (c) 2021, Ouster, Inc. - * All rights reserved. - * - * @file meta_streaming_info.h - * @brief Metadata entry StreamingInfo - * - */ -#pragma once - -#include -#include - -#include "ouster/osf/metadata.h" -#include "ouster/types.h" - -namespace ouster { -namespace osf { - -/** - * Class for keeping track of OSF chunks. - * - * Flat Buffer Reference: - * fb/streaming/streaming_info.fbs :: ChunkInfo - */ -struct ChunkInfo { - /** - * The offset in the flatbuffer where - * the chunk is located. - * - * Flat Buffer Reference: - * fb/streaming/streaming_info.fbs :: ChunkInfo :: offset - */ - uint64_t offset; - - /** - * The specific stream the chunk is associated with. - * - * Flat Buffer Reference: - * fb/streaming/streaming_info.fbs :: ChunkInfo :: stream_id - */ - uint32_t stream_id; - - /** - * The number of messages in the chunk - * - * Flat Buffer Reference: - * fb/streaming/streaming_info.fbs :: ChunkInfo :: message_count - */ - uint32_t message_count; -}; - -/** - * Class for keeping track of OSF stream stats. - * - * Flat Buffer Reference: - * fb/streaming/streaming_info.fbs :: StreamStats - */ -struct StreamStats { - /** - * The specific stream the chunk is associated with. - * - * Flat Buffer Reference: - * fb/streaming/streaming_info.fbs :: StreamStats :: stream_id - */ - uint32_t stream_id; - - /** - * The first timestamp in the stream. - * - * Flat Buffer Reference: - * fb/streaming/streaming_info.fbs :: StreamStats :: start_ts - */ - ts_t start_ts; - - /** - * The last timestamp in the stream. - * - * Flat Buffer Reference: - * fb/streaming/streaming_info.fbs :: StreamStats :: end_ts - */ - ts_t end_ts; - - /** - * The number of messages in the stream. - * - * Flat Buffer Reference: - * fb/streaming/streaming_info.fbs :: StreamStats :: message_count - */ - uint64_t message_count; - - /** - * The average size of the messages in the stream. - * - * Flat Buffer Reference: - * fb/streaming/streaming_info.fbs :: StreamStats :: message_avg_size - */ - uint32_t message_avg_size; - - /** - * Default constructor, sets everthing to 0. - */ - StreamStats() = default; - - /** - * Construct a StreamStats with the specified values - * - * @param[in] s_id Specify the stream_id to use. - * @param[in] t Set the start and end timestamps to the specified value. - * @param[in] msg_size Set the average message size to the specified value. - */ - StreamStats(uint32_t s_id, ts_t t, uint32_t msg_size); - - /** - * Update values within the StreamStats - * - * @param[in] t Add another timestamp and calculate the start and end - * values. - * @param[in] msg_size Add another message size and calculate the average. - */ - void update(ts_t t, uint32_t msg_size); -}; - -/** - * Get the string representation for a ChunkInfo object. - * - * @return The string representation for a ChunkInfo object. - */ -std::string to_string(const ChunkInfo& chunk_info); - -/** - * Get the string representation for a StreamStats object. - * - * @return The string representation for a StreamStats object. - */ -std::string to_string(const StreamStats& stream_stats); - -/** - * Metadata entry to store StreamingInfo, to support StreamingLayout (RFC 0018) - * - * OSF type: - * ouster/v1/streaming/StreamingInfo - * - * Flat Buffer Reference: - * fb/streaming/streaming_info.fbs :: StreamingInfo - */ -class StreamingInfo : public MetadataEntryHelper { - public: - StreamingInfo() {} - - /** - * @param[in] chunks_info Vector containing pairs of - * stream_id/ChunkInfo - * to be used to generate a stream_id/ChunkInfo - * map. - * @param[in] stream_stats Vector containing pairs of - * stream_id/StreamStats - * to be used to generate a - * stream_id/StreamStats map. - */ - StreamingInfo( - const std::vector>& chunks_info, - const std::vector>& stream_stats); - - /** - * @param[in] chunks_info ///< Map containing stream_id/ChunkInfo data. - * @param[in] stream_stats ///< Map containing stream_id/StreamStats data. - */ - StreamingInfo(const std::map& chunks_info, - const std::map& stream_stats); - - /** - * Return the chunk_info map. stream_id/ChunkInfo data. - * - * @return The chunk_info map. stream_id/ChunkInfo data. - */ - std::map& chunks_info(); - - /** - * Return the stream stat map. stream_id/StreamStats data. - * - * @return The stream stat map. stream_id/StreamStats data. - */ - std::map& stream_stats(); - - /** - * @copydoc MetadataEntry::buffer - */ - std::vector buffer() const override final; - - /** - * Create a StreamingInfo object from a byte array. - * - * @todo Figure out why this wasnt just done as a constructor overload. - * - * @relates MetadataEntry::from_buffer - * - * @param[in] buf The raw flatbuffer byte vector to initialize from. - * @return The new StreamingInfo cast as a MetadataEntry - */ - static std::unique_ptr from_buffer( - const std::vector& buf); - - /** - * Get the string representation for the LidarSensor object. - * - * @relates MetadataEntry::repr - * - * @return The string representation for the LidarSensor object. - */ - std::string repr() const override; - - private: - /** - * The internal stream_id to ChunkInfo map. - * - * Flat Buffer Reference: - * fb/streaming/streaming_info.fbs :: StreamingInfo :: chunks - */ - std::map chunks_info_{}; - - /** - * The internal stream_id to StreamStats map. - * - * Flat Buffer Reference: - * fb/streaming/streaming_info.fbs :: StreamingInfo :: stream_stats - */ - std::map stream_stats_{}; -}; - -/** @defgroup OSFTraitsStreamingInfo Templated struct for traits. */ -/** - * Templated struct for returning the OSF type string. - * - * @ingroup OSFTraitsStreamingInfo - */ -template <> -struct MetadataTraits { - /** - * Return the OSF type string. - * - * @return The OSF type string "ouster/v1/streaming/StreamingInfo". - */ - static const std::string type() { - return "ouster/v1/streaming/StreamingInfo"; - } -}; - -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/metadata.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/metadata.h deleted file mode 100644 index c93a3bb8..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/metadata.h +++ /dev/null @@ -1,685 +0,0 @@ -/** - * Copyright (c) 2021, Ouster, Inc. - * All rights reserved. - * - * @file metadata.h - * @brief Core MetadataEntry class with meta store, registry etc. - * - */ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "flatbuffers/flatbuffers.h" -#include "ouster/osf/basics.h" - -/// @todo fix api docs in this file -/// @todo add equality operators -namespace ouster { -namespace osf { - -/** - * Need to be specialized for every derived MetadataEntry class that can be - * stored/recovered as metadata object. - * - * @sa metadata_type(), MetadataEntry - * - * @tparam MetadataDerived The derived subclass cpp type. - */ -template -struct MetadataTraits { - /** - * Default type returning nullptr. - * - * @todo Possible undefined behavior here. - * - * @returns nullptr - */ - static const std::string type() { return nullptr; } -}; - -/** - * Helper function that returns the MetadataEntry type of concrete metadata. - * - * @tparam MetadataDerived The derived subclass cpp type. - */ -template -inline const std::string metadata_type() { - typedef typename std::remove_const::type no_const_type; - typedef typename std::remove_reference::type no_cvref_type; - typedef typename std::remove_pointer::type almost_pure_type; - typedef typename std::remove_const::type pure_type_M; - return MetadataTraits::type(); -} - -/** - * Base abstract metadata entry type for every metadata that can be stored as - * OSF metadata. - * - * Metadata object that is stored/serialized to OSF is a triplet: - * `{id, type, buffer}` - * - * `id` - is a unique identifier per OSF file and used for references from other - * metadata objects or from messages (chunk.StampedMessage.id in chunk.fbs) - * to link messages with the streams. - * - * `type` - string that is unique per OSF generation (i.e. v2) and used to link - * datum buffer representation to the concrete metadata object. - * - * Type is specified when concrete metadata type class defined via - * MetadataTraits struct specialization, example: - * - * @code{.cpp} - * template <> - * struct MetadataTraits { - * static const std::string type() { - * return "ouster/v1/something/MyMeta"; - * } - * }; - * @endcode - * - * `buffer` - byte representation of the metadata content whatever it is defined - * by concrete metadata type. Every metadata object should have a recipe how - * to serialize itself to the bytes buffer by overwriting the buffer() function. - * And the recipe how to recover itserf by providing static - * from_buffer(buf, type) function. - * - */ -class MetadataEntry { - public: - /** - * Function type to recover metadata object from buffer. - */ - using from_buffer_func = - std::unique_ptr (*)(const std::vector&); - - /** - * @return Type of the metadata, used to identify the object type in - * serialized OSF and as key in deserialization registry - */ - virtual std::string type() const = 0; - - /** - * @return Same as type with the difference that type() can be dynamic and - * static_type() should always be defined in compile time. - * NOTE: Introduced as a convenience/(HACK?) to simpler reconstruct - * and cast dynamic objects from MetadataEntryRef - */ - virtual std::string static_type() const = 0; - - /** - * Should be provided by derived class and is used in handling polymorphic - * objects and avoid object slicing - * - * @return Should return a clone of the current MetadataEntry - */ - virtual std::unique_ptr clone() const = 0; - - /** - * Byte represantation of the internal derived metadata type, used as - * serialization function when saving to OSF file. - * - * @return The byte vector representation of the metadata. - */ - virtual std::vector buffer() const = 0; - - /** - * Recover metadata object from the bytes representation if possible. - * If recovery is not possible returns nullptr - * - * @param[in] buf The buffer to recover the metadata object from. - * @param[in] type_str The type string from the derived type. - * @return A new object of the derived type cast as a MetadataEntry - */ - static std::unique_ptr from_buffer( - const std::vector& buf, const std::string type_str); - - /** - * String representation of the internal metadata object, used in - * to_string() for debug/info outputs. - * - * @return The string representation for the internal metadata object. - */ - virtual std::string repr() const; - - /** - * String representation of the whole metadata entry with type and id. - * - * @todo Figure out why we have both repr and to_string - * - * @return The string representation of the whole metadata entry. - */ - virtual std::string to_string() const; - - /** - * Unique id used inside the flatbuffer metadata store to refer to - * metadata entries. - * - * @param[in] id The unique id to set. - */ - void setId(uint32_t id); - - /** - * Unique id used inside the flatbuffer metadata store to refer to - * metadata entries. - * - * @relates setId - * - * @return The unique id of this object. - */ - uint32_t id() const; - - /** - * Casting of the base class to concrete derived metadata entry type. - * Always creates new object with allocation via clone() if the pointer/ref - * is a polymorphic object, or as reconstruction from buffer() - * representation when it used from MetadataEntryRef (i.e. wrapper on - * underlying bytes) - * - * @tparam T The derived metadata type - * @return A unique pointer to the derived metadata object, nullptr on - * error. - */ - template - std::unique_ptr as() const { - if (type() == metadata_type()) { - std::unique_ptr m; - if (type() == static_type()) { - m = clone(); - } else { - m = T::from_buffer(buffer()); - } - if (m != nullptr) { - // Verify the casting - T& test = dynamic_cast(*m); - (void)test; - - m->setId(id()); - // NOTE: Little bit crazy unique_ptr cast (not absolutely - // correct because of no deleter handled). But works - // for our case because we don't mess with it. - return std::unique_ptr(dynamic_cast(m.release())); - } - } - return nullptr; - } - - /** - * Implementation details that emits buffer() content as proper - * Flatbuffer MetadataEntry object. - * - * @param[in] fbb The flatbuffer builder to use to make the entry. - * @return An offset into a flatbuffer for the new entry. - */ - flatbuffers::Offset make_entry( - flatbuffers::FlatBufferBuilder& fbb) const; - - /** - * Method to return the registry that holds from_buffer function by - * type string and is used during deserialization. The registry is - * a static variable defined within the get_registry method. - * - * @return The static registry used to register metadata types. - */ - static std::map& get_registry(); - - virtual ~MetadataEntry() = default; - - private: - /** - * Id as its stored in metadata OSF and used for linking between other - * metadata object and messages to streams. - */ - uint32_t id_{0}; -}; - -/** - * Safe and convenient cast of shared_ptr to concrete derived - * class using either shortcut (dynamic_pointer_cast) when it's save to do so - * or reconstructs a new copy of the object from underlying data. - * - * @tparam MetadataDerived The cpp type of the derived object. - * @tparam MetadataBase The cpp type of the metadata base. - * @param[in] m The MetadataBase to convert to MetadataDerived. - * @return The MetadataBase cast as a MetadataDerived pointer. - */ -template -std::shared_ptr metadata_pointer_as( - const std::shared_ptr& m) { - if (m->type() != metadata_type()) return nullptr; - if (m->type() == m->static_type()) { - return std::dynamic_pointer_cast(m); - } else { - return m->template as(); - } -}; - -/** - * Registrar class helper to add static from_buffer() function of the concrete - * derived metadata class to the registry. - * - * @dot - * digraph { - * subgraph cluster_SpecificMetadataClass { - * SpecificMetadataClass [ - * label="class SpecificMetadataClass", - * shape="rectangle"]; - * SpecificMetadataClassType [ - * label="struct MetadataTraits", - * shape="rectangle" - * ]; - * - * SpecificMetadataClass -> SpecificMetadataClassType; - * } - * - * MetadataEntryHelper [ - * label="class MetadataEntryHelper", - * shape="rectangle"]; - * MetadataTraits [ - * label="struct MetadataTraits", - * shape="rectangle"]; - * - * SpecificMetadataClass -> MetadataEntryHelper; - * SpecificMetadataClassType -> MetadataTraits; - * - * MetadataEntry [ - * label="MetadataEntry", - * shape="rectangle"]; - * MetadataEntryRef [ - * label="MetadataEntryRef", - * shape="rectangle"]; - * - * MetadataEntry -> MetadataEntryRef; - * - * subgraph cluster_RegisterMetadata { - * RegisterMetadata [ - * label="RegisterMetadata", - * shape="rectangle"]; - * RegisterMetadata_Decoder [ - * label="RegisterMetadata::registered=register_type_decoder()", - * shape="rectangle"]; - * RegisterMetadata->RegisterMetadata_Decoder; - * }; - * - * MetadataEntryHelper -> MetadataEntry; - * MetadataEntryHelper -> RegisterMetadata; - * - * subgraph cluster_MetadataStore { - * MetadataStore [ - * label="MetadataStore", - * shape="rectangle"]; - * MetadataStore_Entries [ - * label="MetadataStore::metadata_entries_", - * shape="rectangle"]; - * MetadataStore->MetadataStore_Entries; - * }; - * - * MetadataEntry -> MetadataStore_Entries; - * } - * @enddot - * - * @tparam MetadataDerived The derived subclass cpp type. - */ -template -struct RegisterMetadata { - virtual ~RegisterMetadata() { - assert(registered_); - - /** - * This line is incredibly IMPORTANT. This line ensures - * that the compiler does not optimize out the side effects - * from the register_type_decoder method. Without this line - * the MetadataEntry registry will be empty. - */ - (void)registered_; - } - - /** - * Register the specific derived class decoder into the global registrar. - * - * @return true If class has been registered successfully, - * false otherwise. - */ - static bool register_type_decoder() { - auto& registry = MetadataEntry::get_registry(); - auto type = metadata_type(); - if (registry.find(type) != registry.end()) { - std::cerr << "ERROR: Duplicate metadata type? Already registered " - "type found: " - << type << std::endl; - return false; - } - registry.insert(std::make_pair(type, MetadataDerived::from_buffer)); - return true; - } - - /** - * If the derived class has been registered. - */ - static const bool registered_; -}; - -/** - * This line is incredibly IMPORTANT. This will statically - * run the registration for all derived classes before the class - * constructer is run. - * - * @tparam MetadataDerived The derived subclass cpp type. - */ -template -const bool RegisterMetadata::registered_ = - RegisterMetadata::register_type_decoder(); - -/** - * Helper class used as base class for concrete derived metadata types - * and provides type()/static_type()/clone() functions as boilerplate. - * - * Also registers the from_buffer() function for deserializer registry via - * RegisterMetadata helper trick. - * - * @tparam DerivedMetadataEntry The derived Metadata Entry type. - */ -template -class MetadataEntryHelper : public MetadataEntry, - RegisterMetadata { - public: - /** - * Return the metadata type string for the specific derived class. - * - * @return The specific type string for the derived class. - */ - std::string type() const override { - return metadata_type(); - } - - /** - * @copydoc type() - */ - std::string static_type() const override { - return metadata_type(); - } - - /** - * Clone the specific derived metadata object. - * - * @return The cloned MetadataEntry object. - */ - std::unique_ptr clone() const override { - return std::make_unique( - *dynamic_cast(this)); - } -}; - -/** - * MetadataEntry wrapper for byte Flatbuffers bytes representation. Used during - * deserialization and acts as regular polymorphic metadata type (almost). - * - * Doesn't own the memory of the underlying buffer. - * - * Reconstructs itself to the concrete metadata type with: - * - * as_type() - using the stored type() to recofer deserialization function - * - * as() OR metadata_pointer_as() - using the - * specified derived metadata class. - */ -class MetadataEntryRef : public MetadataEntry { - public: - /** - * Creates the metadata reference from Flatbuffers v2::MetadataEntry buffer. - * No copy involved. - * - * @param[in] buf The buffer to create the MetadataEntryRef from. - */ - explicit MetadataEntryRef(const uint8_t* buf); - - /** - * Return the type of the MetadataEntry. - * - * @return The type of the MetadataEntry. - */ - std::string type() const override; - - /** - * @copydoc type() - */ - std::string static_type() const override; - - /** - * Clone the MetadataEntry. - * - * @return The cloned MetadataEntry object. - */ - std::unique_ptr clone() const override; - - /** - * Return the raw underlying buffer for the MetadataEntryRef. - * - * @return The raw underlying byte vector. - */ - std::vector buffer() const final; - - /** - * Reconstructs the object as concrete metadata of type() from the - * buffer() using registered deserialization function from_buffer() of - * current type - * - * @return The reconstructed object. - */ - std::unique_ptr as_type() const; - - private: - /** - * Internal method to set the specific metadata entry id. - * - * @param[in] id The metadata id to set. - */ - void setId(uint32_t id); - - /** - * Data pointer to the raw MetadataEntry buffer. - */ - const uint8_t* buf_; - - /** - * Internal variable for storing the metadata type string. - */ - std::string buf_type_{}; -}; - -/** - * Implementation detail for MetadataEntryRef to distinguish it from any - * possible metadata type - */ -template <> -struct MetadataTraits { - /** - * Implementation detail for MetadataEntryRef to distinguish it from - * any possible metadata type. - * - * @return The type string "impl/MetadataEntryRef". - */ - static const std::string type() { return "impl/MetadataEntryRef"; } -}; - -/** - * Collection of metadata entries, used as metadata provider in Reader and - * Writer. - * - * Provide functions to retrieve concrete metadata types by id or by type. - * - * Also can serialize itself to Flatbuffers collection of metadata. - */ -class MetadataStore { - /** - * Metadata id to MetadataEntry map. - */ - using MetadataEntriesMap = - std::map>; - - public: - using key_type = MetadataEntriesMap::key_type; - - /** - * Add a specified MetadataEntry to the store - * - * @param[in] entry The entry to add to the store. - */ - uint32_t add(MetadataEntry&& entry); - - /** - * @copydoc add(MetadataEntry&& entry) - */ - uint32_t add(MetadataEntry& entry); - - /** - * Get the first specified MetadataEntry associated to the - * template parameter. - * - * @tparam MetadataEntryClass The metadata cpp type to try and - * retrieve. - * @return The MetadataEntry of type MetadataEntryClass if it exists. - */ - template - std::shared_ptr get() const { - auto it = metadata_entries_.begin(); - while (it != metadata_entries_.end()) { - if (auto m = metadata_pointer_as(it->second)) { - return m; - } - ++it; - } - return nullptr; - } - - /** - * Count the number of specified MetadataEntry associated to the - * template parameter. - * - * @tparam MetadataEntryClass The metadata cpp type to try and - * count. - * @return The count type MetadataEntryClass. - */ - template - size_t count() const { - auto it = metadata_entries_.begin(); - size_t cnt = 0; - while (it != metadata_entries_.end()) { - if (it->second->type() == metadata_type()) - ++cnt; - ++it; - } - return cnt; - } - - /** - * Get the specified MetadataEntry associated to the - * template parameter and metadata_id. - * - * @tparam MetadataEntryClass The metadata cpp type to try and - * retrieve. - * @param[in] metadata_id The id to try and return the associated entry. - * @return The MetadataEntryClass. - */ - template - std::shared_ptr get(const uint32_t metadata_id) const { - auto meta_entry = get(metadata_id); - return metadata_pointer_as(meta_entry); - } - - /** - * Get the specified MetadataEntry associated to the - * metadata_id. - * - * @param[in] metadata_id The id to try and return the associated entry. - * @return The MetadataEntry. - */ - std::shared_ptr get(const uint32_t metadata_id) const { - auto it = metadata_entries_.find(metadata_id); - if (it == metadata_entries_.end()) return nullptr; - return it->second; - } - - /** - * Return a map containing all of the MetadataEntries that match - * the specified template class. - * - * @tparam MetadataEntryClass The metadata cpp type to try and retrieve. - * @return The MetadataEntry map. - */ - template - std::map> find() const { - std::map> res; - auto it = metadata_entries_.begin(); - while (it != metadata_entries_.end()) { - if (auto m = metadata_pointer_as(it->second)) { - res.insert(std::make_pair(it->first, m)); - } - ++it; - } - return res; - } - - /** - * Return the number of MetadataEntries. - * - * @return The number of MetadataEntry objects. - */ - size_t size() const; - - /** - * Return the entire map of MetadataEntry. - * - * @return The entire map of MetadataEnty objects. - */ - const MetadataEntriesMap& entries() const; - - /** - * Serialize the MetadataStore to the specified flatbuffer builder - * and return the resulting byte vector. - * - * @param[in] fbb The flatbuffer builder to use. - * @return The resulting serialized byte vector. - */ - std::vector> - make_entries(flatbuffers::FlatBufferBuilder& fbb) const; - - private: - /** - * Assign and increment an id to the entry. - * - * @param[in] entry The entry to assign a generated id to. - */ - void assignId(MetadataEntry& entry); - - /** - * The autogenerated meta id variable. - */ - uint32_t next_meta_id_{1}; - - /** - * The internal storage for all of the metadata entries. - */ - MetadataEntriesMap metadata_entries_{}; -}; - -/** - * Tag helper for Stream types that need to bind (link) together message - * ObjectType and the corresponding metadata entry (StreamMeta) that form - * together the stream definition. - */ -template -struct MessageStream { - using obj_type = ObjectType; - using meta_type = StreamMeta; -}; - -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/operations.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/operations.h deleted file mode 100644 index 14030b48..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/operations.h +++ /dev/null @@ -1,73 +0,0 @@ -/** - * Copyright (c) 2021, Ouster, Inc. - * All rights reserved. - * - * @file operations.h - * @brief High level OSF operations - * - */ -#pragma once - -#include - -#include "ouster/osf/basics.h" -#include "ouster/osf/metadata.h" -#include "ouster/types.h" - -/// @todo fix parameter directions in api doc -namespace ouster { -namespace osf { - -/** - * Outputs OSF v2 metadata + header info in JSON format. - * - * @param[in] file OSF file (only v2 supported) - * @param[in] full flag print full information (i.e. chunks_offset and decoded - * metas) - * @return JSON formatted string of the OSF metadata + header - */ -std::string dump_metadata(const std::string& file, bool full = true); - -/** - * Reads OSF file and prints (STDOUT) messages types, timestamps and - * overall statistics per message type. - * - * @param[in] file OSF file - * @param[in] with_decoding decode known messages (used to time a - * reading + decoding together) - */ -void parse_and_print(const std::string& file, bool with_decoding = false); - -/** - * Backup the metadata blob in an OSF file. - * - * @param[in] osf_file_name The OSF file to backup from. - * @param[in] backup_file_name The path to store the metadata blob backup. - * @return The number of the bytes written to the backup file. - */ -int64_t backup_osf_file_metablob(const std::string& osf_file_name, - const std::string& backup_file_name); - -/** - * Restore an OSF metadata blob from a backup file. - * - * @param[in] osf_file_name The OSF file to restore to. - * @param[in] backup_file_name The path to the metadata blob backup. - * @return The number of the bytes written to the OSF file. - */ -int64_t restore_osf_file_metablob(const std::string& osf_file_name, - const std::string& backup_file_name); - -/** - * Modify an OSF files sensor_info metadata. - * - * @param[in] file_name The OSF file to modify. - * @param[in] new_metadata The new metadata for the OSF file - * @return The number of the bytes written to the OSF file. - */ -int64_t osf_file_modify_metadata( - const std::string& file_name, - const std::vector& new_metadata); - -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/pcap_source.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/pcap_source.h deleted file mode 100644 index 85d30244..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/pcap_source.h +++ /dev/null @@ -1,144 +0,0 @@ -/** - * Copyright (c) 2021, Ouster, Inc. - * All rights reserved. - * - * @file pcap_source.h - * @brief Pcap raw data source - */ -#pragma once - -#include -#include - -#include "ouster/lidar_scan.h" -#include "ouster/os_pcap.h" -#include "ouster/osf/basics.h" -#include "ouster/types.h" - -namespace ouster { -namespace osf { - -/** - * Wrapper to process pcap files with lidar packet sensor data. - * Currently supports a single sensor, but can be extended easily for - * multisensor pcaps. - */ -class PcapRawSource { - public: - using ts_t = std::chrono::nanoseconds; - - /** - * Lidar data callbacks - * - * @param[in] timestamp The timestamp for the scan. - * @param[in] scan The LidarScan object. - */ - using LidarDataHandler = std::function; - - /** - * General pcap packet handler - * - * @param[in] info The sensor_info for the packet. - * @param[in] buf The raw buffer for the packet. - */ - using PacketHandler = std::function; - - /** - * Predicate to control the bag run loop - * - * @param[in] info The sensor_info for the packet. - * @return True if the loop should continue, False if the loop should halt. - */ - using PacketInfoPredicate = - std::function; - - /** - * Opens pcap file and checks available packets inside with - * heuristics applied to guess Ouster lidar port with data. - * - * @param[in] filename The filename of the pcap file to open. - */ - PcapRawSource(const std::string& filename); - - /** - * Attach lidar data handler to the port that receives already - * batched LidarScans with a timestamp of the first UDP lidar packet. - * LidarScan uses default field types by the profile - * - * @param[in] dst_port The destination port for the target lidar stream. - * @param[in] info The sensor info for the stream. - * @param[in] lidar_handler The callback to call on packet. - */ - void addLidarDataHandler(int dst_port, - const ouster::sensor::sensor_info& info, - LidarDataHandler&& lidar_handler); - - /** - * @copydoc addLidarDataHandler - * @param[in] ls_field_types The LidarScan field types to use. - */ - void addLidarDataHandler(int dst_port, - const ouster::sensor::sensor_info& info, - const LidarScanFieldTypes& ls_field_types, - LidarDataHandler&& lidar_handler); - - /** - * Read all packets from pcap and pass data to the attached handlers - * based on `dst_port` from pcap packets. - */ - void runAll(); - - /** - * Run the internal loop through all packets while the - * `pred(pinfo) == true`. - * `pred` function called before reading packet buffer and passing to the - * appropriate handlers. - * - * @param[in] pred The predicate function to decide whether to continue or - * not. - */ - void runWhile(const PacketInfoPredicate& pred); - - /** - * Close the pcap file. - */ - ~PcapRawSource(); - - private: - /// Remove some stuff - PcapRawSource(const PcapRawSource&) = delete; - PcapRawSource& operator=(const PcapRawSource&) = delete; - - /** - * Read current packet and dispatch handlers accordingly. - * - * @param[in] pinfo The new packet info. - */ - void handleCurrentPacket(const sensor_utils::packet_info& pinfo); - - /** - * The pcap file path. - */ - std::string pcap_filename_; - - /** - * The associated sensor_info. - */ - ouster::sensor::sensor_info info_; - - /** - * The internal pcap file handler. - */ - std::shared_ptr pcap_handle_{ - nullptr}; - - /** - * Map containing a 'destination port' to 'handler' mapping. - */ - std::map packet_handlers_{}; -}; - -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/reader.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/reader.h deleted file mode 100644 index 2fcfaf88..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/reader.h +++ /dev/null @@ -1,1322 +0,0 @@ -/** - * Copyright (c) 2021, Ouster, Inc. - * All rights reserved. - * - * @file reader.h - * @brief OSF file Reader - * - */ -#pragma once - -#include -#include - -#include "ouster/osf/file.h" -#include "ouster/osf/metadata.h" -#include "ouster/types.h" - -namespace ouster { -namespace osf { - -/** - * Enumerator for dealing with chunk validity. - * - * This is synthesized and thus does not have a reference in the Flat Buffer. - * Value is set in Reader::verify_chunk - */ -enum class ChunkValidity { - UNKNOWN = 0, ///< Validity can not be ascertained. - VALID, ///< Chunk is valid. - INVALID ///< Chunk is invalid. -}; - -/** - * The structure for representing chunk information and - * for forward iteration. - * - * This struct is partially mapped to the Flat Buffer data. - * Flat Buffer Reference: fb/metadata.fbs :: ChunkOffset - */ -struct ChunkState { - /** - * The current chunk's offset from the begining of the chunks section. - * - * Flat Buffer Reference: fb/metadata.fbs :: ChunkOffset :: offset - */ - uint64_t offset; - - /** - * The next chunk's offset for forward iteration. - * Should work like a linked list. - * - * This is partially synthesized from the Flat Buffers. - * This will link up with the next chunks offset. - * Value is set in ChunksPile::link_stream_chunks - * Flat Buffer Reference: fb/metadata.fbs :: ChunkOffset :: offset - */ - uint64_t next_offset; - - /** - * The first timestamp in the chunk in ordinality. - * - * Flat Buffer Reference: fb/metadata.fbs :: ChunkOffset :: start_ts - */ - ts_t start_ts; - - /** - * The last timestamp in the chunk in ordinality. - * - * Flat Buffer Reference: fb/metadata.fbs :: ChunkOffset :: end_ts - */ - ts_t end_ts; - - /** - * The validity of the current chunk - * - * This is synthesized and thus does not have a reference in the Flat - * Buffers. Value is set in Reader::verify_chunk - */ - ChunkValidity status; -}; - -/** - * The structure for representing streaming information. - * - * This struct is partially mapped to the Flat Buffer data. - */ -struct ChunkInfoNode { - /** - * The chunk offset from the begining of the chunks section. - * - * Flat Buffer Reference: fb/metadata.fbs :: ChunkOffset :: offset - */ - uint64_t offset; - - /** - * The next chunk's offset for forward iteration. - * Should work like a linked list. - * - * This is partially synthesized from the Flat Buffers. - * This will link up with the next chunks offset. - * Value is set in ChunksPile::link_stream_chunks - * Flat Buffer Reference: fb/metadata.fbs :: ChunkOffset :: offset - */ - uint64_t next_offset; - - /** - * The stream this is associated with. - * - * Flat Buffer Reference: - * fb/streaming/streaming_info.fbs :: ChunkInfo :: stream_id - */ - uint32_t stream_id; - - /** - * Total number of messages in a `stream_id` in the whole OSF file - * - * Flat Buffer Reference: - * fb/streaming/streaming_info.fbs :: ChunkInfo :: message_count - */ - uint32_t message_count; - - /** - * The index of the start of the message. - * @todo try to describe this better - * - * This is partially synthesized from the Flat Buffers. - * Value is set in ChunksPile::link_stream_chunks - * Synthesized from Flat Buffer Reference: - * fb/metadata.fbs :: ChunkOffset :: message_count - */ - uint32_t message_start_idx; -}; - -/** - * Chunks state map. Validity info and next offset. - */ -class ChunksPile { - public: - /** - * stream_id to offset map. - */ - using StreamChunksMap = - std::unordered_map>>; - - /** - * Default blank constructor. - */ - ChunksPile(); - - /** - * Add a new chunk to the ChunkPile. - * - * @param[in] offset The offset for the chunk. - * @param[in] start_ts The first timestamp in the chunk. - * @param[in] end_ts The first timestamp in the chunk. - */ - void add(uint64_t offset, ts_t start_ts, ts_t end_ts); - - /** - * Return the chunk associated with an offset. - * - * @param[in] offset The offset to return the chunk for. - * @return The chunk if found, or nullptr. - */ - ChunkState* get(uint64_t offset); - - /** - * Add a new streaming info to the ChunkPile. - * - * @param[in] offset The offset for the chunk. - * @param[in] stream_id The stream_id associated. - * @param[in] message_count The number of messages. - */ - void add_info(uint64_t offset, uint32_t stream_id, uint32_t message_count); - - /** - * Return the streaming info associated with an offset. - * - * @param[in] offset The offset to return the streaming info for. - * @return The streaming info if found, or nullptr. - */ - ChunkInfoNode* get_info(uint64_t offset); - - /** - * Return the streaming info associated with a message_idx. - * - * @param[in] stream_id The stream to look for infos in. - * @param[in] message_idx The specific message index to look for. - * @return The streaming info if found, or nullptr. - */ - ChunkInfoNode* get_info_by_message_idx(uint32_t stream_id, - uint32_t message_idx); - - /** - * Return the chunk associated with a lower bound timestamp. - * - * @param[in] stream_id The stream to look for chunks in. - * @param[in] ts The lower bound for the chunk. - * @return The chunk if found, or nullptr. - */ - ChunkState* get_by_lower_bound_ts(uint32_t stream_id, const ts_t ts); - - /** - * Return the next chunk identified by the offset. - * - * @param[in] offset The offset to return the next chunk for. - * @return The chunk if found, or nullptr. - */ - ChunkState* next(uint64_t offset); - - /** - * Return the next chunk identified by the offset per stream. - * - * @param[in] offset The offset to return the next chunk for. - * @return The chunk if found, or nullptr. - */ - ChunkState* next_by_stream(uint64_t offset); - - /** - * Return the first chunk. - * - * @return The chunk if found, or nullptr. - */ - ChunkState* first(); - - /** - * Return the size of the chunk pile. - * - * @return The size of the chunk pile. - */ - size_t size() const; - - /** - * Return if there is a message index. - * - * @return If there is a message index. - */ - bool has_message_idx() const; - - /** - * Return the stream_id to chunk offset map. - * - * @return The stream_id to chunk offset map. - */ - StreamChunksMap& stream_chunks(); - - /** - * Builds internal links between ChunkInfoNode per stream. - * - * @throws std::logic_error exception on non increasing timestamps. - * @throws std::logic_error exception on non existent info. - */ - void link_stream_chunks(); - - private: - /** - * The offset to chunk state map. - */ - std::unordered_map pile_{}; - - /** - * The offset to stream info map. - */ - std::unordered_map pile_info_{}; - - /** - * Ordered list of chunks offsets per stream id (only when ChunkInfo - * is present). - */ - StreamChunksMap stream_chunks_{}; -}; - -/** - * To String Functionality For ChunkState - * - * @param[in] chunk_state The data to get the string representation for - * @return The string representation - */ -std::string to_string(const ChunkState& chunk_state); - -/** - * To String Functionality For ChunkInfoNode - * - * @param[in] chunk_info The data to get the string representation format - * @return The string representation - */ -std::string to_string(const ChunkInfoNode& chunk_info); - -// Forward Decls -class Reader; -class MessageRef; -class ChunkRef; -class ChunksPile; -class ChunksRange; -struct MessagesStreamingIter; -struct MessagesChunkIter; -class MessagesStreamingRange; - -/** - * Chunk forward iterator in order of offset. - */ -struct ChunksIter { - using iterator_category = std::forward_iterator_tag; - using value_type = const ChunkRef; - using difference_type = std::ptrdiff_t; - using pointer = const std::unique_ptr; - using reference = const ChunkRef&; - - /** - * Default construction that zeros out member variables. - */ - ChunksIter(); - - /** - * Initialize from another ChunksIter object. - * - * @param[in] other The other ChunksIter object to initalize from. - */ - ChunksIter(const ChunksIter& other); - - /** - * Default assign operator. - * - * @param[in] other The other ChunksIter to assign to this. - */ - ChunksIter& operator=(const ChunksIter& other) = default; - - /** - * Return a ChunkRef object associated with this ChunksIter object. - * - * @throws std::logic_error Exception on end of iteration. - * - * @return The ChunkRef object associated with this ChunksIter object. - */ - const ChunkRef operator*() const; - - /** - * Return a ChunkRef pointer associated with this ChunksIter object. - * - * @return The ChunkRef pointer associated with this ChunksIter object. - */ - const std::unique_ptr operator->() const; - - /** - * Increment the ChunksIter iterator and return *this. - * - * @return The current ChunksIter object. - */ - ChunksIter& operator++(); - - /** - * Equality operator to compare two ChunksIter objects. - * - * @param[in] other The other object to compare. - * @return Whether the two ChunksIter objects are the same. - */ - bool operator==(const ChunksIter& other) const; - - /** - * Equality operator to compare two ChunksIter objects. - * - * @relates operator==(const ChunksIter& other) - * @param[in] other The other object to compare. - * @return Whether the two ChunksIter objects are not the same. - */ - bool operator!=(const ChunksIter& other) const; - - /** - * To String Functionality For ChunksIter. - * - * @return The string representation - */ - std::string to_string() const; - - private: - /** - * Internal constructor. - * - * @param[in] begin_addr The offset in the chunks to start at. - * @param[in] end_addr The offset in the chunks that they end at. - * @param[in] reader The reader object to use for reading. - */ - ChunksIter(const uint64_t begin_addr, const uint64_t end_addr, - Reader* reader); - - /** - * Move iterator to the next chunk of "verified" chunks. - */ - void next(); - - /** - * Move iterator to the next chunk. - */ - void next_any(); - - /** - * Verify that the ChunksIter is not at the end, - * and that the current chunk is valid. - */ - bool is_cleared(); - - /** - * The current offset in the chunks. - */ - uint64_t current_addr_; - - /** - * The offset in the chunks that they end at. - */ - uint64_t end_addr_; - - /** - * The internal reader object to use for reading. - */ - Reader* reader_; - - friend class ChunksRange; -}; // ChunksIter - -/** - * std iterator class for iterating through chunks. - */ -class ChunksRange { - public: - /** - * Begin function for std iterator support. - * - * @return A ChunksIter object for iteration. - */ - ChunksIter begin() const; - - /** - * End function for std iterator support. - * - * @return A ChunksIter object for signifying - * the end of iteration. - */ - ChunksIter end() const; - - /** - * To String Functionality For ChunksRange. - * - * @return The string representation. - */ - std::string to_string() const; - - private: - /** - * Constructor for Reader to call for creating the ChunksRange object. - * - * @param[in] begin_addr The beginning offset into the chunks buffer. - * @param[in] end_addr The end offset into the chunks buffer. - * @param[in,out] reader The Reader object to use for reading. - */ - ChunksRange(const uint64_t begin_addr, const uint64_t end_addr, - Reader* reader); - - /** - * The internal store of the begining offset into the chunks. - */ - uint64_t begin_addr_; - - /** - * The internal store of the ending offset into the chunks. - */ - uint64_t end_addr_; - - /** - * The internal store of the Reader object used for reading. - */ - Reader* reader_; - friend class Reader; -}; // ChunksRange - -/** - * %OSF Reader that simply reads sequentially messages from the OSF file. - * - * @todo Add filtered reads, and other nice things... - */ -class Reader { - public: - /** - * Creates reader from %OSF file resource. - * - * @param[in] osf_file The OsfFile object to use to read from. - */ - Reader(OsfFile& osf_file); - - /** - * Creates reader from %OSF file name. - * - * @param[in] file The OSF file path to read from. - */ - Reader(const std::string& file); - - /** - * Reads the messages from the first OSF chunk in sequental order - * till the end. Doesn't support RandomAccess. - * - * @throws std::logic_error Exception on not having sensor_info. - * - * @return The MessageStreamingRange object to iterate - * through the messages. - */ - MessagesStreamingRange messages(); - - /** - * @copydoc messages() - * @param[in] start_ts Specify the start of the timestamps that - * should be iterated through. - * @param[in] end_ts Specify the end of the timestamps that - * should be iterated through. - */ - MessagesStreamingRange messages(const ts_t start_ts, const ts_t end_ts); - - /** - * @copydoc messages() - * @param[in] stream_ids Filter the message iteration to specific streams. - */ - MessagesStreamingRange messages(const std::vector& stream_ids); - - /** - * @copydoc messages(const ts_t start_ts, const ts_t end_ts) - * @param[in] stream_ids Filter the message iteration to specific streams. - */ - MessagesStreamingRange messages(const std::vector& stream_ids, - const ts_t start_ts, const ts_t end_ts); - - /** - * Find the timestamp of the message by its index and stream_id. - * - * Requires the OSF with message_counts inside, i.e. has_message_idx() - * return ``True``, otherwise return value is always empty (nullopt). - * - * @throws std::logic_error Exception on not having sensor_info. - * - * @param[in] stream_id stream id on which the message_idx search is - * performed - * @param[in] message_idx the message index (i.e. rank/number) to search for - * @return message timestamp that corresponds to the message_idx in the - * stream_id - */ - nonstd::optional ts_by_message_idx(uint32_t stream_id, - uint32_t message_idx); - - /** - * Whether OSF contains the message counts that are needed for - * ``ts_by_message_idx()`` - * - * Message counts was added a bit later to the OSF core - * (ChunkInfo struct), so this function will be obsolete over time. - * - * @return Whether OSF contains the message counts that are needed for - * ``ts_by_message_idx()`` - */ - bool has_message_idx() const; - - /** - * Reads chunks and returns the iterator to valid chunks only. - * NOTE: Every chunk is read in full and validated. (i.e. it's not just - * iterator over chunks index) - * - * @return The iterator to valid chunks only. - */ - ChunksRange chunks(); - - /** - * Return the metadata id. - * - * @return The metadata id. - */ - std::string metadata_id() const; - - /** - * Return the lowest timestamp in the ChunksIter. - * - * @return The lowest timestamp in the ChunksIter. - */ - ts_t start_ts() const; - - /** - * Return the highest timestamp in the ChunksIter. - * - * @return The highest timestamp in the ChunksIter. - */ - ts_t end_ts() const; - - /** - * Return all metadata entries as a MetadataStore - * - * @return All of the metadata entries as a MetadataStore. - */ - const MetadataStore& meta_store() const; - - /** - * If the chunks can be read by stream and in non-decreasing timestamp - * order. - * - * @return The chunks can be read by stream and timestamps are sane. - */ - bool has_stream_info() const; - - private: - /** - * Read, parse and store all of the flatbuffer related metadata. - * - * @throws std::logic_error Exception on invalid metadata block. - */ - void read_metadata(); - - /** - * Verify, store and link all streaming info indicies - * i.e. StreamingInfo.chunks[] information - * - * @throws std::logic_error Exception on invalid chunk size. - */ - void read_chunks_info(); - - /** - * Checks the flatbuffers validity of a chunk by chunk offset. - * - * @param[in] chunk_offset Specify the chunk to verify via offset. - * @return The validity of the chunk. - */ - bool verify_chunk(uint64_t chunk_offset); - - /** - * Internal OsfFile object used to read the OSF file. - */ - OsfFile file_; - - /** - * Internal MetadataStore object to hold all of the - * metadata entries. - */ - MetadataStore meta_store_{}; - - /** - * Internal ChunksPile object to hold all of the - * chunks. - */ - ChunksPile chunks_{}; - - /** - * Internal indicator of if this file has streaming info - */ - bool has_streaming_info_{false}; - - /** - * Absolute offset to the beginning of the chunks in a file. - */ - uint64_t chunks_base_offset_{0}; - - /** - * Internal byte vector containing the raw flatbuffer - * metadata data. - */ - std::vector metadata_buf_{}; - - // NOTE: These classes need an access to private member `chunks_` ... - friend class ChunkRef; - friend struct ChunksIter; - friend struct MessagesStreamingIter; -}; // Reader - -/** - * Thin interface class that holds the pointer to the message - * and reconstructs underlying data to the corresponding object type given - * the Stream type. - */ -class MessageRef { - public: - using ts_t = osf::ts_t; - - /** - * The only way to create the MessageRef is to point to the corresponding - * byte buffer of the message in OSF file. - * - * @param[in] buf The buffer to use to make a MessageRef object. - * @param[in] meta_provider The metadata store that is used in types - * reconstruction - */ - MessageRef(const uint8_t* buf, const MetadataStore& meta_provider); - - /** - * The only way to create the MessageRef is to point to the corresponding - * byte buffer of the message in OSF file. - * - * @param[in] buf The buffer to use to make a MessageRef object. - * @param[in] meta_provider The metadata store that is used in types - * reconstruction - * @param[in,out] chunk_buf The pre-existing chunk buffer to use. - */ - MessageRef(const uint8_t* buf, const MetadataStore& meta_provider, - std::shared_ptr> chunk_buf); - - /** - * Get the message stream id. - * - * @return The message stream id. - */ - uint32_t id() const; - - /** - * Get the timestamp of the message. - * - * @return The timestamp of the message. - */ - ts_t ts() const; - - /// @todo [pb] Type of the stored data (meta of the stream?) - // std::string stream_type() const; - - /** - * Get the pointer to the underlying data. - * - * @return The pointer to the underlying data. - */ - const uint8_t* buf() const; - - /** - * Debug string representation. - * - * @return The string representation of a MessageRef. - */ - std::string to_string() const; - - /** - * Checks whether the message belongs to the specified Stream type. - * - * @tparam Stream The cpp data type to check against. - * @return If the current MessageRef is of type [Stream]. - */ - template - bool is() const { - auto meta = meta_provider_.get(id()); - return (meta != nullptr); - } - - /** - * Checks whether the message belongs to the specified Strean type. - * - * @param[in] type_str The data type in string form to check against. - * @return If the current MessageRef is of type type_str. - */ - bool is(const std::string& type_str) const; - - /** - * Reconstructs the underlying data to the class (copies data). - * - * @tparam Stream The type of the target data. - * @return A smart pointer to the new object. - */ - template - std::unique_ptr decode_msg() const { - auto meta = meta_provider_.get(id()); - - if (meta == nullptr) { - // Stream and metadata entry id is inconsistent - return nullptr; - } - - return Stream::decode_msg(buffer(), *meta, meta_provider_); - } - - /** - * Get the underlying raw message byte vector. - * - * @return Return the underlying raw message byte vector. - */ - std::vector buffer() const; - - /** - * Check if two MessageRefs are equal. - * - * @param[in] other The other MessageRef to check against. - * @return If the two MessageRefs are equal. - */ - bool operator==(const MessageRef& other) const; - - /** - * Check if two MessageRefs are not equal. - * - * @param[in] other The other MessageRef to check against. - * @return If the two MessageRefs are not equal. - */ - bool operator!=(const MessageRef& other) const; - - private: - /** - * The internal raw byte array. - */ - const uint8_t* buf_; - - /** - * The internal store for all of the metadata entries. - */ - const MetadataStore& meta_provider_; - - /** - * The internal chunk buffer to use. - */ - std::shared_ptr chunk_buf_; -}; // MessageRef - -/** - * Thin interface class that holds the pointer to the chunk and hides the - * messages reading routines. It expects that Chunk was "verified" before - * creating a ChunkRef. - */ -class ChunkRef { - public: - /** - * Default ChunkRef constructor that just zeros the internal fields. - */ - ChunkRef(); - - /** - * @param[in] offset The offset into the chunk array for the specified - * chunk. - * @param[in] reader The reader object to use for reading. - */ - ChunkRef(const uint64_t offset, Reader* reader); - - /** - * Check if two ChunkRefs are equal. - * - * @param[in] other The other ChunkRef to check against. - * @return If the two ChunkRef are equal. - */ - bool operator==(const ChunkRef& other) const; - - /** - * Check if two ChunkRefs are not equal. - * - * @param[in] other The other ChunkRef to check against. - * @return If the two ChunkRef are not equal. - */ - bool operator!=(const ChunkRef& other) const; - - /** - * Get the ChunkState for the chunk associated with this ChunkRef. - * - * @relates ChunkState - * - * @return The ChunkState associated with this ChunkRef. - */ - ChunkState* state(); - - /** - * @copydoc state() - */ - const ChunkState* state() const; - - /** - * Get the ChunkInfoNode for the chunk associated with this ChunkRef. - * - * @relates ChunkInfoNode - * - * @return The ChunkInfoNode associated with this ChunkRef. - */ - ChunkInfoNode* info(); - - /** - * @copydoc info() - */ - const ChunkInfoNode* info() const; - - /** - * Begin function for std iterator support. - * - * @return A MessagesChunkIter object for iteration. - */ - MessagesChunkIter begin() const; - - /** - * End function for std iterator support. - * - * @return A MessagesChunkIter object for signifying - * the end of iteration. - */ - MessagesChunkIter end() const; - - /** - * Get the message at a specific index. - * - * @todo Simplify this and any other instance of this - * - * @param[in] msg_idx The message index to get. - * @return The resulting message. - */ - const MessageRef operator[](size_t msg_idx) const; - - /** - * Get the message smart pointer at a specific index. - * - * @todo Simplify this and any other instance of this - * - * @param[in] msg_idx The message index to get. - * @return The resulting message smart pointer, - * returns nullptr if non existent. - */ - std::unique_ptr messages(size_t msg_idx) const; - - /** - * Debug string representation. - * - * @return The string representation of a ChunkRef. - */ - std::string to_string() const; - - /** - * Return the chunk offset in the larger flatbuffer array. - * - * @return The chunk offset in the larger flatbuffer array. - */ - uint64_t offset() const; - - /** - * The lowest timestamp in the chunk. - * A shortcut for state()->start_ts - * - * @relates state - */ - ts_t start_ts() const; - - /** - * The highest timestamp in the chunk. - * A shortcut for state()->end_ts - * - * @relates state - */ - ts_t end_ts() const; - - /** - * Returns the summation of the sizes of the chunks messages - * - * @return The summation of the sizes of the chunks messages, - * 0 on chunk invalidity. - */ - size_t size() const; - - /** - * Get the validity of the chunk. - * - * @return The validity of the chunk. - */ - bool valid() const; - - private: - /** - * Helper method to get the raw chunk pointer. - * Deals with the chunk being memory mapped or not. - * - * @return The raw chunk pointer. - */ - const uint8_t* get_chunk_ptr() const; - - /** - * The internal chunk offset variable. - */ - uint64_t chunk_offset_; - - /** - * Reader object to use for reading. - */ - Reader* reader_; - - /** - * Chunk buffer to use for reading. - */ - std::shared_ptr chunk_buf_; -}; // ChunkRef - -/** - * Convenient iterator class to go over all of the - * messages in a chunk. - */ -struct MessagesChunkIter { - using iterator_category = std::forward_iterator_tag; - using value_type = const MessageRef; - using difference_type = std::ptrdiff_t; - using pointer = const std::unique_ptr; - using reference = const MessageRef&; - - /** - * Default MessagesChunkIter constructor that just zeros - * the internal fields. - */ - MessagesChunkIter(); - - /** - * Initialize the MessagesChunkIter from another - * MessageChunkIter object. - * - * @param[in] other The other MessagesChunkIter to initalize from. - */ - MessagesChunkIter(const MessagesChunkIter& other); - - /** - * Default assignment operation. - * - * @param[in] other The other MessageChunkIter to assign to. - */ - MessagesChunkIter& operator=(const MessagesChunkIter& other) = default; - - /** - * Gets the current ChunkRef via value. - * - * @return The current ChunkRef value. - */ - const MessageRef operator*() const; - - /** - * Gets the current ChunkRef via smart pointer. - * - * @return The current ChunkRef smart pointer. - */ - std::unique_ptr operator->() const; - - /** - * Advance to the next message in the chunk. - * - * @return *this - */ - MessagesChunkIter& operator++(); - - /** - * @copydoc operator++() - */ - MessagesChunkIter operator++(int); - - /** - * Regress to the previous message in the chunk. - * - * @return *this - */ - MessagesChunkIter& operator--(); - - /** - * @copydoc operator--() - */ - MessagesChunkIter operator--(int); - - /** - * Check if two MessagesChunkIter are equal. - * - * @param[in] other The other MessagesChunkIter to check against. - * @return If the two MessagesChunkIter are equal. - */ - bool operator==(const MessagesChunkIter& other) const; - - /** - * Check if two MessagesChunkIter are not equal. - * - * @param[in] other The other MessagesChunkIter to check against. - * @return If the two MessagesChunkIter are not equal. - */ - bool operator!=(const MessagesChunkIter& other) const; - - /** - * Debug string representation. - * - * @return The string representation of a MessagesChunkIter. - */ - std::string to_string() const; - - private: - /** - * Internal constructor for initializing a MessageChunkIter - * off of a ChunkRef and message index. - * - * @param[in] chunk_ref The ChunkRef to use when initializing. - * @param[in] msg_idx The message index in the ChunkRef - * to use when initializing. - */ - MessagesChunkIter(const ChunkRef chunk_ref, const size_t msg_idx); - - /** - * Advance to the next message in the chunk. - */ - void next(); - - /** - * Regress to the previous message in the chunk. - */ - void prev(); - - /** - * The internal ChunkRef var. - */ - ChunkRef chunk_ref_; - - /** - * The current message index. - */ - size_t msg_idx_; - - friend class ChunkRef; -}; // MessagesChunkIter - -class MessagesStreamingRange { - public: - /** - * Begin function for std iterator support. - * - * @return A MessagesStreamingIter object for iteration. - */ - MessagesStreamingIter begin() const; - - /** - * End function for std iterator support. - * - * @return A MessagesStreamingIter object for signifying - * the end of iteration. - */ - MessagesStreamingIter end() const; - - /** - * Debug string representation. - * - * @return The string representation of a MessagesStreamingRange. - */ - std::string to_string() const; - - private: - /** - * Initialize from Reader on a set of filters.. - * //using range [start_ts, end_ts] <---- not inclusive .... !!! - * - * @param[in] start_ts The lowest timestamp to start with. - * @param[in] end_ts The highest timestamp to end with. - * @param[in] stream_ids The stream indicies to use with the streaming - * range. - * @param[in] reader The reader object to use for reading the OSF file. - */ - MessagesStreamingRange(const ts_t start_ts, const ts_t end_ts, - const std::vector& stream_ids, - Reader* reader); - - /** - * The lowest timestamp for the range. - */ - ts_t start_ts_; - - /** - * The highest timestamp for the range. - */ - ts_t end_ts_; - - /** - * The set of stream indicies in the range. - */ - std::vector stream_ids_; - - /** - * The reader object to use to read the OSF file. - */ - Reader* reader_; - friend class Reader; -}; // MessagesStreamingRange - -/** - * Iterator over all messages in Streaming Layout order for specified - * timestamp range. - */ -struct MessagesStreamingIter { - using iterator_category = std::forward_iterator_tag; - using value_type = const MessageRef; - using difference_type = std::ptrdiff_t; - using pointer = const std::unique_ptr; - using reference = const MessageRef&; - - using opened_chunk_type = std::pair; - - /** - * Comparison struct used for determining which chunk is greater. - */ - struct greater_chunk_type { - /** - * Comparison operator used for determining if the first is greater - * than the second. The comparison is based on the timestamps. - * - * @param[in] a The first chunk to compare. - * @param[in] b The second chunk to compare. - * @return If the first chunk is greater than the second chunk. - */ - bool operator()(const opened_chunk_type& a, const opened_chunk_type& b); - }; - - /** - * Default MessagesStreamingIter constructor that just zeros - * the internal fields. - */ - MessagesStreamingIter(); - - /** - * Initialize the MessagesStreamingIter from another - * MessagesStreamingIter object. - * - * @param[in] other The other MessagesStreamingIter to initalize from. - */ - MessagesStreamingIter(const MessagesStreamingIter& other); - - /** - * Default assignment operation. - * - * @param[in] other The other MessagesStreamingIter to assign to. - */ - MessagesStreamingIter& operator=(const MessagesStreamingIter& other) = - default; - - /** - * Gets the current MessageRef via value. - * - * @return The current MessageRef value. - */ - const MessageRef operator*() const; - - /** - * Gets the current MessageRef via smart pointer. - * - * @return The current MessageRef smart pointer. - */ - std::unique_ptr operator->() const; - - /** - * Advance to the next message. - * - * @return *this - */ - MessagesStreamingIter& operator++(); - - /** - * @copydoc operator++() - */ - MessagesStreamingIter operator++(int); - - /** - * Check if two MessagesStreamingIter are equal. - * - * @param[in] other The other MessagesStreamingIter to check against. - * @return If the two MessagesStreamingIter are equal. - */ - bool operator==(const MessagesStreamingIter& other) const; - - /** - * Check if two MessagesStreamingIter are not equal. - * - * @param[in] other The other MessagesStreamingIter to check against. - * @return If the two MessagesStreamingIter are not equal. - */ - bool operator!=(const MessagesStreamingIter& other) const; - - /** - * Debug string representation. - * - * @return The string representation of a MessagesStreamingIter. - */ - std::string to_string() const; - - private: - /** - * Initialize from Reader on a set of filters.. - * //using range [start_ts, end_ts] <---- not inclusive .... !!! - * - * @param[in] start_ts The lowest timestamp to start with. - * @param[in] end_ts The highest timestamp to end with. - * @param[in] stream_ids The stream indicies to use with the streaming - * range. - * @param[in] reader The reader object to use for reading the OSF file. - */ - MessagesStreamingIter(const ts_t start_ts, const ts_t end_ts, - const std::vector& stream_ids, - Reader* reader); - - /** - * Advance to the next message. - */ - void next(); - - /** - * The current timestamp. - */ - ts_t curr_ts_; - - /** - * The last timestamp. - */ - ts_t end_ts_; - - /** - * The streams to iterate on. - */ - std::vector stream_ids_; - - /** - * Used to hash the set of stream indexes. - * - * @todo Look at possibly removing this. - */ - uint32_t stream_ids_hash_; - - /** - * The reader object used to read the OSF file. - */ - Reader* reader_; - - /** - * Priority queue used to hold the chunks in timestamp order. - * - * @relates greater_chunk_type - */ - std::priority_queue, - greater_chunk_type> - curr_chunks_{}; - friend class Reader; - friend class MessagesStreamingRange; -}; // MessagesStreamingIter - -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/stream_lidar_scan.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/stream_lidar_scan.h deleted file mode 100644 index cf393138..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/stream_lidar_scan.h +++ /dev/null @@ -1,232 +0,0 @@ -/** - * Copyright (c) 2021, Ouster, Inc. - * All rights reserved. - * - * @file stream_lidar_scan.h - * @brief Stream of LidarScan - * - */ -#pragma once - -#include "ouster/osf/basics.h" -#include "ouster/osf/meta_lidar_sensor.h" -#include "ouster/osf/metadata.h" -#include "ouster/osf/writer.h" - -namespace ouster { -namespace osf { - -/** - * Cast `ls_src` LidarScan to a subset of fields with possible different - * underlying ChanFieldTypes. - * - * @throws std::logic_error Exception on trying to slice a scan with only - * a subset of the requested scans - * - * @param[in] ls_src The LidarScan to cast. - * @param[in] field_types The field types to cast the LidarScan to. - * @return a copy of `ls_src` with transformed fields. - */ -LidarScan slice_with_cast(const LidarScan& ls_src, - const LidarScanFieldTypes& field_types); - -/** - * Metadata entry for LidarScanStream to store reference to a sensor and - * field_types - * - * OSF type: - * ouster/v1/os_sensor/LidarScanStream - * - * Flat Buffer Reference: - * fb/os_sensor/lidar_scan_stream.fbs - */ -class LidarScanStreamMeta : public MetadataEntryHelper { - public: - /** - * @param[in] sensor_meta_id Reference to LidarSensor metadata that - * describes the sensor configuration. - * @param[in] field_types LidarScan fields specs, this argument is optional. - */ - LidarScanStreamMeta(const uint32_t sensor_meta_id, - const LidarScanFieldTypes field_types = {}); - - /** - * Return the sensor meta id. - * - * @return The sensor meta id. - */ - uint32_t sensor_meta_id() const; - - /** - * Return the field types. - * - * @return The field types. - */ - const LidarScanFieldTypes& field_types() const; - - /** - * @copydoc MetadataEntry::buffer - */ - std::vector buffer() const final; - - /** - * Create a LidarScanStreamMeta object from a byte array. - * - * @todo Figure out why this wasnt just done as a constructor overload. - * - * @relates MetadataEntry::from_buffer - * - * @param[in] buf The raw flatbuffer byte vector to initialize from. - * @return The new LidarScanStreamMeta cast as a MetadataEntry - */ - static std::unique_ptr from_buffer( - const std::vector& buf); - - /** - * Get the string representation for the LidarScanStreamMeta object. - * - * @relates MetadataEntry::repr - * - * @return The string representation for the LidarScanStreamMeta object. - */ - std::string repr() const override; - - private: - /** - * Internal store of the sensor id. - * - * Flat Buffer Reference: - * fb/os_sensor/lidar_scan_stream.fbs :: LidarScanStream :: sensor_id - */ - uint32_t sensor_meta_id_{0}; - - /** - * Internal store of the field types. - * - * Flat Buffer Reference: - * fb/os_sensor/lidar_scan_stream.fbs :: LidarScanStream :: field_types - */ - LidarScanFieldTypes field_types_; -}; - -/** @defgroup OSFTraitsLidarScanStreamMeta Templated struct for traits.*/ - -/** - * Templated struct for returning the OSF type string. - * - * @ingroup OSFTraitsLidarScanStreamMeta - */ -template <> -struct MetadataTraits { - /** - * Return the OSF type string. - * - * @return The OSF type string "ouster/v1/os_sensor/LidarScanStream". - */ - static const std::string type() { - return "ouster/v1/os_sensor/LidarScanStream"; - } -}; - -/** - * LidarScanStream that encodes LidarScan objects into the messages. - * - * Object type: ouster::sensor::LidarScan - * Meta type: LidarScanStreamMeta (sensor_meta_id, field_types) - * - * Flatbuffer definition file: - * fb/os_sensor/lidar_scan_stream.fbs - */ -class LidarScanStream : public MessageStream { - friend class Writer; - friend class MessageRef; - - // Access key pattern used to only allow friends to call our constructor - struct Token {}; - - /** - * Saves the object to the writer applying the coding/serizlization - * algorithm defined in make_msg() function. The function is the same for - * all streams types ... - * - * @todo [pb]: Probably should be abstracted/extracted from all streams - * we also might want to have the corresponding function to read back - * sequentially from Stream that doesn't seem like fit into this model... - * - * @param[in] ts The timestamp to use for the lidar scan. - * @param[in] lidar_scan The lidar scan to write. - */ - void save(const ouster::osf::ts_t ts, const obj_type& lidar_scan); - - /** - * Encode/serialize the object to the buffer of bytes. - * - * @param[in] lidar_scan The lidar scan to turn into a vector of bytes. - * @return The byte vector representation of lidar_scan. - */ - std::vector make_msg(const obj_type& lidar_scan); - - /** - * Decode/deserialize the object from bytes buffer using the concrete - * metadata type for the stream. - * - * @param[in] buf The buffer to decode into an object. - * @param[in] meta The concrete metadata type to use for decoding. - * @param[in] meta_provider Used to reconstruct any references to other - * metadata entries dependencies - * (like sensor_meta_id) - * @return Pointer to the decoded object. - */ - static std::unique_ptr decode_msg( - const std::vector& buf, const meta_type& meta, - const MetadataStore& meta_provider); - - public: - /** - * @param[in] key Private class used to prevent non-friends from calling - * this. - * @param[in] writer The writer object to use to write messages out. - * @param[in] sensor_meta_id The sensor to use. - * @param[in] field_types LidarScan fields specs, this argument is optional. - */ - LidarScanStream(Token key, Writer& writer, const uint32_t sensor_meta_id, - const LidarScanFieldTypes& field_types = {}); - - /** - * Return the concrete metadata type. - * This has templated types. - * - * @return The concrete metadata type. - */ - - const meta_type& meta() const { return meta_; }; - - private: - /** - * The internal writer object to use to write messages out. - */ - Writer& writer_; - - /** - * The internal concrete metadata type. - */ - meta_type meta_; - - /** - * The internal flatbuffer id for the stream. - */ - uint32_t stream_meta_id_{0}; - - /** - * The internal flatbuffer id for the metadata. - */ - uint32_t sensor_meta_id_{0}; - - /** - * The internal sensor_info data. - */ - sensor::sensor_info sensor_info_; -}; - -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/writer.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/writer.h deleted file mode 100644 index dc97c662..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/include/ouster/osf/writer.h +++ /dev/null @@ -1,642 +0,0 @@ -/** - * Copyright (c) 2021, Ouster, Inc. - * All rights reserved. - * - * @file writer.h - * @brief OSF file Writer - */ -#pragma once - -#include - -#include "ouster/osf/basics.h" -#include "ouster/osf/metadata.h" - -namespace ouster { -namespace osf { - -class LidarScanStream; - -/** - * Chunks writing strategy that decides when and how exactly write chunks - * to a file. See RFC 0018 for Standard and Streaming Layout description. - */ -class ChunksWriter { - public: - /** - * Save a message to a specified stream. - * - * @param[in] stream_id The stream id to associate with the message. - * @param[in] ts The timestamp for the messages. - * @param[in] buf A vector of message buffers to record. - */ - virtual void save_message(const uint32_t stream_id, const ts_t ts, - const std::vector& buf) = 0; - - /** - * Finish the process of saving messages and write out the stream stats. - */ - virtual void finish() = 0; - - /** - * Get the chunksize - * - * @return the chunk size - */ - virtual uint32_t chunk_size() const = 0; - - /** - * Default deconstructor. - */ - virtual ~ChunksWriter() = default; -}; - -/** - * %OSF Writer provides the base universal interface to store the collection - * of metadata entries, streams and corresponding objects. - */ -class Writer { - friend class StreamingLayoutCW; - - public: - /** - * @throws std::runtime_error Exception on file writing issues. - * - * @param[in] file_name The filename of the output OSF file. - * @param[in] chunk_size The chunk size in bytes to use for the OSF file. - * This argument is optional, and if not provided the default value of - * 2MB is used. If the current chunk being written exceeds - * the chunk_size, a new chunk will be started on the next call to - * save. This allows an application to tune the number of messages (e.g. - * lidar scans) per chunk, which affects the granularity of the message - * index stored in the StreamingInfo in the file metadata. A smaller - * chunk_size means more messages are indexed and a larger number of - * index entries. A more granular index allows for more precise - * seeking at the slight expense of a larger file. - */ - Writer(const std::string& file_name, uint32_t chunk_size = 0); - - /** - * @param[in] filename The filename to output to. - * @param[in] info The sensor info to use for a single stream OSF file. - * @param[in] chunk_size The chunksize to use for the OSF file, this - * parameter is optional. - * @param[in] field_types The fields from scans to actually save into the - * OSF. If not provided uses the fields from the - * first saved lidar scan for each stream. This - * parameter is optional. - */ - Writer(const std::string& filename, const ouster::sensor::sensor_info& info, - const LidarScanFieldTypes& field_types = LidarScanFieldTypes(), - uint32_t chunk_size = 0); - - /** - * @param[in] filename The filename to output to. - * @param[in] info The sensor info vector to use for a multi stream OSF - * file. - * @param[in] chunk_size The chunksize to use for the OSF file, this - * parameter is optional. - * @param[in] field_types The fields from scans to actually save into the - * OSF. If not provided uses the fields from the - * first saved lidar scan for each stream. This - * parameter is optional. - */ - Writer(const std::string& filename, - const std::vector& info, - const LidarScanFieldTypes& field_types = LidarScanFieldTypes(), - uint32_t chunk_size = 0); - - /** - * Add metadata to the OSF file. - * - * @tparam MetaType The type of metadata to add. - * @tparam MetaParams The type of meta parameters to add. - * - * @param[in] params The parameters to add. - */ - template - uint32_t add_metadata(MetaParams&&... params) { - MetaType entry(std::forward(params)...); - return meta_store_.add(entry); - } - - /** - * Adds a MetadataEntry to the OSF file. - * - * @param[in] entry The metadata entry to add to the OSF file. - */ - uint32_t add_metadata(MetadataEntry&& entry); - - /** - * @copydoc add_metadata(MetadataEntry&& entry) - */ - uint32_t add_metadata(MetadataEntry& entry); - - /** - * @defgroup OSFGetMetadataGroup Get specified metadata. - * Get and return a metadata entry. - * - * @param[in] metadata_id The id of the metadata to get and return. - * @return The correct MetadataEntry. - */ - - /** - * @copydoc OSFGetMetadataGroup - */ - std::shared_ptr get_metadata( - const uint32_t metadata_id) const; - - /** - * @copydoc OSFGetMetadataGroup - * - * @tparam MetadataEntryClass The type of metadata to get and return. - */ - template - std::shared_ptr get_metadata( - const uint32_t metadata_id) const { - return meta_store_.get(metadata_id); - } - - /** - * Creating streams by passing itself as first argument of the ctor and - * following the all other parameters. - * - * @tparam Stream The specified stream object type. - * @tparam StreamParams The specified stream parameter types. - * - * @param[in] params The parameters to use when creating a stream. - */ - template - Stream create_stream(StreamParams&&... params) { - return Stream(*this, std::forward(params)...); - } - - /** - * %Writer accepts messages in the form of bytes buffers with linked meta_id - * and timestamp. - * @todo [pb]: It should be hidden into private/protected, but I don't see - * yet how to do it and give an access to every derived Stream objects. - * - * @throws std::logic_error Exception on non existent stream id. - * - * @param[in] stream_id The stream to save the message to. - * @param[in] ts The timestamp to use for the message. - * @param[in] buf The message to save in the form of a byte vector. - */ - void save_message(const uint32_t stream_id, const ts_t ts, - const std::vector& buf); - - /** - * Adds info about a sensor to the OSF and returns the stream index to - * to write scans to it's stream. - * - * @param[in] info The info of the sensor to add to the file. - * @param[in] field_types The fields from scans to actually save into the - * OSF. If not provided uses the fields from the - * first saved lidar scan for this sensor. This - * parameter is optional. - * - * @return The stream index for the newly added sensor. - */ - uint32_t add_sensor( - const ouster::sensor::sensor_info& info, - const LidarScanFieldTypes& field_types = LidarScanFieldTypes()); - - /** - * Save a single scan to the specified stream_index in an OSF file. - * The concept of the stream_index is related to the sensor_info vector. - * Consider the following: - @code{.cpp} - sensor_info info1; // The first sensor in this OSF file - sensor_info info2; // The second sensor in this OSF file - sensor_info info3; // The third sensor in this OSF file - - Writer output = Writer(filename, {info1, info2, info3}); - - LidarScan scan = RANDOM_SCAN_HERE; - - // To save the LidarScan of scan to the first sensor, you would do the - // following - output.save(0, scan); - - // To save the LidarScan of scan to the second sensor, you would do the - // following - output.save(1, scan); - - // To save the LidarScan of scan to the third sensor, you would do the - // following - output.save(2, scan); - @endcode - * - * @throws std::logic_error Will throw exception on writer being closed. - * @throws std::logic_error ///< Will throw exception on - * ///< out of bound stream_index. - * - * @param[in] stream_index The index of the corrosponding sensor_info to - * use. - * @param[in] scan The scan to save. - */ - void save(uint32_t stream_index, const LidarScan& scan); - - /** - * Save a single scan to the specified stream_index in an OSF file indexed - * with the provided timestamp. - * - * @throws std::logic_error Will throw exception on writer being closed. - * @throws std::logic_error ///< Will throw exception on - * ///< out of bound stream_index. - * - * @param[in] stream_index The index of the corrosponding sensor_info to - * use. - * @param[in] scan The scan to save. - * @param[in] timestamp Timestamp to index this scan with. - */ - void save(uint32_t stream_index, const LidarScan& scan, - const ouster::osf::ts_t timestamp); - - /** - * Save multiple scans in an OSF file. - * The concept of the stream_index is related to the sensor_info vector. - * Consider the following: - @code{.cpp} - sensor_info info1; // The first sensor in this OSF file - sensor_info info2; // The second sensor in this OSF file - sensor_info info3; // The third sensor in this OSF file - - Writer output = Writer(filename, {info1, info2, info3}); - - LidarScan sensor1_scan = RANDOM_SCAN_HERE; - LidarScan sensor2_scan = RANDOM_SCAN_HERE; - LidarScan sensor3_scan = RANDOM_SCAN_HERE; - - // To save the scans matched appropriately to their sensors, you would do - // the following - output.save({sensor1_scan, sensor2_scan, sensor3_scan}); - @endcode - * - * - * @throws std::logic_error Will throw exception on writer being closed - * - * @param[in] scans The vector of scans to save. - */ - void save(const std::vector& scans); - - /** - * Returns the metadata store. This is used for getting the entire - * set of flatbuffer metadata entries. - * - * @return The flatbuffer metadata entries. - */ - const MetadataStore& meta_store() const; - - /** - * Returns the metadata id label. - * - * @return The metadata id label. - */ - const std::string& metadata_id() const; - - /** - * Sets the metadata id label. - * - * @param[in] id The metadata id label to set. - */ - void set_metadata_id(const std::string& id); - - /** - * Return the filename for the OSF file. - * - * @return The filename for the OSF file. - */ - const std::string& filename() const; - - /** - * Get the specific chunks layout of the OSF file. - * - * @relates ChunksLayout - * - * @return The chunks layout of the OSF file. - */ - ChunksLayout chunks_layout() const; - - /** - * Get the chunk size used for the OSF file. - * - * @return The chunk size for the OSF file. - */ - uint32_t chunk_size() const; - - /** - * Return the sensor info vector. - * Consider the following: - @code{.cpp} - sensor_info info1; // The first sensor in this OSF file - sensor_info info2; // The second sensor in this OSF file - sensor_info info3; // The third sensor in this OSF file - - Writer output = Writer(filename, {info1, info2, info3}); - - // The following will be true - output.sensor_info() == std::vector{info1, info2, info3}; - @endcode - * - * @return The sensor info vector. - */ - const std::vector& sensor_info() const; - - /** - * Get the specified sensor info - * Consider the following: - @code{.cpp} - sensor_info info1; // The first sensor in this OSF file - sensor_info info2; // The second sensor in this OSF file - sensor_info info3; // The third sensor in this OSF file - - Writer output = Writer(filename, {info1, info2, info3}); - - // The following will be true - output.sensor_info(0) == info1; - output.sensor_info(1) == info2; - output.sensor_info(2) == info3; - @endcode - * - * @param[in] stream_index The sensor info to return. - * @return The correct sensor info. - */ - const ouster::sensor::sensor_info sensor_info(int stream_index) const; - - /** - * Get the number of sensor_info objects. - * - * @return The sensor_info count. - */ - uint32_t sensor_info_count() const; - - /** - * Finish file with a proper metadata object, and header. - */ - void close(); - - /** - * Returns if the writer is closed or not. - * - * @return If the writer is closed or not. - */ - inline bool is_closed() const { return finished_; } - - /** - * @relates close - */ - ~Writer(); - - /** - * Disallow copying and moving. - */ - Writer(const Writer&) = delete; - - /** - * Disallow copying and moving. - */ - Writer& operator=(const Writer&) = delete; - - /** - * Disallow copying and moving. - */ - Writer(Writer&&) = delete; - - /** - * Disallow copying and moving. - */ - Writer& operator=(Writer&&) = delete; - - private: - /** - * Helper to construct the Metadata OSF Block at the end of writing. - * This function takes the metadata entries from the metadata store - * and generates a raw flatbuffer blob for writing to file. - * - * @return The completed raw flatbuffer byte vector for - * the metadata section. - */ - std::vector make_metadata() const; - - /** - * Internal method used to save a scan to a specified stream_index - * specified stream. This method is here so that we can bypass - * is_closed checking for speed sake. The calling functions will - * do the check for us. - * - * @param[in] stream_index The stream to save to. - * @param[in] scan The scan to save. - * @param[in] time Timestamp to use to index scan. - */ - void _save(uint32_t stream_index, const LidarScan& scan, const ts_t time); - - /** - * Writes buf to the file with CRC32 appended and return the number of - * bytes writen to the file - * - * @throws std::logic_error Exception on bad file position. - * @throws std::logic_error Exception on a closed writer object. - * - * @param[in] buf The buffer to append. - * @param[in] size The size of the buffer to append. - * @return The number of bytes writen to the OSF file. - */ - uint64_t append(const uint8_t* buf, const uint64_t size); - - /** - * Save a specified chunk to the OSF file. - * - * @throws std::logic_error Exception on a size mismatch - * - * @param[in] start_ts The lowest timestamp in the chunk. - * @param[in] end_ts The highest timestamp in the chunk. - * @param[in] chunk_buf The byte vector representation of the chunk. - * @return The result offset in the OSF file. - */ - uint64_t emit_chunk(const ts_t start_ts, const ts_t end_ts, - const std::vector& chunk_buf); - - /** - * Internal filename of the OSF file. - */ - std::string file_name_; - - /** - * The size of the flatbuffer header blob. - */ - uint32_t header_size_{0}; - - /** - * The internal file offset. - */ - int64_t pos_{-1}; - - /** - * Internal status flag for whether we have started writing or not. - */ - bool started_{false}; - - /** - * Internal status flag for whether the file has been closed or not. - * - * @relates close - */ - bool finished_{false}; - - /** - * The internal vector of chunks. - */ - std::vector chunks_{}; - - /** - * The lowest timestamp in the OSF file. - */ - ts_t start_ts_{ts_t::max()}; - - /** - * The highest timestamp in the OSF file. - */ - ts_t end_ts_{ts_t::min()}; - - /** - * Cache of the chunk offset. - */ - uint64_t next_chunk_offset_{0}; - - /** - * The metadata id label. - */ - std::string metadata_id_{}; - - /** - * The internal chunk layout of the OSF file. - */ - ChunksLayout chunks_layout_{ChunksLayout::LAYOUT_STANDARD}; - - /** - * The store of metadata entries. - */ - MetadataStore meta_store_{}; - - /** - * ChunksWriter is reponsible for chunking strategy. - */ - std::shared_ptr chunks_writer_{nullptr}; - - /** - * Internal store of field types to serialize for lidar scans - */ - std::vector field_types_; - - /** - * Internal stream index to metadata map. - */ - std::map lidar_meta_id_; - - /** - * Internal stream index to LidarScanStream map. - */ - std::map> - lidar_streams_; - - /** - * The internal sensor_info store ordered by stream_index. - */ - std::vector sensor_info_; -}; - -/** - * Encapsulate chunk seriualization operations. - */ -class ChunkBuilder { - public: - ChunkBuilder(){}; - - /** - * Save messages to the serialized chunks. - * - * @throws std::logic_error Exception on a size mismatch - * - * @param[in] stream_id The stream to save the message to. - * @param[in] ts The timestamp to use for the message. - * @param[in] msg_buf The message to save in the form of a byte vector. - */ - void save_message(const uint32_t stream_id, const ts_t ts, - const std::vector& msg_buf); - - /** - * Completely wipe all data and start the chunk anew. - */ - void reset(); - - /** - * Finish out the serialization of the chunk and return the raw - * flatbuffer output. - * - * @return The serialized chunk in a raw flatbuffer byte vector. - */ - std::vector finish(); - - /** - * Returns the flatbufferbuilder size. - * - * @return The flatbufferbuilder size. - */ - uint32_t size() const; - - /** - * Returns the number of messages saved so far. - * - * @return The number of messages saved so far. - */ - uint32_t messages_count() const; - - /** - * The lowest timestamp in the chunk. - */ - ts_t start_ts() const; - - /** - * The highest timestamp in the chunk. - */ - ts_t end_ts() const; - - private: - /** - * Internal method for updating the corret start and end - * timestamps. - * - * @param[in] ts The timestamp to check against for start and end. - */ - void update_start_end(const ts_t ts); - - /** - * Internal status flag for whether the builder is finished or not. - */ - bool finished_{false}; - - /** - * Internal FlatBufferBuilder object used for the serialization. - */ - flatbuffers::FlatBufferBuilder fbb_{0x7fff}; - - /** - * The lowest timestamp in the chunk. - */ - ts_t start_ts_{ts_t::max()}; - - /** - * The highest timestamp in the chunk. - */ - ts_t end_ts_{ts_t::min()}; - - /** - * Internal store of messages to be contained within the chunk - */ - std::vector> messages_{}; -}; - -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/basics.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/basics.cpp deleted file mode 100644 index 9d8c69ac..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/basics.cpp +++ /dev/null @@ -1,183 +0,0 @@ -/** - * Copyright(c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#include "ouster/osf/basics.h" - -#include -#include -#include -#include - -#include "nonstd/optional.hpp" -#include "ouster/osf/crc32.h" - -namespace ouster { -namespace osf { - -using nonstd::make_optional; -using nonstd::nullopt; -using nonstd::optional; - -namespace impl { - -// TODO[pb]: Review some time later these enum/strings converters ... -// Copying all functions to handling enums from ouster-example -// later we will not copy and share code in some other way - -template -using Table = std::array, N>; - -extern const Table chunks_layout_strings{ - {{ChunksLayout::LAYOUT_STANDARD, "STANDARD"}, - {ChunksLayout::LAYOUT_STREAMING, "STREAMING"}}}; - -} // namespace impl - -/* String conversion */ - -template -static optional lookup(const impl::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 ? nullopt : make_optional(res->second); -} - -template -static optional rlookup(const impl::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 ? nullopt : make_optional(res->first); -} - -std::string to_string(ChunksLayout chunks_layout) { - auto res = lookup(impl::chunks_layout_strings, chunks_layout); - return res ? res.value() : "UNKNOWN"; -} - -ChunksLayout chunks_layout_of_string(const std::string& s) { - auto res = rlookup(impl::chunks_layout_strings, s.c_str()); - return res ? res.value() : ChunksLayout::LAYOUT_STANDARD; -} - -std::string to_string(const HEADER_STATUS status) { - return v2::EnumNamesHEADER_STATUS()[static_cast(status)]; -} - -std::string to_string(const uint8_t* buf, const size_t count, - const size_t max_show_count) { - std::stringstream ss; - ss << std::hex; - size_t show_count = count; - if (max_show_count != 0 && max_show_count < count) - show_count = max_show_count; - for (size_t i = 0; i < show_count; ++i) { - if (i > 0) ss << " "; - ss << std::setfill('0') << std::setw(2) << static_cast(buf[i]); - } - if (show_count < count) { - ss << " ... and " << std::dec << (count - show_count) << " more ..."; - } - return ss.str(); -} - -std::string read_text_file(const std::string& filename) { - std::stringstream buf{}; - std::ifstream ifs{}; - ifs.open(filename); - buf << ifs.rdbuf(); - ifs.close(); - - if (!ifs) { - std::stringstream ss; - ss << "Failed to read file: " << filename; - throw std::runtime_error(ss.str()); - } - - return buf.str(); -} - -uint32_t get_prefixed_size(const uint8_t* buf) { - return buf[0] + (buf[1] << 8u) + (buf[2] << 16u) + (buf[3] << 24u); -} - -uint32_t get_block_size(const uint8_t* buf) { - return get_prefixed_size(buf) + FLATBUFFERS_PREFIX_LENGTH + CRC_BYTES_SIZE; -} - -bool check_prefixed_size_block_crc(const uint8_t* buf, - const uint32_t buf_length) { - uint32_t prefixed_size = get_prefixed_size(buf); - if (buf_length < prefixed_size + FLATBUFFERS_PREFIX_LENGTH + - ouster::osf::CRC_BYTES_SIZE) { - std::cerr << "ERROR: CRC32 validation failed!" - << " (out of buffer legth)" << std::endl; - return false; - } - - const uint32_t crc_stored = - get_prefixed_size(buf + prefixed_size + FLATBUFFERS_PREFIX_LENGTH); - const uint32_t crc_calculated = - osf::crc32(buf, prefixed_size + FLATBUFFERS_PREFIX_LENGTH); - - const bool res = (crc_stored == crc_calculated); - - if (!res) { - std::cerr << "ERROR: CRC32 validation failed!" << std::endl; - std::cerr << std::hex << " CRC - stored: " << crc_stored - << std::dec << std::endl; - std::cerr << std::hex << " CRC - calculated: " << crc_calculated - << std::dec << std::endl; - } - return res; -} - -std::function make_build_ls( - const ouster::sensor::sensor_info& info, - const LidarScanFieldTypes& ls_field_types, - std::function handler) { - const auto w = info.format.columns_per_frame; - const auto h = info.format.pixels_per_column; - auto temp_ls_field_types = ls_field_types; - std::shared_ptr ls(nullptr); - if (temp_ls_field_types.empty()) { - temp_ls_field_types = get_field_types(info); - } - ls = std::make_shared(w, h, temp_ls_field_types.begin(), - temp_ls_field_types.end()); - - auto pf = ouster::sensor::get_format(info); - auto build_ls_imp = ScanBatcher(w, pf); - osf::ts_t first_msg_ts{-1}; - return [handler, build_ls_imp, ls, first_msg_ts]( - const osf::ts_t msg_ts, const uint8_t* buf) mutable { - if (first_msg_ts == osf::ts_t{-1}) { - first_msg_ts = msg_ts; - } - if (build_ls_imp(buf, *ls)) { - handler(first_msg_ts, *ls); - // At this point we've just started accumulating new LidarScan, so - // we are saving the msg_ts (i.e. timestamp of a UDP packet) - // which contained the first lidar_packet - first_msg_ts = msg_ts; - } - }; -} - -std::function make_build_ls( - const ouster::sensor::sensor_info& info, - std::function handler) { - return make_build_ls(info, {}, handler); -} - -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/compat_ops.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/compat_ops.cpp deleted file mode 100644 index 0e316cfd..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/compat_ops.cpp +++ /dev/null @@ -1,405 +0,0 @@ -/** - * Copyright(c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#include "compat_ops.h" - -#include -#include -#include - -#ifdef _WIN32 -#include -#include -#include -#include -#include -#include -#else -#include -#include -#include -#include - -#include -#endif - -namespace ouster { -namespace osf { - -static const std::string FILE_SEPS = "\\/"; - -namespace { - -#ifdef _WIN32 -// ErrorMessage support function. -// Retrieves the system error message for the GetLastError() code. -// Note: caller must use LocalFree() on the returned LPCTSTR buffer. -LPCTSTR ErrorMessage(DWORD error) { - LPVOID lpMsgBuf; - FormatMessageA(FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM | - FORMAT_MESSAGE_IGNORE_INSERTS, - NULL, error, NULL, (LPTSTR)&lpMsgBuf, 0, NULL); - return ((LPCTSTR)lpMsgBuf); -} - -// PrintError support function. -// Simple wrapper function for error output. -void PrintError(LPCTSTR errDesc) { - LPCTSTR errMsg = ErrorMessage(GetLastError()); - _ftprintf(stderr, TEXT("\nERROR: %s SYSTEM RETURNED: %s\n"), errDesc, - errMsg); - LocalFree((LPVOID)errMsg); -} - -// Get the last system error and return it in a string (not wide string) -std::string LastErrorMessageStr() { - LPCTSTR errMsg = ErrorMessage(GetLastError()); - // NOTE[pb]: errMsg can be a char or wchar and seems the below copy - // construct is OK for MSVC in both cases. (or no? idk) - std::string res{errMsg}; - LocalFree((LPVOID)errMsg); - return res; -} -#endif - -} // namespace - -/// Get the last system error and return it in a string -std::string get_last_error() { -#ifdef _WIN32 - return LastErrorMessageStr(); -#else - return std::string(std::strerror(errno)); -#endif -} - -/// Checking the the path is it directory or not. -bool is_dir(const std::string& path) { -#ifdef _WIN32 - DWORD attrs = GetFileAttributesA(path.c_str()); - if (attrs == INVALID_FILE_ATTRIBUTES) { - return false; - } - if (attrs & FILE_ATTRIBUTE_DIRECTORY) { - return true; - } - return false; -#else - struct stat statbuf; - if (stat(path.c_str(), &statbuf) != 0) { - if (errno != ENOENT) printf("ERROR: stat: %s", std::strerror(errno)); - return false; - } - return S_ISDIR(statbuf.st_mode); -#endif -} - -/// Check path existence on the system -bool path_exists(const std::string& path) { -#ifdef _WIN32 - // taken from here: - // https://devblogs.microsoft.com/oldnewthing/20071023-00/?p=24713 - DWORD attrs = GetFileAttributesA(path.c_str()); - return (attrs != INVALID_FILE_ATTRIBUTES); -#else - struct stat sb; - return !(stat(path.c_str(), &sb) != 0); -#endif -} - -bool is_file_sep(char c) { return (FILE_SEPS.find(c) != std::string::npos); } - -/// Path concatenation with OS specific path separator -// NOTE: Trying not to be too smart here ... and it's impossible function -// to make it fully correct, so use it wisely. -std::string path_concat(const std::string& path1, const std::string& path2) { - if (path1.empty()) return path2; - if (path2.empty()) return path1; - - if (is_file_sep(path2.front())) return path2; -#ifdef _WIN32 - if (path2.size() > 1 && path2.at(1) == ':') return path2; -#endif - - size_t p1_last_slash = path1.size(); - while (p1_last_slash > 0 && is_file_sep(path1.at(p1_last_slash - 1))) { - p1_last_slash--; - } - return path1.substr(0, p1_last_slash) + FILE_SEP + path2; -} - -/// Get the path to unique temp directory and create it. -bool make_tmp_dir(std::string& tmp_path) { -#ifdef _WIN32 - unsigned int uRetVal = 0; - char lpTempPath[MAX_PATH]; - uRetVal = GetTempPathA(MAX_PATH, lpTempPath); - if (uRetVal > MAX_PATH || uRetVal == 0) { - return false; - } - char pTmpPath[MAX_PATH]; - if (!tmpnam_s(pTmpPath)) { - if (CreateDirectoryA(pTmpPath, NULL)) { - tmp_path = pTmpPath; - return true; - } - } - PrintError(TEXT("Can't create temp dir.")); - return false; -#else - // TODO[pb]: Check that it works on Mac OS and especially that - // temp files are cleaned correctly and don't feel up the CI machine ... - char tmpdir[] = "/tmp/ouster-test.XXXXXX"; - if (::mkdtemp(tmpdir) == nullptr) { - std::cerr << "ERROR: Can't create temp dir." << std::endl; - return false; - }; - tmp_path = tmpdir; - return true; -#endif -} - -/// Make directory -bool make_dir(const std::string& path) { -#ifdef _WIN32 - return (CreateDirectoryA(path.c_str(), NULL) != 0); -#else - if (mkdir(path.c_str(), 0777) != 0) { - printf("ERROR: Can't create dir: %s\n", path.c_str()); - return false; - } - return true; -#endif -} - -/// Get environment variable -bool get_env_var(const std::string& name, std::string& value) { -#ifdef _WIN32 - const unsigned int BUFSIZE = 4096; - char var_value[BUFSIZE]; - unsigned int ret_val = - GetEnvironmentVariableA(name.c_str(), var_value, BUFSIZE); - if (ret_val != 0 && ret_val < BUFSIZE) { - value.assign(var_value); - return true; - } - value.clear(); - return false; -#else - char* var_value; - if ((var_value = std::getenv(name.c_str())) != nullptr) { - value = var_value; - return true; - } - value.clear(); - return false; -#endif -} - -/// Unlink path (i.e. almost like remove) -bool unlink_path(const std::string& path) { -#ifdef _WIN32 - return (_unlink(path.c_str()) == 0); -#else - return (unlink(path.c_str()) == 0); -#endif -} - -// Remove directory -bool remove_dir(const std::string& path) { -#ifdef _WIN32 - return (_rmdir(path.c_str()) == 0); -#else - return (rmdir(path.c_str()) == 0); -#endif -} - -/// Get file size -int64_t file_size(const std::string& path) { -#ifdef _WIN32 - LARGE_INTEGER fsize; - WIN32_FILE_ATTRIBUTE_DATA file_attr_data; - if (!GetFileAttributesExA(path.c_str(), GetFileExInfoStandard, - &file_attr_data)) { - return -1; - } - if (file_attr_data.dwFileAttributes & FILE_ATTRIBUTE_DIRECTORY) { - return -2; - } - fsize.LowPart = file_attr_data.nFileSizeLow; - fsize.HighPart = file_attr_data.nFileSizeHigh; - return fsize.QuadPart; -#else - struct stat st; - if (stat(path.c_str(), &st) < 0) { - return -1; - }; - if (!S_ISREG(st.st_mode)) { - return -2; - } - return st.st_size; -#endif -} - -/// File mapping open (read-only operations) -uint8_t* mmap_open(const std::string& path) { -#ifdef _WIN32 - HANDLE hFile; - uint8_t* pBuf; - hFile = CreateFileA(path.c_str(), GENERIC_READ, FILE_SHARE_READ, NULL, - OPEN_EXISTING, - FILE_ATTRIBUTE_NORMAL | FILE_FLAG_OVERLAPPED, NULL); - if (hFile == INVALID_HANDLE_VALUE) { - return NULL; - } - HANDLE hFileMap; - hFileMap = CreateFileMappingA(hFile, NULL, PAGE_READONLY, 0, 0, NULL); - if (hFileMap == NULL) { - CloseHandle(hFile); - return NULL; - } - - pBuf = - static_cast(MapViewOfFile(hFileMap, FILE_MAP_READ, 0, 0, 0)); - - // TODO: check errors? - CloseHandle(hFileMap); - CloseHandle(hFile); - - return pBuf; -#else - struct stat st; - if (stat(path.c_str(), &st) < 0) { - return nullptr; - }; - if (!S_ISREG(st.st_mode)) { - return nullptr; - } - if (st.st_size == 0) { - return nullptr; - } - - int fd = open(path.c_str(), O_RDONLY); - if (fd < 0) { - return nullptr; - } - - void* map_osf_file = mmap(0, st.st_size, PROT_READ, MAP_SHARED, fd, 0); - if (map_osf_file == MAP_FAILED) { - ::close(fd); - return nullptr; - } - return static_cast(map_osf_file); -#endif -} - -/// File mapping close -bool mmap_close(uint8_t* file_buf, const uint64_t file_size) { - if (file_buf == nullptr || file_size == 0) return false; -#ifdef _WIN32 - return (UnmapViewOfFile(file_buf) != 0); -#else - return (munmap(static_cast(file_buf), file_size) >= 0); -#endif -} - -int64_t truncate_file(const std::string& path, uint64_t filesize) { - int64_t actual_file_size = file_size(path); - if (actual_file_size < (int64_t)filesize) { - return -1; - } -#ifdef _WIN32 - int file_handle; - if (file_handle = _sopen(path.c_str(), _O_RDWR, _SH_DENYRW)) { - _chsize(file_handle, filesize); - _close(file_handle); - } -#else - truncate(path.c_str(), filesize); -#endif - return file_size(path); -} - -int64_t append_binary_file(const std::string& append_to_file_name, - const std::string& append_from_file_name) { - int64_t saved_size = -1; - - std::fstream append_to_file_stream; - std::fstream append_from_file_stream; - - // clang-format off - // There something seriously wrong with the clang formatting - // here. - append_to_file_stream.open(append_to_file_name, std::fstream::out | - std::fstream::app | std::fstream::binary); - append_from_file_stream.open(append_from_file_name, - std::fstream::in | std::fstream::binary); - // clang-format on - - if (append_to_file_stream.is_open()) { - if (append_from_file_stream.is_open()) { - append_from_file_stream.seekg(0, std::ios::end); - uint64_t from_file_size = append_from_file_stream.tellg(); - append_from_file_stream.seekg(0, std::ios::beg); - - append_to_file_stream.seekg(0, std::ios::end); - - append_to_file_stream << append_from_file_stream.rdbuf(); - saved_size = append_to_file_stream.tellg(); - } else { - std::cerr << "fail to open " << append_to_file_name << std::endl; - } - } else { - std::cerr << "fail to open " << append_from_file_name << std::endl; - } - - if (append_to_file_stream.is_open()) append_to_file_stream.close(); - if (append_from_file_stream.is_open()) append_from_file_stream.close(); - - return saved_size; -} - -int64_t copy_file_trailing_bytes(const std::string& source_file, - const std::string& target_file, - uint64_t offset) { - int64_t actual_file_size = file_size(source_file); - if (actual_file_size < (int64_t)offset) { - return -1; - } - - int64_t saved_size = -1; - - std::fstream source_file_stream; - std::fstream target_file_stream; - - // clang-format off - // There something seriously wrong with the clang formatting - // here. - target_file_stream.open(target_file, std::fstream::out | - std::fstream::trunc | std::fstream::binary); - source_file_stream.open(source_file, - std::fstream::in | std::fstream::binary); - // clang-format on - - if (target_file_stream.is_open()) { - if (source_file_stream.is_open()) { - source_file_stream.seekg(offset); - target_file_stream << source_file_stream.rdbuf(); - saved_size = target_file_stream.tellg(); - } else { - std::cerr << "fail to open " << source_file << std::endl; - } - } else { - std::cerr << "fail to open " << target_file << std::endl; - } - - if (source_file_stream.is_open()) source_file_stream.close(); - if (target_file_stream.is_open()) target_file_stream.close(); - - return saved_size; -} - -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/compat_ops.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/compat_ops.h deleted file mode 100644 index eb418ca5..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/compat_ops.h +++ /dev/null @@ -1,91 +0,0 @@ -/** - * Copyright(c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#pragma once - -#include -#include - -namespace ouster { -namespace osf { - -/// @todo Fix the api comments in this file -#ifdef _WIN32 -constexpr char FILE_SEP = '\\'; -#else -constexpr char FILE_SEP = '/'; -#endif - -/// Checking the the path is it directory or not. -bool is_dir(const std::string& path); - -/// Check path existence on the system -bool path_exists(const std::string& path); - -/// Path concatenation with OS specific path separator -std::string path_concat(const std::string& path1, const std::string& path2); - -/// Get the path to unique temp directory and create it. -bool make_tmp_dir(std::string& tmp_path); - -/// Make directory -bool make_dir(const std::string& path); - -/// Get environment variable -bool get_env_var(const std::string& name, std::string& value); - -// Unlink path -bool unlink_path(const std::string& path); - -// Remove directory -bool remove_dir(const std::string& path); - -// Get file size -int64_t file_size(const std::string& path); - -// File mapping open -uint8_t* mmap_open(const std::string& path); - -// File mapping close -bool mmap_close(uint8_t* file_buf, const uint64_t file_size); - -/// Get the last system error and return it in a string (not wide string) -std::string get_last_error(); - -/** - * Truncate a file to a certain length - * - * @param[in] path The file to truncate. - * @param[in] filesize The final size of the file. - * - * @return The number of bytes of the final file. - */ -int64_t truncate_file(const std::string& path, uint64_t filesize); - -/** - * Appends one file to another - * - * @param[in] append_to_file_name The file to append to. - * @param[in] append_from_file_name The file to append from. - * - * @return The number of bytes of the final file. - */ -int64_t append_binary_file(const std::string& append_to_file_name, - const std::string& append_from_file_name); - -/** - * Copies trailing bytes from a file - * - * @param[in] source_file The file to copy from. - * @param[in] target_file The file to copy to. - * @param[in] offset The offset in the source_file to start copying from. - * - * @return The number of bytes of the target file. - */ -int64_t copy_file_trailing_bytes(const std::string& source_file, - const std::string& target_file, - uint64_t offset); -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/crc32.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/crc32.cpp deleted file mode 100644 index 6dea8b25..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/crc32.cpp +++ /dev/null @@ -1,32 +0,0 @@ -/** - * Copyright(c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#include "ouster/osf/crc32.h" - -#include - -#include -#include -#include -#include -#include - -namespace ouster { -namespace osf { - -const uint32_t CRC_INITIAL_VALUE = 0L; - -// =============== ZLIB functions wrappers ==================== - -uint32_t crc32(const uint8_t* buf, uint32_t size) { - return crc32_z(CRC_INITIAL_VALUE, buf, size); -} - -uint32_t crc32(uint32_t initial_crc, const uint8_t* buf, uint32_t size) { - return crc32_z(initial_crc, buf, size); -} - -} // namespace osf -} // namespace ouster \ No newline at end of file diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/fb_utils.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/fb_utils.cpp deleted file mode 100644 index f563ef3a..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/fb_utils.cpp +++ /dev/null @@ -1,136 +0,0 @@ -/** - * Copyright(c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#include "fb_utils.h" - -#include -#include - -#include "ouster/osf/basics.h" -#include "ouster/osf/crc32.h" - -namespace ouster { -namespace osf { - -bool check_osf_metadata_buf(const uint8_t* buf, const uint32_t buf_size) { - // note: fb verifier checks for exact size of buffer equal to prefixed size - auto verifier = - flatbuffers::Verifier(buf, buf_size - ouster::osf::CRC_BYTES_SIZE); - return check_prefixed_size_block_crc(buf, buf_size) && - gen::VerifySizePrefixedMetadataBuffer(verifier); -} - -bool check_osf_chunk_buf(const uint8_t* buf, const uint32_t buf_size) { - // note: fb verifier checks for exact size of buffer equal to prefixed size - auto verifier = - flatbuffers::Verifier(buf, buf_size - ouster::osf::CRC_BYTES_SIZE); - return check_prefixed_size_block_crc(buf, buf_size) && - gen::VerifySizePrefixedChunkBuffer(verifier); -} - -template -std::vector vector_from_fb_vector(const flatbuffers::Vector* fb_vec) { - if (fb_vec == nullptr) return {}; - return {fb_vec->data(), fb_vec->data() + fb_vec->size()}; -} - -template std::vector vector_from_fb_vector( - const flatbuffers::Vector* fb_vec); -template std::vector vector_from_fb_vector( - const flatbuffers::Vector* fb_vec); -template std::vector vector_from_fb_vector( - const flatbuffers::Vector* fb_vec); - -// ============ File operations ========================== - -uint64_t buffer_to_file(const uint8_t* buf, const uint64_t size, - const std::string& filename, bool append) { - uint64_t saved_size = 0; - - uint32_t crc_res = osf::crc32(buf, size); - - std::fstream file_stream; - if (append) - file_stream.open(filename, std::fstream::out | std::fstream::app | - std::fstream::binary); - else { - file_stream.open(filename, std::fstream::out | std::fstream::trunc | - std::fstream::binary); - } - - if (file_stream.is_open()) { - file_stream.write(reinterpret_cast(buf), size); - if (!file_stream.good()) return 0; - file_stream.write(reinterpret_cast(&crc_res), sizeof(uint32_t)); - if (!file_stream.good()) return 0; - file_stream.close(); - saved_size = size + 4; - } else { - std::cerr << "fail to open " << filename << std::endl; - } - return saved_size; -} - -uint64_t builder_to_file(flatbuffers::FlatBufferBuilder& builder, - const std::string& filename, bool append) { - // Get buffer and save to file - const uint8_t* buf = builder.GetBufferPointer(); - uint32_t size = builder.GetSize(); - return buffer_to_file(buf, size, filename, append); -} - -uint64_t start_osf_file(const std::string& filename) { - auto header_fbb = flatbuffers::FlatBufferBuilder(1024); - auto header = ouster::osf::gen::CreateHeader( - header_fbb, ouster::osf::OSF_VERSION::V_2_0, - ouster::osf::HEADER_STATUS::INVALID, 0, 0); - header_fbb.FinishSizePrefixed(header, ouster::osf::gen::HeaderIdentifier()); - return builder_to_file(header_fbb, filename, false); -} - -uint64_t finish_osf_file(const std::string& filename, - const uint64_t metadata_offset, - const uint32_t metadata_size) { - auto header_fbb = flatbuffers::FlatBufferBuilder(1024); - auto header = ouster::osf::gen::CreateHeader( - header_fbb, ouster::osf::OSF_VERSION::V_2_0, - ouster::osf::HEADER_STATUS::VALID, metadata_offset, - metadata_offset + metadata_size); - header_fbb.FinishSizePrefixed(header, ouster::osf::gen::HeaderIdentifier()); - - const uint8_t* buf = header_fbb.GetBufferPointer(); - uint32_t size = header_fbb.GetSize(); - - uint32_t crc_res = osf::crc32(buf, size); - - uint64_t saved_size = 0; - - std::ofstream file_stream; - // TODO[pb]: Need to check that file exists here and it contains the OSF - // header before overwrite it ... - file_stream.open(filename, std::fstream::out | std::fstream::in | - std::fstream::ate | std::fstream::binary); - if (file_stream.is_open()) { - file_stream.seekp(0); - - file_stream.write(reinterpret_cast(buf), size); - // TODO[pb]: It's an exception here .... add error processing - if (!file_stream.good()) return saved_size; - saved_size += size; - - file_stream.write(reinterpret_cast(&crc_res), sizeof(uint32_t)); - if (!file_stream.good()) return saved_size; - saved_size += sizeof(uint32_t); - - file_stream.close(); - } else { - std::cout << "fail to open " << filename << std::endl; - } - - return saved_size; -} - -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/fb_utils.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/fb_utils.h deleted file mode 100644 index ddfb5563..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/fb_utils.h +++ /dev/null @@ -1,127 +0,0 @@ -/** - * Copyright(c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#pragma once -#include "chunk_generated.h" -#include "header_generated.h" -#include "metadata_generated.h" -#include "ouster/osf/basics.h" - -// OSF v2 basic types for LidarSensor and LidarScan/Imu Streams -#include "os_sensor/lidar_scan_stream_generated.h" -#include "os_sensor/lidar_sensor_generated.h" - -namespace ouster { -namespace osf { - -inline const gen::Metadata* get_osf_metadata_from_buf(const uint8_t* buf) { - return ouster::osf::gen::GetSizePrefixedMetadata(buf); -} - -inline const gen::Header* get_osf_header_from_buf(const uint8_t* buf) { - return gen::GetSizePrefixedHeader(buf); -} - -/** - * Verifies the validity of Header buffer and whether it's safe to read it. - * It's just checking the well formed Flatbuffer table (not CRC32 check here) - * - * @param[in] buf Header buffer, size prefixed - * @param[in] buf_size buffer size (with prefix size bytes but not including - * CRC32) - * @return true if buffer is valid and can be read - */ -inline bool verify_osf_header_buf(const uint8_t* buf, const uint32_t buf_size) { - auto verifier = flatbuffers::Verifier(buf, buf_size); - return gen::VerifySizePrefixedHeaderBuffer(verifier); -} - -/** - * Checks the validity of a Metadata buffer and whether it's safe to read it. - * It's checking the well formed Flatbuffer table and CRC32. - * - * @param[in] buf metadata buffer, size prefixed - * @param[in] buf_size buffer size (with CRC32 and prefix size bytes) - * @return true if buffer is valid and can be read - */ -bool check_osf_metadata_buf(const uint8_t* buf, const uint32_t buf_size); - -/** - * Checks the validity of a Chunk buffer and whether it's safe to read it. - * It's checking the well formed Flatbuffer table and CRC32. - * - * @param[in] buf metadata buffer, size prefixed - * @param[in] buf_size buffer size (with CRC32 and prefix size bytes) - * @return true if buffer is valid and can be read - */ -bool check_osf_chunk_buf(const uint8_t* buf, const uint32_t buf_size); - -/** - * Transforms Flatbuffers vector to a std::vector. - * - * @tparam T The type of the vector to transform. - * - * @param[in] fb_vec The vector to transform. - * @return The transformed vector. - **/ -template -std::vector vector_from_fb_vector(const flatbuffers::Vector* fb_vec); - -// ============ File operations ========================== - -/** - * Saves the buffer content to the file with additional 4 bytes of calculated - * CRC32 field in the end. Successfull operation writes size + 4 bytes to the - * file. - * - * @param[in] buf pointer to the data to save, full content of the buffer used - * to calculate CRC - * @param[in] size number of bytes to read from buffer and store to the file - * @param[in] filename full path to the file - * @param[in] append if true appends the content to the end of the file, - * otherwise - overwrite the file with the current buffer. - * @return Number of bytes actually written to the file. Successfull write is - * size + 4 bytes (4 bytes for CRC field) - */ -uint64_t buffer_to_file(const uint8_t* buf, const uint64_t size, - const std::string& filename, bool append = false); - -/** - * Saves the content of Flatbuffer builder to the file with CRC32 field - * appended to the actual bytes. Usually it's a size prefixed finished builder - * but not necessarily - * - * @param[in] builder Flatbuffers builder - * @param[in] filename filename to save bytes - * @param[in] append if true appends the content to the end of the file, - * otherwise - overwrite the file with the current buffer. - * @return Number of bytes actually written to the file. Successfull write is - * size + 4 bytes (4 bytes for CRC field) - */ -uint64_t builder_to_file(flatbuffers::FlatBufferBuilder& builder, - const std::string& filename, bool append = false); - -/** - * Starts the OSF v2 file with a header (in INVALID state). - * - * @param[in] filename of the file to be created. Overwrite if file exists. - * @return Number of bytes actually written to the file. - */ -uint64_t start_osf_file(const std::string& filename); - -/** - * Finish OSF v2 file with updated offset to metadata and filesize. As a - * result file left in VALID state. - * - * @param[in] filename of the file to be created. Overwrite if file exists. - * @param[in] metadata_offset The offset to the metadata blob. - * @param[in] metadata_size The size of the metadata blob. - * @return Number of bytes actually written to the file. - */ -uint64_t finish_osf_file(const std::string& filename, - const uint64_t metadata_offset, - const uint32_t metadata_size); -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/file.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/file.cpp deleted file mode 100644 index 70011abf..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/file.cpp +++ /dev/null @@ -1,390 +0,0 @@ -/** - * Copyright(c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#include "ouster/osf/file.h" - -#include -#include -#include -#include -#include -#include - -#include "compat_ops.h" -#include "fb_utils.h" -#include "ouster/osf/crc32.h" - -namespace ouster { -namespace osf { - -namespace { - -// Print errors only in DEBUG mode -#ifndef NDEBUG -inline void print_error(const std::string& filename, const std::string& msg) { - fprintf(stderr, "Error Osf[%s]: %s\n", filename.c_str(), msg.c_str()); -} -#else -#define print_error(a, b) ((void)0) -#endif - -} // namespace - -// ======== Construction ============ - -OsfFile::OsfFile() - : filename_(), - offset_(0), - size_(0), - file_buf_(nullptr), - file_stream_{}, - header_chunk_{nullptr}, - metadata_chunk_{nullptr}, - chunk_cache_{nullptr}, - chunk_cache_offset_{std::numeric_limits::max()}, - state_(FileState::BAD) {} - -OsfFile::OsfFile(const std::string& filename, OpenMode mode) : OsfFile() { - filename_ = filename; - - // TODO[pb]: Extract to open function - if (mode == OpenMode::READ) { - if (is_dir(filename_)) { - error("got a dir, but expected a file"); - return; - } - - int64_t sz = file_size(filename_); - if (sz <= 0) { - error(); - return; - } - // TODO[pb]: This leads to incorrect file size for 4Gb+ files on the - // 32 bit systems like Emscripten/WASM. We need to check - // size_t everywhere and replace it so it can hold file - // sizes and file offsets bigger than 4Gb in 32 bit systems. - size_ = static_cast(sz); - -#ifdef OUSTER_OSF_NO_MMAP - // TODO[pb]: Maybe consider adding a runtime parameter to open file - // with mmap or open/read? Also better handling/removing of class - // members for OsfFile can be done so we are not copying empty - // values when NO_MMAP is absent... But I can't make my mind - // about RUNTIME/COMPILATION time parametrization and for now - // will leave it in a half backed state: COMPILE time directive - // but some fields will be left empty and copied/check during runtime - // there is no hit in performance/memory due to this leftovers - // that I could spot. - file_stream_ = - std::ifstream(filename_, std::ios::in | std::ios::binary); - if (!file_stream_.good()) { - error(); - return; - } -#else - file_buf_ = mmap_open(filename_); - if (!file_buf_) { - error(); - return; - } -#endif - - state_ = FileState::GOOD; - } else { - // Write is not yet implemented within this class. And other modes - // too. - error("write mode not implemented"); - return; - } -} - -uint64_t OsfFile::size() const { return size_; }; - -std::string OsfFile::filename() const { return filename_; } - -OSF_VERSION OsfFile::version() { - if (!good()) { - return OSF_VERSION::V_INVALID; - } - auto osf_header = get_osf_header_from_buf(get_header_chunk_ptr()); - return static_cast(osf_header->version()); -} - -uint64_t OsfFile::metadata_offset() { - if (!good()) throw std::logic_error("bad osf file"); - auto osf_header = get_osf_header_from_buf(get_header_chunk_ptr()); - return osf_header->metadata_offset(); -} - -uint64_t OsfFile::chunks_offset() { - if (!good()) throw std::logic_error("bad osf file"); - const uint32_t header_size = get_prefixed_size(get_header_chunk_ptr()); - if (version() < OSF_VERSION::V_2_0) { - throw std::logic_error("bad osf file: only version >= 20 supported"); - } - return FLATBUFFERS_PREFIX_LENGTH + header_size + osf::CRC_BYTES_SIZE; -} - -bool OsfFile::valid() { - if (!good()) { - return false; - } - - uint32_t header_size = - get_prefixed_size(get_header_chunk_ptr()) + FLATBUFFERS_PREFIX_LENGTH; - - // Check flatbuffers osfHeader validity - if (!verify_osf_header_buf(get_header_chunk_ptr(), header_size)) { - print_error(filename_, "OSF header verification has failed."); - return false; - } - - if (!check_prefixed_size_block_crc(get_header_chunk_ptr(), - header_size + CRC_BYTES_SIZE)) { - print_error(filename_, "OSF header has an invalid CRC."); - return false; - } - - auto osf_header = get_osf_header_from_buf(get_header_chunk_ptr()); - if (osf_header->status() != v2::HEADER_STATUS::VALID) { - print_error(filename_, "OSF header is not valid."); - return false; - } - - if (osf_header->file_length() != size_) { - std::stringstream ss; - ss << "OSF file size does not match the stored value"; - ss << " Expected: " << size_; - ss << " Actual: " << osf_header->file_length(); - print_error(filename_, ss.str()); - return false; - } - - uint64_t metadata_offset = osf_header->metadata_offset(); - - if (osf_header->version() < OSF_VERSION::V_2_0) { - // Check flatbuffers osfSession validity [V1] - print_error(filename_, "OSF prior version 2.0 is not supported!"); - return false; - } - - // Check flatbuffers metadata validity - if (!ouster::osf::check_osf_metadata_buf(get_metadata_chunk_ptr(), - size_ - metadata_offset)) { - print_error(filename_, "OSF metadata verification has failed."); - return false; - } - - return true; -} - -bool OsfFile::good() const { return state_ == FileState::GOOD; } - -bool OsfFile::operator!() const { return !good(); }; - -OsfFile::operator bool() const { return good(); }; - -uint64_t OsfFile::offset() const { return offset_; } -// ========= Geneal Data Access ============= - -OsfFile& OsfFile::seek(uint64_t pos) { - if (!good()) throw std::logic_error("bad osf file"); - if (pos > size_) { - std::stringstream ss; - ss << "seek for " << pos << " but the file size is " << size_; - throw std::out_of_range(ss.str()); - } - if (file_stream_.is_open()) { - file_stream_.seekg(pos); - } - offset_ = pos; - return *this; -} - -OsfFile& OsfFile::read(uint8_t* buf, const uint64_t count) { - // TODO[pb]: Check for errors in full implementation - // and set error flags - // TODO[pb]: Read from disk if it's not mmap? (buffering, etc to be - // considered later) - - // Now we just copy from the mapped region from the current offset - // and advance offset further. - if (!good()) throw std::logic_error("bad osf file"); - if (offset_ + count > size_) { - std::stringstream ss; - ss << "read till " << (offset_ + count) << " but the file size is " - << size_; - throw std::out_of_range(ss.str()); - } - if (file_stream_.is_open()) { - file_stream_.read(reinterpret_cast(buf), count); - offset_ = file_stream_.tellg(); - } else if (file_buf_ != nullptr) { - std::memcpy(buf, file_buf_ + offset_, count); - offset_ += count; - } - return *this; -} - -// ===== Mmapped access to the file content memory ===== - -const uint8_t* OsfFile::buf(const uint64_t offset) const { - if (!good()) throw std::logic_error("bad osf file"); - if (!is_memory_mapped()) throw std::logic_error("not a mmap file"); - if (offset >= size_) - throw std::out_of_range("out of range osf file access"); - return file_buf_ + offset; -} - -bool OsfFile::is_memory_mapped() const { return file_buf_ != nullptr; } - -// ======= Helpers ============= - -void OsfFile::error(const std::string& msg) { - state_ = FileState::BAD; - if (!msg.empty()) { - print_error(filename_, msg); - } else { - print_error(filename_, get_last_error()); - } -} - -std::string OsfFile::to_string() { - std::stringstream ss; - ss << "OsfFile [filename = '" << filename_ << "', " - << "state = " << static_cast(state_) << ", " - << "version = " << version() << ", " - << "size = " << size_ << ", offset = " << offset_; - if (this->good()) { - auto osf_header = get_osf_header_from_buf(get_header_chunk_ptr()); - ss << ", osf.file_length = " << osf_header->file_length() << ", " - << "osf.metadata_offset = " << osf_header->metadata_offset() << ", " - << "osf.status = " << static_cast(osf_header->status()); - } - ss << "]"; - return ss.str(); -} - -// ======= Move semantics =================== - -OsfFile::OsfFile(OsfFile&& other) - : filename_(other.filename_), - offset_(other.offset_), - size_(other.size_), - file_buf_(other.file_buf_), - file_stream_(std::move(other.file_stream_)), - header_chunk_(std::move(other.header_chunk_)), - metadata_chunk_(std::move(other.metadata_chunk_)), - state_(other.state_) { - other.file_buf_ = nullptr; - other.state_ = FileState::BAD; -} - -OsfFile& OsfFile::operator=(OsfFile&& other) { - if (this != &other) { - close(); - filename_ = other.filename_; - offset_ = other.offset_; - size_ = other.size_; - file_buf_ = other.file_buf_; - file_stream_ = std::move(other.file_stream_); - header_chunk_ = std::move(other.header_chunk_); - metadata_chunk_ = std::move(other.metadata_chunk_); - state_ = other.state_; - other.file_buf_ = nullptr; - other.state_ = FileState::BAD; - } - return *this; -}; - -// ========= Release resources ================= - -void OsfFile::close() { - if (file_buf_) { - if (!mmap_close(file_buf_, size_)) { - error(); - return; - } - file_buf_ = nullptr; - state_ = FileState::BAD; - } - if (file_stream_.is_open()) { - file_stream_.close(); - if (file_stream_.fail()) { - error(); - return; - } - state_ = FileState::BAD; - } -} - -OsfFile::~OsfFile() { - // Release file memory mapping - close(); -} - -std::shared_ptr OsfFile::read_chunk(const uint64_t offset) { - if (!good()) { - return nullptr; - } - // check whether it was read last and we have it in cache already - if (chunk_cache_offset_ == offset && chunk_cache_) { - return chunk_cache_; - } - auto chunk_buf = std::make_shared(FLATBUFFERS_PREFIX_LENGTH); - seek(offset); - read(chunk_buf->data(), FLATBUFFERS_PREFIX_LENGTH); - uint32_t full_chunk_size = get_prefixed_size(chunk_buf->data()) + - FLATBUFFERS_PREFIX_LENGTH + CRC_BYTES_SIZE; - if (offset + full_chunk_size > size_) { - std::stringstream ss; - ss << "read till " << (offset + full_chunk_size) - << " but the file size is " << size_; - throw std::out_of_range(ss.str()); - } - chunk_buf->resize(full_chunk_size); - read(chunk_buf->data() + FLATBUFFERS_PREFIX_LENGTH, - full_chunk_size - FLATBUFFERS_PREFIX_LENGTH); - - // update cached chunk - if (chunk_cache_) { - chunk_cache_.swap(chunk_buf); - } else { - chunk_cache_ = std::move(chunk_buf); - } - chunk_cache_offset_ = offset; - - return chunk_cache_; -} - -uint8_t* OsfFile::get_header_chunk_ptr() { - if (!file_stream_.good()) { - if (header_chunk_) header_chunk_.reset(); - return nullptr; - } - if (header_chunk_) return header_chunk_->data(); - - auto tmp_offset = offset_; - header_chunk_ = read_chunk(0); - seek(tmp_offset); - return header_chunk_->data(); -} - -uint8_t* OsfFile::get_metadata_chunk_ptr() { - uint64_t meta_offset = metadata_offset(); - if (!file_stream_.good()) { - if (metadata_chunk_) metadata_chunk_.reset(); - return nullptr; - } - if (metadata_chunk_) return metadata_chunk_->data(); - - auto tmp_offset = offset_; - metadata_chunk_ = read_chunk(meta_offset); - seek(tmp_offset); - return metadata_chunk_->data(); -} - -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/json_utils.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/json_utils.cpp deleted file mode 100644 index 2a3a9cfa..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/json_utils.cpp +++ /dev/null @@ -1,31 +0,0 @@ -/** - * Copyright(c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#include "json_utils.h" - -#include - -namespace ouster { -namespace osf { - -bool parse_json(const std::string json_str, Json::Value& output) { - // Parse Json - Json::CharReaderBuilder jbuilder{}; - jbuilder["collectComments"] = false; - std::stringstream source{json_str}; - std::string jerrs; - return Json::parseFromStream(jbuilder, source, &output, &jerrs); -} - -std::string json_string(const Json::Value& root) { - Json::StreamWriterBuilder builder; - builder["enableYAMLCompatibility"] = true; - builder["precision"] = 6; - builder["indentation"] = " "; - return Json::writeString(builder, root); -} - -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/json_utils.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/json_utils.h deleted file mode 100644 index a4da6bb6..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/json_utils.h +++ /dev/null @@ -1,18 +0,0 @@ -/** - * Copyright(c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#pragma once - -#include "json/json.h" - -namespace ouster { -namespace osf { - -bool parse_json(const std::string json_str, Json::Value& output); - -std::string json_string(const Json::Value& root); - -} // namespace osf -} // namespace ouster \ No newline at end of file diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/layout_streaming.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/layout_streaming.cpp deleted file mode 100644 index d4ba8caa..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/layout_streaming.cpp +++ /dev/null @@ -1,85 +0,0 @@ -/** - * Copyright(c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#include "ouster/osf/layout_streaming.h" - -#include - -#include "ouster/osf/meta_streaming_info.h" -#include "ouster/osf/writer.h" - -namespace ouster { -namespace osf { - -StreamingLayoutCW::StreamingLayoutCW(Writer& writer, uint32_t chunk_size) - : chunk_size_{chunk_size ? chunk_size : STREAMING_DEFAULT_CHUNK_SIZE}, - writer_{writer} {} - -void StreamingLayoutCW::save_message(const uint32_t stream_id, const ts_t ts, - const std::vector& msg_buf) { - if (!chunk_builders_.count(stream_id)) { - chunk_builders_.insert({stream_id, std::make_shared()}); - } - - auto chunk_builder = chunk_builders_[stream_id]; - - // checking non-decreasing invariant of chunks and messages - if (chunk_builder->end_ts() > ts) { - std::stringstream err; - err << "ERROR: Can't write with a decreasing timestamp: " << ts.count() - << " for stream_id: " << stream_id - << " ( previous recorded timestamp: " - << chunk_builder->end_ts().count() << ")"; - throw std::logic_error(err.str()); - } - - if (chunk_builder->size() + msg_buf.size() > chunk_size_) { - finish_chunk(stream_id, chunk_builder); - } - - chunk_builder->save_message(stream_id, ts, msg_buf); - - // update running statistics per stream - stats_message(stream_id, ts, msg_buf); -} - -void StreamingLayoutCW::finish() { - for (auto& cb_it : chunk_builders_) { - finish_chunk(cb_it.first, cb_it.second); - } - - writer_.add_metadata(StreamingInfo{ - chunk_stream_id_, {stream_stats_.begin(), stream_stats_.end()}}); -} - -uint32_t StreamingLayoutCW::chunk_size() const { return chunk_size_; } - -void StreamingLayoutCW::stats_message(const uint32_t stream_id, const ts_t ts, - const std::vector& msg_buf) { - auto msg_size = static_cast(msg_buf.size()); - auto stats_it = stream_stats_.find(stream_id); - if (stats_it == stream_stats_.end()) { - stream_stats_.insert({stream_id, StreamStats(stream_id, ts, msg_size)}); - } else { - stats_it->second.update(ts, msg_size); - } -} - -void StreamingLayoutCW::finish_chunk( - uint32_t stream_id, const std::shared_ptr& chunk_builder) { - std::vector bb = chunk_builder->finish(); - if (!bb.empty()) { - uint64_t chunk_offset = writer_.emit_chunk(chunk_builder->start_ts(), - chunk_builder->end_ts(), bb); - chunk_stream_id_.emplace_back( - chunk_offset, ChunkInfo{chunk_offset, stream_id, - chunk_builder->messages_count()}); - } - - // Prepare for the new chunk messages - chunk_builder->reset(); -} -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/meta_extrinsics.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/meta_extrinsics.cpp deleted file mode 100644 index f9ca941c..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/meta_extrinsics.cpp +++ /dev/null @@ -1,77 +0,0 @@ -/** - * Copyright(c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#include "ouster/osf/meta_extrinsics.h" - -#include "flatbuffers/flatbuffers.h" -#include "os_sensor/extrinsics_generated.h" - -namespace ouster { -namespace osf { - -Extrinsics::Extrinsics(const mat4d& extrinsics, uint32_t ref_meta_id, - const std::string& name) - : extrinsics_(extrinsics), ref_meta_id_{ref_meta_id}, name_{name} {} - -const mat4d& Extrinsics::extrinsics() const { return extrinsics_; } - -const std::string& Extrinsics::name() const { return name_; } - -uint32_t Extrinsics::ref_meta_id() const { return ref_meta_id_; } - -std::vector Extrinsics::buffer() const { - flatbuffers::FlatBufferBuilder fbb = flatbuffers::FlatBufferBuilder(256); - std::vector extrinsic_vec(16); - - // Changing column-major data layout to row-major before storing to OSF buf - // Not using internal mat4d::data() reprensentation here because we - // don't always control how it is created. - for (size_t i = 0; i < 4; i++) { - for (size_t j = 0; j < 4; j++) { - extrinsic_vec[4 * i + j] = extrinsics_(i, j); - } - } - auto ext_offset = osf::gen::CreateExtrinsicsDirect( - fbb, &extrinsic_vec, ref_meta_id_, - name_.empty() ? nullptr : name_.c_str()); - osf::gen::FinishSizePrefixedExtrinsicsBuffer(fbb, ext_offset); - const uint8_t* buf = fbb.GetBufferPointer(); - const uint32_t size = fbb.GetSize(); - return {buf, buf + size}; -}; - -std::unique_ptr Extrinsics::from_buffer( - const std::vector& buf) { - auto ext_fb = gen::GetSizePrefixedExtrinsics(buf.data()); - if (!ext_fb) return nullptr; - std::string name; - if (ext_fb->name()) { - name = ext_fb->name()->str(); - } - mat4d ext_mat = mat4d::Identity(); - if (ext_fb->extrinsics() && ext_fb->extrinsics()->size() == 16) { - for (uint32_t i = 0; i < ext_fb->extrinsics()->size(); ++i) { - uint32_t r = i / 4; - uint32_t c = i % 4; - ext_mat(r, c) = ext_fb->extrinsics()->Get(i); - } - } - return std::make_unique(ext_mat, ext_fb->ref_id(), name); -} - -std::string Extrinsics::repr() const { - std::stringstream ss; - ss << "ExtrinsicsMeta: ref_id = " << ref_meta_id_ << ", name = " << name_ - << ", extrinsics ="; - for (size_t i = 0; i < 4; ++i) { - for (size_t j = 0; j < 4; ++j) { - ss << " " << extrinsics_(i, j); - } - } - return ss.str(); -}; - -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/meta_lidar_sensor.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/meta_lidar_sensor.cpp deleted file mode 100644 index 6162a68c..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/meta_lidar_sensor.cpp +++ /dev/null @@ -1,103 +0,0 @@ -/** - * Copyright(c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#include "ouster/osf/meta_lidar_sensor.h" - -#include "fb_utils.h" -#include "flatbuffers/flatbuffers.h" -#include "json_utils.h" -#include "ouster/osf/basics.h" - -using sensor_info = ouster::sensor::sensor_info; - -namespace ouster { -namespace osf { - -// === Lidar Sensor stream/msgs functions ==================== - -/** - * Internal helper function for creating flatbuffer blobs to represent - * LidarSensor objects. - * - * @param[in] fbb The flatbufferbuilder to use when generating the blob. - * @param[in] sensor_metadata ///< The json string representation of the - * ///< sensor_info to use when creating - * ///< the flatbuffer blob. - * @return The offset pointer inside the flatbufferbuilder to the new section. - */ -flatbuffers::Offset create_lidar_sensor( - flatbuffers::FlatBufferBuilder& fbb, const std::string& sensor_metadata) { - auto ls_offset = - ouster::osf::gen::CreateLidarSensorDirect(fbb, sensor_metadata.c_str()); - return ls_offset; -} - -/** - * Internal helper function for restoring a LidarSensor's json string - * representation of a sensor_info from a raw flatbuffer byte vector. - * - * @param[in] buf The flatbuffer byte vector. - * @return ///< The json string representation of the sensor_info object - * ///< contained within the flatbuffer blob. - */ - -std::unique_ptr restore_lidar_sensor( - const std::vector buf) { - auto lidar_sensor = v2::GetSizePrefixedLidarSensor(buf.data()); - - std::string sensor_metadata{}; - if (lidar_sensor->metadata()) - sensor_metadata = lidar_sensor->metadata()->str(); - - return std::make_unique(sensor_metadata); -} - -LidarSensor::LidarSensor(const sensor_info& si) - : sensor_info_(si), metadata_(si.updated_metadata_string()) {} - -LidarSensor::LidarSensor(const std::string& sensor_metadata) - : sensor_info_(sensor::parse_metadata(sensor_metadata)), - metadata_(sensor_metadata) {} - -const sensor_info& LidarSensor::info() const { return sensor_info_; } - -const std::string& LidarSensor::metadata() const { return metadata_; } - -std::vector LidarSensor::buffer() const { - flatbuffers::FlatBufferBuilder fbb = flatbuffers::FlatBufferBuilder(32768); - auto ls_offset = create_lidar_sensor(fbb, metadata_); - fbb.FinishSizePrefixed(ls_offset, - ouster::osf::gen::LidarSensorIdentifier()); - const uint8_t* buf = fbb.GetBufferPointer(); - const uint32_t size = fbb.GetSize(); - return {buf, buf + size}; -}; - -std::unique_ptr LidarSensor::from_buffer( - const std::vector& buf) { - auto sensor_metadata = restore_lidar_sensor(buf); - if (sensor_metadata) { - return std::make_unique(*sensor_metadata); - } - return nullptr; -} - -std::string LidarSensor::repr() const { - Json::Value lidar_sensor_obj{}; - Json::Value sensor_info_obj{}; - - if (!parse_json(metadata_, sensor_info_obj)) { - lidar_sensor_obj["sensor_info"] = metadata_; - } else { - lidar_sensor_obj["sensor_info"] = sensor_info_obj; - } - - return json_string(lidar_sensor_obj); -}; - -std::string LidarSensor::to_string() const { return repr(); }; - -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/meta_streaming_info.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/meta_streaming_info.cpp deleted file mode 100644 index e8ee7040..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/meta_streaming_info.cpp +++ /dev/null @@ -1,177 +0,0 @@ -/** - * Copyright(c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#include "ouster/osf/meta_streaming_info.h" - -#include -#include - -#include "json/json.h" -#include "json_utils.h" -#include "ouster/osf/meta_streaming_info.h" -#include "streaming/streaming_info_generated.h" - -namespace ouster { -namespace osf { - -std::string to_string(const ChunkInfo& chunk_info) { - std::stringstream ss; - ss << "{offset = " << chunk_info.offset - << ", stream_id = " << chunk_info.stream_id - << ", message_count = " << chunk_info.message_count << "}"; - return ss.str(); -} - -StreamStats::StreamStats(uint32_t s_id, ts_t t, uint32_t msg_size) - : stream_id{s_id}, - start_ts{t}, - end_ts{t}, - message_count{1}, - message_avg_size{msg_size} {}; - -void StreamStats::update(ts_t t, uint32_t msg_size) { - if (start_ts > t) start_ts = t; - if (end_ts < t) end_ts = t; - ++message_count; - int avg_size = static_cast(message_avg_size); - avg_size = avg_size + (static_cast(msg_size) - avg_size) / - static_cast(message_count); - message_avg_size = static_cast(avg_size); -} - -std::string to_string(const StreamStats& stream_stats) { - std::stringstream ss; - ss << "{stream_id = " << stream_stats.stream_id - << ", start_ts = " << stream_stats.start_ts.count() - << ", end_ts = " << stream_stats.end_ts.count() - << ", message_count = " << stream_stats.message_count - << ", message_avg_size = " << stream_stats.message_avg_size << "}"; - return ss.str(); -} - -flatbuffers::Offset create_streaming_info( - flatbuffers::FlatBufferBuilder& fbb, - const std::map& chunks_info, - const std::map& stream_stats) { - // Pack chunks vector - std::vector> chunks_info_vec; - for (const auto& chunk_info : chunks_info) { - const auto& ci = chunk_info.second; - auto ci_offset = gen::CreateChunkInfo(fbb, ci.offset, ci.stream_id, - ci.message_count); - chunks_info_vec.push_back(ci_offset); - } - - // Pack stream_stats vector - std::vector> stream_stats_vec; - for (const auto& stream_stat : stream_stats) { - auto stat = stream_stat.second; - auto ss_offset = gen::CreateStreamStats( - fbb, stat.stream_id, stat.start_ts.count(), stat.end_ts.count(), - stat.message_count, stat.message_avg_size); - stream_stats_vec.push_back(ss_offset); - } - - auto si_offset = ouster::osf::gen::CreateStreamingInfoDirect( - fbb, &chunks_info_vec, &stream_stats_vec); - return si_offset; -} - -StreamingInfo::StreamingInfo( - const std::vector>& chunks_info, - const std::vector>& stream_stats) - : chunks_info_{chunks_info.begin(), chunks_info.end()}, - stream_stats_{stream_stats.begin(), stream_stats.end()} {} - -StreamingInfo::StreamingInfo( - const std::map& chunks_info, - const std::map& stream_stats) - : chunks_info_(chunks_info), stream_stats_(stream_stats) {} - -std::map& StreamingInfo::chunks_info() { - return chunks_info_; -} - -std::map& StreamingInfo::stream_stats() { - return stream_stats_; -} - -std::vector StreamingInfo::buffer() const { - flatbuffers::FlatBufferBuilder fbb = flatbuffers::FlatBufferBuilder(32768); - auto si_offset = create_streaming_info(fbb, chunks_info_, stream_stats_); - fbb.FinishSizePrefixed(si_offset); - const uint8_t* buf = fbb.GetBufferPointer(); - const size_t size = fbb.GetSize(); - return {buf, buf + size}; -}; - -std::unique_ptr StreamingInfo::from_buffer( - const std::vector& buf) { - auto streaming_info = gen::GetSizePrefixedStreamingInfo(buf.data()); - - std::unique_ptr si = std::make_unique(); - - auto& chunks_info = si->chunks_info(); - if (streaming_info->chunks() && streaming_info->chunks()->size()) { - std::transform( - streaming_info->chunks()->begin(), streaming_info->chunks()->end(), - std::inserter(chunks_info, chunks_info.end()), - [](const gen::ChunkInfo* ci) { - return std::make_pair(ci->offset(), - ChunkInfo{ci->offset(), ci->stream_id(), - ci->message_count()}); - }); - } - - auto& stream_stats = si->stream_stats(); - if (streaming_info->stream_stats() && - streaming_info->stream_stats()->size()) { - std::transform(streaming_info->stream_stats()->begin(), - streaming_info->stream_stats()->end(), - std::inserter(stream_stats, stream_stats.end()), - [](const gen::StreamStats* stat) { - StreamStats ss{}; - ss.stream_id = stat->stream_id(); - ss.start_ts = ts_t{stat->start_ts()}; - ss.end_ts = ts_t{stat->end_ts()}; - ss.message_count = stat->message_count(); - ss.message_avg_size = stat->message_avg_size(); - return std::make_pair(stat->stream_id(), ss); - }); - } - - return si; -} - -std::string StreamingInfo::repr() const { - Json::Value si_obj{}; - - si_obj["chunks"] = Json::arrayValue; - for (const auto& ci : chunks_info_) { - Json::Value chunk_info{}; - chunk_info["offset"] = static_cast(ci.second.offset); - chunk_info["stream_id"] = ci.second.stream_id; - chunk_info["message_count"] = ci.second.message_count; - si_obj["chunks"].append(chunk_info); - } - - si_obj["stream_stats"] = Json::arrayValue; - for (const auto& stat : stream_stats_) { - Json::Value ss{}; - ss["stream_id"] = stat.first; - ss["start_ts"] = - static_cast(stat.second.start_ts.count()); - ss["end_ts"] = static_cast(stat.second.end_ts.count()); - ss["message_count"] = - static_cast(stat.second.message_count); - ss["message_avg_size"] = stat.second.message_avg_size; - si_obj["stream_stats"].append(ss); - } - - return json_string(si_obj); -}; - -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/metadata.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/metadata.cpp deleted file mode 100644 index 02623534..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/metadata.cpp +++ /dev/null @@ -1,153 +0,0 @@ -/** - * Copyright(c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#include "ouster/osf/metadata.h" - -#include "fb_utils.h" - -namespace ouster { -namespace osf { - -std::string MetadataEntry::repr() const { - auto b = this->buffer(); - std::stringstream ss; - ss << "MetadataEntry: " - << (b.size() ? osf::to_string(b.data(), b.size(), 50) : ""); - return ss.str(); -}; - -std::string MetadataEntry::to_string() const { - std::stringstream ss; - ss << "MetadataEntry: [" - << "id = " << id() << ", type = " << type() << ", buffer = {" - << this->repr() << "}" - << "]"; - return ss.str(); -} - -void MetadataEntry::setId(uint32_t id) { id_ = id; } -uint32_t MetadataEntry::id() const { return id_; } - -flatbuffers::Offset MetadataEntry::make_entry( - flatbuffers::FlatBufferBuilder& fbb) const { - auto buf = this->buffer(); - return ouster::osf::gen::CreateMetadataEntryDirect(fbb, id(), - type().c_str(), &buf); -} - -std::unique_ptr MetadataEntry::from_buffer( - const std::vector& buf, const std::string type_str) { - auto& registry = MetadataEntry::get_registry(); - auto registered_type = registry.find(type_str); - if (registered_type == registry.end()) { - std::cout << "UNKNOWN TYPE: " << type_str << std::endl; - return nullptr; - } - auto m = registered_type->second(buf); - if (m == nullptr) { - std::cout << "UNRECOVERABLE FROM BUFFER as type: " << type_str - << std::endl; - return nullptr; - } - return m; -}; - -std::map& -MetadataEntry::get_registry() { - static std::map registry_; - return registry_; -} - -MetadataEntryRef::MetadataEntryRef(const uint8_t* buf) : buf_{buf} { - const gen::MetadataEntry* meta_entry = - reinterpret_cast(buf_); - buf_type_ = meta_entry->type()->str(); - setId(meta_entry->id()); -} - -std::string MetadataEntryRef::type() const { return buf_type_; } - -std::string MetadataEntryRef::static_type() const { - return metadata_type(); -} - -std::unique_ptr MetadataEntryRef::clone() const { - return std::make_unique(*this); -} - -std::vector MetadataEntryRef::buffer() const { - const gen::MetadataEntry* meta_entry = - reinterpret_cast(buf_); - return vector_from_fb_vector(meta_entry->buffer()); -} - -std::unique_ptr MetadataEntryRef::as_type() const { - auto& registry = MetadataEntry::get_registry(); - auto registered_type = registry.find(type()); - if (registered_type == registry.end()) { - std::cout << "UNKNOWN TYPE FOUND: " << type() << std::endl; - return nullptr; - } - auto m = registered_type->second(buffer()); - if (m == nullptr) { - std::cout << "UNRECOVERABLE FROM BUFFER: " << to_string() << std::endl; - return nullptr; - } - m->setId(id()); - return m; -} - -void MetadataEntryRef::setId(uint32_t id) { MetadataEntry::setId(id); } - -std::vector> -MetadataStore::make_entries(flatbuffers::FlatBufferBuilder& fbb) const { - using FbEntriesVector = - std::vector>; - FbEntriesVector entries; - for (const auto& md : metadata_entries_) { - auto entry_offset = md.second->make_entry(fbb); - entries.push_back(entry_offset); - } - return entries; -} - -uint32_t MetadataStore::add(MetadataEntry&& entry) { return add(entry); } - -uint32_t MetadataStore::add(MetadataEntry& entry) { - if (entry.id() == 0) { - /// @todo [pb]: Figure out the whole sequence of ids in addMetas in - /// the Reader case - assignId(entry); - } else if (metadata_entries_.find(entry.id()) != metadata_entries_.end()) { - std::cout << "WARNING: MetadataStore: ENTRY EXISTS! id = " << entry.id() - << std::endl; - return entry.id(); - } else if (next_meta_id_ == entry.id()) { - // Find next available next_meta_id_ so we avoid id collisions - ++next_meta_id_; - auto next_it = metadata_entries_.lower_bound(next_meta_id_); - while (next_it != metadata_entries_.end() && - next_it->first == next_meta_id_) { - ++next_meta_id_; - next_it = metadata_entries_.lower_bound(next_meta_id_); - } - } - - metadata_entries_.emplace(entry.id(), entry.clone()); - return entry.id(); -} - -size_t MetadataStore::size() const { return metadata_entries_.size(); } - -const MetadataStore::MetadataEntriesMap& MetadataStore::entries() const { - return metadata_entries_; -} - -void MetadataStore::assignId(MetadataEntry& entry) { - entry.setId(next_meta_id_++); -} - -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/operations.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/operations.cpp deleted file mode 100644 index b1522335..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/operations.cpp +++ /dev/null @@ -1,262 +0,0 @@ -/** - * Copyright(c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#include "ouster/osf/operations.h" - -#include -#include -#include - -#include "compat_ops.h" -#include "fb_utils.h" -#include "json/json.h" -#include "json_utils.h" -#include "ouster/lidar_scan.h" -#include "ouster/osf/file.h" -#include "ouster/osf/meta_extrinsics.h" -#include "ouster/osf/meta_lidar_sensor.h" -#include "ouster/osf/pcap_source.h" -#include "ouster/osf/reader.h" -#include "ouster/osf/stream_lidar_scan.h" -#include "ouster/osf/writer.h" - -namespace ouster { -namespace osf { - -std::string dump_metadata(const std::string& file, bool full) { - OsfFile osf_file(file); - auto osf_header = get_osf_header_from_buf(osf_file.get_header_chunk_ptr()); - - Json::Value root{}; - - root["header"]["size"] = static_cast(osf_file.size()); - root["header"]["version"] = static_cast(osf_file.version()); - root["header"]["status"] = to_string(osf_header->status()); - root["header"]["metadata_offset"] = - static_cast(osf_file.metadata_offset()); - root["header"]["chunks_offset"] = - static_cast(osf_file.chunks_offset()); - - Reader reader(file); - - root["metadata"]["id"] = reader.metadata_id(); - root["metadata"]["start_ts"] = - static_cast(reader.start_ts().count()); - root["metadata"]["end_ts"] = - static_cast(reader.end_ts().count()); - - auto osf_metadata = - get_osf_metadata_from_buf(osf_file.get_metadata_chunk_ptr()); - - if (full) { - root["metadata"]["chunks"] = Json::arrayValue; - for (size_t i = 0; i < osf_metadata->chunks()->size(); ++i) { - auto osf_chunk = osf_metadata->chunks()->Get(i); - Json::Value chunk{}; - chunk["start_ts"] = - static_cast(osf_chunk->start_ts()); - chunk["end_ts"] = static_cast(osf_chunk->end_ts()); - chunk["offset"] = static_cast(osf_chunk->offset()); - root["metadata"]["chunks"].append(chunk); - } - } - - const MetadataStore& meta_store = reader.meta_store(); - - root["metadata"]["entries"] = Json::arrayValue; - - for (const auto& me : meta_store.entries()) { - Json::Value meta_element{}; - meta_element["id"] = static_cast(me.first); - meta_element["type"] = me.second->type(); - - if (full) { - const std::string me_str = me.second->repr(); - Json::Value me_obj{}; - if (parse_json(me_str, me_obj)) { - meta_element["buffer"] = me_obj; - } else { - meta_element["buffer"] = me_str; - } - } - - root["metadata"]["entries"].append(meta_element); - } - - return json_string(root); -} - -void parse_and_print(const std::string& file, bool with_decoding) { - OsfFile osf_file{file}; - - using ouster::osf::LidarScanStream; - using ouster::osf::LidarSensor; - - std::cout << "OSF v2:" << std::endl; - std::cout << " file = " << osf_file.to_string() << std::endl; - - ouster::osf::Reader reader(osf_file); - - int ls_c = 0; - int other_c = 0; - - // TODO[pb]: Remove the SIGINT handlers from C++ wrapped function used in - // Python bindings - // https://pybind11.readthedocs.io/en/stable/faq.html#how-can-i-properly-handle-ctrl-c-in-long-running-functions - thread_local std::atomic_bool quit{false}; - auto sig = std::signal(SIGINT, [](int) { quit = true; }); - - for (const auto msg : reader.messages()) { - if (msg.is()) { - std::cout << " Ls ts: " << msg.ts().count() - << ", stream_id = " << msg.id(); - ++ls_c; - - // Example of code to get an object - if (with_decoding) { - auto obj_ls = msg.decode_msg(); - if (obj_ls) { - std::cout << " [D]"; - } - } - - std::cout << std::endl; - - } else { - // UNKNOWN Message - std::cout << " UK ts: " << msg.ts().count() - << ", stream_id = " << msg.id() << std::endl; - ++other_c; - } - - if (quit) { - std::cout << "Stopped early via SIGINT!" << std::endl; - break; - } - } - - // restore signal handler - std::signal(SIGINT, sig); - - std::cout << "\nSUMMARY (OSF v2): \n"; - std::cout << " lidar_scan (Ls) count = " << ls_c << std::endl; - std::cout << " other (NOT IMPLEMENTED) count = " << other_c << std::endl; -} - -int64_t backup_osf_file_metablob(const std::string& osf_file_name, - const std::string& backup_file_name) { - uint64_t metadata_offset = 0; - { - OsfFile osf_file{osf_file_name}; - metadata_offset = osf_file.metadata_offset(); - } - - // Backup the current metadata blob - return copy_file_trailing_bytes(osf_file_name, backup_file_name, - metadata_offset); -} - -int64_t restore_osf_file_metablob(const std::string& osf_file_name, - const std::string& backup_file_name) { - uint64_t metadata_offset = 0; - { - OsfFile osf_file{osf_file_name}; - metadata_offset = osf_file.metadata_offset(); - } - truncate_file(osf_file_name, metadata_offset); - auto result = append_binary_file(osf_file_name, backup_file_name); - - if (result > 0) { - finish_osf_file(osf_file_name, metadata_offset, - result - metadata_offset); - } else { - return -1; - } - return result; -} - -/** - * Internal simplification function for generating modified - * metadata flatbuffer blobs. - * - * @param[in] file_name Filename of the OSF file to modify - * @param[in] new_metadata List of new sensor infos to populate - * @return The generated flatbuffer metadata blob - */ -flatbuffers::FlatBufferBuilder _generate_modify_metadata_fbb( - const std::string& file_name, - const std::vector& new_metadata) { - auto metadata_fbb = flatbuffers::FlatBufferBuilder(32768); - Reader reader(file_name); - - std::string metadata_id = reader.metadata_id(); - ts_t start_ts = reader.start_ts(); - ts_t end_ts = reader.end_ts(); - - /// @todo on OsfFile refactor, make a copy constructor for MetadataStore - MetadataStore new_meta_store; - auto old_meta_store = reader.meta_store(); - std::cout << "Looking for non sensor info metadata in old metastore" - << std::endl; - for (const auto& entry : old_meta_store.entries()) { - std::cout << "Found: " << entry.second->type() << " "; - /// @todo figure out why there isnt an easy def for this - if (entry.second->type() != "ouster/v1/os_sensor/LidarSensor") { - new_meta_store.add(*entry.second); - std::cout << "Is non sensor_info, adding" << std::endl; - } else { - std::cout << std::endl; - } - } - - for (const auto& entry : new_metadata) { - new_meta_store.add(LidarSensor(entry)); - } - - std::vector> entries = - new_meta_store.make_entries(metadata_fbb); - - std::vector chunks{}; - for (const auto& entry : reader.chunks()) { - chunks.emplace_back(entry.start_ts().count(), entry.end_ts().count(), - entry.offset()); - } - - auto metadata = ouster::osf::gen::CreateMetadataDirect( - metadata_fbb, metadata_id.c_str(), - !chunks.empty() ? start_ts.count() : 0, - !chunks.empty() ? end_ts.count() : 0, &chunks, &entries); - - metadata_fbb.FinishSizePrefixed(metadata, - ouster::osf::gen::MetadataIdentifier()); - return metadata_fbb; -} - -int64_t osf_file_modify_metadata( - const std::string& file_name, - const std::vector& new_metadata) { - std::string temp_dir; - std::string temp_path; - int64_t saved_bytes = -2; - uint64_t metadata_offset = 0; - - // Scope the reading portion so that we dont run into read write file - // locks - { - OsfFile osf_file{file_name}; - metadata_offset = osf_file.metadata_offset(); - } - - auto metadata_fbb = _generate_modify_metadata_fbb(file_name, new_metadata); - - truncate_file(file_name, metadata_offset); - saved_bytes = builder_to_file(metadata_fbb, file_name, true); - finish_osf_file(file_name, metadata_offset, saved_bytes); - - return saved_bytes; -} - -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/pcap_source.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/pcap_source.cpp deleted file mode 100644 index aa80778b..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/pcap_source.cpp +++ /dev/null @@ -1,79 +0,0 @@ -/** - * Copyright(c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#include "ouster/osf/pcap_source.h" - -namespace ouster { -namespace osf { - -template -PcapRawSource::PacketHandler use_packet(H&& handler) { - return - [handler](const sensor_utils::packet_info& p_info, const uint8_t* buf) { - osf::ts_t ts(p_info.timestamp); - handler(ts, buf); - }; -} - -PcapRawSource::PcapRawSource(const std::string& filename) - : pcap_filename_{filename} { - pcap_handle_ = sensor_utils::replay_initialize(pcap_filename_); -} - -void PcapRawSource::runAll() { - sensor_utils::packet_info p_info; - while (sensor_utils::next_packet_info(*pcap_handle_, p_info)) { - handleCurrentPacket(p_info); - } -} - -void PcapRawSource::runWhile(const PacketInfoPredicate& pred) { - sensor_utils::packet_info p_info; - while (sensor_utils::next_packet_info(*pcap_handle_, p_info)) { - if (!pred(p_info)) { - break; - } - handleCurrentPacket(p_info); - } -} - -void PcapRawSource::addLidarDataHandler(int dst_port, - const sensor::sensor_info& info, - LidarDataHandler&& lidar_handler) { - auto build_ls = osf::make_build_ls(info, lidar_handler); - packet_handlers_.insert({dst_port, use_packet(build_ls)}); -} - -void PcapRawSource::addLidarDataHandler( - int dst_port, const sensor::sensor_info& info, - const LidarScanFieldTypes& ls_field_types, - LidarDataHandler&& lidar_handler) { - auto build_ls = osf::make_build_ls(info, ls_field_types, lidar_handler); - packet_handlers_.insert({dst_port, use_packet(build_ls)}); -} - -void PcapRawSource::handleCurrentPacket( - const sensor_utils::packet_info& pinfo) { - constexpr uint32_t buf_size = 65536; // 2^16 - uint8_t buf[buf_size]; - auto handler_it = packet_handlers_.find(pinfo.dst_port); - if (handler_it != packet_handlers_.end()) { - auto size_read = - sensor_utils::read_packet(*pcap_handle_, buf, buf_size); - if (size_read > 0 && size_read < buf_size && - size_read == pinfo.payload_size) { - handler_it->second(pinfo, buf); - } - } -} - -PcapRawSource::~PcapRawSource() { - if (pcap_handle_) { - sensor_utils::replay_uninitialize(*pcap_handle_); - } -} - -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/png_tools.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/png_tools.cpp deleted file mode 100644 index 82cd213d..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/png_tools.cpp +++ /dev/null @@ -1,1384 +0,0 @@ -/** - * Copyright(c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#include "png_tools.h" - -#include - -#include -#include -#include -#include -#include - -#include "ouster/lidar_scan.h" - -namespace ouster { -namespace osf { - -/* - * Effect of png_set_compression(comp level): - * - (no png out): 2s, n/a - * - comp level 1: 39s, 648M (60% speedup vs default, 10% size increase) - * - comp level 2: 38s, 643M - * - comp level 3: 45s, 639M - * - comp level 4: 48s, 590M (47% speedup vs default, <1% size increase) - * - comp level 5: 61s, 589M - * - libpng default: 98s, 586M - * - comp level 9: 328s, 580M - * - * TODO: investigate other zlib options - */ -static constexpr int PNG_OSF_ZLIB_COMPRESSION_LEVEL = 4; - -/** - * Provides the data reader capabilities from std::vector for png_read IO - */ -struct VectorReader { - const std::vector& buffer; - uint32_t offset; - explicit VectorReader(const std::vector& buf) - : buffer(buf), offset(0) {} - void read(void* bytes, const uint32_t bytes_len) { - // Skip safety check and trust libpng? - if (offset >= buffer.size()) return; - uint32_t bytes_to_read = bytes_len; - if (offset + bytes_to_read > buffer.size()) { - bytes_to_read = buffer.size() - offset; - } - std::memcpy(bytes, buffer.data() + offset, bytes_to_read); - offset += bytes_to_read; - } -}; - -/** - * Error callback that will be fired on libpng errors - */ -void png_osf_error(png_structp png_ptr, png_const_charp msg) { - std::cout << "ERROR libpng osf: " << msg << std::endl; - longjmp(png_jmpbuf(png_ptr), 1); -}; - -/** - * Custom png_write handler to write data to std::vector buffer - */ -void png_osf_write_data(png_structp png_ptr, png_bytep bytes, - png_size_t bytes_len) { - std::vector* res_buf = - reinterpret_cast*>(png_get_io_ptr(png_ptr)); - res_buf->insert(res_buf->end(), reinterpret_cast(bytes), - reinterpret_cast(bytes + bytes_len)); -}; - -/** - * Custom png_read handler to read data from std::vector (via VectorReader - * helper) - */ -void png_osf_read_data(png_structp png_ptr, png_bytep bytes, - png_size_t bytes_len) { - VectorReader* vec_read = - reinterpret_cast(png_get_io_ptr(png_ptr)); - vec_read->read(bytes, bytes_len); -}; - -/** - * It's needed for custom png IO operations... but I've never seen it's called. - * And also there are no need to flush writer to std::vector buffer in our case. - */ -void png_osf_flush_data(png_structp){}; - -/** - * Common png WRITE init routine, creates and setups png_ptr and png_info_ptr - */ -bool png_osf_write_init(png_structpp png_ptrp, png_infopp png_info_ptrp) { - *png_ptrp = png_create_write_struct(PNG_LIBPNG_VER_STRING, nullptr, - png_osf_error, png_osf_error); - if (!*png_ptrp) { - std::cout << "ERROR: no png_ptr\n"; - return true; - } - - *png_info_ptrp = png_create_info_struct(*png_ptrp); - if (!*png_info_ptrp) { - std::cout << "ERROR: no png_info_ptr\n"; - png_destroy_write_struct(png_ptrp, nullptr); - return true; - } - - return false; // SUCCESS -} - -/** - * Common png READ init routine, creates and setups png_ptr and png_info_ptr - */ -bool png_osf_read_init(png_structpp png_ptrp, png_infopp png_info_ptrp) { - *png_ptrp = png_create_read_struct(PNG_LIBPNG_VER_STRING, nullptr, - png_osf_error, png_osf_error); - if (!*png_ptrp) { - std::cout << "ERROR: no png_ptr\n"; - return true; - } - - *png_info_ptrp = png_create_info_struct(*png_ptrp); - if (!*png_info_ptrp) { - std::cout << "ERROR: no png_info_ptr\n"; - png_destroy_read_struct(png_ptrp, nullptr, nullptr); - return true; - } - - return false; // SUCCESS -} - -/** - * Common png write setup routine. - * Write destination is res_buf of std::vector type. - */ -void png_osf_write_start(png_structp png_ptr, png_infop png_info_ptr, - ScanChannelData& res_buf, const uint32_t width, - const uint32_t height, const int sample_depth, - const int color_type) { - // Use setjmp() on upper level for errors catching - png_set_write_fn(png_ptr, &res_buf, png_osf_write_data, png_osf_flush_data); - - png_set_compression_level(png_ptr, PNG_OSF_ZLIB_COMPRESSION_LEVEL); - - png_set_IHDR(png_ptr, png_info_ptr, width, height, sample_depth, color_type, - PNG_INTERLACE_NONE, PNG_COMPRESSION_TYPE_DEFAULT, - PNG_FILTER_TYPE_DEFAULT); - - png_write_info(png_ptr, png_info_ptr); -} - -// ========== Encode Functions =================================== -#ifdef OUSTER_OSF_NO_THREADING -ScanData scanEncodeFieldsSingleThread(const LidarScan& lidar_scan, - const std::vector& px_offset, - const LidarScanFieldTypes& field_types) { - // Prepare scan data of size that fits all field_types we are about to - // encode - ScanData fields_data(field_types.size()); - - size_t scan_idx = 0; - for (const auto& f : field_types) { - fieldEncode(lidar_scan, f, px_offset, fields_data, scan_idx); - scan_idx += 1; - } - - return fields_data; -} -#else -ScanData scanEncodeFields(const LidarScan& lidar_scan, - const std::vector& px_offset, - const LidarScanFieldTypes& field_types) { - // Prepare scan data of size that fits all field_types we are about to - // encode - ScanData fields_data(field_types.size()); - - unsigned int con_num = std::thread::hardware_concurrency(); - // looking for at least 4 cores if can't determine - if (!con_num) con_num = 4; - - const size_t fields_num = field_types.size(); - // Number of fields to pack into a single thread coder - size_t per_thread_num = (fields_num + con_num - 1) / con_num; - std::vector> futures{}; - size_t scan_idx = 0; - for (size_t t = 0; t < con_num && t * per_thread_num < fields_num; ++t) { - // Per every thread we pack the `per_thread_num` field_types encodings - // job - const size_t start_idx = t * per_thread_num; - // Fields list for a thread to encode - LidarScanFieldTypes thread_fields{}; - // Scan indices for the corresponding fields where result will be stored - std::vector thread_idxs{}; - for (size_t i = 0; i < per_thread_num && i + start_idx < fields_num; - ++i) { - thread_fields.push_back(field_types[start_idx + i]); - thread_idxs.push_back(scan_idx); - scan_idx += 1; - } - // Start an encoder thread with selected fields and corresponding - // indices list - futures.emplace_back(std::async(fieldEncodeMulti, std::cref(lidar_scan), - thread_fields, std::cref(px_offset), - std::ref(fields_data), thread_idxs)); - } - - for (auto& t : futures) { - t.get(); - } - - return fields_data; -} -#endif - -template -bool encode8bitImage(ScanChannelData& res_buf, - const Eigen::Ref>& img, - const std::vector& px_offset) { - return encode8bitImage(res_buf, destagger(img, px_offset)); -} - -template bool encode8bitImage(ScanChannelData&, - const Eigen::Ref>&, - const std::vector&); -template bool encode8bitImage( - ScanChannelData&, const Eigen::Ref>&, - const std::vector&); -template bool encode8bitImage( - ScanChannelData&, const Eigen::Ref>&, - const std::vector&); -template bool encode8bitImage( - ScanChannelData&, const Eigen::Ref>&, - const std::vector&); - -template -bool encode8bitImage(ScanChannelData& res_buf, - const Eigen::Ref>& img) { - const uint32_t width = static_cast(img.cols()); - const uint32_t height = static_cast(img.rows()); - - // 8 bit Gray - const int sample_depth = 8; - const int color_type = PNG_COLOR_TYPE_GRAY; - - // 8bit Encoding Sizes - std::vector row_data(width); // Gray, 8bit - - // libpng main structs - png_structp png_ptr; - png_infop png_info_ptr; - - if (png_osf_write_init(&png_ptr, &png_info_ptr)) { - return true; - } - - if (setjmp(png_jmpbuf(png_ptr))) { - png_destroy_write_struct(&png_ptr, &png_info_ptr); - return true; - } - - png_osf_write_start(png_ptr, png_info_ptr, res_buf, width, height, - sample_depth, color_type); - - for (size_t u = 0; u < height; ++u) { - for (size_t v = 0; v < width; ++v) { - // 8bit Encoding Logic - row_data[v] = static_cast(img(u, v)); - } - - png_write_row(png_ptr, - reinterpret_cast(row_data.data())); - } - - png_write_end(png_ptr, nullptr); - - png_destroy_write_struct(&png_ptr, &png_info_ptr); - - return false; // SUCCESS -} - -template bool encode8bitImage(ScanChannelData&, - const Eigen::Ref>&); -template bool encode8bitImage( - ScanChannelData&, const Eigen::Ref>&); -template bool encode8bitImage( - ScanChannelData&, const Eigen::Ref>&); -template bool encode8bitImage( - ScanChannelData&, const Eigen::Ref>&); - -template -bool encode16bitImage(ScanChannelData& res_buf, - const Eigen::Ref>& img) { - const uint32_t width = static_cast(img.cols()); - const uint32_t height = static_cast(img.rows()); - - // 16 bit Gray - const int sample_depth = 16; - const int color_type = PNG_COLOR_TYPE_GRAY; - - // 16bit Encoding Sizes - std::vector row_data(width * 2); // Gray, 16bit - - // libpng main structs - png_structp png_ptr; - png_infop png_info_ptr; - - if (png_osf_write_init(&png_ptr, &png_info_ptr)) { - return true; - } - - if (setjmp(png_jmpbuf(png_ptr))) { - png_destroy_write_struct(&png_ptr, &png_info_ptr); - return true; - } - - png_osf_write_start(png_ptr, png_info_ptr, res_buf, width, height, - sample_depth, color_type); - - // Needed to transform provided little-endian samples to internal - // PNG big endian format - png_set_swap(png_ptr); - - for (size_t u = 0; u < height; ++u) { - for (size_t v = 0; v < width; ++v) { - const uint64_t key_val = img(u, v); - - // 16bit Encoding Logic - row_data[v * 2] = static_cast(key_val & 0xff); - row_data[v * 2 + 1] = static_cast((key_val >> 8u) & 0xff); - } - - png_write_row(png_ptr, - reinterpret_cast(row_data.data())); - } - - png_write_end(png_ptr, nullptr); - - png_destroy_write_struct(&png_ptr, &png_info_ptr); - - return false; // SUCCESS -} - -template bool encode16bitImage( - ScanChannelData&, const Eigen::Ref>&); -template bool encode16bitImage( - ScanChannelData&, const Eigen::Ref>&); -template bool encode16bitImage( - ScanChannelData&, const Eigen::Ref>&); -template bool encode16bitImage( - ScanChannelData&, const Eigen::Ref>&); - -template -bool encode16bitImage(ScanChannelData& res_buf, - const Eigen::Ref>& img, - const std::vector& px_offset) { - return encode16bitImage(res_buf, destagger(img, px_offset)); -} - -template bool encode16bitImage(ScanChannelData&, - const Eigen::Ref>&, - const std::vector&); -template bool encode16bitImage( - ScanChannelData&, const Eigen::Ref>&, - const std::vector&); -template bool encode16bitImage( - ScanChannelData&, const Eigen::Ref>&, - const std::vector&); -template bool encode16bitImage( - ScanChannelData&, const Eigen::Ref>&, - const std::vector&); - -template -bool encode24bitImage(ScanChannelData& res_buf, - const Eigen::Ref>& img, - const std::vector& px_offset) { - return encode24bitImage(res_buf, destagger(img, px_offset)); -} - -template bool encode24bitImage(ScanChannelData&, - const Eigen::Ref>&, - const std::vector&); -template bool encode24bitImage( - ScanChannelData&, const Eigen::Ref>&, - const std::vector&); -template bool encode24bitImage( - ScanChannelData&, const Eigen::Ref>&, - const std::vector&); -template bool encode24bitImage( - ScanChannelData&, const Eigen::Ref>&, - const std::vector&); - -template -bool encode24bitImage(ScanChannelData& res_buf, - const Eigen::Ref>& img) { - const uint32_t width = static_cast(img.cols()); - const uint32_t height = static_cast(img.rows()); - - // 8bit RGB - const int sample_depth = 8; - const int color_type = PNG_COLOR_TYPE_RGB; - - // 24bit Encoding Sizes - std::vector row_data(width * 3); // RGB, 8bit - - // libpng main structs - png_structp png_ptr; - png_infop png_info_ptr; - - if (png_osf_write_init(&png_ptr, &png_info_ptr)) { - return true; - } - - if (setjmp(png_jmpbuf(png_ptr))) { - png_destroy_write_struct(&png_ptr, &png_info_ptr); - return true; - } - - png_osf_write_start(png_ptr, png_info_ptr, res_buf, width, height, - sample_depth, color_type); - - for (size_t u = 0; u < height; ++u) { - for (size_t v = 0; v < width; ++v) { - const uint64_t key_val = img(u, v); - - // 24bit Encoding Logic - row_data[v * 3 + 0] = static_cast(key_val & 0xff); - row_data[v * 3 + 1] = static_cast((key_val >> 8u) & 0xff); - row_data[v * 3 + 2] = static_cast((key_val >> 16u) & 0xff); - } - - png_write_row(png_ptr, - reinterpret_cast(row_data.data())); - } - - png_write_end(png_ptr, nullptr); - - png_destroy_write_struct(&png_ptr, &png_info_ptr); - - return false; // SUCCESS -} - -template bool encode24bitImage( - ScanChannelData&, const Eigen::Ref>&); -template bool encode24bitImage( - ScanChannelData&, const Eigen::Ref>&); -template bool encode24bitImage( - ScanChannelData&, const Eigen::Ref>&); -template bool encode24bitImage( - ScanChannelData&, const Eigen::Ref>&); - -template -bool encode32bitImage(ScanChannelData& res_buf, - const Eigen::Ref>& img, - const std::vector& px_offset) { - return encode32bitImage(res_buf, destagger(img, px_offset)); -} - -template bool encode32bitImage(ScanChannelData&, - const Eigen::Ref>&, - const std::vector&); -template bool encode32bitImage( - ScanChannelData&, const Eigen::Ref>&, - const std::vector&); -template bool encode32bitImage( - ScanChannelData&, const Eigen::Ref>&, - const std::vector&); -template bool encode32bitImage( - ScanChannelData&, const Eigen::Ref>&, - const std::vector&); - -template -bool encode32bitImage(ScanChannelData& res_buf, - const Eigen::Ref>& img) { - const uint32_t width = static_cast(img.cols()); - const uint32_t height = static_cast(img.rows()); - - // 8bit RGBA - const int sample_depth = 8; - const int color_type = PNG_COLOR_TYPE_RGB_ALPHA; - - // 32bit Encoding Sizes - std::vector row_data(width * 4); // RGBA, 8bit - - // libpng main structs - png_structp png_ptr; - png_infop png_info_ptr; - - if (png_osf_write_init(&png_ptr, &png_info_ptr)) { - return true; - } - - if (setjmp(png_jmpbuf(png_ptr))) { - png_destroy_write_struct(&png_ptr, &png_info_ptr); - return true; - } - - png_osf_write_start(png_ptr, png_info_ptr, res_buf, width, height, - sample_depth, color_type); - - for (size_t u = 0; u < height; ++u) { - for (size_t v = 0; v < width; ++v) { - const uint64_t key_val = img(u, v); - - // 32bit Encoding Logic - row_data[v * 4 + 0] = static_cast(key_val & 0xff); - row_data[v * 4 + 1] = static_cast((key_val >> 8u) & 0xff); - row_data[v * 4 + 2] = static_cast((key_val >> 16u) & 0xff); - row_data[v * 4 + 3] = static_cast((key_val >> 24u) & 0xff); - } - - png_write_row(png_ptr, - reinterpret_cast(row_data.data())); - } - - png_write_end(png_ptr, nullptr); - - png_destroy_write_struct(&png_ptr, &png_info_ptr); - - return false; // SUCCESS -} - -template bool encode32bitImage( - ScanChannelData&, const Eigen::Ref>&); -template bool encode32bitImage( - ScanChannelData&, const Eigen::Ref>&); -template bool encode32bitImage( - ScanChannelData&, const Eigen::Ref>&); -template bool encode32bitImage( - ScanChannelData&, const Eigen::Ref>&); - -template -bool encode64bitImage(ScanChannelData& res_buf, - const Eigen::Ref>& img, - const std::vector& px_offset) { - return encode64bitImage(res_buf, destagger(img, px_offset)); -} - -template bool encode64bitImage(ScanChannelData&, - const Eigen::Ref>&, - const std::vector&); -template bool encode64bitImage( - ScanChannelData&, const Eigen::Ref>&, - const std::vector&); -template bool encode64bitImage( - ScanChannelData&, const Eigen::Ref>&, - const std::vector&); -template bool encode64bitImage( - ScanChannelData&, const Eigen::Ref>&, - const std::vector&); - -template -bool encode64bitImage(ScanChannelData& res_buf, - const Eigen::Ref>& img) { - const uint32_t width = static_cast(img.cols()); - const uint32_t height = static_cast(img.rows()); - - // 16bit RGBA - const int sample_depth = 16; - const int color_type = PNG_COLOR_TYPE_RGB_ALPHA; - - // 64bit Encoding Sizes - std::vector row_data(width * 8); // RGBA, 16bit - - // libpng main structs - png_structp png_ptr; - png_infop png_info_ptr; - - if (png_osf_write_init(&png_ptr, &png_info_ptr)) { - return true; - } - - if (setjmp(png_jmpbuf(png_ptr))) { - png_destroy_write_struct(&png_ptr, &png_info_ptr); - return true; - } - - png_osf_write_start(png_ptr, png_info_ptr, res_buf, width, height, - sample_depth, color_type); - - // Needed to transform provided little-endian samples to internal - // PNG big endian format - png_set_swap(png_ptr); - - for (size_t u = 0; u < height; ++u) { - for (size_t v = 0; v < width; ++v) { - const uint64_t key_val = img(u, v); - - // 64bit Encoding Logic - row_data[v * 8 + 0] = static_cast(key_val & 0xff); - row_data[v * 8 + 1] = static_cast((key_val >> 8u) & 0xff); - row_data[v * 8 + 2] = static_cast((key_val >> 16u) & 0xff); - row_data[v * 8 + 3] = static_cast((key_val >> 24u) & 0xff); - row_data[v * 8 + 4] = static_cast((key_val >> 32u) & 0xff); - row_data[v * 8 + 5] = static_cast((key_val >> 40u) & 0xff); - row_data[v * 8 + 6] = static_cast((key_val >> 48u) & 0xff); - row_data[v * 8 + 7] = static_cast((key_val >> 56u) & 0xff); - } - - png_write_row(png_ptr, - reinterpret_cast(row_data.data())); - } - - png_write_end(png_ptr, nullptr); - - png_destroy_write_struct(&png_ptr, &png_info_ptr); - - return false; // SUCCESS -} - -template bool encode64bitImage( - ScanChannelData&, const Eigen::Ref>&); -template bool encode64bitImage( - ScanChannelData&, const Eigen::Ref>&); -template bool encode64bitImage( - ScanChannelData&, const Eigen::Ref>&); -template bool encode64bitImage( - ScanChannelData&, const Eigen::Ref>&); - -void fieldEncodeMulti(const LidarScan& lidar_scan, - const LidarScanFieldTypes& field_types, - const std::vector& px_offset, ScanData& scan_data, - const std::vector& scan_idxs) { - if (field_types.size() != scan_idxs.size()) { - throw std::invalid_argument( - "ERROR: in fieldEncodeMulti field_types.size() should " - "match scan_idxs.size()"); - } - for (size_t i = 0; i < field_types.size(); ++i) { - auto err = fieldEncode(lidar_scan, field_types[i], px_offset, scan_data, - scan_idxs[i]); - if (err) { - std::cerr << "ERROR: fieldEncode: Can't encode field [" - << sensor::to_string(field_types[i]) - << "] (in " - "fieldEncodeMulti)" - << std::endl; - } - } -} - -bool fieldEncode( - const LidarScan& lidar_scan, - const std::pair field_type, - const std::vector& px_offset, ScanData& scan_data, size_t scan_idx) { - if (scan_idx >= scan_data.size()) { - throw std::invalid_argument( - "ERROR: scan_data size is not sufficient to hold idx: " + - std::to_string(scan_idx)); - } - bool res = true; - switch (field_type.second) { - case sensor::ChanFieldType::UINT8: - res = encode8bitImage(scan_data[scan_idx], - lidar_scan.field(field_type.first), - px_offset); - break; - case sensor::ChanFieldType::UINT16: - res = encode16bitImage(scan_data[scan_idx], - lidar_scan.field(field_type.first), - px_offset); - break; - case sensor::ChanFieldType::UINT32: - res = encode32bitImage(scan_data[scan_idx], - lidar_scan.field(field_type.first), - px_offset); - break; - case sensor::ChanFieldType::UINT64: - res = encode64bitImage(scan_data[scan_idx], - lidar_scan.field(field_type.first), - px_offset); - break; - default: - std::cerr << "ERROR: fieldEncode: UNKNOWN: ChanFieldType not yet " - "implemented" - << std::endl; - break; - } - if (res) { - std::cerr << "ERROR: fieldEncode: Can't encode field " - << sensor::to_string(field_type.first) << std::endl; - } - return res; -} - -ScanData scanEncode(const LidarScan& lidar_scan, - const std::vector& px_offset) { -#ifdef OUSTER_OSF_NO_THREADING - return scanEncodeFieldsSingleThread(lidar_scan, px_offset, - {lidar_scan.begin(), lidar_scan.end()}); -#else - return scanEncodeFields(lidar_scan, px_offset, - {lidar_scan.begin(), lidar_scan.end()}); -#endif -} - -// ========== Decode Functions =================================== - -bool scanDecode(LidarScan& lidar_scan, const ScanData& scan_data, - const std::vector& px_offset) { -#ifdef OUSTER_OSF_NO_THREADING - return scanDecodeFieldsSingleThread(lidar_scan, scan_data, px_offset); -#else - return scanDecodeFields(lidar_scan, scan_data, px_offset); -#endif -} - -bool fieldDecode( - LidarScan& lidar_scan, const ScanData& scan_data, size_t start_idx, - const std::pair field_type, - const std::vector& px_offset) { - switch (field_type.second) { - case sensor::ChanFieldType::UINT8: - return decode8bitImage(lidar_scan.field(field_type.first), - scan_data[start_idx], px_offset); - return true; - case sensor::ChanFieldType::UINT16: - return decode16bitImage( - lidar_scan.field(field_type.first), - scan_data[start_idx], px_offset); - case sensor::ChanFieldType::UINT32: - return decode32bitImage( - lidar_scan.field(field_type.first), - scan_data[start_idx], px_offset); - case sensor::ChanFieldType::UINT64: - return decode64bitImage( - lidar_scan.field(field_type.first), - scan_data[start_idx], px_offset); - return true; - default: - std::cout << "ERROR: fieldDecode: UNKNOWN: ChanFieldType not yet " - "implemented" - << std::endl; - return true; - } - return true; // ERROR -} - -bool fieldDecodeMulti(LidarScan& lidar_scan, const ScanData& scan_data, - const std::vector& scan_idxs, - const LidarScanFieldTypes& field_types, - const std::vector& px_offset) { - if (field_types.size() != scan_idxs.size()) { - throw std::invalid_argument( - "ERROR: in fieldDecodeMulti field_types.size() should " - "match scan_idxs.size()"); - } - auto res_err = false; - for (size_t i = 0; i < field_types.size(); ++i) { - auto err = fieldDecode(lidar_scan, scan_data, scan_idxs[i], - field_types[i], px_offset); - if (err) { - std::cerr << "ERROR: fieldDecodeMulti: Can't decode field [" - << sensor::to_string(field_types[i]) << "]" << std::endl; - } - res_err = res_err || err; - } - return res_err; -} -#ifdef OUSTER_OSF_NO_THREADING -bool scanDecodeFieldsSingleThread(LidarScan& lidar_scan, - const ScanData& scan_data, - const std::vector& px_offset) { - size_t fields_cnt = std::distance(lidar_scan.begin(), lidar_scan.end()); - if (scan_data.size() != fields_cnt) { - std::cerr << "ERROR: lidar_scan data contains # of channels: " - << scan_data.size() << ", expected: " << fields_cnt - << " for OSF_EUDP" << std::endl; - return true; - } - size_t next_idx = 0; - for (auto f : lidar_scan) { - if (fieldDecode(lidar_scan, scan_data, next_idx, f, px_offset)) { - std::cout << "ERROR: scanDecodeFields: Failed to decode field" - << std::endl; - return true; - } - ++next_idx; - } - return false; -} -#else -// TWS 20240301 TODO: determine if we can deduplicate this code (see -// scanEncodeFields) -bool scanDecodeFields(LidarScan& lidar_scan, const ScanData& scan_data, - const std::vector& px_offset) { - LidarScanFieldTypes field_types(lidar_scan.begin(), lidar_scan.end()); - size_t fields_num = field_types.size(); - if (scan_data.size() != fields_num) { - std::cerr << "ERROR: lidar_scan data contains # of channels: " - << scan_data.size() << ", expected: " << fields_num - << " for OSF EUDP" << std::endl; - return true; - } - - unsigned int con_num = std::thread::hardware_concurrency(); - // looking for at least 4 cores if can't determine - if (!con_num) con_num = 4; - - // Number of fields to pack into a single thread coder - size_t per_thread_num = (fields_num + con_num - 1) / con_num; - std::vector> futures{}; - size_t scan_idx = 0; - - for (size_t t = 0; t < con_num && t * per_thread_num < fields_num; ++t) { - // Per every thread we pack the `per_thread_num` field_types encodings - // job - const size_t start_idx = t * per_thread_num; - // Fields list for a thread to encode - LidarScanFieldTypes thread_fields{}; - // Scan indices for the corresponding fields where result will be stored - std::vector thread_idxs{}; - for (size_t i = 0; i < per_thread_num && i + start_idx < fields_num; - ++i) { - thread_fields.push_back(field_types[start_idx + i]); - thread_idxs.push_back(scan_idx); - scan_idx += 1; // for UINT64 can be 2 (NOT IMPLEMENTED YET) - } - - // Start a decoder thread with selected fields and corresponding - // indices list - - futures.emplace_back(std::async(fieldDecodeMulti, std::ref(lidar_scan), - std::cref(scan_data), thread_idxs, - thread_fields, std::cref(px_offset))); - } - - for (auto& t : futures) { - // TODO: refactor, use return std::all - bool res = t.get(); - if (!res) { - return false; - } - } - - return false; -} -#endif - -template -bool decode24bitImage(Eigen::Ref> img, - const ScanChannelData& channel_buf, - const std::vector& px_offset) { - if (!decode24bitImage(img, channel_buf)) { - img = stagger(img, px_offset); - return false; // SUCCESS - } - return true; // ERROR -} - -template bool decode24bitImage(Eigen::Ref>, - const ScanChannelData&, - const std::vector&); -template bool decode24bitImage(Eigen::Ref>, - const ScanChannelData&, - const std::vector&); -template bool decode24bitImage(Eigen::Ref>, - const ScanChannelData&, - const std::vector&); -template bool decode24bitImage(Eigen::Ref>, - const ScanChannelData&, - const std::vector&); - -template -bool decode24bitImage(Eigen::Ref> img, - const ScanChannelData& channel_buf) { - // libpng main structs - png_structp png_ptr; - png_infop png_info_ptr; - - if (png_osf_read_init(&png_ptr, &png_info_ptr)) { - return true; - } - - if (setjmp(png_jmpbuf(png_ptr))) { - png_destroy_read_struct(&png_ptr, &png_info_ptr, nullptr); - return true; - } - - VectorReader channel_reader(channel_buf); - png_set_read_fn(png_ptr, &channel_reader, png_osf_read_data); - - // only used with Gray 16 bit to get little-endian LSB representation back - // for other color types PNG_TRANSFORM_SWAP_ENDIAN does nothing - int transforms = PNG_TRANSFORM_SWAP_ENDIAN; - png_read_png(png_ptr, png_info_ptr, transforms, nullptr); - - png_uint_32 width; - png_uint_32 height; - int sample_depth; - int color_type; - - png_get_IHDR(png_ptr, png_info_ptr, &width, &height, &sample_depth, - &color_type, nullptr, nullptr, nullptr); - - png_bytepp row_pointers = png_get_rows(png_ptr, png_info_ptr); - - // Sanity checks for encoded PNG size - if (width != static_cast(img.cols()) || - height != static_cast(img.rows())) { - std::cout << "ERROR: img contains data of incompatible size: " << width - << "x" << height << ", expected: " << img.cols() << "x" - << img.rows() << std::endl; - return true; - } - - if (sample_depth != 8) { - std::cout << "ERROR: encoded img contains data with incompatible " - "sample_depth: " - << sample_depth << ", expected: 8" << std::endl; - return true; - } - - if (color_type != PNG_COLOR_TYPE_RGB) { - std::cout << "ERROR: encoded img contains data with incompatible " - "color type: " - << color_type << ", expected: " << PNG_COLOR_TYPE_RGB - << std::endl; - return true; - } - - // 24bit channel data decoding to LidarScan for key channel_index - for (size_t u = 0; u < height; u++) { - for (size_t v = 0; v < width; v++) { - img(u, v) = static_cast(row_pointers[u][v * 3 + 0]) + - (static_cast(row_pointers[u][v * 3 + 1]) << 8u) + - (static_cast(row_pointers[u][v * 3 + 2]) << 16u); - } - } - - png_destroy_read_struct(&png_ptr, &png_info_ptr, nullptr); - - return false; // SUCCESS -} - -template bool decode24bitImage(Eigen::Ref>, - const ScanChannelData&); -template bool decode24bitImage(Eigen::Ref>, - const ScanChannelData&); -template bool decode24bitImage(Eigen::Ref>, - const ScanChannelData&); -template bool decode24bitImage(Eigen::Ref>, - const ScanChannelData&); - -template -bool decode32bitImage(Eigen::Ref> img, - const ScanChannelData& channel_buf, - const std::vector& px_offset) { - if (!decode32bitImage(img, channel_buf)) { - img = stagger(img, px_offset); - return false; // SUCCESS - } - return true; // ERROR -} - -template bool decode32bitImage(Eigen::Ref>, - const ScanChannelData&, - const std::vector&); -template bool decode32bitImage(Eigen::Ref>, - const ScanChannelData&, - const std::vector&); -template bool decode32bitImage(Eigen::Ref>, - const ScanChannelData&, - const std::vector&); -template bool decode32bitImage(Eigen::Ref>, - const ScanChannelData&, - const std::vector&); - -template -bool decode32bitImage(Eigen::Ref> img, - const ScanChannelData& channel_buf) { - if (sizeof(T) < 4) { - std::cerr << "WARNING: Attempt to decode image of bigger pixel size" - << std::endl; - } - // libpng main structs - png_structp png_ptr; - png_infop png_info_ptr; - - if (png_osf_read_init(&png_ptr, &png_info_ptr)) { - return true; - } - - if (setjmp(png_jmpbuf(png_ptr))) { - png_destroy_read_struct(&png_ptr, &png_info_ptr, nullptr); - return true; - } - - VectorReader channel_reader(channel_buf); - png_set_read_fn(png_ptr, &channel_reader, png_osf_read_data); - - // only used with Gray 16 bit to get little-endian LSB representation back - // for other color types PNG_TRANSFORM_SWAP_ENDIAN does nothing - int transforms = PNG_TRANSFORM_SWAP_ENDIAN; - png_read_png(png_ptr, png_info_ptr, transforms, nullptr); - - png_uint_32 width; - png_uint_32 height; - int sample_depth; - int color_type; - - png_get_IHDR(png_ptr, png_info_ptr, &width, &height, &sample_depth, - &color_type, nullptr, nullptr, nullptr); - - png_bytepp row_pointers = png_get_rows(png_ptr, png_info_ptr); - - // Sanity checks for encoded PNG size - if (width != static_cast(img.cols()) || - height != static_cast(img.rows())) { - std::cout << "ERROR: img contains data of incompatible size: " << width - << "x" << height << ", expected: " << img.cols() << "x" - << img.rows() << std::endl; - return true; - } - - if (sample_depth != 8) { - std::cout << "ERROR: encoded img contains data with incompatible " - "sample_depth: " - << sample_depth << ", expected: 8" << std::endl; - return true; - } - - if (color_type != PNG_COLOR_TYPE_RGB_ALPHA) { - std::cout << "ERROR: encoded img contains data with incompatible " - "color type: " - << color_type << ", expected: " << PNG_COLOR_TYPE_RGB_ALPHA - << std::endl; - return true; - } - - // 32bit channel data decoding to LidarScan for key channel_index - for (size_t u = 0; u < height; u++) { - for (size_t v = 0; v < width; v++) { - img(u, v) = static_cast(row_pointers[u][v * 4 + 0]) + - (static_cast(row_pointers[u][v * 4 + 1]) << 8u) + - (static_cast(row_pointers[u][v * 4 + 2]) << 16u) + - (static_cast(row_pointers[u][v * 4 + 3]) << 24u); - } - } - - png_destroy_read_struct(&png_ptr, &png_info_ptr, nullptr); - - return false; // SUCCESS -} - -template bool decode32bitImage(Eigen::Ref>, - const ScanChannelData&); -template bool decode32bitImage(Eigen::Ref>, - const ScanChannelData&); -template bool decode32bitImage(Eigen::Ref>, - const ScanChannelData&); -template bool decode32bitImage(Eigen::Ref>, - const ScanChannelData&); - -template -bool decode64bitImage(Eigen::Ref> img, - const ScanChannelData& channel_buf, - const std::vector& px_offset) { - if (!decode64bitImage(img, channel_buf)) { - img = stagger(img, px_offset); - return false; // SUCCESS - } - return true; // ERROR -} - -template bool decode64bitImage(Eigen::Ref>, - const ScanChannelData&, - const std::vector&); -template bool decode64bitImage(Eigen::Ref>, - const ScanChannelData&, - const std::vector&); -template bool decode64bitImage(Eigen::Ref>, - const ScanChannelData&, - const std::vector&); -template bool decode64bitImage(Eigen::Ref>, - const ScanChannelData&, - const std::vector&); - -template -bool decode64bitImage(Eigen::Ref> img, - const ScanChannelData& channel_buf) { - if (sizeof(T) < 8) { - std::cerr << "WARNING: Attempt to decode image of bigger pixel size" - << std::endl; - } - // libpng main structs - png_structp png_ptr; - png_infop png_info_ptr; - - if (png_osf_read_init(&png_ptr, &png_info_ptr)) { - return true; - } - - if (setjmp(png_jmpbuf(png_ptr))) { - png_destroy_read_struct(&png_ptr, &png_info_ptr, nullptr); - return true; - } - - VectorReader channel_reader(channel_buf); - png_set_read_fn(png_ptr, &channel_reader, png_osf_read_data); - - // only used with Gray 16 bit to get little-endian LSB representation back - // for other color types PNG_TRANSFORM_SWAP_ENDIAN does nothing - int transforms = PNG_TRANSFORM_SWAP_ENDIAN; - png_read_png(png_ptr, png_info_ptr, transforms, nullptr); - - png_uint_32 width; - png_uint_32 height; - int sample_depth; - int color_type; - - png_get_IHDR(png_ptr, png_info_ptr, &width, &height, &sample_depth, - &color_type, nullptr, nullptr, nullptr); - - png_bytepp row_pointers = png_get_rows(png_ptr, png_info_ptr); - - // Sanity checks for encoded PNG size - if (width != static_cast(img.cols()) || - height != static_cast(img.rows())) { - std::cout << "ERROR: img contains data of incompatible size: " << width - << "x" << height << ", expected: " << img.cols() << "x" - << img.rows() << std::endl; - return true; - } - - if (sample_depth != 16) { - std::cout << "ERROR: encoded img contains data with incompatible " - "sample_depth: " - << sample_depth << ", expected: 16" << std::endl; - return true; - } - - if (color_type != PNG_COLOR_TYPE_RGB_ALPHA) { - std::cout << "ERROR: encoded img contains data with incompatible " - "color type: " - << color_type << ", expected: " << PNG_COLOR_TYPE_RGB_ALPHA - << std::endl; - return true; - } - - // 64bit channel data decoding to LidarScan for key channel_index - for (size_t u = 0; u < height; u++) { - for (size_t v = 0; v < width; v++) { - uint64_t val = - static_cast(row_pointers[u][v * 8 + 0]) + - (static_cast(row_pointers[u][v * 8 + 1]) << 8u) + - (static_cast(row_pointers[u][v * 8 + 2]) << 16u) + - (static_cast(row_pointers[u][v * 8 + 3]) << 24u) + - (static_cast(row_pointers[u][v * 8 + 4]) << 32u) + - (static_cast(row_pointers[u][v * 8 + 5]) << 40u) + - (static_cast(row_pointers[u][v * 8 + 6]) << 48u) + - (static_cast(row_pointers[u][v * 8 + 7]) << 56u); - img(u, v) = static_cast(val); - } - } - - png_destroy_read_struct(&png_ptr, &png_info_ptr, nullptr); - - return false; // SUCCESS -} - -template bool decode64bitImage(Eigen::Ref>, - const ScanChannelData&); -template bool decode64bitImage(Eigen::Ref>, - const ScanChannelData&); -template bool decode64bitImage(Eigen::Ref>, - const ScanChannelData&); -template bool decode64bitImage(Eigen::Ref>, - const ScanChannelData&); - -template -bool decode16bitImage(Eigen::Ref> img, - const ScanChannelData& channel_buf, - const std::vector& px_offset) { - if (!decode16bitImage(img, channel_buf)) { - img = stagger(img, px_offset); - return false; // SUCCESS - } - return true; // ERROR -} - -template bool decode16bitImage(Eigen::Ref>, - const ScanChannelData&, - const std::vector&); -template bool decode16bitImage(Eigen::Ref>, - const ScanChannelData&, - const std::vector&); -template bool decode16bitImage(Eigen::Ref>, - const ScanChannelData&, - const std::vector&); -template bool decode16bitImage(Eigen::Ref>, - const ScanChannelData&, - const std::vector&); - -template -bool decode16bitImage(Eigen::Ref> img, - const ScanChannelData& channel_buf) { - if (sizeof(T) < 2) { - std::cerr << "WARNING: Attempt to decode image of bigger pixel size" - << std::endl; - } - // libpng main structs - png_structp png_ptr; - png_infop png_info_ptr; - - if (png_osf_read_init(&png_ptr, &png_info_ptr)) { - return true; - } - - if (setjmp(png_jmpbuf(png_ptr))) { - png_destroy_read_struct(&png_ptr, &png_info_ptr, nullptr); - return true; - } - - VectorReader channel_reader(channel_buf); - png_set_read_fn(png_ptr, &channel_reader, png_osf_read_data); - - // only used with Gray 16 bit to get little-endian LSB representation back - // for other color types PNG_TRANSFORM_SWAP_ENDIAN does nothing - int transforms = PNG_TRANSFORM_SWAP_ENDIAN; - png_read_png(png_ptr, png_info_ptr, transforms, nullptr); - - png_uint_32 width; - png_uint_32 height; - int sample_depth; - int color_type; - - png_get_IHDR(png_ptr, png_info_ptr, &width, &height, &sample_depth, - &color_type, nullptr, nullptr, nullptr); - - png_bytepp row_pointers = png_get_rows(png_ptr, png_info_ptr); - - // Sanity checks for encoded PNG size - if (width != static_cast(img.cols()) || - height != static_cast(img.rows())) { - std::cout << "ERROR: img contains data of incompatible size: " << width - << "x" << height << ", expected: " << img.cols() << "x" - << img.rows() << std::endl; - return true; - } - - if (sample_depth != 16) { - std::cout << "ERROR: encoded img contains data with incompatible " - "sample_depth: " - << sample_depth << ", expected: 16" << std::endl; - return true; - } - - if (color_type != PNG_COLOR_TYPE_GRAY) { - std::cout << "ERROR: encoded img contains data with incompatible " - "color type: " - << color_type << ", expected: " << PNG_COLOR_TYPE_GRAY - << std::endl; - return true; - } - - // 16bit channel data decoding to LidarScan for key channel_index - for (size_t u = 0; u < height; u++) { - for (size_t v = 0; v < width; v++) { - img(u, v) = static_cast(row_pointers[u][v * 2 + 0]) + - (static_cast(row_pointers[u][v * 2 + 1]) << 8u); - } - } - - png_destroy_read_struct(&png_ptr, &png_info_ptr, nullptr); - - return false; // SUCCESS -} - -template bool decode16bitImage(Eigen::Ref>, - const ScanChannelData&); -template bool decode16bitImage(Eigen::Ref>, - const ScanChannelData&); -template bool decode16bitImage(Eigen::Ref>, - const ScanChannelData&); -template bool decode16bitImage(Eigen::Ref>, - const ScanChannelData&); - -template -bool decode8bitImage(Eigen::Ref> img, - const ScanChannelData& channel_buf, - const std::vector& px_offset) { - if (!decode8bitImage(img, channel_buf)) { - img = stagger(img, px_offset); - return false; // SUCCESS - } - return true; // ERROR -} - -template bool decode8bitImage(Eigen::Ref>, - const ScanChannelData&, - const std::vector&); -template bool decode8bitImage(Eigen::Ref>, - const ScanChannelData&, - const std::vector&); -template bool decode8bitImage(Eigen::Ref>, - const ScanChannelData&, - const std::vector&); -template bool decode8bitImage(Eigen::Ref>, - const ScanChannelData&, - const std::vector&); - -template -bool decode8bitImage(Eigen::Ref> img, - const ScanChannelData& channel_buf) { - // libpng main structs - png_structp png_ptr; - png_infop png_info_ptr; - - if (png_osf_read_init(&png_ptr, &png_info_ptr)) { - return true; - } - - if (setjmp(png_jmpbuf(png_ptr))) { - png_destroy_read_struct(&png_ptr, &png_info_ptr, nullptr); - return true; - } - - VectorReader channel_reader(channel_buf); - png_set_read_fn(png_ptr, &channel_reader, png_osf_read_data); - - // only used with Gray 16 bit to get little-endian LSB representation back - // for other color types PNG_TRANSFORM_SWAP_ENDIAN does nothing - int transforms = PNG_TRANSFORM_SWAP_ENDIAN; - png_read_png(png_ptr, png_info_ptr, transforms, nullptr); - - png_uint_32 width; - png_uint_32 height; - int sample_depth; - int color_type; - - png_get_IHDR(png_ptr, png_info_ptr, &width, &height, &sample_depth, - &color_type, nullptr, nullptr, nullptr); - - png_bytepp row_pointers = png_get_rows(png_ptr, png_info_ptr); - - // Sanity checks for encoded PNG size - if (width != static_cast(img.cols()) || - height != static_cast(img.rows())) { - std::cout << "ERROR: img contains data of incompatible size: " << width - << "x" << height << ", expected: " << img.cols() << "x" - << img.rows() << std::endl; - return true; - } - - if (sample_depth != 8) { - std::cout << "ERROR: encoded img contains data with incompatible " - "sample_depth: " - << sample_depth << ", expected: 16" << std::endl; - return true; - } - - if (color_type != PNG_COLOR_TYPE_GRAY) { - std::cout << "ERROR: encoded img contains data with incompatible " - "color type: " - << color_type << ", expected: " << PNG_COLOR_TYPE_GRAY - << std::endl; - return true; - } - - // 16bit channel data decoding to LidarScan for key channel_index - for (size_t u = 0; u < height; u++) { - for (size_t v = 0; v < width; v++) { - img(u, v) = row_pointers[u][v]; - } - } - - png_destroy_read_struct(&png_ptr, &png_info_ptr, nullptr); - - return false; // SUCCESS -} - -template bool decode8bitImage(Eigen::Ref>, - const ScanChannelData&); -template bool decode8bitImage(Eigen::Ref>, - const ScanChannelData&); -template bool decode8bitImage(Eigen::Ref>, - const ScanChannelData&); -template bool decode8bitImage(Eigen::Ref>, - const ScanChannelData&); - -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/png_tools.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/png_tools.h deleted file mode 100644 index a45896d0..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/png_tools.h +++ /dev/null @@ -1,450 +0,0 @@ -/** - * Copyright(c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#pragma once - -#include -#include - -#include "os_sensor/lidar_scan_stream_generated.h" -#include "ouster/lidar_scan.h" - -namespace ouster { -namespace osf { - -// Encoded single PNG buffer -using ScanChannelData = std::vector; - -// Encoded PNG buffers -using ScanData = std::vector; - -// FieldTypes container -using LidarScanFieldTypes = - std::vector>; - -/** - * libpng only versions for Encode/Decode LidarScan to PNG buffers - */ - -// ========== Decode Functions =================================== - -/** - * Decode the PNG buffers into LidarScan object. This is a dispatch function to - * the specific decoding functions. - * - * @param[out] lidar_scan The output object that will be filled as a result of - * decoding. - * @param[in] scan_data PNG buffers to decode. - * @param[in] px_offset Pixel shift per row used to reconstruct staggered range - * image form. - * @return false (0) if operation is successful true (1) if error occured - */ -bool scanDecode(LidarScan& lidar_scan, const ScanData& scan_data, - const std::vector& px_offset); - -#ifdef OUSTER_OSF_NO_THREADING -/// Decoding eUDP LidarScan -// TODO[pb]: Make decoding of just some fields from scan data?? Not now ... -bool scanDecodeFieldsSingleThread(LidarScan& lidar_scan, - const ScanData& scan_data, - const std::vector& px_offset); -#else -/// Decoding eUDP LidarScan, multithreaded version -bool scanDecodeFields(LidarScan& lidar_scan, const ScanData& scan_data, - const std::vector& px_offset); -#endif - -/** - * Decode a single field to lidar_scan - * - * @param[out] lidar_scan The output object that will be filled as a result of - * decoding. - * @param[in] scan_data PNG buffers to decode. - * @param[in] scan_idx Index in `scan_data` of the beginning of field buffers. - * @param[in] field_type The field of `lidar_scan` to fill in with the decoded - * result. - * @param[in] px_offset Pixel shift per row used to reconstruct staggered range - * image form. - * @return false (0) if operation is successful true (1) if error occured - */ -bool fieldDecode( - LidarScan& lidar_scan, const ScanData& scan_data, size_t scan_idx, - const std::pair field_type, - const std::vector& px_offset); - -/** - * Decode multiple fields to lidar_scan - * - * @param[out] lidar_scan The output object that will be filled as a result of - * decoding. - * @param[in] scan_data PNG buffers to decode, sequentially in the order of - * field_types - * @param[in] scan_idxs a vector of indices in `scan_data` of the beginning of - * field buffers that will be decoded. `field_types.size()` - * should be equal to `scan_idxs.size()` i.e. we need to - * provide the index for every field type in - * field_types where it's encoded data located - * @param[in] field_types a vector of filed_types of lidar scan to decode - * @param[in] px_offset pixel shift per row used to reconstruct staggered range - * image form - * @return false (0) if operation is successful true (1) if error occured - */ -bool fieldDecodeMulti(LidarScan& lidar_scan, const ScanData& scan_data, - const std::vector& scan_idxs, - const LidarScanFieldTypes& field_types, - const std::vector& px_offset); -/** - * @defgroup OSFPngDecode8 Decoding Functionality. - * Decode single PNG buffer (channel_buf) of 8 bit Gray encoding into - * img. - * - * @tparam T The type to use for the output array. - * - * @param[out] img The output img that will be filled as a result of - * decoding. - * @param[in] channel_buf Single PNG buffer to decode. - * @return false (0) if operation is successful true (1) if error occured - */ - -/** @copydoc OSFPngDecode8 */ -template -bool decode8bitImage(Eigen::Ref> img, - const ScanChannelData& channel_buf); - -/** - * @copydoc OSFPngDecode8 - * @param[in] px_offset pixel shift per row used to reconstruct staggered range - * image form - */ -template -bool decode8bitImage(Eigen::Ref> img, - const ScanChannelData& channel_buf, - const std::vector& px_offset); - -/** - * @defgroup OSFPngDecode16 Decoding Functionality. - * Decode single PNG buffer (channel_buf) of 16 bit Gray encoding into - * img. - * - * @tparam T The type to use for the output array. - * - * @param[out] img The output img that will be filled as a result of - * decoding. - * @param[in] channel_buf Single PNG buffer to decode. - * @return false (0) if operation is successful true (1) if error occured - */ - -/** - * @copydoc OSFPngDecode16 - * @param[in] px_offset pixel shift per row used to reconstruct staggered range - * image form - */ -template -bool decode16bitImage(Eigen::Ref> img, - const ScanChannelData& channel_buf, - const std::vector& px_offset); - -/** @copydoc OSFPngDecode16 */ -template -bool decode16bitImage(Eigen::Ref> img, - const ScanChannelData& channel_buf); - -/** - * @defgroup OSFPngDecode24 Decoding Functionality. - * Decode single PNG buffer (channel_buf) of 24 bit RGB (8 bit) encoding into - * img object. - * - * @tparam T The type to use for the output array. - * - * @param[out] img The output img that will be filled as a result of - * decoding. - * @param[in] channel_buf Single PNG buffer to decode. - * @return false (0) if operation is successful true (1) if error occured - */ - -/** - * @copydoc OSFPngDecode24 - * @param[in] px_offset Pixel shift per row used to reconstruct staggered range - * image form. - */ -template -bool decode24bitImage(Eigen::Ref> img, - const ScanChannelData& channel_buf, - const std::vector& px_offset); - -/** @copydoc OSFPngDecode24 */ -template -bool decode24bitImage(Eigen::Ref> img, - const ScanChannelData& channel_buf); - -/** - * @defgroup OSFPngDecode32 Decoding Functionality. - * Decode single PNG buffer (channel_buf) of 32 bit RGBA (8 bit) encoding into - * img object. - * - * @tparam T The type to use for the output array. - * - * @param[out] img The output img that will be filled as a result of - * decoding. - * @param[in] channel_buf Single PNG buffer to decode. - * @return false (0) if operation is successful true (1) if error occured - */ - -/** - * @copydoc OSFPngDecode32 - * @param[in] px_offset Pixel shift per row used to reconstruct staggered range - * image form. - */ -template -bool decode32bitImage(Eigen::Ref> img, - const ScanChannelData& channel_buf, - const std::vector& px_offset); - -/** @copydoc OSFPngDecode32 */ -template -bool decode32bitImage(Eigen::Ref> img, - const ScanChannelData& channel_buf); - -/** - * @defgroup OSFPngDecode64 Decoding Functionality. - * Decode single PNG buffer (channel_buf) of 64 bit RGBA (16 bit) encoding into - * img object. - * - * @tparam T The type to use for the output array. - * - * @param[out] img The output img that will be filled as a result of - * decoding. - * @param[in] channel_buf Single PNG buffer to decode. - * @return false (0) if operation is successful true (1) if error occured - */ - -/** - * @copydoc OSFPngDecode64 - * @param[in] px_offset Pixel shift per row used to reconstruct staggered range - * image form. - */ -template -bool decode64bitImage(Eigen::Ref> img, - const ScanChannelData& channel_buf, - const std::vector& px_offset); - -/** @copydoc OSFPngDecode64 */ -template -bool decode64bitImage(Eigen::Ref> img, - const ScanChannelData& channel_buf); - -// ========== Encode Functions =================================== - -/** - * Encode LidarScan to PNG buffers storing all field_types present in an object. - * - * @param[in] lidar_scan The LidarScan object to encode. - * @param[in] px_offset Pixel shift per row used to - * destaggered LidarScan data. - * @return encoded PNG buffers, empty() if error occured. - */ -ScanData scanEncode(const LidarScan& lidar_scan, - const std::vector& px_offset); - -#ifdef OUSTER_OSF_NO_THREADING -/** - * Encode the lidar scan fields to PNGs channel buffers (ScanData). - * Single-threaded implementation. - * - * @param[in] lidar_scan A lidar scan object to encode. - * @param[in] px_offset Pixel shift per row used to construct de-staggered range - * image form. - * @return Encoded PNGs in ScanData in order of field_types. - */ -ScanData scanEncodeFieldsSingleThread(const LidarScan& lidar_scan, - const std::vector& px_offset, - const LidarScanFieldTypes& field_types); -#else -/** - * Encode the lidar scan fields to PNGs channel buffers (ScanData). - * Multi-threaded implementation. - * - * @param[in] lidar_scan A lidar scan object to encode. - * @param[in] px_offset Pixel shift per row used to construct de-staggered range - * image form. - * @param[in] field_types The field types to use for encoding. - * @return Encoded PNGs in ScanData in order of field_types. - */ -ScanData scanEncodeFields(const LidarScan& lidar_scan, - const std::vector& px_offset, - const LidarScanFieldTypes& field_types); -#endif -/** - * Encode a single lidar scan field to PNGs channel buffer and place it to a - * specified `scan_data[scan_idx]` place - * - * @param[in] lidar_scan a lidar scan object to encode - * @param[in] field_type a filed_type of lidar scan to encode - * @param[in] px_offset pixel shift per row used to construct de-staggered - * range image form - * @param[out] scan_data channel buffers storage for the encoded lidar_scan - * @param[in] scan_idx index in `scan_data` of the beginning of field buffers - * where the result of encoding will be inserted - * @return false (0) if operation is successful true (1) if error occured - */ -bool fieldEncode( - const LidarScan& lidar_scan, - const std::pair field_type, - const std::vector& px_offset, ScanData& scan_data, size_t scan_idx); - -/** - * Encode multiple lidar scan fields to PNGs channel buffers and insert them to - * a specified places `scan_idxs` in `scan_data`. - * - * @param[in] lidar_scan a lidar scan object to encode - * @param[in] field_types a vector of filed_types of - * lidar scan to encode - * @param[in] px_offset pixel shift per row used to construct de-staggered range - * image form - * @param[out] scan_data channel buffers storage for the encoded lidar_scan - * @param[in] scan_idxs a vector of indices in `scan_data` of the beginning of - * field buffers where the result of encoding will be - * inserted. `field_types.size()` should be equal to - * `scan_idxs.size()` - */ -void fieldEncodeMulti(const LidarScan& lidar_scan, - const LidarScanFieldTypes& field_types, - const std::vector& px_offset, ScanData& scan_data, - const std::vector& scan_idxs); - -/** - * @defgroup OSFPngEncode8 Encoding Functionality. - * Encode img object into a 8 bit, Gray, PNG buffer. - * - * @tparam T The type to use for the output array. - * - * @param[out] res_buf The output buffer with a single encoded PNG. - * @param[in] img The image object to encode. - * @return false (0) if operation is successful, true (1) if error occured - */ - -/** - * @copydoc OSFPngEncode8 - * @param[in] px_offset Pixel shift per row used to destagger img data. - */ -template -bool encode8bitImage(ScanChannelData& res_buf, - const Eigen::Ref>& img, - const std::vector& px_offset); - -/** @copydoc OSFPngEncode8 */ -template -bool encode8bitImage(ScanChannelData& res_buf, - const Eigen::Ref>& img); - -/** - * Encode img object into a 16 bit, Gray, PNG buffer. - * - * @tparam T The type to use for the output array. - * - * @param[out] res_buf The output buffer with a single encoded PNG. - * @param[in] img The image object to encode. - * @param[in] px_offset Pixel shift per row used to destagger img data. - * @return false (0) if operation is successful, true (1) if error occured - */ -template -bool encode16bitImage(ScanChannelData& res_buf, - const Eigen::Ref>& img, - const std::vector& px_offset); - -/** - * Encode 2D image of a typical lidar scan field channel into a 16 bit, Gray, - * PNG buffer. - * - * @tparam T The type to use for the output array. - * - * @param[out] res_buf The output buffer with a single encoded PNG. - * @param[in] img The image object to encode. - * @return false (0) if operation is successful, true (1) if error occured - */ -template -bool encode16bitImage(ScanChannelData& res_buf, - const Eigen::Ref>& img); - -/** - * @defgroup OSFPngEncode32 Encoding Functionality. - * Encode 2D image of a typical lidar scan field channel into a 32 bit, RGBA, - * PNG buffer. - * - * @tparam T The type to use for the output array. - * - * @param[out] res_buf The output buffer with a single encoded PNG. - * @param[in] img 2D image or a single LidarScan field data. - * @return false (0) if operation is successful, true (1) if error occured - */ - -/** - * @copydoc OSFPngEncode32 - * @param[in] px_offset Pixel shift per row used to destagger img data. - */ -template -bool encode32bitImage(ScanChannelData& res_buf, - const Eigen::Ref>& img, - const std::vector& px_offset); - -/** @copydoc OSFPngEncode32 */ -template -bool encode32bitImage(ScanChannelData& res_buf, - const Eigen::Ref>& img); - -/** - * @defgroup OSFPngEncode24 Encoding Functionality. - * Encode 2D image of a typical lidar scan field channel into a 24 bit, RGB, - * PNG buffer. - * - * @tparam T The type to use for the output array. - * - * @param[out] res_buf The output buffer with a single encoded PNG. - * @param[in] img 2D image or a single LidarScan field data. - * @return false (0) if operation is successful, true (1) if error occured - */ - -/** - * @copydoc OSFPngEncode24 - * @param[in] px_offset Pixel shift per row used to destagger img data. - */ -template -bool encode24bitImage(ScanChannelData& res_buf, - const Eigen::Ref>& img, - const std::vector& px_offset); - -/** @copydoc OSFPngEncode24 */ -template -bool encode24bitImage(ScanChannelData& res_buf, - const Eigen::Ref>& img); - -/** - * @defgroup OSFPngEncode64 Encoding Functionality. - * Encode 2D image of a typical lidar scan field channel into a 64 bit, RGBA, - * PNG buffer. - * - * @tparam T The type to use for the output array. - * - * @param[out] res_buf The output buffer with a single encoded PNG. - * @param[in] img 2D image or a single LidarScan field data. - * @return false (0) if operation is successful, true (1) if error occured - */ - -/** - * @copydoc OSFPngEncode64 - * @param[in] px_offset Pixel shift per row used to destagger img data. - */ -template -bool encode64bitImage(ScanChannelData& res_buf, - const Eigen::Ref>& img, - const std::vector& px_offset); - -/** @copydoc OSFPngEncode64 */ -template -bool encode64bitImage(ScanChannelData& res_buf, - const Eigen::Ref>& img); - -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/reader.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/reader.cpp deleted file mode 100644 index 11e399bf..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/reader.cpp +++ /dev/null @@ -1,1027 +0,0 @@ -/** - * Copyright(c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#include "ouster/osf/reader.h" - -#include -#include - -#include "fb_utils.h" -#include "ouster/osf/basics.h" -#include "ouster/osf/crc32.h" -#include "ouster/osf/file.h" -#include "ouster/osf/meta_streaming_info.h" -#include "ouster/osf/metadata.h" -#include "ouster/types.h" - -using StreamChunksMap = - std::unordered_map>>; - -namespace ouster { -namespace osf { - -namespace { - -inline const ouster::osf::v2::Chunk* get_chunk_from_buf(const uint8_t* buf) { - return ouster::osf::v2::GetSizePrefixedChunk(buf); -} - -} // namespace - -// ======================================================= -// =========== ChunksPile ================================ -// ======================================================= -ChunksPile::ChunksPile() {} - -void ChunksPile::add(uint64_t offset, ts_t start_ts, ts_t end_ts) { - ChunkState cs{}; - cs.offset = offset; - cs.next_offset = std::numeric_limits::max(); - cs.start_ts = start_ts; - cs.end_ts = end_ts; - cs.status = ChunkValidity::UNKNOWN; - pile_[offset] = cs; -} - -void ChunksPile::add_info(uint64_t offset, uint32_t stream_id, - uint32_t message_count) { - auto chunk_state = get(offset); - if (chunk_state == nullptr) { - // allowing adding info on chunks that already present with - // a corresponding chunk states - return; - } - ChunkInfoNode ci{}; - ci.offset = chunk_state->offset; - ci.next_offset = std::numeric_limits::max(); - ci.stream_id = stream_id; - ci.message_count = message_count; - pile_info_[offset] = ci; -} - -ChunkState* ChunksPile::get(uint64_t offset) { - auto cit = pile_.find(offset); - if (cit == pile_.end()) { - return nullptr; - } - return &cit->second; -} - -ChunkInfoNode* ChunksPile::get_info(uint64_t offset) { - auto cit = pile_info_.find(offset); - if (cit == pile_info_.end()) { - return nullptr; - } - return &cit->second; -} - -ChunkInfoNode* ChunksPile::get_info_by_message_idx(uint32_t stream_id, - uint32_t message_idx) { - if (!has_message_idx()) return nullptr; - - auto schunks = stream_chunks_.find(stream_id); - if (schunks == stream_chunks_.end()) return nullptr; - - // out of bounds - auto lci = get_info(schunks->second->back()); - if (message_idx >= lci->message_start_idx + lci->message_count) - return nullptr; - - auto lb = std::lower_bound( - schunks->second->begin(), schunks->second->end(), message_idx, - [&](uint64_t a, uint32_t m_idx) { - const auto* ci = get_info(a); - return ci->message_start_idx + ci->message_count - 1 < m_idx; - }); - - return get_info(*lb); -} - -ChunkState* ChunksPile::get_by_lower_bound_ts(uint32_t stream_id, - const ts_t ts) { - auto schunks = stream_chunks_.find(stream_id); - if (schunks == stream_chunks_.end()) return nullptr; - auto lb_offset = std::lower_bound( - schunks->second->begin(), schunks->second->end(), ts, - [&](uint64_t a, const ts_t t) { return get(a)->end_ts < t; }); - if (lb_offset == schunks->second->end()) return nullptr; - return get(*lb_offset); -} - -ChunkState* ChunksPile::next(uint64_t offset) { - auto chunk = get(offset); - if (!chunk) return nullptr; - return get(chunk->next_offset); -} - -ChunkState* ChunksPile::next_by_stream(uint64_t offset) { - auto chunk_info = get_info(offset); - if (!chunk_info) return nullptr; - return get(chunk_info->next_offset); -} - -ChunkState* ChunksPile::first() { return get(0); } - -size_t ChunksPile::size() const { return pile_.size(); } - -bool ChunksPile::has_message_idx() const { - // return true if we have no chunks (we're functionally indexed) - if (pile_.size() == 0) { - return true; - } - // rely on the fact that message_count in the ChunkInfo, if present - // during Writing/Chunk building, can't be 0 (by construction in - // ChunkBuilder and StreamingLayoutCW) - // In other words we can't have Chunks with 0 messages written to OSF - // file - return pile_info_.size() && pile_info_.begin()->second.message_count > 0; -} - -StreamChunksMap& ChunksPile::stream_chunks() { return stream_chunks_; } - -void ChunksPile::link_stream_chunks() { - // This function does a couple of things: - // 1. Fills the stream_chunks_ map with offsets of chunks per stream id - // 2, Links ChunkInfoNode from pile_info_ to the linked list by offsets - // so the traverse laterally along the same stream_id is easy - // 3. Fills message_start_idx in ChunksInfoNode by counting previous chunks - // message_counts per stream - // Thus the resulting lateral links between ChunkInfoNode allows traversing - // between chunks per stream_id and quick search to the chunk by message_idx - - stream_chunks_.clear(); - - if (pile_info_.size()) { - // Do the next_offset links by streams - auto curr_chunk = first(); - while (curr_chunk != nullptr) { - auto ci = get_info(curr_chunk->offset); - if (ci == nullptr) { - throw std::logic_error("ERROR: Have a missing chunk info"); - } - if (stream_chunks_.count(ci->stream_id)) { - // verifying ts of prev and current chunks on non-decreasing - // invariant. - auto prev_chunk_offset = stream_chunks_[ci->stream_id]->back(); - auto prev_cs = get(prev_chunk_offset); - if (prev_cs->end_ts > curr_chunk->start_ts) { - throw std::logic_error( - "ERROR: Can't have decreasing by timestamp chunks " - "order in StreamingLayout"); - } - // get prev chunk info and update next_offset - auto prev_ci = get_info(prev_chunk_offset); - prev_ci->next_offset = curr_chunk->offset; - ci->message_start_idx = - prev_ci->message_start_idx + prev_ci->message_count; - stream_chunks_[ci->stream_id]->push_back(curr_chunk->offset); - } else { - stream_chunks_.insert( - {ci->stream_id, std::make_shared>( - 1, curr_chunk->offset)}); - } - curr_chunk = get(curr_chunk->next_offset); - } - } -} - -std::string to_string(const ChunkState& chunk_state) { - std::stringstream ss; - ss << "{offset = " << chunk_state.offset - << ", next_offset = " << chunk_state.next_offset - << ", start_ts = " << chunk_state.start_ts.count() - << ", end_ts = " << chunk_state.end_ts.count() - << ", status = " << (int)chunk_state.status << "}"; - return ss.str(); -} - -std::string to_string(const ChunkInfoNode& chunk_info) { - std::stringstream ss; - ss << "{offset = " << chunk_info.offset - << ", next_offset = " << chunk_info.next_offset - << ", stream_id = " << chunk_info.stream_id - << ", message_count = " << chunk_info.message_count - << ", message_start_idx = " << chunk_info.message_start_idx << "}"; - return ss.str(); -} - -// ========================================================== -// ========= Reader::ChunksIter ============================= -// ========================================================== - -ChunksIter::ChunksIter() : current_addr_(0), end_addr_(0), reader_(nullptr) {} - -ChunksIter::ChunksIter(const ChunksIter& other) - : current_addr_(other.current_addr_), - end_addr_(other.end_addr_), - reader_(other.reader_) {} - -ChunksIter::ChunksIter(const uint64_t begin_addr, const uint64_t end_addr, - Reader* reader) - : current_addr_(begin_addr), end_addr_(end_addr), reader_(reader) { - if (current_addr_ != end_addr_ && !is_cleared()) next(); -} - -const ChunkRef ChunksIter::operator*() const { - if (current_addr_ == end_addr_) { - throw std::logic_error("ERROR: Can't dereference end iterator."); - } - return ChunkRef(current_addr_, reader_); -} - -const std::unique_ptr ChunksIter::operator->() const { - return std::make_unique(current_addr_, reader_); -} - -ChunksIter& ChunksIter::operator++() { - this->next(); - return *this; -} - -void ChunksIter::next() { - if (current_addr_ == end_addr_) return; - next_any(); - while (current_addr_ != end_addr_ && !is_cleared()) next_any(); -} - -void ChunksIter::next_any() { - if (current_addr_ == end_addr_) return; - auto next_chunk = reader_->chunks_.next(current_addr_); - if (next_chunk) { - current_addr_ = next_chunk->offset; - } else { - current_addr_ = end_addr_; - } -} - -bool ChunksIter::is_cleared() { - if (current_addr_ == end_addr_) return false; - return reader_->verify_chunk(current_addr_); -} - -bool ChunksIter::operator==(const ChunksIter& other) const { - return (current_addr_ == other.current_addr_ && - end_addr_ == other.end_addr_ && reader_ == other.reader_); -} - -bool ChunksIter::operator!=(const ChunksIter& other) const { - return !this->operator==(other); -} - -std::string ChunksIter::to_string() const { - std::stringstream ss; - ss << "ChunksIter: [ca = " << current_addr_ << ", ea = " << end_addr_ - << "]"; - return ss.str(); -} - -// ======================================================= -// ========= Reader::ChunksRange ========================= -// ======================================================= - -ChunksRange::ChunksRange(const uint64_t begin_addr, const uint64_t end_addr, - Reader* reader) - : begin_addr_(begin_addr), end_addr_(end_addr), reader_(reader) {} - -ChunksIter ChunksRange::begin() const { - return ChunksIter(begin_addr_, end_addr_, reader_); -} - -ChunksIter ChunksRange::end() const { - return ChunksIter(end_addr_, end_addr_, reader_); -} - -std::string ChunksRange::to_string() const { - std::stringstream ss; - ss << "ChunksRange: [ba = " << begin_addr_ << ", ea = " << end_addr_ << "]"; - return ss.str(); -} - -// ========================================================== -// ========= Reader ========================================= -// ========================================================== - -MessagesStreamingRange Reader::messages() { - if (!has_stream_info()) { - throw std::logic_error( - "ERROR: Can't iterate by streams without StreamingInfo " - "available."); - } - return MessagesStreamingRange(start_ts(), end_ts(), {}, this); -} - -MessagesStreamingRange Reader::messages(const ts_t start_ts, - const ts_t end_ts) { - if (!has_stream_info()) { - throw std::logic_error( - "ERROR: Can't iterate by streams without StreamingInfo " - "available."); - } - return MessagesStreamingRange(start_ts, end_ts, {}, this); -} - -MessagesStreamingRange Reader::messages( - const std::vector& stream_ids) { - if (!has_stream_info()) { - throw std::logic_error( - "ERROR: Can't iterate by streams without StreamingInfo " - "available."); - } - return MessagesStreamingRange(start_ts(), end_ts(), stream_ids, this); -} - -MessagesStreamingRange Reader::messages(const std::vector& stream_ids, - const ts_t start_ts, - const ts_t end_ts) { - if (!has_stream_info()) { - throw std::logic_error( - "ERROR: Can't iterate by streams without StreamingInfo " - "available."); - } - return MessagesStreamingRange(start_ts, end_ts, stream_ids, this); -} - -nonstd::optional Reader::ts_by_message_idx(uint32_t stream_id, - uint32_t message_idx) { - if (!has_stream_info()) { - throw std::logic_error( - "ERROR: Can't iterate by streams without StreamingInfo " - "available."); - } - if (!chunks_.has_message_idx()) { - return nonstd::nullopt; - } - // TODO: Check for message_count existence - ChunkInfoNode* cin = - chunks_.get_info_by_message_idx(stream_id, message_idx); - if (!cin) return nonstd::nullopt; - - if (!verify_chunk(cin->offset)) { - return nonstd::nullopt; - } - - auto chunk_msg_index = message_idx - cin->message_start_idx; - - // shortcuting and not reading the chunk content if it's very first message - // and we already checked validity - if (chunk_msg_index == 0) { - return {chunks_.get(cin->offset)->start_ts}; - } - - // reading chunk data to get message timestamp - ChunkRef cref(cin->offset, this); - if (chunk_msg_index < cref.size()) { - return {cref.messages(chunk_msg_index)->ts()}; - } - - return nonstd::nullopt; -} - -bool Reader::has_message_idx() const { return chunks_.has_message_idx(); }; - -ChunksRange Reader::chunks() { - return ChunksRange(0, file_.metadata_offset(), this); -} - -Reader::Reader(const std::string& file) : file_{file} { - if (!file_.valid()) { - std::cerr << "ERROR: While openning OSF file. Expected valid() but " - "got file_ = " - << file_.to_string() << std::endl; - throw std::logic_error("provided OSF file is not a valid OSF file."); - } - - chunks_base_offset_ = file_.chunks_offset(); - - read_metadata(); - - read_chunks_info(); -} - -Reader::Reader(OsfFile& osf_file) : Reader(osf_file.filename()) {} - -void Reader::read_metadata() { - metadata_buf_.resize(FLATBUFFERS_PREFIX_LENGTH); - - file_.seek(file_.metadata_offset()); - file_.read(metadata_buf_.data(), FLATBUFFERS_PREFIX_LENGTH); - size_t meta_size = get_prefixed_size(metadata_buf_.data()); - - const size_t full_meta_size = - meta_size + FLATBUFFERS_PREFIX_LENGTH + CRC_BYTES_SIZE; - - metadata_buf_.resize(full_meta_size); - - // no-op here - file_.seek(file_.metadata_offset() + FLATBUFFERS_PREFIX_LENGTH); - - file_.read(metadata_buf_.data() + FLATBUFFERS_PREFIX_LENGTH, - meta_size + CRC_BYTES_SIZE); - - if (!check_prefixed_size_block_crc(metadata_buf_.data(), full_meta_size)) { - throw std::logic_error("ERROR: Invalid metadata block in OSF file."); - } - - auto metadata = - ouster::osf::gen::GetSizePrefixedMetadata(metadata_buf_.data()); - auto entries = metadata->entries(); - for (uint32_t i = 0; i < entries->size(); ++i) { - auto entry = entries->Get(i); - MetadataEntryRef meta_ref(reinterpret_cast(entry)); - // Option 1: Late reconstruction - // meta_store_.add(meta_ref); - - // Option 2: Early reconstruction (with dynamic_pointer_cast later) - - auto meta_obj = meta_ref.as_type(); - if (meta_obj) { - // Successfull reconstruction of the metadata here. - meta_store_.add(*meta_obj); - } else { - // Can't reconstruct, adding the MetadataEntryRef proxy object - // i.e. late reconstruction path - meta_store_.add(meta_ref); - } - } - - // Get chunks states - std::vector chunk_offsets{}; - if (metadata->chunks() && metadata->chunks()->size() > 0) { - for (uint32_t i = 0; i < metadata->chunks()->size(); ++i) { - auto co = metadata->chunks()->Get(i); - chunks_.add(co->offset(), ts_t{co->start_ts()}, ts_t{co->end_ts()}); - chunk_offsets.push_back(co->offset()); - } - } - - // Assign next_offsets links - if (!chunk_offsets.empty()) { - std::sort(chunk_offsets.begin(), chunk_offsets.end()); - for (size_t i = 0; i < chunk_offsets.size() - 1; ++i) { - chunks_.get(chunk_offsets[i])->next_offset = chunk_offsets[i + 1]; - } - } - - // NOTE: Left here for debugging - // print_metadata_entries(); -} - -void Reader::read_chunks_info() { - // Check that it has StreamingInfo and thus a valid StreamingLayout OSF - // see RFC0018 for details - auto streaming_info = meta_store_.get(); - if (!streaming_info) { - has_streaming_info_ = false; - return; - } - - if (streaming_info->chunks_info().size() != chunks_.size()) { - throw std::logic_error( - "ERROR: StreamingInfo chunks info should equal chunks in the " - "Reader"); - } - - for (const auto& sci : streaming_info->chunks_info()) { - chunks_.add_info(sci.first, sci.second.stream_id, - sci.second.message_count); - } - - has_streaming_info_ = true; - - chunks_.link_stream_chunks(); -} - -// TODO[pb]: MetadataStore to_string() ? - -std::string Reader::metadata_id() const { - if (auto metadata = get_osf_metadata_from_buf(metadata_buf_.data())) { - if (metadata->id()) { - return metadata->id()->str(); - } - } - return std::string{}; -} - -ts_t Reader::start_ts() const { - if (auto metadata = get_osf_metadata_from_buf(metadata_buf_.data())) { - return ts_t{metadata->start_ts()}; - } - return ts_t{}; -} - -ts_t Reader::end_ts() const { - if (auto metadata = get_osf_metadata_from_buf(metadata_buf_.data())) { - return ts_t{metadata->end_ts()}; - } - return ts_t{}; -} - -const MetadataStore& Reader::meta_store() const { return meta_store_; } - -bool Reader::has_stream_info() const { return has_streaming_info_; } - -bool Reader::verify_chunk(uint64_t chunk_offset) { - auto cs = chunks_.get(chunk_offset); - if (!cs) return false; - if (cs->status == ChunkValidity::UNKNOWN) { - auto chunk_buf = file_.read_chunk(chunks_base_offset_ + chunk_offset); - cs->status = - osf::check_osf_chunk_buf(chunk_buf->data(), chunk_buf->size()) - ? ChunkValidity::VALID - : ChunkValidity::INVALID; - } - return (cs->status == ChunkValidity::VALID); -} - -// ========================================================= -// ========= MessageRef ==================================== -// ========================================================= -MessageRef::MessageRef(const uint8_t* buf, const MetadataStore& meta_provider) - : buf_(buf), meta_provider_(meta_provider), chunk_buf_{nullptr} {} - -MessageRef::MessageRef(const uint8_t* buf, const MetadataStore& meta_provider, - std::shared_ptr> chunk_buf) - : buf_(buf), meta_provider_(meta_provider), chunk_buf_{chunk_buf} {} - -uint32_t MessageRef::id() const { - const ouster::osf::v2::StampedMessage* sm = - reinterpret_cast(buf_); - return sm->id(); -} - -MessageRef::ts_t MessageRef::ts() const { - const ouster::osf::v2::StampedMessage* sm = - reinterpret_cast(buf_); - return ts_t(sm->ts()); -} - -const uint8_t* MessageRef::buf() const { return buf_; } - -bool MessageRef::is(const std::string& type_str) const { - auto meta = meta_provider_.get(id()); - return (meta != nullptr) && (meta->type() == type_str); -} - -bool MessageRef::operator==(const MessageRef& other) const { - return (buf_ == other.buf_); -} - -bool MessageRef::operator!=(const MessageRef& other) const { - return !this->operator==(other); -} - -std::string MessageRef::to_string() const { - std::stringstream ss; - const ouster::osf::v2::StampedMessage* sm = - reinterpret_cast(buf_); - ss << "MessageRef: [id = " << id() << ", ts = " << ts().count() - << ", buffer = " - << osf::to_string(sm->buffer()->Data(), sm->buffer()->size(), 100) - << "]"; - return ss.str(); -} - -std::vector MessageRef::buffer() const { - const ouster::osf::gen::StampedMessage* sm = - reinterpret_cast(buf_); - - if (sm->buffer() == nullptr) { - return {}; - } - - return {sm->buffer()->data(), sm->buffer()->data() + sm->buffer()->size()}; -} - -// ======================================================= -// =========== ChunkRef ================================== -// ======================================================= - -ChunkRef::ChunkRef() - : chunk_offset_{std::numeric_limits::max()}, - reader_{nullptr}, - chunk_buf_{nullptr} {} - -ChunkRef::ChunkRef(const uint64_t offset, Reader* reader) - : chunk_offset_(offset), reader_(reader) { - if (!reader->file_.is_memory_mapped()) { - // NOTE[pb]: We just rely on OS file operations caching thus not - // trying to cache the read chunks in reader, however we might - // reconsider it if we will discover the our caching might give - // us better results (for now it's as is) - chunk_buf_ = reader->file_.read_chunk(reader_->chunks_base_offset_ + - chunk_offset_); - } - // Always expects "verified" chunk offset. See Reader::verify_chunk() - assert(reader_->chunks_.get(chunk_offset_)->status != - ChunkValidity::UNKNOWN); -} - -ChunkState* ChunkRef::state() { return reader_->chunks_.get(chunk_offset_); } - -const ChunkState* ChunkRef::state() const { - return reader_->chunks_.get(chunk_offset_); -} - -ChunkInfoNode* ChunkRef::info() { - return reader_->chunks_.get_info(chunk_offset_); -} - -const ChunkInfoNode* ChunkRef::info() const { - return reader_->chunks_.get_info(chunk_offset_); -} - -size_t ChunkRef::size() const { - if (!valid()) return 0; - const ouster::osf::v2::Chunk* chunk = get_chunk_from_buf(get_chunk_ptr()); - if (chunk->messages()) { - return chunk->messages()->size(); - } - return 0; -} - -bool ChunkRef::valid() const { - return (state()->status == ChunkValidity::VALID); -} - -std::unique_ptr ChunkRef::messages(size_t msg_idx) const { - if (!valid()) return nullptr; - const ouster::osf::v2::Chunk* chunk = get_chunk_from_buf(get_chunk_ptr()); - if (!chunk->messages() || msg_idx >= chunk->messages()->size()) - return nullptr; - const ouster::osf::v2::StampedMessage* m = chunk->messages()->Get(msg_idx); - return std::make_unique( - reinterpret_cast(m), reader_->meta_store_, chunk_buf_); -} - -const MessageRef ChunkRef::operator[](size_t msg_idx) const { - const ouster::osf::v2::Chunk* chunk = get_chunk_from_buf(get_chunk_ptr()); - const ouster::osf::v2::StampedMessage* m = chunk->messages()->Get(msg_idx); - return MessageRef(reinterpret_cast(m), reader_->meta_store_, - chunk_buf_); -} - -MessagesChunkIter ChunkRef::begin() const { - return MessagesChunkIter(*this, 0); -} - -MessagesChunkIter ChunkRef::end() const { - return MessagesChunkIter(*this, size()); -} - -bool ChunkRef::operator==(const ChunkRef& other) const { - return (chunk_offset_ == other.chunk_offset_ && reader_ == other.reader_); -} - -bool ChunkRef::operator!=(const ChunkRef& other) const { - return !this->operator==(other); -} - -std::string ChunkRef::to_string() const { - std::stringstream ss; - auto chunk_state = state(); - ss << "ChunkRef: [" - << "msgs_size = " << size() << ", state = (" - << (chunk_state ? osf::to_string(*chunk_state) : "no state") << ")" - << ", chunk_buf_ = " - << (chunk_buf_ ? "size=" + std::to_string(chunk_buf_->size()) - : "nullptr") - << "]"; - return ss.str(); -} - -uint64_t ChunkRef::offset() const { return chunk_offset_; } - -ts_t ChunkRef::start_ts() const { return state()->start_ts; } - -ts_t ChunkRef::end_ts() const { return state()->end_ts; } - -const uint8_t* ChunkRef::get_chunk_ptr() const { - if (reader_->file_.is_memory_mapped()) { - return reader_->file_.buf() + reader_->chunks_base_offset_ + - chunk_offset_; - } - if (chunk_buf_ && !chunk_buf_->empty()) { - return chunk_buf_->data(); - } - - return nullptr; -} - -// ========================================================== -// ========= MessagesChunkIter ============================== -// ========================================================== - -MessagesChunkIter::MessagesChunkIter() : chunk_ref_{}, msg_idx_{0} {} - -MessagesChunkIter::MessagesChunkIter(const MessagesChunkIter& other) - : chunk_ref_(other.chunk_ref_), msg_idx_(other.msg_idx_) {} - -MessagesChunkIter::MessagesChunkIter(const ChunkRef chunk_ref, - const size_t msg_idx) - : chunk_ref_(chunk_ref), msg_idx_(msg_idx) {} - -bool MessagesChunkIter::operator==(const MessagesChunkIter& other) const { - return (chunk_ref_ == other.chunk_ref_ && msg_idx_ == other.msg_idx_); -} - -bool MessagesChunkIter::operator!=(const MessagesChunkIter& other) const { - return !this->operator==(other); -} - -std::string MessagesChunkIter::to_string() const { - std::stringstream ss; - ss << "MessagesChunkIter: [chunk_ref = " << chunk_ref_.to_string() - << ", msg_idx = " << msg_idx_ << "]"; - return ss.str(); -} - -const MessageRef MessagesChunkIter::operator*() const { - return chunk_ref_[msg_idx_]; -} - -std::unique_ptr MessagesChunkIter::operator->() const { - return chunk_ref_.messages(msg_idx_); -} - -MessagesChunkIter& MessagesChunkIter::operator++() { - this->next(); - return *this; -} - -MessagesChunkIter MessagesChunkIter::operator++(int) { - auto res = *this; - this->next(); - return res; -} - -MessagesChunkIter& MessagesChunkIter::operator--() { - this->prev(); - return *this; -} - -MessagesChunkIter MessagesChunkIter::operator--(int) { - auto res = *this; - this->prev(); - return res; -} - -void MessagesChunkIter::next() { - if (msg_idx_ < chunk_ref_.size()) ++msg_idx_; -} - -void MessagesChunkIter::prev() { - if (msg_idx_ > 0) --msg_idx_; -} - -// ========================================================== -// ========= SreeamingReader::MessagesStreamingIter ========= -// ========================================================== - -// Simplest hash to better compare priority que states with stream ids -uint32_t calc_stream_ids_hash(const std::vector& stream_ids) { - uint32_t b = 378551; - uint32_t a = 63689; - uint32_t hash = 0; - std::vector tmp_stream_ids{stream_ids}; - std::sort(tmp_stream_ids.begin(), tmp_stream_ids.end()); - for (std::size_t i = 0; i < tmp_stream_ids.size(); ++i) { - hash = hash * a + tmp_stream_ids[i]; - a *= b; - } - return hash; -} - -bool MessagesStreamingIter::greater_chunk_type::operator()( - const opened_chunk_type& a, const opened_chunk_type& b) { - return a.first[a.second].ts() > b.first[b.second].ts(); -} - -MessagesStreamingIter::MessagesStreamingIter() - : curr_ts_{}, - end_ts_{}, - stream_ids_{}, - stream_ids_hash_{}, - reader_{nullptr}, - curr_chunks_{} {} - -MessagesStreamingIter::MessagesStreamingIter(const MessagesStreamingIter& other) - : curr_ts_{other.curr_ts_}, - end_ts_{other.end_ts_}, - stream_ids_{other.stream_ids_}, - stream_ids_hash_{other.stream_ids_hash_}, - reader_{other.reader_}, - curr_chunks_{other.curr_chunks_} {} - -MessagesStreamingIter::MessagesStreamingIter( - const ts_t start_ts, const ts_t end_ts, - const std::vector& stream_ids, Reader* reader) - : curr_ts_{start_ts}, - end_ts_{end_ts}, - stream_ids_{stream_ids}, - stream_ids_hash_{calc_stream_ids_hash(stream_ids_)}, - reader_{reader} { - if (curr_ts_ == end_ts_) return; - - if (stream_ids_.empty()) { - for (const auto& sm : reader_->chunks_.stream_chunks()) { - stream_ids_.push_back(sm.first); - } - } - - // Per every stream_id open the first valid chunk in [start_ts, end_ts) - // range. Steps: - // 1. find first chunk by start_ts (lower bound) - // 2. if chunk is valid open it, otherwise step 5 - // 3. find first message within chunk in [start_ts, end_ts) range - // 4. addd opened chunk (offset, msg_idx) to the queue of streams we are - // reading - // 5. move to the next chunk within stream, and continue from Step 2. - for (const auto stream_id : stream_ids_) { - // 1. find first chunk by start_ts (lower bound) - auto* cs = reader_->chunks_.get_by_lower_bound_ts(stream_id, start_ts); - bool filled = false; - while (cs != nullptr && cs->start_ts < end_ts && !filled) { - auto curr_offset = cs->offset; - if (reader_->verify_chunk(curr_offset)) { - // 2. if chunk is valid open it, otherwise step 5 - ChunkRef cref{curr_offset, reader_}; - for (size_t msg_idx = 0; msg_idx < cref.size(); ++msg_idx) { - // 3. find first message withing chunk in [start_ts, end_ts) - // range - if (cref[msg_idx].ts() >= start_ts && - cref[msg_idx].ts() < end_ts) { - // 4. addd opened chunk (offset, msg_idx) to the queue - // of streams we are reading - curr_chunks_.emplace(cref, msg_idx); - filled = true; - break; - } - } - } - // 5. move to the next chunk within stream, and continue from - // Step 2 - cs = reader_->chunks_.next_by_stream(curr_offset); - } - } - - if (!curr_chunks_.empty()) { - const auto& curr_item = curr_chunks_.top(); - curr_ts_ = curr_item.first[curr_item.second].ts(); - } else { - curr_ts_ = end_ts_; - } -} - -const MessageRef MessagesStreamingIter::operator*() const { - const auto& curr_item = curr_chunks_.top(); - return curr_item.first[curr_item.second]; -} - -std::unique_ptr MessagesStreamingIter::operator->() const { - const auto& curr_item = curr_chunks_.top(); - return curr_item.first.messages(curr_item.second); -} - -// It's not a full equality because priority_queue requires either gnarly -// hacks to access internal collection or re-implementation. -// But for the purpose of checking the iterator position in a collection -// and checking boundaries it should be somewhat "correct" -// -// TODO[pb]: This should be revisited later with priority_queue -// re-implementation that will be easier to work with for our multi-streams -// case. -bool MessagesStreamingIter::operator==( - const MessagesStreamingIter& other) const { - return (curr_ts_ == other.curr_ts_ && end_ts_ == other.end_ts_ && - reader_ == other.reader_ && - stream_ids_hash_ == other.stream_ids_hash_ && - curr_chunks_.size() == other.curr_chunks_.size() && - (curr_chunks_.empty() || - curr_chunks_.top() == other.curr_chunks_.top())); -} - -bool MessagesStreamingIter::operator!=( - const MessagesStreamingIter& other) const { - return !this->operator==(other); -} - -MessagesStreamingIter& MessagesStreamingIter::operator++() { - this->next(); - return *this; -} - -MessagesStreamingIter MessagesStreamingIter::operator++(int) { - auto res = *this; - this->next(); - return res; -} - -void MessagesStreamingIter::next() { - if (curr_ts_ >= end_ts_) return; - - const auto curr_item = curr_chunks_.top(); - curr_chunks_.pop(); - - const ChunkRef& cref = curr_item.first; - size_t msg_idx = curr_item.second; - - if (msg_idx + 1 < cref.size()) { - // Traversing current chunk - ++msg_idx; - if (cref[msg_idx].ts() < end_ts_) { - curr_chunks_.emplace(cref, msg_idx); - } - } else { - // Looking for the next chunk of the current stream_id - // const auto curr_stream_id = cref[msg_idx].id(); - auto next_chunk_state = - reader_->chunks_.next_by_stream(curr_item.first.offset()); - if (next_chunk_state) { - auto next_chunk_info = - reader_->chunks_.get_info(next_chunk_state->offset); - if (next_chunk_info == nullptr) { - throw std::logic_error( - "ERROR: Can't iterate by streams without StreamingInfo " - "available."); - } - if (next_chunk_state->start_ts < end_ts_) { - if (reader_->verify_chunk(next_chunk_state->offset)) { - ChunkRef cref{next_chunk_state->offset, reader_}; - for (size_t msg_idx = 0; msg_idx < cref.size(); ++msg_idx) { - if (cref[msg_idx].ts() < curr_ts_) { - throw std::logic_error( - "ERROR: Can't have decreasing by timestamp " - "messages in StreamingLayout"); - } - if (cref[msg_idx].ts() < end_ts_) { - curr_chunks_.emplace(cref, msg_idx); - break; - } - } - } - } - } - } - - if (!curr_chunks_.empty()) { - const auto& curr_item = curr_chunks_.top(); - const auto next_ts = curr_item.first[curr_item.second].ts(); - if (next_ts < curr_ts_) { - throw std::logic_error( - "ERROR: Can't have decreasing by timestamp messages " - "in StreamingLayout"); - } - curr_ts_ = next_ts; - } else { - curr_ts_ = end_ts_; - } -} - -std::string MessagesStreamingIter::to_string() const { - std::stringstream ss; - ss << "MessagesStreamingIter: [curr_ts = " << curr_ts_.count() - << ", end_ts = " << end_ts_.count() - << ", curr_chunks_.size = " << curr_chunks_.size() - << ", stream_ids_hash_ = " << stream_ids_hash_; - if (!curr_chunks_.empty()) { - const auto& curr_item = curr_chunks_.top(); - ss << ", top = (ts = " << curr_item.first[curr_item.second].ts().count() - << ", id = " << curr_item.first[curr_item.second].id() << ")"; - } - ss << "]"; - return ss.str(); -} - -// ========================================================= -// ========= StreamingReader::MessagesStreamingRange ======= -// ========================================================= - -MessagesStreamingRange::MessagesStreamingRange( - const ts_t start_ts, const ts_t end_ts, - const std::vector& stream_ids, Reader* reader) - : start_ts_(start_ts), - end_ts_(end_ts), - stream_ids_{stream_ids}, - reader_{reader} {} - -MessagesStreamingIter MessagesStreamingRange::begin() const { - return MessagesStreamingIter(start_ts_, end_ts_ + ts_t{1}, stream_ids_, - reader_); -} - -MessagesStreamingIter MessagesStreamingRange::end() const { - return MessagesStreamingIter(end_ts_ + ts_t{1}, end_ts_ + ts_t{1}, - stream_ids_, reader_); -} - -std::string MessagesStreamingRange::to_string() const { - std::stringstream ss; - ss << "MessagesStreamingRange: [start_ts = " << start_ts_.count() - << ", end_ts = " << end_ts_.count() << "]"; - return ss.str(); -} - -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/stream_lidar_scan.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/stream_lidar_scan.cpp deleted file mode 100644 index ef47e815..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/stream_lidar_scan.cpp +++ /dev/null @@ -1,395 +0,0 @@ -/** - * Copyright(c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#include "ouster/osf/stream_lidar_scan.h" - -#include -#include - -#include "ouster/lidar_scan.h" -#include "ouster/osf/basics.h" -#include "ouster/types.h" -#include "png_tools.h" - -namespace ouster { -namespace osf { - -namespace { - -gen::CHAN_FIELD to_osf_enum(sensor::ChanField f) { - // TODO[pb]: When we start diverging add a better mapping. - return static_cast(f); -} - -sensor::ChanField from_osf_enum(gen::CHAN_FIELD f) { - // TODO[pb]: When we start diverging add a better mapping. - return static_cast(f); -} - -gen::CHAN_FIELD_TYPE to_osf_enum(sensor::ChanFieldType f) { - // TODO[pb]: When we start diverging add a better mapping. - return static_cast(f); -} - -sensor::ChanFieldType from_osf_enum(gen::CHAN_FIELD_TYPE ft) { - // TODO[pb]: When we start diverging add a better mapping. - return static_cast(ft); -} - -} // namespace - -bool poses_present(const LidarScan& ls) { - return std::find_if_not(ls.pose().begin(), ls.pose().end(), - [](const mat4d& m) { return m.isIdentity(); }) != - ls.pose().end(); -} - -LidarScan slice_with_cast(const LidarScan& ls_src, - const LidarScanFieldTypes& field_types) { - LidarScan ls_dest{static_cast(ls_src.w), - static_cast(ls_src.h), field_types.begin(), - field_types.end()}; - - ls_dest.frame_id = ls_src.frame_id; - - // Copy headers - ls_dest.timestamp() = ls_src.timestamp(); - ls_dest.measurement_id() = ls_src.measurement_id(); - ls_dest.status() = ls_src.status(); - ls_dest.pose() = ls_src.pose(); - ls_dest.packet_timestamp() = ls_src.packet_timestamp(); - - // Copy fields - for (const auto& ft : field_types) { - if (ls_src.field_type(ft.first)) { - ouster::impl::visit_field(ls_dest, ft.first, - ouster::impl::copy_and_cast(), ls_src, - ft.first); - } else { - throw std::invalid_argument("Required field '" + - sensor::to_string(ft.first) + - "' is missing from scan."); - } - } - - return ls_dest; -} - -// === LidarScanStream support functions ==== - -// After Flatbuffers >= 22.9.24 the alignment bug was introduced in the #7520 -// https://github.com/google/flatbuffers/pull/7520 -// That is manifested in the additional 2 bytes written to the vector buffer -// before adding the vector size that are not properly handled by the reading -// the buffer back since vector is the 4 bytes length with the data starting -// right after the size. And with those additional 2 zero bytes it's just -// ruining the vector data. -// It started happening because of alignment rules changed in #7520 and it's -// triggering for our code because he have a small structs of just 2 bytes -// which can result in not 4 bytes aligned memory that is min requirement -// for storing the subsequent vector length in uoffset_t type) -// FIX[pb]: We are changing the original CreateVectorOfStructs implementation -// with CreateUninitializedVectorOfStructs impl that is different in -// a way how it's using StartVector underneath. -template -flatbuffers::Offset> CreateVectorOfStructs( - flatbuffers::FlatBufferBuilder& _fbb, const T* v, size_t len) { - T* buf_to_write; - auto res_off = _fbb.CreateUninitializedVectorOfStructs(len, &buf_to_write); - if (len > 0) { - memcpy(buf_to_write, reinterpret_cast(v), - sizeof(T) * len); - } - return res_off; -} - -flatbuffers::Offset create_lidar_scan_msg( - flatbuffers::FlatBufferBuilder& fbb, const LidarScan& lidar_scan, - const ouster::sensor::sensor_info& info, - const LidarScanFieldTypes meta_field_types) { - auto ls = lidar_scan; - if (!meta_field_types.empty()) { - // Make a reduced field LidarScan (erroring if we are missing anything) - ls = slice_with_cast(lidar_scan, meta_field_types); - } - // Encode LidarScan to PNG buffers - ScanData scan_data = scanEncode(ls, info.format.pixel_shift_by_row); - // Prepare PNG encoded channels for LidarScanMsg.channels vector - std::vector> channels; - for (const auto& channel_data : scan_data) { - channels.emplace_back(gen::CreateChannelDataDirect(fbb, &channel_data)); - } - // Prepare field_types for LidarScanMsg - std::vector field_types; - for (const auto& f : ls) { - field_types.emplace_back(to_osf_enum(f.first), to_osf_enum(f.second)); - } - auto channels_off = - fbb.CreateVector<::flatbuffers::Offset>(channels); - auto field_types_off = osf::CreateVectorOfStructs( - fbb, field_types.data(), field_types.size()); - auto timestamp_off = fbb.CreateVector(ls.timestamp().data(), - ls.timestamp().size()); - auto measurement_id_off = - fbb.CreateVector(ls.measurement_id().data(), ls.w); - auto status_off = fbb.CreateVector(ls.status().data(), ls.w); - flatbuffers::Offset> pose_off = 0; - if (poses_present(ls)) { - pose_off = fbb.CreateVector(ls.pose().data()->data(), - ls.pose().size() * 16); - } - auto packet_timestamp_id_off = fbb.CreateVector( - ls.packet_timestamp().data(), ls.packet_timestamp().size()); - return gen::CreateLidarScanMsg( - fbb, channels_off, field_types_off, timestamp_off, measurement_id_off, - status_off, ls.frame_id, pose_off, packet_timestamp_id_off); -} - -std::unique_ptr restore_lidar_scan( - const std::vector buf, const ouster::sensor::sensor_info& info) { - auto ls_msg = - flatbuffers::GetSizePrefixedRoot( - buf.data()); - - uint32_t width = info.format.columns_per_frame; - uint32_t height = info.format.pixels_per_column; - - // read field_types - LidarScanFieldTypes field_types; - if (ls_msg->field_types() && ls_msg->field_types()->size()) { - std::transform( - ls_msg->field_types()->begin(), ls_msg->field_types()->end(), - std::back_inserter(field_types), [](const gen::ChannelField* p) { - return std::make_pair(from_osf_enum(p->chan_field()), - from_osf_enum(p->chan_field_type())); - }); - } - - // Init lidar scan with recovered fields - auto ls = std::make_unique(width, height, field_types.begin(), - field_types.end()); - - ls->frame_id = ls_msg->frame_id(); - - // Set timestamps per column - auto msg_ts_vec = ls_msg->header_timestamp(); - if (msg_ts_vec) { - if (static_cast(ls->w) == msg_ts_vec->size()) { - for (uint32_t i = 0; i < width; ++i) { - ls->timestamp()[i] = msg_ts_vec->Get(i); - } - } else if (msg_ts_vec->size() != 0) { - std::cout << "ERROR: LidarScanMsg has header_timestamp of length: " - << msg_ts_vec->size() << ", expected: " << ls->w - << std::endl; - return nullptr; - } - } - - // Set measurement_id per column - auto msg_mid_vec = ls_msg->header_measurement_id(); - if (msg_mid_vec) { - if (static_cast(ls->w) == msg_mid_vec->size()) { - for (uint32_t i = 0; i < width; ++i) { - ls->measurement_id()[i] = msg_mid_vec->Get(i); - } - } else if (msg_mid_vec->size() != 0) { - std::cout - << "ERROR: LidarScanMsg has header_measurement_id of length: " - << msg_mid_vec->size() << ", expected: " << ls->w << std::endl; - return nullptr; - } - } - - // Set status per column - auto msg_status_vec = ls_msg->header_status(); - if (msg_status_vec) { - if (static_cast(ls->w) == msg_status_vec->size()) { - for (uint32_t i = 0; i < width; ++i) { - ls->status()[i] = msg_status_vec->Get(i); - } - } else if (msg_status_vec->size() != 0) { - std::cout << "ERROR: LidarScanMsg has header_status of length: " - << msg_status_vec->size() << ", expected: " << ls->w - << std::endl; - return nullptr; - } - } - - // Set poses per column - auto pose_vec = ls_msg->pose(); - if (pose_vec) { - if (static_cast(ls->pose().size() * 16) == pose_vec->size()) { - for (uint32_t i = 0; i < static_cast(ls->pose().size()); - ++i) { - for (uint32_t el = 0; el < 16; ++el) { - *(ls->pose()[i].data() + el) = pose_vec->Get(i * 16 + el); - } - } - } else if (pose_vec->size() != 0) { - std::cout << "ERROR: LidarScanMsg has pose of length: " - << pose_vec->size() - << ", expected: " << (ls->pose().size() * 16) - << std::endl; - return nullptr; - } - } - - // Set packet timestamp per lidar packet - auto packet_ts_vec = ls_msg->packet_timestamp(); - if (packet_ts_vec) { - if (static_cast(ls->packet_timestamp().size()) == - packet_ts_vec->size()) { - for (uint32_t i = 0; i < packet_ts_vec->size(); ++i) { - ls->packet_timestamp()[i] = packet_ts_vec->Get(i); - } - } else if (packet_ts_vec->size() != 0) { - std::cout << "ERROR: LidarScanMsg has packet_timestamp of length: " - << packet_ts_vec->size() - << ", expected: " << ls->packet_timestamp().size() - << std::endl; - return nullptr; - } - } - - // Fill Scan Data with scan channels - auto msg_scan_vec = ls_msg->channels(); - if (!msg_scan_vec || !msg_scan_vec->size()) { - std::cout - << "ERROR: lidar_scan msg doesn't have scan field or it's empty.\n"; - return nullptr; - } - ScanData scan_data; - for (uint32_t i = 0; i < msg_scan_vec->size(); ++i) { - auto channel_buffer = msg_scan_vec->Get(i)->buffer(); - scan_data.emplace_back(channel_buffer->begin(), channel_buffer->end()); - } - - // Decode PNGs data to LidarScan - if (scanDecode(*ls, scan_data, info.format.pixel_shift_by_row)) { - return nullptr; - } - - return ls; -} - -LidarScanStreamMeta::LidarScanStreamMeta(const uint32_t sensor_meta_id, - const LidarScanFieldTypes field_types) - : sensor_meta_id_{sensor_meta_id}, - field_types_{field_types.begin(), field_types.end()} {} - -uint32_t LidarScanStreamMeta::sensor_meta_id() const { return sensor_meta_id_; } - -const LidarScanFieldTypes& LidarScanStreamMeta::field_types() const { - return field_types_; -} - -std::vector LidarScanStreamMeta::buffer() const { - flatbuffers::FlatBufferBuilder fbb = flatbuffers::FlatBufferBuilder(512); - - // Make and store field_types with details for LidarScanStream - std::vector field_types; - for (const auto& ft : field_types_) { - field_types.emplace_back(to_osf_enum(ft.first), to_osf_enum(ft.second)); - } - - auto field_types_off = osf::CreateVectorOfStructs( - fbb, field_types.data(), field_types.size()); - - auto lss_offset = ouster::osf::gen::CreateLidarScanStream( - fbb, sensor_meta_id_, field_types_off); - - fbb.FinishSizePrefixed(lss_offset); - - const uint8_t* buf = fbb.GetBufferPointer(); - const size_t size = fbb.GetSize(); - return {buf, buf + size}; -}; - -std::unique_ptr LidarScanStreamMeta::from_buffer( - const std::vector& buf) { - auto lidar_scan_stream = gen::GetSizePrefixedLidarScanStream(buf.data()); - auto sensor_meta_id = lidar_scan_stream->sensor_id(); - - auto field_types_vec = lidar_scan_stream->field_types(); - LidarScanFieldTypes field_types; - if (field_types_vec && field_types_vec->size()) { - std::transform( - field_types_vec->begin(), field_types_vec->end(), - std::back_inserter(field_types), [](const gen::ChannelField* p) { - return std::make_pair(from_osf_enum(p->chan_field()), - from_osf_enum(p->chan_field_type())); - }); - } - - // auto frame_mode = lidar_scan_stream->lidar_frame_mode(); - return std::make_unique(sensor_meta_id, field_types); -}; - -std::string LidarScanStreamMeta::repr() const { - std::stringstream ss; - ss << "LidarScanStreamMeta: sensor_id = " << sensor_meta_id_ - << ", field_types = {"; - bool first = true; - for (const auto& f : field_types_) { - if (!first) ss << ", "; - ss << sensor::to_string(f.first) << ":" - << ouster::sensor::to_string(f.second); - first = false; - } - ss << "}"; - return ss.str(); -} - -// ============== LidarScan Stream ops =========================== - -LidarScanStream::LidarScanStream(Token /*key*/, Writer& writer, - const uint32_t sensor_meta_id, - const LidarScanFieldTypes& field_types) - : writer_{writer}, - meta_(sensor_meta_id, field_types), - sensor_meta_id_(sensor_meta_id) { - // Note key is ignored and just used to gatekeep. - // Check sensor and get sensor_info - auto sensor_meta_entry = writer.get_metadata(sensor_meta_id_); - if (sensor_meta_entry == nullptr) { - std::stringstream ss; - ss << "ERROR: can't find sensor_meta_id = " << sensor_meta_id; - throw std::logic_error(ss.str()); - } - - sensor_info_ = sensor_meta_entry->info(); - - stream_meta_id_ = writer_.add_metadata(meta_); -} - -// TODO[pb]: Every save func in Streams is uniform, need to nicely extract -// it and remove close dependence on Writer? ... -void LidarScanStream::save(const ouster::osf::ts_t ts, - const LidarScan& lidar_scan) { - const auto& msg_buf = make_msg(lidar_scan); - writer_.save_message(meta_.id(), ts, msg_buf); -} - -std::vector LidarScanStream::make_msg(const LidarScan& lidar_scan) { - flatbuffers::FlatBufferBuilder fbb = flatbuffers::FlatBufferBuilder(32768); - auto ls_msg_offset = create_lidar_scan_msg(fbb, lidar_scan, sensor_info_, - meta_.field_types()); - fbb.FinishSizePrefixed(ls_msg_offset); - const uint8_t* buf = fbb.GetBufferPointer(); - const size_t size = fbb.GetSize(); - return {buf, buf + size}; -} - -std::unique_ptr LidarScanStream::decode_msg( - const std::vector& buf, const LidarScanStream::meta_type& meta, - const MetadataStore& meta_provider) { - auto sensor = meta_provider.get(meta.sensor_meta_id()); - auto info = sensor->info(); - return restore_lidar_scan(buf, info); -} - -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/writer.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/writer.cpp deleted file mode 100644 index 4f6e5934..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/src/writer.cpp +++ /dev/null @@ -1,335 +0,0 @@ -/** - * Copyright(c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#include "ouster/osf/writer.h" - -#include - -#include "fb_utils.h" -#include "ouster/osf/basics.h" -#include "ouster/osf/crc32.h" -#include "ouster/osf/layout_streaming.h" -#include "ouster/osf/stream_lidar_scan.h" - -constexpr size_t MAX_CHUNK_SIZE = 500 * 1024 * 1024; - -namespace ouster { -namespace osf { - -Writer::Writer(const std::string& filename, uint32_t chunk_size) - : file_name_(filename), - metadata_id_{"ouster_sdk"}, - chunks_layout_{ChunksLayout::LAYOUT_STREAMING} { - // chunks STREAMING_LAYOUT - chunks_writer_ = std::make_shared(*this, chunk_size); - - // or chunks STANDARD_LAYOUT (left for now to show the mechanisms of - // switching layout strategies) chunks_writer_ = - // std::make_shared(*this, chunk_size); - - // TODO[pb]: Check if file exists, add flag overwrite/not overwrite, etc - - header_size_ = start_osf_file(file_name_); - - if (header_size_ > 0) { - pos_ = static_cast(header_size_); - } else { - throw std::runtime_error("ERROR: Can't write to file :("); - } -} - -Writer::Writer(const std::string& filename, - const ouster::sensor::sensor_info& info, - const LidarScanFieldTypes& field_types, uint32_t chunk_size) - : Writer(filename, std::vector{info}, - field_types, chunk_size) {} - -Writer::Writer(const std::string& filename, - const std::vector& info, - const LidarScanFieldTypes& field_types, uint32_t chunk_size) - : Writer(filename, chunk_size) { - sensor_info_ = info; - for (uint32_t i = 0; i < info.size(); i++) { - lidar_meta_id_[i] = add_metadata(ouster::osf::LidarSensor(info[i])); - field_types_.push_back(field_types); - } -} - -const std::vector& Writer::sensor_info() const { - return sensor_info_; -} - -const ouster::sensor::sensor_info Writer::sensor_info(int stream_index) const { - return sensor_info_[stream_index]; -} - -uint32_t Writer::sensor_info_count() const { return sensor_info_.size(); } - -uint32_t Writer::add_sensor(const ouster::sensor::sensor_info& info, - const LidarScanFieldTypes& field_types) { - lidar_meta_id_[lidar_meta_id_.size()] = - add_metadata(ouster::osf::LidarSensor(info)); - field_types_.push_back(field_types); - sensor_info_.push_back(info); - return lidar_meta_id_.size() - 1; -} - -void Writer::_save(uint32_t stream_index, const LidarScan& scan, - const ts_t time) { - if (stream_index < lidar_meta_id_.size()) { - auto item = lidar_streams_.find(stream_index); - if (item == lidar_streams_.end()) { - const auto& fields = field_types_[stream_index].size() - ? field_types_[stream_index] - : get_field_types(scan); - lidar_streams_[stream_index] = - std::make_unique( - LidarScanStream::Token(), *this, - lidar_meta_id_[stream_index], fields); - } - lidar_streams_[stream_index]->save(time, scan); - } else { - throw std::logic_error("ERROR: Bad Stream ID"); - } -} - -void Writer::save(uint32_t stream_index, const LidarScan& scan) { - if (is_closed()) { - throw std::logic_error("ERROR: Writer is closed"); - } - ts_t time = ts_t(scan.get_first_valid_packet_timestamp()); - _save(stream_index, scan, time); -} - -void Writer::save(uint32_t stream_index, const LidarScan& scan, const ts_t ts) { - if (is_closed()) { - throw std::logic_error("ERROR: Writer is closed"); - } - _save(stream_index, scan, ts); -} - -void Writer::save(const std::vector& scans) { - if (is_closed()) { - throw std::logic_error("ERROR: Writer is closed"); - } - if (scans.size() != lidar_meta_id_.size()) { - throw std::logic_error( - "ERROR: Scans passed in to writer " - "does not match number of sensor infos"); - } else { - for (uint32_t i = 0; i < scans.size(); i++) { - ts_t time = ts_t(scans[i].get_first_valid_packet_timestamp()); - _save(i, scans[i], time); - } - } -} - -uint32_t Writer::add_metadata(MetadataEntry&& entry) { - return add_metadata(entry); -} - -uint32_t Writer::add_metadata(MetadataEntry& entry) { - return meta_store_.add(entry); -} - -std::shared_ptr Writer::get_metadata( - const uint32_t metadata_id) const { - return meta_store_.get(metadata_id); -} - -uint64_t Writer::append(const uint8_t* buf, const uint64_t size) { - if (pos_ < 0) { - throw std::logic_error("ERROR: Writer is not ready (not started?)"); - } - if (finished_) { - throw std::logic_error("ERROR: Hmm, Writer is finished."); - } - if (size == 0) { - std::cout << "nothing to append!!!\n"; - return 0; - } - uint64_t saved_bytes = buffer_to_file(buf, size, file_name_, true); - pos_ += static_cast(saved_bytes); - return saved_bytes; -} - -// > > > ===================== Chunk Emiter operations ====================== - -void Writer::save_message(const uint32_t stream_id, const ts_t ts, - const std::vector& msg_buf) { - if (!meta_store_.get(stream_id)) { - std::stringstream ss; - ss << "ERROR: Attempt to save the non existent stream: id = " - << stream_id << std::endl; - - throw std::logic_error(ss.str()); - - return; - } - - chunks_writer_->save_message(stream_id, ts, msg_buf); -} - -const MetadataStore& Writer::meta_store() const { return meta_store_; } - -const std::string& Writer::metadata_id() const { return metadata_id_; } - -void Writer::set_metadata_id(const std::string& id) { metadata_id_ = id; } - -const std::string& Writer::filename() const { return file_name_; } - -ChunksLayout Writer::chunks_layout() const { return chunks_layout_; } - -uint64_t Writer::emit_chunk(const ts_t chunk_start_ts, const ts_t chunk_end_ts, - const std::vector& chunk_buf) { - uint64_t saved_bytes = append(chunk_buf.data(), chunk_buf.size()); - uint64_t res_chunk_offset{0}; - if (saved_bytes && saved_bytes == chunk_buf.size() + CRC_BYTES_SIZE) { - chunks_.emplace_back(chunk_start_ts.count(), chunk_end_ts.count(), - next_chunk_offset_); - res_chunk_offset = next_chunk_offset_; - if (start_ts_ > chunk_start_ts) start_ts_ = chunk_start_ts; - if (end_ts_ < chunk_end_ts) end_ts_ = chunk_end_ts; - next_chunk_offset_ += saved_bytes; - started_ = true; - } else { - std::stringstream ss; - ss << "ERROR: Can't save to file. saved_bytes = " << saved_bytes - << std::endl; - throw std::logic_error(ss.str()); - } - return res_chunk_offset; -} - -// < < < ================== Chunk Emiter operations ====================== - -std::vector Writer::make_metadata() const { - auto metadata_fbb = flatbuffers::FlatBufferBuilder(32768); - - std::vector> entries = - meta_store_.make_entries(metadata_fbb); - - auto metadata = ouster::osf::gen::CreateMetadataDirect( - metadata_fbb, metadata_id_.c_str(), - !chunks_.empty() ? start_ts_.count() : 0, - !chunks_.empty() ? end_ts_.count() : 0, &chunks_, &entries); - - metadata_fbb.FinishSizePrefixed(metadata, - ouster::osf::gen::MetadataIdentifier()); - - const uint8_t* buf = metadata_fbb.GetBufferPointer(); - uint32_t size = metadata_fbb.GetSize(); - - // Construct the std::vector from the start/end pointers. - return {buf, buf + size}; -} - -void Writer::close() { - if (finished_) { - // already did everything - return; - } - - // Finish all chunks in flight - chunks_writer_->finish(); - - // Encode chunks, metadata entries and other fields into final buffer - auto metadata_buf = make_metadata(); - - uint64_t metadata_offset = pos_; - uint64_t metadata_saved_size = - append(metadata_buf.data(), metadata_buf.size()); - if (metadata_saved_size && - metadata_saved_size == metadata_buf.size() + CRC_BYTES_SIZE) { - if (finish_osf_file(file_name_, metadata_offset, metadata_saved_size) == - header_size_) { - finished_ = true; - } else { - std::cerr << "ERROR: Can't finish OSF file!" - "Recorded header of " - "different sizes ..." - << std::endl; - } - } else { - std::cerr << "ERROR: Oh, why we are here and " - "didn't finish correctly?" - << std::endl; - } -} - -uint32_t Writer::chunk_size() const { return chunks_writer_->chunk_size(); } - -Writer::~Writer() { close(); } - -// ================================================================ - -void ChunkBuilder::save_message(const uint32_t stream_id, const ts_t ts, - const std::vector& msg_buf) { - if (finished_) { - std::cerr - << "ERROR: ChunkBuilder is finished and can't accept new messages!" - << std::endl; - return; - } - - if (fbb_.GetSize() + msg_buf.size() > MAX_CHUNK_SIZE) { - throw std::logic_error( - "ERROR: reached max possible" - " chunk size MAX_SIZE"); - } - - update_start_end(ts); - - // wrap the buffer into StampedMessage - auto stamped_msg = - gen::CreateStampedMessageDirect(fbb_, ts.count(), stream_id, &msg_buf); - messages_.push_back(stamped_msg); -} - -void ChunkBuilder::reset() { - start_ts_ = ts_t::max(); - end_ts_ = ts_t::min(); - fbb_.Clear(); - messages_.clear(); - finished_ = false; -} - -uint32_t ChunkBuilder::size() const { return fbb_.GetSize(); } - -uint32_t ChunkBuilder::messages_count() const { - return static_cast(messages_.size()); -} - -ts_t ChunkBuilder::start_ts() const { return start_ts_; } - -ts_t ChunkBuilder::end_ts() const { return end_ts_; } - -void ChunkBuilder::update_start_end(const ts_t ts) { - if (start_ts_ > ts) start_ts_ = ts; - if (end_ts_ < ts) end_ts_ = ts; -} - -std::vector ChunkBuilder::finish() { - if (messages_.empty()) { - finished_ = true; - return {}; - } - - if (!finished_) { - auto chunk = gen::CreateChunkDirect(fbb_, &messages_); - fbb_.FinishSizePrefixed(chunk, gen::ChunkIdentifier()); - finished_ = true; - } - - const uint8_t* buf = fbb_.GetBufferPointer(); - uint32_t size = fbb_.GetSize(); - - return {buf, buf + size}; -} - -// ================================================================ - -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/CMakeLists.txt b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/CMakeLists.txt deleted file mode 100644 index 156e2304..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/CMakeLists.txt +++ /dev/null @@ -1,45 +0,0 @@ -cmake_minimum_required(VERSION 3.1.0) - -find_package(GTest REQUIRED) -find_package(OpenSSL REQUIRED) - -# Each test file should be in a format "_test.cpp" -set(OSF_TESTS_SOURCES png_tools_test.cpp - writer_test.cpp - writerv2_test.cpp - writer_custom_test.cpp - file_test.cpp - crc_test.cpp - file_ops_test.cpp - reader_test.cpp - operations_test.cpp - pcap_source_test.cpp - basics_test.cpp - meta_streaming_info_test.cpp -) - -message(STATUS "OSF: adding testing .... ") - - -# Create "osf_" tests for every test -foreach(TEST_FULL_NAME ${OSF_TESTS_SOURCES}) - get_filename_component(TEST_FILENAME ${TEST_FULL_NAME} NAME_WE) - add_executable(osf_${TEST_FILENAME} ${TEST_FULL_NAME}) - set_target_properties(osf_${TEST_FILENAME} - PROPERTIES RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/tests") - - target_include_directories(osf_${TEST_FILENAME} PRIVATE ${CMAKE_CURRENT_LIST_DIR}/../src) - target_link_libraries(osf_${TEST_FILENAME} PRIVATE ouster_osf - GTest::gtest - GTest::gtest_main - OpenSSL::Crypto) - CodeCoverageFunctionality(osf_${TEST_FILENAME}) - add_test(NAME osf_${TEST_FILENAME} - COMMAND osf_${TEST_FILENAME} --gtest_output=xml:osf_${TEST_FILENAME}.xml) - set_tests_properties( - osf_${TEST_FILENAME} - PROPERTIES - ENVIRONMENT - DATA_DIR=${CMAKE_CURRENT_LIST_DIR}/../../tests/ - ) -endforeach() diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/basics_test.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/basics_test.cpp deleted file mode 100644 index 1b3e2709..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/basics_test.cpp +++ /dev/null @@ -1,30 +0,0 @@ -#include "ouster/osf/basics.h" - -#include - -#include - -#include "fb_utils.h" -#include "osf_test.h" - -namespace ouster { -namespace osf { -namespace { - -class BasicsTest : public OsfTestWithData {}; - -TEST_F(BasicsTest, GetBlockSizeTest) { - const std::string test_file_name = - path_concat(test_data_dir(), "osfs/OS-1-128_v2.3.0_1024x10_lb_n3.osf"); - auto size = ouster::osf::file_size(test_file_name); - char* buf = new char[size]; - std::fstream file_stream; - file_stream.open(test_file_name, std::fstream::in | std::fstream::binary); - file_stream.read(buf, size); - EXPECT_EQ(ouster::osf::get_block_size((uint8_t*)buf), 60); - delete[] buf; -} - -} // namespace -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/common.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/common.h deleted file mode 100644 index 78546c12..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/common.h +++ /dev/null @@ -1,183 +0,0 @@ -/** - * Copyright(c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#pragma once - -#include - -#include -#include -#include -#include -#include -#include -#include - -#include "compat_ops.h" -#include "ouster/lidar_scan.h" -#include "ouster/osf/basics.h" -#include "ouster/osf/stream_lidar_scan.h" - -namespace ouster { -namespace osf { - -constexpr double TEST_EPS = 1e-8; -constexpr char OSF_OUTPUT_DIR[] = "test_osf"; - -using idx = std::ptrdiff_t; - -/// @todo move this stuff into a cpp file -inline bool get_test_data_dir(std::string& test_data_dir) { - std::string test_data_dir_var; - if (get_env_var("DATA_DIR", test_data_dir_var)) { - // printf("DATA_DIR: %s\n", test_data_dir_var); - if (!test_data_dir_var.empty()) { - if (!is_dir(test_data_dir_var)) { - printf("WARNING: DATA_DIR: %s doesn't exist\n", - test_data_dir_var.c_str()); - return false; - } - test_data_dir = test_data_dir_var; - return true; - } - } - printf("ERROR: DATA_DIR env var: is not set for OSF tests\n"); - return false; -} - -/// Get output dir for test files -bool get_output_dir(std::string& output_dir) { - std::string build_dir; - if (get_env_var("BUILD_DIR", build_dir)) { - if (!build_dir.empty()) { - if (!is_dir(build_dir)) { - printf("ERROR: BUILD_DIR: %s doesn't exist yet\n", - build_dir.c_str()); - return false; - } - output_dir = std::string{build_dir} + "/" + OSF_OUTPUT_DIR; - } - } else { - output_dir = OSF_OUTPUT_DIR; - } - if (!is_dir(output_dir)) { - if (!make_dir(output_dir)) { - printf("ERROR: Can't create output_dir: %s\n", output_dir.c_str()); - return false; - } - } - return true; -} - -inline double normal_d(const double m, const double s) { - std::random_device rd; - std::mt19937 gen{rd()}; - std::normal_distribution d{m, s}; - return d(gen); -} - -inline uint32_t normal_d_bounded(const double m, const double s, - const double b = 1 << 20) { - std::random_device rd; - std::mt19937 gen{rd()}; - std::normal_distribution d{m, s}; - double g = d(gen); - while (g < 0 || g >= b) g = d(gen); - return static_cast(g); -} - -// TODO: Extract all this to data generator with rd, gen etc cached -template -std::array normal_arr(const double& m, const double& s) { - std::array arr; - for (size_t i = 0; i < N; ++i) { - arr[i] = normal_d(m, s); - } - return arr; -} - -// set field to random values, with mask_bits specifienging the number of -// bits to mask -struct set_to_random { - template - void operator()(Eigen::Ref> field_dest, size_t mask_bits = 0) { - field_dest = field_dest.unaryExpr([=](T) { - double sr = static_cast(std::rand()) / RAND_MAX; - return static_cast( - sr * static_cast(std::numeric_limits::max())); - }); - if (mask_bits && sizeof(T) * 8 > mask_bits) { - field_dest = field_dest.unaryExpr([=](T a) { - return static_cast(a & ((1LL << mask_bits) - 1)); - }); - } - } -}; - -inline void random_lidar_scan_data(LidarScan& ls) { - using sensor::ChanField; - using sensor::ChanFieldType; - - for (auto f : ls) { - if (f.first == sensor::ChanField::RANGE || - f.first == sensor::ChanField::RANGE2) { - // Closer to reality that RANGE is just 20bits and not all 32 - ouster::impl::visit_field(ls, f.first, set_to_random(), 20); - } else { - ouster::impl::visit_field(ls, f.first, set_to_random()); - } - } - - ls.frame_id = 5; - - const int32_t columns_per_packet = ls.w / ls.packet_timestamp().size(); - const int64_t t_start = 100; - const int64_t t_start_p = 100000000000; - const int64_t dt = 100 * 1000 / (ls.w - 1); - const int64_t dt_p = 100 * 1000 / (ls.packet_timestamp().size() - 1); - for (ptrdiff_t i = 0; i < ls.w; ++i) { - if (i == 0) - ls.timestamp()[i] = t_start; - else - ls.timestamp()[i] = ls.timestamp()[i - 1] + dt; - ls.status()[i] = static_cast( - (std::numeric_limits::max() / ls.w) * i); - ls.measurement_id()[i] = static_cast( - (std::numeric_limits::max() / ls.w) * i); - ls.pose()[i] = ouster::mat4d::Random(); - - const int32_t pi = i / columns_per_packet; - if (pi == 0) - ls.packet_timestamp()[pi] = t_start_p; - else - ls.packet_timestamp()[pi] = ls.packet_timestamp()[pi - 1] + dt_p; - } -} - -inline LidarScan get_random_lidar_scan( - const size_t w = 1024, const size_t h = 64, - sensor::UDPProfileLidar profile = - sensor::UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL) { - LidarScan ls{w, h, profile}; - random_lidar_scan_data(ls); - return ls; -} - -inline LidarScan get_random_lidar_scan(const size_t w = 1024, - const size_t h = 64, - LidarScanFieldTypes field_types = {}) { - LidarScan ls{w, h, field_types.begin(), field_types.end()}; - random_lidar_scan_data(ls); - return ls; -} - -inline LidarScan get_random_lidar_scan(const sensor::sensor_info& si) { - return get_random_lidar_scan(si.format.columns_per_frame, - si.format.pixels_per_column, - si.format.udp_profile_lidar); -} - -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/crc_test.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/crc_test.cpp deleted file mode 100644 index aebedcd2..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/crc_test.cpp +++ /dev/null @@ -1,39 +0,0 @@ -/** - * Copyright(c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#include -#include - -#include -#include -#include -#include - -#include "ouster/osf/crc32.h" - -namespace ouster { -namespace osf { -namespace { - -class CrcTest : public ::testing::Test {}; - -TEST_F(CrcTest, SmokeSanityCheck) { - const std::vector data = {0, 1, 2, 3, 4, 5, 6, 7}; - const uint32_t crc = osf::crc32(data.data(), data.size()); - EXPECT_EQ(0x88aa689f, crc); - - const std::vector data_rev(data.rbegin(), data.rend()); - const uint32_t crc_rev = osf::crc32(data_rev.data(), data_rev.size()); - EXPECT_EQ(0xa1509ef8, crc_rev); -} - -TEST_F(CrcTest, SmokeSanityCheckAltInit) { - const std::vector data = {0, 1, 2, 3, 4, 5, 6, 7}; - const uint32_t crc = osf::crc32(0L, data.data(), data.size()); - EXPECT_EQ(0x88aa689f, crc); -} -} // namespace -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/file_ops_test.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/file_ops_test.cpp deleted file mode 100644 index adfc4007..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/file_ops_test.cpp +++ /dev/null @@ -1,270 +0,0 @@ -/** - * Copyright(c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#include - -#include -#include - -#include "common.h" -#include "osf_test.h" -#include "ouster/osf/meta_lidar_sensor.h" -#include "ouster/osf/reader.h" -#include "ouster/osf/stream_lidar_scan.h" -#include "ouster/osf/writer.h" - -namespace ouster { -namespace osf { -namespace { - -class FileOpsTest : public OsfTestWithDataAndFiles {}; - -TEST_F(FileOpsTest, TempDir) { - std::string tmp_dir; - EXPECT_TRUE(make_tmp_dir(tmp_dir)); - EXPECT_TRUE(path_exists(tmp_dir)); - EXPECT_TRUE(is_dir(tmp_dir)); - - EXPECT_TRUE(remove_dir(tmp_dir)); - - EXPECT_FALSE(path_exists(tmp_dir)); - EXPECT_FALSE(is_dir(tmp_dir)); - - EXPECT_FALSE(unlink_path(tmp_dir)); - EXPECT_FALSE(remove_dir(tmp_dir)); -} - -TEST_F(FileOpsTest, TempAndMakeDir) { - std::string tmp_dir; - EXPECT_TRUE(make_tmp_dir(tmp_dir)); - std::string tmp_dir_new = path_concat(tmp_dir, "new_dir"); - EXPECT_FALSE(path_exists(tmp_dir_new)); - EXPECT_FALSE(is_dir(tmp_dir_new)); - - EXPECT_TRUE(make_dir(tmp_dir_new)); - - EXPECT_TRUE(path_exists(tmp_dir_new)); - EXPECT_TRUE(is_dir(tmp_dir_new)); - - // Can't remove non-empty dir - EXPECT_FALSE(remove_dir(tmp_dir)); - EXPECT_FALSE(unlink_path(tmp_dir)); - - EXPECT_TRUE(remove_dir(tmp_dir_new)); - EXPECT_FALSE(path_exists(tmp_dir_new)); - EXPECT_FALSE(is_dir(tmp_dir_new)); - - EXPECT_TRUE(remove_dir(tmp_dir)); -} - -TEST_F(FileOpsTest, IsDirGeneral) { - EXPECT_FALSE(is_dir("")); - EXPECT_TRUE(is_dir(".")); -} - -TEST_F(FileOpsTest, PathConcats) { - EXPECT_EQ("", path_concat("", "")); - EXPECT_EQ("hello", path_concat("", "hello")); - EXPECT_EQ("hello", path_concat("hello", "")); -#ifdef _WIN32 - EXPECT_EQ("c:\\b", path_concat("c:", "b")); - EXPECT_EQ("/a/b\\f/g/", path_concat("/a/b//", "f/g/")); - EXPECT_EQ("/f/g/", path_concat("/a/b//", "/f/g/")); - EXPECT_EQ("f:/g/", path_concat("/a/b//", "f:/g/")); -#else - EXPECT_EQ("/", path_concat("/", "")); - EXPECT_EQ("////", path_concat("////", "")); - EXPECT_EQ("//", path_concat("//", "")); - EXPECT_EQ("/a", path_concat("//", "a")); - EXPECT_EQ("/a", path_concat("//", "a")); - EXPECT_EQ("/a", path_concat("//b", "/a")); - EXPECT_EQ("/", path_concat("", "/")); - EXPECT_EQ("//", path_concat("", "//")); - EXPECT_EQ("////", path_concat("/", "////")); - EXPECT_EQ("/a/b", path_concat("/a\\", "b")); - EXPECT_EQ("/a/b", path_concat("/a\\//", "b")); - EXPECT_EQ("c:/b", path_concat("c:", "b")); - EXPECT_EQ("/a/b/f", path_concat("/a/b/", "f")); - EXPECT_EQ("/a/b/f/g", path_concat("/a/b/", "f/g")); - EXPECT_EQ("/a/b/f/g/", path_concat("/a/b/", "f/g/")); - EXPECT_EQ("/a/b/f/g/", path_concat("/a/b//", "f/g/")); - EXPECT_EQ("/f/g/", path_concat("/a/b//", "/f/g/")); - // Yes its weird and this function just can't be ideal ... - EXPECT_EQ("/a/b/f:/g/", path_concat("/a/b//", "f:/g/")); -#endif -} - -TEST_F(FileOpsTest, TestDataDirCheck) { EXPECT_TRUE(is_dir(test_data_dir())); } - -TEST_F(FileOpsTest, TestFileSize) { - // TODO[pb]: Change to file creation later ... - const std::string test_file_name = - path_concat(test_data_dir(), "osfs/OS-1-128_v2.3.0_1024x10_lb_n3.osf"); - int64_t fsize = file_size(test_file_name); - EXPECT_EQ(1021684, fsize); - std::string not_a_file = path_concat(test_data_dir(), "not_a_file"); - EXPECT_TRUE(file_size(not_a_file) < 0); - EXPECT_TRUE(file_size(test_data_dir()) < 0); -} - -TEST_F(FileOpsTest, TestFileMapping) { - // TODO[pb]: Change to file creation later ... - const std::string test_file_name = - path_concat(test_data_dir(), "osfs/OS-1-128_v2.3.0_1024x10_lb_n3.osf"); - - uint8_t* file_buf = mmap_open(test_file_name); - EXPECT_TRUE(file_buf != nullptr); - - int64_t fsize = file_size(test_file_name); - EXPECT_EQ(1021684, fsize); - - if (file_buf != nullptr) { - std::cout << "bytes = " << to_string(file_buf, 64) << std::endl; - std::cout << "bytes = " << to_string(file_buf + 4, 64) << std::endl; - std::cout << "bytes = " << to_string(file_buf + 4 + 4, 64) << std::endl; - std::cout << "bytes = " << to_string(file_buf + fsize - 64, 64) - << std::endl; - } - - EXPECT_TRUE(mmap_close(file_buf, fsize)); -} - -TEST_F(FileOpsTest, TruncateFile) { - const int fsize = 1000; - const int trunc_size = 450; - - std::string temp_dir; - EXPECT_TRUE(make_tmp_dir(temp_dir)); - std::string temp_file = path_concat(temp_dir, "test_file"); - - std::fstream test_file_out; - test_file_out.open(temp_file, std::fstream::out | std::fstream::trunc | - std::fstream::binary); - for (int i = 0; i < fsize; i++) { - test_file_out << (uint8_t)i; - } - test_file_out.close(); - - EXPECT_EQ(file_size(temp_file), fsize); - truncate_file(temp_file, trunc_size); - EXPECT_EQ(file_size(temp_file), trunc_size); - unlink_path(temp_file); - remove_dir(temp_dir); -} - -TEST_F(FileOpsTest, AppendBinaryFileBlank) { - const int fsize = 10; - - std::string temp_dir; - EXPECT_TRUE(make_tmp_dir(temp_dir)); - std::string temp_file = path_concat(temp_dir, "test_file"); - std::string temp_file2 = path_concat(temp_dir, "test_file2"); - - std::fstream test_file; - test_file.open(temp_file2, std::fstream::out | std::fstream::trunc | - std::fstream::binary); - for (int i = 0; i < fsize; i++) { - test_file << (uint8_t)i; - } - test_file.close(); - - EXPECT_EQ(file_size(temp_file2), fsize); - EXPECT_EQ(append_binary_file(temp_file, temp_file2), fsize); - EXPECT_EQ(file_size(temp_file), fsize); - - test_file.open(temp_file2, std::fstream::in | std::fstream::binary); - for (int i = 0; i < fsize; i++) { - char temp; - test_file.read(&temp, 1); - EXPECT_EQ(temp, (char)i); - } - test_file.close(); - - unlink_path(temp_file); - unlink_path(temp_file2); - remove_dir(temp_dir); -} - -TEST_F(FileOpsTest, AppendBinaryFile) { - const int fsize1 = 100; - const int fsize2 = 50; - - std::string temp_dir; - EXPECT_TRUE(make_tmp_dir(temp_dir)); - std::string temp_file = path_concat(temp_dir, "test_file"); - std::string temp_file2 = path_concat(temp_dir, "test_file2"); - - std::fstream test_file; - test_file.open(temp_file, std::fstream::out | std::fstream::trunc | - std::fstream::binary); - for (int i = 0; i < fsize1; i++) { - test_file << (uint8_t)i; - } - test_file.close(); - - test_file.open(temp_file2, std::fstream::out | std::fstream::trunc | - std::fstream::binary); - for (int i = 0; i < fsize2; i++) { - test_file << (uint8_t)(i + fsize1); - } - test_file.close(); - - EXPECT_EQ(file_size(temp_file), fsize1); - EXPECT_EQ(file_size(temp_file2), fsize2); - EXPECT_EQ(append_binary_file(temp_file, temp_file2), (fsize1 + fsize2)); - EXPECT_EQ(file_size(temp_file), (fsize1 + fsize2)); - - test_file.open(temp_file, std::fstream::in | std::fstream::binary); - for (int i = 0; i < (fsize1 + fsize2); i++) { - char temp; - test_file.read(&temp, 1); - EXPECT_EQ(temp, (char)i); - } - test_file.close(); - - unlink_path(temp_file); - unlink_path(temp_file2); - remove_dir(temp_dir); -} - -TEST_F(FileOpsTest, CopyTrailingBytes) { - const int fsize1 = 200; - const int offset = 150; - - std::string temp_dir; - EXPECT_TRUE(make_tmp_dir(temp_dir)); - std::string temp_file = path_concat(temp_dir, "test_file"); - std::string temp_file2 = path_concat(temp_dir, "test_file2"); - - std::fstream test_file; - test_file.open(temp_file, std::fstream::out | std::fstream::trunc | - std::fstream::binary); - for (int i = 0; i < fsize1; i++) { - test_file << (uint8_t)i; - } - test_file.close(); - - EXPECT_EQ(file_size(temp_file), fsize1); - EXPECT_EQ(copy_file_trailing_bytes(temp_file, temp_file2, offset), - (fsize1 - offset)); - EXPECT_EQ(file_size(temp_file2), (fsize1 - offset)); - - std::fstream test_file2; - test_file2.open(temp_file2, std::fstream::in | std::fstream::binary); - for (int i = 0; i < (fsize1 - offset); i++) { - char temp; - test_file2.read(&temp, 1); - EXPECT_EQ(temp, (char)(i + offset)); - } - test_file2.close(); - - unlink_path(temp_file); - unlink_path(temp_file2); - remove_dir(temp_dir); -} - -} // namespace -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/file_test.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/file_test.cpp deleted file mode 100644 index 1f052c3e..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/file_test.cpp +++ /dev/null @@ -1,170 +0,0 @@ -/** - * Copyright(c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#include "ouster/osf/file.h" - -#include - -#include "fb_utils.h" -#include "osf_test.h" -#include "ouster/osf/basics.h" - -namespace ouster { -namespace osf { -namespace { - -class OsfFileTest : public OsfTestWithData {}; - -TEST_F(OsfFileTest, OpensOsfFileDefaultAsBadState) { - // This opens nothing and produces the file in a !good() state - OsfFile osf_file; - - EXPECT_FALSE(osf_file.good()); - - // Check operator! - if (!osf_file) { - SUCCEED(); - } - - // Check bool operator - bool ok = osf_file.good(); - if (ok) FAIL(); -} - -TEST_F(OsfFileTest, OpenOsfFileNominally) { - OsfFile osf_file( - path_concat(test_data_dir(), "osfs/OS-1-128_v2.3.0_1024x10_lb_n3.osf")); - EXPECT_TRUE(osf_file); - EXPECT_EQ(osf_file.version(), OSF_VERSION::V_2_0); - EXPECT_EQ(osf_file.size(), 1021684); - EXPECT_EQ(osf_file.offset(), 0); - - EXPECT_EQ(osf_file.metadata_offset(), 1013976); - std::cout << "file = " << osf_file.to_string() << std::endl; - - EXPECT_EQ(osf_file.seek(100).offset(), 100); - - // Out of range seek should throw - EXPECT_THROW(osf_file.seek(50000000), std::out_of_range); - - // OSF v2 OsfFile should be OK - EXPECT_TRUE(osf_file.valid()); - - // Test move semantics - OsfFile osff(std::move(osf_file)); - EXPECT_FALSE(osf_file.good()); - EXPECT_FALSE(osf_file.valid()); - - EXPECT_EQ(osff.offset(), 100); - EXPECT_TRUE(osff.good()); - - OsfFile osf_new; - EXPECT_FALSE(osf_new.good()); - osf_new = std::move(osff); - EXPECT_TRUE(osf_new.good()); - EXPECT_FALSE(osff); - - EXPECT_EQ(osf_new.seek(1001).offset(), 1001); - EXPECT_TRUE(osf_new.valid()); - - if (osf_new.is_memory_mapped()) { - const uint8_t* b = osf_new.buf(); - EXPECT_TRUE(b != nullptr); - } - - // Read header size from the beginning of the file - uint8_t size_buf[4]; - osf_new.seek(0).read(size_buf, 4); - EXPECT_EQ(osf_new.offset(), 4); - - size_t header_size = get_prefixed_size(size_buf); - - // Header length is always 52 bytes (0x34) - EXPECT_EQ(header_size, 52); - - // Close copied out - osff.close(); - EXPECT_FALSE(osff.good()); - EXPECT_FALSE(osff); - - // Close osf dest istance - osf_new.close(); - EXPECT_FALSE(osf_new.good()); - EXPECT_FALSE(osf_new); -} - -TEST_F(OsfFileTest, OpenOsfFileWithStandardRead) { - OsfFile osf_file( - path_concat(test_data_dir(), "osfs/OS-1-128_v2.3.0_1024x10_lb_n3.osf")); - EXPECT_TRUE(osf_file); - EXPECT_EQ(osf_file.version(), OSF_VERSION::V_2_0); - EXPECT_EQ(osf_file.size(), 1021684); - EXPECT_EQ(osf_file.offset(), 0); - - EXPECT_EQ(osf_file.metadata_offset(), 1013976); - std::cout << "file = " << osf_file.to_string() << std::endl; - - EXPECT_TRUE(osf_file.valid()); -} - -TEST_F(OsfFileTest, OpenOsfFileHandleNonExistent) { - const std::string test_file_name = - path_concat(test_data_dir(), "non-file-thing"); - - OsfFile osf_file(test_file_name); - EXPECT_FALSE(osf_file); - - EXPECT_EQ(0, osf_file.size()); - EXPECT_EQ(test_file_name, osf_file.filename()); - EXPECT_EQ(OSF_VERSION::V_INVALID, osf_file.version()); - EXPECT_EQ(0, osf_file.offset()); - - // Access to a bad file is an error - ASSERT_THROW(osf_file.buf(), std::logic_error); - ASSERT_THROW(osf_file.buf(1), std::logic_error); - ASSERT_THROW(osf_file.seek(10), std::logic_error); - - uint8_t buf[10]; - ASSERT_THROW(osf_file.read(buf, 10), std::logic_error); -} - -TEST_F(OsfFileTest, OsfFileDontOpenDir) { - const std::string test_file_dir = test_data_dir(); - - OsfFile osf_file(test_file_dir); - EXPECT_FALSE(osf_file); - - ASSERT_THROW(osf_file.buf(), std::logic_error); -} - -TEST_F(OsfFileTest, OsfFileCheckOutOfRangeAccess) { - const std::string test_file_name = - path_concat(test_data_dir(), "osfs/OS-1-128_v2.3.0_1024x10_lb_n3.osf"); - - OsfFile osf_file(test_file_name); - EXPECT_TRUE(osf_file); - - // Read buffer for read() - constexpr int kBufSize = 10; - uint8_t buf[kBufSize]; - - // In range file access - if (osf_file.is_memory_mapped()) { - ASSERT_NO_THROW(osf_file.buf(100)); - } - ASSERT_NO_THROW(osf_file.seek(100).seek(0)); - ASSERT_NO_THROW(osf_file.read(buf, kBufSize)); - - // Out of range file access - if (osf_file.is_memory_mapped()) { - ASSERT_THROW(osf_file.buf(100000000), std::out_of_range); - } - ASSERT_THROW(osf_file.seek(100000000), std::out_of_range); - ASSERT_THROW(osf_file.read(buf, 100000000), std::out_of_range); -} - -} // namespace -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/meta_streaming_info_test.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/meta_streaming_info_test.cpp deleted file mode 100644 index 4d879f7a..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/meta_streaming_info_test.cpp +++ /dev/null @@ -1,32 +0,0 @@ -#include "ouster/osf/meta_streaming_info.h" - -#include - -#include - -#include "fb_utils.h" -#include "osf_test.h" -#include "ouster/osf/basics.h" - -namespace ouster { -namespace osf { -namespace { - -class MetaStreamingInfoTests : public OsfTestWithData {}; - -/// @todo move this to a better place -TEST_F(MetaStreamingInfoTests, StreamingPrintTests) { - ChunkInfo data = {1, 2, 3}; - EXPECT_EQ(to_string(data), - "{offset = 1, stream_id = 2, message_count = 3}"); - - ts_t t(5678L); - StreamStats data2(4, t, 6); - EXPECT_EQ(to_string(data2), - "{stream_id = 4, start_ts = 5678, end_ts = 5678," - " message_count = 1, message_avg_size = 6}"); -} - -} // namespace -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/metadata_tests.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/metadata_tests.cpp deleted file mode 100644 index f3275aaf..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/metadata_tests.cpp +++ /dev/null @@ -1,61 +0,0 @@ -#include - -#include - -#include "fb_utils.h" -#include "osf_test.h" -#include "ouster/osf/basics.h" -#include "ouster/osf/meta_lidar_sensor.h" -#include "ouster/osf/reader.h" -#include "ouster/osf/stream_lidar_scan.h" - -namespace ouster { -namespace osf { -namespace { - -class MetadataTest : public OsfTestWithData {}; - -TEST_F(MetadataTest, TestDupeMetadata) { - MetadataStore meta_store_ = {}; - LidarScanStreamMeta data(1011121314, {}); - EXPECT_EQ(meta_store_.add(data), 1); - - std::stringstream output_stream; - std::streambuf* old_output_stream = std::cout.rdbuf(); - std::cout.rdbuf(output_stream.rdbuf()); - EXPECT_EQ(meta_store_.add(data), 1); - std::cout.rdbuf(old_output_stream); - EXPECT_EQ(output_stream.str(), - "WARNING: MetadataStore:" - " ENTRY EXISTS! id = 1\n"); -} - -class MetadataTestApi : public ouster::osf::MetadataEntry { - public: - MetadataTestApi(std::string type, std::string static_type, - std::vector buffer) - : _type(type), _static_type(static_type), _buffer(buffer){}; - std::vector buffer() const { return _buffer; }; - std::unique_ptr clone() const { return nullptr; }; - std::string type() const { return _type; }; - std::string static_type() const { return _static_type; }; - - private: - std::string _type; - std::string _static_type; - std::vector _buffer; -}; - -TEST_F(MetadataTest, MiscMetadataEntryTests) { - MetadataTestApi test("Screams And Whispers - Dance With the Dead", - "Good song", {1, 2, 3, 4, 5}); - EXPECT_EQ(test.repr(), "MetadataEntry: 01 02 03 04 05"); - EXPECT_EQ(test.to_string(), - "MetadataEntry: [id = 0, type = Screams And Whispers - Dance " - "With the Dead," - " buffer = {MetadataEntry: 01 02 03 04 05}]"); -} - -} // namespace -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/operations_test.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/operations_test.cpp deleted file mode 100644 index c17846c0..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/operations_test.cpp +++ /dev/null @@ -1,390 +0,0 @@ -/** - * Copyright(c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#include "ouster/osf/operations.h" - -#include -#include -#include -#include - -#include -#include - -#include "fb_utils.h" -#include "json/json.h" -#include "json_utils.h" -#include "osf_test.h" -#include "ouster/osf/basics.h" -#include "ouster/osf/crc32.h" -#include "ouster/osf/file.h" -#include "ouster/osf/meta_lidar_sensor.h" -#include "ouster/osf/reader.h" -#include "ouster/osf/stream_lidar_scan.h" - -namespace ouster { -namespace osf { -namespace { - -// For some reason windows doesnt like the block_size -// init in the code below -#define BLOCK_SIZE (1024 * 1024) -#define FILESHA_DIGEST_SIZE 64 -class FileSha { - public: - FileSha(const std::string& filename) - : context(EVP_MD_CTX_new()), - block_size(BLOCK_SIZE), - fsize(ouster::osf::file_size(filename)), - digest{0}, - digest_size(FILESHA_DIGEST_SIZE) { - // Using NULL for the openssl C api - if (context == NULL) handleEvpError(); - if (EVP_DigestInit_ex(context, EVP_sha512(), NULL) != 1) { - handleEvpError(); - } - - char buf[BLOCK_SIZE]; - uint64_t i = ouster::osf::file_size(filename); - bool finished = false; - - std::fstream reader; - reader.open(filename, std::fstream::in | std::fstream::binary); - - while (i > 0 && !finished) { - uint64_t size = block_size; - if (i < block_size) { - size = i; - finished = true; - } - reader.read(buf, size); - if (EVP_DigestUpdate(context, buf, size) != 1) { - handleEvpError(); - } - i -= block_size; - } - if (EVP_DigestFinal_ex(context, digest, &digest_size) != 1) { - handleEvpError(); - } - EVP_MD_CTX_free(context); - } - - std::string get_string() { - std::stringstream result; - char buf[3]; - result << "0x"; - for (uint64_t i = 0; i < digest_size; i++) { - // std::hex was misbehaving, just use C - snprintf(buf, 3, "%02x", digest[i]); - result << std::string(buf); - } - return result.str(); - } - - protected: - void handleEvpError() { - const size_t buflen = 100; - char buf[buflen]; - unsigned long errorno; - std::stringstream sstream; - - sstream << "FileSha Sha Errors:" << std::endl; - while ((errorno = ERR_get_error()) != 0) { - ERR_error_string_n(errorno, buf, buflen); - sstream << buf << std::endl; - } - throw sstream.str(); - } - - EVP_MD_CTX* context; - const uint64_t block_size; - int64_t fsize; - unsigned char digest[FILESHA_DIGEST_SIZE]; - unsigned int digest_size; -}; - -class OperationsTest : public OsfTestWithDataAndFiles {}; - -TEST_F(OperationsTest, GetOsfDumpInfo) { - std::string osf_info_str = dump_metadata( - path_concat(test_data_dir(), "osfs/OS-1-128_v2.3.0_1024x10_lb_n3.osf"), - true); - - Json::Value osf_info_obj{}; - - EXPECT_TRUE(parse_json(osf_info_str, osf_info_obj)); - - ASSERT_TRUE(osf_info_obj.isMember("header")); - EXPECT_TRUE(osf_info_obj["header"].isMember("status")); - EXPECT_TRUE(osf_info_obj["header"].isMember("version")); - EXPECT_TRUE(osf_info_obj["header"].isMember("size")); - EXPECT_TRUE(osf_info_obj["header"].isMember("metadata_offset")); - EXPECT_TRUE(osf_info_obj["header"].isMember("chunks_offset")); - - ASSERT_TRUE(osf_info_obj.isMember("metadata")); - EXPECT_TRUE(osf_info_obj["metadata"].isMember("id")); - EXPECT_EQ("from_pcap pythonic", osf_info_obj["metadata"]["id"].asString()); - EXPECT_TRUE(osf_info_obj["metadata"].isMember("start_ts")); - EXPECT_TRUE(osf_info_obj["metadata"].isMember("end_ts")); - EXPECT_TRUE(osf_info_obj["metadata"].isMember("entries")); - EXPECT_EQ(3, osf_info_obj["metadata"]["entries"].size()); -} - -TEST_F(OperationsTest, ParseAndPrintSmoke) { - parse_and_print( - path_concat(test_data_dir(), "osfs/OS-1-128_v2.3.0_1024x10_lb_n3.osf")); -} - -TEST_F(OperationsTest, FileShaTest) { - std::fstream test_file_out; - std::string temp_dir; - EXPECT_TRUE(make_tmp_dir(temp_dir)); - std::string temp_file = path_concat(temp_dir, "test_file"); - test_file_out.open(temp_file, std::fstream::out | std::fstream::trunc); - test_file_out << "Testing here for hashing" << std::endl; - test_file_out.close(); - auto sha = FileSha(temp_file); - EXPECT_EQ( - sha.get_string(), - "0x568c47f13b8a96ab5027037c0a44450fd493e91ba92a95bd1f81e23604d8dd99e687" - "6d5bbdf3d5b05ec7b9d03e84fd678690e57a1ecbc40863637deab9a35253"); - - unlink_path(temp_file); - remove_dir(temp_dir); -} - -TEST_F(OperationsTest, BackupMetadataTest) { - std::string osf_file_path = - path_concat(test_data_dir(), "osfs/OS-1-128_v2.3.0_1024x10_lb_n3.osf"); - std::string temp_dir; - EXPECT_TRUE(make_tmp_dir(temp_dir)); - try { - std::string temp_file = path_concat(temp_dir, "temp.osf"); - EXPECT_EQ(append_binary_file(temp_file, osf_file_path), - file_size(osf_file_path)); - auto size1 = file_size(temp_file); - auto sha1 = FileSha(temp_file).get_string(); - std::string temp_backup = path_concat(temp_dir, "temp_backup"); - auto size2 = backup_osf_file_metablob(temp_file, temp_backup); - truncate_file(temp_file, size1 - size2); - auto sha2 = FileSha(temp_file).get_string(); - std::fstream bad_append_out; - bad_append_out.open(temp_file, std::fstream::out | std::fstream::app); - bad_append_out << "Testing here for hashing" << std::endl; - bad_append_out.close(); - auto sha3 = FileSha(temp_file).get_string(); - auto size3 = restore_osf_file_metablob(temp_file, temp_backup); - auto sha4 = FileSha(temp_file).get_string(); - - EXPECT_NE(size1, size2); - EXPECT_EQ(size1, size3); - EXPECT_EQ(sha1, sha4); - EXPECT_NE(sha1, sha2); - EXPECT_NE(sha1, sha3); - EXPECT_NE(sha2, sha3); - unlink_path(temp_file); - unlink_path(temp_backup); - } catch (...) { - remove_dir(temp_dir); - throw; - } - remove_dir(temp_dir); -} - -bool _parse_json(const std::string& json, Json::Value& root) { - Json::CharReaderBuilder build; - JSONCPP_STRING error; - const std::unique_ptr read(build.newCharReader()); - return read->parse(json.c_str(), (json.c_str() + json.length()), &root, - &error); -} - -ouster::sensor::sensor_info _gen_new_metadata(int start_number) { - ouster::sensor::sensor_info new_metadata; - new_metadata.name = "Foobar"; - new_metadata.sn = "DEADBEEF"; - new_metadata.fw_rev = "sqrt(-1) friends"; - new_metadata.mode = ouster::sensor::MODE_512x10; - new_metadata.prod_line = "LEEROY JENKINS"; - - new_metadata.format.pixels_per_column = 5; - new_metadata.format.columns_per_packet = 2 + start_number; - new_metadata.format.columns_per_frame = 3 + start_number; - new_metadata.format.pixel_shift_by_row = { - 4 + start_number, 5 + start_number, 6 + start_number, 7 + start_number, - 8 + start_number}; - new_metadata.format.column_window = {9 + start_number, 10 + start_number}; - new_metadata.format.udp_profile_lidar = - ouster::sensor::PROFILE_RNG15_RFL8_NIR8; - new_metadata.format.udp_profile_imu = ouster::sensor::PROFILE_IMU_LEGACY; - new_metadata.format.fps = 11 + start_number; - new_metadata.beam_azimuth_angles = { - 12. + (double)start_number, 13. + (double)start_number, - 14. + (double)start_number, 15. + (double)start_number, - 16. + (double)start_number}; - new_metadata.beam_altitude_angles = { - 17. + (double)start_number, 18. + (double)start_number, - 19. + (double)start_number, 20. + (double)start_number, - 21. + (double)start_number}; - new_metadata.lidar_origin_to_beam_origin_mm = 22 + start_number; - - new_metadata.init_id = 23 + start_number; - new_metadata.udp_port_lidar = 24 + start_number; - new_metadata.udp_port_imu = 25 + start_number; - - new_metadata.build_date = "Made in SAN FRANCISCO"; - new_metadata.image_rev = "IDK, ask someone else"; - new_metadata.prod_pn = "import random; print(random.random())"; - new_metadata.status = "Not just good but great"; - - return new_metadata; -} - -void _verify_empty_metadata(Json::Value& test_root, int entry_count = 0) { - EXPECT_EQ(test_root["metadata"]["chunks"].size(), 0); - EXPECT_EQ(test_root["metadata"]["entries"].size(), entry_count); - EXPECT_EQ(test_root["metadata"]["end_ts"], 0); - EXPECT_EQ(test_root["metadata"]["start_ts"], 0); - EXPECT_EQ(test_root["metadata"]["id"], ""); -} - -void _write_init_metadata(std::string& temp_file, uint64_t header_size, - MetadataStore meta_store_ = {}) { - // Copied and modified from writer.cpp under osf/src - flatbuffers::FlatBufferBuilder metadata_fbb = - flatbuffers::FlatBufferBuilder(32768); - - std::vector chunks_{}; - - std::vector> entries = - meta_store_.make_entries(metadata_fbb); - char id[4] = {0}; - auto metadata = ouster::osf::gen::CreateMetadataDirect( - metadata_fbb, id, 0, 0, &chunks_, &entries); - - metadata_fbb.FinishSizePrefixed(metadata, - ouster::osf::gen::MetadataIdentifier()); - - const uint8_t* buf = metadata_fbb.GetBufferPointer(); - uint32_t metadata_size = metadata_fbb.GetSize(); - - uint64_t metadata_offset = header_size; - uint64_t metadata_saved_size = - buffer_to_file(buf, metadata_size, temp_file, true); - EXPECT_TRUE(metadata_saved_size && - metadata_saved_size == metadata_size + CRC_BYTES_SIZE); - EXPECT_TRUE(finish_osf_file(temp_file, metadata_offset, - metadata_saved_size) == header_size); -} - -TEST_F(OperationsTest, MetadataRewriteTestSimple) { - std::string temp_dir; - EXPECT_TRUE(make_tmp_dir(temp_dir)); - std::string temp_file = path_concat(temp_dir, "temp.osf"); - uint64_t header_size = start_osf_file(temp_file); - - _write_init_metadata(temp_file, header_size); - - std::string metadata_json = dump_metadata(temp_file, true); - Json::Value test_root{}; - EXPECT_TRUE(_parse_json(metadata_json, test_root)); - - _verify_empty_metadata(test_root); - - ouster::sensor::sensor_info new_metadata = _gen_new_metadata(100); - - osf_file_modify_metadata(temp_file, {new_metadata}); - std::string output_metadata_json = dump_metadata(temp_file, true); - Json::Value output_root{}; - EXPECT_TRUE(_parse_json(output_metadata_json, output_root)); - EXPECT_NE(test_root, output_root); - - Json::Value new_root{}; - EXPECT_TRUE(_parse_json(new_metadata.updated_metadata_string(), new_root)); - - EXPECT_EQ(new_root, - output_root["metadata"]["entries"][0]["buffer"]["sensor_info"]); - unlink_path(temp_file); -} - -TEST_F(OperationsTest, MetadataRewriteTestMulti) { - std::string temp_dir; - EXPECT_TRUE(make_tmp_dir(temp_dir)); - std::string temp_file = path_concat(temp_dir, "temp.osf"); - uint64_t header_size = start_osf_file(temp_file); - - _write_init_metadata(temp_file, header_size); - - std::string metadata_json = dump_metadata(temp_file, true); - Json::Value test_root{}; - EXPECT_TRUE(_parse_json(metadata_json, test_root)); - - _verify_empty_metadata(test_root); - - ouster::sensor::sensor_info new_metadata = _gen_new_metadata(100); - ouster::sensor::sensor_info new_metadata2 = _gen_new_metadata(200); - - osf_file_modify_metadata(temp_file, {new_metadata, new_metadata2}); - std::string output_metadata_json = dump_metadata(temp_file, true); - Json::Value output_root{}; - EXPECT_TRUE(_parse_json(output_metadata_json, output_root)); - EXPECT_NE(test_root, output_root); - - Json::Value new_root{}; - EXPECT_TRUE(_parse_json(new_metadata.updated_metadata_string(), new_root)); - Json::Value new_root2{}; - auto temp_string = new_metadata2.updated_metadata_string(); - EXPECT_TRUE(_parse_json(temp_string, new_root2)); - - EXPECT_EQ(new_root, - output_root["metadata"]["entries"][0]["buffer"]["sensor_info"]); - EXPECT_EQ(new_root2, - output_root["metadata"]["entries"][1]["buffer"]["sensor_info"]); - unlink_path(temp_file); -} - -TEST_F(OperationsTest, MetadataRewriteTestPreExisting) { - std::string temp_dir; - EXPECT_TRUE(make_tmp_dir(temp_dir)); - std::string temp_file = path_concat(temp_dir, "temp.osf"); - uint64_t header_size = start_osf_file(temp_file); - MetadataStore meta_store_ = {}; - LidarScanStreamMeta pre_existing_data(12345678, {}); - meta_store_.add(pre_existing_data); - _write_init_metadata(temp_file, header_size, meta_store_); - - std::string metadata_json = dump_metadata(temp_file, true); - Json::Value test_root{}; - EXPECT_TRUE(_parse_json(metadata_json, test_root)); - - _verify_empty_metadata(test_root, 1); - - EXPECT_EQ(test_root["metadata"]["entries"][0]["type"], - "ouster/v1/os_sensor/LidarScanStream"); - EXPECT_EQ(test_root["metadata"]["entries"][0]["buffer"], - "LidarScanStreamMeta: sensor_id = 12345678, field_types = {}"); - - ouster::sensor::sensor_info new_metadata = _gen_new_metadata(100); - - osf_file_modify_metadata(temp_file, {new_metadata}); - std::string output_metadata_json = dump_metadata(temp_file, true); - Json::Value output_root{}; - EXPECT_TRUE(_parse_json(output_metadata_json, output_root)); - EXPECT_NE(test_root, output_root); - - Json::Value new_root{}; - EXPECT_TRUE(_parse_json(new_metadata.updated_metadata_string(), new_root)); - - EXPECT_EQ(output_root["metadata"]["entries"][0]["buffer"], - "LidarScanStreamMeta: sensor_id = 12345678, field_types = {}"); - EXPECT_EQ(new_root, - output_root["metadata"]["entries"][1]["buffer"]["sensor_info"]); - EXPECT_EQ(output_root["metadata"]["entries"].size(), 2); - unlink_path(temp_file); -} - -} // namespace -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/osf_test.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/osf_test.h deleted file mode 100644 index ca200d2a..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/osf_test.h +++ /dev/null @@ -1,59 +0,0 @@ -/** - * Copyright(c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#pragma once - -#include - -#include "common.h" - -namespace ouster { -namespace osf { - -// Base class for all osf util tests -class OsfTest : public ::testing::Test {}; - -// Test fixture to get and check test data dir -// use it for tests that needed test data -class OsfTestWithData : public OsfTest { - protected: - virtual void SetUp() { - if (!get_test_data_dir(test_data_dir_)) { - FAIL() << "Can't get DATA_DIR"; - return; - } - } - - std::string test_data_dir() { return test_data_dir_; } - - private: - std::string test_data_dir_; -}; - -class OsfTestWithDataAndFiles : public osf::OsfTestWithData { - public: - static std::string output_dir; - static std::vector files; - std::string tmp_file(const std::string& basename) { - std::string res = path_concat(output_dir, basename); - // TODO[pb]: Switch to map? to avoid overlaps/double delete - files.push_back(res); - return res; - } - static void SetUpTestCase() { - if (!make_tmp_dir(output_dir)) FAIL(); - } - - // clean up temp files - static void TearDownTestCase() { - for (const auto& path : files) unlink_path(path); - remove_dir(output_dir); - } -}; -std::string OsfTestWithDataAndFiles::output_dir = {}; -std::vector OsfTestWithDataAndFiles::files = {}; - -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/pcap_source_test.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/pcap_source_test.cpp deleted file mode 100644 index f869beb4..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/pcap_source_test.cpp +++ /dev/null @@ -1,45 +0,0 @@ -/** - * Copyright(c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#include "ouster/osf/pcap_source.h" - -#include - -#include "osf_test.h" -#include "ouster/osf/pcap_source.h" -#include "ouster/types.h" - -namespace ouster { -namespace osf { -namespace { - -class OsfPcapSourceTest : public OsfTestWithData {}; - -// TODO[pb]: Remove this test and PcapRawSource since it's not matching of what -// we have in the Python -TEST_F(OsfPcapSourceTest, ReadLidarScansAndImus) { - std::string pcap_file = path_concat( - test_data_dir(), "pcaps/OS-1-128_v2.3.0_1024x10_lb_n3.pcap"); - std::string meta_file = - path_concat(test_data_dir(), "pcaps/OS-1-128_v2.3.0_1024x10.json"); - - PcapRawSource pcap_source{pcap_file}; - - auto info = sensor::metadata_from_json(meta_file); - - int ls_cnt = 0; - - pcap_source.addLidarDataHandler( - 7502, info, [&ls_cnt](const osf::ts_t, const LidarScan&) { ls_cnt++; }); - - pcap_source.runAll(); - - EXPECT_EQ(2, ls_cnt); -} - -} // namespace - -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/png_tools_test.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/png_tools_test.cpp deleted file mode 100644 index 30b9b442..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/png_tools_test.cpp +++ /dev/null @@ -1,336 +0,0 @@ -/** - * Copyright(c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#include "png_tools.h" - -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "common.h" -#include "osf_test.h" -#include "ouster/lidar_scan.h" -#include "ouster/osf/basics.h" -#include "ouster/types.h" - -namespace ouster { -namespace osf { - -// Internals to test -void png_osf_flush_data(png_structp); -void png_osf_error(png_structp png_ptr, png_const_charp msg); - -namespace { - -class OsfPngToolsTest : public OsfTestWithDataAndFiles {}; - -using ouster::sensor::lidar_mode; -using ouster::sensor::sensor_info; - -size_t field_size(LidarScan& ls, sensor::ChanField f) { - switch (ls.field_type(f)) { - case sensor::ChanFieldType::UINT8: - return ls.field(f).size(); - break; - case sensor::ChanFieldType::UINT16: - return ls.field(f).size(); - break; - case sensor::ChanFieldType::UINT32: - return ls.field(f).size(); - break; - case sensor::ChanFieldType::UINT64: - return ls.field(f).size(); - break; - default: - return 0; - break; - } -} - -// Check that we can make lidar scan and have fields with expected value inside -TEST_F(OsfPngToolsTest, MakesLidarScan) { - const sensor_info si = sensor::metadata_from_json( - path_concat(test_data_dir(), "pcaps/OS-1-128_v2.3.0_1024x10.json")); - - LidarScan ls = get_random_lidar_scan(si); - - const auto n = si.format.columns_per_frame * si.format.pixels_per_column; - - EXPECT_EQ(ls.w, si.format.columns_per_frame); - EXPECT_EQ(ls.h, si.format.pixels_per_column); - for (const auto& f : ls) { - EXPECT_EQ(field_size(ls, f.first), n); - } - EXPECT_EQ(ls.status().size(), si.format.columns_per_frame); -} - -#define ENCODE_IMAGE_TEST(TEST_NAME, ENCODE_FUNC, DECODE_FUNC) \ - template \ - struct TEST_NAME { \ - template \ - bool to(const LidarScan& ls, const std::vector& px_offset, \ - Ti mask_bits = 0) { \ - ScanChannelData encoded_channel; \ - img_t key_orig(ls.h, ls.w); \ - key_orig = key_orig.unaryExpr([=](Ti) { \ - double sr = static_cast(std::rand()) / RAND_MAX; \ - return static_cast( \ - sr * static_cast(std::numeric_limits::max())); \ - }); \ - if (mask_bits && sizeof(Ti) * 8 > mask_bits) { \ - key_orig = key_orig.unaryExpr([=](Ti a) { \ - return static_cast(a & ((1LL << mask_bits) - 1)); \ - }); \ - } \ - bool res_enc = \ - ENCODE_FUNC(encoded_channel, key_orig, px_offset); \ - EXPECT_FALSE(res_enc); \ - std::cout << #ENCODE_FUNC \ - << ": encoded bytes = " << encoded_channel.size() \ - << " ================= " << std::endl; \ - EXPECT_TRUE(!encoded_channel.empty()); \ - img_t decoded_img{ls.h, ls.w}; \ - bool res_dec = \ - DECODE_FUNC(decoded_img, encoded_channel, px_offset); \ - EXPECT_FALSE(res_dec); \ - bool round_trip = (key_orig.template cast() == \ - decoded_img.template cast()) \ - .all(); \ - auto round_trip_cnt = (key_orig.template cast() == \ - decoded_img.template cast()) \ - .count(); \ - std::cout << "cnt = " << round_trip_cnt << std::endl; \ - return round_trip; \ - } \ - }; - -ENCODE_IMAGE_TEST(test8bitImageCoders, encode8bitImage, decode8bitImage) -ENCODE_IMAGE_TEST(test16bitImageCoders, encode16bitImage, decode16bitImage) -ENCODE_IMAGE_TEST(test24bitImageCoders, encode24bitImage, decode24bitImage) -ENCODE_IMAGE_TEST(test32bitImageCoders, encode32bitImage, decode32bitImage) -ENCODE_IMAGE_TEST(test64bitImageCoders, encode64bitImage, decode64bitImage) - -// Check encodeXXbitImage functions on RANGE fields of LidarScan -// converted to various img_t of different unsigned int sizes. -TEST_F(OsfPngToolsTest, ImageCoders) { - const sensor_info si = sensor::metadata_from_json( - path_concat(test_data_dir(), "pcaps/OS-1-128_v2.3.0_1024x10.json")); - LidarScan ls = get_random_lidar_scan(si); - auto px_offset = si.format.pixel_shift_by_row; - - // ======== 8bit ========== - - EXPECT_TRUE(test8bitImageCoders().to(ls, px_offset, 8)); - EXPECT_TRUE(test8bitImageCoders().to(ls, px_offset, 8)); - EXPECT_TRUE(test8bitImageCoders().to(ls, px_offset, 8)); - EXPECT_TRUE(test8bitImageCoders().to(ls, px_offset, 8)); - - EXPECT_TRUE(test8bitImageCoders().to(ls, px_offset, 8)); - EXPECT_TRUE(test8bitImageCoders().to(ls, px_offset, 8)); - EXPECT_TRUE(test8bitImageCoders().to(ls, px_offset, 8)); - EXPECT_TRUE(test8bitImageCoders().to(ls, px_offset, 8)); - - EXPECT_TRUE(test8bitImageCoders().to(ls, px_offset, 8)); - EXPECT_TRUE(test8bitImageCoders().to(ls, px_offset, 8)); - EXPECT_TRUE(test8bitImageCoders().to(ls, px_offset, 8)); - EXPECT_TRUE(test8bitImageCoders().to(ls, px_offset, 8)); - - EXPECT_TRUE(test8bitImageCoders().to(ls, px_offset, 8)); - EXPECT_TRUE(test8bitImageCoders().to(ls, px_offset, 8)); - EXPECT_TRUE(test8bitImageCoders().to(ls, px_offset, 8)); - EXPECT_TRUE(test8bitImageCoders().to(ls, px_offset, 8)); - - // ======== 16bit ====== - // clang-format off - EXPECT_TRUE(test16bitImageCoders().to(ls, px_offset, 16)); - EXPECT_TRUE(test16bitImageCoders().to(ls, px_offset, 16)); - EXPECT_TRUE(test16bitImageCoders().to(ls, px_offset, 16)); - EXPECT_TRUE(test16bitImageCoders().to(ls, px_offset, 16)); - - EXPECT_FALSE(test16bitImageCoders().to(ls, px_offset, 16)); - EXPECT_TRUE(test16bitImageCoders().to(ls, px_offset, 16)); - EXPECT_TRUE(test16bitImageCoders().to(ls, px_offset, 16)); - EXPECT_TRUE(test16bitImageCoders().to(ls, px_offset, 16)); - - EXPECT_FALSE(test16bitImageCoders().to(ls, px_offset, 16)); - EXPECT_TRUE(test16bitImageCoders().to(ls, px_offset, 16)); - EXPECT_TRUE(test16bitImageCoders().to(ls, px_offset, 16)); - EXPECT_TRUE(test16bitImageCoders().to(ls, px_offset, 16)); - - EXPECT_FALSE(test16bitImageCoders().to(ls, px_offset, 16)); - EXPECT_TRUE(test16bitImageCoders().to(ls, px_offset, 16)); - EXPECT_TRUE(test16bitImageCoders().to(ls, px_offset, 16)); - EXPECT_TRUE(test16bitImageCoders().to(ls, px_offset, 16)); - // clang-format on - - // ======== 24bit ====== - - EXPECT_TRUE(test24bitImageCoders().to(ls, px_offset, 24)); - EXPECT_TRUE( - test24bitImageCoders().to(ls, px_offset, 24)); - EXPECT_TRUE( - test24bitImageCoders().to(ls, px_offset, 24)); - EXPECT_TRUE( - test24bitImageCoders().to(ls, px_offset, 24)); - - EXPECT_FALSE( - test24bitImageCoders().to(ls, px_offset, 24)); - EXPECT_TRUE( - test24bitImageCoders().to(ls, px_offset, 24)); - EXPECT_TRUE( - test24bitImageCoders().to(ls, px_offset, 24)); - EXPECT_TRUE( - test24bitImageCoders().to(ls, px_offset, 24)); - - EXPECT_FALSE( - test24bitImageCoders().to(ls, px_offset, 24)); - EXPECT_FALSE( - test24bitImageCoders().to(ls, px_offset, 24)); - EXPECT_TRUE( - test24bitImageCoders().to(ls, px_offset, 24)); - EXPECT_TRUE( - test24bitImageCoders().to(ls, px_offset, 24)); - - EXPECT_FALSE( - test24bitImageCoders().to(ls, px_offset, 24)); - EXPECT_FALSE( - test24bitImageCoders().to(ls, px_offset, 24)); - EXPECT_TRUE( - test24bitImageCoders().to(ls, px_offset, 24)); - EXPECT_TRUE( - test24bitImageCoders().to(ls, px_offset, 24)); - - // ======== 32bit ====== - - EXPECT_TRUE(test32bitImageCoders().to(ls, px_offset, 32)); - EXPECT_TRUE( - test32bitImageCoders().to(ls, px_offset, 32)); - EXPECT_TRUE( - test32bitImageCoders().to(ls, px_offset, 32)); - EXPECT_TRUE( - test32bitImageCoders().to(ls, px_offset, 32)); - - EXPECT_FALSE( - test32bitImageCoders().to(ls, px_offset, 32)); - EXPECT_TRUE( - test32bitImageCoders().to(ls, px_offset, 32)); - EXPECT_TRUE( - test32bitImageCoders().to(ls, px_offset, 32)); - EXPECT_TRUE( - test32bitImageCoders().to(ls, px_offset, 32)); - - EXPECT_FALSE( - test32bitImageCoders().to(ls, px_offset, 32)); - EXPECT_FALSE( - test32bitImageCoders().to(ls, px_offset, 32)); - EXPECT_TRUE( - test32bitImageCoders().to(ls, px_offset, 32)); - EXPECT_TRUE( - test32bitImageCoders().to(ls, px_offset, 32)); - - EXPECT_FALSE( - test32bitImageCoders().to(ls, px_offset, 32)); - EXPECT_FALSE( - test32bitImageCoders().to(ls, px_offset, 32)); - EXPECT_TRUE( - test32bitImageCoders().to(ls, px_offset, 32)); - EXPECT_TRUE( - test32bitImageCoders().to(ls, px_offset, 32)); - - // ======== 64bit ====== - - EXPECT_TRUE(test64bitImageCoders().to(ls, px_offset, 64)); - EXPECT_TRUE( - test64bitImageCoders().to(ls, px_offset, 64)); - EXPECT_TRUE( - test64bitImageCoders().to(ls, px_offset, 64)); - EXPECT_TRUE( - test64bitImageCoders().to(ls, px_offset, 64)); - - EXPECT_FALSE( - test64bitImageCoders().to(ls, px_offset, 64)); - EXPECT_TRUE( - test64bitImageCoders().to(ls, px_offset, 64)); - EXPECT_TRUE( - test64bitImageCoders().to(ls, px_offset, 64)); - EXPECT_TRUE( - test64bitImageCoders().to(ls, px_offset, 64)); - - EXPECT_FALSE( - test64bitImageCoders().to(ls, px_offset, 64)); - EXPECT_FALSE( - test64bitImageCoders().to(ls, px_offset, 64)); - EXPECT_TRUE( - test64bitImageCoders().to(ls, px_offset, 64)); - EXPECT_TRUE( - test64bitImageCoders().to(ls, px_offset, 64)); - - EXPECT_FALSE( - test64bitImageCoders().to(ls, px_offset, 64)); - EXPECT_FALSE( - test64bitImageCoders().to(ls, px_offset, 64)); - EXPECT_FALSE( - test64bitImageCoders().to(ls, px_offset, 64)); - EXPECT_TRUE( - test64bitImageCoders().to(ls, px_offset, 64)); -} - -TEST_F(OsfPngToolsTest, InternalsTest) { - // This is unused but is still required, test calling it - // Not expecting any returns - png_structp foo = - png_create_read_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL); - png_osf_flush_data(foo); - - bool error_caught = false; - std::stringstream output_stream; - std::streambuf* old_output_stream = std::cout.rdbuf(); - std::cout.rdbuf(output_stream.rdbuf()); - if (setjmp(png_jmpbuf(foo))) { - error_caught = true; - } else { - png_osf_error( - foo, - "Also Checkout Porcupine Tree - Arriving Somewhere But Not Here"); - } - std::cout.rdbuf(old_output_stream); - EXPECT_TRUE(error_caught); - EXPECT_EQ(output_stream.str(), - "ERROR libpng osf: Also Checkout Porcupine Tree" - " - Arriving Somewhere But Not Here\n"); -} - -#ifndef OUSTER_OSF_NO_THREADING -TEST_F(OsfPngToolsTest, scanDecodeFields) { - // it should propagate the exception - // if destagger throws std::invalid_argument - int w = 32; - int h = 32; - auto scan = ouster::LidarScan(w, h); - LidarScanFieldTypes field_types(scan.begin(), scan.end()); - std::vector shift_by_row; - EXPECT_THROW( - { - try { - scanEncodeFields(scan, shift_by_row, field_types); - } catch (const std::invalid_argument& e) { - ASSERT_STREQ(e.what(), - "image height does not match shifts size"); - throw; - } - }, - std::invalid_argument); -} -#endif - -} // namespace -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/reader_test.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/reader_test.cpp deleted file mode 100644 index 1dbe5428..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/reader_test.cpp +++ /dev/null @@ -1,328 +0,0 @@ -/** - * Copyright(c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#include "ouster/osf/reader.h" - -#include - -#include "common.h" -#include "osf_test.h" -#include "ouster/osf/meta_lidar_sensor.h" -#include "ouster/osf/meta_streaming_info.h" -#include "ouster/osf/stream_lidar_scan.h" - -namespace ouster { -namespace osf { -namespace { - -class ReaderTest : public osf::OsfTestWithData {}; - -TEST_F(ReaderTest, Basics) { - OsfFile osf_file( - path_concat(test_data_dir(), "osfs/OS-1-128_v2.3.0_1024x10_lb_n3.osf")); - - Reader reader(osf_file); - - EXPECT_EQ("from_pcap pythonic", reader.metadata_id()); - EXPECT_EQ(991587364520LL, reader.start_ts().count()); - EXPECT_EQ(991787323080LL, reader.end_ts().count()); - - // Get first sensor (it's the first by metadata_id) (i.e. first added) - auto sensor = reader.meta_store().get(); - EXPECT_TRUE(sensor); - EXPECT_EQ( - sensor->to_string(), - "{\n \"sensor_info\": \n {\n \"base_pn\": \"\",\n \"base_sn\": " - "\"\",\n \"beam_altitude_angles\": \n [\n 20.95,\n " - "20.67,\n 20.36,\n 20.03,\n 19.73,\n 19.41,\n " - "19.11,\n 18.76,\n 18.47,\n 18.14,\n 17.82,\n " - "17.5,\n 17.19,\n 16.86,\n 16.53,\n 16.2,\n " - "15.89,\n 15.56,\n 15.23,\n 14.9,\n 14.57,\n " - "14.23,\n 13.9,\n 13.57,\n 13.25,\n 12.91,\n " - "12.57,\n 12.22,\n 11.9,\n 11.55,\n 11.2,\n " - "10.87,\n 10.54,\n 10.18,\n 9.84,\n 9.51,\n " - "9.15,\n 8.81,\n 8.47,\n 8.11,\n 7.78,\n " - "7.43,\n 7.08,\n 6.74,\n 6.39,\n 6.04,\n " - "5.7,\n 5.34,\n 4.98,\n 4.64,\n 4.29,\n " - "3.93,\n 3.58,\n 3.24,\n 2.88,\n 2.53,\n " - "2.17,\n 1.82,\n 1.47,\n 1.12,\n 0.78,\n " - "0.41,\n 0.07,\n -0.28,\n -0.64,\n -0.99,\n " - "-1.35,\n -1.7,\n -2.07,\n -2.4,\n -2.75,\n " - "-3.11,\n -3.46,\n -3.81,\n -4.15,\n -4.5,\n " - "-4.86,\n -5.22,\n -5.57,\n -5.9,\n -6.27,\n " - "-6.61,\n -6.97,\n -7.3,\n -7.67,\n -8.01,\n " - "-8.35,\n -8.69,\n -9.05,\n -9.38,\n -9.71,\n " - "-10.07,\n -10.42,\n -10.76,\n -11.09,\n -11.43,\n " - " -11.78,\n -12.12,\n -12.46,\n -12.78,\n " - "-13.15,\n -13.46,\n -13.8,\n -14.12,\n -14.48,\n " - " -14.79,\n -15.11,\n -15.46,\n -15.79,\n " - "-16.12,\n -16.45,\n -16.76,\n -17.11,\n -17.44,\n " - " -17.74,\n -18.06,\n -18.39,\n -18.72,\n " - "-19.02,\n -19.32,\n -19.67,\n -19.99,\n -20.27,\n " - " -20.57,\n -20.92,\n -21.22,\n -21.54,\n " - "-21.82\n ],\n \"beam_azimuth_angles\": \n [\n 4.21,\n " - " 1.41,\n -1.4,\n -4.22,\n 4.22,\n 1.41,\n " - "-1.4,\n -4.23,\n 4.21,\n 1.4,\n -1.42,\n " - "-4.2,\n 4.22,\n 1.41,\n -1.4,\n -4.23,\n " - "4.21,\n 1.41,\n -1.41,\n -4.21,\n 4.22,\n " - "1.4,\n -1.41,\n -4.2,\n 4.22,\n 1.42,\n " - "-1.4,\n -4.2,\n 4.22,\n 1.41,\n -1.42,\n " - "-4.21,\n 4.22,\n 1.41,\n -1.4,\n -4.21,\n " - "4.2,\n 1.4,\n -1.4,\n -4.22,\n 4.21,\n " - "1.41,\n -1.41,\n -4.21,\n 4.22,\n 1.41,\n " - "-1.4,\n -4.21,\n 4.21,\n 1.41,\n -1.4,\n " - "-4.21,\n 4.2,\n 1.41,\n -1.4,\n -4.21,\n " - "4.2,\n 1.4,\n -1.41,\n -4.21,\n 4.22,\n " - "1.4,\n -1.4,\n -4.21,\n 4.22,\n 1.42,\n " - "-1.4,\n -4.2,\n 4.2,\n 1.42,\n -1.4,\n " - "-4.22,\n 4.22,\n 1.41,\n -1.4,\n -4.2,\n " - "4.23,\n 1.41,\n -1.4,\n -4.2,\n 4.21,\n " - "1.41,\n -1.4,\n -4.21,\n 4.21,\n 1.41,\n " - "-1.4,\n -4.21,\n 4.22,\n 1.41,\n -1.39,\n " - "-4.21,\n 4.23,\n 1.41,\n -1.39,\n -4.22,\n " - "4.23,\n 1.4,\n -1.4,\n -4.2,\n 4.21,\n " - "1.41,\n -1.41,\n -4.2,\n 4.22,\n 1.42,\n " - "-1.39,\n -4.22,\n 4.24,\n 1.41,\n -1.41,\n " - "-4.22,\n 4.23,\n 1.41,\n -1.39,\n -4.21,\n " - "4.23,\n 1.41,\n -1.39,\n -4.2,\n 4.23,\n " - "1.4,\n -1.39,\n -4.2,\n 4.22,\n 1.42,\n " - "-1.39,\n -4.2\n ],\n \"build_date\": " - "\"2022-04-14T21:11:47Z\",\n \"build_rev\": \"v2.3.0\",\n " - "\"client_version\": \"ouster_client 0.3.0\",\n \"data_format\": \n " - " {\n \"column_window\": \n [\n 0,\n 1023\n " - " ],\n \"columns_per_frame\": 1024,\n " - "\"columns_per_packet\": 16,\n \"pixel_shift_by_row\": \n " - "[\n 24,\n 16,\n 8,\n 0,\n 24,\n " - " 16,\n 8,\n 0,\n 24,\n 16,\n " - "8,\n 0,\n 24,\n 16,\n 8,\n 0,\n " - " 24,\n 16,\n 8,\n 0,\n 24,\n " - "16,\n 8,\n 0,\n 24,\n 16,\n 8,\n " - " 0,\n 24,\n 16,\n 8,\n 0,\n " - "24,\n 16,\n 8,\n 0,\n 24,\n 16,\n " - " 8,\n 0,\n 24,\n 16,\n 8,\n " - "0,\n 24,\n 16,\n 8,\n 0,\n 24,\n " - " 16,\n 8,\n 0,\n 24,\n 16,\n " - "8,\n 0,\n 24,\n 16,\n 8,\n 0,\n " - " 24,\n 16,\n 8,\n 0,\n 24,\n " - "16,\n 8,\n 0,\n 24,\n 16,\n 8,\n " - " 0,\n 24,\n 16,\n 8,\n 0,\n " - "24,\n 16,\n 8,\n 0,\n 24,\n 16,\n " - " 8,\n 0,\n 24,\n 16,\n 8,\n " - "0,\n 24,\n 16,\n 8,\n 0,\n 24,\n " - " 16,\n 8,\n 0,\n 24,\n 16,\n " - "8,\n 0,\n 24,\n 16,\n 8,\n 0,\n " - " 24,\n 16,\n 8,\n 0,\n 24,\n " - "16,\n 8,\n 0,\n 24,\n 16,\n 8,\n " - " 0,\n 24,\n 16,\n 8,\n 0,\n " - "24,\n 16,\n 8,\n 0,\n 24,\n 16,\n " - " 8,\n 0\n ],\n \"pixels_per_column\": 128,\n " - " \"udp_profile_imu\": \"LEGACY\",\n \"udp_profile_lidar\": " - "\"RNG15_RFL8_NIR8\"\n },\n \"hostname\": \"\",\n " - "\"image_rev\": \"ousteros-image-prod-aries-v2.3.0+20220415163956\",\n " - " \"imu_to_sensor_transform\": \n [\n 1,\n 0,\n " - "0,\n 6.253,\n 0,\n 1,\n 0,\n -11.775,\n " - "0,\n 0,\n 1,\n 7.645,\n 0,\n 0,\n 0,\n " - " 1\n ],\n \"initialization_id\": 7109750,\n " - "\"json_calibration_version\": 4,\n \"lidar_mode\": \"1024x10\",\n " - " \"lidar_origin_to_beam_origin_mm\": 15.806,\n " - "\"lidar_to_sensor_transform\": \n [\n -1,\n 0,\n " - "0,\n 0,\n 0,\n -1,\n 0,\n 0,\n 0,\n " - " 0,\n 1,\n 36.18,\n 0,\n 0,\n 0,\n 1\n " - " ],\n \"prod_line\": \"OS-1-128\",\n \"prod_pn\": " - "\"840-103575-06\",\n \"prod_sn\": \"122201000998\",\n " - "\"proto_rev\": \"\",\n \"status\": \"RUNNING\",\n " - "\"udp_port_imu\": 7503,\n \"udp_port_lidar\": 7502\n }\n}"); - EXPECT_EQ(1, reader.meta_store().count()); - - EXPECT_EQ( - 3, std::distance(reader.messages().begin(), reader.messages().end())); - - const MetadataStore& meta_store = reader.meta_store(); - EXPECT_EQ(3, meta_store.size()); -} - -TEST_F(ReaderTest, ChunksReading) { - OsfFile osf_file( - path_concat(test_data_dir(), "osfs/OS-1-128_v2.3.0_1024x10_lb_n3.osf")); - - Reader reader(osf_file); - - auto chunks = reader.chunks(); - - EXPECT_EQ(chunks.to_string(), "ChunksRange: [ba = 0, ea = 1013976]"); - EXPECT_EQ(chunks.begin().to_string(), "ChunksIter: [ca = 0, ea = 1013976]"); - std::cout << chunks.begin()->end_ts().count() << std::endl; - EXPECT_EQ(chunks.begin()->start_ts(), ts_t(991587364520L)); - EXPECT_EQ(chunks.begin()->end_ts(), ts_t(991787323080L)); - EXPECT_EQ(chunks.begin()->to_string(), - "ChunkRef: [msgs_size = 3, state = (" - "{offset = 0, next_offset = 18446744073709551615," - " start_ts = 991587364520, end_ts = 991787323080," - " status = 1}), chunk_buf_ = nullptr]"); - EXPECT_EQ(((ChunkRef)*chunks.begin())[0].to_string(), - "MessageRef: [id = 2, ts = 991587364520, buffer = 0c" - " 2b 05 00 14 00 00 00 10 00 1c 00 04 00 08 00 0c 00" - " 10 00 14 00 18 00 10 00 00 00 34 38 00 00 24 38 00" - " 00 18 18 00 00 10 10 00 00 08 00 00 00 03 07 00 00" - " 00 04 00 00 01 00 00 00 01 00 00 00 01 00 00 00 01" - " 00 00 00 01 00 00 00 01 00 00 00 01 00 00 00 01 00" - " 00 00 01 00 00 00 01 00 00 00 01 00 00 00 ... and" - " 338604 more ...]"); - EXPECT_EQ(chunks.begin()->begin().to_string(), - "MessagesChunkIter: [chunk_ref = ChunkRef:" - " [msgs_size = 3, state = ({offset = 0," - " next_offset = 18446744073709551615," - " start_ts = 991587364520, end_ts = 991787323080," - " status = 1}), chunk_buf_ = nullptr], msg_idx = 0]"); - EXPECT_EQ(to_string(*chunks.begin()->state()), - "{offset = 0, next_offset = 18446744073709551615," - " start_ts = 991587364520, end_ts = 991787323080, status = 1}"); - EXPECT_EQ(to_string(*chunks.begin()->info()), - "{offset = 0, next_offset = 18446744073709551615," - " stream_id = 2, message_count = 3, message_start_idx = 0}"); - - EXPECT_EQ(1, std::distance(chunks.begin(), chunks.end())); - - auto first_chunk_it = chunks.begin(); - EXPECT_EQ(3, first_chunk_it->size()); - - EXPECT_EQ(3, std::distance(first_chunk_it->begin(), first_chunk_it->end())); - - auto msg_it = first_chunk_it->begin(); - auto msg0 = *msg_it; - auto msg1 = *(++msg_it); - EXPECT_NE(msg0, msg1); - - auto msg00 = *(--msg_it); - EXPECT_EQ(msg0, msg00); - EXPECT_EQ(msg0, *(--msg_it)); -} - -TEST_F(ReaderTest, ChunksPileBasics) { - ChunkState st{}; - EXPECT_EQ(st.status, ChunkValidity::UNKNOWN); - - ChunksPile cp{}; - cp.add(0, ts_t{1}, ts_t{2}); - cp.add(10, ts_t{10}, ts_t{20}); - EXPECT_FALSE(cp.get(2)); - EXPECT_TRUE(cp.get(0)); - EXPECT_EQ(2, cp.get(0)->end_ts.count()); - EXPECT_EQ(ChunkValidity::UNKNOWN, cp.get(0)->status); - - cp.add(12, ts_t{12}, ts_t{22}); - EXPECT_EQ(3, cp.size()); -} - -TEST_F(ReaderTest, MessagesReadingStreaming) { - OsfFile osf_file( - path_concat(test_data_dir(), "osfs/OS-1-128_v2.3.0_1024x10_lb_n3.osf")); - - Reader reader(osf_file); - - // Reader iterator reads as messages from chunk as they appears on a disk - int it_cnt = 0; - ts_t it_prev{0}; - bool it_ordered = true; - for (const auto msg : reader.messages()) { - it_ordered = it_ordered && (it_prev <= msg.ts()); - ++it_cnt; - it_prev = msg.ts(); - } - - EXPECT_EQ(3, it_cnt); - - // only for this test file, ordering by timestamp while reading by chunks - // order as they layout in the file is not guaranteed - EXPECT_TRUE(it_ordered); - - // StreammingReader reads StreamingLayout OSFs in timestamp order - int sit_cnt = 0; - ts_t sit_prev{0}; - bool sit_ordered = true; - EXPECT_EQ((*reader.messages().begin()).to_string(), - "MessageRef: [id = 2, ts = 991587364520, buffer =" - " 0c 2b 05 00 14 00 00 00 10 00 1c 00 04 00 08 00" - " 0c 00 10 00 14 00 18 00 10 00 00 00 34 38 00 00" - " 24 38 00 00 18 18 00 00 10 10 00 00 08 00 00 00" - " 03 07 00 00 00 04 00 00 01 00 00 00 01 00 00 00" - " 01 00 00 00 01 00 00 00 01 00 00 00 01 00 00 00" - " 01 00 00 00 01 00 00 00 01 00 00 00 01 00 00 00" - " 01 00 00 00 ... and 338604 more ...]"); - EXPECT_EQ(reader.messages().begin().to_string(), - "MessagesStreamingIter: [curr_ts = 991587364520," - " end_ts = 991787323081, curr_chunks_.size = 1," - " stream_ids_hash_ = 0, top = (ts = 991587364520, id = 2)]"); - - for (const auto msg : reader.messages()) { - sit_ordered = sit_ordered && (sit_prev <= msg.ts()); - ++sit_cnt; - sit_prev = msg.ts(); - } - EXPECT_EQ(3, sit_cnt); - EXPECT_TRUE(sit_ordered); - - EXPECT_EQ(reader.messages().to_string(), - "MessagesStreamingRange:" - " [start_ts = 991587364520, end_ts = 991787323080]"); - EXPECT_EQ( - 3, std::distance(reader.messages().begin(), reader.messages().end())); - - // Read time based range of messages from StreamingLayout - ts_t begin_ts{991587364520LL}; - ts_t end_ts{991587364520LL + 1 * 100'000'000LL}; // start_ts + 1 * 0.1s - for (const auto msg : reader.messages(begin_ts, end_ts)) { - EXPECT_TRUE(begin_ts <= msg.ts()); - EXPECT_TRUE(msg.ts() <= end_ts); - } - auto some_msgs = reader.messages(begin_ts, end_ts); - EXPECT_EQ(2, std::distance(some_msgs.begin(), some_msgs.end())); - - // Get any first LidarScan stream from OSF - auto lidar_scan_stream = reader.meta_store().get(); - EXPECT_TRUE(lidar_scan_stream); - - // Get a stream of LidarScan messages only - auto scan_msgs = reader.messages({lidar_scan_stream->id()}); - for (const auto msg : scan_msgs) { - EXPECT_EQ(lidar_scan_stream->id(), msg.id()); - } - EXPECT_EQ(3, std::distance(scan_msgs.begin(), scan_msgs.end())); - - // Get a stream of LidarScan messages only with start/end_ts params - auto scan_msgs_full = reader.messages({lidar_scan_stream->id()}, - reader.start_ts(), reader.end_ts()); - EXPECT_EQ(3, std::distance(scan_msgs_full.begin(), scan_msgs_full.end())); -} - -TEST_F(ReaderTest, MetadataFromBufferTest) { - OsfFile osf_file( - path_concat(test_data_dir(), "osfs/OS-1-128_v2.3.0_1024x10_lb_n3.osf")); - - Reader reader(osf_file); - - auto sensor = reader.meta_store().entries().begin()->second; - - std::vector buf; - std::stringstream output_stream; - std::streambuf* old_output_stream = std::cout.rdbuf(); - - std::cout.rdbuf(output_stream.rdbuf()); - auto result = sensor->from_buffer(buf, "NON EXISTENT"); - std::cout.rdbuf(old_output_stream); - EXPECT_EQ(output_stream.str(), "UNKNOWN TYPE: NON EXISTENT\n"); - EXPECT_EQ(result, nullptr); - std::cout.rdbuf(old_output_stream); - - result = sensor->from_buffer(sensor->buffer(), - "ouster/v1/os_sensor/LidarSensor"); - EXPECT_NE(result, nullptr); - EXPECT_EQ(result->id(), 0); - EXPECT_EQ(result->type(), "ouster/v1/os_sensor/LidarSensor"); -} - -} // namespace -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/writer_custom_test.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/writer_custom_test.cpp deleted file mode 100644 index e14261a1..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/writer_custom_test.cpp +++ /dev/null @@ -1,162 +0,0 @@ -/** - * Copyright(c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#include - -#include - -#include "common.h" -#include "osf_test.h" -#include "ouster/osf/file.h" -#include "ouster/osf/reader.h" -#include "ouster/osf/writer.h" - -namespace ouster { -namespace osf { - -class WriterCustomTest : public osf::OsfTestWithDataAndFiles {}; - -class MyNewMetaInfo : public MetadataEntryHelper { - public: - explicit MyNewMetaInfo(const std::string& text) : text_(text) {} - - const std::string& text() const { return text_; } - - // Pack to byte array - std::vector buffer() const final { - return {text_.begin(), text_.end()}; - } - - // UnPack from byte array - static std::unique_ptr from_buffer( - const std::vector& buf) { - std::string s(buf.begin(), buf.end()); - return std::make_unique(s); - } - - // Custom view for nice to_string() output - std::string repr() const override { return "text: '" + text_ + "'"; } - - private: - std::string text_; -}; - -template <> -struct MetadataTraits { - static const std::string type() { return "ouster/v1/MyNewSuperMetaInfo"; } -}; - -// TODO[pb]: Define a StreamTagHelper for just dummy types/tags for use in -// custom stream definitions -class YoStreamMeta : public MetadataEntryHelper { - public: - YoStreamMeta() {} - std::vector buffer() const final { return {}; }; - static std::unique_ptr from_buffer( - const std::vector&) { - return std::make_unique(); - } -}; - -template <> -struct MetadataTraits { - static const std::string type() { return "ouster/v1/YoStream"; } -}; - -// Message object of YoStream -struct yo { - uint8_t a; -}; - -// Define custom stream with the message type `yo` -class YoStream : public MessageStream { - public: - YoStream(Writer& writer) : writer_{writer}, meta_{} { - stream_meta_id_ = writer_.add_metadata(meta_); - }; - - // Boilerplate for writer - void save(const ouster::osf::ts_t ts, const obj_type& yo_obj) { - const auto& msg_buf = make_msg(yo_obj); - writer_.save_message(meta_.id(), ts, msg_buf); - } - - // Pack yo message into buffer - std::vector make_msg(const obj_type& yo_obj) { return {yo_obj.a}; } - - // UnPack yo message from buffer - static std::unique_ptr decode_msg(const std::vector& buf, - const meta_type&, - const MetadataStore&) { - auto y = std::make_unique(); - y->a = buf[0]; - return y; - } - - private: - Writer& writer_; - - meta_type meta_; - - uint32_t stream_meta_id_{0}; -}; - -TEST_F(WriterCustomTest, WriteCustomMsgExample) { - std::string output_osf_filename = tmp_file("writer_new_meta_info_msg.osf"); - - // Create OSF v2 Writer - osf::Writer writer(output_osf_filename); - - // Create LidarSensor record - writer.add_metadata("Happy New Year!"); - - // Create stream for `yo` objects - auto yo_stream = writer.create_stream(); - - uint8_t yo_cnt = 0; - while (yo_cnt < 100) { - // `yo` object - yo y; - y.a = (uint8_t)yo_cnt; - - // Save `yo` object into stream - ts_t ts{yo_cnt * 10}; - yo_stream.save(ts, y); - - ++yo_cnt; - } - - writer.close(); - - OsfFile file(output_osf_filename); - osf::Reader reader(file); - - // Read all messages from OSF file - int msg_cnt = 0; - for (const auto m : reader.messages()) { - // Decoding messages - if (m.is()) { - auto y = *m.decode_msg(); - // Check `yo` msgs are the same and in the same order as written - EXPECT_EQ(msg_cnt, y.a); - // std::cout << "yo = " << (int)y.a << std::endl; - } - ++msg_cnt; - } - EXPECT_EQ(100, msg_cnt); - - auto my_metas = reader.meta_store().find(); - EXPECT_EQ(1, my_metas.size()); - - // Get MyNewMetaInfo metadata - auto my_meta = my_metas.begin()->second; - EXPECT_EQ(my_meta->text(), "Happy New Year!"); - - std::cout << "my_meta = " << my_meta->to_string() << std::endl; - // std::cout << "output = " << output_osf_filename << std::endl; -} - -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/writer_test.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/writer_test.cpp deleted file mode 100644 index 80bdd83d..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/writer_test.cpp +++ /dev/null @@ -1,530 +0,0 @@ -/** - * Copyright(c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#include "ouster/osf/writer.h" - -#include - -#include - -#include "common.h" -#include "osf_test.h" -#include "ouster/lidar_scan.h" -#include "ouster/osf/file.h" -#include "ouster/osf/meta_extrinsics.h" -#include "ouster/osf/meta_lidar_sensor.h" -#include "ouster/osf/meta_streaming_info.h" -#include "ouster/osf/reader.h" -#include "ouster/osf/stream_lidar_scan.h" -#include "ouster/types.h" - -namespace ouster { -namespace osf { -namespace { - -using ouster::osf::get_random_lidar_scan; -using ouster::sensor::sensor_info; - -class WriterTest : public osf::OsfTestWithDataAndFiles {}; - -TEST_F(WriterTest, ChunksLayoutEnum) { - ChunksLayout cl = ChunksLayout::LAYOUT_STANDARD; - EXPECT_EQ(to_string(cl), "STANDARD"); - ChunksLayout cl1{}; - EXPECT_EQ(to_string(cl1), "STANDARD"); - ChunksLayout cl2{LAYOUT_STREAMING}; - EXPECT_EQ(to_string(cl2), "STREAMING"); - EXPECT_EQ(chunks_layout_of_string("STREAMING"), LAYOUT_STREAMING); - EXPECT_EQ(chunks_layout_of_string("STANDARD"), LAYOUT_STANDARD); - EXPECT_EQ(chunks_layout_of_string("RRR"), LAYOUT_STANDARD); -} - -TEST_F(WriterTest, WriteSingleLidarScan) { - const sensor_info sinfo = sensor::metadata_from_json( - path_concat(test_data_dir(), "pcaps/OS-1-128_v2.3.0_1024x10.json")); - LidarScan ls = get_random_lidar_scan(sinfo); - - std::string output_osf_filename = tmp_file("writer_simple.osf"); - - std::string sinfo_str = sinfo.updated_metadata_string(); - - // Writing LidarScan - Writer writer(output_osf_filename); - writer.set_metadata_id("test_session"); - EXPECT_EQ(writer.chunks_layout(), ChunksLayout::LAYOUT_STREAMING); - - writer.add_sensor(sinfo, get_field_types(sinfo)); - writer.save(0, ls, ts_t{123}); - writer.close(); - - OsfFile osf_file(output_osf_filename); - EXPECT_TRUE(osf_file.good()); - - Reader reader(osf_file); - EXPECT_EQ(reader.metadata_id(), "test_session"); - - auto msg_it = reader.messages().begin(); - EXPECT_NE(msg_it, reader.messages().end()); - - auto ls_recovered = msg_it->decode_msg(); - - EXPECT_TRUE(ls_recovered); - EXPECT_EQ(*ls_recovered, ls); - - EXPECT_EQ(++msg_it, reader.messages().end()); - - // Map of all MetadataEntries of type LidarSensor - auto sensors = reader.meta_store().find(); - EXPECT_EQ(sensors.size(), 1); - - // Use first sensor and get its sensor_info - auto sinfo_recovered = sensors.begin()->second->info(); - EXPECT_EQ(sinfo_recovered.original_string(), sinfo_str); - - auto metadata_recovered = sensors.begin()->second->metadata(); - EXPECT_EQ(metadata_recovered, sinfo_str); -} - -TEST_F(WriterTest, WriteLidarSensorWithExtrinsics) { - sensor_info sinfo = sensor::metadata_from_json( - path_concat(test_data_dir(), "pcaps/OS-1-128_v2.3.0_1024x10.json")); - - std::string output_osf_filename = - tmp_file("writer_lidar_sensor_extrinsics.osf"); - - std::string sinfo_str = sinfo.original_string(); - - sinfo.extrinsic(0, 3) = 10.0; - sinfo.extrinsic(0, 1) = 0.756; - sinfo.extrinsic(1, 0) = 0.756; - sinfo.extrinsic(0, 0) = 0.0; - - // Writing LidarSensor - Writer writer(output_osf_filename); - - auto sensor_meta_id = writer.add_metadata(sinfo_str); - EXPECT_TRUE(sensor_meta_id != 0); - - writer.add_metadata(sinfo.extrinsic, sensor_meta_id); - - writer.close(); - - OsfFile osf_file(output_osf_filename); - EXPECT_TRUE(osf_file.good()); - - Reader reader(osf_file); - - auto sensors = reader.meta_store().find(); - EXPECT_EQ(sensors.size(), 1); - - // Use first sensor and get its sensor_info - auto sinfo_recovered = sensors.begin()->second->info(); - - auto metadata_recovered = sensors.begin()->second->metadata(); - EXPECT_EQ(metadata_recovered, sinfo_str); - - auto extrinsics = reader.meta_store().find(); - EXPECT_EQ(extrinsics.size(), 1); - EXPECT_EQ(extrinsics.begin()->second->repr(), - "ExtrinsicsMeta: ref_id = 1, name = , extrinsics = 0 0.756 0 10 " - "0.756 1 0 0 0 0 1 0 0 0 0 1"); - - auto ext_mat_recovered = extrinsics.begin()->second->extrinsics(); - EXPECT_EQ(sinfo.extrinsic, ext_mat_recovered); - EXPECT_EQ(sensor_meta_id, extrinsics.begin()->second->ref_meta_id()); -} - -TEST_F(WriterTest, WriteSingleLidarScanStreamingLayout) { - const sensor_info sinfo = sensor::metadata_from_json( - path_concat(test_data_dir(), "pcaps/OS-1-128_v2.3.0_1024x10.json")); - LidarScan ls = get_random_lidar_scan(sinfo); - - std::string output_osf_filename = tmp_file("writer_simple_streaming.osf"); - - std::string sinfo_str = sinfo.updated_metadata_string(); - - // Writing LidarScan - Writer writer(output_osf_filename); - writer.set_metadata_id("test_session"); - EXPECT_EQ(writer.chunks_layout(), ChunksLayout::LAYOUT_STREAMING); - - writer.add_sensor(sinfo, get_field_types(sinfo)); - writer.save(0, ls, ts_t{123}); - writer.close(); - - OsfFile osf_file(output_osf_filename); - EXPECT_TRUE(osf_file.good()); - - Reader reader(osf_file); - EXPECT_EQ(reader.metadata_id(), "test_session"); - - // TODO[pb]: Add reader validation CRC - - auto msg_it = reader.messages().begin(); - EXPECT_NE(msg_it, reader.messages().end()); - - auto ls_recovered = msg_it->decode_msg(); - - EXPECT_TRUE(ls_recovered); - EXPECT_EQ(*ls_recovered, ls); - - EXPECT_EQ(++msg_it, reader.messages().end()); - - // Map of all MetadataEntries of type LidarSensor - auto sensors = reader.meta_store().find(); - EXPECT_EQ(sensors.size(), 1); - - // Check that it's an OSF with StreamingLayout - EXPECT_EQ(1, reader.meta_store().count()); - auto streaming_info = reader.meta_store().get(); - - EXPECT_TRUE(streaming_info != nullptr); - - // One stream LidarScanStream - EXPECT_EQ(1, streaming_info->stream_stats().size()); - - // Use first sensor and get its sensor_info - auto sinfo_recovered = sensors.begin()->second->info(); - EXPECT_EQ(sinfo_recovered.original_string(), sinfo_str); - - auto metadata_recovered = sensors.begin()->second->metadata(); - EXPECT_EQ(metadata_recovered, sinfo_str); -} - -TEST_F(WriterTest, WriteSlicedLidarScan) { - const sensor_info sinfo = sensor::metadata_from_json( - path_concat(test_data_dir(), "pcaps/OS-1-128_v2.3.0_1024x10.json")); - LidarScan ls = get_random_lidar_scan(sinfo); - - // Subset of fields to leave in LidarScan - LidarScanFieldTypes field_types; - field_types.emplace_back(sensor::ChanField::RANGE, - ls.field_type(sensor::ChanField::RANGE)); - field_types.emplace_back(sensor::ChanField::REFLECTIVITY, - ls.field_type(sensor::ChanField::REFLECTIVITY)); - - // Make a reduced field LidarScan - ls = slice_with_cast(ls, field_types); - - EXPECT_EQ(field_types.size(), std::distance(ls.begin(), ls.end())); - - std::string output_osf_filename = tmp_file("writer_sliced.osf"); - - std::string sinfo_str = sinfo.updated_metadata_string(); - - // Writing LidarScan - Writer writer(output_osf_filename, sinfo, field_types); - writer.set_metadata_id("test_session"); - writer.save(0, ls, ts_t{123}); - writer.close(); - - OsfFile osf_file(output_osf_filename); - EXPECT_TRUE(osf_file.good()); - - Reader reader(osf_file); - EXPECT_EQ(reader.metadata_id(), "test_session"); - - auto msg_it = reader.messages().begin(); - EXPECT_NE(msg_it, reader.messages().end()); - - auto ls_recovered = msg_it->decode_msg(); - - EXPECT_EQ(field_types.size(), - std::distance(ls_recovered->begin(), ls_recovered->end())); - - EXPECT_TRUE(ls_recovered); - EXPECT_EQ(*ls_recovered, ls); - - EXPECT_EQ(++msg_it, reader.messages().end()); - - // Map of all MetadataEntries of type LidarSensor - auto sensors = reader.meta_store().find(); - EXPECT_EQ(sensors.size(), 1); - - // Use first sensor and get its sensor_info - auto sinfo_recovered = sensors.begin()->second->info(); - EXPECT_EQ(sinfo_recovered.original_string(), sinfo_str); - - auto metadata_recovered = sensors.begin()->second->metadata(); - EXPECT_EQ(metadata_recovered, sinfo_str); -} - -TEST_F(WriterTest, WriteSlicedLegacyLidarScan) { - const sensor_info sinfo = sensor::metadata_from_json(path_concat( - test_data_dir(), "metadata/2_5_0_os-992146000760-128_legacy.json")); - LidarScan ls_orig = get_random_lidar_scan(sinfo); - - // Subset of fields to leave in LidarScan during writing - LidarScanFieldTypes field_types; - field_types.emplace_back(sensor::ChanField::RANGE, - sensor::ChanFieldType::UINT32); - field_types.emplace_back(sensor::ChanField::SIGNAL, - sensor::ChanFieldType::UINT16); - field_types.emplace_back(sensor::ChanField::REFLECTIVITY, - sensor::ChanFieldType::UINT8); - - std::cout << "LidarScan field_types: " << ouster::to_string(field_types) - << std::endl; - - // Make a reduced/extended fields LidarScan - // that will be compared with a recovered LidarScan from OSF - auto ls_reference = slice_with_cast(ls_orig, field_types); - - EXPECT_EQ(field_types.size(), - std::distance(ls_reference.begin(), ls_reference.end())); - - std::string output_osf_filename = tmp_file("writer_sliced_legacy.osf"); - - std::string sinfo_str = sinfo.updated_metadata_string(); - - // Writing LidarScan with custom field types - Writer writer(output_osf_filename, sinfo, field_types); - writer.set_metadata_id("test_session"); - - writer.save(0, ls_orig, ts_t{123}); - writer.close(); - - OsfFile osf_file(output_osf_filename); - EXPECT_TRUE(osf_file.good()); - - Reader reader(osf_file); - EXPECT_EQ(reader.metadata_id(), "test_session"); - - auto msg_it = reader.messages().begin(); - EXPECT_NE(msg_it, reader.messages().end()); - - auto ls_recovered = msg_it->decode_msg(); - - EXPECT_EQ(field_types.size(), - std::distance(ls_recovered->begin(), ls_recovered->end())); - - EXPECT_TRUE(ls_recovered); - - EXPECT_EQ(*ls_recovered, ls_reference); - - EXPECT_EQ(++msg_it, reader.messages().end()); - - // Map of all MetadataEntries of type LidarSensor - auto sensors = reader.meta_store().find(); - EXPECT_EQ(sensors.size(), 1); - - // Use first sensor and get its sensor_info - auto sinfo_recovered = sensors.begin()->second->info(); - EXPECT_EQ(sinfo_recovered.original_string(), sinfo_str); - - auto metadata_recovered = sensors.begin()->second->metadata(); - EXPECT_EQ(metadata_recovered, sinfo_str); -} - -TEST_F(WriterTest, WriteCustomLidarScanWithFlags) { - const sensor_info sinfo = sensor::metadata_from_json(path_concat( - test_data_dir(), "metadata/3_0_1_os-122246000293-128_legacy.json")); - - LidarScanFieldTypes field_types_with_flags; - field_types_with_flags.emplace_back(sensor::ChanField::RANGE, - sensor::ChanFieldType::UINT32); - field_types_with_flags.emplace_back(sensor::ChanField::SIGNAL, - sensor::ChanFieldType::UINT16); - field_types_with_flags.emplace_back(sensor::ChanField::RANGE2, - sensor::ChanFieldType::UINT32); - field_types_with_flags.emplace_back(sensor::ChanField::SIGNAL2, - sensor::ChanFieldType::UINT16); - field_types_with_flags.emplace_back(sensor::ChanField::REFLECTIVITY, - sensor::ChanFieldType::UINT8); - field_types_with_flags.emplace_back(sensor::ChanField::NEAR_IR, - sensor::ChanFieldType::UINT16); - field_types_with_flags.emplace_back(sensor::ChanField::FLAGS, - sensor::ChanFieldType::UINT8); - field_types_with_flags.emplace_back(sensor::ChanField::FLAGS2, - sensor::ChanFieldType::UINT8); - field_types_with_flags.emplace_back(sensor::ChanField::CUSTOM0, - sensor::ChanFieldType::UINT64); - field_types_with_flags.emplace_back(sensor::ChanField::CUSTOM7, - sensor::ChanFieldType::UINT16); - - LidarScan ls = get_random_lidar_scan(sinfo.format.columns_per_frame, - sinfo.format.pixels_per_column, - field_types_with_flags); - - std::cout << "LidarScan field_types_with_flags: " - << ouster::to_string(field_types_with_flags) << std::endl; - - // Check that we have non zero FLAGS - img_t flags{ls.h, ls.w}; - impl::visit_field(ls, sensor::ChanField::FLAGS, - ouster::impl::read_and_cast(), flags); - EXPECT_FALSE((flags == 0).all()); - // and non zero FLAGS2 - impl::visit_field(ls, sensor::ChanField::FLAGS2, - ouster::impl::read_and_cast(), flags); - EXPECT_FALSE((flags == 0).all()); - - // Check that we have non zero CUSTOM7 - img_t custom{ls.h, ls.w}; - impl::visit_field(ls, sensor::ChanField::CUSTOM7, - ouster::impl::read_and_cast(), custom); - EXPECT_FALSE((custom == 0).all()); - - EXPECT_EQ(field_types_with_flags.size(), - std::distance(ls.begin(), ls.end())); - - std::string output_osf_filename = tmp_file("writer_with_flags.osf"); - - std::string sinfo_str = sinfo.original_string(); - - // Writing LidarScan - Writer writer(output_osf_filename, sinfo, get_field_types(ls)); - writer.set_metadata_id("test_session"); - writer.save(0, ls, ts_t{123}); - writer.close(); - - OsfFile osf_file(output_osf_filename); - EXPECT_TRUE(osf_file.good()); - - Reader reader(osf_file); - EXPECT_EQ(reader.metadata_id(), "test_session"); - - auto msg_it = reader.messages().begin(); - EXPECT_NE(msg_it, reader.messages().end()); - - auto ls_recovered = msg_it->decode_msg(); - - EXPECT_TRUE(ls_recovered); - - EXPECT_EQ(field_types_with_flags.size(), - std::distance(ls_recovered->begin(), ls_recovered->end())); - - EXPECT_EQ(*ls_recovered, ls); - - EXPECT_EQ(++msg_it, reader.messages().end()); -} - -// Used in WriteExample test below -void ReadExample(const std::string filename) { - // Open output: OSF v2 - OsfFile file(filename); - Reader reader(file); - - // Read all messages from OSF file - for (const auto m : reader.messages()) { - auto ts = m.ts(); // << message timestamp - auto stream_meta_id = m.id(); // << link to the stream meta - - EXPECT_GT(ts.count(), 0); - EXPECT_GT(stream_meta_id, uint32_t{0}); - - // Decoding messages - if (m.is()) { - auto ls = m.decode_msg(); - EXPECT_TRUE(ls != nullptr); - // std::cout << "ls = " << *ls << std::endl; - } - } - - // Get meta objects by type map of (meta_id, meta_ptr) - auto sensors = reader.meta_store().find(); - EXPECT_EQ(1, sensors.size()); - - // Get LidarSensor metadata - auto lidar_sensor = reader.meta_store().get(); - EXPECT_TRUE(lidar_sensor); -} - -TEST_F(WriterTest, WriteExample) { - // Get sensor_info - const sensor_info sinfo = sensor::metadata_from_json( - path_concat(test_data_dir(), "pcaps/OS-1-128_v2.3.0_1024x10.json")); - - std::string output_osf_filename = tmp_file("write_example.osf"); - - // Create OSF v2 Writer - osf::Writer writer(output_osf_filename); - writer.set_metadata_id("Example Session 1234"); - EXPECT_EQ(writer.chunks_layout(), ChunksLayout::LAYOUT_STREAMING); - - // Create LidarSensor record - auto sensor_id = writer.add_sensor(sinfo); - - const int LOOP_CNT = 7; - - int timestamp = 0; - while (timestamp++ < LOOP_CNT) { - LidarScan ls = get_random_lidar_scan(sinfo); - - // Save LidarScan - writer.save(sensor_id, ls, ts_t{timestamp}); - } - - writer.close(); - - // Quick test the we have result file - OsfFile result(output_osf_filename); - EXPECT_TRUE(result.good()); - - // Quick test that number of messages in result file is what we wrote - Reader reader(result); - EXPECT_EQ(LOOP_CNT, std::distance(reader.messages().begin(), - reader.messages().end())); - - // Check that it's an OSF with StreamingLayout - auto streaming_info_entry = reader.meta_store().find(); - EXPECT_EQ(1, streaming_info_entry.size()); - auto streaming_info = streaming_info_entry.begin()->second; - - // std::cout << "streaming_info = " << streaming_info->to_string() << - // std::endl; - - // One stream: LidarScanStream - EXPECT_EQ(1, streaming_info->stream_stats().size()); - - EXPECT_TRUE(reader.has_message_idx()); - - auto lsm = reader.meta_store().get(); - - auto stream_msg_count = - streaming_info->stream_stats()[lsm->id()].message_count; - EXPECT_EQ(stream_msg_count, LOOP_CNT); - - for (size_t msg_idx = 0; msg_idx < stream_msg_count; ++msg_idx) { - auto msg_ts = reader.ts_by_message_idx(lsm->id(), msg_idx); - EXPECT_TRUE(msg_ts); - - EXPECT_TRUE(msg_ts >= reader.start_ts()); - EXPECT_TRUE(msg_ts <= reader.end_ts()); - - // by construction of the test - EXPECT_EQ(msg_idx + 1, msg_ts->count()); - - // if we start reading from that msg_ts using stream_id, there is indeed - // the first message returned with this timestamp - auto first_msg = - reader.messages({lsm->id()}, *msg_ts, reader.end_ts()).begin(); - EXPECT_EQ(first_msg->ts(), msg_ts); - } - - auto msg_ts100 = reader.ts_by_message_idx(lsm->id(), 100); - EXPECT_FALSE(msg_ts100); - - auto msg_ts_count = reader.ts_by_message_idx(lsm->id(), stream_msg_count); - EXPECT_FALSE(msg_ts_count); - - auto msg_ts_no_stream = reader.ts_by_message_idx(0, 0); - EXPECT_FALSE(msg_ts_no_stream); - - auto msg_ts_no_stream2 = reader.ts_by_message_idx(100, 0); - EXPECT_FALSE(msg_ts_no_stream2); - - ReadExample(output_osf_filename); -} - -TEST_F(WriterTest, FileNameOnlyWriterTest) { - osf::Writer writer("FOOBARBAT"); - EXPECT_EQ(writer.filename(), "FOOBARBAT"); - EXPECT_EQ(writer.metadata_id(), "ouster_sdk"); -} - -} // namespace -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/writerv2_test.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/writerv2_test.cpp deleted file mode 100644 index ee62a89e..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_osf/tests/writerv2_test.cpp +++ /dev/null @@ -1,203 +0,0 @@ -/** - * Copyright(c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#include - -#include - -#include "common.h" -#include "osf_test.h" -#include "ouster/lidar_scan.h" -#include "ouster/osf/file.h" -#include "ouster/osf/meta_extrinsics.h" -#include "ouster/osf/meta_lidar_sensor.h" -#include "ouster/osf/meta_streaming_info.h" -#include "ouster/osf/reader.h" -#include "ouster/osf/stream_lidar_scan.h" -#include "ouster/osf/writer.h" -#include "ouster/types.h" - -namespace ouster { -namespace osf { -namespace { - -using ouster::osf::get_random_lidar_scan; -using ouster::sensor::sensor_info; - -class WriterV2Test : public osf::OsfTestWithDataAndFiles {}; - -TEST_F(WriterV2Test, WriterV2AccessorTest) { - const int chunk_size = 1234; - std::string output_osf_filename = tmp_file("WriterV2AccessorTest.osf"); - const sensor::sensor_info info = sensor::metadata_from_json( - path_concat(test_data_dir(), "pcaps/OS-1-128_v2.3.0_1024x10.json")); - const sensor::sensor_info info2 = sensor::metadata_from_json( - path_concat(test_data_dir(), "pcaps/OS-0-128-U1_v2.3.0_1024x10.json")); - { - std::vector info_compare = {info}; - Writer writer(output_osf_filename, info, LidarScanFieldTypes(), - chunk_size); - EXPECT_EQ(writer.chunk_size(), chunk_size); - EXPECT_EQ(writer.sensor_info_count(), 1); - EXPECT_EQ(writer.filename(), output_osf_filename); - EXPECT_EQ(writer.sensor_info(), info_compare); - EXPECT_EQ(writer.sensor_info(0), info); - } - { - std::vector info_compare = {info, info2}; - Writer writer(output_osf_filename, info_compare, LidarScanFieldTypes(), - chunk_size); - EXPECT_EQ(writer.sensor_info_count(), 2); - - EXPECT_EQ(writer.sensor_info(), info_compare); - EXPECT_EQ(writer.sensor_info(0), info); - EXPECT_EQ(writer.sensor_info(1), info2); - } -} - -TEST_F(WriterV2Test, WriterV2BoundingTest) { - const int chunk_size = 1234; - std::string output_osf_filename = tmp_file("WriterV2BoundingTest.osf"); - const sensor::sensor_info info = sensor::metadata_from_json( - path_concat(test_data_dir(), "pcaps/OS-1-128_v2.3.0_1024x10.json")); - Writer writer(output_osf_filename, info, LidarScanFieldTypes(), chunk_size); - - bool caught = false; - try { - LidarScan one; - writer.save(1, one); - } catch (const std::logic_error& e) { - EXPECT_EQ(std::string(e.what()), "ERROR: Bad Stream ID"); - caught = true; - } catch (...) { - FAIL(); - } - EXPECT_TRUE(caught); - caught = false; - try { - LidarScan one; - LidarScan two; - writer.save({one, two}); - } catch (const std::logic_error& e) { - EXPECT_EQ(std::string(e.what()), - "ERROR: Scans passed in to writer " - "does not match number of sensor infos"); - caught = true; - } catch (...) { - FAIL(); - } - EXPECT_TRUE(caught); -} - -TEST_F(WriterV2Test, WriterV2CloseTest) { - std::string output_osf_filename = tmp_file("WriterV2CloseTest.osf"); - const sensor::sensor_info info = sensor::metadata_from_json( - path_concat(test_data_dir(), "pcaps/OS-1-128_v2.3.0_1024x10.json")); - LidarScan ls = get_random_lidar_scan(info); - Writer writer(output_osf_filename, info); - writer.save(0, ls); - EXPECT_FALSE(writer.is_closed()); - writer.close(); - EXPECT_TRUE(writer.is_closed()); - bool caught = false; - try { - writer.save({ls}); - } catch (const std::logic_error& e) { - EXPECT_EQ(std::string(e.what()), "ERROR: Writer is closed"); - caught = true; - } catch (...) { - FAIL(); - } - EXPECT_TRUE(caught); -} - -void test_single_file(std::string& output_osf_filename, LidarScan& ls) { - Reader reader(output_osf_filename); - - auto msg_it = reader.messages().begin(); - EXPECT_NE(msg_it, reader.messages().end()); - - auto ls_recovered = msg_it->decode_msg(); - - EXPECT_TRUE(ls_recovered); - EXPECT_EQ(*ls_recovered, ls); - - EXPECT_EQ(++msg_it, reader.messages().end()); -} - -TEST_F(WriterV2Test, WriterV2SingleIndexedTest) { - std::string output_osf_filename = tmp_file("WriterV2SingleIndexedTest.osf"); - const sensor::sensor_info info = sensor::metadata_from_json( - path_concat(test_data_dir(), "pcaps/OS-1-128_v2.3.0_1024x10.json")); - LidarScan ls = get_random_lidar_scan(info); - { - Writer writer(output_osf_filename, info); - writer.save(0, ls); - } - test_single_file(output_osf_filename, ls); -} - -TEST_F(WriterV2Test, WriterV2SingleVectorTest) { - std::string output_osf_filename = tmp_file("WriterV2SingleVectorTest.osf"); - const sensor::sensor_info info = sensor::metadata_from_json( - path_concat(test_data_dir(), "pcaps/OS-1-128_v2.3.0_1024x10.json")); - LidarScan ls = get_random_lidar_scan(info); - { - Writer writer(output_osf_filename, info); - writer.save({ls}); - } - test_single_file(output_osf_filename, ls); -} - -void test_multi_file(std::string& output_osf_filename, LidarScan& ls, - LidarScan& ls2) { - Reader reader(output_osf_filename); - auto msg_it = reader.messages().begin(); - EXPECT_NE(msg_it, reader.messages().end()); - auto ls_recovered = msg_it->decode_msg(); - EXPECT_TRUE(ls_recovered); - EXPECT_EQ(*ls_recovered, ls); - EXPECT_NE(++msg_it, reader.messages().end()); - auto ls_recovered2 = msg_it->decode_msg(); - EXPECT_TRUE(ls_recovered2); - EXPECT_EQ(*ls_recovered2, ls2); - EXPECT_EQ(++msg_it, reader.messages().end()); -} - -TEST_F(WriterV2Test, WriterV2MultiIndexedTest) { - std::string output_osf_filename = tmp_file("WriterV2MultiIndexedTest.osf"); - const sensor::sensor_info info = sensor::metadata_from_json( - path_concat(test_data_dir(), "pcaps/OS-1-128_v2.3.0_1024x10.json")); - const sensor::sensor_info info2 = sensor::metadata_from_json( - path_concat(test_data_dir(), "pcaps/OS-0-128-U1_v2.3.0_1024x10.json")); - - LidarScan ls = get_random_lidar_scan(info); - LidarScan ls2 = get_random_lidar_scan(info2); - { - Writer writer(output_osf_filename, {info, info2}); - writer.save(0, ls); - writer.save(1, ls2); - } - test_multi_file(output_osf_filename, ls2, ls); -} - -TEST_F(WriterV2Test, WriterV2MultiVectorTest) { - std::string output_osf_filename = tmp_file("WriterV2MultiVectorTest.osf"); - const sensor::sensor_info info = sensor::metadata_from_json( - path_concat(test_data_dir(), "pcaps/OS-1-128_v2.3.0_1024x10.json")); - const sensor::sensor_info info2 = sensor::metadata_from_json( - path_concat(test_data_dir(), "pcaps/OS-0-128-U1_v2.3.0_1024x10.json")); - - LidarScan ls = get_random_lidar_scan(info); - LidarScan ls2 = get_random_lidar_scan(info2); - { - Writer writer(output_osf_filename, {info, info2}); - writer.save({ls, ls2}); - } - test_multi_file(output_osf_filename, ls2, ls); -} -} // namespace -} // namespace osf -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_pcap/CMakeLists.txt b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_pcap/CMakeLists.txt deleted file mode 100644 index 611d55f0..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_pcap/CMakeLists.txt +++ /dev/null @@ -1,34 +0,0 @@ -# ==== Requirements ==== -find_package(Pcap REQUIRED) -find_package(libtins REQUIRED) - -include(Coverage) - -# ==== Libraries ==== -add_library(ouster_pcap src/pcap.cpp src/os_pcap.cpp src/indexed_pcap_reader.cpp) -target_include_directories(ouster_pcap SYSTEM PRIVATE - ${PCAP_INCLUDE_DIR}) -target_include_directories(ouster_pcap PUBLIC - $ - $) -CodeCoverageFunctionality(ouster_pcap) - -if(WIN32) - target_compile_options(ouster_pcap PRIVATE /wd4200) - target_link_libraries(ouster_pcap PUBLIC ws2_32) -endif() -target_link_libraries(ouster_pcap - PUBLIC - OusterSDK::ouster_client - PRIVATE libpcap::libpcap libtins::libtins) -add_library(OusterSDK::ouster_pcap ALIAS ouster_pcap) - -# ==== Install ==== -install(TARGETS ouster_pcap - EXPORT ouster-sdk-targets - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include) - -install(DIRECTORY include/ouster DESTINATION include) diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_pcap/include/ouster/indexed_pcap_reader.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_pcap/include/ouster/indexed_pcap_reader.h deleted file mode 100644 index 8fa5137e..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_pcap/include/ouster/indexed_pcap_reader.h +++ /dev/null @@ -1,146 +0,0 @@ -/** - * Copyright (c) 2023, Ouster Inc. - */ - -#pragma once - -#include "ouster/os_pcap.h" -#include "ouster/pcap.h" -#include "ouster/types.h" - -namespace ouster { -namespace sensor_utils { - -struct PcapIndex { - using frame_index = - std::vector; ///< Maps a frame number to a file offset - - std::vector frame_indices_; ///< frame index for each sensor - - using timestamp_index = std::unordered_map; - - // TODO: this isn't used for now but in the future might be used to - // solve the issue with the limited frame_id span (we could remove it) - std::vector frame_timestamp_indices_; - - using frame_id_index = std::unordered_map; - - // TODO[IMPORTANT]: this has an issue if the recorded pcap file to span - // over 50 mins. - std::vector frame_id_indices_; - - PcapIndex(size_t num_sensors) - : frame_indices_(num_sensors), - frame_timestamp_indices_(num_sensors), - frame_id_indices_(num_sensors) {} - - /** - * Simple method to clear the index. - */ - void clear(); - - /** - * Returns the number of frames in the frame index for the given sensor - * index. - * - * @param[in] sensor_index The position of the sensor for which to retrieve - * the desired frame count. - * @return The number of frames in the sensor's frame index. - */ - size_t frame_count(size_t sensor_index) const; - - /** - * Seeks the given reader to the given frame number for the given sensor - * index - */ - // TODO[UN]: in my opinion we are better off removing this method from this - // class It is better if we keep this class as a simple POD object. Another - // problem with this method specifically is that it creates a cyclic and - // this is the reason why we are passing PcapReader instead of - // IndexedPcapReader to avoid this cyclic relation. If it mounts to anything - // this method should be part of the IndexedPcapReader. - void seek_to_frame(PcapReader& reader, size_t sensor_index, - unsigned int frame_number); -}; - -/** - * A PcapReader that allows seeking to the start of a lidar frame. - * - * The index must be computed by iterating through all packets and calling - * `update_index_for_current_packet()` for each one. - */ -struct IndexedPcapReader : public PcapReader { - /** - * @param[in] pcap_filename A file path of the pcap to read - * @param[in] metadata_filenames A vector of sensor metadata file paths - */ - IndexedPcapReader(const std::string& pcap_filename, - const std::vector& metadata_filenames); - - /** - * @param[in] pcap_filename A file path of the pcap to read - * @param[in] sensor_infos A vector of sensor info structures for each - * sensors - */ - IndexedPcapReader( - const std::string& pcap_filename, - const std::vector& sensor_infos); - - /** - * This method constructs the index. Call this method before requesting the - * index information using get_index() - */ - void build_index(); - - /** - * Get index for the underlying pcap - * - * @return returns a PcapIndex object - */ - const PcapIndex& get_index() const; - - /** - * Attempts to match the current packet to one of the sensor info objects - * and returns the appropriate packet format if there is one - * - * @return An optional packet format for the current packet - */ - nonstd::optional sensor_idx_for_current_packet() const; - - /** - * @return the current packet's frame_id - * if the packet is associated with a sensor (and its corresponding packet - * format) - */ - nonstd::optional current_frame_id() const; - - /** - * Updates the frame index for the current packet - * @return the progress of indexing as an int from [0, 100] - */ - // TODO: I recommend take this a private method, the problem with exposing - // this method is that it only yields right results if invoked sequentially - // ; the results are dependent on the internal state. - int update_index_for_current_packet(); - - /** - * Return true if the frame_id from the packet stream has rolled over, - * hopefully avoiding spurious result that could occur from out of order or - * dropped packets. - * - * @return true if the frame id has rolled over. - */ - static bool frame_id_rolled_over(uint16_t previous, uint16_t current); - - std::vector - sensor_infos_; ///< A vector of sensor_info that correspond to the - ///< provided metadata files - PcapIndex index_; - - // TODO: remove, this should be a transient variable - std::vector> - previous_frame_ids_; ///< previous frame id for each sensor -}; - -} // namespace sensor_utils -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_pcap/include/ouster/os_pcap.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_pcap/include/ouster/os_pcap.h deleted file mode 100644 index 38d2e641..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_pcap/include/ouster/os_pcap.h +++ /dev/null @@ -1,305 +0,0 @@ -/** - * Copyright (c) 2021, Ouster, Inc. - * All rights reserved. - * - * @file - * @brief OS-1 pcap replay - */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "ouster/pcap.h" -#include "ouster/types.h" -namespace ouster { -namespace sensor_utils { -/** - * Structure representing a hash key/sorting key for a udp stream - */ -struct stream_key { - std::string dst_ip; ///< The destination IP - std::string src_ip; ///< The source IP - int src_port; ///< The src port - int dst_port; ///< The destination port - - bool operator==(const struct stream_key& other) const; -}; -} // namespace sensor_utils -} // namespace ouster - -template <> -struct std::hash { - std::size_t operator()( - const ouster::sensor_utils::stream_key& key) const noexcept { - return std::hash{}(key.src_ip) ^ - (std::hash{}(key.src_ip) << 1) ^ - (std::hash{}(key.src_port << 2)) ^ - (std::hash{}(key.dst_port << 3)); - } -}; - -namespace ouster { -namespace sensor_utils { - -using ts = std::chrono::microseconds; ///< Microsecond timestamp - -/** - * To string method for packet info structs. - * - * @param[inout] stream_in The pre-existing ostream to concat with data. - * @param[in] data The packet_info to output. - * - * @return The new output stream containing concatted stream_in and data. - */ -std::ostream& operator<<(std::ostream& stream_in, const packet_info& data); - -/** - * Structure representing a hash key/sorting key for a udp stream - */ -struct guessed_ports { - int lidar; ///< Guessed lidar port - int imu; ///< Guessed imu port -}; - -/** - * To string method for stream_key structs. - * - * @param[inout] stream_in The pre-existing ostream to concat with data. - * @param[in] data The stream_key to output. - * - * @return The new output stream containing concatted stream_in and data. - */ -std::ostream& operator<<(std::ostream& stream_in, const stream_key& data); - -struct stream_data { - uint64_t count; ///< Number of packets in a specified stream - std::map - payload_size_counts; ///< Packet sizes detected in a specified stream - ///< Key: Packet Size - ///< Value: Count of a specific packet size - std::map - fragment_counts; ///< Fragments detected in a specified stream - ///< Key: Number of fragments - ///< Value: Count of a specific number of packets - std::map - ip_version_counts; ///< IP version detected in a specified stream - ///< Key: IP Version - ///< Value: Count of the specific ip version -}; - -/** - * To string method for stream_data structs. - * - * @param[inout] stream_in The pre-existing ostream to concat with data. - * @param[in] data The stream_data to output. - * - * @return The new output stream containing concatted stream_in and data. - */ -std::ostream& operator<<(std::ostream& stream_in, const stream_data& data); - -/** - * Structure representing the information about network streams in a pcap file - */ -struct stream_info { - uint64_t total_packets; ///< The total number of packets detected - uint32_t encapsulation_protocol; ///< The encapsulation protocol for the - ///< pcap file - - ts timestamp_max; ///< The latest timestamp detected - ts timestamp_min; ///< The earliest timestamp detected - - std::unordered_map - udp_streams; ///< Datastructure containing info on all of the different - ///< streams -}; - -/** - * To string method for stream info structs. - * - * @param[inout] stream_in The pre-existing ostream to concat with data. - * @param[in] data The stream_info to output. - * - * @return The new output stream containing concatted stream_info and data. - */ -std::ostream& operator<<(std::ostream& stream_in, const stream_info& data); - -/** Struct to hide the stepwise playback details. */ -struct playback_handle; - -/** Struct to hide the record details. */ -struct record_handle; - -/** - * Initialize the stepwise playback handle. - * - * @param[in] file The file path of the pcap file. - * - * @return A handle to the initialized playback struct. - */ -std::shared_ptr replay_initialize(const std::string& file); - -/** - * Uninitialize the stepwise playback handle. - * - * @param[in] handle A handle to the initialized playback struct. - */ -void replay_uninitialize(playback_handle& handle); - -/** - * Restart playback from the beginning of the pcap file. - * - * @param[in] handle A handle to the initialized playback struct. - */ -void replay_reset(playback_handle& handle); - -/** - * Return the information on the next packet avaliable in the playback_handle. - * This must be called BEFORE calling the read_next_packet function. - * - * @param[in] handle The playback handle. - * @param[out] info The returned information on the next packet. - * - * @return The status on whether there is a new packet or not. - */ -bool next_packet_info(playback_handle& handle, packet_info& info); - -/** - * Read the data from the next packet avaliable in the playback_handle. - * This must be called AFTER calling the next_packet_info function. - * - * @param[in] handle The playback handle. - * @param[out] buf The buffer to write the recieved data to (Must be sized - * appropriately. - * @param[in] buffer_size The size of the output buffer. - * - * @return 0 on no new packet, > 0 the size of the bytes recieved. - */ -size_t read_packet(playback_handle& handle, uint8_t* buf, size_t buffer_size); - -/** - * Initialize the record handle for recording multi sensor pcap files. Source - * and destination IPs must be provided with each packet. - * - * @param[in] file The file path to the target pcap to record to. - * @param[in] frag_size The size of the fragments for packet fragmentation. - * @param[in] use_sll_encapsulation Whether to use sll encapsulation. - */ -std::shared_ptr record_initialize( - const std::string& file, int frag_size, bool use_sll_encapsulation = false); - -/** - * Uninitialize the record handle, closing underlying file. - * - * @param[in] handle An initialized handle for the recording state. - */ -void record_uninitialize(record_handle& handle); - -/** - * Record a buffer to a multi sensor record_handle pcap file. - * - * @param[in] handle The record handle that record_initialize has initted. - * @param[in] src_ip The source address to label the packets with. - * @param[in] dst_ip The destination address to label the packets with. - * @param[in] src_port The source port to label the packets with. - * @param[in] dst_port The destination port to label the packets with. - * @param[in] buf The buffer to record to the pcap file. - * @param[in] buffer_size The size of the buffer to record to the pcap file. - * @param[in] microsecond_timestamp The timestamp to record the packet as - * microseconds. - */ -void record_packet(record_handle& handle, const std::string& src_ip, - const std::string& dst_ip, int src_port, int dst_port, - const uint8_t* buf, size_t buffer_size, - uint64_t microsecond_timestamp); - -/** - * Record a buffer to a multi sensor record_handle pcap file. - * - * @param[in] handle The record handle that record_initialize has initted. - * @param[in] info The packet_info object to use for the packet. - * @param[in] buf The buffer to record to the pcap file. - * @param[in] buffer_size The size of the buffer to record to the pcap file. - */ -void record_packet(record_handle& handle, const packet_info& info, - const uint8_t* buf, size_t buffer_size); - -/** - * Return the information about network streams in a pcap file. - * - * @param[in] file The pcap file to read. - * @param[in] packets_to_process Number of packets to process < 0 for all of - * them - * - * @return A pointer to the resulting stream_info - */ -std::shared_ptr get_stream_info(const std::string& file, - int packets_to_process = -1); - -/** - * Return the information about network streams in a pcap file. - * - * @param[in] file The pcap file to read. - * @param[in] progress_callback A callback to invoke after each packet is - * scanned - * current: The current file offset - * delta: The delta in file offset - * total: The total size of the file - * @param[in] packets_per_callback Callback every n packets - * @param[in] packets_to_process Number of packets to process < 0 for all of - * them - * - * @return A pointer to the resulting stream_info - */ -std::shared_ptr get_stream_info( - const std::string& file, - std::function - progress_callback, - int packets_per_callback, int packets_to_process = -1); - -/** - * Return the information about network streams in a PcapReader and generate - * indicies (if the PcapReader is an IndexedPcapReader). - * - * @param[in] pcap_reader The PcapReader - * @param[in] progress_callback A callback to invoke after each packet is - * scanned - * current: The current file offset - * delta: The delta in file offset - * total: The total size of the file - * @param[in] packets_per_callback Callback every n packets - * @param[in] packets_to_process Number of packets to process < 0 for all of - * them - * - * @return A pointer to the resulting stream_info - */ -std::shared_ptr get_stream_info( - PcapReader& pcap_reader, - std::function progress_callback, - int packets_per_callback, int packets_to_process = -1); -/** - * Return a guess of the correct ports located in a pcap file. - * - * @param[in] info The stream_info structure generated from a specific pcap file - * @param[in] lidar_packet_size The size of the lidar packets - * @param[in] imu_packet_size The size of the imu packets - * @param[in] expected_lidar_port The expected lidar port from the metadata - * (pass 0 for unknown) - * @param[in] expected_imu_port The expected imu port from the metadata - * (pass 0 for unknown) - * - * @return A vector (sorted by most likely to least likely) of the guessed ports - */ -std::vector guess_ports(stream_info& info, int lidar_packet_size, - int imu_packet_size, - int expected_lidar_port, - int expected_imu_port); -} // namespace sensor_utils -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_pcap/include/ouster/pcap.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_pcap/include/ouster/pcap.h deleted file mode 100644 index f959c816..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_pcap/include/ouster/pcap.h +++ /dev/null @@ -1,206 +0,0 @@ -/** - * Copyright (c) 2022, Ouster, Inc. - * All rights reserved. - * - */ - -#pragma once - -#include -#include -#include -#include -#include - -namespace ouster { -namespace sensor_utils { - -struct pcap_impl; -struct pcap_writer_impl; - -static constexpr int IANA_UDP = 17; - -struct packet_info { - using ts = std::chrono::microseconds; - - // TODO: use numerical IPs for efficient filtering - std::string dst_ip; ///< The destination IP - std::string src_ip; ///< The source IP - int dst_port; ///< The destination port - int src_port; ///< The source port - size_t payload_size; ///< The size of the packet payload - size_t packet_size; ///< The size of the full packet - ts timestamp; ///< The packet capture timestamp - int fragments_in_packet; ///< Number of fragments in the packet - int ip_version; ///< The ip version, 4 or 6 - int encapsulation_protocol; ///< PCAP encapsulation type - uint64_t file_offset; ///< Where the packet is in the pcap - // TODO: remove, library ignores non-UDP packes - int network_protocol; ///< IANA protocol number. Always 17 (UDP) -}; - -/** - * Class for dealing with reading pcap files - */ -class PcapReader { - std::unique_ptr impl; ///< Private implementation pointer - packet_info info; ///< Cached packet info - std::map fragment_count; ///< Map to count fragments per packet - uint8_t* data; ///< Cached packet data - - public: - /** - * @param[in] file A filepath of the pcap to read - */ - PcapReader(const std::string& file); - virtual ~PcapReader(); - - /** - * Advances to the next packet and returns the size of that packet. - * Will also populate data and info for next_packet(), current_data(), - * current_length(), and current_info() - * - * @return The size of the packet payload - */ - size_t next_packet(); - - /** - * Return the current packets data. - * To advance to a new packet please use next_packet() - * To get the size of the data use current_length() - * - * @return A pointer to a byte array containing the packet data - */ - const uint8_t* current_data() const; - - /** - * Return the current packets data size. - * To advance to a new packet please use next_packet() - * - * @return The size of the byte array - */ - size_t current_length() const; - - /** - * Return the current packets info. - * To advance to a new packet please use next_packet() - * - * @return A packet_info object on the current packet - */ - const packet_info& current_info() const; - - /** - * @return The size of the PCAP file in bytes - */ - int64_t file_size() const; - - /** - * Return the read position to the start of the PCAP file - */ - void reset(); - - /** - * Seek to the position in the file represented by the - * number of bytes from the beginning of the file. - * - * @param[in] offset The position to seek to in bytes, - * starting from the beginning of the file. - * - * @pre offset must be the offset of a PCAP - * record header. If any other value is provided, - * subsequent packet reads from this PcapReader will be - * invalid until PcapReader::reset is called. - */ - void seek(uint64_t offset); - - int64_t current_offset() const; - - private: - int64_t file_size_{}; - int64_t file_start_{}; -}; - -/** - * Class for dealing with writing udp pcap files - */ -class PcapWriter { - public: - /** - * Enum to describe the current encapsulation for a pcap file - */ - enum PacketEncapsulation { - NULL_LOOPBACK = 0x0, ///< Null Loopback Encapsulation - ETHERNET = 0x1, ///< Ethernet II Encapsulation - SLL = 0x71, ///< Linux Cooked Capture Encapsulation - }; - - /** - * @param[in] file The file path to write the pcap to - * @param[in] encap The encapsulation to use for the pcap - * @param[in] frag_size The fragmentation size to use (Currently broken) - */ - PcapWriter(const std::string& file, PacketEncapsulation encap, - uint16_t frag_size); - virtual ~PcapWriter(); - - /** - * Write a packet using a buffer to the pcap - * - * @param[in] buf The buffer to write - * @param[in] buf_size The size of the buffer to write - * @param[in] src_ip The source ip address to use for the packet - * @param[in] dst_ip The destination ip address to use for the packet - * @param[in] src_port The source port number to use for the packet - * @param[in] dst_port The destination port number to use for the packet - * @param[in] timestamp The timestamp of the packet to record - * @note The timestamp parameter does not affect the order of packets being - * recorded, it is strictly recorded FIFO. - */ - void write_packet(const uint8_t* buf, size_t buf_size, - const std::string& src_ip, const std::string& dst_ip, - uint16_t src_port, uint16_t dst_port, - packet_info::ts timestamp); - - /** - * Write a packet using a buffer to the pcap - * - * @param[in] buf The buffer to write - * @param[in] buf_size The size of the buffer to write - * @param[in] info The packet info object to use for the recording - * parameters - * @note The timestamp parameter in info does not affect the order of - * packets being recorded, it is strictly recorded FIFO. - */ - void write_packet(const uint8_t* buf, size_t buf_size, - const packet_info& info); - - /** - * Write all pending data to the pcap file - */ - void flush(); - - /** - * Flushes and cleans up all memory in use by the pap writer - */ - void close(); - - protected: - std::unique_ptr impl; ///< Internal data - - uint16_t id; ///< An incrementing id to record packets with - PacketEncapsulation encap; ///< Encapsulation to record with - uint16_t frag_size; ///< Fragmentation size(not currently used) - bool closed; -}; - -/** - * To string method for packet info structs. - * - * @param[inout] stream_in The pre-existing ostream to concat with data. - * @param[in] data The packet_info to output. - * - * @return The new output stream containing concatted stream_in and data. - */ -std::ostream& operator<<(std::ostream& stream_in, const packet_info& data); -} // namespace sensor_utils -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_pcap/src/indexed_pcap_reader.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_pcap/src/indexed_pcap_reader.cpp deleted file mode 100644 index 960cd748..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_pcap/src/indexed_pcap_reader.cpp +++ /dev/null @@ -1,106 +0,0 @@ -#include "ouster/indexed_pcap_reader.h" - -namespace ouster { -namespace sensor_utils { - -IndexedPcapReader::IndexedPcapReader( - const std::string& pcap_filename, - const std::vector& metadata_filenames) - : PcapReader(pcap_filename), - index_(metadata_filenames.size()), - previous_frame_ids_(metadata_filenames.size()) { - for (const std::string& metadata_filename : metadata_filenames) { - sensor_infos_.push_back( - ouster::sensor::metadata_from_json(metadata_filename)); - } -} - -IndexedPcapReader::IndexedPcapReader( - const std::string& pcap_filename, - const std::vector& sensor_infos) - : PcapReader(pcap_filename), - sensor_infos_(sensor_infos), - index_(sensor_infos.size()), - previous_frame_ids_(sensor_infos.size()) {} - -nonstd::optional IndexedPcapReader::sensor_idx_for_current_packet() - const { - const auto& pkt_info = current_info(); - for (size_t i = 0; i < sensor_infos_.size(); i++) { - if (pkt_info.dst_port == sensor_infos_[i].udp_port_lidar) { - // TODO use the packet format and match serial number if it's - // available this will allow us to have multiple sensors on the same - // port - return i; - } - } - return nonstd::nullopt; -} - -nonstd::optional IndexedPcapReader::current_frame_id() const { - if (nonstd::optional sensor_idx = sensor_idx_for_current_packet()) { - const ouster::sensor::packet_format& pf = - ouster::sensor::packet_format(sensor_infos_[*sensor_idx]); - return pf.frame_id(current_data()); - } - return nonstd::nullopt; -} - -bool IndexedPcapReader::frame_id_rolled_over(uint16_t previous, - uint16_t current) { - static_assert(sizeof(uint16_t) == 2, "expected frame_id to be two bytes"); - return previous > 0xff00 && current < 0x00ff; -} - -int IndexedPcapReader::update_index_for_current_packet() { - if (nonstd::optional sensor_info_idx = - sensor_idx_for_current_packet()) { - if (nonstd::optional frame_id = current_frame_id()) { - if (!previous_frame_ids_[*sensor_info_idx] || - *previous_frame_ids_[*sensor_info_idx] < *frame_id || - frame_id_rolled_over(*previous_frame_ids_[*sensor_info_idx], - *frame_id)) { - index_.frame_indices_[*sensor_info_idx].push_back( - current_info().file_offset); - index_.frame_timestamp_indices_[*sensor_info_idx].insert( - {current_info().timestamp.count(), - current_info().file_offset}); - index_.frame_id_indices_[*sensor_info_idx].insert( - {*frame_id, current_info().file_offset}); - previous_frame_ids_[*sensor_info_idx] = *frame_id; - } - } - } - - return static_cast(100 * static_cast(current_offset()) / - file_size()); -} - -void IndexedPcapReader::build_index() { - index_.clear(); - reset(); - while (next_packet() != 0) update_index_for_current_packet(); - reset(); -} - -const PcapIndex& IndexedPcapReader::get_index() const { return index_; } - -void PcapIndex::clear() { - for (size_t i = 0; i < frame_indices_.size(); ++i) { - frame_indices_[i].clear(); - frame_timestamp_indices_[i].clear(); - frame_id_indices_[i].clear(); - } -} - -void PcapIndex::seek_to_frame(PcapReader& reader, size_t sensor_index, - unsigned int frame_number) { - reader.seek(frame_indices_.at(sensor_index).at(frame_number)); -} - -size_t PcapIndex::frame_count(size_t sensor_index) const { - return frame_indices_.at(sensor_index).size(); -} - -} // namespace sensor_utils -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_pcap/src/os_pcap.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_pcap/src/os_pcap.cpp deleted file mode 100644 index e090079d..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_pcap/src/os_pcap.cpp +++ /dev/null @@ -1,357 +0,0 @@ -/** - * Copyright (c) 2021, Ouster, Inc. - * All rights reserved. - * - */ - -#include "ouster/os_pcap.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "ouster/indexed_pcap_reader.h" -#include "ouster/pcap.h" - -namespace ouster { -namespace sensor_utils { - -// TODO: IndexedPcapReader bypasses playback_handle and record_handle, either -// use the same fencing mechansim or switch to use OOP style - -struct record_handle { - record_handle(const std::string& path, - PcapWriter::PacketEncapsulation encap, uint16_t frag_size) - : writer{std::make_unique(path, encap, frag_size)} {} - - std::unique_ptr writer; -}; - -struct playback_handle { - std::string path; - std::unique_ptr pcap; - - playback_handle(const std::string& path) - : path{path}, pcap{std::make_unique(path)} {} - - playback_handle& operator=(playback_handle&& other) = default; - - ~playback_handle() {} -}; - -std::ostream& operator<<(std::ostream& stream_in, const packet_info& data) { - stream_in << "Source IP: \"" << data.src_ip << "\" "; - stream_in << "Source Port: " << data.src_port << std::endl; - - stream_in << "Destination IP: \"" << data.dst_ip << "\" "; - stream_in << "Destination Port: " << data.dst_port << std::endl; - - stream_in << "Payload Size: " << data.payload_size << std::endl; - stream_in << "Timestamp: " << data.timestamp.count() << std::endl; - - stream_in << "Fragments In Packet: " << data.fragments_in_packet - << std::endl; - stream_in << "Encapsulation Protocol: " << data.encapsulation_protocol - << std::endl; - stream_in << "Network Protocol: " << data.network_protocol << std::endl; - - return stream_in; -} - -bool stream_key::operator==(const struct stream_key& other) const { - return dst_ip == other.dst_ip && src_ip == other.src_ip && - src_port == other.src_port && dst_port == other.dst_port; -} - -std::ostream& operator<<(std::ostream& stream_in, const stream_key& data) { - stream_in << "Source IP: \"" << data.src_ip << "\" " << std::endl; - stream_in << "Destination IP: \"" << data.dst_ip << "\" " << std::endl; - stream_in << "Source Port: " << data.src_port << std::endl; - stream_in << "Destination Port: " << data.dst_port << std::endl; - return stream_in; -} - -std::ostream& operator<<(std::ostream& stream_in, const stream_data& data) { - stream_in << "Count: " << data.count << " "; - stream_in << "Payload Sizes: " << std::endl; - for (auto const& it : data.payload_size_counts) { - stream_in << "Size: " << it.first << " Count: " << it.second - << std::endl; - } - - stream_in << "Fragments In Packets: " << std::endl; - for (auto const& it : data.fragment_counts) { - stream_in << "Number of Fragments: " << it.first - << " Count: " << it.second << std::endl; - } - - stream_in << "IP Versions: " << std::endl; - for (auto const& it : data.ip_version_counts) { - stream_in << "IP Version: " << it.first << " Count: " << it.second - << std::endl; - } - - return stream_in; -} - -std::ostream& operator<<(std::ostream& stream_in, const stream_info& data) { - stream_in << "Total Packets: " << data.total_packets << std::endl; - stream_in << "Encapsultion Protocol: " << data.encapsulation_protocol - << std::endl; - stream_in << "Max Timestamp: " << data.timestamp_max.count() << std::endl; - stream_in << "Min Timestamp: " << data.timestamp_min.count() << std::endl; - - for (auto it : data.udp_streams) { - stream_in << "Key: " << std::endl << it.first << std::endl; - stream_in << "Data: " << std::endl << it.second << std::endl; - stream_in << std::endl << std::endl << std::endl; - } - return stream_in; -} - -std::shared_ptr replay_initialize( - const std::string& file_path) { - return std::make_shared(file_path); -} - -void replay_uninitialize(playback_handle& handle) { handle.pcap.reset(); } - -void replay_reset(playback_handle& handle) { - handle = playback_handle{handle.path}; -} - -bool next_packet_info(playback_handle& handle, packet_info& info) { - if (handle.pcap && handle.pcap->next_packet()) { - info = handle.pcap->current_info(); - return true; - } - return false; -} - -size_t read_packet(playback_handle& handle, uint8_t* buf, size_t buffer_size) { - size_t len = handle.pcap->current_length(); - if (len <= buffer_size) { - std::memcpy(buf, handle.pcap->current_data(), len); - } else { - len = 0; - } - return len; -} - -std::shared_ptr record_initialize(const std::string& file_name, - int frag_size, - bool use_sll_encapsulation) { - PcapWriter::PacketEncapsulation encap = - (use_sll_encapsulation) ? PcapWriter::PacketEncapsulation::SLL - : PcapWriter::PacketEncapsulation::ETHERNET; - return std::make_shared(file_name, encap, frag_size); -} - -void record_uninitialize(record_handle& handle) { handle.writer.reset(); } - -void record_packet(record_handle& handle, const packet_info& info, - const uint8_t* buf, size_t buffer_size) { - handle.writer->write_packet(buf, buffer_size, info); -} - -void record_packet(record_handle& handle, const std::string& src_ip, - const std::string& dst_ip, int src_port, int dst_port, - const uint8_t* buf, size_t buffer_size, - uint64_t microsecond_timestamp) { - if (!handle.writer) return; - - auto time = packet_info::ts{microsecond_timestamp}; - handle.writer->write_packet(buf, buffer_size, src_ip, dst_ip, src_port, - dst_port, time); -} - -// TODO: make a member of `PcapReader` ? -std::shared_ptr get_stream_info( - PcapReader& pcap_reader, - std::function progress_callback, - int packets_per_callback, int packets_to_process) { - uint64_t fileSize = pcap_reader.file_size(); - std::shared_ptr result = std::make_shared(); - - int callback_count = 0; - uint64_t last_current = 0; - uint64_t diff_acc = 0; - - int i = 0; - packet_info info; - bool first = true; - uint64_t prev_location = 0; - - while (((packets_to_process <= 0) || (i < packets_to_process)) && - pcap_reader.next_packet()) { - info = pcap_reader.current_info(); - - // TODO: if `pcap_reader` is an IndexedPcapReader, - // get the `sensor_info` that matches the packet - // and the `packet_format` from the `sensor_info` - // and possibly use a `ScanBatcher` (one for each sensor stream) - // to determine if a scan boundary has been found - if (IndexedPcapReader* indexed_pcap_reader_ptr = - dynamic_cast(&pcap_reader)) { - indexed_pcap_reader_ptr->update_index_for_current_packet(); - } - - callback_count++; - if (first) { - first = false; - result->encapsulation_protocol = info.encapsulation_protocol; - result->timestamp_max = info.timestamp; - result->timestamp_min = info.timestamp; - } - result->total_packets++; - - if (info.timestamp < result->timestamp_min) - result->timestamp_min = info.timestamp; - if (info.timestamp > result->timestamp_max) - result->timestamp_max = info.timestamp; - - stream_key key; - - key.dst_ip = info.dst_ip; - key.src_ip = info.src_ip; - key.dst_port = info.dst_port; - key.src_port = info.src_port; - - auto& stream = result->udp_streams[key]; - stream.count++; - stream.payload_size_counts[info.payload_size]++; - stream.fragment_counts[info.fragments_in_packet]++; - stream.ip_version_counts[info.ip_version]++; - - diff_acc += (info.file_offset - prev_location); - last_current = info.file_offset; - - if (callback_count > packets_per_callback && - packets_per_callback >= 0) { - progress_callback(info.file_offset, diff_acc, fileSize); - diff_acc = 0; - callback_count = 0; - } - prev_location = info.file_offset; - i++; - } - if (diff_acc > 0 && packets_per_callback >= 0) { - progress_callback(last_current, diff_acc, fileSize); - } - pcap_reader.reset(); - - return result; -} - -std::shared_ptr get_stream_info( - const std::string& file, - std::function progress_callback, - int packets_per_callback, int packets_to_process) { - auto handle = replay_initialize(file); - if (handle) { - return get_stream_info(*(handle->pcap), progress_callback, - packets_per_callback, packets_to_process); - } - return std::make_shared(); -} - -std::shared_ptr get_stream_info(const std::string& file, - int packets_to_process) { - return get_stream_info( - file, [](uint64_t, uint64_t, uint64_t) {}, -1, packets_to_process); -} -/* - The current approach is roughly: 1) treat each unique source / - destination port and IP as a single logical 'stream' of data, 2) filter out - streams that don't match the expected packet sizes specified by the metadata, - 3) pair up any potential lidar/imu streams that appear to be coming from the - same sensor (have matching source IPs) 4) and finally, filter out the pairs - that contradict any ports specified in the metadata. -*/ -std::vector guess_ports(stream_info& info, - int lidar_packet_sizes, - int imu_packet_sizes, int lidar_spec, - int imu_spec) { - std::vector temp_result; - std::vector lidar_result; - std::vector imu_result; - std::vector result; - - std::vector lidar_keys; - std::vector imu_keys; - std::vector lidar_src_ips; - std::vector imu_src_ips; - - for (auto it : info.udp_streams) { - if (it.second.payload_size_counts.count(lidar_packet_sizes) > 0) { - lidar_keys.push_back(it.first); - lidar_src_ips.push_back(it.first.src_ip); - } - if (it.second.payload_size_counts.count(imu_packet_sizes) > 0) { - imu_keys.push_back(it.first); - imu_src_ips.push_back(it.first.src_ip); - } - } - - // Full join on lidar and IMU - for (auto lidar_it : lidar_keys) { - bool imu_processed = false; - for (auto imu_it : imu_keys) { - // This case runs for when we have both Lidar and IMU data - if (lidar_it.src_ip == imu_it.src_ip) { - guessed_ports ports; - ports.lidar = lidar_it.dst_port; - ports.imu = imu_it.dst_port; - imu_processed = true; - temp_result.push_back(ports); - } - // This case runs if we just have an IMU unmatched with the lidar - else if (std::find(lidar_src_ips.begin(), lidar_src_ips.end(), - imu_it.src_ip) == lidar_src_ips.end()) { - guessed_ports ports; - ports.lidar = 0; - ports.imu = imu_it.dst_port; - imu_result.push_back(ports); - } - } - // This case runs if we just have Lidar data - if (!imu_processed && std::find(imu_src_ips.begin(), imu_src_ips.end(), - lidar_it.src_ip) == imu_src_ips.end()) { - guessed_ports ports; - ports.lidar = lidar_it.dst_port; - ports.imu = 0; - lidar_result.push_back(ports); - } - } - // This case runs if we just have IMU data - if (lidar_keys.empty()) { - for (auto imu_it : imu_keys) { - guessed_ports ports; - ports.lidar = 0; - ports.imu = imu_it.dst_port; - imu_result.push_back(ports); - } - } - - temp_result.insert(temp_result.end(), lidar_result.begin(), - lidar_result.end()); - temp_result.insert(temp_result.end(), imu_result.begin(), imu_result.end()); - - for (auto it : temp_result) { - if (((it.lidar == lidar_spec) || (lidar_spec == 0) || - (it.lidar == 0)) && - ((it.imu == imu_spec) || (imu_spec == 0) || (it.imu == 0))) { - result.push_back(it); - } - } - return result; -} - -} // namespace sensor_utils -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_pcap/src/pcap.cpp b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_pcap/src/pcap.cpp deleted file mode 100644 index ffcf2477..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_pcap/src/pcap.cpp +++ /dev/null @@ -1,369 +0,0 @@ -/** - * Copyright (c) 2022, Ouster, Inc. - * All rights reserved. - * - * @todo check that the header casting is idiomatic libpcap - * @todo warn on dropped packets when pcap contains garbage, when fragments - * missing, buffer reused before sending - * @todo split up reading / playback - * @todo improve error reporting - */ - -#define _FILE_OFFSET_BITS 64 -#include "ouster/pcap.h" - -#if defined _WIN32 -#include -#define FTELL _ftelli64 -#define FSEEK _fseeki64 -#elif defined __EMSCRIPTEN__ -#define FTELL ftell -#define FSEEK fseek -#else -#include // inet_ntop -#include // timeval -#define FTELL ftello -#define FSEEK fseeko -#endif - -#include -#if defined _WIN32 -#include -#else -#include -#endif -#include -#include -#include - -#include -#include -#include -#include - -using us = std::chrono::microseconds; -using timepoint = std::chrono::system_clock::time_point; -using namespace Tins; - -static constexpr size_t UDP_BUF_SIZE = 65535; -static constexpr int PROTOCOL_UDP = 17; - -namespace ouster { -namespace sensor_utils { - -struct pcap_impl { - pcap_t* handle; - std::unique_ptr - pcap_reader; ///< Object that holds the unified pcap reader - FILE* pcap_reader_internals; - Tins::Packet packet_cache; - Tins::IPv4Reassembler - reassembler; ///< The reassembler mainly for lidar packets - bool have_new_packet; - int encap_proto; -}; - -struct pcap_writer_impl { - pcap_t* handle; - pcap_dumper* dumper; - std::unique_ptr - pcap_file_writer; ///< Object that holds the pcap writer -}; - -PcapReader::PcapReader(const std::string& file) : impl(new pcap_impl) { - std::ifstream fileSizeStream(file, std::ios::binary); - if (fileSizeStream) { - fileSizeStream.seekg(0, std::ios::end); - file_size_ = fileSizeStream.tellg(); - } - impl->pcap_reader = std::make_unique(file); - impl->encap_proto = impl->pcap_reader->link_type(); - impl->pcap_reader_internals = - pcap_file(impl->pcap_reader->get_pcap_handle()); - file_start_ = FTELL(impl->pcap_reader_internals); -} - -PcapReader::~PcapReader() {} - -const uint8_t* PcapReader::current_data() const { return data; } - -size_t PcapReader::current_length() const { return info.payload_size; } - -const packet_info& PcapReader::current_info() const { return info; } - -void PcapReader::seek(uint64_t offset) { - if (offset < sizeof(struct pcap_file_header)) { - offset = sizeof(struct pcap_file_header); - } - if (FSEEK(impl->pcap_reader_internals, offset, SEEK_SET)) { - throw std::runtime_error("pcap seek failed"); - } -} - -int64_t PcapReader::file_size() const { return file_size_; } - -int64_t PcapReader::current_offset() const { - int64_t ret = FTELL(impl->pcap_reader_internals); - - if (ret == -1L) { - fclose(impl->pcap_reader_internals); - throw std::runtime_error("ftell error: errno " + std::to_string(errno)); - } - return ret; -} - -void PcapReader::reset() { seek(file_start_); } - -size_t PcapReader::next_packet() { - size_t result = 0; - - bool reassm = false; - int reassm_packets = 0; - while (!reassm) { - reassm_packets++; - info.file_offset = current_offset(); - impl->packet_cache = impl->pcap_reader->next_packet(); - if (impl->packet_cache) { - auto pdu = impl->packet_cache.pdu(); - if (pdu) { - info.packet_size = pdu->size(); - IP* ip = pdu->find_pdu(); - IPv6* ipv6 = pdu->find_pdu(); - // Using short circuiting here - if ((ip && ip->protocol() == PROTOCOL_UDP) || - (ipv6 && ipv6->next_header() == PROTOCOL_UDP)) { - // reassm is also used in the while loop - reassm = (impl->reassembler.process(*pdu) != - IPv4Reassembler::FRAGMENTED); - if (reassm) { - info.fragments_in_packet = reassm_packets; - info.encapsulation_protocol = impl->encap_proto; - - if (ip) { - info.dst_ip = ip->dst_addr().to_string(); - info.src_ip = ip->src_addr().to_string(); - info.ip_version = 4; - info.network_protocol = ip->protocol(); - } else if (ipv6) { - info.dst_ip = ipv6->dst_addr().to_string(); - info.src_ip = ipv6->src_addr().to_string(); - info.ip_version = 6; - info.network_protocol = ipv6->next_header(); - } - - // find_pdu will only return NULL when the ipv4 - // reassembly succeeds on an ipv6 packet, leading to a - // malformed packet - UDP* udp = pdu->find_pdu(); - if (udp != nullptr) { - auto raw = pdu->find_pdu(); - info.dst_port = udp->dport(); - info.src_port = udp->sport(); - info.payload_size = raw->payload_size(); - result = info.payload_size; - data = (uint8_t*)&(raw->payload()[0]); - info.timestamp = impl->packet_cache.timestamp(); - impl->have_new_packet = true; - } else { - throw std::runtime_error( - "Malformed Packet: No UDP Detected"); - } - } - } - } - } else { - reassm = true; - } - } - - return result; -} - -PcapWriter::PcapWriter( - const std::string& file, - PcapWriter::PacketEncapsulation encap = PcapWriter::ETHERNET, - uint16_t frag_size = 1500) - : impl(new pcap_writer_impl), - id{0}, - encap(encap), - frag_size(frag_size), - closed(false) { - if (encap != PcapWriter::ETHERNET) { - impl->pcap_file_writer.reset( - new Tins::PacketWriter((file), Tins::DataLinkType())); - } else { - impl->pcap_file_writer.reset(new Tins::PacketWriter( - (file), Tins::DataLinkType())); - } -} - -void PcapWriter::flush() {} - -void PcapWriter::close() { - flush(); - closed = true; -} - -PcapWriter::~PcapWriter() { close(); } - -/* - * This was a tricky problem, due to how the ip stack is set up. - * - * SLL Container -> ipv4 container -> udp container -> raw data - * - * The ipv4 container is what does the packet fragmentation and reassembly. - * With each full packet we need the ipv4 reassembly to contain only one udp -header. -* Due to this, we have to only create one official Tins::UDP packet. -* We grab the packet id from this packet -* Every packet after the first Tins::UDP packet needs to just be a ipv4 -* container with a manually set packet type of UDP(dec 17) -* Every packet but the final packet needs to have the current flag set: -* -* pkt.flags(IP::MORE_FRAGMENTS); -* -*/ -size_t global_id = 1; -// SLL is the linux pcap capture container -std::vector buffer_to_frag_packets(size_t frag_size, - const std::string& src_ip, - const std::string& dst_ip, int src_port, - int dst_port, const uint8_t* buf, - size_t buf_size) { - /// @todo check fragsize to make sure it is in acceptable bounds - frag_size = UDP_BUF_SIZE; - - std::vector result; - - int id = -1; ///< This variable is used to track the packet id, - ///< if -1 then create a packet and grab its id - size_t i = 0; ///< Loop variable that contains current bytes processed - size_t offset_modifier = - 0; ///< This variable contains the offset to account - ///< for the udp packet with the fragment_offset - - while (i < buf_size) { - // First create the ipv4 packet - IP pkt = IP(dst_ip, src_ip); - - // Now figure out the size of the packet payload - size_t size = std::min(frag_size, (buf_size - i)); - - // Correctly set this packets fragment offset - // NOTE: for some reason this has to be divided by 8 - // NOTE: Reference here - // http://libtins.github.io/docs/latest/dd/d3f/classTins_1_1IP.html#a32a6bf84af274748317ef61ce1a91ce5 - pkt.fragment_offset((i + offset_modifier) / 8); - - // If this is the first packet - if (i == 0) { - // Fully create the libtins udp structure - auto udp = UDP(dst_port, src_port); - - if ((size + udp.header_size()) > frag_size) { - // Due to the udp header being included in the payload, - // we need to subtract its size from the payload - size -= udp.header_size(); - // Set the "There is more data to follow" flag - pkt.flags(IP::MORE_FRAGMENTS); - } - - // Pack what we can minus the udp header size into the payload - pkt /= udp / RawPDU((uint8_t*)(buf + i), size); - - // Manually set the ipv4 protocol to UDP - pkt.protocol(PROTOCOL_UDP); - - // Set the fragment_offset offset with the size of the udp header - offset_modifier = udp.header_size(); - } - // This is a packet in the middle or end - else { - // Set the "There is more data to follow" flag - if (i + size < buf_size) pkt.flags(IP::MORE_FRAGMENTS); - - // Manually set the ipv4 protocol to UDP - pkt.protocol(PROTOCOL_UDP); - - // Pack what we can into the payload - pkt /= RawPDU((uint8_t*)(buf + i), size); - } - - // Here is where we correctly set the packet id - if (id < 0) { - // If this is the first packet, set id to the generated packet id - id = global_id; - pkt.id(id); - global_id++; - } else { - // If this is a following packet, use the first packets id - pkt.id(id); - } - - // Add the resulting packet to the vector - result.push_back(pkt); - - // Increment the current byte being processed - i += size; - } - - return result; -} - -void PcapWriter::write_packet(const uint8_t* buf, size_t buf_size, - const std::string& src_ip, - const std::string& dst_ip, uint16_t src_port, - uint16_t dst_port, packet_info::ts timestamp) { - // ensure IPs were provided - if (dst_ip.empty() || src_ip.empty()) { - throw std::invalid_argument( - "PcapWriter: dst_ip and/or src_ip arguments to write_packet cannot " - "be empty."); - } - // For each of the packets write it to the pcap file - for (auto item : buffer_to_frag_packets(frag_size, src_ip, dst_ip, src_port, - dst_port, buf, buf_size)) { - Packet packet; - PDU* pdu; - switch (encap) { - case PcapWriter::PacketEncapsulation::ETHERNET: - pdu = new Tins::EthernetII(); - break; - case PcapWriter::PacketEncapsulation::SLL: - pdu = new Tins::SLL(); - break; - case PcapWriter::PacketEncapsulation::NULL_LOOPBACK: - throw std::runtime_error( - "PcapWriter: NULL_LOOPBACK packet encapsulation not " - "supported"); - break; - default: - throw std::runtime_error( - "PcapWriter: packet encapsulation not supported"); - } - *pdu /= item; - // Nasty libtins bug that causes write to fail - // https://www.gitmemory.com/issue/mfontanini/libtins/348/488141933 - auto _ = pdu->serialize(); - /* - * The next block is due to the fact that the previous serialize does - * not treat the udp packet as if it were decodable. Manually tell - * libtins to go in and serialize the udp packet as well - */ - if (pdu->inner_pdu()->inner_pdu()->inner_pdu() != NULL) { - _ = pdu->inner_pdu()->inner_pdu()->inner_pdu()->serialize(); - } - packet = Packet(*pdu, timestamp); - impl->pcap_file_writer->write(packet); - delete pdu; - } -} - -void PcapWriter::write_packet(const uint8_t* buf, size_t buf_size, - const packet_info& info) { - write_packet(buf, buf_size, info.src_ip, info.dst_ip, info.src_port, - info.dst_port, info.timestamp); -} - -} // namespace sensor_utils -} // namespace ouster diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_viz/CMakeLists.txt b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_viz/CMakeLists.txt deleted file mode 100644 index 4f9eff5a..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_viz/CMakeLists.txt +++ /dev/null @@ -1,46 +0,0 @@ -# ==== Requirements ==== -set(OpenGL_GL_PREFERENCE LEGACY) -find_package(OpenGL REQUIRED) -include(Coverage) - -# default to glad, if found. Note: this can be overridden from the command line -find_package(glad QUIET) -option(OUSTER_VIZ_USE_GLAD "Use GLAD instead of GLEW." ${glad_FOUND}) -if(OUSTER_VIZ_USE_GLAD) - message(STATUS "Configured GL loader: glad") - find_package(glad REQUIRED) - set(GL_LOADER glad::glad) - add_definitions("-DOUSTER_VIZ_USE_GLAD") -else() - message(STATUS "Configured GL loader: GLEW") - find_package(GLEW REQUIRED) - set(GL_LOADER GLEW::GLEW) -endif() - -find_package(glfw3 REQUIRED) -find_package(Eigen3 REQUIRED) - -# ==== Libraries ==== -# use only MPL-licensed parts of eigen -add_definitions(-DEIGEN_MPL2_ONLY) - -add_library(ouster_viz src/point_viz.cpp src/cloud.cpp src/camera.cpp src/image.cpp - src/gltext.cpp src/misc.cpp src/glfw.cpp) -target_link_libraries(ouster_viz - PRIVATE Eigen3::Eigen glfw ${GL_LOADER} OpenGL::GL ouster_client) -CodeCoverageFunctionality(ouster_viz) - -target_include_directories(ouster_viz PUBLIC - $ - $) -add_library(OusterSDK::ouster_viz ALIAS ouster_viz) - -# ==== Install ==== -install(TARGETS ouster_viz - EXPORT ouster-sdk-targets - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include) - -install(DIRECTORY include/ouster DESTINATION include) diff --git a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_viz/include/ouster/point_viz.h b/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_viz/include/ouster/point_viz.h deleted file mode 100644 index 320db0dc..00000000 --- a/src/third-party/ouster-ros/ouster-ros/ouster-sdk/ouster_viz/include/ouster/point_viz.h +++ /dev/null @@ -1,1136 +0,0 @@ -/** - * Copyright (c) 2020, Ouster, Inc. - * All rights reserved. - * - * @file - * @brief Point cloud and image visualizer for Ouster Lidar using OpenGL - */ -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -namespace ouster { -namespace viz { - -/** - * @todo document me - */ -using mat4d = std::array; - -/** - * @todo document me - */ -constexpr mat4d identity4d = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1}; - -using vec4f = std::array; -using vec3d = std::array; -using vec2d = std::array; - -// TODO: messes up lidar_scan_viz -namespace impl { -class GLCloud; -class GLImage; -class GLCuboid; -class GLLabel; -class GLRings; -struct CameraData; -} // namespace impl - -struct WindowCtx; -class Camera; -class Cloud; -class Image; -class Cuboid; -class Label; -class TargetDisplay; - -/** - * @todo document me - */ -constexpr int default_window_width = 800; - -/** - * @todo document me - */ -constexpr int default_window_height = 600; - -/** - * @brief A basic visualizer for sensor data - * - * Displays a set of point clouds, images, cuboids, and text labels with a few - * options for coloring and handling input. - * - * All operations are thread safe when running rendering (run() or run_once()) - * in a separate thread. This is the intended way to use the visualizer library - * when a nontrivial amount of processing needs to run concurrently with - * rendering (e.g. when streaming data from a running sensor). - */ -class PointViz { - public: - struct Impl; - - /** - * Creates a window and initializes the rendering context - * - * @param[in] name name of the visualizer, shown in the title bar - * @param[in] fix_aspect @todo document me - * @param[in] window_width @todo document me - * @param[in] window_height @todo document me - */ - PointViz(const std::string& name, bool fix_aspect = false, - int window_width = default_window_width, - int window_height = default_window_height); - - /** - * Tears down the rendering context and closes the viz window - */ - ~PointViz(); - - /** - * Main drawing loop, keeps drawing things until running(false) - * - * Should be called from the main thread for macos compatibility - */ - void run(); - - /** - * Run one iteration of the main loop for rendering and input handling - * - * Should be called from the main thread for macos compatibility - */ - void run_once(); - - /** - * Check if the run() has been signaled to exit - * - * @return true if the run() loop is currently executing - */ - bool running(); - - /** - * Set the running flag. Will signal run() to exit - * - * @param[in] state new value of the flag - */ - void running(bool state); - - /** - * Show or hide the visualizer window - * - * @param[in] state true to show - */ - void visible(bool state); - - /** - * Check if viz update_on_input state - * - * @return true if the viz will update on input - */ - bool update_on_input(); - - /** - * Set viz update_on_input flag. - * - * @param[in] state new value of the flag - */ - void update_on_input(bool state); - - /** - * Update visualization state - * - * Send state updates to be rendered on the next frame - * - * @return whether state was successfully sent. If not, will be sent on next - * call to update(). This can happen if update() is called more - * frequently than the frame rate. - */ - bool update(); - - /** - * Add a callback for handling keyboard input - * - * @param[in] f the callback. The second argument is the ascii value of the - * key pressed. Third argument is a bitmask of the modifier keys - */ - void push_key_handler(std::function&& f); - - /** - * Add a callback for handling mouse button input - * - * @param[in] f @todo document me - */ - void push_mouse_button_handler( - std::function&& f); - - /** - * Add a callback for handling mouse scrolling input - * - * @param[in] f @todo document me - */ - void push_scroll_handler( - std::function&& f); - - /** - * Add a callback for handling mouse movement - * - * @param[in] f @todo document me - */ - void push_mouse_pos_handler( - std::function&& f); - - /** - * Add a callback for processing every new draw frame buffer. - * - * NOTE: Any processing in the callback slows the renderer update loop - * dramatically. Primary use to store frame buffer images to disk - * for further processing. - * - * @param[in] f function callback of a form f(fb_data, fb_width, fb_height) - */ - void push_frame_buffer_handler( - std::function&, int, int)>&& f); - - /** - * Remove the last added callback for handling keyboard input - */ - void pop_key_handler(); - - /** - * @copydoc pop_key_handler() - */ - void pop_mouse_button_handler(); - - /** - * @copydoc pop_key_handler() - */ - void pop_scroll_handler(); - - /** - * @copydoc pop_key_handler() - */ - void pop_mouse_pos_handler(); - - /** - * @copydoc pop_key_handler() - */ - void pop_frame_buffer_handler(); - - /** - * Get a reference to the camera controls - * - * @return @todo document me - */ - Camera& camera(); - - /** - * Get a reference to the target display controls - * - * @return @todo document me - */ - TargetDisplay& target_display(); - - /** - * Add an object to the scene - * - * @param[in] cloud @todo document me - */ - void add(const std::shared_ptr& cloud); - - /** - * Add an object to the scene - * - * @param[in] image @todo document me - */ - void add(const std::shared_ptr& image); - - /** - * Add an object to the scene - * - * @param[in] cuboid @todo document me - */ - void add(const std::shared_ptr& cuboid); - - /** - * Add an object to the scene - * - * @param[in] label @todo document me - */ - void add(const std::shared_ptr