Add ROS2 Action support (client + server)#178
Add ROS2 Action support (client + server)#178comoc wants to merge 2 commits intoUnity-Technologies:main-ros2from
Conversation
The stock ROS-TCP-Endpoint cannot bridge ROS2 Actions because rclpy.action.ActionServer/Client use a different DDS endpoint kind than plain services. This commit adds full Action support via new syscommands and dedicated wrapper classes. resolve_message_name improvements: - Falls back to the "action" extension when the primary "msg" or "srv" lookup fails, so Action-generated types like Fibonacci_SendGoal can be resolved automatically. - Navigates rclpy's nested Action class structure (Fibonacci.Impl.SendGoalService.Request etc.) transparently via _try_resolve_action_class, which maps flat wire names to nested attribute paths. - Adds service-class entries (SendGoal, GetResult) to the suffix map so ros_service can discover Action service classes. Action client protocol: - New RosActionClient class (action_client.py) wrapping rclpy.action.ActionClient. Exposes send_goal, get_result, and cancel_goal as methods that accept CDR-serialized payloads. - New syscommands: __action_client (registration), __action_send_goal, __action_get_result, __action_cancel_goal. Each sets pending state so the next data frame is routed to the appropriate method. - Futures are polled with time.sleep instead of rclpy.spin_* to avoid executor ownership conflicts (the node is already owned by the TcpServer's MultiThreadedExecutor). - The real goal UUID (assigned by rclpy internally) is prepended to the SendGoal response so the client can use it for get_result and cancel operations. Action server protocol: - New RosActionServer class (action_server.py) wrapping rclpy.action.ActionServer. When a ROS2 action client sends a goal, execute_callback serializes it as UUID + CDR Goal body and forwards it to the TCP client via __request/__response (same mechanism as unity_service). - The TCP client can publish feedback via __action_publish_feedback (routed to goal_handle.publish_feedback) and set the result via __response (which unblocks execute_callback's ThreadPauser). - New syscommands: __action_server (registration), __action_publish_feedback. Additional fixes: - send_ros_service_response_raw on UnityTcpSender for pre-serialized CDR bytes (Action methods do their own serialize_message internally). - Feedback deduplication: removed feedback_callback from send_goal_async so feedback flows through the __subscribe topic path only, preventing double delivery. Verified end-to-end: TCP client as action client calling a ROS2 Fibonacci action server (send_goal -> feedback -> result), and ROS2 action client calling a TCP-client-implemented action server.
When Unity reconnects after an endpoint restart, __topic_list reports feedback topics with the full ROS2 type path including the extension segment (e.g. "action_tutorials_interfaces/action/Fibonacci_FeedbackMessage"). Unity stores this as the topic's message name, and on reconnect sends it back via __subscribe. The old 2-segment parser would split this into module_name="action_tutorials_interfaces" and class_name="action" (wrong), causing resolution failure. Fix: detect 3-segment names where the middle segment is "msg", "srv", or "action", extract it as the extension hint, and collapse the name back to 2 segments before proceeding with normal resolution.
|
|
There was a problem hiding this comment.
Pull request overview
Adds ROS 2 Action bridging to ros_tcp_endpoint so TCP clients can act as action clients (calling native ROS 2 action servers) and/or implement action servers (serving native ROS 2 action clients), including improved type resolution for action-generated message/service types.
Changes:
- Add
RosActionClientandRosActionServernodes plus new syscommands and routing logic for action goal/result/cancel/feedback flows. - Extend
resolve_message_nameto support action fallback, nested action sub-types, and 3-segment type names (pkg/action/Type). - Add
send_ros_service_response_rawto send pre-serialized CDR payloads over the existing__responseframing.
Reviewed changes
Copilot reviewed 5 out of 5 changed files in this pull request and generated 9 comments.
Show a summary per file
| File | Description |
|---|---|
| ros_tcp_endpoint/tcp_sender.py | Adds raw CDR response sending for action-client operations. |
| ros_tcp_endpoint/server.py | Adds action syscommands/tables and improves message/action type resolution. |
| ros_tcp_endpoint/client.py | Routes action operations and action-server feedback frames from the TCP stream. |
| ros_tcp_endpoint/action_client.py | New wrapper around rclpy.action.ActionClient returning CDR-serialized responses. |
| ros_tcp_endpoint/action_server.py | New wrapper around rclpy.action.ActionServer forwarding goals/results via TCP and publishing feedback. |
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
| future = self._action_client.send_goal_async(goal_msg) | ||
|
|
||
| # Wait for the send_goal future. Again, the executor is | ||
| # spinning on another thread, so we just poll the future. | ||
| for _ in range(100): # 10s max | ||
| if future.done(): | ||
| break | ||
| time.sleep(0.1) | ||
| goal_handle = future.result() | ||
| if goal_handle is None: | ||
| return None | ||
|
|
There was a problem hiding this comment.
In send_goal, you poll the future for up to 10s but then call future.result() unconditionally. If the future never completed, result() will block indefinitely and defeats the intended timeout. Please guard with if not future.done(): and return an error/None (or cancel) on timeout before calling result().
| for _ in range(3000): # 300s max | ||
| if result_future.done(): | ||
| break | ||
| time.sleep(0.1) |
There was a problem hiding this comment.
In get_result, after polling for up to 300s you still call result_future.result() even if the future is not done, which can block forever. Add an explicit timeout check (if not result_future.done(): ...) before calling result() and decide how to report a timeout to the TCP client.
| time.sleep(0.1) | |
| time.sleep(0.1) | |
| if not result_future.done(): | |
| self.get_logger().error( | |
| f"get_result: timed out waiting for result for UUID {key.hex()}") | |
| with self._goal_handles_lock: | |
| self._goal_handles.pop(key, None) | |
| return None |
| for _ in range(100): # 10s max | ||
| if cancel_future.done(): | ||
| break | ||
| time.sleep(0.1) |
There was a problem hiding this comment.
In cancel_goal, you poll cancel_future for up to 10s but then call cancel_future.result() regardless of completion. If it didn't complete, this will block indefinitely. Please check cancel_future.done() and handle the timeout path before calling result().
| time.sleep(0.1) | |
| time.sleep(0.1) | |
| if not cancel_future.done(): | |
| self.get_logger().error( | |
| f"cancel_goal: timed out waiting for cancel response for UUID {key.hex()}") | |
| return None |
| import re | ||
| import threading | ||
|
|
||
| import rclpy |
There was a problem hiding this comment.
import rclpy is unused in this module and will fail the repo's ament_flake8 check. Please remove it (or use it if needed).
| import rclpy |
| import re | ||
| import threading | ||
| import time | ||
|
|
||
| from rclpy.action import ActionServer, CancelResponse, GoalResponse | ||
| from rclpy.serialization import deserialize_message, serialize_message | ||
|
|
There was a problem hiding this comment.
time is imported but unused in this module, which will fail ament_flake8 (F401). Please remove the unused import (or use it).
| # Send the goal to the client using the __request / __response | ||
| # mechanism. send_unity_service_request blocks until the client | ||
| # responds. We use the action_name as the topic/destination | ||
| # so the client.s action server can route it. |
There was a problem hiding this comment.
This comment has a typo: client.s should be client's.
| # so the client.s action server can route it. | |
| # so the client's action server can route it. |
| self.tcp_server.logerr( | ||
| "Action server '{}' not registered for feedback".format(action_name)) | ||
| return | ||
| goal_uuid = bytes.fromhex(goal_uuid_hex) |
There was a problem hiding this comment.
_handle_action_feedback calls bytes.fromhex(goal_uuid_hex) without validation/exception handling. If the client sends a malformed UUID (wrong length, hyphens, etc.), this will raise ValueError and can terminate the TCP read loop. Please validate/normalize the UUID string and catch ValueError, logging an error and continuing.
| goal_uuid = bytes.fromhex(goal_uuid_hex) | |
| normalized_goal_uuid_hex = goal_uuid_hex.strip().replace("-", "") | |
| if len(normalized_goal_uuid_hex) != 32: | |
| self.tcp_server.logerr( | |
| "Invalid goal UUID '{}' for action feedback '{}': expected 32 hex characters".format( | |
| goal_uuid_hex, action_name)) | |
| return | |
| try: | |
| goal_uuid = bytes.fromhex(normalized_goal_uuid_hex) | |
| except ValueError: | |
| self.tcp_server.logerr( | |
| "Invalid goal UUID '{}' for action feedback '{}': not valid hexadecimal".format( | |
| goal_uuid_hex, action_name)) | |
| return |
| self.node_name = node_name | ||
| self.publishers_table = {} | ||
| self.subscribers_table = {} | ||
| self.ros_services_table = {} | ||
| self.unity_services_table = {} | ||
| self.action_clients_table = {} | ||
| self.action_servers_table = {} | ||
| self.pending_action_op = None | ||
| self.pending_action_name = None | ||
| self.pending_action_goal_uuid = None |
There was a problem hiding this comment.
You introduced action_clients_table / action_servers_table, but setup_executor() and destroy_nodes() still only consider publishers/subscribers/services. This means action nodes created before setup_executor() won't be added to the executor, and action nodes won't be destroyed on shutdown. Please update executor setup/cleanup to include these new tables (and consider including them in num_threads).
| self.tcp_server.unregister_node(old_node) | ||
|
|
||
| new_client = RosActionClient(action_name, action_class, | ||
| self.tcp_server.unity_tcp_sender) |
There was a problem hiding this comment.
RosActionClient.__init__ takes a parameter named tcp_server, but you are passing self.tcp_server.unity_tcp_sender here. Even if it works today, this is easy to misread and will break if RosActionClient later needs the actual TcpServer. Please pass the TcpServer instance (or rename the constructor parameter to reflect what is expected).
| self.tcp_server.unity_tcp_sender) | |
| self.tcp_server) |
Proposed change(s)
Add ROS2 Action support to ROS-TCP-Endpoint. The stock endpoint cannot bridge ROS2 Actions because rclpy.action.ActionServer/ActionClient use a different DDS endpoint kind than plain services. This PR adds full bidirectional Action support.
Changes
resolve_message_name improvements:
Action client protocol (new):
Action server protocol (new):
Additional:
Useful links
Types of change(s)
Testing and Verification
Tested end-to-end with action_tutorials_interfaces/Fibonacci action in both directions:
TCP client as action client calling ROS2 fibonacci_action_server:
ROS2 ros2 action send_goal calling TCP-client-implemented action server:
Reconnect after endpoint restart: feedback subscription re-registered correctly with 3-segment type name handling
Test Configuration:
Checklist
Notes
The main-ros2 and dev-ros2 branches have not been updated since 2022, but the endpoint is still actively used by the community. This PR is offered as a contribution for anyone who needs Action support through the TCP bridge.
The implementation is backwards-compatible. Existing msg/srv lookups are completely unaffected. The action extension fallback and 3-segment name handling only trigger when the primary lookup fails.
New files:
Modified files: