-
Notifications
You must be signed in to change notification settings - Fork 151
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Signed-off-by: Ivan Santiago Paunovic <[email protected]> Co-authored-by: Louise Poubel <[email protected]>
- Loading branch information
Showing
14 changed files
with
572 additions
and
32 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -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()) { | ||
|
|
||
77 changes: 77 additions & 0 deletions
77
ros_ign_bridge/src/service_factories/ros_ign_interfaces.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 |
34 changes: 34 additions & 0 deletions
34
ros_ign_bridge/src/service_factories/ros_ign_interfaces.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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_ |
Oops, something went wrong.