Support bridging services

ros_ign_bridge/CMakeLists.txt

@@ -100,6 +100,7 @@ add_library(${bridge_lib}
   SHARED
   src/factories.cpp
   src/factory_interface.cpp
+  src/service_factories/ros_ign_interfaces.cpp
 )
 
 target_include_directories(${bridge_lib}
@@ -198,10 +199,18 @@ if(BUILD_TESTING)
   add_executable(test_ign_subscriber test/subscribers/ign_subscriber.cpp)
   target_link_libraries(test_ign_subscriber test_utils)
 
+  add_executable(test_ign_server test/serverclient/ign_server.cpp)
+  target_link_libraries(test_ign_server test_utils)
+
+  add_executable(test_ros_client test/serverclient/ros_client.cpp)
+  target_link_libraries(test_ros_client test_utils)
+
   install(TARGETS
+    test_ros_client
     test_ros_publisher
     test_ros_subscriber
     test_ign_publisher
+    test_ign_server
     test_ign_subscriber
     DESTINATION lib/${PROJECT_NAME}
   )
@@ -213,6 +222,10 @@ if(BUILD_TESTING)
   add_launch_test(test/launch/test_ign_subscriber.launch.py
     TIMEOUT 200
   )
+
+  add_launch_test(test/launch/test_ign_server.launch.py
+    TIMEOUT 200
+  )
 endif()
 
 ament_export_dependencies(

ros_ign_bridge/src/bridge.hpp

@@ -44,6 +44,12 @@ struct BridgeHandles
   BridgeIgnToRosHandles bridgeIgnToRos;
 };
 
+struct BridgeIgnServicesToRosHandles
+{
+  std::shared_ptr<ignition::transport::Node> ign_node;
+  rclcpp::ServiceBase::SharedPtr ros_service;
+};
+
 BridgeRosToIgnHandles
 create_bridge_from_ros_to_ign(
   rclcpp::Node::SharedPtr ros_node,
@@ -108,6 +114,23 @@ create_bidirectional_bridge(
   return handles;
 }
 
+BridgeIgnServicesToRosHandles
+create_service_bridge(
+  rclcpp::Node::SharedPtr ros_node,
+  std::shared_ptr<ignition::transport::Node> ign_node,
+  const std::string & ros_type_name,
+  const std::string & ign_req_type_name,
+  const std::string & ign_rep_type_name,
+  const std::string & service_name)
+{
+  BridgeIgnServicesToRosHandles handles;
+  auto factory = get_service_factory(ros_type_name, ign_req_type_name, ign_rep_type_name);
+  auto ros_srv = factory->create_ros_service(ros_node, ign_node, service_name);
+  handles.ros_service = ros_srv;
+  handles.ign_node = ign_node;
+  return handles;
+}
+
 }  // namespace ros_ign_bridge
 
 #endif  // BRIDGE_HPP_

ros_ign_bridge/src/factories.cpp

@@ -27,6 +27,8 @@
 #include "factories/tf2_msgs.hpp"
 #include "factories/trajectory_msgs.hpp"
 
+#include "service_factories/ros_ign_interfaces.hpp"
+
 namespace ros_ign_bridge
 {
 
@@ -80,4 +82,23 @@ get_factory(
   throw std::runtime_error("No template specialization for the pair");
 }
 
+std::shared_ptr<ServiceFactoryInterface>
+get_service_factory(
+  const std::string & ros_type_name,
+  const std::string & ign_req_type_name,
+  const std::string & ign_rep_type_name)
+{
+  std::shared_ptr<ServiceFactoryInterface> impl;
+
+  impl = get_service_factory__ros_ign_interfaces(
+    ros_type_name, ign_req_type_name, ign_rep_type_name);
+  if (impl) {return impl;}
+
+  std::ostringstream oss{"No template specialization for the specified service type {"};
+  oss << ros_type_name << "}, ign request type {" << ign_req_type_name
+      << "}, ign request type {" << ign_req_type_name << "}, ign reply type name {"
+      << ign_rep_type_name << "}";
+  throw std::runtime_error(oss.str());
+}
+
 }  // namespace ros_ign_bridge

ros_ign_bridge/src/factories.hpp

@@ -19,6 +19,7 @@
 #include <string>
 
 #include "factory_interface.hpp"
+#include "service_factory_interface.hpp"
 
 namespace ros_ign_bridge
 {
@@ -28,6 +29,12 @@ get_factory(
   const std::string & ros_type_name,
   const std::string & ign_type_name);
 
+std::shared_ptr<ServiceFactoryInterface>
+get_service_factory(
+  const std::string & ros_type_name,
+  const std::string & ign_req_type_name,
+  const std::string & ign_rep_type_name);
+
 }  // namespace ros_ign_bridge
 
 #endif  // FACTORIES_HPP_

ros_ign_bridge/src/parameter_bridge.cpp

@@ -35,22 +35,31 @@ enum Direction
   FROM_IGN_TO_ROS = 1,
   // Only from ROS to IGN
   FROM_ROS_TO_IGN = 2,
+  // Unspecified, only used for services
+  DIR_UNSPECIFIED = 3,
 };
 
 //////////////////////////////////////////////////
 void usage()
 {
-  std::cout << "Bridge a collection of ROS2 and Ignition Transport topics.\n\n" <<
-    "  parameter_bridge <topic@ROS2_type@Ign_type> .. " <<
-    " <topic@ROS2_type@Ign_type>\n\n" <<
-    "The first @ symbol delimits the topic name from the message types.\n" <<
+  std::cout << "Bridge a collection of ROS2 and Ignition Transport topics and services.\n\n" <<
+    "  parameter_bridge [<topic@ROS2_type@Ign_type> ..] " <<
+    " [<service@ROS2_srv_type[@Ign_req_type@Ign_rep_type]> ..]\n\n" <<
+    "Topics: The first @ symbol delimits the topic name from the message types.\n" <<
     "Following the first @ symbol is the ROS message type.\n" <<
     "The ROS message type is followed by an @, [, or ] symbol where\n" <<
     "    @  == a bidirectional bridge, \n" <<
     "    [  == a bridge from Ignition to ROS,\n" <<
     "    ]  == a bridge from ROS to Ignition.\n" <<
     "Following the direction symbol is the Ignition Transport message " <<
     "type.\n\n" <<
+    "Services: The first @ symbol delimits the service name from the types.\n" <<
+    "Following the first @ symbol is the ROS service type.\n" <<
+    "Optionally, you can include the Ignition request and response type\n" <<
+    "separated by the @ symbol.\n" <<
+    "It is only supported to expose Ignition servces as ROS services, i.e.\n"
+    "the ROS service will forward request to the Ignition service and then forward\n"
+    "the response back to the ROS client.\n\n"
     "A bidirectional bridge example:\n" <<
     "    parameter_bridge /chatter@std_msgs/[email protected]" <<
     ".StringMsg\n\n" <<
@@ -59,27 +68,12 @@ void usage()
     ".StringMsg\n\n" <<
     "A bridge from ROS to Ignition example:\n" <<
     "    parameter_bridge /chatter@std_msgs/String]ignition.msgs" <<
-    ".StringMsg" << std::endl;
-}
-
-//////////////////////////////////////////////////
-std::vector<std::string> filter_args(int argc, char * argv[])
-{
-  const std::string rosArgsBeginDelim = "--ros-args";
-  const std::string rosArgsEndDelim = "--";
-  // Skip first argument (executable path)
-  std::vector<std::string> args(argv + 1, argv + argc);
-  auto rosArgsPos = std::find(args.begin(), args.end(), rosArgsBeginDelim);
-  auto rosArgsEndPos = std::find(rosArgsPos, args.end(), rosArgsEndDelim);
-  // If -- was found, delete it as well
-  if (rosArgsEndPos != args.end()) {
-    ++rosArgsEndPos;
-  }
-  // Delete args between --ros-args and -- (or --ros-args to end if not found)
-  if (rosArgsPos != args.end()) {
-    args.erase(rosArgsPos, rosArgsEndPos);
-  }
-  return args;
+    ".StringMsg\n" <<
+    "A service bridge:\n" <<
+    "    parameter_bridge /world/default/control@ros_ign_interfaces/srv/ControlWorld\n" <<
+    "Or equivalently:\n" <<
+    "    parameter_bridge /world/default/control@ros_ign_interfaces/srv/ControlWorld@"
+    "[email protected]\n" << std::endl;
 }
 
 //////////////////////////////////////////////////
