From ea81bdedff79e469d68dd3d63b694bbc50d18889 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Wed, 23 Mar 2016 15:14:39 -0700 Subject: [PATCH 01/49] notify guard condition cleanup --- rclcpp/src/rclcpp/node.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index df4ab3d9fd..da49a5002f 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -75,6 +75,7 @@ Node::Node( fprintf( stderr, "Failed to destroy notify guard condition: %s\n", rmw_get_error_string_safe()); } + throw std::runtime_error( std::string("could not create node: ") + rmw_get_error_string_safe()); From bf227d6c9fe35f25e01d0327936d2feacd1cd07e Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Thu, 24 Mar 2016 17:10:30 -0700 Subject: [PATCH 02/49] Warnings --- rclcpp/src/rclcpp/node.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index da49a5002f..df4ab3d9fd 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -75,7 +75,6 @@ Node::Node( fprintf( stderr, "Failed to destroy notify guard condition: %s\n", rmw_get_error_string_safe()); } - throw std::runtime_error( std::string("could not create node: ") + rmw_get_error_string_safe()); From 73bc7dd3ba676aabd569e94157a871098b25fdf0 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Fri, 25 Mar 2016 11:42:18 -0700 Subject: [PATCH 03/49] Clean up guard conditions properly --- rclcpp/src/rclcpp/executor.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 041bee77e2..557cdbbfda 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -406,6 +406,10 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) if (status != RMW_RET_OK && status != RMW_RET_TIMEOUT) { throw std::runtime_error(rmw_get_error_string_safe()); } + guard_cond_handles_.erase( + std::remove(guard_cond_handles_.begin(), guard_cond_handles_.end(), nullptr), + guard_cond_handles_.end() + ); memory_strategy_->remove_null_handles(); } From 93ba85d87bf8b92381628f3ecf343081af116038 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Fri, 25 Mar 2016 12:16:11 -0700 Subject: [PATCH 04/49] Temporary storage --- rclcpp/src/rclcpp/executor.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 557cdbbfda..eca5e091d0 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -406,10 +406,7 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) if (status != RMW_RET_OK && status != RMW_RET_TIMEOUT) { throw std::runtime_error(rmw_get_error_string_safe()); } - guard_cond_handles_.erase( - std::remove(guard_cond_handles_.begin(), guard_cond_handles_.end(), nullptr), - guard_cond_handles_.end() - ); + // For now we don't do anything with the null guard handles. memory_strategy_->remove_null_handles(); } From 0d16715676a39a686def0887215129006ebfb26c Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Fri, 25 Mar 2016 18:12:54 -0700 Subject: [PATCH 05/49] Working towards getting that tlsf test passing again --- rclcpp/src/rclcpp/executor.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index eca5e091d0..041bee77e2 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -406,7 +406,6 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) if (status != RMW_RET_OK && status != RMW_RET_TIMEOUT) { throw std::runtime_error(rmw_get_error_string_safe()); } - // For now we don't do anything with the null guard handles. memory_strategy_->remove_null_handles(); } From e0717f20414dcc7fa36fb760c576d5a5da67448c Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Thu, 10 Mar 2016 11:39:32 -0800 Subject: [PATCH 06/49] pub/sub mostly --- rclcpp/CMakeLists.txt | 2 + rclcpp/include/rclcpp/node.hpp | 13 +++-- rclcpp/include/rclcpp/node_impl.hpp | 73 ++++++++++++++++---------- rclcpp/include/rclcpp/publisher.hpp | 31 ++++++----- rclcpp/include/rclcpp/subscription.hpp | 28 +++++----- rclcpp/package.xml | 1 + rclcpp/src/rclcpp/executor.cpp | 36 ++++++------- rclcpp/src/rclcpp/node.cpp | 29 +++++----- rclcpp/src/rclcpp/publisher.cpp | 32 ++++++----- rclcpp/src/rclcpp/subscription.cpp | 24 +++++---- 10 files changed, 154 insertions(+), 115 deletions(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index d5bee53a19..986c080184 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -3,6 +3,7 @@ cmake_minimum_required(VERSION 2.8.3) project(rclcpp) find_package(ament_cmake REQUIRED) +find_package(rcl REQUIRED) find_package(rcl_interfaces REQUIRED) find_package(rmw REQUIRED) find_package(rmw_implementation REQUIRED) @@ -45,6 +46,7 @@ macro(target) add_library(${PROJECT_NAME}${target_suffix} SHARED ${${PROJECT_NAME}_SRCS}) ament_target_dependencies(${PROJECT_NAME}${target_suffix} + "rcl" "rcl_interfaces" "rmw" "rosidl_generator_cpp" diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 3f9eaf038a..edb99a9f21 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -22,6 +22,9 @@ #include #include +#include +#include + #include "rcl_interfaces/msg/list_parameters_result.hpp" #include "rcl_interfaces/msg/parameter_descriptor.hpp" #include "rcl_interfaces/msg/parameter_event.hpp" @@ -39,11 +42,10 @@ #include "rclcpp/timer.hpp" #include "rclcpp/visibility_control.hpp" -// Forward declaration of ROS middleware class -namespace rmw +namespace rcl { -struct rmw_node_t; -} // namespace rmw +struct rcl_node_t; +} // namespace rcl namespace rclcpp { @@ -269,7 +271,8 @@ class Node std::string name_; - std::shared_ptr node_handle_; + //std::shared_ptr node_handle_; + std::shared_ptr node_handle_; rclcpp::context::Context::SharedPtr context_; diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 7148571abb..9998137553 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -30,6 +30,9 @@ #include #include +#include +#include + #include "rcl_interfaces/msg/intra_process_message.hpp" #include "rclcpp/contexts/default_context.hpp" @@ -72,29 +75,34 @@ Node::create_publisher( } 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_.get(), type_support_handle, topic_name.c_str(), &qos_profile); - if (!publisher_handle) { - // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) + + rcl_publisher_t * publisher_handle = new rcl_publisher_t; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + publisher_options.qos = qos_profile; + if (rcl_publisher_init( + publisher_handle, node_handle_.get(), type_support_handle, + topic_name.c_str(), &publisher_options) != RCL_RET_OK) + { throw std::runtime_error( std::string("could not create publisher: ") + - rmw_get_error_string_safe()); - // *INDENT-ON* + rcl_get_error_string_safe()); } auto publisher = publisher::Publisher::make_shared( node_handle_, publisher_handle, topic_name, qos_profile.depth, allocator); if (use_intra_process_comms_) { - rmw_publisher_t * intra_process_publisher_handle = rmw_create_publisher( - node_handle_.get(), rclcpp::type_support::get_intra_process_message_msg_type_support(), - (topic_name + "__intra").c_str(), &qos_profile); - if (!intra_process_publisher_handle) { - // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) + rcl_publisher_t * intra_process_publisher_handle = new rcl_publisher_t; + rcl_publisher_options_t intra_process_publisher_options = rcl_publisher_get_default_options(); + intra_process_publisher_options.qos = qos_profile; + if (rcl_publisher_init( + intra_process_publisher_handle, node_handle_.get(), + rclcpp::type_support::get_intra_process_message_msg_type_support(), + (topic_name + "__intra").c_str(), &intra_process_publisher_options) != RCL_RET_OK) + { throw std::runtime_error( std::string("could not create intra process publisher: ") + - rmw_get_error_string_safe()); - // *INDENT-ON* + rcl_get_error_string_safe()); } auto intra_process_manager = @@ -170,14 +178,17 @@ Node::create_subscription( } auto type_support_handle = get_message_type_support_handle(); - rmw_subscription_t * subscriber_handle = rmw_create_subscription( - node_handle_.get(), type_support_handle, - topic_name.c_str(), &qos_profile, ignore_local_publications); - if (!subscriber_handle) { - // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) + // TODO Allocator + rcl_subscription_t * subscriber_handle = new rcl_subscription_t(); + auto subscription_options = rcl_subscription_get_default_options(); + subscription_options.qos = qos_profile; + subscription_options.ignore_local_publications = ignore_local_publications; + if (rcl_subscription_init( + subscriber_handle, node_handle_.get(), type_support_handle, topic_name.c_str(), + &subscription_options) != RCL_RET_OK) + { throw std::runtime_error( - std::string("could not create subscription: ") + rmw_get_error_string_safe()); - // *INDENT-ON* + std::string("could not create subscription: ") + rcl_get_error_string_safe()); } using rclcpp::subscription::Subscription; @@ -193,13 +204,19 @@ Node::create_subscription( auto sub_base_ptr = std::dynamic_pointer_cast(sub); // Setup intra process. if (use_intra_process_comms_) { - rmw_subscription_t * intra_process_subscriber_handle = rmw_create_subscription( - node_handle_.get(), rclcpp::type_support::get_intra_process_message_msg_type_support(), - (topic_name + "__intra").c_str(), &qos_profile, false); - if (!subscriber_handle) { + rcl_subscription_t * intra_process_subscriber_handle = new rcl_subscription_t; + auto intra_process_subscription_options = rcl_subscription_get_default_options(); + intra_process_subscription_options.qos = qos_profile; + intra_process_subscription_options.ignore_local_publications = false; + if (rcl_subscription_init( + subscriber_handle, node_handle_.get(), + rclcpp::type_support::get_intra_process_message_msg_type_support(), + (topic_name + "__intra").c_str(), + &subscription_options) != RCL_RET_OK) + { // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) throw std::runtime_error( - std::string("could not create intra process subscription: ") + rmw_get_error_string_safe()); + std::string("could not create intra process subscription: ") + rcl_get_error_string_safe()); // *INDENT-ON* } auto intra_process_manager = @@ -319,7 +336,8 @@ Node::create_client( get_service_type_support_handle(); rmw_client_t * client_handle = rmw_create_client( - this->node_handle_.get(), service_type_support_handle, service_name.c_str(), &qos_profile); + rcl_node_get_rmw_handle(this->node_handle_.get()), + service_type_support_handle, service_name.c_str(), &qos_profile); if (!client_handle) { // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) throw std::runtime_error( @@ -372,7 +390,8 @@ Node::create_service( any_service_callback.set(std::forward(callback)); rmw_service_t * service_handle = rmw_create_service( - node_handle_.get(), service_type_support_handle, service_name.c_str(), &qos_profile); + rcl_node_get_rmw_handle(node_handle_.get()), + service_type_support_handle, service_name.c_str(), &qos_profile); if (!service_handle) { // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) throw std::runtime_error( diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 7ec46e8ada..623ecf2609 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -24,6 +24,9 @@ #include #include +#include +#include + #include "rcl_interfaces/msg/intra_process_message.hpp" #include "rmw/impl/cpp/demangle.hpp" @@ -59,8 +62,8 @@ class PublisherBase */ RCLCPP_PUBLIC PublisherBase( - std::shared_ptr node_handle, - rmw_publisher_t * publisher_handle, + std::shared_ptr node_handle, + rcl_publisher_t * publisher_handle, std::string topic, size_t queue_size); @@ -120,12 +123,12 @@ class PublisherBase setup_intra_process( uint64_t intra_process_publisher_id, StoreMessageCallbackT callback, - rmw_publisher_t * intra_process_publisher_handle); + rcl_publisher_t * intra_process_publisher_handle); - std::shared_ptr node_handle_; + std::shared_ptr node_handle_; - rmw_publisher_t * publisher_handle_; - rmw_publisher_t * intra_process_publisher_handle_; + rcl_publisher_t * publisher_handle_; + rcl_publisher_t * intra_process_publisher_handle_; std::string topic_; size_t queue_size_; @@ -152,8 +155,8 @@ class Publisher : public PublisherBase RCLCPP_SMART_PTR_DEFINITIONS(Publisher); Publisher( - std::shared_ptr node_handle, - rmw_publisher_t * publisher_handle, + std::shared_ptr node_handle, + rcl_publisher_t * publisher_handle, std::string topic, size_t queue_size, std::shared_ptr allocator) @@ -188,11 +191,11 @@ class Publisher : public PublisherBase rcl_interfaces::msg::IntraProcessMessage ipm; ipm.publisher_id = intra_process_publisher_id_; ipm.message_sequence = message_seq; - auto status = rmw_publish(intra_process_publisher_handle_, &ipm); - if (status != RMW_RET_OK) { + auto status = rcl_publish(intra_process_publisher_handle_, &ipm); + if (status != RCL_RET_OK) { // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) throw std::runtime_error( - std::string("failed to publish intra process message: ") + rmw_get_error_string_safe()); + std::string("failed to publish intra process message: ") + rcl_get_error_string_safe()); // *INDENT-ON* } } else { @@ -263,11 +266,11 @@ class Publisher : public PublisherBase void do_inter_process_publish(const MessageT * msg) { - auto status = rmw_publish(publisher_handle_, msg); - if (status != RMW_RET_OK) { + auto status = rcl_publish(publisher_handle_, msg); + if (status != RCL_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()); + std::string("failed to publish message: ") + rcl_get_error_string_safe()); // *INDENT-ON* } } diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 0044b205c3..f2435ddd87 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -1,5 +1,4 @@ // Copyright 2014 Open Source Robotics Foundation, Inc. -// // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at @@ -24,6 +23,9 @@ #include #include +#include +#include + #include "rcl_interfaces/msg/intra_process_message.hpp" #include "rclcpp/macros.hpp" @@ -51,14 +53,14 @@ class SubscriptionBase /// Default constructor. /** - * \param[in] node_handle The rmw representation of the node that owns this subscription. + * \param[in] node_handle The rcl representation of the node that owns this subscription. * \param[in] topic_name Name of the topic to subscribe to. * \param[in] ignore_local_publications True to ignore local publications (unused). */ RCLCPP_PUBLIC SubscriptionBase( - std::shared_ptr node_handle, - rmw_subscription_t * subscription_handle, + std::shared_ptr node_handle, + rcl_subscription_t * subscription_handle, const std::string & topic_name, bool ignore_local_publications); @@ -72,11 +74,11 @@ class SubscriptionBase get_topic_name() const; RCLCPP_PUBLIC - const rmw_subscription_t * + const rcl_subscription_t * get_subscription_handle() const; RCLCPP_PUBLIC - const rmw_subscription_t * + const rcl_subscription_t * get_intra_process_subscription_handle() const; /// Borrow a new message. @@ -101,14 +103,14 @@ class SubscriptionBase const rmw_message_info_t & message_info) = 0; protected: - rmw_subscription_t * intra_process_subscription_handle_; + rcl_subscription_t * intra_process_subscription_handle_; private: RCLCPP_DISABLE_COPY(SubscriptionBase); - std::shared_ptr node_handle_; + std::shared_ptr node_handle_; - rmw_subscription_t * subscription_handle_; + rcl_subscription_t * subscription_handle_; std::string topic_name_; bool ignore_local_publications_; @@ -134,15 +136,15 @@ class Subscription : public SubscriptionBase /** * The constructor for a subscription is almost never called directly. Instead, subscriptions * should be instantiated through Node::create_subscription. - * \param[in] node_handle rmw representation of the node that owns this subscription. + * \param[in] node_handle rcl representation of the node that owns this subscription. * \param[in] topic_name Name of the topic to subscribe to. * \param[in] ignore_local_publications True to ignore local publications (unused). * \param[in] callback User-defined callback to call when a message is received. * \param[in] memory_strategy The memory strategy to be used for managing message memory. */ Subscription( - std::shared_ptr node_handle, - rmw_subscription_t * subscription_handle, + std::shared_ptr node_handle, + rcl_subscription_t * subscription_handle, const std::string & topic_name, bool ignore_local_publications, AnySubscriptionCallback callback, @@ -231,7 +233,7 @@ class Subscription : public SubscriptionBase void setup_intra_process( uint64_t intra_process_subscription_id, - rmw_subscription_t * intra_process_subscription, + rcl_subscription_t * intra_process_subscription, GetMessageCallbackType get_message_callback, MatchesAnyPublishersCallbackType matches_any_publisher_callback) { diff --git a/rclcpp/package.xml b/rclcpp/package.xml index db04382dc8..b17862c1f5 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -10,6 +10,7 @@ rmw + rcl rcl_interfaces rmw_implementation_cmake rosidl_generator_cpp diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 041bee77e2..94da488957 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -235,20 +235,17 @@ Executor::execute_subscription( rclcpp::subscription::SubscriptionBase::SharedPtr subscription) { std::shared_ptr message = subscription->create_message(); - bool taken = false; rmw_message_info_t message_info; - auto ret = - rmw_take_with_info(subscription->get_subscription_handle(), - message.get(), &taken, &message_info); - if (ret == RMW_RET_OK) { - if (taken) { - message_info.from_intra_process = false; - subscription->handle_message(message, message_info); - } - } else { + + auto ret = rcl_take(subscription->get_subscription_handle(), + message.get(), &message_info); + if (ret == RCL_RET_OK) { + message_info.from_intra_process = false; + subscription->handle_message(message, message_info); + } else if (ret != RCL_RET_SUBSCRIPTION_TAKE_FAILED) { fprintf(stderr, "[rclcpp::error] take failed for subscription on topic '%s': %s\n", - subscription->get_topic_name().c_str(), rmw_get_error_string_safe()); + subscription->get_topic_name().c_str(), rcl_get_error_string_safe()); } subscription->return_message(message); } @@ -258,22 +255,19 @@ Executor::execute_intra_process_subscription( rclcpp::subscription::SubscriptionBase::SharedPtr subscription) { rcl_interfaces::msg::IntraProcessMessage ipm; - bool taken = false; rmw_message_info_t message_info; - rmw_ret_t status = rmw_take_with_info( + rcl_ret_t status = rcl_take( subscription->get_intra_process_subscription_handle(), &ipm, - &taken, &message_info); - if (status == RMW_RET_OK) { - if (taken) { - message_info.from_intra_process = true; - subscription->handle_intra_process_message(ipm, message_info); - } - } else { + + if (status == RCL_RET_OK) { + message_info.from_intra_process = true; + subscription->handle_intra_process_message(ipm, message_info); + } else if (status != RCL_RET_SUBSCRIPTION_TAKE_FAILED) { fprintf(stderr, "[rclcpp::error] take failed for intra process subscription on topic '%s': %s\n", - subscription->get_topic_name().c_str(), rmw_get_error_string_safe()); + subscription->get_topic_name().c_str(), rcl_get_error_string_safe()); } } diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index df4ab3d9fd..981ff71941 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -68,25 +68,25 @@ Node::Node( #endif } - auto node = rmw_create_node(name_.c_str(), domain_id); - if (!node) { - // *INDENT-OFF* + rcl_node_t node = rcl_get_zero_initialized_node(); + rcl_node_options_t options; + // TODO(jacquelinekay): Allocator options + options.domaind_id = domain_id; + if (rcl_node_init(&node, name_.c_str(), &options) != RCL_RET_OK) { if (rmw_destroy_guard_condition(notify_guard_condition_) != RMW_RET_OK) { fprintf( stderr, "Failed to destroy notify guard condition: %s\n", rmw_get_error_string_safe()); } - throw std::runtime_error( - std::string("could not create node: ") + - rmw_get_error_string_safe()); - // *INDENT-ON* + throw std::runtime_error("Could not initialize rcl node"); } + // Initialize node handle shared_ptr with custom deleter. // *INDENT-OFF* - node_handle_.reset(node, [](rmw_node_t * node) { - auto ret = rmw_destroy_node(node); + node_handle_.reset(&node, [](rcl_node_t * node) { + auto ret = rcl_fini_node(node); if (ret != RMW_RET_OK) { fprintf( - stderr, "Error in destruction of rmw node handle: %s\n", rmw_get_error_string_safe()); + stderr, "Error in destruction of rmw node handle: %s\n", rcl_get_error_string_safe()); } }); // *INDENT-ON* @@ -297,7 +297,8 @@ Node::get_topic_names_and_types() const topic_names_and_types.topic_names = nullptr; topic_names_and_types.type_names = nullptr; - auto ret = rmw_get_topic_names_and_types(node_handle_.get(), &topic_names_and_types); + auto ret = rmw_get_topic_names_and_types(rcl_node_get_rmw_handle(node_handle_.get()), + &topic_names_and_types); if (ret != RMW_RET_OK) { // *INDENT-OFF* throw std::runtime_error( @@ -325,7 +326,8 @@ size_t Node::count_publishers(const std::string & topic_name) const { size_t count; - auto ret = rmw_count_publishers(node_handle_.get(), topic_name.c_str(), &count); + auto ret = rmw_count_publishers(rcl_node_get_rmw_handle(node_handle_.get()), + topic_name.c_str(), &count); if (ret != RMW_RET_OK) { // *INDENT-OFF* throw std::runtime_error( @@ -339,7 +341,8 @@ size_t Node::count_subscribers(const std::string & topic_name) const { size_t count; - auto ret = rmw_count_subscribers(node_handle_.get(), topic_name.c_str(), &count); + auto ret = rmw_count_subscribers(rcl_node_get_rmw_handle(node_handle_.get()), + topic_name.c_str(), &count); if (ret != RMW_RET_OK) { // *INDENT-OFF* throw std::runtime_error( diff --git a/rclcpp/src/rclcpp/publisher.cpp b/rclcpp/src/rclcpp/publisher.cpp index 4ad9b87561..662581edf8 100644 --- a/rclcpp/src/rclcpp/publisher.cpp +++ b/rclcpp/src/rclcpp/publisher.cpp @@ -33,8 +33,8 @@ using rclcpp::publisher::PublisherBase; PublisherBase::PublisherBase( - std::shared_ptr node_handle, - rmw_publisher_t * publisher_handle, + std::shared_ptr node_handle, + rcl_publisher_t * publisher_handle, std::string topic, size_t queue_size) : node_handle_(node_handle), publisher_handle_(publisher_handle), @@ -43,7 +43,9 @@ PublisherBase::PublisherBase( intra_process_publisher_id_(0), store_intra_process_message_(nullptr) { // Life time of this object is tied to the publisher handle. - if (rmw_get_gid_for_publisher(publisher_handle_, &rmw_gid_) != RMW_RET_OK) { + if (rmw_get_gid_for_publisher( + rcl_publisher_get_rmw_handle(publisher_handle_), &rmw_gid_) != RMW_RET_OK) + { // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) throw std::runtime_error( std::string("failed to get publisher gid: ") + rmw_get_error_string_safe()); @@ -54,21 +56,26 @@ PublisherBase::PublisherBase( PublisherBase::~PublisherBase() { if (intra_process_publisher_handle_) { - if (rmw_destroy_publisher(node_handle_.get(), intra_process_publisher_handle_)) { + if (rcl_publisher_fini(intra_process_publisher_handle_, node_handle_.get()) != RCL_RET_OK) { fprintf( stderr, - "Error in destruction of intra process rmw publisher handle: %s\n", - rmw_get_error_string_safe()); + "Error in destruction of intra process rcl publisher handle: %s\n", + rcl_get_error_string_safe()); + } + // TODO custom deallocator + delete intra_process_publisher_handle_; } if (publisher_handle_) { - if (rmw_destroy_publisher(node_handle_.get(), publisher_handle_) != RMW_RET_OK) { + if (rcl_publisher_fini(publisher_handle_, node_handle_.get()) != RCL_RET_OK) { fprintf( stderr, - "Error in destruction of rmw publisher handle: %s\n", - rmw_get_error_string_safe()); + "Error in destruction of rcl publisher handle: %s\n", + rcl_get_error_string_safe()); + } - } + // TODO custom deallocator + delete publisher_handle_; } } const std::string & @@ -124,13 +131,14 @@ void PublisherBase::setup_intra_process( uint64_t intra_process_publisher_id, StoreMessageCallbackT callback, - rmw_publisher_t * intra_process_publisher_handle) + rcl_publisher_t * intra_process_publisher_handle) { intra_process_publisher_id_ = intra_process_publisher_id; store_intra_process_message_ = callback; intra_process_publisher_handle_ = intra_process_publisher_handle; // Life time of this object is tied to the publisher handle. - auto ret = rmw_get_gid_for_publisher(intra_process_publisher_handle_, &intra_process_rmw_gid_); + auto ret = rmw_get_gid_for_publisher( + rcl_publisher_get_rmw_handle(intra_process_publisher_handle_), &intra_process_rmw_gid_); if (ret != RMW_RET_OK) { // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) throw std::runtime_error( diff --git a/rclcpp/src/rclcpp/subscription.cpp b/rclcpp/src/rclcpp/subscription.cpp index caf6578afe..6f2413e2cd 100644 --- a/rclcpp/src/rclcpp/subscription.cpp +++ b/rclcpp/src/rclcpp/subscription.cpp @@ -24,8 +24,8 @@ using rclcpp::subscription::SubscriptionBase; SubscriptionBase::SubscriptionBase( - std::shared_ptr node_handle, - rmw_subscription_t * subscription_handle, + std::shared_ptr node_handle, + rcl_subscription_t * subscription_handle, const std::string & topic_name, bool ignore_local_publications) : intra_process_subscription_handle_(nullptr), @@ -41,22 +41,26 @@ SubscriptionBase::SubscriptionBase( SubscriptionBase::~SubscriptionBase() { if (subscription_handle_) { - if (rmw_destroy_subscription(node_handle_.get(), subscription_handle_) != RMW_RET_OK) { + // TODO + if (rcl_subscription_fini(subscription_handle_, node_handle_.get()) != RMW_RET_OK) { std::stringstream ss; - ss << "Error in destruction of rmw subscription handle: " << - rmw_get_error_string_safe() << '\n'; + ss << "Error in destruction of rcl subscription handle: " << + rcl_get_error_string_safe() << '\n'; (std::cerr << ss.str()).flush(); } } + // TODO: deallocate with allocator + delete subscription_handle_; if (intra_process_subscription_handle_) { - auto ret = rmw_destroy_subscription(node_handle_.get(), intra_process_subscription_handle_); - if (ret != RMW_RET_OK) { + auto ret = rcl_subscription_fini(intra_process_subscription_handle_, node_handle_.get()); + if (ret != RCL_RET_OK) { std::stringstream ss; ss << "Error in destruction of rmw intra process subscription handle: " << - rmw_get_error_string_safe() << '\n'; + rcl_get_error_string_safe() << '\n'; (std::cerr << ss.str()).flush(); } } + delete intra_process_subscription_handle_; } const std::string & @@ -65,13 +69,13 @@ SubscriptionBase::get_topic_name() const return this->topic_name_; } -const rmw_subscription_t * +const rcl_subscription_t * SubscriptionBase::get_subscription_handle() const { return subscription_handle_; } -const rmw_subscription_t * +const rcl_subscription_t * SubscriptionBase::get_intra_process_subscription_handle() const { return intra_process_subscription_handle_; From 87adf80a162b22c1b8172b9c349756931f8194cc Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Thu, 10 Mar 2016 12:17:12 -0800 Subject: [PATCH 07/49] Starting client/service --- rclcpp/include/rclcpp/client.hpp | 16 +++++++++------- rclcpp/include/rclcpp/node_impl.hpp | 14 ++++++++------ rclcpp/include/rclcpp/service.hpp | 21 ++++++++++++--------- rclcpp/src/rclcpp/service.cpp | 9 +++++---- 4 files changed, 34 insertions(+), 26 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index ad176dfc11..c7d8139447 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -23,6 +23,8 @@ #include #include +#include + #include "rclcpp/function_traits.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/utilities.hpp" @@ -42,8 +44,8 @@ class ClientBase RCLCPP_PUBLIC ClientBase( - std::shared_ptr node_handle, - rmw_client_t * client_handle, + std::shared_ptr node_handle, + rcl_client_t * client_handle, const std::string & service_name); RCLCPP_PUBLIC @@ -54,7 +56,7 @@ class ClientBase get_service_name() const; RCLCPP_PUBLIC - const rmw_client_t * + const rcl_client_t * get_client_handle() const; virtual std::shared_ptr create_response() = 0; @@ -65,9 +67,9 @@ class ClientBase private: RCLCPP_DISABLE_COPY(ClientBase); - std::shared_ptr node_handle_; + std::shared_ptr node_handle_; - rmw_client_t * client_handle_; + rcl_client_t * client_handle_; std::string service_name_; }; @@ -93,8 +95,8 @@ class Client : public ClientBase RCLCPP_SMART_PTR_DEFINITIONS(Client); Client( - std::shared_ptr node_handle, - rmw_client_t * client_handle, + std::shared_ptr node_handle, + rcl_client_t * client_handle, const std::string & service_name) : ClientBase(node_handle, client_handle, service_name) {} diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 9998137553..7bcafac524 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -389,15 +389,17 @@ Node::create_service( rclcpp::service::AnyServiceCallback any_service_callback; any_service_callback.set(std::forward(callback)); - rmw_service_t * service_handle = rmw_create_service( - rcl_node_get_rmw_handle(node_handle_.get()), - service_type_support_handle, service_name.c_str(), &qos_profile); - if (!service_handle) { - // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) + rcl_service_t * service_handle = new rcl_service_t; + rcl_service_options_t service_options = rcl_service_get_default_options(); + service_options.qos = qos_profile; + // TODO allocator + if (rcl_service_init( + service_handle, node_handle_.get(), service_type_support_handle, service_name.c_str(), + &service_options) != RCL_RET_OK) + { throw std::runtime_error( std::string("could not create service: ") + rmw_get_error_string_safe()); - // *INDENT-ON* } auto serv = service::Service::make_shared( diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index b267cbcaa9..a9629015e1 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -21,6 +21,8 @@ #include #include +#include + #include "rclcpp/any_service_callback.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/visibility_control.hpp" @@ -39,8 +41,8 @@ class ServiceBase RCLCPP_PUBLIC ServiceBase( - std::shared_ptr node_handle, - rmw_service_t * service_handle, + std::shared_ptr node_handle, + rcl_service_t * service_handle, const std::string service_name); RCLCPP_PUBLIC @@ -63,9 +65,9 @@ class ServiceBase private: RCLCPP_DISABLE_COPY(ServiceBase); - std::shared_ptr node_handle_; + std::shared_ptr node_handle_; - rmw_service_t * service_handle_; + rcl_service_t * service_handle_; std::string service_name_; }; @@ -88,8 +90,8 @@ class Service : public ServiceBase RCLCPP_SMART_PTR_DEFINITIONS(Service); Service( - std::shared_ptr node_handle, - rmw_service_t * service_handle, + std::shared_ptr node_handle, + rcl_service_t * service_handle, const std::string & service_name, AnyServiceCallback any_callback) : ServiceBase(node_handle, service_handle, service_name), any_callback_(any_callback) @@ -122,11 +124,12 @@ class Service : public ServiceBase std::shared_ptr req_id, std::shared_ptr response) { - rmw_ret_t status = rmw_send_response(get_service_handle(), req_id.get(), response.get()); - if (status != RMW_RET_OK) { + rcl_ret_t status = rcl_send_response(get_service_handle(), req_id.get(), response.get()); + + if (status != RCL_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()); + std::string("failed to send response: ") + rcl_get_error_string_safe()); // *INDENT-ON* } } diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index f2f588c78e..dcf21edb69 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -28,8 +28,8 @@ using rclcpp::service::ServiceBase; ServiceBase::ServiceBase( - std::shared_ptr node_handle, - rmw_service_t * service_handle, + std::shared_ptr node_handle, + rcl_service_t * service_handle, const std::string service_name) : node_handle_(node_handle), service_handle_(service_handle), service_name_(service_name) {} @@ -37,12 +37,13 @@ ServiceBase::ServiceBase( ServiceBase::~ServiceBase() { if (service_handle_) { - if (rmw_destroy_service(service_handle_) != RMW_RET_OK) { + if (rcl_service_fini(service_handle_, node_handle_.get())) { std::stringstream ss; ss << "Error in destruction of rmw service_handle_ handle: " << rmw_get_error_string_safe() << '\n'; (std::cerr << ss.str()).flush(); } + delete service_handle_; } } @@ -52,7 +53,7 @@ ServiceBase::get_service_name() return this->service_name_; } -const rmw_service_t * +const rcl_service_t * ServiceBase::get_service_handle() { return this->service_handle_; From ffb5da6f9739d0e1a224069197b0ce31a1bbcbf2 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Fri, 11 Mar 2016 11:26:28 -0800 Subject: [PATCH 08/49] blocked by guard condition (obviously) --- rclcpp/include/rclcpp/client.hpp | 7 +++--- rclcpp/include/rclcpp/executor.hpp | 2 +- rclcpp/include/rclcpp/service.hpp | 3 ++- rclcpp/src/rclcpp/client.cpp | 9 ++++---- rclcpp/src/rclcpp/executor.cpp | 36 +++++++++++++----------------- 5 files changed, 28 insertions(+), 29 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index c7d8139447..0cb0939f41 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -23,6 +23,7 @@ #include #include +#include #include #include "rclcpp/function_traits.hpp" @@ -149,12 +150,12 @@ class Client : public ClientBase > SharedFuture async_send_request(SharedRequest request, CallbackT && cb) { - std::lock_guard lock(pending_requests_mutex_); int64_t sequence_number; - if (RMW_RET_OK != rmw_send_request(get_client_handle(), request.get(), &sequence_number)) { + std::lock_guard lock(pending_requests_mutex_); + if (RMW_RET_OK != rcl_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()); + std::string("failed to send request: ") + rcl_get_error_string_safe()); // *INDENT-ON* } diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index de38dde09c..287e7988c3 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -307,7 +307,7 @@ class Executor std::atomic_bool spinning; /// Guard condition for signaling the rmw layer to wake up for special events. - rmw_guard_condition_t * interrupt_guard_condition_; + rcl_guard_condition_t * interrupt_guard_condition_; /// Waitset for managing entities that the rmw layer waits on. rmw_waitset_t * waitset_; diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index a9629015e1..a757a20556 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -21,6 +21,7 @@ #include #include +#include #include #include "rclcpp/any_service_callback.hpp" @@ -53,7 +54,7 @@ class ServiceBase get_service_name(); RCLCPP_PUBLIC - const rmw_service_t * + const rcl_service_t * get_service_handle(); virtual std::shared_ptr create_request() = 0; diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index 29aa62a690..6d4fb9bfd5 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -22,8 +22,8 @@ using rclcpp::client::ClientBase; ClientBase::ClientBase( - std::shared_ptr node_handle, - rmw_client_t * client_handle, + std::shared_ptr node_handle, + rcl_client_t * client_handle, const std::string & service_name) : node_handle_(node_handle), client_handle_(client_handle), service_name_(service_name) {} @@ -31,10 +31,11 @@ ClientBase::ClientBase( ClientBase::~ClientBase() { if (client_handle_) { - if (rmw_destroy_client(client_handle_) != RMW_RET_OK) { + if (rcl_client_fini(client_handle_, node_handle_.get()) != RMW_RET_OK) { fprintf(stderr, "Error in destruction of rmw client handle: %s\n", rmw_get_error_string_safe()); } + delete client_handle_; } } @@ -44,7 +45,7 @@ ClientBase::get_service_name() const return this->service_name_; } -const rmw_client_t * +const rcl_client_t * ClientBase::get_client_handle() const { return this->client_handle_; diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 94da488957..bb8d388336 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -284,20 +284,18 @@ Executor::execute_service( { auto request_header = service->create_request_header(); std::shared_ptr request = service->create_request(); - bool taken = false; - rmw_ret_t status = rmw_take_request( + rcl_ret_t status = rcl_take_request( service->get_service_handle(), request_header.get(), - request.get(), - &taken); - if (status == RMW_RET_OK) { - if (taken) { + request.get()); + if (status != RCL_RET_SERVICE_TAKE_FAILED) { + if (status == RMW_RET_OK) { service->handle_request(request_header, request); + } else { + fprintf(stderr, + "[rclcpp::error] take request failed for server of service '%s': %s\n", + service->get_service_name().c_str(), rcl_get_error_string_safe()); } - } else { - 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()); } } @@ -307,20 +305,18 @@ Executor::execute_client( { auto request_header = client->create_request_header(); std::shared_ptr response = client->create_response(); - bool taken = false; - rmw_ret_t status = rmw_take_response( + rcl_ret_t status = rcl_take_response( client->get_client_handle(), request_header.get(), - response.get(), - &taken); - if (status == RMW_RET_OK) { - if (taken) { + response.get()); + if (status != RCL_RET_SERVICE_TAKE_FAILED) { + if (status == RMW_RET_OK) { client->handle_response(request_header, response); + } else { + fprintf(stderr, + "[rclcpp::error] take response failed for client of service '%s': %s\n", + client->get_service_name().c_str(), rcl_get_error_string_safe()); } - } else { - 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()); } } From 84cfe94b80369b5e8880b956dd338df717da28f2 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Fri, 11 Mar 2016 15:42:32 -0800 Subject: [PATCH 09/49] Begin replacing guard conditions --- rclcpp/include/rclcpp/executor.hpp | 10 ++++- rclcpp/src/rclcpp/executor.cpp | 69 ++++++++++++++++-------------- 2 files changed, 44 insertions(+), 35 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 287e7988c3..c63163465c 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -25,6 +25,9 @@ #include #include +#include "rcl/guard_condition.h" +#include "rcl/wait.h" + #include "rclcpp/any_executable.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/memory_strategies.hpp" @@ -306,11 +309,14 @@ class Executor /// Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins. std::atomic_bool spinning; + // array of fixed guard conditions + rcl_guard_condition_t * fixed_guard_conditions_; + /// Guard condition for signaling the rmw layer to wake up for special events. - rcl_guard_condition_t * interrupt_guard_condition_; + rcl_guard_condition_t interrupt_guard_condition_; /// Waitset for managing entities that the rmw layer waits on. - rmw_waitset_t * waitset_; + rcl_wait_set_t waitset_; /// The memory strategy: an interface for handling user-defined memory allocation strategies. memory_strategy::MemoryStrategy::SharedPtr memory_strategy_; diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index bb8d388336..59c6c464cd 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -30,8 +30,10 @@ Executor::Executor(const ExecutorArgs & args) : spinning(false), memory_strategy_(args.memory_strategy) { - interrupt_guard_condition_ = rmw_create_guard_condition(); - if (!interrupt_guard_condition_) { + rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options(); + if (rcl_guard_condition_init( + &interrupt_guard_condition_, &guard_condition_options) != RCL_RET_OK) + { throw std::runtime_error("Failed to create interrupt guard condition in Executor constructor"); } @@ -44,13 +46,12 @@ Executor::Executor(const ExecutorArgs & args) // Put the executor's guard condition in memory_strategy_->add_guard_condition(interrupt_guard_condition_); - waitset_ = rmw_create_waitset(args.max_conditions); - - if (!waitset_) { + if (rcl_wait_set_init( + &waitset_, &fixed_guard_conditions, 0, 0, 0, 0, 0, 0, rcl_get_default_allocator()) != RCL_RET_OK) + { fprintf(stderr, "[rclcpp::error] failed to create waitset: %s\n", rmw_get_error_string_safe()); - rmw_ret_t status = rmw_destroy_guard_condition(interrupt_guard_condition_); - if (status != RMW_RET_OK) { + if (rcl_guard_condition_fini(interrupt_guard_condition_) != RCL_RET_OK) { fprintf(stderr, "[rclcpp::error] failed to destroy guard condition: %s\n", rmw_get_error_string_safe()); } @@ -60,21 +61,15 @@ Executor::Executor(const ExecutorArgs & args) Executor::~Executor() { - // Try to deallocate the waitset. - if (waitset_) { - rmw_ret_t status = rmw_destroy_waitset(waitset_); - if (status != RMW_RET_OK) { - fprintf(stderr, - "[rclcpp::error] failed to destroy waitset: %s\n", rmw_get_error_string_safe()); - } + // Finalize the waitset. + if (rcl_waitset_fini(&waitset_) != RMW_RET_OK) { + fprintf(stderr, + "[rclcpp::error] failed to destroy waitset: %s\n", rcl_get_error_string_safe()); } - // Try to deallocate the interrupt guard condition. - if (interrupt_guard_condition_ != nullptr) { - rmw_ret_t status = rmw_destroy_guard_condition(interrupt_guard_condition_); - if (status != RMW_RET_OK) { - fprintf(stderr, - "[rclcpp::error] failed to destroy guard condition: %s\n", rmw_get_error_string_safe()); - } + // Finalize the interrupt guard condition. + if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) { + fprintf(stderr, + "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe()); } } @@ -96,9 +91,8 @@ Executor::add_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify) weak_nodes_.push_back(node_ptr); if (notify) { // Interrupt waiting to handle new node - rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_); - if (status != RMW_RET_OK) { - throw std::runtime_error(rmw_get_error_string_safe()); + if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RMW_RET_OK) { + throw std::runtime_error(rcl_get_error_string_safe()); } } // Add the node's notify condition to the guard condition handles @@ -126,9 +120,8 @@ Executor::remove_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify) if (notify) { // If the node was matched and removed, interrupt waiting if (node_removed) { - rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_); - if (status != RMW_RET_OK) { - throw std::runtime_error(rmw_get_error_string_safe()); + if (rcl_trigger_guard_condition(&interrupt_guard_condition_)!= RCL_RET_OK) { + throw std::runtime_error(rcl_get_error_string_safe()); } } } @@ -184,9 +177,8 @@ void Executor::cancel() { spinning.store(false); - rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_); - if (status != RMW_RET_OK) { - throw std::runtime_error(rmw_get_error_string_safe()); + if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { + throw std::runtime_error(rcl_get_error_string_safe()); } } @@ -224,9 +216,8 @@ Executor::execute_any_executable(AnyExecutable::SharedPtr any_exec) any_exec->callback_group->can_be_taken_from().store(true); // Wake the wait, because it may need to be recalculated or work that // was previously blocked is now available. - rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_); - if (status != RMW_RET_OK) { - throw std::runtime_error(rmw_get_error_string_safe()); + if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { + throw std::runtime_error(rcl_get_error_string_safe()); } } @@ -320,6 +311,18 @@ Executor::execute_client( } } +/* +basic outline +get ready entities + +For all the entities, check the size of the rcl wait_set array and resize if necessary +Add the entities to the wait_set +rcl_wait +check ready subscriptions +clear subscriptions + +memory strategy is the worst!!! :( +*/ void Executor::wait_for_work(std::chrono::nanoseconds timeout) { From bb4cc986e1ec59204ce0d15aa561913f18ecb31d Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Fri, 11 Mar 2016 17:45:29 -0800 Subject: [PATCH 10/49] memory strategy conversion --- rclcpp/include/rclcpp/executor.hpp | 2 +- rclcpp/include/rclcpp/memory_strategy.hpp | 14 ++++- .../strategies/allocator_memory_strategy.hpp | 61 ++++++++++++------- rclcpp/src/rclcpp/executor.cpp | 45 ++++++++++---- rclcpp/src/rclcpp/memory_strategy.cpp | 15 +++-- 5 files changed, 93 insertions(+), 44 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index c63163465c..e8288a86e8 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -310,7 +310,7 @@ class Executor std::atomic_bool spinning; // array of fixed guard conditions - rcl_guard_condition_t * fixed_guard_conditions_; + std::array fixed_guard_conditions_; /// Guard condition for signaling the rmw layer to wake up for special events. rcl_guard_condition_t interrupt_guard_condition_; diff --git a/rclcpp/include/rclcpp/memory_strategy.hpp b/rclcpp/include/rclcpp/memory_strategy.hpp index 9386a80382..b876f96f58 100644 --- a/rclcpp/include/rclcpp/memory_strategy.hpp +++ b/rclcpp/include/rclcpp/memory_strategy.hpp @@ -18,6 +18,8 @@ #include #include +#include + #include "rclcpp/any_executable.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node.hpp" @@ -40,6 +42,7 @@ class RCLCPP_PUBLIC MemoryStrategy RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(MemoryStrategy); using WeakNodeVector = std::vector>; +/* // return the new number of subscribers virtual size_t fill_subscriber_handles(void ** & ptr) = 0; @@ -56,8 +59,13 @@ class RCLCPP_PUBLIC MemoryStrategy virtual void clear_handles() = 0; virtual void remove_null_handles() = 0; +*/ virtual bool collect_entities(const WeakNodeVector & weak_nodes) = 0; + virtual size_t number_of_ready_subscriptions() const; + + virtual bool add_handles_to_waitset(rcl_wait_set_t * wait_set); + /// Provide a newly initialized AnyExecutable object. // \return Shared pointer to the fresh executable. virtual rclcpp::executor::AnyExecutable::SharedPtr instantiate_next_executable() = 0; @@ -79,14 +87,14 @@ class RCLCPP_PUBLIC MemoryStrategy const WeakNodeVector & weak_nodes) = 0; static rclcpp::subscription::SubscriptionBase::SharedPtr - get_subscription_by_handle(void * subscriber_handle, + get_subscription_by_handle(rcl_subscription_t * subscriber_handle, const WeakNodeVector & weak_nodes); static rclcpp::service::ServiceBase::SharedPtr - get_service_by_handle(void * service_handle, const WeakNodeVector & weak_nodes); + get_service_by_handle(rcl_service_t * service_handle, const WeakNodeVector & weak_nodes); static rclcpp::client::ClientBase::SharedPtr - get_client_by_handle(void * client_handle, const WeakNodeVector & weak_nodes); + get_client_by_handle(rcl_client_t * client_handle, const WeakNodeVector & weak_nodes); static rclcpp::node::Node::SharedPtr get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group, diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index c0a0a4b0ca..88ac0b3200 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -84,16 +84,17 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } } +/* size_t fill_subscriber_handles(void ** & ptr) { for (auto & subscription : subscriptions_) { - subscriber_handles_.push_back(subscription->get_subscription_handle()->data); + subscription_handles_.push_back(subscription->get_subscription_handle()->data); if (subscription->get_intra_process_subscription_handle()) { - subscriber_handles_.push_back(subscription->get_intra_process_subscription_handle()->data); + subscription_handles_.push_back(subscription->get_intra_process_subscription_handle()->data); } } - ptr = subscriber_handles_.data(); - return subscriber_handles_.size(); + ptr = subscription_handles_.data(); + return subscription_handles_.size(); } // return the new number of services @@ -136,7 +137,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy void clear_handles() { - subscriber_handles_.clear(); + subscription_handles_.clear(); service_handles_.clear(); client_handles_.clear(); guard_condition_handles_.clear(); @@ -144,9 +145,9 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy void remove_null_handles() { - subscriber_handles_.erase( - std::remove(subscriber_handles_.begin(), subscriber_handles_.end(), nullptr), - subscriber_handles_.end() + subscription_handles_.erase( + std::remove(subscription_handles_.begin(), subscription_handles_.end(), nullptr), + subscription_handles_.end() ); service_handles_.erase( @@ -164,6 +165,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy guard_condition_handles_.end() ); } +*/ bool collect_entities(const WeakNodeVector & weak_nodes) { @@ -182,25 +184,36 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy for (auto & weak_subscription : group->get_subscription_ptrs()) { auto subscription = weak_subscription.lock(); if (subscription) { - subscriptions_.push_back(subscription); + subscription_handles_.push_back(subscription->get_subscription_handle()); } } for (auto & service : group->get_service_ptrs()) { if (service) { - services_.push_back(service); + service_handles_.push_back(service->get_service_handle()); } } for (auto & weak_client : group->get_client_ptrs()) { auto client = weak_client.lock(); if (client) { - clients_.push_back(client); + client_handles_.push_back(client->get_client_handle()); } } + // TODO: Add timers } } return has_invalid_weak_nodes; } + bool add_handles_to_waitset(rcl_wait_set_t * wait_set) { + for (auto subscription : subscription_handles_) { + if (rcl_wait_set_add_subscription(wait_set, subscription) != RCL_RET_OK) { + return false; + } + } + + // TODO: etc. + return true; + } /// Provide a newly initialized AnyExecutable object. // \return Shared pointer to the fresh executable. @@ -213,21 +226,22 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy get_next_subscription(executor::AnyExecutable::SharedPtr any_exec, const WeakNodeVector & weak_nodes) { - auto it = subscriber_handles_.begin(); - while (it != subscriber_handles_.end()) { + // TODO hmMMmmm we need to redo this whole API + auto it = subscription_handles_.begin(); + while (it != subscription_handles_.end()) { auto subscription = get_subscription_by_handle(*it, weak_nodes); if (subscription) { // Figure out if this is for intra-process or not. bool is_intra_process = false; if (subscription->get_intra_process_subscription_handle()) { - is_intra_process = subscription->get_intra_process_subscription_handle()->data == *it; + is_intra_process = subscription->get_intra_process_subscription_handle() == *it; } // Find the group for this handle and see if it can be serviced auto group = get_group_by_subscription(subscription, weak_nodes); if (!group) { // Group was not found, meaning the subscription is not valid... // Remove it from the ready list and continue looking - subscriber_handles_.erase(it); + subscription_handles_.erase(it); continue; } if (!group->can_be_taken_from().load()) { @@ -244,11 +258,11 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } any_exec->callback_group = group; any_exec->node = get_node_by_group(group, weak_nodes); - subscriber_handles_.erase(it); + subscription_handles_.erase(it); return; } // Else, the subscription is no longer valid, remove it and continue - subscriber_handles_.erase(it); + subscription_handles_.erase(it); } } @@ -319,20 +333,25 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } } + size_t number_of_ready_subscriptions() const { + return subscription_handles_.size(); + } + private: template using VectorRebind = std::vector::template rebind_alloc>; +/* VectorRebind subscriptions_; VectorRebind services_; VectorRebind clients_; VectorRebind guard_conditions_; +*/ - VectorRebind subscriber_handles_; - VectorRebind service_handles_; - VectorRebind client_handles_; - VectorRebind guard_condition_handles_; + VectorRebind subscription_handles_; + VectorRebind service_handles_; + VectorRebind client_handles_; std::shared_ptr executable_allocator_; std::shared_ptr allocator_; diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 59c6c464cd..9e50894a35 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -32,14 +32,18 @@ Executor::Executor(const ExecutorArgs & args) { rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options(); if (rcl_guard_condition_init( - &interrupt_guard_condition_, &guard_condition_options) != RCL_RET_OK) + &interrupt_guard_condition_, guard_condition_options) != RCL_RET_OK) { throw std::runtime_error("Failed to create interrupt guard condition in Executor constructor"); } // The number of guard conditions is always at least 2: 1 for the ctrl-c guard cond, // and one for the executor's guard cond (interrupt_guard_condition_) - // The size of guard conditions is variable because the number of nodes can vary + + // These guard conditions are permanently attached to the waitset. + // const size_t number_of_guard_conds = 2; + //fixed_guard_conditions_.guard_condition_count = number_of_guard_conds; + //fixed_guard_conditions_.guard_conditions = static_cast(guard_cond_handles_.data()); // Put the global ctrl-c guard condition in memory_strategy_->add_guard_condition(rclcpp::utilities::get_global_sigint_guard_condition()); @@ -47,11 +51,11 @@ Executor::Executor(const ExecutorArgs & args) memory_strategy_->add_guard_condition(interrupt_guard_condition_); if (rcl_wait_set_init( - &waitset_, &fixed_guard_conditions, 0, 0, 0, 0, 0, 0, rcl_get_default_allocator()) != RCL_RET_OK) + &waitset_, &(fixed_guard_conditions_.data()), 0, 0, 0, 0, 0, 0, rcl_get_default_allocator()) != RCL_RET_OK) { fprintf(stderr, "[rclcpp::error] failed to create waitset: %s\n", rmw_get_error_string_safe()); - if (rcl_guard_condition_fini(interrupt_guard_condition_) != RCL_RET_OK) { + if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) { fprintf(stderr, "[rclcpp::error] failed to destroy guard condition: %s\n", rmw_get_error_string_safe()); } @@ -62,7 +66,7 @@ Executor::Executor(const ExecutorArgs & args) Executor::~Executor() { // Finalize the waitset. - if (rcl_waitset_fini(&waitset_) != RMW_RET_OK) { + if (rcl_wait_set_fini(&waitset_) != RMW_RET_OK) { fprintf(stderr, "[rclcpp::error] failed to destroy waitset: %s\n", rcl_get_error_string_safe()); } @@ -91,7 +95,7 @@ Executor::add_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify) weak_nodes_.push_back(node_ptr); if (notify) { // Interrupt waiting to handle new node - if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RMW_RET_OK) { + if (rcl_guard_condition_trigger(&interrupt_guard_condition_) != RMW_RET_OK) { throw std::runtime_error(rcl_get_error_string_safe()); } } @@ -120,7 +124,7 @@ Executor::remove_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify) if (notify) { // If the node was matched and removed, interrupt waiting if (node_removed) { - if (rcl_trigger_guard_condition(&interrupt_guard_condition_)!= RCL_RET_OK) { + if (rcl_guard_condition_trigger(&interrupt_guard_condition_)!= RCL_RET_OK) { throw std::runtime_error(rcl_get_error_string_safe()); } } @@ -177,7 +181,7 @@ void Executor::cancel() { spinning.store(false); - if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { + if (rcl_guard_condition_trigger(&interrupt_guard_condition_) != RCL_RET_OK) { throw std::runtime_error(rcl_get_error_string_safe()); } } @@ -216,7 +220,7 @@ Executor::execute_any_executable(AnyExecutable::SharedPtr any_exec) any_exec->callback_group->can_be_taken_from().store(true); // Wake the wait, because it may need to be recalculated or work that // was previously blocked is now available. - if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { + if (rcl_guard_condition_trigger(&interrupt_guard_condition_) != RCL_RET_OK) { throw std::runtime_error(rcl_get_error_string_safe()); } } @@ -326,7 +330,7 @@ memory strategy is the worst!!! :( void Executor::wait_for_work(std::chrono::nanoseconds timeout) { - memory_strategy_->clear_active_entities(); + //memory_strategy_->clear_active_entities(); // Collect the subscriptions and timers to be waited on bool has_invalid_weak_nodes = memory_strategy_->collect_entities(weak_nodes_); @@ -347,7 +351,24 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) } memory_strategy_->clear_handles(); + if (waitset_.size_of_subscriptions < memory_strategy_->number_of_ready_subscriptions()) { + rcl_wait_set_resize_subscriptions( + &waitset_, memory_strategy_->number_of_ready_subscriptions()); + } + // TODO etc... + + // for all ready subscriptions: + // rcl_wait_set_add_subscription(&waitset_, ready_subscription); + // etc... + + rcl_ret_t status = rcl_wait(&waitset_, timeout.count()); + if (status != RCL_RET_OK && status != RCL_RET_TIMEOUT && status != RCL_RET_WAIT_SET_EMPTY) { + throw std::runtime_error(rcl_get_error_string_safe()); + } + // Use the number of subscriptions to allocate memory in the handles + // TODO We need to add timers to the waitset now I guess!! +/* rmw_subscriptions_t subscriber_handles; subscriber_handles.subscriber_count = memory_strategy_->fill_subscriber_handles(subscriber_handles.subscribers); @@ -399,8 +420,10 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) if (status != RMW_RET_OK && status != RMW_RET_TIMEOUT) { throw std::runtime_error(rmw_get_error_string_safe()); } +*/ - memory_strategy_->remove_null_handles(); + // eh? I think rcl_wait_set does this for us! + // memory_strategy_->remove_null_handles(); } rclcpp::node::Node::SharedPtr diff --git a/rclcpp/src/rclcpp/memory_strategy.cpp b/rclcpp/src/rclcpp/memory_strategy.cpp index a196fa3fd9..72a08dcdeb 100644 --- a/rclcpp/src/rclcpp/memory_strategy.cpp +++ b/rclcpp/src/rclcpp/memory_strategy.cpp @@ -18,7 +18,7 @@ using rclcpp::memory_strategy::MemoryStrategy; rclcpp::subscription::SubscriptionBase::SharedPtr MemoryStrategy::get_subscription_by_handle( - void * subscriber_handle, const WeakNodeVector & weak_nodes) + rcl_subscription_t * subscriber_handle, const WeakNodeVector & weak_nodes) { for (auto & weak_node : weak_nodes) { auto node = weak_node.lock(); @@ -33,11 +33,10 @@ MemoryStrategy::get_subscription_by_handle( for (auto & weak_subscription : group->get_subscription_ptrs()) { auto subscription = weak_subscription.lock(); if (subscription) { - if (subscription->get_subscription_handle()->data == subscriber_handle) { + if (subscription->get_subscription_handle() == subscriber_handle) { return subscription; } - if (subscription->get_intra_process_subscription_handle() && - subscription->get_intra_process_subscription_handle()->data == subscriber_handle) + if (subscription->get_intra_process_subscription_handle() == subscriber_handle) { return subscription; } @@ -49,7 +48,7 @@ MemoryStrategy::get_subscription_by_handle( } rclcpp::service::ServiceBase::SharedPtr -MemoryStrategy::get_service_by_handle(void * service_handle, const WeakNodeVector & weak_nodes) +MemoryStrategy::get_service_by_handle(rcl_service_t * service_handle, const WeakNodeVector & weak_nodes) { for (auto & weak_node : weak_nodes) { auto node = weak_node.lock(); @@ -62,7 +61,7 @@ MemoryStrategy::get_service_by_handle(void * service_handle, const WeakNodeVecto continue; } for (auto & service : group->get_service_ptrs()) { - if (service->get_service_handle()->data == service_handle) { + if (service->get_service_handle() == service_handle) { return service; } } @@ -72,7 +71,7 @@ MemoryStrategy::get_service_by_handle(void * service_handle, const WeakNodeVecto } rclcpp::client::ClientBase::SharedPtr -MemoryStrategy::get_client_by_handle(void * client_handle, const WeakNodeVector & weak_nodes) +MemoryStrategy::get_client_by_handle(rcl_client_t * client_handle, const WeakNodeVector & weak_nodes) { for (auto & weak_node : weak_nodes) { auto node = weak_node.lock(); @@ -86,7 +85,7 @@ MemoryStrategy::get_client_by_handle(void * client_handle, const WeakNodeVector } for (auto & weak_client : group->get_client_ptrs()) { auto client = weak_client.lock(); - if (client && client->get_client_handle()->data == client_handle) { + if (client && client->get_client_handle() == client_handle) { return client; } } From 85bd43b8e67b9d2f9dc4cf912ded009860e0db10 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Sun, 13 Mar 2016 14:41:06 -0700 Subject: [PATCH 11/49] Compiles with warnings --- rclcpp/CMakeLists.txt | 1 + rclcpp/include/rclcpp/memory_strategy.hpp | 10 +++---- rclcpp/include/rclcpp/node_impl.hpp | 9 ++++--- .../strategies/allocator_memory_strategy.hpp | 6 ++--- rclcpp/include/rclcpp/utilities.hpp | 5 +++- rclcpp/src/rclcpp/executor.cpp | 11 ++++---- rclcpp/src/rclcpp/memory_strategy.cpp | 6 ++--- rclcpp/src/rclcpp/node.cpp | 4 +-- rclcpp/src/rclcpp/utilities.cpp | 27 ++++++++++--------- 9 files changed, 44 insertions(+), 35 deletions(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 986c080184..5c37527a6b 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -68,6 +68,7 @@ endmacro() call_for_each_rmw_implementation(target GENERATE_DEFAULT) ament_export_dependencies(ament_cmake) +ament_export_dependencies(rcl) ament_export_dependencies(rcl_interfaces) ament_export_dependencies(rmw) ament_export_dependencies(rmw_implementation) diff --git a/rclcpp/include/rclcpp/memory_strategy.hpp b/rclcpp/include/rclcpp/memory_strategy.hpp index b876f96f58..6edd2d5578 100644 --- a/rclcpp/include/rclcpp/memory_strategy.hpp +++ b/rclcpp/include/rclcpp/memory_strategy.hpp @@ -62,9 +62,9 @@ class RCLCPP_PUBLIC MemoryStrategy */ virtual bool collect_entities(const WeakNodeVector & weak_nodes) = 0; - virtual size_t number_of_ready_subscriptions() const; + virtual size_t number_of_ready_subscriptions() const = 0; - virtual bool add_handles_to_waitset(rcl_wait_set_t * wait_set); + virtual bool add_handles_to_waitset(rcl_wait_set_t * wait_set) = 0; /// Provide a newly initialized AnyExecutable object. // \return Shared pointer to the fresh executable. @@ -87,14 +87,14 @@ class RCLCPP_PUBLIC MemoryStrategy const WeakNodeVector & weak_nodes) = 0; static rclcpp::subscription::SubscriptionBase::SharedPtr - get_subscription_by_handle(rcl_subscription_t * subscriber_handle, + get_subscription_by_handle(const rcl_subscription_t * subscriber_handle, const WeakNodeVector & weak_nodes); static rclcpp::service::ServiceBase::SharedPtr - get_service_by_handle(rcl_service_t * service_handle, const WeakNodeVector & weak_nodes); + get_service_by_handle(const rcl_service_t * service_handle, const WeakNodeVector & weak_nodes); static rclcpp::client::ClientBase::SharedPtr - get_client_by_handle(rcl_client_t * client_handle, const WeakNodeVector & weak_nodes); + get_client_by_handle(const rcl_client_t * client_handle, const WeakNodeVector & weak_nodes); static rclcpp::node::Node::SharedPtr get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group, diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 7bcafac524..c2848f5211 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -335,9 +335,12 @@ Node::create_client( auto service_type_support_handle = get_service_type_support_handle(); - rmw_client_t * client_handle = rmw_create_client( - rcl_node_get_rmw_handle(this->node_handle_.get()), - service_type_support_handle, service_name.c_str(), &qos_profile); + rcl_client_t * client_handle = new rcl_client_t; + + rcl_client_options_t options = rcl_client_get_default_options(); + options.qos = qos_profile; + rcl_client_init(client_handle, this->node_handle_.get(), + service_type_support_handle, service_name.c_str(), &options); if (!client_handle) { // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) throw std::runtime_error( diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 88ac0b3200..e3d00a5e27 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -349,9 +349,9 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy VectorRebind guard_conditions_; */ - VectorRebind subscription_handles_; - VectorRebind service_handles_; - VectorRebind client_handles_; + VectorRebind subscription_handles_; + VectorRebind service_handles_; + VectorRebind client_handles_; std::shared_ptr executable_allocator_; std::shared_ptr allocator_; diff --git a/rclcpp/include/rclcpp/utilities.hpp b/rclcpp/include/rclcpp/utilities.hpp index dbcac72ad6..ace7200656 100644 --- a/rclcpp/include/rclcpp/utilities.hpp +++ b/rclcpp/include/rclcpp/utilities.hpp @@ -18,6 +18,9 @@ #include #include "rclcpp/visibility_control.hpp" + +#include "rcl/guard_condition.h" + #include "rmw/macros.h" #include "rmw/rmw.h" @@ -48,7 +51,7 @@ shutdown(); /// Get a handle to the rmw guard condition that manages the signal handler. RCLCPP_PUBLIC -rmw_guard_condition_t * +rcl_guard_condition_t * get_global_sigint_guard_condition(); /// Use the global condition variable to block for the specified amount of time. diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 9e50894a35..cabe9d8d4a 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -47,11 +47,12 @@ Executor::Executor(const ExecutorArgs & args) // Put the global ctrl-c guard condition in memory_strategy_->add_guard_condition(rclcpp::utilities::get_global_sigint_guard_condition()); + // Put the executor's guard condition in memory_strategy_->add_guard_condition(interrupt_guard_condition_); if (rcl_wait_set_init( - &waitset_, &(fixed_guard_conditions_.data()), 0, 0, 0, 0, 0, 0, rcl_get_default_allocator()) != RCL_RET_OK) + &waitset_, fixed_guard_conditions_.data(), 0, 0, 0, 0, 0, 0, rcl_get_default_allocator()) != RCL_RET_OK) { fprintf(stderr, "[rclcpp::error] failed to create waitset: %s\n", rmw_get_error_string_safe()); @@ -95,7 +96,7 @@ Executor::add_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify) weak_nodes_.push_back(node_ptr); if (notify) { // Interrupt waiting to handle new node - if (rcl_guard_condition_trigger(&interrupt_guard_condition_) != RMW_RET_OK) { + if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RMW_RET_OK) { throw std::runtime_error(rcl_get_error_string_safe()); } } @@ -124,7 +125,7 @@ Executor::remove_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify) if (notify) { // If the node was matched and removed, interrupt waiting if (node_removed) { - if (rcl_guard_condition_trigger(&interrupt_guard_condition_)!= RCL_RET_OK) { + if (rcl_trigger_guard_condition(&interrupt_guard_condition_)!= RCL_RET_OK) { throw std::runtime_error(rcl_get_error_string_safe()); } } @@ -181,7 +182,7 @@ void Executor::cancel() { spinning.store(false); - if (rcl_guard_condition_trigger(&interrupt_guard_condition_) != RCL_RET_OK) { + if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { throw std::runtime_error(rcl_get_error_string_safe()); } } @@ -220,7 +221,7 @@ Executor::execute_any_executable(AnyExecutable::SharedPtr any_exec) any_exec->callback_group->can_be_taken_from().store(true); // Wake the wait, because it may need to be recalculated or work that // was previously blocked is now available. - if (rcl_guard_condition_trigger(&interrupt_guard_condition_) != RCL_RET_OK) { + if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { throw std::runtime_error(rcl_get_error_string_safe()); } } diff --git a/rclcpp/src/rclcpp/memory_strategy.cpp b/rclcpp/src/rclcpp/memory_strategy.cpp index 72a08dcdeb..20810aa8b4 100644 --- a/rclcpp/src/rclcpp/memory_strategy.cpp +++ b/rclcpp/src/rclcpp/memory_strategy.cpp @@ -18,7 +18,7 @@ using rclcpp::memory_strategy::MemoryStrategy; rclcpp::subscription::SubscriptionBase::SharedPtr MemoryStrategy::get_subscription_by_handle( - rcl_subscription_t * subscriber_handle, const WeakNodeVector & weak_nodes) + const rcl_subscription_t * subscriber_handle, const WeakNodeVector & weak_nodes) { for (auto & weak_node : weak_nodes) { auto node = weak_node.lock(); @@ -48,7 +48,7 @@ MemoryStrategy::get_subscription_by_handle( } rclcpp::service::ServiceBase::SharedPtr -MemoryStrategy::get_service_by_handle(rcl_service_t * service_handle, const WeakNodeVector & weak_nodes) +MemoryStrategy::get_service_by_handle(const rcl_service_t * service_handle, const WeakNodeVector & weak_nodes) { for (auto & weak_node : weak_nodes) { auto node = weak_node.lock(); @@ -71,7 +71,7 @@ MemoryStrategy::get_service_by_handle(rcl_service_t * service_handle, const Weak } rclcpp::client::ClientBase::SharedPtr -MemoryStrategy::get_client_by_handle(rcl_client_t * client_handle, const WeakNodeVector & weak_nodes) +MemoryStrategy::get_client_by_handle(const rcl_client_t * client_handle, const WeakNodeVector & weak_nodes) { for (auto & weak_node : weak_nodes) { auto node = weak_node.lock(); diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 981ff71941..a886feb828 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -71,7 +71,7 @@ Node::Node( rcl_node_t node = rcl_get_zero_initialized_node(); rcl_node_options_t options; // TODO(jacquelinekay): Allocator options - options.domaind_id = domain_id; + options.domain_id = domain_id; if (rcl_node_init(&node, name_.c_str(), &options) != RCL_RET_OK) { if (rmw_destroy_guard_condition(notify_guard_condition_) != RMW_RET_OK) { fprintf( @@ -83,7 +83,7 @@ Node::Node( // Initialize node handle shared_ptr with custom deleter. // *INDENT-OFF* node_handle_.reset(&node, [](rcl_node_t * node) { - auto ret = rcl_fini_node(node); + auto ret = rcl_node_fini(node); if (ret != RMW_RET_OK) { fprintf( stderr, "Error in destruction of rmw node handle: %s\n", rcl_get_error_string_safe()); diff --git a/rclcpp/src/rclcpp/utilities.cpp b/rclcpp/src/rclcpp/utilities.cpp index bc0b365749..4a8043131d 100644 --- a/rclcpp/src/rclcpp/utilities.cpp +++ b/rclcpp/src/rclcpp/utilities.cpp @@ -22,6 +22,9 @@ #include #include +#include "rcl/error_handling.h" +#include "rcl/rcl.h" + #include "rmw/error_handling.h" #include "rmw/rmw.h" @@ -33,7 +36,7 @@ /// Represent the status of the global interrupt signal. static volatile sig_atomic_t g_signal_status = 0; /// Guard condition for interrupting the rmw implementation when the global interrupt signal fired. -static rmw_guard_condition_t * g_sigint_guard_cond_handle = rmw_create_guard_condition(); +static rcl_guard_condition_t g_sigint_guard_cond_handle = rcl_get_zero_initialized_guard_condition(); /// Condition variable for timed sleep (see sleep_for). static std::condition_variable g_interrupt_condition_variable; static std::atomic g_is_interrupted(false); @@ -77,10 +80,10 @@ signal_handler(int signal_value) } #endif g_signal_status = signal_value; - rmw_ret_t status = rmw_trigger_guard_condition(g_sigint_guard_cond_handle); - if (status != RMW_RET_OK) { + rcl_ret_t status = rcl_trigger_guard_condition(&g_sigint_guard_cond_handle); + if (status != RCL_RET_OK) { fprintf(stderr, - "[rclcpp::error] failed to trigger guard condition: %s\n", rmw_get_error_string_safe()); + "[rclcpp::error] failed to trigger guard condition: %s\n", rcl_get_error_string_safe()); } g_is_interrupted.store(true); g_interrupt_condition_variable.notify_all(); @@ -89,14 +92,11 @@ signal_handler(int signal_value) void rclcpp::utilities::init(int argc, char * argv[]) { - (void)argc; - (void)argv; g_is_interrupted.store(false); - rmw_ret_t status = rmw_init(); - if (status != RMW_RET_OK) { + if (rcl_init(argc, argv, rcl_get_default_allocator()) != RCL_RET_OK) { // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) throw std::runtime_error( - std::string("failed to initialize rmw implementation: ") + rmw_get_error_string_safe()); + std::string("failed to initialize rmw implementation: ") + rcl_get_error_string_safe()); // *INDENT-ON* } #ifdef HAS_SIGACTION @@ -138,6 +138,8 @@ rclcpp::utilities::init(int argc, char * argv[]) error_string); // *INDENT-ON* } + rcl_guard_condition_options_t options = rcl_guard_condition_get_default_options(); + rcl_guard_condition_init(&g_sigint_guard_cond_handle, options); } bool @@ -150,8 +152,7 @@ void rclcpp::utilities::shutdown() { g_signal_status = SIGINT; - rmw_ret_t status = rmw_trigger_guard_condition(g_sigint_guard_cond_handle); - if (status != RMW_RET_OK) { + if (rcl_trigger_guard_condition(&g_sigint_guard_cond_handle) != RMW_RET_OK) { fprintf(stderr, "[rclcpp::error] failed to trigger guard condition: %s\n", rmw_get_error_string_safe()); } @@ -159,10 +160,10 @@ rclcpp::utilities::shutdown() g_interrupt_condition_variable.notify_all(); } -rmw_guard_condition_t * +rcl_guard_condition_t * rclcpp::utilities::get_global_sigint_guard_condition() { - return ::g_sigint_guard_cond_handle; + return &::g_sigint_guard_cond_handle; } bool From 61f9f345d8506796db7f1a834e56f7d3426627b7 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Sun, 13 Mar 2016 15:59:05 -0700 Subject: [PATCH 12/49] Services and clients in mem strat --- rclcpp/include/rclcpp/memory_strategy.hpp | 20 ++--------- .../strategies/allocator_memory_strategy.hpp | 27 ++++++++++----- rclcpp/src/rclcpp/executor.cpp | 33 ++++++++----------- 3 files changed, 34 insertions(+), 46 deletions(-) diff --git a/rclcpp/include/rclcpp/memory_strategy.hpp b/rclcpp/include/rclcpp/memory_strategy.hpp index 6edd2d5578..cedf3567cb 100644 --- a/rclcpp/include/rclcpp/memory_strategy.hpp +++ b/rclcpp/include/rclcpp/memory_strategy.hpp @@ -42,27 +42,11 @@ class RCLCPP_PUBLIC MemoryStrategy RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(MemoryStrategy); using WeakNodeVector = std::vector>; -/* - // return the new number of subscribers - virtual size_t fill_subscriber_handles(void ** & ptr) = 0; - - // return the new number of services - virtual size_t fill_service_handles(void ** & ptr) = 0; - - // return the new number of clients - virtual size_t fill_client_handles(void ** & ptr) = 0; - - // return the new number of guard_conditions - virtual size_t fill_guard_condition_handles(void ** & ptr) = 0; - - virtual void clear_active_entities() = 0; - - virtual void clear_handles() = 0; - virtual void remove_null_handles() = 0; -*/ virtual bool collect_entities(const WeakNodeVector & weak_nodes) = 0; virtual size_t number_of_ready_subscriptions() const = 0; + virtual size_t number_of_ready_services() const = 0; + virtual size_t number_of_ready_clients() const = 0; virtual bool add_handles_to_waitset(rcl_wait_set_t * wait_set) = 0; diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index e3d00a5e27..bedc2878b3 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -198,7 +198,6 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy client_handles_.push_back(client->get_client_handle()); } } - // TODO: Add timers } } return has_invalid_weak_nodes; @@ -211,7 +210,17 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } } - // TODO: etc. + for (auto client : client_handles_) { + if (rcl_wait_set_add_client(wait_set, client) != RCL_RET_OK) { + return false; + } + } + + for (auto service : service_handles_) { + if (rcl_wait_set_add_service(wait_set, service) != RCL_RET_OK) { + return false; + } + } return true; } @@ -226,7 +235,6 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy get_next_subscription(executor::AnyExecutable::SharedPtr any_exec, const WeakNodeVector & weak_nodes) { - // TODO hmMMmmm we need to redo this whole API auto it = subscription_handles_.begin(); while (it != subscription_handles_.end()) { auto subscription = get_subscription_by_handle(*it, weak_nodes); @@ -337,17 +345,20 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy return subscription_handles_.size(); } + size_t number_of_ready_services() const { + return service_handles_.size(); + } + + size_t number_of_ready_clients() const { + return client_handles_.size(); + } + private: template using VectorRebind = std::vector::template rebind_alloc>; -/* - VectorRebind subscriptions_; - VectorRebind services_; - VectorRebind clients_; VectorRebind guard_conditions_; -*/ VectorRebind subscription_handles_; VectorRebind service_handles_; diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index cabe9d8d4a..a1854a47fd 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -316,23 +316,9 @@ Executor::execute_client( } } -/* -basic outline -get ready entities - -For all the entities, check the size of the rcl wait_set array and resize if necessary -Add the entities to the wait_set -rcl_wait -check ready subscriptions -clear subscriptions - -memory strategy is the worst!!! :( -*/ void Executor::wait_for_work(std::chrono::nanoseconds timeout) { - //memory_strategy_->clear_active_entities(); - // Collect the subscriptions and timers to be waited on bool has_invalid_weak_nodes = memory_strategy_->collect_entities(weak_nodes_); @@ -356,19 +342,26 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) rcl_wait_set_resize_subscriptions( &waitset_, memory_strategy_->number_of_ready_subscriptions()); } - // TODO etc... - // for all ready subscriptions: - // rcl_wait_set_add_subscription(&waitset_, ready_subscription); - // etc... + if (waitset_.size_of_services < memory_strategy_->number_of_ready_services()) { + rcl_wait_set_resize_services( + &waitset_, memory_strategy_->number_of_ready_services()); + } + + if (waitset_.size_of_clients < memory_strategy_->number_of_ready_clients()) { + rcl_wait_set_resize_clients( + &waitset_, memory_strategy_->number_of_ready_clients()); + } + + // TODO Only add the soonest timer to the waitset + + memory_strategy_->add_handles_to_waitset(&waitset_); rcl_ret_t status = rcl_wait(&waitset_, timeout.count()); if (status != RCL_RET_OK && status != RCL_RET_TIMEOUT && status != RCL_RET_WAIT_SET_EMPTY) { throw std::runtime_error(rcl_get_error_string_safe()); } - // Use the number of subscriptions to allocate memory in the handles - // TODO We need to add timers to the waitset now I guess!! /* rmw_subscriptions_t subscriber_handles; subscriber_handles.subscriber_count = From 69bb1760462b1cfe573f46c4c97349eb751c64d9 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Sun, 13 Mar 2016 23:30:07 -0700 Subject: [PATCH 13/49] hacking on timers --- rclcpp/include/rclcpp/timer.hpp | 39 ++++++++++++++++++++++++--------- 1 file changed, 29 insertions(+), 10 deletions(-) diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index d70a2e994b..50d03df777 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -27,6 +27,9 @@ #include "rclcpp/rate.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" + +#include "rcl/timer.h" + #include "rmw/error_handling.h" #include "rmw/rmw.h" @@ -72,9 +75,12 @@ class TimerBase virtual bool check_and_trigger() = 0; protected: + //virtual void initialize_rcl_handle(CallbackT && callback) = 0; + std::chrono::nanoseconds period_; bool canceled_; + rcl_timer_t timer_handle_; }; @@ -113,12 +119,7 @@ class GenericTimer : public TimerBase { // Stop the timer from running. cancel(); - } - - void - execute_callback() - { - execute_callback_delegate<>(); + rcl_timer_fini(&timer_handle_); } // void specialization @@ -129,9 +130,16 @@ class GenericTimer : public TimerBase >::type * = nullptr > void - execute_callback_delegate() + initialize_rcl_handle(CallbackT && callback) { - callback_(); + // TODO Should the API mirror rcl? + // rcl callback needs to wrap + auto rcl_lambda = [this](rcl_timer_t * timer, uint64_t last_call_time) { + callback_(); + }; + auto rcl_callback = std::function(rcl_lambda); + + rcl_timer_init(&timer_handle_, period_.count(), rcl_callback.target()); } template< @@ -141,11 +149,22 @@ class GenericTimer : public TimerBase >::type * = nullptr > void - execute_callback_delegate() + initialize_rcl_handle() { - callback_(*this); + auto rcl_callback = std::function([this](rcl_timer_t * timer, uint64_t last_call_time) { + callback_(*this); + }); + rcl_timer_init(&timer_handle_, period_.count(), rcl_callback.target()); } + + void + execute_callback() + { + rcl_timer_call(&timer_handle_); + } + + bool check_and_trigger() { From 9bfbb8e6d00b8c9790d7ed4610603cef5f266a36 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Mon, 14 Mar 2016 10:43:02 -0700 Subject: [PATCH 14/49] Reduce Timer API --- rclcpp/include/rclcpp/timer.hpp | 70 +++++++++++---------------------- rclcpp/src/rclcpp/executor.cpp | 2 +- rclcpp/src/rclcpp/timer.cpp | 8 ++-- 3 files changed, 29 insertions(+), 51 deletions(-) diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index 50d03df777..7ff50eee33 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -44,7 +44,7 @@ class TimerBase RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(TimerBase); RCLCPP_PUBLIC - explicit TimerBase(std::chrono::nanoseconds period); + explicit TimerBase(); RCLCPP_PUBLIC virtual ~TimerBase(); @@ -66,20 +66,15 @@ class TimerBase // \return True if the clock used by this timer is steady. virtual bool is_steady() = 0; - /// Check if the timer needs to trigger the callback. + /// Check if the timer is ready to trigger the callback. /** * This function expects its caller to immediately trigger the callback after this function, * since it maintains the last time the callback was triggered. * \return True if the timer needs to trigger. */ - virtual bool check_and_trigger() = 0; + virtual bool is_ready() = 0; protected: - //virtual void initialize_rcl_handle(CallbackT && callback) = 0; - - std::chrono::nanoseconds period_; - - bool canceled_; rcl_timer_t timer_handle_; }; @@ -108,10 +103,9 @@ class GenericTimer : public TimerBase * \param[in] callback User-specified callback function. */ GenericTimer(std::chrono::nanoseconds period, FunctorT && callback) - : TimerBase(period), callback_(std::forward(callback)), loop_rate_(period) + : TimerBase(period), callback_(std::forward(callback)) { - /* Set last_triggered_time_ so that the timer fires at least one period after being created. */ - last_triggered_time_ = Clock::now(); + initialize_rcl_handle(period); } /// Default destructor. @@ -130,16 +124,12 @@ class GenericTimer : public TimerBase >::type * = nullptr > void - initialize_rcl_handle(CallbackT && callback) + initialize_rcl_handle(std::chrono::nanoseconds & period) { - // TODO Should the API mirror rcl? - // rcl callback needs to wrap - auto rcl_lambda = [this](rcl_timer_t * timer, uint64_t last_call_time) { + auto rcl_callback = std::function([this](rcl_timer_t * timer, uint64_t last_call_time) { callback_(); - }; - auto rcl_callback = std::function(rcl_lambda); - - rcl_timer_init(&timer_handle_, period_.count(), rcl_callback.target()); + }); + rcl_timer_init(&timer_handle_, period.count(), rcl_callback.target(), rcl_get_default_allocator()); } template< @@ -149,53 +139,43 @@ class GenericTimer : public TimerBase >::type * = nullptr > void - initialize_rcl_handle() + initialize_rcl_handle(std::chrono::nanoseconds & period) { - auto rcl_callback = std::function([this](rcl_timer_t * timer, uint64_t last_call_time) { + auto rcl_callback = std::function([this](rcl_timer_t * timer, uint64_t last_call_time) { callback_(*this); }); - rcl_timer_init(&timer_handle_, period_.count(), rcl_callback.target()); + rcl_timer_init(&timer_handle_, period.count(), rcl_callback.target(), rcl_get_default_allocator()); } void execute_callback() { - rcl_timer_call(&timer_handle_); + if (rcl_timer_call(&timer_handle_) != RCL_RET_OK) { + throw std::runtime_error("Execution of timer callback failed"); + }; } bool - check_and_trigger() + is_ready() { - if (canceled_) { - return false; - } - if (Clock::now() < last_triggered_time_) { - return false; - } - if (std::chrono::duration_cast(Clock::now() - last_triggered_time_) >= - loop_rate_.period()) + bool ready = false; + if (rcl_timer_is_ready(&timer_handle_, &ready) != RCL_RET_OK) { - last_triggered_time_ = Clock::now(); - return true; + throw std::runtime_error("Timer check failed"); } - return false; + return ready; } std::chrono::nanoseconds time_until_trigger() { - std::chrono::nanoseconds time_until_trigger; - // Calculate the time between the next trigger and the current time - if (last_triggered_time_ + loop_rate_.period() < Clock::now()) { - // time is overdue, need to trigger immediately - time_until_trigger = std::chrono::nanoseconds::zero(); - } else { - time_until_trigger = std::chrono::duration_cast( - last_triggered_time_ - Clock::now()) + loop_rate_.period(); + int64_t time_until_next_call = 0; + if (rcl_timer_get_time_until_next_call(&timer_handle_, &time_until_next_call) != RCL_RET_OK) { + throw std::runtime_error("Timer could not get time until next call"); } - return time_until_trigger; + return std::chrono::nanoseconds(time_until_next_call); } virtual bool @@ -208,8 +188,6 @@ class GenericTimer : public TimerBase RCLCPP_DISABLE_COPY(GenericTimer); FunctorT callback_; - rclcpp::rate::GenericRate loop_rate_; - std::chrono::time_point last_triggered_time_; }; template diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index a1854a47fd..53e4b02216 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -480,7 +480,7 @@ Executor::get_next_timer(AnyExecutable::SharedPtr any_exec) } for (auto & timer_ref : group->get_timer_ptrs()) { auto timer = timer_ref.lock(); - if (timer && timer->check_and_trigger()) { + if (timer && timer->is_ready()) { any_exec->timer = timer; any_exec->callback_group = group; node = get_node_by_group(group); diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index 8b1b09c207..f7fa2cd930 100644 --- a/rclcpp/src/rclcpp/timer.cpp +++ b/rclcpp/src/rclcpp/timer.cpp @@ -18,9 +18,7 @@ using rclcpp::timer::TimerBase; -TimerBase::TimerBase(std::chrono::nanoseconds period) -: period_(period), - canceled_(false) +TimerBase::TimerBase() {} TimerBase::~TimerBase() @@ -29,5 +27,7 @@ TimerBase::~TimerBase() void TimerBase::cancel() { - this->canceled_ = true; + if (rcl_timer_cancel(&timer_handle_) != RCL_RET_OK) { + throw std::runtime_error("Couldn't cancel timer"); + } } From 84731024c93c6f201a59274d9d0d8dee45e92a01 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Mon, 14 Mar 2016 10:47:14 -0700 Subject: [PATCH 15/49] Make some pure virtual functions in Timer implemented in the base class --- rclcpp/include/rclcpp/timer.hpp | 41 ++++----------------------------- rclcpp/src/rclcpp/timer.cpp | 29 +++++++++++++++++++++++ 2 files changed, 34 insertions(+), 36 deletions(-) diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index 7ff50eee33..bad6866d67 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -54,13 +54,13 @@ class TimerBase cancel(); RCLCPP_PUBLIC - virtual void - execute_callback() = 0; + void + execute_callback(); /// Check how long the timer has until its next scheduled callback. // \return A std::chrono::duration representing the relative time until the next callback. - virtual std::chrono::nanoseconds - time_until_trigger() = 0; + std::chrono::nanoseconds + time_until_trigger(); /// Is the clock steady (i.e. is the time between ticks constant?) // \return True if the clock used by this timer is steady. @@ -72,7 +72,7 @@ class TimerBase * since it maintains the last time the callback was triggered. * \return True if the timer needs to trigger. */ - virtual bool is_ready() = 0; + bool is_ready(); protected: rcl_timer_t timer_handle_; @@ -147,37 +147,6 @@ class GenericTimer : public TimerBase rcl_timer_init(&timer_handle_, period.count(), rcl_callback.target(), rcl_get_default_allocator()); } - - void - execute_callback() - { - if (rcl_timer_call(&timer_handle_) != RCL_RET_OK) { - throw std::runtime_error("Execution of timer callback failed"); - }; - } - - - bool - is_ready() - { - bool ready = false; - if (rcl_timer_is_ready(&timer_handle_, &ready) != RCL_RET_OK) - { - throw std::runtime_error("Timer check failed"); - } - return ready; - } - - std::chrono::nanoseconds - 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) { - throw std::runtime_error("Timer could not get time until next call"); - } - return std::chrono::nanoseconds(time_until_next_call); - } - virtual bool is_steady() { diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index f7fa2cd930..7629f08f8d 100644 --- a/rclcpp/src/rclcpp/timer.cpp +++ b/rclcpp/src/rclcpp/timer.cpp @@ -31,3 +31,32 @@ TimerBase::cancel() throw std::runtime_error("Couldn't cancel timer"); } } + +void +TimerBase::execute_callback() +{ + if (rcl_timer_call(&timer_handle_) != RCL_RET_OK) { + throw std::runtime_error("Execution of timer callback failed"); + }; +} + +bool +TimerBase::is_ready() +{ + bool ready = false; + if (rcl_timer_is_ready(&timer_handle_, &ready) != RCL_RET_OK) + { + throw std::runtime_error("Timer check failed"); + } + return ready; +} + +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) { + throw std::runtime_error("Timer could not get time until next call"); + } + return std::chrono::nanoseconds(time_until_next_call); +} From d65ae9b92516586ef09b46584f062ce31f4c1ea9 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Tue, 15 Mar 2016 09:28:08 -0700 Subject: [PATCH 16/49] Add rcl to get_rclcpp_information --- rclcpp/cmake/get_rclcpp_information.cmake | 1 + 1 file changed, 1 insertion(+) diff --git a/rclcpp/cmake/get_rclcpp_information.cmake b/rclcpp/cmake/get_rclcpp_information.cmake index 309c6b7985..fd9254da08 100644 --- a/rclcpp/cmake/get_rclcpp_information.cmake +++ b/rclcpp/cmake/get_rclcpp_information.cmake @@ -64,6 +64,7 @@ macro(get_rclcpp_information rmw_implementation var_prefix) set(_exported_dependencies "rcl_interfaces" "rmw" + "rcl" "${rmw_implementation}" "rosidl_generator_cpp") set(${var_prefix}_DEFINITIONS) From cb8c54c141775745a20d0df85916dbff558ecb1a Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Tue, 15 Mar 2016 11:22:59 -0700 Subject: [PATCH 17/49] Change heap-allocated rcl handles to stack-allocated, add all timers to waitset --- rclcpp/include/rclcpp/client.hpp | 26 +++-- rclcpp/include/rclcpp/executor.hpp | 4 - rclcpp/include/rclcpp/node_impl.hpp | 104 +++--------------- rclcpp/include/rclcpp/publisher.hpp | 30 +++-- rclcpp/include/rclcpp/service.hpp | 26 +++-- .../strategies/allocator_memory_strategy.hpp | 13 +++ rclcpp/include/rclcpp/subscription.hpp | 39 +++++-- rclcpp/include/rclcpp/timer.hpp | 20 +++- rclcpp/src/rclcpp/client.cpp | 14 +-- rclcpp/src/rclcpp/executor.cpp | 102 +++-------------- rclcpp/src/rclcpp/publisher.cpp | 53 ++++----- rclcpp/src/rclcpp/service.cpp | 18 ++- rclcpp/src/rclcpp/subscription.cpp | 40 +++---- rclcpp/src/rclcpp/timer.cpp | 12 +- 14 files changed, 210 insertions(+), 291 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 0cb0939f41..fe483f308d 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -29,6 +29,7 @@ #include "rclcpp/function_traits.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/utilities.hpp" +#include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" #include "rmw/error_handling.h" #include "rmw/rmw.h" @@ -46,7 +47,6 @@ class ClientBase RCLCPP_PUBLIC ClientBase( std::shared_ptr node_handle, - rcl_client_t * client_handle, const std::string & service_name); RCLCPP_PUBLIC @@ -65,12 +65,12 @@ class ClientBase virtual void handle_response( std::shared_ptr request_header, std::shared_ptr response) = 0; -private: +protected: RCLCPP_DISABLE_COPY(ClientBase); std::shared_ptr node_handle_; - rcl_client_t * client_handle_; + rcl_client_t client_handle_; std::string service_name_; }; @@ -97,10 +97,22 @@ class Client : public ClientBase Client( std::shared_ptr node_handle, - rcl_client_t * client_handle, - const std::string & service_name) - : ClientBase(node_handle, client_handle, service_name) - {} + const std::string & service_name, + rcl_client_options_t & client_options) + : ClientBase(node_handle, service_name) + { + using rosidl_generator_cpp::get_service_type_support_handle; + auto service_type_support_handle = + get_service_type_support_handle(); + if (rcl_client_init(&client_handle_, this->node_handle_.get(), + service_type_support_handle, service_name.c_str(), &client_options) != RCL_RET_OK) { + // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) + throw std::runtime_error( + std::string("could not create client: ") + + rmw_get_error_string_safe()); + // *INDENT-ON* + } + } std::shared_ptr create_response() { diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index e8288a86e8..19564f0f88 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -294,10 +294,6 @@ class Executor void get_next_timer(AnyExecutable::SharedPtr any_exec); - RCLCPP_PUBLIC - std::chrono::nanoseconds - get_earliest_timer(); - RCLCPP_PUBLIC AnyExecutable::SharedPtr get_next_ready_executable(); diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index c2848f5211..5e5f711f32 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -73,38 +73,13 @@ Node::create_publisher( if (!allocator) { allocator = std::make_shared(); } - using rosidl_generator_cpp::get_message_type_support_handle; - auto type_support_handle = get_message_type_support_handle(); - rcl_publisher_t * publisher_handle = new rcl_publisher_t; - rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + auto publisher_options = rcl_publisher_get_default_options(); publisher_options.qos = qos_profile; - if (rcl_publisher_init( - publisher_handle, node_handle_.get(), type_support_handle, - topic_name.c_str(), &publisher_options) != RCL_RET_OK) - { - throw std::runtime_error( - std::string("could not create publisher: ") + - rcl_get_error_string_safe()); - } - auto publisher = publisher::Publisher::make_shared( - node_handle_, publisher_handle, topic_name, qos_profile.depth, allocator); + node_handle_, topic_name, publisher_options, allocator); if (use_intra_process_comms_) { - rcl_publisher_t * intra_process_publisher_handle = new rcl_publisher_t; - rcl_publisher_options_t intra_process_publisher_options = rcl_publisher_get_default_options(); - intra_process_publisher_options.qos = qos_profile; - if (rcl_publisher_init( - intra_process_publisher_handle, node_handle_.get(), - rclcpp::type_support::get_intra_process_message_msg_type_support(), - (topic_name + "__intra").c_str(), &intra_process_publisher_options) != RCL_RET_OK) - { - throw std::runtime_error( - std::string("could not create intra process publisher: ") + - rcl_get_error_string_safe()); - } - auto intra_process_manager = context_->get_sub_context(); uint64_t intra_process_publisher_id = @@ -137,10 +112,12 @@ Node::create_publisher( return message_seq; }; // *INDENT-ON* + rcl_publisher_options_t intra_process_options = rcl_publisher_get_default_options(); + intra_process_options.qos = qos_profile; publisher->setup_intra_process( intra_process_publisher_id, shared_publish_callback, - intra_process_publisher_handle); + intra_process_options); } if (rmw_trigger_guard_condition(notify_guard_condition_) != RMW_RET_OK) { throw std::runtime_error( @@ -170,55 +147,32 @@ Node::create_subscription( Alloc> any_subscription_callback(allocator); any_subscription_callback.set(std::forward(callback)); - using rosidl_generator_cpp::get_message_type_support_handle; - if (!msg_mem_strat) { msg_mem_strat = rclcpp::message_memory_strategy::MessageMemoryStrategy::create_default(); } - auto type_support_handle = get_message_type_support_handle(); // TODO Allocator - rcl_subscription_t * subscriber_handle = new rcl_subscription_t(); auto subscription_options = rcl_subscription_get_default_options(); subscription_options.qos = qos_profile; subscription_options.ignore_local_publications = ignore_local_publications; - if (rcl_subscription_init( - subscriber_handle, node_handle_.get(), type_support_handle, topic_name.c_str(), - &subscription_options) != RCL_RET_OK) - { - throw std::runtime_error( - std::string("could not create subscription: ") + rcl_get_error_string_safe()); - } using rclcpp::subscription::Subscription; using rclcpp::subscription::SubscriptionBase; auto sub = Subscription::make_shared( node_handle_, - subscriber_handle, topic_name, - ignore_local_publications, + subscription_options, any_subscription_callback, msg_mem_strat); auto sub_base_ptr = std::dynamic_pointer_cast(sub); // Setup intra process. if (use_intra_process_comms_) { - rcl_subscription_t * intra_process_subscriber_handle = new rcl_subscription_t; - auto intra_process_subscription_options = rcl_subscription_get_default_options(); - intra_process_subscription_options.qos = qos_profile; - intra_process_subscription_options.ignore_local_publications = false; - if (rcl_subscription_init( - subscriber_handle, node_handle_.get(), - rclcpp::type_support::get_intra_process_message_msg_type_support(), - (topic_name + "__intra").c_str(), - &subscription_options) != RCL_RET_OK) - { - // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) - throw std::runtime_error( - std::string("could not create intra process subscription: ") + rcl_get_error_string_safe()); - // *INDENT-ON* - } + auto intra_process_options = rcl_subscription_get_default_options(); + intra_process_options.qos = qos_profile; + intra_process_options.ignore_local_publications = false; + auto intra_process_manager = context_->get_sub_context(); rclcpp::intra_process_manager::IntraProcessManager::WeakPtr weak_ipm = intra_process_manager; @@ -227,7 +181,6 @@ Node::create_subscription( // *INDENT-OFF* sub->setup_intra_process( intra_process_subscription_id, - intra_process_subscriber_handle, [weak_ipm]( uint64_t publisher_id, uint64_t message_sequence, @@ -250,7 +203,8 @@ Node::create_subscription( "intra process publisher check called after destruction of intra process manager"); } return ipm->matches_any_publishers(sender_gid); - } + }, + intra_process_options ); // *INDENT-ON* } @@ -331,31 +285,16 @@ Node::create_client( const rmw_qos_profile_t & qos_profile, rclcpp::callback_group::CallbackGroup::SharedPtr group) { - using rosidl_generator_cpp::get_service_type_support_handle; - auto service_type_support_handle = - get_service_type_support_handle(); - - rcl_client_t * client_handle = new rcl_client_t; - rcl_client_options_t options = rcl_client_get_default_options(); options.qos = qos_profile; - rcl_client_init(client_handle, this->node_handle_.get(), - service_type_support_handle, service_name.c_str(), &options); - 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_safe()); - // *INDENT-ON* - } using rclcpp::client::Client; using rclcpp::client::ClientBase; auto cli = Client::make_shared( node_handle_, - client_handle, - service_name); + service_name, + options); auto cli_base_ptr = std::dynamic_pointer_cast(cli); if (group) { @@ -385,28 +324,15 @@ Node::create_service( const rmw_qos_profile_t & qos_profile, rclcpp::callback_group::CallbackGroup::SharedPtr group) { - using rosidl_generator_cpp::get_service_type_support_handle; - auto service_type_support_handle = - get_service_type_support_handle(); - rclcpp::service::AnyServiceCallback any_service_callback; any_service_callback.set(std::forward(callback)); - rcl_service_t * service_handle = new rcl_service_t; rcl_service_options_t service_options = rcl_service_get_default_options(); service_options.qos = qos_profile; // TODO allocator - if (rcl_service_init( - service_handle, node_handle_.get(), service_type_support_handle, service_name.c_str(), - &service_options) != RCL_RET_OK) - { - throw std::runtime_error( - std::string("could not create service: ") + - rmw_get_error_string_safe()); - } auto serv = service::Service::make_shared( - node_handle_, service_handle, service_name, any_service_callback); + node_handle_, service_name, any_service_callback, service_options); 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 623ecf2609..a8679c6fdb 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -33,6 +33,7 @@ #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/allocator/allocator_deleter.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp @@ -56,14 +57,12 @@ class PublisherBase * Typically, a publisher is not created through this method, but instead is created through a * call to `Node::create_publisher`. * \param[in] node_handle The corresponding rmw representation of the owner node. - * \param[in] publisher_handle The rmw publisher handle corresponding to this publisher. * \param[in] topic The topic that this publisher publishes on. * \param[in] queue_size The maximum number of unpublished messages to queue. */ RCLCPP_PUBLIC PublisherBase( std::shared_ptr node_handle, - rcl_publisher_t * publisher_handle, std::string topic, size_t queue_size); @@ -123,12 +122,12 @@ class PublisherBase setup_intra_process( uint64_t intra_process_publisher_id, StoreMessageCallbackT callback, - rcl_publisher_t * intra_process_publisher_handle); + rcl_publisher_options_t & intra_process_options); std::shared_ptr node_handle_; - rcl_publisher_t * publisher_handle_; - rcl_publisher_t * intra_process_publisher_handle_; + rcl_publisher_t publisher_handle_; + rcl_publisher_t intra_process_publisher_handle_; std::string topic_; size_t queue_size_; @@ -156,14 +155,25 @@ class Publisher : public PublisherBase Publisher( std::shared_ptr node_handle, - rcl_publisher_t * publisher_handle, std::string topic, - size_t queue_size, + //size_t queue_size, + rcl_publisher_options_t & publisher_options, std::shared_ptr allocator) - : PublisherBase(node_handle, publisher_handle, topic, queue_size) + : PublisherBase(node_handle, topic, publisher_options.qos.depth) { + using rosidl_generator_cpp::get_message_type_support_handle; message_allocator_ = std::make_shared(*allocator.get()); allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get()); + + auto type_support_handle = get_message_type_support_handle(); + if (rcl_publisher_init( + &publisher_handle_, node_handle_.get(), type_support_handle, + topic.c_str(), &publisher_options) != RCL_RET_OK) + { + throw std::runtime_error( + std::string("could not create publisher: ") + + rcl_get_error_string_safe()); + } } @@ -191,7 +201,7 @@ class Publisher : public PublisherBase rcl_interfaces::msg::IntraProcessMessage ipm; ipm.publisher_id = intra_process_publisher_id_; ipm.message_sequence = message_seq; - auto status = rcl_publish(intra_process_publisher_handle_, &ipm); + auto status = rcl_publish(&intra_process_publisher_handle_, &ipm); if (status != RCL_RET_OK) { // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) throw std::runtime_error( @@ -266,7 +276,7 @@ class Publisher : public PublisherBase void do_inter_process_publish(const MessageT * msg) { - auto status = rcl_publish(publisher_handle_, msg); + auto status = rcl_publish(&publisher_handle_, msg); if (status != RCL_RET_OK) { // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) throw std::runtime_error( diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index a757a20556..68e0515526 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -26,6 +26,7 @@ #include "rclcpp/any_service_callback.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" #include "rmw/error_handling.h" #include "rmw/rmw.h" @@ -43,7 +44,6 @@ class ServiceBase RCLCPP_PUBLIC ServiceBase( std::shared_ptr node_handle, - rcl_service_t * service_handle, const std::string service_name); RCLCPP_PUBLIC @@ -63,12 +63,12 @@ class ServiceBase std::shared_ptr request_header, std::shared_ptr request) = 0; -private: +protected: RCLCPP_DISABLE_COPY(ServiceBase); std::shared_ptr node_handle_; - rcl_service_t * service_handle_; + rcl_service_t service_handle_; std::string service_name_; }; @@ -92,11 +92,23 @@ class Service : public ServiceBase Service( std::shared_ptr node_handle, - rcl_service_t * service_handle, const std::string & service_name, - AnyServiceCallback any_callback) - : ServiceBase(node_handle, service_handle, service_name), any_callback_(any_callback) - {} + AnyServiceCallback any_callback, + rcl_service_options_t & service_options) + : ServiceBase(node_handle, service_name), any_callback_(any_callback) + { + using rosidl_generator_cpp::get_service_type_support_handle; + auto service_type_support_handle = get_service_type_support_handle(); + + if (rcl_service_init( + &service_handle_, node_handle.get(), service_type_support_handle, service_name.c_str(), + &service_options) != RCL_RET_OK) + { + throw std::runtime_error( + std::string("could not create service: ") + + rmw_get_error_string_safe()); + } + } Service() = delete; diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index bedc2878b3..b5799a7978 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -198,6 +198,12 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy client_handles_.push_back(client->get_client_handle()); } } + for (auto & weak_timer : group->get_timer_ptrs()) { + auto timer = weak_timer.lock(); + if (timer) { + timer_handles_.push_back(timer->get_timer_handle()); + } + } } } return has_invalid_weak_nodes; @@ -221,6 +227,12 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy return false; } } + + for (auto timer : timer_handles_) { + if (rcl_wait_set_add_timer(wait_set, timer) != RCL_RET_OK) { + return false; + } + } return true; } @@ -363,6 +375,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy 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 f2435ddd87..2b97a84387 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -31,6 +31,7 @@ #include "rclcpp/macros.hpp" #include "rclcpp/message_memory_strategy.hpp" #include "rclcpp/any_subscription_callback.hpp" +#include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp @@ -60,7 +61,6 @@ class SubscriptionBase RCLCPP_PUBLIC SubscriptionBase( std::shared_ptr node_handle, - rcl_subscription_t * subscription_handle, const std::string & topic_name, bool ignore_local_publications); @@ -103,14 +103,14 @@ class SubscriptionBase const rmw_message_info_t & message_info) = 0; protected: - rcl_subscription_t * intra_process_subscription_handle_; + rcl_subscription_t intra_process_subscription_handle_; private: RCLCPP_DISABLE_COPY(SubscriptionBase); std::shared_ptr node_handle_; - rcl_subscription_t * subscription_handle_; + rcl_subscription_t subscription_handle_; std::string topic_name_; bool ignore_local_publications_; @@ -144,19 +144,29 @@ class Subscription : public SubscriptionBase */ Subscription( std::shared_ptr node_handle, - rcl_subscription_t * subscription_handle, const std::string & topic_name, - bool ignore_local_publications, + rcl_subscription_options_t & subscription_options, AnySubscriptionCallback callback, typename message_memory_strategy::MessageMemoryStrategy::SharedPtr memory_strategy = message_memory_strategy::MessageMemoryStrategy::create_default()) - : SubscriptionBase(node_handle, subscription_handle, topic_name, ignore_local_publications), + : SubscriptionBase( + node_handle, topic_name, subscription_options.ignore_local_publications), any_callback_(callback), message_memory_strategy_(memory_strategy), get_intra_process_message_callback_(nullptr), matches_any_intra_process_publishers_(nullptr) { + using rosidl_generator_cpp::get_message_type_support_handle; + + auto type_support_handle = get_message_type_support_handle(); + if (rcl_subscription_init( + &subscription_handle_, node_handle_.get(), type_support_handle, topic_name.c_str(), + &subscription_options) != RCL_RET_OK) + { + throw std::runtime_error( + std::string("could not create subscription: ") + rcl_get_error_string_safe()); + } } /// Support dynamically setting the message memory strategy. @@ -233,12 +243,23 @@ class Subscription : public SubscriptionBase void setup_intra_process( uint64_t intra_process_subscription_id, - rcl_subscription_t * intra_process_subscription, GetMessageCallbackType get_message_callback, - MatchesAnyPublishersCallbackType matches_any_publisher_callback) + MatchesAnyPublishersCallbackType matches_any_publisher_callback, + rcl_subscription_options_t & intra_process_options) { + if (rcl_subscription_init( + &intra_process_subscription_handle_, node_handle_.get(), + rclcpp::type_support::get_intra_process_message_msg_type_support(), + (topic_name_ + "__intra").c_str(), + &intra_process_options) != RCL_RET_OK) + { + // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) + throw std::runtime_error( + std::string("could not create intra process subscription: ") + rcl_get_error_string_safe()); + // *INDENT-ON* + } + intra_process_subscription_id_ = intra_process_subscription_id; - intra_process_subscription_handle_ = intra_process_subscription; get_intra_process_message_callback_ = get_message_callback; matches_any_intra_process_publishers_ = matches_any_publisher_callback; } diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index bad6866d67..e755bcae7c 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -44,7 +44,7 @@ class TimerBase RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(TimerBase); RCLCPP_PUBLIC - explicit TimerBase(); + explicit TimerBase(std::chrono::nanoseconds period); RCLCPP_PUBLIC virtual ~TimerBase(); @@ -57,6 +57,10 @@ class TimerBase void execute_callback(); + RCLCPP_PUBLIC + const rcl_timer_t * + get_timer_handle(); + /// Check how long the timer has until its next scheduled callback. // \return A std::chrono::duration representing the relative time until the next callback. std::chrono::nanoseconds @@ -126,10 +130,13 @@ class GenericTimer : public TimerBase void initialize_rcl_handle(std::chrono::nanoseconds & period) { - auto rcl_callback = std::function([this](rcl_timer_t * timer, uint64_t last_call_time) { + auto rcl_callback = std::function( + [this](rcl_timer_t * timer, uint64_t last_call_time) { callback_(); }); - rcl_timer_init(&timer_handle_, period.count(), rcl_callback.target(), rcl_get_default_allocator()); + rcl_timer_init( + &timer_handle_, period.count(), rcl_callback.target(), + rcl_get_default_allocator()); } template< @@ -141,10 +148,13 @@ class GenericTimer : public TimerBase void initialize_rcl_handle(std::chrono::nanoseconds & period) { - auto rcl_callback = std::function([this](rcl_timer_t * timer, uint64_t last_call_time) { + auto rcl_callback = std::function( + [this](rcl_timer_t * timer, uint64_t last_call_time) { callback_(*this); }); - rcl_timer_init(&timer_handle_, period.count(), rcl_callback.target(), rcl_get_default_allocator()); + rcl_timer_init( + &timer_handle_, period.count(), rcl_callback.target(), + rcl_get_default_allocator()); } virtual bool diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index 6d4fb9bfd5..768a80fa10 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -23,19 +23,15 @@ using rclcpp::client::ClientBase; ClientBase::ClientBase( std::shared_ptr node_handle, - rcl_client_t * client_handle, const std::string & service_name) -: node_handle_(node_handle), client_handle_(client_handle), service_name_(service_name) +: node_handle_(node_handle), service_name_(service_name) {} ClientBase::~ClientBase() { - if (client_handle_) { - if (rcl_client_fini(client_handle_, node_handle_.get()) != RMW_RET_OK) { - fprintf(stderr, - "Error in destruction of rmw client handle: %s\n", rmw_get_error_string_safe()); - } - delete client_handle_; + if (rcl_client_fini(&client_handle_, node_handle_.get()) != RMW_RET_OK) { + fprintf(stderr, + "Error in destruction of rmw client handle: %s\n", rmw_get_error_string_safe()); } } @@ -48,5 +44,5 @@ ClientBase::get_service_name() const const rcl_client_t * ClientBase::get_client_handle() const { - return this->client_handle_; + return &client_handle_; } diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 53e4b02216..4407d73e14 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -339,18 +339,27 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) memory_strategy_->clear_handles(); if (waitset_.size_of_subscriptions < memory_strategy_->number_of_ready_subscriptions()) { - rcl_wait_set_resize_subscriptions( - &waitset_, memory_strategy_->number_of_ready_subscriptions()); + if (rcl_wait_set_resize_subscriptions( + &waitset_, memory_strategy_->number_of_ready_subscriptions()) != RCL_RET_OK) + { + throw std::runtime_error("Couldn't resize waitset to number of subscriptions"); + } } if (waitset_.size_of_services < memory_strategy_->number_of_ready_services()) { - rcl_wait_set_resize_services( - &waitset_, memory_strategy_->number_of_ready_services()); + if (rcl_wait_set_resize_services( + &waitset_, memory_strategy_->number_of_ready_services()) != RCL_RET_OK) + { + throw std::runtime_error("Couldn't resize waitset to number of services"); + } } if (waitset_.size_of_clients < memory_strategy_->number_of_ready_clients()) { - rcl_wait_set_resize_clients( - &waitset_, memory_strategy_->number_of_ready_clients()); + if (rcl_wait_set_resize_clients( + &waitset_, memory_strategy_->number_of_ready_clients()) != RCL_RET_OK) + { + throw std::runtime_error("Couldn't resize waitset to number of clients"); + } } // TODO Only add the soonest timer to the waitset @@ -363,61 +372,11 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) } /* - rmw_subscriptions_t subscriber_handles; - subscriber_handles.subscriber_count = - memory_strategy_->fill_subscriber_handles(subscriber_handles.subscribers); - - rmw_services_t service_handles; - service_handles.service_count = - memory_strategy_->fill_service_handles(service_handles.services); - - rmw_clients_t client_handles; - client_handles.client_count = - memory_strategy_->fill_client_handles(client_handles.clients); - // construct a guard conditions struct rmw_guard_conditions_t guard_conditions; guard_conditions.guard_condition_count = memory_strategy_->fill_guard_condition_handles(guard_conditions.guard_conditions); - - rmw_time_t * wait_timeout = NULL; - rmw_time_t rmw_timeout; - - auto next_timer_duration = get_earliest_timer(); - // If the next timer timeout must preempt the requested timeout - // or if the requested timeout blocks forever, and there exists a valid timer, - // replace the requested timeout with the next timeout. - bool has_valid_timer = next_timer_duration >= std::chrono::nanoseconds::zero(); - if ((next_timer_duration < timeout || - timeout < std::chrono::nanoseconds::zero()) && has_valid_timer) - { - rmw_timeout.sec = - std::chrono::duration_cast(next_timer_duration).count(); - rmw_timeout.nsec = next_timer_duration.count() % (1000 * 1000 * 1000); - wait_timeout = &rmw_timeout; - } else if (timeout >= std::chrono::nanoseconds::zero()) { - // Convert timeout representation to rmw_time - rmw_timeout.sec = std::chrono::duration_cast(timeout).count(); - rmw_timeout.nsec = std::chrono::duration_cast(timeout).count() % - (1000 * 1000 * 1000); - wait_timeout = &rmw_timeout; - } - - // Now wait on the waitable subscriptions and timers - rmw_ret_t status = rmw_wait( - &subscriber_handles, - &guard_conditions, - &service_handles, - &client_handles, - waitset_, - wait_timeout); - if (status != RMW_RET_OK && status != RMW_RET_TIMEOUT) { - throw std::runtime_error(rmw_get_error_string_safe()); - } */ - - // eh? I think rcl_wait_set does this for us! - // memory_strategy_->remove_null_handles(); } rclcpp::node::Node::SharedPtr @@ -491,37 +450,6 @@ Executor::get_next_timer(AnyExecutable::SharedPtr any_exec) } } -std::chrono::nanoseconds -Executor::get_earliest_timer() -{ - std::chrono::nanoseconds latest = std::chrono::nanoseconds::max(); - bool timers_empty = true; - for (auto & weak_node : weak_nodes_) { - auto node = weak_node.lock(); - if (!node) { - continue; - } - for (auto & weak_group : node->get_callback_groups()) { - auto group = weak_group.lock(); - if (!group || !group->can_be_taken_from().load()) { - continue; - } - for (auto & timer_ref : group->get_timer_ptrs()) { - timers_empty = false; - // Check the expected trigger time - auto timer = timer_ref.lock(); - if (timer && timer->time_until_trigger() < latest) { - latest = timer->time_until_trigger(); - } - } - } - } - if (timers_empty) { - return std::chrono::nanoseconds(-1); - } - return latest; -} - AnyExecutable::SharedPtr Executor::get_next_ready_executable() { diff --git a/rclcpp/src/rclcpp/publisher.cpp b/rclcpp/src/rclcpp/publisher.cpp index 662581edf8..0073d1a034 100644 --- a/rclcpp/src/rclcpp/publisher.cpp +++ b/rclcpp/src/rclcpp/publisher.cpp @@ -34,17 +34,17 @@ using rclcpp::publisher::PublisherBase; PublisherBase::PublisherBase( std::shared_ptr node_handle, - rcl_publisher_t * publisher_handle, std::string topic, size_t queue_size) -: node_handle_(node_handle), publisher_handle_(publisher_handle), - intra_process_publisher_handle_(nullptr), +: node_handle_(node_handle), topic_(topic), queue_size_(queue_size), intra_process_publisher_id_(0), store_intra_process_message_(nullptr) { // Life time of this object is tied to the publisher handle. + // TODO this won't be valid until we initialize the publisher handle. argh + // What is the point of this check? if (rmw_get_gid_for_publisher( - rcl_publisher_get_rmw_handle(publisher_handle_), &rmw_gid_) != RMW_RET_OK) + rcl_publisher_get_rmw_handle(&publisher_handle_), &rmw_gid_) != RMW_RET_OK) { // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) throw std::runtime_error( @@ -55,27 +55,19 @@ PublisherBase::PublisherBase( PublisherBase::~PublisherBase() { - if (intra_process_publisher_handle_) { - if (rcl_publisher_fini(intra_process_publisher_handle_, node_handle_.get()) != RCL_RET_OK) { - fprintf( - stderr, - "Error in destruction of intra process rcl publisher handle: %s\n", - rcl_get_error_string_safe()); - - } - // TODO custom deallocator - delete intra_process_publisher_handle_; + if (rcl_publisher_fini(&intra_process_publisher_handle_, node_handle_.get()) != RCL_RET_OK) { + fprintf( + stderr, + "Error in destruction of intra process rcl publisher handle: %s\n", + rcl_get_error_string_safe()); } - if (publisher_handle_) { - if (rcl_publisher_fini(publisher_handle_, node_handle_.get()) != RCL_RET_OK) { - fprintf( - stderr, - "Error in destruction of rcl publisher handle: %s\n", - rcl_get_error_string_safe()); - } - // TODO custom deallocator - delete publisher_handle_; } + if (rcl_publisher_fini(&publisher_handle_, node_handle_.get()) != RCL_RET_OK) { + fprintf( + stderr, + "Error in destruction of rcl publisher handle: %s\n", + rcl_get_error_string_safe()); + } } const std::string & @@ -131,14 +123,23 @@ void PublisherBase::setup_intra_process( uint64_t intra_process_publisher_id, StoreMessageCallbackT callback, - rcl_publisher_t * intra_process_publisher_handle) + rcl_publisher_options_t & intra_process_options) { + if (rcl_publisher_init( + &intra_process_publisher_handle_, node_handle_.get(), + rclcpp::type_support::get_intra_process_message_msg_type_support(), + (topic_ + "__intra").c_str(), &intra_process_options) != RCL_RET_OK) + { + throw std::runtime_error( + std::string("could not create intra process publisher: ") + + rcl_get_error_string_safe()); + } + intra_process_publisher_id_ = intra_process_publisher_id; store_intra_process_message_ = callback; - intra_process_publisher_handle_ = intra_process_publisher_handle; // Life time of this object is tied to the publisher handle. auto ret = rmw_get_gid_for_publisher( - rcl_publisher_get_rmw_handle(intra_process_publisher_handle_), &intra_process_rmw_gid_); + rcl_publisher_get_rmw_handle(&intra_process_publisher_handle_), &intra_process_rmw_gid_); if (ret != RMW_RET_OK) { // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) throw std::runtime_error( diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index dcf21edb69..12587afc23 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -29,21 +29,17 @@ using rclcpp::service::ServiceBase; ServiceBase::ServiceBase( std::shared_ptr node_handle, - rcl_service_t * service_handle, const std::string service_name) -: node_handle_(node_handle), service_handle_(service_handle), service_name_(service_name) +: node_handle_(node_handle), service_name_(service_name) {} ServiceBase::~ServiceBase() { - if (service_handle_) { - if (rcl_service_fini(service_handle_, node_handle_.get())) { - std::stringstream ss; - ss << "Error in destruction of rmw service_handle_ handle: " << - rmw_get_error_string_safe() << '\n'; - (std::cerr << ss.str()).flush(); - } - delete service_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(); } } @@ -56,5 +52,5 @@ ServiceBase::get_service_name() const rcl_service_t * ServiceBase::get_service_handle() { - return this->service_handle_; + return &service_handle_; } diff --git a/rclcpp/src/rclcpp/subscription.cpp b/rclcpp/src/rclcpp/subscription.cpp index 6f2413e2cd..51c21d4504 100644 --- a/rclcpp/src/rclcpp/subscription.cpp +++ b/rclcpp/src/rclcpp/subscription.cpp @@ -25,12 +25,9 @@ using rclcpp::subscription::SubscriptionBase; SubscriptionBase::SubscriptionBase( std::shared_ptr node_handle, - rcl_subscription_t * subscription_handle, const std::string & topic_name, bool ignore_local_publications) -: intra_process_subscription_handle_(nullptr), - node_handle_(node_handle), - subscription_handle_(subscription_handle), +: node_handle_(node_handle), topic_name_(topic_name), ignore_local_publications_(ignore_local_publications) { @@ -40,27 +37,20 @@ SubscriptionBase::SubscriptionBase( SubscriptionBase::~SubscriptionBase() { - if (subscription_handle_) { - // TODO - if (rcl_subscription_fini(subscription_handle_, node_handle_.get()) != RMW_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(&subscription_handle_, node_handle_.get()) != RMW_RET_OK) { + std::stringstream ss; + ss << "Error in destruction of rcl subscription handle: " << + rcl_get_error_string_safe() << '\n'; + (std::cerr << ss.str()).flush(); } - // TODO: deallocate with allocator - delete subscription_handle_; - if (intra_process_subscription_handle_) { - auto ret = rcl_subscription_fini(intra_process_subscription_handle_, node_handle_.get()); - if (ret != 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(); - } + 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(); } - delete intra_process_subscription_handle_; } const std::string & @@ -72,11 +62,11 @@ SubscriptionBase::get_topic_name() const const rcl_subscription_t * SubscriptionBase::get_subscription_handle() const { - return subscription_handle_; + return &subscription_handle_; } const rcl_subscription_t * 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 7629f08f8d..d28dee1ac5 100644 --- a/rclcpp/src/rclcpp/timer.cpp +++ b/rclcpp/src/rclcpp/timer.cpp @@ -18,8 +18,10 @@ using rclcpp::timer::TimerBase; -TimerBase::TimerBase() -{} +TimerBase::TimerBase(std::chrono::nanoseconds period) +{ + (void)period; +} TimerBase::~TimerBase() {} @@ -60,3 +62,9 @@ TimerBase::time_until_trigger() } return std::chrono::nanoseconds(time_until_next_call); } + +const rcl_timer_t * +TimerBase::get_timer_handle() +{ + return &timer_handle_; +} From 486e18766cf3160710f3c13fd34487ecf0dd785f Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Tue, 15 Mar 2016 12:07:01 -0700 Subject: [PATCH 18/49] Starting to use rcl allocators --- .../rclcpp/allocator/allocator_common.hpp | 35 +++++++++++++++++++ rclcpp/include/rclcpp/node_impl.hpp | 2 ++ 2 files changed, 37 insertions(+) diff --git a/rclcpp/include/rclcpp/allocator/allocator_common.hpp b/rclcpp/include/rclcpp/allocator/allocator_common.hpp index d32b200ced..78882dc212 100644 --- a/rclcpp/include/rclcpp/allocator/allocator_common.hpp +++ b/rclcpp/include/rclcpp/allocator/allocator_common.hpp @@ -17,6 +17,8 @@ #include +#include "rcl/allocator.h" + #include "rclcpp/allocator/allocator_deleter.hpp" namespace rclcpp @@ -27,6 +29,39 @@ namespace allocator template using AllocRebind = typename std::allocator_traits::template rebind_traits; +// Convert a std::allocator_traits-formatted Allocator into an rcl allocator +// TODO: Do we need to specify a value type for the Allocator? I think we do... o_o +template +rcl_allocator_t get_rcl_allocator(Alloc & allocator) +{ + rcl_allocator_t rcl_allocator = rcl_get_default_allocator(); + + // Argh + auto allocate_callback = std::function( + [](size_t size, void * state) { + auto allocator_ptr = static_cast(state); + // TODO check if null + std::allocator_traits::allocate(*allocator_ptr, size); + }); + auto deallocate_callback = std::function( + [](void * pointer, void * state) { + auto allocator_ptr = static_cast(state); + // TODO check if null + // TODO size? + auto typed_pointer = static_cast(pointer); + std::allocator_traits::deallocate(*allocator_ptr, typed_pointer, sizeof(T)); + }); + + rcl_allocator.allocate = allocate_callback; + rcl_allocator.deallocate = deallocate_callback; + rcl_allocator.reallocate = nullptr; + rcl_allocator.state = &allocator; + return std::move(rcl_allocator); +} + +// TODO provide a nicer way of making a C++ allocator maybe? +// or passing rcl allcoators directly? + } // namespace allocator } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 5e5f711f32..a30a48e0fe 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -76,6 +76,8 @@ Node::create_publisher( auto publisher_options = rcl_publisher_get_default_options(); publisher_options.qos = qos_profile; + publisher_options.allocator = rclcpp::allocator::get_rcl_allocator( + *allocator.get()); auto publisher = publisher::Publisher::make_shared( node_handle_, topic_name, publisher_options, allocator); From ea54212538698e1703c1f9ceac985d1b5a67b455 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Tue, 15 Mar 2016 16:11:08 -0700 Subject: [PATCH 19/49] Finish allocator interface --- .../rclcpp/allocator/allocator_common.hpp | 26 ++++++++++++------- rclcpp/include/rclcpp/node_impl.hpp | 8 +++++- 2 files changed, 24 insertions(+), 10 deletions(-) diff --git a/rclcpp/include/rclcpp/allocator/allocator_common.hpp b/rclcpp/include/rclcpp/allocator/allocator_common.hpp index 78882dc212..e535145cf8 100644 --- a/rclcpp/include/rclcpp/allocator/allocator_common.hpp +++ b/rclcpp/include/rclcpp/allocator/allocator_common.hpp @@ -30,19 +30,18 @@ template using AllocRebind = typename std::allocator_traits::template rebind_traits; // Convert a std::allocator_traits-formatted Allocator into an rcl allocator -// TODO: Do we need to specify a value type for the Allocator? I think we do... o_o -template +template>::value>::type * = nullptr> rcl_allocator_t get_rcl_allocator(Alloc & allocator) { rcl_allocator_t rcl_allocator = rcl_get_default_allocator(); // Argh - auto allocate_callback = std::function( - [](size_t size, void * state) { + auto allocate_callback = std::function( + [](size_t size, void * state) -> void * { auto allocator_ptr = static_cast(state); // TODO check if null - std::allocator_traits::allocate(*allocator_ptr, size); - }); + return std::allocator_traits::allocate(*allocator_ptr, size); + }).target(); auto deallocate_callback = std::function( [](void * pointer, void * state) { auto allocator_ptr = static_cast(state); @@ -50,17 +49,26 @@ rcl_allocator_t get_rcl_allocator(Alloc & allocator) // TODO size? auto typed_pointer = static_cast(pointer); std::allocator_traits::deallocate(*allocator_ptr, typed_pointer, sizeof(T)); - }); + }).target(); rcl_allocator.allocate = allocate_callback; rcl_allocator.deallocate = deallocate_callback; rcl_allocator.reallocate = nullptr; rcl_allocator.state = &allocator; - return std::move(rcl_allocator); + return rcl_allocator; +} + +// TODO(jacquelinekay) Workaround for an incomplete implementation of std::allocator +template>::value>::type * = nullptr> +rcl_allocator_t get_rcl_allocator(Alloc & allocator) +{ + (void)allocator; + return rcl_get_default_allocator(); } + // TODO provide a nicer way of making a C++ allocator maybe? -// or passing rcl allcoators directly? +// or passing rcl allocators directly? } // namespace allocator } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index a30a48e0fe..07d6502250 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -116,6 +116,8 @@ Node::create_publisher( // *INDENT-ON* rcl_publisher_options_t intra_process_options = rcl_publisher_get_default_options(); intra_process_options.qos = qos_profile; + intra_process_options.allocator = rclcpp::allocator::get_rcl_allocator( + *allocator.get()); publisher->setup_intra_process( intra_process_publisher_id, shared_publish_callback, @@ -157,6 +159,8 @@ Node::create_subscription( // TODO Allocator auto subscription_options = rcl_subscription_get_default_options(); subscription_options.qos = qos_profile; + subscription_options.allocator = rclcpp::allocator::get_rcl_allocator( + *allocator.get()); subscription_options.ignore_local_publications = ignore_local_publications; using rclcpp::subscription::Subscription; @@ -172,6 +176,8 @@ Node::create_subscription( // Setup intra process. if (use_intra_process_comms_) { auto intra_process_options = rcl_subscription_get_default_options(); + intra_process_options.allocator = rclcpp::allocator::get_rcl_allocator( + *allocator.get()); intra_process_options.qos = qos_profile; intra_process_options.ignore_local_publications = false; @@ -289,6 +295,7 @@ Node::create_client( { rcl_client_options_t options = rcl_client_get_default_options(); options.qos = qos_profile; + // options.allocator = rclcpp::allocator::get_rcl_allocator(*allocator.get()); using rclcpp::client::Client; using rclcpp::client::ClientBase; @@ -331,7 +338,6 @@ Node::create_service( rcl_service_options_t service_options = rcl_service_get_default_options(); service_options.qos = qos_profile; - // TODO allocator auto serv = service::Service::make_shared( node_handle_, service_name, any_service_callback, service_options); From 06d66a4edf895c128f7ef7d58db7a40a7d10bcac Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Wed, 16 Mar 2016 11:07:03 -0700 Subject: [PATCH 20/49] Address some warnings due to allocators --- .../rclcpp/allocator/allocator_common.hpp | 6 ++--- rclcpp/include/rclcpp/client.hpp | 2 +- rclcpp/include/rclcpp/node_impl.hpp | 24 +++++++++++-------- rclcpp/include/rclcpp/publisher.hpp | 6 ++--- rclcpp/include/rclcpp/service.hpp | 2 +- .../strategies/allocator_memory_strategy.hpp | 4 ++++ rclcpp/include/rclcpp/subscription.hpp | 9 +++---- rclcpp/include/rclcpp/timer.hpp | 23 ++++++++++++++---- rclcpp/src/rclcpp/executor.cpp | 24 ++++++++++++------- rclcpp/src/rclcpp/timer.cpp | 10 ++++---- 10 files changed, 68 insertions(+), 42 deletions(-) diff --git a/rclcpp/include/rclcpp/allocator/allocator_common.hpp b/rclcpp/include/rclcpp/allocator/allocator_common.hpp index e535145cf8..5b7e2d364e 100644 --- a/rclcpp/include/rclcpp/allocator/allocator_common.hpp +++ b/rclcpp/include/rclcpp/allocator/allocator_common.hpp @@ -46,9 +46,9 @@ rcl_allocator_t get_rcl_allocator(Alloc & allocator) [](void * pointer, void * state) { auto allocator_ptr = static_cast(state); // TODO check if null - // TODO size? auto typed_pointer = static_cast(pointer); - std::allocator_traits::deallocate(*allocator_ptr, typed_pointer, sizeof(T)); + // TODO size: we don't know how many entries were allocated + std::allocator_traits::deallocate(*allocator_ptr, typed_pointer, 1); }).target(); rcl_allocator.allocate = allocate_callback; @@ -66,10 +66,10 @@ rcl_allocator_t get_rcl_allocator(Alloc & allocator) return rcl_get_default_allocator(); } - // TODO provide a nicer way of making a C++ allocator maybe? // or passing rcl allocators directly? + } // namespace allocator } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index fe483f308d..fe53219c4b 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -109,7 +109,7 @@ class Client : public ClientBase // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) throw std::runtime_error( std::string("could not create client: ") + - rmw_get_error_string_safe()); + rcl_get_error_string_safe()); // *INDENT-ON* } } diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 07d6502250..2511d5b54f 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -76,10 +76,13 @@ Node::create_publisher( auto publisher_options = rcl_publisher_get_default_options(); publisher_options.qos = qos_profile; - publisher_options.allocator = rclcpp::allocator::get_rcl_allocator( - *allocator.get()); + + auto message_alloc = std::make_shared::MessageAlloc>(*allocator.get()); + publisher_options.allocator = allocator::get_rcl_allocator( + *message_alloc.get()); + auto publisher = publisher::Publisher::make_shared( - node_handle_, topic_name, publisher_options, allocator); + node_handle_, topic_name, publisher_options, message_alloc); if (use_intra_process_comms_) { auto intra_process_manager = @@ -116,8 +119,8 @@ Node::create_publisher( // *INDENT-ON* rcl_publisher_options_t intra_process_options = rcl_publisher_get_default_options(); intra_process_options.qos = qos_profile; - intra_process_options.allocator = rclcpp::allocator::get_rcl_allocator( - *allocator.get()); + intra_process_options.allocator = rclcpp::allocator::get_rcl_allocator( + *message_alloc.get()); publisher->setup_intra_process( intra_process_publisher_id, shared_publish_callback, @@ -155,12 +158,13 @@ Node::create_subscription( msg_mem_strat = rclcpp::message_memory_strategy::MessageMemoryStrategy::create_default(); } + // TODO scope + auto message_alloc = std::make_shared::MessageAlloc>(); - // TODO Allocator auto subscription_options = rcl_subscription_get_default_options(); subscription_options.qos = qos_profile; - subscription_options.allocator = rclcpp::allocator::get_rcl_allocator( - *allocator.get()); + subscription_options.allocator = rclcpp::allocator::get_rcl_allocator( + *message_alloc.get()); subscription_options.ignore_local_publications = ignore_local_publications; using rclcpp::subscription::Subscription; @@ -176,8 +180,8 @@ Node::create_subscription( // Setup intra process. if (use_intra_process_comms_) { auto intra_process_options = rcl_subscription_get_default_options(); - intra_process_options.allocator = rclcpp::allocator::get_rcl_allocator( - *allocator.get()); + intra_process_options.allocator = rclcpp::allocator::get_rcl_allocator( + *message_alloc.get()); intra_process_options.qos = qos_profile; intra_process_options.ignore_local_publications = false; diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index a8679c6fdb..872a1ba8cb 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -156,13 +156,11 @@ class Publisher : public PublisherBase Publisher( std::shared_ptr node_handle, std::string topic, - //size_t queue_size, rcl_publisher_options_t & publisher_options, - std::shared_ptr allocator) - : PublisherBase(node_handle, topic, publisher_options.qos.depth) + std::shared_ptr allocator) + : PublisherBase(node_handle, topic, publisher_options.qos.depth), message_allocator_(allocator) { using rosidl_generator_cpp::get_message_type_support_handle; - message_allocator_ = std::make_shared(*allocator.get()); allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get()); auto type_support_handle = get_message_type_support_handle(); diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 68e0515526..c896934457 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -106,7 +106,7 @@ class Service : public ServiceBase { throw std::runtime_error( std::string("could not create service: ") + - rmw_get_error_string_safe()); + rcl_get_error_string_safe()); } } diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index b5799a7978..60dbf5e6a4 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -212,24 +212,28 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy bool add_handles_to_waitset(rcl_wait_set_t * wait_set) { for (auto subscription : subscription_handles_) { if (rcl_wait_set_add_subscription(wait_set, subscription) != RCL_RET_OK) { + fprintf(stderr, "Couldn't add subscription to waitset: %s\n", rcl_get_error_string_safe()); return false; } } for (auto client : client_handles_) { if (rcl_wait_set_add_client(wait_set, client) != RCL_RET_OK) { + fprintf(stderr, "Couldn't add client to waitset: %s\n", rcl_get_error_string_safe()); return false; } } for (auto service : service_handles_) { if (rcl_wait_set_add_service(wait_set, service) != RCL_RET_OK) { + fprintf(stderr, "Couldn't add service to waitset: %s\n", rcl_get_error_string_safe()); return false; } } for (auto timer : timer_handles_) { if (rcl_wait_set_add_timer(wait_set, timer) != RCL_RET_OK) { + fprintf(stderr, "Couldn't add timer to waitset: %s\n", rcl_get_error_string_safe()); return false; } } diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 2b97a84387..8007cac9d3 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -104,14 +104,11 @@ class SubscriptionBase protected: rcl_subscription_t intra_process_subscription_handle_; + rcl_subscription_t subscription_handle_; + std::shared_ptr node_handle_; private: RCLCPP_DISABLE_COPY(SubscriptionBase); - - std::shared_ptr node_handle_; - - rcl_subscription_t subscription_handle_; - std::string topic_name_; bool ignore_local_publications_; }; @@ -250,7 +247,7 @@ class Subscription : public SubscriptionBase if (rcl_subscription_init( &intra_process_subscription_handle_, node_handle_.get(), rclcpp::type_support::get_intra_process_message_msg_type_support(), - (topic_name_ + "__intra").c_str(), + (get_topic_name() + "__intra").c_str(), &intra_process_options) != RCL_RET_OK) { // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index e755bcae7c..164c50cb65 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -28,6 +28,7 @@ #include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" +#include "rcl/error_handling.h" #include "rcl/timer.h" #include "rmw/error_handling.h" @@ -117,7 +118,9 @@ class GenericTimer : public TimerBase { // Stop the timer from running. cancel(); - rcl_timer_fini(&timer_handle_); + if (rcl_timer_fini(&timer_handle_) != RCL_RET_OK) { + fprintf(stderr, "Failed to clean up rcl timer handle"); + } } // void specialization @@ -132,11 +135,16 @@ class GenericTimer : public TimerBase { auto rcl_callback = std::function( [this](rcl_timer_t * timer, uint64_t last_call_time) { + (void)timer; + (void)last_call_time; callback_(); }); - rcl_timer_init( + if (rcl_timer_init( &timer_handle_, period.count(), rcl_callback.target(), - rcl_get_default_allocator()); + rcl_get_default_allocator()) != RCL_RET_OK) + { + fprintf(stderr, "Couldn't initialize rcl timer handle: %s\n", rcl_get_error_string_safe()); + } } template< @@ -150,11 +158,16 @@ class GenericTimer : public TimerBase { auto rcl_callback = std::function( [this](rcl_timer_t * timer, uint64_t last_call_time) { + (void)timer; + (void)last_call_time; callback_(*this); }); - rcl_timer_init( + if (rcl_timer_init( &timer_handle_, period.count(), rcl_callback.target(), - rcl_get_default_allocator()); + rcl_get_default_allocator()) != RCL_RET_OK) + { + fprintf(stderr, "Couldn't initialize rcl timer handle: %s\n", rcl_get_error_string_safe()); + } } virtual bool diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 4407d73e14..0078c9115d 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -16,6 +16,8 @@ #include #include +#include "rcl/error_handling.h" + #include "rclcpp/executor.hpp" #include "rclcpp/scope_exit.hpp" @@ -34,7 +36,9 @@ Executor::Executor(const ExecutorArgs & args) if (rcl_guard_condition_init( &interrupt_guard_condition_, guard_condition_options) != RCL_RET_OK) { - throw std::runtime_error("Failed to create interrupt guard condition in Executor constructor"); + throw std::runtime_error( + std::string("Failed to create interrupt guard condition in Executor constructor: ") + + rcl_get_error_string_safe()); } // The number of guard conditions is always at least 2: 1 for the ctrl-c guard cond, @@ -55,10 +59,10 @@ Executor::Executor(const ExecutorArgs & args) &waitset_, fixed_guard_conditions_.data(), 0, 0, 0, 0, 0, 0, rcl_get_default_allocator()) != RCL_RET_OK) { fprintf(stderr, - "[rclcpp::error] failed to create waitset: %s\n", rmw_get_error_string_safe()); + "[rclcpp::error] failed to create waitset: %s\n", rcl_get_error_string_safe()); if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) { fprintf(stderr, - "[rclcpp::error] failed to destroy guard condition: %s\n", rmw_get_error_string_safe()); + "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe()); } throw std::runtime_error("Failed to create waitset in Executor constructor"); } @@ -342,7 +346,9 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) if (rcl_wait_set_resize_subscriptions( &waitset_, memory_strategy_->number_of_ready_subscriptions()) != RCL_RET_OK) { - throw std::runtime_error("Couldn't resize waitset to number of subscriptions"); + throw std::runtime_error( + std::string("Couldn't resize waitset to number of subscriptions: ") + + rcl_get_error_string_safe()); } } @@ -350,7 +356,9 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) if (rcl_wait_set_resize_services( &waitset_, memory_strategy_->number_of_ready_services()) != RCL_RET_OK) { - throw std::runtime_error("Couldn't resize waitset to number of services"); + throw std::runtime_error( + std::string("Couldn't resize waitset to number of services: ") + + rcl_get_error_string_safe()); } } @@ -358,12 +366,12 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) if (rcl_wait_set_resize_clients( &waitset_, memory_strategy_->number_of_ready_clients()) != RCL_RET_OK) { - throw std::runtime_error("Couldn't resize waitset to number of clients"); + throw std::runtime_error( + std::string("Couldn't resize waitset to number of clients: ") + + rcl_get_error_string_safe()); } } - // TODO Only add the soonest timer to the waitset - memory_strategy_->add_handles_to_waitset(&waitset_); rcl_ret_t status = rcl_wait(&waitset_, timeout.count()); diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index d28dee1ac5..4044c7715a 100644 --- a/rclcpp/src/rclcpp/timer.cpp +++ b/rclcpp/src/rclcpp/timer.cpp @@ -30,7 +30,7 @@ void TimerBase::cancel() { if (rcl_timer_cancel(&timer_handle_) != RCL_RET_OK) { - throw std::runtime_error("Couldn't cancel timer"); + throw std::runtime_error(std::string("Couldn't cancel timer: ") + rcl_get_error_string_safe()); } } @@ -38,7 +38,8 @@ void TimerBase::execute_callback() { if (rcl_timer_call(&timer_handle_) != RCL_RET_OK) { - throw std::runtime_error("Execution of timer callback failed"); + throw std::runtime_error( + std::string("Execution of timer callback failed: ") + rcl_get_error_string_safe()); }; } @@ -48,7 +49,7 @@ TimerBase::is_ready() bool ready = false; if (rcl_timer_is_ready(&timer_handle_, &ready) != RCL_RET_OK) { - throw std::runtime_error("Timer check failed"); + throw std::runtime_error(std::string("Failed to check timer: ") + rcl_get_error_string_safe()); } return ready; } @@ -58,7 +59,8 @@ 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) { - throw std::runtime_error("Timer could not get time until next call"); + throw std::runtime_error( + std::string("Timer could not get time until next call: ") + rcl_get_error_string_safe()); } return std::chrono::nanoseconds(time_until_next_call); } From b0fba6cc5e2d2f1a515b37762aedb1955075aa22 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Thu, 24 Mar 2016 17:41:19 -0700 Subject: [PATCH 21/49] Rebase/compile/fix warnings, still need to convert notify guard conditions and add to waitset --- rclcpp/include/rclcpp/executor.hpp | 3 --- rclcpp/src/rclcpp/executor.cpp | 8 ++------ rclcpp/src/rclcpp/utilities.cpp | 4 +++- 3 files changed, 5 insertions(+), 10 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 19564f0f88..56dca6545c 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -305,9 +305,6 @@ class Executor /// Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins. std::atomic_bool spinning; - // array of fixed guard conditions - std::array fixed_guard_conditions_; - /// Guard condition for signaling the rmw layer to wake up for special events. rcl_guard_condition_t interrupt_guard_condition_; diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 0078c9115d..5cb4cb5132 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -16,6 +16,7 @@ #include #include +#include "rcl/allocator.h" #include "rcl/error_handling.h" #include "rclcpp/executor.hpp" @@ -44,11 +45,6 @@ Executor::Executor(const ExecutorArgs & args) // The number of guard conditions is always at least 2: 1 for the ctrl-c guard cond, // and one for the executor's guard cond (interrupt_guard_condition_) - // These guard conditions are permanently attached to the waitset. - // const size_t number_of_guard_conds = 2; - //fixed_guard_conditions_.guard_condition_count = number_of_guard_conds; - //fixed_guard_conditions_.guard_conditions = static_cast(guard_cond_handles_.data()); - // Put the global ctrl-c guard condition in memory_strategy_->add_guard_condition(rclcpp::utilities::get_global_sigint_guard_condition()); @@ -56,7 +52,7 @@ Executor::Executor(const ExecutorArgs & args) memory_strategy_->add_guard_condition(interrupt_guard_condition_); if (rcl_wait_set_init( - &waitset_, fixed_guard_conditions_.data(), 0, 0, 0, 0, 0, 0, rcl_get_default_allocator()) != RCL_RET_OK) + &waitset_, 0, 0, 0, 0, 0, rcl_get_default_allocator()) != RCL_RET_OK) { fprintf(stderr, "[rclcpp::error] failed to create waitset: %s\n", rcl_get_error_string_safe()); diff --git a/rclcpp/src/rclcpp/utilities.cpp b/rclcpp/src/rclcpp/utilities.cpp index 4a8043131d..80a58d5577 100644 --- a/rclcpp/src/rclcpp/utilities.cpp +++ b/rclcpp/src/rclcpp/utilities.cpp @@ -139,7 +139,9 @@ rclcpp::utilities::init(int argc, char * argv[]) // *INDENT-ON* } rcl_guard_condition_options_t options = rcl_guard_condition_get_default_options(); - rcl_guard_condition_init(&g_sigint_guard_cond_handle, options); + if (rcl_guard_condition_init(&g_sigint_guard_cond_handle, options) != RCL_RET_OK) { + throw std::runtime_error(std::string("Couldn't initialize guard condition: ") + rcl_get_error_string_safe()); + } } bool From 6bd0477082a8a355babea981f8522ffb0acf38b3 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Thu, 24 Mar 2016 17:54:03 -0700 Subject: [PATCH 22/49] Convert rmw to rcl guard conditions, still need to actually add them --- rclcpp/include/rclcpp/node.hpp | 4 +-- rclcpp/include/rclcpp/node_impl.hpp | 10 +++---- rclcpp/src/rclcpp/node.cpp | 41 ++++++++++++++++++----------- 3 files changed, 32 insertions(+), 23 deletions(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index edb99a9f21..b8e9e9a7f8 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -258,7 +258,7 @@ class Node get_callback_groups() const; RCLCPP_PUBLIC - rmw_guard_condition_t * get_notify_guard_condition() const; + const rcl_guard_condition_t * get_notify_guard_condition() const; std::atomic_bool has_executor; @@ -289,7 +289,7 @@ class Node mutable std::mutex mutex_; /// Guard condition for notifying the Executor of changes to this node. - rmw_guard_condition_t * notify_guard_condition_; + rcl_guard_condition_t notify_guard_condition_; std::map parameters_; diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 2511d5b54f..7e9ed1f301 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -126,7 +126,7 @@ Node::create_publisher( shared_publish_callback, intra_process_options); } - if (rmw_trigger_guard_condition(notify_guard_condition_) != RMW_RET_OK) { + if (rcl_trigger_guard_condition(¬ify_guard_condition_) != RMW_RET_OK) { throw std::runtime_error( std::string( "Failed to notify waitset on publisher creation: ") + rmw_get_error_string()); @@ -231,7 +231,7 @@ Node::create_subscription( default_callback_group_->add_subscription(sub_base_ptr); } number_of_subscriptions_++; - if (rmw_trigger_guard_condition(notify_guard_condition_) != RMW_RET_OK) { + if (rcl_trigger_guard_condition(¬ify_guard_condition_) != RMW_RET_OK) { throw std::runtime_error( std::string( "Failed to notify waitset on subscription creation: ") + rmw_get_error_string()); @@ -282,7 +282,7 @@ Node::create_wall_timer( default_callback_group_->add_timer(timer); } number_of_timers_++; - if (rmw_trigger_guard_condition(notify_guard_condition_) != RMW_RET_OK) { + if (rcl_trigger_guard_condition(¬ify_guard_condition_) != RMW_RET_OK) { throw std::runtime_error( std::string( "Failed to notify waitset on timer creation: ") + rmw_get_error_string()); @@ -321,7 +321,7 @@ Node::create_client( } number_of_clients_++; - if (rmw_trigger_guard_condition(notify_guard_condition_) != RMW_RET_OK) { + if (rcl_trigger_guard_condition(¬ify_guard_condition_) != RMW_RET_OK) { throw std::runtime_error( std::string( "Failed to notify waitset on client creation: ") + rmw_get_error_string()); @@ -356,7 +356,7 @@ Node::create_service( default_callback_group_->add_service(serv_base_ptr); } number_of_services_++; - if (rmw_trigger_guard_condition(notify_guard_condition_) != RMW_RET_OK) { + if (rcl_trigger_guard_condition(¬ify_guard_condition_) != RMW_RET_OK) { throw std::runtime_error( std::string( "Failed to notify waitset on service creation: ") + rmw_get_error_string()); diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index a886feb828..1a4fbf5040 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -36,13 +36,18 @@ Node::Node( bool use_intra_process_comms) : name_(node_name), context_(context), number_of_subscriptions_(0), number_of_timers_(0), number_of_services_(0), - use_intra_process_comms_(use_intra_process_comms), - notify_guard_condition_(rmw_create_guard_condition()) + use_intra_process_comms_(use_intra_process_comms) { - if (!notify_guard_condition_) { - throw std::runtime_error("Failed to create guard condition for node: " + - std::string(rmw_get_error_string_safe())); + + rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options(); + if (rcl_guard_condition_init( + ¬ify_guard_condition_, guard_condition_options) != RCL_RET_OK) + { + throw std::runtime_error( + std::string("Failed to create interrupt guard condition in Executor constructor: ") + + rcl_get_error_string_safe()); } + has_executor.store(false); size_t domain_id = 0; char * ros_domain_id = nullptr; @@ -56,9 +61,10 @@ Node::Node( if (ros_domain_id) { uint32_t number = strtoul(ros_domain_id, NULL, 0); if (number == (std::numeric_limits::max)()) { - if (rmw_destroy_guard_condition(notify_guard_condition_) != RMW_RET_OK) { - fprintf( - stderr, "Failed to destroy notify guard condition: %s\n", rmw_get_error_string_safe()); + // Finalize the interrupt guard condition. + if (rcl_guard_condition_fini(¬ify_guard_condition_) != RCL_RET_OK) { + fprintf(stderr, + "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe()); } throw std::runtime_error("failed to interpret ROS_DOMAIN_ID as integral number"); } @@ -73,10 +79,12 @@ Node::Node( // TODO(jacquelinekay): Allocator options options.domain_id = domain_id; if (rcl_node_init(&node, name_.c_str(), &options) != RCL_RET_OK) { - if (rmw_destroy_guard_condition(notify_guard_condition_) != RMW_RET_OK) { - fprintf( - stderr, "Failed to destroy notify guard condition: %s\n", rmw_get_error_string_safe()); + // Finalize the interrupt guard condition. + if (rcl_guard_condition_fini(¬ify_guard_condition_) != RCL_RET_OK) { + fprintf(stderr, + "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe()); } + throw std::runtime_error("Could not initialize rcl node"); } @@ -100,9 +108,10 @@ Node::Node( Node::~Node() { - if (rmw_destroy_guard_condition(notify_guard_condition_) != RMW_RET_OK) { - fprintf(stderr, "Warning! Failed to destroy guard condition in Node destructor: %s\n", - rmw_get_error_string_safe()); + // Finalize the interrupt guard condition. + if (rcl_guard_condition_fini(¬ify_guard_condition_) != RCL_RET_OK) { + fprintf(stderr, + "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe()); } } @@ -359,7 +368,7 @@ Node::get_callback_groups() const return callback_groups_; } -rmw_guard_condition_t * Node::get_notify_guard_condition() const +const rcl_guard_condition_t * Node::get_notify_guard_condition() const { - return notify_guard_condition_; + return ¬ify_guard_condition_; } From f3b1cc8f2e272aa2b616f63e675a8343bc57bf4a Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Thu, 24 Mar 2016 18:47:34 -0700 Subject: [PATCH 23/49] Allocator is going out of scope --- rclcpp/include/rclcpp/publisher.hpp | 9 +++++++++ rclcpp/src/rclcpp/executor.cpp | 21 ++++++++++++++++++--- rclcpp/src/rclcpp/node.cpp | 4 ++-- rclcpp/src/rclcpp/publisher.cpp | 11 ----------- 4 files changed, 29 insertions(+), 16 deletions(-) diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 872a1ba8cb..19a22f56cf 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -172,6 +172,15 @@ class Publisher : public PublisherBase std::string("could not create publisher: ") + rcl_get_error_string_safe()); } + // Life time of this object is tied to the publisher handle. + if (rmw_get_gid_for_publisher( + rcl_publisher_get_rmw_handle(&publisher_handle_), &rmw_gid_) != RMW_RET_OK) + { + // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) + throw std::runtime_error( + std::string("failed to get publisher gid: ") + rmw_get_error_string_safe()); + // *INDENT-ON* + } } diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 5cb4cb5132..3d330f9800 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -343,7 +343,7 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) &waitset_, memory_strategy_->number_of_ready_subscriptions()) != RCL_RET_OK) { throw std::runtime_error( - std::string("Couldn't resize waitset to number of subscriptions: ") + + std::string("Couldn't resize the number of subscriptions in waitset : ") + rcl_get_error_string_safe()); } } @@ -353,7 +353,7 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) &waitset_, memory_strategy_->number_of_ready_services()) != RCL_RET_OK) { throw std::runtime_error( - std::string("Couldn't resize waitset to number of services: ") + + std::string("Couldn't resize the number of services in waitset : ") + rcl_get_error_string_safe()); } } @@ -363,13 +363,28 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) &waitset_, memory_strategy_->number_of_ready_clients()) != RCL_RET_OK) { throw std::runtime_error( - std::string("Couldn't resize waitset to number of clients: ") + + std::string("Couldn't resize the number of clients in waitset : ") + rcl_get_error_string_safe()); } } memory_strategy_->add_handles_to_waitset(&waitset_); + // TODO It seems like guard conditions need to be in the "memory strategy" god I hate that name + if (waitset_.size_of_guard_conditions < guard_cond_handles_.size()) { + if (rcl_wait_set_resize_guard_conditions( + &waitset_, guard_cond_handles_.size()) != RCL_RET_OK) + { + throw std::runtime_error( + std::string("Couldn't resize the number of guard conditions in waitset : ") + + rcl_get_error_string_safe()); + } + } + // TODO: Will need to re-add guard condition handles every wait call... + for (auto handle : guard_cond_handles_) { + rcl_wait_set_add_guard_condition(&waitset_, handle); + } + rcl_ret_t status = rcl_wait(&waitset_, timeout.count()); if (status != RCL_RET_OK && status != RCL_RET_TIMEOUT && status != RCL_RET_WAIT_SET_EMPTY) { throw std::runtime_error(rcl_get_error_string_safe()); diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 1a4fbf5040..452b5a8ea8 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -75,7 +75,7 @@ Node::Node( } rcl_node_t node = rcl_get_zero_initialized_node(); - rcl_node_options_t options; + rcl_node_options_t options = rcl_node_get_default_options(); // TODO(jacquelinekay): Allocator options options.domain_id = domain_id; if (rcl_node_init(&node, name_.c_str(), &options) != RCL_RET_OK) { @@ -85,7 +85,7 @@ Node::Node( "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe()); } - throw std::runtime_error("Could not initialize rcl node"); + throw std::runtime_error(std::string("Could not initialize rcl node: ") + rcl_get_error_string_safe()); } // Initialize node handle shared_ptr with custom deleter. diff --git a/rclcpp/src/rclcpp/publisher.cpp b/rclcpp/src/rclcpp/publisher.cpp index 0073d1a034..0fdfae2a44 100644 --- a/rclcpp/src/rclcpp/publisher.cpp +++ b/rclcpp/src/rclcpp/publisher.cpp @@ -40,17 +40,6 @@ PublisherBase::PublisherBase( topic_(topic), queue_size_(queue_size), intra_process_publisher_id_(0), store_intra_process_message_(nullptr) { - // Life time of this object is tied to the publisher handle. - // TODO this won't be valid until we initialize the publisher handle. argh - // What is the point of this check? - if (rmw_get_gid_for_publisher( - rcl_publisher_get_rmw_handle(&publisher_handle_), &rmw_gid_) != RMW_RET_OK) - { - // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) - throw std::runtime_error( - std::string("failed to get publisher gid: ") + rmw_get_error_string_safe()); - // *INDENT-ON* - } } PublisherBase::~PublisherBase() From 1965af95602fc4518e069708662af50150daab5f Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Fri, 25 Mar 2016 10:27:07 -0700 Subject: [PATCH 24/49] Ownership of options --- rclcpp/include/rclcpp/node_impl.hpp | 5 +---- rclcpp/include/rclcpp/publisher.hpp | 2 ++ 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 7e9ed1f301..1d05eab01c 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -117,10 +117,7 @@ Node::create_publisher( return message_seq; }; // *INDENT-ON* - rcl_publisher_options_t intra_process_options = rcl_publisher_get_default_options(); - intra_process_options.qos = qos_profile; - intra_process_options.allocator = rclcpp::allocator::get_rcl_allocator( - *message_alloc.get()); + rcl_publisher_options_t intra_process_options = publisher_options; publisher->setup_intra_process( intra_process_publisher_id, shared_publish_callback, diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 19a22f56cf..28cf50319b 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -128,6 +128,7 @@ class PublisherBase rcl_publisher_t publisher_handle_; rcl_publisher_t intra_process_publisher_handle_; + rcl_allocator_t rcl_allocator_; std::string topic_; size_t queue_size_; @@ -163,6 +164,7 @@ class Publisher : public PublisherBase using rosidl_generator_cpp::get_message_type_support_handle; allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get()); + rcl_allocator_ = publisher_options.allocator; auto type_support_handle = get_message_type_support_handle(); if (rcl_publisher_init( &publisher_handle_, node_handle_.get(), type_support_handle, From ac5161eb90d7b5cdc7f7982f33550d6d2693aff4 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Wed, 30 Mar 2016 10:00:43 -0700 Subject: [PATCH 25/49] Compile again after rebase --- rclcpp/include/rclcpp/memory_strategy.hpp | 6 ++-- .../strategies/allocator_memory_strategy.hpp | 31 +++++++++++++------ rclcpp/src/rclcpp/executor.cpp | 24 +++----------- 3 files changed, 30 insertions(+), 31 deletions(-) diff --git a/rclcpp/include/rclcpp/memory_strategy.hpp b/rclcpp/include/rclcpp/memory_strategy.hpp index cedf3567cb..19e7fa698b 100644 --- a/rclcpp/include/rclcpp/memory_strategy.hpp +++ b/rclcpp/include/rclcpp/memory_strategy.hpp @@ -47,16 +47,18 @@ class RCLCPP_PUBLIC MemoryStrategy virtual size_t number_of_ready_subscriptions() const = 0; virtual size_t number_of_ready_services() const = 0; virtual size_t number_of_ready_clients() const = 0; + virtual size_t number_of_guard_conditions() const = 0; virtual bool add_handles_to_waitset(rcl_wait_set_t * wait_set) = 0; + virtual void clear_active_entities() = 0; /// Provide a newly initialized AnyExecutable object. // \return Shared pointer to the fresh executable. virtual rclcpp::executor::AnyExecutable::SharedPtr instantiate_next_executable() = 0; - virtual void add_guard_condition(const rmw_guard_condition_t * guard_condition) = 0; + virtual void add_guard_condition(const rcl_guard_condition_t * guard_condition) = 0; - virtual void remove_guard_condition(const rmw_guard_condition_t * guard_condition) = 0; + virtual void remove_guard_condition(const rcl_guard_condition_t * guard_condition) = 0; virtual void get_next_subscription(rclcpp::executor::AnyExecutable::SharedPtr any_exec, diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 60dbf5e6a4..979abbd82b 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -64,7 +64,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy allocator_ = std::make_shared(); } - void add_guard_condition(const rmw_guard_condition_t * guard_condition) + void add_guard_condition(const rcl_guard_condition_t * guard_condition) { for (const auto & existing_guard_condition : guard_conditions_) { if (existing_guard_condition == guard_condition) { @@ -74,7 +74,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy guard_conditions_.push_back(guard_condition); } - void remove_guard_condition(const rmw_guard_condition_t * guard_condition) + void remove_guard_condition(const rcl_guard_condition_t * guard_condition) { for (auto it = guard_conditions_.begin(); it != guard_conditions_.end(); ++it) { if (*it == guard_condition) { @@ -84,6 +84,13 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } } + void clear_active_entities() + { + subscription_handles_.clear(); + service_handles_.clear(); + client_handles_.clear(); + } + /* size_t fill_subscriber_handles(void ** & ptr) { @@ -128,13 +135,6 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy return guard_condition_handles_.size(); } - void clear_active_entities() - { - subscriptions_.clear(); - services_.clear(); - clients_.clear(); - } - void clear_handles() { subscription_handles_.clear(); @@ -237,6 +237,13 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy return false; } } + + for (auto guard_condition : guard_conditions_) { + if (rcl_wait_set_add_guard_condition(wait_set, guard_condition) != RCL_RET_OK) { + fprintf(stderr, "Couldn't add guard_condition to waitset: %s\n", rcl_get_error_string_safe()); + return false; + } + } return true; } @@ -369,12 +376,16 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy return client_handles_.size(); } + size_t number_of_guard_conditions() const { + return guard_conditions_.size(); + } + private: template using VectorRebind = std::vector::template rebind_alloc>; - VectorRebind guard_conditions_; + VectorRebind guard_conditions_; VectorRebind subscription_handles_; VectorRebind service_handles_; diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 3d330f9800..c67a7a1f87 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -49,7 +49,7 @@ Executor::Executor(const ExecutorArgs & args) memory_strategy_->add_guard_condition(rclcpp::utilities::get_global_sigint_guard_condition()); // Put the executor's guard condition in - memory_strategy_->add_guard_condition(interrupt_guard_condition_); + memory_strategy_->add_guard_condition(&interrupt_guard_condition_); if (rcl_wait_set_init( &waitset_, 0, 0, 0, 0, 0, rcl_get_default_allocator()) != RCL_RET_OK) @@ -337,7 +337,6 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) ); } - memory_strategy_->clear_handles(); if (waitset_.size_of_subscriptions < memory_strategy_->number_of_ready_subscriptions()) { if (rcl_wait_set_resize_subscriptions( &waitset_, memory_strategy_->number_of_ready_subscriptions()) != RCL_RET_OK) @@ -367,35 +366,22 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) rcl_get_error_string_safe()); } } - - memory_strategy_->add_handles_to_waitset(&waitset_); - - // TODO It seems like guard conditions need to be in the "memory strategy" god I hate that name - if (waitset_.size_of_guard_conditions < guard_cond_handles_.size()) { + if (waitset_.size_of_guard_conditions < memory_strategy_->number_of_guard_conditions()) { if (rcl_wait_set_resize_guard_conditions( - &waitset_, guard_cond_handles_.size()) != RCL_RET_OK) + &waitset_, memory_strategy_->number_of_guard_conditions()) != RCL_RET_OK) { throw std::runtime_error( - std::string("Couldn't resize the number of guard conditions in waitset : ") + + std::string("Couldn't resize the number of guard_conditions in waitset : ") + rcl_get_error_string_safe()); } } - // TODO: Will need to re-add guard condition handles every wait call... - for (auto handle : guard_cond_handles_) { - rcl_wait_set_add_guard_condition(&waitset_, handle); - } rcl_ret_t status = rcl_wait(&waitset_, timeout.count()); if (status != RCL_RET_OK && status != RCL_RET_TIMEOUT && status != RCL_RET_WAIT_SET_EMPTY) { throw std::runtime_error(rcl_get_error_string_safe()); } -/* - // construct a guard conditions struct - rmw_guard_conditions_t guard_conditions; - guard_conditions.guard_condition_count = - memory_strategy_->fill_guard_condition_handles(guard_conditions.guard_conditions); -*/ + } rclcpp::node::Node::SharedPtr From da190eb11ecc550b71147d74ad5ca12057fd6d37 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Wed, 30 Mar 2016 12:50:21 -0700 Subject: [PATCH 26/49] Fix allocator --- .../rclcpp/allocator/allocator_common.hpp | 32 ++++++++----------- 1 file changed, 14 insertions(+), 18 deletions(-) diff --git a/rclcpp/include/rclcpp/allocator/allocator_common.hpp b/rclcpp/include/rclcpp/allocator/allocator_common.hpp index 5b7e2d364e..668b949822 100644 --- a/rclcpp/include/rclcpp/allocator/allocator_common.hpp +++ b/rclcpp/include/rclcpp/allocator/allocator_common.hpp @@ -29,30 +29,26 @@ namespace allocator template using AllocRebind = typename std::allocator_traits::template rebind_traits; +template +void * retyped_allocate(size_t size, void * untyped_allocator) +{ + return std::allocator_traits::allocate(*static_cast(untyped_allocator), size); +} +template +void retyped_deallocate(void * untyped_allocator, void * untyped_pointer) +{ + std::allocator_traits::deallocate( + *static_cast(untyped_allocator), static_cast(untyped_pointer), 1); +} + // Convert a std::allocator_traits-formatted Allocator into an rcl allocator template>::value>::type * = nullptr> rcl_allocator_t get_rcl_allocator(Alloc & allocator) { rcl_allocator_t rcl_allocator = rcl_get_default_allocator(); - // Argh - auto allocate_callback = std::function( - [](size_t size, void * state) -> void * { - auto allocator_ptr = static_cast(state); - // TODO check if null - return std::allocator_traits::allocate(*allocator_ptr, size); - }).target(); - auto deallocate_callback = std::function( - [](void * pointer, void * state) { - auto allocator_ptr = static_cast(state); - // TODO check if null - auto typed_pointer = static_cast(pointer); - // TODO size: we don't know how many entries were allocated - std::allocator_traits::deallocate(*allocator_ptr, typed_pointer, 1); - }).target(); - - rcl_allocator.allocate = allocate_callback; - rcl_allocator.deallocate = deallocate_callback; + rcl_allocator.allocate = &retyped_allocate; + rcl_allocator.deallocate = &retyped_deallocate; rcl_allocator.reallocate = nullptr; rcl_allocator.state = &allocator; return rcl_allocator; From 6992ed8048086c31b26aaa91b3757594bd6133d8 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Wed, 30 Mar 2016 14:53:29 -0700 Subject: [PATCH 27/49] Memory issues --- rclcpp/include/rclcpp/executor.hpp | 4 ++-- rclcpp/include/rclcpp/node.hpp | 3 +-- rclcpp/include/rclcpp/node_impl.hpp | 3 ++- rclcpp/include/rclcpp/publisher.hpp | 12 ++++++++---- rclcpp/include/rclcpp/subscription.hpp | 4 ++-- rclcpp/src/rclcpp/executor.cpp | 6 ++---- rclcpp/src/rclcpp/node.cpp | 3 +-- 7 files changed, 18 insertions(+), 17 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 56dca6545c..fe9f92058f 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -306,10 +306,10 @@ class Executor std::atomic_bool spinning; /// Guard condition for signaling the rmw layer to wake up for special events. - rcl_guard_condition_t interrupt_guard_condition_; + rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition(); /// Waitset for managing entities that the rmw layer waits on. - rcl_wait_set_t waitset_; + rcl_wait_set_t waitset_ = rcl_get_zero_initialized_wait_set(); /// The memory strategy: an interface for handling user-defined memory allocation strategies. memory_strategy::MemoryStrategy::SharedPtr memory_strategy_; diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index b8e9e9a7f8..07516473c8 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -271,7 +271,6 @@ class Node std::string name_; - //std::shared_ptr node_handle_; std::shared_ptr node_handle_; rclcpp::context::Context::SharedPtr context_; @@ -289,7 +288,7 @@ class Node mutable std::mutex mutex_; /// Guard condition for notifying the Executor of changes to this node. - rcl_guard_condition_t notify_guard_condition_; + rcl_guard_condition_t notify_guard_condition_ = rcl_get_zero_initialized_guard_condition(); std::map parameters_; diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 1d05eab01c..eb89cec2c9 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -341,7 +341,8 @@ Node::create_service( service_options.qos = qos_profile; auto serv = service::Service::make_shared( - node_handle_, service_name, any_service_callback, service_options); + node_handle_, + service_name, any_service_callback, service_options); 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 28cf50319b..2bef87cca8 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -126,8 +126,8 @@ class PublisherBase std::shared_ptr node_handle_; - rcl_publisher_t publisher_handle_; - rcl_publisher_t intra_process_publisher_handle_; + rcl_publisher_t publisher_handle_ = rcl_get_zero_initialized_publisher(); + rcl_publisher_t intra_process_publisher_handle_ = rcl_get_zero_initialized_publisher(); rcl_allocator_t rcl_allocator_; std::string topic_; @@ -175,8 +175,12 @@ class Publisher : public PublisherBase rcl_get_error_string_safe()); } // Life time of this object is tied to the publisher handle. - if (rmw_get_gid_for_publisher( - rcl_publisher_get_rmw_handle(&publisher_handle_), &rmw_gid_) != RMW_RET_OK) + rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle(&publisher_handle_); + if (!publisher_rmw_handle) { + throw std::runtime_error( + std::string("failed to get rmw handle: ") + rcl_get_error_string_safe()); + } + if (rmw_get_gid_for_publisher(publisher_rmw_handle, &rmw_gid_) != RMW_RET_OK) { // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) throw std::runtime_error( diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 8007cac9d3..dc268d5ba2 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -103,8 +103,8 @@ class SubscriptionBase const rmw_message_info_t & message_info) = 0; protected: - rcl_subscription_t intra_process_subscription_handle_; - rcl_subscription_t subscription_handle_; + 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 node_handle_; private: diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index c67a7a1f87..8563eb16a0 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -52,7 +52,7 @@ Executor::Executor(const ExecutorArgs & args) memory_strategy_->add_guard_condition(&interrupt_guard_condition_); if (rcl_wait_set_init( - &waitset_, 0, 0, 0, 0, 0, rcl_get_default_allocator()) != RCL_RET_OK) + &waitset_, 0, 2, 0, 0, 0, rcl_get_default_allocator()) != RCL_RET_OK) { fprintf(stderr, "[rclcpp::error] failed to create waitset: %s\n", rcl_get_error_string_safe()); @@ -378,10 +378,8 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) rcl_ret_t status = rcl_wait(&waitset_, timeout.count()); if (status != RCL_RET_OK && status != RCL_RET_TIMEOUT && status != RCL_RET_WAIT_SET_EMPTY) { - throw std::runtime_error(rcl_get_error_string_safe()); + throw std::runtime_error(std::string("rcl_wait() failed: ") + rcl_get_error_string_safe()); } - - } rclcpp::node::Node::SharedPtr diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 452b5a8ea8..30ab94ee45 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -91,8 +91,7 @@ Node::Node( // Initialize node handle shared_ptr with custom deleter. // *INDENT-OFF* node_handle_.reset(&node, [](rcl_node_t * node) { - auto ret = rcl_node_fini(node); - if (ret != RMW_RET_OK) { + if (rcl_node_fini(node) != RMW_RET_OK) { fprintf( stderr, "Error in destruction of rmw node handle: %s\n", rcl_get_error_string_safe()); } From a7dcef50af1e4abdf25f8c2e971645df6fa9c381 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Wed, 30 Mar 2016 15:51:34 -0700 Subject: [PATCH 28/49] memory problems in initialization fixed --- .../rclcpp/allocator/allocator_common.hpp | 8 ++++++-- rclcpp/src/rclcpp/node.cpp | 18 ++++++++++-------- rclcpp/src/rclcpp/publisher.cpp | 6 +++++- 3 files changed, 21 insertions(+), 11 deletions(-) diff --git a/rclcpp/include/rclcpp/allocator/allocator_common.hpp b/rclcpp/include/rclcpp/allocator/allocator_common.hpp index 668b949822..4a5a1fb1d3 100644 --- a/rclcpp/include/rclcpp/allocator/allocator_common.hpp +++ b/rclcpp/include/rclcpp/allocator/allocator_common.hpp @@ -32,10 +32,14 @@ using AllocRebind = typename std::allocator_traits::template rebind_trait template void * retyped_allocate(size_t size, void * untyped_allocator) { - return std::allocator_traits::allocate(*static_cast(untyped_allocator), size); + auto typed_allocator = static_cast(untyped_allocator); + if (!typed_allocator) { + throw std::runtime_error("Received incorrect allocator type"); + } + return std::allocator_traits::allocate(*typed_allocator, size); } template -void retyped_deallocate(void * untyped_allocator, void * untyped_pointer) +void retyped_deallocate(void * untyped_pointer, void * untyped_allocator) { std::allocator_traits::deallocate( *static_cast(untyped_allocator), static_cast(untyped_pointer), 1); diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 30ab94ee45..7a2158ad0a 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -74,11 +74,19 @@ Node::Node( #endif } - rcl_node_t node = rcl_get_zero_initialized_node(); + //node_handle_ = std::make_shared(rcl_get_zero_initialized_node()); + rcl_node_t * rcl_node = new rcl_node_t(rcl_get_zero_initialized_node()); + node_handle_.reset(rcl_node, [](rcl_node_t * node) { + if (rcl_node_fini(node) != RMW_RET_OK) { + fprintf( + stderr, "Error in destruction of rmw node handle: %s\n", rcl_get_error_string_safe()); + } + delete node; + }); rcl_node_options_t options = rcl_node_get_default_options(); // TODO(jacquelinekay): Allocator options options.domain_id = domain_id; - if (rcl_node_init(&node, name_.c_str(), &options) != RCL_RET_OK) { + if (rcl_node_init(node_handle_.get(), name_.c_str(), &options) != RCL_RET_OK) { // Finalize the interrupt guard condition. if (rcl_guard_condition_fini(¬ify_guard_condition_) != RCL_RET_OK) { fprintf(stderr, @@ -90,12 +98,6 @@ Node::Node( // Initialize node handle shared_ptr with custom deleter. // *INDENT-OFF* - node_handle_.reset(&node, [](rcl_node_t * node) { - if (rcl_node_fini(node) != RMW_RET_OK) { - fprintf( - stderr, "Error in destruction of rmw node handle: %s\n", rcl_get_error_string_safe()); - } - }); // *INDENT-ON* using rclcpp::callback_group::CallbackGroupType; diff --git a/rclcpp/src/rclcpp/publisher.cpp b/rclcpp/src/rclcpp/publisher.cpp index 0fdfae2a44..a803b53070 100644 --- a/rclcpp/src/rclcpp/publisher.cpp +++ b/rclcpp/src/rclcpp/publisher.cpp @@ -127,8 +127,12 @@ PublisherBase::setup_intra_process( intra_process_publisher_id_ = intra_process_publisher_id; store_intra_process_message_ = callback; // Life time of this object is tied to the publisher handle. + rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle(&intra_process_publisher_handle_); + if (publisher_rmw_handle == nullptr) { + throw std::runtime_error(std::string("Failed to get rmw publisher handle") + rcl_get_error_string_safe()); + } auto ret = rmw_get_gid_for_publisher( - rcl_publisher_get_rmw_handle(&intra_process_publisher_handle_), &intra_process_rmw_gid_); + publisher_rmw_handle, &intra_process_rmw_gid_); if (ret != RMW_RET_OK) { // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) throw std::runtime_error( From 0af71eb8d58dd933589fe634b5869bda29473593 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Wed, 30 Mar 2016 16:48:56 -0700 Subject: [PATCH 29/49] publisher working (?) --- rclcpp/src/rclcpp/executor.cpp | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 8563eb16a0..5feb9df3f3 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -376,10 +376,24 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) } } + if (!memory_strategy_->add_handles_to_waitset(&waitset_)) { + throw std::runtime_error("Couldn't fill waitset"); + } + fprintf(stderr, "waiting\n"); + rcl_ret_t status = rcl_wait(&waitset_, timeout.count()); - if (status != RCL_RET_OK && status != RCL_RET_TIMEOUT && status != RCL_RET_WAIT_SET_EMPTY) { + if (status == RCL_RET_WAIT_SET_EMPTY) + { + fprintf(stderr, "Warning: empty waitset received in rcl_wait(). This should never happen.\n"); + } else if (status == RCL_RET_TIMEOUT) { + fprintf(stderr, "Warning: timout in rcl_wait().\n"); + } else if (status != RCL_RET_OK) { throw std::runtime_error(std::string("rcl_wait() failed: ") + rcl_get_error_string_safe()); } + + // check the null handles in the waitset and remove them from the handles in + + // Then clear the waitset } rclcpp::node::Node::SharedPtr From 2572a32823c615c24863475c94ef36c92fb36dfa Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Wed, 30 Mar 2016 17:40:56 -0700 Subject: [PATCH 30/49] talker/listener works --- rclcpp/include/rclcpp/memory_strategy.hpp | 4 +- .../strategies/allocator_memory_strategy.hpp | 74 ++++++------------- rclcpp/src/rclcpp/executor.cpp | 33 +++++++-- 3 files changed, 51 insertions(+), 60 deletions(-) diff --git a/rclcpp/include/rclcpp/memory_strategy.hpp b/rclcpp/include/rclcpp/memory_strategy.hpp index 19e7fa698b..65ddcd9016 100644 --- a/rclcpp/include/rclcpp/memory_strategy.hpp +++ b/rclcpp/include/rclcpp/memory_strategy.hpp @@ -47,10 +47,12 @@ class RCLCPP_PUBLIC MemoryStrategy virtual size_t number_of_ready_subscriptions() const = 0; virtual size_t number_of_ready_services() const = 0; virtual size_t number_of_ready_clients() const = 0; + virtual size_t number_of_ready_timers() const = 0; virtual size_t number_of_guard_conditions() const = 0; virtual bool add_handles_to_waitset(rcl_wait_set_t * wait_set) = 0; - virtual void clear_active_entities() = 0; + virtual void clear_handles() = 0; + virtual void remove_null_handles(rcl_wait_set_t * wait_set) = 0; /// Provide a newly initialized AnyExecutable object. // \return Shared pointer to the fresh executable. diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 979abbd82b..345ae7d5e4 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -84,67 +84,36 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } } - void clear_active_entities() + void clear_handles() { subscription_handles_.clear(); service_handles_.clear(); client_handles_.clear(); } -/* - size_t fill_subscriber_handles(void ** & ptr) + virtual void remove_null_handles(rcl_wait_set_t * wait_set) { - for (auto & subscription : subscriptions_) { - subscription_handles_.push_back(subscription->get_subscription_handle()->data); - if (subscription->get_intra_process_subscription_handle()) { - subscription_handles_.push_back(subscription->get_intra_process_subscription_handle()->data); + for (size_t i = 0; i < wait_set->size_of_subscriptions; ++i) { + if (!wait_set->subscriptions[i]) { + subscription_handles_[i] = nullptr; } } - ptr = subscription_handles_.data(); - return subscription_handles_.size(); - } - - // return the new number of services - size_t fill_service_handles(void ** & ptr) - { - for (auto & service : services_) { - service_handles_.push_back(service->get_service_handle()->data); + for (size_t i = 0; i < wait_set->size_of_services; ++i) { + if (!wait_set->services[i]) { + service_handles_[i] = nullptr; + } } - ptr = service_handles_.data(); - return service_handles_.size(); - } - - // return the new number of clients - size_t fill_client_handles(void ** & ptr) - { - for (auto & client : clients_) { - client_handles_.push_back(client->get_client_handle()->data); + for (size_t i = 0; i < wait_set->size_of_clients; ++i) { + if (!wait_set->clients[i]) { + client_handles_[i] = nullptr; + } } - ptr = client_handles_.data(); - return client_handles_.size(); - } - - size_t fill_guard_condition_handles(void ** & ptr) - { - for (const auto & guard_condition : guard_conditions_) { - if (guard_condition) { - guard_condition_handles_.push_back(guard_condition->data); + for (size_t i = 0; i < wait_set->size_of_timers; ++i) { + if (!wait_set->timers[i]) { + timer_handles_[i] = nullptr; } } - ptr = guard_condition_handles_.data(); - return guard_condition_handles_.size(); - } - void clear_handles() - { - subscription_handles_.clear(); - service_handles_.clear(); - client_handles_.clear(); - guard_condition_handles_.clear(); - } - - void remove_null_handles() - { subscription_handles_.erase( std::remove(subscription_handles_.begin(), subscription_handles_.end(), nullptr), subscription_handles_.end() @@ -160,12 +129,11 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy client_handles_.end() ); - guard_condition_handles_.erase( - std::remove(guard_condition_handles_.begin(), guard_condition_handles_.end(), nullptr), - guard_condition_handles_.end() + timer_handles_.erase( + std::remove(timer_handles_.begin(), timer_handles_.end(), nullptr), + timer_handles_.end() ); } -*/ bool collect_entities(const WeakNodeVector & weak_nodes) { @@ -380,6 +348,10 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy return guard_conditions_.size(); } + size_t number_of_ready_timers() const { + return timer_handles_.size(); + } + private: template using VectorRebind = diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 5feb9df3f3..147fbbdc8b 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -320,6 +320,7 @@ void Executor::wait_for_work(std::chrono::nanoseconds timeout) { // Collect the subscriptions and timers to be waited on + memory_strategy_->clear_handles(); bool has_invalid_weak_nodes = memory_strategy_->collect_entities(weak_nodes_); // Clean up any invalid nodes, if they were detected @@ -337,7 +338,8 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) ); } - if (waitset_.size_of_subscriptions < memory_strategy_->number_of_ready_subscriptions()) { + rcl_wait_set_clear_subscriptions(&waitset_); + if (waitset_.size_of_subscriptions != memory_strategy_->number_of_ready_subscriptions()) { if (rcl_wait_set_resize_subscriptions( &waitset_, memory_strategy_->number_of_ready_subscriptions()) != RCL_RET_OK) { @@ -347,7 +349,8 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) } } - if (waitset_.size_of_services < memory_strategy_->number_of_ready_services()) { + rcl_wait_set_clear_services(&waitset_); + if (waitset_.size_of_services != memory_strategy_->number_of_ready_services()) { if (rcl_wait_set_resize_services( &waitset_, memory_strategy_->number_of_ready_services()) != RCL_RET_OK) { @@ -357,7 +360,8 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) } } - if (waitset_.size_of_clients < memory_strategy_->number_of_ready_clients()) { + rcl_wait_set_clear_clients(&waitset_); + if (waitset_.size_of_clients != memory_strategy_->number_of_ready_clients()) { if (rcl_wait_set_resize_clients( &waitset_, memory_strategy_->number_of_ready_clients()) != RCL_RET_OK) { @@ -366,7 +370,9 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) rcl_get_error_string_safe()); } } - if (waitset_.size_of_guard_conditions < memory_strategy_->number_of_guard_conditions()) { + + rcl_wait_set_clear_guard_conditions(&waitset_); + if (waitset_.size_of_guard_conditions != memory_strategy_->number_of_guard_conditions()) { if (rcl_wait_set_resize_guard_conditions( &waitset_, memory_strategy_->number_of_guard_conditions()) != RCL_RET_OK) { @@ -376,6 +382,17 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) } } + rcl_wait_set_clear_timers(&waitset_); + if (waitset_.size_of_timers != memory_strategy_->number_of_ready_timers()) { + if (rcl_wait_set_resize_timers( + &waitset_, memory_strategy_->number_of_ready_timers()) != RCL_RET_OK) + { + throw std::runtime_error( + std::string("Couldn't resize the number of timers in waitset : ") + + rcl_get_error_string_safe()); + } + } + if (!memory_strategy_->add_handles_to_waitset(&waitset_)) { throw std::runtime_error("Couldn't fill waitset"); } @@ -385,15 +402,15 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) if (status == RCL_RET_WAIT_SET_EMPTY) { fprintf(stderr, "Warning: empty waitset received in rcl_wait(). This should never happen.\n"); - } else if (status == RCL_RET_TIMEOUT) { - fprintf(stderr, "Warning: timout in rcl_wait().\n"); - } else if (status != RCL_RET_OK) { + } else if (status != RCL_RET_OK && status != RCL_RET_TIMEOUT) { throw std::runtime_error(std::string("rcl_wait() failed: ") + rcl_get_error_string_safe()); } - // check the null handles in the waitset and remove them from the handles in + // check the null handles in the waitset and remove them from the handles in memory strategy + // for callback-based entities // Then clear the waitset + memory_strategy_->remove_null_handles(&waitset_); } rclcpp::node::Node::SharedPtr From 0a630188447db96a54f0a6fac7864f6d48214235 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Wed, 30 Mar 2016 18:19:13 -0700 Subject: [PATCH 31/49] Test tweaking --- .../rclcpp/allocator/allocator_common.hpp | 16 ++++++++++---- rclcpp/include/rclcpp/publisher.hpp | 21 +++++++++++++++---- rclcpp/src/rclcpp/publisher.cpp | 17 --------------- 3 files changed, 29 insertions(+), 25 deletions(-) diff --git a/rclcpp/include/rclcpp/allocator/allocator_common.hpp b/rclcpp/include/rclcpp/allocator/allocator_common.hpp index 4a5a1fb1d3..3028d224ea 100644 --- a/rclcpp/include/rclcpp/allocator/allocator_common.hpp +++ b/rclcpp/include/rclcpp/allocator/allocator_common.hpp @@ -38,11 +38,19 @@ void * retyped_allocate(size_t size, void * untyped_allocator) } return std::allocator_traits::allocate(*typed_allocator, size); } -template +template void retyped_deallocate(void * untyped_pointer, void * untyped_allocator) { - std::allocator_traits::deallocate( - *static_cast(untyped_allocator), static_cast(untyped_pointer), 1); + auto typed_allocator = static_cast(untyped_allocator); + if (!typed_allocator) { + throw std::runtime_error("Received incorrect allocator type"); + } + auto typed_ptr = static_cast(untyped_pointer); + if (!typed_ptr) { + //throw std::runtime_error("Received incorrect allocator type"); + fprintf(stderr, "received null pointer in deallocate\n"); + } + std::allocator_traits::deallocate(*typed_allocator, typed_ptr, 1); } // Convert a std::allocator_traits-formatted Allocator into an rcl allocator @@ -52,7 +60,7 @@ rcl_allocator_t get_rcl_allocator(Alloc & allocator) rcl_allocator_t rcl_allocator = rcl_get_default_allocator(); rcl_allocator.allocate = &retyped_allocate; - rcl_allocator.deallocate = &retyped_deallocate; + rcl_allocator.deallocate = &retyped_deallocate; rcl_allocator.reallocate = nullptr; rcl_allocator.state = &allocator; return rcl_allocator; diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 2bef87cca8..fcf1919b83 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -66,10 +66,6 @@ class PublisherBase std::string topic, size_t queue_size); - /// Default destructor. - RCLCPP_PUBLIC - virtual ~PublisherBase(); - /// Get the topic that this publisher publishes on. // \return The topic name. RCLCPP_PUBLIC @@ -189,6 +185,23 @@ class Publisher : public PublisherBase } } + ~Publisher() + { + if (rcl_publisher_fini(&intra_process_publisher_handle_, node_handle_.get()) != RCL_RET_OK) { + fprintf( + stderr, + "Error in destruction of intra process rcl publisher handle: %s\n", + rcl_get_error_string_safe()); + } + + if (rcl_publisher_fini(&publisher_handle_, node_handle_.get()) != RCL_RET_OK) { + fprintf( + stderr, + "Error in destruction of rcl publisher handle: %s\n", + rcl_get_error_string_safe()); + } + } + /// Send a message to the topic for this publisher. /** diff --git a/rclcpp/src/rclcpp/publisher.cpp b/rclcpp/src/rclcpp/publisher.cpp index a803b53070..6c001244e8 100644 --- a/rclcpp/src/rclcpp/publisher.cpp +++ b/rclcpp/src/rclcpp/publisher.cpp @@ -42,23 +42,6 @@ PublisherBase::PublisherBase( { } -PublisherBase::~PublisherBase() -{ - if (rcl_publisher_fini(&intra_process_publisher_handle_, node_handle_.get()) != RCL_RET_OK) { - fprintf( - stderr, - "Error in destruction of intra process rcl publisher handle: %s\n", - rcl_get_error_string_safe()); - } - - if (rcl_publisher_fini(&publisher_handle_, node_handle_.get()) != RCL_RET_OK) { - fprintf( - stderr, - "Error in destruction of rcl publisher handle: %s\n", - rcl_get_error_string_safe()); - } -} - const std::string & PublisherBase::get_topic_name() const { From 321a17f4446799c8a9b777f5ccdb11f321ae6cac Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Thu, 31 Mar 2016 10:13:44 -0700 Subject: [PATCH 32/49] Fix linking to rcl --- rclcpp/CMakeLists.txt | 5 +- .../rclcpp/allocator/allocator_common.hpp | 1 - rclcpp/src/rclcpp/executor.cpp | 84 ++++++++----------- 3 files changed, 40 insertions(+), 50 deletions(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 5c37527a6b..72eaf17363 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -43,10 +43,13 @@ set(${PROJECT_NAME}_SRCS ) macro(target) + if(NOT "${target_suffix} " STREQUAL " ") + get_rcl_information("${rmw_implementation}" "rcl${target_suffix}") + endif() add_library(${PROJECT_NAME}${target_suffix} SHARED ${${PROJECT_NAME}_SRCS}) ament_target_dependencies(${PROJECT_NAME}${target_suffix} - "rcl" + "rcl${target_suffix}" "rcl_interfaces" "rmw" "rosidl_generator_cpp" diff --git a/rclcpp/include/rclcpp/allocator/allocator_common.hpp b/rclcpp/include/rclcpp/allocator/allocator_common.hpp index 3028d224ea..117c92d626 100644 --- a/rclcpp/include/rclcpp/allocator/allocator_common.hpp +++ b/rclcpp/include/rclcpp/allocator/allocator_common.hpp @@ -47,7 +47,6 @@ void retyped_deallocate(void * untyped_pointer, void * untyped_allocator) } auto typed_ptr = static_cast(untyped_pointer); if (!typed_ptr) { - //throw std::runtime_error("Received incorrect allocator type"); fprintf(stderr, "received null pointer in deallocate\n"); } std::allocator_traits::deallocate(*typed_allocator, typed_ptr, 1); diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 147fbbdc8b..a621bc711b 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -338,65 +338,50 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) ); } - rcl_wait_set_clear_subscriptions(&waitset_); - if (waitset_.size_of_subscriptions != memory_strategy_->number_of_ready_subscriptions()) { - if (rcl_wait_set_resize_subscriptions( - &waitset_, memory_strategy_->number_of_ready_subscriptions()) != RCL_RET_OK) - { - throw std::runtime_error( - std::string("Couldn't resize the number of subscriptions in waitset : ") + - rcl_get_error_string_safe()); - } + fprintf(stderr, "Got %d ready subscriptions\n", memory_strategy_->number_of_ready_subscriptions()); + if (rcl_wait_set_resize_subscriptions( + &waitset_, memory_strategy_->number_of_ready_subscriptions()) != RCL_RET_OK) + { + throw std::runtime_error( + std::string("Couldn't resize the number of subscriptions in waitset : ") + + rcl_get_error_string_safe()); } - rcl_wait_set_clear_services(&waitset_); - if (waitset_.size_of_services != memory_strategy_->number_of_ready_services()) { - if (rcl_wait_set_resize_services( - &waitset_, memory_strategy_->number_of_ready_services()) != RCL_RET_OK) - { - throw std::runtime_error( - std::string("Couldn't resize the number of services in waitset : ") + - rcl_get_error_string_safe()); - } + if (rcl_wait_set_resize_services( + &waitset_, memory_strategy_->number_of_ready_services()) != RCL_RET_OK) + { + throw std::runtime_error( + std::string("Couldn't resize the number of services in waitset : ") + + rcl_get_error_string_safe()); } - rcl_wait_set_clear_clients(&waitset_); - if (waitset_.size_of_clients != memory_strategy_->number_of_ready_clients()) { - if (rcl_wait_set_resize_clients( - &waitset_, memory_strategy_->number_of_ready_clients()) != RCL_RET_OK) - { - throw std::runtime_error( - std::string("Couldn't resize the number of clients in waitset : ") + - rcl_get_error_string_safe()); - } + if (rcl_wait_set_resize_clients( + &waitset_, memory_strategy_->number_of_ready_clients()) != RCL_RET_OK) + { + throw std::runtime_error( + std::string("Couldn't resize the number of clients in waitset : ") + + rcl_get_error_string_safe()); } - rcl_wait_set_clear_guard_conditions(&waitset_); - if (waitset_.size_of_guard_conditions != memory_strategy_->number_of_guard_conditions()) { - if (rcl_wait_set_resize_guard_conditions( - &waitset_, memory_strategy_->number_of_guard_conditions()) != RCL_RET_OK) - { - throw std::runtime_error( - std::string("Couldn't resize the number of guard_conditions in waitset : ") + - rcl_get_error_string_safe()); - } + if (rcl_wait_set_resize_guard_conditions( + &waitset_, memory_strategy_->number_of_guard_conditions()) != RCL_RET_OK) + { + throw std::runtime_error( + std::string("Couldn't resize the number of guard_conditions in waitset : ") + + rcl_get_error_string_safe()); } - rcl_wait_set_clear_timers(&waitset_); - if (waitset_.size_of_timers != memory_strategy_->number_of_ready_timers()) { - if (rcl_wait_set_resize_timers( - &waitset_, memory_strategy_->number_of_ready_timers()) != RCL_RET_OK) - { - throw std::runtime_error( - std::string("Couldn't resize the number of timers in waitset : ") + - rcl_get_error_string_safe()); - } + if (rcl_wait_set_resize_timers( + &waitset_, memory_strategy_->number_of_ready_timers()) != RCL_RET_OK) + { + throw std::runtime_error( + std::string("Couldn't resize the number of timers in waitset : ") + + rcl_get_error_string_safe()); } if (!memory_strategy_->add_handles_to_waitset(&waitset_)) { throw std::runtime_error("Couldn't fill waitset"); } - fprintf(stderr, "waiting\n"); rcl_ret_t status = rcl_wait(&waitset_, timeout.count()); if (status == RCL_RET_WAIT_SET_EMPTY) @@ -408,9 +393,12 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) // check the null handles in the waitset and remove them from the handles in memory strategy // for callback-based entities - - // Then clear the waitset memory_strategy_->remove_null_handles(&waitset_); + rcl_wait_set_clear_subscriptions(&waitset_); + rcl_wait_set_clear_services(&waitset_); + rcl_wait_set_clear_clients(&waitset_); + rcl_wait_set_clear_guard_conditions(&waitset_); + rcl_wait_set_clear_timers(&waitset_); } rclcpp::node::Node::SharedPtr From 1ac062fbbd8c55755a92fc34a496cecb74148383 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Fri, 1 Apr 2016 11:08:09 -0700 Subject: [PATCH 33/49] zero init timer --- rclcpp/include/rclcpp/timer.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index 164c50cb65..fee71424e7 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -80,7 +80,7 @@ class TimerBase bool is_ready(); protected: - rcl_timer_t timer_handle_; + rcl_timer_t timer_handle_ = rcl_get_zero_initialized_timer(); }; From ad0f2c219075317e0fb4f93008e6cefba121cbc7 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Fri, 1 Apr 2016 14:10:58 -0700 Subject: [PATCH 34/49] don't use rcl callback path --- rclcpp/include/rclcpp/timer.hpp | 45 +++++++++++++-------------------- rclcpp/src/rclcpp/timer.cpp | 9 ------- 2 files changed, 17 insertions(+), 37 deletions(-) diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index fee71424e7..4f03b66f40 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -56,7 +56,7 @@ class TimerBase RCLCPP_PUBLIC void - execute_callback(); + execute_callback() = 0; RCLCPP_PUBLIC const rcl_timer_t * @@ -110,7 +110,12 @@ class GenericTimer : public TimerBase GenericTimer(std::chrono::nanoseconds period, FunctorT && callback) : TimerBase(period), callback_(std::forward(callback)) { - initialize_rcl_handle(period); + if (rcl_timer_init( + &timer_handle_, 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()); + } } /// Default destructor. @@ -123,6 +128,12 @@ class GenericTimer : public TimerBase } } + void + execute_callback() + { + execute_callback_delegate<>(); + } + // void specialization template< typename CallbackT = FunctorT, @@ -131,20 +142,9 @@ class GenericTimer : public TimerBase >::type * = nullptr > void - initialize_rcl_handle(std::chrono::nanoseconds & period) + execute_callback_delegate() { - auto rcl_callback = std::function( - [this](rcl_timer_t * timer, uint64_t last_call_time) { - (void)timer; - (void)last_call_time; - callback_(); - }); - if (rcl_timer_init( - &timer_handle_, period.count(), rcl_callback.target(), - rcl_get_default_allocator()) != RCL_RET_OK) - { - fprintf(stderr, "Couldn't initialize rcl timer handle: %s\n", rcl_get_error_string_safe()); - } + callback_(); } template< @@ -154,20 +154,9 @@ class GenericTimer : public TimerBase >::type * = nullptr > void - initialize_rcl_handle(std::chrono::nanoseconds & period) + execute_callback_delegate() { - auto rcl_callback = std::function( - [this](rcl_timer_t * timer, uint64_t last_call_time) { - (void)timer; - (void)last_call_time; - callback_(*this); - }); - if (rcl_timer_init( - &timer_handle_, period.count(), rcl_callback.target(), - rcl_get_default_allocator()) != RCL_RET_OK) - { - fprintf(stderr, "Couldn't initialize rcl timer handle: %s\n", rcl_get_error_string_safe()); - } + callback_(*this); } virtual bool diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index 4044c7715a..d357731f89 100644 --- a/rclcpp/src/rclcpp/timer.cpp +++ b/rclcpp/src/rclcpp/timer.cpp @@ -34,15 +34,6 @@ TimerBase::cancel() } } -void -TimerBase::execute_callback() -{ - if (rcl_timer_call(&timer_handle_) != RCL_RET_OK) { - throw std::runtime_error( - std::string("Execution of timer callback failed: ") + rcl_get_error_string_safe()); - }; -} - bool TimerBase::is_ready() { From 11a33fc8af4099b773dac0dee95451e341160f60 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Fri, 1 Apr 2016 16:01:08 -0700 Subject: [PATCH 35/49] Timers not working --- .../rclcpp/strategies/allocator_memory_strategy.hpp | 1 + rclcpp/include/rclcpp/timer.hpp | 9 ++++++++- rclcpp/src/rclcpp/executor.cpp | 1 - 3 files changed, 9 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 345ae7d5e4..1ee608a109 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -89,6 +89,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy subscription_handles_.clear(); service_handles_.clear(); client_handles_.clear(); + timer_handles_.clear(); } virtual void remove_null_handles(rcl_wait_set_t * wait_set) diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index 4f03b66f40..da16686f85 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -55,7 +55,7 @@ class TimerBase cancel(); RCLCPP_PUBLIC - void + virtual void execute_callback() = 0; RCLCPP_PUBLIC @@ -131,6 +131,13 @@ class GenericTimer : public TimerBase void execute_callback() { + rcl_ret_t ret = rcl_timer_call(&timer_handle_); + if (ret == RCL_RET_TIMER_CANCELED) { + return; + } + if (ret != RCL_RET_OK) { + throw std::runtime_error("Failed to notify timer that callback occurred"); + } execute_callback_delegate<>(); } diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index a621bc711b..5cc901f18c 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -338,7 +338,6 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) ); } - fprintf(stderr, "Got %d ready subscriptions\n", memory_strategy_->number_of_ready_subscriptions()); if (rcl_wait_set_resize_subscriptions( &waitset_, memory_strategy_->number_of_ready_subscriptions()) != RCL_RET_OK) { From d37e00e522fd4c76eab81d015390b2b3ca577e37 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Fri, 1 Apr 2016 17:45:51 -0700 Subject: [PATCH 36/49] Linting --- .../rclcpp/allocator/allocator_common.hpp | 10 ++- rclcpp/include/rclcpp/client.hpp | 7 ++- rclcpp/include/rclcpp/memory_strategy.hpp | 2 +- rclcpp/include/rclcpp/node.hpp | 4 +- rclcpp/include/rclcpp/node_impl.hpp | 12 ++-- rclcpp/include/rclcpp/publisher.hpp | 17 +++-- rclcpp/include/rclcpp/service.hpp | 12 ++-- .../strategies/allocator_memory_strategy.hpp | 21 ++++--- rclcpp/include/rclcpp/subscription.hpp | 11 ++-- rclcpp/include/rclcpp/timer.hpp | 4 +- rclcpp/src/rclcpp/executor.cpp | 63 +++++++++++-------- rclcpp/src/rclcpp/memory_strategy.cpp | 9 +-- rclcpp/src/rclcpp/node.cpp | 17 +++-- rclcpp/src/rclcpp/publisher.cpp | 16 ++--- rclcpp/src/rclcpp/subscription.cpp | 2 +- rclcpp/src/rclcpp/timer.cpp | 7 ++- rclcpp/src/rclcpp/utilities.cpp | 6 +- 17 files changed, 121 insertions(+), 99 deletions(-) diff --git a/rclcpp/include/rclcpp/allocator/allocator_common.hpp b/rclcpp/include/rclcpp/allocator/allocator_common.hpp index 117c92d626..c1b1bac485 100644 --- a/rclcpp/include/rclcpp/allocator/allocator_common.hpp +++ b/rclcpp/include/rclcpp/allocator/allocator_common.hpp @@ -53,7 +53,8 @@ void retyped_deallocate(void * untyped_pointer, void * untyped_allocator) } // Convert a std::allocator_traits-formatted Allocator into an rcl allocator -template>::value>::type * = nullptr> +template>::value>::type * = nullptr> rcl_allocator_t get_rcl_allocator(Alloc & allocator) { rcl_allocator_t rcl_allocator = rcl_get_default_allocator(); @@ -66,17 +67,14 @@ rcl_allocator_t get_rcl_allocator(Alloc & allocator) } // TODO(jacquelinekay) Workaround for an incomplete implementation of std::allocator -template>::value>::type * = nullptr> +template>::value>::type * = nullptr> rcl_allocator_t get_rcl_allocator(Alloc & allocator) { (void)allocator; return rcl_get_default_allocator(); } -// TODO provide a nicer way of making a C++ allocator maybe? -// or passing rcl allocators directly? - - } // namespace allocator } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index fe53219c4b..b824caf2b4 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -23,8 +23,8 @@ #include #include -#include -#include +#include "rcl/error_handling.h" +#include "rcl/client.h" #include "rclcpp/function_traits.hpp" #include "rclcpp/macros.hpp" @@ -105,7 +105,8 @@ class Client : public ClientBase auto service_type_support_handle = get_service_type_support_handle(); if (rcl_client_init(&client_handle_, this->node_handle_.get(), - service_type_support_handle, service_name.c_str(), &client_options) != RCL_RET_OK) { + service_type_support_handle, service_name.c_str(), &client_options) != RCL_RET_OK) + { // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) throw std::runtime_error( std::string("could not create client: ") + diff --git a/rclcpp/include/rclcpp/memory_strategy.hpp b/rclcpp/include/rclcpp/memory_strategy.hpp index 65ddcd9016..dbd8d5a684 100644 --- a/rclcpp/include/rclcpp/memory_strategy.hpp +++ b/rclcpp/include/rclcpp/memory_strategy.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include "rcl/wait.h" #include "rclcpp/any_executable.hpp" #include "rclcpp/macros.hpp" diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 07516473c8..987c333c2a 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -22,8 +22,8 @@ #include #include -#include -#include +#include "rcl/error_handling.h" +#include "rcl/node.h" #include "rcl_interfaces/msg/list_parameters_result.hpp" #include "rcl_interfaces/msg/parameter_descriptor.hpp" diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index eb89cec2c9..53ea06f5ab 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -30,8 +30,8 @@ #include #include -#include -#include +#include "rcl/publisher.h" +#include "rcl/subscription.h" #include "rcl_interfaces/msg/intra_process_message.hpp" @@ -77,7 +77,9 @@ Node::create_publisher( auto publisher_options = rcl_publisher_get_default_options(); publisher_options.qos = qos_profile; - auto message_alloc = std::make_shared::MessageAlloc>(*allocator.get()); + auto message_alloc = + std::make_shared::MessageAlloc>( + *allocator.get()); publisher_options.allocator = allocator::get_rcl_allocator( *message_alloc.get()); @@ -155,8 +157,8 @@ Node::create_subscription( msg_mem_strat = rclcpp::message_memory_strategy::MessageMemoryStrategy::create_default(); } - // TODO scope - auto message_alloc = std::make_shared::MessageAlloc>(); + auto message_alloc = + std::make_shared::MessageAlloc>(); auto subscription_options = rcl_subscription_get_default_options(); subscription_options.qos = qos_profile; diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index fcf1919b83..f6320a680f 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -24,8 +24,8 @@ #include #include -#include -#include +#include "rcl/error_handling.h" +#include "rcl/publisher.h" #include "rcl_interfaces/msg/intra_process_message.hpp" #include "rmw/impl/cpp/demangle.hpp" @@ -163,21 +163,20 @@ class Publisher : public PublisherBase rcl_allocator_ = publisher_options.allocator; auto type_support_handle = get_message_type_support_handle(); if (rcl_publisher_init( - &publisher_handle_, node_handle_.get(), type_support_handle, - topic.c_str(), &publisher_options) != RCL_RET_OK) + &publisher_handle_, node_handle_.get(), type_support_handle, + topic.c_str(), &publisher_options) != RCL_RET_OK) { throw std::runtime_error( - std::string("could not create publisher: ") + - rcl_get_error_string_safe()); + std::string("could not create publisher: ") + + rcl_get_error_string_safe()); } // Life time of this object is tied to the publisher handle. rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle(&publisher_handle_); if (!publisher_rmw_handle) { throw std::runtime_error( - std::string("failed to get rmw handle: ") + rcl_get_error_string_safe()); + std::string("failed to get rmw handle: ") + rcl_get_error_string_safe()); } - if (rmw_get_gid_for_publisher(publisher_rmw_handle, &rmw_gid_) != RMW_RET_OK) - { + if (rmw_get_gid_for_publisher(publisher_rmw_handle, &rmw_gid_) != RMW_RET_OK) { // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) throw std::runtime_error( std::string("failed to get publisher gid: ") + rmw_get_error_string_safe()); diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index c896934457..82412aedc2 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -21,8 +21,8 @@ #include #include -#include -#include +#include "rcl/error_handling.h" +#include "rcl/service.h" #include "rclcpp/any_service_callback.hpp" #include "rclcpp/macros.hpp" @@ -101,12 +101,12 @@ class Service : public ServiceBase auto service_type_support_handle = get_service_type_support_handle(); if (rcl_service_init( - &service_handle_, node_handle.get(), service_type_support_handle, service_name.c_str(), - &service_options) != RCL_RET_OK) + &service_handle_, node_handle.get(), service_type_support_handle, service_name.c_str(), + &service_options) != RCL_RET_OK) { throw std::runtime_error( - std::string("could not create service: ") + - rcl_get_error_string_safe()); + std::string("could not create service: ") + + rcl_get_error_string_safe()); } } diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 1ee608a109..06c22dc2e7 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -178,7 +178,8 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy return has_invalid_weak_nodes; } - bool add_handles_to_waitset(rcl_wait_set_t * wait_set) { + bool add_handles_to_waitset(rcl_wait_set_t * wait_set) + { for (auto subscription : subscription_handles_) { if (rcl_wait_set_add_subscription(wait_set, subscription) != RCL_RET_OK) { fprintf(stderr, "Couldn't add subscription to waitset: %s\n", rcl_get_error_string_safe()); @@ -209,7 +210,8 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy for (auto guard_condition : guard_conditions_) { if (rcl_wait_set_add_guard_condition(wait_set, guard_condition) != RCL_RET_OK) { - fprintf(stderr, "Couldn't add guard_condition to waitset: %s\n", rcl_get_error_string_safe()); + fprintf(stderr, "Couldn't add guard_condition to waitset: %s\n", + rcl_get_error_string_safe()); return false; } } @@ -333,23 +335,28 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } } - size_t number_of_ready_subscriptions() const { + size_t number_of_ready_subscriptions() const + { return subscription_handles_.size(); } - size_t number_of_ready_services() const { + size_t number_of_ready_services() const + { return service_handles_.size(); } - size_t number_of_ready_clients() const { + size_t number_of_ready_clients() const + { return client_handles_.size(); } - size_t number_of_guard_conditions() const { + size_t number_of_guard_conditions() const + { return guard_conditions_.size(); } - size_t number_of_ready_timers() const { + size_t number_of_ready_timers() const + { return timer_handles_.size(); } diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index dc268d5ba2..b6d8b47524 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -1,4 +1,5 @@ // Copyright 2014 Open Source Robotics Foundation, Inc. +// // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at @@ -23,8 +24,8 @@ #include #include -#include -#include +#include "rcl/error_handling.h" +#include "rcl/subscription.h" #include "rcl_interfaces/msg/intra_process_message.hpp" @@ -158,11 +159,11 @@ class Subscription : public SubscriptionBase auto type_support_handle = get_message_type_support_handle(); if (rcl_subscription_init( - &subscription_handle_, node_handle_.get(), type_support_handle, topic_name.c_str(), - &subscription_options) != RCL_RET_OK) + &subscription_handle_, node_handle_.get(), type_support_handle, topic_name.c_str(), + &subscription_options) != RCL_RET_OK) { throw std::runtime_error( - std::string("could not create subscription: ") + rcl_get_error_string_safe()); + std::string("could not create subscription: ") + rcl_get_error_string_safe()); } } diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index da16686f85..7109bdae98 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -111,8 +111,8 @@ class GenericTimer : public TimerBase : TimerBase(period), callback_(std::forward(callback)) { if (rcl_timer_init( - &timer_handle_, period.count(), nullptr, - rcl_get_default_allocator()) != RCL_RET_OK) + &timer_handle_, 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()); } diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 5cc901f18c..6f1e8d48ca 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -35,11 +35,11 @@ Executor::Executor(const ExecutorArgs & args) { rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options(); if (rcl_guard_condition_init( - &interrupt_guard_condition_, guard_condition_options) != RCL_RET_OK) + &interrupt_guard_condition_, guard_condition_options) != RCL_RET_OK) { throw std::runtime_error( - std::string("Failed to create interrupt guard condition in Executor constructor: ") + - rcl_get_error_string_safe()); + std::string("Failed to create interrupt guard condition in Executor constructor: ") + + rcl_get_error_string_safe()); } // The number of guard conditions is always at least 2: 1 for the ctrl-c guard cond, @@ -52,7 +52,7 @@ Executor::Executor(const ExecutorArgs & args) memory_strategy_->add_guard_condition(&interrupt_guard_condition_); if (rcl_wait_set_init( - &waitset_, 0, 2, 0, 0, 0, rcl_get_default_allocator()) != RCL_RET_OK) + &waitset_, 0, 2, 0, 0, 0, rcl_get_default_allocator()) != RCL_RET_OK) { fprintf(stderr, "[rclcpp::error] failed to create waitset: %s\n", rcl_get_error_string_safe()); @@ -125,7 +125,7 @@ Executor::remove_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify) if (notify) { // If the node was matched and removed, interrupt waiting if (node_removed) { - if (rcl_trigger_guard_condition(&interrupt_guard_condition_)!= RCL_RET_OK) { + if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { throw std::runtime_error(rcl_get_error_string_safe()); } } @@ -339,43 +339,43 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) } if (rcl_wait_set_resize_subscriptions( - &waitset_, memory_strategy_->number_of_ready_subscriptions()) != RCL_RET_OK) + &waitset_, memory_strategy_->number_of_ready_subscriptions()) != RCL_RET_OK) { throw std::runtime_error( - std::string("Couldn't resize the number of subscriptions in waitset : ") + - rcl_get_error_string_safe()); + std::string("Couldn't resize the number of subscriptions in waitset : ") + + rcl_get_error_string_safe()); } if (rcl_wait_set_resize_services( - &waitset_, memory_strategy_->number_of_ready_services()) != RCL_RET_OK) + &waitset_, memory_strategy_->number_of_ready_services()) != RCL_RET_OK) { throw std::runtime_error( - std::string("Couldn't resize the number of services in waitset : ") + - rcl_get_error_string_safe()); + std::string("Couldn't resize the number of services in waitset : ") + + rcl_get_error_string_safe()); } if (rcl_wait_set_resize_clients( - &waitset_, memory_strategy_->number_of_ready_clients()) != RCL_RET_OK) + &waitset_, memory_strategy_->number_of_ready_clients()) != RCL_RET_OK) { throw std::runtime_error( - std::string("Couldn't resize the number of clients in waitset : ") + - rcl_get_error_string_safe()); + std::string("Couldn't resize the number of clients in waitset : ") + + rcl_get_error_string_safe()); } if (rcl_wait_set_resize_guard_conditions( - &waitset_, memory_strategy_->number_of_guard_conditions()) != RCL_RET_OK) + &waitset_, memory_strategy_->number_of_guard_conditions()) != RCL_RET_OK) { throw std::runtime_error( - std::string("Couldn't resize the number of guard_conditions in waitset : ") + - rcl_get_error_string_safe()); + std::string("Couldn't resize the number of guard_conditions in waitset : ") + + rcl_get_error_string_safe()); } if (rcl_wait_set_resize_timers( - &waitset_, memory_strategy_->number_of_ready_timers()) != RCL_RET_OK) + &waitset_, memory_strategy_->number_of_ready_timers()) != RCL_RET_OK) { throw std::runtime_error( - std::string("Couldn't resize the number of timers in waitset : ") + - rcl_get_error_string_safe()); + std::string("Couldn't resize the number of timers in waitset : ") + + rcl_get_error_string_safe()); } if (!memory_strategy_->add_handles_to_waitset(&waitset_)) { @@ -383,8 +383,7 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) } rcl_ret_t status = rcl_wait(&waitset_, timeout.count()); - if (status == RCL_RET_WAIT_SET_EMPTY) - { + if (status == RCL_RET_WAIT_SET_EMPTY) { fprintf(stderr, "Warning: empty waitset received in rcl_wait(). This should never happen.\n"); } else if (status != RCL_RET_OK && status != RCL_RET_TIMEOUT) { throw std::runtime_error(std::string("rcl_wait() failed: ") + rcl_get_error_string_safe()); @@ -393,11 +392,21 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) // check the null handles in the waitset and remove them from the handles in memory strategy // for callback-based entities memory_strategy_->remove_null_handles(&waitset_); - rcl_wait_set_clear_subscriptions(&waitset_); - rcl_wait_set_clear_services(&waitset_); - rcl_wait_set_clear_clients(&waitset_); - rcl_wait_set_clear_guard_conditions(&waitset_); - rcl_wait_set_clear_timers(&waitset_); + if (rcl_wait_set_clear_subscriptions(&waitset_) != RCL_RET_OK) { + throw std::runtime_error("Couldn't clear subscriptions from waitset"); + } + if (rcl_wait_set_clear_services(&waitset_) != RCL_RET_OK) { + throw std::runtime_error("Couldn't clear servicess from waitset"); + } + if (rcl_wait_set_clear_clients(&waitset_) != RCL_RET_OK) { + throw std::runtime_error("Couldn't clear clients from waitset"); + } + if (rcl_wait_set_clear_guard_conditions(&waitset_) != RCL_RET_OK) { + throw std::runtime_error("Couldn't clear guard conditions from waitset"); + } + if (rcl_wait_set_clear_timers(&waitset_) != RCL_RET_OK) { + throw std::runtime_error("Couldn't clear timers from waitset"); + } } rclcpp::node::Node::SharedPtr diff --git a/rclcpp/src/rclcpp/memory_strategy.cpp b/rclcpp/src/rclcpp/memory_strategy.cpp index 20810aa8b4..b068e6880e 100644 --- a/rclcpp/src/rclcpp/memory_strategy.cpp +++ b/rclcpp/src/rclcpp/memory_strategy.cpp @@ -36,8 +36,7 @@ MemoryStrategy::get_subscription_by_handle( if (subscription->get_subscription_handle() == subscriber_handle) { return subscription; } - if (subscription->get_intra_process_subscription_handle() == subscriber_handle) - { + if (subscription->get_intra_process_subscription_handle() == subscriber_handle) { return subscription; } } @@ -48,7 +47,8 @@ MemoryStrategy::get_subscription_by_handle( } rclcpp::service::ServiceBase::SharedPtr -MemoryStrategy::get_service_by_handle(const rcl_service_t * service_handle, const WeakNodeVector & weak_nodes) +MemoryStrategy::get_service_by_handle(const rcl_service_t * service_handle, + const WeakNodeVector & weak_nodes) { for (auto & weak_node : weak_nodes) { auto node = weak_node.lock(); @@ -71,7 +71,8 @@ MemoryStrategy::get_service_by_handle(const rcl_service_t * service_handle, cons } rclcpp::client::ClientBase::SharedPtr -MemoryStrategy::get_client_by_handle(const rcl_client_t * client_handle, const WeakNodeVector & weak_nodes) +MemoryStrategy::get_client_by_handle(const rcl_client_t * client_handle, + const WeakNodeVector & weak_nodes) { for (auto & weak_node : weak_nodes) { auto node = weak_node.lock(); diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 7a2158ad0a..49b827faa1 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -38,14 +38,13 @@ Node::Node( number_of_subscriptions_(0), number_of_timers_(0), number_of_services_(0), use_intra_process_comms_(use_intra_process_comms) { - rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options(); if (rcl_guard_condition_init( - ¬ify_guard_condition_, guard_condition_options) != RCL_RET_OK) + ¬ify_guard_condition_, guard_condition_options) != RCL_RET_OK) { throw std::runtime_error( - std::string("Failed to create interrupt guard condition in Executor constructor: ") + - rcl_get_error_string_safe()); + std::string("Failed to create interrupt guard condition in Executor constructor: ") + + rcl_get_error_string_safe()); } has_executor.store(false); @@ -74,7 +73,6 @@ Node::Node( #endif } - //node_handle_ = std::make_shared(rcl_get_zero_initialized_node()); rcl_node_t * rcl_node = new rcl_node_t(rcl_get_zero_initialized_node()); node_handle_.reset(rcl_node, [](rcl_node_t * node) { if (rcl_node_fini(node) != RMW_RET_OK) { @@ -93,7 +91,8 @@ Node::Node( "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe()); } - throw std::runtime_error(std::string("Could not initialize rcl node: ") + rcl_get_error_string_safe()); + throw std::runtime_error(std::string( + "Could not initialize rcl node: ") + rcl_get_error_string_safe()); } // Initialize node handle shared_ptr with custom deleter. @@ -308,7 +307,7 @@ Node::get_topic_names_and_types() const topic_names_and_types.type_names = nullptr; auto ret = rmw_get_topic_names_and_types(rcl_node_get_rmw_handle(node_handle_.get()), - &topic_names_and_types); + &topic_names_and_types); if (ret != RMW_RET_OK) { // *INDENT-OFF* throw std::runtime_error( @@ -337,7 +336,7 @@ Node::count_publishers(const std::string & topic_name) const { size_t count; auto ret = rmw_count_publishers(rcl_node_get_rmw_handle(node_handle_.get()), - topic_name.c_str(), &count); + topic_name.c_str(), &count); if (ret != RMW_RET_OK) { // *INDENT-OFF* throw std::runtime_error( @@ -352,7 +351,7 @@ Node::count_subscribers(const std::string & topic_name) const { size_t count; auto ret = rmw_count_subscribers(rcl_node_get_rmw_handle(node_handle_.get()), - topic_name.c_str(), &count); + topic_name.c_str(), &count); if (ret != RMW_RET_OK) { // *INDENT-OFF* throw std::runtime_error( diff --git a/rclcpp/src/rclcpp/publisher.cpp b/rclcpp/src/rclcpp/publisher.cpp index 6c001244e8..97c310278e 100644 --- a/rclcpp/src/rclcpp/publisher.cpp +++ b/rclcpp/src/rclcpp/publisher.cpp @@ -98,21 +98,23 @@ PublisherBase::setup_intra_process( rcl_publisher_options_t & intra_process_options) { if (rcl_publisher_init( - &intra_process_publisher_handle_, node_handle_.get(), - rclcpp::type_support::get_intra_process_message_msg_type_support(), - (topic_ + "__intra").c_str(), &intra_process_options) != RCL_RET_OK) + &intra_process_publisher_handle_, node_handle_.get(), + rclcpp::type_support::get_intra_process_message_msg_type_support(), + (topic_ + "__intra").c_str(), &intra_process_options) != RCL_RET_OK) { throw std::runtime_error( - std::string("could not create intra process publisher: ") + - rcl_get_error_string_safe()); + std::string("could not create intra process publisher: ") + + rcl_get_error_string_safe()); } intra_process_publisher_id_ = intra_process_publisher_id; store_intra_process_message_ = callback; // Life time of this object is tied to the publisher handle. - rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle(&intra_process_publisher_handle_); + rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle( + &intra_process_publisher_handle_); if (publisher_rmw_handle == nullptr) { - throw std::runtime_error(std::string("Failed to get rmw publisher handle") + rcl_get_error_string_safe()); + throw std::runtime_error(std::string( + "Failed to get rmw publisher handle") + rcl_get_error_string_safe()); } auto ret = rmw_get_gid_for_publisher( publisher_rmw_handle, &intra_process_rmw_gid_); diff --git a/rclcpp/src/rclcpp/subscription.cpp b/rclcpp/src/rclcpp/subscription.cpp index 51c21d4504..34cd52563c 100644 --- a/rclcpp/src/rclcpp/subscription.cpp +++ b/rclcpp/src/rclcpp/subscription.cpp @@ -44,7 +44,7 @@ SubscriptionBase::~SubscriptionBase() (std::cerr << ss.str()).flush(); } if (rcl_subscription_fini( - &intra_process_subscription_handle_, node_handle_.get()) != RCL_RET_OK) + &intra_process_subscription_handle_, node_handle_.get()) != RCL_RET_OK) { std::stringstream ss; ss << "Error in destruction of rmw intra process subscription handle: " << diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index d357731f89..ec2501f94e 100644 --- a/rclcpp/src/rclcpp/timer.cpp +++ b/rclcpp/src/rclcpp/timer.cpp @@ -15,6 +15,7 @@ #include "rclcpp/timer.hpp" #include +#include using rclcpp::timer::TimerBase; @@ -38,8 +39,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_, &ready) != RCL_RET_OK) { throw std::runtime_error(std::string("Failed to check timer: ") + rcl_get_error_string_safe()); } return ready; @@ -51,7 +51,8 @@ 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) { throw std::runtime_error( - std::string("Timer could not get time until next call: ") + rcl_get_error_string_safe()); + std::string("Timer could not get time until next call: ") + + rcl_get_error_string_safe()); } return std::chrono::nanoseconds(time_until_next_call); } diff --git a/rclcpp/src/rclcpp/utilities.cpp b/rclcpp/src/rclcpp/utilities.cpp index 80a58d5577..3c1d487a54 100644 --- a/rclcpp/src/rclcpp/utilities.cpp +++ b/rclcpp/src/rclcpp/utilities.cpp @@ -36,7 +36,8 @@ /// Represent the status of the global interrupt signal. static volatile sig_atomic_t g_signal_status = 0; /// Guard condition for interrupting the rmw implementation when the global interrupt signal fired. -static rcl_guard_condition_t g_sigint_guard_cond_handle = rcl_get_zero_initialized_guard_condition(); +static rcl_guard_condition_t g_sigint_guard_cond_handle = + rcl_get_zero_initialized_guard_condition(); /// Condition variable for timed sleep (see sleep_for). static std::condition_variable g_interrupt_condition_variable; static std::atomic g_is_interrupted(false); @@ -140,7 +141,8 @@ rclcpp::utilities::init(int argc, char * argv[]) } rcl_guard_condition_options_t options = rcl_guard_condition_get_default_options(); if (rcl_guard_condition_init(&g_sigint_guard_cond_handle, options) != RCL_RET_OK) { - throw std::runtime_error(std::string("Couldn't initialize guard condition: ") + rcl_get_error_string_safe()); + throw std::runtime_error(std::string( + "Couldn't initialize guard condition: ") + rcl_get_error_string_safe()); } } From 153cf13315766111d79f617f763eef695d47d1a0 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Fri, 1 Apr 2016 19:25:10 -0700 Subject: [PATCH 37/49] Fix intra process --- rclcpp/include/rclcpp/client.hpp | 2 +- rclcpp/include/rclcpp/node_impl.hpp | 3 +- rclcpp/include/rclcpp/publisher.hpp | 6 +- rclcpp/include/rclcpp/service.hpp | 154 +++++++++--------- .../strategies/allocator_memory_strategy.hpp | 3 + rclcpp/include/rclcpp/subscription.hpp | 4 +- rclcpp/src/rclcpp/publisher.cpp | 2 +- 7 files changed, 88 insertions(+), 86 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index b824caf2b4..a58b1d0698 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -70,7 +70,7 @@ class ClientBase std::shared_ptr node_handle_; - rcl_client_t client_handle_; + rcl_client_t client_handle_ = rcl_get_zero_initialized_client(); std::string service_name_; }; diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 53ea06f5ab..eb826dcd53 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -119,11 +119,10 @@ Node::create_publisher( return message_seq; }; // *INDENT-ON* - rcl_publisher_options_t intra_process_options = publisher_options; publisher->setup_intra_process( intra_process_publisher_id, shared_publish_callback, - intra_process_options); + publisher_options); } if (rcl_trigger_guard_condition(¬ify_guard_condition_) != RMW_RET_OK) { throw std::runtime_error( diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index f6320a680f..b67157eb05 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -56,7 +56,7 @@ class PublisherBase /** * Typically, a publisher is not created through this method, but instead is created through a * call to `Node::create_publisher`. - * \param[in] node_handle The corresponding rmw representation of the owner node. + * \param[in] node_handle The corresponding rcl representation of the owner node. * \param[in] topic The topic that this publisher publishes on. * \param[in] queue_size The maximum number of unpublished messages to queue. */ @@ -118,7 +118,7 @@ class PublisherBase setup_intra_process( uint64_t intra_process_publisher_id, StoreMessageCallbackT callback, - rcl_publisher_options_t & intra_process_options); + const rcl_publisher_options_t & intra_process_options); std::shared_ptr node_handle_; @@ -153,7 +153,7 @@ class Publisher : public PublisherBase Publisher( std::shared_ptr node_handle, std::string topic, - rcl_publisher_options_t & publisher_options, + const rcl_publisher_options_t & publisher_options, std::shared_ptr allocator) : PublisherBase(node_handle, topic, publisher_options.qos.depth), message_allocator_(allocator) { diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 82412aedc2..197abe233a 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -1,90 +1,90 @@ -// Copyright 2014 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef RCLCPP__SERVICE_HPP_ -#define RCLCPP__SERVICE_HPP_ - -#include -#include -#include -#include -#include - -#include "rcl/error_handling.h" -#include "rcl/service.h" - -#include "rclcpp/any_service_callback.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp/type_support_decl.hpp" -#include "rclcpp/visibility_control.hpp" -#include "rmw/error_handling.h" -#include "rmw/rmw.h" - -namespace rclcpp -{ -namespace service -{ - -class ServiceBase -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ServiceBase); - - RCLCPP_PUBLIC - ServiceBase( - std::shared_ptr node_handle, - const std::string service_name); + // Copyright 2014 Open Source Robotics Foundation, Inc. + // + // Licensed under the Apache License, Version 2.0 (the "License"); + // you may not use this file except in compliance with the License. + // You may obtain a copy of the License at + // + // http://www.apache.org/licenses/LICENSE-2.0 + // + // Unless required by applicable law or agreed to in writing, software + // distributed under the License is distributed on an "AS IS" BASIS, + // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + // See the License for the specific language governing permissions and + // limitations under the License. + + #ifndef RCLCPP__SERVICE_HPP_ + #define RCLCPP__SERVICE_HPP_ + + #include + #include + #include + #include + #include + + #include "rcl/error_handling.h" + #include "rcl/service.h" + + #include "rclcpp/any_service_callback.hpp" + #include "rclcpp/macros.hpp" + #include "rclcpp/type_support_decl.hpp" + #include "rclcpp/visibility_control.hpp" + #include "rmw/error_handling.h" + #include "rmw/rmw.h" + + namespace rclcpp + { + namespace service + { - RCLCPP_PUBLIC - virtual ~ServiceBase(); + class ServiceBase + { + public: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ServiceBase); - RCLCPP_PUBLIC - std::string - get_service_name(); + RCLCPP_PUBLIC + ServiceBase( + std::shared_ptr node_handle, + const std::string service_name); - RCLCPP_PUBLIC - const rcl_service_t * - get_service_handle(); + RCLCPP_PUBLIC + virtual ~ServiceBase(); - virtual std::shared_ptr create_request() = 0; - virtual std::shared_ptr create_request_header() = 0; - virtual void handle_request( - std::shared_ptr request_header, - std::shared_ptr request) = 0; + RCLCPP_PUBLIC + std::string + get_service_name(); -protected: - RCLCPP_DISABLE_COPY(ServiceBase); + RCLCPP_PUBLIC + const rcl_service_t * + get_service_handle(); - std::shared_ptr node_handle_; + virtual std::shared_ptr create_request() = 0; + virtual std::shared_ptr create_request_header() = 0; + virtual void handle_request( + std::shared_ptr request_header, + std::shared_ptr request) = 0; - rcl_service_t service_handle_; - std::string service_name_; -}; + protected: + RCLCPP_DISABLE_COPY(ServiceBase); -using any_service_callback::AnyServiceCallback; + std::shared_ptr node_handle_; -template -class Service : public ServiceBase -{ -public: - using CallbackType = std::function< - void( - const std::shared_ptr, - std::shared_ptr)>; + rcl_service_t service_handle_ = rcl_get_zero_initialized_service(); + std::string service_name_; + }; - using CallbackWithHeaderType = std::function< - void( + using any_service_callback::AnyServiceCallback; + + template + class Service : public ServiceBase + { + public: + using CallbackType = std::function< + void( + const std::shared_ptr, + std::shared_ptr)>; + + using CallbackWithHeaderType = std::function< + void( const std::shared_ptr, const std::shared_ptr, std::shared_ptr)>; diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 06c22dc2e7..7dee45aeda 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -154,6 +154,9 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy auto subscription = weak_subscription.lock(); if (subscription) { subscription_handles_.push_back(subscription->get_subscription_handle()); + if (subscription->get_intra_process_subscription_handle()) { + subscription_handles_.push_back(subscription->get_intra_process_subscription_handle()); + } } } for (auto & service : group->get_service_ptrs()) { diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index b6d8b47524..f352d7a2a8 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -143,7 +143,7 @@ class Subscription : public SubscriptionBase Subscription( std::shared_ptr node_handle, const std::string & topic_name, - rcl_subscription_options_t & subscription_options, + const rcl_subscription_options_t & subscription_options, AnySubscriptionCallback callback, typename message_memory_strategy::MessageMemoryStrategy::SharedPtr memory_strategy = message_memory_strategy::MessageMemoryStrategy Date: Fri, 1 Apr 2016 19:39:09 -0700 Subject: [PATCH 38/49] Uncrustify and fix intra process for real now --- rclcpp/include/rclcpp/service.hpp | 112 +++++++++--------- .../strategies/allocator_memory_strategy.hpp | 3 +- rclcpp/include/rclcpp/subscription.hpp | 13 +- 3 files changed, 69 insertions(+), 59 deletions(-) diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 197abe233a..81466d4bb2 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -1,16 +1,16 @@ - // Copyright 2014 Open Source Robotics Foundation, Inc. - // - // Licensed under the Apache License, Version 2.0 (the "License"); - // you may not use this file except in compliance with the License. - // You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, - // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - // See the License for the specific language governing permissions and - // limitations under the License. +// Copyright 2014 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef RCLCPP__SERVICE_HPP_ #define RCLCPP__SERVICE_HPP_ @@ -31,60 +31,60 @@ #include "rmw/error_handling.h" #include "rmw/rmw.h" - namespace rclcpp - { - namespace service - { +namespace rclcpp +{ +namespace service +{ - class ServiceBase - { - public: - RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ServiceBase); +class ServiceBase +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ServiceBase); - RCLCPP_PUBLIC - ServiceBase( - std::shared_ptr node_handle, - const std::string service_name); + RCLCPP_PUBLIC + ServiceBase( + std::shared_ptr node_handle, + const std::string service_name); - RCLCPP_PUBLIC - virtual ~ServiceBase(); + RCLCPP_PUBLIC + virtual ~ServiceBase(); - RCLCPP_PUBLIC - std::string - get_service_name(); + RCLCPP_PUBLIC + std::string + get_service_name(); - RCLCPP_PUBLIC - const rcl_service_t * - get_service_handle(); + RCLCPP_PUBLIC + const rcl_service_t * + get_service_handle(); - virtual std::shared_ptr create_request() = 0; - virtual std::shared_ptr create_request_header() = 0; - virtual void handle_request( - std::shared_ptr request_header, - std::shared_ptr request) = 0; + virtual std::shared_ptr create_request() = 0; + virtual std::shared_ptr create_request_header() = 0; + virtual void handle_request( + std::shared_ptr request_header, + std::shared_ptr request) = 0; - protected: - RCLCPP_DISABLE_COPY(ServiceBase); +protected: + RCLCPP_DISABLE_COPY(ServiceBase); - std::shared_ptr node_handle_; + std::shared_ptr node_handle_; - rcl_service_t service_handle_ = rcl_get_zero_initialized_service(); - std::string service_name_; - }; + rcl_service_t service_handle_ = rcl_get_zero_initialized_service(); + std::string service_name_; +}; - using any_service_callback::AnyServiceCallback; +using any_service_callback::AnyServiceCallback; - template - class Service : public ServiceBase - { - public: - using CallbackType = std::function< - void( - const std::shared_ptr, - std::shared_ptr)>; - - using CallbackWithHeaderType = std::function< - void( +template +class Service : public ServiceBase +{ +public: + using CallbackType = std::function< + void( + const std::shared_ptr, + std::shared_ptr)>; + + using CallbackWithHeaderType = std::function< + void( const std::shared_ptr, const std::shared_ptr, std::shared_ptr)>; diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 7dee45aeda..5a875812fd 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -155,7 +155,8 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy if (subscription) { subscription_handles_.push_back(subscription->get_subscription_handle()); if (subscription->get_intra_process_subscription_handle()) { - subscription_handles_.push_back(subscription->get_intra_process_subscription_handle()); + subscription_handles_.push_back( + subscription->get_intra_process_subscription_handle()); } } } diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index f352d7a2a8..77259167bf 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -1,5 +1,5 @@ // Copyright 2014 Open Source Robotics Foundation, Inc. -// +// // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at @@ -79,7 +79,7 @@ class SubscriptionBase get_subscription_handle() const; RCLCPP_PUBLIC - const rcl_subscription_t * + virtual const rcl_subscription_t * get_intra_process_subscription_handle() const; /// Borrow a new message. @@ -262,6 +262,15 @@ class Subscription : public SubscriptionBase matches_any_intra_process_publishers_ = matches_any_publisher_callback; } + const rcl_subscription_t * + get_intra_process_subscription_handle() const + { + if (!get_intra_process_message_callback_) { + return nullptr; + } + return &intra_process_subscription_handle_; + } + RCLCPP_DISABLE_COPY(Subscription); AnySubscriptionCallback any_callback_; From 88e2cd8b08197fbb4429ff241a8583b967005535 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Mon, 4 Apr 2016 10:20:15 -0700 Subject: [PATCH 39/49] Add rcl as a general depend, not just a build depend --- rclcpp/CMakeLists.txt | 10 +++------- rclcpp/cmake/get_rclcpp_information.cmake | 15 +++++++++------ rclcpp/package.xml | 2 +- 3 files changed, 13 insertions(+), 14 deletions(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 72eaf17363..25a4b1b209 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -50,10 +50,7 @@ macro(target) ${${PROJECT_NAME}_SRCS}) ament_target_dependencies(${PROJECT_NAME}${target_suffix} "rcl${target_suffix}" - "rcl_interfaces" - "rmw" - "rosidl_generator_cpp" - "${rmw_implementation}") + "rosidl_generator_cpp") # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. @@ -72,9 +69,6 @@ call_for_each_rmw_implementation(target GENERATE_DEFAULT) ament_export_dependencies(ament_cmake) ament_export_dependencies(rcl) -ament_export_dependencies(rcl_interfaces) -ament_export_dependencies(rmw) -ament_export_dependencies(rmw_implementation) ament_export_dependencies(rosidl_generator_cpp) ament_export_include_directories(include) @@ -96,6 +90,7 @@ if(AMENT_ENABLE_TESTING) ament_add_gtest(test_mapped_ring_buffer test/test_mapped_ring_buffer.cpp) if(TARGET test_mapped_ring_buffer) target_include_directories(test_mapped_ring_buffer PUBLIC + ${rcl_INCLUDE_DIRS} ${rcl_interfaces_INCLUDE_DIRS} ${rmw_INCLUDE_DIRS} ${rosidl_generator_cpp_INCLUDE_DIRS} @@ -104,6 +99,7 @@ if(AMENT_ENABLE_TESTING) ament_add_gtest(test_intra_process_manager test/test_intra_process_manager.cpp) if(TARGET test_intra_process_manager) target_include_directories(test_intra_process_manager PUBLIC + ${rcl_INCLUDE_DIRS} ${rcl_interfaces_INCLUDE_DIRS} ${rmw_INCLUDE_DIRS} ${rosidl_generator_cpp_INCLUDE_DIRS} diff --git a/rclcpp/cmake/get_rclcpp_information.cmake b/rclcpp/cmake/get_rclcpp_information.cmake index fd9254da08..81c630ac9c 100644 --- a/rclcpp/cmake/get_rclcpp_information.cmake +++ b/rclcpp/cmake/get_rclcpp_information.cmake @@ -24,16 +24,22 @@ # :type var_prefix: string # macro(get_rclcpp_information rmw_implementation var_prefix) + message(STATUS "calling get_rclcpp_information with var_prefix=${var_prefix}") # pretend to be a "package" # so that the variables can be used by various functions / macros set(${var_prefix}_FOUND TRUE) + # Get rcl using the existing macro + if(NOT "${target_suffix} " STREQUAL " ") + get_rcl_information("${rmw_implementation}" "rcl${target_suffix}") + endif() + # include directories normalize_path(${var_prefix}_INCLUDE_DIRS "${rclcpp_DIR}/../../../include") - # libraries - set(_libs) + # The list of libraries was set in get_rcl_information + # set(_libs) # search for library relative to this CMake file set(_library_target "rclcpp") get_available_rmw_implementations(_rmw_impls) @@ -61,11 +67,8 @@ macro(get_rclcpp_information rmw_implementation var_prefix) endif() # dependencies - set(_exported_dependencies - "rcl_interfaces" - "rmw" + list_append_unique(_exported_dependencies "rcl" - "${rmw_implementation}" "rosidl_generator_cpp") set(${var_prefix}_DEFINITIONS) foreach(_dep ${_exported_dependencies}) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index b17862c1f5..a16a756eca 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -10,13 +10,13 @@ rmw - rcl rcl_interfaces rmw_implementation_cmake rosidl_generator_cpp rcl_interfaces rosidl_generator_cpp + rcl rmw_implementation ament_cmake From 9d2661b2fe3fdb7a628ac4d67d795433b1577660 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Mon, 4 Apr 2016 10:26:41 -0700 Subject: [PATCH 40/49] remove message --- rclcpp/cmake/get_rclcpp_information.cmake | 1 - 1 file changed, 1 deletion(-) diff --git a/rclcpp/cmake/get_rclcpp_information.cmake b/rclcpp/cmake/get_rclcpp_information.cmake index 81c630ac9c..da9659b683 100644 --- a/rclcpp/cmake/get_rclcpp_information.cmake +++ b/rclcpp/cmake/get_rclcpp_information.cmake @@ -24,7 +24,6 @@ # :type var_prefix: string # macro(get_rclcpp_information rmw_implementation var_prefix) - message(STATUS "calling get_rclcpp_information with var_prefix=${var_prefix}") # pretend to be a "package" # so that the variables can be used by various functions / macros set(${var_prefix}_FOUND TRUE) From 2472194451a29bcdcfc1f109f15f5bcc2342cc4b Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Mon, 4 Apr 2016 16:39:22 -0700 Subject: [PATCH 41/49] fix linking again --- rclcpp/cmake/get_rclcpp_information.cmake | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rclcpp/cmake/get_rclcpp_information.cmake b/rclcpp/cmake/get_rclcpp_information.cmake index da9659b683..f69eca17d5 100644 --- a/rclcpp/cmake/get_rclcpp_information.cmake +++ b/rclcpp/cmake/get_rclcpp_information.cmake @@ -38,7 +38,7 @@ macro(get_rclcpp_information rmw_implementation var_prefix) "${rclcpp_DIR}/../../../include") # The list of libraries was set in get_rcl_information - # set(_libs) + set(_libs) # search for library relative to this CMake file set(_library_target "rclcpp") get_available_rmw_implementations(_rmw_impls) @@ -66,8 +66,8 @@ macro(get_rclcpp_information rmw_implementation var_prefix) endif() # dependencies - list_append_unique(_exported_dependencies - "rcl" + set(_exported_dependencies + "rcl${target_suffix}" "rosidl_generator_cpp") set(${var_prefix}_DEFINITIONS) foreach(_dep ${_exported_dependencies}) From 6c2296ae2fa28184bea3c846d52a7f54275a2d5e Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Mon, 4 Apr 2016 19:11:22 -0700 Subject: [PATCH 42/49] Trying to fix services --- rclcpp/cmake/get_rclcpp_information.cmake | 2 +- rclcpp/include/rclcpp/client.hpp | 13 ++++--- rclcpp/include/rclcpp/service.hpp | 43 +++++++++++++---------- rclcpp/src/rclcpp/client.cpp | 8 ----- rclcpp/src/rclcpp/executor.cpp | 16 ++++----- rclcpp/src/rclcpp/service.cpp | 10 ------ 6 files changed, 43 insertions(+), 49 deletions(-) diff --git a/rclcpp/cmake/get_rclcpp_information.cmake b/rclcpp/cmake/get_rclcpp_information.cmake index f69eca17d5..7cb78ce50a 100644 --- a/rclcpp/cmake/get_rclcpp_information.cmake +++ b/rclcpp/cmake/get_rclcpp_information.cmake @@ -37,7 +37,7 @@ macro(get_rclcpp_information rmw_implementation var_prefix) normalize_path(${var_prefix}_INCLUDE_DIRS "${rclcpp_DIR}/../../../include") - # The list of libraries was set in get_rcl_information + # libraries set(_libs) # search for library relative to this CMake file set(_library_target "rclcpp") diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index a58b1d0698..a1dca22ddb 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -49,9 +49,6 @@ class ClientBase std::shared_ptr node_handle, const std::string & service_name); - RCLCPP_PUBLIC - virtual ~ClientBase(); - RCLCPP_PUBLIC const std::string & get_service_name() const; @@ -115,6 +112,14 @@ class Client : public ClientBase } } + ~Client() + { + if (rcl_client_fini(&client_handle_, node_handle_.get()) != RMW_RET_OK) { + fprintf(stderr, + "Error in destruction of rmw client handle: %s\n", rmw_get_error_string_safe()); + } + } + std::shared_ptr create_response() { return std::shared_ptr(new typename ServiceT::Response()); @@ -163,8 +168,8 @@ class Client : public ClientBase > SharedFuture async_send_request(SharedRequest request, CallbackT && cb) { - int64_t sequence_number; std::lock_guard lock(pending_requests_mutex_); + int64_t sequence_number; if (RMW_RET_OK != rcl_send_request(get_client_handle(), request.get(), &sequence_number)) { // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) throw std::runtime_error( diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 81466d4bb2..2c19c58588 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -12,24 +12,24 @@ // See the License for the specific language governing permissions and // limitations under the License. - #ifndef RCLCPP__SERVICE_HPP_ - #define RCLCPP__SERVICE_HPP_ +#ifndef RCLCPP__SERVICE_HPP_ +#define RCLCPP__SERVICE_HPP_ - #include - #include - #include - #include - #include +#include +#include +#include +#include +#include - #include "rcl/error_handling.h" - #include "rcl/service.h" +#include "rcl/error_handling.h" +#include "rcl/service.h" - #include "rclcpp/any_service_callback.hpp" - #include "rclcpp/macros.hpp" - #include "rclcpp/type_support_decl.hpp" - #include "rclcpp/visibility_control.hpp" - #include "rmw/error_handling.h" - #include "rmw/rmw.h" +#include "rclcpp/any_service_callback.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/type_support_decl.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" namespace rclcpp { @@ -46,9 +46,6 @@ class ServiceBase std::shared_ptr node_handle, const std::string service_name); - RCLCPP_PUBLIC - virtual ~ServiceBase(); - RCLCPP_PUBLIC std::string get_service_name(); @@ -112,6 +109,16 @@ class Service : public ServiceBase Service() = delete; + ~Service() + { + 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(); + } + } + std::shared_ptr create_request() { return std::shared_ptr(new typename ServiceT::Request()); diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index 768a80fa10..393c43f116 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -27,14 +27,6 @@ ClientBase::ClientBase( : node_handle_(node_handle), service_name_(service_name) {} -ClientBase::~ClientBase() -{ - if (rcl_client_fini(&client_handle_, node_handle_.get()) != RMW_RET_OK) { - fprintf(stderr, - "Error in destruction of rmw client handle: %s\n", rmw_get_error_string_safe()); - } -} - const std::string & ClientBase::get_service_name() const { diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 6f1e8d48ca..6a840f125a 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -287,11 +287,11 @@ Executor::execute_service( if (status != RCL_RET_SERVICE_TAKE_FAILED) { if (status == RMW_RET_OK) { service->handle_request(request_header, request); - } else { - fprintf(stderr, - "[rclcpp::error] take request failed for server of service '%s': %s\n", - service->get_service_name().c_str(), rcl_get_error_string_safe()); } + } else { + fprintf(stderr, + "[rclcpp::error] take request failed for server of service '%s': %s\n", + service->get_service_name().c_str(), rcl_get_error_string_safe()); } } @@ -308,11 +308,11 @@ Executor::execute_client( if (status != RCL_RET_SERVICE_TAKE_FAILED) { if (status == RMW_RET_OK) { client->handle_response(request_header, response); - } else { - fprintf(stderr, - "[rclcpp::error] take response failed for client of service '%s': %s\n", - client->get_service_name().c_str(), rcl_get_error_string_safe()); } + } else { + fprintf(stderr, + "[rclcpp::error] take response failed for client of service '%s': %s\n", + client->get_service_name().c_str(), rcl_get_error_string_safe()); } } diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index 12587afc23..12c99b2e6d 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -33,16 +33,6 @@ ServiceBase::ServiceBase( : node_handle_(node_handle), service_name_(service_name) {} -ServiceBase::~ServiceBase() -{ - 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(); - } -} - std::string ServiceBase::get_service_name() { From 5fbd54ba4b729b44409dd85675743a3393b43617 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Fri, 15 Apr 2016 17:20:22 -0700 Subject: [PATCH 43/49] destructor inheritance --- rclcpp/cmake/get_rclcpp_information.cmake | 1 + rclcpp/include/rclcpp/client.hpp | 8 ++++++-- rclcpp/include/rclcpp/node_impl.hpp | 1 - rclcpp/include/rclcpp/publisher.hpp | 5 ++++- rclcpp/include/rclcpp/service.hpp | 5 ++++- rclcpp/src/rclcpp/client.cpp | 4 ++++ rclcpp/src/rclcpp/publisher.cpp | 4 ++++ rclcpp/src/rclcpp/service.cpp | 3 +++ 8 files changed, 26 insertions(+), 5 deletions(-) diff --git a/rclcpp/cmake/get_rclcpp_information.cmake b/rclcpp/cmake/get_rclcpp_information.cmake index 7cb78ce50a..fff5371ebf 100644 --- a/rclcpp/cmake/get_rclcpp_information.cmake +++ b/rclcpp/cmake/get_rclcpp_information.cmake @@ -67,6 +67,7 @@ macro(get_rclcpp_information rmw_implementation var_prefix) # dependencies set(_exported_dependencies + "rcl_interfaces" "rcl${target_suffix}" "rosidl_generator_cpp") set(${var_prefix}_DEFINITIONS) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index a1dca22ddb..7512ac8672 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -28,9 +28,10 @@ #include "rclcpp/function_traits.hpp" #include "rclcpp/macros.hpp" -#include "rclcpp/utilities.hpp" #include "rclcpp/type_support_decl.hpp" +#include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" + #include "rmw/error_handling.h" #include "rmw/rmw.h" @@ -49,6 +50,9 @@ class ClientBase std::shared_ptr node_handle, const std::string & service_name); + RCLCPP_PUBLIC + ~ClientBase(); + RCLCPP_PUBLIC const std::string & get_service_name() const; @@ -112,7 +116,7 @@ class Client : public ClientBase } } - ~Client() + virtual ~Client() { if (rcl_client_fini(&client_handle_, node_handle_.get()) != RMW_RET_OK) { fprintf(stderr, diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index eb826dcd53..21b8092405 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -297,7 +297,6 @@ Node::create_client( { rcl_client_options_t options = rcl_client_get_default_options(); options.qos = qos_profile; - // options.allocator = rclcpp::allocator::get_rcl_allocator(*allocator.get()); using rclcpp::client::Client; using rclcpp::client::ClientBase; diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index b67157eb05..05194d7ab0 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -66,6 +66,9 @@ class PublisherBase std::string topic, size_t queue_size); + RCLCPP_PUBLIC + ~PublisherBase(); + /// Get the topic that this publisher publishes on. // \return The topic name. RCLCPP_PUBLIC @@ -184,7 +187,7 @@ class Publisher : public PublisherBase } } - ~Publisher() + virtual ~Publisher() { if (rcl_publisher_fini(&intra_process_publisher_handle_, node_handle_.get()) != RCL_RET_OK) { fprintf( diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 2c19c58588..19ae8f3e6e 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -46,6 +46,9 @@ class ServiceBase std::shared_ptr node_handle, const std::string service_name); + RCLCPP_PUBLIC + ~ServiceBase(); + RCLCPP_PUBLIC std::string get_service_name(); @@ -109,7 +112,7 @@ class Service : public ServiceBase Service() = delete; - ~Service() + virtual ~Service() { if (rcl_service_fini(&service_handle_, node_handle_.get()) != RCL_RET_OK) { std::stringstream ss; diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index 393c43f116..0fd26a80fb 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -27,6 +27,10 @@ ClientBase::ClientBase( : node_handle_(node_handle), service_name_(service_name) {} +ClientBase::~ClientBase() +{ +} + const std::string & ClientBase::get_service_name() const { diff --git a/rclcpp/src/rclcpp/publisher.cpp b/rclcpp/src/rclcpp/publisher.cpp index 93c06c32ba..d2d5728383 100644 --- a/rclcpp/src/rclcpp/publisher.cpp +++ b/rclcpp/src/rclcpp/publisher.cpp @@ -42,6 +42,10 @@ PublisherBase::PublisherBase( { } +PublisherBase::~PublisherBase() +{ +} + const std::string & PublisherBase::get_topic_name() const { diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index 12c99b2e6d..24d0ad68ab 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -33,6 +33,9 @@ ServiceBase::ServiceBase( : node_handle_(node_handle), service_name_(service_name) {} +ServiceBase::~ServiceBase() +{} + std::string ServiceBase::get_service_name() { From ac1f22ddf8422b7f42a3603dd55b9fd300547201 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Tue, 19 Apr 2016 00:10:43 -0700 Subject: [PATCH 44/49] use allocator interface in waitset --- rclcpp/include/rclcpp/memory_strategy.hpp | 4 ++++ .../rclcpp/strategies/allocator_memory_strategy.hpp | 7 +++++++ rclcpp/src/rclcpp/executor.cpp | 3 ++- 3 files changed, 13 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/memory_strategy.hpp b/rclcpp/include/rclcpp/memory_strategy.hpp index dbd8d5a684..30f91cfd1d 100644 --- a/rclcpp/include/rclcpp/memory_strategy.hpp +++ b/rclcpp/include/rclcpp/memory_strategy.hpp @@ -18,6 +18,7 @@ #include #include +#include "rcl/allocator.h" #include "rcl/wait.h" #include "rclcpp/any_executable.hpp" @@ -74,6 +75,9 @@ class RCLCPP_PUBLIC MemoryStrategy get_next_client(rclcpp::executor::AnyExecutable::SharedPtr any_exec, const WeakNodeVector & weak_nodes) = 0; + virtual rcl_allocator_t + get_allocator() = 0; + static rclcpp::subscription::SubscriptionBase::SharedPtr get_subscription_by_handle(const rcl_subscription_t * subscriber_handle, const WeakNodeVector & weak_nodes); diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 5a875812fd..24a4d39c43 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -18,6 +18,8 @@ #include #include +#include "rcl/allocator.h" + #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/memory_strategy.hpp" #include "rclcpp/node.hpp" @@ -339,6 +341,11 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } } + virtual rcl_allocator_t get_allocator() + { + return rclcpp::allocator::get_rcl_allocator(*allocator_); + } + size_t number_of_ready_subscriptions() const { return subscription_handles_.size(); diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 6a840f125a..5a1b7c6871 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -50,9 +50,10 @@ Executor::Executor(const ExecutorArgs & args) // Put the executor's guard condition in memory_strategy_->add_guard_condition(&interrupt_guard_condition_); + rcl_allocator_t allocator = memory_strategy_->get_allocator(); if (rcl_wait_set_init( - &waitset_, 0, 2, 0, 0, 0, rcl_get_default_allocator()) != RCL_RET_OK) + &waitset_, 0, 2, 0, 0, 0, allocator) != RCL_RET_OK) { fprintf(stderr, "[rclcpp::error] failed to create waitset: %s\n", rcl_get_error_string_safe()); From 1c715fefa417802c746e8dca5a50ebf3131f5581 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Tue, 19 Apr 2016 00:32:01 -0700 Subject: [PATCH 45/49] Fix reallocate --- .../rclcpp/allocator/allocator_common.hpp | 17 ++++++++++++++--- .../strategies/allocator_memory_strategy.hpp | 2 +- 2 files changed, 15 insertions(+), 4 deletions(-) diff --git a/rclcpp/include/rclcpp/allocator/allocator_common.hpp b/rclcpp/include/rclcpp/allocator/allocator_common.hpp index c1b1bac485..07878e4b2e 100644 --- a/rclcpp/include/rclcpp/allocator/allocator_common.hpp +++ b/rclcpp/include/rclcpp/allocator/allocator_common.hpp @@ -38,6 +38,7 @@ void * retyped_allocate(size_t size, void * untyped_allocator) } return std::allocator_traits::allocate(*typed_allocator, size); } + template void retyped_deallocate(void * untyped_pointer, void * untyped_allocator) { @@ -46,12 +47,22 @@ void retyped_deallocate(void * untyped_pointer, void * untyped_allocator) throw std::runtime_error("Received incorrect allocator type"); } auto typed_ptr = static_cast(untyped_pointer); - if (!typed_ptr) { - fprintf(stderr, "received null pointer in deallocate\n"); + std::allocator_traits::deallocate(*typed_allocator, typed_ptr, 1); +} + +template +void * retyped_reallocate(void * untyped_pointer, size_t size, void * untyped_allocator) +{ + auto typed_allocator = static_cast(untyped_allocator); + if (!typed_allocator) { + throw std::runtime_error("Received incorrect allocator type"); } + auto typed_ptr = static_cast(untyped_pointer); std::allocator_traits::deallocate(*typed_allocator, typed_ptr, 1); + return std::allocator_traits::allocate(*typed_allocator, size); } + // Convert a std::allocator_traits-formatted Allocator into an rcl allocator template>::value>::type * = nullptr> @@ -61,7 +72,7 @@ rcl_allocator_t get_rcl_allocator(Alloc & allocator) rcl_allocator.allocate = &retyped_allocate; rcl_allocator.deallocate = &retyped_deallocate; - rcl_allocator.reallocate = nullptr; + rcl_allocator.reallocate = &retyped_reallocate; rcl_allocator.state = &allocator; return rcl_allocator; } diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 24a4d39c43..10510b1773 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -343,7 +343,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy virtual rcl_allocator_t get_allocator() { - return rclcpp::allocator::get_rcl_allocator(*allocator_); + return rclcpp::allocator::get_rcl_allocator(*allocator_.get()); } size_t number_of_ready_subscriptions() const From 7b9e270ea6c0923f4f5f4074fd2d915e636d5e92 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Tue, 19 Apr 2016 17:41:06 -0700 Subject: [PATCH 46/49] Correct retcode enum from rmw to rcl --- rclcpp/include/rclcpp/client.hpp | 6 +++--- rclcpp/include/rclcpp/node_impl.hpp | 10 +++++----- rclcpp/src/rclcpp/executor.cpp | 8 ++++---- rclcpp/src/rclcpp/node.cpp | 2 +- rclcpp/src/rclcpp/subscription.cpp | 2 +- rclcpp/src/rclcpp/utilities.cpp | 2 +- 6 files changed, 15 insertions(+), 15 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 7512ac8672..02372a9da7 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -23,8 +23,8 @@ #include #include -#include "rcl/error_handling.h" #include "rcl/client.h" +#include "rcl/error_handling.h" #include "rclcpp/function_traits.hpp" #include "rclcpp/macros.hpp" @@ -118,7 +118,7 @@ class Client : public ClientBase virtual ~Client() { - if (rcl_client_fini(&client_handle_, node_handle_.get()) != RMW_RET_OK) { + if (rcl_client_fini(&client_handle_, node_handle_.get()) != RCL_RET_OK) { fprintf(stderr, "Error in destruction of rmw client handle: %s\n", rmw_get_error_string_safe()); } @@ -174,7 +174,7 @@ class Client : public ClientBase { std::lock_guard lock(pending_requests_mutex_); int64_t sequence_number; - if (RMW_RET_OK != rcl_send_request(get_client_handle(), request.get(), &sequence_number)) { + if (RCL_RET_OK != rcl_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: ") + rcl_get_error_string_safe()); diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 21b8092405..f13d2417b6 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -124,7 +124,7 @@ Node::create_publisher( shared_publish_callback, publisher_options); } - if (rcl_trigger_guard_condition(¬ify_guard_condition_) != RMW_RET_OK) { + if (rcl_trigger_guard_condition(¬ify_guard_condition_) != RCL_RET_OK) { throw std::runtime_error( std::string( "Failed to notify waitset on publisher creation: ") + rmw_get_error_string()); @@ -229,7 +229,7 @@ Node::create_subscription( default_callback_group_->add_subscription(sub_base_ptr); } number_of_subscriptions_++; - if (rcl_trigger_guard_condition(¬ify_guard_condition_) != RMW_RET_OK) { + if (rcl_trigger_guard_condition(¬ify_guard_condition_) != RCL_RET_OK) { throw std::runtime_error( std::string( "Failed to notify waitset on subscription creation: ") + rmw_get_error_string()); @@ -280,7 +280,7 @@ Node::create_wall_timer( default_callback_group_->add_timer(timer); } number_of_timers_++; - if (rcl_trigger_guard_condition(¬ify_guard_condition_) != RMW_RET_OK) { + if (rcl_trigger_guard_condition(¬ify_guard_condition_) != RCL_RET_OK) { throw std::runtime_error( std::string( "Failed to notify waitset on timer creation: ") + rmw_get_error_string()); @@ -318,7 +318,7 @@ Node::create_client( } number_of_clients_++; - if (rcl_trigger_guard_condition(¬ify_guard_condition_) != RMW_RET_OK) { + if (rcl_trigger_guard_condition(¬ify_guard_condition_) != RCL_RET_OK) { throw std::runtime_error( std::string( "Failed to notify waitset on client creation: ") + rmw_get_error_string()); @@ -354,7 +354,7 @@ Node::create_service( default_callback_group_->add_service(serv_base_ptr); } number_of_services_++; - if (rcl_trigger_guard_condition(¬ify_guard_condition_) != RMW_RET_OK) { + if (rcl_trigger_guard_condition(¬ify_guard_condition_) != RCL_RET_OK) { throw std::runtime_error( std::string( "Failed to notify waitset on service creation: ") + rmw_get_error_string()); diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 5a1b7c6871..b94b5e4f41 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -68,7 +68,7 @@ Executor::Executor(const ExecutorArgs & args) Executor::~Executor() { // Finalize the waitset. - if (rcl_wait_set_fini(&waitset_) != RMW_RET_OK) { + if (rcl_wait_set_fini(&waitset_) != RCL_RET_OK) { fprintf(stderr, "[rclcpp::error] failed to destroy waitset: %s\n", rcl_get_error_string_safe()); } @@ -97,7 +97,7 @@ Executor::add_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify) weak_nodes_.push_back(node_ptr); if (notify) { // Interrupt waiting to handle new node - if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RMW_RET_OK) { + if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { throw std::runtime_error(rcl_get_error_string_safe()); } } @@ -286,7 +286,7 @@ Executor::execute_service( request_header.get(), request.get()); if (status != RCL_RET_SERVICE_TAKE_FAILED) { - if (status == RMW_RET_OK) { + if (status == RCL_RET_OK) { service->handle_request(request_header, request); } } else { @@ -307,7 +307,7 @@ Executor::execute_client( request_header.get(), response.get()); if (status != RCL_RET_SERVICE_TAKE_FAILED) { - if (status == RMW_RET_OK) { + if (status == RCL_RET_OK) { client->handle_response(request_header, response); } } else { diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 49b827faa1..ce4c7acb2e 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -75,7 +75,7 @@ Node::Node( rcl_node_t * rcl_node = new rcl_node_t(rcl_get_zero_initialized_node()); node_handle_.reset(rcl_node, [](rcl_node_t * node) { - if (rcl_node_fini(node) != RMW_RET_OK) { + if (rcl_node_fini(node) != RCL_RET_OK) { fprintf( stderr, "Error in destruction of rmw node handle: %s\n", rcl_get_error_string_safe()); } diff --git a/rclcpp/src/rclcpp/subscription.cpp b/rclcpp/src/rclcpp/subscription.cpp index 34cd52563c..0bd7cc9cfa 100644 --- a/rclcpp/src/rclcpp/subscription.cpp +++ b/rclcpp/src/rclcpp/subscription.cpp @@ -37,7 +37,7 @@ SubscriptionBase::SubscriptionBase( SubscriptionBase::~SubscriptionBase() { - if (rcl_subscription_fini(&subscription_handle_, node_handle_.get()) != RMW_RET_OK) { + 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'; diff --git a/rclcpp/src/rclcpp/utilities.cpp b/rclcpp/src/rclcpp/utilities.cpp index 3c1d487a54..b3c7c2b544 100644 --- a/rclcpp/src/rclcpp/utilities.cpp +++ b/rclcpp/src/rclcpp/utilities.cpp @@ -156,7 +156,7 @@ void rclcpp::utilities::shutdown() { g_signal_status = SIGINT; - if (rcl_trigger_guard_condition(&g_sigint_guard_cond_handle) != RMW_RET_OK) { + if (rcl_trigger_guard_condition(&g_sigint_guard_cond_handle) != RCL_RET_OK) { fprintf(stderr, "[rclcpp::error] failed to trigger guard condition: %s\n", rmw_get_error_string_safe()); } From 6739c793a28c752b135a0fab2a791b088f6cdfbc Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Tue, 19 Apr 2016 18:11:54 -0700 Subject: [PATCH 47/49] initialize publisher (necessary?) --- rclcpp/include/rclcpp/publisher.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 05194d7ab0..bc506350a7 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -127,7 +127,7 @@ class PublisherBase rcl_publisher_t publisher_handle_ = rcl_get_zero_initialized_publisher(); rcl_publisher_t intra_process_publisher_handle_ = rcl_get_zero_initialized_publisher(); - rcl_allocator_t rcl_allocator_; + rcl_allocator_t rcl_allocator_ = rcl_get_default_allocator(); std::string topic_; size_t queue_size_; From c1fc16e69f71b961c289856bda57e25babad9123 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Wed, 20 Apr 2016 17:37:19 -0700 Subject: [PATCH 48/49] Tweaks to fix timer on Windows --- .../include/rclcpp/allocator/allocator_common.hpp | 3 ++- rclcpp/include/rclcpp/executor.hpp | 12 ++++++------ rclcpp/include/rclcpp/timer.hpp | 14 +++++--------- rclcpp/src/rclcpp/executor.cpp | 3 +-- rclcpp/src/rclcpp/timer.cpp | 7 ++++++- 5 files changed, 20 insertions(+), 19 deletions(-) diff --git a/rclcpp/include/rclcpp/allocator/allocator_common.hpp b/rclcpp/include/rclcpp/allocator/allocator_common.hpp index 07878e4b2e..01333e781e 100644 --- a/rclcpp/include/rclcpp/allocator/allocator_common.hpp +++ b/rclcpp/include/rclcpp/allocator/allocator_common.hpp @@ -69,11 +69,12 @@ typename std::enable_if>::value>::type rcl_allocator_t get_rcl_allocator(Alloc & allocator) { rcl_allocator_t rcl_allocator = rcl_get_default_allocator(); - +#ifndef _WIN32 rcl_allocator.allocate = &retyped_allocate; rcl_allocator.deallocate = &retyped_deallocate; rcl_allocator.reallocate = &retyped_reallocate; rcl_allocator.state = &allocator; +#endif return rcl_allocator; } diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index fe9f92058f..8f16eb519a 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -196,10 +196,11 @@ class Executor } auto end_time = std::chrono::steady_clock::now(); - if (timeout > std::chrono::nanoseconds::zero()) { - end_time += timeout; + std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast(timeout); + if (timeout_ns > std::chrono::nanoseconds::zero()) { + end_time += timeout_ns; } - auto timeout_left = timeout; + std::chrono::nanoseconds timeout_left = timeout_ns; while (rclcpp::utilities::ok()) { // Do one item of work. @@ -210,7 +211,7 @@ class Executor return FutureReturnCode::SUCCESS; } // If the original timeout is < 0, then this is blocking, never TIMEOUT. - if (timeout < std::chrono::nanoseconds::zero()) { + if (timeout_ns < std::chrono::nanoseconds::zero()) { continue; } // Otherwise check if we still have time to wait, return TIMEOUT if not. @@ -219,8 +220,7 @@ class Executor return FutureReturnCode::TIMEOUT; } // Subtract the elapsed time from the original timeout. - using duration_type = std::chrono::duration; - timeout_left = std::chrono::duration_cast(end_time - now); + timeout_left = std::chrono::duration_cast(end_time - now); } // The future did not complete before ok() returned false, return INTERRUPTED. diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index 7109bdae98..4ebef0325f 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -46,9 +46,9 @@ class TimerBase RCLCPP_PUBLIC explicit TimerBase(std::chrono::nanoseconds period); - + RCLCPP_PUBLIC - virtual ~TimerBase(); + ~TimerBase(); RCLCPP_PUBLIC void @@ -64,6 +64,7 @@ class TimerBase /// Check how long the timer has until its next scheduled callback. // \return A std::chrono::duration representing the relative time until the next callback. + RCLCPP_PUBLIC std::chrono::nanoseconds time_until_trigger(); @@ -77,6 +78,7 @@ class TimerBase * since it maintains the last time the callback was triggered. * \return True if the timer needs to trigger. */ + RCLCPP_PUBLIC bool is_ready(); protected: @@ -110,12 +112,6 @@ class GenericTimer : public TimerBase GenericTimer(std::chrono::nanoseconds period, FunctorT && callback) : TimerBase(period), callback_(std::forward(callback)) { - if (rcl_timer_init( - &timer_handle_, 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()); - } } /// Default destructor. @@ -124,7 +120,7 @@ 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"); + fprintf(stderr, "Failed to clean up rcl timer handle: %s\n", rcl_get_error_string_safe()); } } diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index b94b5e4f41..e82d2dbde0 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -382,8 +382,7 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) if (!memory_strategy_->add_handles_to_waitset(&waitset_)) { throw std::runtime_error("Couldn't fill waitset"); } - - rcl_ret_t status = rcl_wait(&waitset_, timeout.count()); + rcl_ret_t status = rcl_wait(&waitset_, std::chrono::duration_cast(timeout).count()); if (status == RCL_RET_WAIT_SET_EMPTY) { fprintf(stderr, "Warning: empty waitset received in rcl_wait(). This should never happen.\n"); } else if (status != RCL_RET_OK && status != RCL_RET_TIMEOUT) { diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index ec2501f94e..1128bbfee5 100644 --- a/rclcpp/src/rclcpp/timer.cpp +++ b/rclcpp/src/rclcpp/timer.cpp @@ -21,7 +21,12 @@ using rclcpp::timer::TimerBase; TimerBase::TimerBase(std::chrono::nanoseconds period) { - (void)period; + if (rcl_timer_init( + &timer_handle_, 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()); + } } TimerBase::~TimerBase() From a67fe8012bc7e705f72e68aaf00d52520042793d Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Wed, 20 Apr 2016 17:38:20 -0700 Subject: [PATCH 49/49] Lint --- rclcpp/include/rclcpp/executor.hpp | 3 ++- rclcpp/include/rclcpp/timer.hpp | 2 +- rclcpp/src/rclcpp/executor.cpp | 3 ++- 3 files changed, 5 insertions(+), 3 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 8f16eb519a..14cb8e3201 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -196,7 +196,8 @@ class Executor } auto end_time = std::chrono::steady_clock::now(); - std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast(timeout); + std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast( + timeout); if (timeout_ns > std::chrono::nanoseconds::zero()) { end_time += timeout_ns; } diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index 4ebef0325f..afa683d1b9 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -46,7 +46,7 @@ class TimerBase RCLCPP_PUBLIC explicit TimerBase(std::chrono::nanoseconds period); - + RCLCPP_PUBLIC ~TimerBase(); diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index e82d2dbde0..ad05e50305 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -382,7 +382,8 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) if (!memory_strategy_->add_handles_to_waitset(&waitset_)) { throw std::runtime_error("Couldn't fill waitset"); } - rcl_ret_t status = rcl_wait(&waitset_, std::chrono::duration_cast(timeout).count()); + rcl_ret_t status = + rcl_wait(&waitset_, std::chrono::duration_cast(timeout).count()); if (status == RCL_RET_WAIT_SET_EMPTY) { fprintf(stderr, "Warning: empty waitset received in rcl_wait(). This should never happen.\n"); } else if (status != RCL_RET_OK && status != RCL_RET_TIMEOUT) {