From 56e97381208091b715d8de1c4b765e2c545f4477 Mon Sep 17 00:00:00 2001 From: peifeng-unity <56408141+peifeng-unity@users.noreply.github.com> Date: Thu, 7 Oct 2021 05:08:22 +0000 Subject: [PATCH 01/33] Configurable north direction for NED and ENU conversions Co-authored-by: LaurieCheers <73140792+LaurieCheers-unity@users.noreply.github.com> --- ROSGeometry.md | 10 + .../CHANGELOG.md | 2 + .../Editor/ROSSettingsEditor.cs | 7 + .../Runtime/ROSGeometry/CoordinateSpaces.cs | 189 +++++++++++++++++- .../Runtime/ROSGeometry/GeometryCompass.cs | 71 +++++++ .../ROSGeometry/GeometryCompass.cs.meta | 11 + .../Runtime/ROSGeometry/ROSVector3.cs | 10 +- .../Tests/Runtime/ROSGeometryTests.cs | 79 ++++++++ 8 files changed, 366 insertions(+), 13 deletions(-) create mode 100644 com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/GeometryCompass.cs create mode 100644 com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/GeometryCompass.cs.meta diff --git a/ROSGeometry.md b/ROSGeometry.md index 768667be..52107884 100644 --- a/ROSGeometry.md +++ b/ROSGeometry.md @@ -98,3 +98,13 @@ Or, if you need to work with it in the FLU coordinate space for now, you can wri (Note, the As function does NOT do any coordinate conversion. It simply assumes the point is in the FLU coordinate frame already, and transfers it into an appropriate container.) And again, the same goes for converting a Quaternion message into a Unity Quaternion or `Quaternion`. + +# Geographical Coordinates + +The geographical coordinate systems NED and ENU are more complicated than the rest: besides the obvious "north" direction, they also provide a convention for which direction is "forward", which is not even consistent between them - In NED, forward (i.e. yaw 0) is north, whereas in ENU forward is east. Because of this inconsistency, there's no universal convention that we could establish for which direction is north in Unity. Instead, we're forced to make a distinction between local and world rotations. + +To correctly convert a *world* position or rotation to or from NED or ENU coordinates, you should use the standard NED or ENU coordinate space - for example, `To()`. This conversion will respect what direction you have set as North. By default, the Unity Z axis points north, but this is a configuration option that you can change in the Robotics/ROS Settings menu. + +If you have a *local* rotation represented in NED or ENU coordinates, you should convert it using the coordinate space NEDLocal or ENULocal: for example, `To()`. These conversions are not appropriate for handling world rotations, because they don't respect the north direction, only what direction is forward (i.e. the Unity Z axis). In other words, with NEDLocal and ENULocal, an identity quaternion is still identity. This is not necessarily true for NED or ENU. NEDLocal is a synonym for the FRD coordinate space (north = forward, east = right, down = down) and ENULocal is a synonym for the FLU coordinate space (east = forward, north = left, up = up). + +If you only care about NED coordinates, you can avoid having to deal with this distinction by setting the Z axis direction to North (the default) - this will make the NED coordinate space equal to NEDLocal. Similarly, if you only care about ENU coordinates, setting the Z axis direction to East will make ENU equal to ENULocal. diff --git a/com.unity.robotics.ros-tcp-connector/CHANGELOG.md b/com.unity.robotics.ros-tcp-connector/CHANGELOG.md index bf23365e..24f8376c 100644 --- a/com.unity.robotics.ros-tcp-connector/CHANGELOG.md +++ b/com.unity.robotics.ros-tcp-connector/CHANGELOG.md @@ -49,6 +49,8 @@ Upgrade the TestRosTcpConnector project to use Unity LTS version 2020.3.11f1 - Unity service implementations can be async + - Added geographical world coordinate transformation by a Compass component + ### Changed - Publishing a message to an unregistered topic will show an error. - Registering a service now requires both the request and response message type. diff --git a/com.unity.robotics.ros-tcp-connector/Editor/ROSSettingsEditor.cs b/com.unity.robotics.ros-tcp-connector/Editor/ROSSettingsEditor.cs index 83f6ebff..4d161663 100644 --- a/com.unity.robotics.ros-tcp-connector/Editor/ROSSettingsEditor.cs +++ b/com.unity.robotics.ros-tcp-connector/Editor/ROSSettingsEditor.cs @@ -3,6 +3,7 @@ using System.IO; using System.Collections.Generic; using System.Linq; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; using UnityEditor.Callbacks; namespace Unity.Robotics.ROSTCPConnector.Editor @@ -20,6 +21,7 @@ public static void OpenWindow() GameObject prefabObj; Unity.Robotics.ROSTCPConnector.ROSConnection prefab; + GeometryCompass geometryCompassPrefab; public enum RosProtocol { @@ -138,6 +140,11 @@ protected virtual void OnGUI() { PrefabUtility.SavePrefabAsset(prefab.gameObject); } + + // ROS Geometry Compass Settings + EditorGUILayout.Space(); + EditorGUILayout.LabelField("Compass settings", EditorStyles.boldLabel); + GeometryCompass.UnityZAxisDirection = (CardinalDirection)EditorGUILayout.EnumPopup("Unity Z Axis Direction", GeometryCompass.UnityZAxisDirection); } void OnInspectorUpdate() { Repaint(); } diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/CoordinateSpaces.cs b/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/CoordinateSpaces.cs index 57e53c89..ee7ff758 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/CoordinateSpaces.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/CoordinateSpaces.cs @@ -1,7 +1,5 @@ using RosMessageTypes.Geometry; using System; -using System.Collections; -using System.Collections.Generic; using UnityEngine; namespace Unity.Robotics.ROSTCPConnector.ROSGeometry @@ -37,7 +35,9 @@ public class FLU : ICoordinateSpace public Quaternion ConvertToRUF(Quaternion q) => new Quaternion(-q.y, q.z, q.x, -q.w); } - public class NED : ICoordinateSpace + public class ENULocal : FLU { } + + public class FRD : ICoordinateSpace { public Vector3 ConvertFromRUF(Vector3 v) => new Vector3(v.z, v.x, -v.y); public Vector3 ConvertToRUF(Vector3 v) => new Vector3(v.y, -v.z, v.x); @@ -45,20 +45,169 @@ public class NED : ICoordinateSpace public Quaternion ConvertToRUF(Quaternion q) => new Quaternion(q.y, -q.z, q.x, -q.w); } + public class NEDLocal : FRD { } + + public class NED : ICoordinateSpace + { + public Vector3 ConvertFromRUF(Vector3 v) + { + switch (GeometryCompass.UnityZAxisDirection) + { + case CardinalDirection.North: + return new Vector3(v.z, v.x, -v.y); + case CardinalDirection.East: + return new Vector3(-v.x, v.z, -v.y); + case CardinalDirection.South: + return new Vector3(-v.z, -v.x, -v.y); + case CardinalDirection.West: + return new Vector3(v.x, -v.z, -v.y); + default: + throw new NotSupportedException(); + } + } + + public Vector3 ConvertToRUF(Vector3 v) + { + switch (GeometryCompass.UnityZAxisDirection) + { + case CardinalDirection.North: + return new Vector3(v.y, -v.z, v.x); + case CardinalDirection.East: + return new Vector3(-v.x, -v.z, v.y); + case CardinalDirection.South: + return new Vector3(-v.y, -v.z, -v.x); + case CardinalDirection.West: + return new Vector3(v.x, -v.z, -v.y); + default: + throw new NotSupportedException(); + } + } + + public Quaternion ConvertFromRUF(Quaternion q) + { + switch (GeometryCompass.UnityZAxisDirection) + { + case CardinalDirection.North: + break; + case CardinalDirection.East: + q = GeometryCompass.k_NinetyYaw * q; + break; + case CardinalDirection.South: + q = GeometryCompass.k_OneEightyYaw * q; + break; + case CardinalDirection.West: + q = GeometryCompass.k_NegativeNinetyYaw * q; + break; + default: + throw new NotSupportedException(); + } + return new Quaternion(q.z, q.x, -q.y, -q.w); + } + + public Quaternion ConvertToRUF(Quaternion q) + { + var r = new Quaternion(q.y, -q.z, q.x, -q.w); + switch (GeometryCompass.UnityZAxisDirection) + { + case CardinalDirection.North: + return r; + case CardinalDirection.East: + return GeometryCompass.k_NegativeNinetyYaw * r; + case CardinalDirection.South: + return GeometryCompass.k_OneEightyYaw * r; + case CardinalDirection.West: + return GeometryCompass.k_NinetyYaw * r; + default: + throw new NotSupportedException(); + } + } + } + public class ENU : ICoordinateSpace { - public Vector3 ConvertFromRUF(Vector3 v) => new Vector3(v.x, v.z, v.y); - public Vector3 ConvertToRUF(Vector3 v) => new Vector3(v.x, v.z, v.y); - public Quaternion ConvertFromRUF(Quaternion q) => new Quaternion(q.x, q.z, q.y, -q.w); - public Quaternion ConvertToRUF(Quaternion q) => new Quaternion(q.x, q.z, q.y, -q.w); + public Vector3 ConvertFromRUF(Vector3 v) + { + switch (GeometryCompass.UnityZAxisDirection) + { + case CardinalDirection.North: + return new Vector3(v.x, v.z, v.y); + case CardinalDirection.East: + return new Vector3(v.z, -v.x, v.y); + case CardinalDirection.South: + return new Vector3(-v.x, -v.z, v.y); + case CardinalDirection.West: + return new Vector3(-v.z, v.x, v.y); + default: + throw new NotSupportedException(); + } + } + public Vector3 ConvertToRUF(Vector3 v) + { + switch (GeometryCompass.UnityZAxisDirection) + { + case CardinalDirection.North: + return new Vector3(v.x, v.z, v.y); + case CardinalDirection.East: + return new Vector3(-v.y, v.z, v.x); + case CardinalDirection.South: + return new Vector3(-v.x, v.z, -v.y); + case CardinalDirection.West: + return new Vector3(v.y, v.z, -v.x); + default: + throw new NotSupportedException(); + } + } + + public Quaternion ConvertFromRUF(Quaternion q) + { + switch (GeometryCompass.UnityZAxisDirection) + { + case CardinalDirection.North: + q = GeometryCompass.k_NegativeNinetyYaw * q; + break; + case CardinalDirection.East: + break; + case CardinalDirection.South: + q = GeometryCompass.k_NinetyYaw * q; + break; + case CardinalDirection.West: + q = GeometryCompass.k_OneEightyYaw * q; + break; + default: + throw new NotSupportedException(); + } + + return new Quaternion(q.z, -q.x, q.y, -q.w); + } + + public Quaternion ConvertToRUF(Quaternion q) + { + q = new Quaternion(-q.y, q.z, q.x, -q.w); + switch (GeometryCompass.UnityZAxisDirection) + { + case CardinalDirection.North: + return GeometryCompass.k_NinetyYaw * q; + case CardinalDirection.East: + return q; + case CardinalDirection.South: + return GeometryCompass.k_NegativeNinetyYaw * q; + case CardinalDirection.West: + return GeometryCompass.k_OneEightyYaw * q; + default: + throw new NotSupportedException(); + } + } } public enum CoordinateSpaceSelection { RUF, FLU, + FRD, NED, - ENU + ENU, + NEDLocal, + ENULocal, } public static class CoordinateSpaceExtensions @@ -133,10 +282,16 @@ public static Vector3 From(this PointMsg self, CoordinateSpaceSelection selectio return self.From(); case CoordinateSpaceSelection.FLU: return self.From(); + case CoordinateSpaceSelection.FRD: + return self.From(); case CoordinateSpaceSelection.ENU: return self.From(); case CoordinateSpaceSelection.NED: return self.From(); + case CoordinateSpaceSelection.ENULocal: + return self.From(); + case CoordinateSpaceSelection.NEDLocal: + return self.From(); default: Debug.LogError("Invalid coordinate space " + selection); return self.From(); @@ -151,10 +306,16 @@ public static Vector3 From(this Point32Msg self, CoordinateSpaceSelection select return self.From(); case CoordinateSpaceSelection.FLU: return self.From(); + case CoordinateSpaceSelection.FRD: + return self.From(); case CoordinateSpaceSelection.ENU: return self.From(); case CoordinateSpaceSelection.NED: return self.From(); + case CoordinateSpaceSelection.ENULocal: + return self.From(); + case CoordinateSpaceSelection.NEDLocal: + return self.From(); default: Debug.LogError("Invalid coordinate space " + selection); return self.From(); @@ -169,10 +330,16 @@ public static Vector3 From(this Vector3Msg self, CoordinateSpaceSelection select return self.From(); case CoordinateSpaceSelection.FLU: return self.From(); + case CoordinateSpaceSelection.FRD: + return self.From(); case CoordinateSpaceSelection.ENU: return self.From(); case CoordinateSpaceSelection.NED: return self.From(); + case CoordinateSpaceSelection.ENULocal: + return self.From(); + case CoordinateSpaceSelection.NEDLocal: + return self.From(); default: Debug.LogError("Invalid coordinate space " + selection); return self.From(); @@ -187,10 +354,16 @@ public static Quaternion From(this QuaternionMsg self, CoordinateSpaceSelection return self.From(); case CoordinateSpaceSelection.FLU: return self.From(); + case CoordinateSpaceSelection.FRD: + return self.From(); case CoordinateSpaceSelection.ENU: return self.From(); case CoordinateSpaceSelection.NED: return self.From(); + case CoordinateSpaceSelection.ENULocal: + return self.From(); + case CoordinateSpaceSelection.NEDLocal: + return self.From(); default: Debug.LogError("Invalid coordinate space " + selection); return self.From(); diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/GeometryCompass.cs b/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/GeometryCompass.cs new file mode 100644 index 00000000..2aced3d6 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/GeometryCompass.cs @@ -0,0 +1,71 @@ +using System; +using UnityEngine; + +namespace Unity.Robotics.ROSTCPConnector.ROSGeometry +{ + public enum CardinalDirection + { + North = 0, + East = 1, + South = 2, + West = 3, + } + + public class GeometryCompass : ScriptableObject + { + public const string k_CompassSettingsAsset = "GeometryCompassSettings.asset"; + + [SerializeField] + CardinalDirection m_ZAxisDirection; + + static GeometryCompass s_Instance; + + static GeometryCompass GetOrCreateSettings() + { + if (s_Instance != null) + return s_Instance; +#if UNITY_EDITOR + string assetPath = System.IO.Path.Combine("Assets/Resources", k_CompassSettingsAsset); + s_Instance = UnityEditor.AssetDatabase.LoadAssetAtPath(assetPath); + if (s_Instance == null) + { + s_Instance = ScriptableObject.CreateInstance(); + s_Instance.m_ZAxisDirection = CardinalDirection.North; + if(!UnityEditor.AssetDatabase.IsValidFolder("Assets/Resources")) + UnityEditor.AssetDatabase.CreateFolder("Assets", "Resources"); + UnityEditor.AssetDatabase.CreateAsset(s_Instance, assetPath); + UnityEditor.AssetDatabase.SaveAssets(); + } +#else + s_Instance = Resources.Load(k_CompassSettingsAsset); +#endif + return s_Instance; + } + + public static CardinalDirection UnityZAxisDirection + { + get + { + GeometryCompass compass = GetOrCreateSettings(); + return (compass == null) ? CardinalDirection.North : compass.m_ZAxisDirection; + } + +#if UNITY_EDITOR + set + { + GeometryCompass compass = GetOrCreateSettings(); + if (compass.m_ZAxisDirection != value) + { + compass.m_ZAxisDirection = value; + UnityEditor.EditorUtility.SetDirty(compass); + UnityEditor.AssetDatabase.SaveAssets(); + } + } +#endif + } + + public static readonly Quaternion k_NinetyYaw = Quaternion.Euler(0, 90, 0); + public static readonly Quaternion k_OneEightyYaw = Quaternion.Euler(0, 180, 0); + public static readonly Quaternion k_NegativeNinetyYaw = Quaternion.Euler(0, -90, 0); + } +} diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/GeometryCompass.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/GeometryCompass.cs.meta new file mode 100644 index 00000000..4dd896d7 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/GeometryCompass.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6ad8cf01110e54df290b36a418e91285 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/ROSVector3.cs b/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/ROSVector3.cs index 82820c63..6fb1c34e 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/ROSVector3.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/ROSVector3.cs @@ -17,7 +17,7 @@ namespace Unity.Robotics.ROSTCPConnector.ROSGeometry public float y { get => internalVector.y; set => internalVector.y = value; } public float z { get => internalVector.z; set => internalVector.z = value; } - static C coordinateSpace = new C(); + static C s_CoordinateSpace = new C(); public Vector3(float x, float y, float z) { @@ -26,19 +26,19 @@ public Vector3(float x, float y, float z) public Vector3(Vector3 vec_ruf) { - internalVector = coordinateSpace.ConvertFromRUF(vec_ruf); + internalVector = s_CoordinateSpace.ConvertFromRUF(vec_ruf); } - public Vector3 toUnity => coordinateSpace.ConvertToRUF(internalVector); + public Vector3 toUnity => s_CoordinateSpace.ConvertToRUF(internalVector); public static explicit operator Vector3(Vector3 vec) { - return MakeInternal(coordinateSpace.ConvertFromRUF(vec)); + return MakeInternal(s_CoordinateSpace.ConvertFromRUF(vec)); } public static explicit operator Vector3(Vector3 vec) { - return coordinateSpace.ConvertToRUF(vec.internalVector); + return s_CoordinateSpace.ConvertToRUF(vec.internalVector); } public Vector3 To() where C2 : ICoordinateSpace, new() diff --git a/com.unity.robotics.ros-tcp-connector/Tests/Runtime/ROSGeometryTests.cs b/com.unity.robotics.ros-tcp-connector/Tests/Runtime/ROSGeometryTests.cs index d31b921e..7f7440e6 100644 --- a/com.unity.robotics.ros-tcp-connector/Tests/Runtime/ROSGeometryTests.cs +++ b/com.unity.robotics.ros-tcp-connector/Tests/Runtime/ROSGeometryTests.cs @@ -1,3 +1,4 @@ +using System.Collections.Generic; using NUnit.Framework; using Unity.Robotics.ROSTCPConnector.ROSGeometry; using UnityEngine; @@ -28,6 +29,84 @@ public void TransfromtoLocal_ChildObjectTransform_LocalTransform() Assert.AreApproximatelyEqual(fluRotation.y, (float)testMsg.rotation.y); Assert.AreApproximatelyEqual(fluRotation.z, (float)testMsg.rotation.z); Assert.AreApproximatelyEqual(fluRotation.w, (float)testMsg.rotation.w); + + Object.DestroyImmediate(parent); + } + + [Test, TestCaseSource(nameof(EnuTestCases))] + public void CompassENU_Success(CardinalDirection direction, + Vector3 unityPosition, Vector3 enuPosition, + Quaternion unityRotation, Quaternion enuRotation) + { + GeometryCompass.UnityZAxisDirection = direction; + Assert.AreEqual(enuPosition, unityPosition.To()); + Assert.AreEqual(unityPosition, enuPosition.toUnity); + AssertApproximatelyEqual(enuRotation, unityRotation.To()); + AssertApproximatelyEqual(unityRotation, enuRotation.toUnity); + } + + [Test, TestCaseSource(nameof(NedTestCases))] + public void CompassNED_Success(CardinalDirection direction, + Vector3 unityPosition, Vector3 nedPosition, + Quaternion unityRotation, Quaternion nedRotation) + { + GeometryCompass.UnityZAxisDirection = direction; + Assert.AreEqual(nedPosition, unityPosition.To()); + Assert.AreEqual(unityPosition, nedPosition.toUnity); + AssertApproximatelyEqual(nedRotation, unityRotation.To()); + AssertApproximatelyEqual(unityRotation, nedRotation.toUnity); + } + + static IEnumerable EnuTestCases + { + get + { + yield return new TestCaseData(CardinalDirection.North, + new Vector3(1, 2, 3), new Vector3(1, 3, 2), + Quaternion.identity, new Quaternion(0, 0, -Mathf.Sqrt(2) / 2, -Mathf.Sqrt(2) / 2)); + yield return new TestCaseData(CardinalDirection.East, + new Vector3(1, 2, 3), new Vector3(3, -1, 2), + Quaternion.identity, new Quaternion(0, 0, 0, -1)); + yield return new TestCaseData(CardinalDirection.South, + new Vector3(1, 2, 3), new Vector3(-1, -3, 2), + Quaternion.identity, new Quaternion(0, 0, Mathf.Sqrt(2) / 2, -Mathf.Sqrt(2) / 2)); + yield return new TestCaseData(CardinalDirection.West, + new Vector3(1, 2, 3), new Vector3(-3, 1, 2), + Quaternion.identity, new Quaternion(0, 0, 1, 0)); + } + } + + static IEnumerable NedTestCases + { + get + { + yield return new TestCaseData(CardinalDirection.North, + new Vector3(1, 2, 3), new Vector3(3, 1, -2), + Quaternion.identity, new Quaternion(0, 0, 0, -1)); + yield return new TestCaseData(CardinalDirection.East, + new Vector3(1, 2, 3), new Vector3(-1, 3, -2), + Quaternion.identity, new Quaternion(0, 0, -Mathf.Sqrt(2) / 2, -Mathf.Sqrt(2) / 2)); + yield return new TestCaseData(CardinalDirection.South, + new Vector3(1, 2, 3), new Vector3(-3, -1, -2), + Quaternion.identity, new Quaternion(0, 0, -1, 0)); + yield return new TestCaseData(CardinalDirection.West, + new Vector3(1, 2, 3), new Vector3(1, -3, -2), + Quaternion.identity, new Quaternion(0, 0, -Mathf.Sqrt(2) / 2, Mathf.Sqrt(2) / 2)); + } + } + + void AssertApproximatelyEqual(Quaternion expectedRotation, Quaternion rotation) where C : ICoordinateSpace, new() + { + AssertApproximatelyEqual( + new Quaternion(expectedRotation.x, expectedRotation.y, expectedRotation.z, expectedRotation.w), + new Quaternion(rotation.x, rotation.y, rotation.z, rotation.w)); + } + + void AssertApproximatelyEqual(Quaternion expectedRotation, Quaternion rotation) + { + Assert.AreApproximatelyEqual(expectedRotation.eulerAngles.x, rotation.eulerAngles.x); + Assert.AreApproximatelyEqual(expectedRotation.eulerAngles.y, rotation.eulerAngles.y); + Assert.AreApproximatelyEqual(expectedRotation.eulerAngles.z, rotation.eulerAngles.z); } } } From 65c406614b050e643c916852f4c54610251d73ee Mon Sep 17 00:00:00 2001 From: LaurieCheers-unity <73140792+LaurieCheers-unity@users.noreply.github.com> Date: Thu, 7 Oct 2021 10:50:00 -0700 Subject: [PATCH 02/33] Update readmes --- com.unity.robotics.visualizations/Documentation~/README.md | 6 +++--- com.unity.robotics.visualizations/Visualizations.md | 2 +- com.unity.robotics.visualizations/package.json | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/com.unity.robotics.visualizations/Documentation~/README.md b/com.unity.robotics.visualizations/Documentation~/README.md index fe5d226d..344656a8 100644 --- a/com.unity.robotics.visualizations/Documentation~/README.md +++ b/com.unity.robotics.visualizations/Documentation~/README.md @@ -1,4 +1,4 @@ -# Robotics Visualizations Package +# Unity Robotics Visualizations Package The Visualizations Package enables Unity projects to visualize incoming and outgoing information from ROS, such as sensor data, navigation messages, markers, and more. This package provides default configurations for common message types as well as APIs to create custom visualizations. @@ -28,11 +28,11 @@ Get started with the Visualizations Package with our [Nav2-SLAM tutorial](https: ![image](https://user-images.githubusercontent.com/29758400/110989310-8ea36180-8326-11eb-8318-f67ee200a23d.png) -3. Enter `https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.visualizations`. +3. Enter `https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.visualizations` and click `Add`. > Note: you can append a version tag to the end of the git url, like `#v0.4.0` or `#v0.5.0`, to declare a specific package version, or exclude the tag to get the latest from the package's `main` branch. -4. Click `Add`. +4. The Visualizations package requires the corresponding version of the ROS TCP Connector package. If you haven't already installed it, click the + button again, enter `https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connector` and click `Add`. ## Configuring a Visualization Suite diff --git a/com.unity.robotics.visualizations/Visualizations.md b/com.unity.robotics.visualizations/Visualizations.md index e768d2d4..fd323dee 100644 --- a/com.unity.robotics.visualizations/Visualizations.md +++ b/com.unity.robotics.visualizations/Visualizations.md @@ -1,4 +1,4 @@ -# Unity Visualization Package +# Unity Robotics Visualizations Package To install the Visualization Package in an existing Unity robotics project, open Window/Package Manager click the plus button and select "Add package from Git URL". Paste in the following text, then click Install: diff --git a/com.unity.robotics.visualizations/package.json b/com.unity.robotics.visualizations/package.json index cbec7f32..6321ef6a 100644 --- a/com.unity.robotics.visualizations/package.json +++ b/com.unity.robotics.visualizations/package.json @@ -1,7 +1,7 @@ { "name": "com.unity.robotics.visualizations", "version": "0.1.0-preview", - "displayName": "Robotics Visualization", + "displayName": "Unity Robotics Visualizations", "description": "Components for displaying specific ROS messages", "unity": "2020.2", "unityRelease": "0b9", From 70284f5d6a26e1cbeb810cf3eed1ee6300626584 Mon Sep 17 00:00:00 2001 From: Peter Smith Date: Thu, 14 Oct 2021 07:40:23 +1000 Subject: [PATCH 03/33] Fixing a small bug in the RosTopicState.cs where MessageSender would always be null. (#208) --- .../Runtime/TcpConnector/RosTopicState.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/RosTopicState.cs b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/RosTopicState.cs index 86a79c0c..fe322c6a 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/RosTopicState.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/RosTopicState.cs @@ -20,7 +20,7 @@ public class RosTopicState public string RosMessageName => m_RosMessageName; TopicMessageSender m_MessageSender; - public TopicMessageSender MessageSender; + public TopicMessageSender MessageSender => m_MessageSender; public bool IsPublisher { get; private set; } public bool IsPublisherLatched { get; private set; } public bool SentPublisherRegistration { get; private set; } From b320cb99e1eb41e113ada65742bb5afcbfb4bede Mon Sep 17 00:00:00 2001 From: LaurieCheers-unity <73140792+LaurieCheers-unity@users.noreply.github.com> Date: Thu, 14 Oct 2021 11:49:28 -0700 Subject: [PATCH 04/33] Visualizing services (#205) --- .../MessageGeneration/MessageRegistry.cs | 7 ++ .../Runtime/TcpConnector/ROSConnection.cs | 17 ++--- .../Runtime/TcpConnector/RosTopicState.cs | 21 +++--- .../Runtime/Scripts/BaseVisualFactory.cs | 19 ++++- .../Runtime/Scripts/DrawingVisualizer.cs | 2 +- .../Runtime/Scripts/GuiVisualizer.cs | 2 +- .../Scripts/HistoryDrawingVisualizer.cs | 3 +- .../Runtime/Scripts/TextureVisualizer.cs | 3 +- .../Runtime/Scripts/VisualFactoryRegistry.cs | 4 +- .../Runtime/Scripts/VisualizationTopicsTab.cs | 17 +++-- .../Scripts/VisualizationTopicsTabEntry.cs | 72 ++++++++++--------- 11 files changed, 102 insertions(+), 65 deletions(-) diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageRegistry.cs b/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageRegistry.cs index d0005a83..45c15ee0 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageRegistry.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageRegistry.cs @@ -18,12 +18,14 @@ class RegistryEntry { public static string s_RosMessageName; public static Func s_DeserializeFunction; + public static MessageSubtopic s_Subtopic; } public static void Register(string rosMessageName, Func deserialize, MessageSubtopic subtopic = MessageSubtopic.Default) where T : Message { RegistryEntry.s_RosMessageName = rosMessageName; RegistryEntry.s_DeserializeFunction = deserialize; + RegistryEntry.s_Subtopic = subtopic; if (s_DeserializeFunctionsByName[(int)subtopic].ContainsKey(rosMessageName)) Debug.LogWarning($"More than one message was registered as \"{rosMessageName}\" \"{subtopic}\""); s_DeserializeFunctionsByName[(int)subtopic][rosMessageName] = deserialize; @@ -45,5 +47,10 @@ public static string GetRosMessageName() where T : Message { return RegistryEntry.s_RosMessageName; } + + public static MessageSubtopic GetSubtopic() where T : Message + { + return RegistryEntry.s_Subtopic; + } } } 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 8fbaa5f2..d4dfcb3e 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSConnection.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSConnection.cs @@ -142,9 +142,9 @@ public void ListenForTopics(Action callback, bool notifyAllExisti } } - RosTopicState AddTopic(string topic, string rosMessageName) + RosTopicState AddTopic(string topic, string rosMessageName, bool isService = false) { - RosTopicState newTopic = new RosTopicState(topic, rosMessageName, this, new InternalAPI(this)); + RosTopicState newTopic = new RosTopicState(topic, rosMessageName, this, new InternalAPI(this), isService); lock (m_Topics) { m_Topics.Add(topic, newTopic); @@ -161,7 +161,7 @@ public RosTopicState GetTopic(string topic) public IEnumerable AllTopics => m_Topics.Values; - public RosTopicState GetOrCreateTopic(string topic, string rosMessageName) + public RosTopicState GetOrCreateTopic(string topic, string rosMessageName, bool isService = false) { RosTopicState state = GetTopic(topic); if (state != null) @@ -173,7 +173,7 @@ public RosTopicState GetOrCreateTopic(string topic, string rosMessageName) return state; } - RosTopicState result = AddTopic(topic, rosMessageName); + RosTopicState result = AddTopic(topic, rosMessageName, isService); foreach (Action callback in m_NewTopicCallbacks) { callback(result); @@ -243,7 +243,7 @@ public void ImplementService(string topic, Func(string topic, Func(string topic, Func(rawResponse); return result; } @@ -356,7 +357,7 @@ public void RegisterRosService(string topic) where TRequest public void RegisterRosService(string topic, string requestMessageName, string responseMessageName, int? queueSize = null) { - RosTopicState info = GetOrCreateTopic(topic, requestMessageName); + RosTopicState info = GetOrCreateTopic(topic, requestMessageName, isService: true); int resolvedQueueSize = queueSize.GetValueOrDefault(k_DefaultPublisherQueueSize); info.RegisterRosService(responseMessageName, resolvedQueueSize); } diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/RosTopicState.cs b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/RosTopicState.cs index fe322c6a..135da98b 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/RosTopicState.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/RosTopicState.cs @@ -25,20 +25,21 @@ public class RosTopicState public bool IsPublisherLatched { get; private set; } public bool SentPublisherRegistration { get; private set; } - bool m_IsRosService; - public bool IsRosService => m_IsRosService; - ROSConnection m_Connection; public ROSConnection Connection => m_Connection; ROSConnection.InternalAPI m_ConnectionInternal; Func m_Deserializer; Func m_ServiceImplementation; - private Func> m_ServiceImplementationAsync; + Func> m_ServiceImplementationAsync; + RosTopicState m_ServiceResponseTopic; public RosTopicState ServiceResponseTopic => m_ServiceResponseTopic; + bool m_IsRosService; + public bool IsRosService => m_IsRosService; public bool IsUnityService => m_ServiceImplementation != null || m_ServiceImplementationAsync != null; + public bool IsService => m_ServiceResponseTopic != null || m_Subtopic == MessageSubtopic.Response; List> m_SubscriberCallbacks = new List>(); public bool HasSubscriberCallback => m_SubscriberCallbacks.Count > 0; @@ -49,13 +50,17 @@ public class RosTopicState public float LastMessageReceivedRealtime => m_LastMessageReceivedRealtime; public float LastMessageSentRealtime => m_LastMessageSentRealtime; - internal RosTopicState(string topic, string rosMessageName, ROSConnection connection, ROSConnection.InternalAPI connectionInternal, MessageSubtopic subtopic = MessageSubtopic.Default) + internal RosTopicState(string topic, string rosMessageName, ROSConnection connection, ROSConnection.InternalAPI connectionInternal, bool isService, MessageSubtopic subtopic = MessageSubtopic.Default) { m_Topic = topic; m_Subtopic = subtopic; m_RosMessageName = rosMessageName; m_Connection = connection; m_ConnectionInternal = connectionInternal; + if (isService && subtopic == MessageSubtopic.Default) + { + m_ServiceResponseTopic = new RosTopicState(topic, rosMessageName, m_Connection, m_ConnectionInternal, isService, MessageSubtopic.Response); + } } internal void ChangeRosMessageName(string rosMessageName) @@ -151,7 +156,7 @@ public void AddSubscriber(Action callback) void RegisterSubscriber(NetworkStream stream = null) { - if (m_Connection.HasConnectionThread && !SentSubscriberRegistration) + if (m_Connection.HasConnectionThread && !SentSubscriberRegistration && !IsService) { m_ConnectionInternal.SendSubscriberRegistration(m_Topic, m_RosMessageName, stream); SentSubscriberRegistration = true; @@ -174,7 +179,6 @@ public void ImplementService(Func impl 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); } @@ -188,7 +192,6 @@ public void ImplementService(Func 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); } @@ -224,7 +227,6 @@ public void RegisterRosService(string responseMessageName, int queueSize) { m_IsRosService = true; m_ConnectionInternal.SendRosServiceRegistration(m_Topic, m_RosMessageName); - m_ServiceResponseTopic = new RosTopicState(m_Topic, responseMessageName, m_Connection, m_ConnectionInternal, MessageSubtopic.Response); CreateMessageSender(queueSize); } @@ -233,6 +235,7 @@ internal void SendServiceRequest(Message requestMessage, int serviceId) m_ConnectionInternal.SendServiceRequest(serviceId); m_MessageSender.Queue(requestMessage); m_ConnectionInternal.AddSenderToQueue(m_MessageSender); + OnMessageSent(requestMessage); } internal void OnConnectionEstablished(NetworkStream stream) diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/BaseVisualFactory.cs b/com.unity.robotics.visualizations/Runtime/Scripts/BaseVisualFactory.cs index ebd56e6f..40748c8a 100644 --- a/com.unity.robotics.visualizations/Runtime/Scripts/BaseVisualFactory.cs +++ b/com.unity.robotics.visualizations/Runtime/Scripts/BaseVisualFactory.cs @@ -4,7 +4,7 @@ using Unity.Robotics.Visualizations; using Unity.Robotics.ROSTCPConnector.MessageGeneration; using UnityEngine; - +using Unity.Robotics.ROSTCPConnector; namespace Unity.Robotics.Visualizations { @@ -68,8 +68,23 @@ public virtual void Start() } else { - VisualFactoryRegistry.RegisterTopicVisualizer(m_Topic, this, Priority); + VisualFactoryRegistry.RegisterTopicVisualizer(m_Topic, this, Priority, MessageRegistry.GetSubtopic()); + } + } + + public static void ListenForMessages(string topic, System.Action callback) + { + RosTopicState state = ROSConnection.GetOrCreateInstance().GetTopic(topic); + MessageSubtopic subtopic = MessageRegistry.GetSubtopic(); + if (subtopic == MessageSubtopic.Response && state != null) + { + if (state.ServiceResponseTopic == null) + Debug.Log("Failed to subscribe to " + topic + " response!"); + else + state.ServiceResponseTopic.AddSubscriber(callback); } + else + state.AddSubscriber(callback); } } } diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/DrawingVisualizer.cs b/com.unity.robotics.visualizations/Runtime/Scripts/DrawingVisualizer.cs index a8eb2b0b..daafbacf 100644 --- a/com.unity.robotics.visualizations/Runtime/Scripts/DrawingVisualizer.cs +++ b/com.unity.robotics.visualizations/Runtime/Scripts/DrawingVisualizer.cs @@ -57,7 +57,7 @@ public DrawingVisual(string topic, DrawingVisualizer factory) m_Topic = topic; m_Factory = factory; - ROSConnection.GetOrCreateInstance().Subscribe(topic, AddMessage); + ListenForMessages(topic, AddMessage); } public virtual void AddMessage(Message message) diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/GuiVisualizer.cs b/com.unity.robotics.visualizations/Runtime/Scripts/GuiVisualizer.cs index 3b1b997e..92bdfb5d 100644 --- a/com.unity.robotics.visualizations/Runtime/Scripts/GuiVisualizer.cs +++ b/com.unity.robotics.visualizations/Runtime/Scripts/GuiVisualizer.cs @@ -33,7 +33,7 @@ public GuiVisual(string topic, GuiVisualizer factory) { m_Topic = topic; m_Factory = factory; - ROSConnection.GetOrCreateInstance().Subscribe(m_Topic, AddMessage); + ListenForMessages(topic, AddMessage); } public void AddMessage(Message message) diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/HistoryDrawingVisualizer.cs b/com.unity.robotics.visualizations/Runtime/Scripts/HistoryDrawingVisualizer.cs index e258f17a..fb081ba4 100644 --- a/com.unity.robotics.visualizations/Runtime/Scripts/HistoryDrawingVisualizer.cs +++ b/com.unity.robotics.visualizations/Runtime/Scripts/HistoryDrawingVisualizer.cs @@ -62,8 +62,7 @@ public HistoryDrawingVisual(string topic, HistoryDrawingVisualizer factory, i m_Topic = topic; m_Factory = factory; m_HistoryLength = historyLength; - - ROSConnection.GetOrCreateInstance().Subscribe(m_Topic, AddMessage); + ListenForMessages(topic, AddMessage); } public void AddMessage(Message message) diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/TextureVisualizer.cs b/com.unity.robotics.visualizations/Runtime/Scripts/TextureVisualizer.cs index 8be4ac7b..078e6a0b 100644 --- a/com.unity.robotics.visualizations/Runtime/Scripts/TextureVisualizer.cs +++ b/com.unity.robotics.visualizations/Runtime/Scripts/TextureVisualizer.cs @@ -42,8 +42,7 @@ public TextureVisual(string topic, TextureVisualizer factory) { m_Topic = topic; m_Factory = factory; - - ROSConnection.GetOrCreateInstance().Subscribe(topic, AddMessage); + ListenForMessages(topic, AddMessage); } public void AddMessage(Message message) diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/VisualFactoryRegistry.cs b/com.unity.robotics.visualizations/Runtime/Scripts/VisualFactoryRegistry.cs index 423f2e1e..57652049 100644 --- a/com.unity.robotics.visualizations/Runtime/Scripts/VisualFactoryRegistry.cs +++ b/com.unity.robotics.visualizations/Runtime/Scripts/VisualFactoryRegistry.cs @@ -51,9 +51,9 @@ public void Add(T value, int priority) static ToStringVisualizer s_DefaultVisualFactory = new ToStringVisualizer(MessageSubtopic.Default); static ToStringVisualizer s_DefaultResponseVisualFactory = new ToStringVisualizer(MessageSubtopic.Response); - public static void RegisterTypeVisualizer(IVisualFactory visualFactory, int priority = 0, MessageSubtopic subtopic = MessageSubtopic.Default) where MsgType : Message + public static void RegisterTypeVisualizer(IVisualFactory visualFactory, int priority = 0) where MsgType : Message { - RegisterTypeVisualizer(MessageRegistry.GetRosMessageName(), visualFactory, priority, subtopic); + RegisterTypeVisualizer(MessageRegistry.GetRosMessageName(), visualFactory, priority, MessageRegistry.GetSubtopic()); } public static void RegisterTypeVisualizer(string rosMessageName, IVisualFactory visualFactory, int priority = 0, MessageSubtopic subtopic = MessageSubtopic.Default) diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/VisualizationTopicsTab.cs b/com.unity.robotics.visualizations/Runtime/Scripts/VisualizationTopicsTab.cs index 1689389c..a7d334f4 100644 --- a/com.unity.robotics.visualizations/Runtime/Scripts/VisualizationTopicsTab.cs +++ b/com.unity.robotics.visualizations/Runtime/Scripts/VisualizationTopicsTab.cs @@ -2,6 +2,7 @@ using System.Collections.Generic; using System.Linq; using Unity.Robotics.ROSTCPConnector; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; using UnityEngine; namespace Unity.Robotics.Visualizations @@ -193,9 +194,7 @@ public void AddRules(IEnumerable rules) { if (rule == null) continue; - var save = rule.CreateSaveState(); - if (save != null) - topicRuleSaves.Add(save); + rule.AddSaveStates(topicRuleSaves); } Rules = topicRuleSaves.ToArray(); @@ -240,9 +239,15 @@ void LoadLayout(HUDLayoutSave saveState) { foreach (var savedRule in saveState.Rules) { - RosTopicState topicState = m_Connection.GetOrCreateTopic(savedRule.Topic, savedRule.RosMessageName); - VisualizationTopicsTabEntry vis = new VisualizationTopicsTabEntry(savedRule, topicState, m_FillTexture); - m_Topics.Add(savedRule.Topic, vis); + RosTopicState topicState = m_Connection.GetOrCreateTopic(savedRule.Topic, savedRule.RosMessageName, savedRule.IsService); + VisualizationTopicsTabEntry vis; + if (!m_Topics.TryGetValue(savedRule.Topic, out vis)) + { + vis = new VisualizationTopicsTabEntry(topicState, m_FillTexture); + m_Topics.Add(savedRule.Topic, vis); + } + + vis.LoadSaveState(savedRule); } } diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/VisualizationTopicsTabEntry.cs b/com.unity.robotics.visualizations/Runtime/Scripts/VisualizationTopicsTabEntry.cs index f0cd586a..de84e4c4 100644 --- a/com.unity.robotics.visualizations/Runtime/Scripts/VisualizationTopicsTabEntry.cs +++ b/com.unity.robotics.visualizations/Runtime/Scripts/VisualizationTopicsTabEntry.cs @@ -170,6 +170,9 @@ public void DrawGUILine() public void SetVisualizing(bool ui, bool drawing) { + m_IsVisualizingUI = ui; + m_IsVisualizingDrawing = drawing; + if (m_VisualWindow != null) { m_VisualWindow.SetActive(ui); @@ -194,18 +197,17 @@ public void SetVisualizing(bool ui, bool drawing) m_VisualWindow.SetOnGUI(m_Visual.OnGUI); } } - - m_IsVisualizingUI = ui; - m_IsVisualizingDrawing = drawing; } [Serializable] public class SaveState { public string Topic; + public MessageSubtopic Subtopic; public string RosMessageName; public Rect Rect; public bool HasRect; + public bool IsService; public bool ShowWindow; public bool ShowDrawing; } @@ -223,29 +225,30 @@ public VisualizationTopicsTabEntry(RosTopicState baseState, Texture2D background m_CachedRosMessageName = RosMessageName; } - internal VisualizationTopicsTabEntry(SaveState save, RosTopicState topicState, Texture2D background) + public void LoadSaveState(SaveState save) { - m_TopicState = topicState; - m_Background = background; - - if (save != null) + if (m_TopicState.ServiceResponseTopic != null && save.Subtopic == MessageSubtopic.Response) { - if (save.HasRect && save.Rect.width > 0 && save.Rect.height > 0) - { - m_VisualWindow = new HudWindow(Title, save.Rect); - } - else if (save.ShowWindow) - { - m_VisualWindow = new HudWindow(Title); - } + // this save actually applies to the response topic + m_ServiceResponseTopic.LoadSaveState(save); + return; + } - if (m_VisualWindow != null) - { - HudPanel.AddWindow(m_VisualWindow); - } + if (save.HasRect && save.Rect.width > 0 && save.Rect.height > 0) + { + m_VisualWindow = new HudWindow(Title, save.Rect); + } + else if (save.ShowWindow) + { + m_VisualWindow = new HudWindow(Title); + } - SetVisualizing(save.ShowWindow, save.ShowDrawing); + if (m_VisualWindow != null) + { + HudPanel.AddWindow(m_VisualWindow); } + + SetVisualizing(save.ShowWindow, save.ShowDrawing); } void InitLineStyle() @@ -259,22 +262,27 @@ void InitLineStyle() m_HoverStyle.normal.background = m_Background; } - public SaveState CreateSaveState() + public void AddSaveStates(List results) { - if (!IsVisualizingUI && !IsVisualizingDrawing) + if (IsVisualizingUI || IsVisualizingDrawing) { - return null; + results.Add(new SaveState + { + Topic = m_TopicState.Topic, + Subtopic = m_TopicState.Subtopic, + RosMessageName = m_TopicState.RosMessageName, + Rect = m_VisualWindow != null ? m_VisualWindow.WindowRect : new Rect(0, 0, 0, 0), + HasRect = m_VisualWindow != null, + IsService = m_TopicState.ServiceResponseTopic != null || m_TopicState.Subtopic == MessageSubtopic.Response, + ShowWindow = m_IsVisualizingUI, + ShowDrawing = m_IsVisualizingDrawing, + }); } - return new SaveState + if (m_ServiceResponseTopic != null) { - Topic = m_TopicState.Topic, - RosMessageName = m_TopicState.RosMessageName, - Rect = m_VisualWindow != null ? m_VisualWindow.WindowRect : new Rect(0, 0, 0, 0), - HasRect = m_VisualWindow != null, - ShowWindow = m_IsVisualizingUI, - ShowDrawing = m_IsVisualizingDrawing, - }; + m_ServiceResponseTopic.AddSaveStates(results); + } } #if UNITY_EDITOR From eb23b5b105f3e0e7da42cb7ce8fbb67c7db85d6b Mon Sep 17 00:00:00 2001 From: Peter Smith Date: Fri, 15 Oct 2021 05:04:43 +1000 Subject: [PATCH 05/33] Fixing a bug for publishers publishing on a different thread to the main. Time.realtimeSinceStartup can't be used so use a cached value instead. (#211) Co-authored-by: LaurieCheers-unity <73140792+LaurieCheers-unity@users.noreply.github.com> --- .../Runtime/TcpConnector/RosTopicState.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/RosTopicState.cs b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/RosTopicState.cs index 135da98b..3d166b64 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/RosTopicState.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/RosTopicState.cs @@ -96,7 +96,7 @@ internal void OnMessageReceived(byte[] data) void OnMessageSent(Message message) { - m_LastMessageSentRealtime = Time.realtimeSinceStartup; + m_LastMessageSentRealtime = ROSConnection.s_RealTimeSinceStartup; if (m_RosMessageName == null) { ChangeRosMessageName(message.RosMessageName); @@ -210,7 +210,7 @@ public void RegisterPublisher(int queueSize, bool latch) public void Publish(Message message) { - m_LastMessageSentRealtime = Time.realtimeSinceStartup; + m_LastMessageSentRealtime = ROSConnection.s_RealTimeSinceStartup; OnMessageSent(message); m_MessageSender.Queue(message); m_ConnectionInternal.AddSenderToQueue(m_MessageSender); From ae2dea2b14a8dfcb7a80bd3bbd765eea1d57d165 Mon Sep 17 00:00:00 2001 From: Peter Smith Date: Fri, 15 Oct 2021 05:06:36 +1000 Subject: [PATCH 06/33] Adding additional helper functions to the GeometryCompass.cs (#209) * Adding additional helper functions to the GeometryCompass.cs * Changing GetWorldXZDirection and GetUnityYawDegrees to Extension functions ToWorldDirection and ToUnityYawDegrees respectively. Co-authored-by: LaurieCheers-unity <73140792+LaurieCheers-unity@users.noreply.github.com> --- .../Runtime/ROSGeometry/GeometryCompass.cs | 138 +++++++++++++++++- 1 file changed, 135 insertions(+), 3 deletions(-) diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/GeometryCompass.cs b/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/GeometryCompass.cs index 2aced3d6..24774681 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/GeometryCompass.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/GeometryCompass.cs @@ -15,8 +15,7 @@ public class GeometryCompass : ScriptableObject { public const string k_CompassSettingsAsset = "GeometryCompassSettings.asset"; - [SerializeField] - CardinalDirection m_ZAxisDirection; + [SerializeField] CardinalDirection m_ZAxisDirection; static GeometryCompass s_Instance; @@ -31,7 +30,7 @@ static GeometryCompass GetOrCreateSettings() { s_Instance = ScriptableObject.CreateInstance(); s_Instance.m_ZAxisDirection = CardinalDirection.North; - if(!UnityEditor.AssetDatabase.IsValidFolder("Assets/Resources")) + if (!UnityEditor.AssetDatabase.IsValidFolder("Assets/Resources")) UnityEditor.AssetDatabase.CreateFolder("Assets", "Resources"); UnityEditor.AssetDatabase.CreateAsset(s_Instance, assetPath); UnityEditor.AssetDatabase.SaveAssets(); @@ -68,4 +67,137 @@ public static CardinalDirection UnityZAxisDirection public static readonly Quaternion k_OneEightyYaw = Quaternion.Euler(0, 180, 0); public static readonly Quaternion k_NegativeNinetyYaw = Quaternion.Euler(0, -90, 0); } + + public static class GeometryCompassExtensions + { + public static Vector3 ToWorldDirection(this CardinalDirection desiredDirection) + { + switch (GeometryCompass.UnityZAxisDirection) + { + case CardinalDirection.North: + switch (desiredDirection) + { + case CardinalDirection.North: + return new Vector3(0, 0, 1); + case CardinalDirection.East: + return new Vector3(1, 0, 0); + case CardinalDirection.South: + return new Vector3(0, 0, -1); + case CardinalDirection.West: + return new Vector3(-1, 0, 0); + default: + throw new Exception($"Unsupported CardinalDirection: {desiredDirection}"); + } + case CardinalDirection.East: + switch (desiredDirection) + { + case CardinalDirection.North: + return new Vector3(-1, 0, 0); + case CardinalDirection.East: + return new Vector3(0, 0, 1); + case CardinalDirection.South: + return new Vector3(1, 0, 0); + case CardinalDirection.West: + return new Vector3(0, 0, -1); + default: + throw new Exception($"Unsupported CardinalDirection: {desiredDirection}"); + } + case CardinalDirection.South: + switch (desiredDirection) + { + case CardinalDirection.North: + return new Vector3(0, 0, -1); + case CardinalDirection.East: + return new Vector3(-1, 0, 0); + case CardinalDirection.South: + return new Vector3(0, 0, 1); + case CardinalDirection.West: + return new Vector3(1, 0, 0); + default: + throw new Exception($"Unsupported CardinalDirection: {desiredDirection}"); + } + case CardinalDirection.West: + switch (desiredDirection) + { + case CardinalDirection.North: + return new Vector3(1, 0, 0); + case CardinalDirection.East: + return new Vector3(0, 0, -1); + case CardinalDirection.South: + return new Vector3(-1, 0, 0); + case CardinalDirection.West: + return new Vector3(0, 0, 1); + default: + throw new Exception($"Unsupported CardinalDirection: {desiredDirection}"); + } + default: + throw new Exception($"Unsupported CardinalDirection: {GeometryCompass.UnityZAxisDirection}"); + } + } + + public static float ToUnityYawDegrees(this CardinalDirection desiredDirection) + { + switch (GeometryCompass.UnityZAxisDirection) + { + case CardinalDirection.North: + switch (desiredDirection) + { + case CardinalDirection.North: + return 0.0f; + case CardinalDirection.East: + return 90.0f; + case CardinalDirection.South: + return 180.0f; + case CardinalDirection.West: + return -90.0f; + default: + throw new Exception($"Unsupported CardinalDirection: {desiredDirection}"); + } + case CardinalDirection.East: + switch (desiredDirection) + { + case CardinalDirection.North: + return -90.0f; + case CardinalDirection.East: + return 0.0f; + case CardinalDirection.South: + return 90.0f; + case CardinalDirection.West: + return 180.0f; + default: + throw new Exception($"Unsupported CardinalDirection: {desiredDirection}"); + } + case CardinalDirection.South: + switch (desiredDirection) + { + case CardinalDirection.North: + return 180.0f; + case CardinalDirection.East: + return -90.0f; + case CardinalDirection.South: + return 0.0f; + case CardinalDirection.West: + return 90.0f; + default: + throw new Exception($"Unsupported CardinalDirection: {desiredDirection}"); + } + case CardinalDirection.West: + switch (desiredDirection) + { + case CardinalDirection.North: + return 90.0f; + case CardinalDirection.East: + return 180.0f; + case CardinalDirection.South: + return -90.0f; + case CardinalDirection.West: + return 0.0f; + default: + throw new Exception($"Unsupported CardinalDirection: {desiredDirection}"); + } + default: + throw new Exception($"Unsupported CardinalDirection: {GeometryCompass.UnityZAxisDirection}"); + } + } + } } From 00281fc70513540b84fee98c3a8d2d6d90dd85f0 Mon Sep 17 00:00:00 2001 From: chuck-saunders <92817584+chuck-saunders@users.noreply.github.com> Date: Tue, 19 Oct 2021 18:35:47 -0400 Subject: [PATCH 07/33] Rename HUDPanel.cs to HudPanel.cs (#216) The class name was changed from "HUDPanel" to "HudPanel" so the script filename also needs to change - Unity fails to load or run the script as it is currently written. The alternative to renaming the file would be to rename the class back to HUDPanel, but I can see it's at least used in the interface, and I don't know where else. Changing the file name is the easiest/least painful way to fix this. Co-authored-by: LaurieCheers-unity <73140792+LaurieCheers-unity@users.noreply.github.com> --- .../Runtime/TcpConnector/{HUDPanel.cs => HudPanel.cs} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/{HUDPanel.cs => HudPanel.cs} (100%) diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/HUDPanel.cs b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/HudPanel.cs similarity index 100% rename from com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/HUDPanel.cs rename to com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/HudPanel.cs From c12d0b80f44c91b1ababd4617c25fa223b8e8d07 Mon Sep 17 00:00:00 2001 From: LaurieCheers-unity <73140792+LaurieCheers-unity@users.noreply.github.com> Date: Wed, 20 Oct 2021 09:50:25 -0700 Subject: [PATCH 08/33] Prefer to use the RosConnection from the scene (#217) --- .../Runtime/TcpConnector/ROSConnection.cs | 5 +++++ 1 file changed, 5 insertions(+) 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 d4dfcb3e..212680a0 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSConnection.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSConnection.cs @@ -439,6 +439,11 @@ public static ROSConnection GetOrCreateInstance() { if (_instance == null) { + // Prefer to use the ROSConnection in the scene, if any + _instance = FindObjectOfType(); + if (_instance != null) + return _instance; + GameObject prefab = Resources.Load("ROSConnectionPrefab"); if (prefab == null) { From 57eff381c938dd361c51c1e3494592a36e9a85a4 Mon Sep 17 00:00:00 2001 From: LaurieCheers-unity <73140792+LaurieCheers-unity@users.noreply.github.com> Date: Wed, 20 Oct 2021 10:13:04 -0700 Subject: [PATCH 09/33] Laurie/point cloud shader fix (#218) Fixes from ROSConWorkshop branch --- .../Drawing3d/Shaders/UnlitPointCloudAdditive.shader | 6 +++++- .../Runtime/Drawing3d/Shaders/UnlitPointCloudCutout.shader | 6 +++++- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitPointCloudAdditive.shader b/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitPointCloudAdditive.shader index 64b158fb..da4056cd 100644 --- a/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitPointCloudAdditive.shader +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitPointCloudAdditive.shader @@ -47,9 +47,13 @@ ); float2 uv = v.uv; - //Packing index into color.a: float2 uv = float2(round(v.color.a * 255 * 0.4), round(v.color.a * 255 % 2)); + //If packing index into color.a: float2 uv = float2(round(v.color.a * 255 * 0.4), round(v.color.a * 255 % 2)); o.vertex.x += (uv.x - 0.5) * 2 * _Radius * _ScreenParams.y / _ScreenParams.x; +#if UNITY_UV_STARTS_AT_TOP o.vertex.y -= (uv.y - 0.5) * 2 * _Radius; +#else + o.vertex.y += (uv.y - 0.5) * 2 * _Radius; +#endif o.uv = uv; o.color = v.color; return o; diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitPointCloudCutout.shader b/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitPointCloudCutout.shader index e2f7543f..1832e1b9 100644 --- a/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitPointCloudCutout.shader +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitPointCloudCutout.shader @@ -46,9 +46,13 @@ float2 uv = v.uvr.xy; float radius = v.uvr.z; - //Pack index into color.a: float2 uv = float2(round(v.color.a * 255 * 0.4), round(v.color.a * 255 % 2)); + //If packing index into color.a: float2 uv = float2(round(v.color.a * 255 * 0.4), round(v.color.a * 255 % 2)); o.vertex.x += (uv.x - 0.5) * 2 * radius * _ScreenParams.y / _ScreenParams.x; +#if UNITY_UV_STARTS_AT_TOP o.vertex.y -= (uv.y - 0.5) * 2 * radius; +#else + o.vertex.y += (uv.y - 0.5) * 2 * radius; +#endif o.uv = uv; o.color = v.color; o.color.a = 1; From 8a0471e810414cffc112a6b8d331e40a977ad3cf Mon Sep 17 00:00:00 2001 From: LaurieCheers-unity <73140792+LaurieCheers-unity@users.noreply.github.com> Date: Wed, 20 Oct 2021 10:36:48 -0700 Subject: [PATCH 10/33] Make package naming for services match messages. (#214) Also don't make an invalid namespace if there's a hyphen in a package name. --- .../Editor/MessageGeneration/MsgAutoGenUtilities.cs | 1 + .../Editor/MessageGeneration/ServiceAutoGen.cs | 3 +-- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/MsgAutoGenUtilities.cs b/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/MsgAutoGenUtilities.cs index f10398bc..987b872d 100644 --- a/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/MsgAutoGenUtilities.cs +++ b/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/MsgAutoGenUtilities.cs @@ -129,6 +129,7 @@ public static string PascalCase(string s) public static string ResolvePackageName(string s) { + s = s.Replace('-', '_'); if (s.Contains("_msgs") || s.Contains("_srvs") || s.Contains("_actions")) { return PascalCase(s.Substring(0, s.LastIndexOf('_'))); diff --git a/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/ServiceAutoGen.cs b/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/ServiceAutoGen.cs index 569cd751..02d7e8a4 100644 --- a/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/ServiceAutoGen.cs +++ b/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/ServiceAutoGen.cs @@ -41,8 +41,7 @@ public static List GenerateSingleService(string inPath, string outPath, // If no ROS package name is provided, extract from path if (rosPackageName.Equals("")) { - string[] hierarchy = inPath.Split(new char[] { '/', '\\' }); - rosPackageName = hierarchy[hierarchy.Length - 3]; + rosPackageName = MessageAutoGen.GetRosPackageName(inPath); } outPath = Path.Combine(outPath, MsgAutoGenUtilities.ResolvePackageName(rosPackageName)); From f44c408e8c9548f464435392b1f8d3e046ca17d2 Mon Sep 17 00:00:00 2001 From: LaurieCheers-unity <73140792+LaurieCheers-unity@users.noreply.github.com> Date: Wed, 20 Oct 2021 15:02:57 -0700 Subject: [PATCH 11/33] Added MessagePool class (#213) * Added MessagePool class * MessagePool uses ConcurrentQueue * Minor API tweaks * XML comment --- .../Runtime/TcpConnector/MessagePool.cs | 60 +++++++++++++++ .../Runtime/TcpConnector/MessagePool.cs.meta | 11 +++ .../Runtime/TcpConnector/ROSConnection.cs | 10 --- .../Runtime/TcpConnector/RosTopicState.cs | 7 +- .../TcpConnector/TopicMessageSender.cs | 73 ++----------------- 5 files changed, 83 insertions(+), 78 deletions(-) create mode 100644 com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/MessagePool.cs create mode 100644 com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/MessagePool.cs.meta diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/MessagePool.cs b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/MessagePool.cs new file mode 100644 index 00000000..30f1bce1 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/MessagePool.cs @@ -0,0 +1,60 @@ +using System.Collections; +using System.Collections.Concurrent; +using System.Collections.Generic; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using UnityEngine; + +namespace Unity.Robotics.ROSTCPConnector +{ + public interface IMessagePool + { + void AddMessage(Message messageToRecycle); + } + + public class MessagePool : IMessagePool where T : Message, new() + { + ConcurrentQueue m_Contents; + int m_MaxSize; + + public MessagePool(int maxSize = 10) + { + m_MaxSize = maxSize; + m_Contents = new ConcurrentQueue(); + } + + public void AddMessage(T messageToRecycle) + { + // not bothering to lock here - if multiple threads add things simultaneously we could end up a little over m_MaxSize but it's not a big deal + if (m_Contents.Count < m_MaxSize) + { + m_Contents.Enqueue(messageToRecycle); + } + } + + void IMessagePool.AddMessage(Message message) + { + if (!(message is T)) + { + throw new System.ArgumentException($"MessagePool<{MessageRegistry.GetRosMessageName()}> can't store a \"{message.RosMessageName}\" message."); + } + + AddMessage((T)message); + } + + /// + /// Get a message from the pool, or create one if necessary. + /// + /// A new or recycled message of type T, which may contain garbage data. Be sure to fill it in. + public T GetOrCreateMessage() + { + T result; + + if (!m_Contents.TryDequeue(out result)) + { + result = new T(); + } + + return result; + } + } +} diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/MessagePool.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/MessagePool.cs.meta new file mode 100644 index 00000000..09e6394d --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/MessagePool.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 93fe324c7f4cd194cb6e7bdf8d9bbd2d +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: 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 212680a0..138b82ac 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSConnection.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSConnection.cs @@ -961,16 +961,6 @@ public void Publish(string rosTopicName, Message message) } } - public T GetFromPool(string rosTopicName) where T : Message - { - RosTopicState topicState = GetTopic(rosTopicName); - if (topicState != null) - { - return (T)topicState.GetMessageFromPool(); - } - throw new Exception($"No publisher on topic {rosTopicName} of type {MessageRegistry.GetRosMessageName()} to get pooled messages from!"); - } - void InitializeHUD() { if (!Application.isPlaying || (!m_ShowHUD && m_HudPanel == null)) diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/RosTopicState.cs b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/RosTopicState.cs index 3d166b64..02f47baf 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/RosTopicState.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/RosTopicState.cs @@ -216,13 +216,16 @@ public void Publish(Message message) m_ConnectionInternal.AddSenderToQueue(m_MessageSender); } - public Message GetMessageFromPool() => m_MessageSender.GetMessageFromPool(); - void CreateMessageSender(int queueSize) { m_MessageSender = new TopicMessageSender(Topic, m_RosMessageName, queueSize); } + public void SetMessagePool(IMessagePool messagePool) + { + m_MessageSender.SetMessagePool(messagePool); + } + public void RegisterRosService(string responseMessageName, int queueSize) { m_IsRosService = true; diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TopicMessageSender.cs b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TopicMessageSender.cs index e49abed7..c9a7a8ea 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TopicMessageSender.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TopicMessageSender.cs @@ -29,7 +29,8 @@ public class TopicMessageSender : OutgoingMessageSender Message m_LastMessageSent = null; //Optional, used if you want to pool messages and reuse them when they are no longer in use. - Queue m_InactiveMessagePool = new Queue(); + IMessagePool m_MessagePool; + public bool MessagePoolEnabled => m_MessagePool != null; public TopicMessageSender(string topicName, string rosMessageName, int queueSize) { @@ -163,76 +164,16 @@ public override void ClearAllQueuedData() void TryRecycleMessage(Message toRecycle) { - if (!MessagePoolEnabled) + if (m_MessagePool != null) { - return; - } - - //Add the message back to the pool. - AddMessageToPool(toRecycle); - } - - #region Message Pooling - - //Whether you want to pool messages for reuse (Used to reduce GC calls). - volatile bool m_MessagePoolEnabled; - - public bool MessagePoolEnabled => m_MessagePoolEnabled; - - public void SetMessagePoolEnabled(bool enabled) - { - if (m_MessagePoolEnabled == enabled) - return; - - m_MessagePoolEnabled = enabled; - if (!m_MessagePoolEnabled) - { - lock (m_InactiveMessagePool) - { - m_InactiveMessagePool.Clear(); - } - } - } - - public void AddMessageToPool(Message messageToRecycle) - { - Debug.Assert(MessagePoolEnabled, - "Adding a message to a message pool that is not enabled, please set MessagePoolEnabled to true."); - - lock (m_InactiveMessagePool) - { - if (MessagePoolEnabled && m_InactiveMessagePool.Count < (QueueSize + 5)) - { - //Make sure we're only pooling a reasonable amount. - //We shouldn't need any more than the queue size plus a little. - m_InactiveMessagePool.Enqueue(messageToRecycle); - } + //Add the message back to the pool. + m_MessagePool.AddMessage(toRecycle); } } - /** - * @return a message of type T full of garbage data, be sure to update it accordingly. - */ - public Message GetMessageFromPool() + public void SetMessagePool(IMessagePool messagePool) { - Message result = null; - SetMessagePoolEnabled(true); - lock (m_InactiveMessagePool) - { - if (m_InactiveMessagePool.Count > 0) - { - result = m_InactiveMessagePool.Dequeue(); - } - } - - if (result == null) - { - result = Activator.CreateInstance(); - } - - return result; + m_MessagePool = messagePool; } - - #endregion } } From b03467816be6534bec44c0040b34ad8838658ffe Mon Sep 17 00:00:00 2001 From: Amanda <31416491+at669@users.noreply.github.com> Date: Fri, 22 Oct 2021 11:11:12 -0600 Subject: [PATCH 12/33] AIRO-1345, AIRO-1134: Readme updates (#222) --- README.md | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 5077362f..f3753369 100644 --- a/README.md +++ b/README.md @@ -2,11 +2,16 @@ [![Version](https://img.shields.io/github/v/tag/Unity-Technologies/ROS-TCP-Connector)](https://github.com/Unity-Technologies/ROS-TCP-Connector/releases) [![License](https://img.shields.io/badge/license-Apache--2.0-green.svg)](LICENSE.md) -![ROS](https://img.shields.io/badge/ros-melodic-brightgreen) -![ROS](https://img.shields.io/badge/ros-noetic-brightgreen) -![ROS](https://img.shields.io/badge/ros2-foxy-brightgreen) +![ROS](https://img.shields.io/badge/ros-melodic,noetic-brightgreen) +![ROS](https://img.shields.io/badge/ros2-foxy,galactic-brightgreen) ![Unity](https://img.shields.io/badge/unity-2020.2+-brightgreen) +--- + +We're currently working on lots of things! Please take a short moment fill out our [survey](https://unitysoftware.co1.qualtrics.com/jfe/form/SV_0ojVkDVW0nNrHkW) to help us identify what products and packages to build next. + +--- + ## Introduction This repository contains two Unity packages: the ROS TCP Connector, for sending/receiving messages from ROS, and the Visualizations Package, for adding visualizations of incoming and outgoing messages in the Unity scene. From 468b28fbb3d5c4f183248aefec8cef794be468a8 Mon Sep 17 00:00:00 2001 From: LaurieCheers-unity <73140792+LaurieCheers-unity@users.noreply.github.com> Date: Fri, 22 Oct 2021 17:02:29 -0700 Subject: [PATCH 13/33] Delete old metafiles (#224) --- .../Runtime/Messages/HandwrittenMessages/srv.meta | 8 -------- com.unity.robotics.ros-tcp-connector/Tools.meta | 8 -------- 2 files changed, 16 deletions(-) delete mode 100644 com.unity.robotics.ros-tcp-connector/Runtime/Messages/HandwrittenMessages/srv.meta delete mode 100644 com.unity.robotics.ros-tcp-connector/Tools.meta diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/HandwrittenMessages/srv.meta b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/HandwrittenMessages/srv.meta deleted file mode 100644 index 5737fc8d..00000000 --- a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/HandwrittenMessages/srv.meta +++ /dev/null @@ -1,8 +0,0 @@ -fileFormatVersion: 2 -guid: 7616fee199c3ce545a29adf052c60af4 -folderAsset: yes -DefaultImporter: - externalObjects: {} - userData: - assetBundleName: - assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Tools.meta b/com.unity.robotics.ros-tcp-connector/Tools.meta deleted file mode 100644 index cce9f940..00000000 --- a/com.unity.robotics.ros-tcp-connector/Tools.meta +++ /dev/null @@ -1,8 +0,0 @@ -fileFormatVersion: 2 -guid: bea5d2e440e289646985b5afccde8e04 -folderAsset: yes -DefaultImporter: - externalObjects: {} - userData: - assetBundleName: - assetBundleVariant: From 1488ad24ad0f7887438462fb8ad463512d53cb3c Mon Sep 17 00:00:00 2001 From: peifeng-unity <56408141+peifeng-unity@users.noreply.github.com> Date: Wed, 27 Oct 2021 11:18:43 -0700 Subject: [PATCH 14/33] add cloud rendering (#225) --- .../Runtime/Unity.Robotics.ROSTCPConnector.asmdef | 1 + 1 file changed, 1 insertion(+) diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Unity.Robotics.ROSTCPConnector.asmdef b/com.unity.robotics.ros-tcp-connector/Runtime/Unity.Robotics.ROSTCPConnector.asmdef index ca790bfc..06ea6b7c 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/Unity.Robotics.ROSTCPConnector.asmdef +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Unity.Robotics.ROSTCPConnector.asmdef @@ -3,6 +3,7 @@ "rootNamespace": "Unity.Robotics.ROSTCPConnector", "references": [], "includePlatforms": [ + "CloudRendering", "Editor", "LinuxStandalone64", "macOSStandalone", From fde82a815b7b5f4d20f770988a6c5b639dbfed2f Mon Sep 17 00:00:00 2001 From: peifeng-unity <56408141+peifeng-unity@users.noreply.github.com> Date: Fri, 29 Oct 2021 10:52:08 -0700 Subject: [PATCH 15/33] add jira link check (#226) --- .github/workflows/jira-link.yaml | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) create mode 100644 .github/workflows/jira-link.yaml diff --git a/.github/workflows/jira-link.yaml b/.github/workflows/jira-link.yaml new file mode 100644 index 00000000..18240fb7 --- /dev/null +++ b/.github/workflows/jira-link.yaml @@ -0,0 +1,22 @@ +name: jira-link + +on: + pull_request: + types: [opened, edited, reopened, synchronize] + +jobs: + jira-link: + runs-on: ubuntu-20.04 + steps: + - name: check pull request title and source branch name + run: | + echo "Checking pull request with title ${{ github.event.pull_request.title }} from source branch ${{ github.event.pull_request.head.ref }}" + if ! [[ "${{ github.event.pull_request.title }}" =~ ^AIRO-[0-9]+[[:space:]].*$ ]] && ! [[ "${{ github.event.pull_request.head.ref }}" =~ ^AIRO-[0-9]+.*$ ]] + then + echo -e "Please make sure one of the following is true:\n \ + 1. the pull request title starts with 'AIRO-xxxx ', e.g. 'AIRO-1024 My Pull Request'\n \ + 2. the source branch starts with 'AIRO-xxx', e.g. 'AIRO-1024-my-branch'" + exit 1 + else + echo "Completed checking" + fi From 2451a6d717c5964c3902864b6509a562073c915a Mon Sep 17 00:00:00 2001 From: "Devin Miller (Unity)" Date: Tue, 2 Nov 2021 09:07:53 -0700 Subject: [PATCH 16/33] Restructuring directories for better code coverage analysis (#230) --- .yamato/yamato-config.yml | 6 ++-- TestRosTcpConnector/.gitignore | 1 + TestRosTcpConnector/Assets/Resources.meta | 8 ++++++ .../Resources/GeometryCompassSettings.asset | 15 ++++++++++ .../GeometryCompassSettings.asset.meta | 8 ++++++ .../Assets/Settings/ForwardRenderer.asset | 6 ++++ .../Settings.json | 28 +++++++++++++++++++ ...ity.Robotics.ROSTCPConnector.Editor.asmdef | 3 +- .../Runtime/Extensions.meta | 8 ++++++ .../CameraInfoGenerator.cs | 0 .../CameraInfoGenerator.cs.meta | 0 .../MatrixExtensions.cs | 0 .../MatrixExtensions.cs.meta | 0 .../MessageExtensions.cs | 0 .../MessageExtensions.cs.meta | 0 ...s.ROSTCPConnector.MessageGeneration.asmdef | 3 ++ ...TCPConnector.MessageGeneration.asmdef.meta | 7 +++++ .../MessageBackwardsCompat.cs | 0 .../MessageBackwardsCompat.cs.meta | 0 ...y.Robotics.ROSTCPConnector.Messages.asmdef | 16 +++++++++++ ...otics.ROSTCPConnector.Messages.asmdef.meta | 7 +++++ .../Unity.Robotics.ROSTCPConnector.asmdef | 5 +++- .../Unity.Perception.Runtime.Tests.asmdef | 4 ++- 23 files changed, 118 insertions(+), 7 deletions(-) create mode 100644 TestRosTcpConnector/Assets/Resources.meta create mode 100644 TestRosTcpConnector/Assets/Resources/GeometryCompassSettings.asset create mode 100644 TestRosTcpConnector/Assets/Resources/GeometryCompassSettings.asset.meta create mode 100644 TestRosTcpConnector/ProjectSettings/Packages/com.unity.testtools.codecoverage/Settings.json create mode 100644 com.unity.robotics.ros-tcp-connector/Runtime/Extensions.meta rename com.unity.robotics.ros-tcp-connector/Runtime/{MessageGeneration => Extensions}/CameraInfoGenerator.cs (100%) rename com.unity.robotics.ros-tcp-connector/Runtime/{MessageGeneration => Extensions}/CameraInfoGenerator.cs.meta (100%) rename com.unity.robotics.ros-tcp-connector/Runtime/{MessageGeneration => Extensions}/MatrixExtensions.cs (100%) rename com.unity.robotics.ros-tcp-connector/Runtime/{MessageGeneration => Extensions}/MatrixExtensions.cs.meta (100%) rename com.unity.robotics.ros-tcp-connector/Runtime/{MessageGeneration => Extensions}/MessageExtensions.cs (100%) rename com.unity.robotics.ros-tcp-connector/Runtime/{MessageGeneration => Extensions}/MessageExtensions.cs.meta (100%) create mode 100644 com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/Unity.Robotics.ROSTCPConnector.MessageGeneration.asmdef create mode 100644 com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/Unity.Robotics.ROSTCPConnector.MessageGeneration.asmdef.meta rename com.unity.robotics.ros-tcp-connector/Runtime/{MessageGeneration => Messages}/MessageBackwardsCompat.cs (100%) rename com.unity.robotics.ros-tcp-connector/Runtime/{MessageGeneration => Messages}/MessageBackwardsCompat.cs.meta (100%) create mode 100644 com.unity.robotics.ros-tcp-connector/Runtime/Messages/Unity.Robotics.ROSTCPConnector.Messages.asmdef create mode 100644 com.unity.robotics.ros-tcp-connector/Runtime/Messages/Unity.Robotics.ROSTCPConnector.Messages.asmdef.meta diff --git a/.yamato/yamato-config.yml b/.yamato/yamato-config.yml index 1f37a9a0..156b185d 100644 --- a/.yamato/yamato-config.yml +++ b/.yamato/yamato-config.yml @@ -18,10 +18,8 @@ commands: triggers: cancel_old_ci: true expression: | - (pull_request.target eq "main" AND - NOT pull_request.push.changes.all match "**/*.md") OR - (pull_request.target eq "dev" AND - NOT pull_request.push.changes.all match "**/*.md") + (pull_request.target eq "main" OR push.branch eq "main" OR pull_request.target eq "dev" OR push.branch eq "dev") + AND NOT pull_request.push.changes.all match "**/*.md" artifacts: logs: paths: diff --git a/TestRosTcpConnector/.gitignore b/TestRosTcpConnector/.gitignore index 7a373b94..5c9291b3 100644 --- a/TestRosTcpConnector/.gitignore +++ b/TestRosTcpConnector/.gitignore @@ -11,6 +11,7 @@ /[Bb]uild.app/ /[Ll]ogs/ /[Uu]ser[Ss]ettings/ +/[Cc]ode[Cc]overage/ # MemoryCaptures can get excessive in size. # They also could contain extremely sensitive data diff --git a/TestRosTcpConnector/Assets/Resources.meta b/TestRosTcpConnector/Assets/Resources.meta new file mode 100644 index 00000000..2b2d2b2b --- /dev/null +++ b/TestRosTcpConnector/Assets/Resources.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: ac470971a18ed3c43a90f472a8d45bbd +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/TestRosTcpConnector/Assets/Resources/GeometryCompassSettings.asset b/TestRosTcpConnector/Assets/Resources/GeometryCompassSettings.asset new file mode 100644 index 00000000..86206906 --- /dev/null +++ b/TestRosTcpConnector/Assets/Resources/GeometryCompassSettings.asset @@ -0,0 +1,15 @@ +%YAML 1.1 +%TAG !u! tag:unity3d.com,2011: +--- !u!114 &11400000 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 0} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 6ad8cf01110e54df290b36a418e91285, type: 3} + m_Name: GeometryCompassSettings + m_EditorClassIdentifier: + m_ZAxisDirection: 3 diff --git a/TestRosTcpConnector/Assets/Resources/GeometryCompassSettings.asset.meta b/TestRosTcpConnector/Assets/Resources/GeometryCompassSettings.asset.meta new file mode 100644 index 00000000..fc6eda59 --- /dev/null +++ b/TestRosTcpConnector/Assets/Resources/GeometryCompassSettings.asset.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: c00bf9f40eec1ae4eac920dc5f953f97 +NativeFormatImporter: + externalObjects: {} + mainObjectFileID: 11400000 + userData: + assetBundleName: + assetBundleVariant: diff --git a/TestRosTcpConnector/Assets/Settings/ForwardRenderer.asset b/TestRosTcpConnector/Assets/Settings/ForwardRenderer.asset index 53fa2c4f..b9a801ce 100644 --- a/TestRosTcpConnector/Assets/Settings/ForwardRenderer.asset +++ b/TestRosTcpConnector/Assets/Settings/ForwardRenderer.asset @@ -21,7 +21,11 @@ MonoBehaviour: copyDepthPS: {fileID: 4800000, guid: d6dae50ee9e1bfa4db75f19f99355220, type: 3} screenSpaceShadowPS: {fileID: 4800000, guid: 0f854b35a0cf61a429bd5dcfea30eddd, type: 3} samplingPS: {fileID: 4800000, guid: 04c410c9937594faa893a11dceb85f7e, type: 3} + tileDepthInfoPS: {fileID: 0} + tileDeferredPS: {fileID: 0} + stencilDeferredPS: {fileID: 4800000, guid: e9155b26e1bc55942a41e518703fe304, type: 3} fallbackErrorPS: {fileID: 4800000, guid: e6e9a19c3678ded42a3bc431ebef7dbd, type: 3} + materialErrorPS: {fileID: 4800000, guid: 5fd9a8feb75a4b5894c241777f519d4e, type: 3} m_OpaqueLayerMask: serializedVersion: 2 m_Bits: 4294967295 @@ -36,3 +40,5 @@ MonoBehaviour: failOperation: 0 zFailOperation: 0 m_ShadowTransparentReceive: 1 + m_RenderingMode: 0 + m_AccurateGbufferNormals: 0 diff --git a/TestRosTcpConnector/ProjectSettings/Packages/com.unity.testtools.codecoverage/Settings.json b/TestRosTcpConnector/ProjectSettings/Packages/com.unity.testtools.codecoverage/Settings.json new file mode 100644 index 00000000..d828ddd0 --- /dev/null +++ b/TestRosTcpConnector/ProjectSettings/Packages/com.unity.testtools.codecoverage/Settings.json @@ -0,0 +1,28 @@ +{ + "m_Name": "Settings", + "m_Path": "ProjectSettings/Packages/com.unity.testtools.codecoverage/Settings.json", + "m_Dictionary": { + "m_DictionaryValues": [ + { + "type": "System.String, mscorlib, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089", + "key": "Path", + "value": "{\"m_Value\":\"{ProjectPath}\"}" + }, + { + "type": "System.String, mscorlib, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089", + "key": "HistoryPath", + "value": "{\"m_Value\":\"{ProjectPath}\"}" + }, + { + "type": "System.String, mscorlib, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089", + "key": "IncludeAssemblies", + "value": "{\"m_Value\":\"Unity.Robotics.ROSTCPConnector,Unity.Robotics.ROSTCPConnector.Editor,Unity.Robotics.ROSTCPConnector.MessageGeneration\"}" + }, + { + "type": "System.Boolean, mscorlib, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089", + "key": "EnableCodeCoverage", + "value": "{\"m_Value\":true}" + } + ] + } +} \ No newline at end of file diff --git a/com.unity.robotics.ros-tcp-connector/Editor/Unity.Robotics.ROSTCPConnector.Editor.asmdef b/com.unity.robotics.ros-tcp-connector/Editor/Unity.Robotics.ROSTCPConnector.Editor.asmdef index 93e8276c..e81257b6 100644 --- a/com.unity.robotics.ros-tcp-connector/Editor/Unity.Robotics.ROSTCPConnector.Editor.asmdef +++ b/com.unity.robotics.ros-tcp-connector/Editor/Unity.Robotics.ROSTCPConnector.Editor.asmdef @@ -2,7 +2,8 @@ "name": "Unity.Robotics.ROSTCPConnector.Editor", "rootNamespace": "", "references": [ - "GUID:625bfc588fb96c74696858f2c467e978" + "GUID:625bfc588fb96c74696858f2c467e978", + "GUID:5db6cbfd5d0e86a46862a70f4b4067f2" ], "includePlatforms": [ "Editor" diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Extensions.meta b/com.unity.robotics.ros-tcp-connector/Runtime/Extensions.meta new file mode 100644 index 00000000..58b7eba9 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Extensions.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: b2760f5cab166f94cb3c026151c4b63b +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/CameraInfoGenerator.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Extensions/CameraInfoGenerator.cs similarity index 100% rename from com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/CameraInfoGenerator.cs rename to com.unity.robotics.ros-tcp-connector/Runtime/Extensions/CameraInfoGenerator.cs diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/CameraInfoGenerator.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/Extensions/CameraInfoGenerator.cs.meta similarity index 100% rename from com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/CameraInfoGenerator.cs.meta rename to com.unity.robotics.ros-tcp-connector/Runtime/Extensions/CameraInfoGenerator.cs.meta diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MatrixExtensions.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Extensions/MatrixExtensions.cs similarity index 100% rename from com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MatrixExtensions.cs rename to com.unity.robotics.ros-tcp-connector/Runtime/Extensions/MatrixExtensions.cs diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MatrixExtensions.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/Extensions/MatrixExtensions.cs.meta similarity index 100% rename from com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MatrixExtensions.cs.meta rename to com.unity.robotics.ros-tcp-connector/Runtime/Extensions/MatrixExtensions.cs.meta diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageExtensions.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Extensions/MessageExtensions.cs similarity index 100% rename from com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageExtensions.cs rename to com.unity.robotics.ros-tcp-connector/Runtime/Extensions/MessageExtensions.cs diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageExtensions.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/Extensions/MessageExtensions.cs.meta similarity index 100% rename from com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageExtensions.cs.meta rename to com.unity.robotics.ros-tcp-connector/Runtime/Extensions/MessageExtensions.cs.meta diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/Unity.Robotics.ROSTCPConnector.MessageGeneration.asmdef b/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/Unity.Robotics.ROSTCPConnector.MessageGeneration.asmdef new file mode 100644 index 00000000..0346d195 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/Unity.Robotics.ROSTCPConnector.MessageGeneration.asmdef @@ -0,0 +1,3 @@ +{ + "name": "Unity.Robotics.ROSTCPConnector.MessageGeneration" +} diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/Unity.Robotics.ROSTCPConnector.MessageGeneration.asmdef.meta b/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/Unity.Robotics.ROSTCPConnector.MessageGeneration.asmdef.meta new file mode 100644 index 00000000..27ae989f --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/Unity.Robotics.ROSTCPConnector.MessageGeneration.asmdef.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 5db6cbfd5d0e86a46862a70f4b4067f2 +AssemblyDefinitionImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageBackwardsCompat.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/MessageBackwardsCompat.cs similarity index 100% rename from com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageBackwardsCompat.cs rename to com.unity.robotics.ros-tcp-connector/Runtime/Messages/MessageBackwardsCompat.cs diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageBackwardsCompat.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/MessageBackwardsCompat.cs.meta similarity index 100% rename from com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageBackwardsCompat.cs.meta rename to com.unity.robotics.ros-tcp-connector/Runtime/Messages/MessageBackwardsCompat.cs.meta diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Unity.Robotics.ROSTCPConnector.Messages.asmdef b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Unity.Robotics.ROSTCPConnector.Messages.asmdef new file mode 100644 index 00000000..e52ff2d2 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Unity.Robotics.ROSTCPConnector.Messages.asmdef @@ -0,0 +1,16 @@ +{ + "name": "Unity.Robotics.ROSTCPConnector.Messages", + "rootNamespace": "", + "references": [ + "GUID:5db6cbfd5d0e86a46862a70f4b4067f2" + ], + "includePlatforms": [], + "excludePlatforms": [], + "allowUnsafeCode": false, + "overrideReferences": false, + "precompiledReferences": [], + "autoReferenced": false, + "defineConstraints": [], + "versionDefines": [], + "noEngineReferences": false +} \ No newline at end of file diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Unity.Robotics.ROSTCPConnector.Messages.asmdef.meta b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Unity.Robotics.ROSTCPConnector.Messages.asmdef.meta new file mode 100644 index 00000000..4862602d --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Unity.Robotics.ROSTCPConnector.Messages.asmdef.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: ba58c43074cf72549aa2ac5937dae44b +AssemblyDefinitionImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Unity.Robotics.ROSTCPConnector.asmdef b/com.unity.robotics.ros-tcp-connector/Runtime/Unity.Robotics.ROSTCPConnector.asmdef index 06ea6b7c..22d563f8 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/Unity.Robotics.ROSTCPConnector.asmdef +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Unity.Robotics.ROSTCPConnector.asmdef @@ -1,7 +1,10 @@ { "name": "Unity.Robotics.ROSTCPConnector", "rootNamespace": "Unity.Robotics.ROSTCPConnector", - "references": [], + "references": [ + "GUID:ba58c43074cf72549aa2ac5937dae44b", + "GUID:5db6cbfd5d0e86a46862a70f4b4067f2" + ], "includePlatforms": [ "CloudRendering", "Editor", diff --git a/com.unity.robotics.ros-tcp-connector/Tests/Runtime/Unity.Perception.Runtime.Tests.asmdef b/com.unity.robotics.ros-tcp-connector/Tests/Runtime/Unity.Perception.Runtime.Tests.asmdef index edf72986..de51ae0e 100644 --- a/com.unity.robotics.ros-tcp-connector/Tests/Runtime/Unity.Perception.Runtime.Tests.asmdef +++ b/com.unity.robotics.ros-tcp-connector/Tests/Runtime/Unity.Perception.Runtime.Tests.asmdef @@ -3,7 +3,9 @@ "rootNamespace": "", "references": [ "UnityEngine.TestRunner", - "Unity.Robotics.ROSTCPConnector" + "Unity.Robotics.ROSTCPConnector", + "Unity.Robotics.ROSTCPConnector.MessageGeneration", + "Unity.Robotics.ROSTCPConnector.Messages" ], "includePlatforms": [], "excludePlatforms": [], From 9f2cab885e14d5ec817519d8edc4755032ea5090 Mon Sep 17 00:00:00 2001 From: "Devin Miller (Unity)" Date: Tue, 2 Nov 2021 09:54:02 -0700 Subject: [PATCH 17/33] Activating Code Coverage requirements (#231) * AIRO-618: Activating code coverage requirements * Adding searchable error for when code coverage fails. * Lowering coverage threshold to below current coverage * Tweaking yamato config logic to ensure on-merge tests> --- .yamato/yamato-config.yml | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/.yamato/yamato-config.yml b/.yamato/yamato-config.yml index 156b185d..30500330 100644 --- a/.yamato/yamato-config.yml +++ b/.yamato/yamato-config.yml @@ -4,22 +4,27 @@ agent: image: robotics/ci-ubuntu20:v0.1.0-795910 flavor: i1.large variables: + # Coverage is defined in a percentage (out of 100) + COVERAGE_EXPECTED: 2.5 PATH: /root/.local/bin:/home/bokken/bin:/home/bokken/.local/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/usr/games:/usr/local/games:/snap/bin:/sbin:/home/bokken/.npm-global/bin commands: - python3 -m pip install unity-downloader-cli --index-url https://artifactory.prd.it.unity3d.com/artifactory/api/pypi/pypi/simple --upgrade - unity-downloader-cli -u 2020.2.0b9 -c editor -c StandaloneSupport-IL2CPP -c Linux --wait --published - git clone git@github.cds.internal.unity3d.com:unity/utr.git utr - - utr/utr --testproject=./TestRosTcpConnector --editor-location=.Editor --reruncount=0 --artifacts_path=test-results --suite=playmode --suite=editor --platform=Editor --enable-code-coverage --coverage-results-path=../test-results --coverage-options="assemblyFilters:+Unity.Robotics.ROSTCPConnector,+Unity.Robotics.ROSTCPConnector.Editor;generateHtmlReport;generateBadgeReport;generateAdditionalMetrics" + - utr/utr --testproject=./TestRosTcpConnector --editor-location=.Editor --reruncount=0 --artifacts_path=test-results --suite=playmode --suite=editor --platform=Editor --enable-code-coverage --coverage-results-path=../test-results --coverage-options="assemblyFilters:+Unity.Robotics.ROSTCPConnector,+Unity.Robotics.ROSTCPConnector.Editor,+Unity.Robotics.ROSTCPConnector.MessageGeneration;generateHtmlReport;generateBadgeReport;generateAdditionalMetrics" # check test coverage - command: | linecoverage=$(cat test-results/Report/Summary.xml | grep Linecoverage | grep -Eo '[+-]?[0-9]+([.][0-9]+)?') echo "Line coverage: $linecoverage%" - if (( $(echo "$linecoverage < 0" | bc -l) )); then exit 1; fi + if (( $(echo "$linecoverage < $COVERAGE_EXPECTED" | bc -l) )) + then echo "ERROR: Code coverage is under threshold of $COVERAGE_EXPECTED%" && exit 1 + fi triggers: cancel_old_ci: true expression: | - (pull_request.target eq "main" OR push.branch eq "main" OR pull_request.target eq "dev" OR push.branch eq "dev") - AND NOT pull_request.push.changes.all match "**/*.md" + ((pull_request.target eq "main" OR pull_request.target eq "dev") + AND NOT pull_request.push.changes.all match "**/*.md") OR + (push.branch eq "main" OR push.branch eq "dev") artifacts: logs: paths: From 12a53cb7bdd6671b3840ff412c2414d731d93e76 Mon Sep 17 00:00:00 2001 From: peifeng-unity <56408141+peifeng-unity@users.noreply.github.com> Date: Mon, 8 Nov 2021 14:13:49 -0800 Subject: [PATCH 18/33] Add a try catch so that bad logic from one received message doesn't cause the Update method to exit without processing other received messages. (#227) (#233) Co-authored-by: Hamid Younesy Co-authored-by: Peter Smith Co-authored-by: Hamid Younesy --- .../Runtime/TcpConnector/ROSConnection.cs | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) 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 138b82ac..df3e64aa 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSConnection.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSConnection.cs @@ -589,7 +589,19 @@ void Update() // if this is null, we have received a message on a topic we've never heard of...!? // all we can do is ignore it, we don't even know what type it is if (topicInfo != null) - topicInfo.OnMessageReceived(contents); + { + try + { + //Add a try catch so that bad logic from one received message doesn't + //cause the Update method to exit without processing other received messages. + topicInfo.OnMessageReceived(contents); + } + catch (Exception e) + { + Debug.LogException(e); + } + + } } } } From 0ed2ad90aa78dd6f42670a0770c6f7f7dc5145b1 Mon Sep 17 00:00:00 2001 From: LaurieCheers-unity <73140792+LaurieCheers-unity@users.noreply.github.com> Date: Thu, 18 Nov 2021 14:35:05 -0800 Subject: [PATCH 19/33] Visualizations reference messages (#236) --- .../Messages/Unity.Robotics.ROSTCPConnector.Messages.asmdef | 2 +- .../Editor/Unity.Robotics.Visualizations.Editor.asmdef | 6 ++++-- .../Runtime/Unity.Robotics.Visualizations.asmdef | 6 ++++-- 3 files changed, 9 insertions(+), 5 deletions(-) diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Unity.Robotics.ROSTCPConnector.Messages.asmdef b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Unity.Robotics.ROSTCPConnector.Messages.asmdef index e52ff2d2..a08613fc 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Unity.Robotics.ROSTCPConnector.Messages.asmdef +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Unity.Robotics.ROSTCPConnector.Messages.asmdef @@ -9,7 +9,7 @@ "allowUnsafeCode": false, "overrideReferences": false, "precompiledReferences": [], - "autoReferenced": false, + "autoReferenced": true, "defineConstraints": [], "versionDefines": [], "noEngineReferences": false diff --git a/com.unity.robotics.visualizations/Editor/Unity.Robotics.Visualizations.Editor.asmdef b/com.unity.robotics.visualizations/Editor/Unity.Robotics.Visualizations.Editor.asmdef index 8f9a11d8..7b28c8af 100644 --- a/com.unity.robotics.visualizations/Editor/Unity.Robotics.Visualizations.Editor.asmdef +++ b/com.unity.robotics.visualizations/Editor/Unity.Robotics.Visualizations.Editor.asmdef @@ -4,7 +4,9 @@ "references": [ "GUID:625bfc588fb96c74696858f2c467e978", "GUID:b1ef917f7a8a86a4eb639ec2352edbf8", - "GUID:3f8053c1a58fb7f47b2af9d34d2e4b1e" + "GUID:3f8053c1a58fb7f47b2af9d34d2e4b1e", + "GUID:5db6cbfd5d0e86a46862a70f4b4067f2", + "GUID:ba58c43074cf72549aa2ac5937dae44b" ], "includePlatforms": [ "Editor" @@ -17,4 +19,4 @@ "defineConstraints": [], "versionDefines": [], "noEngineReferences": false -} +} \ No newline at end of file diff --git a/com.unity.robotics.visualizations/Runtime/Unity.Robotics.Visualizations.asmdef b/com.unity.robotics.visualizations/Runtime/Unity.Robotics.Visualizations.asmdef index fe08f88b..0b898c1b 100644 --- a/com.unity.robotics.visualizations/Runtime/Unity.Robotics.Visualizations.asmdef +++ b/com.unity.robotics.visualizations/Runtime/Unity.Robotics.Visualizations.asmdef @@ -3,7 +3,9 @@ "rootNamespace": "", "references": [ "GUID:625bfc588fb96c74696858f2c467e978", - "GUID:b1ef917f7a8a86a4eb639ec2352edbf8" + "GUID:b1ef917f7a8a86a4eb639ec2352edbf8", + "GUID:ba58c43074cf72549aa2ac5937dae44b", + "GUID:5db6cbfd5d0e86a46862a70f4b4067f2" ], "includePlatforms": [ "Editor", @@ -26,4 +28,4 @@ } ], "noEngineReferences": false -} +} \ No newline at end of file From e47b3585c9be943f02f71553ec78598719002771 Mon Sep 17 00:00:00 2001 From: LaurieCheers-unity <73140792+LaurieCheers-unity@users.noreply.github.com> Date: Mon, 22 Nov 2021 16:18:04 -0800 Subject: [PATCH 20/33] AIRO-1428 serialization fix (#237) * Visualizations reference messages * Serialization fix - don't align empty arrays --- .../MessageGeneration/MessageDeserializer.cs | 6 ++++ .../MessageGeneration/MessageSerializer.cs | 33 +++++++++++++++++++ 2 files changed, 39 insertions(+) diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageDeserializer.cs b/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageDeserializer.cs index bb38d8ce..a1335c85 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageDeserializer.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageDeserializer.cs @@ -145,6 +145,12 @@ public void Read(out string value) public void Read(out T[] values, int elementSize, int length) { + if (length == 0) + { + values = new T[0]; + return; + } + Align(elementSize); T[] result = new T[length]; Buffer.BlockCopy(data, offset, result, 0, length * elementSize); diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageSerializer.cs b/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageSerializer.cs index edfb181d..d3950e6f 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageSerializer.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageSerializer.cs @@ -201,6 +201,9 @@ public void Write(T[] values) where T : Message public void Write(bool[] values) { + if (values.Length == 0) + return; + byte[] buffer = new byte[values.Length]; Buffer.BlockCopy(values, 0, buffer, 0, buffer.Length); m_ListOfSerializations.Add(buffer); @@ -209,12 +212,18 @@ public void Write(bool[] values) public void Write(byte[] values) { + if (values.Length == 0) + return; + m_ListOfSerializations.Add(values); m_AlignmentOffset += values.Length; } public void Write(sbyte[] values) { + if (values.Length == 0) + return; + byte[] buffer = new byte[values.Length]; Buffer.BlockCopy(values, 0, buffer, 0, buffer.Length); m_ListOfSerializations.Add(buffer); @@ -223,6 +232,9 @@ public void Write(sbyte[] values) public void Write(short[] values) { + if (values.Length == 0) + return; + Align(sizeof(short)); byte[] buffer = new byte[values.Length * sizeof(short)]; Buffer.BlockCopy(values, 0, buffer, 0, buffer.Length); @@ -232,6 +244,9 @@ public void Write(short[] values) public void Write(ushort[] values) { + if (values.Length == 0) + return; + Align(sizeof(ushort)); byte[] buffer = new byte[values.Length * sizeof(ushort)]; Buffer.BlockCopy(values, 0, buffer, 0, buffer.Length); @@ -241,6 +256,9 @@ public void Write(ushort[] values) public void Write(int[] values) { + if (values.Length == 0) + return; + Align(sizeof(int)); byte[] buffer = new byte[values.Length * sizeof(int)]; Buffer.BlockCopy(values, 0, buffer, 0, buffer.Length); @@ -250,6 +268,9 @@ public void Write(int[] values) public void Write(uint[] values) { + if (values.Length == 0) + return; + Align(sizeof(uint)); byte[] buffer = new byte[values.Length * sizeof(uint)]; Buffer.BlockCopy(values, 0, buffer, 0, buffer.Length); @@ -259,6 +280,9 @@ public void Write(uint[] values) public void Write(float[] values) { + if (values.Length == 0) + return; + Align(sizeof(float)); byte[] buffer = new byte[values.Length * sizeof(float)]; Buffer.BlockCopy(values, 0, buffer, 0, buffer.Length); @@ -268,6 +292,9 @@ public void Write(float[] values) public void Write(double[] values) { + if (values.Length == 0) + return; + Align(sizeof(double)); byte[] buffer = new byte[values.Length * sizeof(double)]; Buffer.BlockCopy(values, 0, buffer, 0, buffer.Length); @@ -277,6 +304,9 @@ public void Write(double[] values) public void Write(long[] values) { + if (values.Length == 0) + return; + Align(sizeof(long)); byte[] buffer = new byte[values.Length * sizeof(long)]; Buffer.BlockCopy(values, 0, buffer, 0, buffer.Length); @@ -286,6 +316,9 @@ public void Write(long[] values) public void Write(ulong[] values) { + if (values.Length == 0) + return; + Align(sizeof(ulong)); byte[] buffer = new byte[values.Length * sizeof(ulong)]; Buffer.BlockCopy(values, 0, buffer, 0, buffer.Length); From c15ee962e91eed149bfb2b954b8c12f8409f6baf Mon Sep 17 00:00:00 2001 From: LaurieCheers-unity <73140792+LaurieCheers-unity@users.noreply.github.com> Date: Wed, 24 Nov 2021 07:51:16 -0800 Subject: [PATCH 21/33] Handle tabs, warning instead of error on field named "time" (#238) --- .../Editor/MessageGeneration/MessageParser.cs | 2 +- .../Editor/MessageGeneration/MessageTokenizer.cs | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/MessageParser.cs b/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/MessageParser.cs index 0a472359..1933d950 100644 --- a/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/MessageParser.cs +++ b/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/MessageParser.cs @@ -319,7 +319,7 @@ private void Declaration() // Check if identifier is a ROS message built-in type if (builtInTypeMapping.ContainsKey(identifier)) { - throw new MessageParserException( + Debug.LogWarning( "Invalid field identifier '" + identifier + "' at " + inFilePath + ":" + lineNum + ". '" + identifier + "' is a ROS message built-in type."); diff --git a/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/MessageTokenizer.cs b/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/MessageTokenizer.cs index 96f6c6e2..989409eb 100644 --- a/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/MessageTokenizer.cs +++ b/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/MessageTokenizer.cs @@ -287,7 +287,7 @@ private MessageToken NextTypeIdentifierToken() } // Otherwise, consume input until seperator, EOF or '[' - while (reader.Peek() != ' ' && reader.Peek() != '[' && !reader.EndOfStream) + while (reader.Peek() != ' ' && reader.Peek() != '[' && reader.Peek() != '\t' && !reader.EndOfStream) { if (!Char.IsLetterOrDigit((char)reader.Peek()) && !allowedSpecialCharacterForTypeIdentifier.Contains((char)reader.Peek())) { From 973bcce0cb018ed7d7f1c0fdd065867eefb890f6 Mon Sep 17 00:00:00 2001 From: LaurieCheers-unity <73140792+LaurieCheers-unity@users.noreply.github.com> Date: Thu, 25 Nov 2021 09:43:02 -0800 Subject: [PATCH 22/33] Angular velocity conversions (#240) --- .../Runtime/ROSGeometry/CoordinateSpaces.cs | 78 ++++++++++++++----- .../Runtime/ROSGeometry/ROSVector3.cs | 6 ++ 2 files changed, 64 insertions(+), 20 deletions(-) diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/CoordinateSpaces.cs b/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/CoordinateSpaces.cs index ee7ff758..6f0a27be 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/CoordinateSpaces.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/CoordinateSpaces.cs @@ -11,6 +11,9 @@ public interface ICoordinateSpace Quaternion ConvertFromRUF(Quaternion q); // convert this quaternion from the Unity coordinate space into mine Quaternion ConvertToRUF(Quaternion q); // convert from my coordinate space into the Unity coordinate space + + Vector3 ConvertAngularVelocityFromRUF(Vector3 angularVelocity); // convert this angular velocity from the Unity coordinate space into mine + Vector3 ConvertAngularVelocityToRUF(Vector3 angularVelocity); // convert from my coordinate space into the Unity coordinate space } [Obsolete("CoordinateSpace has been renamed to ICoordinateSpace")] @@ -21,35 +24,55 @@ public interface CoordinateSpace : ICoordinateSpace //RUF is the Unity coordinate space, so no conversion needed public class RUF : ICoordinateSpace { - public Vector3 ConvertFromRUF(Vector3 v) => v; - public Vector3 ConvertToRUF(Vector3 v) => v; - public Quaternion ConvertFromRUF(Quaternion q) => q; - public Quaternion ConvertToRUF(Quaternion q) => q; + Vector3 ICoordinateSpace.ConvertFromRUF(Vector3 v) => v; + Vector3 ICoordinateSpace.ConvertToRUF(Vector3 v) => v; + Quaternion ICoordinateSpace.ConvertFromRUF(Quaternion q) => q; + Quaternion ICoordinateSpace.ConvertToRUF(Quaternion q) => q; + Vector3 ICoordinateSpace.ConvertAngularVelocityFromRUF(Vector3 angularVelocity) => angularVelocity; + Vector3 ICoordinateSpace.ConvertAngularVelocityToRUF(Vector3 angularVelocity) => angularVelocity; } public class FLU : ICoordinateSpace { - public Vector3 ConvertFromRUF(Vector3 v) => new Vector3(v.z, -v.x, v.y); - public Vector3 ConvertToRUF(Vector3 v) => new Vector3(-v.y, v.z, v.x); - public Quaternion ConvertFromRUF(Quaternion q) => new Quaternion(q.z, -q.x, q.y, -q.w); - public Quaternion ConvertToRUF(Quaternion q) => new Quaternion(-q.y, q.z, q.x, -q.w); + public static Vector3 ConvertFromRUF(Vector3 v) => new Vector3(v.z, -v.x, v.y); + public static Vector3 ConvertToRUF(Vector3 v) => new Vector3(-v.y, v.z, v.x); + public static Quaternion ConvertFromRUF(Quaternion q) => new Quaternion(q.z, -q.x, q.y, -q.w); + public static Quaternion ConvertToRUF(Quaternion q) => new Quaternion(-q.y, q.z, q.x, -q.w); + public static Vector3 ConvertAngularVelocityFromRUF(Vector3 angularVelocity) => -FLU.ConvertFromRUF(angularVelocity); + public static Vector3 ConvertAngularVelocityToRUF(Vector3 angularVelocity) => -FLU.ConvertToRUF(angularVelocity); + + Vector3 ICoordinateSpace.ConvertFromRUF(Vector3 v) => FLU.ConvertFromRUF(v); + Vector3 ICoordinateSpace.ConvertToRUF(Vector3 v) => FLU.ConvertToRUF(v); + Quaternion ICoordinateSpace.ConvertFromRUF(Quaternion q) => FLU.ConvertFromRUF(q); + Quaternion ICoordinateSpace.ConvertToRUF(Quaternion q) => FLU.ConvertToRUF(q); + Vector3 ICoordinateSpace.ConvertAngularVelocityFromRUF(Vector3 angularVelocity) => FLU.ConvertAngularVelocityFromRUF(angularVelocity); + Vector3 ICoordinateSpace.ConvertAngularVelocityToRUF(Vector3 angularVelocity) => FLU.ConvertAngularVelocityToRUF(angularVelocity); } public class ENULocal : FLU { } public class FRD : ICoordinateSpace { - public Vector3 ConvertFromRUF(Vector3 v) => new Vector3(v.z, v.x, -v.y); - public Vector3 ConvertToRUF(Vector3 v) => new Vector3(v.y, -v.z, v.x); - public Quaternion ConvertFromRUF(Quaternion q) => new Quaternion(q.z, q.x, -q.y, -q.w); - public Quaternion ConvertToRUF(Quaternion q) => new Quaternion(q.y, -q.z, q.x, -q.w); + public static Vector3 ConvertFromRUF(Vector3 v) => new Vector3(v.z, v.x, -v.y); + public static Vector3 ConvertToRUF(Vector3 v) => new Vector3(v.y, -v.z, v.x); + public static Quaternion ConvertFromRUF(Quaternion q) => new Quaternion(q.z, q.x, -q.y, -q.w); + public static Quaternion ConvertToRUF(Quaternion q) => new Quaternion(q.y, -q.z, q.x, -q.w); + public static Vector3 ConvertAngularVelocityFromRUF(Vector3 angularVelocity) => -ConvertFromRUF(angularVelocity); + public static Vector3 ConvertAngularVelocityToRUF(Vector3 angularVelocity) => -ConvertToRUF(angularVelocity); + + Vector3 ICoordinateSpace.ConvertFromRUF(Vector3 v) => FRD.ConvertFromRUF(v); + Vector3 ICoordinateSpace.ConvertToRUF(Vector3 v) => FRD.ConvertToRUF(v); + Quaternion ICoordinateSpace.ConvertFromRUF(Quaternion q) => FRD.ConvertFromRUF(q); + Quaternion ICoordinateSpace.ConvertToRUF(Quaternion q) => FRD.ConvertToRUF(q); + Vector3 ICoordinateSpace.ConvertAngularVelocityFromRUF(Vector3 angularVelocity) => FRD.ConvertAngularVelocityFromRUF(angularVelocity); + Vector3 ICoordinateSpace.ConvertAngularVelocityToRUF(Vector3 angularVelocity) => FRD.ConvertAngularVelocityToRUF(angularVelocity); } public class NEDLocal : FRD { } public class NED : ICoordinateSpace { - public Vector3 ConvertFromRUF(Vector3 v) + public static Vector3 ConvertFromRUF(Vector3 v) { switch (GeometryCompass.UnityZAxisDirection) { @@ -66,7 +89,7 @@ public Vector3 ConvertFromRUF(Vector3 v) } } - public Vector3 ConvertToRUF(Vector3 v) + public static Vector3 ConvertToRUF(Vector3 v) { switch (GeometryCompass.UnityZAxisDirection) { @@ -83,7 +106,7 @@ public Vector3 ConvertToRUF(Vector3 v) } } - public Quaternion ConvertFromRUF(Quaternion q) + public static Quaternion ConvertFromRUF(Quaternion q) { switch (GeometryCompass.UnityZAxisDirection) { @@ -104,7 +127,7 @@ public Quaternion ConvertFromRUF(Quaternion q) return new Quaternion(q.z, q.x, -q.y, -q.w); } - public Quaternion ConvertToRUF(Quaternion q) + public static Quaternion ConvertToRUF(Quaternion q) { var r = new Quaternion(q.y, -q.z, q.x, -q.w); switch (GeometryCompass.UnityZAxisDirection) @@ -121,11 +144,18 @@ public Quaternion ConvertToRUF(Quaternion q) throw new NotSupportedException(); } } + Vector3 ICoordinateSpace.ConvertFromRUF(Vector3 v) => NED.ConvertFromRUF(v); + Vector3 ICoordinateSpace.ConvertToRUF(Vector3 v) => NED.ConvertToRUF(v); + Quaternion ICoordinateSpace.ConvertFromRUF(Quaternion q) => NED.ConvertFromRUF(q); + Quaternion ICoordinateSpace.ConvertToRUF(Quaternion q) => NED.ConvertToRUF(q); + //Note: Angular Velocity is the same as FRD / NEDLocal + Vector3 ICoordinateSpace.ConvertAngularVelocityFromRUF(Vector3 angularVelocity) => FRD.ConvertAngularVelocityFromRUF(angularVelocity); + Vector3 ICoordinateSpace.ConvertAngularVelocityToRUF(Vector3 angularVelocity) => FRD.ConvertAngularVelocityToRUF(angularVelocity); } public class ENU : ICoordinateSpace { - public Vector3 ConvertFromRUF(Vector3 v) + public static Vector3 ConvertFromRUF(Vector3 v) { switch (GeometryCompass.UnityZAxisDirection) { @@ -141,7 +171,7 @@ public Vector3 ConvertFromRUF(Vector3 v) throw new NotSupportedException(); } } - public Vector3 ConvertToRUF(Vector3 v) + public static Vector3 ConvertToRUF(Vector3 v) { switch (GeometryCompass.UnityZAxisDirection) { @@ -158,7 +188,7 @@ public Vector3 ConvertToRUF(Vector3 v) } } - public Quaternion ConvertFromRUF(Quaternion q) + public static Quaternion ConvertFromRUF(Quaternion q) { switch (GeometryCompass.UnityZAxisDirection) { @@ -180,7 +210,7 @@ public Quaternion ConvertFromRUF(Quaternion q) return new Quaternion(q.z, -q.x, q.y, -q.w); } - public Quaternion ConvertToRUF(Quaternion q) + public static Quaternion ConvertToRUF(Quaternion q) { q = new Quaternion(-q.y, q.z, q.x, -q.w); switch (GeometryCompass.UnityZAxisDirection) @@ -197,6 +227,14 @@ public Quaternion ConvertToRUF(Quaternion q) throw new NotSupportedException(); } } + + Vector3 ICoordinateSpace.ConvertFromRUF(Vector3 v) => ENU.ConvertFromRUF(v); + Vector3 ICoordinateSpace.ConvertToRUF(Vector3 v) => ENU.ConvertToRUF(v); + Quaternion ICoordinateSpace.ConvertFromRUF(Quaternion q) => ENU.ConvertFromRUF(q); + Quaternion ICoordinateSpace.ConvertToRUF(Quaternion q) => ENU.ConvertToRUF(q); + //Note: Angular Velocity is the same as FLU / ENULocal + Vector3 ICoordinateSpace.ConvertAngularVelocityFromRUF(Vector3 angularVelocity) => FLU.ConvertAngularVelocityFromRUF(angularVelocity); + Vector3 ICoordinateSpace.ConvertAngularVelocityToRUF(Vector3 angularVelocity) => FLU.ConvertAngularVelocityToRUF(angularVelocity); } public enum CoordinateSpaceSelection diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/ROSVector3.cs b/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/ROSVector3.cs index 6fb1c34e..de4cb0f2 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/ROSVector3.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/ROSVector3.cs @@ -30,6 +30,7 @@ public Vector3(Vector3 vec_ruf) } public Vector3 toUnity => s_CoordinateSpace.ConvertToRUF(internalVector); + public Vector3 toUnityAngularVelocity => s_CoordinateSpace.ConvertAngularVelocityToRUF(internalVector); public static explicit operator Vector3(Vector3 vec) { @@ -46,6 +47,11 @@ public static explicit operator Vector3(Vector3 vec) return (Vector3)(Vector3)this; } + public static Vector3 FromUnityAngularVelocity(Vector3 angularVelocityRUF) + { + return new Vector3 { internalVector = s_CoordinateSpace.ConvertAngularVelocityFromRUF(angularVelocityRUF) }; + } + // for internal use only - this function does not convert coordinate spaces correctly private static Vector3 MakeInternal(Vector3 vec) { From 136976da5d76e283fbc0f046dedf6bc20a14336b Mon Sep 17 00:00:00 2001 From: LaurieCheers-unity <73140792+LaurieCheers-unity@users.noreply.github.com> Date: Mon, 29 Nov 2021 14:42:58 -0800 Subject: [PATCH 23/33] AIRO-1592 Correct CameraInfo field names (#241) * Correct CameraInfo field names * Update CameraInfoMsg.cs --- .../MessageGeneration/MsgAutoGenUtilities.cs | 15 ++++++--- .../msg/CameraInfoMsg.cs | 31 +++++++++++++++++++ .../msg/CameraInfoMsg.cs.meta | 0 3 files changed, 42 insertions(+), 4 deletions(-) rename com.unity.robotics.ros-tcp-connector/Runtime/Messages/{Sensor => HandwrittenMessages}/msg/CameraInfoMsg.cs (92%) rename com.unity.robotics.ros-tcp-connector/Runtime/Messages/{Sensor => HandwrittenMessages}/msg/CameraInfoMsg.cs.meta (100%) diff --git a/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/MsgAutoGenUtilities.cs b/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/MsgAutoGenUtilities.cs index 987b872d..ab27e4dc 100644 --- a/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/MsgAutoGenUtilities.cs +++ b/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/MsgAutoGenUtilities.cs @@ -100,10 +100,17 @@ public class MsgAutoGenUtilities public static bool HasHandwrittenMessage(string rosPackageName, string inFileName) { - if (!rosPackageName.Equals("std_msgs")) - return false; - - return inFileName.Equals("Time") || inFileName.Equals("Duration") || inFileName.Equals("Header"); + switch (inFileName) + { + case "Header": + case "Time": + case "Duration": + return rosPackageName == "std_msgs"; + case "CameraInfo": + return rosPackageName == "sensor_msgs"; + default: + return false; + } } public static string CapitalizeFirstLetter(string s) diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Sensor/msg/CameraInfoMsg.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/HandwrittenMessages/msg/CameraInfoMsg.cs similarity index 92% rename from com.unity.robotics.ros-tcp-connector/Runtime/Messages/Sensor/msg/CameraInfoMsg.cs rename to com.unity.robotics.ros-tcp-connector/Runtime/Messages/HandwrittenMessages/msg/CameraInfoMsg.cs index f644cc85..42439afc 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Sensor/msg/CameraInfoMsg.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/HandwrittenMessages/msg/CameraInfoMsg.cs @@ -70,7 +70,13 @@ public class CameraInfoMsg : Message public string distortion_model; // The distortion parameters, size depending on the distortion model. // For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3). +#if ROS2 public double[] d; + public double[] D { get => d; set => d = value; } +#else + public double[] D; + public double[] d { get => D; set => D = value; } +#endif // Intrinsic camera matrix for the raw (distorted) images. // [fx 0 cx] // K = [ 0 fy cy] @@ -78,13 +84,25 @@ public class CameraInfoMsg : Message // Projects 3D points in the camera coordinate frame to 2D pixel // coordinates using the focal lengths (fx, fy) and principal point // (cx, cy). +#if ROS2 public double[] k; + public double[] K { get => k; set => k = value; } +#else + public double[] K; + public double[] k { get => K; set => K = value; } +#endif // 3x3 row-major matrix // Rectification matrix (stereo cameras only) // A rotation matrix aligning the camera coordinate system to the ideal // stereo image plane so that epipolar lines in both stereo images are // parallel. +#if ROS2 public double[] r; + public double[] R { get => r; set => r = value; } +#else + public double[] R; + public double[] r { get => R; set => R = value; } +#endif // 3x3 row-major matrix // Projection/camera matrix // [fx' 0 cx' Tx] @@ -110,7 +128,13 @@ public class CameraInfoMsg : Message // x = u / w // y = v / w // This holds for both images of a stereo pair. +#if ROS2 public double[] p; + public double[] P { get => p; set => p = value; } +#else + public double[] P; + public double[] p { get => P; set => P = value; } +#endif // 3x4 row-major matrix // ###################################################################### // Operational Parameters # @@ -173,10 +197,17 @@ private CameraInfoMsg(MessageDeserializer deserializer) deserializer.Read(out this.height); deserializer.Read(out this.width); deserializer.Read(out this.distortion_model); +#if ROS2 deserializer.Read(out this.d, sizeof(double), deserializer.ReadLength()); deserializer.Read(out this.k, sizeof(double), 9); deserializer.Read(out this.r, sizeof(double), 9); deserializer.Read(out this.p, sizeof(double), 12); +#else + deserializer.Read(out this.D, sizeof(double), deserializer.ReadLength()); + deserializer.Read(out this.K, sizeof(double), 9); + deserializer.Read(out this.R, sizeof(double), 9); + deserializer.Read(out this.P, sizeof(double), 12); +#endif deserializer.Read(out this.binning_x); deserializer.Read(out this.binning_y); this.roi = RegionOfInterestMsg.Deserialize(deserializer); diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Sensor/msg/CameraInfoMsg.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/HandwrittenMessages/msg/CameraInfoMsg.cs.meta similarity index 100% rename from com.unity.robotics.ros-tcp-connector/Runtime/Messages/Sensor/msg/CameraInfoMsg.cs.meta rename to com.unity.robotics.ros-tcp-connector/Runtime/Messages/HandwrittenMessages/msg/CameraInfoMsg.cs.meta From 9c2cca46ad53729ebdb5b671ebdd3f6177263ab8 Mon Sep 17 00:00:00 2001 From: LaurieCheers-unity <73140792+LaurieCheers-unity@users.noreply.github.com> Date: Mon, 29 Nov 2021 15:26:29 -0800 Subject: [PATCH 24/33] AIRO-672 Added more ROS-TCP-Connector tests (#235) * Delete old metafiles * Add tests * Update MessageTests.cs * TFSystem tests * More ROSConnection tests * Remove editor tests & moq * Renamed tests to match conventions --- .../Runtime/TcpConnector/TFStream.cs | 12 ++ .../Runtime/TcpConnector/TFSystem.cs | 20 +- ...botics.ROSTCPConnector.Editor.Tests.asmdef | 3 +- .../Tests/Runtime/MessagePoolTests.cs | 42 ++++ .../Tests/Runtime/MessagePoolTests.cs.meta | 11 ++ .../Tests/Runtime/MessageTests.cs | 124 ++++++++++++ .../Tests/Runtime/MessageTests.cs.meta | 11 ++ .../Tests/Runtime/ROSGeometryTests.cs | 2 +- .../Tests/Runtime/RosConnectionTests.cs | 35 ++++ .../Tests/Runtime/RosConnectionTests.cs.meta | 11 ++ .../Tests/Runtime/TFSystemTests.cs | 185 ++++++++++++++++++ .../Tests/Runtime/TFSystemTests.cs.meta | 11 ++ ...tics.RosTcpConnector.Runtime.Tests.asmdef} | 0 ...RosTcpConnector.Runtime.Tests.asmdef.meta} | 0 .../package.json | 1 + 15 files changed, 459 insertions(+), 9 deletions(-) create mode 100644 com.unity.robotics.ros-tcp-connector/Tests/Runtime/MessagePoolTests.cs create mode 100644 com.unity.robotics.ros-tcp-connector/Tests/Runtime/MessagePoolTests.cs.meta create mode 100644 com.unity.robotics.ros-tcp-connector/Tests/Runtime/MessageTests.cs create mode 100644 com.unity.robotics.ros-tcp-connector/Tests/Runtime/MessageTests.cs.meta create mode 100644 com.unity.robotics.ros-tcp-connector/Tests/Runtime/RosConnectionTests.cs create mode 100644 com.unity.robotics.ros-tcp-connector/Tests/Runtime/RosConnectionTests.cs.meta create mode 100644 com.unity.robotics.ros-tcp-connector/Tests/Runtime/TFSystemTests.cs create mode 100644 com.unity.robotics.ros-tcp-connector/Tests/Runtime/TFSystemTests.cs.meta rename com.unity.robotics.ros-tcp-connector/Tests/Runtime/{Unity.Perception.Runtime.Tests.asmdef => Unity.Robotics.RosTcpConnector.Runtime.Tests.asmdef} (100%) rename com.unity.robotics.ros-tcp-connector/Tests/Runtime/{Unity.Perception.Runtime.Tests.asmdef.meta => Unity.Robotics.RosTcpConnector.Runtime.Tests.asmdef.meta} (100%) diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TFStream.cs b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TFStream.cs index d0385e5c..c0712c49 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TFStream.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TFStream.cs @@ -1,4 +1,6 @@ +using RosMessageTypes.BuiltinInterfaces; using System.Collections.Generic; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; using UnityEngine; // Represents a transform - position and rotation. @@ -159,6 +161,11 @@ public TFFrame GetLocalTF(long time = 0) } } + public TFFrame GetLocalTF(TimeMsg time) + { + return GetLocalTF(time.ToLongTime()); + } + public TFFrame GetWorldTF(long time = 0) { TFFrame parent; @@ -170,6 +177,11 @@ public TFFrame GetWorldTF(long time = 0) return parent.Compose(GetLocalTF(time)); } + public TFFrame GetWorldTF(TimeMsg time) + { + return GetWorldTF(time.ToLongTime()); + } + // Can we safely stop polling for updates to a transform at this time? public bool IsTimeStable(long time) { diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TFSystem.cs b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TFSystem.cs index 03aeca08..af4fd483 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TFSystem.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TFSystem.cs @@ -13,7 +13,7 @@ public class TFSystem public static TFSystem instance { get; private set; } Dictionary m_TFTopics = new Dictionary(); - class TFTopicState + public class TFTopicState { string m_TFTopic; Dictionary m_TransformTable = new Dictionary(); @@ -25,7 +25,7 @@ public TFTopicState(string tfTopic = "/tf") ROSConnection.GetOrCreateInstance().Subscribe(tfTopic, ReceiveTF); } - public TFStream GetOrCreateTFStream(string frame_id) + public TFStream GetOrCreateFrame(string frame_id) { TFStream tf; while (frame_id.EndsWith("/")) @@ -43,7 +43,7 @@ public TFStream GetOrCreateTFStream(string frame_id) } else { - var parent = GetOrCreateTFStream(frame_id.Substring(0, slash)); + var parent = GetOrCreateFrame(frame_id.Substring(0, slash)); tf = new TFStream(parent, singleName, m_TFTopic); } @@ -52,7 +52,7 @@ public TFStream GetOrCreateTFStream(string frame_id) } else if (slash > 0 && tf.Parent == null) { - tf.SetParent(GetOrCreateTFStream(frame_id.Substring(0, slash))); + tf.SetParent(GetOrCreateFrame(frame_id.Substring(0, slash))); } return tf; @@ -63,7 +63,7 @@ public void ReceiveTF(TFMessageMsg message) foreach (var tf_message in message.transforms) { var frame_id = tf_message.header.frame_id + "/" + tf_message.child_frame_id; - var tf = GetOrCreateTFStream(frame_id); + var tf = GetOrCreateFrame(frame_id); tf.Add( tf_message.header.stamp.ToLongTime(), tf_message.transform.translation.From(), @@ -177,11 +177,11 @@ public TFStream GetTransformStream(string frame_id, string tfTopic = "/tf") public GameObject GetTransformObject(string frame_id, string tfTopic = "/tf") { - TFStream stream = GetOrCreateTFTopic(tfTopic).GetOrCreateTFStream(frame_id); + TFStream stream = GetOrCreateTFTopic(tfTopic).GetOrCreateFrame(frame_id); return stream.GameObject; } - TFTopicState GetOrCreateTFTopic(string tfTopic) + public TFTopicState GetOrCreateTFTopic(string tfTopic = "/tf") { TFTopicState tfTopicState; if (!m_TFTopics.TryGetValue(tfTopic, out tfTopicState)) @@ -191,4 +191,10 @@ TFTopicState GetOrCreateTFTopic(string tfTopic) } return tfTopicState; } + + public TFStream GetOrCreateFrame(string frame_id, string tfTopic = "/tf") + { + TFTopicState topicState = GetOrCreateTFTopic(tfTopic); + return topicState.GetOrCreateFrame(frame_id); + } } diff --git a/com.unity.robotics.ros-tcp-connector/Tests/Editor/Unity.Robotics.ROSTCPConnector.Editor.Tests.asmdef b/com.unity.robotics.ros-tcp-connector/Tests/Editor/Unity.Robotics.ROSTCPConnector.Editor.Tests.asmdef index 813bba22..4da22c21 100644 --- a/com.unity.robotics.ros-tcp-connector/Tests/Editor/Unity.Robotics.ROSTCPConnector.Editor.Tests.asmdef +++ b/com.unity.robotics.ros-tcp-connector/Tests/Editor/Unity.Robotics.ROSTCPConnector.Editor.Tests.asmdef @@ -4,7 +4,8 @@ "references": [ "Unity.Robotics.ROSTCPConnector.Editor", "UnityEngine.TestRunner", - "UnityEditor.TestRunner" + "UnityEditor.TestRunner", + "Unity.Robotics.ROSTCPConnector" ], "includePlatforms": [ "Editor" diff --git a/com.unity.robotics.ros-tcp-connector/Tests/Runtime/MessagePoolTests.cs b/com.unity.robotics.ros-tcp-connector/Tests/Runtime/MessagePoolTests.cs new file mode 100644 index 00000000..96bf801c --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Tests/Runtime/MessagePoolTests.cs @@ -0,0 +1,42 @@ +using System.Collections.Generic; +using NUnit.Framework; +using Unity.Robotics.ROSTCPConnector; +using UnityEngine; +using RosMessageTypes.Std; + +namespace UnitTests +{ + public class MessagePoolTests + { + [Test] + public void AddAndGet_OneMessage_ReturnsSameMessage() + { + MessagePool pool = new MessagePool(); + const string firstMessageContents = "first message"; + StringMsg str1 = new StringMsg(firstMessageContents); + pool.AddMessage(str1); + StringMsg str2 = pool.GetOrCreateMessage(); + StringMsg str3 = pool.GetOrCreateMessage(); + Assert.AreEqual(str1, str2); + Assert.AreNotEqual(str2, str3); + } + + [Test] + public void AddAndGet_MultipleMessages_ReturnCorrectMessage() + { + MessagePool pool = new MessagePool(); + const string firstMessageContents = "first message"; + StringMsg str1 = new StringMsg(firstMessageContents); + StringMsg str2 = new StringMsg(firstMessageContents); + pool.AddMessage(str1); + pool.AddMessage(str2); + StringMsg str3 = pool.GetOrCreateMessage(); + StringMsg str4 = pool.GetOrCreateMessage(); + StringMsg str5 = pool.GetOrCreateMessage(); + Assert.AreEqual(str1, str3); + Assert.AreEqual(str2, str4); + Assert.AreNotEqual(str1, str5); + Assert.AreNotEqual(str2, str5); + } + } +} diff --git a/com.unity.robotics.ros-tcp-connector/Tests/Runtime/MessagePoolTests.cs.meta b/com.unity.robotics.ros-tcp-connector/Tests/Runtime/MessagePoolTests.cs.meta new file mode 100644 index 00000000..6e2e9a3e --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Tests/Runtime/MessagePoolTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: cadd2bab8ec1f5d4c8067e9a47bd4c86 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Tests/Runtime/MessageTests.cs b/com.unity.robotics.ros-tcp-connector/Tests/Runtime/MessageTests.cs new file mode 100644 index 00000000..a9960635 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Tests/Runtime/MessageTests.cs @@ -0,0 +1,124 @@ +using System.Collections; +using System.Collections.Generic; +using UnityEngine; +using RosMessageTypes.Std; +using NUnit.Framework; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using System; + +namespace UnitTests +{ + public class MessageTests + { + class TestMessage : Message + { + } + + class TestResponse : Message + { + } + + [Test] + public void RegisterAndGet_OneMessage_ReturnsMessageNameAndDeserialize() + { + const string rosMessageName = "testmessage"; + Func deserializer = des => new TestMessage(); + + MessageRegistry.Register(rosMessageName, deserializer); + + Assert.AreEqual(rosMessageName, MessageRegistry.GetRosMessageName()); + Assert.AreEqual(deserializer, MessageRegistry.GetDeserializeFunction()); + Assert.AreEqual(deserializer, MessageRegistry.GetDeserializeFunction(rosMessageName)); + } + + [Test] + public void RegisterAndGet_Subtopics_ReturnCorrectDeserializers() + { + const string rosMessageName = "testmessage"; + Func deserializer = des => new TestMessage(); + Func deserializer2 = des => new TestResponse(); + + MessageRegistry.Register(rosMessageName, deserializer, MessageSubtopic.Default); + MessageRegistry.Register(rosMessageName, deserializer2, MessageSubtopic.Response); + + Assert.AreEqual(rosMessageName, MessageRegistry.GetRosMessageName()); + Assert.AreEqual(rosMessageName, MessageRegistry.GetRosMessageName()); + Assert.AreEqual(MessageSubtopic.Default, MessageRegistry.GetSubtopic()); + Assert.AreEqual(MessageSubtopic.Response, MessageRegistry.GetSubtopic()); + Assert.AreEqual(deserializer, MessageRegistry.GetDeserializeFunction()); + Assert.AreEqual(deserializer, MessageRegistry.GetDeserializeFunction(rosMessageName, MessageSubtopic.Default)); + Assert.AreEqual(deserializer2, MessageRegistry.GetDeserializeFunction()); + Assert.AreEqual(deserializer2, MessageRegistry.GetDeserializeFunction(rosMessageName, MessageSubtopic.Response)); + Assert.IsNull(MessageRegistry.GetDeserializeFunction(rosMessageName, MessageSubtopic.Goal)); + } + + [Test] + public void SerializeDeserialize_Int32RoundTrip_ReturnsSameValue() + { + const int v = 1234; + Int32Msg inMsg = new Int32Msg(v); + Int32Msg outMsg = MessageRoundTrip(inMsg, Int32Msg.Deserialize); + Assert.AreEqual(inMsg.data, v); + Assert.AreEqual(inMsg.data, outMsg.data); + } + + [Test] + public void SerializeDeserialize_MaxInt32RoundTrip_ReturnsMaxInt() + { + const int v = int.MaxValue; + Int32Msg inMsg = new Int32Msg(v); + Int32Msg outMsg = MessageRoundTrip(inMsg, Int32Msg.Deserialize); + Assert.AreEqual(inMsg.data, v); + Assert.AreEqual(inMsg.data, outMsg.data); + } + + [Test] + public void SerializeDeserialize_MinInt32RoundTrip_ReturnsMinInt() + { + const int v = int.MinValue; + Int32Msg inMsg = new Int32Msg(v); + Int32Msg outMsg = MessageRoundTrip(inMsg, Int32Msg.Deserialize); + Assert.AreEqual(inMsg.data, v); + Assert.AreEqual(inMsg.data, outMsg.data); + } + + [Test] + public void SerializeDeserialize_String_ReturnsSameString() + { + const string v = "hello"; + StringMsg inMsg = new StringMsg(v); + StringMsg outMsg = MessageRoundTrip(inMsg, StringMsg.Deserialize); + Assert.AreEqual(inMsg.data, v); + Assert.AreEqual(inMsg.data, outMsg.data); + } + + [Test] + public void SerializeDeserialize_UnicodeString_ReturnsSameString() + { + const string v = "ಠ_ಠ"; + StringMsg inMsg = new StringMsg(v); + StringMsg outMsg = MessageRoundTrip(inMsg, StringMsg.Deserialize); + Assert.AreEqual(inMsg.data, v); + Assert.AreEqual(inMsg.data, outMsg.data); + } + + [Test] + public void SerializeDeserialize_EmptyString_ReturnsSameString() + { + const string v = ""; + StringMsg inMsg = new StringMsg(v); + StringMsg outMsg = MessageRoundTrip(inMsg, StringMsg.Deserialize); + Assert.AreEqual(inMsg.data, v); + Assert.AreEqual(inMsg.data, outMsg.data); + } + + public T MessageRoundTrip(T inMsg, Func deserialize) where T : Message + { + MessageSerializer ser = new MessageSerializer(); + ser.SerializeMessage(inMsg); + MessageDeserializer deser = new MessageDeserializer(); + deser.InitWithBuffer(ser.GetBytes()); + return deserialize(deser); + } + } +} diff --git a/com.unity.robotics.ros-tcp-connector/Tests/Runtime/MessageTests.cs.meta b/com.unity.robotics.ros-tcp-connector/Tests/Runtime/MessageTests.cs.meta new file mode 100644 index 00000000..2fe480bc --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Tests/Runtime/MessageTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 5a03abaea2df85745a250de101c7fa23 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Tests/Runtime/ROSGeometryTests.cs b/com.unity.robotics.ros-tcp-connector/Tests/Runtime/ROSGeometryTests.cs index 7f7440e6..b4318e8c 100644 --- a/com.unity.robotics.ros-tcp-connector/Tests/Runtime/ROSGeometryTests.cs +++ b/com.unity.robotics.ros-tcp-connector/Tests/Runtime/ROSGeometryTests.cs @@ -4,7 +4,7 @@ using UnityEngine; using Assert = UnityEngine.Assertions.Assert; -namespace Tests.Runtime +namespace UnitTests { public class RosGeometryTests { diff --git a/com.unity.robotics.ros-tcp-connector/Tests/Runtime/RosConnectionTests.cs b/com.unity.robotics.ros-tcp-connector/Tests/Runtime/RosConnectionTests.cs new file mode 100644 index 00000000..649d7381 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Tests/Runtime/RosConnectionTests.cs @@ -0,0 +1,35 @@ +using System.Collections; +using System.Collections.Generic; +using NUnit.Framework; +using RosMessageTypes.BuiltinInterfaces; +using RosMessageTypes.Geometry; +using RosMessageTypes.Tf2; +using Unity.Robotics.ROSTCPConnector; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; +using UnityEngine.TestTools; + +namespace UnitTests +{ + public class RosConnectionTests + { + [Test] + public void GetOrCreateInstance_CallOnce_ReturnsValidInstance() + { + ROSConnection ros = ROSConnection.GetOrCreateInstance(); + ros.ConnectOnStart = false; + Assert.NotNull(ros); + } + + [Test] + public void GetOrCreateInstance_CallTwice_ReturnsSameInstance() + { + ROSConnection ros = ROSConnection.GetOrCreateInstance(); + Assert.NotNull(ros); + ros.ConnectOnStart = false; + ROSConnection ros2 = ROSConnection.GetOrCreateInstance(); + Assert.AreEqual(ros, ros2); + } + } +} diff --git a/com.unity.robotics.ros-tcp-connector/Tests/Runtime/RosConnectionTests.cs.meta b/com.unity.robotics.ros-tcp-connector/Tests/Runtime/RosConnectionTests.cs.meta new file mode 100644 index 00000000..f37e53c7 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Tests/Runtime/RosConnectionTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: e7a8df595f6128b46b2f1995f98dc170 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Tests/Runtime/TFSystemTests.cs b/com.unity.robotics.ros-tcp-connector/Tests/Runtime/TFSystemTests.cs new file mode 100644 index 00000000..65b44b26 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Tests/Runtime/TFSystemTests.cs @@ -0,0 +1,185 @@ +using System.Collections; +using System.Collections.Generic; +using NUnit.Framework; +using RosMessageTypes.BuiltinInterfaces; +using RosMessageTypes.Geometry; +using RosMessageTypes.Tf2; +using RosMessageTypes.Std; +using Unity.Robotics.ROSTCPConnector; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; +using UnityEngine.TestTools; + +namespace UnitTests +{ + public class TFSystemTests + { + const string simple_frame_id = "test_frame_id"; + const string simple_frame_id2 = "test_frame_id2"; + const string parent_frame_id = "parent_frame_id"; + const string composite_frame_id = parent_frame_id + "/" + simple_frame_id; + const string composite_frame_id2 = parent_frame_id + "/" + simple_frame_id2; + + HeaderMsg MakeHeaderMsg(TimeMsg time, string frameID) + { +#if !ROS2 + return new HeaderMsg((uint)0, time, frameID); +#else + return new HeaderMsg(time, frameID); +#endif + } + + TimeMsg MakeTimeMsg(int secs, int nsecs) + { +#if !ROS2 + return new TimeMsg((uint)secs, (uint)nsecs); +#else + return new TimeMsg(secs, (uint)nsecs); +#endif + } + + [Test] + public void ReceiveTF_SingleMessage_ReturnsSameTranslation() + { + ROSConnection ros = ROSConnection.GetOrCreateInstance(); + ros.ConnectOnStart = false; + TFSystem system = TFSystem.GetOrCreateInstance(); + TFSystem.TFTopicState topic = system.GetOrCreateTFTopic(); + TFStream stream = system.GetOrCreateFrame(simple_frame_id); + TimeMsg time = MakeTimeMsg(4567, 890); + Vector3 unityPosition = new Vector3(1, 2, 3); + topic.ReceiveTF(new TFMessageMsg(new TransformStampedMsg[] { new TransformStampedMsg( + MakeHeaderMsg( time, parent_frame_id ), + simple_frame_id, + new TransformMsg( unityPosition.To(), new QuaternionMsg() + ))})); + + TFFrame frame = stream.GetWorldTF(time.ToLongTime()); + Assert.AreEqual(frame.translation.x, unityPosition.x); + Assert.AreEqual(frame.translation.y, unityPosition.y); + Assert.AreEqual(frame.translation.z, unityPosition.z); + } + + [Test] + public void ReceiveTF_FrameWithParent_ReturnsSameTranslation() + { + ROSConnection ros = ROSConnection.GetOrCreateInstance(); + ros.ConnectOnStart = false; + TFSystem system = TFSystem.GetOrCreateInstance(); + TFSystem.TFTopicState topic = system.GetOrCreateTFTopic(); + TFStream stream = system.GetOrCreateFrame(composite_frame_id); + Assert.AreEqual(stream.Name, simple_frame_id); + Assert.AreEqual(stream.Parent.Name, parent_frame_id); + TimeMsg time = MakeTimeMsg(4567, 890); + Vector3 unityPosition = new Vector3(1, 2, 3); + topic.ReceiveTF(new TFMessageMsg(new TransformStampedMsg[] { new TransformStampedMsg( + MakeHeaderMsg( time, parent_frame_id ), + composite_frame_id, + new TransformMsg( unityPosition.To(), new QuaternionMsg() + ))})); + + TFFrame frame = stream.GetWorldTF(time.ToLongTime()); + Assert.AreEqual(frame.translation.x, unityPosition.x); + Assert.AreEqual(frame.translation.y, unityPosition.y); + Assert.AreEqual(frame.translation.z, unityPosition.z); + Vector3 gameObjectPos = stream.GameObject.transform.position; + Assert.AreEqual(gameObjectPos.x, unityPosition.x); + Assert.AreEqual(gameObjectPos.y, unityPosition.y); + Assert.AreEqual(gameObjectPos.z, unityPosition.z); + } + + [Test] + public void GetOrCreateFrame_Hierarchy_BuildsValidHierarchy() + { + ROSConnection ros = ROSConnection.GetOrCreateInstance(); + ros.ConnectOnStart = false; + TFSystem.TFTopicState topic = TFSystem.GetOrCreateInstance().GetOrCreateTFTopic(); + TFStream stream = topic.GetOrCreateFrame(composite_frame_id); + GameObject gameObject = stream.GameObject; + Assert.IsNotNull(stream.GameObject); + Assert.AreEqual(stream.GameObject.name, simple_frame_id); + Assert.AreEqual(stream.GameObject.transform.parent.name, parent_frame_id); + TFStream stream2 = topic.GetOrCreateFrame(composite_frame_id2); + Assert.IsNotNull(stream2.GameObject); + Assert.AreEqual(stream2.GameObject.name, simple_frame_id2); + Assert.AreNotEqual(stream.GameObject, stream2.GameObject); + Assert.AreEqual(stream.GameObject.transform.parent, stream2.GameObject.transform.parent); + } + + [Test] + public void ReceiveTF_MultipleMessages_InterpolatesTimes() + { + ROSConnection ros = ROSConnection.GetOrCreateInstance(); + ros.ConnectOnStart = false; + TFSystem.TFTopicState topic = TFSystem.GetOrCreateInstance().GetOrCreateTFTopic(); + TFStream stream = topic.GetOrCreateFrame(simple_frame_id); + int time1_secs = 1000; + int time2_secs = 2000; + TimeMsg time1 = MakeTimeMsg(time1_secs, 0); + Vector3 unityPosition1 = new Vector3(1, 2, 3); + topic.ReceiveTF(new TFMessageMsg(new TransformStampedMsg[] { new TransformStampedMsg( + MakeHeaderMsg( time1, parent_frame_id ), + simple_frame_id, + new TransformMsg( unityPosition1.To(), new QuaternionMsg() + ))})); + TimeMsg time2 = MakeTimeMsg(time2_secs, 0); + Vector3 unityPosition2 = new Vector3(2, 3, 4); + topic.ReceiveTF(new TFMessageMsg(new TransformStampedMsg[] { new TransformStampedMsg( + MakeHeaderMsg( time2, parent_frame_id ), + simple_frame_id, + new TransformMsg( unityPosition2.To(), new QuaternionMsg() + ))})); + TimeMsg time1_5 = MakeTimeMsg((time1_secs + time2_secs) / 2, 0); + Vector3 pointAtTime1 = stream.GetWorldTF(time1).translation; + Vector3 pointAtTime1_5 = stream.GetWorldTF(time1_5).translation; + Vector3 pointAtTime2 = stream.GetWorldTF(time2).translation; + Vector3 unityPosition1_5 = (unityPosition1 + unityPosition2) / 2; + Assert.AreEqual(pointAtTime1, unityPosition1); + Assert.AreEqual(pointAtTime1_5, unityPosition1_5); + Assert.AreEqual(pointAtTime2, unityPosition2); + } + + [Test] + public void ReceiveTF_MultipleMessagesOutOfOrder_InterpolatesTimes() + { + ROSConnection ros = ROSConnection.GetOrCreateInstance(); + ros.ConnectOnStart = false; + TFSystem.TFTopicState topic = TFSystem.GetOrCreateInstance().GetOrCreateTFTopic(); + TFStream stream = topic.GetOrCreateFrame(simple_frame_id); + int time1_secs = 1000; + int time2_secs = 2000; + int time3_secs = 3000; + TimeMsg time1 = MakeTimeMsg(time1_secs, 0); + TimeMsg time2 = MakeTimeMsg(time2_secs, 0); + TimeMsg time3 = MakeTimeMsg(time3_secs, 0); + Vector3 unityPosition1 = new Vector3(1, 1, 1); + Vector3 unityPosition2 = new Vector3(3, 1, 1); + Vector3 unityPosition3 = new Vector3(3, 3, 1); + topic.ReceiveTF(new TFMessageMsg(new TransformStampedMsg[] { new TransformStampedMsg( + MakeHeaderMsg( time1, parent_frame_id ), + simple_frame_id, + new TransformMsg( unityPosition1.To(), new QuaternionMsg() + ))})); + topic.ReceiveTF(new TFMessageMsg(new TransformStampedMsg[] { new TransformStampedMsg( + MakeHeaderMsg( time3, parent_frame_id ), + simple_frame_id, + new TransformMsg( unityPosition3.To(), new QuaternionMsg() + ))})); + topic.ReceiveTF(new TFMessageMsg(new TransformStampedMsg[] { new TransformStampedMsg( + MakeHeaderMsg( time2, parent_frame_id ), + simple_frame_id, + new TransformMsg( unityPosition2.To(), new QuaternionMsg() + ))})); + TimeMsg time1_5 = MakeTimeMsg((time1_secs + time2_secs) / 2, 0); + TimeMsg time2_5 = MakeTimeMsg((time2_secs + time3_secs) / 2, 0); + Vector3 unityPosition1_5 = (unityPosition1 + unityPosition2) / 2; + Vector3 unityPosition2_5 = (unityPosition2 + unityPosition3) / 2; + Assert.AreEqual(stream.GetWorldTF(time1).translation, unityPosition1); + Assert.AreEqual(stream.GetWorldTF(time1_5).translation, unityPosition1_5); + Assert.AreEqual(stream.GetWorldTF(time2).translation, unityPosition2); + Assert.AreEqual(stream.GetWorldTF(time2_5).translation, unityPosition2_5); + Assert.AreEqual(stream.GetWorldTF(time3).translation, unityPosition3); + } + } +} diff --git a/com.unity.robotics.ros-tcp-connector/Tests/Runtime/TFSystemTests.cs.meta b/com.unity.robotics.ros-tcp-connector/Tests/Runtime/TFSystemTests.cs.meta new file mode 100644 index 00000000..958ce823 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Tests/Runtime/TFSystemTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6822666f791d8c4488c99177a41512ce +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Tests/Runtime/Unity.Perception.Runtime.Tests.asmdef b/com.unity.robotics.ros-tcp-connector/Tests/Runtime/Unity.Robotics.RosTcpConnector.Runtime.Tests.asmdef similarity index 100% rename from com.unity.robotics.ros-tcp-connector/Tests/Runtime/Unity.Perception.Runtime.Tests.asmdef rename to com.unity.robotics.ros-tcp-connector/Tests/Runtime/Unity.Robotics.RosTcpConnector.Runtime.Tests.asmdef diff --git a/com.unity.robotics.ros-tcp-connector/Tests/Runtime/Unity.Perception.Runtime.Tests.asmdef.meta b/com.unity.robotics.ros-tcp-connector/Tests/Runtime/Unity.Robotics.RosTcpConnector.Runtime.Tests.asmdef.meta similarity index 100% rename from com.unity.robotics.ros-tcp-connector/Tests/Runtime/Unity.Perception.Runtime.Tests.asmdef.meta rename to com.unity.robotics.ros-tcp-connector/Tests/Runtime/Unity.Robotics.RosTcpConnector.Runtime.Tests.asmdef.meta diff --git a/com.unity.robotics.ros-tcp-connector/package.json b/com.unity.robotics.ros-tcp-connector/package.json index 4f2768f7..573c73f5 100644 --- a/com.unity.robotics.ros-tcp-connector/package.json +++ b/com.unity.robotics.ros-tcp-connector/package.json @@ -6,5 +6,6 @@ "unity": "2020.2", "unityRelease": "0b9", "dependencies": { + "nuget.moq": "1.0.0" } } From e1ec6156d25a0a9f42f4ee47a1205367ff6b6cb8 Mon Sep 17 00:00:00 2001 From: Amanda <31416491+at669@users.noreply.github.com> Date: Thu, 9 Dec 2021 16:49:55 -0800 Subject: [PATCH 25/33] AIRO-1561 Add Sonarqube (#244) * Initial Sonarqube yml commit * Fix sonarqube yamato * Update changelog, switch to PRD --- .yamato/sonar.yml | 26 +++++++++++++++++++ .../CHANGELOG.md | 2 ++ 2 files changed, 28 insertions(+) create mode 100644 .yamato/sonar.yml diff --git a/.yamato/sonar.yml b/.yamato/sonar.yml new file mode 100644 index 00000000..dc6749ed --- /dev/null +++ b/.yamato/sonar.yml @@ -0,0 +1,26 @@ +name: Sonarqube Scan +agent: + type: Unity::metal::macmini + image: package-ci/mac + flavor: m1.mac +commands: + - npm install upm-ci-utils@stable -g --registry https://artifactory.prd.it.unity3d.com/artifactory/api/npm/upm-npm + - unity-downloader-cli -u 2020.3.11f1 -c Editor + - brew install mono corretto + - curl https://github.com/SonarSource/sonar-scanner-msbuild/releases/download/5.2.1.31210/sonar-scanner-msbuild-5.2.1.31210-net46.zip -o sonar-scanner-msbuild-net46.zip -L + - unzip sonar-scanner-msbuild-net46.zip -d ~ + - chmod a+x ~/sonar-scanner-4.6.1.2450/bin/sonar-scanner + - .Editor/Unity.app/Contents/MacOS/Unity -projectPath ./TestRosTcpConnector -batchmode -quit -nographics -logFile - -executeMethod "UnityEditor.SyncVS.SyncSolution" + - command: | + cd TestRosTcpConnector + for file in *.csproj; do sed -i.backup "s/^[[:blank:]]*false<\/ReferenceOutputAssembly>/true<\/ReferenceOutputAssembly>/g" $file; rm $file.backup; done + cd ../ + - mono ~/SonarScanner.MSBuild.exe begin /k:"ai-robotics-ros-tcp-connector" /d:sonar.host.url=$SONARQUBE_ENDPOINT_URL_PRD /d:sonar.login=$SONARQUBE_TOKEN_PRD /d:sonar.projectBaseDir=/Users/bokken/build/output/Unity-Technologies/ROS-TCP-Connector/com.unity.robotics.ros-tcp-connector + - msbuild ./TestRosTcpConnector/TestRosTcpConnector.sln + - mono ~/SonarScanner.MSBuild.exe end /d:sonar.login=$SONARQUBE_TOKEN_PRD +triggers: + cancel_old_ci: true + expression: | + ((pull_request.target eq "main" OR pull_request.target eq "dev") + AND NOT pull_request.push.changes.all match "**/*.md") OR + (push.branch eq "main" OR push.branch eq "dev") \ No newline at end of file diff --git a/com.unity.robotics.ros-tcp-connector/CHANGELOG.md b/com.unity.robotics.ros-tcp-connector/CHANGELOG.md index 24f8376c..b1e417b7 100644 --- a/com.unity.robotics.ros-tcp-connector/CHANGELOG.md +++ b/com.unity.robotics.ros-tcp-connector/CHANGELOG.md @@ -12,6 +12,8 @@ The format is based on [Keep a Changelog](http://keepachangelog.com/en/1.0.0/) a ### Added +Added Sonarqube scanner + ### Changed ### Deprecated From c17c42ab13de938c7cc7f3edea789e4508f6bb8d Mon Sep 17 00:00:00 2001 From: Amanda <31416491+at669@users.noreply.github.com> Date: Wed, 15 Dec 2021 12:43:42 -0800 Subject: [PATCH 26/33] Variable cleaup sonar.yml (#245) --- .yamato/sonar.yml | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/.yamato/sonar.yml b/.yamato/sonar.yml index dc6749ed..6387260f 100644 --- a/.yamato/sonar.yml +++ b/.yamato/sonar.yml @@ -3,20 +3,26 @@ agent: type: Unity::metal::macmini image: package-ci/mac flavor: m1.mac +variables: + PROJECT_PATH: TestRosTcpConnector + SONARQUBE_PROJECT_KEY: ai-robotics-ros-tcp-connector + SONARQUBE_PROJECT_BASE_DIR: /Users/bokken/build/output/Unity-Technologies/ROS-TCP-Connector/com.unity.robotics.ros-tcp-connector + MSBUILD_SLN_PATH: ./TestRosTcpConnector/TestRosTcpConnector.sln + UNITY_VERSION: 2020.3.11f1 commands: - npm install upm-ci-utils@stable -g --registry https://artifactory.prd.it.unity3d.com/artifactory/api/npm/upm-npm - - unity-downloader-cli -u 2020.3.11f1 -c Editor + - unity-downloader-cli -u $UNITY_VERSION -c Editor - brew install mono corretto - curl https://github.com/SonarSource/sonar-scanner-msbuild/releases/download/5.2.1.31210/sonar-scanner-msbuild-5.2.1.31210-net46.zip -o sonar-scanner-msbuild-net46.zip -L - unzip sonar-scanner-msbuild-net46.zip -d ~ - chmod a+x ~/sonar-scanner-4.6.1.2450/bin/sonar-scanner - - .Editor/Unity.app/Contents/MacOS/Unity -projectPath ./TestRosTcpConnector -batchmode -quit -nographics -logFile - -executeMethod "UnityEditor.SyncVS.SyncSolution" + - .Editor/Unity.app/Contents/MacOS/Unity -projectPath $PROJECT_PATH -batchmode -quit -nographics -logFile - -executeMethod "UnityEditor.SyncVS.SyncSolution" - command: | - cd TestRosTcpConnector + cd $PROJECT_PATH for file in *.csproj; do sed -i.backup "s/^[[:blank:]]*false<\/ReferenceOutputAssembly>/true<\/ReferenceOutputAssembly>/g" $file; rm $file.backup; done cd ../ - - mono ~/SonarScanner.MSBuild.exe begin /k:"ai-robotics-ros-tcp-connector" /d:sonar.host.url=$SONARQUBE_ENDPOINT_URL_PRD /d:sonar.login=$SONARQUBE_TOKEN_PRD /d:sonar.projectBaseDir=/Users/bokken/build/output/Unity-Technologies/ROS-TCP-Connector/com.unity.robotics.ros-tcp-connector - - msbuild ./TestRosTcpConnector/TestRosTcpConnector.sln + - mono ~/SonarScanner.MSBuild.exe begin /k:$SONARQUBE_PROJECT_KEY /d:sonar.host.url=$SONARQUBE_ENDPOINT_URL_PRD /d:sonar.login=$SONARQUBE_TOKEN_PRD /d:sonar.projectBaseDir=$SONARQUBE_PROJECT_BASE_DIR + - msbuild $MSBUILD_SLN_PATH - mono ~/SonarScanner.MSBuild.exe end /d:sonar.login=$SONARQUBE_TOKEN_PRD triggers: cancel_old_ci: true From 36b7a434a5366862d95fbb850184c6ad6085491d Mon Sep 17 00:00:00 2001 From: Vishwajeet Narwal Date: Wed, 12 Jan 2022 12:49:30 -0600 Subject: [PATCH 27/33] AIRO-1648 Fix step size in ToImageMsg() Function (#247) Fixes incorrect step size for image message in ToImageMsg() function, which was causing error while trying to view Image topic in rqt, image_view and rviz. --- .../Runtime/Extensions/MessageExtensions.cs | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Extensions/MessageExtensions.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Extensions/MessageExtensions.cs index 0f2bcb11..a1a09e37 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/Extensions/MessageExtensions.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Extensions/MessageExtensions.cs @@ -699,31 +699,36 @@ public static Texture2D RegionOfInterestTexture(this RegionOfInterestMsg message public static ImageMsg ToImageMsg(this Texture2D tex, HeaderMsg header) { byte[] data = null; - string encoding = "rgba8"; + string encoding; + int step; switch (tex.format) { case TextureFormat.RGB24: data = new byte[tex.width * tex.height * 3]; tex.GetPixelData(0).CopyTo(data); encoding = "rgb8"; + step = 3 * tex.width; ReverseInBlocks(data, tex.width * 3, tex.height); break; case TextureFormat.RGBA32: data = new byte[tex.width * tex.height * 4]; tex.GetPixelData(0).CopyTo(data); encoding = "rgba8"; + step = 4 * tex.width; ReverseInBlocks(data, tex.width * 4, tex.height); break; case TextureFormat.R8: data = new byte[tex.width * tex.height]; tex.GetPixelData(0).CopyTo(data); encoding = "8UC1"; + step = 1 * tex.width; ReverseInBlocks(data, tex.width, tex.height); break; case TextureFormat.R16: data = new byte[tex.width * tex.height * 2]; tex.GetPixelData(0).CopyTo(data); encoding = "16UC1"; + step = 2 * tex.width; ReverseInBlocks(data, tex.width * 2, tex.height); break; default: @@ -742,9 +747,10 @@ public static ImageMsg ToImageMsg(this Texture2D tex, HeaderMsg header) } ReverseInBlocks(data, tex.width * 4, tex.height); encoding = "rgba8"; + step = 4 * tex.width; break; } - return new ImageMsg(header, height: (uint)tex.height, width: (uint)tex.width, encoding: encoding, is_bigendian: 0, step: 4, data: data); + return new ImageMsg(header, height: (uint)tex.height, width: (uint)tex.width, encoding: encoding, is_bigendian: 0, step: (uint)step, data: data); } public static string ToLatLongString(this NavSatFixMsg message) From 07bdf308ccb741837b744d5fba48c905f6ff563e Mon Sep 17 00:00:00 2001 From: "Devin Miller (Unity)" Date: Wed, 19 Jan 2022 15:38:35 -0800 Subject: [PATCH 28/33] AIRO-1647 Adding scripted Action importer and fixing action message registration (#250) * Adding scripted Action importer and fixing action message registration Co-authored-by: LaurieCheers <73140792+LaurieCheers-unity@users.noreply.github.com> --- .../Editor/MessageGeneration/ActionAutoGen.cs | 3 ++ .../Editor/MessageGeneration/MessageParser.cs | 16 ++-------- .../MessageGeneration/MsgAutoGenUtilities.cs | 14 ++++++++ .../ScriptedActionImporter.cs | 32 +++++++++++++++++++ .../ScriptedActionImporter.cs.meta | 11 +++++++ 5 files changed, 63 insertions(+), 13 deletions(-) create mode 100644 com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/ScriptedActionImporter.cs create mode 100644 com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/ScriptedActionImporter.cs.meta diff --git a/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/ActionAutoGen.cs b/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/ActionAutoGen.cs index 065bc6b9..8c1a19bd 100644 --- a/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/ActionAutoGen.cs +++ b/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/ActionAutoGen.cs @@ -354,6 +354,9 @@ public void WrapActionSections(string type) writer.Write(GenerateSerializationStatements(type)); + // Omit the subtype because subtype is baked into the class name + writer.Write(MsgAutoGenUtilities.InitializeOnLoad()); + // Close class writer.Write(ONE_TAB + "}\n"); // Close namespace diff --git a/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/MessageParser.cs b/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/MessageParser.cs index 1933d950..8a3dc958 100644 --- a/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/MessageParser.cs +++ b/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/MessageParser.cs @@ -154,20 +154,10 @@ public void Parse() // Write ToString override writer.Write(GenerateToString()); - string subtopicParameter = subtopic == MessageSubtopic.Default ? "" : $", MessageSubtopic.{subtopic}"; + var subtopicParameter = + subtopic == MessageSubtopic.Default ? "" : $", MessageSubtopic.{subtopic}"; - writer.Write( - "\n" + - "#if UNITY_EDITOR\n" + - TWO_TABS + "[UnityEditor.InitializeOnLoadMethod]\n" + - "#else\n" + - TWO_TABS + "[UnityEngine.RuntimeInitializeOnLoadMethod]\n" + - "#endif\n" + - TWO_TABS + "public static void Register()\n" + - TWO_TABS + "{\n" + - THREE_TABS + $"MessageRegistry.Register(k_RosMessageName, Deserialize{subtopicParameter});\n" + - TWO_TABS + "}\n" - ); + writer.Write(InitializeOnLoad(subtopicParameter)); // Close class writer.Write(MsgAutoGenUtilities.ONE_TAB + "}\n"); diff --git a/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/MsgAutoGenUtilities.cs b/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/MsgAutoGenUtilities.cs index ab27e4dc..4127c679 100644 --- a/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/MsgAutoGenUtilities.cs +++ b/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/MsgAutoGenUtilities.cs @@ -147,5 +147,19 @@ public static string ResolvePackageName(string s) } return CapitalizeFirstLetter(s); } + + public static string InitializeOnLoad(string subtopicArgument = "") + { + return "\n" + + "#if UNITY_EDITOR\n" + + TWO_TABS + "[UnityEditor.InitializeOnLoadMethod]\n" + + "#else\n" + + TWO_TABS + "[UnityEngine.RuntimeInitializeOnLoadMethod]\n" + + "#endif\n" + + TWO_TABS + "public static void Register()\n" + + TWO_TABS + "{\n" + + THREE_TABS + $"MessageRegistry.Register(k_RosMessageName, Deserialize{subtopicArgument});\n" + + TWO_TABS + "}\n"; + } } } diff --git a/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/ScriptedActionImporter.cs b/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/ScriptedActionImporter.cs new file mode 100644 index 00000000..faf1e8ad --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/ScriptedActionImporter.cs @@ -0,0 +1,32 @@ +using System.IO; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using UnityEditor; +using UnityEngine; +#if UNITY_2020_2_OR_NEWER +using UnityEditor.AssetImporters; +#else +using UnityEditor.Experimental.AssetImporters; +#endif + +namespace Unity.Robotics.ROSTCPConnector.Editor.MessageGeneration +{ + [ScriptedImporter(1, "action")] + public class ScriptedActionImporter : ScriptedImporter + { + public override void OnImportAsset(AssetImportContext ctx) + { + var inputPath = Path.Combine(Directory.GetCurrentDirectory(), ctx.assetPath); + var outputPath = MessageGenBrowserSettings.Get().outputPath; + ActionAutoGen.GenerateSingleAction(inputPath, MessageGenBrowserSettings.Get().outputPath); + + foreach (string builtPath in ServiceAutoGen.GetServiceClassPaths(inputPath, outputPath)) + { + var builtAssetPath = Path.Combine("Assets", MessageGenBrowserSettings.ToRelativePath(builtPath)); + AssetDatabase.ImportAsset(builtAssetPath); + var messageClass = AssetDatabase.LoadAssetAtPath(builtAssetPath, typeof(MonoScript)); + if (messageClass != null) + ctx.AddObjectToAsset("messageClass", messageClass); + } + } + } +} diff --git a/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/ScriptedActionImporter.cs.meta b/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/ScriptedActionImporter.cs.meta new file mode 100644 index 00000000..d2f90cc3 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/ScriptedActionImporter.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: a80f940ccb8ea1b4f9bd32fdc136bdce +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: From a9478059bceb33fbb000266f779ff5e3193245d0 Mon Sep 17 00:00:00 2001 From: LaurieCheers-unity <73140792+LaurieCheers-unity@users.noreply.github.com> Date: Wed, 26 Jan 2022 12:23:59 -0800 Subject: [PATCH 29/33] AIRO-1381 Version check (#210) --- .../Runtime/TcpConnector/ROSConnection.cs | 42 ++++++++++++++++++- .../Runtime/TcpConnector/SysCommand.cs | 17 ++++++++ 2 files changed, 58 insertions(+), 1 deletion(-) 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 df3e64aa..8031478a 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSConnection.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSConnection.cs @@ -15,6 +15,9 @@ namespace Unity.Robotics.ROSTCPConnector { public class ROSConnection : MonoBehaviour { + public const string k_Version = "v0.7.0"; + public const string k_CompatibleVersionPrefix = "v0.7."; + // Variables required for ROS communication [SerializeField] [FormerlySerializedAs("hostName")] @@ -633,6 +636,32 @@ void ReceiveSysCommand(string topic, string json) { switch (topic) { + case SysCommand.k_SysCommand_Handshake: + { + var handshakeCommand = JsonUtility.FromJson(json); + if (handshakeCommand.version == null) + { + Debug.LogError($"Corrupted or unreadable ROS-TCP-Endpoint version data! Expected: {k_Version}"); + } + else if (!handshakeCommand.version.StartsWith(k_CompatibleVersionPrefix)) + { + Debug.LogError($"Incompatible ROS-TCP-Endpoint version: {handshakeCommand.version}. Expected: {k_Version}"); + } + + var handshakeMetadata = JsonUtility.FromJson(handshakeCommand.metadata); +#if ROS2 + if (handshakeMetadata.protocol != "ROS2") + { + Debug.LogError($"Incompatible protocol: ROS-TCP-Endpoint is using {handshakeMetadata.protocol}, but Unity is in ROS2 mode. Switch it from the Robotics/Ros Settings menu."); + } +#else + if (handshakeMetadata.protocol != "ROS1") + { + Debug.LogError($"Incompatible protocol: ROS-TCP-Endpoint is using {handshakeMetadata.protocol}, but Unity is in ROS1 mode. Switch it from the Robotics/Ros Settings menu."); + } +#endif + } + break; case SysCommand.k_SysCommand_Log: { var logCommand = JsonUtility.FromJson(json); @@ -843,12 +872,23 @@ static async Task ConnectionThread( static async Task ReaderThread(int readerIdx, NetworkStream networkStream, ConcurrentQueue> queue, int sleepMilliseconds, CancellationToken token) { + // First message should be the handshake + Tuple handshakeContent = await ReadMessageContents(networkStream, sleepMilliseconds, token); + if (handshakeContent.Item1 == SysCommand.k_SysCommand_Handshake) + { + ROSConnection.m_HasConnectionError = false; + queue.Enqueue(handshakeContent); + } + else + { + Debug.LogError($"Invalid ROS-TCP-Endpoint version detected: 0.6.0 or older. Expected: {k_Version}."); + } + while (!token.IsCancellationRequested) { try { Tuple content = await ReadMessageContents(networkStream, sleepMilliseconds, token); - // Debug.Log($"Message {content.Item1} received"); ROSConnection.m_HasConnectionError = false; if (content.Item1 != "") // ignore keepalive messages 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 0518fd53..c7ee7d1d 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/SysCommand.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/SysCommand.cs @@ -7,6 +7,7 @@ namespace Unity.Robotics.ROSTCPConnector { public abstract class SysCommand { + public const string k_SysCommand_Handshake = "__handshake"; public const string k_SysCommand_Log = "__log"; public const string k_SysCommand_Warning = "__warn"; public const string k_SysCommand_Error = "__error"; @@ -67,6 +68,22 @@ public struct SysCommand_TopicAndType public string message_name; } + // For backwards compatibility, we encode the handshake in two stages: + // Stage 1 - which must NEVER change - is just a version string and a metadata string. + public struct SysCommand_Handshake + { + public string version; + public string metadata; + } + + // Stage 2 is the json encoded contents of the metadata string. + // Because this structure may change with future versions of ROS TCP Connector, we only decode it + // after checking the version number is correct. + public struct SysCommand_Handshake_Metadata + { + public string protocol; // "ROS1" or "ROS2" + } + public struct SysCommand_Log { public string text; From 4334a419a28b622d961363c4856f2db036b925d6 Mon Sep 17 00:00:00 2001 From: LaurieCheers-unity <73140792+LaurieCheers-unity@users.noreply.github.com> Date: Fri, 28 Jan 2022 16:16:40 -0800 Subject: [PATCH 30/33] Update CHANGELOG.md (#255) --- .../CHANGELOG.md | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/com.unity.robotics.ros-tcp-connector/CHANGELOG.md b/com.unity.robotics.ros-tcp-connector/CHANGELOG.md index b1e417b7..5b3567bb 100644 --- a/com.unity.robotics.ros-tcp-connector/CHANGELOG.md +++ b/com.unity.robotics.ros-tcp-connector/CHANGELOG.md @@ -14,8 +14,26 @@ The format is based on [Keep a Changelog](http://keepachangelog.com/en/1.0.0/) a Added Sonarqube scanner +Added more tests + +Add support for cloud rendering + +Added MessagePool to enable message reuse and reduce garbage collection + +Can configure what direction is "north" for NED and ENU coordinates + ### Changed +CameraInfo.msg field names are different in ROS2 + +Bug fix - cope with tab character in a .msg file + +Bug fix - no padding when serializing an empty array + +Can publish messages from threads other than main + +Visualizations work with ROS services + ### Deprecated ### Removed From bd7e38301c493a32c5f68becdbbd5dc3cbf8b593 Mon Sep 17 00:00:00 2001 From: LaurieCheers-unity <73140792+LaurieCheers-unity@users.noreply.github.com> Date: Fri, 28 Jan 2022 16:29:26 -0800 Subject: [PATCH 31/33] AIRO-1695 Building for android and ios (#254) --- com.unity.robotics.ros-tcp-connector/CHANGELOG.md | 2 ++ .../Runtime/Unity.Robotics.ROSTCPConnector.asmdef | 4 +++- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/com.unity.robotics.ros-tcp-connector/CHANGELOG.md b/com.unity.robotics.ros-tcp-connector/CHANGELOG.md index 5b3567bb..d163ab3e 100644 --- a/com.unity.robotics.ros-tcp-connector/CHANGELOG.md +++ b/com.unity.robotics.ros-tcp-connector/CHANGELOG.md @@ -12,6 +12,8 @@ The format is based on [Keep a Changelog](http://keepachangelog.com/en/1.0.0/) a ### Added +Enabled Android and IOS builds + Added Sonarqube scanner Added more tests diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Unity.Robotics.ROSTCPConnector.asmdef b/com.unity.robotics.ros-tcp-connector/Runtime/Unity.Robotics.ROSTCPConnector.asmdef index 22d563f8..2cedc815 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/Unity.Robotics.ROSTCPConnector.asmdef +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Unity.Robotics.ROSTCPConnector.asmdef @@ -6,8 +6,10 @@ "GUID:5db6cbfd5d0e86a46862a70f4b4067f2" ], "includePlatforms": [ - "CloudRendering", + "CloudRendering", + "Android", "Editor", + "iOS", "LinuxStandalone64", "macOSStandalone", "WSA", From 51b27d2bccd7d36d36cbce6804e86e7786f34dda Mon Sep 17 00:00:00 2001 From: LaurieCheers-unity <73140792+LaurieCheers-unity@users.noreply.github.com> Date: Mon, 31 Jan 2022 12:05:04 -0800 Subject: [PATCH 32/33] AIRO-1624 "Listen for TF messages" option in Settings (#256) * Update ROSSettingsEditor.cs * Update CHANGELOG.md --- com.unity.robotics.ros-tcp-connector/CHANGELOG.md | 2 ++ .../Editor/ROSSettingsEditor.cs | 4 ++++ 2 files changed, 6 insertions(+) diff --git a/com.unity.robotics.ros-tcp-connector/CHANGELOG.md b/com.unity.robotics.ros-tcp-connector/CHANGELOG.md index d163ab3e..ff4d1c0a 100644 --- a/com.unity.robotics.ros-tcp-connector/CHANGELOG.md +++ b/com.unity.robotics.ros-tcp-connector/CHANGELOG.md @@ -12,6 +12,8 @@ The format is based on [Keep a Changelog](http://keepachangelog.com/en/1.0.0/) a ### Added +Added "Listen for TF Messages" to Settings + Enabled Android and IOS builds Added Sonarqube scanner diff --git a/com.unity.robotics.ros-tcp-connector/Editor/ROSSettingsEditor.cs b/com.unity.robotics.ros-tcp-connector/Editor/ROSSettingsEditor.cs index 4d161663..45c36ffd 100644 --- a/com.unity.robotics.ros-tcp-connector/Editor/ROSSettingsEditor.cs +++ b/com.unity.robotics.ros-tcp-connector/Editor/ROSSettingsEditor.cs @@ -131,6 +131,10 @@ protected virtual void OnGUI() "Sleep this long before checking for new network messages. (Decreasing this time will make it respond faster, but consume more CPU)."), prefab.SleepTimeSeconds); + EditorGUILayout.Space(); + + prefab.listenForTFMessages = EditorGUILayout.Toggle("Listen for TF Messages", prefab.listenForTFMessages); + // TODO: make the settings input update the prefab // EditorGUILayout.Space(); // if (!editor) { editor = UnityEditor.Editor.CreateEditor(this); } From 74fc85a7bd3cbed86c9643cea02e0d4301ee540d Mon Sep 17 00:00:00 2001 From: Hamid Younesy Date: Tue, 1 Feb 2022 14:04:51 -0800 Subject: [PATCH 33/33] AIRO-1696 v0.7.0 pre release fixes (#257) * Making the package version consistent with the ROS-TCP-Connector release tag to avoid confusion. * fix filename for meta file (inconsistent casing) * updated change log and package version 0.7.0 --- .../CHANGELOG.md | 22 +++++++---- .../{HUDPanel.cs.meta => HudPanel.cs.meta} | 0 .../package.json | 6 +-- .../CHANGELOG.md | 38 +++++++++++++++++++ .../CHANGELOG.md.meta | 7 ++++ .../package.json | 7 ++-- 6 files changed, 65 insertions(+), 15 deletions(-) rename com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/{HUDPanel.cs.meta => HudPanel.cs.meta} (100%) create mode 100644 com.unity.robotics.visualizations/CHANGELOG.md create mode 100644 com.unity.robotics.visualizations/CHANGELOG.md.meta diff --git a/com.unity.robotics.ros-tcp-connector/CHANGELOG.md b/com.unity.robotics.ros-tcp-connector/CHANGELOG.md index ff4d1c0a..f883a80f 100644 --- a/com.unity.robotics.ros-tcp-connector/CHANGELOG.md +++ b/com.unity.robotics.ros-tcp-connector/CHANGELOG.md @@ -4,6 +4,7 @@ All notable changes to this repository will be documented in this file. The format is based on [Keep a Changelog](http://keepachangelog.com/en/1.0.0/) and this project adheres to [Semantic Versioning](http://semver.org/spec/v2.0.0.html). + ## Unreleased ### Upgrade Notes @@ -12,6 +13,19 @@ The format is based on [Keep a Changelog](http://keepachangelog.com/en/1.0.0/) a ### Added +### Changed + +### Deprecated + +### Removed + +### Fixed + + +## [0.7.0-preview] - 2022-02-01 + +### Added + Added "Listen for TF Messages" to Settings Enabled Android and IOS builds @@ -36,14 +50,6 @@ Bug fix - no padding when serializing an empty array Can publish messages from threads other than main -Visualizations work with ROS services - -### Deprecated - -### Removed - -### Fixed - ## [0.6.0-preview] - 2021-09-30 diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/HUDPanel.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/HudPanel.cs.meta similarity index 100% rename from com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/HUDPanel.cs.meta rename to com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/HudPanel.cs.meta diff --git a/com.unity.robotics.ros-tcp-connector/package.json b/com.unity.robotics.ros-tcp-connector/package.json index 573c73f5..1aa4cc23 100644 --- a/com.unity.robotics.ros-tcp-connector/package.json +++ b/com.unity.robotics.ros-tcp-connector/package.json @@ -1,11 +1,9 @@ { "name": "com.unity.robotics.ros-tcp-connector", - "version": "0.6.0-preview", + "version": "0.7.0-preview", "displayName": "ROS TCP Connector", "description": "Bridge components and message generation allowing Unity to communicate with ROS and ROS2 services", "unity": "2020.2", "unityRelease": "0b9", - "dependencies": { - "nuget.moq": "1.0.0" - } + "dependencies": {} } diff --git a/com.unity.robotics.visualizations/CHANGELOG.md b/com.unity.robotics.visualizations/CHANGELOG.md new file mode 100644 index 00000000..32d71496 --- /dev/null +++ b/com.unity.robotics.visualizations/CHANGELOG.md @@ -0,0 +1,38 @@ +# Changelog + +All notable changes to this repository will be documented in this file. + +The format is based on [Keep a Changelog](http://keepachangelog.com/en/1.0.0/) and this project adheres to [Semantic Versioning](http://semver.org/spec/v2.0.0.html). + + +## Unreleased + +### Upgrade Notes + +### Known Issues + +### Added + +### Changed + +### Deprecated + +### Removed + +### Fixed + + +## [0.7.0-preview] - 2022-02-01 + +### Upgrade Notes + +Making the package version consistent with the ROS-TCP-Connector release tag to avoid confusion. + +### Added + +Visualizations work with ROS services + + +## [0.1.0-preview] - 2021-09-30 + +Visualization package 0.1.0 preview is public. diff --git a/com.unity.robotics.visualizations/CHANGELOG.md.meta b/com.unity.robotics.visualizations/CHANGELOG.md.meta new file mode 100644 index 00000000..1150a8c1 --- /dev/null +++ b/com.unity.robotics.visualizations/CHANGELOG.md.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 811646b6132b81ad59be8225a980c342 +TextScriptImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/package.json b/com.unity.robotics.visualizations/package.json index 6321ef6a..c3418f0b 100644 --- a/com.unity.robotics.visualizations/package.json +++ b/com.unity.robotics.visualizations/package.json @@ -1,10 +1,11 @@ { "name": "com.unity.robotics.visualizations", - "version": "0.1.0-preview", + "version": "0.7.0-preview", "displayName": "Unity Robotics Visualizations", "description": "Components for displaying specific ROS messages", "unity": "2020.2", "unityRelease": "0b9", - "dependencies": { - } + "dependencies": + { + } }