Add example test of how to access message type supports from rmw_dds_…

…common

Signed-off-by: ivanpauno <[email protected]>

Move from one participant per node to one participant per context.

Signed-off-by: ivanpauno <[email protected]>

Reimplemented rmw_node_info_and_types

Signed-off-by: Ivan Santiago Paunovic <[email protected]>

Implement rmw_count

Signed-off-by: Ivan Santiago Paunovic <[email protected]>

Please linter

Signed-off-by: Ivan Santiago Paunovic <[email protected]>

Correct rmw_service_server_is_available

Signed-off-by: Ivan Santiago Paunovic <[email protected]>

Implement rmw_service_names_and_types and rmw_topic_names_and_types

Signed-off-by: Ivan Santiago Paunovic <[email protected]>

Add guard condition to listener thread

Signed-off-by: Ivan Santiago Paunovic <[email protected]>

Stop using group data. Use the new group cache

Signed-off-by: Ivan Santiago Paunovic <[email protected]>

Delete old group data stuff

Signed-off-by: Ivan Santiago Paunovic <[email protected]>

rmw_fastrtps_cpp/CMakeLists.txt

@@ -32,6 +32,7 @@ endif()
 find_package(ament_cmake_ros REQUIRED)
 
 find_package(rcutils REQUIRED)
+find_package(rmw_dds_common REQUIRED)
 find_package(rmw_fastrtps_shared_cpp REQUIRED)
 
 find_package(fastrtps_cmake_module REQUIRED)
