Add generic publisher and generic subscription for serialized messages (

ros2#1452) * Copying files from rosbag2 The generic_* files are from rosbag2_transport typesupport_helpers incl. test is from rosbag2_cpp memory_management.hpp is from rosbag2_test_common test_pubsub.cpp was renamed from test_rosbag2_node.cpp from rosbag2_transport Signed-off-by: Nikolai Morin <nikolai.morin@apex.ai> * Rebrand into rclcpp_generic Add package.xml, CMakeLists.txt, Doxyfile, README.md and CHANGELOG.rst Rename namespaces Make GenericPublisher and GenericSubscription self-contained by storing shared library New create() methods that return shared pointers Add docstrings Include only what is needed Make linters & tests pass Signed-off-by: Nikolai Morin <nikolai.morin@apex.ai> * Review feedback * Delete CHANGELOG.rst * Enable cppcheck * Remove all references to rosbag2/ros2bag Signed-off-by: Nikolai Morin <nikolai.morin@apex.ai> * Move rclpp_generic into rclcpp Signed-off-by: Nikolai Morin <nikolai.morin@apex.ai> * Rename namespace rclcpp_generic to rclcpp::generic Signed-off-by: Nikolai Morin <nikolai.morin@apex.ai> * Free 'create' functions instead of static functions in class Signed-off-by: Nikolai Morin <nikolai.morin@apex.ai> * Remove 'generic' subdirectory and namespace hierarchy Signed-off-by: Nikolai Morin <nikolai.morin@apex.ai> * Order includes according to style guide Signed-off-by: Nikolai Morin <nikolai.morin@apex.ai> * Remove extra README.md Signed-off-by: Nikolai Morin <nikolai.morin@apex.ai> * Also add brief to class docs Signed-off-by: Nikolai Morin <nikolai.morin@apex.ai> * Make ament_index_cpp a build_depend Signed-off-by: Nikolai Morin <nikolai.morin@apex.ai> * Add to rclcpp.hpp Signed-off-by: Nikolai Morin <nikolai.morin@apex.ai> * Remove memory_management, use rclcpp::SerializedMessage in GenericPublisher::publish Signed-off-by: Nikolai Morin <nikolai.morin@apex.ai> * Clean up the typesupport_helpers Signed-off-by: Nikolai Morin <nikolai.morin@apex.ai> * Use make_shared, add UnimplementedError Signed-off-by: Nikolai Morin <nikolai.morin@apex.ai> * Add more comments, make member variable private, remove unnecessary include Signed-off-by: Nikolai Morin <nikolai.morin@apex.ai> * Apply suggestions from code review Co-authored-by: William Woodall <william+github@osrfoundation.org> Signed-off-by: Nikolai Morin <nikolai.morin@apex.ai> * Rename test Signed-off-by: Nikolai Morin <nikolai.morin@apex.ai> * Update copyright and remove ament_target_dependencies for test Signed-off-by: Nikolai Morin <nikolai.morin@apex.ai> * Accept PublisherOptions and SubscriptionOptions Signed-off-by: Nikolai Morin <nikolai.morin@apex.ai> * Remove target_include_directories Signed-off-by: Nikolai Morin <nikolai.morin@apex.ai> * Add explanatory comment to SubscriptionBase Signed-off-by: Nikolai Morin <nikolai.morin@apex.ai> * Use kSolibPrefix and kSolibExtension from rcpputils Signed-off-by: Nikolai Morin <nikolai.morin@apex.ai> * Fix downstream build failure by making ament_index_cpp a build_export_depend Signed-off-by: Nikolai Morin <nikolai.morin@apex.ai> * Use path_for_library(), fix documentation nitpicks Signed-off-by: Nikolai Morin <nikolai.morin@apex.ai> * Improve error handling in get_typesupport_handle Signed-off-by: Nikolai Morin <nikolai.morin@apex.ai> * Accept SubscriptionOptions in GenericSubscription Signed-off-by: Nikolai Morin <nikolai.morin@apex.ai> * Make use of PublisherOptions in GenericPublisher Signed-off-by: Nikolai Morin <nikolai.morin@apex.ai> * Document typesupport_helpers Signed-off-by: Nikolai Morin <nikolai.morin@apex.ai> * Improve documentation Signed-off-by: Nikolai Morin <nikolai.morin@apex.ai> * Use std::function instead of function pointer Co-authored-by: William Woodall <william+github@osrfoundation.org> Signed-off-by: Nikolai Morin <nikolai.morin@apex.ai> * Minimize vertical whitespace Signed-off-by: Nikolai Morin <nikolai.morin@apex.ai> * Add TODO for callback with message info Signed-off-by: Nikolai Morin <nikolai.morin@apex.ai> * Link issue in TODO Signed-off-by: Nikolai Morin <nikolai.morin@apex.ai> * Add missing include for functional Signed-off-by: nnmm <nnmmgit@gmail.com> * Fix compilation Signed-off-by: Jacob Perron <jacob@openrobotics.org> * Fix lint Signed-off-by: Jacob Perron <jacob@openrobotics.org> * Address review comments (#1) * fix redefinition of default template arguments Signed-off-by: Karsten Knese <Karsten1987@users.noreply.github.com> * address review comments Signed-off-by: Karsten Knese <Karsten1987@users.noreply.github.com> * rename test executable Signed-off-by: Karsten Knese <Karsten1987@users.noreply.github.com> * add functionality to lifecycle nodes Signed-off-by: Karsten Knese <Karsten1987@users.noreply.github.com> * Refactor typesupport helpers * Make extract_type_identifier function private * Remove unused extract_type_and_package function * Update unit tests Signed-off-by: Jacob Perron <jacob@openrobotics.org> * Remove note about ament from classes This comment only applies to the free functions. Signed-off-by: Jacob Perron <jacob@openrobotics.org> * Fix formatting Co-authored-by: Karsten Knese <Karsten1987@users.noreply.github.com> * Fix warning Possible loss of data from double to rcutils_duration_value_t Signed-off-by: Jacob Perron <jacob@openrobotics.org> * Add missing visibility macros Signed-off-by: Jacob Perron <jacob@openrobotics.org> Co-authored-by: William Woodall <william+github@osrfoundation.org> Co-authored-by: Jacob Perron <jacob@openrobotics.org> Co-authored-by: Karsten Knese <Karsten1987@users.noreply.github.com>
clearpathrobotics · Apr 3, 2021 · 95adde2 · 95adde2
1 parent cd3fd53
commit 95adde2
Show file tree

Hide file tree

Showing 19 changed files with 1,216 additions and 9 deletions.
diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt
@@ -5,6 +5,7 @@ project(rclcpp)
 find_package(Threads REQUIRED)
 
 find_package(ament_cmake_ros REQUIRED)
+find_package(ament_index_cpp REQUIRED)
 find_package(builtin_interfaces REQUIRED)
 find_package(libstatistics_collector REQUIRED)
 find_package(rcl REQUIRED)
@@ -51,12 +52,14 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/executable_list.cpp
   src/rclcpp/executor.cpp
   src/rclcpp/executors.cpp
-  src/rclcpp/expand_topic_or_service_name.cpp
   src/rclcpp/executors/multi_threaded_executor.cpp
   src/rclcpp/executors/single_threaded_executor.cpp
   src/rclcpp/executors/static_executor_entities_collector.cpp
   src/rclcpp/executors/static_single_threaded_executor.cpp
+  src/rclcpp/expand_topic_or_service_name.cpp
   src/rclcpp/future_return_code.cpp
+  src/rclcpp/generic_publisher.cpp
+  src/rclcpp/generic_subscription.cpp
   src/rclcpp/graph_listener.cpp
   src/rclcpp/guard_condition.cpp
   src/rclcpp/init_options.cpp
@@ -67,7 +70,6 @@ set(${PROJECT_NAME}_SRCS
   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
   src/rclcpp/node_interfaces/node_clock.cpp
   src/rclcpp/node_interfaces/node_graph.cpp
@@ -78,13 +80,14 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/node_interfaces/node_timers.cpp
   src/rclcpp/node_interfaces/node_topics.cpp
   src/rclcpp/node_interfaces/node_waitables.cpp
+  src/rclcpp/node_options.cpp
   src/rclcpp/parameter.cpp
-  src/rclcpp/parameter_value.cpp
   src/rclcpp/parameter_client.cpp
   src/rclcpp/parameter_event_handler.cpp
   src/rclcpp/parameter_events_filter.cpp
   src/rclcpp/parameter_map.cpp
   src/rclcpp/parameter_service.cpp
+  src/rclcpp/parameter_value.cpp
   src/rclcpp/publisher_base.cpp
   src/rclcpp/qos.cpp
   src/rclcpp/qos_event.cpp
@@ -99,6 +102,7 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/time_source.cpp
   src/rclcpp/timer.cpp
   src/rclcpp/type_support.cpp
+  src/rclcpp/typesupport_helpers.cpp
   src/rclcpp/utilities.cpp
   src/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.cpp
   src/rclcpp/waitable.cpp
@@ -179,6 +183,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC
 target_link_libraries(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT})
 # specific order: dependents before dependencies
 ament_target_dependencies(${PROJECT_NAME}
+  "ament_index_cpp"
   "libstatistics_collector"
   "rcl"
   "rcl_yaml_param_parser"
@@ -209,6 +214,7 @@ ament_export_include_directories(include)
 ament_export_libraries(${PROJECT_NAME})
 ament_export_targets(${PROJECT_NAME})
 
+ament_export_dependencies(ament_index_cpp)
 ament_export_dependencies(libstatistics_collector)
 ament_export_dependencies(rcl)
 ament_export_dependencies(rcpputils)

diff --git a/rclcpp/include/rclcpp/create_generic_publisher.hpp b/rclcpp/include/rclcpp/create_generic_publisher.hpp
@@ -0,0 +1,69 @@
+// Copyright 2020, Apex.AI 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__CREATE_GENERIC_PUBLISHER_HPP_
+#define RCLCPP__CREATE_GENERIC_PUBLISHER_HPP_
+
+#include <memory>
+#include <string>
+#include <utility>
+
+#include "rclcpp/generic_publisher.hpp"
+#include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/publisher_options.hpp"
+#include "rclcpp/qos.hpp"
+#include "rclcpp/typesupport_helpers.hpp"
+
+namespace rclcpp
+{
+
+/// Create and return a GenericPublisher.
+/**
+ * The returned pointer will never be empty, but this function can throw various exceptions, for
+ * instance when the message's package can not be found on the AMENT_PREFIX_PATH.
+ *
+ * \param topics_interface NodeTopicsInterface pointer used in parts of the setup
+ * \param topic_name Topic name
+ * \param topic_type Topic type
+ * \param qos %QoS settings
+ * \param options %Publisher options.
+ * Not all publisher options are currently respected, the only relevant options for this
+ * publisher are `event_callbacks`, `use_default_callbacks`, and `%callback_group`.
+ */
+template<typename AllocatorT = std::allocator<void>>
+std::shared_ptr<GenericPublisher> create_generic_publisher(
+  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics_interface,
+  const std::string & topic_name,
+  const std::string & topic_type,
+  const rclcpp::QoS & qos,
+  const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options = (
+    rclcpp::PublisherOptionsWithAllocator<AllocatorT>()
+  )
+)
+{
+  auto ts_lib = rclcpp::get_typesupport_library(topic_type, "rosidl_typesupport_cpp");
+  auto pub = std::make_shared<GenericPublisher>(
+    topics_interface->get_node_base_interface(),
+    std::move(ts_lib),
+    topic_name,
+    topic_type,
+    qos,
+    options);
+  topics_interface->add_publisher(pub, options.callback_group);
+  return pub;
+}
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__CREATE_GENERIC_PUBLISHER_HPP_
diff --git a/rclcpp/include/rclcpp/create_generic_subscription.hpp b/rclcpp/include/rclcpp/create_generic_subscription.hpp
@@ -0,0 +1,79 @@
+// Copyright 2020, Apex.AI 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__CREATE_GENERIC_SUBSCRIPTION_HPP_
+#define RCLCPP__CREATE_GENERIC_SUBSCRIPTION_HPP_
+
+#include <functional>
+#include <memory>
+#include <string>
+#include <utility>
+
+#include "rcl/subscription.h"
+#include "rclcpp/generic_subscription.hpp"
+#include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/qos.hpp"
+#include "rclcpp/serialized_message.hpp"
+#include "rclcpp/subscription_options.hpp"
+#include "rclcpp/typesupport_helpers.hpp"
+
+namespace rclcpp
+{
+
+/// Create and return a GenericSubscription.
+/**
+ * The returned pointer will never be empty, but this function can throw various exceptions, for
+ * instance when the message's package can not be found on the AMENT_PREFIX_PATH.
+ *
+ * \param topics_interface NodeTopicsInterface pointer used in parts of the setup.
+ * \param topic_name Topic name
+ * \param topic_type Topic type
+ * \param qos %QoS settings
+ * \param callback Callback for new messages of serialized form
+ * \param options %Publisher options.
+ * Not all publisher options are currently respected, the only relevant options for this
+ * publisher are `event_callbacks`, `use_default_callbacks`, and `%callback_group`.
+ */
+template<typename AllocatorT = std::allocator<void>>
+std::shared_ptr<GenericSubscription> create_generic_subscription(
+  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics_interface,
+  const std::string & topic_name,
+  const std::string & topic_type,
+  const rclcpp::QoS & qos,
+  std::function<void(std::shared_ptr<rclcpp::SerializedMessage>)> callback,
+  const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options = (
+    rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>()
+  )
+)
+{
+  auto ts_lib = rclcpp::get_typesupport_library(
+    topic_type, "rosidl_typesupport_cpp");
+
+  auto subscription = std::make_shared<GenericSubscription>(
+    topics_interface->get_node_base_interface(),
+    std::move(ts_lib),
+    topic_name,
+    topic_type,
+    qos,
+    callback,
+    options);
+
+  topics_interface->add_subscription(subscription, options.callback_group);
+
+  return subscription;
+}
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__CREATE_GENERIC_SUBSCRIPTION_HPP_
diff --git a/rclcpp/include/rclcpp/generic_publisher.hpp b/rclcpp/include/rclcpp/generic_publisher.hpp
@@ -0,0 +1,127 @@
+// Copyright 2018, Bosch Software Innovations GmbH.
+// Copyright 2021, Apex.AI 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__GENERIC_PUBLISHER_HPP_
+#define RCLCPP__GENERIC_PUBLISHER_HPP_
+
+#include <memory>
+#include <string>
+
+#include "rcpputils/shared_library.hpp"
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/publisher_base.hpp"
+#include "rclcpp/qos.hpp"
+#include "rclcpp/serialized_message.hpp"
+#include "rclcpp/typesupport_helpers.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// %Publisher for serialized messages whose type is not known at compile time.
+/**
+ * Since the type is not known at compile time, this is not a template, and the dynamic library
+ * containing type support information has to be identified and loaded based on the type name.
+ *
+ * It does not support intra-process handling.
+ */
+class GenericPublisher : public rclcpp::PublisherBase
+{
+public:
+  // cppcheck-suppress unknownMacro
+  RCLCPP_SMART_PTR_DEFINITIONS(GenericPublisher)
+
+  /// Constructor.
+  /**
+   * In order to properly publish to a topic, this publisher needs to be added to
+   * the node_topic_interface of the node passed into this constructor.
+   *
+   * \sa rclcpp::Node::create_generic_publisher() or rclcpp::create_generic_publisher() for
+   * creating an instance of this class and adding it to the node_topic_interface.
+   *
+   * \param node_base Pointer to parent node's NodeBaseInterface
+   * \param ts_lib Type support library, needs to correspond to topic_type
+   * \param topic_name Topic name
+   * \param topic_type Topic type
+   * \param qos %QoS settings
+   * \param callback Callback for new messages of serialized form
+   * \param options %Publisher options.
+   * Not all publisher options are currently respected, the only relevant options for this
+   * publisher are `event_callbacks`, `use_default_callbacks`, and `%callback_group`.
+   */
+  template<typename AllocatorT = std::allocator<void>>
+  GenericPublisher(
+    rclcpp::node_interfaces::NodeBaseInterface * node_base,
+    std::shared_ptr<rcpputils::SharedLibrary> ts_lib,
+    const std::string & topic_name,
+    const std::string & topic_type,
+    const rclcpp::QoS & qos,
+    const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options)
+  : rclcpp::PublisherBase(
+      node_base,
+      topic_name,
+      *rclcpp::get_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib),
+      options.template to_rcl_publisher_options<rclcpp::SerializedMessage>(qos)),
+    ts_lib_(ts_lib)
+  {
+    // This is unfortunately duplicated with the code in publisher.hpp.
+    // TODO(nnmm): Deduplicate by moving this into PublisherBase.
+    if (options.event_callbacks.deadline_callback) {
+      this->add_event_handler(
+        options.event_callbacks.deadline_callback,
+        RCL_PUBLISHER_OFFERED_DEADLINE_MISSED);
+    }
+    if (options.event_callbacks.liveliness_callback) {
+      this->add_event_handler(
+        options.event_callbacks.liveliness_callback,
+        RCL_PUBLISHER_LIVELINESS_LOST);
+    }
+    if (options.event_callbacks.incompatible_qos_callback) {
+      this->add_event_handler(
+        options.event_callbacks.incompatible_qos_callback,
+        RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS);
+    } else if (options.use_default_callbacks) {
+      // Register default callback when not specified
+      try {
+        this->add_event_handler(
+          [this](QOSOfferedIncompatibleQoSInfo & info) {
+            this->default_incompatible_qos_callback(info);
+          },
+          RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS);
+      } catch (UnsupportedEventTypeException & /*exc*/) {
+        // pass
+      }
+    }
+  }
+
+  RCLCPP_PUBLIC
+  virtual ~GenericPublisher() = default;
+
+  /// Publish a rclcpp::SerializedMessage.
+  RCLCPP_PUBLIC
+  void publish(const rclcpp::SerializedMessage & message);
+
+private:
+  // The type support library should stay loaded, so it is stored in the GenericPublisher
+  std::shared_ptr<rcpputils::SharedLibrary> ts_lib_;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__GENERIC_PUBLISHER_HPP_