Open sloretz opened 6 years ago
One issue is it has the bridge always accepting goals from ROS 2 action clients before contacting the ROS 1 action server, but that hides what's happening on the ROS 1 action server from ROS 2 introspection tools.
Why not use a timeout when submitting the goal to the ROS 1 action server? Submit the goal to the ROS 1 server before accepting it on the ROS 2 side, and if the server never responds before the timeout then the bridge can respond to the ROS 2 client appropriately.
Another issue is it "detects" a ROS 1 action server or client using only a single topic, but servers or clients aren't actually functional until all topics are in use.
This is an easy problem to solve, isn't it? Look for a set of topics, and when all are available the server/client is up and running. Perhaps add a timeout so if a server partially starts or something that won't stall things.
Yet another issue is it always creates an action server if there is an action client on one side, which makes it seem like there is an action server available when there isn't.
Can the bridge look for matching pairs?
We have a proposal for action_bridge. It currently supports forwarding goals from ROS1 action client to ROS2 action server. Please see the discussion here:
https://discourse.ros.org/t/introducing-action-bridge/9103/3
@ipa-hsd What is the progress on this specific issue? Is it possible to use ros1_bridge for actions?
@ravijo the CI fails because a ROS1 dependency could not be found https://github.com/ros2/ros1_bridge/pull/256#issuecomment-1086393654. Since it's a ROS2 package, I am not sure how to specify ROS1 deps. A cheap hack that worked, but not recommended is https://github.com/ros2/ros1_bridge/pull/256#issuecomment-1156632545
But otherwise yes, https://github.com/ros2/ros1_bridge/pull/256 works for actions now. Please test it if it works for your use-case.
@ipa-hsd Thanks for the update. I tested it and it works like a charm.
There was a warning about unused parameters, which I ignored. For reference, I am sharing the complete logs below:
$ colcon build --symlink-install --packages-select ros1_bridge --cmake-force-configure
Starting >>> ros1_bridge
[Processing: ros1_bridge]
[Processing: ros1_bridge]
[Processing: ros1_bridge]
[Processing: ros1_bridge]
[Processing: ros1_bridge]
[Processing: ros1_bridge]
[Processing: ros1_bridge]
[Processing: ros1_bridge]
[Processing: ros1_bridge]
--- stderr: ros1_bridge
/home/ravi/bridge_test/ros2_ws/src/ros1_bridge/src/dynamic_bridge.cpp: In function ‘void update_bridge(ros::NodeHandle&, rclcpp::Node::SharedPtr, const std::map<std::__cxx11::basic_string<char>, std::__cxx11::basic_string<char> >&, const std::map<std::__cxx11::basic_string<char>, std::__cxx11::basic_string<char> >&, const std::map<std::__cxx11::basic_string<char>, std::__cxx11::basic_string<char> >&, const std::map<std::__cxx11::basic_string<char>, std::__cxx11::basic_string<char> >&, const std::map<std::__cxx11::basic_string<char>, std::map<std::__cxx11::basic_string<char>, std::__cxx11::basic_string<char> > >&, const std::map<std::__cxx11::basic_string<char>, std::map<std::__cxx11::basic_string<char>, std::__cxx11::basic_string<char> > >&, const std::map<std::__cxx11::basic_string<char>, std::map<std::__cxx11::basic_string<char>, std::__cxx11::basic_string<char> > >&, const std::map<std::__cxx11::basic_string<char>, std::map<std::__cxx11::basic_string<char>, std::__cxx11::basic_string<char> > >&, const std::map<std::__cxx11::basic_string<char>, std::map<std::__cxx11::basic_string<char>, std::__cxx11::basic_string<char> > >&, const std::map<std::__cxx11::basic_string<char>, std::map<std::__cxx11::basic_string<char>, std::__cxx11::basic_string<char> > >&, std::map<std::__cxx11::basic_string<char>, Bridge1to2HandlesAndMessageTypes>&, std::map<std::__cxx11::basic_string<char>, Bridge2to1HandlesAndMessageTypes>&, std::map<std::__cxx11::basic_string<char>, ros1_bridge::ServiceBridge1to2>&, std::map<std::__cxx11::basic_string<char>, ros1_bridge::ServiceBridge2to1>&, std::map<std::__cxx11::basic_string<char>, std::unique_ptr<ros1_bridge::ActionFactoryInterface> >&, std::map<std::__cxx11::basic_string<char>, std::unique_ptr<ros1_bridge::ActionFactoryInterface> >&, bool, bool)’:
/home/ravi/bridge_test/ros2_ws/src/ros1_bridge/src/dynamic_bridge.cpp:147:69: warning: unused parameter ‘ros1_action_clients’ [-Wunused-parameter]
147 | const std::map<std::string, std::map<std::string, std::string>> & ros1_action_clients,
| ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~
/home/ravi/bridge_test/ros2_ws/src/ros1_bridge/src/dynamic_bridge.cpp:149:69: warning: unused parameter ‘ros2_action_clients’ [-Wunused-parameter]
149 | const std::map<std::string, std::map<std::string, std::string>> & ros2_action_clients,
| ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~
---
Finished <<< ros1_bridge [5min 0s]
Summary: 1 package finished [5min 0s]
1 package had stderr output: ros1_bridge
great to hear that! :)
In ros2/design#193 there is no description of how actions would be transmitted across the ROS 1 bridge. This section should be written and included in the design document.
Some content was written, but then deleted after feedback found a lot of issues.
Deleted content for `Bridging between ROS 1 and ROS 2`
```markdown ## Bridging between ROS 1 and ROS 2 ### Detecting Action Servers and Clients There will be an API for the bridge to get a list of all ROS 2 action servers and clients. ROS 1 action servers and clients can be discovered by looking for topic ending in `/status` with message type `actionlib_msgs/GoalStatusArray`. If a publisher exists then an action server exists. If a subscriber exists then a client exists. ### Bridging ROS 1 Action Client and ROS 2 Action Server If either a ROS 2 action server or a ROS 1 action client exists then the ROS 1 bridge will create the following: 1. a ROS 1 action server 2. a ROS 2 action client #### Goal submission When the ROS 1 action client submits a goal the bridge will check if a ROS 2 action server exists. If no server exists then the goal is rejected, otherwise the bridge calls the ROS 2 goal submission service. If the ROS 2 server accepts the goal then the bridge will call `setAccepted` on the ROS 1 bridge, otherwise it will call `setRejected`. When a goal is accepted the bridge stores a map between the ROS 1 and ROS 2 goal IDs. If a client submits a goal with the same ID then it will be rejected until the first goal is finished. #### Goal Cancellation When a ROS 1 client tries to cancel a goal the bridge must try to cancel it on the ROS 2 server. First the bridge must look up the mapping of ROS 1 to ROS 2 goal IDs. If a mapping exists then the bridge will call the cancellation service. If the mapping does not exist because the server has not yet accepted or rejected the goal then the bridge will wait until the goal is accepted. If it is rejected the bridge will call setRejected and ignore the cancellation request. If it is accepted then it will call the cancellation service. #### Feedback The bridge will only publish feedback messages for goals that were sent through it. Since the goal ID is generated by the server, the bridge can only know the mapping of ROS 1 to ROS 2 goal IDs if the bridge sent the goal. #### Result Once a goal has been accepted the bridge will wait for the result. When it gets the result the bridge will call If the bridge notices the ROS 2 action server disappears from the node graph then bridge will wait for the result indefinitely. When the bridge reappears the bridge will try to get the result from the goal again. If the ROS 2 server does not know of the goal, then the bridge will notify the ROS 1 client that the goal was canceled. ### Bridging ROS 1 Action Server and ROS 2 Action Client If either a ROS 1 action server or a ROS 2 action client exist then the bridge will create: 1. a ROS 1 action client 2. a ROS 2 action server #### Goal submission When the ROS 2 action client submits a goal the bridge will check if a ROS 1 action server exists. If it does not exist the bridge will reject the goal, otherwise it will submit the goal to the ROS 1 server. The same goal ID is to be used for both ROS 1 and ROS 2. Goals sent to the bridge from ROS 2 are always immediately accepted by the bridge. The bridge will then submit the goal to the ROS 1 server. The reason for instantly accepting the goal is that if the ROS 1 action server never responds to a goal request then the bridge cannot return a result. Goals that are rejected by ROS 1 action servers will be reported as canceled to ROS 2 action clients. #### Goal Cancellation When a ROS 2 client tries to cancel a goal the bridge will immediately accept the cancellation request. It will then try to cancel the request on the ROS 1 action server. If the cancellation request is rejected by the ROS 1 server then the ROS 2 bridge will stay in the *CANCELING* state until the result of the goal is known. #### Feedback Since the goal ID is the same for both ROS 1 and ROS 2, the bridge will always publish feedback from ROS 1 servers to the ROS 2 feedback topic. #### Result When the ROS 1 action server publishes a result it will be set as the result on the ROS 1 bridge. If the ROS 2 action client never calls the service to get the result then it is subject to the same timeout as if it were a normal ROS 2 action server. ```One issue is it has the bridge always accepting goals from ROS 2 action clients before contacting the ROS 1 action server, but that hides what's happening on the ROS 1 action server from ROS 2 introspection tools. Another issue is it "detects" a ROS 1 action server or client using only a single topic, but servers or clients aren't actually functional until all topics are in use. Yet another issue is it always creates an action server if there is an action client on one side, which makes it seem like there is an action server available when there isn't.
Since the section was written goal ids where changed back to being generated by the client, so there may not be a reason to prevent feedback from ROS 2 goals being transmitted to ROS 2.