From a5e8ca9abdb1c917cc375c9f34ebcb37fd62cd23 Mon Sep 17 00:00:00 2001 From: pb Date: Wed, 7 Aug 2024 20:23:21 +0200 Subject: [PATCH] Extend and refactor log_point_cloud2 * log_point_cloud2 refactored to avoid redundant allocations * PointCloudProcessor and ColorMapper added * reversed colormap added * ColormapsLUT::supportedColormaps added * additional compilation flags added * rerun-sdk verison updated --- rerun_bridge/CMakeLists.txt | 10 +- .../include/rerun_bridge/color_mapper.hpp | 44 +++++ .../include/rerun_bridge/color_maps.hpp | 13 ++ .../rerun_bridge/point_cloud_processor.hpp | 46 +++++ .../rerun_bridge/rerun_ros_interface.hpp | 23 +-- .../src/rerun_bridge/color_mapper.cpp | 100 ++++++++++ rerun_bridge/src/rerun_bridge/color_maps.cpp | 84 +++++++++ .../rerun_bridge/point_cloud_processor.cpp | 111 +++++++++++ .../src/rerun_bridge/rerun_ros_interface.cpp | 172 ------------------ .../src/rerun_bridge/visualizer_node.cpp | 49 +++-- .../src/rerun_bridge/visualizer_node.hpp | 8 +- 11 files changed, 448 insertions(+), 212 deletions(-) create mode 100644 rerun_bridge/include/rerun_bridge/color_mapper.hpp create mode 100644 rerun_bridge/include/rerun_bridge/color_maps.hpp create mode 100644 rerun_bridge/include/rerun_bridge/point_cloud_processor.hpp create mode 100644 rerun_bridge/src/rerun_bridge/color_mapper.cpp create mode 100644 rerun_bridge/src/rerun_bridge/color_maps.cpp create mode 100644 rerun_bridge/src/rerun_bridge/point_cloud_processor.cpp diff --git a/rerun_bridge/CMakeLists.txt b/rerun_bridge/CMakeLists.txt index 29b620e..7013903 100644 --- a/rerun_bridge/CMakeLists.txt +++ b/rerun_bridge/CMakeLists.txt @@ -4,6 +4,7 @@ project(rerun_bridge) if(NOT DEFINED CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) endif() +add_compile_options(-Wall -Wextra -Wpedantic) # Avoid warning about CMP0135 if(CMAKE_VERSION VERSION_GREATER_EQUAL "3.24.0") @@ -22,11 +23,16 @@ find_package(OpenCV REQUIRED) find_package(yaml-cpp REQUIRED) include(FetchContent) -FetchContent_Declare(rerun_sdk URL https://github.com/rerun-io/rerun/releases/download/0.16.0/rerun_cpp_sdk.zip) +FetchContent_Declare(rerun_sdk URL https://github.com/rerun-io/rerun/releases/download/0.17.0/rerun_cpp_sdk.zip) FetchContent_MakeAvailable(rerun_sdk) # setup targets (has to be done before ament_package call) -add_library(${PROJECT_NAME} src/rerun_bridge/rerun_ros_interface.cpp) +add_library(${PROJECT_NAME} + src/rerun_bridge/rerun_ros_interface.cpp + src/rerun_bridge/color_maps.cpp + src/rerun_bridge/point_cloud_processor.cpp + src/rerun_bridge/color_mapper.cpp +) add_executable(visualizer src/rerun_bridge/visualizer_node.cpp) # add system dependencies diff --git a/rerun_bridge/include/rerun_bridge/color_mapper.hpp b/rerun_bridge/include/rerun_bridge/color_mapper.hpp new file mode 100644 index 0000000..f965283 --- /dev/null +++ b/rerun_bridge/include/rerun_bridge/color_mapper.hpp @@ -0,0 +1,44 @@ +#pragma once + +#include +#include +#include + +#include "sensor_msgs/point_cloud2_iterator.hpp" + +#include "rerun_bridge/color_maps.hpp" +#include "rerun_bridge/rerun_ros_interface.hpp" + +using pclConstIter = sensor_msgs::PointCloud2ConstIterator; + +struct PointCloud2Options { + std::optional colormapName; + std::optional colormapField; + std::optional colormapMin; + std::optional colormapMax; +}; + +class ColorMapper { + std::optional colormap_{}; + std::vector fieldValues_{}; + std::vector colors_{}; + + std::pair getMinMax(const std::vector& values) const; + std::size_t calculateIdx(const float& value, const float& minValue, const float& maxValue) + const; + void setColormap(); + std::string toUppercase(std::string str) const; + void convertPclMsgToColorapFieldVec(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg); + void remapValuesToColors(); + + public: + // data from yaml file + const PointCloud2Options options_{}; + + ColorMapper(const PointCloud2Options& options); + void calculateRerunColors(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg); + bool isValid() const; + void reserve(const size_t& size); + void clear(); + const std::vector& getColors() const; +}; diff --git a/rerun_bridge/include/rerun_bridge/color_maps.hpp b/rerun_bridge/include/rerun_bridge/color_maps.hpp new file mode 100644 index 0000000..35ee5b0 --- /dev/null +++ b/rerun_bridge/include/rerun_bridge/color_maps.hpp @@ -0,0 +1,13 @@ +#pragma once + +#include +#include + +using colormapLUT = std::array, 256>; + +namespace ColormapsLUT { + extern const std::unordered_map supportedColormaps; + + extern const colormapLUT Turbo; + extern const colormapLUT Rainbow; +} // namespace ColormapsLUT diff --git a/rerun_bridge/include/rerun_bridge/point_cloud_processor.hpp b/rerun_bridge/include/rerun_bridge/point_cloud_processor.hpp new file mode 100644 index 0000000..f1fab57 --- /dev/null +++ b/rerun_bridge/include/rerun_bridge/point_cloud_processor.hpp @@ -0,0 +1,46 @@ +#pragma once + +#include +#include +#include + +#include "rerun.hpp" +#include "rerun_bridge/color_mapper.hpp" // PointCloud2Options +#include "rerun_bridge/rerun_ros_interface.hpp" + +class PointCloudProcessor { + ColorMapper colorMapper_; + std::size_t maxNumPerMsg_{}; + std::vector points_{}; + + std::pair, std::optional> getMinMax(const std::vector& values + ) const; + + void reserve(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg); + bool isMsgLonger(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg) const; + const std::vector& convertPclMsgToPosition3DVec( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg + ); + void convertPclMsgToColorMapFieldVec( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg, std::vector& values + ) const; + void convertPclMsgToPosition3DVecIter( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg, + std::vector& points + ) const; + bool areFieldNamesSupported( + const rerun::RecordingStream& rec, const std::string& entity_path, + const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg + ) const; + bool isFieldTypeFloat( + const rerun::RecordingStream& rec, const std::string& entity_path, + const sensor_msgs::msg::PointField& field + ) const; + + public: + PointCloudProcessor(const PointCloud2Options& options); + void logPointCloud2( + const rerun::RecordingStream& rec, const std::string& entity_path, + const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg + ); +}; \ No newline at end of file diff --git a/rerun_bridge/include/rerun_bridge/rerun_ros_interface.hpp b/rerun_bridge/include/rerun_bridge/rerun_ros_interface.hpp index 850d775..f384cf6 100644 --- a/rerun_bridge/include/rerun_bridge/rerun_ros_interface.hpp +++ b/rerun_bridge/include/rerun_bridge/rerun_ros_interface.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include @@ -14,16 +15,16 @@ #include +struct ImageOptions { + std::optional min_depth{}; + std::optional max_depth{}; +}; + void log_imu( const rerun::RecordingStream& rec, const std::string& entity_path, const sensor_msgs::msg::Imu::ConstSharedPtr& msg ); -struct ImageOptions { - std::optional min_depth; - std::optional max_depth; -}; - void log_image( const rerun::RecordingStream& rec, const std::string& entity_path, const sensor_msgs::msg::Image::ConstSharedPtr& msg, const ImageOptions& options = ImageOptions{} @@ -54,15 +55,3 @@ void log_transform( const rerun::RecordingStream& rec, const std::string& entity_path, const geometry_msgs::msg::TransformStamped::ConstSharedPtr& msg ); - -struct PointCloud2Options { - std::optional colormap; - std::optional colormap_field; - std::optional colormap_min; - std::optional colormap_max; -}; - -void log_point_cloud2( - const rerun::RecordingStream& rec, const std::string& entity_path, - const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg, const PointCloud2Options& options -); diff --git a/rerun_bridge/src/rerun_bridge/color_mapper.cpp b/rerun_bridge/src/rerun_bridge/color_mapper.cpp new file mode 100644 index 0000000..cd6083c --- /dev/null +++ b/rerun_bridge/src/rerun_bridge/color_mapper.cpp @@ -0,0 +1,100 @@ +#include +#include // std::toupper +#include +#include +#include + +#include "rerun_bridge/color_mapper.hpp" +#include "rerun_bridge/color_maps.hpp" + +ColorMapper::ColorMapper(const PointCloud2Options& options) : options_{options} { + setColormap(); +} + +bool ColorMapper::isValid() const { + return (colormap_.has_value() && options_.colormapField.has_value()); +} + +void ColorMapper::calculateRerunColors(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg) { + convertPclMsgToColorapFieldVec(msg); + remapValuesToColors(); +} + +std::size_t ColorMapper::calculateIdx( + const float& value, const float& minValue, const float& maxValue +) const { + return static_cast(255 * (value - minValue) / (maxValue - minValue)); +} + +std::pair ColorMapper::getMinMax(const std::vector& values) const { + float min_value{0}, max_value{0}; + if (!options_.colormapMin) { + min_value = *std::min_element(values.begin(), values.end()); + } else { + min_value = *options_.colormapMin; + } + + if (!options_.colormapMax) { + max_value = *std::max_element(values.begin(), values.end()); + } else { + max_value = *options_.colormapMax; + } + return {min_value, max_value}; +} + +void ColorMapper::reserve(const size_t& size) { + colors_.reserve(size); + fieldValues_.reserve(size); +} + +void ColorMapper::clear() { + colors_.clear(); + fieldValues_.clear(); +} + +std::string ColorMapper::toUppercase(std::string str) const { + std::transform(str.begin(), str.end(), str.begin(), [](unsigned char c) { + return std::toupper(c); + }); + return str; +} + +void ColorMapper::setColormap() { + if (!options_.colormapName.has_value()) { + return; + } + + // name from YAML file + auto colormapName = toUppercase(*options_.colormapName); + auto it = ColormapsLUT::supportedColormaps.find(colormapName); + if (it != ColormapsLUT::supportedColormaps.end()) { + colormap_ = it->second; + } else { + std::cout << "Colormap: " << colormapName << " is not supported" << std::endl; + } +} + +void ColorMapper::convertPclMsgToColorapFieldVec( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg +) { + auto iterColorField = pclConstIter(*msg, options_.colormapField.value()); + for (; iterColorField != iterColorField.end(); ++iterColorField) { + fieldValues_.emplace_back(*iterColorField); + } +} + +void ColorMapper::remapValuesToColors() { + if (!options_.colormapName || !colormap_) { + return; + } + + const auto [min, max] = getMinMax(fieldValues_); + for (const auto& value : fieldValues_) { + const auto idx = calculateIdx(value, min, max); + colors_.emplace_back((*colormap_)[idx][0], (*colormap_)[idx][1], (*colormap_)[idx][2]); + } +} + +const std::vector& ColorMapper::getColors() const { + return colors_; +} \ No newline at end of file diff --git a/rerun_bridge/src/rerun_bridge/color_maps.cpp b/rerun_bridge/src/rerun_bridge/color_maps.cpp new file mode 100644 index 0000000..a593fe4 --- /dev/null +++ b/rerun_bridge/src/rerun_bridge/color_maps.cpp @@ -0,0 +1,84 @@ +#include +#include +#include +#include + +#include "rerun_bridge/color_maps.hpp" + +constexpr colormapLUT reverse(const colormapLUT& input) { + colormapLUT reversed{}; + constexpr std::size_t N = std::tuple_size::value; + for (std::size_t i = 0; i < N; ++i) { + reversed[i] = input[N - 1 - i]; + } + return reversed; +} + +namespace ColormapsLUT { + const std::unordered_map supportedColormaps = { + {"TURBO", Turbo}, {"RAINBOW", Rainbow}}; + + // Turbo colormap lookup table + // from: https://gist.github.com/mikhailov-work/6a308c20e494d9e0ccc29036b28faa7a + // Copyright 2019 Google LLC. + // SPDX-License-Identifier: Apache-2.0 + // + // Author: Anton Mikhailov + + constexpr colormapLUT Turbo = { + {{48, 18, 59}, {50, 21, 67}, {51, 24, 74}, {52, 27, 81}, {53, 30, 88}, + {54, 33, 95}, {55, 36, 102}, {56, 39, 109}, {57, 42, 115}, {58, 45, 121}, + {59, 47, 128}, {60, 50, 134}, {61, 53, 139}, {62, 56, 145}, {63, 59, 151}, + {63, 62, 156}, {64, 64, 162}, {65, 67, 167}, {65, 70, 172}, {66, 73, 177}, + {66, 75, 181}, {67, 78, 186}, {68, 81, 191}, {68, 84, 195}, {68, 86, 199}, + {69, 89, 203}, {69, 92, 207}, {69, 94, 211}, {70, 97, 214}, {70, 100, 218}, + {70, 102, 221}, {70, 105, 224}, {70, 107, 227}, {71, 110, 230}, {71, 113, 233}, + {71, 115, 235}, {71, 118, 238}, {71, 120, 240}, {71, 123, 242}, {70, 125, 244}, + {70, 128, 246}, {70, 130, 248}, {70, 133, 250}, {70, 135, 251}, {69, 138, 252}, + {69, 140, 253}, {68, 143, 254}, {67, 145, 254}, {66, 148, 255}, {65, 150, 255}, + {64, 153, 255}, {62, 155, 254}, {61, 158, 254}, {59, 160, 253}, {58, 163, 252}, + {56, 165, 251}, {55, 168, 250}, {53, 171, 248}, {51, 173, 247}, {49, 175, 245}, + {47, 178, 244}, {46, 180, 242}, {44, 183, 240}, {42, 185, 238}, {40, 188, 235}, + {39, 190, 233}, {37, 192, 231}, {35, 195, 228}, {34, 197, 226}, {32, 199, 223}, + {31, 201, 221}, {30, 203, 218}, {28, 205, 216}, {27, 208, 213}, {26, 210, 210}, + {26, 212, 208}, {25, 213, 205}, {24, 215, 202}, {24, 217, 200}, {24, 219, 197}, + {24, 221, 194}, {24, 222, 192}, {24, 224, 189}, {25, 226, 187}, {25, 227, 185}, + {26, 228, 182}, {28, 230, 180}, {29, 231, 178}, {31, 233, 175}, {32, 234, 172}, + {34, 235, 170}, {37, 236, 167}, {39, 238, 164}, {42, 239, 161}, {44, 240, 158}, + {47, 241, 155}, {50, 242, 152}, {53, 243, 148}, {56, 244, 145}, {60, 245, 142}, + {63, 246, 138}, {67, 247, 135}, {70, 248, 132}, {74, 248, 128}, {78, 249, 125}, + {82, 250, 122}, {85, 250, 118}, {89, 251, 115}, {93, 252, 111}, {97, 252, 108}, + {101, 253, 105}, {105, 253, 102}, {109, 254, 98}, {113, 254, 95}, {117, 254, 92}, + {121, 254, 89}, {125, 255, 86}, {128, 255, 83}, {132, 255, 81}, {136, 255, 78}, + {139, 255, 75}, {143, 255, 73}, {146, 255, 71}, {150, 254, 68}, {153, 254, 66}, + {156, 254, 64}, {159, 253, 63}, {161, 253, 61}, {164, 252, 60}, {167, 252, 58}, + {169, 251, 57}, {172, 251, 56}, {175, 250, 55}, {177, 249, 54}, {180, 248, 54}, + {183, 247, 53}, {185, 246, 53}, {188, 245, 52}, {190, 244, 52}, {193, 243, 52}, + {195, 241, 52}, {198, 240, 52}, {200, 239, 52}, {203, 237, 52}, {205, 236, 52}, + {208, 234, 52}, {210, 233, 53}, {212, 231, 53}, {215, 229, 53}, {217, 228, 54}, + {219, 226, 54}, {221, 224, 55}, {223, 223, 55}, {225, 221, 55}, {227, 219, 56}, + {229, 217, 56}, {231, 215, 57}, {233, 213, 57}, {235, 211, 57}, {236, 209, 58}, + {238, 207, 58}, {239, 205, 58}, {241, 203, 58}, {242, 201, 58}, {244, 199, 58}, + {245, 197, 58}, {246, 195, 58}, {247, 193, 58}, {248, 190, 57}, {249, 188, 57}, + {250, 186, 57}, {251, 184, 56}, {251, 182, 55}, {252, 179, 54}, {252, 177, 54}, + {253, 174, 53}, {253, 172, 52}, {254, 169, 51}, {254, 167, 50}, {254, 164, 49}, + {254, 161, 48}, {254, 158, 47}, {254, 155, 45}, {254, 153, 44}, {254, 150, 43}, + {254, 147, 42}, {254, 144, 41}, {253, 141, 39}, {253, 138, 38}, {252, 135, 37}, + {252, 132, 35}, {251, 129, 34}, {251, 126, 33}, {250, 123, 31}, {249, 120, 30}, + {249, 117, 29}, {248, 114, 28}, {247, 111, 26}, {246, 108, 25}, {245, 105, 24}, + {244, 102, 23}, {243, 99, 21}, {242, 96, 20}, {241, 93, 19}, {240, 91, 18}, + {239, 88, 17}, {237, 85, 16}, {236, 83, 15}, {235, 80, 14}, {234, 78, 13}, + {232, 75, 12}, {231, 73, 12}, {229, 71, 11}, {228, 69, 10}, {226, 67, 10}, + {225, 65, 9}, {223, 63, 8}, {221, 61, 8}, {220, 59, 7}, {218, 57, 7}, + {216, 55, 6}, {214, 53, 6}, {212, 51, 5}, {210, 49, 5}, {208, 47, 5}, + {206, 45, 4}, {204, 43, 4}, {202, 42, 4}, {200, 40, 3}, {197, 38, 3}, + {195, 37, 3}, {193, 35, 2}, {190, 33, 2}, {188, 32, 2}, {185, 30, 2}, + {183, 29, 2}, {180, 27, 1}, {178, 26, 1}, {175, 24, 1}, {172, 23, 1}, + {169, 22, 1}, {167, 20, 1}, {164, 19, 1}, {161, 18, 1}, {158, 16, 1}, + {155, 15, 1}, {152, 14, 1}, {149, 13, 1}, {146, 11, 1}, {142, 10, 1}, + {139, 9, 2}, {136, 8, 2}, {133, 7, 2}, {129, 6, 2}, {126, 5, 2}, + {122, 4, 3}}}; + + constexpr colormapLUT Rainbow = reverse(Turbo); + +} // namespace ColormapsLUT diff --git a/rerun_bridge/src/rerun_bridge/point_cloud_processor.cpp b/rerun_bridge/src/rerun_bridge/point_cloud_processor.cpp new file mode 100644 index 0000000..31a28bb --- /dev/null +++ b/rerun_bridge/src/rerun_bridge/point_cloud_processor.cpp @@ -0,0 +1,111 @@ +#include "rerun_bridge/point_cloud_processor.hpp" +#include "rerun_bridge/color_mapper.hpp" + +#include "rclcpp/rclcpp.hpp" + +PointCloudProcessor::PointCloudProcessor(const PointCloud2Options& options) + : colorMapper_(options) {} + +bool PointCloudProcessor::isMsgLonger(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg +) const { + return (msg->width * msg->height) > maxNumPerMsg_; +} + +void PointCloudProcessor::reserve(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg) { + if (isMsgLonger(msg)) { + const auto size = msg->width * msg->height; + points_.reserve(size); + colorMapper_.reserve(size); + maxNumPerMsg_ = size; + } + points_.clear(); + colorMapper_.clear(); +} + +const std::vector& PointCloudProcessor::convertPclMsgToPosition3DVec( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg +) { + // TODO(leo) if xyz are consecutive fields we can do this in a single memcpy + auto iterX = pclConstIter(*msg, "x"); + auto iterY = pclConstIter(*msg, "y"); + auto iterZ = pclConstIter(*msg, "z"); + for (; iterX != iterX.end(); ++iterX, ++iterY, ++iterZ) { + points_.emplace_back(*iterX, *iterY, *iterZ); + } + return points_; +} + +bool PointCloudProcessor::isFieldTypeFloat( + const rerun::RecordingStream& rec, const std::string& entity_path, + const sensor_msgs::msg::PointField& field +) const { + auto result = (field.datatype == sensor_msgs::msg::PointField::FLOAT32); + if (!result) { + rec.log(entity_path, rerun::TextLog("Only FLOAT32 " + field.name + " field supported")); + } + return result; +} + +bool PointCloudProcessor::areFieldNamesSupported( + const rerun::RecordingStream& rec, const std::string& entity_path, + const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg +) const { + // TODO(leo) should match the behavior described here + // https://wiki.ros.org/rviz/DisplayTypes/PointCloud + // TODO(leo) if not specified, check if 2D points or 3D points + + bool hasXfield{false}, hasYfield{false}, hasZfield{false}; + + for (const auto& field : msg->fields) { + if (field.name == "x") { + hasXfield = true; + if (!isFieldTypeFloat(rec, entity_path, field)) { + return false; + } + } else if (field.name == "y") { + hasYfield = true; + if (!isFieldTypeFloat(rec, entity_path, field)) { + return false; + } + } else if (field.name == "z") { + hasZfield = true; + if (!isFieldTypeFloat(rec, entity_path, field)) { + return false; + } + } + } + return (hasXfield && hasYfield && hasZfield); +} + +void PointCloudProcessor::logPointCloud2( + const rerun::RecordingStream& rec, const std::string& entity_path, + const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg +) { + reserve(msg); + + rec.set_time_seconds( + "timestamp", + rclcpp::Time(msg->header.stamp.sec, msg->header.stamp.nanosec).seconds() + ); + + if (!areFieldNamesSupported(rec, entity_path, msg)) { + rec.log( + entity_path, + rerun::TextLog("Currently only PointCloud2 messages with x, y, z fields are supported") + ); + return; + } + + const auto& points = convertPclMsgToPosition3DVec(msg); + if (colorMapper_.isValid()) { + colorMapper_.calculateRerunColors(msg); + } else if (colorMapper_.options_.colormapName) { + rec.log( + "/", + rerun::TextLog( + "Unsupported colormap specified: " + colorMapper_.options_.colormapName.value() + ) + ); + } + rec.log(entity_path, rerun::Points3D(points).with_colors(colorMapper_.getColors())); +} diff --git a/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp b/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp index a285286..27cc230 100644 --- a/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp +++ b/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp @@ -5,88 +5,6 @@ #include #include -// Turbo colormap lookup table -// from: https://gist.github.com/mikhailov-work/6a308c20e494d9e0ccc29036b28faa7a -// Copyright 2019 Google LLC. -// SPDX-License-Identifier: Apache-2.0 -// -// Author: Anton Mikhailov -constexpr float TurboBytes[256][3] = { - {48, 18, 59}, {50, 21, 67}, {51, 24, 74}, {52, 27, 81}, {53, 30, 88}, - {54, 33, 95}, {55, 36, 102}, {56, 39, 109}, {57, 42, 115}, {58, 45, 121}, - {59, 47, 128}, {60, 50, 134}, {61, 53, 139}, {62, 56, 145}, {63, 59, 151}, - {63, 62, 156}, {64, 64, 162}, {65, 67, 167}, {65, 70, 172}, {66, 73, 177}, - {66, 75, 181}, {67, 78, 186}, {68, 81, 191}, {68, 84, 195}, {68, 86, 199}, - {69, 89, 203}, {69, 92, 207}, {69, 94, 211}, {70, 97, 214}, {70, 100, 218}, - {70, 102, 221}, {70, 105, 224}, {70, 107, 227}, {71, 110, 230}, {71, 113, 233}, - {71, 115, 235}, {71, 118, 238}, {71, 120, 240}, {71, 123, 242}, {70, 125, 244}, - {70, 128, 246}, {70, 130, 248}, {70, 133, 250}, {70, 135, 251}, {69, 138, 252}, - {69, 140, 253}, {68, 143, 254}, {67, 145, 254}, {66, 148, 255}, {65, 150, 255}, - {64, 153, 255}, {62, 155, 254}, {61, 158, 254}, {59, 160, 253}, {58, 163, 252}, - {56, 165, 251}, {55, 168, 250}, {53, 171, 248}, {51, 173, 247}, {49, 175, 245}, - {47, 178, 244}, {46, 180, 242}, {44, 183, 240}, {42, 185, 238}, {40, 188, 235}, - {39, 190, 233}, {37, 192, 231}, {35, 195, 228}, {34, 197, 226}, {32, 199, 223}, - {31, 201, 221}, {30, 203, 218}, {28, 205, 216}, {27, 208, 213}, {26, 210, 210}, - {26, 212, 208}, {25, 213, 205}, {24, 215, 202}, {24, 217, 200}, {24, 219, 197}, - {24, 221, 194}, {24, 222, 192}, {24, 224, 189}, {25, 226, 187}, {25, 227, 185}, - {26, 228, 182}, {28, 230, 180}, {29, 231, 178}, {31, 233, 175}, {32, 234, 172}, - {34, 235, 170}, {37, 236, 167}, {39, 238, 164}, {42, 239, 161}, {44, 240, 158}, - {47, 241, 155}, {50, 242, 152}, {53, 243, 148}, {56, 244, 145}, {60, 245, 142}, - {63, 246, 138}, {67, 247, 135}, {70, 248, 132}, {74, 248, 128}, {78, 249, 125}, - {82, 250, 122}, {85, 250, 118}, {89, 251, 115}, {93, 252, 111}, {97, 252, 108}, - {101, 253, 105}, {105, 253, 102}, {109, 254, 98}, {113, 254, 95}, {117, 254, 92}, - {121, 254, 89}, {125, 255, 86}, {128, 255, 83}, {132, 255, 81}, {136, 255, 78}, - {139, 255, 75}, {143, 255, 73}, {146, 255, 71}, {150, 254, 68}, {153, 254, 66}, - {156, 254, 64}, {159, 253, 63}, {161, 253, 61}, {164, 252, 60}, {167, 252, 58}, - {169, 251, 57}, {172, 251, 56}, {175, 250, 55}, {177, 249, 54}, {180, 248, 54}, - {183, 247, 53}, {185, 246, 53}, {188, 245, 52}, {190, 244, 52}, {193, 243, 52}, - {195, 241, 52}, {198, 240, 52}, {200, 239, 52}, {203, 237, 52}, {205, 236, 52}, - {208, 234, 52}, {210, 233, 53}, {212, 231, 53}, {215, 229, 53}, {217, 228, 54}, - {219, 226, 54}, {221, 224, 55}, {223, 223, 55}, {225, 221, 55}, {227, 219, 56}, - {229, 217, 56}, {231, 215, 57}, {233, 213, 57}, {235, 211, 57}, {236, 209, 58}, - {238, 207, 58}, {239, 205, 58}, {241, 203, 58}, {242, 201, 58}, {244, 199, 58}, - {245, 197, 58}, {246, 195, 58}, {247, 193, 58}, {248, 190, 57}, {249, 188, 57}, - {250, 186, 57}, {251, 184, 56}, {251, 182, 55}, {252, 179, 54}, {252, 177, 54}, - {253, 174, 53}, {253, 172, 52}, {254, 169, 51}, {254, 167, 50}, {254, 164, 49}, - {254, 161, 48}, {254, 158, 47}, {254, 155, 45}, {254, 153, 44}, {254, 150, 43}, - {254, 147, 42}, {254, 144, 41}, {253, 141, 39}, {253, 138, 38}, {252, 135, 37}, - {252, 132, 35}, {251, 129, 34}, {251, 126, 33}, {250, 123, 31}, {249, 120, 30}, - {249, 117, 29}, {248, 114, 28}, {247, 111, 26}, {246, 108, 25}, {245, 105, 24}, - {244, 102, 23}, {243, 99, 21}, {242, 96, 20}, {241, 93, 19}, {240, 91, 18}, - {239, 88, 17}, {237, 85, 16}, {236, 83, 15}, {235, 80, 14}, {234, 78, 13}, - {232, 75, 12}, {231, 73, 12}, {229, 71, 11}, {228, 69, 10}, {226, 67, 10}, - {225, 65, 9}, {223, 63, 8}, {221, 61, 8}, {220, 59, 7}, {218, 57, 7}, - {216, 55, 6}, {214, 53, 6}, {212, 51, 5}, {210, 49, 5}, {208, 47, 5}, - {206, 45, 4}, {204, 43, 4}, {202, 42, 4}, {200, 40, 3}, {197, 38, 3}, - {195, 37, 3}, {193, 35, 2}, {190, 33, 2}, {188, 32, 2}, {185, 30, 2}, - {183, 29, 2}, {180, 27, 1}, {178, 26, 1}, {175, 24, 1}, {172, 23, 1}, - {169, 22, 1}, {167, 20, 1}, {164, 19, 1}, {161, 18, 1}, {158, 16, 1}, - {155, 15, 1}, {152, 14, 1}, {149, 13, 1}, {146, 11, 1}, {142, 10, 1}, - {139, 9, 2}, {136, 8, 2}, {133, 7, 2}, {129, 6, 2}, {126, 5, 2}, - {122, 4, 3}}; - -std::vector colormap( - const std::vector& values, std::optional min_value, std::optional max_value -) { - if (!min_value) { - min_value = *std::min_element(values.begin(), values.end()); - } - if (!max_value) { - max_value = *std::max_element(values.begin(), values.end()); - } - - std::vector colors; - - for (const auto& value : values) { - auto idx = static_cast( - 255 * (value - min_value.value()) / (max_value.value() - min_value.value()) - ); - colors.emplace_back(rerun::Color(TurboBytes[idx][0], TurboBytes[idx][1], TurboBytes[idx][2]) - ); - } - return colors; -} - void log_imu( const rerun::RecordingStream& rec, const std::string& entity_path, const sensor_msgs::msg::Imu::ConstSharedPtr& msg @@ -292,93 +210,3 @@ void log_transform( ) ); } - -void log_point_cloud2( - const rerun::RecordingStream& rec, const std::string& entity_path, - const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg, const PointCloud2Options& options -) { - rec.set_time_seconds( - "timestamp", - rclcpp::Time(msg->header.stamp.sec, msg->header.stamp.nanosec).seconds() - ); - - // TODO(leo) should match the behavior described here - // https://wiki.ros.org/rviz/DisplayTypes/PointCloud - // TODO(leo) if not specified, check if 2D points or 3D points - // TODO(leo) allow arbitrary color mapping - - size_t x_offset, y_offset, z_offset; - bool has_x{false}, has_y{false}, has_z{false}; - - for (const auto& field : msg->fields) { - if (field.name == "x") { - x_offset = field.offset; - if (field.datatype != sensor_msgs::msg::PointField::FLOAT32) { - rec.log(entity_path, rerun::TextLog("Only FLOAT32 x field supported")); - return; - } - has_x = true; - } else if (field.name == "y") { - y_offset = field.offset; - if (field.datatype != sensor_msgs::msg::PointField::FLOAT32) { - rec.log(entity_path, rerun::TextLog("Only FLOAT32 y field supported")); - return; - } - has_y = true; - } else if (field.name == "z") { - z_offset = field.offset; - if (field.datatype != sensor_msgs::msg::PointField::FLOAT32) { - rec.log(entity_path, rerun::TextLog("Only FLOAT32 z field supported")); - return; - } - has_z = true; - } - } - - if (!has_x || !has_y || !has_z) { - rec.log( - entity_path, - rerun::TextLog("Currently only PointCloud2 messages with x, y, z fields are supported") - ); - return; - } - - std::vector points(msg->width * msg->height); - std::vector colors; - - for (size_t i = 0; i < msg->height; ++i) { - for (size_t j = 0; j < msg->width; ++j) { - auto point_offset = i * msg->row_step + j * msg->point_step; - rerun::Position3D position; - // TODO(leo) if xyz are consecutive fields we can do this in a single memcpy - std::memcpy(&position.xyz.xyz[0], &msg->data[point_offset + x_offset], sizeof(float)); - std::memcpy(&position.xyz.xyz[1], &msg->data[point_offset + y_offset], sizeof(float)); - std::memcpy(&position.xyz.xyz[2], &msg->data[point_offset + z_offset], sizeof(float)); - points.emplace_back(std::move(position)); - } - } - - if (options.colormap == "turbo") { - std::vector values(msg->width * msg->height); - auto& colormap_field = - *std::find_if(msg->fields.begin(), msg->fields.end(), [&](const auto& field) { - return field.name == options.colormap_field; - }); - for (size_t i = 0; i < msg->height; ++i) { - for (size_t j = 0; j < msg->width; ++j) { - float value; - std::memcpy( - &value, - &msg->data[i * msg->row_step + j * msg->point_step + colormap_field.offset], - sizeof(float) - ); - values.emplace_back(value); - } - } - colors = colormap(values, options.colormap_min, options.colormap_max); - } else if (options.colormap) { - rec.log("/", rerun::TextLog("Unsupported colormap specified: " + options.colormap.value())); - } - - rec.log(entity_path, rerun::Points3D(points).with_colors(colors)); -} diff --git a/rerun_bridge/src/rerun_bridge/visualizer_node.cpp b/rerun_bridge/src/rerun_bridge/visualizer_node.cpp index 412b1af..e9a4b68 100644 --- a/rerun_bridge/src/rerun_bridge/visualizer_node.cpp +++ b/rerun_bridge/src/rerun_bridge/visualizer_node.cpp @@ -1,4 +1,5 @@ #include "visualizer_node.hpp" +#include "rerun_bridge/point_cloud_processor.hpp" #include "rerun_bridge/rerun_ros_interface.hpp" #include @@ -22,18 +23,18 @@ ImageOptions image_options_from_topic_options(const TopicOptions& topic_options) } PointCloud2Options point_cloud2_options_from_topic_options(const TopicOptions& topic_options) { - PointCloud2Options options; + PointCloud2Options options{}; if (topic_options.find("colormap") != topic_options.end()) { - options.colormap = topic_options.at("colormap").as(); + options.colormapName = topic_options.at("colormap").as(); } if (topic_options.find("colormap_field") != topic_options.end()) { - options.colormap_field = topic_options.at("colormap_field").as(); + options.colormapField = topic_options.at("colormap_field").as(); } if (topic_options.find("colormap_min") != topic_options.end()) { - options.colormap_min = topic_options.at("colormap_min").as(); + options.colormapMin = topic_options.at("colormap_min").as(); } if (topic_options.find("colormap_max") != topic_options.end()) { - options.colormap_max = topic_options.at("colormap_max").as(); + options.colormapMax = topic_options.at("colormap_max").as(); } return options; } @@ -133,20 +134,27 @@ std::string RerunLoggerNode::_resolve_entity_path( // 3. Options for the exact topic name (such as /ns/topic) // Aside from these rules, the options are merged in the order they are defined in the yaml file. TopicOptions RerunLoggerNode::_resolve_topic_options( - const std::string& topic, const std::string& message_type + const std::string& topic, const std::string& message_type, bool merge /* = true */ ) const { - TopicOptions merged_options; - // 1. partial topic names - for (const auto& [prefix, options] : _raw_topic_options) { - if (topic.find(prefix) == 0) { - merged_options.insert(options.begin(), options.end()); + TopicOptions merged_options{}; + + // if (merge == false): + // Be explicit, use exactly settings specified in YAML. + // i.e. don't combine options for different topics, + // even if they have same type (sensor_msgs/msg/PointCloud2) + if (merge) { + // 1. partial topic names + for (const auto& [prefix, options] : _raw_topic_options) { + if (topic.find(prefix) == 0) { + merged_options.insert(options.begin(), options.end()); + } } - } - // 2. message types - if (_raw_topic_options.find(message_type) != _raw_topic_options.end()) { - auto options = _raw_topic_options.at(message_type); - merged_options.insert(options.begin(), options.end()); + // 2. message types + if (_raw_topic_options.find(message_type) != _raw_topic_options.end()) { + auto options = _raw_topic_options.at(message_type); + merged_options.insert(options.begin(), options.end()); + } } // 3. exact topic name @@ -535,11 +543,14 @@ std::shared_ptr> if (topic_options.find("restamp") != topic_options.end()) { restamp = topic_options.at("restamp").as(); } - auto point_cloud2_options = point_cloud2_options_from_topic_options(topic_options); + const auto point_cloud2_options = point_cloud2_options_from_topic_options(topic_options); rclcpp::SubscriptionOptions subscription_options; subscription_options.callback_group = _parallel_callback_group; + // create new instance of PointCloudProcessor and bound it to the topic + _topic_to_point_cloud_processor.emplace(topic, point_cloud2_options); + RCLCPP_INFO( this->get_logger(), "Subscribing to PointCloud2 topic %s, logging to entity path %s", @@ -550,11 +561,11 @@ std::shared_ptr> return this->create_subscription( topic, 1000, - [&, entity_path, lookup_transform, restamp, point_cloud2_options]( + [&, entity_path, lookup_transform, restamp, topic]( const sensor_msgs::msg::PointCloud2::SharedPtr msg ) { _handle_msg_header(restamp, lookup_transform, entity_path, msg->header); - log_point_cloud2(_rec, entity_path, msg, point_cloud2_options); + _topic_to_point_cloud_processor.at(topic).logPointCloud2(_rec, entity_path, msg); }, subscription_options ); diff --git a/rerun_bridge/src/rerun_bridge/visualizer_node.hpp b/rerun_bridge/src/rerun_bridge/visualizer_node.hpp index 6c4f902..96621b8 100644 --- a/rerun_bridge/src/rerun_bridge/visualizer_node.hpp +++ b/rerun_bridge/src/rerun_bridge/visualizer_node.hpp @@ -17,6 +17,8 @@ #include #include +#include "rerun_bridge/point_cloud_processor.hpp" +#include "rerun_bridge/rerun_ros_interface.hpp" using TopicOptions = std::map; @@ -32,11 +34,13 @@ class RerunLoggerNode : public rclcpp::Node { std::map _tf_frame_to_parent; std::map _raw_topic_options; rclcpp::Time _last_tf_update_time; + std::map _topic_to_point_cloud_processor; void _read_yaml_config(std::string yaml_path); - TopicOptions _resolve_topic_options(const std::string& topic, const std::string& message_type) - const; + TopicOptions _resolve_topic_options( + const std::string& topic, const std::string& message_type, bool mergeOptions = true + ) const; std::string _resolve_entity_path(const std::string& topic, const TopicOptions& topic_options) const;