Robotics on WSL2 using ROS, Docker and Unity 3D (Part II)
Tags: robotics, wsl, unity3d, ros
Published: 2021-03-16 19:13:58

*Not that this is the second part on how to setup ROS to work with WSL2 ([Part I](https://dev.to/szandara/robotics-on-wsl2-using-ros-docker-and-unity-3d-part-i-3752)). However, the upcoming section would work in a full Linux setup as well.* 

## Robotics for Unity
There are several ways of using [Unity for robotics](https://unity.com/solutions/automotive-transportation-manufacturing/robotics). Robotics for Unity is a new field and support is still custom made. Most of the applications out there are proprietary software created by private companies and not easily accessible or configurable by others. [ROS support](https://blogs.unity3d.com/2020/11/19/robotics-simulation-in-unity-is-as-easy-as-1-2-3/) has been recently introduced by Unity itself, and it is a fork from a previous library from Siemens' [RosSharp](https://github.com/siemens/ros-sharp). In my setup, I used the [Unity ROS library](https://github.com/Unity-Technologies/Unity-Robotics-Hub) which works with the latest [ArticulationBody](https://docs.unity3d.com/2020.1/Documentation/ScriptReference/ArticulationBody.html) types.

For this demo project I have chosen a cute robotic manipulator which is open sourced: [Reachy from Pollen Robotics](https://www.pollen-robotics.com/). To use a robot with ROS in Unity you need an existing [URDF](http://wiki.ros.org/urdf/Tutorials) (robot file descriptor) which is usually shipped by the robot manufacturer. In specific I have decided to work on a static robotic manipulator and operate it with [MoveIt!](https://moveit.ros.org/).

There are already quite few tutorials on how to do that in Unity, however, I took the opportunity to create a MoveIt! connector for Unity which allows to control the robot using the [MoveIt! MoveGroup API](http://docs.ros.org/en/kinetic/api/moveit_tutorials/html/doc/moveit_commander_scripting/moveit_commander_scripting_tutorial.html) rather than creating custom services as done by other tutorials. I find this solution easier to plug to other robots since it uses all standard ROS messages and does not require an additional service.

![image](https://dev-to-uploads.s3.amazonaws.com/uploads/articles/tvl7pjo0laxvik8hto2y.png) 

Both Unity and ROS share the same copy of the robot descriptor URDF. Unity will serve as main source of the joint state of the robot. MoveIt! will use the information streaming from Unity to plan the route and send the arm trajectory back to Unity which will actuate the joints.

MoveIt! will communicate the trajectory to Unity using a custom ROS topic on which a custom created Unity MoveIt Handler publishes the trajectory to execute.

The custom code for the project has 3 main components which are in Github:
* [unity_connector_manager.cpp](https://github.com/szandara/unity_moveit_manager/tree/master/src/unity_moveit_manager) which sends the trajectory to Unity after calling execute() on a planned MoveIt! trajectory.
* [unity_tcp_server.py](https://github.com/szandara/unity_moveit_manager/blob/master/src/unity_tcp_server/scripts/server_endpoint.py) which acts as proxy between ROS and Unity
* [ROSMoveItControllerGeneric.cs](https://github.com/szandara/unity_moveit_connector/blob/master/Editor/ROSMoveItControllerGeneric.cs) which controls the robot in Unity and communicates with ROS.


### ROS
Let's now install our ROS distribution. I am using ROS1 because lots of necessary libraries are missing in ROS2. ROS will only be installed inside the docker even though I have a ROS version also installed in my WLS2 for quick tests. 
Along with ROS, the docker image will contain few libraries which are available in Github and we will use for our demo.

* First, create a workspace folder in your WSL home (or wherever you please)

```
zandara@XXX:$ cd ~
zandara@XXX:$ mkdir unity_demo
zandara@XXX:$ cd unity_demo
zandara@XXX:$ git clone https://github.com/szandara/unity_reachy_tutorial
zandara@XXX:$ cd unity_reachy_tutorial
```
* We will now build the [Docker image](https://github.com/szandara/unity_reachy_tutorial/blob/master/Dockerfile) which contains the custom code and the ROS distribution. Run ```./build_docker``` and go grab a tea.

* Next, we will collect the code for Reachy in ROS. This code is outside of the Docker and will be mounted when the Docker is started.
```
zandara@XXX:$ cd src
zandara@XXX:$ git clone https://github.com/szandara/reachy_description_ros1
zandara@XXX:$ git clone https://github.com/pollen-robotics/reachy_moveit_config
```
I am using a fork of Reachy's URDF for ROS1 since they recently moved to ROS2. 

Now all is setup to start using our ROS system!

### Unity 3D
Let's now setup Unity to work with ROS over Docker. But first, let's install Unity3D, if you still do not have it. https://unity3d.com/get-unity/download. I am working with the community version (2020.2.5f1).

* Let's create a project called *reachy_demo*
* Open Unity and go to the Package Manager. Window -> Package Manager -> + sign -> * Add package from git URL.
* Add the following packages. 
```
https://github.com/Unity-Technologies/ROS-TCP-Connector.git#v0.1.2 
https://github.com/Unity-Technologies/URDF-Importer.git#v0.1.2
```
* Make sure they compile without errors. The two packages allow Unity to:
  * Import URDF files for robot models (which is the ROS standard).
  * Communicate with a ROS proxy (which will run on the docker).
* Keep the ROS/Unity settings in *Robotics* -> *ROS Settings* as default (unless you need to change it for some reason) 

![image](https://dev-to-uploads.s3.amazonaws.com/uploads/articles/dsxughgyrwgn2mbg9nqw.png)
 
* Finally open the TCP 5005 port for ROS to talk to Unity. It's fairly [straight-forward as explained here](https://medium.com/r/?url=https%3A%2F%2Fwww.howtogeek.com%2F394735%2Fhow-do-i-open-a-port-on-windows-firewall%2F). See [Part I](https://dev.to/szandara/robotics-on-wsl2-using-ros-docker-and-unity-3d-part-i-3752?preview=8f8f29b8bfa6bff63d49a701398380b9c272ed4f2ef0dc3db12dde5bc8aa2f2644cf471565887242543e873ba753e99e078059d25eb0a1534dd1159f) for the reasons.

### Setup Reachy in Unity
* Go to GitHub and download https://github.com/szandara/reachy_description_ros1 as a zip file.
* Go to your Unity Asset directory and create a subfolder called URDF.
* Copy and extract the downloaded reachy_description.zip into *Assets/URDF/reachy*
* Copy the file *Assets/URDF/reachy/reachy_description_ros1/reachy.URDF* to *Assets/URDF/reachy/reachy.URDF*
* Right click on *reachy.URDF* and *Import Robot from selected URDF*. A window will popup giving you few options. Change Mesh Decomposer to Unity unless you have VHACD.

![image](https://dev-to-uploads.s3.amazonaws.com/uploads/articles/xzzrk6nvvsffcgu5b0gn.png) 

* Select *reachy* from the GameObject list on the left. In the inspector on the right, find Controller (Script) and set the following values.

![image](https://dev-to-uploads.s3.amazonaws.com/uploads/articles/r67nayng3dtxds6rzv7o.png)

*Note that these are arbitrary values which I manually tuned. Accurate values could be provided by the manufacturer.*

* Select *torso* from the GameObject list on the left and tick *Immovable* in *Articulation Body*

![image](https://dev-to-uploads.s3.amazonaws.com/uploads/articles/umwjlyfjrqrs3vmfxnhd.png)
 
We are done with importing Reachy. We should now see it in our simulated world!

![image](https://dev-to-uploads.s3.amazonaws.com/uploads/articles/840j4ymabs8f60kg2e0o.png)

### Setup MoveIt! in Unity
Here we setup a script to control Unity's ArticulationBody objects with MoveIt! messages from ROS. I have uploaded the script in this repository https://github.com/szandara/unity_moveit_connector/blob/master/Editor/ROSMoveItControllerGeneric.cs

* Copy paste the content of [the file](https://github.com/szandara/unity_moveit_connector/blob/master/Editor/ROSMoveItControllerGeneric.cs) directly into a file named *Assets/moveit/ROSMoveItControllerGeneric.cs* make sure Unity compiles it without errors.
* Finally, add the script to Reachy by selecting *r_shoulder* then *Add Component* -> *Scripts* -> *ROS Move It Controller Generic*

* Add the parameters as seen in this example
  * *Joint State Topic Name*: joint_states
  * *Move It Move Group*: unity_right_arm_controller
  * *Joint Group*: Drag and Drop the *r_shoulder* joint from the Unity tree as visible in this screenshot.

![image](https://dev-to-uploads.s3.amazonaws.com/uploads/articles/248scen0vlquo9vovn5h.png)

We are done! We have setup Reachy to talk to ROS!

## Combining all 
As last step, we need to start the system. Let's dive right into it.

* First, let's start Unity if it's not started already and open our project *reachy_demo*
* Open a WSL shell and go to our project folder *unity_demo*
* Edit the file [unity_demo/src/unity_reachy_tutorial/src/unity_reachy_tutorial/launch/unity_connector.launch](https://github.com/szandara/unity_reachy_tutorial/blob/master/src/unity_reachy_tutorial/launch/unity_connector.launch) and set these parameters using the values that were [configured in Unity](#unity-3d). Note that *UNITY_IP* is set by Windows and it's the IP used to communicate with WSL2. You can find this IP from WSL by running ```grep nameserver /etc/resolv.conf```

```
    <param name="ROS_IP" type="str" value="$(env ROS_IP)" />
    <param name="ROS_TCP_PORT" type="int" value="10000" />
    <param name="TCP_NODE_NAME" type="str" value="TCPServer" />
    <param name="TCP_BUFFER_SIZE" type="int" value="1024" />
    <param name="TCP_CONNECTIONS" type="int" value="10" />

    <param name="UNITY_IP" type="str" value="XXX.XXX.XXX.XXX" />
    <param name="UNITY_SERVER_PORT" type="int" value="5005" />
```
* Run the following
```
zandara@XXX:$ ./launch_docker
root@docker:~# source /opt/unity_demo/unity_reachy_tutorial/devel/setup.bash 
root@docker:~# roslaunch unity_reachy_tutorial reachy_demo.launch
```
You should see a confirmation that you can start planning.

* Open another WSL and go to our project folder *unity_demo*
* Run the following
```
zandara@XXX:$ ./launch_docker
root@docker:~# rosrun moveit_commander moveit_commander_cmdline.py
```
You should see the MoveIt command line.
* Go to Unity and start the game, there should be no errors in the console
* Go to the MoveIt command line (in WSL2) and run the following
```
> use right_arm
> current
```
Make sure you see no errors. If you receive an error the problem could be related to the time offset of Windows vs WSL2. To fix it, open a WSL2 console and run the following 
```sudo ntpdate time.windows.com```
* If you have no errors continue planning the route in MoveIt
```
> rec c
> goal = c
> goal[0] = -2.1
> plan goal
> execute
```

You should see Reachy moving its right arm to the top! Congratulations in planning your first Unity simulation!

![image](https://dev-to-uploads.s3.amazonaws.com/uploads/articles/sg3br56dcbc26krzyp6w.png)

## Conclusions

* We have shown how to setup Windows to work as powerful Linux development environment.
* We have shown how Unity can be used as simulation environment for robotics
* We have shown how ROS and Unity exchange messages
* We have shown how to control Reachy on Unity with MoveIt!.

I had lots of fun doing this project and I am going to pursue a personal project using this setup. However, the solution is far from perfect. The pitfalls experienced due to Windows and WSL2 not working well together is substantial and by no means ensure a relaxed development environment for professional use. 

The lack of support for ROS2 by Unity has also lead to produce a probably soon outdated ROS project and an eventually painful migration. [Unity has announced](https://github.com/Unity-Technologies/Unity-Robotics-Hub) that ROS2 support is under development and I will make sure to update this article with the new ROS2 version.

Nonetheless, it was rewarding to know that I can now develop robotics comfortably from my Windows host!