├── resource └── rclcppyy ├── media ├── benchmark_pub_sub_1k_hz.png ├── benchmark_pub_sub_10k_hz.png └── rclcppyy_presentation_logo.jpg ├── scripts ├── big_messages_demos │ ├── pr2_robot_magic_lab.jpg │ ├── images_sub.py │ └── demo_images.py ├── target ├── benchmarks │ ├── bench_pub_rclpy.py │ ├── bench_pub_rclcppyy_monkeypatch.py │ ├── bench_sub_rclpy.py │ ├── bench_sub_rclcppyy_monkeypatch2.py │ ├── bench_pub_rclcppyy.py │ ├── bench_sub_rclcppyy_monkeypatched.py │ └── bench_sub_rclcppyy.py ├── ros_tutorials │ └── publisher_member_function.py ├── bag2bag_py_images.py ├── bag2bag_cppyy_hybrid.py ├── bench_rosbag2_py_apis.py ├── bag2bag_cppyy_purecpp.py ├── bench_rosbag2_cpp_vs_py.py └── create_stubs.py ├── roscon_uk_2025 ├── reliability_override.yaml ├── presentation │ ├── .hypothesis │ │ ├── unicode_data │ │ │ └── 15.0.0 │ │ │ │ └── charmap.json.gz │ │ └── examples │ │ │ ├── 04e6b3400353b141 │ │ │ ├── 274656f5a34e416d │ │ │ └── 71d89507725dbee6 │ │ │ ├── 274656f5a34e416d │ │ │ └── 2f62ebbbbeda0b51 │ │ │ └── 71d89507725dbee6 │ │ │ ├── 4a27b2917e14db1b │ │ │ ├── 4bdaab7c14705eec │ │ │ ├── a09b95f6791bb286 │ │ │ └── e03886aeb91d68f0 │ ├── 0_setup_pointcloud.sh │ ├── 1_pointcloud_passthrough.py │ ├── 5_hypothesis_test.py │ ├── 2_pointcloud_voxelgrid.py │ ├── 4_gstreamer_cli.py │ └── 3_cloudini_cli.py └── pointcloud.rviz ├── test ├── test_node.py └── test_bringup.py ├── package.xml ├── setup.py ├── fix_cppyy_api_path.sh ├── CMakeLists.txt ├── LICENSE ├── rclcppyy ├── __init__.py ├── monkey.py ├── serialization.py ├── monkeypatch_messages.py ├── rosbag2_py_compat.py ├── rosbag2_cpp.py ├── bringup_rclcpp.py └── node.py └── README.md /resource/rclcppyy: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /media/benchmark_pub_sub_1k_hz.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awesomebytes/rclcppyy/HEAD/media/benchmark_pub_sub_1k_hz.png -------------------------------------------------------------------------------- /media/benchmark_pub_sub_10k_hz.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awesomebytes/rclcppyy/HEAD/media/benchmark_pub_sub_10k_hz.png -------------------------------------------------------------------------------- /media/rclcppyy_presentation_logo.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awesomebytes/rclcppyy/HEAD/media/rclcppyy_presentation_logo.jpg -------------------------------------------------------------------------------- /scripts/big_messages_demos/pr2_robot_magic_lab.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awesomebytes/rclcppyy/HEAD/scripts/big_messages_demos/pr2_robot_magic_lab.jpg -------------------------------------------------------------------------------- /roscon_uk_2025/reliability_override.yaml: -------------------------------------------------------------------------------- 1 | /tf_static: 2 | reliability: reliable 3 | history: keep_last 4 | depth: 10 5 | durability: transient_local 6 | -------------------------------------------------------------------------------- /roscon_uk_2025/presentation/.hypothesis/unicode_data/15.0.0/charmap.json.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awesomebytes/rclcppyy/HEAD/roscon_uk_2025/presentation/.hypothesis/unicode_data/15.0.0/charmap.json.gz -------------------------------------------------------------------------------- /roscon_uk_2025/presentation/.hypothesis/examples/04e6b3400353b141/274656f5a34e416d: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awesomebytes/rclcppyy/HEAD/roscon_uk_2025/presentation/.hypothesis/examples/04e6b3400353b141/274656f5a34e416d -------------------------------------------------------------------------------- /roscon_uk_2025/presentation/.hypothesis/examples/04e6b3400353b141/71d89507725dbee6: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awesomebytes/rclcppyy/HEAD/roscon_uk_2025/presentation/.hypothesis/examples/04e6b3400353b141/71d89507725dbee6 -------------------------------------------------------------------------------- /roscon_uk_2025/presentation/.hypothesis/examples/274656f5a34e416d/2f62ebbbbeda0b51: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awesomebytes/rclcppyy/HEAD/roscon_uk_2025/presentation/.hypothesis/examples/274656f5a34e416d/2f62ebbbbeda0b51 -------------------------------------------------------------------------------- /roscon_uk_2025/presentation/.hypothesis/examples/71d89507725dbee6/4a27b2917e14db1b: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awesomebytes/rclcppyy/HEAD/roscon_uk_2025/presentation/.hypothesis/examples/71d89507725dbee6/4a27b2917e14db1b -------------------------------------------------------------------------------- /roscon_uk_2025/presentation/.hypothesis/examples/71d89507725dbee6/4bdaab7c14705eec: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awesomebytes/rclcppyy/HEAD/roscon_uk_2025/presentation/.hypothesis/examples/71d89507725dbee6/4bdaab7c14705eec -------------------------------------------------------------------------------- /roscon_uk_2025/presentation/.hypothesis/examples/71d89507725dbee6/a09b95f6791bb286: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awesomebytes/rclcppyy/HEAD/roscon_uk_2025/presentation/.hypothesis/examples/71d89507725dbee6/a09b95f6791bb286 -------------------------------------------------------------------------------- /roscon_uk_2025/presentation/.hypothesis/examples/71d89507725dbee6/e03886aeb91d68f0: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awesomebytes/rclcppyy/HEAD/roscon_uk_2025/presentation/.hypothesis/examples/71d89507725dbee6/e03886aeb91d68f0 -------------------------------------------------------------------------------- /scripts/target: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import rclpy 4 | from rclcppyy import Node 5 | 6 | 7 | def main(): 8 | rclpy.init() 9 | 10 | node = Node('target_node') 11 | node.get_logger().info('Target script is running!') 12 | 13 | try: 14 | rclpy.spin(node) 15 | except KeyboardInterrupt: 16 | pass 17 | finally: 18 | node.get_logger().info('Shutting down...') 19 | node.destroy_node() 20 | rclpy.shutdown() 21 | 22 | 23 | if __name__ == '__main__': 24 | main() -------------------------------------------------------------------------------- /test/test_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import unittest 4 | 5 | import rclpy 6 | from rclcppyy import Node 7 | 8 | 9 | class TestNode(unittest.TestCase): 10 | 11 | def setUp(self): 12 | rclpy.init() 13 | 14 | def tearDown(self): 15 | rclpy.shutdown() 16 | 17 | def test_node_creation(self): 18 | node = Node('test_node') 19 | self.assertEqual(node.get_name(), 'test_node') 20 | node.destroy_node() 21 | 22 | 23 | if __name__ == '__main__': 24 | unittest.main() -------------------------------------------------------------------------------- /roscon_uk_2025/presentation/0_setup_pointcloud.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | export ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST 3 | export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp 4 | 5 | # Setup cleanup trap 6 | trap 'kill $(jobs -p)' EXIT 7 | 8 | echo "Playing pointcloud bag..." 9 | ros2 bag play --loop ../pointcloud_lexus3-2024-04-05-gyor.mcap --qos-profile-overrides-path ../reliability_override.yaml & 10 | 11 | echo "Running Rviz2..." 12 | source /opt/ros/jazzy/setup.bash 13 | export ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST 14 | export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp 15 | /opt/ros/jazzy/bin/rviz2 -d ../pointcloud.rviz 16 | 17 | # Wait for all background processes 18 | wait -------------------------------------------------------------------------------- /test/test_bringup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import unittest 4 | import cppyy 5 | 6 | from rclcppyy.bringup_rclcpp import bringup_rclcpp 7 | 8 | 9 | class TestBringup(unittest.TestCase): 10 | 11 | def test_bringup_rclcpp(self): 12 | # Should not raise any exceptions 13 | bringup_rclcpp() 14 | 15 | # After bringing up rclcpp, we should be able to access rclcpp classes 16 | # through cppyy's global namespace 17 | node_attributes = dir(cppyy.gbl.rclcpp.Node) 18 | 19 | # The Node class should have some basic expected attributes 20 | self.assertIn('get_name', node_attributes) 21 | self.assertIn('get_namespace', node_attributes) 22 | 23 | 24 | if __name__ == '__main__': 25 | unittest.main() 26 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | rclcppyy 5 | 0.0.1 6 | rclcpp bindings via cppyy and examples on how to use cppyy in ROS2 7 | Sam Pfeiffer 8 | BSD 9 | 10 | ament_cmake 11 | ament_cmake_python 12 | 13 | rclpy 14 | rclcpp 15 | 16 | ament_lint_auto 17 | ament_lint_common 18 | python3-pytest 19 | 20 | 21 | ament_cmake 22 | 23 | 24 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | import os 3 | from glob import glob 4 | 5 | package_name = 'rclcppyy' 6 | 7 | setup( 8 | name=package_name, 9 | version='0.0.1', 10 | packages=[package_name], 11 | data_files=[ 12 | ('share/ament_index/resource_index/packages', 13 | ['resource/' + package_name]), 14 | ('share/' + package_name, ['package.xml']), 15 | (os.path.join('share', package_name), glob('launch/*launch.[pxy][yma]*')), 16 | ], 17 | install_requires=['setuptools'], 18 | zip_safe=True, 19 | maintainer='Sam Pfeiffer', 20 | maintainer_email='sammypfeiffer@gmail.com', 21 | description='rclcpp bindings via cppyy and examples on how to use cppyy in ROS2', 22 | license='BSD', 23 | tests_require=['pytest'], 24 | entry_points={ 25 | 'console_scripts': [ 26 | # Entry points are not needed here as we're using the CMake install method 27 | ], 28 | }, 29 | ) -------------------------------------------------------------------------------- /fix_cppyy_api_path.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Check if $PIXI_PROJECT_ROOT/.pixi/envs/default/include/python3.12/CPyCppyy exists 3 | if [ ! -d "$PIXI_PROJECT_ROOT/.pixi/envs/default/include/python3.12/CPyCppyy" ]; then 4 | echo "CPyCppyy not found in $PIXI_PROJECT_ROOT/.pixi/envs/default/include/python3.12/CPyCppyy" 5 | # Check if $PIXI_PROJECT_ROOT/.pixi/envs/default/include/python3.12/cpycppyy exists 6 | if [ ! -d "$PIXI_PROJECT_ROOT/.pixi/envs/default/include/python3.12/cpycppyy" ]; then 7 | echo "cpycppyy not found in $PIXI_PROJECT_ROOT/.pixi/envs/default/include/python3.12/cpycppyy" 8 | exit 1 9 | else 10 | # Create a symlink from cpycppyy to CPyCppyy 11 | echo "Found cpycppyy in $PIXI_PROJECT_ROOT/.pixi/envs/default/include/python3.12/cpycppyy, creating symlink to CPyCppyy" 12 | ln -s $PIXI_PROJECT_ROOT/.pixi/envs/default/include/python3.12/cpycppyy $PIXI_PROJECT_ROOT/.pixi/envs/default/include/python3.12/CPyCppyy 13 | fi 14 | fi 15 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(rclcppyy) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(ament_cmake_python REQUIRED) 11 | find_package(rclpy REQUIRED) 12 | find_package(rclcpp REQUIRED) 13 | 14 | # Install Python modules 15 | ament_python_install_package(${PROJECT_NAME}) 16 | 17 | # Install Python executables 18 | install(DIRECTORY scripts/ 19 | DESTINATION lib/${PROJECT_NAME} 20 | USE_SOURCE_PERMISSIONS 21 | PATTERN "*.py" 22 | ) 23 | 24 | if(BUILD_TESTING) 25 | find_package(ament_lint_auto REQUIRED) 26 | # the following line skips the linter which checks for copyrights 27 | # comment the line when a copyright and license is added to all source files 28 | set(ament_cmake_copyright_FOUND TRUE) 29 | # the following line skips cpplint (only works in a git repo) 30 | # comment the line when this package is in a git repo and when 31 | # a copyright and license is added to all source files 32 | set(ament_cmake_cpplint_FOUND TRUE) 33 | ament_lint_auto_find_test_dependencies() 34 | endif() 35 | 36 | ament_package() 37 | -------------------------------------------------------------------------------- /roscon_uk_2025/presentation/1_pointcloud_passthrough.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | import rclcppyy; rclcppyy.enable_cpp_acceleration() 3 | import rclpy 4 | from rclpy.node import Node 5 | from sensor_msgs.msg import PointCloud2 6 | from rclpy.qos import qos_profile_sensor_data 7 | 8 | class ImageSubscriber(Node): 9 | def __init__(self) -> None: 10 | super().__init__("pointcloud_passthrough") 11 | self.publisher = self.create_publisher(PointCloud2, "/pointcloud_passthrough", qos_profile_sensor_data) 12 | # Subscribe to the same topic as the demo publisher 13 | self.subscription = self.create_subscription( 14 | PointCloud2, 15 | "/lexus3/os_center/points", 16 | self._on_pointcloud, 17 | 10, 18 | ) 19 | print("subscription created") 20 | 21 | def _on_pointcloud(self, msg: PointCloud2) -> None: 22 | print(f"callback, received pointcloud.") 23 | self.publisher.publish(msg) 24 | 25 | def main() -> None: 26 | rclpy.init() 27 | node = ImageSubscriber() 28 | try: 29 | rclpy.spin(node) 30 | finally: 31 | node.destroy_node() 32 | rclpy.shutdown() 33 | 34 | 35 | if __name__ == "__main__": 36 | main() 37 | 38 | 39 | -------------------------------------------------------------------------------- /roscon_uk_2025/presentation/5_hypothesis_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | from hypothesis import given, strategies as st 3 | import cppyy 4 | 5 | # A function that accepts strings but will throw if the string contains "DDS" 6 | cppyy.cppdef(""" 7 | #include 8 | void accept_string(std::string s) { 9 | if (s.find("DDS") != std::string::npos) { 10 | throw std::runtime_error("Oh no! It's DDS!"); 11 | } 12 | return; 13 | } 14 | """) 15 | 16 | # Example C++ function that accepts strings 17 | accept_string_cpp = cppyy.gbl.accept_string 18 | @given(st.text(alphabet="ABCDES", min_size=3, max_size=9)) 19 | def test_cpp_strings(s): 20 | print(f"(c++) called with {s}") 21 | accept_string_cpp(s) 22 | assert isinstance(s, str) 23 | 24 | # Example pure python 25 | @given(st.integers()) 26 | def test_integers(n): 27 | print(f"called with {n}") 28 | assert isinstance(n, int) 29 | 30 | test_integers() 31 | 32 | test_cpp_strings() 33 | 34 | # # Define a C++ functon that takes an integer and prints it 35 | # cppyy.cppdef(""" 36 | # #include 37 | # void accept_integer(int n) { 38 | # // We do nothing 39 | # return; 40 | # } 41 | # """) 42 | 43 | # # Example C++ function 44 | # accept_integer_cpp = cppyy.gbl.accept_integer 45 | # @given(st.integers()) 46 | # def test_cpp_integers(n): 47 | # print(f"(c++) called with {n}") 48 | # accept_integer_cpp(n) 49 | # assert isinstance(n, int) 50 | 51 | # test_cpp_integers() 52 | 53 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2025, Sam Pfeiffer 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | -------------------------------------------------------------------------------- /scripts/big_messages_demos/images_sub.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | import rclcppyy; rclcppyy.enable_cpp_acceleration() 3 | import rclpy 4 | from rclpy.node import Node 5 | from sensor_msgs.msg import Image 6 | 7 | 8 | class ImageSubscriber(Node): 9 | def __init__(self) -> None: 10 | super().__init__("image_subscriber") 11 | self.message_count = 0 12 | # Subscribe to the same topic as the demo publisher 13 | self.subscription = self.create_subscription( 14 | Image, 15 | "image", 16 | self._on_image, 17 | 10, 18 | ) 19 | print("subscription created") 20 | 21 | def _on_image(self, msg: Image) -> None: 22 | print(f"callback, received image: {msg.width}x{msg.height}, encoding: {msg.encoding}") 23 | print(msg) 24 | print(dir(msg)) 25 | self.message_count += 1 26 | if self.message_count % 100 == 0: 27 | width = getattr(msg, "width", 0) 28 | height = getattr(msg, "height", 0) 29 | data_size = len(getattr(msg, "data", [])) 30 | stamp = getattr(msg, "header", None) 31 | if stamp is not None: 32 | stamp_str = f"{stamp.stamp.sec}.{stamp.stamp.nanosec:09d}" 33 | else: 34 | stamp_str = "n/a" 35 | print( 36 | f"Received {self.message_count} images | stamp={stamp_str} | " 37 | f"resolution={width}x{height} | bytes={data_size}" 38 | ) 39 | 40 | 41 | def main() -> None: 42 | rclpy.init() 43 | node = ImageSubscriber() 44 | try: 45 | rclpy.spin(node) 46 | finally: 47 | node.destroy_node() 48 | rclpy.shutdown() 49 | 50 | 51 | if __name__ == "__main__": 52 | main() 53 | 54 | 55 | -------------------------------------------------------------------------------- /scripts/benchmarks/bench_pub_rclpy.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Enable using C++ with just this one liner 4 | # import rclcppyy; rclcppyy.enable_cpp_acceleration() 5 | import rclpy 6 | from rclpy.node import Node 7 | from std_msgs.msg import String 8 | import time 9 | import sys 10 | 11 | class PerfPublisher(Node): 12 | def __init__(self, rate_hz=1000): 13 | super().__init__('perf_publisher_py') 14 | self.publisher = self.create_publisher(String, 'perf_topic_py', 10) 15 | self.rate_hz = rate_hz 16 | self.timer_period = 1.0 / rate_hz 17 | self.timer = self.create_timer(self.timer_period, self.timer_callback) 18 | self.total_count = 0 19 | self.count = 0 20 | self.start_time = time.time() 21 | 22 | def timer_callback(self): 23 | msg = String() 24 | timestamp = time.time_ns() 25 | msg.data = f'{self.total_count}:{timestamp}' 26 | self.publisher.publish(msg) 27 | self.count += 1 28 | self.total_count += 1 29 | 30 | # Print stats every second 31 | if self.count % self.rate_hz == 0: 32 | elapsed = time.time() - self.start_time 33 | rate = self.count / elapsed 34 | print(f"(rclpy) Messages: {self.count}, Rate: {rate:.2f} msgs/sec") 35 | # Reset start time and counter, so we check on every period 36 | self.start_time = time.time() 37 | self.count = 0 38 | 39 | def main(args=None): 40 | rate_hz = 1000 41 | if len(sys.argv) > 1: 42 | rate_hz = int(sys.argv[1]) 43 | 44 | print(f"Starting Python publisher benchmark at target rate: {rate_hz} Hz") 45 | rclpy.init(args=args) 46 | publisher = PerfPublisher(rate_hz) 47 | 48 | try: 49 | rclpy.spin(publisher) 50 | except KeyboardInterrupt: 51 | pass 52 | finally: 53 | publisher.destroy_node() 54 | rclpy.shutdown() 55 | 56 | if __name__ == '__main__': 57 | main() -------------------------------------------------------------------------------- /rclcppyy/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | rclcppyy - High-performance ROS2 with Python API and C++ internals 3 | 4 | This package allows ROS2 Python code to use C++ implementations 5 | under the hood for improved performance without requiring changes 6 | to existing code. 7 | """ 8 | 9 | # Public API 10 | from rclcppyy.bringup_rclcpp import bringup_rclcpp 11 | from rclcppyy import rosbag2_cpp 12 | from rclcppyy import serialization 13 | from rclcppyy.node import RclcppyyNode 14 | from rclcppyy.monkey import patch_ros2, patch_node_class 15 | 16 | def enable_cpp_acceleration(patch_node=True): 17 | """ 18 | Enable C++ acceleration for ROS2 Python code. 19 | 20 | This will: 21 | 1. Set up automatic message conversion from Python to C++ 22 | 2. Monkey-patch rclpy.create_node to return RclcppyyNode 23 | 3. Monkey-patch ROS2 message imports to use C++ versions 24 | 25 | Args: 26 | patch_node (bool): If True, also monkey-patch rclpy.node.Node class directly. 27 | This is more aggressive but ensures all nodes use C++. 28 | 29 | Returns: 30 | bool: True if successful 31 | 32 | Example: 33 | ```python 34 | import rclpy 35 | from std_msgs.msg import String 36 | 37 | # Add this single line to your existing ROS2 Python code: 38 | import rclcppyy; rclcppyy.enable_cpp_acceleration() 39 | 40 | # Rest of your code remains unchanged 41 | rclpy.init() 42 | node = rclpy.create_node('my_node') # Actually returns a RclcppyyNode 43 | ``` 44 | """ 45 | # Apply monkey patching 46 | result = patch_ros2() 47 | 48 | # Optionally patch the Node class directly 49 | if patch_node: 50 | patch_node_class() 51 | 52 | return result 53 | 54 | __all__ = [ 55 | 'bringup_rclcpp', 56 | 'RclcppyyNode', 57 | 'enable_cpp_acceleration', 58 | 'patch_ros2', 59 | 'patch_node_class', 60 | 'rosbag2_cpp', 61 | 'serialization' 62 | ] -------------------------------------------------------------------------------- /scripts/benchmarks/bench_pub_rclcppyy_monkeypatch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Enable using C++ with just this one liner 4 | import rclcppyy; rclcppyy.enable_cpp_acceleration() 5 | import rclpy 6 | from rclpy.node import Node 7 | from std_msgs.msg import String 8 | import time 9 | import sys 10 | 11 | class PerfPublisher(Node): 12 | def __init__(self, rate_hz=1000): 13 | super().__init__('perf_publisher_cppyy') 14 | self.publisher = self.create_publisher(String, 'perf_topic_rclcppyy', 10) 15 | self.rate_hz = rate_hz 16 | self.timer_period = 1.0 / rate_hz 17 | self.timer = self.create_timer(self.timer_period, self.timer_callback) 18 | self.total_count = 0 19 | self.count = 0 20 | self.start_time = time.time() 21 | 22 | def timer_callback(self): 23 | msg = String() 24 | timestamp = time.time_ns() 25 | msg.data = f'{self.total_count}:{timestamp}' 26 | self.publisher.publish(msg) 27 | self.count += 1 28 | self.total_count += 1 29 | 30 | # Print stats every second 31 | if self.count % self.rate_hz == 0: 32 | elapsed = time.time() - self.start_time 33 | rate = self.count / elapsed 34 | print(f"(rclcppyy) Messages: {self.count}, Rate: {rate:.2f} msgs/sec") 35 | # Reset start time and counter, so we check on every period 36 | self.start_time = time.time() 37 | self.count = 0 38 | 39 | def main(args=None): 40 | rate_hz = 1000 41 | if len(sys.argv) > 1: 42 | rate_hz = int(sys.argv[1]) 43 | 44 | print(f"Starting Python publisher benchmark at target rate: {rate_hz} Hz") 45 | rclpy.init(args=args) 46 | publisher = PerfPublisher(rate_hz) 47 | 48 | try: 49 | rclpy.spin(publisher) 50 | except KeyboardInterrupt: 51 | pass 52 | finally: 53 | publisher.destroy_node() 54 | rclpy.shutdown() 55 | 56 | if __name__ == '__main__': 57 | main() -------------------------------------------------------------------------------- /scripts/ros_tutorials/publisher_member_function.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Copyright 2016 Open Source Robotics Foundation, Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | # import rclpy 17 | # from rclpy.node import Node 18 | # For now we need to replace these by hand, but monkeypatching will make it transparent 19 | from rclcppyy.bringup_rclcpp import bringup_rclcpp 20 | rclcpp = bringup_rclcpp() 21 | rclpy = rclcpp 22 | Node = rclcpp.Node 23 | 24 | 25 | from std_msgs.msg import String 26 | 27 | 28 | class MinimalPublisher(Node): 29 | 30 | def __init__(self): 31 | super().__init__('minimal_publisher') 32 | self.publisher_ = self.create_publisher(String, 'topic', 10) 33 | timer_period = 0.5 # seconds 34 | self.timer = self.create_timer(timer_period, self.timer_callback) 35 | self.i = 0 36 | 37 | def timer_callback(self): 38 | msg = String() 39 | msg.data = 'Hello World: %d' % self.i 40 | self.publisher_.publish(msg) 41 | self.get_logger().info('Publishing: "%s"' % msg.data) 42 | self.i += 1 43 | 44 | 45 | def main(args=None): 46 | rclpy.init(args=args) 47 | 48 | minimal_publisher = MinimalPublisher() 49 | 50 | rclpy.spin(minimal_publisher) 51 | 52 | # Destroy the node explicitly 53 | # (optional - otherwise it will be done automatically 54 | # when the garbage collector destroys the node object) 55 | minimal_publisher.destroy_node() 56 | rclpy.shutdown() 57 | 58 | 59 | if __name__ == '__main__': 60 | main() 61 | -------------------------------------------------------------------------------- /scripts/big_messages_demos/demo_images.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | import rclcppyy; rclcppyy.enable_cpp_acceleration() 4 | import cppyy 5 | import rclpy 6 | from rclpy.node import Node 7 | import os 8 | from sensor_msgs.msg import Image 9 | import numpy as np 10 | import time 11 | 12 | def get_path_of_this_file(): 13 | import os 14 | return os.path.dirname(os.path.abspath(__file__)) 15 | 16 | def get_image_from_disk(path): 17 | import cv2 18 | 19 | img = cv2.imread(path) 20 | if img is None: 21 | raise ValueError(f"Failed to load image from {path}") 22 | 23 | ros_img = Image() 24 | ros_img.width = img.shape[1] 25 | ros_img.height = img.shape[0] 26 | ros_img.encoding = "bgr8" 27 | ros_img.is_bigendian = False 28 | ros_img.step = img.shape[1] * 3 29 | 30 | # Get the data as a simple Python list of integers 31 | flat_img = np.ascontiguousarray(img, dtype=np.uint8).flatten() 32 | byte_list = [int(b) for b in flat_img] 33 | 34 | # Create an empty vector 35 | vector_type = cppyy.gbl.std.vector["unsigned char"]() 36 | 37 | # Add elements using the std::vector::insert method 38 | # This avoids having to iterate through each element 39 | vector_type.insert(vector_type.end(), byte_list) 40 | 41 | ros_img.data = vector_type 42 | print(ros_img) 43 | print(ros_img.data[:20]) 44 | print(f"Size of image: {len(ros_img.data)}, in MB: {len(ros_img.data) / 1024 / 1024}") 45 | return ros_img 46 | 47 | class ImagePublisher(Node): 48 | def __init__(self): 49 | super().__init__("image_publisher") 50 | self.publisher = self.create_publisher(Image, "image", 10) 51 | # Convert the image to a ROS2 Image message 52 | self.image_msg = get_image_from_disk(os.path.join(get_path_of_this_file(), "pr2_robot_magic_lab.jpg")) 53 | self.hz = 4 54 | self.timer = self.create_timer(1.0 / self.hz, self.timer_callback) 55 | 56 | def timer_callback(self): 57 | start_time = time.time() 58 | # Rough edge, TODO: make dealing with stamps more Pythonic 59 | self.image_msg.header.stamp.sec = self.get_clock().now().to_msg().sec 60 | self.image_msg.header.stamp.nanosec = self.get_clock().now().to_msg().nanosec 61 | self.publisher.publish(self.image_msg) 62 | end_time = time.time() 63 | # print(f"Time taken to publish image: {(end_time - start_time) * 1000} ms") 64 | 65 | if __name__ == "__main__": 66 | rclpy.init() 67 | node = ImagePublisher() 68 | rclpy.spin(node) 69 | rclpy.shutdown() -------------------------------------------------------------------------------- /rclcppyy/monkey.py: -------------------------------------------------------------------------------- 1 | """ 2 | This file handles monkey-patching rclpy to use rclcpp implementations. 3 | """ 4 | 5 | import rclpy 6 | import sys 7 | from rclpy.node import Node 8 | from rclcppyy.node import RclcppyyNode 9 | from rclcppyy.monkeypatch_messages import install_ros_message_hook, convert_already_imported_python_msgs_to_cpp 10 | from rclcppyy.bringup_rclcpp import bringup_rclcpp 11 | 12 | # Store original functions 13 | _original_create_node = rclpy.create_node 14 | _original_node_create_subscription = Node.create_subscription 15 | 16 | def patch_ros2(): 17 | """ 18 | Monkey-patch ROS2 Python API to use C++ implementations. 19 | This makes existing Python ROS2 code use C++ under the hood. 20 | """ 21 | # Initialize rclcpp 22 | rclcpp = bringup_rclcpp() 23 | 24 | # Set up automatic message conversion 25 | install_ros_message_hook() 26 | 27 | # Convert already imported Python messages to C++ 28 | convert_already_imported_python_msgs_to_cpp() 29 | 30 | # Monkey-patch rclpy.create_node 31 | rclpy.create_node = _create_node_wrapper 32 | 33 | # Monkey-patch rclpy.spin 34 | rclpy.spin = _spin_wrapper 35 | 36 | # Monkey-patch Node.create_subscription 37 | Node.create_subscription = _create_subscription_wrapper 38 | 39 | print("ROS2 C++ acceleration enabled!") 40 | return True 41 | 42 | def _create_node_wrapper(*args, **kwargs): 43 | """ 44 | Wrapper for rclpy.create_node that returns RclcppyyNode instead. 45 | This maintains the same API but uses our C++-backed node. 46 | """ 47 | return RclcppyyNode(*args, **kwargs) 48 | 49 | def _spin_wrapper(*args, **kwargs): 50 | """ 51 | Wrapper for rclpy.spin that uses rclcpp.spin 52 | """ 53 | rclcpp = bringup_rclcpp() 54 | node = args[0] 55 | rclcpp.spin(node._rclcpp_node) 56 | 57 | def _create_subscription_wrapper(self, *args, **kwargs): 58 | """ 59 | Wrapper for Node.create_subscription that uses RclcppyyNode implementation 60 | when the node is an RclcppyyNode, otherwise falls back to original. 61 | """ 62 | if isinstance(self, RclcppyyNode): 63 | return self.create_subscription(*args, **kwargs) 64 | else: 65 | return _original_node_create_subscription(self, *args, **kwargs) 66 | 67 | def patch_node_class(): 68 | """ 69 | Monkey-patch the Node class so that any direct instantiations 70 | of rclpy.node.Node get our RclcppyyNode instead. 71 | """ 72 | # Replace the Node class with RclcppyyNode 73 | # This is a more aggressive approach and might cause issues 74 | # so we make it optional 75 | rclpy.node.Node = RclcppyyNode 76 | return True -------------------------------------------------------------------------------- /scripts/benchmarks/bench_sub_rclpy.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Enable using C++ with just this one liner 4 | # import rclcppyy; rclcppyy.enable_cpp_acceleration() 5 | import rclpy 6 | from rclpy.node import Node 7 | from std_msgs.msg import String 8 | import time 9 | import numpy as np 10 | import sys 11 | 12 | class PerfSubscriber(Node): 13 | def __init__(self): 14 | super().__init__('perf_subscriber_py') 15 | self.subscription = self.create_subscription( 16 | String, 17 | 'perf_topic_py', 18 | self.listener_callback, 19 | 500) 20 | 21 | self.count = 0 22 | self.start_time = time.time() 23 | self.latencies = [] 24 | self.last_seq = -1 25 | self.dropped_msgs = 0 26 | 27 | def listener_callback(self, msg): 28 | seq, pub_timestamp = msg.data.split(':') 29 | seq = int(seq) 30 | pub_timestamp = int(pub_timestamp) 31 | 32 | # Calculate latency 33 | recv_timestamp = time.time_ns() 34 | latency_us = (recv_timestamp - pub_timestamp) / 1000.0 # Convert to microseconds 35 | self.latencies.append(latency_us) 36 | 37 | # Check for dropped messages 38 | if self.last_seq >= 0 and seq != self.last_seq + 1: 39 | self.dropped_msgs += seq - self.last_seq - 1 40 | self.last_seq = seq 41 | 42 | self.count += 1 43 | 44 | # Print stats every second (assuming 1000Hz rate) 45 | if self.count % 1000 == 0: 46 | elapsed = time.time() - self.start_time 47 | rate = self.count / elapsed 48 | 49 | # Calculate latency statistics from recent messages 50 | recent_latencies = self.latencies[-1000:] 51 | avg_latency = np.mean(recent_latencies) 52 | p99_latency = np.percentile(recent_latencies, 99) 53 | 54 | print(f"(rclpy) Messages: {self.count}, Rate: {rate:07.1f} msgs/sec, " 55 | f"Latency (μs) - Avg: {avg_latency:.1f}, P99: {p99_latency:.1f}, " 56 | f"Dropped: {self.dropped_msgs}") 57 | 58 | # Reset counters for next period 59 | self.start_time = time.time() 60 | self.count = 0 61 | 62 | def main(args=None): 63 | print("Starting Python subscriber benchmark") 64 | rclpy.init(args=args) 65 | subscriber = PerfSubscriber() 66 | 67 | try: 68 | rclpy.spin(subscriber) 69 | except KeyboardInterrupt: 70 | pass 71 | finally: 72 | # Print final statistics 73 | if subscriber.latencies: 74 | print("\nFinal Statistics:") 75 | print(f"Total messages received: {len(subscriber.latencies)}") 76 | print(f"Total messages dropped: {subscriber.dropped_msgs}") 77 | print(f"Overall latency - Avg: {np.mean(subscriber.latencies):.1f} μs, " 78 | f"P99: {np.percentile(subscriber.latencies, 99):.1f} μs") 79 | 80 | subscriber.destroy_node() 81 | rclpy.shutdown() 82 | 83 | if __name__ == '__main__': 84 | main() -------------------------------------------------------------------------------- /scripts/benchmarks/bench_sub_rclcppyy_monkeypatch2.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Enable using C++ with just this one liner 4 | import rclcppyy; rclcppyy.enable_cpp_acceleration() 5 | import rclpy 6 | from rclpy.node import Node 7 | from std_msgs.msg import String 8 | import time 9 | import numpy as np 10 | import sys 11 | 12 | class PerfSubscriber(Node): 13 | def __init__(self): 14 | super().__init__('perf_subscriber_monkeypatched') 15 | self.subscription = self.create_subscription( 16 | String, 17 | 'perf_topic_rclcppyy', 18 | self.listener_callback, 19 | 500) 20 | 21 | self.count = 0 22 | self.start_time = time.time() 23 | self.latencies = [] 24 | self.last_seq = -1 25 | self.dropped_msgs = 0 26 | 27 | def listener_callback(self, msg): 28 | seq, pub_timestamp = msg.data.split(':') 29 | seq = int(seq) 30 | pub_timestamp = int(pub_timestamp) 31 | 32 | # Calculate latency 33 | recv_timestamp = time.time_ns() 34 | latency_us = (recv_timestamp - pub_timestamp) / 1000.0 # Convert to microseconds 35 | self.latencies.append(latency_us) 36 | 37 | # Check for dropped messages 38 | if self.last_seq >= 0 and seq != self.last_seq + 1: 39 | self.dropped_msgs += seq - self.last_seq - 1 40 | self.last_seq = seq 41 | 42 | self.count += 1 43 | 44 | # Print stats every second (assuming 1000Hz rate) 45 | if self.count % 1000 == 0: 46 | elapsed = time.time() - self.start_time 47 | rate = self.count / elapsed 48 | 49 | # Calculate latency statistics from recent messages 50 | recent_latencies = self.latencies[-1000:] 51 | avg_latency = np.mean(recent_latencies) 52 | p99_latency = np.percentile(recent_latencies, 99) 53 | 54 | print(f"(rclcppyy) Messages: {self.count}, Rate: {rate:07.1f} msgs/sec, " 55 | f"Latency (μs) - Avg: {avg_latency:.1f}, P99: {p99_latency:.1f}, " 56 | f"Dropped: {self.dropped_msgs}") 57 | 58 | # Reset counters for next period 59 | self.start_time = time.time() 60 | self.count = 0 61 | 62 | def main(args=None): 63 | print("Starting Python subscriber benchmark") 64 | rclpy.init(args=args) 65 | subscriber = PerfSubscriber() 66 | 67 | try: 68 | rclpy.spin(subscriber) 69 | except KeyboardInterrupt: 70 | pass 71 | finally: 72 | # Print final statistics 73 | if subscriber.latencies: 74 | print("\nFinal Statistics:") 75 | print(f"Total messages received: {len(subscriber.latencies)}") 76 | print(f"Total messages dropped: {subscriber.dropped_msgs}") 77 | print(f"Overall latency - Avg: {np.mean(subscriber.latencies):.1f} μs, " 78 | f"P99: {np.percentile(subscriber.latencies, 99):.1f} μs") 79 | 80 | subscriber.destroy_node() 81 | rclpy.shutdown() 82 | 83 | if __name__ == '__main__': 84 | main() -------------------------------------------------------------------------------- /scripts/bag2bag_py_images.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import time 4 | import argparse 5 | from dataclasses import dataclass 6 | import numpy as np 7 | from array import array 8 | 9 | from rosbag2_py import SequentialReader, SequentialWriter, StorageOptions, ConverterOptions, TopicMetadata 10 | from rclpy.serialization import deserialize_message, serialize_message 11 | from rosidl_runtime_py.utilities import get_message 12 | 13 | 14 | IMG_TYPE = 'sensor_msgs/msg/Image' 15 | 16 | 17 | @dataclass 18 | class Args: 19 | input_uri: str 20 | output_uri: str 21 | image_topic: str 22 | suffix: str 23 | 24 | 25 | def to_gray_python(img): 26 | h, w, step = img.height, img.width, img.step 27 | enc = img.encoding 28 | assert enc in ('bgr8', 'rgb8'), f'Unsupported encoding: {enc}' 29 | buf = memoryview(img.data) 30 | arr = np.frombuffer(buf, dtype=np.uint8).reshape(h, step) 31 | rgb = arr[:, :w * 3].reshape(h, w, 3) 32 | if enc == 'bgr8': 33 | b, g, r = rgb[..., 0], rgb[..., 1], rgb[..., 2] 34 | else: 35 | r, g, b = rgb[..., 0], rgb[..., 1], rgb[..., 2] 36 | gray = (0.114 * b + 0.587 * g + 0.299 * r).astype(np.uint8) 37 | img.encoding = 'mono8' 38 | img.step = w 39 | img.data = array('B', gray.tobytes()) 40 | return img 41 | 42 | 43 | def main(): 44 | p = argparse.ArgumentParser(description='Bag-to-bag image grayscale converter (pure Python baseline).') 45 | p.add_argument('input_uri') 46 | p.add_argument('output_uri') 47 | p.add_argument('--image-topic', default='/camera/image_raw') 48 | p.add_argument('--suffix', default='_gray') 49 | a = p.parse_args() 50 | args = Args(a.input_uri, a.output_uri, a.image_topic, a.suffix) 51 | 52 | reader = SequentialReader() 53 | reader.open( 54 | StorageOptions(uri=args.input_uri, storage_id='mcap'), 55 | ConverterOptions(input_serialization_format='cdr', output_serialization_format='cdr') 56 | ) 57 | 58 | writer = SequentialWriter() 59 | writer.open( 60 | StorageOptions(uri=args.output_uri, storage_id='mcap'), 61 | ConverterOptions(input_serialization_format='cdr', output_serialization_format='cdr') 62 | ) 63 | 64 | start_time = time.time() 65 | try: 66 | topics = reader.get_all_topics_and_types() 67 | except AttributeError: 68 | topics = reader.get_topics_and_types() 69 | topic_types = {t.name: t.type for t in topics} 70 | 71 | for t in topics: 72 | if t.type == IMG_TYPE and t.name == args.image_topic: 73 | writer.create_topic(TopicMetadata( 74 | id=0, 75 | name=t.name + args.suffix, 76 | type=t.type, 77 | serialization_format='cdr' 78 | )) 79 | 80 | Image = get_message(IMG_TYPE) 81 | 82 | while reader.has_next(): 83 | topic, data, t = reader.read_next() 84 | typ = topic_types.get(topic) 85 | if typ == IMG_TYPE and topic == args.image_topic: 86 | msg = deserialize_message(data, Image) 87 | msg = to_gray_python(msg) 88 | out_ser = serialize_message(msg) 89 | writer.write(topic + args.suffix, out_ser, t) 90 | 91 | end_time = time.time() 92 | print(f"Time taken: {end_time - start_time} seconds") 93 | 94 | if __name__ == '__main__': 95 | main() 96 | 97 | # 0.343s 98 | # 0.369s -------------------------------------------------------------------------------- /roscon_uk_2025/presentation/2_pointcloud_voxelgrid.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | import os 3 | import rclcppyy; rclcppyy.enable_cpp_acceleration() 4 | import rclpy 5 | from rclpy.node import Node 6 | from rclpy.qos import qos_profile_sensor_data 7 | import cppyy 8 | from sensor_msgs.msg import PointCloud2 9 | 10 | def prepare(): 11 | print("including pcl headers") 12 | cppyy.add_include_path(f"{os.environ["CONDA_PREFIX"]}/include/pcl-1.15") 13 | cppyy.add_include_path(f"{os.environ["CONDA_PREFIX"]}/include/eigen3") 14 | cppyy.include("pcl/point_types.h") 15 | cppyy.include("pcl/point_cloud.h") 16 | cppyy.include("pcl/filters/voxel_grid.h") 17 | cppyy.include("pcl/filters/impl/voxel_grid.hpp") 18 | cppyy.include("pcl_conversions/pcl_conversions.h") 19 | print("included pcl headers") 20 | 21 | # cppyy: VoxelGrid wrapper for ROS2 PointCloud2 in/out via pcl_conversions 22 | print("defining voxelgrid_filter_pointcloud2") 23 | cppyy.cppdef(""" 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | extern "C" void voxelgrid_filter_pointcloud2(const sensor_msgs::msg::PointCloud2& in_msg, 32 | float leaf_x, float leaf_y, float leaf_z, 33 | sensor_msgs::msg::PointCloud2& out_msg) { 34 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); 35 | pcl::fromROSMsg(in_msg, *cloud); 36 | 37 | pcl::VoxelGrid voxel; 38 | voxel.setInputCloud(cloud); 39 | voxel.setLeafSize(leaf_x, leaf_y, leaf_z); 40 | 41 | pcl::PointCloud out_cloud; 42 | voxel.filter(out_cloud); 43 | 44 | pcl::toROSMsg(out_cloud, out_msg); 45 | out_msg.header = in_msg.header; // preserve frame/time 46 | } 47 | """) 48 | print("defined voxelgrid_filter_pointcloud2") 49 | 50 | def voxelgrid_downsample_msg(msg: PointCloud2, leaf=0.1, leaf_y=None, leaf_z=None) -> PointCloud2: 51 | leaf_y = leaf if leaf_y is None else leaf_y 52 | leaf_z = leaf if leaf_z is None else leaf_z 53 | out = PointCloud2() 54 | cppyy.gbl.voxelgrid_filter_pointcloud2(msg, float(leaf), float(leaf_y), float(leaf_z), out) 55 | return out 56 | 57 | class ImageSubscriber(Node): 58 | def __init__(self, topic_in: str, topic_out: str, leaf: float = 0.25) -> None: 59 | super().__init__("pointcloud_voxelgrid") 60 | self.leaf = leaf 61 | self.publisher = self.create_publisher(PointCloud2, topic_out, qos_profile_sensor_data) 62 | # Subscribe to the same topic as the demo publisher 63 | self.subscription = self.create_subscription( 64 | PointCloud2, 65 | topic_in, 66 | self._on_pointcloud, 67 | 10, 68 | ) 69 | print("subscription created") 70 | 71 | def _on_pointcloud(self, msg: PointCloud2) -> None: 72 | msg = voxelgrid_downsample_msg(msg, leaf=self.leaf) 73 | self.publisher.publish(msg) 74 | 75 | def run_voxelgrid(topic_in: str, topic_out: str, leaf: float = 0.25) -> None: 76 | """Run the voxelgrid downsampling node.""" 77 | prepare() 78 | rclpy.init() 79 | node = ImageSubscriber(topic_in, topic_out, leaf) 80 | try: 81 | rclpy.spin(node) 82 | finally: 83 | node.destroy_node() 84 | rclpy.shutdown() 85 | 86 | if __name__ == "__main__": 87 | run_voxelgrid("/lexus3/os_center/points", "/voxelgrid", 0.5) 88 | 89 | 90 | -------------------------------------------------------------------------------- /scripts/bag2bag_cppyy_hybrid.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import time 4 | import argparse 5 | from dataclasses import dataclass 6 | import numpy as np 7 | from array import array 8 | import cppyy 9 | 10 | from rosbag2_py import SequentialReader, SequentialWriter, StorageOptions, ConverterOptions, TopicMetadata 11 | from rclpy.serialization import deserialize_message, serialize_message 12 | from rosidl_runtime_py.utilities import get_message 13 | 14 | 15 | IMG_TYPE = 'sensor_msgs/msg/Image' 16 | 17 | 18 | cppyy.cppdef(r""" 19 | #include 20 | #include 21 | 22 | extern "C" void bgr_or_rgb_to_gray( 23 | const uint8_t* src, uint8_t* dst, 24 | size_t width, size_t height, size_t step_bytes, 25 | bool is_bgr) 26 | { 27 | for (size_t y = 0; y < height; ++y) { 28 | const uint8_t* row = src + y*step_bytes; 29 | uint8_t* grow = dst + y*width; 30 | for (size_t x = 0; x < width; ++x) { 31 | uint8_t c0 = row[3*x+0]; 32 | uint8_t c1 = row[3*x+1]; 33 | uint8_t c2 = row[3*x+2]; 34 | uint8_t b = is_bgr ? c0 : c2; 35 | uint8_t g = c1; 36 | uint8_t r = is_bgr ? c2 : c0; 37 | float yf = 0.114f*b + 0.587f*g + 0.299f*r; 38 | grow[x] = (uint8_t)(yf); 39 | } 40 | } 41 | } 42 | """) 43 | 44 | 45 | @dataclass 46 | class Args: 47 | input_uri: str 48 | output_uri: str 49 | image_topic: str 50 | suffix: str 51 | 52 | 53 | def to_gray_cppyy(img): 54 | h, w, step = img.height, img.width, img.step 55 | enc = img.encoding 56 | assert enc in ('bgr8', 'rgb8'), f'Unsupported encoding: {enc}' 57 | is_bgr = (enc == 'bgr8') 58 | 59 | src = np.frombuffer(memoryview(img.data), dtype=np.uint8) 60 | gray = np.empty(h * w, dtype=np.uint8) 61 | 62 | cppyy.gbl.bgr_or_rgb_to_gray( 63 | src.ctypes.data, 64 | gray.ctypes.data, 65 | w, h, step, is_bgr) 66 | 67 | gray = gray.reshape(h, w) 68 | 69 | img.encoding = 'mono8' 70 | img.step = w 71 | img.data = array('B', gray.tobytes()) 72 | return img 73 | 74 | 75 | def main(): 76 | p = argparse.ArgumentParser(description='Bag-to-bag image grayscale converter (hybrid: Python IO + C++ pixel loop).') 77 | p.add_argument('input_uri') 78 | p.add_argument('output_uri') 79 | p.add_argument('--image-topic', default='/camera/image_raw') 80 | p.add_argument('--suffix', default='_gray') 81 | a = p.parse_args() 82 | args = Args(a.input_uri, a.output_uri, a.image_topic, a.suffix) 83 | 84 | reader = SequentialReader() 85 | reader.open( 86 | StorageOptions(uri=args.input_uri, storage_id='mcap'), 87 | ConverterOptions(input_serialization_format='cdr', output_serialization_format='cdr') 88 | ) 89 | 90 | writer = SequentialWriter() 91 | writer.open( 92 | StorageOptions(uri=args.output_uri, storage_id='mcap'), 93 | ConverterOptions(input_serialization_format='cdr', output_serialization_format='cdr') 94 | ) 95 | 96 | start_time = time.time() 97 | try: 98 | topics = reader.get_all_topics_and_types() 99 | except AttributeError: 100 | topics = reader.get_topics_and_types() 101 | topic_types = {t.name: t.type for t in topics} 102 | 103 | for t in topics: 104 | if t.type == IMG_TYPE and t.name == args.image_topic: 105 | writer.create_topic(TopicMetadata( 106 | id=0, 107 | name=t.name + args.suffix, 108 | type=t.type, 109 | serialization_format='cdr', 110 | )) 111 | 112 | Image = get_message(IMG_TYPE) 113 | 114 | while reader.has_next(): 115 | topic, data, t = reader.read_next() 116 | if topic_types.get(topic) == IMG_TYPE and topic == args.image_topic: 117 | msg = deserialize_message(data, Image) 118 | msg = to_gray_cppyy(msg) 119 | writer.write(topic + args.suffix, serialize_message(msg), t) 120 | end_time = time.time() 121 | print(f"Time taken: {end_time - start_time} seconds") 122 | 123 | 124 | if __name__ == '__main__': 125 | main() 126 | 127 | 128 | -------------------------------------------------------------------------------- /roscon_uk_2025/presentation/4_gstreamer_cli.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import typer 3 | import numpy as np 4 | import cppyy 5 | import gi 6 | gi.require_version('Gst', '1.0') 7 | from gi.repository import Gst, GLib 8 | Gst.init(None) 9 | 10 | def run_gstreamer(device: str = "/dev/video0", 11 | width: int = 640, 12 | height: int = 480, 13 | fps: int = 30, 14 | viz: bool = True): 15 | """ 16 | Run a gstreamer pipeline to invert the colors of a video stream. 17 | Args: 18 | device: The device to use for the video stream. 19 | width: The width of the video stream. 20 | height: The height of the video stream. 21 | fps: The frames per second of the video stream. 22 | """ 23 | # JIT a small C++ kernel (in-place fast path is trivial to swap for blur/Sobel/IPP, etc.) 24 | print("Compiling C++ kernel") 25 | cppyy.cppdef(r""" 26 | #include 27 | extern "C" void invert_rgb_addr(uint64_t src_addr, uint64_t dst_addr, int nbytes) { 28 | const uint8_t* src = reinterpret_cast(src_addr); 29 | uint8_t* dst = reinterpret_cast(dst_addr); 30 | for (int i = 0; i < nbytes; ++i) dst[i] = 255 - src[i]; 31 | } 32 | """) 33 | print("Compiled C++ kernel") 34 | invert = cppyy.gbl.invert_rgb_addr 35 | 36 | W, H, FPS = width, height, fps 37 | CAPS = f"video/x-raw,format=RGB,width={W},height={H},framerate={FPS}/1" 38 | FRAME_DURATION = Gst.util_uint64_scale_int(1, Gst.SECOND, FPS) 39 | 40 | cap = Gst.parse_launch( 41 | f"v4l2src device={device} ! videoconvert ! {CAPS} " 42 | f"! appsink name=sink emit-signals=true max-buffers=1 drop=true" 43 | ) 44 | sink = cap.get_by_name("sink") 45 | 46 | if viz: 47 | disp = Gst.parse_launch( 48 | f"appsrc name=src is-live=true do-timestamp=true caps={CAPS} " 49 | f"! videoconvert ! autovideosink sync=false" 50 | ) 51 | else: 52 | disp = Gst.parse_launch( 53 | f"appsrc name=src is-live=true do-timestamp=true caps={CAPS} " 54 | f"! shmsink socket-path=/tmp/gst_shm wait-for-connection=false sync=false" 55 | ) 56 | src = disp.get_by_name("src") 57 | src.set_property("format", Gst.Format.TIME) 58 | 59 | out = np.empty(width*height*3, dtype=np.uint8) 60 | 61 | def on_sample(sink): 62 | sample = sink.emit("pull-sample") 63 | buf = sample.get_buffer() 64 | ok, m = buf.map(Gst.MapFlags.READ) 65 | if not ok: return Gst.FlowReturn.OK 66 | 67 | try: 68 | size = m.size 69 | # Create a zero-copy NumPy view of the mapped bytes, and pass its 70 | # raw pointer (ctypes.data) directly to the C++ kernel. This avoids 71 | # Python loops and extra copies on the input side. 72 | src_arr = np.frombuffer(m.data, dtype=np.uint8, count=size) 73 | # out = np.empty(size, dtype=np.uint8) 74 | invert(src_arr.ctypes.data, out.ctypes.data, size) 75 | finally: 76 | buf.unmap(m) 77 | 78 | out_buf = Gst.Buffer.new_wrapped(out.tobytes()) 79 | # Provide explicit PTS/DTS and duration in TIME format to keep downstream 80 | # elements happy and avoid segment format assertions. 81 | if not hasattr(on_sample, "ts"): on_sample.ts = 0 82 | out_buf.pts = on_sample.ts 83 | out_buf.dts = on_sample.ts 84 | out_buf.duration = FRAME_DURATION 85 | on_sample.ts += FRAME_DURATION 86 | src.emit("push-buffer", out_buf) 87 | return Gst.FlowReturn.OK 88 | 89 | sink.connect("new-sample", on_sample) 90 | cap.set_state(Gst.State.PLAYING) 91 | disp.set_state(Gst.State.PLAYING) 92 | 93 | loop = GLib.MainLoop() 94 | print("Running gstreamer pipeline") 95 | if not viz: 96 | print("No viz enabled, viz with:") 97 | print(f"gst-launch-1.0 -v shmsrc socket-path=/tmp/gst_shm do-timestamp=true ! capsfilter caps=\"{CAPS}\" ! videoconvert ! autovideosink sync=false") 98 | try: loop.run() 99 | except KeyboardInterrupt: pass 100 | finally: 101 | cap.set_state(Gst.State.NULL) 102 | disp.set_state(Gst.State.NULL) 103 | 104 | if __name__ == "__main__": 105 | typer.run(run_gstreamer) -------------------------------------------------------------------------------- /scripts/benchmarks/bench_pub_rclcppyy.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import sys 4 | import signal 5 | from rclcppyy import bringup_rclcpp 6 | import cppyy 7 | 8 | bringup_rclcpp() 9 | 10 | cppyy.include("std_msgs/msg/string.hpp") 11 | cppyy.include("chrono") 12 | cppyy.include("functional") 13 | 14 | class PythonPerfPublisher: 15 | def __init__(self, rate_hz): 16 | # Initialize ROS2 if not already initialized 17 | if not cppyy.gbl.rclcpp.ok(): 18 | cppyy.gbl.rclcpp.init(len(sys.argv), sys.argv) 19 | 20 | # Create a ROS2 node 21 | self.node = cppyy.gbl.rclcpp.Node("perf_publisher_pythonic") 22 | 23 | # Create a publisher 24 | self.publisher = self.node.create_publisher[cppyy.gbl.std_msgs.msg.String]( 25 | "perf_topic_pythonic", 10) 26 | 27 | # Set up timer 28 | self.rate_hz = rate_hz 29 | self.period_ns = int(1e9 / rate_hz) # Convert Hz to nanoseconds 30 | 31 | # Define the callback wrapper with proper Python.h include 32 | cppyy.cppdef(""" 33 | #include 34 | #include 35 | 36 | static std::function create_timer_callback(PyObject* self) { 37 | return [self]() { 38 | if (self && PyObject_HasAttrString(self, "timer_callback")) { 39 | PyObject_CallMethod(self, "timer_callback", nullptr); 40 | } 41 | }; 42 | } 43 | """) 44 | 45 | # Create the timer with the wrapped callback 46 | callback = cppyy.gbl.create_timer_callback(self) 47 | self.timer = self.node.create_wall_timer( 48 | cppyy.gbl.std.chrono.nanoseconds(self.period_ns), 49 | callback) 50 | 51 | # Initialize counter and start time 52 | self.total_count = 0 53 | self.count = 0 54 | self.start_time = cppyy.gbl.std.chrono.steady_clock.now() 55 | 56 | def timer_callback(self): 57 | # Create and publish message 58 | message = cppyy.gbl.std_msgs.msg.String() 59 | now_ns = cppyy.gbl.std.chrono.duration_cast[cppyy.gbl.std.chrono.nanoseconds]( 60 | cppyy.gbl.std.chrono.high_resolution_clock.now().time_since_epoch()).count() 61 | message.data = f"{self.total_count}:{now_ns}" 62 | self.publisher.publish(message) 63 | 64 | self.total_count += 1 65 | self.count += 1 66 | 67 | # Print stats every 1000 messages 68 | if self.count % self.rate_hz == 0: 69 | self.print_stats() 70 | 71 | def print_stats(self): 72 | # Calculate elapsed time and message rate 73 | now = cppyy.gbl.std.chrono.steady_clock.now() 74 | elapsed = cppyy.gbl.std.chrono.duration[cppyy.gbl.double](now - self.start_time).count() 75 | rate = self.count / elapsed 76 | print(f"(rclcppyy) Messages: {self.count}, Rate: {rate:.2f} msgs/sec") 77 | # Reset start time and counter, so we check on every period 78 | self.start_time = cppyy.gbl.std.chrono.steady_clock.now() 79 | self.count = 0 80 | 81 | def get_elapsed_seconds(self): 82 | now = cppyy.gbl.std.chrono.steady_clock.now() 83 | return cppyy.gbl.std.chrono.duration[cppyy.gbl.double](now - self.start_time).count() 84 | 85 | def get_count(self): 86 | return self.count 87 | 88 | def spin(self): 89 | try: 90 | # Spin the node 91 | cppyy.gbl.rclcpp.spin(self.node) 92 | except KeyboardInterrupt: 93 | pass 94 | finally: 95 | # Print final stats 96 | final_count = self.get_count() 97 | elapsed = self.get_elapsed_seconds() 98 | rate = final_count / elapsed 99 | print(f"Final stats - Messages: {final_count}, Avg Rate: {rate:.2f} msgs/sec") 100 | 101 | # Shutdown ROS2 102 | cppyy.gbl.rclcpp.shutdown() 103 | 104 | def main(): 105 | rate_hz = 1000 106 | if len(sys.argv) > 1: 107 | rate_hz = int(sys.argv[1]) 108 | 109 | print(f"Starting pythonic cppyy publisher benchmark at target rate: {rate_hz} Hz") 110 | 111 | # Set up signal handler for clean shutdown 112 | def signal_handler(sig, frame): 113 | print("Shutting down...") 114 | cppyy.gbl.rclcpp.shutdown() 115 | sys.exit(0) 116 | 117 | signal.signal(signal.SIGINT, signal_handler) 118 | 119 | # Create and run the publisher 120 | publisher = PythonPerfPublisher(rate_hz) 121 | publisher.spin() 122 | 123 | if __name__ == "__main__": 124 | main() -------------------------------------------------------------------------------- /scripts/benchmarks/bench_sub_rclcppyy_monkeypatched.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import sys 4 | import signal 5 | import numpy as np 6 | import rclpy 7 | from std_msgs.msg import String 8 | import time 9 | 10 | # Enable C++ acceleration - this should make subscriptions use C++ under the hood 11 | import rclcppyy 12 | rclcppyy.enable_cpp_acceleration() 13 | 14 | class PythonPerfSubscriber: 15 | def __init__(self): 16 | # Initialize ROS2 using standard rclpy API 17 | if not rclpy.ok(): 18 | rclpy.init() 19 | 20 | # Create a ROS2 node using standard rclpy API - but it will actually be RclcppyyNode 21 | self.node = rclpy.create_node("perf_subscriber_monkeypatched") 22 | 23 | self.warmup() 24 | 25 | # Initialize tracking variables 26 | self.count = 0 27 | self.start_time = time.time_ns() 28 | self.latencies = [] 29 | self.last_seq = -1 30 | self.dropped_msgs = 0 31 | 32 | # Create the subscription using standard rclpy API - but it will use C++ under the hood 33 | self.subscription = self.node.create_subscription( 34 | String, 35 | "perf_topic_rclcppyy", 36 | self.subscription_callback, 37 | 500 # QoS depth 38 | ) 39 | 40 | print(f"Created subscription with type: {type(self.subscription)}") 41 | print(f"Node type: {type(self.node)}") 42 | 43 | def warmup(self): 44 | """ 45 | Basic warmup to prepare timing operations. 46 | """ 47 | # Simple warmup operations 48 | _ = time.time_ns() 49 | _ = time.time() 50 | 51 | def subscription_callback(self, msg): 52 | """ 53 | Standard callback that receives the ROS message. 54 | With monkeypatching, this should receive a C++ message object. 55 | """ 56 | try: 57 | # Extract data from the message 58 | # The message should be a C++ message object due to monkeypatching 59 | msg_data = msg.data 60 | 61 | seq, pub_timestamp = msg_data.split(':') 62 | seq = int(seq) 63 | pub_timestamp = int(pub_timestamp) 64 | 65 | # Calculate latency using Python time functions 66 | recv_timestamp = time.time_ns() 67 | latency_us = (recv_timestamp - pub_timestamp) / 1000.0 # Convert to microseconds 68 | self.latencies.append(latency_us) 69 | 70 | # Check for dropped messages 71 | if self.last_seq >= 0 and seq != self.last_seq + 1: 72 | self.dropped_msgs += seq - self.last_seq - 1 73 | self.last_seq = seq 74 | 75 | self.count += 1 76 | 77 | # Print stats every second (assuming 1000Hz rate) 78 | if self.count % 1000 == 0: 79 | elapsed = (time.time_ns() - self.start_time) / 1e9 # Convert to seconds 80 | rate = self.count / elapsed 81 | 82 | # Calculate latency statistics from recent messages 83 | recent_latencies = self.latencies[-1000:] 84 | avg_latency = np.mean(recent_latencies) 85 | p99_latency = np.percentile(recent_latencies, 99) 86 | 87 | print(f"(rclcppyy) Messages: {self.count}, Rate: {rate:07.1f} msgs/sec, " 88 | f"Latency (μs) - Avg: {avg_latency:.1f}, P99: {p99_latency:.1f}, " 89 | f"Dropped: {self.dropped_msgs}") 90 | 91 | # Reset counters for next period 92 | self.start_time = time.time_ns() 93 | self.count = 0 94 | 95 | except Exception as e: 96 | print(f"Error in callback: {e}") 97 | print(f"Message type: {type(msg)}") 98 | print(f"Message dir: {dir(msg)}") 99 | 100 | def spin(self): 101 | try: 102 | # Spin using standard rclpy API - but it will use C++ under the hood 103 | rclpy.spin(self.node) 104 | except KeyboardInterrupt: 105 | pass 106 | finally: 107 | # Print final statistics 108 | if self.latencies: 109 | print("\nFinal Statistics:") 110 | print(f"Total messages received: {len(self.latencies)}") 111 | print(f"Total messages dropped: {self.dropped_msgs}") 112 | print(f"Overall latency - Avg: {np.mean(self.latencies):.1f} μs, " 113 | f"P99: {np.percentile(self.latencies, 99):.1f} μs") 114 | 115 | # Shutdown ROS2 116 | rclpy.shutdown() 117 | 118 | def main(): 119 | print("Starting monkeypatched rclcppyy subscriber benchmark") 120 | 121 | # Set up signal handler for clean shutdown 122 | def signal_handler(sig, frame): 123 | print("Shutting down...") 124 | rclpy.shutdown() 125 | sys.exit(0) 126 | 127 | signal.signal(signal.SIGINT, signal_handler) 128 | 129 | # Create and run the subscriber 130 | subscriber = PythonPerfSubscriber() 131 | subscriber.spin() 132 | 133 | if __name__ == "__main__": 134 | main() -------------------------------------------------------------------------------- /roscon_uk_2025/presentation/3_cloudini_cli.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import typer 3 | from typing import Optional 4 | from enum import Enum 5 | import os 6 | from pathlib import Path 7 | import sys 8 | import cppyy 9 | 10 | 11 | def _find_workspace_root(start: Path) -> Path: 12 | p = start.resolve() 13 | for _ in range(8): 14 | if (p / "build").is_dir() and (p / "src").is_dir(): 15 | return p 16 | p = p.parent 17 | return start.resolve() 18 | 19 | 20 | def _setup_cppyy(ws_root: Path) -> None: 21 | # Headers 22 | include_candidates = [ 23 | ws_root / "src/cloudini/cloudini_lib/tools/include", 24 | ws_root / "src/cloudini/cloudini_lib/include", 25 | ws_root / "install/cloudini_lib/include", 26 | ] 27 | for inc in include_candidates: 28 | if inc.is_dir(): 29 | cppyy.add_include_path(str(inc)) 30 | 31 | # Libraries (ensure primary deps come first) 32 | lib_candidates = [ 33 | ws_root / "build/cloudini_lib/libcloudini_lib.so", 34 | ws_root / "build/cloudini_lib/tools/libmcap_converter.so", 35 | ] 36 | for lib in lib_candidates: 37 | if lib.is_file(): 38 | cppyy.load_library(str(lib)) 39 | 40 | # Ensure optional is known for constructing std::optional 41 | cppyy.cppdef("#include ") 42 | 43 | # Bring converter + Cloudini types into scope 44 | cppyy.include("mcap_converter.hpp") 45 | cppyy.include("cloudini_lib/cloudini.hpp") 46 | 47 | 48 | app = typer.Typer(add_completion=False) 49 | 50 | 51 | class CompressionMethod(str, Enum): 52 | none = "none" 53 | lz4 = "lz4" 54 | zstd = "zstd" 55 | 56 | 57 | @app.command() 58 | def convert( 59 | filename: str = typer.Option(..., "-f", "--filename", help="Input .mcap file"), 60 | output: Optional[str] = typer.Option(None, "-o", "--output", help="Output .mcap file (auto-named if omitted)"), 61 | resolution: float = typer.Option(0.001, "-r", "--resolution", help="Resolution for lossy fields (encode only)"), 62 | profile: Optional[str] = typer.Option(None, "--profile", help="Profile string or path for field resolutions (encode only)"), 63 | compress: bool = typer.Option(False, "-c", "--compress", help="Convert PointCloud2 to CompressedPointCloud2"), 64 | decode: bool = typer.Option(False, "-d", "--decode", help="Convert CompressedPointCloud2 to PointCloud2"), 65 | method: CompressionMethod = typer.Option(CompressionMethod.zstd, "-m", "--method", help="Compression for MCAP chunks when writing"), 66 | yes: bool = typer.Option(False, "-y", "--yes", help="Overwrite output if it exists"), 67 | ) -> None: 68 | in_path = Path(filename) 69 | if in_path.suffix != ".mcap": 70 | typer.echo(f"Error: input must be a .mcap file: {in_path}", err=True) 71 | raise typer.Exit(code=1) 72 | if not in_path.is_file(): 73 | typer.echo(f"Error: file not found: {in_path}", err=True) 74 | raise typer.Exit(code=1) 75 | 76 | if compress and decode: 77 | typer.echo("Error: cannot specify both --compress and --decode", err=True) 78 | raise typer.Exit(code=1) 79 | if not compress and not decode: 80 | typer.echo("Error: must specify either --compress or --decode", err=True) 81 | raise typer.Exit(code=1) 82 | if decode and profile: 83 | typer.echo("Error: --profile is only valid with --compress", err=True) 84 | raise typer.Exit(code=1) 85 | 86 | # Resolve workspace root and set up cppyy 87 | ws_root = _find_workspace_root(Path(__file__).parent) 88 | _setup_cppyy(ws_root) 89 | 90 | # Compression method mapping 91 | CompressionOption = cppyy.gbl.Cloudini.CompressionOption 92 | method_map = { 93 | "none": CompressionOption.NONE, 94 | "lz4": CompressionOption.LZ4, 95 | "zstd": CompressionOption.ZSTD, 96 | } 97 | mcap_writer_compression = method_map[method.value] 98 | 99 | # Output filename 100 | if output: 101 | out_path = Path(output) 102 | if out_path.suffix != ".mcap": 103 | out_path = out_path.with_suffix(".mcap") 104 | else: 105 | suffix = "_encoded.mcap" if compress else "_decoded.mcap" 106 | out_path = in_path.with_name(in_path.stem + suffix) 107 | 108 | if out_path.exists() and not yes: 109 | typer.echo(f"Error: output exists: {out_path} (use -y to overwrite)", err=True) 110 | raise typer.Exit(code=1) 111 | 112 | # Run conversion 113 | converter = cppyy.gbl.McapConverter() 114 | converter.open(str(in_path)) 115 | 116 | if compress: 117 | # Optional profile 118 | if profile: 119 | prof = profile 120 | p = Path(prof) 121 | if p.is_file(): 122 | prof = p.read_text().strip() 123 | converter.addProfile(prof) 124 | # std::optional(resolution) 125 | std_optional_float = cppyy.gbl.std.optional['float'] 126 | default_res = std_optional_float(float(resolution)) 127 | converter.encodePointClouds(str(out_path), default_res, mcap_writer_compression) 128 | else: 129 | converter.decodePointClouds(str(out_path), mcap_writer_compression) 130 | 131 | typer.echo(str(out_path)) 132 | 133 | 134 | if __name__ == "__main__": 135 | app() 136 | 137 | # ./3_cloudini_cli.py -o testout.mcap -r 0.1 -f ../pointcloud_lexus3-2024-04-05-gyor.mcap --compress -y 138 | # Original size: 541MB 139 | # Compressed : 289MB -------------------------------------------------------------------------------- /scripts/benchmarks/bench_sub_rclcppyy.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import sys 4 | import signal 5 | import numpy as np 6 | from rclcppyy import bringup_rclcpp 7 | import cppyy 8 | 9 | bringup_rclcpp() 10 | 11 | cppyy.include("std_msgs/msg/string.hpp") 12 | cppyy.include("chrono") 13 | cppyy.include("functional") 14 | 15 | class PythonPerfSubscriber: 16 | def __init__(self): 17 | # Initialize ROS2 if not already initialized 18 | if not cppyy.gbl.rclcpp.ok(): 19 | cppyy.gbl.rclcpp.init(len(sys.argv), sys.argv) 20 | 21 | # Create a ROS2 node 22 | self.node = cppyy.gbl.rclcpp.Node("perf_subscriber_pythonic") 23 | 24 | # Create a subscription - minimal C++ wrapper for callback 25 | cppyy.cppdef(""" 26 | #include 27 | #include 28 | 29 | static std::function 30 | create_subscription_callback(PyObject* self) { 31 | return [self](const std_msgs::msg::String::SharedPtr msg) { 32 | if (self && PyObject_HasAttrString(self, "subscription_callback")) { 33 | PyObject* py_data = PyUnicode_FromString(msg->data.c_str()); 34 | PyObject_CallMethod(self, "subscription_callback", "O", py_data); 35 | Py_XDECREF(py_data); 36 | } 37 | }; 38 | } 39 | """) 40 | 41 | self.warmup() 42 | 43 | # Initialize tracking variables 44 | self.count = 0 45 | self.start_time = cppyy.gbl.std.chrono.steady_clock.now() 46 | self.latencies = [] 47 | self.last_seq = -1 48 | self.dropped_msgs = 0 49 | 50 | # Create the subscription 51 | callback = cppyy.gbl.create_subscription_callback(self) 52 | self.subscription = self.node.create_subscription[cppyy.gbl.std_msgs.msg.String]( 53 | "perf_topic_pythonic", 500, callback) 54 | 55 | def warmup(self): 56 | """ 57 | These two operations imply a JIT compilation of the code. 58 | This is a warmup to avoid the first call being slower and dropping some messages. 59 | """ 60 | _ = cppyy.gbl.std.chrono.duration_cast[cppyy.gbl.std.chrono.nanoseconds]( 61 | cppyy.gbl.std.chrono.high_resolution_clock.now().time_since_epoch()).count() 62 | _ = cppyy.gbl.std.chrono.steady_clock.now() 63 | 64 | 65 | def subscription_callback(self, msg_data): 66 | seq, pub_timestamp = msg_data.split(':') 67 | seq = int(seq) 68 | pub_timestamp = int(pub_timestamp) 69 | 70 | # Calculate latency 71 | recv_timestamp = cppyy.gbl.std.chrono.duration_cast[cppyy.gbl.std.chrono.nanoseconds]( 72 | cppyy.gbl.std.chrono.high_resolution_clock.now().time_since_epoch()).count() 73 | latency_us = (recv_timestamp - pub_timestamp) / 1000.0 # Convert to microseconds 74 | self.latencies.append(latency_us) 75 | 76 | # Check for dropped messages 77 | if self.last_seq >= 0 and seq != self.last_seq + 1: 78 | self.dropped_msgs += seq - self.last_seq - 1 79 | self.last_seq = seq 80 | 81 | self.count += 1 82 | 83 | # Print stats every second (assuming 1000Hz rate) 84 | if self.count % 1000 == 0: 85 | elapsed = cppyy.gbl.std.chrono.duration[cppyy.gbl.double]( 86 | cppyy.gbl.std.chrono.steady_clock.now() - self.start_time).count() 87 | rate = self.count / elapsed 88 | 89 | # Calculate latency statistics from recent messages 90 | recent_latencies = self.latencies[-1000:] 91 | avg_latency = np.mean(recent_latencies) 92 | p99_latency = np.percentile(recent_latencies, 99) 93 | 94 | print(f"(rclcppyy) Messages: {self.count}, Rate: {rate:.2f} msgs/sec, " 95 | f"Latency (μs) - Avg: {avg_latency:.1f}, P99: {p99_latency:.1f}, " 96 | f"Dropped: {self.dropped_msgs}") 97 | 98 | # Reset counters for next period 99 | self.start_time = cppyy.gbl.std.chrono.steady_clock.now() 100 | self.count = 0 101 | 102 | def spin(self): 103 | try: 104 | # Spin the node 105 | cppyy.gbl.rclcpp.spin(self.node) 106 | except KeyboardInterrupt: 107 | pass 108 | finally: 109 | # Print final statistics 110 | if self.latencies: 111 | print("\nFinal Statistics:") 112 | print(f"Total messages received: {len(self.latencies)}") 113 | print(f"Total messages dropped: {self.dropped_msgs}") 114 | print(f"Overall latency - Avg: {np.mean(self.latencies):.1f} μs, " 115 | f"P99: {np.percentile(self.latencies, 99):.1f} μs") 116 | 117 | # Shutdown ROS2 118 | cppyy.gbl.rclcpp.shutdown() 119 | 120 | def main(): 121 | print("Starting pythonic cppyy subscriber benchmark") 122 | 123 | # Set up signal handler for clean shutdown 124 | def signal_handler(sig, frame): 125 | print("Shutting down...") 126 | cppyy.gbl.rclcpp.shutdown() 127 | sys.exit(0) 128 | 129 | signal.signal(signal.SIGINT, signal_handler) 130 | 131 | # Create and run the subscriber 132 | subscriber = PythonPerfSubscriber() 133 | subscriber.spin() 134 | 135 | if __name__ == "__main__": 136 | main() -------------------------------------------------------------------------------- /rclcppyy/serialization.py: -------------------------------------------------------------------------------- 1 | """ 2 | Expose rclcpp Serialization for use from Python. 3 | 4 | This gives access to functions for serializing/deserializing C++ messages 5 | and helpers to construct C++ message classes with cppyy. 6 | """ 7 | 8 | from typing import Tuple, Any 9 | import os 10 | import cppyy 11 | from ament_index_python.packages import get_package_prefix 12 | 13 | from rclcppyy.bringup_rclcpp import bringup_rclcpp, _resolve_message_type 14 | 15 | 16 | def _ensure_headers_for_msg(package: str, python_msg_module: str) -> None: 17 | # python_msg_module looks like 'std_msgs.msg._string' 18 | # header should be '/msg/.hpp' where name is module third part without leading '_' 19 | parts = python_msg_module.split('.') 20 | if len(parts) >= 3: 21 | header_name = parts[2][1:] + '.hpp' 22 | cppyy.add_include_path(os.path.join(get_package_prefix(package), 'include', package)) 23 | cppyy.include(f'{package}/msg/{header_name}') 24 | 25 | 26 | def cpp_message_type_from_python(py_msg_cls: Any) -> Tuple[str, Any]: 27 | """ 28 | Given a Python message class (e.g. sensor_msgs.msg.Image), return 29 | (cpp_type_string, cppyy_class) for the corresponding C++ message. 30 | """ 31 | bringup_rclcpp() 32 | return _resolve_message_type(py_msg_cls) 33 | 34 | 35 | def get_serialization_for(py_msg_or_cpp_cls: Any) -> Any: 36 | """ 37 | Return rclcpp::Serialization bound for the provided message type. 38 | Accepts either a Python msg class or a cppyy C++ msg class. 39 | """ 40 | bringup_rclcpp() 41 | cpp_type_str, cpp_cls = _resolve_message_type(py_msg_or_cpp_cls) 42 | SerializationT = cppyy.gbl.rclcpp.Serialization[cpp_cls] 43 | return SerializationT() 44 | 45 | 46 | def serialize_message(msg_cpp: Any) -> Any: 47 | """ 48 | Serialize a C++ message (cppyy instance) into rclcpp::SerializedMessage. 49 | """ 50 | bringup_rclcpp() 51 | # Deduce T from the instance type by asking its class 52 | cpp_cls = type(msg_cpp) 53 | SerializationT = cppyy.gbl.rclcpp.Serialization[cpp_cls] 54 | ser = SerializationT() 55 | out = cppyy.gbl.rclcpp.SerializedMessage() 56 | ser.serialize_message(msg_cpp, out) 57 | return out 58 | 59 | 60 | def deserialize_message(serialized: Any, py_msg_or_cpp_cls: Any) -> Any: 61 | """ 62 | Deserialize rclcpp::SerializedMessage into a cppyy C++ message instance. 63 | Accepts a Python msg class or cppyy class to identify T. 64 | """ 65 | bringup_rclcpp() 66 | cpp_type_str, cpp_cls = _resolve_message_type(py_msg_or_cpp_cls) 67 | SerializationT = cppyy.gbl.rclcpp.Serialization[cpp_cls] 68 | ser = SerializationT() 69 | out = cpp_cls() 70 | ser.deserialize_message(serialized, out) 71 | return out 72 | 73 | 74 | _HELPERS_DEFINED = False 75 | 76 | 77 | def _ensure_serialization_helpers() -> None: 78 | global _HELPERS_DEFINED 79 | if _HELPERS_DEFINED: 80 | return 81 | bringup_rclcpp() 82 | # Helper C++ functions to convert between bytes and rclcpp::SerializedMessage 83 | cppyy.include('rclcpp/serialized_message.hpp') 84 | cppyy.include('rosbag2_storage/serialized_bag_message.hpp') 85 | cppyy.include('rosbag2_storage/ros_helper.hpp') 86 | cppyy.cppdef(r""" 87 | #include 88 | #include 89 | #include 90 | #include 91 | #include 92 | #include 93 | 94 | rclcpp::SerializedMessage rclcppyy_make_serialized_message_from_string(const std::string& s) { 95 | rclcpp::SerializedMessage out; 96 | out.reserve(s.size()); 97 | auto& r = out.get_rcl_serialized_message(); 98 | std::memcpy(r.buffer, s.data(), s.size()); 99 | r.buffer_length = s.size(); 100 | return out; 101 | } 102 | 103 | PyObject* rclcppyy_serialized_message_to_pybytes(const rclcpp::SerializedMessage& sm) { 104 | const auto& r = sm.get_rcl_serialized_message(); 105 | return PyBytes_FromStringAndSize((const char*)r.buffer, (Py_ssize_t)r.buffer_length); 106 | } 107 | 108 | rclcpp::SerializedMessage rclcppyy_make_serialized_message_from_buffer(const void* buf, size_t len) { 109 | rclcpp::SerializedMessage out; 110 | out.reserve(len); 111 | auto& r = out.get_rcl_serialized_message(); 112 | std::memcpy(r.buffer, buf, len); 113 | r.buffer_length = len; 114 | return out; 115 | } 116 | 117 | std::shared_ptr rclcppyy_make_sbm_from_serialized_message( 118 | const rclcpp::SerializedMessage& sm, 119 | const std::string& topic, 120 | long long recv_ts, 121 | long long send_ts) { 122 | auto out = std::make_shared(); 123 | const auto& r = sm.get_rcl_serialized_message(); 124 | out->serialized_data = rosbag2_storage::make_serialized_message((const char*)r.buffer, r.buffer_length); 125 | out->topic_name = topic; 126 | out->recv_timestamp = (int64_t)recv_ts; 127 | out->send_timestamp = (int64_t)send_ts; 128 | return out; 129 | } 130 | """) 131 | _HELPERS_DEFINED = True 132 | 133 | 134 | def serialized_message_from_bytes(data: bytes) -> Any: 135 | """ 136 | Build a C++ rclcpp::SerializedMessage from Python bytes. 137 | """ 138 | _ensure_serialization_helpers() 139 | return cppyy.gbl.rclcppyy_make_serialized_message_from_string(data) 140 | 141 | 142 | def serialized_message_to_bytes(serialized_msg: Any) -> bytes: 143 | """ 144 | Convert a C++ rclcpp::SerializedMessage to Python bytes. 145 | """ 146 | _ensure_serialization_helpers() 147 | return cppyy.gbl.rclcppyy_serialized_message_to_pybytes(serialized_msg) 148 | 149 | 150 | -------------------------------------------------------------------------------- /scripts/bench_rosbag2_py_apis.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import argparse 4 | import os 5 | import gc 6 | import shutil 7 | import time 8 | from dataclasses import dataclass 9 | from typing import List 10 | 11 | from rosidl_runtime_py.utilities import get_message 12 | from rclpy.serialization import serialize_message as rclpy_serialize, deserialize_message as rclpy_deserialize 13 | from rclcppyy.serialization import deserialize_message as cxx_deserialize, serialize_message as cxx_serialize, serialized_message_from_bytes, serialized_message_to_bytes, cpp_message_type_from_python 14 | 15 | from rosbag2_py import SequentialReader as PyReader, SequentialWriter as PyWriter, StorageOptions as PyStorageOptions, ConverterOptions as PyConverterOptions, TopicMetadata as PyTopicMetadata 16 | 17 | from rclcppyy.rosbag2_py_compat import SequentialReader as CxxReaderCompat, SequentialWriter as CxxWriterCompat, StorageOptions as CxxStorageOptions, ConverterOptions as CxxConverterOptions, TopicMetadata as CxxTopicMetadata 18 | 19 | 20 | @dataclass 21 | class Args: 22 | input_uri: str 23 | storage_id: str 24 | serialization: str 25 | runs: int 26 | add_serialisation_roundtrip: bool 27 | disable_gc: bool 28 | 29 | 30 | def run_py_api(args: Args, out_dir: str): 31 | reader = PyReader() 32 | reader.open(PyStorageOptions(uri=args.input_uri, storage_id=args.storage_id), 33 | PyConverterOptions(input_serialization_format=args.serialization, output_serialization_format=args.serialization)) 34 | writer = PyWriter() 35 | writer.open(PyStorageOptions(uri=out_dir, storage_id=args.storage_id), 36 | PyConverterOptions(input_serialization_format=args.serialization, output_serialization_format=args.serialization)) 37 | 38 | try: 39 | topics = reader.get_all_topics_and_types() 40 | except AttributeError: 41 | topics = reader.get_topics_and_types() 42 | 43 | topic_type = {t.name: t.type for t in topics} 44 | 45 | for t in topics: 46 | writer.create_topic(PyTopicMetadata( 47 | id=0, 48 | name=t.name, 49 | type=t.type, 50 | serialization_format=args.serialization 51 | )) 52 | 53 | t0 = time.time() 54 | count = 0 55 | while reader.has_next(): 56 | topic, data, ts = reader.read_next() 57 | if args.add_serialisation_roundtrip: 58 | msg_py_cls = get_message(topic_type.get(topic, '')) 59 | if msg_py_cls is not None: 60 | msg_py = rclpy_deserialize(data, msg_py_cls) 61 | # We could do something here with the message 62 | data = rclpy_serialize(msg_py) 63 | writer.write(topic, data, ts) 64 | count += 1 65 | t1 = time.time() 66 | return t1 - t0, count 67 | 68 | 69 | def run_cxx_api(args: Args, out_dir: str): 70 | reader = CxxReaderCompat() 71 | reader.open(CxxStorageOptions(uri=args.input_uri, storage_id=args.storage_id), 72 | CxxConverterOptions(input_serialization_format=args.serialization, output_serialization_format=args.serialization)) 73 | writer = CxxWriterCompat() 74 | writer.open(CxxStorageOptions(uri=out_dir, storage_id=args.storage_id), 75 | CxxConverterOptions(input_serialization_format=args.serialization, output_serialization_format=args.serialization)) 76 | 77 | try: 78 | topics = reader.get_all_topics_and_types() 79 | except Exception: 80 | topics = reader.get_topics_and_types() 81 | 82 | topic_type = {t.name: t.type for t in topics} 83 | 84 | for t in topics: 85 | writer.create_topic(CxxTopicMetadata( 86 | id=0, 87 | name=t.name, 88 | type=t.type, 89 | serialization_format=args.serialization 90 | )) 91 | 92 | t0 = time.time() 93 | count = 0 94 | while reader.has_next(): 95 | topic, data, ts = reader.read_next() 96 | if args.add_serialisation_roundtrip: 97 | msg_py_cls = get_message(topic_type.get(topic, '')) 98 | if msg_py_cls is not None: 99 | msg_cpp = cxx_deserialize(data, msg_py_cls) 100 | # We could do something here with the message 101 | data = cxx_serialize(msg_cpp) 102 | writer.write(topic, data, ts) 103 | count += 1 104 | t1 = time.time() 105 | return t1 - t0, count 106 | 107 | 108 | def main(): 109 | p = argparse.ArgumentParser(description='Benchmark rosbag2_py vs rclcppyy.rosbag2_py_compat APIs.') 110 | p.add_argument('input_uri') 111 | p.add_argument('--storage-id', default='mcap') 112 | p.add_argument('--serialization', default='cdr') 113 | p.add_argument('--runs', type=int, default=int(os.environ.get('RUN_BENCHMARK_TIMES', '50'))) 114 | p.add_argument('--add-serialisation-roundtrip', action='store_true', help='Deserialize bytes to message and re-serialize before writing') 115 | p.add_argument('--disable-gc', action='store_true', help='Disable Python GC inside each run loop') 116 | a = p.parse_args() 117 | 118 | args = Args(a.input_uri, a.storage_id, a.serialization, max(1, a.runs), bool(a.add_serialisation_roundtrip), bool(a.disable_gc)) 119 | 120 | base_tmp_dir = os.path.join('/tmp', 'some_temporary_folder_pyapi') 121 | shutil.rmtree(base_tmp_dir, ignore_errors=True) 122 | os.makedirs(base_tmp_dir, exist_ok=True) 123 | 124 | py_times: List[float] = [] 125 | cxx_times: List[float] = [] 126 | 127 | try: 128 | for i in range(args.runs): 129 | out_dir = os.path.join(base_tmp_dir, f'py_run_{i}') 130 | shutil.rmtree(out_dir, ignore_errors=True) 131 | gc_was_enabled = gc.isenabled() 132 | if args.disable_gc and gc_was_enabled: 133 | gc.disable() 134 | try: 135 | t, n = run_py_api(args, out_dir) 136 | finally: 137 | if args.disable_gc and gc_was_enabled: 138 | gc.enable() 139 | gc.collect() 140 | py_times.append(t) 141 | print(f"rosbag2_py run {i+1}/{args.runs}: {n} messages in {t:.3f}s -> {n/max(t,1e-9):.1f} msgs/s") 142 | 143 | for i in range(args.runs): 144 | out_dir = os.path.join(base_tmp_dir, f'cxx_run_{i}') 145 | shutil.rmtree(out_dir, ignore_errors=True) 146 | gc_was_enabled = gc.isenabled() 147 | if args.disable_gc and gc_was_enabled: 148 | gc.disable() 149 | try: 150 | t, n = run_cxx_api(args, out_dir) 151 | finally: 152 | if args.disable_gc and gc_was_enabled: 153 | gc.enable() 154 | gc.collect() 155 | cxx_times.append(t) 156 | print(f"py_compat run {i+1}/{args.runs}: {n} messages in {t:.3f}s -> {n/max(t,1e-9):.1f} msgs/s") 157 | 158 | if py_times: 159 | print(f"rosbag2_py summary over {len(py_times)} runs: min={min(py_times):.3f}s max={max(py_times):.3f}s avg={sum(py_times)/len(py_times):.3f}s") 160 | if cxx_times: 161 | print(f"py_compat summary over {len(cxx_times)} runs: min={min(cxx_times):.3f}s max={max(cxx_times):.3f}s avg={sum(cxx_times)/len(cxx_times):.3f}s") 162 | finally: 163 | shutil.rmtree(base_tmp_dir, ignore_errors=True) 164 | 165 | 166 | if __name__ == '__main__': 167 | main() 168 | -------------------------------------------------------------------------------- /scripts/bag2bag_cppyy_purecpp.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import time 4 | import argparse 5 | import os 6 | import glob 7 | import cppyy 8 | from dataclasses import dataclass 9 | from ament_index_python.packages import get_packages_with_prefixes 10 | from rclcppyy.bringup_rclcpp import bringup_rclcpp 11 | 12 | 13 | def _try_load_library_candidates(lib_basenames): 14 | for name in lib_basenames: 15 | try: 16 | cppyy.load_library(name) 17 | except Exception: 18 | pass 19 | 20 | # Try absolute paths under all package prefixes 21 | pkgs = get_packages_with_prefixes() 22 | for _, prefix in pkgs.items(): 23 | lib_dir = os.path.join(prefix, 'lib') 24 | if not os.path.isdir(lib_dir): 25 | continue 26 | for base in lib_basenames: 27 | pattern = os.path.join(lib_dir, base.replace('.so', '') + '*.so*') 28 | for candidate in glob.glob(pattern): 29 | try: 30 | cppyy.load_library(candidate) 31 | except Exception: 32 | continue 33 | 34 | 35 | @dataclass 36 | class Args: 37 | input_uri: str 38 | output_uri: str 39 | image_topic: str 40 | suffix: str 41 | 42 | 43 | def main(): 44 | # Ensure rclcpp includes are configured 45 | bringup_rclcpp() 46 | 47 | # Load likely-needed shared libs (best effort) 48 | _try_load_library_candidates([ 49 | 'librosbag2_cpp.so', 50 | 'librosbag2_storage.so', 51 | 'librosbag2_storage_default_plugins.so', 52 | 'librosbag2_storage_mcap_plugin.so', 53 | 'librclcpp.so', 54 | 'librcl.so', 55 | 'librcutils.so', 56 | 'librcpputils.so', 57 | 'librmw.so' 58 | ]) 59 | 60 | # Includes 61 | cppyy.include('rosbag2_cpp/reader.hpp') 62 | cppyy.include('rosbag2_cpp/writer.hpp') 63 | cppyy.include('rosbag2_storage/serialized_bag_message.hpp') 64 | cppyy.include('rosbag2_storage/storage_options.hpp') 65 | cppyy.include('rosbag2_cpp/converter_options.hpp') 66 | cppyy.include('rclcpp/serialization.hpp') 67 | cppyy.include('rclcpp/serialized_message.hpp') 68 | cppyy.include('sensor_msgs/msg/image.hpp') 69 | # cppyy.include('rcutils/types/rcutils_uint8_array.h') 70 | 71 | cppyy.include('string') 72 | cppyy.include('vector') 73 | cppyy.include('memory') 74 | cppyy.include('unordered_map') 75 | 76 | cppyy.cppdef(r""" 77 | #include 78 | #include 79 | #include 80 | #include 81 | #include 82 | #include 83 | #include 84 | #include 85 | // #include 86 | #include 87 | #include 88 | #include 89 | #include 90 | 91 | static void bgr_or_rgb_to_gray_vec(std::vector& data, 92 | uint32_t width, uint32_t height, uint32_t step, 93 | bool is_bgr, 94 | std::vector& out_gray) 95 | { 96 | out_gray.resize(width*height); 97 | for (uint32_t y = 0; y < height; ++y) { 98 | const uint8_t* row = data.data() + y*step; 99 | uint8_t* grow = out_gray.data() + y*width; 100 | for (uint32_t x = 0; x < width; ++x) { 101 | uint8_t c0 = row[3*x+0]; 102 | uint8_t c1 = row[3*x+1]; 103 | uint8_t c2 = row[3*x+2]; 104 | uint8_t b = is_bgr ? c0 : c2; 105 | uint8_t g = c1; 106 | uint8_t r = is_bgr ? c2 : c0; 107 | float yf = 0.114f*b + 0.587f*g + 0.299f*r; 108 | grow[x] = (uint8_t)(yf); 109 | } 110 | } 111 | } 112 | 113 | extern "C" void process_bag_to_gray(const std::string& input_uri, 114 | const std::string& output_uri, 115 | const std::string& image_topic, 116 | const std::string& suffix) 117 | { 118 | rosbag2_cpp::Reader reader; 119 | rosbag2_storage::StorageOptions inopt; 120 | inopt.uri = input_uri; 121 | inopt.storage_id = "mcap"; 122 | rosbag2_cpp::ConverterOptions conv; 123 | conv.input_serialization_format = "cdr"; 124 | conv.output_serialization_format = "cdr"; 125 | reader.open(inopt, conv); 126 | 127 | rosbag2_cpp::Writer writer; 128 | rosbag2_storage::StorageOptions outopt; 129 | outopt.uri = output_uri; 130 | outopt.storage_id = "mcap"; 131 | writer.open(outopt, conv); 132 | 133 | auto topics = reader.get_all_topics_and_types(); 134 | std::unordered_map topic_types; 135 | for (auto & md : topics) { 136 | topic_types[md.name] = md.type; 137 | if (md.name == image_topic && md.type == "sensor_msgs/msg/Image") { 138 | rosbag2_storage::TopicMetadata omd; 139 | omd.name = md.name + suffix; 140 | omd.type = md.type; 141 | omd.serialization_format = "cdr"; 142 | writer.create_topic(omd); 143 | } 144 | } 145 | 146 | rclcpp::Serialization ser; 147 | 148 | while (reader.has_next()) { 149 | auto in_msg = reader.read_next(); 150 | auto it = topic_types.find(in_msg->topic_name); 151 | if (it == topic_types.end()) continue; 152 | 153 | if (in_msg->topic_name == image_topic && it->second == "sensor_msgs/msg/Image") { 154 | rclcpp::SerializedMessage smsg(*in_msg->serialized_data); 155 | sensor_msgs::msg::Image im; 156 | ser.deserialize_message(&smsg, &im); 157 | 158 | bool supported = (im.encoding == "bgr8" || im.encoding == "rgb8"); 159 | if (supported) { 160 | std::vector gray; 161 | bgr_or_rgb_to_gray_vec(im.data, im.width, im.height, im.step, im.encoding == "bgr8", gray); 162 | im.encoding = "mono8"; 163 | im.step = im.width; 164 | im.data = std::move(gray); 165 | } 166 | 167 | rclcpp::SerializedMessage out_smsg; 168 | ser.serialize_message(&im, &out_smsg); 169 | 170 | auto out = std::make_shared(); 171 | out->topic_name = in_msg->topic_name + suffix; 172 | auto arr = std::make_shared(); 173 | *arr = out_smsg.release_rcl_serialized_message(); 174 | out->serialized_data = arr; 175 | out->recv_timestamp = in_msg->recv_timestamp; 176 | 177 | writer.write(out); 178 | } 179 | } 180 | } 181 | """) 182 | 183 | ap = argparse.ArgumentParser(description='Bag-to-bag image grayscale converter (pure C++ via rosbag2_cpp).') 184 | ap.add_argument('input_uri') 185 | ap.add_argument('output_uri') 186 | ap.add_argument('--image-topic', default='/camera/image_raw') 187 | ap.add_argument('--suffix', default='_gray') 188 | a = ap.parse_args() 189 | 190 | # Time it 191 | start_time = time.time() 192 | cppyy.gbl.process_bag_to_gray(a.input_uri, a.output_uri, a.image_topic, a.suffix) 193 | end_time = time.time() 194 | print(f"Time taken: {end_time - start_time} seconds") 195 | 196 | 197 | if __name__ == '__main__': 198 | main() 199 | 200 | 201 | # After the slow upbringing... 202 | # 0.283s 203 | # 0.307s 204 | # 0.338s -------------------------------------------------------------------------------- /scripts/bench_rosbag2_cpp_vs_py.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import argparse 4 | import os 5 | import gc 6 | import shutil 7 | import time 8 | from dataclasses import dataclass 9 | from typing import List, Tuple 10 | 11 | from rosbag2_py import SequentialReader, SequentialWriter, StorageOptions, ConverterOptions, TopicMetadata 12 | 13 | from rclcppyy import rosbag2_cpp as rosbag2_cxx 14 | 15 | 16 | @dataclass 17 | class Args: 18 | input_uri: str 19 | output_uri_py: str 20 | output_uri_cxx: str 21 | storage_id: str 22 | serialization: str 23 | 24 | 25 | def warmup_cppyy(): 26 | # Bring up C++ symbols and instantiate common types to avoid JIT during timing 27 | import cppyy 28 | rosbag2_cxx.bringup_rosbag2_cpp() 29 | _ = rosbag2_cxx.types() 30 | # Instantiate key classes (no opening) using factories to avoid unique_ptr copy issues 31 | _ = cppyy.gbl.rclcppyy_make_reader_default() 32 | _ = cppyy.gbl.rclcppyy_make_writer_default() 33 | _ = cppyy.gbl.rosbag2_storage.SerializedBagMessage() 34 | 35 | 36 | def run_python_copy(args: Args): 37 | reader = SequentialReader() 38 | reader.open(StorageOptions(uri=args.input_uri, storage_id=args.storage_id), 39 | ConverterOptions(input_serialization_format=args.serialization, output_serialization_format=args.serialization)) 40 | writer = SequentialWriter() 41 | writer.open(StorageOptions(uri=args.output_uri_py, storage_id=args.storage_id), 42 | ConverterOptions(input_serialization_format=args.serialization, output_serialization_format=args.serialization)) 43 | 44 | try: 45 | topics = reader.get_all_topics_and_types() 46 | except AttributeError: 47 | topics = reader.get_topics_and_types() 48 | 49 | for t in topics: 50 | writer.create_topic(TopicMetadata( 51 | id=0, 52 | name=t.name, 53 | type=t.type, 54 | serialization_format=args.serialization 55 | )) 56 | 57 | has_next = reader.has_next 58 | read_next = reader.read_next 59 | write = writer.write 60 | 61 | gc_was_enabled = gc.isenabled() 62 | if gc_was_enabled: 63 | gc.disable() 64 | try: 65 | t0 = time.time() 66 | count = 0 67 | while has_next(): 68 | topic, data, ts = read_next() 69 | write(topic, data, ts) 70 | count += 1 71 | t1 = time.time() 72 | finally: 73 | if gc_was_enabled: 74 | gc.enable() 75 | gc.collect() 76 | return t1 - t0, count 77 | 78 | 79 | def run_cxx_copy(args: Args): 80 | rosbag2_cxx.bringup_rosbag2_cpp() 81 | reader = rosbag2_cxx.open_reader(args.input_uri, storage_id=args.storage_id, serialization=args.serialization) 82 | writer = rosbag2_cxx.open_writer(args.output_uri_cxx, storage_id=args.storage_id, serialization=args.serialization) 83 | 84 | # Create all topics on writer using direct C++ calls (avoid Python wrappers) 85 | try: 86 | topics = reader.get_all_topics_and_types() 87 | except Exception: 88 | topics = reader.get_topics_and_types() 89 | create_topic = writer.create_topic 90 | for md in topics: 91 | create_topic(md) 92 | 93 | has_next = reader.has_next 94 | read_next = reader.read_next 95 | write = writer.write 96 | 97 | gc_was_enabled = gc.isenabled() 98 | if gc_was_enabled: 99 | gc.disable() 100 | try: 101 | t0 = time.time() 102 | count = 0 103 | while has_next(): 104 | sbm = read_next() 105 | write(sbm) 106 | count += 1 107 | t1 = time.time() 108 | finally: 109 | if gc_was_enabled: 110 | gc.enable() 111 | gc.collect() 112 | return t1 - t0, count 113 | 114 | 115 | def main(): 116 | p = argparse.ArgumentParser(description='Benchmark pure read+write throughput: rosbag2_py vs rosbag2_cpp (cppyy).') 117 | p.add_argument('input_uri') 118 | p.add_argument('--out-py', default='out_py_copy') 119 | p.add_argument('--out-cxx', default='out_cxx_copy') 120 | p.add_argument('--storage-id', default='mcap') 121 | p.add_argument('--serialization', default='cdr') 122 | a = p.parse_args() 123 | args = Args(a.input_uri, a.out_py, a.out_cxx, a.storage_id, a.serialization) 124 | 125 | # Benchmark configuration 126 | runs_env = os.environ.get('RUN_BENCHMARK_TIMES', '100') 127 | try: 128 | RUNS = max(1, int(runs_env)) 129 | except Exception: 130 | RUNS = 100 131 | 132 | # Use a dedicated temp base directory for all outputs 133 | base_tmp_dir = os.path.join('/tmp', 'some_temporary_folder') 134 | # Start fresh 135 | shutil.rmtree(base_tmp_dir, ignore_errors=True) 136 | os.makedirs(base_tmp_dir, exist_ok=True) 137 | 138 | # Warm up cppyy/rosbag2_cpp to avoid JIT in measured section 139 | warmup_cppyy() 140 | 141 | py_times: List[float] = [] 142 | cxx_times: List[float] = [] 143 | py_counts: List[int] = [] 144 | cxx_counts: List[int] = [] 145 | 146 | try: 147 | # Run Python implementation RUNS times 148 | for i in range(RUNS): 149 | out_py_i = os.path.join(base_tmp_dir, f'py_run_{i}') 150 | # Ensure bag directory does not exist; writer expects to create it 151 | shutil.rmtree(out_py_i, ignore_errors=True) 152 | run_args_py = Args( 153 | input_uri=args.input_uri, 154 | output_uri_py=out_py_i, 155 | output_uri_cxx='', 156 | storage_id=args.storage_id, 157 | serialization=args.serialization, 158 | ) 159 | t_py, n_py = run_python_copy(run_args_py) 160 | py_times.append(t_py) 161 | py_counts.append(n_py) 162 | print(f"Python copy run {i+1}/{RUNS}: {n_py} messages in {t_py:.3f}s -> {n_py/max(t_py,1e-9):.1f} msgs/s") 163 | 164 | # Run C++ implementation RUNS times 165 | for i in range(RUNS): 166 | out_cxx_i = os.path.join(base_tmp_dir, f'cxx_run_{i}') 167 | # Ensure bag directory does not exist; writer expects to create it 168 | shutil.rmtree(out_cxx_i, ignore_errors=True) 169 | run_args_cxx = Args( 170 | input_uri=args.input_uri, 171 | output_uri_py='', 172 | output_uri_cxx=out_cxx_i, 173 | storage_id=args.storage_id, 174 | serialization=args.serialization, 175 | ) 176 | t_cxx, n_cxx = run_cxx_copy(run_args_cxx) 177 | cxx_times.append(t_cxx) 178 | cxx_counts.append(n_cxx) 179 | print(f"C++ copy run {i+1}/{RUNS}: {n_cxx} messages in {t_cxx:.3f}s -> {n_cxx/max(t_cxx,1e-9):.1f} msgs/s") 180 | 181 | # Summaries 182 | if py_times: 183 | py_min = min(py_times) 184 | py_max = max(py_times) 185 | py_avg = sum(py_times) / len(py_times) 186 | print(f"Python summary over {len(py_times)} runs: min={py_min:.3f}s max={py_max:.3f}s avg={py_avg:.3f}s") 187 | if cxx_times: 188 | cxx_min = min(cxx_times) 189 | cxx_max = max(cxx_times) 190 | cxx_avg = sum(cxx_times) / len(cxx_times) 191 | print(f"C++ summary over {len(cxx_times)} runs: min={cxx_min:.3f}s max={cxx_max:.3f}s avg={cxx_avg:.3f}s") 192 | finally: 193 | # Cleanup all generated outputs 194 | shutil.rmtree(base_tmp_dir, ignore_errors=True) 195 | 196 | 197 | if __name__ == '__main__': 198 | main() 199 | 200 | 201 | -------------------------------------------------------------------------------- /rclcppyy/monkeypatch_messages.py: -------------------------------------------------------------------------------- 1 | """ 2 | This file implements monkeypatching of ROS2 imported messages 3 | so they are created as rclcpp messages instead of rclpy messages. 4 | This avoids message conversion overhead when publishing/subscribing. 5 | """ 6 | 7 | import sys 8 | import inspect 9 | import importlib.util 10 | from importlib.abc import Loader, MetaPathFinder 11 | from importlib.machinery import ModuleSpec 12 | from rclcppyy.bringup_rclcpp import _resolve_message_type, _is_msg_python 13 | 14 | def _running_in_ipython(): 15 | try: 16 | get_ipython 17 | except NameError: 18 | return False 19 | else: 20 | return True 21 | 22 | def enable_kwargs_for_cpp_msg(msg_class): 23 | """Enable keyword arguments for C++ message constructors""" 24 | original_init = msg_class.__init__ 25 | def new_init(self, *args, **kwargs): 26 | # Call the original constructor with default arguments 27 | if args: 28 | original_init(self, *args) 29 | else: 30 | original_init(self) 31 | 32 | # Set any keyword arguments as attributes 33 | for key, value in kwargs.items(): 34 | if hasattr(self, key): 35 | setattr(self, key, value) 36 | else: 37 | raise AttributeError(f"'{self.__class__.__name__}' object has no attribute '{key}'") 38 | 39 | msg_class.__init__ = new_init 40 | 41 | def add_python_compatibility(cpp_msg_class, py_msg_class): 42 | """Add Python message methods to C++ message class for compatibility""" 43 | if hasattr(py_msg_class, '__repr__'): 44 | cpp_msg_class.__repr__ = py_msg_class.__repr__ 45 | 46 | # Get the attrs from py_msg_class that are not in cpp_msg_class already and that do not start with __ 47 | py_msg_class_attrs = [attr for attr in dir(py_msg_class) if not attr.startswith("__")] 48 | cpp_msg_class_attrs = [attr for attr in dir(cpp_msg_class) if not attr.startswith("__")] 49 | attrs_to_add = [attr for attr in py_msg_class_attrs if attr not in cpp_msg_class_attrs] 50 | 51 | for attr_name in attrs_to_add: 52 | setattr(cpp_msg_class, attr_name, getattr(py_msg_class, attr_name)) 53 | 54 | # Special case for service messages, they depend on their __class__ having the following attributes 55 | if hasattr(py_msg_class.__class__,"_TYPE_SUPPORT"): 56 | cpp_msg_class.__class__._TYPE_SUPPORT = py_msg_class.__class__._TYPE_SUPPORT 57 | if hasattr(py_msg_class.__class__, "__import_type_support__"): 58 | cpp_msg_class.__class__.__import_type_support__ = py_msg_class.__class__.__import_type_support__ 59 | 60 | class ROSMessageFinder(MetaPathFinder): 61 | """Import hook to automatically convert ROS messages to C++ equivalents""" 62 | 63 | def __init__(self): 64 | self._importing = set() 65 | self._original_loaders = {} 66 | 67 | def find_spec(self, fullname, path, target=None): 68 | # Only intercept ROS message type imports 69 | if '.msg._' in fullname and fullname not in self._importing: 70 | self._importing.add(fullname) 71 | try: 72 | spec = importlib.util.find_spec(fullname) 73 | finally: 74 | self._importing.remove(fullname) 75 | 76 | if spec is not None and spec.loader is not None: 77 | self._original_loaders[fullname] = spec.loader 78 | return ModuleSpec( 79 | fullname, 80 | ROSMessageLoader(self._original_loaders[fullname]), 81 | origin=spec.origin, 82 | is_package=spec.parent == '' 83 | ) 84 | return None 85 | 86 | class ROSMessageLoader(Loader): 87 | """Custom loader that converts ROS messages to C++ after normal loading""" 88 | 89 | def __init__(self, original_loader): 90 | self._original_loader = original_loader 91 | 92 | def create_module(self, spec): 93 | return self._original_loader.create_module(spec) 94 | 95 | def exec_module(self, module): 96 | # Execute with original loader first 97 | self._original_loader.exec_module(module) 98 | 99 | # Get the message name from the module 100 | msg_names = [attr for attr in dir(module) if attr.startswith("Metaclass_")] 101 | for metaclass_name in msg_names: 102 | msg_name = metaclass_name.replace("Metaclass_", "") 103 | 104 | # Only convert the actual message class 105 | if hasattr(module, msg_name): 106 | msg_class = getattr(module, msg_name) 107 | try: 108 | if _is_msg_python(msg_class): 109 | # Get C++ equivalent 110 | _, cpp_msg_class = _resolve_message_type(msg_class) 111 | enable_kwargs_for_cpp_msg(cpp_msg_class) 112 | add_python_compatibility(cpp_msg_class, msg_class) 113 | 114 | # Replace the class in the module 115 | setattr(module, msg_name, cpp_msg_class) 116 | except Exception as e: 117 | print(f"Failed to convert {msg_name}: {e}") 118 | 119 | def convert_already_imported_python_msgs_to_cpp(target_globals=None): 120 | """ 121 | Convert all Python ROS messages in globals() to their C++ equivalents with keyword support. 122 | 123 | Args: 124 | target_globals: The globals dict to modify. If None, uses caller's globals(). 125 | """ 126 | 127 | 128 | if target_globals is None: 129 | # Get the caller's globals 130 | if not _running_in_ipython: 131 | frame = inspect.currentframe() 132 | else: 133 | frame = inspect.currentframe().f_back 134 | target_globals = frame.f_globals 135 | 136 | # Find all Python ROS message classes in globals 137 | python_msgs = {} 138 | for name, obj in list(target_globals.items()): 139 | try: 140 | # Check if this is a Python ROS message class (not instance) 141 | if (hasattr(obj, '__module__') and 142 | hasattr(obj, '__name__') and 143 | _is_msg_python(obj) and 144 | inspect.isclass(obj)): 145 | python_msgs[name] = obj 146 | except Exception: 147 | # Skip any objects that cause issues during inspection 148 | continue 149 | 150 | print(f"Found {len(python_msgs)} Python ROS message classes: {list(python_msgs.keys())}") 151 | 152 | # Convert each Python message to C++ equivalent 153 | for name, py_msg_class in python_msgs.items(): 154 | try: 155 | # Get the C++ equivalent 156 | cpp_msg_type, cpp_msg_class = _resolve_message_type(py_msg_class) 157 | 158 | # Apply keyword argument monkey-patch 159 | enable_kwargs_for_cpp_msg(cpp_msg_class) 160 | add_python_compatibility(cpp_msg_class, py_msg_class) 161 | 162 | # Replace in globals 163 | target_globals[name] = cpp_msg_class 164 | print(f"Converted {name}: {py_msg_class} -> {cpp_msg_class}") 165 | 166 | except Exception as e: 167 | print(f"Failed to convert {name}: {e}") 168 | # Keep the original Python version if conversion fails 169 | continue 170 | 171 | 172 | def install_ros_message_hook(): 173 | """Install the import hook if not already installed""" 174 | if not any(isinstance(finder, ROSMessageFinder) for finder in sys.meta_path): 175 | sys.meta_path.insert(0, ROSMessageFinder()) 176 | return True 177 | return False -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # rclcppyy 2 | 3 | **!IMPORTANT!** 4 | 5 | **Note: If you come from ROSCON UK and are aiming to try this, I need a few days to clean up the repo and make it ready to use as you saw it in the presentation.** 6 | 7 | ROS 2 package providing `rclcpp` bindings via [cppyy](https://cppyy.readthedocs.io/en/latest/) and examples on how to use `cppyy` in ROS2. 8 | 9 | ![](media/rclcppyy_presentation_logo.jpg) 10 | 11 | 12 | * Tired of writing python wrappers for your C++ code? 13 | * Missing features from C++ APIs that you'd like to call in Python? 14 | * Do you like to prototype and test in Python but you use a lot of C++ code? 15 | 16 | `cppyy` can help you! Cppyy is a Python-C++ bindings library that provides automatic, runtime-based access to C++ code from Python using reflection and just-in-time compilation. It enables seamless interoperability between the two languages, allowing Python to call C++ functions and manipulate C++ objects directly. 17 | 18 | This repository aims to expose useful ROS2 C++ (and related) APIs via automatic wrapping with `cppyy`. 19 | 20 | For example you will be able to: 21 | * Transparently use `rclcpp`'s 22 | * Node 23 | * Publisher 24 | * Subscriber 25 | * Timer 26 | * Messages (without converting Python<>C++, always working on the C++ representation!) 27 | * init/spin/shutdown 28 | * Loaned messages (TODO!) 29 | 30 | To automatically replace your `rclpy` with `rclcpp` classes/methods just place at the top of your Python file: 31 | ```python 32 | import rclcppyy; rclcppyy.enable_cpp_acceleration() 33 | # Rest of your code doing import rclpy, from sensor_msgs.msg import Image... etc 34 | ``` 35 | 36 | To get an idea of how working with `cppyy` (without the quality of life features of `rclcppyy`) the code looks like (excerpt from an example): 37 | ```python 38 | import cppyy 39 | # include/import your stuff and then... 40 | 41 | if not cppyy.gbl.rclcpp.ok(): 42 | cppyy.gbl.rclcpp.init(len(sys.argv), sys.argv) 43 | 44 | # Some code in a class... 45 | self.node = cppyy.gbl.rclcpp.Node("node_exmaple") 46 | self.publisher = self.node.create_publisher[cppyy.gbl.std_msgs.msg.String]( 47 | "pub_topic", 10) 48 | 49 | # Define the callback wrapper with proper Python.h include 50 | cppyy.cppdef(""" 51 | #include 52 | #include 53 | 54 | static std::function create_timer_callback(PyObject* self) { 55 | return [self]() { 56 | if (self && PyObject_HasAttrString(self, "timer_callback")) { 57 | PyObject_CallMethod(self, "timer_callback", nullptr); 58 | } 59 | }; 60 | } 61 | """) 62 | 63 | callback = cppyy.gbl.create_timer_callback(self) 64 | self.timer = self.node.create_wall_timer( 65 | cppyy.gbl.std.chrono.nanoseconds(10000), 66 | callback) 67 | 68 | self.start_time = cppyy.gbl.std.chrono.steady_clock.now() 69 | ``` 70 | 71 | ## Examples 72 | 73 | * Benchmarks (ran on a Intel® Core™ Ultra 7 165H × 22 on "Performance" mode on Ubuntu 24.04) 74 | * Running a publisher and a subscriber (small `std_msgs/String`) at **1khz** 75 | ![Benchmark results for 1kHz publishing and subscribing](media/benchmark_pub_sub_1k_hz.png) 76 | * `rclpy` uses 15~% CPU for the publisher, and 18~% CPU for the subscriber 77 | * `rclcppyy` uses 4~% CPU for the publisher, and 4~% CPU for the subscriber 78 | 79 | * Running a publisher and a subscriber (small `std_msgs/String`) at **10khz** 80 | ![Benchmark results for 10kHz publishing and subscribing](media/benchmark_pub_sub_10k_hz.png) 81 | * `rclpy` uses 86~% CPU for the publisher, and 88~% CPU for the subscriber 82 | * `rclcppyy` uses 26~% CPU for the publisher, and 22~% CPU for the subscriber 83 | 84 | * The `publisher_member_function.py` [Writing a simple publisher and subscriber (Python)](https://docs.ros.org/en/jazzy/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Py-Publisher-And-Subscriber.html) tutorial using `rclcppyy` as backend. [scripts/ros_tutorials/publisher_member_function.py](scripts/ros_tutorials/publisher_member_function.py). 85 | * Note that the `rclpy` publisher benchmark [bench_pub_rclpy.py](scripts/benchmarks/bench_pub_rclpy.py) can be switched to the `rclcppyy` backend by uncommenting the `enable_cpp_acceleration` line at the top. 86 | 87 | 88 | ## Run demos 89 | Easiest way to test by yourself is using a Pixi workspace. TODO: make a repo with the Pixi workspace ready to use, with this repo as a git submodule. 90 | 91 | For now one can take this `pixi.toml`: 92 | ```toml 93 | [workspace] 94 | authors = ["Sam Pfeiffer "] 95 | channels = ["conda-forge", "robostack-jazzy"] 96 | name = "rclcppyy_ws" 97 | platforms = ["linux-64"] 98 | version = "0.1.0" 99 | 100 | [tasks] 101 | 102 | [dependencies] 103 | ros-jazzy-ros-base = ">=0.11.0,<0.12" 104 | opencv = ">=4.11.0,<5" 105 | 106 | [activation] 107 | # Comment out this line on first build 108 | scripts = ["fix_cppyy_api_path.sh"] 109 | # To silence warning: non-portable path to file '"cpycppyy/API.h"'; specified path differs in case from file name on disk [-Wnonportable-include-path] 110 | # From patching the wrong path due to capitalisation from the current cppyy packaged version 111 | env = { EXTRA_CLING_ARGS = "-Wno-nonportable-include-path" } 112 | 113 | [pypi-dependencies] 114 | cppyy = ">=3.5.0, <4" 115 | ``` 116 | **Workaround needed**: Comment out the `scripts = ["fix_cppyy_api_path.sh"]` line on the first build as it will fail otherwise. There's an issue with cppyy packaging and the capitalisation of the `cpycppyy` directory. 117 | 118 | And do: 119 | ```bash 120 | # If you haven't installed pixi: 121 | curl -fsSL https://pixi.sh/install.sh | sh 122 | source ~/.bashrc 123 | 124 | # Clone this repo 125 | mkdir -p rclcppyy_ws/src 126 | cd rclcppyy_ws/src 127 | git clone https://github.com/awesomebytes/rclcppyy 128 | # workaround... 129 | cp rclcppyy/fix_cppyy_api_path.sh .. 130 | cd .. 131 | # Copy the `pixi.toml` here and (apply the workaround mentioned above) 132 | pixi install 133 | # Undo the workaround done before, e.g. uncomment the line 134 | pixi shell 135 | # You'll be in a shell with the environment fully ready after a few seconds 136 | ``` 137 | 138 | 139 | ### Build 140 | 141 | Once in the pixi workspace or in a system with all dependencies... 142 | 143 | ```bash 144 | cd /path/to/workspace 145 | colcon build --packages-select rclcppyy 146 | source install/setup.bash 147 | ``` 148 | 149 | ### Run 150 | 151 | ```bash 152 | # Default is fastrtps, which has random latency issues, and big messages don't pass through 153 | export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp 154 | export ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST 155 | 156 | # One on each shell probably 157 | ros2 run rclcppyy bench_pub_rclpy.py 10000 158 | ros2 run rclcppyy bench_sub_rclpy.py 159 | 160 | ros2 run rclcppyy bench_pub_rclcppyy_monkeypatched.py 10000 161 | ros2 run rclcppyy bench_sub_rclcppyy_monkeypatched.py 162 | 163 | # Monitor with 164 | # top -c -p $(pgrep -d, -f bench_) 165 | 166 | # Or the tutorial example 167 | ros2 run rclcppyy publisher_member_function.py 168 | ``` 169 | 170 | ## Roadmap 171 | 172 | [x] Benchmark pub/sub 173 | 174 | [x] Get rclpy tutorials code to run with rclcppyy backend. 175 | 176 | [x] Monkeypatch/substitute rclpy with rclcppyy and make your Python nodes use less CPU! 177 | 178 | [x] Monkeypatch/substitute rclpy messages for rclcpp messages (so to avoid conversions). 179 | 180 | [ ] (WIP) Generate stubs to get IDE autocompletion. 181 | 182 | [ ] (WIP) Demo images (these big images ones should be done with loaned or zero-cost copy messages). 183 | 184 | [x] Demo use python testing packages with C++. 185 | 186 | [x] Demo use python CLI generator with C++. 187 | 188 | [ ] Demo use C++ Markers classes from Python. 189 | 190 | [ ] Demo use zero-copy torch. 191 | 192 | [x] (WIP) Demo use C++ rosbag reader (to C++ messages). 193 | 194 | [x] Demo pointclouds. 195 | 196 | [ ] Demo Nav2. 197 | 198 | [ ] Demo Moveit2. 199 | 200 | [ ] Demo ROS control. 201 | 202 | [ ] Separate into different packages the base `rclcppyy` and other demos/reusable pieces. 203 | 204 | 205 | ## TODO 206 | 207 | * Bring down the bringup of rclcppyy time (currently 2.5s~) by figuring out how to build a `.pcm` + `.so` dictionary that is pre-compiled (or at least compiled just once per machine) -------------------------------------------------------------------------------- /scripts/create_stubs.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | """ 3 | Dynamic exploration of all rclcpp symbols using cppyy. 4 | This script forces discovery of all classes, functions, and other elements. 5 | """ 6 | import cppyy 7 | import inspect 8 | import pydoc 9 | import re 10 | from pathlib import Path 11 | from typing import Set, Dict, List, Any, Optional, Tuple 12 | from rclcppyy.bringup_rclcpp import bringup_rclcpp 13 | 14 | def force_symbol_discovery(namespace: Any, known_symbols: Set[str] = None) -> Dict[str, Any]: 15 | """ 16 | Force discovery of all symbols in a cppyy namespace by accessing them. 17 | Returns a dictionary of discovered symbols and their types. 18 | """ 19 | if known_symbols is None: 20 | known_symbols = set() 21 | 22 | discovered = {} 23 | current_symbols = set(dir(namespace)) 24 | new_symbols = current_symbols - known_symbols 25 | 26 | for symbol_name in new_symbols: 27 | if symbol_name.startswith('__'): 28 | continue 29 | 30 | try: 31 | symbol = getattr(namespace, symbol_name) 32 | symbol_type = type(symbol).__name__ 33 | 34 | # Try to determine what kind of symbol this is 35 | if hasattr(symbol, '__call__'): 36 | if 'function' in symbol_type.lower(): 37 | discovered[symbol_name] = {'type': 'function', 'object': symbol} 38 | else: 39 | discovered[symbol_name] = {'type': 'callable', 'object': symbol} 40 | elif 'type' in symbol_type.lower() or 'class' in symbol_type.lower(): 41 | discovered[symbol_name] = {'type': 'class', 'object': symbol} 42 | 43 | # For classes, also explore their methods 44 | try: 45 | class_methods = dir(symbol) 46 | discovered[symbol_name]['methods'] = [m for m in class_methods if not m.startswith('__')] 47 | except: 48 | pass 49 | elif symbol_type == 'CPPScope': 50 | discovered[symbol_name] = {'type': 'namespace', 'object': symbol} 51 | else: 52 | discovered[symbol_name] = {'type': 'other', 'object': symbol, 'python_type': symbol_type} 53 | 54 | except Exception as e: 55 | discovered[symbol_name] = {'type': 'error', 'error': str(e)} 56 | 57 | return discovered 58 | 59 | 60 | def recursive_symbol_discovery(namespace: Any, namespace_name: str = "", max_depth: int = 3, current_depth: int = 0) -> Dict[str, Dict]: 61 | """ 62 | Recursively discover symbols in nested namespaces. 63 | """ 64 | if current_depth >= max_depth: 65 | return {} 66 | 67 | print(f"{' ' * current_depth}Exploring namespace: {namespace_name}") 68 | 69 | # Get initial symbols 70 | initial_symbols = set(dir(namespace)) 71 | discovered = force_symbol_discovery(namespace, set()) 72 | 73 | # Check if accessing symbols revealed new ones 74 | iterations = 0 75 | while iterations < 5: # Limit iterations to prevent infinite loops 76 | current_symbols = set(dir(namespace)) 77 | if current_symbols == initial_symbols: 78 | break 79 | 80 | new_discovered = force_symbol_discovery(namespace, initial_symbols) 81 | discovered.update(new_discovered) 82 | initial_symbols = current_symbols 83 | iterations += 1 84 | 85 | # Recursively explore namespaces 86 | for symbol_name, symbol_info in discovered.items(): 87 | if symbol_info.get('type') == 'namespace': 88 | sub_namespace_name = f"{namespace_name}.{symbol_name}" if namespace_name else symbol_name 89 | try: 90 | sub_discovered = recursive_symbol_discovery( 91 | symbol_info['object'], 92 | sub_namespace_name, 93 | max_depth, 94 | current_depth + 1 95 | ) 96 | discovered[symbol_name]['sub_symbols'] = sub_discovered 97 | except Exception as e: 98 | discovered[symbol_name]['sub_error'] = str(e) 99 | 100 | return discovered 101 | 102 | 103 | def explore_known_rclcpp_classes(): 104 | """ 105 | Force discovery of commonly known rclcpp classes to trigger their loading. 106 | """ 107 | known_classes = [ 108 | 'Node', 'NodeOptions', 'Publisher', 'Subscription', 'Timer', 'Client', 'Service', 109 | 'QoS', 'KeepAll', 'KeepLast', 'Clock', 'Time', 'Duration', 'Rate', 110 | 'Logger', 'Parameter', 'ParameterValue', 'CallbackGroup', 'Executor', 111 | 'SingleThreadedExecutor', 'MultiThreadedExecutor', 'Context' 112 | ] 113 | 114 | print("Attempting to access known rclcpp classes...") 115 | discovered_classes = [] 116 | 117 | for class_name in known_classes: 118 | try: 119 | cls = getattr(cppyy.gbl.rclcpp, class_name) 120 | discovered_classes.append(class_name) 121 | print(f" ✓ Found: {class_name}") 122 | except AttributeError: 123 | print(f" ✗ Not found: {class_name}") 124 | except Exception as e: 125 | print(f" ⚠ Error accessing {class_name}: {e}") 126 | 127 | return discovered_classes 128 | 129 | 130 | def print_symbol_summary(discovered: Dict[str, Dict], indent: int = 0): 131 | """ 132 | Print a summary of discovered symbols organized by type. 133 | """ 134 | indent_str = " " * indent 135 | 136 | by_type = {} 137 | for name, info in discovered.items(): 138 | symbol_type = info.get('type', 'unknown') 139 | if symbol_type not in by_type: 140 | by_type[symbol_type] = [] 141 | by_type[symbol_type].append(name) 142 | 143 | for symbol_type, names in by_type.items(): 144 | print(f"{indent_str}{symbol_type.upper()}S ({len(names)}):") 145 | for name in sorted(names): 146 | print(f"{indent_str} - {name}") 147 | 148 | # Show methods for classes 149 | if symbol_type == 'class' and 'methods' in discovered[name]: 150 | methods = discovered[name]['methods'] 151 | if methods: 152 | print(f"{indent_str} Methods: {', '.join(methods[:5])}") 153 | if len(methods) > 5: 154 | print(f"{indent_str} ... and {len(methods) - 5} more") 155 | 156 | # Show sub-symbols for namespaces 157 | if symbol_type == 'namespace' and 'sub_symbols' in discovered[name]: 158 | sub_symbols = discovered[name]['sub_symbols'] 159 | if sub_symbols: 160 | print(f"{indent_str} Sub-namespace content:") 161 | print_symbol_summary(sub_symbols, indent + 2) 162 | print() 163 | 164 | 165 | def warmup_rclcpp_symbols(verbose: bool = False) -> None: 166 | """ 167 | Warm up all rclcpp symbols for better autocomplete experience in IPython. 168 | This function will access all available symbols to ensure they are loaded into cppyy. 169 | 170 | Args: 171 | verbose: If True, print progress information 172 | """ 173 | # First ensure rclcpp is loaded 174 | bringup_rclcpp() 175 | 176 | if verbose: 177 | print("Warming up rclcpp symbols...") 178 | 179 | # Common rclcpp classes to ensure are loaded 180 | known_classes = [ 181 | 'Node', 'NodeOptions', 'Publisher', 'Subscription', 'Timer', 'Client', 'Service', 182 | 'QoS', 'KeepAll', 'KeepLast', 'Clock', 'Time', 'Duration', 'Rate', 183 | 'Logger', 'Parameter', 'ParameterValue', 'CallbackGroup', 'Executor', 184 | 'Context' 185 | ] 186 | # 'SingleThreadedExecutor', 'MultiThreadedExecutor' not found 187 | 188 | # Force access of known classes 189 | for class_name in known_classes: 190 | try: 191 | _ = getattr(cppyy.gbl.rclcpp, class_name) 192 | if verbose: 193 | print(f" ✓ Loaded {class_name}") 194 | except: 195 | if verbose: 196 | print(f" ✗ Could not load {class_name}") 197 | 198 | # Perform deep symbol discovery 199 | _ = recursive_symbol_discovery(cppyy.gbl.rclcpp, "rclcpp", max_depth=3) 200 | 201 | if verbose: 202 | print(f"Finished warming up {len(dir(cppyy.gbl.rclcpp))} rclcpp symbols") 203 | 204 | 205 | # Note: if we do: 206 | # import pydoc 207 | # string_with_help_text = pydoc.render_doc(cppyy.gbl.rclcpp.Node, "rclcpp") 208 | # We can get all the info about any element in rclcpp 209 | 210 | if __name__ == "__main__": 211 | warmup_rclcpp_symbols(verbose=True) 212 | 213 | -------------------------------------------------------------------------------- /rclcppyy/rosbag2_py_compat.py: -------------------------------------------------------------------------------- 1 | """ 2 | Drop-in replacement for rosbag2_py backed by rosbag2_cpp via cppyy. 3 | 4 | Goals: 5 | - Mirror the common rosbag2_py classes and methods (SequentialReader/Writer, 6 | StorageOptions, ConverterOptions, TopicMetadata) so existing user code works. 7 | - Keep per-message overhead low by avoiding extra Python wrappers, binding 8 | C++ methods to locals in hot paths. 9 | """ 10 | 11 | from __future__ import annotations 12 | 13 | import ctypes 14 | from dataclasses import dataclass 15 | from typing import Any, Iterable, List, Optional, Tuple 16 | 17 | import cppyy 18 | 19 | from .rosbag2_cpp import bringup_rosbag2_cpp 20 | 21 | 22 | _HELPERS_DEFINED = False 23 | 24 | 25 | def _ensure_helpers() -> None: 26 | global _HELPERS_DEFINED 27 | if _HELPERS_DEFINED: 28 | return 29 | 30 | bringup_rosbag2_cpp() 31 | 32 | # Include helpers for constructing SerializedBagMessage from bytes and 33 | # writing via Writer without Python assembling shared_ptr. 34 | cppyy.include('rosbag2_storage/ros_helper.hpp') 35 | cppyy.cppdef(r""" 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | extern "C" void rclcppyy_writer_write_bytes( 46 | rosbag2_cpp::Writer* writer, 47 | const std::string& topic_name, 48 | const char* data, 49 | size_t len, 50 | long long recv_ts, 51 | long long send_ts) { 52 | auto bag_message = std::make_shared(); 53 | bag_message->topic_name = topic_name; 54 | bag_message->serialized_data = rosbag2_storage::make_serialized_message(data, len); 55 | bag_message->recv_timestamp = (int64_t)recv_ts; 56 | bag_message->send_timestamp = (int64_t)send_ts; 57 | writer->write(bag_message); 58 | } 59 | 60 | extern "C" void rclcppyy_writer_write_serialized_message( 61 | rosbag2_cpp::Writer* writer, 62 | const std::string& topic_name, 63 | const rclcpp::SerializedMessage& sm, 64 | long long recv_ts, 65 | long long send_ts) { 66 | auto bag_message = std::make_shared(); 67 | const auto& r = sm.get_rcl_serialized_message(); 68 | bag_message->topic_name = topic_name; 69 | bag_message->serialized_data = rosbag2_storage::make_serialized_message((const char*)r.buffer, r.buffer_length); 70 | bag_message->recv_timestamp = (int64_t)recv_ts; 71 | bag_message->send_timestamp = (int64_t)send_ts; 72 | writer->write(bag_message); 73 | } 74 | 75 | extern "C" PyObject* rclcppyy_reader_read_next_tuple(rosbag2_cpp::Reader* reader) { 76 | auto sbm = reader->read_next(); 77 | rcutils_uint8_array_t& arr = *sbm->serialized_data.get(); 78 | PyObject* pybytes = PyBytes_FromStringAndSize((const char*)arr.buffer, (Py_ssize_t)arr.buffer_length); 79 | if (!pybytes) return nullptr; 80 | PyObject* pytopic = PyUnicode_FromString(sbm->topic_name.c_str()); 81 | if (!pytopic) { Py_DECREF(pybytes); return nullptr; } 82 | PyObject* pytimestamp = PyLong_FromLongLong((long long)sbm->recv_timestamp); 83 | if (!pytimestamp) { Py_DECREF(pybytes); Py_DECREF(pytopic); return nullptr; } 84 | PyObject* tup = PyTuple_New(3); 85 | if (!tup) { Py_DECREF(pybytes); Py_DECREF(pytopic); Py_DECREF(pytimestamp); return nullptr; } 86 | PyTuple_SET_ITEM(tup, 0, pytopic); 87 | PyTuple_SET_ITEM(tup, 1, pybytes); 88 | PyTuple_SET_ITEM(tup, 2, pytimestamp); 89 | return tup; 90 | } 91 | 92 | // Return (topic, rclcpp::SerializedMessage, timestamp) by value 93 | static inline rclcpp::SerializedMessage rclcppyy_make_sm_from_rcutils(const rcutils_uint8_array_t& arr) { 94 | rclcpp::SerializedMessage out; 95 | out.reserve(arr.buffer_length); 96 | auto& r = out.get_rcl_serialized_message(); 97 | std::memcpy(r.buffer, arr.buffer, arr.buffer_length); 98 | r.buffer_length = arr.buffer_length; 99 | return out; 100 | } 101 | 102 | std::tuple 103 | rclcppyy_reader_read_next_sm_tuple(rosbag2_cpp::Reader* reader) { 104 | auto sbm = reader->read_next(); 105 | rcutils_uint8_array_t& arr = *sbm->serialized_data.get(); 106 | auto sm = rclcppyy_make_sm_from_rcutils(arr); 107 | return std::make_tuple(sbm->topic_name, sm, (long long)sbm->recv_timestamp); 108 | } 109 | """) 110 | 111 | _HELPERS_DEFINED = True 112 | 113 | 114 | # Thin Python mirrors of rosbag2_py option/metadata classes 115 | @dataclass 116 | class StorageOptions: 117 | uri: str 118 | storage_id: str = 'mcap' 119 | max_bagfile_size: int = 0 120 | max_bagfile_duration: int = 0 121 | max_cache_size: int = 0 122 | storage_preset_profile: str = '' 123 | storage_config_uri: str = '' 124 | snapshot_mode: bool = False 125 | start_time_ns: int = 0 126 | end_time_ns: int = 0 127 | custom_data: Optional[dict] = None 128 | 129 | 130 | @dataclass 131 | class ConverterOptions: 132 | input_serialization_format: str = 'cdr' 133 | output_serialization_format: str = 'cdr' 134 | 135 | 136 | @dataclass 137 | class TopicMetadata: 138 | id: int 139 | name: str 140 | type: str 141 | serialization_format: str 142 | offered_qos_profiles: Optional[List[Any]] = None 143 | type_description_hash: str = '' 144 | 145 | 146 | def _to_cxx_storage_options(py_opt: StorageOptions) -> Any: 147 | cxx = cppyy.gbl.rosbag2_storage.StorageOptions() 148 | cxx.uri = py_opt.uri 149 | cxx.storage_id = py_opt.storage_id 150 | cxx.max_bagfile_size = py_opt.max_bagfile_size 151 | cxx.max_bagfile_duration = py_opt.max_bagfile_duration 152 | cxx.max_cache_size = py_opt.max_cache_size 153 | cxx.storage_preset_profile = py_opt.storage_preset_profile 154 | cxx.storage_config_uri = py_opt.storage_config_uri 155 | cxx.snapshot_mode = py_opt.snapshot_mode 156 | cxx.start_time_ns = py_opt.start_time_ns 157 | cxx.end_time_ns = py_opt.end_time_ns 158 | if py_opt.custom_data: 159 | for k, v in py_opt.custom_data.items(): 160 | cxx.custom_data[k] = v 161 | return cxx 162 | 163 | 164 | def _to_cxx_converter_options(py_opt: ConverterOptions) -> Any: 165 | cxx = cppyy.gbl.rosbag2_cpp.ConverterOptions() 166 | cxx.input_serialization_format = py_opt.input_serialization_format 167 | cxx.output_serialization_format = py_opt.output_serialization_format 168 | return cxx 169 | 170 | 171 | def _to_cxx_topic_metadata(py_md: TopicMetadata) -> Any: 172 | md = cppyy.gbl.rosbag2_storage.TopicMetadata() 173 | md.name = py_md.name 174 | md.type = py_md.type 175 | md.serialization_format = py_md.serialization_format 176 | try: 177 | if py_md.offered_qos_profiles is not None: 178 | md.offered_qos_profiles = py_md.offered_qos_profiles 179 | except Exception: 180 | pass 181 | try: 182 | if py_md.type_description_hash: 183 | md.type_description_hash = py_md.type_description_hash 184 | except Exception: 185 | pass 186 | return md 187 | 188 | 189 | class SequentialReader: 190 | def __init__(self) -> None: 191 | bringup_rosbag2_cpp() 192 | self._cxx = cppyy.gbl.rclcppyy_make_reader_default() 193 | 194 | # Compatibility API 195 | def open(self, storage_options: StorageOptions, converter_options: ConverterOptions) -> None: 196 | cxo = _to_cxx_storage_options(storage_options) 197 | cco = _to_cxx_converter_options(converter_options) 198 | self._cxx.open(cxo, cco) 199 | 200 | def open_uri(self, uri: str) -> None: 201 | self._cxx.open(uri) 202 | 203 | def close(self) -> None: 204 | self._cxx.close() 205 | 206 | def has_next(self) -> bool: 207 | return bool(self._cxx.has_next()) 208 | 209 | # def read_next(self) -> Tuple[str, bytes, int]: 210 | # # Fast path: build the Python tuple in C++ to avoid pointer fiddling 211 | # return cppyy.gbl.rclcppyy_reader_read_next_tuple(self._cxx) 212 | 213 | # Fast-path access to SerializedBagMessage, useful for calling deserialize immediately from Python 214 | def read_next_serialized(self) -> Any: 215 | return self._cxx.read_next() 216 | 217 | # Zero-copy view variant: returns (topic, memoryview, ts) 218 | def read_next_view(self) -> Tuple[str, memoryview, int]: 219 | return cppyy.gbl.rclcppyy_reader_read_next_tuple_view(self._cxx) 220 | 221 | # SerializedMessage variant: returns (topic, rclcpp::SerializedMessage, ts) 222 | def read_next(self) -> Tuple[str, Any, int]: 223 | return cppyy.gbl.rclcppyy_reader_read_next_sm_tuple(self._cxx) 224 | 225 | 226 | def get_all_topics_and_types(self) -> Iterable[Any]: 227 | try: 228 | return self._cxx.get_all_topics_and_types() 229 | except Exception: 230 | return self._cxx.get_topics_and_types() 231 | 232 | # Optional pass-throughs for completeness 233 | def seek(self, timestamp: int) -> None: 234 | self._cxx.seek(int(timestamp)) 235 | 236 | def set_filter(self, storage_filter: Any) -> None: 237 | self._cxx.set_filter(storage_filter) 238 | 239 | def reset_filter(self) -> None: 240 | self._cxx.reset_filter() 241 | 242 | def set_read_order(self, read_order: Any) -> bool: 243 | return bool(self._cxx.set_read_order(read_order)) 244 | 245 | 246 | class SequentialWriter: 247 | def __init__(self) -> None: 248 | bringup_rosbag2_cpp() 249 | _ensure_helpers() 250 | self._cxx = cppyy.gbl.rclcppyy_make_writer_default() 251 | 252 | def open(self, storage_options: StorageOptions, converter_options: ConverterOptions) -> None: 253 | cxo = _to_cxx_storage_options(storage_options) 254 | cco = _to_cxx_converter_options(converter_options) 255 | self._cxx.open(cxo, cco) 256 | 257 | def close(self) -> None: 258 | self._cxx.close() 259 | 260 | def create_topic(self, topic_metadata: Any) -> None: 261 | if hasattr(topic_metadata, 'name') and not hasattr(topic_metadata, 'serialization_format'): 262 | # Not expected, but try to be forgiving 263 | md = _to_cxx_topic_metadata(TopicMetadata( 264 | id=0, 265 | name=topic_metadata.name, 266 | type=getattr(topic_metadata, 'type', ''), 267 | serialization_format=getattr(topic_metadata, 'serialization_format', 'cdr'), 268 | )) 269 | elif isinstance(topic_metadata, TopicMetadata): 270 | md = _to_cxx_topic_metadata(topic_metadata) 271 | else: 272 | md = topic_metadata 273 | self._cxx.create_topic(md) 274 | 275 | # Match rosbag2_py signature(s) 276 | def write(self, topic_name: str, message: Any, recv_timestamp: int, send_timestamp: Optional[int] = None) -> None: 277 | if send_timestamp is None: 278 | send_timestamp = recv_timestamp 279 | # Accept either bytes or rclcpp::SerializedMessage 280 | # Try C++ SerializedMessage first (duck-typing by attribute) 281 | try: 282 | # cppyy proxies expose get_rcl_serialized_message 283 | _ = message.get_rcl_serialized_message # attribute exists 284 | cppyy.gbl.rclcppyy_writer_write_serialized_message(self._cxx, topic_name, message, int(recv_timestamp), int(send_timestamp)) 285 | return 286 | except Exception: 287 | pass 288 | # Fallback: assume bytes-like 289 | cppyy.gbl.rclcppyy_writer_write_bytes(self._cxx, topic_name, message, len(message), int(recv_timestamp), int(send_timestamp)) 290 | 291 | # Convenience fast-path for serialized bag messages 292 | def write_serialized(self, sbm: Any) -> None: 293 | self._cxx.write(sbm) 294 | 295 | 296 | -------------------------------------------------------------------------------- /rclcppyy/rosbag2_cpp.py: -------------------------------------------------------------------------------- 1 | """ 2 | Thin wrapper exposing C++ rosbag2 APIs through cppyy for Python use. 3 | 4 | This module provides access to the common C++ rosbag2 classes and 5 | simple helpers to open readers/writers and iterate messages. 6 | 7 | Usage: 8 | from rclcppyy import rosbag2_cpp as rosbag2_py 9 | reader = rosbag2_py.open_reader("/path/to/input", storage_id="mcap") 10 | writer = rosbag2_py.open_writer("/path/to/output", storage_id="mcap") 11 | """ 12 | 13 | import os 14 | import glob 15 | import sys 16 | import ctypes 17 | from typing import Iterable, Tuple, Dict, Any, Optional, List, Set 18 | 19 | import cppyy 20 | from ament_index_python.packages import get_packages_with_prefixes, get_package_prefix 21 | 22 | from rclcppyy.bringup_rclcpp import bringup_rclcpp 23 | 24 | 25 | _BROUGHT_UP: bool = False 26 | _LOADED_LIBS: Set[str] = set() 27 | 28 | 29 | def _load_library_once(so_path: str) -> None: 30 | if not so_path or not os.path.exists(so_path): 31 | return 32 | if so_path in _LOADED_LIBS: 33 | return 34 | try: 35 | # Ensure global symbol visibility for plugin resolution 36 | ctypes.CDLL(so_path, mode=ctypes.RTLD_GLOBAL) 37 | except Exception: 38 | pass 39 | try: 40 | cppyy.load_library(so_path) 41 | except Exception: 42 | # cppyy may fail to load some libs that are not strictly needed; ignore 43 | pass 44 | _LOADED_LIBS.add(so_path) 45 | 46 | 47 | def _glob_and_load(lib_dir: str, patterns: List[str]) -> None: 48 | if not os.path.isdir(lib_dir): 49 | return 50 | for pat in patterns: 51 | # Try naked, .so and versioned .so.* 52 | for glob_pat in [pat, pat + '.so', pat + '.so.*']: 53 | for so_path in sorted(set(glob.glob(os.path.join(lib_dir, glob_pat)))): 54 | _load_library_once(so_path) 55 | 56 | 57 | def _preload_rosbag2_libraries() -> None: 58 | """ 59 | Load core rosbag2 and dependency libraries from their package prefixes, 60 | in a stable order, avoiding wide scans across all prefixes. 61 | """ 62 | pkg_load_order: List[List[str]] = [ 63 | ['rcutils'], 64 | ['rcpputils'], 65 | ['console_bridge'], 66 | ['tinyxml2', 'tinyxml2_vendor'], 67 | ['yaml-cpp', 'yaml_cpp_vendor'], 68 | ['class_loader'], 69 | ['pluginlib'], 70 | ['rosbag2_storage'], 71 | ['rosbag2_cpp'], 72 | ['rosbag2_cpp_serialization'], # may not exist on all distros 73 | ] 74 | 75 | pkg_to_patterns: Dict[str, List[str]] = { 76 | 'rcutils': ['librcutils*'], 77 | 'rcpputils': ['librcpputils*'], 78 | 'console_bridge': ['libconsole_bridge*'], 79 | 'tinyxml2': ['libtinyxml2*'], 80 | 'tinyxml2_vendor': ['libtinyxml2*'], 81 | 'yaml-cpp': ['libyaml-cpp*'], 82 | 'yaml_cpp_vendor': ['libyaml-cpp*'], 83 | 'class_loader': ['libclass_loader*'], 84 | 'pluginlib': ['libpluginlib*'], 85 | 'rosbag2_storage': ['librosbag2_storage*'], 86 | 'rosbag2_cpp': ['librosbag2_cpp*'], 87 | 'rosbag2_cpp_serialization': ['librosbag2_cpp_serialization*'], 88 | } 89 | 90 | for pkg_group in pkg_load_order: 91 | loaded_any = False 92 | for pkg in pkg_group: 93 | try: 94 | prefix = get_package_prefix(pkg) 95 | except Exception: 96 | continue 97 | lib_dir = os.path.join(prefix, 'lib') 98 | patterns = pkg_to_patterns.get(pkg, []) 99 | if patterns: 100 | _glob_and_load(lib_dir, patterns) 101 | loaded_any = True 102 | # Continue regardless of whether group was found; some groups are optional 103 | 104 | # Optionally, load rosbag2_py extensions (can carry symbols) from all known prefixes 105 | try: 106 | for _, prefix in get_packages_with_prefixes().items(): 107 | site_pkgs = os.path.join(prefix, 'lib', f'python{sys.version_info.major}.{sys.version_info.minor}', 'site-packages') 108 | rosbag2_py_dir = os.path.join(site_pkgs, 'rosbag2_py') 109 | if os.path.isdir(rosbag2_py_dir): 110 | for so_path in sorted(glob.glob(os.path.join(rosbag2_py_dir, '*.so'))): 111 | _load_library_once(so_path) 112 | except Exception: 113 | pass 114 | 115 | 116 | def _ensure_storage_plugin_loaded(storage_id: str) -> None: 117 | """ 118 | Best-effort load of the storage plugin matching the given storage_id (e.g., 'mcap', 'sqlite3'). 119 | Scans package lib directories for likely plugin names and loads them. 120 | """ 121 | if not storage_id: 122 | return 123 | try: 124 | prefixes = list(set(get_packages_with_prefixes().values())) 125 | except Exception: 126 | prefixes = [] 127 | # Also include commonly relevant prefixes directly if resolvable 128 | for pkg in ['rosbag2_storage', f'rosbag2_storage_{storage_id}', f'rosbag2_{storage_id}']: 129 | try: 130 | prefixes.append(get_package_prefix(pkg)) 131 | except Exception: 132 | pass 133 | 134 | patterns = [ 135 | f'librosbag2_storage*{storage_id}*', 136 | f'lib*{storage_id}*plugin*', 137 | f'lib*{storage_id}*', 138 | ] 139 | for prefix in sorted(set(prefixes)): 140 | lib_dir = os.path.join(prefix, 'lib') 141 | _glob_and_load(lib_dir, patterns) 142 | 143 | 144 | def _add_minimal_include_paths() -> None: 145 | for pkg in ['rosbag2_cpp', 'rosbag2_storage', 'rclcpp']: 146 | try: 147 | cppyy.add_include_path(os.path.join(get_package_prefix(pkg), 'include')) 148 | except Exception as e: 149 | print(f"Error adding include path for {pkg}: {e}") 150 | 151 | 152 | def bringup_rosbag2_cpp() -> Any: 153 | """ 154 | Ensure rosbag2_cpp C++ symbols are available through cppyy. 155 | Returns the cppyy.gbl.rosbag2_cpp namespace. 156 | """ 157 | global _BROUGHT_UP 158 | if _BROUGHT_UP: 159 | return cppyy.gbl.rosbag2_cpp 160 | 161 | bringup_rclcpp() 162 | 163 | # Preload libraries and add minimal include paths 164 | _preload_rosbag2_libraries() 165 | _add_minimal_include_paths() 166 | 167 | # Headers (rosbag2 + concrete sequential impls for factories) 168 | cppyy.include('rosbag2_cpp/reader.hpp') 169 | cppyy.include('rosbag2_cpp/writer.hpp') 170 | cppyy.include('rosbag2_cpp/converter_options.hpp') 171 | cppyy.include('rosbag2_cpp/serialization_format_converter_factory.hpp') 172 | cppyy.include('rosbag2_cpp/readers/sequential_reader.hpp') 173 | cppyy.include('rosbag2_cpp/writers/sequential_writer.hpp') 174 | cppyy.include('rosbag2_storage/storage_options.hpp') 175 | cppyy.include('rosbag2_storage/serialized_bag_message.hpp') 176 | cppyy.include('rosbag2_storage/topic_metadata.hpp') 177 | cppyy.include('rosbag2_storage/storage_factory.hpp') 178 | cppyy.include('rosbag2_storage/metadata_io.hpp') 179 | # Optional convenience type 180 | try: 181 | cppyy.include('rclcpp/serialized_message.hpp') 182 | except Exception: 183 | pass 184 | 185 | # Factory helpers to avoid unique_ptr copy semantics in constructor binding 186 | cppyy.cppdef(r""" 187 | #include 188 | #include 189 | #include 190 | #include 191 | #include 192 | #include 193 | #include 194 | #include 195 | extern "C" rosbag2_cpp::Reader* rclcppyy_make_reader_default() { 196 | auto storage_factory = std::make_unique(); 197 | auto converter_factory = std::make_shared(); 198 | auto metadata_io = std::make_unique(); 199 | auto seq_reader = std::make_unique( 200 | std::move(storage_factory), converter_factory, std::move(metadata_io)); 201 | return new rosbag2_cpp::Reader(std::move(seq_reader)); 202 | } 203 | extern "C" rosbag2_cpp::Writer* rclcppyy_make_writer_default() { 204 | auto storage_factory = std::make_unique(); 205 | auto converter_factory = std::make_shared(); 206 | auto metadata_io = std::make_unique(); 207 | auto seq_writer = std::make_unique( 208 | std::move(storage_factory), converter_factory, std::move(metadata_io)); 209 | return new rosbag2_cpp::Writer(std::move(seq_writer)); 210 | } 211 | """) 212 | 213 | _BROUGHT_UP = True 214 | return cppyy.gbl.rosbag2_cpp 215 | 216 | 217 | def open_reader(uri: str, *, storage_id: str = 'mcap', serialization: str = 'cdr') -> Any: 218 | """ 219 | Create and open a C++ rosbag2_cpp::Reader. 220 | """ 221 | bringup_rosbag2_cpp() 222 | _ensure_storage_plugin_loaded(storage_id) 223 | ReaderPtrFactory = cppyy.gbl.rclcppyy_make_reader_default 224 | StorageOptions = cppyy.gbl.rosbag2_storage.StorageOptions 225 | ConverterOptions = cppyy.gbl.rosbag2_cpp.ConverterOptions 226 | 227 | reader = ReaderPtrFactory() 228 | inopt = StorageOptions() 229 | inopt.uri = uri 230 | inopt.storage_id = storage_id 231 | 232 | conv = ConverterOptions() 233 | conv.input_serialization_format = serialization 234 | conv.output_serialization_format = serialization 235 | 236 | # Open with full options 237 | reader.open(inopt, conv) 238 | return reader 239 | 240 | 241 | def open_writer(uri: str, *, storage_id: str = 'mcap', serialization: str = 'cdr') -> Any: 242 | """ 243 | Create and open a C++ rosbag2_cpp::Writer. 244 | """ 245 | bringup_rosbag2_cpp() 246 | _ensure_storage_plugin_loaded(storage_id) 247 | WriterPtrFactory = cppyy.gbl.rclcppyy_make_writer_default 248 | StorageOptions = cppyy.gbl.rosbag2_storage.StorageOptions 249 | ConverterOptions = cppyy.gbl.rosbag2_cpp.ConverterOptions 250 | 251 | writer = WriterPtrFactory() 252 | outopt = StorageOptions() 253 | outopt.uri = uri 254 | outopt.storage_id = storage_id 255 | 256 | conv = ConverterOptions() 257 | conv.input_serialization_format = serialization 258 | conv.output_serialization_format = serialization 259 | 260 | writer.open(outopt, conv) 261 | return writer 262 | 263 | 264 | def create_topic(writer: Any, name: str, type_: str, *, serialization_format: str = 'cdr', offered_qos_profiles: str = '') -> None: 265 | """ 266 | Create a topic on a C++ rosbag2_cpp::Writer. 267 | """ 268 | bringup_rosbag2_cpp() 269 | TopicMetadata = cppyy.gbl.rosbag2_storage.TopicMetadata 270 | md = TopicMetadata() 271 | md.name = name 272 | md.type = type_ 273 | md.serialization_format = serialization_format 274 | # offered_qos_profiles may not exist on all distros; wrap defensively 275 | try: 276 | md.offered_qos_profiles = offered_qos_profiles 277 | except Exception: 278 | pass 279 | writer.create_topic(md) 280 | 281 | 282 | def iter_topics(reader: Any) -> Iterable[Any]: 283 | """ 284 | Yield topic metadata objects from a C++ reader. 285 | """ 286 | bringup_rosbag2_cpp() 287 | try: 288 | topics = reader.get_all_topics_and_types() 289 | except Exception: 290 | topics = reader.get_topics_and_types() 291 | # Return as-is (vector of TopicMetadata) 292 | for md in topics: 293 | yield md 294 | 295 | 296 | def iter_messages(reader: Any) -> Iterable[Any]: 297 | """ 298 | Yield C++ rosbag2_storage::SerializedBagMessage shared_ptrs from a reader. 299 | """ 300 | while reader.has_next(): 301 | yield reader.read_next() 302 | 303 | 304 | def write_serialized_message(writer: Any, sbm_shared_ptr: Any) -> None: 305 | """ 306 | Write a C++ rosbag2_storage::SerializedBagMessage shared_ptr to the writer. 307 | """ 308 | writer.write(sbm_shared_ptr) 309 | 310 | 311 | # Convenience shortcuts to C++ types for advanced users 312 | def types() -> Dict[str, Any]: 313 | bringup_rosbag2_cpp() 314 | t: Dict[str, Any] = { 315 | 'Reader': cppyy.gbl.rosbag2_cpp.Reader, 316 | 'Writer': cppyy.gbl.rosbag2_cpp.Writer, 317 | 'StorageOptions': cppyy.gbl.rosbag2_storage.StorageOptions, 318 | 'ConverterOptions': cppyy.gbl.rosbag2_cpp.ConverterOptions, 319 | 'TopicMetadata': cppyy.gbl.rosbag2_storage.TopicMetadata, 320 | 'SerializedBagMessage': cppyy.gbl.rosbag2_storage.SerializedBagMessage, 321 | } 322 | try: 323 | t['SerializedMessage'] = cppyy.gbl.rclcpp.SerializedMessage 324 | except Exception: 325 | pass 326 | return t 327 | 328 | 329 | -------------------------------------------------------------------------------- /roscon_uk_2025/pointcloud.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /center1 10 | - /left1 11 | - /right1 12 | - /passthrough1 13 | - /voxelgrid1 14 | Splitter Ratio: 0.5 15 | Tree Height: 1138 16 | - Class: rviz_common/Selection 17 | Name: Selection 18 | - Class: rviz_common/Tool Properties 19 | Expanded: 20 | - /2D Goal Pose1 21 | - /Publish Point1 22 | Name: Tool Properties 23 | Splitter Ratio: 0.5886790156364441 24 | - Class: rviz_common/Views 25 | Expanded: 26 | - /Current View1 27 | Name: Views 28 | Splitter Ratio: 0.5 29 | - Class: rviz_common/Time 30 | Experimental: false 31 | Name: Time 32 | SyncMode: 0 33 | SyncSource: center 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 1 39 | Class: rviz_default_plugins/Grid 40 | Color: 160; 160; 164 41 | Enabled: true 42 | Line Style: 43 | Line Width: 0.029999999329447746 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 10 53 | Reference Frame: 54 | Value: true 55 | - Class: rviz_default_plugins/TF 56 | Enabled: true 57 | Filter (blacklist): "" 58 | Filter (whitelist): "" 59 | Frame Timeout: 15 60 | Frames: 61 | All Enabled: true 62 | base_link: 63 | Value: true 64 | lexus3/_left_camera_frame: 65 | Value: true 66 | lexus3/base_link: 67 | Value: true 68 | lexus3/duro_gps: 69 | Value: true 70 | lexus3/duro_gps_imu: 71 | Value: true 72 | lexus3/gps: 73 | Value: true 74 | lexus3/ground_link: 75 | Value: true 76 | lexus3/os_center_a: 77 | Value: true 78 | lexus3/os_center_a_imu_data_frame: 79 | Value: true 80 | lexus3/os_center_a_laser_data_frame: 81 | Value: true 82 | lexus3/os_left_a: 83 | Value: true 84 | lexus3/os_left_a_imu_data_frame: 85 | Value: true 86 | lexus3/os_left_a_laser_data_frame: 87 | Value: true 88 | lexus3/os_right_a: 89 | Value: true 90 | lexus3/os_right_a_imu_data_frame: 91 | Value: true 92 | lexus3/os_right_a_laser_data_frame: 93 | Value: true 94 | map: 95 | Value: true 96 | map_gyor_0: 97 | Value: true 98 | map_zala_0: 99 | Value: true 100 | zed2i_base_link: 101 | Value: true 102 | zed2i_camera_center: 103 | Value: true 104 | zed2i_imu_link: 105 | Value: true 106 | zed2i_left_camera_frame: 107 | Value: true 108 | zed2i_left_camera_optical_frame: 109 | Value: true 110 | zed2i_right_camera_frame: 111 | Value: true 112 | zed2i_right_camera_optical_frame: 113 | Value: true 114 | Marker Scale: 1 115 | Name: TF 116 | Show Arrows: true 117 | Show Axes: true 118 | Show Names: false 119 | Tree: 120 | lexus3/base_link: 121 | lexus3/_left_camera_frame: 122 | {} 123 | lexus3/duro_gps: 124 | {} 125 | lexus3/duro_gps_imu: 126 | {} 127 | lexus3/ground_link: 128 | {} 129 | lexus3/os_center_a: 130 | lexus3/os_center_a_imu_data_frame: 131 | {} 132 | lexus3/os_center_a_laser_data_frame: 133 | {} 134 | lexus3/os_left_a: 135 | lexus3/os_left_a_imu_data_frame: 136 | {} 137 | lexus3/os_left_a_laser_data_frame: 138 | {} 139 | lexus3/os_right_a: 140 | lexus3/os_right_a_imu_data_frame: 141 | {} 142 | lexus3/os_right_a_laser_data_frame: 143 | {} 144 | lexus3/gps: 145 | {} 146 | map: 147 | map_gyor_0: 148 | {} 149 | map_zala_0: 150 | {} 151 | Update Interval: 0 152 | Value: true 153 | - Alpha: 1 154 | Autocompute Intensity Bounds: true 155 | Autocompute Value Bounds: 156 | Max Value: 10 157 | Min Value: -10 158 | Value: true 159 | Axis: Z 160 | Channel Name: intensity 161 | Class: rviz_default_plugins/PointCloud2 162 | Color: 255; 255; 255 163 | Color Transformer: Intensity 164 | Decay Time: 0 165 | Enabled: true 166 | Invert Rainbow: false 167 | Max Color: 255; 255; 255 168 | Max Intensity: 1680 169 | Min Color: 0; 0; 0 170 | Min Intensity: 4 171 | Name: center 172 | Position Transformer: XYZ 173 | Selectable: true 174 | Size (Pixels): 3 175 | Size (m): 0.03999999910593033 176 | Style: Flat Squares 177 | Topic: 178 | Depth: 5 179 | Durability Policy: Volatile 180 | History Policy: Keep Last 181 | Reliability Policy: Reliable 182 | Value: /lexus3/os_center/points 183 | Use Fixed Frame: true 184 | Use rainbow: true 185 | Value: true 186 | - Alpha: 1 187 | Autocompute Intensity Bounds: true 188 | Autocompute Value Bounds: 189 | Max Value: 10 190 | Min Value: -10 191 | Value: true 192 | Axis: Z 193 | Channel Name: intensity 194 | Class: rviz_default_plugins/PointCloud2 195 | Color: 255; 255; 255 196 | Color Transformer: Intensity 197 | Decay Time: 0 198 | Enabled: true 199 | Invert Rainbow: false 200 | Max Color: 255; 255; 255 201 | Max Intensity: 778 202 | Min Color: 0; 0; 0 203 | Min Intensity: 2 204 | Name: left 205 | Position Transformer: XYZ 206 | Selectable: true 207 | Size (Pixels): 3 208 | Size (m): 0.03999999910593033 209 | Style: Flat Squares 210 | Topic: 211 | Depth: 5 212 | Durability Policy: Volatile 213 | History Policy: Keep Last 214 | Reliability Policy: Reliable 215 | Value: /lexus3/os_left/points 216 | Use Fixed Frame: true 217 | Use rainbow: true 218 | Value: true 219 | - Alpha: 1 220 | Autocompute Intensity Bounds: true 221 | Autocompute Value Bounds: 222 | Max Value: 10 223 | Min Value: -10 224 | Value: true 225 | Axis: Z 226 | Channel Name: intensity 227 | Class: rviz_default_plugins/PointCloud2 228 | Color: 255; 255; 255 229 | Color Transformer: Intensity 230 | Decay Time: 0 231 | Enabled: true 232 | Invert Rainbow: false 233 | Max Color: 255; 255; 255 234 | Max Intensity: 1154 235 | Min Color: 0; 0; 0 236 | Min Intensity: 1 237 | Name: right 238 | Position Transformer: XYZ 239 | Selectable: true 240 | Size (Pixels): 3 241 | Size (m): 0.03999999910593033 242 | Style: Flat Squares 243 | Topic: 244 | Depth: 5 245 | Durability Policy: Volatile 246 | History Policy: Keep Last 247 | Reliability Policy: Reliable 248 | Value: /lexus3/os_right/points 249 | Use Fixed Frame: true 250 | Use rainbow: true 251 | Value: true 252 | - Alpha: 1 253 | Autocompute Intensity Bounds: true 254 | Autocompute Value Bounds: 255 | Max Value: 10 256 | Min Value: -10 257 | Value: true 258 | Axis: Z 259 | Channel Name: intensity 260 | Class: rviz_default_plugins/PointCloud2 261 | Color: 0; 0; 0 262 | Color Transformer: FlatColor 263 | Decay Time: 0 264 | Enabled: true 265 | Invert Rainbow: false 266 | Max Color: 255; 255; 255 267 | Max Intensity: 1683 268 | Min Color: 0; 0; 0 269 | Min Intensity: 4 270 | Name: passthrough 271 | Position Transformer: XYZ 272 | Selectable: true 273 | Size (Pixels): 3 274 | Size (m): 0.05000000074505806 275 | Style: Spheres 276 | Topic: 277 | Depth: 5 278 | Durability Policy: Volatile 279 | History Policy: Keep Last 280 | Reliability Policy: Best Effort 281 | Value: /pointcloud_passthrough 282 | Use Fixed Frame: true 283 | Use rainbow: true 284 | Value: true 285 | - Alpha: 1 286 | Autocompute Intensity Bounds: true 287 | Autocompute Value Bounds: 288 | Max Value: 10 289 | Min Value: -10 290 | Value: true 291 | Axis: Z 292 | Channel Name: intensity 293 | Class: rviz_default_plugins/PointCloud2 294 | Color: 255; 0; 255 295 | Color Transformer: FlatColor 296 | Decay Time: 0 297 | Enabled: true 298 | Invert Rainbow: false 299 | Max Color: 255; 255; 255 300 | Max Intensity: 4096 301 | Min Color: 0; 0; 0 302 | Min Intensity: 0 303 | Name: voxelgrid 304 | Position Transformer: XYZ 305 | Selectable: true 306 | Size (Pixels): 3 307 | Size (m): 0.25 308 | Style: Spheres 309 | Topic: 310 | Depth: 5 311 | Durability Policy: Volatile 312 | History Policy: Keep Last 313 | Reliability Policy: Best Effort 314 | Value: /voxelgrid 315 | Use Fixed Frame: true 316 | Use rainbow: true 317 | Value: true 318 | Enabled: true 319 | Global Options: 320 | Background Color: 255; 255; 255 321 | Fixed Frame: lexus3/base_link 322 | Frame Rate: 30 323 | Name: root 324 | Tools: 325 | - Class: rviz_default_plugins/Interact 326 | Hide Inactive Objects: true 327 | - Class: rviz_default_plugins/MoveCamera 328 | - Class: rviz_default_plugins/Select 329 | - Class: rviz_default_plugins/FocusCamera 330 | - Class: rviz_default_plugins/Measure 331 | Line color: 128; 128; 0 332 | - Class: rviz_default_plugins/SetInitialPose 333 | Covariance x: 0.25 334 | Covariance y: 0.25 335 | Covariance yaw: 0.06853891909122467 336 | Topic: 337 | Depth: 5 338 | Durability Policy: Volatile 339 | History Policy: Keep Last 340 | Reliability Policy: Reliable 341 | Value: /initialpose 342 | - Class: rviz_default_plugins/SetGoal 343 | Topic: 344 | Depth: 5 345 | Durability Policy: Volatile 346 | History Policy: Keep Last 347 | Reliability Policy: Reliable 348 | Value: /goal_pose 349 | - Class: rviz_default_plugins/PublishPoint 350 | Single click: true 351 | Topic: 352 | Depth: 5 353 | Durability Policy: Volatile 354 | History Policy: Keep Last 355 | Reliability Policy: Reliable 356 | Value: /clicked_point 357 | Transformation: 358 | Current: 359 | Class: rviz_default_plugins/TF 360 | Value: true 361 | Views: 362 | Current: 363 | Class: rviz_default_plugins/Orbit 364 | Distance: 22.505891799926758 365 | Enable Stereo Rendering: 366 | Stereo Eye Separation: 0.05999999865889549 367 | Stereo Focal Distance: 1 368 | Swap Stereo Eyes: false 369 | Value: false 370 | Focal Point: 371 | X: 0 372 | Y: 0 373 | Z: 0 374 | Focal Shape Fixed Size: true 375 | Focal Shape Size: 0.05000000074505806 376 | Invert Z Axis: false 377 | Name: Current View 378 | Near Clip Distance: 0.009999999776482582 379 | Pitch: 0.45539796352386475 380 | Target Frame: 381 | Value: Orbit (rviz) 382 | Yaw: 3.005396842956543 383 | Saved: ~ 384 | Window Geometry: 385 | Displays: 386 | collapsed: false 387 | Height: 1435 388 | Hide Left Dock: false 389 | Hide Right Dock: false 390 | QMainWindow State: 000000ff00000000fd000000040000000000000156000004fdfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004fd000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004fdfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000004fd000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000008d00000003efc0100000002fb0000000800540069006d00650100000000000008d00000026600fffffffb0000000800540069006d006501000000000000045000000000000000000000065f000004fd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 391 | Selection: 392 | collapsed: false 393 | Time: 394 | collapsed: false 395 | Tool Properties: 396 | collapsed: false 397 | Views: 398 | collapsed: false 399 | Width: 2256 400 | X: 0 401 | Y: 32 402 | -------------------------------------------------------------------------------- /rclcppyy/bringup_rclcpp.py: -------------------------------------------------------------------------------- 1 | import cppyy 2 | import os 3 | import re 4 | from functools import lru_cache 5 | from typing import Any, List, Optional, Set, Dict 6 | from ament_index_python.packages import get_package_prefix, get_packages_with_prefixes 7 | 8 | RCLCPP_BRINGUP_DONE = False 9 | 10 | def get_ros2_include_path() -> str: 11 | ROS2_INCLUDE_PATH = get_package_prefix("rclcpp") + "/include" 12 | return ROS2_INCLUDE_PATH 13 | 14 | def add_ros2_include_path() -> bool: 15 | ROS2_INCLUDE_PATH = get_ros2_include_path() 16 | return cppyy.add_include_path(ROS2_INCLUDE_PATH) 17 | 18 | def add_ros2_include_paths() -> bool: 19 | ROS2_INCLUDE_PATH = get_ros2_include_path() 20 | ROS2_LIB_PATH = get_package_prefix("rclcpp") + "/lib" 21 | # All of these are needed for rclcpp to work 22 | 23 | # Instead of hardcoding the list, just get all the available packages 24 | pkgs_with_prefixes = get_packages_with_prefixes() 25 | for pkg, prefix in pkgs_with_prefixes.items(): 26 | include_path = os.path.join(prefix, "include", pkg) 27 | # The path may not exist if its a Python package (or simply does not have headers) 28 | if os.path.exists(include_path): 29 | cppyy.add_include_path(include_path) 30 | 31 | return True 32 | 33 | def force_symbol_discovery(namespace: Any, known_symbols: Set[str] = None) -> Dict[str, Any]: 34 | """ 35 | Force discovery of all symbols in a cppyy namespace by accessing them. 36 | Returns a dictionary of discovered symbols and their types. 37 | """ 38 | if known_symbols is None: 39 | known_symbols = set() 40 | 41 | discovered = {} 42 | current_symbols = set(dir(namespace)) 43 | new_symbols = current_symbols - known_symbols 44 | 45 | for symbol_name in new_symbols: 46 | if symbol_name.startswith('__'): 47 | continue 48 | 49 | try: 50 | symbol = getattr(namespace, symbol_name) 51 | symbol_type = type(symbol).__name__ 52 | 53 | # Try to determine what kind of symbol this is 54 | if hasattr(symbol, '__call__'): 55 | if 'function' in symbol_type.lower(): 56 | discovered[symbol_name] = {'type': 'function', 'object': symbol} 57 | else: 58 | discovered[symbol_name] = {'type': 'callable', 'object': symbol} 59 | elif 'type' in symbol_type.lower() or 'class' in symbol_type.lower(): 60 | discovered[symbol_name] = {'type': 'class', 'object': symbol} 61 | 62 | # For classes, also explore their methods 63 | try: 64 | class_methods = dir(symbol) 65 | discovered[symbol_name]['methods'] = [m for m in class_methods if not m.startswith('__')] 66 | except: 67 | pass 68 | elif symbol_type == 'CPPScope': 69 | discovered[symbol_name] = {'type': 'namespace', 'object': symbol} 70 | else: 71 | discovered[symbol_name] = {'type': 'other', 'object': symbol, 'python_type': symbol_type} 72 | 73 | except Exception as e: 74 | discovered[symbol_name] = {'type': 'error', 'error': str(e)} 75 | 76 | return discovered 77 | 78 | 79 | def recursive_symbol_discovery(namespace: Any, namespace_name: str = "", max_depth: int = 3, current_depth: int = 0) -> Dict[str, Dict]: 80 | """ 81 | Recursively discover symbols in nested namespaces. 82 | """ 83 | if current_depth >= max_depth: 84 | return {} 85 | 86 | # print(f"{' ' * current_depth}Exploring namespace: {namespace_name}") 87 | 88 | # Get initial symbols 89 | initial_symbols = set(dir(namespace)) 90 | discovered = force_symbol_discovery(namespace, set()) 91 | 92 | # Check if accessing symbols revealed new ones 93 | iterations = 0 94 | while iterations < 5: # Limit iterations to prevent infinite loops 95 | current_symbols = set(dir(namespace)) 96 | if current_symbols == initial_symbols: 97 | break 98 | 99 | new_discovered = force_symbol_discovery(namespace, initial_symbols) 100 | discovered.update(new_discovered) 101 | initial_symbols = current_symbols 102 | iterations += 1 103 | 104 | # Recursively explore namespaces 105 | for symbol_name, symbol_info in discovered.items(): 106 | if symbol_info.get('type') == 'namespace': 107 | sub_namespace_name = f"{namespace_name}.{symbol_name}" if namespace_name else symbol_name 108 | try: 109 | sub_discovered = recursive_symbol_discovery( 110 | symbol_info['object'], 111 | sub_namespace_name, 112 | max_depth, 113 | current_depth + 1 114 | ) 115 | discovered[symbol_name]['sub_symbols'] = sub_discovered 116 | except Exception as e: 117 | discovered[symbol_name]['sub_error'] = str(e) 118 | 119 | return discovered 120 | 121 | 122 | def explore_known_rclcpp_classes(verbose: bool = False): 123 | """ 124 | Force discovery of commonly known rclcpp classes to trigger their loading. 125 | """ 126 | known_classes = [ 127 | 'Node', 'NodeOptions', 'Publisher', 'Subscription', 'Client', 'Service', 128 | 'QoS', 'KeepAll', 'KeepLast', 'Clock', 'Time', 'Duration', 'Rate', 129 | 'Logger', 'Parameter', 'ParameterValue', 'CallbackGroup', 'Executor', 130 | 'Context' 131 | ] 132 | # Not found: 133 | # 'SingleThreadedExecutor', 'MultiThreadedExecutor', 'Timer' 134 | 135 | if verbose: 136 | print("Attempting to access known rclcpp classes...") 137 | discovered_classes = [] 138 | 139 | for class_name in known_classes: 140 | try: 141 | cls = getattr(cppyy.gbl.rclcpp, class_name) 142 | discovered_classes.append(class_name) 143 | if verbose: 144 | print(f" ✓ Found: {class_name}") 145 | except AttributeError: 146 | if verbose: 147 | print(f" ✗ Not found: {class_name}") 148 | except Exception as e: 149 | print(f" ⚠ Error accessing {class_name}: {e}") 150 | 151 | return discovered_classes 152 | 153 | @staticmethod 154 | def _is_msg_cpp(message_type): 155 | """ 156 | Check if the message type is a C++ cppyy message class. 157 | Fast check using unique cppyy attributes. 158 | """ 159 | # Fast check: cppyy objects have __smartptr__ attribute that Python messages don't 160 | return hasattr(message_type, '__smartptr__') 161 | 162 | @staticmethod 163 | def _is_msg_python(message_type): 164 | """ 165 | Check if the message type is a Python ROS message class. 166 | Fast check using __slots__ attribute that cppyy messages don't have. 167 | """ 168 | # Fast check: Python ROS messages have __slots__ attribute that cppyy messages don't 169 | return hasattr(message_type, '__slots__') 170 | 171 | # @lru_cache(maxsize=1000) 172 | @staticmethod 173 | def _resolve_message_type(message_type): 174 | """ 175 | Resolve message type to C++ type string and cppyy type object. 176 | 177 | Args: 178 | message_type: Can be: 179 | - Python ROS message class (e.g., std_msgs.msg.String) 180 | - C++ cppyy message class (e.g., cppyy.gbl.std_msgs.msg.String) 181 | 182 | Returns: 183 | tuple: (cpp_type_string, cppyy_type_object) 184 | """ 185 | # Case 1: C++ cppyy message class (check this BEFORE Python messages) 186 | if _is_msg_cpp(message_type): 187 | # Extract type information from cppyy type 188 | module_parts = message_type.__module__.split('.') 189 | 190 | # Handle cppyy.gbl.package_msgs.msg structure (4 parts) 191 | if len(module_parts) >= 4: # ['cppyy', 'gbl', 'package_msgs', 'msg'] 192 | package = module_parts[2] # e.g., 'std_msgs' 193 | # Get message name from class name, removing template suffixes 194 | class_name = message_type.__name__ 195 | # Remove template parameters like '_>' 196 | msg_name = re.sub(r'_<.*?>$', '', class_name) 197 | cpp_type_str = f"{package}::msg::{msg_name}" 198 | 199 | # Since the cppyy type is already available and the header is included, just return it 200 | return cpp_type_str, message_type 201 | else: 202 | raise ValueError(f"Cannot parse C++ cppyy message type: {message_type}") 203 | 204 | # Case 2: Python ROS message class (e.g., std_msgs.msg.String) 205 | elif _is_msg_python(message_type): 206 | # Extract package and message name from Python ROS message type 207 | module_parts = message_type.__module__.split('.') 208 | if len(module_parts) >= 2: 209 | package = module_parts[0] # e.g., 'std_msgs' 210 | 211 | # Get message name from class name, removing 'Metaclass_' prefix if present 212 | class_name = message_type.__class__.__name__ 213 | if class_name.startswith('Metaclass_'): 214 | msg_name = class_name[10:] # Remove 'Metaclass_' prefix 215 | else: 216 | msg_name = class_name 217 | 218 | # Convert to lowercase for header file 219 | msg_name_lower = msg_name.lower() 220 | 221 | # Build C++ type string and include header 222 | cpp_type_str = f"{package}::msg::{msg_name}" 223 | # module_parts looks like: 224 | # std_msgs.msg._multi_array_layout or rcl_interfaces.msg._parameter_event 225 | hpp_file_name = f"{module_parts[2][1:]}.hpp" 226 | header_path = f"{package}/msg/{hpp_file_name}" 227 | # print(f"header_path: {header_path}, package: {package}, msg_name: {msg_name}, msg_name_lower: {msg_name_lower}, cpp_type_str: {cpp_type_str}, module_parts: {module_parts}, hpp_file_name: {hpp_file_name}") 228 | cppyy.add_include_path(os.path.join(get_package_prefix(package), "include", package)) 229 | cppyy.include(header_path) 230 | # print(f"included header: {header_path}") 231 | 232 | # Get the C++ type 233 | cppyy_type = getattr(getattr(getattr(cppyy.gbl, package), 'msg'), msg_name) 234 | return cpp_type_str, cppyy_type 235 | else: 236 | raise ValueError(f"Cannot parse Python ROS message type: {message_type}") 237 | 238 | else: 239 | raise ValueError(f"Unsupported message type: {message_type} (type: {type(message_type)})") 240 | 241 | 242 | def adapt_rclcpp_to_python(rclcpp: Any): 243 | """ 244 | Adapt the rclcpp namespace to be more Pythonic. 245 | """ 246 | rclcpp_init = rclcpp.init 247 | def rclpy_init(args: Optional[List[str]] = None, *, context=None) -> None: 248 | if args is None: 249 | args = [] 250 | rclcpp_init(len(args), args) 251 | 252 | def init_wrapper(*args, **kwargs): 253 | # Handle both rclpy and rclcpp style calls 254 | if len(args) == 2 and isinstance(args[0], int): 255 | # rclcpp style: init(argc, argv) 256 | return rclcpp_init(*args) 257 | else: 258 | # rclpy style: init(args=None, *, context=None) 259 | return rclpy_init(*args, **kwargs) 260 | 261 | rclcpp.init = init_wrapper 262 | 263 | def adapt_node_pub_sub_to_python(rclcpp: Any): 264 | """ 265 | Adapt the rclcpp publisher and subscriber classes so they automatically 266 | adapt to the message types and when someone calls either the rclpy Node api 267 | self.publisher_ = self.create_publisher(String, 'topic', 10) 268 | or the rclcpp Node api 269 | self.publisher = self.node.create_publisher[cppyy.gbl.std_msgs.msg.String]( 270 | "perf_topic_pythonic", 10) 271 | It just works. 272 | """ 273 | # Store original methods 274 | original_create_publisher = rclcpp.Node.create_publisher 275 | original_create_subscription = rclcpp.Node.create_subscription 276 | 277 | def create_publisher_wrapper(self, *args, **kwargs): 278 | # Handle rclpy style: create_publisher(msg_type, topic, qos) 279 | if len(args) == 3: 280 | msg_type, topic, qos = args 281 | cpp_type_str, cppyy_type = _resolve_message_type(msg_type) 282 | ret_publisher = original_create_publisher[cppyy_type](self, topic, qos) 283 | # Handle rclcpp style: create_publisher[msg_type](topic, qos) 284 | else: 285 | ret_publisher = original_create_publisher(*args, **kwargs) 286 | 287 | # We need to make sure that the Publisher.publish, if its given a Python message, we convert it to a C++ message 288 | # and then call the original publish method. 289 | original_publish = ret_publisher.publish 290 | def publish_wrapper(msg): 291 | if _is_msg_cpp(msg): 292 | return original_publish(msg) 293 | else: 294 | cpp_type_str, cppyy_type = _resolve_message_type(msg) 295 | cpp_msg = cppyy_type() 296 | 297 | # Note: this is not very efficient, but it works. 298 | # We should instead monkeypatch the imported messages to convert them to cpp from the start 299 | # That way we will work directly with cpp objects and we avoid this overhead 300 | for dict_key, dict_val in msg.get_fields_and_field_types().items(): 301 | setattr(cpp_msg, dict_key, getattr(msg, dict_key)) 302 | 303 | return original_publish(cpp_msg) 304 | ret_publisher.publish = publish_wrapper 305 | return ret_publisher 306 | 307 | def create_subscription_wrapper(self, *args, **kwargs): 308 | # Handle rclpy style: create_subscription(msg_type, topic, callback, qos) 309 | if len(args) == 4: 310 | msg_type, topic, callback, qos = args 311 | cpp_type_str, cppyy_type = _resolve_message_type(msg_type) 312 | 313 | # Use the generic callback template 314 | # Need to use the template with bracket syntax for cppyy 315 | make_callback = cppyy.gbl.make_py_sub_callback[cpp_type_str] 316 | cpp_callback = make_callback(callback) 317 | return original_create_subscription[cppyy_type](self, topic, qos, cpp_callback) 318 | # Handle rclcpp style: create_subscription[msg_type](topic, qos, callback) 319 | else: 320 | return original_create_subscription(*args, **kwargs) 321 | 322 | # Replace the original methods with our wrappers 323 | rclcpp.Node.create_publisher = create_publisher_wrapper 324 | rclcpp.Node.create_subscription = create_subscription_wrapper 325 | 326 | def adapt_get_logger_to_python(rclcpp: Any): 327 | """ 328 | Adapt the rclcpp get_logger method so it can be called the python way. 329 | """ 330 | original_get_logger = rclcpp.Node.get_logger 331 | def get_logger_wrapper(this_self, *args, **kwargs): 332 | # we need to return a logger that can do all the levels 333 | # debug, info, warn, error, fatal 334 | # Given the logger and logger levels in c++ are macros, here, we will just go ahead and use the 335 | # rclpy logger api to get the logger and then use the logger in the python way 336 | from rclpy.node import get_logger 337 | return get_logger(this_self.get_name()) 338 | 339 | rclcpp.Node.get_logger = get_logger_wrapper 340 | 341 | 342 | def bringup_rclcpp(): 343 | """ 344 | Bring up the rclcpp library. 345 | This function should be called before using any rclcpp classes. 346 | It adds the necessary include paths and loads the necessary libraries. 347 | Returns the rclcpp namespace. 348 | """ 349 | global RCLCPP_BRINGUP_DONE 350 | if RCLCPP_BRINGUP_DONE: 351 | return cppyy.gbl.rclcpp 352 | 353 | add_ros2_include_paths() 354 | print("Including rclcpp.hpp with cppyy, this may take a few seconds... WIP to remove this slowdown") 355 | cppyy.include("rclcpp/rclcpp.hpp") 356 | cppyy.include("rcl_interfaces/msg/parameter_event.hpp") 357 | cppyy.include("chrono") 358 | cppyy.include("functional") 359 | print("Done bringing up rclcpp.") 360 | 361 | explore_known_rclcpp_classes() 362 | recursive_symbol_discovery(cppyy.gbl.rclcpp, "rclcpp", max_depth=5) 363 | adapt_rclcpp_to_python(cppyy.gbl.rclcpp) 364 | # adapt_node_pub_sub_to_python(cppyy.gbl.rclcpp) 365 | adapt_get_logger_to_python(cppyy.gbl.rclcpp) 366 | 367 | RCLCPP_BRINGUP_DONE = True 368 | # TODO: Add rclcpp to the global namespace or another user-friendly way of givin access 369 | return cppyy.gbl.rclcpp 370 | -------------------------------------------------------------------------------- /rclcppyy/node.py: -------------------------------------------------------------------------------- 1 | import inspect 2 | import uuid 3 | from typing import Type, Union, Optional, Callable, Any, List 4 | import rclpy 5 | from rclpy import init, shutdown, spin, spin_once, create_node 6 | from rclpy.callback_groups import CallbackGroup 7 | from rclpy.event_handler import PublisherEventCallbacks, SubscriptionEventCallbacks 8 | from rclpy.node import Node 9 | from rclpy.publisher import Publisher 10 | from rclpy.subscription import Subscription 11 | from rclpy.qos import QoSProfile, HistoryPolicy, ReliabilityPolicy, DurabilityPolicy, LivelinessPolicy 12 | from rclpy.context import Context 13 | from rclpy.qos_overriding_options import QoSOverridingOptions 14 | from rclpy.exceptions import InvalidTopicNameException 15 | from rclpy.validate_topic_name import validate_topic_name 16 | from rclpy.type_support import check_is_valid_msg_type 17 | from rclpy.qos_overriding_options import _declare_qos_parameters 18 | from rclpy.parameter import Parameter 19 | import cppyy 20 | from rclcppyy import bringup_rclcpp 21 | from rclcppyy.bringup_rclcpp import _resolve_message_type, _is_msg_python 22 | from rclcppyy.monkeypatch_messages import add_python_compatibility 23 | 24 | class RclcppyyNode(Node): 25 | """ 26 | This node inherits from rclpy Node but it uses rclcpp for publishers/subscribers. 27 | """ 28 | def __init__(self, 29 | node_name: str, 30 | *, 31 | context: Optional[Context] = None, 32 | cli_args: Optional[List[str]] = None, 33 | namespace: Optional[str] = None, 34 | use_global_arguments: bool = True, 35 | enable_rosout: bool = True, 36 | start_parameter_services: bool = True, 37 | parameter_overrides: Optional[List[Parameter]] = None, 38 | allow_undeclared_parameters: bool = False, 39 | automatically_declare_parameters_from_overrides: bool = False, 40 | enable_logger_service: bool = False) -> None: 41 | # Before creating the rclpy node, we create the rclcpp node (as internally it calls create_publisher for the parameters) 42 | self._rclcpp = bringup_rclcpp() 43 | # We need to init rclcpp if we havent already 44 | if not self._rclcpp.ok(): 45 | self._rclcpp.init() 46 | 47 | # Just cause its annoying, initialise if we havent yet 48 | if not rclpy.ok(): 49 | rclpy.init() 50 | 51 | self._rclcpp_node = self._rclcpp.Node(node_name + "_rclcpp") 52 | self._cpp_publishers = {} 53 | 54 | # Define the C++ callback wrapper template for timers 55 | # cppyy.cppdef(""" 56 | # #include 57 | # #include 58 | 59 | # static std::function create_node_timer_callback_PLACEHOLDER_UUID(PyObject* self) { 60 | # return [self]() { 61 | # if (self && PyObject_HasAttrString(self, "_node_timer_callback_PLACEHOLDER_UUID")) { 62 | # PyObject_CallMethod(self, "_node_timer_callback_PLACEHOLDER_UUID", nullptr); 63 | # } 64 | # }; 65 | # } 66 | # """) 67 | 68 | self._timer_cpp_template = """ 69 | #include 70 | #include 71 | 72 | static std::function create_node_timer_callback_PLACEHOLDER_UUID(PyObject* self) { 73 | return [self]() { 74 | if (self && PyObject_HasAttrString(self, "_node_timer_callback_PLACEHOLDER_UUID")) { 75 | PyObject_CallMethod(self, "_node_timer_callback_PLACEHOLDER_UUID", nullptr); 76 | } 77 | }; 78 | } 79 | """ 80 | self._cpp_timers = {} 81 | 82 | # Subscription callback template for dynamic C++ callback generation 83 | self._subscription_cpp_template = """ 84 | #include 85 | #include 86 | 87 | static std::function 88 | create_subscription_callback_PLACEHOLDER_UUID(PyObject* self) { 89 | return [self](const PLACEHOLDER_MSG_TYPE::SharedPtr msg) { 90 | if (self && PyObject_HasAttrString(self, "_subscription_callback_PLACEHOLDER_UUID")) { 91 | // Acquire the GIL before calling into Python to avoid crashes 92 | PyGILState_STATE gstate = PyGILState_Ensure(); 93 | // Pass the raw pointer as a Python int; Python will bind it back to a proxy 94 | PyObject* py_ptr = PyLong_FromVoidPtr(reinterpret_cast(msg.get())); 95 | PyObject_CallMethod(self, "_subscription_callback_PLACEHOLDER_UUID", "O", py_ptr); 96 | Py_XDECREF(py_ptr); 97 | PyGILState_Release(gstate); 98 | } 99 | }; 100 | } 101 | """ 102 | self._cpp_subscriptions = {} 103 | self._subscription_callbacks = {} 104 | 105 | # Now we create the rclpy node 106 | super().__init__(node_name, 107 | context=context, 108 | cli_args=cli_args, 109 | namespace=namespace, 110 | use_global_arguments=use_global_arguments, 111 | enable_rosout=enable_rosout, 112 | start_parameter_services=start_parameter_services, 113 | parameter_overrides=parameter_overrides, 114 | allow_undeclared_parameters=allow_undeclared_parameters, 115 | automatically_declare_parameters_from_overrides=automatically_declare_parameters_from_overrides, 116 | enable_logger_service=enable_logger_service) 117 | 118 | # TODO: deal with spinners, if spinning on one, we should spin on both 119 | 120 | def _convert_qos_to_rclcpp(self, qos: QoSProfile) -> 'rclcpp.QoS': 121 | """ 122 | Convert a rclpy QoSProfile to a rclcpp QoS. 123 | """ 124 | rclcpp_qos = self._rclcpp.QoS(qos.depth) 125 | 126 | # History policy 127 | if qos.history == HistoryPolicy.KEEP_LAST: 128 | rclcpp_qos.keep_last(qos.depth) 129 | elif qos.history == HistoryPolicy.KEEP_ALL: 130 | rclcpp_qos.keep_all() 131 | 132 | # Reliability 133 | if qos.reliability == ReliabilityPolicy.RELIABLE: 134 | rclcpp_qos.reliable() 135 | elif qos.reliability == ReliabilityPolicy.BEST_EFFORT: 136 | rclcpp_qos.best_effort() 137 | 138 | # Durability 139 | if qos.durability == DurabilityPolicy.TRANSIENT_LOCAL: 140 | rclcpp_qos.transient_local() 141 | elif qos.durability == DurabilityPolicy.VOLATILE: 142 | rclcpp_qos.durability_volatile() 143 | 144 | # Liveliness 145 | if qos.liveliness == LivelinessPolicy.AUTOMATIC: 146 | rclcpp_qos.liveliness(self._rclcpp.LivelinessPolicy.Automatic) 147 | elif qos.liveliness == LivelinessPolicy.MANUAL_BY_TOPIC: 148 | rclcpp_qos.liveliness(self._rclcpp.LivelinessPolicy.ManualByTopic) 149 | 150 | # Deadline 151 | if qos.deadline is not None: 152 | # Translate rclpy deadline (Duration) to rclcpp::Duration 153 | rclcpp_qos.deadline(self._rclcpp.Duration(qos.deadline.nanoseconds)) 154 | 155 | # Lifespan 156 | if qos.lifespan is not None: 157 | # Translate rclpy lifespan (Duration) to rclcpp::Duration 158 | rclcpp_qos.lifespan(self._rclcpp.Duration(qos.lifespan.nanoseconds)) 159 | 160 | # Lease Duration 161 | if qos.liveliness_lease_duration is not None: 162 | # Translate rclpy liveliness lease duration (Duration) to rclcpp::Duration 163 | rclcpp_qos.liveliness_lease_duration(self._rclcpp.Duration(qos.liveliness_lease_duration.nanoseconds)) 164 | 165 | return rclcpp_qos 166 | 167 | def create_publisher(self, 168 | msg_type: Any, 169 | topic: str, 170 | qos_profile: QoSProfile | int, 171 | *, 172 | callback_group: CallbackGroup | None = None, 173 | event_callbacks: PublisherEventCallbacks | None = None, 174 | qos_overriding_options: QoSOverridingOptions | None = None, 175 | publisher_class: type[Publisher] = ...) -> Publisher: 176 | # Validate and process inputs like rclpy does 177 | qos_profile = self._validate_qos_or_depth_parameter(qos_profile) 178 | 179 | # Resolve the topic name 180 | try: 181 | final_topic = self.resolve_topic_name(topic) 182 | except RuntimeError: 183 | try: 184 | self._validate_topic_or_service_name(topic) 185 | except InvalidTopicNameException as ex: 186 | raise ex from None 187 | raise 188 | 189 | # Handle QoS parameter overrides if needed 190 | # if qos_overriding_options is None: 191 | # qos_overriding_options = QoSOverridingOptions([]) 192 | # _declare_qos_parameters( 193 | # Publisher, self, final_topic, qos_profile, qos_overriding_options) 194 | 195 | # Convert rclpy message to rclcpp message type 196 | rclcpp_msg_type, rclcpp_msg_class = _resolve_message_type(msg_type) 197 | 198 | # Create publisher options for rclcpp 199 | # Note: not fully sure if this is correct, but it works for now 200 | options = self._rclcpp.PublisherOptionsWithAllocator["std::allocator"]() 201 | if callback_group: 202 | # TODO: if we get a callback group from rclpy, we need to convert it to rclcpp 203 | # Here we do nothing, so the default callback group from rclcpp Node is used 204 | pass 205 | if qos_overriding_options: 206 | # options.qos_overriding_options = qos_overriding_options 207 | # TODO: implement this 208 | pass 209 | 210 | # Convert QoS profile to rclcpp QoS 211 | rclcpp_qos = self._convert_qos_to_rclcpp(qos_profile) 212 | 213 | # Create the actual rclcpp publisher 214 | publisher = self._rclcpp.create_publisher[rclcpp_msg_type]( 215 | self._rclcpp_node, 216 | final_topic, 217 | rclcpp_qos, 218 | options 219 | ) 220 | 221 | # Wrap in rclpy publisher for compatibility if needed 222 | if event_callbacks: 223 | # TODO: Handle event callbacks through rclpy wrapper 224 | pass 225 | 226 | self._publishers.append(publisher) 227 | self._cpp_publishers[publisher] = publisher 228 | # Wake executor like rclpy does 229 | self._wake_executor() 230 | 231 | # Only wrap publish method if the message type is Python (which shouldn't happen with proper import hooks) 232 | if _is_msg_python(msg_type): 233 | # Replace the publish method so we translate the message to rclcpp message if needed 234 | original_publish = publisher.publish 235 | def publish_wrapper(msg: Any) -> Any: 236 | """ 237 | Make sure that when calling publish, the message is converted to rclcpp message if needed 238 | """ 239 | if _is_msg_python(msg): 240 | print(f"Converting message to C++: {type(msg)}") 241 | _, rclcpp_msg_class = _resolve_message_type(msg) 242 | # Inefficient, but it works, ideally we should monkeypatch the imported messages to convert them to cpp from the start 243 | cpp_msg = rclcpp_msg_class() 244 | cpp_msg = self._convert_msg_to_cpp(msg, cpp_msg) 245 | else: 246 | cpp_msg = msg 247 | return original_publish(cpp_msg) 248 | 249 | publisher.publish = publish_wrapper 250 | # else: no wrapping needed, publish directly uses C++ messages 251 | 252 | return publisher 253 | 254 | @staticmethod 255 | def _convert_msg_to_cpp(msg_py: Any, msg_cpp: Any) -> Any: 256 | """ 257 | Convert a rclpy message to a rclcpp message, recursively iterating over the message fields 258 | """ 259 | for dict_key, _ in msg_py.get_fields_and_field_types().items(): 260 | field = getattr(msg_py, dict_key) 261 | # If the field is a message, it will have a get_fields_and_field_types method, so we recursively convert it 262 | if hasattr(field, "get_fields_and_field_types"): 263 | setattr(msg_cpp, dict_key, RclcppyyNode._convert_msg_to_cpp(field, getattr(msg_cpp, dict_key))) 264 | else: 265 | # Check if the field is a list of some python type of list, as if it is, we will need to convert each element (superslow) 266 | if isinstance(field, list) or isinstance(field, tuple): 267 | # Get the field type of this field, as its an array, the instance if its an empty list does not tell us the type 268 | field_type_py = msg_py.__class__.get_fields_and_field_types().get(dict_key) 269 | # Looks like: 'sequence' 270 | # Translate to the rclcpp type like rcl_interfaces::Parameter 271 | field_type_py_without_sequence = field_type_py.replace("sequence<", "").replace(">", "") 272 | # Map Python primitive types to C++ types 273 | primitive_type_map = { 274 | 'octet': 'unsigned char', 275 | 'boolean': 'bool', 276 | 'int64': 'int64_t', 277 | 'double': 'double', 278 | 'string': 'std::string' 279 | } 280 | 281 | rclcpp_field_type = primitive_type_map.get( 282 | field_type_py_without_sequence, 283 | field_type_py_without_sequence.replace("/", "::msg::") 284 | ) 285 | # We need the empty vector for C++ no matter what, so we create it here 286 | rclcpp_vector = cppyy.gbl.std.vector[rclcpp_field_type]() 287 | # If we have some element, we will need to convert them to rclcpp, we check once to avoid resolving for every element 288 | if len(field) > 0: 289 | rclcpp_field_type, rclcpp_field_class = _resolve_message_type(field[0]) 290 | for i, element in enumerate(field): 291 | rclcpp_vector.push_back(RclcppyyNode._convert_msg_to_cpp(element, rclcpp_field_class())) 292 | setattr(msg_cpp, dict_key, rclcpp_vector) 293 | # Yay, its just a normal member 294 | else: 295 | setattr(msg_cpp, dict_key, field) 296 | return msg_cpp 297 | 298 | def create_timer(self, period: float, callback: Callable[[], None], *, oneshot: bool = False, callback_group: CallbackGroup | None = None): 299 | """ 300 | Create a timer using the C++ rclcpp timer. 301 | """ 302 | # Convert period to nanoseconds for rclcpp 303 | period_ns = int(period * 1e9) 304 | # Generate a unique UUID for the timer 305 | uuid_str = str(uuid.uuid4()).replace("-", "_") 306 | # Replace the PLACEHOLDER_UUID in the C++ template with the actual UUID 307 | cpp_timer_template = self._timer_cpp_template.replace("PLACEHOLDER_UUID", uuid_str) 308 | # Compile the C++ callback wrapper 309 | cppyy.cppdef(cpp_timer_template) 310 | # Add dynamically a method to the node that will call the callback 311 | setattr(self, f"_node_timer_callback_{uuid_str}", callback) 312 | 313 | # Create the C++ callback wrapper 314 | cpp_callback = getattr(cppyy.gbl, f"create_node_timer_callback_{uuid_str}")(self) 315 | 316 | # Create the timer using the rclcpp node's create_wall_timer 317 | wall_timer = self._rclcpp_node.create_wall_timer( 318 | cppyy.gbl.std.chrono.nanoseconds(period_ns), 319 | cpp_callback 320 | ) 321 | 322 | # TODO: Handle oneshot and callback_group parameters if needed 323 | # For now, we ignore these parameters as the provided implementation doesn't handle them 324 | 325 | return wall_timer 326 | 327 | def create_subscription(self, 328 | msg_type: Any, 329 | topic: str, 330 | callback: Callable[[Any], None], 331 | qos_profile: QoSProfile | int, 332 | *, 333 | callback_group: CallbackGroup | None = None, 334 | event_callbacks: SubscriptionEventCallbacks | None = None, 335 | qos_overriding_options: QoSOverridingOptions | None = None, 336 | raw: bool = False) -> Any: 337 | """ 338 | Create a subscription using the C++ rclcpp subscription. 339 | """ 340 | # Validate and process inputs like rclpy does 341 | qos_profile = self._validate_qos_or_depth_parameter(qos_profile) 342 | 343 | # Resolve the topic name 344 | try: 345 | final_topic = self.resolve_topic_name(topic) 346 | except RuntimeError: 347 | try: 348 | self._validate_topic_or_service_name(topic) 349 | except InvalidTopicNameException as ex: 350 | raise ex from None 351 | raise 352 | 353 | # Convert rclpy message to rclcpp message type 354 | rclcpp_msg_type, rclcpp_msg_class = _resolve_message_type(msg_type) 355 | # We could make this optional for efficiency 356 | # add_python_compatibility(cpp_msg, msg_type) 357 | 358 | # Generate a unique UUID for the subscription 359 | uuid_str = str(uuid.uuid4()).replace("-", "_") 360 | 361 | # Replace placeholders in the C++ template 362 | cpp_subscription_template = self._subscription_cpp_template.replace( 363 | "PLACEHOLDER_MSG_TYPE", rclcpp_msg_type 364 | ).replace("PLACEHOLDER_UUID", uuid_str) 365 | 366 | # Compile the C++ callback wrapper 367 | cppyy.cppdef(cpp_subscription_template) 368 | 369 | # Store a Python wrapper that translates raw pointer (int) back to C++ proxy and calls user callback 370 | def _cpp_bound_subscription_callback(py_ptr): 371 | try: 372 | # Resolve cpp class for binding 373 | # _, cpp_msg_class = _resolve_message_type(msg_type) 374 | # Rebind raw pointer (Python int) to a cppyy proxy 375 | cpp_msg = cppyy.bind_object(py_ptr, rclcpp_msg_class) 376 | callback(cpp_msg) 377 | except Exception as e: 378 | print(f"Error in subscription callback: {e}") 379 | import traceback 380 | traceback.print_exc() 381 | 382 | # Store the wrapper on the node instance 383 | setattr(self, f"_subscription_callback_{uuid_str}", _cpp_bound_subscription_callback) 384 | 385 | # Create the C++ callback wrapper 386 | cpp_callback = getattr(cppyy.gbl, f"create_subscription_callback_{uuid_str}")(self) 387 | 388 | # Create subscription options for rclcpp 389 | options = self._rclcpp.SubscriptionOptionsWithAllocator["std::allocator"]() 390 | if callback_group: 391 | # TODO: Convert callback group from rclpy to rclcpp if needed 392 | pass 393 | if qos_overriding_options: 394 | # TODO: Handle QoS overriding options 395 | pass 396 | 397 | # Convert QoS profile to rclcpp QoS 398 | rclcpp_qos = self._convert_qos_to_rclcpp(qos_profile) 399 | 400 | # Create the actual rclcpp subscription 401 | subscription = self._rclcpp.create_subscription[rclcpp_msg_type]( 402 | self._rclcpp_node, 403 | final_topic, 404 | rclcpp_qos, 405 | cpp_callback, 406 | options 407 | ) 408 | 409 | # Store the subscription and callback reference 410 | self._cpp_subscriptions[subscription] = { 411 | 'callback': callback, 412 | 'uuid': uuid_str, 413 | 'msg_type': rclcpp_msg_type 414 | } 415 | self._subscription_callbacks[uuid_str] = callback 416 | 417 | # Add to subscriptions list for compatibility 418 | self._subscriptions.append(subscription) 419 | 420 | # Wake executor like rclpy does 421 | self._wake_executor() 422 | 423 | return subscription 424 | 425 | def _validate_topic_or_service_name(self, name: str) -> None: 426 | """ 427 | Validate a topic or service name. 428 | """ 429 | validate_topic_name(name) 430 | 431 | def _validate_qos_or_depth_parameter(self, qos_profile: Union[QoSProfile, int]) -> QoSProfile: 432 | """ 433 | Validate a QoS profile or convert a history depth to a QoSProfile. 434 | """ 435 | if isinstance(qos_profile, QoSProfile): 436 | return qos_profile 437 | if isinstance(qos_profile, int): 438 | return QoSProfile(depth=qos_profile) 439 | raise TypeError('qos_profile should be a QoSProfile or an integer') 440 | 441 | def _wake_executor(self) -> None: 442 | """ 443 | Wake the executor if one exists. 444 | """ 445 | # TODO: Implement waking the executor when we have one 446 | pass 447 | 448 | def enable_kwargs_for_cpp_msg(msg_class): 449 | """Enable keyword arguments for C++ message constructors""" 450 | original_init = msg_class.__init__ 451 | def new_init(self, *args, **kwargs): 452 | if args: 453 | original_init(self, *args) 454 | else: 455 | original_init(self) 456 | 457 | for key, value in kwargs.items(): 458 | if hasattr(self, key): 459 | setattr(self, key, value) 460 | else: 461 | raise AttributeError(f"'{self.__class__.__name__}' object has no attribute '{key}'") 462 | 463 | msg_class.__init__ = new_init 464 | 465 | def add_python_msg_sugar_to_cpp_msg(cpp_msg_class, py_msg_class): 466 | """ 467 | Python messages pretty print and also you can check the fields and field types, so lets forward that to the C++ messages 468 | """ 469 | cpp_msg_class.__repr__ = py_msg_class.__repr__ 470 | cpp_msg_class.SLOT_TYPES = py_msg_class.SLOT_TYPES 471 | cpp_msg_class.get_fields_and_field_types = py_msg_class.get_fields_and_field_types 472 | cpp_msg_class._TYPE_SUPPORT = py_msg_class._TYPE_SUPPORT 473 | 474 | # add a fallback to the Python message if an attribute is not found 475 | def fallback_getattr(self, name): 476 | print(f"Called fallback_getattr with name: {name}") 477 | if not hasattr(self, name): 478 | return getattr(py_msg_class, name) 479 | return super().__getattr__(name) 480 | cpp_msg_class.__getattr__ = fallback_getattr 481 | 482 | 483 | def convert_python_msgs_to_cpp(target_globals=None): 484 | """ 485 | Convert all Python ROS messages in globals() to their C++ equivalents with keyword support. 486 | 487 | Args: 488 | target_globals: The globals dict to modify. If None, uses caller's globals(). 489 | """ 490 | import inspect 491 | 492 | if target_globals is None: 493 | # Get the caller's globals 494 | frame = inspect.currentframe().f_back 495 | target_globals = frame.f_globals 496 | 497 | # Find all Python ROS message classes in globals 498 | python_msgs = {} 499 | for name, obj in list(target_globals.items()): 500 | try: 501 | # Check if this is a Python ROS message class (not instance) 502 | if (hasattr(obj, '__module__') and 503 | hasattr(obj, '__name__') and 504 | _is_msg_python(obj) and 505 | inspect.isclass(obj)): 506 | python_msgs[name] = obj 507 | except Exception: 508 | # Skip any objects that cause issues during inspection 509 | continue 510 | 511 | print(f"Found {len(python_msgs)} Python ROS message classes: {list(python_msgs.keys())}") 512 | 513 | # Convert each Python message to C++ equivalent 514 | for name, py_msg_class in python_msgs.items(): 515 | try: 516 | # Get the C++ equivalent 517 | cpp_msg_type, cpp_msg_class = _resolve_message_type(py_msg_class) 518 | 519 | # Apply keyword argument monkey-patch 520 | enable_kwargs_for_cpp_msg(cpp_msg_class) 521 | 522 | # Replace in globals 523 | target_globals[name] = cpp_msg_class 524 | print(f"Converted {name}: {py_msg_class} -> {cpp_msg_class}") 525 | 526 | except Exception as e: 527 | print(f"Failed to convert {name}: {e}") 528 | # Keep the original Python version if conversion fails 529 | continue 530 | 531 | def setup_automatic_cpp_conversion(): 532 | """ 533 | Set up automatic conversion of Python ROS messages to C++ equivalents. 534 | This sets up an import hook that will automatically convert messages during import. 535 | """ 536 | import sys 537 | import importlib.util 538 | from importlib.abc import Loader, MetaPathFinder 539 | from importlib.machinery import ModuleSpec 540 | 541 | class ROSMessageConverter(MetaPathFinder): 542 | """Import hook to automatically convert ROS messages to C++ equivalents""" 543 | 544 | def __init__(self): 545 | # Keep track of modules we're currently importing to avoid recursion 546 | self._importing = set() 547 | # Store the original loaders we wrap 548 | self._original_loaders = {} 549 | 550 | def find_spec(self, fullname, path, target=None): 551 | """Called by Python's import system for each import statement""" 552 | 553 | # Only intercept ROS message type imports (e.g. std_msgs.msg._string) 554 | # The actual message types are in the _ modules 555 | if '.msg._' in fullname and fullname not in self._importing: 556 | # Let the normal import happen first 557 | self._importing.add(fullname) 558 | try: 559 | spec = importlib.util.find_spec(fullname) 560 | finally: 561 | self._importing.remove(fullname) 562 | 563 | if spec is not None and spec.loader is not None: 564 | # Remember the original loader 565 | self._original_loaders[fullname] = spec.loader 566 | # Return a new spec with our custom loader 567 | return ModuleSpec( 568 | fullname, 569 | ROSMessageLoader(self._original_loaders[fullname]), 570 | origin=spec.origin, 571 | is_package=spec.parent == '' 572 | ) 573 | return None 574 | 575 | class ROSMessageLoader(Loader): 576 | """Custom loader that converts ROS messages to C++ after normal loading""" 577 | 578 | def __init__(self, original_loader): 579 | self._original_loader = original_loader 580 | 581 | def create_module(self, spec): 582 | """Let the original loader create the module""" 583 | return self._original_loader.create_module(spec) 584 | 585 | def exec_module(self, module): 586 | """Execute the module normally, then convert any messages to C++""" 587 | # Execute with original loader first 588 | self._original_loader.exec_module(module) 589 | # Get the message name from the module dir(), we know it has an attr that is called 'Metaclass_MESSAGENAME' 590 | msg_name = [attr for attr in dir(module) if attr.startswith("Metaclass_")][0] 591 | msg_name = msg_name.replace("Metaclass_", "") 592 | # Only convert the actual message class 593 | if hasattr(module, msg_name): 594 | msg_class = getattr(module, msg_name) 595 | try: 596 | if _is_msg_python(msg_class): 597 | # Get C++ equivalent 598 | cpp_msg_type, cpp_msg_class = _resolve_message_type(msg_class) 599 | enable_kwargs_for_cpp_msg(cpp_msg_class) 600 | add_python_msg_sugar_to_cpp_msg(cpp_msg_class, msg_class) 601 | 602 | # Replace the class in the module 603 | setattr(module, msg_name, cpp_msg_class) 604 | print(f"Auto-converted {module.__name__}.{msg_name} to C++") 605 | except Exception as e: 606 | print(f"Failed to convert {msg_name}: {e}") 607 | 608 | # Install the import hook if not already installed 609 | if not any(isinstance(finder, ROSMessageConverter) for finder in sys.meta_path): 610 | sys.meta_path.insert(0, ROSMessageConverter()) 611 | print("Automatic C++ message conversion enabled!") 612 | 613 | if __name__ == "__main__": 614 | # Example 1: Manual approach (what you had before) 615 | print("=== Manual Approach ===") 616 | rclcpp = bringup_rclcpp() 617 | # from std_msgs.msg import String 618 | import cppyy 619 | cppyy.include("std_msgs/msg/string.hpp") 620 | CppString = cppyy.gbl.std_msgs.msg.String 621 | enable_kwargs_for_cpp_msg(CppString) 622 | 623 | node = RclcppyyNode("rclcppyy_node") 624 | publisher = node.create_publisher(CppString, "test_topic", 10) 625 | msg = CppString(data="Hello from manual approach!") 626 | print(f"Manual: Created message with data='{msg.data}'") 627 | 628 | # # Example 2: Post-import global replacement (Approach 1 - Recommended) 629 | # print("\n=== Post-Import Global Replacement ===") 630 | # from std_msgs.msg import String as PyString 631 | # from geometry_msgs.msg import TwistStamped 632 | # from rcl_interfaces.msg import ParameterValue 633 | 634 | # print("Before conversion:", type(PyString)) 635 | # convert_python_msgs_to_cpp() # This will convert all Python messages in globals() 636 | # print("After conversion:", type(PyString)) 637 | 638 | # # Now PyString is actually a C++ class with keyword support! 639 | # cpp_msg = PyString(data="Hello from auto-converted C++!") 640 | # print(f"Auto-converted: Created message with data='{cpp_msg.data}'") 641 | 642 | # Example 3: Automatic import hook (Approach 2 - Advanced) 643 | print("\n=== Automatic Import Hook ===") 644 | setup_automatic_cpp_conversion() # Enable the import hook 645 | 646 | from diagnostic_msgs.msg import DiagnosticStatus 647 | from nav_msgs.msg import Odometry 648 | from sensor_msgs.msg import Image, PointCloud2, JointState 649 | print(f"Image: {Image}") 650 | print(f"PointCloud2: {PointCloud2}") 651 | print(f"JointState: {JointState}") 652 | print(f"Image(): {Image()}") 653 | print(f"PointCloud2(): {PointCloud2()}") 654 | print(f"JointState(): {JointState()}") 655 | 656 | 657 | # Any future imports will be automatically converted 658 | # (This would work for fresh imports in a new Python session) 659 | 660 | timer = node.create_timer(1.0, lambda: publisher.publish(msg)) 661 | 662 | print("\nStarting ROS spinning...") 663 | node._rclcpp.spin(node._rclcpp_node) 664 | rclpy.shutdown() --------------------------------------------------------------------------------