diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index df45ff93cd..51389fc2a2 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -125,6 +125,7 @@ ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 445f1d22af..d32a3b335e 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -68,11 +68,11 @@ class ClientBase get_service_name() const; RCLCPP_PUBLIC - rcl_client_t * + std::shared_ptr get_client_handle(); RCLCPP_PUBLIC - const rcl_client_t * + std::shared_ptr get_client_handle() const; RCLCPP_PUBLIC @@ -112,7 +112,7 @@ class ClientBase rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_; std::shared_ptr node_handle_; - rcl_client_t client_handle_ = rcl_get_zero_initialized_client(); + std::shared_ptr client_handle_; std::string service_name_; }; @@ -148,7 +148,7 @@ class Client : public ClientBase auto service_type_support_handle = get_service_type_support_handle(); rcl_ret_t ret = rcl_client_init( - &client_handle_, + client_handle_.get(), this->get_rcl_node_handle(), service_type_support_handle, service_name.c_str(), @@ -170,11 +170,6 @@ class Client : public ClientBase virtual ~Client() { - if (rcl_client_fini(&client_handle_, this->get_rcl_node_handle()) != RCL_RET_OK) { - fprintf(stderr, - "Error in destruction of rcl client handle: %s\n", rcl_get_error_string_safe()); - rcl_reset_error(); - } } std::shared_ptr @@ -238,7 +233,7 @@ class Client : public ClientBase { std::lock_guard lock(pending_requests_mutex_); int64_t sequence_number; - rcl_ret_t ret = rcl_send_request(get_client_handle(), request.get(), &sequence_number); + rcl_ret_t ret = rcl_send_request(get_client_handle().get(), request.get(), &sequence_number); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request"); } diff --git a/rclcpp/include/rclcpp/memory_strategy.hpp b/rclcpp/include/rclcpp/memory_strategy.hpp index 0421555c6c..da1b160888 100644 --- a/rclcpp/include/rclcpp/memory_strategy.hpp +++ b/rclcpp/include/rclcpp/memory_strategy.hpp @@ -83,14 +83,18 @@ class RCLCPP_PUBLIC MemoryStrategy static rclcpp::SubscriptionBase::SharedPtr get_subscription_by_handle( - const rcl_subscription_t * subscriber_handle, + std::shared_ptr subscriber_handle, const WeakNodeVector & weak_nodes); static rclcpp::ServiceBase::SharedPtr - get_service_by_handle(const rcl_service_t * service_handle, const WeakNodeVector & weak_nodes); + get_service_by_handle( + std::shared_ptr service_handle, + const WeakNodeVector & weak_nodes); static rclcpp::ClientBase::SharedPtr - get_client_by_handle(const rcl_client_t * client_handle, const WeakNodeVector & weak_nodes); + get_client_by_handle( + std::shared_ptr client_handle, + const WeakNodeVector & weak_nodes); static rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_by_group( diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index ec8193fa08..27081b5fa0 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -21,6 +21,8 @@ #include #include +#include "rcutils/logging_macros.h" + #include "rcl/error_handling.h" #include "rcl/service.h" @@ -30,6 +32,7 @@ #include "rclcpp/type_support_decl.hpp" #include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/visibility_control.hpp" +#include "rclcpp/logging.hpp" #include "rmw/error_handling.h" #include "rmw/rmw.h" @@ -58,11 +61,11 @@ class ServiceBase get_service_name(); RCLCPP_PUBLIC - rcl_service_t * + std::shared_ptr get_service_handle(); RCLCPP_PUBLIC - const rcl_service_t * + std::shared_ptr get_service_handle() const; virtual std::shared_ptr create_request() = 0; @@ -84,7 +87,7 @@ class ServiceBase std::shared_ptr node_handle_; - rcl_service_t * service_handle_ = nullptr; + std::shared_ptr service_handle_; std::string service_name_; bool owns_rcl_handle_ = true; }; @@ -116,11 +119,22 @@ class Service : public ServiceBase auto service_type_support_handle = get_service_type_support_handle(); // rcl does the static memory allocation here - service_handle_ = new rcl_service_t; - *service_handle_ = rcl_get_zero_initialized_service(); + service_handle_ = std::shared_ptr( + new rcl_service_t, [ = ](rcl_service_t * service) + { + if (rcl_service_fini(service, node_handle_.get()) != RCL_RET_OK) { + RCLCPP_ERROR( + rclcpp::get_logger(rcl_node_get_name(node_handle.get())).get_child("rclcpp"), + "Error in destruction of rcl service handle: %s", + rcl_get_error_string_safe()); + rcl_reset_error(); + } + delete service; + }); + *service_handle_.get() = rcl_get_zero_initialized_service(); rcl_ret_t ret = rcl_service_init( - service_handle_, + service_handle_.get(), node_handle.get(), service_type_support_handle, service_name.c_str(), @@ -141,6 +155,29 @@ class Service : public ServiceBase } } + Service( + std::shared_ptr node_handle, + std::shared_ptr service_handle, + AnyServiceCallback any_callback) + : ServiceBase(node_handle), + any_callback_(any_callback) + { + // check if service handle was initialized + if (!rcl_service_is_valid(service_handle.get(), nullptr)) { + // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) + throw std::runtime_error( + std::string("rcl_service_t in constructor argument must be initialized beforehand.")); + // *INDENT-ON* + } + + const char * service_name = rcl_service_get_service_name(service_handle.get()); + if (!service_name) { + throw std::runtime_error("failed to get service name"); + } + service_handle_ = service_handle; + service_name_ = std::string(service_name); + } + Service( std::shared_ptr node_handle, rcl_service_t * service_handle, @@ -149,10 +186,7 @@ class Service : public ServiceBase any_callback_(any_callback) { // check if service handle was initialized - // TODO(karsten1987): Take this verification - // directly in rcl_*_t - // see: https://github.com/ros2/rcl/issues/81 - if (!service_handle->impl) { + if (!rcl_service_is_valid(service_handle, nullptr)) { // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) throw std::runtime_error( std::string("rcl_service_t in constructor argument must be initialized beforehand.")); @@ -163,26 +197,17 @@ class Service : public ServiceBase if (!service_name) { throw std::runtime_error("failed to get service name"); } - service_handle_ = service_handle; service_name_ = std::string(service_name); - owns_rcl_handle_ = false; + + // In this case, rcl owns the service handle memory + service_handle_ = std::shared_ptr(new rcl_service_t); + service_handle_->impl = service_handle->impl; } Service() = delete; virtual ~Service() { - // check if you have ownership of the handle - if (owns_rcl_handle_) { - if (rcl_service_fini(service_handle_, node_handle_.get()) != RCL_RET_OK) { - std::stringstream ss; - ss << "Error in destruction of rcl service_handle_ handle: " << - rcl_get_error_string_safe() << '\n'; - (std::cerr << ss.str()).flush(); - rcl_reset_error(); - } - delete service_handle_; - } } std::shared_ptr create_request() @@ -211,7 +236,7 @@ class Service : public ServiceBase std::shared_ptr req_id, std::shared_ptr response) { - rcl_ret_t status = rcl_send_response(get_service_handle(), req_id.get(), response.get()); + rcl_ret_t status = rcl_send_response(get_service_handle().get(), req_id.get(), response.get()); if (status != RCL_RET_OK) { rclcpp::exceptions::throw_from_rcl_error(status, "failed to send response"); diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 588c587cbd..ef820d6c0b 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -98,22 +98,22 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy { for (size_t i = 0; i < wait_set->size_of_subscriptions; ++i) { if (!wait_set->subscriptions[i]) { - subscription_handles_[i] = nullptr; + subscription_handles_[i].reset(); } } for (size_t i = 0; i < wait_set->size_of_services; ++i) { if (!wait_set->services[i]) { - service_handles_[i] = nullptr; + service_handles_[i].reset(); } } for (size_t i = 0; i < wait_set->size_of_clients; ++i) { if (!wait_set->clients[i]) { - client_handles_[i] = nullptr; + client_handles_[i].reset(); } } for (size_t i = 0; i < wait_set->size_of_timers; ++i) { if (!wait_set->timers[i]) { - timer_handles_[i] = nullptr; + timer_handles_[i].reset(); } } @@ -188,7 +188,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy bool add_handles_to_wait_set(rcl_wait_set_t * wait_set) { for (auto subscription : subscription_handles_) { - if (rcl_wait_set_add_subscription(wait_set, subscription) != RCL_RET_OK) { + if (rcl_wait_set_add_subscription(wait_set, subscription.get()) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", "Couldn't add subscription to wait set: %s", rcl_get_error_string_safe()); @@ -197,7 +197,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } for (auto client : client_handles_) { - if (rcl_wait_set_add_client(wait_set, client) != RCL_RET_OK) { + if (rcl_wait_set_add_client(wait_set, client.get()) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", "Couldn't add client to wait set: %s", rcl_get_error_string_safe()); @@ -206,7 +206,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } for (auto service : service_handles_) { - if (rcl_wait_set_add_service(wait_set, service) != RCL_RET_OK) { + if (rcl_wait_set_add_service(wait_set, service.get()) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", "Couldn't add service to wait set: %s", rcl_get_error_string_safe()); @@ -215,7 +215,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } for (auto timer : timer_handles_) { - if (rcl_wait_set_add_timer(wait_set, timer) != RCL_RET_OK) { + if (rcl_wait_set_add_timer(wait_set, timer.get()) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", "Couldn't add timer to wait set: %s", rcl_get_error_string_safe()); @@ -391,10 +391,10 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy VectorRebind guard_conditions_; - VectorRebind subscription_handles_; - VectorRebind service_handles_; - VectorRebind client_handles_; - VectorRebind timer_handles_; + VectorRebind> subscription_handles_; + VectorRebind> service_handles_; + VectorRebind> client_handles_; + VectorRebind> timer_handles_; std::shared_ptr executable_allocator_; std::shared_ptr allocator_; diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index bfbb0fe7aa..91c1248452 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -76,15 +76,15 @@ class SubscriptionBase get_topic_name() const; RCLCPP_PUBLIC - rcl_subscription_t * + std::shared_ptr get_subscription_handle(); RCLCPP_PUBLIC - const rcl_subscription_t * + const std::shared_ptr get_subscription_handle() const; RCLCPP_PUBLIC - virtual const rcl_subscription_t * + virtual const std::shared_ptr get_intra_process_subscription_handle() const; /// Borrow a new message. @@ -110,8 +110,8 @@ class SubscriptionBase const rmw_message_info_t & message_info) = 0; protected: - rcl_subscription_t intra_process_subscription_handle_ = rcl_get_zero_initialized_subscription(); - rcl_subscription_t subscription_handle_ = rcl_get_zero_initialized_subscription(); + std::shared_ptr intra_process_subscription_handle_; + std::shared_ptr subscription_handle_; std::shared_ptr node_handle_; private: @@ -241,7 +241,7 @@ class Subscription : public SubscriptionBase { std::string intra_process_topic_name = std::string(get_topic_name()) + "/_intra"; rcl_ret_t ret = rcl_subscription_init( - &intra_process_subscription_handle_, + intra_process_subscription_handle_.get(), node_handle_.get(), rclcpp::type_support::get_intra_process_message_msg_type_support(), intra_process_topic_name.c_str(), @@ -266,13 +266,13 @@ class Subscription : public SubscriptionBase } /// Implemenation detail. - const rcl_subscription_t * + const std::shared_ptr get_intra_process_subscription_handle() const { if (!get_intra_process_message_callback_) { return nullptr; } - return &intra_process_subscription_handle_; + return intra_process_subscription_handle_; } private: diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index 94cd537a39..6dc9815ec1 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -62,7 +62,7 @@ class TimerBase execute_callback() = 0; RCLCPP_PUBLIC - const rcl_timer_t * + std::shared_ptr get_timer_handle(); /// Check how long the timer has until its next scheduled callback. @@ -85,7 +85,7 @@ class TimerBase bool is_ready(); protected: - rcl_timer_t timer_handle_ = rcl_get_zero_initialized_timer(); + std::shared_ptr timer_handle_; }; @@ -122,15 +122,12 @@ class GenericTimer : public TimerBase { // Stop the timer from running. cancel(); - if (rcl_timer_fini(&timer_handle_) != RCL_RET_OK) { - fprintf(stderr, "Failed to clean up rcl timer handle: %s\n", rcl_get_error_string_safe()); - } } void execute_callback() { - rcl_ret_t ret = rcl_timer_call(&timer_handle_); + rcl_ret_t ret = rcl_timer_call(timer_handle_.get()); if (ret == RCL_RET_TIMER_CANCELED) { return; } diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index d646005b1f..70034b60e0 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -19,6 +19,8 @@ #include #include +#include "rcutils/logging_macros.h" + #include "rcl/graph.h" #include "rcl/node.h" #include "rcl/wait.h" @@ -38,7 +40,20 @@ ClientBase::ClientBase( : node_graph_(node_graph), node_handle_(node_base->get_shared_rcl_node_handle()), service_name_(service_name) -{} +{ + client_handle_ = std::shared_ptr( + new rcl_client_t, [ = ](rcl_client_t * client) + { + if (rcl_client_fini(client, node_handle_.get()) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Error in destruction of rcl client handle: %s", rcl_get_error_string_safe()); + rcl_reset_error(); + delete client; + } + }); + *client_handle_.get() = rcl_get_zero_initialized_client(); +} ClientBase::~ClientBase() {} @@ -48,16 +63,16 @@ ClientBase::get_service_name() const return this->service_name_; } -rcl_client_t * +std::shared_ptr ClientBase::get_client_handle() { - return &client_handle_; + return client_handle_; } -const rcl_client_t * +std::shared_ptr ClientBase::get_client_handle() const { - return &client_handle_; + return client_handle_; } bool @@ -65,7 +80,7 @@ ClientBase::service_is_ready() const { bool is_ready; rcl_ret_t ret = - rcl_service_server_is_available(this->get_rcl_node_handle(), &client_handle_, &is_ready); + rcl_service_server_is_available(this->get_rcl_node_handle(), client_handle_.get(), &is_ready); if (ret != RCL_RET_OK) { throw_from_rcl_error(ret, "rcl_service_server_is_available failed"); } diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index f6a97bf157..ae2a34d4c8 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -280,7 +280,7 @@ Executor::execute_subscription( std::shared_ptr message = subscription->create_message(); rmw_message_info_t message_info; - auto ret = rcl_take(subscription->get_subscription_handle(), + auto ret = rcl_take(subscription->get_subscription_handle().get(), message.get(), &message_info); if (ret == RCL_RET_OK) { message_info.from_intra_process = false; @@ -302,7 +302,7 @@ Executor::execute_intra_process_subscription( rcl_interfaces::msg::IntraProcessMessage ipm; rmw_message_info_t message_info; rcl_ret_t status = rcl_take( - subscription->get_intra_process_subscription_handle(), + subscription->get_intra_process_subscription_handle().get(), &ipm, &message_info); @@ -332,7 +332,7 @@ Executor::execute_service( auto request_header = service->create_request_header(); std::shared_ptr request = service->create_request(); rcl_ret_t status = rcl_take_request( - service->get_service_handle(), + service->get_service_handle().get(), request_header.get(), request.get()); if (status == RCL_RET_OK) { @@ -353,7 +353,7 @@ Executor::execute_client( auto request_header = client->create_request_header(); std::shared_ptr response = client->create_response(); rcl_ret_t status = rcl_take_response( - client->get_client_handle(), + client->get_client_handle().get(), request_header.get(), response.get()); if (status == RCL_RET_OK) { diff --git a/rclcpp/src/rclcpp/memory_strategy.cpp b/rclcpp/src/rclcpp/memory_strategy.cpp index f4efb3f1b2..5ef79a1dd8 100644 --- a/rclcpp/src/rclcpp/memory_strategy.cpp +++ b/rclcpp/src/rclcpp/memory_strategy.cpp @@ -13,12 +13,14 @@ // limitations under the License. #include "rclcpp/memory_strategy.hpp" +#include using rclcpp::memory_strategy::MemoryStrategy; rclcpp::SubscriptionBase::SharedPtr MemoryStrategy::get_subscription_by_handle( - const rcl_subscription_t * subscriber_handle, const WeakNodeVector & weak_nodes) + std::shared_ptr subscriber_handle, + const WeakNodeVector & weak_nodes) { for (auto & weak_node : weak_nodes) { auto node = weak_node.lock(); @@ -48,7 +50,7 @@ MemoryStrategy::get_subscription_by_handle( rclcpp::ServiceBase::SharedPtr MemoryStrategy::get_service_by_handle( - const rcl_service_t * service_handle, + std::shared_ptr service_handle, const WeakNodeVector & weak_nodes) { for (auto & weak_node : weak_nodes) { @@ -74,7 +76,7 @@ MemoryStrategy::get_service_by_handle( rclcpp::ClientBase::SharedPtr MemoryStrategy::get_client_by_handle( - const rcl_client_t * client_handle, + std::shared_ptr client_handle, const WeakNodeVector & weak_nodes) { for (auto & weak_node : weak_nodes) { diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index 441c2f67e6..6ea503080c 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -46,13 +46,13 @@ ServiceBase::get_service_name() return this->service_name_; } -rcl_service_t * +std::shared_ptr ServiceBase::get_service_handle() { return service_handle_; } -const rcl_service_t * +std::shared_ptr ServiceBase::get_service_handle() const { return service_handle_; diff --git a/rclcpp/src/rclcpp/subscription.cpp b/rclcpp/src/rclcpp/subscription.cpp index 79170acdf8..767624931e 100644 --- a/rclcpp/src/rclcpp/subscription.cpp +++ b/rclcpp/src/rclcpp/subscription.cpp @@ -14,6 +14,8 @@ #include "rclcpp/subscription.hpp" +#include + #include #include #include @@ -24,7 +26,6 @@ #include "rmw/error_handling.h" #include "rmw/rmw.h" - using rclcpp::SubscriptionBase; SubscriptionBase::SubscriptionBase( @@ -34,8 +35,28 @@ SubscriptionBase::SubscriptionBase( const rcl_subscription_options_t & subscription_options) : node_handle_(node_handle) { + auto custom_deletor = [ = ](rcl_subscription_t * rcl_subs) + { + if (rcl_subscription_fini(rcl_subs, node_handle_.get()) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Error in destruction of rcl subscription handle: %s", + rcl_get_error_string_safe()); + rcl_reset_error(); + } + delete rcl_subs; + }; + + subscription_handle_ = std::shared_ptr( + new rcl_subscription_t, custom_deletor); + *subscription_handle_.get() = rcl_get_zero_initialized_subscription(); + + intra_process_subscription_handle_ = std::shared_ptr( + new rcl_subscription_t, custom_deletor); + *intra_process_subscription_handle_.get() = rcl_get_zero_initialized_subscription(); + rcl_ret_t ret = rcl_subscription_init( - &subscription_handle_, + subscription_handle_.get(), node_handle_.get(), &type_support_handle, topic_name.c_str(), @@ -57,42 +78,28 @@ SubscriptionBase::SubscriptionBase( SubscriptionBase::~SubscriptionBase() { - if (rcl_subscription_fini(&subscription_handle_, node_handle_.get()) != RCL_RET_OK) { - std::stringstream ss; - ss << "Error in destruction of rcl subscription handle: " << - rcl_get_error_string_safe() << '\n'; - (std::cerr << ss.str()).flush(); - } - if (rcl_subscription_fini( - &intra_process_subscription_handle_, node_handle_.get()) != RCL_RET_OK) - { - std::stringstream ss; - ss << "Error in destruction of rmw intra process subscription handle: " << - rcl_get_error_string_safe() << '\n'; - (std::cerr << ss.str()).flush(); - } } const char * SubscriptionBase::get_topic_name() const { - return rcl_subscription_get_topic_name(&subscription_handle_); + return rcl_subscription_get_topic_name(subscription_handle_.get()); } -rcl_subscription_t * +std::shared_ptr SubscriptionBase::get_subscription_handle() { - return &subscription_handle_; + return subscription_handle_; } -const rcl_subscription_t * +const std::shared_ptr SubscriptionBase::get_subscription_handle() const { - return &subscription_handle_; + return subscription_handle_; } -const rcl_subscription_t * +const std::shared_ptr SubscriptionBase::get_intra_process_subscription_handle() const { - return &intra_process_subscription_handle_; + return intra_process_subscription_handle_; } diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index db03d6b69e..138e19d42f 100644 --- a/rclcpp/src/rclcpp/timer.cpp +++ b/rclcpp/src/rclcpp/timer.cpp @@ -16,16 +16,36 @@ #include #include +#include + +#include "rcutils/logging_macros.h" using rclcpp::TimerBase; TimerBase::TimerBase(std::chrono::nanoseconds period) { + timer_handle_ = std::shared_ptr( + new rcl_timer_t, [ = ](rcl_timer_t * timer) + { + if (rcl_timer_fini(timer) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Failed to clean up rcl timer handle: %s", rcl_get_error_string_safe()); + rcl_reset_error(); + } + delete timer; + }); + + *timer_handle_.get() = rcl_get_zero_initialized_timer(); + if (rcl_timer_init( - &timer_handle_, period.count(), nullptr, + timer_handle_.get(), period.count(), nullptr, rcl_get_default_allocator()) != RCL_RET_OK) { - fprintf(stderr, "Couldn't initialize rcl timer handle: %s\n", rcl_get_error_string_safe()); + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't initialize rcl timer handle: %s\n", rcl_get_error_string_safe()); + rcl_reset_error(); } } @@ -35,7 +55,7 @@ TimerBase::~TimerBase() void TimerBase::cancel() { - if (rcl_timer_cancel(&timer_handle_) != RCL_RET_OK) { + if (rcl_timer_cancel(timer_handle_.get()) != RCL_RET_OK) { throw std::runtime_error(std::string("Couldn't cancel timer: ") + rcl_get_error_string_safe()); } } @@ -43,7 +63,7 @@ TimerBase::cancel() void TimerBase::reset() { - if (rcl_timer_reset(&timer_handle_) != RCL_RET_OK) { + if (rcl_timer_reset(timer_handle_.get()) != RCL_RET_OK) { throw std::runtime_error(std::string("Couldn't reset timer: ") + rcl_get_error_string_safe()); } } @@ -52,7 +72,7 @@ bool TimerBase::is_ready() { bool ready = false; - if (rcl_timer_is_ready(&timer_handle_, &ready) != RCL_RET_OK) { + if (rcl_timer_is_ready(timer_handle_.get(), &ready) != RCL_RET_OK) { throw std::runtime_error(std::string("Failed to check timer: ") + rcl_get_error_string_safe()); } return ready; @@ -62,7 +82,9 @@ std::chrono::nanoseconds TimerBase::time_until_trigger() { int64_t time_until_next_call = 0; - if (rcl_timer_get_time_until_next_call(&timer_handle_, &time_until_next_call) != RCL_RET_OK) { + if (rcl_timer_get_time_until_next_call(timer_handle_.get(), + &time_until_next_call) != RCL_RET_OK) + { throw std::runtime_error( std::string("Timer could not get time until next call: ") + rcl_get_error_string_safe()); @@ -70,8 +92,8 @@ TimerBase::time_until_trigger() return std::chrono::nanoseconds(time_until_next_call); } -const rcl_timer_t * +std::shared_ptr TimerBase::get_timer_handle() { - return &timer_handle_; + return timer_handle_; }