Merge pull request #288 from Unity-Technologies/dev

Syncing dev to main

Author: mrpropellers

CHANGELOG.md

@@ -20,4 +20,10 @@ Changed the Pick and Place Demo's topic from SourceDestination to SourceDestinat
 
 ### Removed
 
-### Fixed
+### Fixed
+
+Fixed the ROS-Unity Integration tutorial to use the correct link to install ROS-TCP-Connector package
+
+Fixed the Pick and Place Tutorial to use ArticulationBody jointPositions, rather than the xDrive.target, for updating the current joint angle positions
+
+Fixed network.md in ROS-Unity Integration tutorial by removing the obsolete UNITY_IP

tutorials/pick_and_place/PickAndPlaceProject/Assets/Scripts/Unity.Robotics.PickAndPlace.asmdef

@@ -3,7 +3,8 @@
     "rootNamespace": "",
     "references": [
         "GUID:204b12c73a0ba074c888ec09a2713605",
-        "GUID:625bfc588fb96c74696858f2c467e978"
+        "GUID:625bfc588fb96c74696858f2c467e978",
+        "GUID:b1ef917f7a8a86a4eb639ec2352edbf8"
     ],
     "includePlatforms": [],
     "excludePlatforms": [],

tutorials/pick_and_place/Scripts/SourceDestinationPublisher.cs

@@ -2,6 +2,7 @@
 using UnityEngine;
 using Unity.Robotics.ROSTCPConnector;
 using Unity.Robotics.ROSTCPConnector.ROSGeometry;
+using Unity.Robotics.UrdfImporter;
 using RosMessageTypes.Geometry;
 
 public class SourceDestinationPublisher : MonoBehaviour
@@ -19,8 +20,8 @@ public class SourceDestinationPublisher : MonoBehaviour
     private int numRobotJoints = 6;
     private readonly Quaternion pickOrientation = Quaternion.Euler(90, 90, 0);
 
-    // Articulation Bodies
-    private ArticulationBody[] jointArticulationBodies;
+    // Robot Joints
+    private UrdfJointRevolute[] revoluteJoints;
 
     /// <summary>
     /// 
@@ -30,36 +31,36 @@ void Start()
         // Get ROS connection static instance
         ros = ROSConnection.instance;
 
-        jointArticulationBodies = new ArticulationBody[numRobotJoints];
+        revoluteJoints = new UrdfJointRevolute[numRobotJoints];
         string shoulder_link = "world/base_link/shoulder_link";
-        jointArticulationBodies[0] = niryoOne.transform.Find(shoulder_link).GetComponent<ArticulationBody>();
+        revoluteJoints[0] = niryoOne.transform.Find(shoulder_link).GetComponent<UrdfJointRevolute>();
 
         string arm_link = shoulder_link + "/arm_link";
-        jointArticulationBodies[1] = niryoOne.transform.Find(arm_link).GetComponent<ArticulationBody>();
+        revoluteJoints[1] = niryoOne.transform.Find(arm_link).GetComponent<UrdfJointRevolute>();
 
         string elbow_link = arm_link + "/elbow_link";
-        jointArticulationBodies[2] = niryoOne.transform.Find(elbow_link).GetComponent<ArticulationBody>();
+        revoluteJoints[2] = niryoOne.transform.Find(elbow_link).GetComponent<UrdfJointRevolute>();
 
         string forearm_link = elbow_link + "/forearm_link";
-        jointArticulationBodies[3] = niryoOne.transform.Find(forearm_link).GetComponent<ArticulationBody>();
+        revoluteJoints[3] = niryoOne.transform.Find(forearm_link).GetComponent<UrdfJointRevolute>();
 
         string wrist_link = forearm_link + "/wrist_link";
-        jointArticulationBodies[4] = niryoOne.transform.Find(wrist_link).GetComponent<ArticulationBody>();
+        revoluteJoints[4] = niryoOne.transform.Find(wrist_link).GetComponent<UrdfJointRevolute>();
 
         string hand_link = wrist_link + "/hand_link";
