Merge branch 'master' into prajaktg/topic_stats_options

ros2 · Apr 14, 2020 · 22bd55f · 22bd55f
2 parents a2e4486 + aaf8b38
commit 22bd55f
Show file tree

Hide file tree

Showing 45 changed files with 4,547 additions and 172 deletions.
diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt
@@ -11,7 +11,7 @@ find_package(rcpputils REQUIRED)
 find_package(rcutils REQUIRED)
 find_package(rmw REQUIRED)
 find_package(rosgraph_msgs REQUIRED)
-find_package(rosidl_generator_cpp REQUIRED)
+find_package(rosidl_runtime_cpp REQUIRED)
 find_package(rosidl_typesupport_c REQUIRED)
 find_package(rosidl_typesupport_cpp REQUIRED)
 find_package(tracetools REQUIRED)
@@ -49,11 +49,13 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/executors/static_executor_entities_collector.cpp
   src/rclcpp/executors/static_single_threaded_executor.cpp
   src/rclcpp/graph_listener.cpp
+  src/rclcpp/guard_condition.cpp
   src/rclcpp/init_options.cpp
   src/rclcpp/intra_process_manager.cpp
   src/rclcpp/logger.cpp
   src/rclcpp/memory_strategies.cpp
   src/rclcpp/memory_strategy.cpp
+  src/rclcpp/message_info.cpp
   src/rclcpp/node.cpp
   src/rclcpp/node_options.cpp
   src/rclcpp/node_interfaces/node_base.cpp
@@ -84,6 +86,7 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/timer.cpp
   src/rclcpp/type_support.cpp
   src/rclcpp/utilities.cpp
+  src/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.cpp
   src/rclcpp/waitable.cpp
 )
 
@@ -120,7 +123,7 @@ ament_target_dependencies(${PROJECT_NAME}
   "builtin_interfaces"
   "rosgraph_msgs"
   "rosidl_typesupport_cpp"
-  "rosidl_generator_cpp"
+  "rosidl_runtime_cpp"
   "tracetools"
 )
 
@@ -147,7 +150,7 @@ ament_export_dependencies(builtin_interfaces)
 ament_export_dependencies(rosgraph_msgs)
 ament_export_dependencies(rosidl_typesupport_cpp)
 ament_export_dependencies(rosidl_typesupport_c)
-ament_export_dependencies(rosidl_generator_cpp)
+ament_export_dependencies(rosidl_runtime_cpp)
 ament_export_dependencies(rcl_yaml_param_parser)
 ament_export_dependencies(tracetools)
 
@@ -169,7 +172,7 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_client
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_client ${PROJECT_NAME})
@@ -180,7 +183,7 @@ if(BUILD_TESTING)
       "rcl_interfaces"
       "rmw"
       "rcl"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_create_timer ${PROJECT_NAME})
@@ -191,7 +194,7 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_expand_topic_or_service_name
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_expand_topic_or_service_name ${PROJECT_NAME})
@@ -201,7 +204,7 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_function_traits
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
     )
   endif()
@@ -211,7 +214,7 @@ if(BUILD_TESTING)
       "rcl"
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_intra_process_manager ${PROJECT_NAME})
@@ -221,7 +224,7 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_ring_buffer_implementation
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_ring_buffer_implementation ${PROJECT_NAME})
@@ -231,7 +234,7 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_intra_process_buffer
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_intra_process_buffer ${PROJECT_NAME})
@@ -249,7 +252,7 @@ if(BUILD_TESTING)
       "rcl_interfaces"
       "rcpputils"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
       "test_msgs"
     )
@@ -288,7 +291,7 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_node_global_args
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_node_global_args ${PROJECT_NAME})
@@ -310,7 +313,7 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_parameter_events_filter
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_parameter_events_filter ${PROJECT_NAME})
@@ -320,7 +323,7 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_parameter
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_parameter ${PROJECT_NAME})
@@ -333,7 +336,7 @@ if(BUILD_TESTING)
   if(TARGET test_publisher)
     ament_target_dependencies(test_publisher
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
       "test_msgs"
     )
