From edd0b093589c40dacd119bc558707ab18fa87ba8 Mon Sep 17 00:00:00 2001 From: Miguel Xochicale Date: Thu, 22 Feb 2024 11:41:20 +0000 Subject: [PATCH] adds rerun/examples/cpp-example-ros-bridge #30 --- .gitignore | 7 +- .../examples/cpp-example-ros-bridge/README.md | 33 ++++ .../examples/cpp-example-ros-bridge/pixi.toml | 25 +++ .../rerun_bridge/CMakeLists.txt | 38 +++++ .../rerun_bridge/rerun_ros_interface.hpp | 28 +++ .../rerun_bridge/launch/example.launch | 11 ++ .../rerun_bridge/launch/example_params.yaml | 30 ++++ .../rerun_bridge/package.xml | 15 ++ .../src/rerun_bridge/collection_adapters.hpp | 32 ++++ .../src/rerun_bridge/rerun_ros_interface.cpp | 86 ++++++++++ .../src/rerun_bridge/visualizer_node.cpp | 161 ++++++++++++++++++ .../src/rerun_bridge/visualizer_node.hpp | 26 +++ 12 files changed, 491 insertions(+), 1 deletion(-) create mode 100644 rerun/examples/cpp-example-ros-bridge/README.md create mode 100644 rerun/examples/cpp-example-ros-bridge/pixi.toml create mode 100644 rerun/examples/cpp-example-ros-bridge/rerun_bridge/CMakeLists.txt create mode 100644 rerun/examples/cpp-example-ros-bridge/rerun_bridge/include/rerun_bridge/rerun_ros_interface.hpp create mode 100644 rerun/examples/cpp-example-ros-bridge/rerun_bridge/launch/example.launch create mode 100644 rerun/examples/cpp-example-ros-bridge/rerun_bridge/launch/example_params.yaml create mode 100644 rerun/examples/cpp-example-ros-bridge/rerun_bridge/package.xml create mode 100644 rerun/examples/cpp-example-ros-bridge/rerun_bridge/src/rerun_bridge/collection_adapters.hpp create mode 100644 rerun/examples/cpp-example-ros-bridge/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp create mode 100644 rerun/examples/cpp-example-ros-bridge/rerun_bridge/src/rerun_bridge/visualizer_node.cpp create mode 100644 rerun/examples/cpp-example-ros-bridge/rerun_bridge/src/rerun_bridge/visualizer_node.hpp diff --git a/.gitignore b/.gitignore index 238d8ea..0a68db6 100644 --- a/.gitignore +++ b/.gitignore @@ -4,9 +4,14 @@ ## temporal vim files *.sw* -##temporal model files +##temporal files for rerun blaze_face_short_range.tflite face_landmarker.task +.pixi +noetic_ws +pixi.lock +indoor_forward_3_snapdragon_with_gt.bag + ## files for html-slides _site/ diff --git a/rerun/examples/cpp-example-ros-bridge/README.md b/rerun/examples/cpp-example-ros-bridge/README.md new file mode 100644 index 0000000..af173bf --- /dev/null +++ b/rerun/examples/cpp-example-ros-bridge/README.md @@ -0,0 +1,33 @@ +# cpp-example-ros-bridge + + +## Installation +``` +curl -fsSL https://pixi.sh/install.sh | bash +echo 'eval "$(pixi completion --shell bash)"' >> ~/.bashrc +#And then restart the shell or source the shell config file. +#rm ~/.pixi/bin/pixi + +git clone git@github.com:rerun-io/cpp-example-ros-bridge.git +mv pixi.toml .. +mv rerun_bridge/ .. +rm -rf cpp-example-ros-bridge/ + +$HOME/.pixi/bin/./pixi run example +``` + +## some logs +``` +$HOME/.pixi/bin/./pixi run example +✨ Pixi task (default): mkdir -p noetic_ws/src && ln -sfn $(pwd)/rerun_bridge noetic_ws/src/rerun_bridge +⠉ updating packages in 'default' + ⠁ downloading [00:01:18] [━───────────────────] 21/542 ogre (+49) ⠁ linking [00:01:18] [━───────────────────] 21/542 +``` + + +## References +https://github.com/rerun-io/cpp-example-ros-bridge?tab=readme-ov-file +https://github.com/prefix-dev/pixi/ + + + diff --git a/rerun/examples/cpp-example-ros-bridge/pixi.toml b/rerun/examples/cpp-example-ros-bridge/pixi.toml new file mode 100644 index 0000000..cc4c53c --- /dev/null +++ b/rerun/examples/cpp-example-ros-bridge/pixi.toml @@ -0,0 +1,25 @@ +[project] +name = "rerun_cpp_example_ros" +authors = ["rerun.io "] +channels = ["robostack-staging", "conda-forge"] +description = "Use the Rerun C++ SDK together with ROS" +homepage = "https://rerun.io" +license = "Apache-2.0" +platforms = ["linux-64"] +repository = "https://github.com/rerun-io/cpp-example-ros" +version = "0.1.0" + +[tasks] +ws = { cmd = "mkdir -p noetic_ws/src && ln -sfn $(pwd)/rerun_bridge noetic_ws/src/rerun_bridge", cwd = "." } +build = { cmd = "ls && catkin_make --cmake-args -DCMAKE_BUILD_TYPE=Release -DCMAKE_EXPORT_COMPILE_COMMANDS=ON", depends_on = ["ws"], cwd = "noetic_ws"} +example_data = { cmd = "curl -L -C - -O http://rpg.ifi.uzh.ch/datasets/uzh-fpv-newer-versions/v3/indoor_forward_3_snapdragon_with_gt.bag", depends_on=["ws"], cwd = "noetic_ws/src/rerun_bridge"} +example = { cmd = "bash -c 'source ./devel/setup.bash && roslaunch rerun_bridge example.launch'", depends_on = [ "build", "example_data", "ws"], cwd = "noetic_ws" } + +[dependencies] +compilers = ">=1.7.0,<1.8" +opencv = ">=4.9.0,<4.10" +ros-noetic-catkin = ">=0.8.10,<0.9" +ros-noetic-desktop = ">=1.5.0,<1.6" +ros-noetic-rosbag = ">=1.16.0,<1.17" +yaml-cpp = ">=0.8.0,<0.9" +rerun-sdk = ">=0.13.0,<0.14" diff --git a/rerun/examples/cpp-example-ros-bridge/rerun_bridge/CMakeLists.txt b/rerun/examples/cpp-example-ros-bridge/rerun_bridge/CMakeLists.txt new file mode 100644 index 0000000..f1747a2 --- /dev/null +++ b/rerun/examples/cpp-example-ros-bridge/rerun_bridge/CMakeLists.txt @@ -0,0 +1,38 @@ +cmake_minimum_required(VERSION 3.0.2) +project(rerun_bridge) + +if(NOT DEFINED CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs nav_msgs cv_bridge) +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.13.0/rerun_cpp_sdk.zip) +FetchContent_MakeAvailable(rerun_sdk) + +include_directories( + include + ${YAML_CPP_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} +) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS roscpp sensor_msgs nav_msgs cv_bridge + DEPENDS opencv yaml-cpp + ) + +add_library(${PROJECT_NAME} src/rerun_bridge/rerun_ros_interface.cpp) +add_executable(visualizer src/rerun_bridge/visualizer_node.cpp) + +add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(visualizer ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBS} ${YAML_CPP_LIBRARIES} rerun_sdk) +target_link_libraries(visualizer ${PROJECT_NAME} ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES} rerun_sdk) + +install(TARGETS visualizer DESTINATION bin) diff --git a/rerun/examples/cpp-example-ros-bridge/rerun_bridge/include/rerun_bridge/rerun_ros_interface.hpp b/rerun/examples/cpp-example-ros-bridge/rerun_bridge/include/rerun_bridge/rerun_ros_interface.hpp new file mode 100644 index 0000000..92a9833 --- /dev/null +++ b/rerun/examples/cpp-example-ros-bridge/rerun_bridge/include/rerun_bridge/rerun_ros_interface.hpp @@ -0,0 +1,28 @@ +#pragma once + +#include +#include +#include +#include + +#include + +void log_imu( + const rerun::RecordingStream& rec, const std::string& entity_path, + const sensor_msgs::Imu::ConstPtr& msg +); + +void log_image( + const rerun::RecordingStream& rec, const std::string& entity_path, + const sensor_msgs::Image::ConstPtr& msg +); + +void log_pose_stamped( + const rerun::RecordingStream& rec, const std::string& entity_path, + const geometry_msgs::PoseStamped::ConstPtr& msg +); + +void log_odometry( + const rerun::RecordingStream& rec, const std::string& entity_path, + const nav_msgs::Odometry::ConstPtr& msg +); diff --git a/rerun/examples/cpp-example-ros-bridge/rerun_bridge/launch/example.launch b/rerun/examples/cpp-example-ros-bridge/rerun_bridge/launch/example.launch new file mode 100644 index 0000000..52458f5 --- /dev/null +++ b/rerun/examples/cpp-example-ros-bridge/rerun_bridge/launch/example.launch @@ -0,0 +1,11 @@ + + + + + + + + $(find rerun_bridge)/launch/example_params.yaml + + + diff --git a/rerun/examples/cpp-example-ros-bridge/rerun_bridge/launch/example_params.yaml b/rerun/examples/cpp-example-ros-bridge/rerun_bridge/launch/example_params.yaml new file mode 100644 index 0000000..655920c --- /dev/null +++ b/rerun/examples/cpp-example-ros-bridge/rerun_bridge/launch/example_params.yaml @@ -0,0 +1,30 @@ +topic_to_entity_path: + /snappy_cam/stereo_l: /groundtruth/pose/snappy_cam/stereo_l + /snappy_cam/stereo_r: /groundtruth/pose/snappy_cam/stereo_r +extra_transform3ds: + - entity_path: /groundtruth/pose/snappy_cam/stereo_l + transform: [ + -0.028228787368606456, -0.999601488301944, 1.2175294828553618e-05, 0.02172388268966517, + 0.014401251861751119, -0.00041887083271471837, -0.9998962088597202, -6.605455433829172e-05, + 0.999497743623523, -0.028225682131089447, 0.014407337010089172, -0.00048817563004522853, + 0.0, 0.0, 0.0, 1.0 + ] + from_parent: true + - entity_path: /groundtruth/pose/snappy_cam/stereo_r + transform: [ + -0.011823057800830705, -0.9998701444077991, -0.010950325390841398, -0.057904961033265645, + 0.011552991631909482, 0.01081376681432078, -0.9998747875767439, 0.00043766687615362694, + 0.9998633625093938, -0.011948086424720228, 0.011423639621249038, -0.00039944945687402214, + 0.0, 0.0, 0.0, 1.0 + ] + from_parent: true +extra_pinholes: + - entity_path: /groundtruth/pose/snappy_cam/stereo_l + height: 480 + width: 640 + image_from_camera: [278.66723066149086, 0, 319.75221200593535, 0, 278.48991409740296, 241.96858910358173, 0, 0, 1] + - entity_path: /groundtruth/pose/snappy_cam/stereo_r + height: 480 + width: 640 + image_from_camera: [277.61640629770613, 0, 314.8944703346039, 0, 277.63749695723294, 236.04310050462587, 0, 0, 1] + diff --git a/rerun/examples/cpp-example-ros-bridge/rerun_bridge/package.xml b/rerun/examples/cpp-example-ros-bridge/rerun_bridge/package.xml new file mode 100644 index 0000000..a623263 --- /dev/null +++ b/rerun/examples/cpp-example-ros-bridge/rerun_bridge/package.xml @@ -0,0 +1,15 @@ + + + rerun_bridge + 0.1.0 + The rerun_bridge package + rerun.io + Apache-2.0 + roscpp + sensor_msgs + nav_msgs + cv_bridge + geometry_msgs + yaml-cpp + catkin + diff --git a/rerun/examples/cpp-example-ros-bridge/rerun_bridge/src/rerun_bridge/collection_adapters.hpp b/rerun/examples/cpp-example-ros-bridge/rerun_bridge/src/rerun_bridge/collection_adapters.hpp new file mode 100644 index 0000000..fc59375 --- /dev/null +++ b/rerun/examples/cpp-example-ros-bridge/rerun_bridge/src/rerun_bridge/collection_adapters.hpp @@ -0,0 +1,32 @@ +#pragma once + +#include +#include + +// Adapters so we can borrow an OpenCV image easily into Rerun images without copying: +template <> +struct rerun::CollectionAdapter { + /// Borrow for non-temporary. + Collection operator()(const cv::Mat& img) { + assert( + "OpenCV matrix was expected have bit depth CV_U8" && CV_MAT_DEPTH(img.type()) == CV_8U + ); + + return Collection::borrow(img.data, img.total() * img.channels()); + } + + // Do a full copy for temporaries (otherwise the data might be deleted when the temporary is destroyed). + Collection operator()(cv::Mat&& img) { + assert( + "OpenCV matrix was expected have bit depth CV_U8" && CV_MAT_DEPTH(img.type()) == CV_8U + ); + + std::vector img_vec(img.total() * img.channels()); + img_vec.assign(img.data, img.data + img.total() * img.channels()); + return Collection::take_ownership(std::move(img_vec)); + } +}; + +inline rerun::Collection tensor_shape(const cv::Mat& img) { + return {img.rows, img.cols, img.channels()}; +}; diff --git a/rerun/examples/cpp-example-ros-bridge/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp b/rerun/examples/cpp-example-ros-bridge/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp new file mode 100644 index 0000000..d9370a8 --- /dev/null +++ b/rerun/examples/cpp-example-ros-bridge/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp @@ -0,0 +1,86 @@ +#include +#include +#include +#include +#include +#include +#include + +#include "collection_adapters.hpp" + +void log_imu( + const rerun::RecordingStream& rec, const std::string& entity_path, + const sensor_msgs::Imu::ConstPtr& msg +) { + rec.set_time_seconds("timestamp", msg->header.stamp.toSec()); + + rec.log(entity_path + "/x", rerun::Scalar(msg->linear_acceleration.x)); + rec.log(entity_path + "/y", rerun::Scalar(msg->linear_acceleration.y)); + rec.log(entity_path + "/z", rerun::Scalar(msg->linear_acceleration.z)); +} + +void log_image( + const rerun::RecordingStream& rec, const std::string& entity_path, + const sensor_msgs::Image::ConstPtr& msg +) { + rec.set_time_seconds("timestamp", msg->header.stamp.toSec()); + + cv::Mat img = cv_bridge::toCvCopy(msg, "rgb8")->image; + rec.log(entity_path, rerun::Image(tensor_shape(img), rerun::TensorBuffer::u8(img))); +} + +void log_pose_stamped( + const rerun::RecordingStream& rec, const std::string& entity_path, + const geometry_msgs::PoseStamped::ConstPtr& msg +) { + rec.set_time_seconds("timestamp", msg->header.stamp.toSec()); + + rec.log( + entity_path, + rerun::Transform3D( + rerun::Vector3D(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z), + rerun::Quaternion::from_wxyz( + msg->pose.orientation.w, + msg->pose.orientation.x, + msg->pose.orientation.y, + msg->pose.orientation.z + ) + ) + ); + + // this is a somewhat hacky way to get a trajectory visualization in Rerun + // this should be be easier in the future, see https://github.com/rerun-io/rerun/issues/723 + std::string trajectory_entity_path = "/trajectories/" + entity_path; + rec.log( + trajectory_entity_path, + rerun::Points3D( + {{static_cast(msg->pose.position.x), + static_cast(msg->pose.position.y), + static_cast(msg->pose.position.z)}} + ) + ); +} + +void log_odometry( + const rerun::RecordingStream& rec, const std::string& entity_path, + const nav_msgs::Odometry::ConstPtr& msg +) { + rec.set_time_seconds("timestamp", msg->header.stamp.toSec()); + + rec.log( + entity_path, + rerun::Transform3D( + rerun::Vector3D( + msg->pose.pose.position.x, + msg->pose.pose.position.y, + msg->pose.pose.position.z + ), + rerun::Quaternion::from_wxyz( + msg->pose.pose.orientation.w, + msg->pose.pose.orientation.x, + msg->pose.pose.orientation.y, + msg->pose.pose.orientation.z + ) + ) + ); +} diff --git a/rerun/examples/cpp-example-ros-bridge/rerun_bridge/src/rerun_bridge/visualizer_node.cpp b/rerun/examples/cpp-example-ros-bridge/rerun_bridge/src/rerun_bridge/visualizer_node.cpp new file mode 100644 index 0000000..2259f0c --- /dev/null +++ b/rerun/examples/cpp-example-ros-bridge/rerun_bridge/src/rerun_bridge/visualizer_node.cpp @@ -0,0 +1,161 @@ +#include "visualizer_node.hpp" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +RerunLoggerNode::RerunLoggerNode() { + _rec.spawn().exit_on_failure(); + + // Read additional config from yaml file + // NOTE We're not using the ROS parameter server for this, because roscpp doesn't support + // reading nested data structures. + std::string yaml_path; + if (_nh.getParam("yaml_path", yaml_path)) { + ROS_INFO("Read yaml config at %s", yaml_path.c_str()); + } + _read_yaml_config(yaml_path); +} + +std::string RerunLoggerNode::_topic_to_entity_path(const std::string& topic) const { + if (_topic_to_entity_path_map.find(topic) != _topic_to_entity_path_map.end()) { + return _topic_to_entity_path_map.at(topic); + } else { + return topic; + } +} + +void RerunLoggerNode::_read_yaml_config(std::string yaml_path) { + const YAML::Node config = YAML::LoadFile(yaml_path); + + // see https://www.rerun.io/docs/howto/ros2-nav-turtlebot#tf-to-rrtransform3d + if (config["topic_to_entity_path"]) { + _topic_to_entity_path_map = + config["topic_to_entity_path"].as>(); + + for (auto const& [key, val] : _topic_to_entity_path_map) { + ROS_INFO("Mapping topic %s to entity path %s", key.c_str(), val.c_str()); + } + } + if (config["extra_transform3ds"]) { + for (const auto& extra_transform3d : config["extra_transform3ds"]) { + const std::array translation = { + extra_transform3d["transform"][3].as(), + extra_transform3d["transform"][7].as(), + extra_transform3d["transform"][11].as() + }; + // Rerun uses column-major order for Mat3x3 + const std::array mat3x3 = { + extra_transform3d["transform"][0].as(), + extra_transform3d["transform"][4].as(), + extra_transform3d["transform"][8].as(), + extra_transform3d["transform"][1].as(), + extra_transform3d["transform"][5].as(), + extra_transform3d["transform"][9].as(), + extra_transform3d["transform"][2].as(), + extra_transform3d["transform"][6].as(), + extra_transform3d["transform"][10].as() + }; + _rec.log_timeless( + extra_transform3d["entity_path"].as(), + rerun::Transform3D( + rerun::Vec3D(translation), + rerun::Mat3x3(mat3x3), + extra_transform3d["from_parent"].as() + ) + ); + } + } + if (config["extra_pinholes"]) { + for (const auto& extra_pinhole : config["extra_pinholes"]) { + // Rerun uses column-major order for Mat3x3 + const std::array image_from_camera = { + extra_pinhole["image_from_camera"][0].as(), + extra_pinhole["image_from_camera"][3].as(), + extra_pinhole["image_from_camera"][6].as(), + extra_pinhole["image_from_camera"][1].as(), + extra_pinhole["image_from_camera"][4].as(), + extra_pinhole["image_from_camera"][7].as(), + extra_pinhole["image_from_camera"][2].as(), + extra_pinhole["image_from_camera"][5].as(), + extra_pinhole["image_from_camera"][8].as(), + }; + _rec.log_timeless( + extra_pinhole["entity_path"].as(), + rerun::Pinhole(image_from_camera) + .with_resolution( + extra_pinhole["width"].as(), + extra_pinhole["height"].as() + ) + ); + } + } +} + +void RerunLoggerNode::spin() { + while (ros::ok()) { + ros::spinOnce(); + + // NOTE We are currently checking in each iteration if there are new topics. + // This is not efficient, but it's the easiest way to support new topics being added at runtime. + // If you have a lot of topics, you might want to optimize this. + ros::master::V_TopicInfo topic_infos; + ros::master::getTopics(topic_infos); + for (const auto& topic_info : topic_infos) { + if (_topic_to_subscriber.find(topic_info.name) == _topic_to_subscriber.end()) { + auto entity_path = _topic_to_entity_path(topic_info.name); + if (topic_info.datatype == "sensor_msgs/Image") { + _topic_to_subscriber[topic_info.name] = _nh.subscribe( + topic_info.name, + 100, + [&, entity_path](const sensor_msgs::Image::ConstPtr& msg) { + log_image(_rec, entity_path, msg); + } + ); + } else if (topic_info.datatype == "sensor_msgs/Imu") { + _topic_to_subscriber[topic_info.name] = _nh.subscribe( + topic_info.name, + 1000, + [&, entity_path](const sensor_msgs::Imu::ConstPtr& msg) { + log_imu(_rec, entity_path, msg); + } + ); + } else if (topic_info.datatype == "geometry_msgs/PoseStamped") { + _topic_to_subscriber[topic_info.name] = _nh.subscribe( + topic_info.name, + 1000, + [&, entity_path](const geometry_msgs::PoseStamped::ConstPtr& msg) { + log_pose_stamped(_rec, entity_path, msg); + } + ); + } else if (topic_info.datatype == "nav_msgs/Odometry") { + _topic_to_subscriber[topic_info.name] = _nh.subscribe( + topic_info.name, + 1000, + [&, entity_path](const nav_msgs::Odometry::ConstPtr& msg) { + log_odometry(_rec, entity_path, msg); + } + ); + } + } + } + + _loop_rate.sleep(); + } +} + +int main(int argc, char** argv) { + ros::init(argc, argv, "rerun_logger_node"); + RerunLoggerNode node; + node.spin(); + return 0; +} diff --git a/rerun/examples/cpp-example-ros-bridge/rerun_bridge/src/rerun_bridge/visualizer_node.hpp b/rerun/examples/cpp-example-ros-bridge/rerun_bridge/src/rerun_bridge/visualizer_node.hpp new file mode 100644 index 0000000..bcc4f50 --- /dev/null +++ b/rerun/examples/cpp-example-ros-bridge/rerun_bridge/src/rerun_bridge/visualizer_node.hpp @@ -0,0 +1,26 @@ +#pragma once + +#include +#include + +#include +#include + + +class RerunLoggerNode { + public: + RerunLoggerNode(); + void spin(); + + private: + std::map _topic_to_entity_path_map; + std::map _topic_to_subscriber; + + void _read_yaml_config(std::string yaml_path); + + std::string _topic_to_entity_path(const std::string& topic) const; + + const rerun::RecordingStream _rec{"rerun_logger_node"}; + ros::NodeHandle _nh{"~"}; + ros::Rate _loop_rate{1000}; +};