@@ -89,23 +83,26 @@ int main(int argc, char * argv[])
     usage();
     return -1;
   }
-
-  rclcpp::init(argc, argv);
+  // skip the process name in argument procesing
+  ++argv;
+  --argc;
+  auto filteredArgs = rclcpp::init_and_remove_ros_arguments(argc, argv);
 
   // ROS 2 node
   auto ros_node = std::make_shared<rclcpp::Node>("ros_ign_bridge");
 
   // Ignition node
   auto ign_node = std::make_shared<ignition::transport::Node>();
 
-  std::list<ros_ign_bridge::BridgeHandles> bidirectional_handles;
-  std::list<ros_ign_bridge::BridgeIgnToRosHandles> ign_to_ros_handles;
-  std::list<ros_ign_bridge::BridgeRosToIgnHandles> ros_to_ign_handles;
+  std::vector<ros_ign_bridge::BridgeHandles> bidirectional_handles;
+  std::vector<ros_ign_bridge::BridgeIgnToRosHandles> ign_to_ros_handles;
+  std::vector<ros_ign_bridge::BridgeRosToIgnHandles> ros_to_ign_handles;
+  std::vector<ros_ign_bridge::BridgeIgnServicesToRosHandles> service_bridge_handles;
 
   // Filter arguments (i.e. remove ros args) then parse all the remaining ones
   const std::string delim = "@";
   const size_t queue_size = 10;
