├── .clang-format ├── .gitattributes ├── .gitignore ├── LICENSE ├── README.md ├── pixi.lock ├── pixi.toml └── rerun_bridge ├── CMakeLists.txt ├── include └── rerun_bridge │ └── rerun_ros_interface.hpp ├── launch ├── drone_example.launch ├── drone_example_params.yaml ├── spot_example.launch └── spot_example_params.yaml ├── package.xml └── src └── rerun_bridge ├── collection_adapters.hpp ├── rerun_ros_interface.cpp ├── visualizer_node.cpp └── visualizer_node.hpp /.clang-format: -------------------------------------------------------------------------------- 1 | BasedOnStyle: Google 2 | 3 | # Make it slightly more similar to Rust. 4 | # Based loosely on https://gist.github.com/YodaEmbedding/c2c77dc693d11f3734d78489f9a6eea4 5 | AccessModifierOffset: -2 6 | AlignAfterOpenBracket: BlockIndent 7 | AllowAllArgumentsOnNextLine: false 8 | AllowShortBlocksOnASingleLine: false 9 | AllowShortCaseLabelsOnASingleLine: false 10 | AllowShortFunctionsOnASingleLine: Empty 11 | AllowShortIfStatementsOnASingleLine: Never 12 | AlwaysBreakAfterReturnType: None 13 | AlwaysBreakBeforeMultilineStrings: true 14 | BinPackArguments: false 15 | BreakStringLiterals: false 16 | ColumnLimit: 100 17 | ContinuationIndentWidth: 4 18 | DerivePointerAlignment: false 19 | EmptyLineBeforeAccessModifier: LogicalBlock 20 | IndentWidth: 4 21 | IndentWrappedFunctionNames: true 22 | InsertBraces: true 23 | InsertTrailingCommas: Wrapped 24 | MaxEmptyLinesToKeep: 1 25 | NamespaceIndentation: All 26 | PointerAlignment: Left 27 | ReflowComments: false 28 | SeparateDefinitionBlocks: Always 29 | SpacesBeforeTrailingComments: 1 30 | 31 | # Don't change include blocks, we want to control this manually. 32 | # Sorting headers however is allowed as all our headers should be standalone. 33 | IncludeBlocks: Preserve 34 | SortIncludes: CaseInsensitive 35 | -------------------------------------------------------------------------------- /.gitattributes: -------------------------------------------------------------------------------- 1 | # GitHub syntax highlighting 2 | pixi.lock linguist-language=YAML 3 | 4 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # pixi environments 2 | .pixi 3 | 4 | # catkin workspace 5 | noetic_ws 6 | 7 | # miscellaneous 8 | compile_commands.json 9 | *.bag 10 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # C++ Example: ROS Bridge 2 | 3 | This is an example that shows how to use Rerun's C++ API to log and visualize [ROS](https://www.ros.org/) messages. 4 | 5 | It works by subscribing to all topics with supported types, converting the messages, and logging the data to Rerun. It further allows to remap topic names to specific entity paths, specify additional timeless transforms, and pinhole parameters via an external config file. See the [launch](https://github.com/rerun-io/cpp-example-ros-bridge/tree/main/rerun_bridge/launch) directory for usage examples. 6 | 7 | https://github.com/rerun-io/cpp-example-ros-bridge/assets/9785832/fcdf62ad-89f3-47c6-8996-9bc88a5bfb70 8 | 9 | This example is built for ROS 1. For more ROS examples, also check out the [ROS 2 example](https://www.rerun.io/docs/howto/ros2-nav-turtlebot), and the [URDF data-loader](https://github.com/rerun-io/rerun-loader-python-example-urdf). 10 | 11 | > NOTE: Currently only `geometry_msgs/{PoseStamped,TransformStamped}`, `nav_msgs/Odometry`, `tf2_msgs/TFMessage`, and `sensor_msgs/{Image,CameraInfo,Imu}` are supported. However, extending to other messages should be straightforward. 12 | 13 | ## Compile and run using pixi 14 | The easiest way to get started is to install [pixi](https://prefix.dev/docs/pixi/overview). 15 | 16 | The pixi environment described in `pixi.toml` contains all required dependencies, including rosbags, and the Rerun viewer. To run the [drone example](https://fpv.ifi.uzh.ch/datasets/) use 17 | ```bash 18 | pixi run drone_example 19 | ``` 20 | and to run the [Spot example](http://ptak.felk.cvut.cz/darpa-subt/qualification_videos/spot/) use 21 | ```bash 22 | pixi run spot_example 23 | ``` 24 | 25 | ## Compile and run using existing ROS environment 26 | If you have an existing ROS workspace and would like to add the Rerun node to it, clone this repository into the workspace's `src` directory and build the workspace. 27 | -------------------------------------------------------------------------------- /pixi.toml: -------------------------------------------------------------------------------- 1 | [project] 2 | name = "rerun_cpp_example_ros" 3 | authors = ["rerun.io "] 4 | channels = ["robostack-staging", "conda-forge"] 5 | description = "Use the Rerun C++ SDK together with ROS" 6 | homepage = "https://rerun.io" 7 | license = "Apache-2.0" 8 | platforms = ["linux-64", "osx-arm64"] 9 | repository = "https://github.com/rerun-io/cpp-example-ros" 10 | version = "0.1.0" 11 | 12 | [tasks.ws] 13 | cmd = "mkdir -p noetic_ws/src && ln -sfn $(pwd)/rerun_bridge noetic_ws/src/rerun_bridge" 14 | cwd = "." 15 | 16 | [tasks.build] 17 | cmd = "ls && catkin_make --cmake-args -DCMAKE_BUILD_TYPE=Release -DCMAKE_EXPORT_COMPILE_COMMANDS=ON" 18 | depends_on = ["ws"] 19 | cwd = "noetic_ws" 20 | 21 | # Download the drone example rosbag. 22 | # 23 | # To avoid the slow download / unreliable server we only download if the file doesn't already exist. 24 | [tasks.drone_example_data] 25 | cmd = "ls indoor_forward_3_snapdragon_with_gt.bag || curl -L -C - -O http://rpg.ifi.uzh.ch/datasets/uzh-fpv-newer-versions/v3/indoor_forward_3_snapdragon_with_gt.bag" 26 | depends_on = ["ws"] 27 | cwd = "noetic_ws/src/rerun_bridge" 28 | 29 | [tasks.drone_example] 30 | cmd = "bash -c 'source ./devel/setup.bash && roslaunch rerun_bridge drone_example.launch'" 31 | depends_on = ["build", "drone_example_data", "ws", "rerun_viewer"] 32 | cwd = "noetic_ws" 33 | 34 | # Get the spot_description package from the heuristicus/spot_ros repository. 35 | # 36 | # To avoid the slow git clone, we check if the directory already exists. 37 | # To avoid extra dependencies we only use the spot_description package not the whole metapackage. 38 | [tasks.spot_description] 39 | cmd = """ 40 | cd spot_description 41 | || (git clone https://github.com/heuristicus/spot_ros.git 42 | && mv spot_ros/spot_description . 43 | && rm -rf spot_ros) 44 | """ 45 | cwd = "noetic_ws/src" 46 | depends_on = ["ws"] 47 | 48 | # Download the Spot example rosbag. 49 | [tasks.spot_example_data] 50 | cmd = """ 51 | ls spot_ros1/spot_ros1.bag 52 | || (curl -L -C - -O https://storage.googleapis.com/rerun-example-datasets/spot_ros1.zip 53 | && unzip spot_ros1.zip -d spot_ros1) 54 | """ 55 | depends_on = ["ws"] 56 | cwd = "noetic_ws/src/rerun_bridge" 57 | 58 | [tasks.spot_example] 59 | cmd = "bash -c 'source ./devel/setup.bash && roslaunch rerun_bridge spot_example.launch'" 60 | depends_on = [ 61 | "build", 62 | "spot_example_data", 63 | "ws", 64 | "rerun_viewer", 65 | "spot_description", 66 | "rerun_urdf_loader", 67 | ] 68 | cwd = "noetic_ws" 69 | 70 | # Install Rerun and URDF loader manually via pip3, this should be replaced with direct pypi dependencies in the future. 71 | # Wait for direct branch and find-links support in pixi. Otherwise updating to a prerelease becomes a hassle. 72 | # See: 73 | # https://pixi.sh/latest/reference/configuration/#pypi-dependencies-beta-feature 74 | # https://github.com/prefix-dev/pixi/issues/1163 75 | 76 | [tasks.rerun_viewer] 77 | cmd = "pip install rerun-sdk==0.16" 78 | 79 | [tasks.rerun_urdf_loader] 80 | cmd = "pip install git+https://github.com/rerun-io/rerun-loader-python-example-urdf.git" 81 | 82 | [dependencies] 83 | pip = ">=24.0,<25" # To install rerun-sdk and rerun-loader-python-example-urdf 84 | compilers = ">=1.7.0,<1.8" 85 | opencv = ">=4.9.0,<4.10" 86 | ros-noetic-catkin = ">=0.8.10,<0.9" 87 | ros-noetic-desktop = ">=1.5.0,<1.6" 88 | ros-noetic-rosbag = ">=1.16.0,<1.17" 89 | yaml-cpp = ">=0.8.0,<0.9" 90 | unzip = ">=6.0,<7" 91 | 92 | [target.linux-64.dependencies] 93 | sysroot_linux-64 = ">=2.17,<3" # rustc 1.64+ requires glibc 2.17+, see https://blog.rust-lang.org/2022/08/01/Increasing-glibc-kernel-requirements.html 94 | -------------------------------------------------------------------------------- /rerun_bridge/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(rerun_bridge) 3 | 4 | if(NOT DEFINED CMAKE_CXX_STANDARD) 5 | set(CMAKE_CXX_STANDARD 17) 6 | endif() 7 | 8 | find_package(catkin REQUIRED COMPONENTS roscpp roslib sensor_msgs nav_msgs geometry_msgs cv_bridge tf2_ros tf2_msgs) 9 | find_package(OpenCV REQUIRED) 10 | find_package(yaml-cpp REQUIRED) 11 | 12 | include(FetchContent) 13 | FetchContent_Declare(rerun_sdk URL https://github.com/rerun-io/rerun/releases/download/0.16.0/rerun_cpp_sdk.zip) 14 | FetchContent_MakeAvailable(rerun_sdk) 15 | 16 | include_directories( 17 | include 18 | ${YAML_CPP_INCLUDE_DIRS} 19 | ${catkin_INCLUDE_DIRS} 20 | ) 21 | 22 | catkin_package( 23 | INCLUDE_DIRS include 24 | LIBRARIES ${PROJECT_NAME} 25 | CATKIN_DEPENDS roscpp roslib sensor_msgs nav_msgs geometry_msgs cv_bridge tf2_ros tf2_msgs 26 | DEPENDS opencv yaml-cpp 27 | ) 28 | 29 | add_library(${PROJECT_NAME} src/rerun_bridge/rerun_ros_interface.cpp) 30 | add_executable(visualizer src/rerun_bridge/visualizer_node.cpp) 31 | 32 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 33 | add_dependencies(visualizer ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 34 | 35 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBS} ${YAML_CPP_LIBRARIES} rerun_sdk) 36 | target_link_libraries(visualizer ${PROJECT_NAME} ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES} rerun_sdk) 37 | 38 | install(TARGETS visualizer DESTINATION bin) 39 | -------------------------------------------------------------------------------- /rerun_bridge/include/rerun_bridge/rerun_ros_interface.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | void log_imu( 17 | const rerun::RecordingStream& rec, const std::string& entity_path, 18 | const sensor_msgs::Imu::ConstPtr& msg 19 | ); 20 | 21 | void log_image( 22 | const rerun::RecordingStream& rec, const std::string& entity_path, 23 | const sensor_msgs::Image::ConstPtr& msg 24 | ); 25 | 26 | void log_pose_stamped( 27 | const rerun::RecordingStream& rec, const std::string& entity_path, 28 | const geometry_msgs::PoseStamped::ConstPtr& msg 29 | ); 30 | 31 | void log_odometry( 32 | const rerun::RecordingStream& rec, const std::string& entity_path, 33 | const nav_msgs::Odometry::ConstPtr& msg 34 | ); 35 | 36 | void log_camera_info( 37 | const rerun::RecordingStream& rec, const std::string& entity_path, 38 | const sensor_msgs::CameraInfo::ConstPtr& msg 39 | ); 40 | 41 | void log_tf_message( 42 | const rerun::RecordingStream& rec, 43 | const std::map& tf_frame_to_entity_path, 44 | const tf2_msgs::TFMessage::ConstPtr& msg 45 | ); 46 | 47 | void log_transform( 48 | const rerun::RecordingStream& rec, const std::string& entity_path, 49 | const geometry_msgs::TransformStamped& msg 50 | ); 51 | -------------------------------------------------------------------------------- /rerun_bridge/launch/drone_example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | $(find rerun_bridge)/launch/drone_example_params.yaml 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /rerun_bridge/launch/drone_example_params.yaml: -------------------------------------------------------------------------------- 1 | topic_to_entity_path: 2 | /snappy_cam/stereo_l: /groundtruth/pose/snappy_cam/stereo_l 3 | /snappy_cam/stereo_r: /groundtruth/pose/snappy_cam/stereo_r 4 | /groundtruth/pose: /groundtruth/pose 5 | extra_transform3ds: 6 | - entity_path: /groundtruth/pose/snappy_cam/stereo_l 7 | transform: [ 8 | -0.028228787368606456, -0.999601488301944, 1.2175294828553618e-05, 0.02172388268966517, 9 | 0.014401251861751119, -0.00041887083271471837, -0.9998962088597202, -6.605455433829172e-05, 10 | 0.999497743623523, -0.028225682131089447, 0.014407337010089172, -0.00048817563004522853, 11 | 0.0, 0.0, 0.0, 1.0 12 | ] 13 | from_parent: true 14 | - entity_path: /groundtruth/pose/snappy_cam/stereo_r 15 | transform: [ 16 | -0.011823057800830705, -0.9998701444077991, -0.010950325390841398, -0.057904961033265645, 17 | 0.011552991631909482, 0.01081376681432078, -0.9998747875767439, 0.00043766687615362694, 18 | 0.9998633625093938, -0.011948086424720228, 0.011423639621249038, -0.00039944945687402214, 19 | 0.0, 0.0, 0.0, 1.0 20 | ] 21 | from_parent: true 22 | extra_pinholes: 23 | - entity_path: /groundtruth/pose/snappy_cam/stereo_l 24 | height: 480 25 | width: 640 26 | image_from_camera: [278.66723066149086, 0, 319.75221200593535, 0, 278.48991409740296, 241.96858910358173, 0, 0, 1] 27 | - entity_path: /groundtruth/pose/snappy_cam/stereo_r 28 | height: 480 29 | width: 640 30 | image_from_camera: [277.61640629770613, 0, 314.8944703346039, 0, 277.63749695723294, 236.04310050462587, 0, 0, 1] 31 | 32 | -------------------------------------------------------------------------------- /rerun_bridge/launch/spot_example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | $(find rerun_bridge)/launch/spot_example_params.yaml 11 | 12 | 13 | -------------------------------------------------------------------------------- /rerun_bridge/launch/spot_example_params.yaml: -------------------------------------------------------------------------------- 1 | topic_to_entity_path: 2 | /spot/camera/left/image: /odom/body/head/left/left_fisheye 3 | /spot/camera/left/camera_info: /odom/body/head/left/left_fisheye 4 | /spot/camera/right/image: /odom/body/head/right/right_fisheye 5 | /spot/camera/right/camera_info: /odom/body/head/right/right_fisheye 6 | /spot/camera/frontleft/image: /odom/body/head/frontleft/frontleft_fisheye 7 | /spot/camera/frontleft/camera_info: /odom/body/head/frontleft/frontleft_fisheye 8 | /spot/camera/frontright/image: /odom/body/head/frontright/frontright_fisheye 9 | /spot/camera/frontright/camera_info: /odom/body/head/frontright/frontright_fisheye 10 | /spot/camera/back/image: /odom/body/head/back/back_fisheye 11 | /spot/camera/back/camera_info: /odom/body/head/back/back_fisheye 12 | extra_transform3ds: [] 13 | extra_pinholes: [] 14 | tf: 15 | update_rate: 30.0 # set to 0 to log raw tf data instead (i.e., without interoplation) 16 | 17 | # We need to predefine the tf-tree currently to define the entity paths 18 | # See: https://github.com/rerun-io/rerun/issues/5242 19 | tree: 20 | odom: 21 | body: 22 | base_link: {} 23 | flat_body: {} 24 | front_left_hip: 25 | front_left_upper_leg: 26 | front_left_lower_leg: {} 27 | front_rail: {} 28 | front_right_hip: 29 | front_right_upper_leg: 30 | front_right_lower_leg: {} 31 | head: 32 | back: 33 | back_fisheye: {} 34 | frontleft: 35 | frontleft_fisheye: {} 36 | frontright: 37 | frontright_fisheye: {} 38 | left: 39 | left_fisheye: {} 40 | right: 41 | right_fisheye: {} 42 | rear_left_hip: 43 | rear_left_upper_leg: 44 | rear_left_lower_leg: {} 45 | rear_rail: {} 46 | rear_right_hip: 47 | rear_right_upper_leg: 48 | rear_right_lower_leg: {} 49 | vision: {} 50 | gpe: {} 51 | urdf: 52 | file_path: "package://spot_description/urdf/spot.urdf.xacro" 53 | entity_path: "odom" 54 | -------------------------------------------------------------------------------- /rerun_bridge/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rerun_bridge 4 | 0.1.0 5 | The rerun_bridge package 6 | rerun.io 7 | Apache-2.0 8 | cv_bridge 9 | geometry_msgs 10 | nav_msgs 11 | roscpp 12 | roslib 13 | sensor_msgs 14 | tf2_ros 15 | tf2_msgs 16 | yaml-cpp 17 | catkin 18 | 19 | -------------------------------------------------------------------------------- /rerun_bridge/src/rerun_bridge/collection_adapters.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | // Adapters so we can borrow an OpenCV image easily into Rerun images without copying: 7 | template 8 | struct rerun::CollectionAdapter { 9 | /// Borrow for non-temporary. 10 | Collection operator()(const cv::Mat& img) { 11 | return Collection::borrow( 12 | reinterpret_cast(img.data), 13 | img.total() * img.channels() 14 | ); 15 | } 16 | 17 | // Do a full copy for temporaries (otherwise the data might be deleted when the temporary is destroyed). 18 | Collection operator()(cv::Mat&& img) { 19 | std::vector img_vec(img.total() * img.channels()); 20 | img_vec.assign(img.data, img.data + img.total() * img.channels()); 21 | return Collection::take_ownership(std::move(img_vec)); 22 | } 23 | }; 24 | 25 | inline rerun::Collection tensor_shape(const cv::Mat& img) { 26 | return { 27 | static_cast(img.rows), 28 | static_cast(img.cols), 29 | static_cast(img.channels()) 30 | }; 31 | }; 32 | -------------------------------------------------------------------------------- /rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp: -------------------------------------------------------------------------------- 1 | #include "rerun_bridge/rerun_ros_interface.hpp" 2 | #include "collection_adapters.hpp" 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | void log_imu( 9 | const rerun::RecordingStream& rec, const std::string& entity_path, 10 | const sensor_msgs::Imu::ConstPtr& msg 11 | ) { 12 | rec.set_time_seconds("timestamp", msg->header.stamp.toSec()); 13 | 14 | rec.log(entity_path + "/x", rerun::Scalar(msg->linear_acceleration.x)); 15 | rec.log(entity_path + "/y", rerun::Scalar(msg->linear_acceleration.y)); 16 | rec.log(entity_path + "/z", rerun::Scalar(msg->linear_acceleration.z)); 17 | } 18 | 19 | void log_image( 20 | const rerun::RecordingStream& rec, const std::string& entity_path, 21 | const sensor_msgs::Image::ConstPtr& msg 22 | ) { 23 | rec.set_time_seconds("timestamp", msg->header.stamp.toSec()); 24 | 25 | // Depth images are 32-bit float (in meters) or 16-bit uint (in millimeters) 26 | // See: https://ros.org/reps/rep-0118.html 27 | if (msg->encoding == "16UC1") { 28 | cv::Mat img = cv_bridge::toCvCopy(msg)->image; 29 | rec.log( 30 | entity_path, 31 | rerun::DepthImage({img.rows, img.cols}, rerun::TensorBuffer::u16(img)).with_meter(1000) 32 | ); 33 | } else if (msg->encoding == "32FC1") { 34 | // NOTE this has not been tested 35 | cv::Mat img = cv_bridge::toCvCopy(msg)->image; 36 | rec.log( 37 | entity_path, 38 | rerun::DepthImage({img.rows, img.cols}, rerun::TensorBuffer::f32(img)).with_meter(1.0) 39 | ); 40 | } else { 41 | cv::Mat img = cv_bridge::toCvCopy(msg, "rgb8")->image; 42 | rec.log( 43 | entity_path, 44 | rerun::Image(tensor_shape(img), rerun::TensorBuffer::u8(img)) 45 | ); 46 | } 47 | } 48 | 49 | void log_pose_stamped( 50 | const rerun::RecordingStream& rec, const std::string& entity_path, 51 | const geometry_msgs::PoseStamped::ConstPtr& msg 52 | ) { 53 | rec.set_time_seconds("timestamp", msg->header.stamp.toSec()); 54 | 55 | rec.log( 56 | entity_path, 57 | rerun::Transform3D( 58 | rerun::Vector3D(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z), 59 | rerun::Quaternion::from_wxyz( 60 | msg->pose.orientation.w, 61 | msg->pose.orientation.x, 62 | msg->pose.orientation.y, 63 | msg->pose.orientation.z 64 | ) 65 | ) 66 | ); 67 | 68 | // this is a somewhat hacky way to get a trajectory visualization in Rerun 69 | // this should be be easier in the future, see https://github.com/rerun-io/rerun/issues/723 70 | std::string trajectory_entity_path = "/trajectories/" + entity_path; 71 | rec.log( 72 | trajectory_entity_path, 73 | rerun::Points3D( 74 | {{static_cast(msg->pose.position.x), 75 | static_cast(msg->pose.position.y), 76 | static_cast(msg->pose.position.z)}} 77 | ) 78 | ); 79 | } 80 | 81 | void log_tf_message( 82 | const rerun::RecordingStream& rec, 83 | const std::map& tf_frame_to_entity_path, 84 | const tf2_msgs::TFMessage::ConstPtr& msg 85 | ) { 86 | for (const auto& transform : msg->transforms) { 87 | if (tf_frame_to_entity_path.find(transform.child_frame_id) == 88 | tf_frame_to_entity_path.end()) { 89 | ROS_WARN("No entity path for frame_id %s, skipping", transform.child_frame_id.c_str()); 90 | continue; 91 | } 92 | 93 | rec.set_time_seconds("timestamp", transform.header.stamp.toSec()); 94 | 95 | rec.log( 96 | tf_frame_to_entity_path.at(transform.child_frame_id), 97 | rerun::Transform3D( 98 | rerun::Vector3D( 99 | transform.transform.translation.x, 100 | transform.transform.translation.y, 101 | transform.transform.translation.z 102 | ), 103 | rerun::Quaternion::from_wxyz( 104 | transform.transform.rotation.w, 105 | transform.transform.rotation.x, 106 | transform.transform.rotation.y, 107 | transform.transform.rotation.z 108 | ) 109 | ) 110 | ); 111 | } 112 | } 113 | 114 | void log_odometry( 115 | const rerun::RecordingStream& rec, const std::string& entity_path, 116 | const nav_msgs::Odometry::ConstPtr& msg 117 | ) { 118 | rec.set_time_seconds("timestamp", msg->header.stamp.toSec()); 119 | 120 | rec.log( 121 | entity_path, 122 | rerun::Transform3D( 123 | rerun::Vector3D( 124 | msg->pose.pose.position.x, 125 | msg->pose.pose.position.y, 126 | msg->pose.pose.position.z 127 | ), 128 | rerun::Quaternion::from_wxyz( 129 | msg->pose.pose.orientation.w, 130 | msg->pose.pose.orientation.x, 131 | msg->pose.pose.orientation.y, 132 | msg->pose.pose.orientation.z 133 | ) 134 | ) 135 | ); 136 | } 137 | 138 | void log_camera_info( 139 | const rerun::RecordingStream& rec, const std::string& entity_path, 140 | const sensor_msgs::CameraInfo::ConstPtr& msg 141 | ) { 142 | // Rerun uses column-major order for Mat3x3 143 | const std::array image_from_camera = { 144 | static_cast(msg->K[0]), 145 | static_cast(msg->K[3]), 146 | static_cast(msg->K[6]), 147 | static_cast(msg->K[1]), 148 | static_cast(msg->K[4]), 149 | static_cast(msg->K[7]), 150 | static_cast(msg->K[2]), 151 | static_cast(msg->K[5]), 152 | static_cast(msg->K[8]), 153 | }; 154 | rec.log( 155 | entity_path, 156 | rerun::Pinhole(image_from_camera) 157 | .with_resolution(static_cast(msg->width), static_cast(msg->height)) 158 | ); 159 | } 160 | 161 | void log_transform( 162 | const rerun::RecordingStream& rec, const std::string& entity_path, 163 | const geometry_msgs::TransformStamped& msg 164 | ) { 165 | rec.set_time_seconds("timestamp", msg.header.stamp.toSec()); 166 | 167 | rec.log( 168 | entity_path, 169 | rerun::Transform3D( 170 | rerun::Vector3D( 171 | msg.transform.translation.x, 172 | msg.transform.translation.y, 173 | msg.transform.translation.z 174 | ), 175 | rerun::Quaternion::from_wxyz( 176 | msg.transform.rotation.w, 177 | msg.transform.rotation.x, 178 | msg.transform.rotation.y, 179 | msg.transform.rotation.z 180 | ) 181 | ) 182 | ); 183 | } 184 | -------------------------------------------------------------------------------- /rerun_bridge/src/rerun_bridge/visualizer_node.cpp: -------------------------------------------------------------------------------- 1 | #include "visualizer_node.hpp" 2 | #include "rerun_bridge/rerun_ros_interface.hpp" 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | std::string parent_entity_path(const std::string& entity_path) { 15 | auto last_slash = entity_path.rfind('/'); 16 | if (last_slash == std::string::npos) { 17 | return ""; 18 | } 19 | return entity_path.substr(0, last_slash); 20 | } 21 | 22 | std::string resolve_ros_path(const std::string& path) { 23 | if (path.find("package://") == 0) { 24 | std::string package_name = path.substr(10, path.find('/', 10) - 10); 25 | std::string relative_path = path.substr(10 + package_name.size()); 26 | std::string package_path = ros::package::getPath(package_name); 27 | if (package_path.empty()) { 28 | throw std::runtime_error( 29 | "Could not resolve " + path + 30 | ". Replace with relative / absolute path, source the correct ROS environment, or install " + 31 | package_name + "." 32 | ); 33 | } 34 | return ros::package::getPath(package_name) + relative_path; 35 | } else if (path.find("file://") == 0) { 36 | return path.substr(7); 37 | } else { 38 | return path; 39 | } 40 | } 41 | 42 | RerunLoggerNode::RerunLoggerNode() { 43 | _rec.spawn().exit_on_failure(); 44 | 45 | // Read additional config from yaml file 46 | // NOTE We're not using the ROS parameter server for this, because roscpp doesn't support 47 | // reading nested data structures. 48 | std::string yaml_path; 49 | if (_nh.getParam("yaml_path", yaml_path)) { 50 | ROS_INFO("Read yaml config at %s", yaml_path.c_str()); 51 | } 52 | _read_yaml_config(yaml_path); 53 | } 54 | 55 | /// Convert a topic name to its entity path. 56 | /// If the topic is explicitly mapped to an entity path, use that. 57 | /// Otherwise, the topic name will be automatically converted to a flattened entity path like this: 58 | /// "/one/two/three/four" -> "/topics/one-two-three/four" 59 | std::string RerunLoggerNode::_resolve_entity_path(const std::string& topic) const { 60 | if (_topic_to_entity_path.find(topic) != _topic_to_entity_path.end()) { 61 | return _topic_to_entity_path.at(topic); 62 | } else { 63 | std::string flattened_topic = topic; 64 | auto last_slash = 65 | (std::find(flattened_topic.rbegin(), flattened_topic.rend(), '/') + 1).base(); 66 | 67 | if (last_slash != flattened_topic.begin()) { 68 | // keep leading slash and last slash 69 | std::replace(flattened_topic.begin() + 1, last_slash, '/', '-'); 70 | } 71 | 72 | return "/topics" + flattened_topic; 73 | } 74 | } 75 | 76 | void RerunLoggerNode::_read_yaml_config(std::string yaml_path) { 77 | const YAML::Node config = YAML::LoadFile(yaml_path); 78 | 79 | // see https://www.rerun.io/docs/howto/ros2-nav-turtlebot#tf-to-rrtransform3d 80 | if (config["topic_to_entity_path"]) { 81 | _topic_to_entity_path = 82 | config["topic_to_entity_path"].as>(); 83 | 84 | for (auto const& [key, val] : _topic_to_entity_path) { 85 | ROS_INFO("Mapping topic %s to entity path %s", key.c_str(), val.c_str()); 86 | } 87 | } 88 | if (config["extra_transform3ds"]) { 89 | for (const auto& extra_transform3d : config["extra_transform3ds"]) { 90 | const std::array translation = { 91 | extra_transform3d["transform"][3].as(), 92 | extra_transform3d["transform"][7].as(), 93 | extra_transform3d["transform"][11].as() 94 | }; 95 | // Rerun uses column-major order for Mat3x3 96 | const std::array mat3x3 = { 97 | extra_transform3d["transform"][0].as(), 98 | extra_transform3d["transform"][4].as(), 99 | extra_transform3d["transform"][8].as(), 100 | extra_transform3d["transform"][1].as(), 101 | extra_transform3d["transform"][5].as(), 102 | extra_transform3d["transform"][9].as(), 103 | extra_transform3d["transform"][2].as(), 104 | extra_transform3d["transform"][6].as(), 105 | extra_transform3d["transform"][10].as() 106 | }; 107 | _rec.log_static( 108 | extra_transform3d["entity_path"].as(), 109 | rerun::Transform3D( 110 | rerun::Vec3D(translation), 111 | rerun::Mat3x3(mat3x3), 112 | extra_transform3d["from_parent"].as() 113 | ) 114 | ); 115 | } 116 | } 117 | if (config["extra_pinholes"]) { 118 | for (const auto& extra_pinhole : config["extra_pinholes"]) { 119 | // Rerun uses column-major order for Mat3x3 120 | const std::array image_from_camera = { 121 | extra_pinhole["image_from_camera"][0].as(), 122 | extra_pinhole["image_from_camera"][3].as(), 123 | extra_pinhole["image_from_camera"][6].as(), 124 | extra_pinhole["image_from_camera"][1].as(), 125 | extra_pinhole["image_from_camera"][4].as(), 126 | extra_pinhole["image_from_camera"][7].as(), 127 | extra_pinhole["image_from_camera"][2].as(), 128 | extra_pinhole["image_from_camera"][5].as(), 129 | extra_pinhole["image_from_camera"][8].as(), 130 | }; 131 | _rec.log_static( 132 | extra_pinhole["entity_path"].as(), 133 | rerun::Pinhole(image_from_camera) 134 | .with_resolution( 135 | extra_pinhole["width"].as(), 136 | extra_pinhole["height"].as() 137 | ) 138 | ); 139 | } 140 | } 141 | if (config["tf"]) { 142 | if (config["tf"]["update_rate"]) { 143 | _tf_fixed_rate = config["tf"]["update_rate"].as(); 144 | } 145 | 146 | if (config["tf"]["tree"]) { 147 | // set root frame, all messages with frame_id will be logged relative to this frame 148 | _root_frame = config["tf"]["tree"].begin()->first.as(); 149 | 150 | // recurse through the tree and add all transforms 151 | _add_tf_tree(config["tf"]["tree"], "", ""); 152 | } 153 | } 154 | 155 | if (config["urdf"]) { 156 | std::string urdf_entity_path; 157 | if (config["urdf"]["entity_path"]) { 158 | urdf_entity_path = config["urdf"]["entity_path"].as(); 159 | } 160 | if (config["urdf"]["file_path"]) { 161 | std::string urdf_file_path = 162 | resolve_ros_path(config["urdf"]["file_path"].as()); 163 | ROS_INFO("Logging URDF from file path %s", urdf_file_path.c_str()); 164 | _rec.log_file_from_path(urdf_file_path, urdf_entity_path, true); 165 | } 166 | } 167 | } 168 | 169 | void RerunLoggerNode::_add_tf_tree( 170 | const YAML::Node& node, const std::string& parent_entity_path, const ::std::string& parent_frame 171 | ) { 172 | for (const auto& child : node) { 173 | auto frame = child.first.as(); 174 | auto value = child.second; 175 | const std::string entity_path = parent_entity_path + "/" + frame; 176 | _tf_frame_to_entity_path[frame] = entity_path; 177 | _tf_frame_to_parent[frame] = parent_frame; 178 | ROS_INFO("Mapping tf frame %s to entity path %s", frame.c_str(), entity_path.c_str()); 179 | if (value.size() >= 1) { 180 | _add_tf_tree(value, entity_path, frame); 181 | } 182 | } 183 | } 184 | 185 | void RerunLoggerNode::_create_subscribers() { 186 | ros::master::V_TopicInfo topic_infos; 187 | ros::master::getTopics(topic_infos); 188 | for (const auto& topic_info : topic_infos) { 189 | // already subscribed to this topic? 190 | if (_topic_to_subscriber.find(topic_info.name) != _topic_to_subscriber.end()) { 191 | continue; 192 | } 193 | 194 | if (topic_info.datatype == "sensor_msgs/Image") { 195 | _topic_to_subscriber[topic_info.name] = _create_image_subscriber(topic_info.name); 196 | } else if (topic_info.datatype == "sensor_msgs/Imu") { 197 | _topic_to_subscriber[topic_info.name] = _create_imu_subscriber(topic_info.name); 198 | } else if (topic_info.datatype == "geometry_msgs/PoseStamped") { 199 | _topic_to_subscriber[topic_info.name] = 200 | _create_pose_stamped_subscriber(topic_info.name); 201 | } else if (topic_info.datatype == "tf2_msgs/TFMessage") { 202 | _topic_to_subscriber[topic_info.name] = _create_tf_message_subscriber(topic_info.name); 203 | } else if (topic_info.datatype == "nav_msgs/Odometry") { 204 | _topic_to_subscriber[topic_info.name] = _create_odometry_subscriber(topic_info.name); 205 | } else if (topic_info.datatype == "sensor_msgs/CameraInfo") { 206 | _topic_to_subscriber[topic_info.name] = _create_camera_info_subscriber(topic_info.name); 207 | } 208 | } 209 | } 210 | 211 | void RerunLoggerNode::_update_tf() const { 212 | // NOTE We log the interpolated transforms with an offset assuming the whole tree has 213 | // been updated after this offset. This is not an ideal solution. If a frame is updated 214 | // with a delay longer than this offset we will never log interpolated transforms for it. 215 | // It might be possible to always log the interpolated transforms on a per frame basis whenever 216 | // a new message is received for that frame. This would require maintaining the latest 217 | // transform for each frame. However, this would not work if transforms for a frame arrive 218 | // out of order (maybe this is not a problem in practice?). 219 | 220 | auto now = ros::Time::now(); 221 | for (const auto& [frame, entity_path] : _tf_frame_to_entity_path) { 222 | auto parent = _tf_frame_to_parent.find(frame); 223 | if (parent == _tf_frame_to_parent.end() or parent->second.empty()) { 224 | continue; 225 | } 226 | try { 227 | auto transform = 228 | _tf_buffer.lookupTransform(parent->second, frame, now - ros::Duration(1.0)); 229 | log_transform(_rec, entity_path, transform); 230 | } catch (tf2::TransformException& ex) { 231 | ROS_WARN_THROTTLE( 232 | 1.0, 233 | "Skipping interpolated logging for %s -> %s because %s", 234 | parent->second.c_str(), 235 | frame.c_str(), 236 | ex.what() 237 | ); 238 | } 239 | } 240 | } 241 | 242 | ros::Subscriber RerunLoggerNode::_create_image_subscriber(const std::string& topic) { 243 | std::string entity_path = _resolve_entity_path(topic); 244 | bool lookup_transform = (_topic_to_entity_path.find(topic) == _topic_to_entity_path.end()); 245 | 246 | return _nh.subscribe( 247 | topic, 248 | 100, 249 | [&, entity_path, lookup_transform](const sensor_msgs::Image::ConstPtr& msg) { 250 | if (!_root_frame.empty() && lookup_transform) { 251 | try { 252 | auto transform = _tf_buffer.lookupTransform( 253 | _root_frame, 254 | msg->header.frame_id, 255 | msg->header.stamp, 256 | ros::Duration(0.1) 257 | ); 258 | log_transform(_rec, parent_entity_path(entity_path), transform); 259 | } catch (tf2::TransformException& ex) { 260 | ROS_WARN("%s", ex.what()); 261 | } 262 | } 263 | log_image(_rec, entity_path, msg); 264 | } 265 | ); 266 | } 267 | 268 | ros::Subscriber RerunLoggerNode::_create_imu_subscriber(const std::string& topic) { 269 | std::string entity_path = _resolve_entity_path(topic); 270 | 271 | return _nh.subscribe( 272 | topic, 273 | 100, 274 | [&, entity_path](const sensor_msgs::Imu::ConstPtr& msg) { log_imu(_rec, entity_path, msg); } 275 | ); 276 | } 277 | 278 | ros::Subscriber RerunLoggerNode::_create_pose_stamped_subscriber(const std::string& topic) { 279 | std::string entity_path = _resolve_entity_path(topic); 280 | 281 | return _nh.subscribe( 282 | topic, 283 | 100, 284 | [&, entity_path](const geometry_msgs::PoseStamped::ConstPtr& msg) { 285 | log_pose_stamped(_rec, entity_path, msg); 286 | } 287 | ); 288 | } 289 | 290 | ros::Subscriber RerunLoggerNode::_create_tf_message_subscriber(const std::string& topic) { 291 | std::string entity_path = _resolve_entity_path(topic); 292 | 293 | return _nh 294 | .subscribe(topic, 100, [&](const tf2_msgs::TFMessage::ConstPtr& msg) { 295 | log_tf_message(_rec, _tf_frame_to_entity_path, msg); 296 | }); 297 | } 298 | 299 | ros::Subscriber RerunLoggerNode::_create_odometry_subscriber(const std::string& topic) { 300 | std::string entity_path = _resolve_entity_path(topic); 301 | 302 | return _nh.subscribe( 303 | topic, 304 | 100, 305 | [&, entity_path](const nav_msgs::Odometry::ConstPtr& msg) { 306 | log_odometry(_rec, entity_path, msg); 307 | } 308 | ); 309 | } 310 | 311 | ros::Subscriber RerunLoggerNode::_create_camera_info_subscriber(const std::string& topic) { 312 | std::string entity_path = _resolve_entity_path(topic); 313 | 314 | // If the camera_info topic has not been explicility mapped to an entity path, 315 | // we assume that the camera_info topic is a sibling of the image topic, and 316 | // hence use the parent as the entity path for the pinhole model. 317 | if (_topic_to_entity_path.find(topic) == _topic_to_entity_path.end()) { 318 | entity_path = parent_entity_path(entity_path); 319 | } 320 | 321 | return _nh.subscribe( 322 | topic, 323 | 100, 324 | [&, entity_path](const sensor_msgs::CameraInfo::ConstPtr& msg) { 325 | log_camera_info(_rec, entity_path, msg); 326 | } 327 | ); 328 | } 329 | 330 | void RerunLoggerNode::spin() { 331 | // check for new topics every 0.1 seconds 332 | ros::Timer timer = 333 | _nh.createTimer(ros::Duration(0.1), [&](const ros::TimerEvent&) { _create_subscribers(); }); 334 | 335 | ros::Timer tf_timer; 336 | if (_tf_fixed_rate != 0.0) { 337 | tf_timer = 338 | _nh.createTimer(ros::Duration(1.0 / _tf_fixed_rate), [&](const ros::TimerEvent&) { 339 | _update_tf(); 340 | }); 341 | } 342 | 343 | ros::MultiThreadedSpinner spinner(8); // Use 8 threads 344 | spinner.spin(); 345 | } 346 | 347 | int main(int argc, char** argv) { 348 | ros::init(argc, argv, "rerun_logger_node"); 349 | RerunLoggerNode node; 350 | node.spin(); 351 | return 0; 352 | } 353 | -------------------------------------------------------------------------------- /rerun_bridge/src/rerun_bridge/visualizer_node.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | class RerunLoggerNode { 13 | public: 14 | RerunLoggerNode(); 15 | void spin(); 16 | 17 | private: 18 | std::map _topic_to_entity_path; 19 | std::map _topic_to_subscriber; 20 | std::map _tf_frame_to_entity_path; 21 | std::map _tf_frame_to_parent; 22 | 23 | void _read_yaml_config(std::string yaml_path); 24 | 25 | std::string _resolve_entity_path(const std::string& topic) const; 26 | 27 | void _add_tf_tree(const YAML::Node& node, const std::string& parent_entity_path, const std::string& parent_frame); 28 | 29 | const rerun::RecordingStream _rec{"rerun_logger_node"}; 30 | ros::NodeHandle _nh{"~"}; 31 | std::string _root_frame; 32 | float _tf_fixed_rate; 33 | tf2_ros::Buffer _tf_buffer; 34 | tf2_ros::TransformListener _tf_listener{_tf_buffer}; 35 | 36 | void _create_subscribers(); 37 | void _update_tf() const; 38 | 39 | /* Message specific subscriber factory functions */ 40 | ros::Subscriber _create_image_subscriber(const std::string& topic); 41 | ros::Subscriber _create_imu_subscriber(const std::string& topic); 42 | ros::Subscriber _create_pose_stamped_subscriber(const std::string& topic); 43 | ros::Subscriber _create_tf_message_subscriber(const std::string& topic); 44 | ros::Subscriber _create_odometry_subscriber(const std::string& topic); 45 | ros::Subscriber _create_camera_info_subscriber(const std::string& topic); 46 | }; 47 | --------------------------------------------------------------------------------