Fixed implementation of Async Unity services (#199)

Author: LaurieCheers-unity

com.unity.robotics.ros-tcp-connector/CHANGELOG.md

@@ -39,9 +39,15 @@ Upgrade the TestRosTcpConnector project to use Unity LTS version 2020.3.11f1
     [CameraInfo Generator](https://github.com/Unity-Technologies/ROS-TCP-Connector/issues/133)
   - Added API to create TransformMsg using local frame of a transform in Unity
 
-- Added an optional pooling system for ros publishers
-- Implemented a queueing and latching system to mimic the ROS implementation in Unity
-- Add support for visualizations
+  - Added an optional pooling system for ros publishers
+
+  - Implemented a queueing and latching system to mimic the ROS implementation in Unity
+
+  - Collected the various service/publisher/subscriber tables into a single table of RosTopicState
+
+  - Hud becomes a generic display platform to support visualizations
+
+  - Unity service implementations can be async
 
 ### Changed
 - Publishing a message to an unregistered topic will show an error.

com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSConnection.cs

@@ -234,8 +234,9 @@ void AddSubscriberInternal(string topic, string rosMessageName, Action<Message>
         }
 
         // Implement a service in Unity
-        public void ImplementService<TRequest>(string topic, Func<TRequest, Message> callback, int? queueSize = null)
+        public void ImplementService<TRequest, TResponse>(string topic, Func<TRequest, TResponse> callback, int? queueSize = null)
             where TRequest : Message
+            where TResponse : Message
         {
             string rosMessageName = rosMessageName = MessageRegistry.GetRosMessageName<TRequest>();
 
@@ -246,7 +247,29 @@ public void ImplementService<TRequest>(string topic, Func<TRequest, Message> cal
             }
 
             int resolvedQueueSize = queueSize.GetValueOrDefault(k_DefaultPublisherQueueSize);
-            info.ImplementService((Message msg) => callback((TRequest)msg), resolvedQueueSize);
+            info.ImplementService(callback, resolvedQueueSize);
+
+            foreach (Action<RosTopicState> topicCallback in m_NewTopicCallbacks)
+            {
+                topicCallback(info);
+            }
+        }
+
+        // Implement a service in Unity
+        public void ImplementService<TRequest, TResponse>(string topic, Func<TRequest, Task<TResponse>> callback, int? queueSize = null)
+            where TRequest : Message
+            where TResponse : Message
+        {
+            string rosMessageName = rosMessageName = MessageRegistry.GetRosMessageName<TRequest>();
+
+            RosTopicState info;
+            if (!m_Topics.TryGetValue(topic, out info))
+            {
+                info = AddTopic(topic, rosMessageName);
+            }
+
+            int resolvedQueueSize = queueSize.GetValueOrDefault(k_DefaultPublisherQueueSize);
+            info.ImplementService(callback, resolvedQueueSize);
 
             foreach (Action<RosTopicState> topicCallback in m_NewTopicCallbacks)
             {

com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/RosTopicState.cs

@@ -165,17 +165,28 @@ public void UnsubscribeAll()
             SentSubscriberRegistration = false;
         }
 
-        public void ImplementService(Func<Message, Message> implementation, int queueSize)
+        public void ImplementService<TRequest, TResponse>(Func<TRequest, TResponse> implementation, int queueSize)
+            where TRequest : Message
+            where TResponse : Message
         {
-            m_ServiceImplementation = implementation;
+            m_ServiceImplementation = (Message msg) =>
+            {
+                return implementation((TRequest)msg);
+            };
             m_ConnectionInternal.SendUnityServiceRegistration(m_Topic, m_RosMessageName);
             m_ServiceResponseTopic = new RosTopicState(m_Topic, m_RosMessageName, m_Connection, m_ConnectionInternal, MessageSubtopic.Response);
             CreateMessageSender(queueSize);
         }
 
-        public void ImplementService(Func<Message, Task<Message>> implementation, int queueSize)
+        public void ImplementService<TRequest, TResponse>(Func<TRequest, Task<TResponse>> implementation, int queueSize)
+            where TRequest : Message
+            where TResponse : Message
         {
-            m_ServiceImplementationAsync = implementation;
+            m_ServiceImplementationAsync = async (Message msg) =>
+            {
+                TResponse response = await implementation((TRequest)msg);
+                return response;
+            };
             m_ConnectionInternal.SendUnityServiceRegistration(m_Topic, m_RosMessageName);
             m_ServiceResponseTopic = new RosTopicState(m_Topic, m_RosMessageName, m_Connection, m_ConnectionInternal, MessageSubtopic.Response);
             CreateMessageSender(queueSize);