diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index f531079..553c9b5 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -16,10 +16,13 @@ #define RCLCPP_RCLCPP_CLIENT_HPP_ #include +#include #include #include +#include #include +#include #include #include @@ -44,15 +47,20 @@ class ClientBase public: RCLCPP_MAKE_SHARED_DEFINITIONS(ClientBase); - ClientBase(rmw_client_t * client_handle, const std::string & service_name) - : client_handle_(client_handle), service_name_(service_name) + ClientBase( + std::shared_ptr node_handle, + rmw_client_t * client_handle, + const std::string & service_name) + : node_handle_(node_handle), client_handle_(client_handle), service_name_(service_name) {} - ~ClientBase() + virtual ~ClientBase() { - if (client_handle_ != nullptr) { - rmw_destroy_client(client_handle_); - client_handle_ = nullptr; + if (client_handle_) { + if (rmw_destroy_client(client_handle_) != RMW_RET_OK) { + fprintf(stderr, + "Error in destruction of rmw client handle: %s\n", rmw_get_error_string_safe()); + } } } @@ -74,6 +82,8 @@ public: private: RCLCPP_DISABLE_COPY(ClientBase); + std::shared_ptr node_handle_; + rmw_client_t * client_handle_; std::string service_name_; @@ -91,8 +101,11 @@ public: RCLCPP_MAKE_SHARED_DEFINITIONS(Client); - Client(rmw_client_t * client_handle, const std::string & service_name) - : ClientBase(client_handle, service_name) + Client( + std::shared_ptr node_handle, + rmw_client_t * client_handle, + const std::string & service_name) + : ClientBase(node_handle, client_handle, service_name) {} std::shared_ptr create_response() @@ -133,8 +146,12 @@ public: CallbackType cb) { int64_t sequence_number; - // TODO(wjwwood): Check the return code. - rmw_send_request(get_client_handle(), request.get(), &sequence_number); + if (RMW_RET_OK != rmw_send_request(get_client_handle(), request.get(), &sequence_number)) { + // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) + throw std::runtime_error( + std::string("failed to send request: ") + rmw_get_error_string_safe()); + // *INDENT-ON* + } SharedPromise call_promise = std::make_shared(); SharedFuture f(call_promise->get_future()); diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index ad6eb61..b404be0 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -163,8 +163,9 @@ protected: subscription->handle_message(message); } } else { - std::cout << "[rclcpp::error] take failed for subscription on topic: " << - subscription->get_topic_name() << std::endl; + fprintf(stderr, + "[rclcpp::error] take failed for subscription on topic '%s': %s\n", + subscription->get_topic_name().c_str(), rmw_get_error_string_safe()); } } @@ -192,8 +193,9 @@ protected: service->handle_request(request_header, request); } } else { - std::cout << "[rclcpp::error] take failed for service on service: " << - service->get_service_name() << std::endl; + fprintf(stderr, + "[rclcpp::error] take request failed for server of service '%s': %s\n", + service->get_service_name().c_str(), rmw_get_error_string_safe()); } } @@ -204,15 +206,19 @@ protected: std::shared_ptr request_header = client->create_request_header(); std::shared_ptr response = client->create_response(); bool taken = false; - rmw_take_response( + rmw_ret_t status = rmw_take_response( client->client_handle_, request_header.get(), response.get(), &taken); - if (taken) { - client->handle_response(request_header, response); + if (status == RMW_RET_OK) { + if (taken) { + client->handle_response(request_header, response); + } } else { - std::cout << "[rclcpp::error] take failed for service on client" << std::endl; + fprintf(stderr, + "[rclcpp::error] take response failed for client of service '%s': %s\n", + client->get_service_name().c_str(), rmw_get_error_string_safe()); } } diff --git a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp index 11a49f4..40f0c5d 100644 --- a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp @@ -48,7 +48,7 @@ public: } } - ~MultiThreadedExecutor() {} + virtual ~MultiThreadedExecutor() {} void spin() diff --git a/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp index e75007f..fa8e7e0 100644 --- a/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp @@ -43,7 +43,7 @@ public: SingleThreadedExecutor() {} - ~SingleThreadedExecutor() {} + virtual ~SingleThreadedExecutor() {} void spin() { diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index f4b5962..602316a 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -187,7 +187,7 @@ private: std::string name_; - rmw_node_t * node_handle_; + std::shared_ptr node_handle_; rclcpp::context::Context::SharedPtr context_; @@ -226,6 +226,7 @@ private: > typename rclcpp::service::Service::SharedPtr create_service_internal( + std::shared_ptr node_handle, rmw_service_t * service_handle, const std::string & service_name, FunctorT callback) @@ -233,7 +234,7 @@ private: typename rclcpp::service::Service::CallbackType callback_without_header = callback; return service::Service::make_shared( - service_handle, service_name, callback_without_header); + node_handle, service_handle, service_name, callback_without_header); } template< @@ -271,6 +272,7 @@ private: > typename rclcpp::service::Service::SharedPtr create_service_internal( + std::shared_ptr node_handle, rmw_service_t * service_handle, const std::string & service_name, FunctorT callback) @@ -284,7 +286,7 @@ private: typename rclcpp::service::Service::CallbackWithHeaderType callback_with_header = callback; return service::Service::make_shared( - service_handle, service_name, callback_with_header); + node_handle, service_handle, service_name, callback_with_header); } }; diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 2d0aaf0..7e18dae 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -16,7 +16,9 @@ #define RCLCPP_RCLCPP_NODE_IMPL_HPP_ #include +#include #include +#include #include #include @@ -44,17 +46,30 @@ Node::Node(std::string node_name, context::Context::SharedPtr context) : name_(node_name), context_(context), number_of_subscriptions_(0), number_of_timers_(0), number_of_services_(0) { - node_handle_ = rmw_create_node(name_.c_str()); + // Initialize node handle shared_ptr with custom deleter. + node_handle_.reset(rmw_create_node(name_.c_str()), [ = ](rmw_node_t * node) { + if (node_handle_) { + auto ret = rmw_destroy_node(node); + if (ret != RMW_RET_OK) { + // *INDENT-OFF* + std::stringstream ss; + ss << "Error in destruction of rmw node handle: " + << rmw_get_error_string_safe() << '\n'; + // *INDENT-ON* + (std::cerr << ss.str()).flush(); + } + } + }); if (!node_handle_) { - // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) + // *INDENT-OFF* throw std::runtime_error( std::string("could not create node: ") + - (rmw_get_error_string() ? rmw_get_error_string() : "")); + rmw_get_error_string_safe()); // *INDENT-ON* } using rclcpp::callback_group::CallbackGroupType; - default_callback_group_ = \ + default_callback_group_ = create_callback_group(CallbackGroupType::MutuallyExclusive); // TODO(esteve): remove hardcoded values events_publisher_ = @@ -79,16 +94,16 @@ Node::create_publisher(std::string topic_name, size_t queue_size) using rosidl_generator_cpp::get_message_type_support_handle; auto type_support_handle = get_message_type_support_handle(); rmw_publisher_t * publisher_handle = rmw_create_publisher( - node_handle_, type_support_handle, topic_name.c_str(), queue_size); + node_handle_.get(), type_support_handle, topic_name.c_str(), queue_size); if (!publisher_handle) { // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) throw std::runtime_error( std::string("could not create publisher: ") + - (rmw_get_error_string() ? rmw_get_error_string() : "")); + rmw_get_error_string_safe()); // *INDENT-ON* } - return publisher::Publisher::make_shared(publisher_handle); + return publisher::Publisher::make_shared(node_handle_, publisher_handle); } bool @@ -116,18 +131,20 @@ Node::create_subscription( using rosidl_generator_cpp::get_message_type_support_handle; auto type_support_handle = get_message_type_support_handle(); rmw_subscription_t * subscriber_handle = rmw_create_subscription( - node_handle_, type_support_handle, topic_name.c_str(), queue_size, ignore_local_publications); + node_handle_.get(), type_support_handle, + topic_name.c_str(), queue_size, ignore_local_publications); if (!subscriber_handle) { // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) throw std::runtime_error( std::string("could not create subscription: ") + - (rmw_get_error_string() ? rmw_get_error_string() : "")); + rmw_get_error_string_safe()); // *INDENT-ON* } using namespace rclcpp::subscription; auto sub = Subscription::make_shared( + node_handle_, subscriber_handle, topic_name, ignore_local_publications, @@ -189,18 +206,19 @@ Node::create_client( get_service_type_support_handle(); rmw_client_t * client_handle = rmw_create_client( - this->node_handle_, service_type_support_handle, service_name.c_str()); + this->node_handle_.get(), service_type_support_handle, service_name.c_str()); if (!client_handle) { // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) throw std::runtime_error( std::string("could not create client: ") + - (rmw_get_error_string() ? rmw_get_error_string() : "")); + rmw_get_error_string_safe()); // *INDENT-ON* } using namespace rclcpp::client; auto cli = Client::make_shared( + node_handle_, client_handle, service_name); @@ -231,17 +249,17 @@ Node::create_service( get_service_type_support_handle(); rmw_service_t * service_handle = rmw_create_service( - this->node_handle_, service_type_support_handle, service_name.c_str()); + node_handle_.get(), service_type_support_handle, service_name.c_str()); if (!service_handle) { // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) throw std::runtime_error( std::string("could not create service: ") + - (rmw_get_error_string() ? rmw_get_error_string() : "")); + rmw_get_error_string_safe()); // *INDENT-ON* } auto serv = create_service_internal( - service_handle, service_name, callback); + node_handle_, service_handle, service_name, callback); auto serv_base_ptr = std::dynamic_pointer_cast(serv); if (group) { if (!group_in_node(group)) { diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 7ae9450..b31b2dc 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -15,8 +15,11 @@ #ifndef RCLCPP_RCLCPP_PUBLISHER_HPP_ #define RCLCPP_RCLCPP_PUBLISHER_HPP_ +#include #include +#include +#include #include #include @@ -38,18 +41,40 @@ class Publisher public: RCLCPP_MAKE_SHARED_DEFINITIONS(Publisher); - Publisher(rmw_publisher_t * publisher_handle) - : publisher_handle_(publisher_handle) + Publisher(std::shared_ptr node_handle, rmw_publisher_t * publisher_handle) + : node_handle_(node_handle), publisher_handle_(publisher_handle) {} + virtual ~Publisher() + { + if (publisher_handle_) { + if (rmw_destroy_publisher(node_handle_.get(), publisher_handle_) != RMW_RET_OK) { + // *INDENT-OFF* + std::stringstream ss; + ss << "Error in destruction of rmw publisher handle: " + << rmw_get_error_string_safe() << '\n'; + // *INDENT-ON* + (std::cerr << ss.str()).flush(); + } + } + } + template void publish(std::shared_ptr & msg) { - rmw_publish(publisher_handle_, msg.get()); + rmw_ret_t status = rmw_publish(publisher_handle_, msg.get()); + if (status != RMW_RET_OK) { + // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) + throw std::runtime_error( + std::string("failed to publish message: ") + rmw_get_error_string_safe()); + // *INDENT-ON* + } } private: + std::shared_ptr node_handle_; + rmw_publisher_t * publisher_handle_; }; diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index ed345a3..6c756c1 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -16,9 +16,12 @@ #define RCLCPP_RCLCPP_SERVICE_HPP_ #include +#include #include +#include #include +#include #include #include @@ -44,16 +47,21 @@ public: RCLCPP_MAKE_SHARED_DEFINITIONS(ServiceBase); ServiceBase( + std::shared_ptr node_handle, rmw_service_t * service_handle, const std::string service_name) - : service_handle_(service_handle), service_name_(service_name) + : node_handle_(node_handle), service_handle_(service_handle), service_name_(service_name) {} - ~ServiceBase() + virtual ~ServiceBase() { - if (service_handle_ != nullptr) { - rmw_destroy_service(service_handle_); - service_handle_ = nullptr; + if (service_handle_) { + if (rmw_destroy_service(service_handle_) != RMW_RET_OK) { + std::stringstream ss; + ss << "Error in destruction of rmw service_handle_ handle: " << + rmw_get_error_string_safe() << '\n'; + (std::cerr << ss.str()).flush(); + } } } @@ -76,6 +84,8 @@ public: private: RCLCPP_DISABLE_COPY(ServiceBase); + std::shared_ptr node_handle_; + rmw_service_t * service_handle_; std::string service_name_; @@ -98,17 +108,20 @@ public: RCLCPP_MAKE_SHARED_DEFINITIONS(Service); Service( + std::shared_ptr node_handle, rmw_service_t * service_handle, const std::string & service_name, CallbackType callback) - : ServiceBase(service_handle, service_name), callback_(callback), callback_with_header_(nullptr) + : ServiceBase(node_handle, service_handle, service_name), callback_(callback), + callback_with_header_(nullptr) {} Service( + std::shared_ptr node_handle, rmw_service_t * service_handle, const std::string & service_name, CallbackWithHeaderType callback_with_header) - : ServiceBase(service_handle, service_name), callback_(nullptr), + : ServiceBase(node_handle, service_handle, service_name), callback_(nullptr), callback_with_header_(callback_with_header) {} @@ -141,7 +154,13 @@ public: std::shared_ptr & req_id, std::shared_ptr & response) { - rmw_send_response(get_service_handle(), req_id.get(), response.get()); + rmw_ret_t status = rmw_send_response(get_service_handle(), req_id.get(), response.get()); + if (status != RMW_RET_OK) { + // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) + throw std::runtime_error( + std::string("failed to send response: ") + rmw_get_error_string_safe()); + // *INDENT-ON* + } } private: diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index b93dbd2..5aa0878 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -16,9 +16,12 @@ #define RCLCPP_RCLCPP_SUBSCRIPTION_HPP_ #include +#include #include +#include #include +#include #include #include @@ -43,13 +46,30 @@ public: RCLCPP_MAKE_SHARED_DEFINITIONS(SubscriptionBase); SubscriptionBase( + std::shared_ptr node_handle, rmw_subscription_t * subscription_handle, std::string & topic_name, bool ignore_local_publications) - : subscription_handle_(subscription_handle), + : node_handle_(node_handle), + subscription_handle_(subscription_handle), topic_name_(topic_name), ignore_local_publications_(ignore_local_publications) - {} + { + // To avoid unused private member warnings. + (void)ignore_local_publications_; + } + + virtual ~SubscriptionBase() + { + if (subscription_handle_) { + if (rmw_destroy_subscription(node_handle_.get(), subscription_handle_) != RMW_RET_OK) { + std::stringstream ss; + ss << "Error in destruction of rmw subscription handle: " << + rmw_get_error_string_safe() << '\n'; + (std::cerr << ss.str()).flush(); + } + } + } std::string get_topic_name() { @@ -62,6 +82,8 @@ public: private: RCLCPP_DISABLE_COPY(SubscriptionBase); + std::shared_ptr node_handle_; + rmw_subscription_t * subscription_handle_; std::string topic_name_; bool ignore_local_publications_; @@ -76,11 +98,12 @@ public: RCLCPP_MAKE_SHARED_DEFINITIONS(Subscription); Subscription( + std::shared_ptr node_handle, rmw_subscription_t * subscription_handle, std::string & topic_name, bool ignore_local_publications, CallbackType callback) - : SubscriptionBase(subscription_handle, topic_name, ignore_local_publications), + : SubscriptionBase(node_handle, subscription_handle, topic_name, ignore_local_publications), callback_(callback) {} diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index d378669..6b7c969 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -18,8 +18,10 @@ #include #include #include +#include #include +#include #include #include @@ -50,9 +52,28 @@ public: TimerBase(std::chrono::nanoseconds period, CallbackType callback) : period_(period), callback_(callback), + guard_condition_(rmw_create_guard_condition()), canceled_(false) { - guard_condition_ = rmw_create_guard_condition(); + if (!guard_condition_) { + // TODO(wjwwood): use custom exception + throw std::runtime_error( + std::string("failed to create guard condition: ") + + rmw_get_error_string_safe() + ); + } + } + + virtual ~TimerBase() + { + if (guard_condition_) { + if (rmw_destroy_guard_condition(guard_condition_) != RMW_RET_OK) { + std::stringstream ss; + ss << "Error in TimerBase destructor, rmw_destroy_guard_condition failed: " << + rmw_get_error_string_safe() << '\n'; + (std::cerr << ss.str()).flush(); + } + } } void @@ -86,9 +107,10 @@ public: thread_ = std::thread(&GenericTimer::run, this); } - ~GenericTimer() + virtual ~GenericTimer() { cancel(); + thread_.join(); } void