make type support helper supported for service and action as well

Signed-off-by: Chen Lihui <lihui.chen@sony.com>

rclcpp/include/rclcpp/typesupport_helpers.hpp

@@ -21,7 +21,9 @@
 #include <tuple>
 
 #include "rcpputils/shared_library.hpp"
+#include "rosidl_runtime_cpp/action_type_support_decl.hpp"
 #include "rosidl_runtime_cpp/message_type_support_decl.hpp"
+#include "rosidl_runtime_cpp/service_type_support_decl.hpp"
 
 #include "rclcpp/visibility_control.hpp"
 
@@ -37,20 +39,88 @@ RCLCPP_PUBLIC
 std::shared_ptr<rcpputils::SharedLibrary>
 get_typesupport_library(const std::string & type, const std::string & typesupport_identifier);
 
+namespace internal
+{
+
+struct typesupport_message_tag {};
+struct typesupport_service_tag {};
+struct typesupport_action_tag {};
+
+template<typename T>
+struct typesupport_traits
+{
+  using type_tag = T;
+};
+
+template<>
+struct typesupport_traits<rosidl_message_type_support_t>
+{
+  using type_tag = typesupport_message_tag;
+};
+
+template<>
+struct typesupport_traits<rosidl_service_type_support_t>
+{
+  using type_tag = typesupport_service_tag;
+};
+
+template<>
+struct typesupport_traits<rosidl_action_type_support_t>
+{
+  using type_tag = typesupport_action_tag;
+};
+
+
+RCLCPP_PUBLIC
+const rosidl_message_type_support_t * _get_typesupport_handle(
+  const std::string & type,
+  const std::string & typesupport_identifier,
+  rcpputils::SharedLibrary & library,
+  typesupport_message_tag
+);
+
+RCLCPP_PUBLIC
+const rosidl_service_type_support_t * _get_typesupport_handle(
+  const std::string & type,
+  const std::string & typesupport_identifier,
+  rcpputils::SharedLibrary & library,
+  typesupport_service_tag
+);
+
+RCLCPP_PUBLIC
+const rosidl_action_type_support_t * _get_typesupport_handle(
+  const std::string & type,
+  const std::string & typesupport_identifier,
+  rcpputils::SharedLibrary & library,
+  typesupport_action_tag
+);
+
+}  // namespace internal
+
 /// Extract the type support handle from the library.
 /**
- * The library needs to match the topic type. The shared library must stay loaded for the lifetime of the result.
+ * The library needs to match the type of topic, service or action.
+ * The shared library must stay loaded for the lifetime of the result.
+ *
+ * The template parameter can be `rosidl_message_type_support_t`, `rosidl_service_type_support_t`
+ * and `rosidl_action_type_support_t`.
+ *
  * \param[in] type The topic type, e.g. "std_msgs/msg/String"
  * \param[in] typesupport_identifier Type support identifier, typically "rosidl_typesupport_cpp"
  * \param[in] library The shared type support library
  * \return A type support handle
+ * \throws std::runtime_error if library could not be found.
  */
-RCLCPP_PUBLIC
-const rosidl_message_type_support_t *
+template<typename T = rosidl_message_type_support_t>
+const T *
 get_typesupport_handle(
   const std::string & type,
   const std::string & typesupport_identifier,
-  rcpputils::SharedLibrary & library);
+  rcpputils::SharedLibrary & library)
+{
+  return internal::_get_typesupport_handle(
+    type, typesupport_identifier, library, typename internal::typesupport_traits<T>::type_tag());
+}
 
 }  // namespace rclcpp
 

rclcpp/src/rclcpp/typesupport_helpers.cpp

@@ -101,30 +101,39 @@ get_typesupport_library(const std::string & type, const std::string & typesuppor
   return std::make_shared<rcpputils::SharedLibrary>(library_path);
 }
 
