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>

rclcpp_generic/include/rclcpp_generic/generic_publisher.hpp

@@ -0,0 +1,42 @@
+// Copyright 2018, Bosch Software Innovations GmbH.
+//
+// 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 ROSBAG2_TRANSPORT__GENERIC_PUBLISHER_HPP_
+#define ROSBAG2_TRANSPORT__GENERIC_PUBLISHER_HPP_
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+
+namespace rosbag2_transport
+{
+
+class GenericPublisher : public rclcpp::PublisherBase
+{
+public:
+  GenericPublisher(
+    rclcpp::node_interfaces::NodeBaseInterface * node_base,
+    const rosidl_message_type_support_t & type_support,
+    const std::string & topic_name,
+    const rclcpp::QoS & qos);
+
+  virtual ~GenericPublisher() = default;
+
+  void publish(std::shared_ptr<rmw_serialized_message_t> message);
+};
+
+}  // namespace rosbag2_transport
+
+#endif  // ROSBAG2_TRANSPORT__GENERIC_PUBLISHER_HPP_

rclcpp_generic/include/rclcpp_generic/generic_subscription.hpp

@@ -0,0 +1,86 @@
+// Copyright 2018, Bosch Software Innovations GmbH.
+//
+// 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 ROSBAG2_TRANSPORT__GENERIC_SUBSCRIPTION_HPP_
+#define ROSBAG2_TRANSPORT__GENERIC_SUBSCRIPTION_HPP_
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/any_subscription_callback.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/serialization.hpp"
+#include "rclcpp/subscription.hpp"
+
+namespace rosbag2_transport
+{
+
+/**
+ * This class is an implementation of an rclcpp::Subscription for serialized messages whose topic
+ * is not known at compile time (hence templating does not work).
+ *
+ * It does not support intra-process handling
+ */
+class GenericSubscription : public rclcpp::SubscriptionBase
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(GenericSubscription)
+
+  /**
+   * Constructor. In order to properly subscribe to a topic, this subscription needs to be added to
+   * the node_topic_interface of the node passed into this constructor.
+   *
+   * \param node_base NodeBaseInterface pointer used in parts of the setup.
+   * \param ts Type support handle
+   * \param topic_name Topic name
+   * \param callback Callback for new messages of serialized form
+   */
+  GenericSubscription(
+    rclcpp::node_interfaces::NodeBaseInterface * node_base,
+    const rosidl_message_type_support_t & ts,
+    const std::string & topic_name,
+    const rclcpp::QoS & qos,
+    std::function<void(std::shared_ptr<rclcpp::SerializedMessage>)> callback);
+
+  // Same as create_serialized_message() as the subscription is to serialized_messages only
+  std::shared_ptr<void> create_message() override;
+
+  std::shared_ptr<rclcpp::SerializedMessage> create_serialized_message() override;
+
+  void handle_message(
+    std::shared_ptr<void> & message, const rclcpp::MessageInfo & message_info) override;
+
+  void handle_loaned_message(
+    void * loaned_message, const rclcpp::MessageInfo & message_info) override;
+
+  // Same as return_serialized_message() as the subscription is to serialized_messages only
+  void return_message(std::shared_ptr<void> & message) override;
+
+  void return_serialized_message(std::shared_ptr<rclcpp::SerializedMessage> & message) override;
+
+  // Provide a const reference to the QoS Profile used to create this subscription.
+  const rclcpp::QoS & qos_profile() const;
+
+private:
+  RCLCPP_DISABLE_COPY(GenericSubscription)
+
+  std::shared_ptr<rclcpp::SerializedMessage> borrow_serialized_message(size_t capacity);
+  rcutils_allocator_t default_allocator_;
+  std::function<void(std::shared_ptr<rclcpp::SerializedMessage>)> callback_;
+  const rclcpp::QoS qos_;
+};
+
+}  // namespace rosbag2_transport
+
+#endif  // ROSBAG2_TRANSPORT__GENERIC_SUBSCRIPTION_HPP_

rclcpp_generic/include/rclcpp_generic/typesupport_helpers.hpp

@@ -0,0 +1,49 @@
+// Copyright 2018, Bosch Software Innovations GmbH.
+//
+// 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 ROSBAG2_CPP__TYPESUPPORT_HELPERS_HPP_
+#define ROSBAG2_CPP__TYPESUPPORT_HELPERS_HPP_
+
+#include <string>
+#include <tuple>
+#include <utility>
+#include <memory>
+
+#include "rosbag2_cpp/visibility_control.hpp"
+
+#include "rcpputils/shared_library.hpp"
+
+#include "rosidl_runtime_cpp/message_type_support_decl.hpp"
+
+namespace rosbag2_cpp
+{
+
+ROSBAG2_CPP_PUBLIC
+std::shared_ptr<rcpputils::SharedLibrary>
+get_typesupport_library(const std::string & type, const std::string & typesupport_identifier);
+
+ROSBAG2_CPP_PUBLIC
+const rosidl_message_type_support_t *
+get_typesupport_handle(
+  const std::string & type,
+  const std::string & typesupport_identifier,
+  std::shared_ptr<rcpputils::SharedLibrary> library);
+
+ROSBAG2_CPP_PUBLIC
+const std::tuple<std::string, std::string, std::string>
+extract_type_identifier(const std::string & full_type);
+
+}  // namespace rosbag2_cpp
+
+#endif  // ROSBAG2_CPP__TYPESUPPORT_HELPERS_HPP_

