From 6606c8a74540a6ff2ec081b22bf257b81c1c7830 Mon Sep 17 00:00:00 2001 From: Nadav Elkabets Date: Sat, 30 Aug 2025 21:55:52 +0300 Subject: [PATCH 01/13] Added set_on_new_response_callback Signed-off-by: Nadav Elkabets --- rclpy/src/rclpy/client.cpp | 43 +++++++++++++++++++++++++++++++++++++- rclpy/src/rclpy/client.hpp | 11 ++++++++++ 2 files changed, 53 insertions(+), 1 deletion(-) diff --git a/rclpy/src/rclpy/client.cpp b/rclpy/src/rclpy/client.cpp index ccbddfd6a..b10884497 100644 --- a/rclpy/src/rclpy/client.cpp +++ b/rclpy/src/rclpy/client.cpp @@ -23,6 +23,7 @@ #include #include +#include #include "client.hpp" #include "clock.hpp" @@ -30,9 +31,11 @@ #include "node.hpp" #include "python_allocator.hpp" #include "utils.hpp" +#include "events_executor/rcl_support.hpp" namespace rclpy { +using events_executor::RclEventCallbackTrampoline; void Client::destroy() @@ -181,6 +184,40 @@ Client::get_logger_name() const return node_logger_name; } +void +Client::set_callback( + rcl_event_callback_t callback, + const void * user_data) +{ + rcl_ret_t ret = rcl_client_set_on_new_response_callback( + rcl_client_.get(), + callback, + user_data); + + if (RCL_RET_OK != ret) { + throw RCLError("failed to set the on new message callback for subscription"); + } +} + +void +Client::set_on_new_response_callback(std::function callback) +{ + clear_on_new_response_callback(); + on_new_response_callback_ = std::move(callback); + set_callback( + RclEventCallbackTrampoline, + static_cast(&on_new_response_callback_)); +} + +void +Client::clear_on_new_response_callback() +{ + if (on_new_response_callback_) { + set_callback(nullptr, nullptr); + on_new_response_callback_ = nullptr; + } +} + void define_client(py::object module) { @@ -208,6 +245,10 @@ define_client(py::object module) "Configure whether introspection is enabled") .def( "get_logger_name", &Client::get_logger_name, - "Get the name of the logger associated with the node of the client."); + "Get the name of the logger associated with the node of the client.") + .def( + "set_on_new_response_callback", &Client::set_on_new_response_callback, + py::arg("callback")) + .def("clear_on_new_response_callback", &Client::clear_on_new_response_callback); } } // namespace rclpy diff --git a/rclpy/src/rclpy/client.hpp b/rclpy/src/rclpy/client.hpp index 4428dfcc4..8fa8a3f07 100644 --- a/rclpy/src/rclpy/client.hpp +++ b/rclpy/src/rclpy/client.hpp @@ -16,6 +16,7 @@ #define RCLPY__CLIENT_HPP_ #include +#include #include #include @@ -120,10 +121,20 @@ class Client : public Destroyable, public std::enable_shared_from_this const char * get_logger_name() const; + void + set_on_new_response_callback(std::function callback); + + void + clear_on_new_response_callback(); + private: Node node_; + std::function on_new_response_callback_{nullptr}; std::shared_ptr rcl_client_; rosidl_service_type_support_t * srv_type_; + + void + set_callback(rcl_event_callback_t callback, const void * user_data); }; /// Define a pybind11 wrapper for an rclpy::Client From ef3918a25a328fa7b4ea614b54d975b0f93c2f85 Mon Sep 17 00:00:00 2001 From: Nadav Elkabets Date: Sat, 30 Aug 2025 22:00:27 +0300 Subject: [PATCH 02/13] Added set_on_new_request_callback Signed-off-by: Nadav Elkabets --- rclpy/src/rclpy/service.cpp | 43 ++++++++++++++++++++++++++++++++++++- rclpy/src/rclpy/service.hpp | 11 ++++++++++ 2 files changed, 53 insertions(+), 1 deletion(-) diff --git a/rclpy/src/rclpy/service.cpp b/rclpy/src/rclpy/service.cpp index e24d07b97..a912941bb 100644 --- a/rclpy/src/rclpy/service.cpp +++ b/rclpy/src/rclpy/service.cpp @@ -22,15 +22,18 @@ #include #include +#include #include "clock.hpp" #include "exceptions.hpp" #include "node.hpp" #include "service.hpp" #include "utils.hpp" +#include "events_executor/rcl_support.hpp" namespace rclpy { +using events_executor::RclEventCallbackTrampoline; void Service::destroy() @@ -184,6 +187,40 @@ Service::configure_introspection( } } +void +Service::set_callback( + rcl_event_callback_t callback, + const void * user_data) +{ + rcl_ret_t ret = rcl_service_set_on_new_request_callback( + rcl_service_.get(), + callback, + user_data); + + if (RCL_RET_OK != ret) { + throw RCLError("failed to set the on new message callback for subscription"); + } +} + +void +Service::set_on_new_request_callback(std::function callback) +{ + clear_on_new_request_callback(); + on_new_request_callback_ = std::move(callback); + set_callback( + RclEventCallbackTrampoline, + static_cast(&on_new_request_callback_)); +} + +void +Service::clear_on_new_request_callback() +{ + if (on_new_request_callback_) { + set_callback(nullptr, nullptr); + on_new_request_callback_ = nullptr; + } +} + void define_service(py::object module) { @@ -211,6 +248,10 @@ define_service(py::object module) "Configure whether introspection is enabled") .def( "get_logger_name", &Service::get_logger_name, - "Get the name of the logger associated with the node of the service."); + "Get the name of the logger associated with the node of the service.") + .def( + "set_on_new_request_callback", &Service::set_on_new_request_callback, + py::arg("callback")) + .def("clear_on_new_request_callback", &Service::clear_on_new_request_callback); } } // namespace rclpy diff --git a/rclpy/src/rclpy/service.hpp b/rclpy/src/rclpy/service.hpp index 6b178e18b..6f63ebb04 100644 --- a/rclpy/src/rclpy/service.hpp +++ b/rclpy/src/rclpy/service.hpp @@ -16,6 +16,7 @@ #define RCLPY__SERVICE_HPP_ #include +#include #include #include @@ -125,10 +126,20 @@ class Service : public Destroyable, public std::enable_shared_from_this void destroy() override; + void + set_on_new_request_callback(std::function callback); + + void + clear_on_new_request_callback(); + private: Node node_; + std::function on_new_request_callback_{nullptr}; std::shared_ptr rcl_service_; rosidl_service_type_support_t * srv_type_; + + void + set_callback(rcl_event_callback_t callback, const void * user_data); }; /// Define a pybind11 wrapper for an rclpy::Service From 0644188207c766dfac198da4fea58c2dd4d28957 Mon Sep 17 00:00:00 2001 From: Nadav Elkabets Date: Sat, 30 Aug 2025 22:03:57 +0300 Subject: [PATCH 03/13] Added set_on_new_message_callback Signed-off-by: Nadav Elkabets --- rclpy/src/rclpy/subscription.cpp | 44 +++++++++++++++++++++++++++++++- rclpy/src/rclpy/subscription.hpp | 11 ++++++++ 2 files changed, 54 insertions(+), 1 deletion(-) diff --git a/rclpy/src/rclpy/subscription.cpp b/rclpy/src/rclpy/subscription.cpp index eab4de9a6..33b652d5b 100644 --- a/rclpy/src/rclpy/subscription.cpp +++ b/rclpy/src/rclpy/subscription.cpp @@ -24,17 +24,21 @@ #include #include #include +#include #include "exceptions.hpp" #include "node.hpp" #include "serialization.hpp" #include "subscription.hpp" #include "utils.hpp" +#include "events_executor/rcl_support.hpp" using pybind11::literals::operator""_a; namespace rclpy { +using events_executor::RclEventCallbackTrampoline; + Subscription::Subscription( Node & node, py::object pymsg_type, std::string topic, py::object pyqos_profile) @@ -194,6 +198,40 @@ Subscription::get_publisher_count() const return count; } +void +Subscription::set_callback( + rcl_event_callback_t callback, + const void * user_data) +{ + rcl_ret_t ret = rcl_subscription_set_on_new_message_callback( + rcl_subscription_.get(), + callback, + user_data); + + if (RCL_RET_OK != ret) { + throw RCLError("failed to set the on new message callback for subscription"); + } +} + +void +Subscription::set_on_new_message_callback(std::function callback) +{ + clear_on_new_message_callback(); + on_new_message_callback_ = std::move(callback); + set_callback( + RclEventCallbackTrampoline, + static_cast(&on_new_message_callback_)); +} + +void +Subscription::clear_on_new_message_callback() +{ + if (on_new_message_callback_) { + set_callback(nullptr, nullptr); + on_new_message_callback_ = nullptr; + } +} + void define_subscription(py::object module) { @@ -215,6 +253,10 @@ define_subscription(py::object module) "Return the resolved topic name of a subscription.") .def( "get_publisher_count", &Subscription::get_publisher_count, - "Count the publishers from a subscription."); + "Count the publishers from a subscription.") + .def( + "set_on_new_message_callback", &Subscription::set_on_new_message_callback, + py::arg("callback")) + .def("clear_on_new_message_callback", &Subscription::clear_on_new_message_callback); } } // namespace rclpy diff --git a/rclpy/src/rclpy/subscription.hpp b/rclpy/src/rclpy/subscription.hpp index f46401ea2..7563de01b 100644 --- a/rclpy/src/rclpy/subscription.hpp +++ b/rclpy/src/rclpy/subscription.hpp @@ -16,6 +16,7 @@ #define RCLPY__SUBSCRIPTION_HPP_ #include +#include #include @@ -105,9 +106,19 @@ class Subscription : public Destroyable, public std::enable_shared_from_this callback); + + void + clear_on_new_message_callback(); + private: Node node_; + std::function on_new_message_callback_{nullptr}; std::shared_ptr rcl_subscription_; + + void + set_callback(rcl_event_callback_t callback, const void * user_data); }; /// Define a pybind11 wrapper for an rclpy::Subscription void define_subscription(py::object module); From 46dffff4a7bd0ca2c40fcb2b22c863ef0cd1dbf7 Mon Sep 17 00:00:00 2001 From: Nadav Elkabets Date: Sat, 30 Aug 2025 22:08:30 +0300 Subject: [PATCH 04/13] Added set_on_reset_callback Signed-off-by: Nadav Elkabets --- rclpy/src/rclpy/timer.cpp | 44 ++++++++++++++++++++++++++++++++++++++- rclpy/src/rclpy/timer.hpp | 11 ++++++++++ 2 files changed, 54 insertions(+), 1 deletion(-) diff --git a/rclpy/src/rclpy/timer.cpp b/rclpy/src/rclpy/timer.cpp index 239e7b48d..b7b77d4bf 100644 --- a/rclpy/src/rclpy/timer.cpp +++ b/rclpy/src/rclpy/timer.cpp @@ -19,14 +19,18 @@ #include #include +#include #include "clock.hpp" #include "context.hpp" #include "exceptions.hpp" #include "timer.hpp" +#include "events_executor/rcl_support.hpp" namespace rclpy { +using events_executor::RclEventCallbackTrampoline; + void Timer::destroy() { @@ -175,6 +179,40 @@ bool Timer::is_timer_canceled() return is_canceled; } +void +Timer::set_callback( + rcl_event_callback_t callback, + const void * user_data) +{ + rcl_ret_t ret = rcl_timer_set_on_reset_callback( + rcl_timer_.get(), + callback, + user_data); + + if (RCL_RET_OK != ret) { + throw RCLError("failed to set the on reset callback for timer"); + } +} + +void +Timer::set_on_reset_callback(std::function callback) +{ + clear_on_reset_callback(); + on_reset_callback_ = std::move(callback); + set_callback( + RclEventCallbackTrampoline, + static_cast(&on_reset_callback_)); +} + +void +Timer::clear_on_reset_callback() +{ + if (on_reset_callback_) { + set_callback(nullptr, nullptr); + on_reset_callback_ = nullptr; + } +} + void define_timer(py::object module) { @@ -212,7 +250,11 @@ define_timer(py::object module) "Cancel a timer.") .def( "is_timer_canceled", &Timer::is_timer_canceled, - "Check if a timer is canceled."); + "Check if a timer is canceled.") + .def( + "set_on_reset_callback", &Timer::set_on_reset_callback, + py::arg("callback")) + .def("clear_on_reset_callback", &Timer::clear_on_reset_callback); } } // namespace rclpy diff --git a/rclpy/src/rclpy/timer.hpp b/rclpy/src/rclpy/timer.hpp index 5cbab817f..be0fa17c2 100644 --- a/rclpy/src/rclpy/timer.hpp +++ b/rclpy/src/rclpy/timer.hpp @@ -17,6 +17,7 @@ #include #include +#include #include @@ -145,10 +146,20 @@ class Timer : public Destroyable, public std::enable_shared_from_this /// Force an early destruction of this object void destroy() override; + void + set_on_reset_callback(std::function callback); + + void + clear_on_reset_callback(); + private: Context context_; Clock clock_; + std::function on_reset_callback_{nullptr}; std::shared_ptr rcl_timer_; + + void + set_callback(rcl_event_callback_t callback, const void * user_data); }; /// Define a pybind11 wrapper for an rcl_timer_t From b26be00f617a0128e6d327120e7c8807425badae Mon Sep 17 00:00:00 2001 From: Nadav Elkabets Date: Sat, 30 Aug 2025 22:47:28 +0300 Subject: [PATCH 05/13] Added type hints to pyi Signed-off-by: Nadav Elkabets --- rclpy/rclpy/impl/_rclpy_pybind11.pyi | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/rclpy/rclpy/impl/_rclpy_pybind11.pyi b/rclpy/rclpy/impl/_rclpy_pybind11.pyi index c8c306f76..603b4c858 100644 --- a/rclpy/rclpy/impl/_rclpy_pybind11.pyi +++ b/rclpy/rclpy/impl/_rclpy_pybind11.pyi @@ -195,6 +195,12 @@ class Client(Destroyable, Generic[SrvRequestT, SrvResponseT]): def get_logger_name(self) -> str: """Get the name of the logger associated with the node of the client.""" + def set_on_new_response_callback(self, callback: Callable[[int], None]) -> None: + ... + + def clear_on_new_response_callback(self) -> None: + ... + class Context(Destroyable): @@ -286,6 +292,12 @@ class Service(Destroyable, Generic[SrvRequestT, SrvResponseT]): def get_logger_name(self) -> str: """Get the name of the logger associated with the node of the service.""" + def set_on_new_request_callback(self, callback: Callable[[int], None]) -> None: + ... + + def clear_on_new_request_callback(self) -> None: + ... + class TypeDescriptionService(Destroyable): @@ -578,6 +590,12 @@ class Timer(Destroyable): def is_timer_canceled(self) -> bool: """Check if a timer is canceled.""" + def set_on_reset_callback(self, callback: Callable[[int], None]) -> None: + ... + + def clear_on_reset_callback(self) -> None: + ... + class Subscription(Destroyable, Generic[MsgT]): @@ -600,6 +618,12 @@ class Subscription(Destroyable, Generic[MsgT]): def get_publisher_count(self) -> int: """Count the publishers from a subscription.""" + def set_on_new_message_callback(self, callback: Callable[[int], None]) -> None: + ... + + def clear_on_new_message_callback(self) -> None: + ... + class rcl_time_point_t: From 8094f155d5bc8fe2df13fbaa91b0162082d83c7a Mon Sep 17 00:00:00 2001 From: Nadav Elkabets Date: Sat, 30 Aug 2025 22:51:24 +0300 Subject: [PATCH 06/13] Added test for subscription Signed-off-by: Nadav Elkabets --- rclpy/test/test_subscription.py | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/rclpy/test/test_subscription.py b/rclpy/test/test_subscription.py index 122e626d0..8f34f57d0 100644 --- a/rclpy/test/test_subscription.py +++ b/rclpy/test/test_subscription.py @@ -14,7 +14,9 @@ import time +from numpy import cbrt import pytest +from unittest.mock import Mock import rclpy from rclpy.node import Node @@ -172,3 +174,19 @@ def test_subscription_publisher_count() -> None: sub.destroy() node.destroy_node() + +def test_on_new_message_callback(test_node) -> None: + topic_name = "/topic" + cb = Mock() + sub = test_node.create_subscription( + msg_type=Empty, + topic=topic_name, + qos_profile=10, + callback=cb) + pub = test_node.create_publisher(Empty, topic_name, 10) + sub.handle.set_on_new_message_callback(cb) + pub.publish(Empty()) + cb.assert_called_once_with(1) + sub.handle.clear_on_new_message_callback() + pub.publish(Empty()) + cb.assert_called_once() From 418767408e7ab7f39daa85639951689a054cc63f Mon Sep 17 00:00:00 2001 From: Nadav Elkabets Date: Sat, 30 Aug 2025 23:05:20 +0300 Subject: [PATCH 07/13] Added test for timer Signed-off-by: Nadav Elkabets --- rclpy/test/test_timer.py | 31 +++++++++++++++++++++++++++++++ 1 file changed, 31 insertions(+) diff --git a/rclpy/test/test_timer.py b/rclpy/test/test_timer.py index 561dbfcad..1ba2ecc79 100644 --- a/rclpy/test/test_timer.py +++ b/rclpy/test/test_timer.py @@ -18,6 +18,7 @@ import time from typing import List from typing import Optional +from unittest.mock import Mock import pytest import rclpy @@ -27,6 +28,25 @@ from rclpy.timer import TimerInfo +@pytest.fixture +def context() -> None: + return rclpy.context.Context() + + +@pytest.fixture +def setup_ros(context) -> None: + rclpy.init(context=context) + yield + rclpy.shutdown(context=context) + + +@pytest.fixture +def test_node(context, setup_ros): + node = rclpy.create_node('test_node', context=context) + yield node + node.destroy_node() + + TEST_PERIODS = ( 0.1, pytest.param( @@ -328,3 +348,14 @@ def timer_callback(info: TimerInfo) -> None: node.destroy_timer(timer) node.destroy_node() rclpy.shutdown(context=context) + +def test_on_reset_callback(test_node): + tmr = test_node.create_timer(1, lambda: None) + cb = Mock() + tmr.handle.set_on_reset_callback(cb) + cb.assert_not_called() + tmr.reset() + cb.assert_called_once_with(1) + tmr.handle.clear_on_reset_callback() + tmr.reset() + cb.assert_called_once() From e10863c6346db1d0b08fb1a4471fa16dab7f176e Mon Sep 17 00:00:00 2001 From: Nadav Elkabets Date: Sat, 30 Aug 2025 23:18:21 +0300 Subject: [PATCH 08/13] Added test for client Signed-off-by: Nadav Elkabets --- rclpy/test/test_client.py | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/rclpy/test/test_client.py b/rclpy/test/test_client.py index 93a40ac95..26d8bc8d8 100644 --- a/rclpy/test/test_client.py +++ b/rclpy/test/test_client.py @@ -18,6 +18,7 @@ import traceback from typing import TYPE_CHECKING import unittest +from unittest.mock import Mock from rcl_interfaces.srv import GetParameters import rclpy @@ -275,6 +276,27 @@ def test_logger_name_is_equal_to_node_name(self) -> None: with self.node.create_client(GetParameters, 'get/parameters') as cli: self.assertEqual(cli.logger_name, 'TestClient') + def test_on_new_response_callback(self) -> None: + def _service(request, response): + return response + with self.node.create_client(Empty, '/service') as cli: + with self.node.create_service(Empty, '/service', _service): + executor = rclpy.executors.SingleThreadedExecutor(context=self.context) + try: + self.assertTrue(cli.wait_for_service(timeout_sec=20)) + executor.add_node(self.node) + cb = Mock() + cli.handle.set_on_new_response_callback(cb) + cli.call_async(Empty.Request()) + executor.spin_once(0) + cb.assert_called_once_with(1) + cli.handle.clear_on_new_response_callback() + cli.call_async(Empty.Request()) + executor.spin_once(0) + cb.assert_called_once() + finally: + executor.shutdown() + if __name__ == '__main__': unittest.main() From cffb9c0d45c88cf0b8ec7aef3287d35b2d8eecaa Mon Sep 17 00:00:00 2001 From: Nadav Elkabets Date: Sat, 30 Aug 2025 23:27:30 +0300 Subject: [PATCH 09/13] Added test for service Signed-off-by: Nadav Elkabets --- rclpy/test/test_client.py | 1 + rclpy/test/test_service.py | 14 ++++++++++++++ rclpy/test/test_subscription.py | 1 + 3 files changed, 16 insertions(+) diff --git a/rclpy/test/test_client.py b/rclpy/test/test_client.py index 26d8bc8d8..0f6087a97 100644 --- a/rclpy/test/test_client.py +++ b/rclpy/test/test_client.py @@ -287,6 +287,7 @@ def _service(request, response): executor.add_node(self.node) cb = Mock() cli.handle.set_on_new_response_callback(cb) + cb.assert_not_called() cli.call_async(Empty.Request()) executor.spin_once(0) cb.assert_called_once_with(1) diff --git a/rclpy/test/test_service.py b/rclpy/test/test_service.py index 9ac770071..243d37302 100644 --- a/rclpy/test/test_service.py +++ b/rclpy/test/test_service.py @@ -13,6 +13,7 @@ # limitations under the License. import pytest +from unittest.mock import Mock import rclpy from rclpy.node import Node @@ -105,3 +106,16 @@ def test_service_context_manager() -> None: with node.create_service( srv_type=Empty, srv_name='empty_service', callback=lambda _, _1: None) as srv: assert srv.service_name == '/empty_service' + + +def test_set_on_new_request_callback(test_node) -> None: + cli = test_node.create_client(Empty, '/service') + srv = test_node.create_service(Empty, '/service', lambda req, res: res) + cb = Mock() + srv.handle.set_on_new_request_callback(cb) + cb.assert_not_called() + cli.call_async(Empty.Request()) + cb.assert_called_once_with(1) + srv.handle.clear_on_new_request_callback() + cli.call_async(Empty.Request()) + cb.assert_called_once() diff --git a/rclpy/test/test_subscription.py b/rclpy/test/test_subscription.py index 8f34f57d0..16b01ceed 100644 --- a/rclpy/test/test_subscription.py +++ b/rclpy/test/test_subscription.py @@ -185,6 +185,7 @@ def test_on_new_message_callback(test_node) -> None: callback=cb) pub = test_node.create_publisher(Empty, topic_name, 10) sub.handle.set_on_new_message_callback(cb) + cb.assert_not_called() pub.publish(Empty()) cb.assert_called_once_with(1) sub.handle.clear_on_new_message_callback() From 0b1c08abeb39a6bf1b4a14d3510d335baa990be6 Mon Sep 17 00:00:00 2001 From: Nadav Elkabets Date: Sat, 30 Aug 2025 23:34:09 +0300 Subject: [PATCH 10/13] Fixed lint Signed-off-by: Nadav Elkabets --- rclpy/test/test_service.py | 3 ++- rclpy/test/test_subscription.py | 6 +++--- rclpy/test/test_timer.py | 1 + 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/rclpy/test/test_service.py b/rclpy/test/test_service.py index 243d37302..5c4d696a0 100644 --- a/rclpy/test/test_service.py +++ b/rclpy/test/test_service.py @@ -12,9 +12,10 @@ # See the License for the specific language governing permissions and # limitations under the License. -import pytest from unittest.mock import Mock +import pytest + import rclpy from rclpy.node import Node diff --git a/rclpy/test/test_subscription.py b/rclpy/test/test_subscription.py index 16b01ceed..f7d308528 100644 --- a/rclpy/test/test_subscription.py +++ b/rclpy/test/test_subscription.py @@ -13,10 +13,9 @@ # limitations under the License. import time +from unittest.mock import Mock -from numpy import cbrt import pytest -from unittest.mock import Mock import rclpy from rclpy.node import Node @@ -175,8 +174,9 @@ def test_subscription_publisher_count() -> None: node.destroy_node() + def test_on_new_message_callback(test_node) -> None: - topic_name = "/topic" + topic_name = '/topic' cb = Mock() sub = test_node.create_subscription( msg_type=Empty, diff --git a/rclpy/test/test_timer.py b/rclpy/test/test_timer.py index 1ba2ecc79..f50692657 100644 --- a/rclpy/test/test_timer.py +++ b/rclpy/test/test_timer.py @@ -349,6 +349,7 @@ def timer_callback(info: TimerInfo) -> None: node.destroy_node() rclpy.shutdown(context=context) + def test_on_reset_callback(test_node): tmr = test_node.create_timer(1, lambda: None) cb = Mock() From c83a7790d4df1001e98082606ae3004a0052db02 Mon Sep 17 00:00:00 2001 From: Nadav Elkabets Date: Sun, 7 Sep 2025 00:58:14 +0300 Subject: [PATCH 11/13] Avoid throwing into c code Signed-off-by: Nadav Elkabets --- rclpy/src/rclpy/events_executor/rcl_support.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/rclpy/src/rclpy/events_executor/rcl_support.cpp b/rclpy/src/rclpy/events_executor/rcl_support.cpp index 3a8826f70..1fd26d80a 100644 --- a/rclpy/src/rclpy/events_executor/rcl_support.cpp +++ b/rclpy/src/rclpy/events_executor/rcl_support.cpp @@ -26,7 +26,13 @@ namespace events_executor extern "C" void RclEventCallbackTrampoline(const void * user_data, size_t number_of_events) { const auto cb = reinterpret_cast *>(user_data); - (*cb)(number_of_events); + try { + (*cb)(number_of_events); + } catch (const std::exception & e) { + // Catch and print any exception to avoid propagation to c code + std::fputs(e.what(), stderr); + std::exit(EXIT_FAILURE); + } } RclCallbackManager::RclCallbackManager(EventsQueue * events_queue) From bbfb08e8c35d3e6c67ace0e60b19a1dc50fc903c Mon Sep 17 00:00:00 2001 From: Nadav Elkabets <32939935+nadavelkabets@users.noreply.github.com> Date: Sat, 13 Sep 2025 01:46:16 +0300 Subject: [PATCH 12/13] Print newline after logging exception Co-authored-by: William Woodall Signed-off-by: Nadav Elkabets <32939935+nadavelkabets@users.noreply.github.com> --- rclpy/src/rclpy/events_executor/rcl_support.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclpy/src/rclpy/events_executor/rcl_support.cpp b/rclpy/src/rclpy/events_executor/rcl_support.cpp index 1fd26d80a..f048431d8 100644 --- a/rclpy/src/rclpy/events_executor/rcl_support.cpp +++ b/rclpy/src/rclpy/events_executor/rcl_support.cpp @@ -30,7 +30,7 @@ extern "C" void RclEventCallbackTrampoline(const void * user_data, size_t number (*cb)(number_of_events); } catch (const std::exception & e) { // Catch and print any exception to avoid propagation to c code - std::fputs(e.what(), stderr); + std::fprintf(stderr, "%s\n", e.what()); std::exit(EXIT_FAILURE); } } From e2452bb2a9321aee43ae57b98b29bd44486b5c97 Mon Sep 17 00:00:00 2001 From: Nadav Elkabets Date: Sat, 13 Sep 2025 22:35:05 +0300 Subject: [PATCH 13/13] Improve logging and cleanup Signed-off-by: Nadav Elkabets --- rclpy/rclpy/impl/_rclpy_pybind11.pyi | 16 ++++++++-------- rclpy/src/rclpy/client.cpp | 7 ++++++- rclpy/src/rclpy/events_executor/rcl_support.cpp | 2 +- rclpy/src/rclpy/service.cpp | 7 ++++++- rclpy/src/rclpy/subscription.cpp | 7 ++++++- rclpy/src/rclpy/timer.cpp | 8 +++++++- 6 files changed, 34 insertions(+), 13 deletions(-) diff --git a/rclpy/rclpy/impl/_rclpy_pybind11.pyi b/rclpy/rclpy/impl/_rclpy_pybind11.pyi index 603b4c858..878e87f53 100644 --- a/rclpy/rclpy/impl/_rclpy_pybind11.pyi +++ b/rclpy/rclpy/impl/_rclpy_pybind11.pyi @@ -196,10 +196,10 @@ class Client(Destroyable, Generic[SrvRequestT, SrvResponseT]): """Get the name of the logger associated with the node of the client.""" def set_on_new_response_callback(self, callback: Callable[[int], None]) -> None: - ... + """Set the on new response callback function for the client.""" def clear_on_new_response_callback(self) -> None: - ... + """Clear the on new response callback function for the client.""" class Context(Destroyable): @@ -293,10 +293,10 @@ class Service(Destroyable, Generic[SrvRequestT, SrvResponseT]): """Get the name of the logger associated with the node of the service.""" def set_on_new_request_callback(self, callback: Callable[[int], None]) -> None: - ... + """Set the on new request callback function for the service.""" def clear_on_new_request_callback(self) -> None: - ... + """Clear the on new request callback function for the service.""" class TypeDescriptionService(Destroyable): @@ -591,10 +591,10 @@ class Timer(Destroyable): """Check if a timer is canceled.""" def set_on_reset_callback(self, callback: Callable[[int], None]) -> None: - ... + """Set the on reset callback function for the timer.""" def clear_on_reset_callback(self) -> None: - ... + """Clear the on reset callback function for the timer.""" class Subscription(Destroyable, Generic[MsgT]): @@ -619,10 +619,10 @@ class Subscription(Destroyable, Generic[MsgT]): """Count the publishers from a subscription.""" def set_on_new_message_callback(self, callback: Callable[[int], None]) -> None: - ... + """Set the on new message callback function for the subscription.""" def clear_on_new_message_callback(self) -> None: - ... + """Clear the on new message callback function for the subscription.""" class rcl_time_point_t: diff --git a/rclpy/src/rclpy/client.cpp b/rclpy/src/rclpy/client.cpp index b10884497..0c12108da 100644 --- a/rclpy/src/rclpy/client.cpp +++ b/rclpy/src/rclpy/client.cpp @@ -40,6 +40,10 @@ using events_executor::RclEventCallbackTrampoline; void Client::destroy() { + try { + clear_on_new_response_callback(); + } catch (RCLError) { + } rcl_client_.reset(); node_.destroy(); } @@ -195,7 +199,8 @@ Client::set_callback( user_data); if (RCL_RET_OK != ret) { - throw RCLError("failed to set the on new message callback for subscription"); + throw RCLError(std::string("Failed to set the on new response callback for client: ") + + rcl_get_error_string().str); } } diff --git a/rclpy/src/rclpy/events_executor/rcl_support.cpp b/rclpy/src/rclpy/events_executor/rcl_support.cpp index f048431d8..3a8e54c68 100644 --- a/rclpy/src/rclpy/events_executor/rcl_support.cpp +++ b/rclpy/src/rclpy/events_executor/rcl_support.cpp @@ -31,7 +31,7 @@ extern "C" void RclEventCallbackTrampoline(const void * user_data, size_t number } catch (const std::exception & e) { // Catch and print any exception to avoid propagation to c code std::fprintf(stderr, "%s\n", e.what()); - std::exit(EXIT_FAILURE); + std::terminate(); } } diff --git a/rclpy/src/rclpy/service.cpp b/rclpy/src/rclpy/service.cpp index a912941bb..f7714d499 100644 --- a/rclpy/src/rclpy/service.cpp +++ b/rclpy/src/rclpy/service.cpp @@ -38,6 +38,10 @@ using events_executor::RclEventCallbackTrampoline; void Service::destroy() { + try { + clear_on_new_request_callback(); + } catch (RCLError) { + } rcl_service_.reset(); node_.destroy(); } @@ -198,7 +202,8 @@ Service::set_callback( user_data); if (RCL_RET_OK != ret) { - throw RCLError("failed to set the on new message callback for subscription"); + throw RCLError(std::string("Failed to set the on new request callback for service: ") + + rcl_get_error_string().str); } } diff --git a/rclpy/src/rclpy/subscription.cpp b/rclpy/src/rclpy/subscription.cpp index 33b652d5b..e9700a198 100644 --- a/rclpy/src/rclpy/subscription.cpp +++ b/rclpy/src/rclpy/subscription.cpp @@ -91,6 +91,10 @@ Subscription::Subscription( void Subscription::destroy() { + try { + clear_on_new_message_callback(); + } catch (RCLError) { + } rcl_subscription_.reset(); node_.destroy(); } @@ -209,7 +213,8 @@ Subscription::set_callback( user_data); if (RCL_RET_OK != ret) { - throw RCLError("failed to set the on new message callback for subscription"); + throw RCLError(std::string("Failed to set the on new message callback for subscription: ") + + rcl_get_error_string().str); } } diff --git a/rclpy/src/rclpy/timer.cpp b/rclpy/src/rclpy/timer.cpp index b7b77d4bf..4561eec78 100644 --- a/rclpy/src/rclpy/timer.cpp +++ b/rclpy/src/rclpy/timer.cpp @@ -19,6 +19,7 @@ #include #include +#include #include #include "clock.hpp" @@ -34,6 +35,10 @@ using events_executor::RclEventCallbackTrampoline; void Timer::destroy() { + try { + clear_on_reset_callback(); + } catch (RCLError) { + } rcl_timer_.reset(); clock_.destroy(); context_.destroy(); @@ -190,7 +195,8 @@ Timer::set_callback( user_data); if (RCL_RET_OK != ret) { - throw RCLError("failed to set the on reset callback for timer"); + throw RCLError(std::string("Failed to set the on reset callback for timer: ") + + rcl_get_error_string().str); } }