From 699a195d6ca7eb17f19374e9a9496f339f3f36e9 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 22 Apr 2022 19:12:53 -0300 Subject: [PATCH] Support bridging services (#211) Signed-off-by: Ivan Santiago Paunovic Co-authored-by: Louise Poubel --- ros_ign_bridge/CMakeLists.txt | 13 +++ ros_ign_bridge/src/bridge.hpp | 23 +++++ ros_ign_bridge/src/factories.cpp | 21 ++++ ros_ign_bridge/src/factories.hpp | 7 ++ ros_ign_bridge/src/parameter_bridge.cpp | 94 ++++++++++++------ .../service_factories/ros_ign_interfaces.cpp | 77 +++++++++++++++ .../service_factories/ros_ign_interfaces.hpp | 34 +++++++ ros_ign_bridge/src/service_factory.hpp | 97 +++++++++++++++++++ .../src/service_factory_interface.hpp | 42 ++++++++ ros_ign_bridge/src/static_bridge.cpp | 1 + .../test/launch/test_ign_server.launch.py | 69 +++++++++++++ .../test/serverclient/ign_server.cpp | 75 ++++++++++++++ .../test/serverclient/ros_client.cpp | 49 ++++++++++ ros_ign_gazebo/CMakeLists.txt | 2 + 14 files changed, 572 insertions(+), 32 deletions(-) create mode 100644 ros_ign_bridge/src/service_factories/ros_ign_interfaces.cpp create mode 100644 ros_ign_bridge/src/service_factories/ros_ign_interfaces.hpp create mode 100644 ros_ign_bridge/src/service_factory.hpp create mode 100644 ros_ign_bridge/src/service_factory_interface.hpp create mode 100644 ros_ign_bridge/test/launch/test_ign_server.launch.py create mode 100644 ros_ign_bridge/test/serverclient/ign_server.cpp create mode 100644 ros_ign_bridge/test/serverclient/ros_client.cpp diff --git a/ros_ign_bridge/CMakeLists.txt b/ros_ign_bridge/CMakeLists.txt index 8aeb4a30..e7179c67 100644 --- a/ros_ign_bridge/CMakeLists.txt +++ b/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( diff --git a/ros_ign_bridge/src/bridge.hpp b/ros_ign_bridge/src/bridge.hpp index 45f86a51..4a536e6d 100644 --- a/ros_ign_bridge/src/bridge.hpp +++ b/ros_ign_bridge/src/bridge.hpp @@ -44,6 +44,12 @@ struct BridgeHandles BridgeIgnToRosHandles bridgeIgnToRos; }; +struct BridgeIgnServicesToRosHandles +{ + std::shared_ptr 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 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_ diff --git a/ros_ign_bridge/src/factories.cpp b/ros_ign_bridge/src/factories.cpp index 877e5ab0..210c5824 100644 --- a/ros_ign_bridge/src/factories.cpp +++ b/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 +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 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 diff --git a/ros_ign_bridge/src/factories.hpp b/ros_ign_bridge/src/factories.hpp index ccb3a5f4..105b09eb 100644 --- a/ros_ign_bridge/src/factories.hpp +++ b/ros_ign_bridge/src/factories.hpp @@ -19,6 +19,7 @@ #include #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 +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_ diff --git a/ros_ign_bridge/src/parameter_bridge.cpp b/ros_ign_bridge/src/parameter_bridge.cpp index 5680b145..28160287 100644 --- a/ros_ign_bridge/src/parameter_bridge.cpp +++ b/ros_ign_bridge/src/parameter_bridge.cpp @@ -35,15 +35,17 @@ 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 .. " << - " \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 [ ..] " << + " [ ..]\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" << @@ -51,6 +53,13 @@ void usage() " ] == 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/String@ignition.msgs" << ".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 filter_args(int argc, char * argv[]) -{ - const std::string rosArgsBeginDelim = "--ros-args"; - const std::string rosArgsEndDelim = "--"; - // Skip first argument (executable path) - std::vector 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@" + "ignition.msgs.WorldControl@ignition.msgs.Boolean\n" << std::endl; } ////////////////////////////////////////////////// @@ -89,8 +83,10 @@ 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("ros_ign_bridge"); @@ -98,14 +94,15 @@ int main(int argc, char * argv[]) // Ignition node auto ign_node = std::make_shared(); - std::list bidirectional_handles; - std::list ign_to_ros_handles; - std::list ros_to_ign_handles; + std::vector bidirectional_handles; + std::vector ign_to_ros_handles; + std::vector ros_to_ign_handles; + std::vector 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()) { diff --git a/ros_ign_bridge/src/service_factories/ros_ign_interfaces.cpp b/ros_ign_bridge/src/service_factories/ros_ign_interfaces.cpp new file mode 100644 index 00000000..70f9e788 --- /dev/null +++ b/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 +#include + +#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 +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 diff --git a/ros_ign_bridge/src/service_factories/ros_ign_interfaces.hpp b/ros_ign_bridge/src/service_factories/ros_ign_interfaces.hpp new file mode 100644 index 00000000..b28a9e27 --- /dev/null +++ b/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 +#include + +#include "service_factory_interface.hpp" + +namespace ros_ign_bridge +{ + +std::shared_ptr +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_ diff --git a/ros_ign_bridge/src/service_factory.hpp b/ros_ign_bridge/src/service_factory.hpp new file mode 100644 index 00000000..f71d3bf7 --- /dev/null +++ b/ros_ign_bridge/src/service_factory.hpp @@ -0,0 +1,97 @@ +// 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_FACTORY_HPP_ +#define SERVICE_FACTORY_HPP_ + +#include + +#include +#include +#include +#include + +#include + +#include "ros_ign_bridge/convert_decl.hpp" + +#include "service_factory_interface.hpp" + +namespace ros_ign_bridge +{ + +template +bool +send_response_on_error(RosResT & ros_response); + +template +class ServiceFactory : public ServiceFactoryInterface +{ +public: + ServiceFactory( + const std::string & ros_type_name, const std::string & ign_req_type_name, + const std::string & ign_rep_type_name) + : ros_type_name_(ros_type_name), + ign_req_type_name_(ign_req_type_name), + ign_rep_type_name_(ign_rep_type_name) + {} + + rclcpp::ServiceBase::SharedPtr + create_ros_service( + rclcpp::Node::SharedPtr ros_node, + std::shared_ptr ign_node, + const std::string & service_name) override + { + return ros_node->create_service( + service_name, + [ign_node = std::move(ign_node), service_name]( + std::shared_ptr> srv_handle, + std::shared_ptr reqid, + std::shared_ptr ros_req) + { + std::function callback; + callback = [ + srv_handle = std::move(srv_handle), + reqid + ]( + const IgnReplyT & reply, + const bool result) + { + typename RosServiceT::Response ros_res; + if (!result) { + if (send_response_on_error(ros_res)) { + srv_handle->send_response(*reqid, ros_res); + } + } + convert_ign_to_ros(reply, ros_res); + srv_handle->send_response(*reqid, ros_res); + }; + IgnRequestT ign_req; + convert_ros_to_ign(*ros_req, ign_req); + ign_node->Request( + service_name, + ign_req, + callback); + }); + } + +private: + std::string ros_type_name_; + std::string ign_req_type_name_; + std::string ign_rep_type_name_; +}; + +} // namespace ros_ign_bridge + +#endif // SERVICE_FACTORY_HPP_ diff --git a/ros_ign_bridge/src/service_factory_interface.hpp b/ros_ign_bridge/src/service_factory_interface.hpp new file mode 100644 index 00000000..bf6bee84 --- /dev/null +++ b/ros_ign_bridge/src/service_factory_interface.hpp @@ -0,0 +1,42 @@ +// 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_FACTORY_INTERFACE_HPP_ +#define SERVICE_FACTORY_INTERFACE_HPP_ + +#include + +#include +#include + +#include +#include + +namespace ros_ign_bridge +{ + +class ServiceFactoryInterface +{ +public: + virtual + rclcpp::ServiceBase::SharedPtr + create_ros_service( + rclcpp::Node::SharedPtr ros_node, + std::shared_ptr ign_node, + const std::string & service_name) = 0; +}; + +} // namespace ros_ign_bridge + +#endif // SERVICE_FACTORY_INTERFACE_HPP_ diff --git a/ros_ign_bridge/src/static_bridge.cpp b/ros_ign_bridge/src/static_bridge.cpp index f87be010..c0e4c024 100644 --- a/ros_ign_bridge/src/static_bridge.cpp +++ b/ros_ign_bridge/src/static_bridge.cpp @@ -22,6 +22,7 @@ #include #include "bridge.hpp" +#include "service_factory.hpp" ////////////////////////////////////////////////// int main(int argc, char * argv[]) diff --git a/ros_ign_bridge/test/launch/test_ign_server.launch.py b/ros_ign_bridge/test/launch/test_ign_server.launch.py new file mode 100644 index 00000000..f9fd4d53 --- /dev/null +++ b/ros_ign_bridge/test/launch/test_ign_server.launch.py @@ -0,0 +1,69 @@ +# 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. + +import unittest + +from launch import LaunchDescription + +from launch_ros.actions import Node + +import launch_testing + + +def generate_test_description(): + + server = Node( + package='ros_ign_bridge', + executable='test_ign_server', + output='screen' + ) + process_under_test = Node( + package='ros_ign_bridge', + executable='test_ros_client', + output='screen' + ) + + # Bridge + bridge = Node( + package='ros_ign_bridge', + executable='parameter_bridge', + arguments=[ + '/ign_ros/test/serviceclient/world_control@ros_ign_interfaces/srv/ControlWorld', + ], + output='screen' + ) + return LaunchDescription([ + bridge, + server, + process_under_test, + launch_testing.util.KeepAliveProc(), + launch_testing.actions.ReadyToTest(), + ]), locals() + + +class ROSSubscriberTest(unittest.TestCase): + + def test_termination(self, process_under_test, proc_info): + proc_info.assertWaitForShutdown(process=process_under_test, timeout=200) + + +@launch_testing.post_shutdown_test() +class ROSSubscriberTestAfterShutdown(unittest.TestCase): + + def test_exit_code(self, process_under_test, proc_info): + launch_testing.asserts.assertExitCodes( + proc_info, + [launch_testing.asserts.EXIT_OK], + process_under_test + ) diff --git a/ros_ign_bridge/test/serverclient/ign_server.cpp b/ros_ign_bridge/test/serverclient/ign_server.cpp new file mode 100644 index 00000000..c255887a --- /dev/null +++ b/ros_ign_bridge/test/serverclient/ign_server.cpp @@ -0,0 +1,75 @@ +// 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 +#include + +#include + +#include +#include +#include +#include +#include + +#include "utils/test_utils.hpp" +#include "utils/ign_test_msg.hpp" + +/// \brief Flag used to break the waiting loop and terminate the program. +static std::atomic_flag g_terminateSrv(false); + +////////////////////////////////////////////////// +/// \brief Function callback executed when a SIGINT or SIGTERM signals are +/// captured. This is used to break the infinite loop that handles requests +/// and exit the program smoothly. +void signal_handler(int _signal) +{ + if (_signal == SIGINT || _signal == SIGTERM) { + g_terminateSrv.clear(); + } +} + +////////////////////////////////////////////////// +bool control_world( + const ignition::msgs::WorldControl &, + ignition::msgs::Boolean & _res) +{ + _res.set_data(true); + return true; +} + +////////////////////////////////////////////////// +int main(int /*argc*/, char **/*argv*/) +{ + g_terminateSrv.test_and_set(); + // Install a signal handler for SIGINT and SIGTERM. + std::signal(SIGINT, signal_handler); + std::signal(SIGTERM, signal_handler); + + // Create a transport node and advertise a topic. + ignition::transport::Node node; + + // ignition::msgs::WorldControl. + node.Advertise( + "/ign_ros/test/serviceclient/world_control", + &control_world); + + // Requests are handled in another thread. + // Wait until a signal is sent. + while (g_terminateSrv.test_and_set()) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + return 0; +} diff --git a/ros_ign_bridge/test/serverclient/ros_client.cpp b/ros_ign_bridge/test/serverclient/ros_client.cpp new file mode 100644 index 00000000..1785ced9 --- /dev/null +++ b/ros_ign_bridge/test/serverclient/ros_client.cpp @@ -0,0 +1,49 @@ +// 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 + +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "ros_ign_interfaces/srv/control_world.hpp" + +using namespace std::chrono_literals; + +///////////////////////////////////////////////// +TEST(ROSClientTest, WorldControl) +{ + rclcpp::init(0, NULL); + auto node = std::make_shared("test_ros_client_to_ign_service"); + auto client = node->create_client( + "/ign_ros/test/serviceclient/world_control"); + std::this_thread::sleep_for(1s); + ASSERT_TRUE(client->wait_for_service(5s)); + auto msg = std::make_shared(); + auto future = client->async_send_request(msg); + rclcpp::executors::SingleThreadedExecutor ex; + ex.add_node(node); + ex.spin_until_future_complete(future); + auto res = future.get(); + ASSERT_TRUE(res->success); +} + +///////////////////////////////////////////////// +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/ros_ign_gazebo/CMakeLists.txt b/ros_ign_gazebo/CMakeLists.txt index 5d5628dc..88bbc780 100644 --- a/ros_ign_gazebo/CMakeLists.txt +++ b/ros_ign_gazebo/CMakeLists.txt @@ -38,6 +38,7 @@ endif() ign_find_package(gflags REQUIRED PKGCONFIG gflags) +find_package(std_msgs REQUIRED) add_executable(create src/create.cpp) ament_target_dependencies(create @@ -50,6 +51,7 @@ target_link_libraries(create ignition-msgs${IGN_MSGS_VER}::core ignition-transport${IGN_TRANSPORT_VER}::core ) +ament_target_dependencies(create std_msgs) install(DIRECTORY launch