-  auto filteredArgs = filter_args(argc, argv);
+  // TODO(ivanpauno): Improve the parsing code later, it's hard to read ...
   for (auto & arg : filteredArgs) {
     auto delimPos = arg.find(delim);
     if (delimPos == std::string::npos || delimPos == 0) {
@@ -125,9 +122,11 @@ int main(int argc, char * argv[])
       delimPos = arg.find("[");
       if (delimPos == std::string::npos || delimPos == 0) {
         delimPos = arg.find("]");
-        if (delimPos == std::string::npos || delimPos == 0) {
+        if (delimPos == 0) {
           usage();
           return -1;
+        } else if (delimPos == std::string::npos) {
+          direction = DIR_UNSPECIFIED;
         } else {
           direction = FROM_ROS_TO_IGN;
         }
@@ -137,6 +136,37 @@ int main(int argc, char * argv[])
     }
     std::string ros_type_name = arg.substr(0, delimPos);
     arg.erase(0, delimPos + delim.size());
+    if (ros_type_name.find("/srv/") != std::string::npos) {
+      std::string ign_req_type_name;
+      std::string ign_rep_type_name;
+      if (direction != DIR_UNSPECIFIED && direction != BIDIRECTIONAL) {
+        usage();
+        return -1;
+      }
+      if (direction == BIDIRECTIONAL) {
+        delimPos = arg.find("@");
+        if (delimPos == std::string::npos || delimPos == 0) {
+          usage();
+          return -1;
+        }
+        ign_req_type_name = arg.substr(0, delimPos);
+        arg.erase(0, delimPos + delim.size());
+        ign_rep_type_name = std::move(arg);
+      }
+      try {
+        service_bridge_handles.push_back(
+          ros_ign_bridge::create_service_bridge(
+            ros_node,
+            ign_node,
+            ros_type_name,
+            ign_req_type_name,
+            ign_rep_type_name,
+            topic_name));
+      } catch (std::runtime_error & e) {
+        std::cerr << e.what() << std::endl;
+      }
+      continue;
+    }
 
     delimPos = arg.find(delim);
     if (delimPos != std::string::npos || arg.empty()) {

ros_ign_bridge/src/service_factories/ros_ign_interfaces.cpp

@@ -0,0 +1,77 @@
+// Copyright 2022 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 "factories/ros_ign_interfaces.hpp"
+
+#include <memory>
+#include <string>
+
+#include "ros_ign_interfaces/srv/control_world.hpp"
+
+#include "service_factory.hpp"
+#include "ros_ign_bridge/convert/ros_ign_interfaces.hpp"
+
+
+namespace ros_ign_bridge
+{
+
+std::shared_ptr<ServiceFactoryInterface>
+get_service_factory__ros_ign_interfaces(
+  const std::string & ros_type_name,
+  const std::string & ign_req_type_name,
+  const std::string & ign_rep_type_name)
+{
+  if (
+    ros_type_name == "ros_ign_interfaces/srv/ControlWorld" &&
+    (ign_req_type_name.empty() || ign_req_type_name == "ignition.msgs.WorldControl") &&
+    (ign_rep_type_name.empty() || ign_rep_type_name == "ignition.msgs.Boolean"))
+  {
+    return std::make_shared<
+      ServiceFactory<
+        ros_ign_interfaces::srv::ControlWorld,
+        ignition::msgs::WorldControl,
+        ignition::msgs::Boolean>
+    >(ros_type_name, "ignition.msgs.WorldControl", "ignition.msgs.Boolean");
+  }
+  return nullptr;
+}
+
+template<>
+void
+convert_ros_to_ign(
+  const ros_ign_interfaces::srv::ControlWorld::Request & ros_req,
+  ignition::msgs::WorldControl & ign_req)
+{
+  convert_ros_to_ign(ros_req.world_control, ign_req);
+}
+
+template<>
+void
+convert_ign_to_ros(
+  const ignition::msgs::Boolean & ign_rep,
+  ros_ign_interfaces::srv::ControlWorld::Response & ros_res)
+{
+  ros_res.success = ign_rep.data();
+}
+
+template<>
+bool
+send_response_on_error(ros_ign_interfaces::srv::ControlWorld::Response & ros_res)
+{
+  // TODO(now): Is it worth it to have a different field to encode ignition request errors?
+  //  Currently we're reusing the success field, which seems fine for this case.
+  ros_res.success = false;
+  return true;
+}
+}  // namespace ros_ign_bridge

ros_ign_bridge/src/service_factories/ros_ign_interfaces.hpp

@@ -0,0 +1,34 @@
+// Copyright 2022 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 SERVICE_FACTORIES__ROS_IGN_INTERFACES_HPP_
+#define SERVICE_FACTORIES__ROS_IGN_INTERFACES_HPP_
+
+#include <memory>
+#include <string>
+
+#include "service_factory_interface.hpp"
+
+namespace ros_ign_bridge
+{
+
+std::shared_ptr<ServiceFactoryInterface>
+get_service_factory__ros_ign_interfaces(
+  const std::string & ros_type_name,
+  const std::string & ign_req_type_name,
+  const std::string & ign_rep_type_name);
+
+}  // namespace ros_ign_bridge
+
+#endif  // SERVICE_FACTORIES__ROS_IGN_INTERFACES_HPP_