From 30c83398585168b91487905a53f1eff0b6185760 Mon Sep 17 00:00:00 2001 From: Tamas Foldi Date: Wed, 2 Oct 2024 17:38:31 +0200 Subject: [PATCH 1/7] chore: bump rerun API compatibiliy to 0.18.2 --- rerun_bridge/CMakeLists.txt | 2 +- .../src/rerun_bridge/collection_adapters.hpp | 19 +++++++++++++++---- .../src/rerun_bridge/rerun_ros_interface.cpp | 18 +++++++++--------- 3 files changed, 25 insertions(+), 14 deletions(-) diff --git a/rerun_bridge/CMakeLists.txt b/rerun_bridge/CMakeLists.txt index 29b620e..71c03f7 100644 --- a/rerun_bridge/CMakeLists.txt +++ b/rerun_bridge/CMakeLists.txt @@ -22,7 +22,7 @@ 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.18.2/rerun_cpp_sdk.zip) FetchContent_MakeAvailable(rerun_sdk) # setup targets (has to be done before ament_package call) diff --git a/rerun_bridge/src/rerun_bridge/collection_adapters.hpp b/rerun_bridge/src/rerun_bridge/collection_adapters.hpp index d7d42aa..7993c75 100644 --- a/rerun_bridge/src/rerun_bridge/collection_adapters.hpp +++ b/rerun_bridge/src/rerun_bridge/collection_adapters.hpp @@ -22,9 +22,20 @@ struct rerun::CollectionAdapter { } }; -inline rerun::Collection tensor_shape(const cv::Mat& img) { - return { - static_cast(img.rows), +inline rerun::WidthHeight width_height(const cv::Mat& img) { + return rerun::WidthHeight( static_cast(img.cols), - static_cast(img.channels())}; + static_cast(img.rows) + ); }; + +template +rerun::Collection img_data_as_collection(const cv::Mat& img) { + const T* img_data = reinterpret_cast(img.data); + + size_t img_size = img.total() * img.channels(); + + std::vector img_vec(img_data, img_data + img_size); + + return rerun::Collection::take_ownership(std::move(img_vec)); +} \ No newline at end of file diff --git a/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp b/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp index a285286..c4c8a5f 100644 --- a/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp +++ b/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp @@ -117,8 +117,8 @@ void log_image( rec.log( entity_path, rerun::DepthImage( - {static_cast(img.rows), static_cast(img.cols)}, - rerun::TensorBuffer::u16(img) + img_data_as_collection(img), + {static_cast(img.rows), static_cast(img.cols)} ) .with_meter(1000) ); @@ -133,14 +133,14 @@ void log_image( rec.log( entity_path, rerun::DepthImage( - {static_cast(img.rows), static_cast(img.cols)}, - rerun::TensorBuffer::f32(img) + img_data_as_collection(img), + {static_cast(img.rows), static_cast(img.cols)} ) .with_meter(1.0) ); } else { cv::Mat img = cv_bridge::toCvCopy(msg, "rgb8")->image; - rec.log(entity_path, rerun::Image(tensor_shape(img), rerun::TensorBuffer::u8(img))); + rec.log(entity_path, rerun::Image::from_rgb24(img_data_as_collection(img), width_height(img))); } } @@ -156,7 +156,7 @@ void log_pose_stamped( rec.log( entity_path, rerun::Transform3D( - rerun::Vector3D(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z), + rerun::components::Translation3D(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z), rerun::Quaternion::from_wxyz( msg->pose.orientation.w, msg->pose.orientation.x, @@ -200,7 +200,7 @@ void log_tf_message( rec.log( tf_frame_to_entity_path.at(transform.child_frame_id), rerun::Transform3D( - rerun::Vector3D( + rerun::components::Translation3D( transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z @@ -228,7 +228,7 @@ void log_odometry( rec.log( entity_path, rerun::Transform3D( - rerun::Vector3D( + rerun::components::Translation3D( msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z @@ -278,7 +278,7 @@ void log_transform( rec.log( entity_path, rerun::Transform3D( - rerun::Vector3D( + rerun::components::Translation3D( msg->transform.translation.x, msg->transform.translation.y, msg->transform.translation.z From 503b5a4914a4d6edc9523164cab660791a5df6fa Mon Sep 17 00:00:00 2001 From: Tamas Foldi Date: Wed, 2 Oct 2024 18:37:48 +0200 Subject: [PATCH 2/7] format: apply clang-format --- .../src/rerun_bridge/collection_adapters.hpp | 9 +++----- .../src/rerun_bridge/rerun_ros_interface.cpp | 23 +++++++++---------- 2 files changed, 14 insertions(+), 18 deletions(-) diff --git a/rerun_bridge/src/rerun_bridge/collection_adapters.hpp b/rerun_bridge/src/rerun_bridge/collection_adapters.hpp index 7993c75..7b7bee0 100644 --- a/rerun_bridge/src/rerun_bridge/collection_adapters.hpp +++ b/rerun_bridge/src/rerun_bridge/collection_adapters.hpp @@ -23,13 +23,10 @@ struct rerun::CollectionAdapter { }; inline rerun::WidthHeight width_height(const cv::Mat& img) { - return rerun::WidthHeight( - static_cast(img.cols), - static_cast(img.rows) - ); + return rerun::WidthHeight(static_cast(img.cols), static_cast(img.rows)); }; -template +template rerun::Collection img_data_as_collection(const cv::Mat& img) { const T* img_data = reinterpret_cast(img.data); @@ -38,4 +35,4 @@ rerun::Collection img_data_as_collection(const cv::Mat& img) { std::vector img_vec(img_data, img_data + img_size); return rerun::Collection::take_ownership(std::move(img_vec)); -} \ No newline at end of file +} diff --git a/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp b/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp index c4c8a5f..a07b5e5 100644 --- a/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp +++ b/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp @@ -116,11 +116,7 @@ void log_image( cv::Mat img = cv_bridge::toCvCopy(msg)->image; rec.log( entity_path, - rerun::DepthImage( - img_data_as_collection(img), - {static_cast(img.rows), static_cast(img.cols)} - ) - .with_meter(1000) + rerun::DepthImage(rerun::TensorBuffer::u16(img), width_height(img)).with_meter(1000) ); } else if (msg->encoding == "32FC1") { cv::Mat img = cv_bridge::toCvCopy(msg)->image; @@ -132,15 +128,14 @@ void log_image( } rec.log( entity_path, - rerun::DepthImage( - img_data_as_collection(img), - {static_cast(img.rows), static_cast(img.cols)} - ) - .with_meter(1.0) + rerun::DepthImage(img_data_as_collection(img), width_height(img)).with_meter(1.0) ); } else { cv::Mat img = cv_bridge::toCvCopy(msg, "rgb8")->image; - rec.log(entity_path, rerun::Image::from_rgb24(img_data_as_collection(img), width_height(img))); + rec.log( + entity_path, + rerun::Image::from_rgb24(img_data_as_collection(img), width_height(img)) + ); } } @@ -156,7 +151,11 @@ void log_pose_stamped( rec.log( entity_path, rerun::Transform3D( - rerun::components::Translation3D(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z), + rerun::components::Translation3D( + msg->pose.position.x, + msg->pose.position.y, + msg->pose.position.z + ), rerun::Quaternion::from_wxyz( msg->pose.orientation.w, msg->pose.orientation.x, From 9e4cead473238af3cf54dfe4ac54311202acfdf3 Mon Sep 17 00:00:00 2001 From: Tamas Foldi Date: Wed, 2 Oct 2024 19:47:00 +0200 Subject: [PATCH 3/7] chore: use img_data_as_collection everywhere --- rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp b/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp index a07b5e5..7984ae4 100644 --- a/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp +++ b/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp @@ -116,7 +116,7 @@ void log_image( cv::Mat img = cv_bridge::toCvCopy(msg)->image; rec.log( entity_path, - rerun::DepthImage(rerun::TensorBuffer::u16(img), width_height(img)).with_meter(1000) + rerun::DepthImage(img_data_as_collection(img), width_height(img)).with_meter(1000) ); } else if (msg->encoding == "32FC1") { cv::Mat img = cv_bridge::toCvCopy(msg)->image; From b9e280fc888e9355450bec9889b1e9a0c6d466f7 Mon Sep 17 00:00:00 2001 From: Tamas Foldi Date: Wed, 2 Oct 2024 21:29:22 +0200 Subject: [PATCH 4/7] chore: trying to find out the proper way to use Adapters --- rerun_bridge/src/rerun_bridge/collection_adapters.hpp | 11 ----------- rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp | 6 +++--- 2 files changed, 3 insertions(+), 14 deletions(-) diff --git a/rerun_bridge/src/rerun_bridge/collection_adapters.hpp b/rerun_bridge/src/rerun_bridge/collection_adapters.hpp index 7b7bee0..5c25d45 100644 --- a/rerun_bridge/src/rerun_bridge/collection_adapters.hpp +++ b/rerun_bridge/src/rerun_bridge/collection_adapters.hpp @@ -25,14 +25,3 @@ struct rerun::CollectionAdapter { inline rerun::WidthHeight width_height(const cv::Mat& img) { return rerun::WidthHeight(static_cast(img.cols), static_cast(img.rows)); }; - -template -rerun::Collection img_data_as_collection(const cv::Mat& img) { - const T* img_data = reinterpret_cast(img.data); - - size_t img_size = img.total() * img.channels(); - - std::vector img_vec(img_data, img_data + img_size); - - return rerun::Collection::take_ownership(std::move(img_vec)); -} diff --git a/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp b/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp index 7984ae4..de506eb 100644 --- a/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp +++ b/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp @@ -116,7 +116,7 @@ void log_image( cv::Mat img = cv_bridge::toCvCopy(msg)->image; rec.log( entity_path, - rerun::DepthImage(img_data_as_collection(img), width_height(img)).with_meter(1000) + rerun::DepthImage(rerun::Collection(img), width_height(img)).with_meter(1000) ); } else if (msg->encoding == "32FC1") { cv::Mat img = cv_bridge::toCvCopy(msg)->image; @@ -128,13 +128,13 @@ void log_image( } rec.log( entity_path, - rerun::DepthImage(img_data_as_collection(img), width_height(img)).with_meter(1.0) + rerun::DepthImage(rerun::Collection(img), width_height(img)).with_meter(1.0) ); } else { cv::Mat img = cv_bridge::toCvCopy(msg, "rgb8")->image; rec.log( entity_path, - rerun::Image::from_rgb24(img_data_as_collection(img), width_height(img)) + rerun::Image::from_rgb24(rerun::Collection(img), width_height(img)) ); } } From b36a0e44c17194189360d1aed085721940c9c750 Mon Sep 17 00:00:00 2001 From: Tamas Foldi Date: Thu, 3 Oct 2024 09:07:09 +0200 Subject: [PATCH 5/7] feat: add rgb8 color mode for pc2 --- .../src/rerun_bridge/rerun_ros_interface.cpp | 29 ++++++++++++++++--- 1 file changed, 25 insertions(+), 4 deletions(-) diff --git a/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp b/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp index de506eb..2582537 100644 --- a/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp +++ b/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp @@ -306,8 +306,8 @@ void log_point_cloud2( // 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}; + size_t x_offset, y_offset, z_offset, rgb_offset; + bool has_x{false}, has_y{false}, has_z{false}, has_rgb{false}; for (const auto& field : msg->fields) { if (field.name == "x") { @@ -331,6 +331,14 @@ void log_point_cloud2( return; } has_z = true; + } else if (field.name == options.colormap_field.value_or("rgb")) { + rgb_offset = field.offset; + if (field.datatype != sensor_msgs::msg::PointField::UINT32 && + field.datatype != sensor_msgs::msg::PointField::FLOAT32) { + rec.log(entity_path, rerun::TextLog("Only UINT32 and FLOAT32 rgb field supported")); + return; + } + has_rgb = true; } } @@ -345,6 +353,10 @@ void log_point_cloud2( std::vector points(msg->width * msg->height); std::vector colors; + if (has_rgb) { + colors.reserve(msg->width * msg->height); + } + 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; @@ -353,7 +365,12 @@ void log_point_cloud2( 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 (has_rgb) { + uint8_t rgba[4]; + std::memcpy(&rgba, &msg->data[point_offset + rgb_offset], sizeof(uint32_t)); + colors.emplace_back(rerun::Color(rgba[2], rgba[1], rgba[0])); + } + points[i * msg->width + j] = position; } } @@ -371,10 +388,14 @@ void log_point_cloud2( &msg->data[i * msg->row_step + j * msg->point_step + colormap_field.offset], sizeof(float) ); - values.emplace_back(value); + values[i * msg->width + j] = value; } } colors = colormap(values, options.colormap_min, options.colormap_max); + } else if (options.colormap == "rgb") { + if (!has_rgb) { + rec.log(entity_path, rerun::TextLog("RGB colormap specified but no RGB field present")); + } } else if (options.colormap) { rec.log("/", rerun::TextLog("Unsupported colormap specified: " + options.colormap.value())); } From b0f5a3502e0db7a86f42f49861fdf361c95f7ed1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?F=C3=B6ldi=20Tam=C3=A1s?= Date: Thu, 3 Oct 2024 14:28:20 +0200 Subject: [PATCH 6/7] chore: apply suggestions from code review Co-authored-by: Emil Ernerfeldt --- .../src/rerun_bridge/rerun_ros_interface.cpp | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp b/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp index 2582537..a96a02f 100644 --- a/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp +++ b/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp @@ -332,13 +332,14 @@ void log_point_cloud2( } has_z = true; } else if (field.name == options.colormap_field.value_or("rgb")) { - rgb_offset = field.offset; - if (field.datatype != sensor_msgs::msg::PointField::UINT32 && - field.datatype != sensor_msgs::msg::PointField::FLOAT32) { + if (field.datatype == sensor_msgs::msg::PointField::UINT32 || + field.datatype == sensor_msgs::msg::PointField::FLOAT32) { + has_rgb = true; + rgb_offset = field.offset; + } else { rec.log(entity_path, rerun::TextLog("Only UINT32 and FLOAT32 rgb field supported")); - return; + continue; } - has_rgb = true; } } @@ -366,9 +367,9 @@ void log_point_cloud2( 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)); if (has_rgb) { - uint8_t rgba[4]; - std::memcpy(&rgba, &msg->data[point_offset + rgb_offset], sizeof(uint32_t)); - colors.emplace_back(rerun::Color(rgba[2], rgba[1], rgba[0])); + uint8_t bgra[4]; + std::memcpy(&bgra, &msg->data[point_offset + rgb_offset], sizeof(uint32_t)); + colors.emplace_back(rerun::Color(bgra[2], bgra[1], bgra[0])); } points[i * msg->width + j] = position; } From eeb63aeaf76293c42d151e4f2ce8ade03cb08581 Mon Sep 17 00:00:00 2001 From: Tamas Foldi Date: Thu, 3 Oct 2024 14:30:54 +0200 Subject: [PATCH 7/7] style: reformat --- rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp b/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp index a96a02f..6f781d7 100644 --- a/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp +++ b/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp @@ -334,9 +334,9 @@ void log_point_cloud2( } else if (field.name == options.colormap_field.value_or("rgb")) { if (field.datatype == sensor_msgs::msg::PointField::UINT32 || field.datatype == sensor_msgs::msg::PointField::FLOAT32) { - has_rgb = true; - rgb_offset = field.offset; - } else { + has_rgb = true; + rgb_offset = field.offset; + } else { rec.log(entity_path, rerun::TextLog("Only UINT32 and FLOAT32 rgb field supported")); continue; }