diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces.meta b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces.meta new file mode 100644 index 00000000..7bef1fc1 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 684de9439bfc12c409ea25c6fb7d611a +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces/action.meta b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces/action.meta new file mode 100644 index 00000000..59d87139 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces/action.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 5b55dcaf5b037944c9eedef3415b66cc +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces/action/FibonacciFeedback.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces/action/FibonacciFeedback.cs new file mode 100644 index 00000000..9eb8405f --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces/action/FibonacciFeedback.cs @@ -0,0 +1,47 @@ +using System; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; + +namespace RosMessageTypes.ActionTutorialsInterfaces +{ + [Serializable] + public class FibonacciFeedback : Message + { + public const string k_RosMessageName = "action_tutorials_interfaces/Fibonacci_Feedback"; + public override string RosMessageName => k_RosMessageName; + + public int[] partial_sequence; + + public FibonacciFeedback() + { + this.partial_sequence = new int[0]; + } + + public FibonacciFeedback(int[] partial_sequence) + { + this.partial_sequence = partial_sequence; + } + + public static FibonacciFeedback Deserialize(MessageDeserializer deserializer) => new FibonacciFeedback(deserializer); + + private FibonacciFeedback(MessageDeserializer deserializer) + { + deserializer.Read(out this.partial_sequence, sizeof(int), deserializer.ReadLength()); + } + + public override void SerializeTo(MessageSerializer serializer) + { + serializer.WriteLength(this.partial_sequence); + serializer.Write(this.partial_sequence); + } + +#if UNITY_EDITOR + [UnityEditor.InitializeOnLoadMethod] +#else + [UnityEngine.RuntimeInitializeOnLoadMethod] +#endif + public static void Register() + { + MessageRegistry.Register(k_RosMessageName, Deserialize); + } + } +} diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces/action/FibonacciFeedback.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces/action/FibonacciFeedback.cs.meta new file mode 100644 index 00000000..ffbcf710 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces/action/FibonacciFeedback.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 9b7aa11f45efedd449674f25b4140411 \ No newline at end of file diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces/action/FibonacciFeedbackMessage.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces/action/FibonacciFeedbackMessage.cs new file mode 100644 index 00000000..fda9a49a --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces/action/FibonacciFeedbackMessage.cs @@ -0,0 +1,55 @@ +using System; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; + +namespace RosMessageTypes.ActionTutorialsInterfaces +{ + /// + /// The wire-level FeedbackMessage for Fibonacci actions. + /// Layout: unique_identifier_msgs/UUID goal_id + Fibonacci_Feedback feedback. + /// + [Serializable] + public class FibonacciFeedbackMessage : Message + { + public const string k_RosMessageName = "action_tutorials_interfaces/Fibonacci_FeedbackMessage"; + // The endpoint may also report this type with /action/ in the path + // when it refreshes the topic list from the ROS2 graph. + public const string k_RosMessageNameAlt = "action_tutorials_interfaces/action/Fibonacci_FeedbackMessage"; + public override string RosMessageName => k_RosMessageName; + + public byte[] goal_id = new byte[16]; + public FibonacciFeedback feedback = new FibonacciFeedback(); + + public FibonacciFeedbackMessage() { } + + public static FibonacciFeedbackMessage Deserialize(MessageDeserializer deserializer) + => new FibonacciFeedbackMessage(deserializer); + + private FibonacciFeedbackMessage(MessageDeserializer deserializer) + { + // UUID is uint8[16] — 16 raw bytes, no length prefix. + goal_id = new byte[16]; + for (int i = 0; i < 16; i++) + deserializer.Read(out goal_id[i]); + + // Feedback body follows inline. + feedback = FibonacciFeedback.Deserialize(deserializer); + } + + public override void SerializeTo(MessageSerializer serializer) + { + serializer.Write(goal_id); + feedback.SerializeTo(serializer); + } + +#if UNITY_EDITOR + [UnityEditor.InitializeOnLoadMethod] +#else + [UnityEngine.RuntimeInitializeOnLoadMethod] +#endif + public static void Register() + { + MessageRegistry.Register(k_RosMessageName, Deserialize); + MessageRegistry.Register(k_RosMessageNameAlt, Deserialize); + } + } +} diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces/action/FibonacciFeedbackMessage.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces/action/FibonacciFeedbackMessage.cs.meta new file mode 100644 index 00000000..40ec98be --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces/action/FibonacciFeedbackMessage.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 74456a0e45846244d9ea4126e08f71e5 \ No newline at end of file diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces/action/FibonacciGoal.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces/action/FibonacciGoal.cs new file mode 100644 index 00000000..c598d830 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces/action/FibonacciGoal.cs @@ -0,0 +1,46 @@ +using System; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; + +namespace RosMessageTypes.ActionTutorialsInterfaces +{ + [Serializable] + public class FibonacciGoal : Message + { + public const string k_RosMessageName = "action_tutorials_interfaces/Fibonacci_Goal"; + public override string RosMessageName => k_RosMessageName; + + public int order; + + public FibonacciGoal() + { + this.order = 0; + } + + public FibonacciGoal(int order) + { + this.order = order; + } + + public static FibonacciGoal Deserialize(MessageDeserializer deserializer) => new FibonacciGoal(deserializer); + + private FibonacciGoal(MessageDeserializer deserializer) + { + deserializer.Read(out this.order); + } + + public override void SerializeTo(MessageSerializer serializer) + { + serializer.Write(this.order); + } + +#if UNITY_EDITOR + [UnityEditor.InitializeOnLoadMethod] +#else + [UnityEngine.RuntimeInitializeOnLoadMethod] +#endif + public static void Register() + { + MessageRegistry.Register(k_RosMessageName, Deserialize); + } + } +} diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces/action/FibonacciGoal.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces/action/FibonacciGoal.cs.meta new file mode 100644 index 00000000..c9f1ecc2 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces/action/FibonacciGoal.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 887590542df5ade42bd75dbe0eb0b9e6 \ No newline at end of file diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces/action/FibonacciResult.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces/action/FibonacciResult.cs new file mode 100644 index 00000000..259d8745 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces/action/FibonacciResult.cs @@ -0,0 +1,47 @@ +using System; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; + +namespace RosMessageTypes.ActionTutorialsInterfaces +{ + [Serializable] + public class FibonacciResult : Message + { + public const string k_RosMessageName = "action_tutorials_interfaces/Fibonacci_Result"; + public override string RosMessageName => k_RosMessageName; + + public int[] sequence; + + public FibonacciResult() + { + this.sequence = new int[0]; + } + + public FibonacciResult(int[] sequence) + { + this.sequence = sequence; + } + + public static FibonacciResult Deserialize(MessageDeserializer deserializer) => new FibonacciResult(deserializer); + + private FibonacciResult(MessageDeserializer deserializer) + { + deserializer.Read(out this.sequence, sizeof(int), deserializer.ReadLength()); + } + + public override void SerializeTo(MessageSerializer serializer) + { + serializer.WriteLength(this.sequence); + serializer.Write(this.sequence); + } + +#if UNITY_EDITOR + [UnityEditor.InitializeOnLoadMethod] +#else + [UnityEngine.RuntimeInitializeOnLoadMethod] +#endif + public static void Register() + { + MessageRegistry.Register(k_RosMessageName, Deserialize); + } + } +} diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces/action/FibonacciResult.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces/action/FibonacciResult.cs.meta new file mode 100644 index 00000000..c9f1deb1 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ActionTutorialsInterfaces/action/FibonacciResult.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 90561f1ab67d0e146abaf2dd8d15e4a2 \ No newline at end of file diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSActionClient.cs b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSActionClient.cs new file mode 100644 index 00000000..119a355f --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSActionClient.cs @@ -0,0 +1,247 @@ +using System; +using System.Collections.Generic; +using System.Threading.Tasks; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using UnityEngine; + +namespace Unity.Robotics.ROSTCPConnector +{ + /// + /// Client-side handle for a ROS2 Action. Sends goals, receives feedback, + /// awaits results, and cancels goals through the patched ROS-TCP-Endpoint. + /// Requires the comoc/ROS-TCP-Endpoint fork (main-ros2 branch). + /// + public class ROSActionClient + where TGoal : Message, new() + where TResult : Message, new() + where TFeedback : Message, new() + { + readonly ROSConnection m_Connection; + readonly string m_ActionName; + readonly string m_ActionType; + bool m_Registered; + + readonly Dictionary> m_GoalHandles = + new Dictionary>(); + + public ROSActionClient(ROSConnection connection, string actionName, string actionType) + { + m_Connection = connection; + m_ActionName = actionName; + m_ActionType = actionType; + } + + public async Task> SendGoal(TGoal goal) + { + EnsureRegistered(); + + var (srvId, pauser) = m_Connection.AllocateServiceRequest(); + + m_Connection.QueueSysCommand( + SysCommand.k_SysCommand_ActionSendGoal, + new SysCommand_ActionGoalOp { action_name = m_ActionName, srv_id = srvId }); + m_Connection.QueueRawMessage(m_ActionName, goal); + + byte[] responseBytes = (byte[])await pauser.PauseUntilResumed(); + + if (responseBytes == null || responseBytes.Length < 16) + { + Debug.LogWarning($"ROSActionClient: invalid send_goal response for {m_ActionName}"); + return null; + } + + byte[] goalId = new byte[16]; + Array.Copy(responseBytes, 0, goalId, 0, 16); + + // CDR header (4 bytes) + bool accepted (1 byte) starts at offset 16 + byte[] cdrResponse = new byte[responseBytes.Length - 16]; + Array.Copy(responseBytes, 16, cdrResponse, 0, cdrResponse.Length); + bool accepted = cdrResponse.Length > 4 && cdrResponse[4] != 0; + + var handle = new ActionGoalHandle( + m_Connection, m_ActionName, goalId, accepted); + + if (accepted) + m_GoalHandles[BytesToHex(goalId)] = handle; + + return handle; + } + + internal void OnFeedbackReceived(byte[] goalIdBytes, TFeedback feedback) + { + string key = BytesToHex(goalIdBytes); + if (m_GoalHandles.TryGetValue(key, out var handle)) + handle.InvokeFeedback(feedback); + } + + 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 + }); + } + + /// + /// Subscribe to the feedback topic using a FeedbackMessage type that + /// contains both goal_id (byte[16]) and feedback (TFeedback). + /// Call this after creating the client to enable FeedbackReceived + /// events on goal handles. + /// + /// Example: + /// client.RegisterFeedbackSubscription<FibonacciFeedbackMessage>( + /// msg => msg.goal_id, msg => msg.feedback); + /// + public void RegisterFeedbackSubscription( + Func getGoalId, + Func getFeedback) + where TFeedbackMessage : Message, new() + { + string feedbackTopic = m_ActionName + "/_action/feedback"; + // Use the short type name (without /action/) for the __subscribe + // syscommand — this is what the endpoint uses to resolve the type. + // The endpoint sends feedback with this short name, but __topic_list + // reports the full /action/ path, causing a type mismatch in + // Subscribe. Using SubscribeByMessageName with the short name + // avoids the generic type check. + string feedbackTypeName = m_ActionType + "_FeedbackMessage"; + m_Connection.SubscribeByMessageName(feedbackTopic, feedbackTypeName, (Message msg) => + { + if (msg is TFeedbackMessage typedMsg) + { + byte[] goalId = getGoalId(typedMsg); + TFeedback feedback = getFeedback(typedMsg); + OnFeedbackReceived(goalId, feedback); + } + }); + } + + static string BytesToHex(byte[] bytes) + { + return BitConverter.ToString(bytes).Replace("-", "").ToLowerInvariant(); + } + } + + /// + /// Handle to a single in-flight action goal. + /// + public class ActionGoalHandle + where TResult : Message, new() + where TFeedback : Message, new() + { + readonly ROSConnection m_Connection; + readonly string m_ActionName; + + public byte[] GoalId { get; } + public bool Accepted { get; } + public event Action FeedbackReceived; + + internal ActionGoalHandle(ROSConnection connection, string actionName, + byte[] goalId, bool accepted) + { + m_Connection = connection; + m_ActionName = actionName; + GoalId = goalId; + Accepted = accepted; + } + + public async Task 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(resultCdr); + } + + public async Task 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; + } + + internal void InvokeFeedback(TFeedback feedback) + { + FeedbackReceived?.Invoke(feedback); + } + } + + internal class GetResultRequestProxy : Message + { + public byte[] goal_id = new byte[16]; + public override string RosMessageName => ""; + + public override void SerializeTo(MessageSerializer serializer) + { + serializer.Write(goal_id); + } + } + + 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 + } + } +} diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSActionClient.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSActionClient.cs.meta new file mode 100644 index 00000000..5d990b70 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSActionClient.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 75fe46779a0a2bf448942ff4c203479d \ No newline at end of file diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSActionServer.cs b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSActionServer.cs new file mode 100644 index 00000000..450a17af --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSActionServer.cs @@ -0,0 +1,172 @@ +using System; +using System.Collections.Generic; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using UnityEngine; + +namespace Unity.Robotics.ROSTCPConnector +{ + /// + /// Server-side handle for a ROS2 Action implemented in Unity. + /// When a ROS2 action client sends a goal, the endpoint forwards it + /// to Unity via __request/__response. Unity processes the goal, + /// publishes feedback, and sends the result back. + /// + /// Requires the comoc/ROS-TCP-Endpoint fork (main-ros2 branch). + /// + public class ROSActionServer + where TGoal : Message, new() + where TResult : Message, new() + where TFeedback : Message, new() + { + readonly ROSConnection m_Connection; + readonly string m_ActionName; + readonly string m_ActionType; + bool m_Registered; + + /// + /// Fired when a ROS2 action client sends a goal. The handler + /// receives the goal and a handle to publish feedback and set + /// the result. + /// + public event Action> GoalReceived; + + // Active goals keyed by hex UUID. + readonly Dictionary m_ActiveGoals = + new Dictionary(); + + struct ActiveGoal + { + public int SrvId; + public ActionServerGoalHandle Handle; + } + + public ROSActionServer(ROSConnection connection, string actionName, string actionType) + { + m_Connection = connection; + m_ActionName = actionName; + m_ActionType = actionType; + } + + /// + /// Register the action server with the endpoint. Called + /// automatically by ROSConnection.CreateActionServer, but can + /// also be called manually if needed. + /// + public void EnsureRegistered() + { + if (m_Registered) + return; + m_Registered = true; + + m_Connection.QueueSysCommand( + SysCommand.k_SysCommand_ActionServer, + new SysCommand_ActionRegistration + { + action_name = m_ActionName, + action_type = m_ActionType + }); + } + + /// + /// Called by ROSConnection when a __request arrives whose + /// destination matches this action's name. The payload is + /// 16-byte UUID + CDR Goal body. + /// + internal void OnGoalRequest(int srvId, byte[] payload) + { + if (payload == null || payload.Length < 16) + { + Debug.LogWarning($"ROSActionServer: invalid goal payload for {m_ActionName}"); + return; + } + + byte[] goalId = new byte[16]; + Array.Copy(payload, 0, goalId, 0, 16); + + byte[] goalCdr = new byte[payload.Length - 16]; + Array.Copy(payload, 16, goalCdr, 0, goalCdr.Length); + + // Deserialize the Goal body. + var deserializer = new MessageDeserializer(); + TGoal goal = deserializer.DeserializeMessage(goalCdr); + + var handle = new ActionServerGoalHandle( + m_Connection, m_ActionName, goalId, srvId); + + string key = BytesToHex(goalId); + m_ActiveGoals[key] = new ActiveGoal { SrvId = srvId, Handle = handle }; + + GoalReceived?.Invoke(goal, handle); + } + + /// + /// Remove a goal from the active set after result is sent. + /// Called by ActionServerGoalHandle.SetResult. + /// + internal void RemoveGoal(byte[] goalId) + { + m_ActiveGoals.Remove(BytesToHex(goalId)); + } + + static string BytesToHex(byte[] bytes) + { + return BitConverter.ToString(bytes).Replace("-", "").ToLowerInvariant(); + } + } + + /// + /// Handle for a single goal being executed by Unity. + /// + public class ActionServerGoalHandle + where TResult : Message, new() + where TFeedback : Message, new() + { + readonly ROSConnection m_Connection; + readonly string m_ActionName; + readonly int m_SrvId; + + public byte[] GoalId { get; } + + internal ActionServerGoalHandle(ROSConnection connection, string actionName, + byte[] goalId, int srvId) + { + m_Connection = connection; + m_ActionName = actionName; + GoalId = goalId; + m_SrvId = srvId; + } + + /// + /// Publish feedback for this goal. The endpoint forwards it + /// to the ROS2 action client via goal_handle.publish_feedback(). + /// + public void PublishFeedback(TFeedback feedback) + { + string uuidHex = BitConverter.ToString(GoalId).Replace("-", "").ToLowerInvariant(); + + m_Connection.QueueSysCommand( + SysCommand.k_SysCommand_ActionPublishFeedback, + new SysCommand_ActionFeedback + { + action_name = m_ActionName, + goal_uuid_hex = uuidHex + }); + m_Connection.QueueRawMessage(m_ActionName, feedback); + } + + /// + /// 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. + /// + 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); + } + } +} diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSActionServer.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSActionServer.cs.meta new file mode 100644 index 00000000..a0cf6584 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSActionServer.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 4f46d6d822a77cd47a31fb41dc34b3fd \ No newline at end of file diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSConnection.cs b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSConnection.cs index 8031478a..bad9b281 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSConnection.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSConnection.cs @@ -105,6 +105,39 @@ public bool TryDequeue(out OutgoingMessageSender outgoingMessageSender) int m_NextSrvID = 101; Dictionary m_ServicesWaiting = new Dictionary(); + /// + /// Allocate a service request ID and create a TaskPauser that will be + /// resumed when the matching __response arrives. Used internally by + /// ROSActionClient and ROSActionServer to share the existing service + /// request/response infrastructure for action calls. + /// + internal (int srvId, TaskPauser pauser) AllocateServiceRequest() + { + var pauser = new TaskPauser(); + int srvId; + lock (m_ServiceRequestLock) + { + srvId = m_NextSrvID++; + m_ServicesWaiting.Add(srvId, pauser); + } + return (srvId, pauser); + } + + /// + /// Serialize a Message as a raw data frame (destination + CDR payload) + /// and enqueue it. Used by ROSActionClient/Server to send the data + /// frame that follows an action syscommand, without triggering + /// Publish()'s publisher-registration check. + /// + internal void QueueRawMessage(string destination, Message message) + { + m_MessageSerializer.Clear(); + m_MessageSerializer.Write(destination); + m_MessageSerializer.SerializeMessageWithLength(message); + m_OutgoingMessageQueue.Enqueue( + new SysCommandSender(m_MessageSerializer.GetBytesSequence())); + } + public bool listenForTFMessages = true; float m_LastMessageReceivedRealtime; @@ -319,6 +352,77 @@ public void ImplementService(string topic, Func m_ActionServers = new Dictionary(); + + /// + /// Create an Action client for calling a ROS2 action server. + /// Requires the patched comoc/ROS-TCP-Endpoint (main-ros2 branch). + /// + public ROSActionClient CreateActionClient( + string actionName, string actionType = null) + where TGoal : Message, new() + where TResult : Message, new() + where TFeedback : Message, new() + { + if (string.IsNullOrEmpty(actionType)) + { + // Derive from TGoal's RosMessageName by stripping the _Goal suffix + // e.g. "example_interfaces/Fibonacci_Goal" -> "example_interfaces/Fibonacci" + string goalName = MessageRegistry.GetRosMessageName(); + if (goalName.EndsWith("_Goal")) + actionType = goalName.Substring(0, goalName.Length - 5); + else + actionType = goalName; + } + return new ROSActionClient(this, actionName, actionType); + } + + /// + /// Create an Action server implemented in Unity. + /// Requires the patched comoc/ROS-TCP-Endpoint (main-ros2 branch). + /// + public ROSActionServer CreateActionServer( + 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(); + if (goalName.EndsWith("_Goal")) + actionType = goalName.Substring(0, goalName.Length - 5); + else + actionType = goalName; + } + var server = new ROSActionServer(this, actionName, actionType); + m_ActionServers[actionName] = server; + server.EnsureRegistered(); + return server; + } + + /// + /// Route a __request payload to an action server if the destination + /// matches a registered action. Returns true if handled. + /// Called from the syscommand dispatcher. + /// + internal bool TryRouteToActionServer(int srvId, string destination, byte[] data) + { + if (m_ActionServers.TryGetValue(destination, out var serverObj)) + { + // serverObj is ROSActionServer<,,> but we stored it as object. + // 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 }); + return true; + } + return false; + } + public void GetTopicList(Action callback) { m_TopicsListCallbacks.Add(callback); @@ -443,7 +547,7 @@ public static ROSConnection GetOrCreateInstance() if (_instance == null) { // Prefer to use the ROSConnection in the scene, if any - _instance = FindObjectOfType(); + _instance = FindAnyObjectByType(); if (_instance != null) return _instance; @@ -684,11 +788,17 @@ void ReceiveSysCommand(string topic, string json) { var serviceCommand = JsonUtility.FromJson(json); - // the next incoming message will be a request for a Unity service, so set a special callback to process it + // the next incoming message will be a request for a Unity service + // (or an action goal from the endpoint's RosActionServer), + // so set a special callback to process it m_SpecialIncomingMessageHandler = (string serviceTopic, byte[] requestBytes) => { m_SpecialIncomingMessageHandler = null; + // Try action servers first (they use the same __request mechanism). + if (TryRouteToActionServer(serviceCommand.srv_id, serviceTopic, requestBytes)) + return; + RosTopicState topicState = GetTopic(serviceTopic); if (topicState == null) { diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/SysCommand.cs b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/SysCommand.cs index c7ee7d1d..3b2cb46e 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/SysCommand.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/SysCommand.cs @@ -23,6 +23,15 @@ public abstract class SysCommand public const string k_SysCommand_RemoveRosService = "__remove_ros_service"; public const string k_SysCommand_RemoveUnityService = "__remove_unity_service"; + // Action support - requires patched ROS-TCP-Endpoint + // (comoc/ROS-TCP-Endpoint, main-ros2 branch) + public const string k_SysCommand_ActionClient = "__action_client"; + public const string k_SysCommand_ActionSendGoal = "__action_send_goal"; + public const string k_SysCommand_ActionGetResult = "__action_get_result"; + public const string k_SysCommand_ActionCancelGoal = "__action_cancel_goal"; + public const string k_SysCommand_ActionServer = "__action_server"; + public const string k_SysCommand_ActionPublishFeedback = "__action_publish_feedback"; + public abstract string Command { get; @@ -111,4 +120,23 @@ public struct SysCommand_PublisherRegistration public int queue_size; public bool latch; } + + // Action support structures + public struct SysCommand_ActionRegistration + { + public string action_name; + public string action_type; + } + + public struct SysCommand_ActionGoalOp + { + public string action_name; + public int srv_id; + } + + public struct SysCommand_ActionFeedback + { + public string action_name; + public string goal_uuid_hex; + } }