rclcpp_generic/src/rclcpp_generic/generic_publisher.cpp

@@ -0,0 +1,51 @@
+// Copyright 2018, Bosch Software Innovations GmbH.
+//
+// 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.
+
+#include "generic_publisher.hpp"
+
+#include <memory>
+#include <string>
+
+namespace
+{
+rcl_publisher_options_t rosbag2_get_publisher_options(const rclcpp::QoS & qos)
+{
+  auto options = rcl_publisher_get_default_options();
+  options.qos = qos.get_rmw_qos_profile();
+  return options;
+}
+}  // unnamed namespace
+
+namespace rosbag2_transport
+{
+
+GenericPublisher::GenericPublisher(
+  rclcpp::node_interfaces::NodeBaseInterface * node_base,
+  const rosidl_message_type_support_t & type_support,
+  const std::string & topic_name,
+  const rclcpp::QoS & qos)
+: rclcpp::PublisherBase(node_base, topic_name, type_support, rosbag2_get_publisher_options(qos))
+{}
+
+void GenericPublisher::publish(std::shared_ptr<rmw_serialized_message_t> message)
+{
+  auto return_code = rcl_publish_serialized_message(
+    get_publisher_handle().get(), message.get(), NULL);
+
+  if (return_code != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(return_code, "failed to publish serialized message");
+  }
+}
+
+}  // namespace rosbag2_transport

rclcpp_generic/src/rclcpp_generic/generic_subscription.cpp

@@ -0,0 +1,103 @@
+// Copyright 2018, Bosch Software Innovations GmbH.
+//
+// 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.
+
+#include "generic_subscription.hpp"
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/any_subscription_callback.hpp"
+#include "rclcpp/subscription.hpp"
+
+#include "rosbag2_transport/logging.hpp"
+
+namespace
+{
+rcl_subscription_options_t rosbag2_get_subscription_options(const rclcpp::QoS & qos)
+{
+  auto options = rcl_subscription_get_default_options();
+  options.qos = qos.get_rmw_qos_profile();
+  return options;
+}
+}  // unnamed namespace
+
+namespace rosbag2_transport
+{
+
+GenericSubscription::GenericSubscription(
+  rclcpp::node_interfaces::NodeBaseInterface * node_base,
+  const rosidl_message_type_support_t & ts,
+  const std::string & topic_name,
+  const rclcpp::QoS & qos,
+  std::function<void(std::shared_ptr<rclcpp::SerializedMessage>)> callback)
+: SubscriptionBase(
+    node_base,
+    ts,
+    topic_name,
+    rosbag2_get_subscription_options(qos),
+    true),
+  default_allocator_(rcutils_get_default_allocator()),
+  callback_(callback),
+  qos_(qos)
+{}
+
+std::shared_ptr<void> GenericSubscription::create_message()
+{
+  return create_serialized_message();
+}
+
+std::shared_ptr<rclcpp::SerializedMessage> GenericSubscription::create_serialized_message()
+{
+  return borrow_serialized_message(0);
+}
+
+void GenericSubscription::handle_message(
+  std::shared_ptr<void> & message, const rclcpp::MessageInfo & message_info)
+{
+  (void) message_info;
+  auto typed_message = std::static_pointer_cast<rclcpp::SerializedMessage>(message);
+  callback_(typed_message);
+}
+
+void GenericSubscription::handle_loaned_message(
+  void * message, const rclcpp::MessageInfo & message_info)
+{
+  (void) message;
+  (void) message_info;
+}
+
+void GenericSubscription::return_message(std::shared_ptr<void> & message)
+{
+  auto typed_message = std::static_pointer_cast<rclcpp::SerializedMessage>(message);
+  return_serialized_message(typed_message);
+}
+
+void GenericSubscription::return_serialized_message(
+  std::shared_ptr<rclcpp::SerializedMessage> & message)
+{
+  message.reset();
+}
+
+const rclcpp::QoS & GenericSubscription::qos_profile() const
+{
+  return qos_;
+}
+
+std::shared_ptr<rclcpp::SerializedMessage>
+GenericSubscription::borrow_serialized_message(size_t capacity)
+{
+  return std::make_shared<rclcpp::SerializedMessage>(capacity);
+}
+
+}  // namespace rosbag2_transport