Add ROS2 Action client and server support#326
Conversation
Adds C# Action client and server classes that work with the patched ROS-TCP-Endpoint (comoc/ROS-TCP-Endpoint, main-ros2 branch) to bridge ROS2 Actions through the TCP connection. SysCommand.cs: - 6 new command constants (__action_client, __action_send_goal, __action_get_result, __action_cancel_goal, __action_server, __action_publish_feedback) - 3 new param structs (SysCommand_ActionRegistration, SysCommand_ActionGoalOp, SysCommand_ActionFeedback) ROSActionClient.cs: - ROSActionClient<TGoal, TResult, TFeedback> with async SendGoal() returning ActionGoalHandle - ActionGoalHandle with async GetResult(), Cancel(), and FeedbackReceived event - RegisterFeedbackSubscription<TFeedbackMessage>() for wiring up goal-specific feedback routing via SubscribeByMessageName - GetResultRequestProxy and CancelGoalRequestProxy internal messages - Handles the endpoint's UUID-prepend convention on SendGoal response - Skips int8 status + alignment padding in GetResult response ROSActionServer.cs: - ROSActionServer<TGoal, TResult, TFeedback> with GoalReceived event - ActionServerGoalHandle with PublishFeedback() and SetResult() - Goal routing via __request/__response reusing existing service infrastructure ROSConnection.cs: - AllocateServiceRequest() internal helper for thread-safe srv_id allocation without exposing private fields - QueueRawMessage() internal helper for sending data frames without triggering Publish()'s publisher-registration check - CreateActionClient<>() and CreateActionServer<>() factory methods - TryRouteToActionServer() for __request dispatch to action servers - Action server routing integrated into ReceiveSysCommand's m_SpecialIncomingMessageHandler - FindObjectOfType -> FindAnyObjectByType (deprecation fix) Hand-written Fibonacci message classes for testing: - FibonacciGoal, FibonacciResult, FibonacciFeedback, FibonacciFeedbackMessage (with dual MessageRegistry registration for short and /action/ type name variants) Verified end-to-end: - Action client: Unity -> ROS2 fibonacci_action_server, all 9 feedback messages + result [0,1,1,2,3,5,8,13,21,34,55] - Action server: ros2 action send_goal -> Unity, 4 feedback messages + result [0,1,1,2,3,5] with SUCCEEDED status - Reconnect after endpoint restart works correctly
Add ROS2 Action client and server support
|
|
There was a problem hiding this comment.
Pull request overview
Adds ROS2 Action client/server support to the Unity ROS TCP Connector, bridging ROS2 Actions over the existing TCP protocol (via new syscommands expected by a patched ROS-TCP-Endpoint).
Changes:
- Introduces
ROSActionClient/ROSActionServerand per-goal handle types for SendGoal/Feedback/GetResult/Cancel and Unity-hosted action execution. - Extends
ROSConnectionwith action factories, thread-safe service request ID allocation reuse, raw-frame enqueueing, and routing of incoming__requestframes to action servers. - Adds action-related syscommand constants/param structs and includes handwritten Fibonacci action message types used for end-to-end testing.
Reviewed changes
Copilot reviewed 16 out of 16 changed files in this pull request and generated 10 comments.
Show a summary per file
| File | Description |
|---|---|
| com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/SysCommand.cs | Adds action syscommand constants and JSON param structs for endpoint interop. |
| com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSConnection.cs | Adds action client/server factories, raw message enqueue helper, srv_id allocator, and __request routing for action goals. |
| com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSActionServer.cs | New Unity-side ROS2 action server implementation and per-goal server handle. |
| com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSActionServer.cs.meta | Unity asset metadata for the new action server source file. |
| com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSActionClient.cs | New Unity-side ROS2 action client implementation and per-goal handle (SendGoal/GetResult/Cancel + feedback subscription helper). |
| com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSActionClient.cs.meta | Unity asset metadata for the new action client source file. |
| com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces/action/FibonacciGoal.cs | Handwritten Fibonacci action Goal message used for testing. |
| com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces/action/FibonacciGoal.cs.meta | Unity asset metadata for FibonacciGoal. |
| com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces/action/FibonacciResult.cs | Handwritten Fibonacci action Result message used for testing. |
| com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces/action/FibonacciResult.cs.meta | Unity asset metadata for FibonacciResult. |
| com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces/action/FibonacciFeedback.cs | Handwritten Fibonacci action Feedback message used for testing. |
| com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces/action/FibonacciFeedback.cs.meta | Unity asset metadata for FibonacciFeedback. |
| com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces/action/FibonacciFeedbackMessage.cs | Handwritten Fibonacci FeedbackMessage wrapper (goal_id + feedback), including alternate ROS name registration. |
| com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces/action/FibonacciFeedbackMessage.cs.meta | Unity asset metadata for FibonacciFeedbackMessage. |
| com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces/action.meta | Unity folder metadata for ActionTutorialsInterfaces/action. |
| com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces.meta | Unity folder metadata for ActionTutorialsInterfaces. |
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
| // Use reflection to call OnGoalRequest. | ||
| var method = serverObj.GetType().GetMethod("OnGoalRequest", | ||
| System.Reflection.BindingFlags.Instance | System.Reflection.BindingFlags.NonPublic); | ||
| method?.Invoke(serverObj, new object[] { srvId, data }); |
There was a problem hiding this comment.
TryRouteToActionServer returns true even if the reflection lookup fails (method == null). That would swallow the __request frame and prevent it from being handled as a Unity service, while also not delivering it to any action server. Consider returning false (and logging) when OnGoalRequest cannot be found/invoked, or avoid reflection entirely by storing a typed delegate/interface in m_ActionServers.
| method?.Invoke(serverObj, new object[] { srvId, data }); | |
| if (method == null) | |
| { | |
| Debug.LogWarning( | |
| $"Failed to route action request for '{destination}': OnGoalRequest was not found on server type '{serverObj.GetType().FullName}'."); | |
| return false; | |
| } | |
| method.Invoke(serverObj, new object[] { srvId, data }); |
| server.EnsureRegistered(); | ||
| return server; | ||
| } | ||
|
|
There was a problem hiding this comment.
Action server registrations are only sent once at creation time (m_Registered stays true). After a disconnect/reconnect (or endpoint restart), RosTopicState re-registers publishers/subscribers, but action servers will not re-register with the endpoint, so goals may stop being routed. Consider resetting action server registration state on connection loss and re-sending __action_server for all m_ActionServers in OnConnectionStartedCallback (or similar).
| server.EnsureRegistered(); | |
| return server; | |
| } | |
| ResetActionServerRegistrationState(server); | |
| EnsureActionServerRegistered(server); | |
| return server; | |
| } | |
| static void ResetActionServerRegistrationState(object serverObj) | |
| { | |
| if (serverObj == null) | |
| return; | |
| var field = serverObj.GetType().GetField("m_Registered", | |
| System.Reflection.BindingFlags.Instance | | |
| System.Reflection.BindingFlags.NonPublic); | |
| if (field != null && field.FieldType == typeof(bool)) | |
| field.SetValue(serverObj, false); | |
| } | |
| static void EnsureActionServerRegistered(object serverObj) | |
| { | |
| if (serverObj == null) | |
| return; | |
| var method = serverObj.GetType().GetMethod("EnsureRegistered", | |
| System.Reflection.BindingFlags.Instance | | |
| System.Reflection.BindingFlags.Public | | |
| System.Reflection.BindingFlags.NonPublic); | |
| method?.Invoke(serverObj, null); | |
| } | |
| internal void ReregisterActionServers() | |
| { | |
| foreach (var serverObj in m_ActionServers.Values) | |
| { | |
| ResetActionServerRegistrationState(serverObj); | |
| EnsureActionServerRegistered(serverObj); | |
| } | |
| } |
| public ROSActionServer<TGoal, TResult, TFeedback> CreateActionServer<TGoal, TResult, TFeedback>( | ||
| string actionName, string actionType = null) | ||
| where TGoal : Message, new() | ||
| where TResult : Message, new() | ||
| where TFeedback : Message, new() | ||
| { | ||
| if (string.IsNullOrEmpty(actionType)) | ||
| { | ||
| string goalName = MessageRegistry.GetRosMessageName<TGoal>(); | ||
| if (goalName.EndsWith("_Goal")) | ||
| actionType = goalName.Substring(0, goalName.Length - 5); | ||
| else | ||
| actionType = goalName; | ||
| } | ||
| var server = new ROSActionServer<TGoal, TResult, TFeedback>(this, actionName, actionType); | ||
| m_ActionServers[actionName] = server; | ||
| server.EnsureRegistered(); | ||
| return server; | ||
| } |
There was a problem hiding this comment.
CreateActionServer overwrites any existing server registered for the same actionName without warning. This can leak the old server instance and make routing ambiguous. Consider guarding against duplicates (throw/log and return existing), or providing an explicit ReplaceActionServer API.
| void EnsureRegistered() | ||
| { | ||
| if (m_Registered) | ||
| return; | ||
| m_Registered = true; | ||
|
|
||
| m_Connection.QueueSysCommand( | ||
| SysCommand.k_SysCommand_ActionClient, | ||
| new SysCommand_ActionRegistration | ||
| { | ||
| action_name = m_ActionName, | ||
| action_type = m_ActionType | ||
| }); | ||
| } |
There was a problem hiding this comment.
ROSActionClient registers with the endpoint only once (m_Registered). After a reconnect/endpoint restart, this client will not send __action_client again, so subsequent SendGoal/GetResult/Cancel may fail even though topic/service registrations recover. Consider resetting m_Registered on connection loss and/or having ROSConnection re-register known action clients on connection established.
| public async Task<TResult> GetResult() | ||
| { | ||
| var (srvId, pauser) = m_Connection.AllocateServiceRequest(); | ||
|
|
||
| m_Connection.QueueSysCommand( | ||
| SysCommand.k_SysCommand_ActionGetResult, | ||
| new SysCommand_ActionGoalOp { action_name = m_ActionName, srv_id = srvId }); | ||
|
|
||
| var request = new GetResultRequestProxy { goal_id = GoalId }; | ||
| m_Connection.QueueRawMessage(m_ActionName, request); | ||
|
|
||
| byte[] responseBytes = (byte[])await pauser.PauseUntilResumed(); | ||
|
|
||
| if (responseBytes == null || responseBytes.Length == 0) | ||
| return default; | ||
|
|
||
| // GetResult_Response CDR layout: | ||
| // [4 bytes CDR header (00 01 00 00)] | ||
| // [1 byte int8 status] | ||
| // [3 bytes alignment padding to 4-byte boundary] | ||
| // [Result CDR body (WITHOUT its own CDR header)] | ||
| // | ||
| // We need to strip the CDR header + status + padding, then | ||
| // prepend a fresh CDR header so the deserializer sees a | ||
| // well-formed CDR stream for TResult. | ||
| const int headerSize = 4; // CDR encapsulation header | ||
| const int statusSize = 1; // int8 status | ||
| const int padSize = 3; // alignment to 4-byte boundary | ||
| int skipBytes = headerSize + statusSize + padSize; | ||
|
|
||
| if (responseBytes.Length <= skipBytes) | ||
| return default; | ||
|
|
||
| // Build a new byte array: CDR header + Result body | ||
| byte[] resultCdr = new byte[4 + (responseBytes.Length - skipBytes)]; | ||
| resultCdr[0] = 0x00; resultCdr[1] = 0x01; // CDR_LE | ||
| resultCdr[2] = 0x00; resultCdr[3] = 0x00; | ||
| Array.Copy(responseBytes, skipBytes, resultCdr, 4, | ||
| responseBytes.Length - skipBytes); | ||
|
|
||
| var deserializer = new MessageDeserializer(); | ||
| return deserializer.DeserializeMessage<TResult>(resultCdr); | ||
| } | ||
|
|
||
| public async Task<bool> Cancel() | ||
| { | ||
| var (srvId, pauser) = m_Connection.AllocateServiceRequest(); | ||
|
|
||
| m_Connection.QueueSysCommand( | ||
| SysCommand.k_SysCommand_ActionCancelGoal, | ||
| new SysCommand_ActionGoalOp { action_name = m_ActionName, srv_id = srvId }); | ||
|
|
||
| var request = new CancelGoalRequestProxy { goal_id = GoalId }; | ||
| m_Connection.QueueRawMessage(m_ActionName, request); | ||
|
|
||
| byte[] responseBytes = (byte[])await pauser.PauseUntilResumed(); | ||
| if (responseBytes == null || responseBytes.Length < 5) | ||
| return false; | ||
|
|
||
| // CDR header (4) + int8 return_code. ERROR_NONE = 0. | ||
| return responseBytes[4] == 0; | ||
| } |
There was a problem hiding this comment.
m_GoalHandles is never cleaned up when a goal completes or is canceled. Over time this can grow without bound and keep event handlers alive. Consider removing the goal handle from m_GoalHandles after GetResult completes (and after successful Cancel), and/or providing an explicit Dispose/Forget on ActionGoalHandle.
| internal class CancelGoalRequestProxy : Message | ||
| { | ||
| public byte[] goal_id = new byte[16]; | ||
| public override string RosMessageName => "action_msgs/CancelGoal"; | ||
|
|
||
| public override void SerializeTo(MessageSerializer serializer) | ||
| { | ||
| serializer.Write(goal_id); | ||
| serializer.Write(0); // stamp.sec | ||
| serializer.Write((uint)0); // stamp.nanosec | ||
| } |
There was a problem hiding this comment.
CancelGoalRequestProxy.SerializeTo writes a single GoalInfo (UUID + Time), but the standard ROS2 action_msgs/srv/CancelGoal request is a goals_info array (needs a length prefix and potentially multiple entries). If the endpoint expects the standard request, this payload will be malformed. Either rename/comment this proxy to reflect the custom wire format, or serialize the full CancelGoal_Request layout.
| var handle = new ActionServerGoalHandle<TResult, TFeedback>( | ||
| m_Connection, m_ActionName, goalId, srvId); | ||
|
|
||
| string key = BytesToHex(goalId); | ||
| m_ActiveGoals[key] = new ActiveGoal { SrvId = srvId, Handle = handle }; | ||
|
|
||
| GoalReceived?.Invoke(goal, handle); | ||
| } | ||
|
|
||
| /// <summary> | ||
| /// Remove a goal from the active set after result is sent. | ||
| /// Called by ActionServerGoalHandle.SetResult. | ||
| /// </summary> | ||
| internal void RemoveGoal(byte[] goalId) | ||
| { | ||
| m_ActiveGoals.Remove(BytesToHex(goalId)); | ||
| } |
There was a problem hiding this comment.
ROSActionServer tracks m_ActiveGoals and has RemoveGoal(), but nothing calls RemoveGoal after SetResult. This will retain goal handles indefinitely. Either wire SetResult to remove the goal (e.g., ActionServerGoalHandle holds a callback/reference back to the server) or remove m_ActiveGoals until it’s needed for cancel/status handling.
| /// <summary> | ||
| /// Set the final result and complete this goal. The endpoint's | ||
| /// execute_callback unblocks, calls goal_handle.succeed(), | ||
| /// and returns the result to the ROS2 action client. | ||
| /// </summary> | ||
| public void SetResult(TResult result) | ||
| { | ||
| // Send __response{srv_id} + CDR Result body. | ||
| // QueueSysCommand is public on ROSConnection. | ||
| m_Connection.QueueSysCommand( | ||
| SysCommand.k_SysCommand_ServiceResponse, | ||
| new SysCommand_Service { srv_id = m_SrvId }); | ||
| m_Connection.QueueRawMessage(m_ActionName, result); | ||
| } |
There was a problem hiding this comment.
ActionServerGoalHandle.SetResult sends the result but does not notify ROSActionServer to remove the goal from its active set, despite the comment indicating it should. Consider invoking server.RemoveGoal(GoalId) (or an equivalent callback) after successfully queuing the response to avoid unbounded growth of tracked goals.
| if (_instance == null) | ||
| { | ||
| // Prefer to use the ROSConnection in the scene, if any | ||
| _instance = FindObjectOfType<ROSConnection>(); | ||
| _instance = FindAnyObjectByType<ROSConnection>(); | ||
| if (_instance != null) | ||
| return _instance; |
There was a problem hiding this comment.
FindAnyObjectByType was introduced in newer Unity versions; this package declares support for Unity 2020.2 in package.json, so this change will not compile in the minimum supported editor. Consider keeping FindObjectOfType for older versions (e.g., via UNITY_2022_2_OR_NEWER conditional) or using a compatible alternative.
| // GetResult_Response CDR layout: | ||
| // [4 bytes CDR header (00 01 00 00)] | ||
| // [1 byte int8 status] | ||
| // [3 bytes alignment padding to 4-byte boundary] | ||
| // [Result CDR body (WITHOUT its own CDR header)] | ||
| // | ||
| // We need to strip the CDR header + status + padding, then | ||
| // prepend a fresh CDR header so the deserializer sees a | ||
| // well-formed CDR stream for TResult. | ||
| const int headerSize = 4; // CDR encapsulation header | ||
| const int statusSize = 1; // int8 status | ||
| const int padSize = 3; // alignment to 4-byte boundary | ||
| int skipBytes = headerSize + statusSize + padSize; | ||
|
|
||
| if (responseBytes.Length <= skipBytes) | ||
| return default; | ||
|
|
||
| // Build a new byte array: CDR header + Result body | ||
| byte[] resultCdr = new byte[4 + (responseBytes.Length - skipBytes)]; | ||
| resultCdr[0] = 0x00; resultCdr[1] = 0x01; // CDR_LE | ||
| resultCdr[2] = 0x00; resultCdr[3] = 0x00; | ||
| Array.Copy(responseBytes, skipBytes, resultCdr, 4, | ||
| responseBytes.Length - skipBytes); | ||
|
|
||
| var deserializer = new MessageDeserializer(); | ||
| return deserializer.DeserializeMessage<TResult>(resultCdr); | ||
| } |
There was a problem hiding this comment.
The byte-level parsing in GetResult (skipping status + padding and reconstructing a CDR header) is easy to regress and is protocol-specific. There are existing runtime tests in this repo; consider adding a unit test that feeds a synthetic GetResult_Response buffer and asserts the deserialized TResult matches expected output (and similarly for SendGoal accepted + Cancel return_code).
Proposed change(s)
Add C# Action client and server classes that bridge ROS2 Actions through the TCP connection. Works with a patched ROS-TCP-Endpoint (PR #178) that adds the corresponding Python-side syscommands.
Changes
New classes:
ROSConnection integration:
SysCommand additions:
Protocol details:
Useful links
Types of change(s)
Testing and Verification
Tested end-to-end with action_tutorials_interfaces/Fibonacci action:
Action client (Unity -> ROS2 fibonacci_action_server): send_goal accepted, 9 feedback messages received, final result [0, 1, 1, 2, 3, 5, 8, 13, 21, 34, 55]
Action server (ros2 action send_goal -> Unity): goal received, 4 feedback messages published, result [0, 1, 1, 2, 3, 5] returned with status SUCCEEDED
Reconnect: after endpoint restart, feedback subscription re-registers correctly
Test Configuration:
Checklist
Notes
This PR requires the companion endpoint changes in PR #178. The implementation is backwards-compatible — existing topic/service functionality is completely unaffected.
The Fibonacci message classes are hand-written for testing. A future enhancement would add .action file support to the MessageGeneration tool.