Implement 'rmw_get_service_names_and_types' (ros2#76)

Signed-off-by: Simon Hoinkis <[email protected]>

rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_topic_names_and_types.hpp

@@ -35,6 +35,7 @@ void fill_topic_containers(
   std::map<std::string, std::vector<std::string>> & topic_publishers_);
 
 std::map<std::string, std::string> get_topic_names_and_types();
+std::map<std::string, std::string> get_service_names_and_types();
 
 std::map<std::string, std::vector<std::string>> get_nodes_and_publishers();
 

rmw_iceoryx_cpp/src/internal/iceoryx_get_topic_endpoint_info.cpp

@@ -117,7 +117,7 @@ fill_rmw_publisher_end_info(
 
   int i = 0;
   // store all data in rmw_topic_endpoint_info_array_t
-  for (const auto node_full_name : full_name_array) {
+  for (const auto& node_full_name : full_name_array) {
     auto name_n_space = get_name_n_space_from_node_full_name(node_full_name);
     auto rmw_topic_endpoint_info = rmw_get_zero_initialized_topic_endpoint_info();
     // duplicate and store the topic_name
@@ -204,7 +204,7 @@ fill_rmw_subscriber_end_info(
 
   int i = 0;
   // store all data in rmw_topic_endpoint_info_array_t
-  for (const auto node_full_name : full_name_array) {
+  for (const auto& node_full_name : full_name_array) {
     auto name_n_space = get_name_n_space_from_node_full_name(node_full_name);
     auto rmw_topic_endpoint_info = rmw_get_zero_initialized_topic_endpoint_info();
     // duplicate and store the topic_name

rmw_iceoryx_cpp/src/internal/iceoryx_topic_names_and_types.cpp

@@ -28,6 +28,8 @@
 #include "rmw_iceoryx_cpp/iceoryx_name_conversion.hpp"
 #include "rmw_iceoryx_cpp/iceoryx_topic_names_and_types.hpp"
 
+#include "iceoryx_posh/runtime/service_discovery.hpp"
+
 namespace rmw_iceoryx_cpp
 {
 
@@ -140,6 +142,40 @@ std::map<std::string, std::string> get_topic_names_and_types()
   return names_n_types;
 }
 
+std::map<std::string, std::string> get_service_names_and_types()
+{
+  std::map<std::string, std::string> names_n_types;
+  std::map<std::string, std::vector<std::string>> clients_topics;  // Currently, no possibility to query for clients..
+  std::map<std::string, std::vector<std::string>> servers_topics;  // Can be discovered via 'ServiceDiscovery'
+  std::map<std::string, std::vector<std::string>> topic_clients;   // Currently, no possibility to query for clients..
+  std::map<std::string, std::vector<std::string>> topic_servers;   // Can be discovered, but missing associated node 
+
+  std::vector<iox::capro::ServiceDescription> available_servers;
+
+  iox::runtime::ServiceDiscovery service_discovery;
+  service_discovery.findService(iox::cxx::nullopt,
+                                iox::cxx::nullopt,
+                                iox::cxx::nullopt,
+                                [&](auto& service_description){ available_servers.push_back(service_description); },
+                                iox::popo::MessagingPattern::REQ_RES);
+
+  for (auto & server : available_servers) {
+    auto name_and_type = rmw_iceoryx_cpp::get_name_n_type_from_service_description(
+      std::string(server.getServiceIDString().c_str()),
+      std::string(server.getInstanceIDString().c_str()),
+      std::string(server.getEventIDString().c_str()));
+
+    names_n_types[std::get<0>(name_and_type)] = std::get<1>(name_and_type);
+    /// @todo There is no API to find out which 'ServiceDescription' is offered by which node, for now we use 'NodeFoo'..
+    servers_topics[std::string("NodeFoo")].push_back(
+      std::get<0>(
+        name_and_type));
+    topic_servers[std::get<0>(name_and_type)].push_back(std::string("NodeFoo"));
+  }
+
+  return names_n_types;
+}
+
 std::map<std::string, std::vector<std::string>> get_nodes_and_publishers()
 {
   std::map<std::string, std::string> names_n_types;

rmw_iceoryx_cpp/src/rmw_request.cpp

@@ -165,7 +165,7 @@ rmw_take_request(
       ros_request);
   }
   request_header->source_timestamp = 0; // Unsupported until needed
-  rcutils_system_time_now(&request_header->received_timestamp);
+  ret = rcutils_system_time_now(&request_header->received_timestamp);
   request_header->request_id.sequence_number = iceoryx_request_header->getSequenceId();
   request_header->request_id.writer_guid[0] = 42; /// @todo
 

rmw_iceoryx_cpp/src/rmw_response.cpp

@@ -68,7 +68,7 @@ rmw_take_response(
       /// @todo check writer guid?
       request_header->request_id.sequence_number = iceoryx_response_header->getSequenceId();
       request_header->source_timestamp = 0; // Unsupported until needed
-      rcutils_system_time_now(&request_header->received_timestamp);
+      ret = rcutils_system_time_now(&request_header->received_timestamp);
 
       if (iceoryx_response_header->getSequenceId() == iceoryx_client_abstraction->sequence_id_ - 1)
       {

rmw_iceoryx_cpp/src/rmw_service_names_and_types.cpp

@@ -19,6 +19,8 @@
 #include "rmw/names_and_types.h"
 #include "rmw/rmw.h"
 
+#include "rmw_iceoryx_cpp/iceoryx_topic_names_and_types.hpp"
+
 extern "C"
 {
 rmw_ret_t
@@ -31,7 +33,21 @@ rmw_get_service_names_and_types(
   RCUTILS_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_ERROR);
   RCUTILS_CHECK_ARGUMENT_FOR_NULL(service_names_and_types, RMW_RET_ERROR);
 
-  RMW_SET_ERROR_MSG("rmw_iceoryx_cpp does not support services.");
-  return RMW_RET_UNSUPPORTED;
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    rmw_get_service_names_and_types
+    : node, node->implementation_identifier,
+    rmw_get_implementation_identifier(), return RMW_RET_ERROR);
+
+  rmw_ret_t rmw_ret = rmw_names_and_types_check_zero(service_names_and_types);
+  if (rmw_ret != RMW_RET_OK) {
+    return rmw_ret;  // error already set
+  }
+
+  auto iceoryx_service_names_and_types = rmw_iceoryx_cpp::get_service_names_and_types();
+
+  return rmw_iceoryx_cpp::fill_rmw_names_and_types(
+    service_names_and_types, iceoryx_service_names_and_types, allocator);
+
+  return RMW_RET_OK;
 }
 }  // extern "C"

rmw_iceoryx_cpp/src/rmw_service_server_is_available.cpp

@@ -19,7 +19,7 @@
 
 #include "./types/iceoryx_client.hpp"
 
-#include "iceoryx_posh/runtime/service_discovery.hpp"
+//#include "iceoryx_posh/runtime/service_discovery.hpp"
 
 extern "C"
 {