@@ -52,7 +53,9 @@ add_library(rmw_fastrtps_cpp
   src/get_publisher.cpp
   src/get_service.cpp
   src/get_subscriber.cpp
+  src/listener_thread.cpp
   src/identifier.cpp
+  src/publisher.cpp
   src/rmw_logging.cpp
   src/rmw_client.cpp
   src/rmw_compare_gids_equal.cpp
@@ -80,7 +83,11 @@ add_library(rmw_fastrtps_cpp
   src/rmw_wait.cpp
   src/rmw_wait_set.cpp
   src/serialization_format.cpp
+  src/subscription.cpp
   src/type_support_common.cpp
+  src/gid__type_support.cpp
+  src/node_entities_info__type_support.cpp
+  src/participant_entities_info__type_support.cpp
 )
 target_link_libraries(rmw_fastrtps_cpp
   fastcdr fastrtps)
@@ -90,6 +97,7 @@ ament_target_dependencies(rmw_fastrtps_cpp
   "rcutils"
   "rosidl_typesupport_fastrtps_c"
   "rosidl_typesupport_fastrtps_cpp"
+  "rmw_dds_common"
   "rmw_fastrtps_shared_cpp"
   "rmw"
   "rosidl_generator_c"
@@ -120,6 +128,12 @@ register_rmw_implementation(
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
+
+  ament_add_gtest(test_gid_message test/test_gid_message.cpp)
+  if(TARGET test_gid_message)
+    target_link_libraries(test_gid_message ${PROJECT_NAME})
+    # target_include_directories(test_gid_message ${PROJECT_NAME})
+  endif()
 endif()
 
 ament_package(

rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/publisher.hpp

@@ -0,0 +1,35 @@
+// Copyright 2017-2019 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 RMW_FASTRTPS_CPP__PUBLISHER_HPP_
+#define RMW_FASTRTPS_CPP__PUBLISHER_HPP_
+
+#include "rmw/rmw.h"
+#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp"
+
+namespace rmw_fastrtps_cpp
+{
+
+rmw_publisher_t *
+create_publisher(
+  const CustomParticipantInfo * participant_info,
+  const rosidl_message_type_support_t * type_supports,
+  const char * topic_name,
+  const rmw_qos_profile_t * qos_policies,
+  bool keyed,
+  bool create_publisher_listener);
+
+}  // namespace rmw_fastrtps_cpp
+
+#endif  // RMW_FASTRTPS_CPP__PUBLISHER_HPP_

rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/subscription.hpp

@@ -0,0 +1,36 @@
+// Copyright 2019 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 RMW_FASTRTPS_CPP__SUBSCRIPTION_HPP_
+#define RMW_FASTRTPS_CPP__SUBSCRIPTION_HPP_
+
+#include "rmw/rmw.h"
+#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp"
+
+namespace rmw_fastrtps_cpp
+{
+
+rmw_subscription_t *
+create_subscription(
+  const CustomParticipantInfo * participant_info,
+  const rosidl_message_type_support_t * type_supports,
+  const char * topic_name,
+  const rmw_qos_profile_t * qos_policies,
+  bool ignore_local_publications,
+  bool keyed,
+  bool create_subscription_listener);
+
+}  // namespace rmw_fastrtps_cpp
+
+#endif  // RMW_FASTRTPS_CPP__SUBSCRIPTION_HPP_

rmw_fastrtps_cpp/package.xml

@@ -20,6 +20,7 @@
   <build_depend>fastrtps_cmake_module</build_depend>
   <build_depend>rcutils</build_depend>
   <build_depend>rmw</build_depend>
+  <build_depend>rmw_dds_common</build_depend>
   <build_depend>rmw_fastrtps_shared_cpp</build_depend>
   <build_depend>rosidl_generator_c</build_depend>
   <build_depend>rosidl_generator_cpp</build_depend>
@@ -30,6 +31,7 @@
   <build_export_depend>fastrtps</build_export_depend>
   <build_export_depend>fastrtps_cmake_module</build_export_depend>
   <build_export_depend>rcutils</build_export_depend>
+  <build_export_depend>rmw_dds_common</build_export_depend>
   <build_export_depend>rmw_fastrtps_shared_cpp</build_export_depend>
   <build_export_depend>rmw</build_export_depend>
   <build_export_depend>rosidl_generator_c</build_export_depend>

rmw_fastrtps_cpp/src/get_participant.cpp

@@ -15,6 +15,8 @@
 #include "rmw_fastrtps_cpp/get_participant.hpp"
 
 #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp"
+#include "rmw_fastrtps_shared_cpp/rmw_context_impl.h"
+
 #include "rmw_fastrtps_cpp/identifier.hpp"
 
 namespace rmw_fastrtps_cpp
@@ -29,7 +31,7 @@ get_participant(rmw_node_t * node)
   if (node->implementation_identifier != eprosima_fastrtps_identifier) {
     return nullptr;
   }
-  auto impl = static_cast<CustomParticipantInfo *>(node->data);
+  auto impl = static_cast<CustomParticipantInfo *>(node->context->impl->participant_info);
   return impl->participant;
 }
 

rmw_fastrtps_cpp/src/gid__type_support.cpp

@@ -0,0 +1,52 @@
+// Copyright 2019 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.
+
+#include <cstddef>
+
+#include "rosidl_generator_c/message_type_support_struct.h"
+// rosidl_typesupport_cpp/message_type_support.hpp is installed by rosidl_generator_cpp
+#include "rosidl_typesupport_cpp/message_type_support.hpp"
+#include "rmw_dds_common/msg/gid__struct.hpp"
+#include "rmw_dds_common/msg/gid__rosidl_typesupport_fastrtps_cpp.hpp"
+#include "rmw_fastrtps_cpp/visibility_control.h"
+
+namespace rosidl_typesupport_cpp
+{
+
+template<>
+RMW_FASTRTPS_CPP_PUBLIC
+const rosidl_message_type_support_t *
+get_message_type_support_handle<rmw_dds_common::msg::Gid>()
+{
+  return ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME(
+    rosidl_typesupport_fastrtps_cpp, rmw_dds_common, msg, Gid)();
+}
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+RMW_FASTRTPS_CPP_PUBLIC
+const rosidl_message_type_support_t *
+ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME(
+  rosidl_typesupport_cpp, rmw_dds_common, msg, Gid)()
+{
+  return get_message_type_support_handle<rmw_dds_common::msg::Gid>();
+}
+
+#ifdef __cplusplus
+}
+#endif
+}  // namespace rosidl_typesupport_cpp

rmw_fastrtps_cpp/src/listener_thread.cpp

@@ -0,0 +1,169 @@
+// Copyright 2019 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.
+
+#include <atomic>
+#include <cassert>
+#include <cstring>
+#include <thread>
+
+#include "rcutils/logging_macros.h"
+
+#include "rmw/allocators.h"
+#include "rmw/error_handling.h"
+#include "rmw/init.h"
+#include "rmw/ret_types.h"
+#include "rmw/rmw.h"
+#include "rmw/types.h"
+#include "rmw/impl/cpp/macros.hpp"
+
+#include "rmw_dds_common/context.hpp"
+#include "rmw_dds_common/gid_utils.hpp"
+#include "rmw_dds_common/msg/participant_entities_info.hpp"
+
+#include "rmw_fastrtps_shared_cpp/rmw_context_impl.h"
+
+#include "listener_thread.hpp"
+
+using rmw_dds_common::operator<<;
+
+static const char log_tag[] = "rmw_dds_common";
+
+static
+void
+node_listener(rmw_context_t * context);
+
+rmw_ret_t
+rmw_fastrtps_cpp::run_listener_thread(rmw_context_t * context)
+{
+  auto common_context = static_cast<rmw_dds_common::Context *>(context->impl->common);
+  common_context->thread_is_running.store(true);
+  common_context->listener_thread_gc = rmw_create_guard_condition(context);
+  if (!common_context->listener_thread_gc) {
+    goto fail;
+  }
+  try {
+    common_context->listener_thread = std::thread(node_listener, context);
+  } catch (...) {
+    goto fail;
+  }
+  return RMW_RET_OK;
+fail:
+  common_context->thread_is_running.store(false);
+  if (common_context->listener_thread_gc) {
+    if (RMW_RET_OK != rmw_destroy_guard_condition(common_context->listener_thread_gc)) {
+      RCUTILS_LOG_ERROR_NAMED(log_tag, "Failed to destroy guard condition");
+    }
+  }
+  return RMW_RET_ERROR;
+}
+
+rmw_ret_t
+rmw_fastrtps_cpp::join_listener_thread(rmw_context_t * context)
+{
+  auto common_context = static_cast<rmw_dds_common::Context *>(context->impl->common);
+  common_context->thread_is_running.exchange(false);
+  if (RMW_RET_OK != rmw_trigger_guard_condition(common_context->listener_thread_gc)) {
+    goto fail;
+  }
+  try {
+    common_context->listener_thread.join();
+  } catch (...) {
+    goto fail;
+  }
+  if (RMW_RET_OK != rmw_destroy_guard_condition(common_context->listener_thread_gc)) {
+    goto fail;
+  }
+  return RMW_RET_OK;
+fail:
+  return RMW_RET_ERROR;
+}
+
+static
+void
+terminate(const char * error_message)
+{
+  RCUTILS_LOG_ERROR_NAMED(log_tag, "%s, terminating ...", error_message);
+  std::terminate();
+}
+
+void
+node_listener(rmw_context_t * context)
+{
+  assert(nullptr != context);
+  assert(nullptr != context->impl);
+  assert(nullptr != context->impl->common);
+  auto common_context = static_cast<rmw_dds_common::Context *>(context->impl->common);
+  while (common_context->thread_is_running.load()) {
+    assert(nullptr != common_context->sub);
+    assert(nullptr != common_context->sub->data);
+    void * subscriptions_buffer[] = {common_context->sub->data};
+    void * guard_conditions_buffer[] = {common_context->listener_thread_gc->data};
+    rmw_subscriptions_t subscriptions;
+    rmw_guard_conditions_t guard_conditions;
+    subscriptions.subscriber_count = 1;
+    subscriptions.subscribers = subscriptions_buffer;
+    guard_conditions.guard_condition_count = 1;
+    guard_conditions.guard_conditions = guard_conditions_buffer;
+    // number of conditions of a subscription is 2
+    rmw_wait_set_t * wait_set = rmw_create_wait_set(context, 2);
+    if (nullptr == wait_set) {
+      terminate("failed to create wait set");
+    }
+    if (RMW_RET_OK != rmw_wait(
+        &subscriptions,
+        &guard_conditions,
+        nullptr,
+        nullptr,
+        nullptr,
+        wait_set,
+        nullptr))
+    {
+      terminate("rmw_wait failed");
+    }
+    if (subscriptions_buffer[0]) {
+      rmw_dds_common::msg::ParticipantEntitiesInfo msg;
+      bool taken;
+      if (RMW_RET_OK != rmw_take(
+          common_context->sub,
+          static_cast<void *>(&msg),
+          &taken,
+          nullptr))
+      {
+        terminate("rmw_take failed");
+      }
+      if (taken) {
+        // TODO(ivanpauno): Should the program be terminated if taken is false?
+        if (std::strncmp(
+            reinterpret_cast<char *>(common_context->gid.data),
+            reinterpret_cast<char *>(&msg.gid.data),
+            RMW_GID_STORAGE_SIZE) == 0)
+        {
+          // ignore local messages
+          continue;
+        }
+        common_context->graph_cache.update_participant_entities(msg);
+        if (rcutils_logging_logger_is_enabled_for("rmw_dds_common",
+          RCUTILS_LOG_SEVERITY_DEBUG))
+        {
+          std::ostringstream ss;
+          ss << common_context->graph_cache;
+          RCUTILS_LOG_DEBUG_NAMED("rmw_fastrtps_cpp", "%s", ss.str().c_str());
+        }
+      }
+    }
+    if (RMW_RET_OK != rmw_destroy_wait_set(wait_set)) {
+      terminate("failed to destroy wait set");
+    }
+  }
+}