-const rosidl_message_type_support_t *
-get_typesupport_handle(
+namespace internal
+{
+
+static const void * _get_typesupport_handle_impl(
   const std::string & type,
   const std::string & typesupport_identifier,
+  const std::string & typesupport_name,
+  const std::string & symbol_part_name,
+  const std::string & middle_module_additional,
   rcpputils::SharedLibrary & library)
 {
   std::string package_name;
   std::string middle_module;
   std::string type_name;
   std::tie(package_name, middle_module, type_name) = extract_type_identifier(type);
 
-  auto mk_error = [&package_name, &type_name](auto reason) {
+  if (middle_module.empty()) {
+    middle_module = middle_module_additional;
+  }
+
+  auto mk_error = [&package_name, &type_name, &typesupport_name](auto reason) {
       std::stringstream rcutils_dynamic_loading_error;
       rcutils_dynamic_loading_error <<
-        "Something went wrong loading the typesupport library for message type " << package_name <<
+        "Something went wrong loading the typesupport library for " <<
+        typesupport_name << " type " << package_name <<
         "/" << type_name << ". " << reason;
       return rcutils_dynamic_loading_error.str();
     };
 
   try {
-    std::string symbol_name = typesupport_identifier + "__get_message_type_support_handle__" +
-      package_name + "__" + (middle_module.empty() ? "msg" : middle_module) + "__" + type_name;
-
-    const rosidl_message_type_support_t * (* get_ts)() = nullptr;
+    std::string symbol_name = typesupport_identifier + symbol_part_name +
+      package_name + "__" + middle_module + "__" + type_name;
+    const void * (* get_ts)() = nullptr;
     // This will throw runtime_error if the symbol was not found.
     get_ts = reinterpret_cast<decltype(get_ts)>(library.get_symbol(symbol_name));
     return get_ts();
@@ -133,4 +142,55 @@ get_typesupport_handle(
   }
 }
 
+const rosidl_message_type_support_t * _get_typesupport_handle(
+  const std::string & type,
+  const std::string & typesupport_identifier,
+  rcpputils::SharedLibrary & library,
+  typesupport_message_tag)
+{
+  static const std::string typesupport_name = "message";
+  static const std::string symbol_part_name = "__get_message_type_support_handle__";
+  static const std::string middle_module_additional = "msg";
+
+  return static_cast<const rosidl_message_type_support_t *>(_get_typesupport_handle_impl(
+           type, typesupport_identifier, typesupport_name, symbol_part_name,
+           middle_module_additional, library
+  ));
+}
+
+const rosidl_service_type_support_t * _get_typesupport_handle(
+  const std::string & type,
+  const std::string & typesupport_identifier,
+  rcpputils::SharedLibrary & library,
+  typesupport_service_tag)
+{
+  static const std::string typesupport_name = "service";
+  static const std::string symbol_part_name = "__get_service_type_support_handle__";
+  static const std::string middle_module_additional = "srv";
+
+  return static_cast<const rosidl_service_type_support_t *>(_get_typesupport_handle_impl(
+           type, typesupport_identifier, typesupport_name, symbol_part_name,
+           middle_module_additional, library
+  ));
+}
+
+const rosidl_action_type_support_t * _get_typesupport_handle(
+  const std::string & type,
+  const std::string & typesupport_identifier,
+  rcpputils::SharedLibrary & library,
+  typesupport_action_tag)
+{
+  static const std::string typesupport_name = "action";
+  static const std::string symbol_part_name = "__get_action_type_support_handle__";
+  static const std::string middle_module_additional = "action";
+
+  return static_cast<const rosidl_action_type_support_t *>(_get_typesupport_handle_impl(
+           type, typesupport_identifier, typesupport_name, symbol_part_name,
+           middle_module_additional, library
+  ));
+}
+
+}  // namespace internal
+
+
 }  // namespace rclcpp

rclcpp/test/rclcpp/test_typesupport_helpers.cpp

@@ -75,3 +75,88 @@ TEST(TypesupportHelpersTest, returns_c_type_info_for_valid_library) {
     FAIL() << e.what();
   }
 }
+
+TEST(TypesupportHelpersTest, returns_service_type_info_for_valid_legacy_library) {
+  try {
+    auto library = rclcpp::get_typesupport_library(
+      "test_msgs/Empty", "rosidl_typesupport_cpp");
+    auto empty_typesupport = rclcpp::get_typesupport_handle<rosidl_service_type_support_t>(
+      "test_msgs/Empty", "rosidl_typesupport_cpp", *library);
+
+    EXPECT_THAT(
+      std::string(empty_typesupport->typesupport_identifier),
+      ContainsRegex("rosidl_typesupport"));
+  } catch (const std::runtime_error & e) {
+    FAIL() << e.what();
+  }
+}
+
+TEST(TypesupportHelpersTest, returns_service_type_info_for_valid_library) {
+  try {
+    auto library = rclcpp::get_typesupport_library(
+      "test_msgs/srv/Empty", "rosidl_typesupport_cpp");
+    auto empty_typesupport = rclcpp::get_typesupport_handle<rosidl_service_type_support_t>(
+      "test_msgs/srv/Empty", "rosidl_typesupport_cpp", *library);
+
+    EXPECT_THAT(
+      std::string(empty_typesupport->typesupport_identifier),
+      ContainsRegex("rosidl_typesupport"));
+  } catch (const std::runtime_error & e) {
+    FAIL() << e.what();
+  }
+}
+
+TEST(TypesupportHelpersTest, returns_action_type_info_for_valid_legacy_library) {
+  try {
+    auto library = rclcpp::get_typesupport_library(
+      "test_msgs/Fibonacci", "rosidl_typesupport_cpp");
+    auto fibonacci_typesupport = rclcpp::get_typesupport_handle<rosidl_action_type_support_t>(
+      "test_msgs/Fibonacci", "rosidl_typesupport_cpp", *library);
+
+    EXPECT_NE(nullptr, fibonacci_typesupport);
+  } catch (const std::runtime_error & e) {
+    FAIL() << e.what();
+  }
+}
+
+TEST(TypesupportHelpersTest, returns_action_type_info_for_valid_library) {
+  try {
+    auto library = rclcpp::get_typesupport_library(
+      "test_msgs/action/Fibonacci", "rosidl_typesupport_cpp");
+    auto fibonacci_typesupport = rclcpp::get_typesupport_handle<rosidl_action_type_support_t>(
+      "test_msgs/action/Fibonacci", "rosidl_typesupport_cpp", *library);
+
+    EXPECT_NE(nullptr, fibonacci_typesupport);
+  } catch (const std::runtime_error & e) {
+    FAIL() << e.what();
+  }
+}
+
+TEST(TypesupportHelpersTest, test_throw_exception_with_invalid_type) {
+  // message
+  std::string invalid_type = "test_msgs/msg/InvalidType";
+  auto library = rclcpp::get_typesupport_library(invalid_type, "rosidl_typesupport_cpp");
+  EXPECT_THROW(
+    rclcpp::get_typesupport_handle(invalid_type, "rosidl_typesupport_cpp", *library),
+    std::runtime_error);
+  EXPECT_THROW(
+    rclcpp::get_typesupport_handle<rosidl_message_type_support_t>(
+      invalid_type, "rosidl_typesupport_cpp", *library),
+    std::runtime_error);
+
+  // service
+  invalid_type = "test_msgs/srv/InvalidType";
+  library = rclcpp::get_typesupport_library(invalid_type, "rosidl_typesupport_cpp");
+  EXPECT_THROW(
+    rclcpp::get_typesupport_handle<rosidl_service_type_support_t>(
+      invalid_type, "rosidl_typesupport_cpp", *library),
+    std::runtime_error);
+
+  // action
+  invalid_type = "test_msgs/action/InvalidType";
+  library = rclcpp::get_typesupport_library(invalid_type, "rosidl_typesupport_cpp");
+  EXPECT_THROW(
+    rclcpp::get_typesupport_handle<rosidl_action_type_support_t>(
+      invalid_type, "rosidl_typesupport_cpp", *library),
+    std::runtime_error);
+}