-        jointArticulationBodies[5] = niryoOne.transform.Find(hand_link).GetComponent<ArticulationBody>();
+        revoluteJoints[5] = niryoOne.transform.Find(hand_link).GetComponent<UrdfJointRevolute>();
     }
 
     public void Publish()
     {
         NiryoMoveitJointsMsg sourceDestinationMessage = new NiryoMoveitJointsMsg();
 
-        sourceDestinationMessage.joint_00 = jointArticulationBodies[0].xDrive.target;
-        sourceDestinationMessage.joint_01 = jointArticulationBodies[1].xDrive.target;
-        sourceDestinationMessage.joint_02 = jointArticulationBodies[2].xDrive.target;
-        sourceDestinationMessage.joint_03 = jointArticulationBodies[3].xDrive.target;
-        sourceDestinationMessage.joint_04 = jointArticulationBodies[4].xDrive.target;
-        sourceDestinationMessage.joint_05 = jointArticulationBodies[5].xDrive.target;
+        sourceDestinationMessage.joint_00 = revoluteJoints[0].GetPosition() * Mathf.Rad2Deg;
+        sourceDestinationMessage.joint_01 = revoluteJoints[1].GetPosition() * Mathf.Rad2Deg;
+        sourceDestinationMessage.joint_02 = revoluteJoints[2].GetPosition() * Mathf.Rad2Deg;
+        sourceDestinationMessage.joint_03 = revoluteJoints[3].GetPosition() * Mathf.Rad2Deg;
+        sourceDestinationMessage.joint_04 = revoluteJoints[4].GetPosition() * Mathf.Rad2Deg;
+        sourceDestinationMessage.joint_05 = revoluteJoints[5].GetPosition() * Mathf.Rad2Deg;
 
         // Pick Pose
         sourceDestinationMessage.pick_pose = new PoseMsg

tutorials/pick_and_place/Scripts/TrajectoryPlanner.cs

@@ -82,12 +82,12 @@ NiryoMoveitJointsMsg CurrentJointConfig()
     {
         NiryoMoveitJointsMsg joints = new NiryoMoveitJointsMsg();
 
-        joints.joint_00 = jointArticulationBodies[0].xDrive.target;
-        joints.joint_01 = jointArticulationBodies[1].xDrive.target;
-        joints.joint_02 = jointArticulationBodies[2].xDrive.target;
-        joints.joint_03 = jointArticulationBodies[3].xDrive.target;
-        joints.joint_04 = jointArticulationBodies[4].xDrive.target;
-        joints.joint_05 = jointArticulationBodies[5].xDrive.target;
+        joints.joint_00 = jointArticulationBodies[0].jointPosition[0] * Mathf.Rad2Deg;
+        joints.joint_01 = jointArticulationBodies[1].jointPosition[0] * Mathf.Rad2Deg;
+        joints.joint_02 = jointArticulationBodies[2].jointPosition[0] * Mathf.Rad2Deg;
+        joints.joint_03 = jointArticulationBodies[3].jointPosition[0] * Mathf.Rad2Deg;
+        joints.joint_04 = jointArticulationBodies[4].jointPosition[0] * Mathf.Rad2Deg;
+        joints.joint_05 = jointArticulationBodies[5].jointPosition[0] * Mathf.Rad2Deg;
 
         return joints;
     }

tutorials/ros_unity_integration/images/troubleshoot-ros-settings.png

@@ -4,7 +4,6 @@
 - [Definitions](#definitions)
 - [Communicaton Minimum Requirements](#communication-minimum-requirements)
 	- [If Using Docker](#if-using-docker)
-	- [Explicitly Setting UNITY_IP](#explicitly-setting-unity_ip)
 - [Troubleshoooting](#troubleshooting)
 	- [Where Does Communication Break Down](#where-does-communication-break-down)
 		- [1. Can Unity send messages to ROS?](#1-can-unity-send-messages-to-ros)
@@ -34,26 +33,32 @@ The container will need to be started with the following arguments to forward th
 
 
 - On the ROS side, set `ROS_IP` to `0.0.0.0`.
+  ```bash
+  rosparam set ROS_IP 0.0.0.0
+  ```
 
 - On the Unity side, set `ROS_IP` to `127.0.0.1`.
 
-![](images/troubleshoot-docker-unity.png)
+![](images/settings_ros_ip.png)
 
 # Troubleshooting
 
 ## Where Does Communication Break Down
 
 ### 1. Can Unity send messages to ROS?
 
-When play is pressed in the Editor, a handshake message is sent from Unity to ROS.
+When play is pressed in the Editor, Unity will establish the connection to ROS using the ROS_IP.
 
-If Unity can communicate with ROS, the following message should be printed to the console screen running the `server_endpoint.py` script.
+If Unity can communicate with ROS, you should see a heads-up display showing the connection in Unity Editor.
 
-```[UnityTcpSender]: ROS-Unity Handshake received, will connect to UNITY_IP_ADDRESS:5005```
+![](images/troubleshoot_hud_success.png)
 
-With the corresponding response message printed to the Unity console,
+The icon in front of ROS IP should be blue indicating the connection between Unity and ROS is successful.
 
-```ROS-Unity server listening on UNITY_IP_ADDRESS:5005```
+
+On ROS side, you should see a message printed to the console screen running the `server_endpoint.py` script, something similar to the following:
+
+```Connection from 172.17.0.1```
 
 
 If the previous message is not shown and either of the following errors are thrown instead:
@@ -68,25 +73,24 @@ SocketException: Connection refused
 
 Confirm that:
 
-- `server_endpoint` is running
+- `server_endpoint` is running. On ROS side, you can run ```rosrun ros_tcp_endpoint default_server_endpoint.py```
 - You can ping ROS machine from Unity machine
 	- From a terminal on the Unity machine, run the following command to confirm whether the ROS machine is reachable over the network. ```ping ROS_IP```
 
 If issue still persists:
 
-- Confirm your IP addresses
 - If on Windows you may need to [open ports for the firewall](#open-port-on-windows-firewall).
 
 ### 2. Can ROS send messages to Unity?
 
 After it is confirmed that Unity can communicate with ROS, publish a message to a ROS topic to which Unity has instantiated a subscriber.
 
+You can confirm the connection status by checking the heads-up display in your Unity Scene after entering the Play mode.
+
 If an error is thrown in the `server_endpoint` console then ROS cannot connect to Unity.
 
 If issue still persists:
 
-- Confirm your IP addresses
-- Explicitly set the `UNITY_IP`
 - If on Windows you may need to [open ports for the firewall](#open-port-on-windows-firewall).
 
 ## Open port on Windows Firewall

tutorials/ros_unity_integration/images/troubleshoot_hud_success.png

@@ -101,7 +101,7 @@ Follow these steps if using ROS2:
 
 ## <img src="images/unity-tab-square-white.png" alt="ros2" width="24" height="24"/> Unity Setup
 1. Launch Unity and create a new project. The Robotics package works best with a version of Unity no older than 2020.
-2. Open Package Manager and click the + button at the top left corner. Select "add package from git URL" and enter "https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connector#dev" to install the [ROS-TCP-Connector](https://github.com/Unity-Technologies/ROS-TCP-Connector) package.
+2. Open Package Manager and click the + button at the top left corner. Select "add package from git URL" and enter "https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connector" to install the [ROS-TCP-Connector](https://github.com/Unity-Technologies/ROS-TCP-Connector) package.
 
   ![](images/add_package.png)