@@ -344,7 +347,7 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_publisher_subscription_count_api
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
       "test_msgs"
     )
@@ -374,7 +377,7 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_rate
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_rate
@@ -395,7 +398,7 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_service
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_service ${PROJECT_NAME})
@@ -405,7 +408,7 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_subscription
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
       "test_msgs"
     )
@@ -416,7 +419,7 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_subscription_publisher_count_api
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
       "test_msgs"
     )
@@ -523,6 +526,18 @@ if(BUILD_TESTING)
   if(TARGET test_subscription_options)
     ament_target_dependencies(test_subscription_options "rcl")
     target_link_libraries(test_subscription_options ${PROJECT_NAME})
+
+  ament_add_gtest(test_guard_condition test/test_guard_condition.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_guard_condition)
+    target_link_libraries(test_guard_condition ${PROJECT_NAME})
+  endif()
+
+  ament_add_gtest(test_wait_set test/test_wait_set.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_wait_set)
+    ament_target_dependencies(test_wait_set "test_msgs")
+    target_link_libraries(test_wait_set ${PROJECT_NAME})
   endif()
 
   # Install test resources

diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp
@@ -25,6 +25,7 @@
 
 #include "rclcpp/allocator/allocator_common.hpp"
 #include "rclcpp/function_traits.hpp"
+#include "rclcpp/message_info.hpp"
 #include "rclcpp/visibility_control.hpp"
 #include "tracetools/tracetools.h"
 #include "tracetools/utils.hpp"
@@ -43,13 +44,13 @@ class AnySubscriptionCallback
 
   using SharedPtrCallback = std::function<void (const std::shared_ptr<MessageT>)>;
   using SharedPtrWithInfoCallback =
-    std::function<void (const std::shared_ptr<MessageT>, const rmw_message_info_t &)>;
+    std::function<void (const std::shared_ptr<MessageT>, const rclcpp::MessageInfo &)>;
   using ConstSharedPtrCallback = std::function<void (const std::shared_ptr<const MessageT>)>;
   using ConstSharedPtrWithInfoCallback =
-    std::function<void (const std::shared_ptr<const MessageT>, const rmw_message_info_t &)>;
+    std::function<void (const std::shared_ptr<const MessageT>, const rclcpp::MessageInfo &)>;
   using UniquePtrCallback = std::function<void (MessageUniquePtr)>;
   using UniquePtrWithInfoCallback =
-    std::function<void (MessageUniquePtr, const rmw_message_info_t &)>;
+    std::function<void (MessageUniquePtr, const rclcpp::MessageInfo &)>;
 
   SharedPtrCallback shared_ptr_callback_;
   SharedPtrWithInfoCallback shared_ptr_with_info_callback_;
