Unity service example (#199)

hyounesy · web-flow · commit 7c4708d72c80 · 2021-04-14T18:35:30.000-07:00
* added example of implementing a UnityService

* adding tutorial for unity service

* added a couple comments to the example code

* typos and code standard fixes

* consistency: changing the service name for position_service to pos_srv

* updating the tutorial to match the corresponding script
diff --git a/tutorials/ros_packages/robotics_demo/scripts/object_pose_client.py b/tutorials/ros_packages/robotics_demo/scripts/object_pose_client.py
@@ -0,0 +1,32 @@
+#!/usr/bin/env python
+
+from __future__ import print_function
+
+import random
+import rospy
+import sys
+
+from robotics_demo.srv import ObjectPoseService, ObjectPoseServiceResponse
+
+
+def get_object_pose_client(name):
+    rospy.wait_for_service('obj_pose_srv')
+    try:
+        get_obj_pose = rospy.ServiceProxy('obj_pose_srv', ObjectPoseService)
+        obj_pose_resp = get_obj_pose(name)
+        return obj_pose_resp.object_pose
+    except rospy.ServiceException as e:
+        print("Service call failed: %s"%e)
+
+
+def usage():
+    return "%s [object_name]"%sys.argv[0]
+
+if __name__ == "__main__":
+    if len(sys.argv) == 2:
+        name = str(sys.argv[1])
+    else:
+        print(usage())
+        sys.exit(1)
+    print("Requesting pose for %s"%(name))
+    print("Pose for %s: %s"%(name, get_object_pose_client(name)))
diff --git a/tutorials/ros_packages/robotics_demo/scripts/position_service.py b/tutorials/ros_packages/robotics_demo/scripts/position_service.py
@@ -18,7 +18,7 @@ def new_position(req):
 
 def translate_position_server():
     rospy.init_node('position_server')
-    s = rospy.Service('position_service', PositionService, new_position)
+    s = rospy.Service('pos_srv', PositionService, new_position)
     print("Ready to move cubes!")
     rospy.spin()
 
diff --git a/tutorials/ros_packages/robotics_demo/scripts/server_endpoint.py b/tutorials/ros_packages/robotics_demo/scripts/server_endpoint.py
@@ -2,9 +2,9 @@
 
 import rospy
 
-from ros_tcp_endpoint import TcpServer, RosPublisher, RosSubscriber, RosService
+from ros_tcp_endpoint import TcpServer, RosPublisher, RosSubscriber, RosService, UnityService
 from robotics_demo.msg import PosRot, UnityColor
-from robotics_demo.srv import PositionService
+from robotics_demo.srv import PositionService, ObjectPoseService
 
 def main():
     ros_node_name = rospy.get_param("/TCP_NODE_NAME", 'TCPServer')
@@ -14,9 +14,10 @@ def main():
     rospy.init_node(ros_node_name, anonymous=True)
     
     tcp_server.start({
-        'pos_srv': RosService('position_service', PositionService),
         'pos_rot': RosPublisher('pos_rot', PosRot, queue_size=10),
-        'color': RosSubscriber('color', UnityColor, tcp_server)
+        'color': RosSubscriber('color', UnityColor, tcp_server),
+        'pos_srv': RosService('pos_srv', PositionService),
+        'obj_pose_srv': UnityService('obj_pose_srv', ObjectPoseService, tcp_server),
     })
     
     rospy.spin()
diff --git a/tutorials/ros_packages/robotics_demo/srv/ObjectPoseService.srv b/tutorials/ros_packages/robotics_demo/srv/ObjectPoseService.srv
@@ -0,0 +1,3 @@
+string object_name
+---
+geometry_msgs/Pose object_pose
diff --git a/tutorials/ros_unity_integration/README.md b/tutorials/ros_unity_integration/README.md
@@ -17,6 +17,7 @@ The `ROSConnection` plugin (also from [ROS TCP Connector](https://github.com/Uni
 - [ROS–Unity Integration: Publisher](publisher.md) - Adding a Publisher to a Unity Scene
 - [ROS–Unity Integration: Subscriber](subscriber.md) - Adding a Subscriber to a Unity Scene
 - [ROS–Unity Integration: Service](service.md) - Adding a Service call to a Unity Scene
+- [ROS–Unity Integration: UnityService](unity_service.md) - Adding a Service that runs in a Unity Scene
 - [ROS–Unity Integration: Server Endpoint](server_endpoint.md) - How to write a Server Endpoint
 
 ## Example Unity Scripts
@@ -26,8 +27,12 @@ Example scripts implemented in tutorials:
 - `unity_scripts/RosPublisherExample.cs`
 	- Publishes the position of a GameObject every 0.5 seconds.
 
+- `unity_scripts/RosSubscriberExample.cs`
+	- Subscribes to a topic that accepts color messages and uses them to change the color of a GameObject in the Unity scene.
+
 - `unity_scripts/RosServiceExample.cs`
 	- Returns a destination position for a GameObject to move towards each time the service is called.
 
-- `unity_scripts/RosSubscriberExample.cs`
-	- Subscribes to a topic that accepts color messages and uses them to change the color of a GameObject in the Unity scene.
+- `unity_scripts/RosUnityServiceExample.cs`
+	- Runs a service in the Unity scene that takes a GameObject's name and responds with the Pose of that object.
+
diff --git a/tutorials/ros_unity_integration/images/generate_messages_1.png b/tutorials/ros_unity_integration/images/generate_messages_1.png
diff --git a/tutorials/ros_unity_integration/images/generate_messages_2.png b/tutorials/ros_unity_integration/images/generate_messages_2.png
diff --git a/tutorials/ros_unity_integration/server_endpoint.md b/tutorials/ros_unity_integration/server_endpoint.md
@@ -14,9 +14,9 @@ The following is an example of a server endpoint Python script that:
 
 import rospy
 
-from ros_tcp_endpoint import TcpServer, RosPublisher, RosSubscriber, RosService
+from ros_tcp_endpoint import TcpServer, RosPublisher, RosSubscriber, RosService, UnityService
 from robotics_demo.msg import PosRot, UnityColor
-from robotics_demo.srv import PositionService
+from robotics_demo.srv import PositionService, ObjectPoseService
 
 def main():
     ros_node_name = rospy.get_param("/TCP_NODE_NAME", 'TCPServer')
@@ -26,9 +26,10 @@ def main():
     rospy.init_node(ros_node_name, anonymous=True)
     
     tcp_server.start({
-        'pos_srv': RosService('position_service', PositionService),
         'pos_rot': RosPublisher('pos_rot', PosRot, queue_size=10),
-        'color': RosSubscriber('color', UnityColor, tcp_server)
+        'color': RosSubscriber('color', UnityColor, tcp_server),
+        'pos_srv': RosService('pos_srv', PositionService),
+        'obj_pose_srv': UnityService('obj_pose_srv', ObjectPoseService, tcp_server),
     })
     
     rospy.spin()
@@ -41,9 +42,9 @@ if __name__ == "__main__":
 
 ## Import Statements for Services and Messages
 ```python
-from ros_tcp_endpoint import TcpServer, RosPublisher, RosSubscriber, RosService
+from ros_tcp_endpoint import TcpServer, RosPublisher, RosSubscriber, RosService, UnityService
 from robotics_demo.msg import PosRot, UnityColor
-from robotics_demo.srv import PositionService
+from robotics_demo.srv import PositionService, ObjectPoseService
 ```
 
 ## Creating the Server
@@ -68,9 +69,10 @@ The `ros_node_name` argument is required and the `buffer_size` and `connections`
 
 ```python
     tcp_server.start({
-        'pos_srv': RosService('position_service', PositionService),
         'pos_rot': RosPublisher('pos_rot', PosRot, queue_size=10),
-        'color': RosSubscriber('color', UnityColor, tcp_server)
+        'color': RosSubscriber('color', UnityColor, tcp_server),
+        'pos_srv': RosService('pos_srv', PositionService),
+        'obj_pose_srv': UnityService('obj_pose_srv', ObjectPoseService, tcp_server),
     })
     
     rospy.spin()
@@ -104,7 +106,7 @@ A ROS Service is similar to a RosPublisher, in that a Unity component sends a Re
 - Service name
 - ROS Service class generated from running `catkin_make` command
 
-`RosService('position_service', PositionService)`
+`RosService('pos_srv', PositionService)`
 
 ## Unity Service
 
@@ -114,7 +116,7 @@ A Unity Service is similar to a RosSubscriber, in that a Unity component receive
 - ROS Service class generated from running `catkin_make` command
 - The tcp server that will connect to Unity
 
-`UnityService('unity_service', PositionService, tcp_server)`
+`UnityService('obj_pose_srv', ObjectPoseService, tcp_server)`
 
 
 ## Parameters
diff --git a/tutorials/ros_unity_integration/service.md b/tutorials/ros_unity_integration/service.md
@@ -29,7 +29,7 @@ Once the server_endpoint has started, it will print something similar to `[INFO]
 
 ## Setting Up Unity Scene
 - Generate the C# code for `PositionService`'s messages by going to `Robotics` -> `Generate ROS Messages...`
-- Set the input file path to `PATH/TO/Unity-Robotics-Hub/tutorials/ros_packages/robotics_demo`, expand the robotics_demo folder and click `Build 1 srv`.
+- Set the input file path to `PATH/TO/Unity-Robotics-Hub/tutorials/ros_packages/robotics_demo`, expand the robotics_demo folder and click `Build 2 srvs`.
 
 ![](images/generate_messages_2.png)
 
diff --git a/tutorials/ros_unity_integration/unity_scripts/RosUnityServiceExample.cs b/tutorials/ros_unity_integration/unity_scripts/RosUnityServiceExample.cs
@@ -0,0 +1,43 @@
+using RosMessageTypes.RoboticsDemo;
+using UnityEngine;
+using Unity.Robotics.ROSTCPConnector;
+using Unity.Robotics.ROSTCPConnector.ROSGeometry;
+
+/// <summary>
+/// Example demonstration of implementing a UnityService that receives a Request message from another ROS node and sends a Response back
+/// </summary>
+public class RosUnityServiceExample : MonoBehaviour
+{
+    [SerializeField]
+    string m_ServiceName = "obj_pose_srv";
+
+    void Start()
+    {
+        // register the service with ROS
+        ROSConnection.instance.ImplementService<MObjectPoseServiceRequest>(m_ServiceName, GetObjectPose);
+    }
+
+    /// <summary>
+    ///  Callback to respond to the request
+    /// </summary>
+    /// <param name="request">service request containing the object name</param>
+    /// <returns>service response containing the object pose (or 0 if object not found)</returns>
+    private MObjectPoseServiceResponse GetObjectPose(MObjectPoseServiceRequest request)
+    {
+        // process the service request
+        Debug.Log("Received request for object: " + request.object_name);
+
+        // prepare a response
+        MObjectPoseServiceResponse objectPoseResponse = new MObjectPoseServiceResponse();
+        // Find a game object with the requested name
+        GameObject gameObject = GameObject.Find(request.object_name);
+        if (gameObject) 
+        {
+            // Fill-in the response with the object pose converted from Unity coordinate to ROS coordinate system
+            objectPoseResponse.object_pose.position = gameObject.transform.position.To<FLU>();
+            objectPoseResponse.object_pose.orientation = gameObject.transform.rotation.To<FLU>();
+        }
+       
+        return objectPoseResponse;
+    }
+}
diff --git a/tutorials/ros_unity_integration/unity_service.md b/tutorials/ros_unity_integration/unity_service.md
@@ -0,0 +1,126 @@
+# ROS–Unity Integration: UnityService
+
+Create a simple Unity scene which create a [Service](http://wiki.ros.org/Services) in Unity that takes a request with a GameObject's name and responds with the GameObject's pose (position and orientation) in the ROS coordinate system.
+
+## Setting Up ROS
+
+(Skip to [Setting Up the Unity Scene](unity_service.md#setting-up-the-unity-scene) if you already did the [ROS–Unity Integration Publisher](publisher.md) or [Subscriber](subscriber.md) tutorials.)
+
+- Copy the `tutorials/ros_packages/robotics_demo` folder of this repo into the `src` folder in your Catkin workspace.
+
+- Follow the [ROS–Unity Initial Setup](setup.md) guide.
+
+- Open a new terminal window, navigate to your ROS workspace, and run the following commands:
+  
+   ```bash
+    source devel/setup.bash
+    rosrun robotics_demo server_endpoint.py
+   ```
+
+Once the server_endpoint has started, it will print something similar to `[INFO] [1603488341.950794]: Starting server on 192.168.50.149:10000`.
+
+## Setting Up the Unity Scene
+- Generate the C# code for `ObjectPoseService`'s messages by going to `Robotics` -> `Generate ROS Messages...`
+ - Set the input file path to `PATH/TO/Unity-Robotics-Hub/tutorials/ros_packages/robotics_demo`, expand the robotics_demo folder and click `Build 2 srvs` (Note that you may skip this step if you have already done it in the previous tutorial).
+
+ ![](images/generate_messages_2.png)
+
+ - The generated files will be saved in the default directory `Assets/RosMessages/RoboticsDemo/srv`.
+ 
+- Create a new C# script and name it `RosUnityServiceExample.cs`
+- Paste the following code into `RosUnityServiceExample.cs`
+    - **Note:** This script can be found at `tutorials/ros_unity_integration/unity_scripts`.
+
+```csharp
+using RosMessageTypes.RoboticsDemo;
+using UnityEngine;
+using Unity.Robotics.ROSTCPConnector;
+using Unity.Robotics.ROSTCPConnector.ROSGeometry;
+
+/// <summary>
+/// Example demonstration of implementing a UnityService that receives a Request message from another ROS node and sends a Response back
+/// </summary>
+public class RosUnityServiceExample : MonoBehaviour
+{
+    [SerializeField]
+    string m_ServiceName = "obj_pose_srv";
+
+    void Start()
+    {
+        // register the service with ROS
+        ROSConnection.instance.ImplementService<MObjectPoseServiceRequest>(m_ServiceName, GetObjectPose);
+    }
+
+    /// <summary>
+    ///  Callback to respond to the request
+    /// </summary>
+    /// <param name="request">service request containing the object name</param>
+    /// <returns>service response containing the object pose (or 0 if object not found)</returns>
+    private MObjectPoseServiceResponse GetObjectPose(MObjectPoseServiceRequest request)
+    {
+        // process the service request
+        Debug.Log("Received request for object: " + request.object_name);
+
+        // prepare a response
+        MObjectPoseServiceResponse objectPoseResponse = new MObjectPoseServiceResponse();
+        // Find a game object with the requested name
+        GameObject gameObject = GameObject.Find(request.object_name);
+        if (gameObject) 
+        {
+            // Fill-in the response with the object pose converted from Unity coordinate to ROS coordinate system
+            objectPoseResponse.object_pose.position = gameObject.transform.position.To<FLU>();
+            objectPoseResponse.object_pose.orientation = gameObject.transform.rotation.To<FLU>();
+        }
+       
+        return objectPoseResponse;
+    }
+}
+```
+
+- From the main menu bar, open `Robotics/ROS Settings`, and change the `ROS IP Address` variable to the ROS IP.
+- Create an empty GameObject and name it `UnityService`.
+- Attach the `RosUnityServiceExample` script to the `UnityService` GameObject. 
+- Pressing play in the Editor should start running as a ROS node, waiting to accept ObjectPose requests. Once a connection to ROS has been established, a message will be printed on the ROS terminal similar to `ROS-Unity Handshake received, will connect to 192.168.50.130:5005`.
+
+
+## Start the Client
+- On your ROS system, open a new terminal window, navigate to your ROS workspace, and run the following commands:
+
+   ```bash
+    source devel/setup.bash
+    rosrun robotics_demo object_pose_client.py Cube
+   ```
+- This wil print an output similar to the following with the current pose information of the game object (note that the coordinates are converted to the ROS coordinate system in our Unity Service):
+
+   ```bash
+   Requesting pose for Cube
+   Pose for Cube: 
+   position: 
+     x: 0.0
+     y: -1.0
+     z: 0.20000000298023224
+   orientation: 
+     x: 0.0
+     y: -0.0
+     z: 0.0
+     w: -1.0
+   ```
+You may replace `Cube` with the name of any other GameObject currently present in the Unity hierarchy.
+
+- Alternatively you may also call the ROS service using `rosservice call`:
+
+   ```bash
+   rosservice call /obj_pose_srv Cube
+   ```
+   ```bash
+   object_pose: 
+    position: 
+      x: 0.0
+      y: -1.0
+      z: 0.20000000298023224
+    orientation: 
+      x: 0.0
+      y: -0.0
+      z: 0.0
+      w: -1.0
+   ```

Original file line number	Diff line number	Diff line change
`@@ -0,0 +1,3 @@`
	`1`	`+string object_name`
	`2`	`+---`
	`3`	`+geometry_msgs/Pose object_pose`