@@ -155,7 +156,7 @@ class AnySubscriptionCallback
   }
 
   void dispatch(
-    std::shared_ptr<MessageT> message, const rmw_message_info_t & message_info)
+    std::shared_ptr<MessageT> message, const rclcpp::MessageInfo & message_info)
   {
     TRACEPOINT(callback_start, (const void *)this, false);
     if (shared_ptr_callback_) {
@@ -181,7 +182,7 @@ class AnySubscriptionCallback
   }
 
   void dispatch_intra_process(
-    ConstMessageSharedPtr message, const rmw_message_info_t & message_info)
+    ConstMessageSharedPtr message, const rclcpp::MessageInfo & message_info)
   {
     TRACEPOINT(callback_start, (const void *)this, true);
     if (const_shared_ptr_callback_) {
@@ -204,7 +205,7 @@ class AnySubscriptionCallback
   }
 
   void dispatch_intra_process(
-    MessageUniquePtr message, const rmw_message_info_t & message_info)
+    MessageUniquePtr message, const rclcpp::MessageInfo & message_info)
   {
     TRACEPOINT(callback_start, (const void *)this, true);
     if (shared_ptr_callback_) {

diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp
@@ -15,6 +15,7 @@
 #ifndef RCLCPP__CLIENT_HPP_
 #define RCLCPP__CLIENT_HPP_
 
+#include <atomic>
 #include <future>
 #include <map>
 #include <memory>
@@ -62,6 +63,27 @@ class ClientBase
   RCLCPP_PUBLIC
   virtual ~ClientBase();
 
+  /// Take the next response for this client as a type erased pointer.
+  /**
+   * The type erased pointer allows for this method to be used in a type
+   * agnostic way along with ClientBase::create_response(),
+   * ClientBase::create_request_header(), and ClientBase::handle_response().
+   * The typed version of this can be used if the Service type is known,
+   * \sa Client::take_response().
+   *
+   * \param[out] response_out The type erased pointer to a Service Response into
+   *   which the middleware will copy the response being taken.
+   * \param[out] request_header_out The request header to be filled by the
+   *   middleware when taking, and which can be used to associte the response
+   *   to a specific request.
+   * \returns true if the response was taken, otherwise false.
+   * \throws rclcpp::exceptions::RCLError based exceptions if the underlying
+   *   rcl function fail.
+   */
+  RCLCPP_PUBLIC
+  bool
+  take_type_erased_response(void * response_out, rmw_request_id_t & request_header_out);
+
   RCLCPP_PUBLIC
   const char *
   get_service_name() const;
@@ -93,6 +115,20 @@ class ClientBase
   virtual void handle_response(
     std::shared_ptr<rmw_request_id_t> request_header, std::shared_ptr<void> response) = 0;
 
+  /// Exchange the "in use by wait set" state for this client.
+  /**
+   * This is used to ensure this client is not used by multiple
+   * wait sets at the same time.
+   *
+   * \param[in] in_use_state the new state to exchange into the state, true
+   *   indicates it is now in use by a wait set, and false is that it is no
+   *   longer in use by a wait set.
+   * \returns the previous state.
+   */
+  RCLCPP_PUBLIC
+  bool
+  exchange_in_use_by_wait_set_state(bool in_use_state);
+
 protected:
   RCLCPP_DISABLE_COPY(ClientBase)
 
@@ -113,6 +149,8 @@ class ClientBase
   std::shared_ptr<rclcpp::Context> context_;
 
   std::shared_ptr<rcl_client_t> client_handle_;
+
+  std::atomic<bool> in_use_by_wait_set_{false};
 };
 
 template<typename ServiceT>
@@ -171,6 +209,25 @@ class Client : public ClientBase
   {
   }
 
+  /// Take the next response for this client.
+  /**
+   * \sa ClientBase::take_type_erased_response().
+   *
+   * \param[out] response_out The reference to a Service Response into
+   *   which the middleware will copy the response being taken.
+   * \param[out] request_header_out The request header to be filled by the
+   *   middleware when taking, and which can be used to associte the response
+   *   to a specific request.
+   * \returns true if the response was taken, otherwise false.
+   * \throws rclcpp::exceptions::RCLError based exceptions if the underlying
+   *   rcl function fail.
+   */
+  bool
+  take_response(typename ServiceT::Response & response_out, rmw_request_id_t & request_header_out)
+  {
+    return this->take_type_erased_response(&response_out, request_header_out);
+  }
+
   std::shared_ptr<void>
   create_response() override
   {

diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp
@@ -159,7 +159,7 @@ class Context : public std::enable_shared_from_this<Context>
    *
    * \param[in] reason the description of why shutdown happened
    * \return true if shutdown was successful, false if context was already shutdown
-   * \throw various exceptions derived from RCLErrorBase, if rcl_shutdown fails
+   * \throw various exceptions derived from rclcpp::exceptions::RCLError, if rcl_shutdown fails
    */
   RCLCPP_PUBLIC
   virtual

diff --git a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp b/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp
@@ -15,6 +15,7 @@
 #ifndef RCLCPP__EXECUTORS__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_
 #define RCLCPP__EXECUTORS__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_
 
+#include <chrono>
 #include <list>
 #include <memory>