diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 4b89a9ba..88fd5c36 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -52,7 +52,8 @@ We run continuous integration on all PRs; all tests must be passing before the P All Python code should follow the [PEP 8 style guidelines](https://pep8.org/). -All C# code should follow the [Microsoft C# Coding Conventions](https://docs.microsoft.com/en-us/dotnet/csharp/programming-guide/inside-a-program/coding-conventions). (Code may be reformatted to Unity coding standards where applicable.) +All C# code should follow the [Microsoft C# Coding Conventions](https://docs.microsoft.com/en-us/dotnet/csharp/programming-guide/inside-a-program/coding-conventions). +(Code may be reformatted to Unity coding standards where applicable.) Please note that even if the code you are changing does not follow these conventions, we expect your submissions to do so. diff --git a/README.md b/README.md index a770f8a8..5077362f 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# ROS-TCP-Connector +# ROS TCP Connector [![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) @@ -7,24 +7,37 @@ ![ROS](https://img.shields.io/badge/ros2-foxy-brightgreen) ![Unity](https://img.shields.io/badge/unity-2020.2+-brightgreen) +## 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. + ## Installation -1. Using Unity 2020.2 or later, open the package manager from `Window` -> `Package Manager` and select "Add package from git URL..." -![image](https://user-images.githubusercontent.com/29758400/110989310-8ea36180-8326-11eb-8318-f67ee200a23d.png) -2. Enter the following URL. If you don't want to use the latest version, substitute your desired version tag where we've put `v0.5.0` in this example: -`https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connector#v0.5.0` -3. Click `Add`. +1. Using Unity 2020.2 or later, open the Package Manager from `Window` -> `Package Manager`. +2. In the Package Manager window, find and click the + button in the upper lefthand corner of the window. Select `Add package from git URL....` + + ![image](https://user-images.githubusercontent.com/29758400/110989310-8ea36180-8326-11eb-8318-f67ee200a23d.png) +3. Enter the git URL for the desired package. 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. + 1. For the ROS-TCP-Connector, enter `https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connector`. + 2. For Visualizations, enter `https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.visualizations`. +4. Click `Add`. + +To install from a local clone of the repository, see [installing a local package](https://docs.unity3d.com/Manual/upm-ui-local.html) in the Unity manual. ## Tutorials -Scripts used to send [ROS](https://www.ros.org/) messages to an [TCP endpoint](https://github.com/Unity-Technologies/ROS_TCP_Endpoint) running as a ROS node. + + +This Unity package provides four main features: -This Unity package provides three main features: +- ROSConnection: A component that sets up communication between ROS and Unity. See the [Unity Robotics Hub](https://github.com/Unity-Technologies/Unity-Robotics-Hub/blob/main/tutorials/ros_unity_integration/README.md) for information and tutorials. -- ROSConnection: See the [Unity Robotics Hub](https://github.com/Unity-Technologies/Unity-Robotics-Hub/blob/main/tutorials/ros_unity_integration/README.md) repository for information and tutorials on how to use this component. +- [Message Generation](MessageGeneration.md): A tool to generate C# classes for ROS message types. -- [Message Generation](MessageGeneration.md) +- Visualizations: A suite of default configurations and APIs to visualize incoming and outgoing information from ROS. + - See the [Nav2 SLAM Example](https://github.com/Unity-Technologies/Robotics-Nav2-SLAM-Example) for tutorials on using the Visualizations Package! + - You can also view the package's [Usage Information](com.unity.robotics.visualizations/Documentation~/README.md) for more details on using the package in your own project. -- [ROSGeometry](ROSGeometry.md) +- [ROSGeometry](ROSGeometry.md): A set of extensions that convert geometries between Unity and other coordinate frames. ## ROS# diff --git a/ROSGeometry.md b/ROSGeometry.md index cfbfd779..768667be 100644 --- a/ROSGeometry.md +++ b/ROSGeometry.md @@ -43,7 +43,7 @@ Unity's standard Transform class also has a `To()` extension method that retu # Internal details -Some more detail about what's going on here: The core of the ROSGeometry package is the two generic structs, `Vector3` and `Quaternion`. The type parameter C here indicates the coordinate frame you're working in - either FLU, or RUF, or perhaps one of the more exotic frames such as NED (north, east, down) or ENU (east, north, up), used in aviation. In conversions between RUF and geographical coordinate systems, such as NED and ENU, the east direction is equivalent to the z-axis (forward) in RUF. +Some more detail about what's going on here: The core of the ROSGeometry package is the two generic structs, `Vector3` and `Quaternion`. The type parameter C here indicates the coordinate frame you're working in - either FLU, or RUF, or perhaps one of the more exotic frames such as NED (north, east, down) or ENU (east, north, up), used in aviation. These are fully-fledged Vector3 and Quaternion classes, so if you want, you can work with them directly to perform geometric operations in an arbitrary coordinate space. (Note, it's a compile time error to add a Vector3 to a Vector3.) diff --git a/com.unity.robotics.ros-tcp-connector/CHANGELOG.md b/com.unity.robotics.ros-tcp-connector/CHANGELOG.md index 3e75c872..b39cb4e3 100644 --- a/com.unity.robotics.ros-tcp-connector/CHANGELOG.md +++ b/com.unity.robotics.ros-tcp-connector/CHANGELOG.md @@ -24,6 +24,7 @@ Upgrade the TestRosTcpConnector project to use Unity LTS version 2020.3.11f1 - Added an optional pooling system for ros publishers - Implemented a queueing and latching system to mimic the ROS implementation in Unity +- Add support for visualizations ### Changed - Publishing a message to an unregistered topic will show an error. @@ -58,8 +59,6 @@ Add badges to main README ### Changed -Update the transformation of coordinate spaces using Unity's coordinate as right, up, forward (RUF) and south, up, east (SUE). - ### Deprecated ### Removed 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 4c03ccd0..065bc6b9 100644 --- a/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/ActionAutoGen.cs +++ b/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/ActionAutoGen.cs @@ -21,6 +21,7 @@ namespace Unity.Robotics.ROSTCPConnector.MessageGeneration public class ActionAutoGen { private static readonly string[] types = { "Goal", "Result", "Feedback" }; + private static readonly MessageSubtopic[] subtopics = { MessageSubtopic.Goal, MessageSubtopic.Result, MessageSubtopic.Feedback }; public static string[] GetActionClassPaths(string inFilePath, string outPath) { @@ -76,7 +77,16 @@ public static List GenerateSingleAction(string inPath, string outPath, s string className = inFileName + types[i] + MsgAutoGenUtilities.ActionClassSuffix; // Parse and generate goal, result, feedback messages - MessageParser parser = new MessageParser(tokens, outPath, rosPackageName, "action", MsgAutoGenUtilities.builtInTypesMapping, MsgAutoGenUtilities.builtInTypesDefaultInitialValues, className); + MessageParser parser = new MessageParser( + tokens, + outPath, + rosPackageName, + "action", + MsgAutoGenUtilities.builtInTypesMapping, + MsgAutoGenUtilities.builtInTypesDefaultInitialValues, + className, + subtopic: subtopics[i] + ); parser.Parse(); warnings.AddRange(parser.GetWarnings()); 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 c36d8806..0a472359 100644 --- a/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/MessageParser.cs +++ b/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/MessageParser.cs @@ -48,10 +48,11 @@ public class MessageParser private HashSet defaultValues = new HashSet(); private string body = ""; + MessageSubtopic subtopic; private List warnings = new List(); - public MessageParser(List tokens, string outPath, string rosPackageName, string type, Dictionary builtInTypeMapping, Dictionary builtInTypesDefaultInitialValues, string className = "", string rosMsgName = "") + public MessageParser(List tokens, string outPath, string rosPackageName, string type, Dictionary builtInTypeMapping, Dictionary builtInTypesDefaultInitialValues, string className = "", string rosMsgName = "", MessageSubtopic subtopic = MessageSubtopic.Default) { this.tokens = tokens; @@ -61,6 +62,8 @@ public MessageParser(List tokens, string outPath, string rosPackag this.rosPackageName = rosPackageName; this.rosPackageNamespace = MsgAutoGenUtilities.ResolvePackageName(rosPackageName); + this.subtopic = subtopic; + if (className.Equals("")) { this.className = MsgAutoGenUtilities.CapitalizeFirstLetter(inFileName) + MsgAutoGenUtilities.MessageClassSuffix; @@ -151,6 +154,8 @@ public void Parse() // Write ToString override writer.Write(GenerateToString()); + string subtopicParameter = subtopic == MessageSubtopic.Default ? "" : $", MessageSubtopic.{subtopic}"; + writer.Write( "\n" + "#if UNITY_EDITOR\n" + @@ -160,7 +165,7 @@ public void Parse() "#endif\n" + TWO_TABS + "public static void Register()\n" + TWO_TABS + "{\n" + - THREE_TABS + "MessageRegistry.Register(k_RosMessageName, Deserialize);\n" + + THREE_TABS + $"MessageRegistry.Register(k_RosMessageName, Deserialize{subtopicParameter});\n" + TWO_TABS + "}\n" ); 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 1470c067..569cd751 100644 --- a/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/ServiceAutoGen.cs +++ b/com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/ServiceAutoGen.cs @@ -72,7 +72,15 @@ public static List GenerateSingleService(string inPath, string outPath, // Service is made up of request and response string className = inFileName + MsgAutoGenUtilities.ServiceClassSuffix + types[i]; - MessageParser parser = new MessageParser(tokens, outPath, rosPackageName, "srv", MsgAutoGenUtilities.builtInTypesMapping, MsgAutoGenUtilities.builtInTypesDefaultInitialValues, className); + MessageParser parser = new MessageParser( + tokens, + outPath, + rosPackageName, + "srv", + MsgAutoGenUtilities.builtInTypesMapping, + MsgAutoGenUtilities.builtInTypesDefaultInitialValues, + className, + subtopic: i == 0 ? MessageSubtopic.Default : MessageSubtopic.Response); parser.Parse(); warnings.AddRange(parser.GetWarnings()); } diff --git a/com.unity.robotics.ros-tcp-connector/Editor/ROSSettingsEditor.cs b/com.unity.robotics.ros-tcp-connector/Editor/ROSSettingsEditor.cs index 53eee4d8..83f6ebff 100644 --- a/com.unity.robotics.ros-tcp-connector/Editor/ROSSettingsEditor.cs +++ b/com.unity.robotics.ros-tcp-connector/Editor/ROSSettingsEditor.cs @@ -14,12 +14,12 @@ public static void OpenWindow() { ROSSettingsEditor window = GetWindow(false, "ROS Settings", true); window.minSize = new Vector2(300, 65); - window.maxSize = new Vector2(600, 250); + window.maxSize = new Vector2(600, 500); window.Show(); } GameObject prefabObj; - ROSConnection prefab; + Unity.Robotics.ROSTCPConnector.ROSConnection prefab; public enum RosProtocol { @@ -35,6 +35,9 @@ public enum RosProtocol const RosProtocol k_AlternateProtocol = RosProtocol.ROS2; #endif + [SerializeField] string[] m_TFTopics; + UnityEditor.Editor editor; + protected virtual void OnGUI() { if (prefab == null) @@ -49,8 +52,7 @@ protected virtual void OnGUI() sceneObj.AddComponent(); if (!Directory.Exists("Assets/Resources")) Directory.CreateDirectory("Assets/Resources"); - prefabObj = PrefabUtility.SaveAsPrefabAsset(sceneObj, - "Assets/Resources/ROSConnectionPrefab.prefab"); + prefabObj = PrefabUtility.SaveAsPrefabAsset(sceneObj, "Assets/Resources/ROSConnectionPrefab.prefab"); if (prefabObj != null) prefab = prefabObj.GetComponent(); DestroyImmediate(sceneObj); @@ -96,8 +98,8 @@ protected virtual void OnGUI() prefab.RosPort = EditorGUILayout.IntField("ROS Port", prefab.RosPort); // Also set the player prefs, for users who hit play in the editor: they will expect the last-used IP address to appear in the hud - HUDPanel.SetIPPref(prefab.RosIPAddress); - HUDPanel.SetPortPref(prefab.RosPort); + ROSConnection.SetIPPref(prefab.RosIPAddress); + ROSConnection.SetPortPref(prefab.RosPort); EditorGUILayout.Space(); @@ -127,10 +129,27 @@ 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); + // TODO: make the settings input update the prefab + // EditorGUILayout.Space(); + // if (!editor) { editor = UnityEditor.Editor.CreateEditor(this); } + // if (editor) { editor.OnInspectorGUI(); } + if (GUI.changed) { PrefabUtility.SavePrefabAsset(prefab.gameObject); } } + + void OnInspectorUpdate() { Repaint(); } + } + + [CustomEditor(typeof(ROSSettingsEditor), true)] + public class TFTopicsEditorDrawer : UnityEditor.Editor + { + public override void OnInspectorGUI() + { + var list = serializedObject.FindProperty("m_TFTopics"); + EditorGUILayout.PropertyField(list, new GUIContent("TF Topics"), true); + } } } 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 4ea1262b..93e8276c 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 @@ -1,5 +1,6 @@ { "name": "Unity.Robotics.ROSTCPConnector.Editor", + "rootNamespace": "", "references": [ "GUID:625bfc588fb96c74696858f2c467e978" ], diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MatrixExtensions.cs b/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MatrixExtensions.cs new file mode 100644 index 00000000..a55a4e25 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MatrixExtensions.cs @@ -0,0 +1,200 @@ +using RosMessageTypes.Sensor; +using RosMessageTypes.Std; +using RosMessageTypes.Geometry; +using System; +using System.Collections; +using System.Collections.Generic; +using UnityEngine; + +namespace Unity.Robotics.ROSTCPConnector.MessageGeneration +{ + // Source: https://jamesmccaffrey.wordpress.com/2015/03/06/inverting-a-matrix-using-c/ + public static class MatrixExtensions + { + public static double[][] MatrixCreate(int rows, int cols) + { + double[][] result = new double[rows][]; + for (int i = 0; i < rows; ++i) + result[i] = new double[cols]; + return result; + } + + public static double[][] MatrixDecompose(double[][] matrix, out int[] perm, out int toggle) + { + // Doolittle LUP decomposition with partial pivoting. + // rerturns: result is L (with 1s on diagonal) and U; + // perm holds row permutations; toggle is +1 or -1 (even or odd) + int rows = matrix.Length; + int cols = matrix[0].Length; // assume square + if (rows != cols) + throw new Exception("Attempt to decompose a non-square m"); + + int n = rows; // convenience + + double[][] result = MatrixDuplicate(matrix); + + perm = new int[n]; // set up row permutation result + for (int i = 0; i < n; ++i) { perm[i] = i; } + + toggle = 1; // toggle tracks row swaps. + // +1 -> even, -1 -> odd. used by MatrixDeterminant + + for (int j = 0; j < n - 1; ++j) // each column + { + double colMax = Math.Abs(result[j][j]); // find largest val in col + int pRow = j; + //for (int i = j + 1; i < n; ++i) + //{ + // if (result[i][j] > colMax) + // { + // colMax = result[i][j]; + // pRow = i; + // } + //} + + // reader Matt V needed this: + for (int i = j + 1; i < n; ++i) + { + if (Math.Abs(result[i][j]) > colMax) + { + colMax = Math.Abs(result[i][j]); + pRow = i; + } + } + // Not sure if this approach is needed always, or not. + + if (pRow != j) // if largest value not on pivot, swap rows + { + double[] rowPtr = result[pRow]; + result[pRow] = result[j]; + result[j] = rowPtr; + + int tmp = perm[pRow]; // and swap perm info + perm[pRow] = perm[j]; + perm[j] = tmp; + + toggle = -toggle; // adjust the row-swap toggle + } + + // -------------------------------------------------- + // This part added later (not in original) + // and replaces the 'return null' below. + // if there is a 0 on the diagonal, find a good row + // from i = j+1 down that doesn't have + // a 0 in column j, and swap that good row with row j + // -------------------------------------------------- + + if (result[j][j] == 0.0) + { + // find a good row to swap + int goodRow = -1; + for (int row = j + 1; row < n; ++row) + { + if (result[row][j] != 0.0) + goodRow = row; + } + + if (goodRow == -1) + throw new Exception("Cannot use Doolittle's method"); + + // swap rows so 0.0 no longer on diagonal + double[] rowPtr = result[goodRow]; + result[goodRow] = result[j]; + result[j] = rowPtr; + + int tmp = perm[goodRow]; // and swap perm info + perm[goodRow] = perm[j]; + perm[j] = tmp; + + toggle = -toggle; // adjust the row-swap toggle + } + // -------------------------------------------------- + // if diagonal after swap is zero . . + //if (Math.Abs(result[j][j]) < 1.0E-20) + // return null; // consider a throw + + for (int i = j + 1; i < n; ++i) + { + result[i][j] /= result[j][j]; + for (int k = j + 1; k < n; ++k) + { + result[i][k] -= result[i][j] * result[j][k]; + } + } + + + } // main j column loop + + return result; + } + + public static double[][] MatrixDuplicate(double[][] matrix) + { + // allocates/creates a duplicate of a matrix. + double[][] result = MatrixCreate(matrix.Length, matrix[0].Length); + for (int i = 0; i < matrix.Length; ++i) // copy the values + for (int j = 0; j < matrix[i].Length; ++j) + result[i][j] = matrix[i][j]; + return result; + } + + public static double[][] MatrixInverse(double[][] matrix) + { + int n = matrix.Length; + double[][] result = MatrixDuplicate(matrix); + + int[] perm; + int toggle; + double[][] lum = MatrixDecompose(matrix, out perm, + out toggle); + if (lum == null) + throw new Exception("Unable to compute inverse"); + + double[] b = new double[n]; + for (int i = 0; i < n; ++i) + { + for (int j = 0; j < n; ++j) + { + if (i == perm[j]) + b[j] = 1.0; + else + b[j] = 0.0; + } + + double[] x = HelperSolve(lum, b); // + + for (int j = 0; j < n; ++j) + result[j][i] = x[j]; + } + return result; + } + + public static double[] HelperSolve(double[][] luMatrix, double[] b) + { + // before calling this helper, permute b using the perm array + // from MatrixDecompose that generated luMatrix + int n = luMatrix.Length; + double[] x = new double[n]; + b.CopyTo(x, 0); + + for (int i = 1; i < n; ++i) + { + double sum = x[i]; + for (int j = 0; j < i; ++j) + sum -= luMatrix[i][j] * x[j]; + x[i] = sum; + } + + x[n - 1] /= luMatrix[n - 1][n - 1]; + for (int i = n - 2; i >= 0; --i) + { + double sum = x[i]; + for (int j = i + 1; j < n; ++j) + sum -= luMatrix[i][j] * x[j]; + x[i] = sum / luMatrix[i][i]; + } + + return x; + } + } +} diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MatrixExtensions.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MatrixExtensions.cs.meta new file mode 100644 index 00000000..c6c996ea --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MatrixExtensions.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 59e6ede11dca14588816d491dbf9d848 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: 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 bf6c316d..bb38d8ce 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageDeserializer.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageDeserializer.cs @@ -13,6 +13,12 @@ public class MessageDeserializer int alignmentCorrection; #endif + public Message DeserializeMessage(string rosMessageName, byte[] data, MessageSubtopic subtopic = MessageSubtopic.Default) + { + InitWithBuffer(data); + return MessageRegistry.GetDeserializeFunction(rosMessageName, subtopic)(this); + } + public T DeserializeMessage(byte[] data) where T : Message { InitWithBuffer(data); diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageExtensions.cs b/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageExtensions.cs new file mode 100644 index 00000000..0f2bcb11 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageExtensions.cs @@ -0,0 +1,757 @@ +using RosMessageTypes.Sensor; +using RosMessageTypes.Std; +using RosMessageTypes.Geometry; +using System; +using System.Collections; +using System.Collections.Generic; +using RosMessageTypes.BuiltinInterfaces; +using UnityEngine; + +namespace Unity.Robotics.ROSTCPConnector.MessageGeneration +{ + public enum BatteryState_Status_Constants + { + UNKNOWN = 0, + CHARGING = 1, + DISCHARGING = 2, + NOT_CHARGING = 3, + FULL = 4 + }; + + public enum BatteryState_Health_Constants + { + UNKNOWN = 0, + GOOD = 1, + OVERHEAT = 2, + DEAD = 3, + OVERVOLTAGE = 4, + UNSPEC_FAILURE = 5, + COLD = 6, + WATCHDOG_TIMER_EXPIRE = 7, + SAFETY_TIMER_EXPIRE = 8 + }; + + public enum BatteryState_Technology_Constants + { + UNKNOWN = 0, + NIMH = 1, + LION = 2, + LIPO = 3, + LIFE = 4, + NICD = 5, + LIMN = 6 + }; + + public enum JoyFeedback_Type_Constants + { + LED = 0, + RUMBLE = 1, + BUZZER = 2, + }; + + public enum JoyRegion + { + BSouth = 0, + BEast = 1, + BWest = 2, + BNorth = 3, + LB = 4, + RB = 5, + Back = 6, + Start = 7, + Power = 8, + LPress = 9, + RPress = 10, + LStick, RStick, LT, RT, DPad, lAxisX, lAxisY, + rAxisX, rAxisY, ltAxis, rtAxis, dAxisX, dAxisY + }; + + public enum NavSatStatus_Type_Constants + { + NO_FIX = -1, // unable to fix position + FIX = 0, // unaugmented fix + SBAS_FIX = 1, // with satellite-based augmentation + GBAS_FIX = 2 // with ground-based augmentation + }; + + public enum NavSatStatus_Service_Constants + { + GPS = 1, + GLONASS = 2, + COMPASS = 4, // includes BeiDou. + GALILEO = 8 + }; + + public enum NavSatFix_Covariance_Constants + { + UNKNOWN = 0, + APPROXIMATED = 1, + DIAGONAL_KNOWN = 2, + KNOWN = 3 + }; + + public enum Range_RadiationType_Constants + { + ULTRASOUND = 0, + INFRARED = 1 + }; + + public enum PointField_Format_Constants + { + INT8 = 1, + UINT8 = 2, + INT16 = 3, + UINT16 = 4, + INT32 = 5, + UINT32 = 6, + FLOAT32 = 7, + FLOAT64 = 8 + } + + // Convenience functions for built-in message types + public static class MessageExtensions + { + public static string ToTimestampString(this TimeMsg message) + { + // G: format using short date and long time + return message.ToDateTime().ToString("G") + $"(+{message.nanosec})"; + } + + public static long ToLongTime(this TimeMsg message) + { + return (long)message.sec << 32 | message.nanosec; + } + + public static DateTime ToDateTime(this TimeMsg message) + { + DateTime time = new DateTime(message.sec); + time = time.AddMilliseconds(message.nanosec / 1E6); + return time; + } + + // public static TimeMsg TimeMsg(this DateTime dateTime) + // { + // return new TimeMsg { sec = Convert.ToUInt32(dateTime.Ticks), nanosec = Convert.ToUInt32(dateTime.Millisecond * 1E6) }; + // } + + public static Color ToUnityColor(this ColorRGBAMsg message) + { + return new Color(message.r, message.g, message.b, message.a); + } + + public static ColorRGBAMsg ColorRGBAMsg(this Color color) + { + return new ColorRGBAMsg(color.r, color.g, color.b, color.a); + } + + /// + /// Converts a byte array from BGR to RGB. + /// + static byte[] EncodingConversion(ImageMsg image, bool convertBGR = true, bool flipY = true) + { + // Number of channels in this encoding + int channels = image.GetNumChannels(); + + if (!image.EncodingRequiresBGRConversion()) + convertBGR = false; + + // If no modifications are necessary, return original array + if (!convertBGR && !flipY) + return image.data; + + int channelStride = image.GetBytesPerChannel(); + int pixelStride = channelStride * channels; + int rowStride = pixelStride * (int)image.width; + + if (flipY) + { + ReverseInBlocks(image.data, rowStride, (int)image.height); + } + + if (convertBGR) + { + // given two channels, we swap R with G (distance = 1). + // given three or more channels, we swap R with B (distance = 2). + int swapDistance = channels == 2 ? channelStride : channelStride * 2; + int dataLength = (int)image.width * (int)image.height * pixelStride; + + if (channelStride == 1) + { + // special case for the 1-byte-per-channel formats: avoid the inner loop + for (int pixelIndex = 0; pixelIndex < dataLength; pixelIndex += pixelStride) + { + int swapB = pixelIndex + swapDistance; + byte temp = image.data[pixelIndex]; + image.data[pixelIndex] = image.data[swapB]; + image.data[swapB] = temp; + } + } + else + { + for (int pixelIndex = 0; pixelIndex < dataLength; pixelIndex += pixelStride) + { + int channelEndByte = pixelIndex + channelStride; + for (int byteIndex = pixelIndex; byteIndex < channelEndByte; byteIndex++) + { + int swapB = byteIndex + swapDistance; + byte temp = image.data[byteIndex]; + image.data[byteIndex] = image.data[swapB]; + image.data[swapB] = temp; + } + } + } + } + return image.data; + } + + static byte[] s_ScratchSpace; + + static void ReverseInBlocks(byte[] array, int blockSize, int numBlocks) + { + if (blockSize * numBlocks > array.Length) + { + Debug.LogError($"Invalid ReverseInBlocks, array length is {array.Length}, should be at least {blockSize * numBlocks}"); + return; + } + + if (s_ScratchSpace == null || s_ScratchSpace.Length < blockSize) + s_ScratchSpace = new byte[blockSize]; + + int startBlockIndex = 0; + int endBlockIndex = ((int)numBlocks - 1) * blockSize; + + while (startBlockIndex < endBlockIndex) + { + Buffer.BlockCopy(array, startBlockIndex, s_ScratchSpace, 0, blockSize); + Buffer.BlockCopy(array, endBlockIndex, array, startBlockIndex, blockSize); + Buffer.BlockCopy(s_ScratchSpace, 0, array, endBlockIndex, blockSize); + startBlockIndex += blockSize; + endBlockIndex -= blockSize; + } + } + + static void ReverseInBlocks(T1[] fromArray, T2[] toArray, int blockSize, int numBlocks) + { + int startBlockIndex = 0; + int endBlockIndex = ((int)numBlocks - 1) * blockSize; + + while (startBlockIndex < endBlockIndex) + { + Buffer.BlockCopy(fromArray, startBlockIndex, toArray, endBlockIndex, blockSize); + Buffer.BlockCopy(fromArray, endBlockIndex, toArray, startBlockIndex, blockSize); + startBlockIndex += blockSize; + endBlockIndex -= blockSize; + } + } + + public static void DebayerConvert(this ImageMsg image, bool flipY = true) + { + int channelStride = image.GetBytesPerChannel(); + int width = (int)image.width; + int height = (int)image.height; + int rowStride = width * channelStride; + int dataSize = rowStride * height; + int finalPixelStride = channelStride * 4; + + int[] reorderIndices; + switch (image.encoding) + { + case "bayer_rggb8": + reorderIndices = new int[] { 0, 1, width + 1 }; + break; + case "bayer_bggr8": + reorderIndices = new int[] { width + 1, 1, 0 }; + break; + case "bayer_gbrg8": + reorderIndices = new int[] { width, 0, 1 }; + break; + case "bayer_grbg8": + reorderIndices = new int[] { 1, 0, width }; + break; + case "bayer_rggb16": + reorderIndices = new int[] { 0, 1, 2, 3, rowStride + 2, rowStride + 3 }; + break; + case "bayer_bggr16": + reorderIndices = new int[] { rowStride + 2, rowStride + 3, 2, 3, 0, 1 }; + break; + case "bayer_gbrg16": + reorderIndices = new int[] { rowStride, rowStride + 1, 0, 1, 2, 3 }; + break; + case "bayer_grbg16": + reorderIndices = new int[] { 2, 3, 0, 1, rowStride, rowStride + 1 }; + break; + default: + return; + } + + if (flipY) + { + ReverseInBlocks(image.data, rowStride * 2, (int)image.height / 2); + } + + if (s_ScratchSpace == null || s_ScratchSpace.Length < rowStride * 2) + s_ScratchSpace = new byte[rowStride * 2]; + + int rowStartIndex = 0; + while (rowStartIndex < dataSize) + { + Buffer.BlockCopy(image.data, rowStartIndex, s_ScratchSpace, 0, rowStride * 2); + int pixelReadIndex = 0; + int pixelWriteIndex = rowStartIndex; + while (pixelReadIndex < rowStride) + { + for (int Idx = 0; Idx < reorderIndices.Length; ++Idx) + { + image.data[pixelWriteIndex + Idx] = s_ScratchSpace[pixelReadIndex + reorderIndices[Idx]]; + } + image.data[pixelWriteIndex + reorderIndices.Length] = 255; + if (channelStride == 2) + image.data[pixelWriteIndex + reorderIndices.Length + 1] = 255; + pixelReadIndex += channelStride * 2; + pixelWriteIndex += finalPixelStride; + } + rowStartIndex += rowStride * 2; + } + + image.width = image.width / 2; + image.height = image.height / 2; + image.encoding = channelStride == 1 ? "rgba8" : "rgba16"; + image.step = (uint)(channelStride * image.width); + } + + public static int GetNumChannels(this ImageMsg image) + { + switch (image.encoding) + { + case "8SC1": + case "8UC1": + case "16SC1": + case "16UC1": + case "32FC1": + case "32SC1": + case "64FC1": + case "mono8": + case "mono16": + case "bayer_rggb8": + case "bayer_bggr8": + case "bayer_gbrg8": + case "bayer_grbg8": + case "bayer_rggb16": + case "bayer_bggr16": + case "bayer_gbrg16": + case "bayer_grbg16": + return 1; + case "8SC2": + case "8UC2": + case "16SC2": + case "16UC2": + case "32FC2": + case "32SC2": + case "64FC2": + return 2; + case "8SC3": + case "8UC3": + case "16SC3": + case "16UC3": + case "32FC3": + case "32SC3": + case "64FC3": + case "bgr8": + case "rgb8": + return 3; + case "8SC4": + case "8UC4": + case "16SC4": + case "16UC4": + case "32FC4": + case "32SC4": + case "64FC4": + case "bgra8": + case "rgba8": + return 4; + } + return 4; + } + + public static bool IsBayerEncoded(this ImageMsg image) + { + switch (image.encoding) + { + case "bayer_rggb8": + case "bayer_bggr8": + case "bayer_gbrg8": + case "bayer_grbg8": + case "bayer_rggb16": + case "bayer_bggr16": + case "bayer_gbrg16": + case "bayer_grbg16": + return true; + default: + return false; + } + } + + public static bool EncodingRequiresBGRConversion(this ImageMsg image) + { + switch (image.encoding) + { + case "8SC1": + case "8UC1": + case "16SC1": + case "16UC1": + case "32FC1": + case "32SC1": + case "64FC1": + case "mono8": + case "mono16": + // single channel = nothing to swap + return false; + case "8UC4": + case "8SC4": + case "bgra8": + return false; // raw BGRA32 texture format + case "rgb8": + return false; // raw RGB24 texture format + case "rgba8": + return false; // raw RGB32 texture format + case "bayer_rggb8": + case "bayer_bggr8": + case "bayer_gbrg8": + case "bayer_grbg8": + return false; // bayer has its own conversions needed + default: + return true; + } + } + + public static int GetBytesPerChannel(this ImageMsg image) + { + switch (image.encoding) + { + case "8SC1": + case "8SC2": + case "8SC3": + case "8SC4": + case "8UC1": + case "8UC2": + case "8UC3": + case "8UC4": + case "mono8": + case "bgr8": + case "rgb8": + case "bgra8": + case "rgba8": + case "bayer_rggb8": + case "bayer_bggr8": + case "bayer_gbrg8": + case "bayer_grbg8": + return 1; + case "16SC1": + case "16SC2": + case "16SC3": + case "16SC4": + case "16UC1": + case "16UC2": + case "16UC3": + case "16UC4": + case "mono16": + case "bayer_rggb16": + case "bayer_bggr16": + case "bayer_gbrg16": + case "bayer_grbg16": + return 2; + case "32FC1": + case "32SC1": + case "32FC2": + case "32SC2": + case "32FC3": + case "32SC3": + case "32FC4": + case "32SC4": + return 4; + case "64FC1": + case "64FC2": + case "64FC3": + case "64FC4": + return 8; + } + return 1; + } + + public static TextureFormat GetTextureFormat(this ImageMsg image) + { + switch (image.encoding) + { + case "8UC1": + case "8SC1": + return TextureFormat.R8; + case "8UC2": + case "8SC2": + return TextureFormat.RG16; + case "8UC3": + case "8SC3": + return TextureFormat.RGB24; + case "8UC4": + case "8SC4": + case "bgra8": + return TextureFormat.BGRA32; // unity supports these natively + case "16UC1": + case "16SC1": + return TextureFormat.R16; + case "16UC2": + case "16SC2": + return TextureFormat.RG32; + case "16UC3": + case "16SC3": + return TextureFormat.RGB48; + case "16UC4": + case "16SC4": + return TextureFormat.RGBA64; + case "32SC1": + case "32SC2": + case "32SC3": + case "32SC4": + throw new NotImplementedException("32 bit integer texture formats are not supported"); + case "32FC1": + return TextureFormat.RFloat; + case "32FC2": + return TextureFormat.RGFloat; + case "32FC3": + throw new NotImplementedException("32FC3 texture format is not supported"); + case "32FC4": + return TextureFormat.RGBAFloat; + case "64FC1": + case "64FC2": + case "64FC3": + case "64FC4": + throw new NotImplementedException("Double precision texture formats are not supported"); + case "mono8": + return TextureFormat.R8; + case "mono16": + return TextureFormat.R16; + case "bgr8": + return TextureFormat.RGB24; + case "rgb8": + return TextureFormat.RGB24; // unity supports this natively + case "rgba8": + return TextureFormat.RGBA32; // unity supports this natively + case "bayer_rggb8": + case "bayer_bggr8": + case "bayer_gbrg8": + case "bayer_grbg8": + return TextureFormat.R8; + case "bayer_rggb16": + case "bayer_bggr16": + case "bayer_gbrg16": + case "bayer_grbg16": + return TextureFormat.R16; + } + return TextureFormat.RGB24; + } + + public static Texture2D ToTexture2D(this CompressedImageMsg message) + { + var tex = new Texture2D(1, 1); + tex.LoadImage(message.data); + return tex; + } + + public static Texture2D ToTexture2D(this ImageMsg message, bool debayer = false, bool convertBGR = true, bool flipY = true) + { + Texture2D tex; + byte[] data; + if (debayer && message.IsBayerEncoded()) + { + tex = new Texture2D((int)message.width / 2, (int)message.height / 2, TextureFormat.RGBA32, false); + message.DebayerConvert(flipY); + data = message.data; + } + else + { + tex = new Texture2D((int)message.width, (int)message.height, message.GetTextureFormat(), false); + data = EncodingConversion(message, convertBGR, flipY); + } + + tex.LoadRawTextureData(data); + tex.Apply(); + return tex; + } + + /// + /// Finds the world coordinates of an image coordinate based on the CameraInfo matrices + /// K matrix P matrix + /// 0 1 2 0 1 2 3 + /// 3 4 5 4 5 6 7 + /// 6 7 8 8 9 10 11 + /// + /// x coord to map + /// y coord to map + /// Projection matrix (3x4) + /// Camera intrinsics (3x3) + /// + static PointMsg FindPixelProjection(int x, int y, double[] P, double[][] invK) + { + PointMsg worldCoord = new PointMsg(x, y, 1); + + return new PointMsg((float)invK[0][0] * worldCoord.x + (float)invK[0][1] * worldCoord.y + (float)invK[0][2] * worldCoord.z, (float)invK[1][0] * worldCoord.x + (float)invK[1][1] * worldCoord.y + (float)invK[1][2] * worldCoord.z, (float)invK[2][0] * worldCoord.x + (float)invK[2][1] * worldCoord.y + (float)invK[2][2] * worldCoord.z); + } + + public static PointMsg[] GetPixelsInWorld(this CameraInfoMsg cameraInfo) + { + List res = new List(); + + double[][] formatK = new double[][] { + new double[] { cameraInfo.k[0], cameraInfo.k[1], cameraInfo.k[2] }, + new double[] { cameraInfo.k[3], cameraInfo.k[4], cameraInfo.k[5] }, + new double[] { cameraInfo.k[6], cameraInfo.k[7], cameraInfo.k[8] }, + }; + + var inverseK = MatrixExtensions.MatrixInverse(formatK); + + for (int i = 0; i < cameraInfo.width; i++) + { + for (int j = 0; j < cameraInfo.height; j++) + { + res.Add(FindPixelProjection(i, j, cameraInfo.p, inverseK)); + } + } + + return res.ToArray(); + } + + public static Texture2D ApplyCameraInfoProjection(this Texture2D tex, CameraInfoMsg cameraInfo) + { + var newTex = new Texture2D(tex.width, tex.height); + + double[][] formatK = new double[][] { + new double[] { cameraInfo.k[0], cameraInfo.k[1], cameraInfo.k[2] }, + new double[] { cameraInfo.k[3], cameraInfo.k[4], cameraInfo.k[5] }, + new double[] { cameraInfo.k[6], cameraInfo.k[7], cameraInfo.k[8] }, + }; + + var inverseK = MatrixExtensions.MatrixInverse(formatK); + for (int i = 0; i < tex.width; i++) + { + for (int j = 0; j < tex.height; j++) + { + var newPix = FindPixelProjection(i, j, cameraInfo.p, inverseK); + var color = tex.GetPixel((int)newPix.x, (int)newPix.y); + newTex.SetPixel(i, j, color); + } + } + newTex.Apply(); + + return newTex; + } + + public static CompressedImageMsg ToCompressedImageMsg_JPG(this Texture2D tex, HeaderMsg header) + { + return new CompressedImageMsg(header, "jpeg", tex.EncodeToJPG()); + } + + public static CompressedImageMsg ToCompressedImageMsg_PNG(this Texture2D tex, HeaderMsg header) + { + return new CompressedImageMsg(header, "png", tex.EncodeToPNG()); + } + + /// + /// Creates a new Texture2D that grabs a region of interest of a given texture, if applicable. Otherwise, an approximated empty texture with a highlighted region is returned. + /// + public static Texture2D RegionOfInterestTexture(this RegionOfInterestMsg message, Texture2D rawTex, int height = 0, int width = 0) + { + int mWidth = (int)message.width; + int mHeight = (int)message.height; + int x_off = ((int)message.x_offset == mWidth) ? 0 : (int)message.x_offset; + int y_off = ((int)message.y_offset == mHeight) ? 0 : (int)message.y_offset; + + Texture2D overlay; + if (rawTex == null) + { + // No texture provided, just return approximation + if (width == 0 || height == 0) + { + overlay = new Texture2D(x_off + mWidth + 10, y_off + mHeight + 10); + } + else + { + overlay = new Texture2D(width, height); + } + + // Initialize ROI color block + Color[] colors = new Color[mHeight * mWidth]; + for (int i = 0; i < colors.Length; i++) + { + colors[i] = Color.red; + } + + overlay.SetPixels(x_off, y_off, mWidth, mHeight, colors); + } + else + { + // Crop out ROI from input texture + overlay = new Texture2D(mWidth - x_off, mHeight - y_off, rawTex.format, true); + overlay.SetPixels(0, 0, overlay.width, overlay.height, rawTex.GetPixels(x_off, 0, mWidth - x_off, mHeight - y_off)); + } + overlay.Apply(); + return overlay; + } + + public static ImageMsg ToImageMsg(this Texture2D tex, HeaderMsg header) + { + byte[] data = null; + string encoding = "rgba8"; + switch (tex.format) + { + case TextureFormat.RGB24: + data = new byte[tex.width * tex.height * 3]; + tex.GetPixelData(0).CopyTo(data); + encoding = "rgb8"; + 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"; + 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"; + 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"; + ReverseInBlocks(data, tex.width * 2, tex.height); + break; + default: + Color32[] pixels = tex.GetPixels32(); + data = new byte[pixels.Length * 4]; + // although this is painfully slow, it does work... Surely there's a better way + int writeIdx = 0; + for (int Idx = 0; Idx < pixels.Length; ++Idx) + { + Color32 p = pixels[Idx]; + data[writeIdx] = p.r; + data[writeIdx + 1] = p.g; + data[writeIdx + 2] = p.b; + data[writeIdx + 3] = p.a; + writeIdx += 4; + } + ReverseInBlocks(data, tex.width * 4, tex.height); + encoding = "rgba8"; + break; + } + return new ImageMsg(header, height: (uint)tex.height, width: (uint)tex.width, encoding: encoding, is_bigendian: 0, step: 4, data: data); + } + + public static string ToLatLongString(this NavSatFixMsg message) + { + string lat = (message.latitude > 0) ? "ºN" : "ºS"; + string lon = (message.longitude > 0) ? "ºE" : "ºW"; + return $"{message.latitude}{lat} {message.longitude}{lon}"; + } + } +} diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageExtensions.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageExtensions.cs.meta new file mode 100644 index 00000000..46cb45ac --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageExtensions.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 7085dfe7a1d55a644b1cbfa3f0cf6770 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: 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 9866a01f..d0005a83 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageRegistry.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageRegistry.cs @@ -7,24 +7,32 @@ namespace Unity.Robotics.ROSTCPConnector.MessageGeneration { public static class MessageRegistry { - static Dictionary> s_DeserializeFunctionsByName = new Dictionary>(); + static Dictionary>[] s_DeserializeFunctionsByName = new Dictionary>[]{ + new Dictionary>(), // default + new Dictionary>(), // response + new Dictionary>(), // goal + new Dictionary>(), // feedback + new Dictionary>(), // result + }; class RegistryEntry { public static string s_RosMessageName; public static Func s_DeserializeFunction; } - public static void Register(string rosMessageName, Func deserialize) where T : Message + public static void Register(string rosMessageName, Func deserialize, MessageSubtopic subtopic = MessageSubtopic.Default) where T : Message { RegistryEntry.s_RosMessageName = rosMessageName; RegistryEntry.s_DeserializeFunction = deserialize; - s_DeserializeFunctionsByName[rosMessageName] = deserialize; + if (s_DeserializeFunctionsByName[(int)subtopic].ContainsKey(rosMessageName)) + Debug.LogWarning($"More than one message was registered as \"{rosMessageName}\" \"{subtopic}\""); + s_DeserializeFunctionsByName[(int)subtopic][rosMessageName] = deserialize; } - public static Func GetDeserializeFunction(string rosMessageName) + public static Func GetDeserializeFunction(string rosMessageName, MessageSubtopic subtopic = MessageSubtopic.Default) { Func result; - s_DeserializeFunctionsByName.TryGetValue(rosMessageName, out result); + s_DeserializeFunctionsByName[(int)subtopic].TryGetValue(rosMessageName, out result); return result; } 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 19008daa..edfb181d 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageSerializer.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageSerializer.cs @@ -71,6 +71,8 @@ public byte[] GetBytes() int writeIndex = 0; foreach (byte[] statement in m_ListOfSerializations) { + if (statement == null) + continue; statement.CopyTo(serializedMessage, writeIndex); writeIndex += statement.Length; } diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageSubtopic.cs b/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageSubtopic.cs new file mode 100644 index 00000000..c6c2ced9 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageSubtopic.cs @@ -0,0 +1,11 @@ +namespace Unity.Robotics.ROSTCPConnector.MessageGeneration +{ + public enum MessageSubtopic + { + Default = 0, + Response = 1, + Goal = 2, + Feedback = 3, + Result = 4, + } +} diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageSubtopic.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageSubtopic.cs.meta new file mode 100644 index 00000000..bf341be9 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageSubtopic.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 7d6200914621dd54e9c17a6e65308726 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Diagnostic/srv/AddDiagnosticsResponse.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Diagnostic/srv/AddDiagnosticsResponse.cs index 4c2f0b8f..1fe0b79a 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Diagnostic/srv/AddDiagnosticsResponse.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Diagnostic/srv/AddDiagnosticsResponse.cs @@ -61,7 +61,7 @@ public override string ToString() #endif public static void Register() { - MessageRegistry.Register(k_RosMessageName, Deserialize); + MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Response); } } } diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Diagnostic/srv/SelfTestResponse.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Diagnostic/srv/SelfTestResponse.cs index 3c05d15c..b0db10af 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Diagnostic/srv/SelfTestResponse.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Diagnostic/srv/SelfTestResponse.cs @@ -63,7 +63,7 @@ public override string ToString() #endif public static void Register() { - MessageRegistry.Register(k_RosMessageName, Deserialize); + MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Response); } } } diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Geometry/msg/Point32Msg.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Geometry/msg/Point32Msg.cs index c771bcc9..8ca37725 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Geometry/msg/Point32Msg.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Geometry/msg/Point32Msg.cs @@ -15,9 +15,9 @@ public class Point32Msg : Message // This contains the position of a point in free space(with 32 bits of precision). // It is recommended to use Point wherever possible instead of Point32. - // + // // This recommendation is to promote interoperability. - // + // // This message is designed to take up less space when sending // lots of points at once, as in the case of a PointCloud. public float x; diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/HandwrittenMessages/msg/HeaderMsg.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/HandwrittenMessages/msg/HeaderMsg.cs index 60d984dc..3e378fc1 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/HandwrittenMessages/msg/HeaderMsg.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/HandwrittenMessages/msg/HeaderMsg.cs @@ -17,7 +17,7 @@ public class HeaderMsg : Message // in a particular coordinate frame. #if !ROS2 - // sequence ID: consecutively increasing ID + // sequence ID: consecutively increasing ID public uint seq; #endif @@ -84,7 +84,7 @@ public override string ToString() return "Header: " + #if !ROS2 "\nseq: " + seq.ToString() + -#endif +#endif "\nstamp: " + stamp.ToString() + "\nframe_id: " + frame_id.ToString(); } 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 new file mode 100644 index 00000000..5737fc8d --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/HandwrittenMessages/srv.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 7616fee199c3ce545a29adf052c60af4 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/action/GetMapAction.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/action/GetMapAction.cs index 9b8c2b07..d1caccd7 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/action/GetMapAction.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/action/GetMapAction.cs @@ -9,6 +9,7 @@ public class GetMapAction : Action k_RosMessageName; + public GetMapAction() : base() { this.action_goal = new GetMapActionGoal(); diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/action/GetMapActionFeedback.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/action/GetMapActionFeedback.cs index d511cced..6d260269 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/action/GetMapActionFeedback.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/action/GetMapActionFeedback.cs @@ -10,6 +10,7 @@ public class GetMapActionFeedback : ActionFeedback public const string k_RosMessageName = "nav_msgs/GetMapActionFeedback"; public override string RosMessageName => k_RosMessageName; + public GetMapActionFeedback() : base() { this.feedback = new GetMapFeedback(); diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/action/GetMapActionGoal.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/action/GetMapActionGoal.cs index 552fb3e6..73508ca4 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/action/GetMapActionGoal.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/action/GetMapActionGoal.cs @@ -10,6 +10,7 @@ public class GetMapActionGoal : ActionGoal public const string k_RosMessageName = "nav_msgs/GetMapActionGoal"; public override string RosMessageName => k_RosMessageName; + public GetMapActionGoal() : base() { this.goal = new GetMapGoal(); diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/action/GetMapActionResult.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/action/GetMapActionResult.cs index 3bfb8af1..5825d1be 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/action/GetMapActionResult.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/action/GetMapActionResult.cs @@ -10,6 +10,7 @@ public class GetMapActionResult : ActionResult public const string k_RosMessageName = "nav_msgs/GetMapActionResult"; public override string RosMessageName => k_RosMessageName; + public GetMapActionResult() : base() { this.result = new GetMapResult(); diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/action/GetMapFeedback.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/action/GetMapFeedback.cs index 546ed30a..e6cccd36 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/action/GetMapFeedback.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/action/GetMapFeedback.cs @@ -40,7 +40,7 @@ public override string ToString() #endif public static void Register() { - MessageRegistry.Register(k_RosMessageName, Deserialize); + MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Feedback); } } } diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/action/GetMapGoal.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/action/GetMapGoal.cs index 0c3b9919..fd1c6963 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/action/GetMapGoal.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/action/GetMapGoal.cs @@ -40,7 +40,7 @@ public override string ToString() #endif public static void Register() { - MessageRegistry.Register(k_RosMessageName, Deserialize); + MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Goal); } } } diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/action/GetMapResult.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/action/GetMapResult.cs index b77cbc12..25b097e3 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/action/GetMapResult.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/action/GetMapResult.cs @@ -50,7 +50,7 @@ public override string ToString() #endif public static void Register() { - MessageRegistry.Register(k_RosMessageName, Deserialize); + MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Result); } } } diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/srv/GetMapResponse.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/srv/GetMapResponse.cs index 4627af60..fd4ce65e 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/srv/GetMapResponse.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/srv/GetMapResponse.cs @@ -51,7 +51,7 @@ public override string ToString() #endif public static void Register() { - MessageRegistry.Register(k_RosMessageName, Deserialize); + MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Response); } } } diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/srv/GetPlanResponse.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/srv/GetPlanResponse.cs index ac3cc9d3..4f64a5d1 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/srv/GetPlanResponse.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/srv/GetPlanResponse.cs @@ -51,7 +51,7 @@ public override string ToString() #endif public static void Register() { - MessageRegistry.Register(k_RosMessageName, Deserialize); + MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Response); } } } diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/srv/LoadMapResponse.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/srv/LoadMapResponse.cs index ea3c22c1..8fb7d037 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/srv/LoadMapResponse.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/srv/LoadMapResponse.cs @@ -63,7 +63,7 @@ public override string ToString() #endif public static void Register() { - MessageRegistry.Register(k_RosMessageName, Deserialize); + MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Response); } } } diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/srv/SetMapResponse.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/srv/SetMapResponse.cs index d7379683..22ec0f54 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/srv/SetMapResponse.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/srv/SetMapResponse.cs @@ -51,7 +51,7 @@ public override string ToString() #endif public static void Register() { - MessageRegistry.Register(k_RosMessageName, Deserialize); + MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Response); } } } diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ObjectRecognition/action/ObjectRecognitionFeedback.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ObjectRecognition/action/ObjectRecognitionFeedback.cs index 64ca9d79..31cdba91 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ObjectRecognition/action/ObjectRecognitionFeedback.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ObjectRecognition/action/ObjectRecognitionFeedback.cs @@ -40,7 +40,7 @@ public override string ToString() #endif public static void Register() { - MessageRegistry.Register(k_RosMessageName, Deserialize); + MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Feedback); } } } diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ObjectRecognition/action/ObjectRecognitionGoal.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ObjectRecognition/action/ObjectRecognitionGoal.cs index 10db0e5a..bac71c8d 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ObjectRecognition/action/ObjectRecognitionGoal.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ObjectRecognition/action/ObjectRecognitionGoal.cs @@ -58,7 +58,7 @@ public override string ToString() #endif public static void Register() { - MessageRegistry.Register(k_RosMessageName, Deserialize); + MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Goal); } } } diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ObjectRecognition/action/ObjectRecognitionResult.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ObjectRecognition/action/ObjectRecognitionResult.cs index dc1ee4d1..8e52b7bd 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ObjectRecognition/action/ObjectRecognitionResult.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ObjectRecognition/action/ObjectRecognitionResult.cs @@ -51,7 +51,7 @@ public override string ToString() #endif public static void Register() { - MessageRegistry.Register(k_RosMessageName, Deserialize); + MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Result); } } } diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ObjectRecognition/srv/GetObjectInformationResponse.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ObjectRecognition/srv/GetObjectInformationResponse.cs index 4366fd29..3083acb8 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ObjectRecognition/srv/GetObjectInformationResponse.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/ObjectRecognition/srv/GetObjectInformationResponse.cs @@ -51,7 +51,7 @@ public override string ToString() #endif public static void Register() { - MessageRegistry.Register(k_RosMessageName, Deserialize); + MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Response); } } } diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Octomap/srv/BoundingBoxQueryResponse.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Octomap/srv/BoundingBoxQueryResponse.cs index 3584680f..e1a74331 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Octomap/srv/BoundingBoxQueryResponse.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Octomap/srv/BoundingBoxQueryResponse.cs @@ -39,7 +39,7 @@ public override string ToString() #endif public static void Register() { - MessageRegistry.Register(k_RosMessageName, Deserialize); + MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Response); } } } diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Octomap/srv/GetOctomapResponse.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Octomap/srv/GetOctomapResponse.cs index 0990bf16..d7017440 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Octomap/srv/GetOctomapResponse.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Octomap/srv/GetOctomapResponse.cs @@ -50,7 +50,7 @@ public override string ToString() #endif public static void Register() { - MessageRegistry.Register(k_RosMessageName, Deserialize); + MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Response); } } } diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Rosgraph.meta b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Rosgraph.meta new file mode 100644 index 00000000..12de3fd0 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Rosgraph.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: e5962ccc8da3648479afd21afdfb74a5 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Rosgraph/msg.meta b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Rosgraph/msg.meta new file mode 100644 index 00000000..e9d84ba1 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Rosgraph/msg.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 1549a2716409b4ac38a8bbb432fcbcdb +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Rosgraph/msg/ClockMsg.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Rosgraph/msg/ClockMsg.cs new file mode 100644 index 00000000..43fe1fcb --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Rosgraph/msg/ClockMsg.cs @@ -0,0 +1,60 @@ +//Do not edit! This file was generated by Unity-ROS MessageGeneration. +using System; +using System.Linq; +using System.Collections.Generic; +using System.Text; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using RosMessageTypes.BuiltinInterfaces; + +namespace RosMessageTypes.Rosgraph +{ + [Serializable] + public class ClockMsg : Message + { + public const string k_RosMessageName = "rosgraph_msgs/Clock"; + public override string RosMessageName => k_RosMessageName; + + // roslib/Clock is used for publishing simulated time in ROS. + // This message simply communicates the current time. + // For more information, see http://www.ros.org/wiki/Clock + public TimeMsg clock; + + public ClockMsg() + { + this.clock = new TimeMsg(); + } + + public ClockMsg(TimeMsg clock) + { + this.clock = clock; + } + + public static ClockMsg Deserialize(MessageDeserializer deserializer) => new ClockMsg(deserializer); + + private ClockMsg(MessageDeserializer deserializer) + { + this.clock = TimeMsg.Deserialize(deserializer); + } + + public override void SerializeTo(MessageSerializer serializer) + { + serializer.Write(this.clock); + } + + public override string ToString() + { + return "ClockMsg: " + + "\nclock: " + clock.ToString(); + } + +#if UNITY_EDITOR + [UnityEditor.InitializeOnLoadMethod] +#else + [UnityEngine.RuntimeInitializeOnLoadMethod] +#endif + public static void Register() + { + MessageRegistry.Register(k_RosMessageName, Deserialize); + } + } +} diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Rosgraph/msg/ClockMsg.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Rosgraph/msg/ClockMsg.cs.meta new file mode 100644 index 00000000..f76b51eb --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Rosgraph/msg/ClockMsg.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 7f0e00b0615af4fcc8b1640447e53ed3 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Rosgraph/msg/LogMsg.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Rosgraph/msg/LogMsg.cs new file mode 100644 index 00000000..0f7fc67e --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Rosgraph/msg/LogMsg.cs @@ -0,0 +1,117 @@ +//Do not edit! This file was generated by Unity-ROS MessageGeneration. +using System; +using System.Linq; +using System.Collections.Generic; +using System.Text; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using RosMessageTypes.Std; + +namespace RosMessageTypes.Rosgraph +{ + [Serializable] + public class LogMsg : Message + { + public const string k_RosMessageName = "rosgraph_msgs/Log"; + public override string RosMessageName => k_RosMessageName; + + // # + // # Severity level constants + // # + public const sbyte DEBUG = 1; // debug level + public const sbyte INFO = 2; // general level + public const sbyte WARN = 4; // warning level + public const sbyte ERROR = 8; // error level + public const sbyte FATAL = 16; // fatal/critical level + // # + // # Fields + // # + public HeaderMsg header; + public sbyte level; + public string name; + // name of the node + public string msg; + // message + public string file; + // file the message came from + public string function; + // function the message came from + public uint line; + // line the message came from + public string[] topics; + // topic names that the node publishes + + public LogMsg() + { + this.header = new HeaderMsg(); + this.level = 0; + this.name = ""; + this.msg = ""; + this.file = ""; + this.function = ""; + this.line = 0; + this.topics = new string[0]; + } + + public LogMsg(HeaderMsg header, sbyte level, string name, string msg, string file, string function, uint line, string[] topics) + { + this.header = header; + this.level = level; + this.name = name; + this.msg = msg; + this.file = file; + this.function = function; + this.line = line; + this.topics = topics; + } + + public static LogMsg Deserialize(MessageDeserializer deserializer) => new LogMsg(deserializer); + + private LogMsg(MessageDeserializer deserializer) + { + this.header = HeaderMsg.Deserialize(deserializer); + deserializer.Read(out this.level); + deserializer.Read(out this.name); + deserializer.Read(out this.msg); + deserializer.Read(out this.file); + deserializer.Read(out this.function); + deserializer.Read(out this.line); + deserializer.Read(out this.topics, deserializer.ReadLength()); + } + + public override void SerializeTo(MessageSerializer serializer) + { + serializer.Write(this.header); + serializer.Write(this.level); + serializer.Write(this.name); + serializer.Write(this.msg); + serializer.Write(this.file); + serializer.Write(this.function); + serializer.Write(this.line); + serializer.WriteLength(this.topics); + serializer.Write(this.topics); + } + + public override string ToString() + { + return "LogMsg: " + + "\nheader: " + header.ToString() + + "\nlevel: " + level.ToString() + + "\nname: " + name.ToString() + + "\nmsg: " + msg.ToString() + + "\nfile: " + file.ToString() + + "\nfunction: " + function.ToString() + + "\nline: " + line.ToString() + + "\ntopics: " + System.String.Join(", ", topics.ToList()); + } + +#if UNITY_EDITOR + [UnityEditor.InitializeOnLoadMethod] +#else + [UnityEngine.RuntimeInitializeOnLoadMethod] +#endif + public static void Register() + { + MessageRegistry.Register(k_RosMessageName, Deserialize); + } + } +} diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Rosgraph/msg/LogMsg.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Rosgraph/msg/LogMsg.cs.meta new file mode 100644 index 00000000..6fb1c40c --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Rosgraph/msg/LogMsg.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 1f2a11cd0d2954866b174eff1c2ab43c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Rosgraph/msg/TopicStatisticsMsg.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Rosgraph/msg/TopicStatisticsMsg.cs new file mode 100644 index 00000000..84246633 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Rosgraph/msg/TopicStatisticsMsg.cs @@ -0,0 +1,146 @@ +//Do not edit! This file was generated by Unity-ROS MessageGeneration. +using System; +using System.Linq; +using System.Collections.Generic; +using System.Text; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using RosMessageTypes.BuiltinInterfaces; + +namespace RosMessageTypes.Rosgraph +{ + [Serializable] + public class TopicStatisticsMsg : Message + { + public const string k_RosMessageName = "rosgraph_msgs/TopicStatistics"; + public override string RosMessageName => k_RosMessageName; + + // name of the topic + public string topic; + // node id of the publisher + public string node_pub; + // node id of the subscriber + public string node_sub; + // the statistics apply to this time window + public TimeMsg window_start; + public TimeMsg window_stop; + // number of messages delivered during the window + public int delivered_msgs; + // numbers of messages dropped during the window + public int dropped_msgs; + // traffic during the window, in bytes + public int traffic; + // mean/stddev/max period between two messages + public DurationMsg period_mean; + public DurationMsg period_stddev; + public DurationMsg period_max; + // mean/stddev/max age of the message based on the + // timestamp in the message header. In case the + // message does not have a header, it will be 0. + public DurationMsg stamp_age_mean; + public DurationMsg stamp_age_stddev; + public DurationMsg stamp_age_max; + + public TopicStatisticsMsg() + { + this.topic = ""; + this.node_pub = ""; + this.node_sub = ""; + this.window_start = new TimeMsg(); + this.window_stop = new TimeMsg(); + this.delivered_msgs = 0; + this.dropped_msgs = 0; + this.traffic = 0; + this.period_mean = new DurationMsg(); + this.period_stddev = new DurationMsg(); + this.period_max = new DurationMsg(); + this.stamp_age_mean = new DurationMsg(); + this.stamp_age_stddev = new DurationMsg(); + this.stamp_age_max = new DurationMsg(); + } + + public TopicStatisticsMsg(string topic, string node_pub, string node_sub, TimeMsg window_start, TimeMsg window_stop, int delivered_msgs, int dropped_msgs, int traffic, DurationMsg period_mean, DurationMsg period_stddev, DurationMsg period_max, DurationMsg stamp_age_mean, DurationMsg stamp_age_stddev, DurationMsg stamp_age_max) + { + this.topic = topic; + this.node_pub = node_pub; + this.node_sub = node_sub; + this.window_start = window_start; + this.window_stop = window_stop; + this.delivered_msgs = delivered_msgs; + this.dropped_msgs = dropped_msgs; + this.traffic = traffic; + this.period_mean = period_mean; + this.period_stddev = period_stddev; + this.period_max = period_max; + this.stamp_age_mean = stamp_age_mean; + this.stamp_age_stddev = stamp_age_stddev; + this.stamp_age_max = stamp_age_max; + } + + public static TopicStatisticsMsg Deserialize(MessageDeserializer deserializer) => new TopicStatisticsMsg(deserializer); + + private TopicStatisticsMsg(MessageDeserializer deserializer) + { + deserializer.Read(out this.topic); + deserializer.Read(out this.node_pub); + deserializer.Read(out this.node_sub); + this.window_start = TimeMsg.Deserialize(deserializer); + this.window_stop = TimeMsg.Deserialize(deserializer); + deserializer.Read(out this.delivered_msgs); + deserializer.Read(out this.dropped_msgs); + deserializer.Read(out this.traffic); + this.period_mean = DurationMsg.Deserialize(deserializer); + this.period_stddev = DurationMsg.Deserialize(deserializer); + this.period_max = DurationMsg.Deserialize(deserializer); + this.stamp_age_mean = DurationMsg.Deserialize(deserializer); + this.stamp_age_stddev = DurationMsg.Deserialize(deserializer); + this.stamp_age_max = DurationMsg.Deserialize(deserializer); + } + + public override void SerializeTo(MessageSerializer serializer) + { + serializer.Write(this.topic); + serializer.Write(this.node_pub); + serializer.Write(this.node_sub); + serializer.Write(this.window_start); + serializer.Write(this.window_stop); + serializer.Write(this.delivered_msgs); + serializer.Write(this.dropped_msgs); + serializer.Write(this.traffic); + serializer.Write(this.period_mean); + serializer.Write(this.period_stddev); + serializer.Write(this.period_max); + serializer.Write(this.stamp_age_mean); + serializer.Write(this.stamp_age_stddev); + serializer.Write(this.stamp_age_max); + } + + public override string ToString() + { + return "TopicStatisticsMsg: " + + "\ntopic: " + topic.ToString() + + "\nnode_pub: " + node_pub.ToString() + + "\nnode_sub: " + node_sub.ToString() + + "\nwindow_start: " + window_start.ToString() + + "\nwindow_stop: " + window_stop.ToString() + + "\ndelivered_msgs: " + delivered_msgs.ToString() + + "\ndropped_msgs: " + dropped_msgs.ToString() + + "\ntraffic: " + traffic.ToString() + + "\nperiod_mean: " + period_mean.ToString() + + "\nperiod_stddev: " + period_stddev.ToString() + + "\nperiod_max: " + period_max.ToString() + + "\nstamp_age_mean: " + stamp_age_mean.ToString() + + "\nstamp_age_stddev: " + stamp_age_stddev.ToString() + + "\nstamp_age_max: " + stamp_age_max.ToString(); + } + +#if UNITY_EDITOR + [UnityEditor.InitializeOnLoadMethod] +#else + [UnityEngine.RuntimeInitializeOnLoadMethod] +#endif + public static void Register() + { + MessageRegistry.Register(k_RosMessageName, Deserialize); + } + } +} diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Rosgraph/msg/TopicStatisticsMsg.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Rosgraph/msg/TopicStatisticsMsg.cs.meta new file mode 100644 index 00000000..e445d0ba --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Rosgraph/msg/TopicStatisticsMsg.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 9c0a4e1f9f89e4ef0af5bb52dfcf6b74 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Sensor/msg/ImuMsg.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Sensor/msg/ImuMsg.cs index 67cd9e37..afc53e43 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Sensor/msg/ImuMsg.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Sensor/msg/ImuMsg.cs @@ -14,14 +14,14 @@ public class ImuMsg : Message public override string RosMessageName => k_RosMessageName; // This is a message to hold data from an IMU (Inertial Measurement Unit) - // + // // Accelerations should be in m/s^2 (not in g's), and rotational velocity should be in rad/sec - // + // // If the covariance of the measurement is known, it should be filled in (if all you know is the // variance of each measurement, e.g. from the datasheet, just put those along the diagonal) // A covariance matrix of all zeros will be interpreted as "covariance unknown", and to use the // data a covariance will have to be assumed or gotten from some other source - // + // // If you have no estimate for one of the data elements (e.g. your IMU doesn't produce an // orientation estimate), please set element 0 of the associated covariance matrix to -1 // If you are interpreting this message, please check for a value of -1 in the first element of each diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Sensor/srv/SetCameraInfoResponse.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Sensor/srv/SetCameraInfoResponse.cs index 88792afc..b9a2b0c4 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Sensor/srv/SetCameraInfoResponse.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Sensor/srv/SetCameraInfoResponse.cs @@ -58,7 +58,7 @@ public override string ToString() #endif public static void Register() { - MessageRegistry.Register(k_RosMessageName, Deserialize); + MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Response); } } } diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Std/srv/EmptyResponse.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Std/srv/EmptyResponse.cs index 945847bf..679831e1 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Std/srv/EmptyResponse.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Std/srv/EmptyResponse.cs @@ -39,7 +39,7 @@ public override string ToString() #endif public static void Register() { - MessageRegistry.Register(k_RosMessageName, Deserialize); + MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Response); } } } diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Std/srv/SetBoolResponse.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Std/srv/SetBoolResponse.cs index 2c136fe5..9d4c7f04 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Std/srv/SetBoolResponse.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Std/srv/SetBoolResponse.cs @@ -58,7 +58,7 @@ public override string ToString() #endif public static void Register() { - MessageRegistry.Register(k_RosMessageName, Deserialize); + MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Response); } } } diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Std/srv/TriggerResponse.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Std/srv/TriggerResponse.cs index b8b0a382..b771646b 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Std/srv/TriggerResponse.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Std/srv/TriggerResponse.cs @@ -58,7 +58,7 @@ public override string ToString() #endif public static void Register() { - MessageRegistry.Register(k_RosMessageName, Deserialize); + MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Response); } } } diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2.meta b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2.meta new file mode 100644 index 00000000..188846d7 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: a5e8e35917a9e0949a00177203401e5e +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action.meta b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action.meta new file mode 100644 index 00000000..aca2b869 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 1dbabffde68444d8ba7a9fc06b3730f4 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformAction.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformAction.cs new file mode 100644 index 00000000..13c89226 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformAction.cs @@ -0,0 +1,37 @@ +using System.Collections.Generic; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; + + +namespace RosMessageTypes.Tf2 +{ + public class LookupTransformAction : Action + { + public const string k_RosMessageName = "tf2_msgs/LookupTransformAction"; + public override string RosMessageName => k_RosMessageName; + + + public LookupTransformAction() : base() + { + this.action_goal = new LookupTransformActionGoal(); + this.action_result = new LookupTransformActionResult(); + this.action_feedback = new LookupTransformActionFeedback(); + } + + public static LookupTransformAction Deserialize(MessageDeserializer deserializer) => new LookupTransformAction(deserializer); + + LookupTransformAction(MessageDeserializer deserializer) + { + this.action_goal = LookupTransformActionGoal.Deserialize(deserializer); + this.action_result = LookupTransformActionResult.Deserialize(deserializer); + this.action_feedback = LookupTransformActionFeedback.Deserialize(deserializer); + } + + public override void SerializeTo(MessageSerializer serializer) + { + serializer.Write(this.action_goal); + serializer.Write(this.action_result); + serializer.Write(this.action_feedback); + } + + } +} diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformAction.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformAction.cs.meta new file mode 100644 index 00000000..6332eb38 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformAction.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: dc5f7f906e8ff4450945b23dfaf6c658 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformActionFeedback.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformActionFeedback.cs new file mode 100644 index 00000000..784d6dcc --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformActionFeedback.cs @@ -0,0 +1,37 @@ +using System.Collections.Generic; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using RosMessageTypes.Std; +using RosMessageTypes.Actionlib; + +namespace RosMessageTypes.Tf2 +{ + public class LookupTransformActionFeedback : ActionFeedback + { + public const string k_RosMessageName = "tf2_msgs/LookupTransformActionFeedback"; + public override string RosMessageName => k_RosMessageName; + + + public LookupTransformActionFeedback() : base() + { + this.feedback = new LookupTransformFeedback(); + } + + public LookupTransformActionFeedback(HeaderMsg header, GoalStatusMsg status, LookupTransformFeedback feedback) : base(header, status) + { + this.feedback = feedback; + } + public static LookupTransformActionFeedback Deserialize(MessageDeserializer deserializer) => new LookupTransformActionFeedback(deserializer); + + LookupTransformActionFeedback(MessageDeserializer deserializer) : base(deserializer) + { + this.feedback = LookupTransformFeedback.Deserialize(deserializer); + } + public override void SerializeTo(MessageSerializer serializer) + { + serializer.Write(this.header); + serializer.Write(this.status); + serializer.Write(this.feedback); + } + + } +} diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformActionFeedback.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformActionFeedback.cs.meta new file mode 100644 index 00000000..92e4f468 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformActionFeedback.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: e2c6074f2dd51401ea42e73f07b17420 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformActionGoal.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformActionGoal.cs new file mode 100644 index 00000000..f07f7a61 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformActionGoal.cs @@ -0,0 +1,37 @@ +using System.Collections.Generic; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using RosMessageTypes.Std; +using RosMessageTypes.Actionlib; + +namespace RosMessageTypes.Tf2 +{ + public class LookupTransformActionGoal : ActionGoal + { + public const string k_RosMessageName = "tf2_msgs/LookupTransformActionGoal"; + public override string RosMessageName => k_RosMessageName; + + + public LookupTransformActionGoal() : base() + { + this.goal = new LookupTransformGoal(); + } + + public LookupTransformActionGoal(HeaderMsg header, GoalIDMsg goal_id, LookupTransformGoal goal) : base(header, goal_id) + { + this.goal = goal; + } + public static LookupTransformActionGoal Deserialize(MessageDeserializer deserializer) => new LookupTransformActionGoal(deserializer); + + LookupTransformActionGoal(MessageDeserializer deserializer) : base(deserializer) + { + this.goal = LookupTransformGoal.Deserialize(deserializer); + } + public override void SerializeTo(MessageSerializer serializer) + { + serializer.Write(this.header); + serializer.Write(this.goal_id); + serializer.Write(this.goal); + } + + } +} diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformActionGoal.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformActionGoal.cs.meta new file mode 100644 index 00000000..285203d4 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformActionGoal.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c978cedffbc704ce2be299d52ecd58c0 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformActionResult.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformActionResult.cs new file mode 100644 index 00000000..2020b048 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformActionResult.cs @@ -0,0 +1,37 @@ +using System.Collections.Generic; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using RosMessageTypes.Std; +using RosMessageTypes.Actionlib; + +namespace RosMessageTypes.Tf2 +{ + public class LookupTransformActionResult : ActionResult + { + public const string k_RosMessageName = "tf2_msgs/LookupTransformActionResult"; + public override string RosMessageName => k_RosMessageName; + + + public LookupTransformActionResult() : base() + { + this.result = new LookupTransformResult(); + } + + public LookupTransformActionResult(HeaderMsg header, GoalStatusMsg status, LookupTransformResult result) : base(header, status) + { + this.result = result; + } + public static LookupTransformActionResult Deserialize(MessageDeserializer deserializer) => new LookupTransformActionResult(deserializer); + + LookupTransformActionResult(MessageDeserializer deserializer) : base(deserializer) + { + this.result = LookupTransformResult.Deserialize(deserializer); + } + public override void SerializeTo(MessageSerializer serializer) + { + serializer.Write(this.header); + serializer.Write(this.status); + serializer.Write(this.result); + } + + } +} diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformActionResult.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformActionResult.cs.meta new file mode 100644 index 00000000..5f24e98b --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformActionResult.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: e20032c52a2f2439384f243b410c1e38 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformFeedback.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformFeedback.cs new file mode 100644 index 00000000..525c4b4e --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformFeedback.cs @@ -0,0 +1,45 @@ +//Do not edit! This file was generated by Unity-ROS MessageGeneration. +using System; +using System.Linq; +using System.Collections.Generic; +using System.Text; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; + +namespace RosMessageTypes.Tf2 +{ + [Serializable] + public class LookupTransformFeedback : Message + { + public const string k_RosMessageName = "tf2_msgs/LookupTransform"; + public override string RosMessageName => k_RosMessageName; + + + public LookupTransformFeedback() + { + } + public static LookupTransformFeedback Deserialize(MessageDeserializer deserializer) => new LookupTransformFeedback(deserializer); + + private LookupTransformFeedback(MessageDeserializer deserializer) + { + } + + public override void SerializeTo(MessageSerializer serializer) + { + } + + public override string ToString() + { + return "LookupTransformFeedback: "; + } + +#if UNITY_EDITOR + [UnityEditor.InitializeOnLoadMethod] +#else + [UnityEngine.RuntimeInitializeOnLoadMethod] +#endif + public static void Register() + { + MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Feedback); + } + } +} diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformFeedback.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformFeedback.cs.meta new file mode 100644 index 00000000..1f7cd704 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformFeedback.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: a0732d6f56146447eb7aff397d53c66d +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformGoal.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformGoal.cs new file mode 100644 index 00000000..17d96a7d --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformGoal.cs @@ -0,0 +1,96 @@ +//Do not edit! This file was generated by Unity-ROS MessageGeneration. +using System; +using System.Linq; +using System.Collections.Generic; +using System.Text; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using RosMessageTypes.BuiltinInterfaces; + +namespace RosMessageTypes.Tf2 +{ + [Serializable] + public class LookupTransformGoal : Message + { + public const string k_RosMessageName = "tf2_msgs/LookupTransform"; + public override string RosMessageName => k_RosMessageName; + + // Simple API + public string target_frame; + public string source_frame; + public TimeMsg source_time; + public DurationMsg timeout; + // Advanced API + public TimeMsg target_time; + public string fixed_frame; + // Whether or not to use the advanced API + public bool advanced; + + public LookupTransformGoal() + { + this.target_frame = ""; + this.source_frame = ""; + this.source_time = new TimeMsg(); + this.timeout = new DurationMsg(); + this.target_time = new TimeMsg(); + this.fixed_frame = ""; + this.advanced = false; + } + + public LookupTransformGoal(string target_frame, string source_frame, TimeMsg source_time, DurationMsg timeout, TimeMsg target_time, string fixed_frame, bool advanced) + { + this.target_frame = target_frame; + this.source_frame = source_frame; + this.source_time = source_time; + this.timeout = timeout; + this.target_time = target_time; + this.fixed_frame = fixed_frame; + this.advanced = advanced; + } + + public static LookupTransformGoal Deserialize(MessageDeserializer deserializer) => new LookupTransformGoal(deserializer); + + private LookupTransformGoal(MessageDeserializer deserializer) + { + deserializer.Read(out this.target_frame); + deserializer.Read(out this.source_frame); + this.source_time = TimeMsg.Deserialize(deserializer); + this.timeout = DurationMsg.Deserialize(deserializer); + this.target_time = TimeMsg.Deserialize(deserializer); + deserializer.Read(out this.fixed_frame); + deserializer.Read(out this.advanced); + } + + public override void SerializeTo(MessageSerializer serializer) + { + serializer.Write(this.target_frame); + serializer.Write(this.source_frame); + serializer.Write(this.source_time); + serializer.Write(this.timeout); + serializer.Write(this.target_time); + serializer.Write(this.fixed_frame); + serializer.Write(this.advanced); + } + + public override string ToString() + { + return "LookupTransformGoal: " + + "\ntarget_frame: " + target_frame.ToString() + + "\nsource_frame: " + source_frame.ToString() + + "\nsource_time: " + source_time.ToString() + + "\ntimeout: " + timeout.ToString() + + "\ntarget_time: " + target_time.ToString() + + "\nfixed_frame: " + fixed_frame.ToString() + + "\nadvanced: " + advanced.ToString(); + } + +#if UNITY_EDITOR + [UnityEditor.InitializeOnLoadMethod] +#else + [UnityEngine.RuntimeInitializeOnLoadMethod] +#endif + public static void Register() + { + MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Goal); + } + } +} diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformGoal.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformGoal.cs.meta new file mode 100644 index 00000000..0b4242d0 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformGoal.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: dbdd5b79defed454a927d42f2828103f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformResult.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformResult.cs new file mode 100644 index 00000000..57d2f8a5 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformResult.cs @@ -0,0 +1,62 @@ +//Do not edit! This file was generated by Unity-ROS MessageGeneration. +using System; +using System.Linq; +using System.Collections.Generic; +using System.Text; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; + +namespace RosMessageTypes.Tf2 +{ + [Serializable] + public class LookupTransformResult : Message + { + public const string k_RosMessageName = "tf2_msgs/LookupTransform"; + public override string RosMessageName => k_RosMessageName; + + public Geometry.TransformStampedMsg transform; + public TF2ErrorMsg error; + + public LookupTransformResult() + { + this.transform = new Geometry.TransformStampedMsg(); + this.error = new TF2ErrorMsg(); + } + + public LookupTransformResult(Geometry.TransformStampedMsg transform, TF2ErrorMsg error) + { + this.transform = transform; + this.error = error; + } + + public static LookupTransformResult Deserialize(MessageDeserializer deserializer) => new LookupTransformResult(deserializer); + + private LookupTransformResult(MessageDeserializer deserializer) + { + this.transform = Geometry.TransformStampedMsg.Deserialize(deserializer); + this.error = TF2ErrorMsg.Deserialize(deserializer); + } + + public override void SerializeTo(MessageSerializer serializer) + { + serializer.Write(this.transform); + serializer.Write(this.error); + } + + public override string ToString() + { + return "LookupTransformResult: " + + "\ntransform: " + transform.ToString() + + "\nerror: " + error.ToString(); + } + +#if UNITY_EDITOR + [UnityEditor.InitializeOnLoadMethod] +#else + [UnityEngine.RuntimeInitializeOnLoadMethod] +#endif + public static void Register() + { + MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Result); + } + } +} diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformResult.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformResult.cs.meta new file mode 100644 index 00000000..bdf1d3a1 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformResult.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 5f645ff91c32c4059aebd7e2444407f8 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/msg.meta b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/msg.meta new file mode 100644 index 00000000..aeb8f203 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/msg.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: dce99d42918c044d9b04c887e1d5e42b +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/msg/TF2ErrorMsg.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/msg/TF2ErrorMsg.cs new file mode 100644 index 00000000..de7bb745 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/msg/TF2ErrorMsg.cs @@ -0,0 +1,69 @@ +//Do not edit! This file was generated by Unity-ROS MessageGeneration. +using System; +using System.Linq; +using System.Collections.Generic; +using System.Text; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; + +namespace RosMessageTypes.Tf2 +{ + [Serializable] + public class TF2ErrorMsg : Message + { + public const string k_RosMessageName = "tf2_msgs/TF2Error"; + public override string RosMessageName => k_RosMessageName; + + public const byte NO_ERROR = 0; + public const byte LOOKUP_ERROR = 1; + public const byte CONNECTIVITY_ERROR = 2; + public const byte EXTRAPOLATION_ERROR = 3; + public const byte INVALID_ARGUMENT_ERROR = 4; + public const byte TIMEOUT_ERROR = 5; + public const byte TRANSFORM_ERROR = 6; + public byte error; + public string error_string; + + public TF2ErrorMsg() + { + this.error = 0; + this.error_string = ""; + } + + public TF2ErrorMsg(byte error, string error_string) + { + this.error = error; + this.error_string = error_string; + } + + public static TF2ErrorMsg Deserialize(MessageDeserializer deserializer) => new TF2ErrorMsg(deserializer); + + private TF2ErrorMsg(MessageDeserializer deserializer) + { + deserializer.Read(out this.error); + deserializer.Read(out this.error_string); + } + + public override void SerializeTo(MessageSerializer serializer) + { + serializer.Write(this.error); + serializer.Write(this.error_string); + } + + public override string ToString() + { + return "TF2ErrorMsg: " + + "\nerror: " + error.ToString() + + "\nerror_string: " + error_string.ToString(); + } + +#if UNITY_EDITOR + [UnityEditor.InitializeOnLoadMethod] +#else + [UnityEngine.RuntimeInitializeOnLoadMethod] +#endif + public static void Register() + { + MessageRegistry.Register(k_RosMessageName, Deserialize); + } + } +} diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/msg/TF2ErrorMsg.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/msg/TF2ErrorMsg.cs.meta new file mode 100644 index 00000000..dbbf96f9 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/msg/TF2ErrorMsg.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 48281dc85fc2b4d67bfc4df260db0089 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/msg/TFMessageMsg.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/msg/TFMessageMsg.cs new file mode 100644 index 00000000..82416a69 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/msg/TFMessageMsg.cs @@ -0,0 +1,57 @@ +//Do not edit! This file was generated by Unity-ROS MessageGeneration. +using System; +using System.Linq; +using System.Collections.Generic; +using System.Text; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; + +namespace RosMessageTypes.Tf2 +{ + [Serializable] + public class TFMessageMsg : Message + { + public const string k_RosMessageName = "tf2_msgs/TFMessage"; + public override string RosMessageName => k_RosMessageName; + + public Geometry.TransformStampedMsg[] transforms; + + public TFMessageMsg() + { + this.transforms = new Geometry.TransformStampedMsg[0]; + } + + public TFMessageMsg(Geometry.TransformStampedMsg[] transforms) + { + this.transforms = transforms; + } + + public static TFMessageMsg Deserialize(MessageDeserializer deserializer) => new TFMessageMsg(deserializer); + + private TFMessageMsg(MessageDeserializer deserializer) + { + deserializer.Read(out this.transforms, Geometry.TransformStampedMsg.Deserialize, deserializer.ReadLength()); + } + + public override void SerializeTo(MessageSerializer serializer) + { + serializer.WriteLength(this.transforms); + serializer.Write(this.transforms); + } + + public override string ToString() + { + return "TFMessageMsg: " + + "\ntransforms: " + System.String.Join(", ", transforms.ToList()); + } + +#if UNITY_EDITOR + [UnityEditor.InitializeOnLoadMethod] +#else + [UnityEngine.RuntimeInitializeOnLoadMethod] +#endif + public static void Register() + { + MessageRegistry.Register(k_RosMessageName, Deserialize); + } + } +} diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/msg/TFMessageMsg.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/msg/TFMessageMsg.cs.meta new file mode 100644 index 00000000..c9f3119b --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/msg/TFMessageMsg.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8ccdd0a385d6a4f7bb1b7d5b855a3fbb +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/srv.meta b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/srv.meta new file mode 100644 index 00000000..1062f567 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/srv.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: b1af9364b9b8d4d359a3bfd9db3ffd0d +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/srv/FrameGraphRequest.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/srv/FrameGraphRequest.cs new file mode 100644 index 00000000..2e249221 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/srv/FrameGraphRequest.cs @@ -0,0 +1,45 @@ +//Do not edit! This file was generated by Unity-ROS MessageGeneration. +using System; +using System.Linq; +using System.Collections.Generic; +using System.Text; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; + +namespace RosMessageTypes.Tf2 +{ + [Serializable] + public class FrameGraphRequest : Message + { + public const string k_RosMessageName = "tf2_msgs/FrameGraph"; + public override string RosMessageName => k_RosMessageName; + + + public FrameGraphRequest() + { + } + public static FrameGraphRequest Deserialize(MessageDeserializer deserializer) => new FrameGraphRequest(deserializer); + + private FrameGraphRequest(MessageDeserializer deserializer) + { + } + + public override void SerializeTo(MessageSerializer serializer) + { + } + + public override string ToString() + { + return "FrameGraphRequest: "; + } + +#if UNITY_EDITOR + [UnityEditor.InitializeOnLoadMethod] +#else + [UnityEngine.RuntimeInitializeOnLoadMethod] +#endif + public static void Register() + { + MessageRegistry.Register(k_RosMessageName, Deserialize); + } + } +} diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/srv/FrameGraphRequest.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/srv/FrameGraphRequest.cs.meta new file mode 100644 index 00000000..f35d0922 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/srv/FrameGraphRequest.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 612710bb2e09848eaa17623543cd3bae +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/srv/FrameGraphResponse.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/srv/FrameGraphResponse.cs new file mode 100644 index 00000000..f9271b5f --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/srv/FrameGraphResponse.cs @@ -0,0 +1,56 @@ +//Do not edit! This file was generated by Unity-ROS MessageGeneration. +using System; +using System.Linq; +using System.Collections.Generic; +using System.Text; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; + +namespace RosMessageTypes.Tf2 +{ + [Serializable] + public class FrameGraphResponse : Message + { + public const string k_RosMessageName = "tf2_msgs/FrameGraph"; + public override string RosMessageName => k_RosMessageName; + + public string frame_yaml; + + public FrameGraphResponse() + { + this.frame_yaml = ""; + } + + public FrameGraphResponse(string frame_yaml) + { + this.frame_yaml = frame_yaml; + } + + public static FrameGraphResponse Deserialize(MessageDeserializer deserializer) => new FrameGraphResponse(deserializer); + + private FrameGraphResponse(MessageDeserializer deserializer) + { + deserializer.Read(out this.frame_yaml); + } + + public override void SerializeTo(MessageSerializer serializer) + { + serializer.Write(this.frame_yaml); + } + + public override string ToString() + { + return "FrameGraphResponse: " + + "\nframe_yaml: " + frame_yaml.ToString(); + } + +#if UNITY_EDITOR + [UnityEditor.InitializeOnLoadMethod] +#else + [UnityEngine.RuntimeInitializeOnLoadMethod] +#endif + public static void Register() + { + MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Response); + } + } +} diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/srv/FrameGraphResponse.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/srv/FrameGraphResponse.cs.meta new file mode 100644 index 00000000..dceb4cba --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/srv/FrameGraphResponse.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 7da27b37dd62f4f2ea969cd3ec68dfdb +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Visualization/srv/GetInteractiveMarkersResponse.cs b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Visualization/srv/GetInteractiveMarkersResponse.cs index 1cf3b44b..f36e3a8c 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Visualization/srv/GetInteractiveMarkersResponse.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/Messages/Visualization/srv/GetInteractiveMarkersResponse.cs @@ -62,7 +62,7 @@ public override string ToString() #endif public static void Register() { - MessageRegistry.Register(k_RosMessageName, Deserialize); + MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Response); } } } 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 f873c9bf..57e53c89 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/CoordinateSpaces.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/CoordinateSpaces.cs @@ -20,14 +20,7 @@ public interface CoordinateSpace : ICoordinateSpace { } - /// - /// RUF is the Unity coordinate space, so no conversion needed - /// - /// X axis: Right and South - /// Y axis: Up - /// Z axis: Forward and East - /// - /// + //RUF is the Unity coordinate space, so no conversion needed public class RUF : ICoordinateSpace { public Vector3 ConvertFromRUF(Vector3 v) => v; @@ -36,14 +29,6 @@ public class RUF : ICoordinateSpace public Quaternion ConvertToRUF(Quaternion q) => q; } - /// - /// ROS standard forward, left, up (FLU) coordinate (REP-103) - /// - /// X axis: Forward and East - /// Y axis: Left and North - /// Z axis: Up - /// - /// public class FLU : ICoordinateSpace { public Vector3 ConvertFromRUF(Vector3 v) => new Vector3(v.z, -v.x, v.y); @@ -52,31 +37,21 @@ public class FLU : ICoordinateSpace public Quaternion ConvertToRUF(Quaternion q) => new Quaternion(-q.y, q.z, q.x, -q.w); } - /// - /// Local north, east, down (NED) coordinates for outdoor systems, such as airplane and submarine (REP-103) - /// - /// X axis: North - /// Y axis: East - /// Z axis: Down - /// - /// public class NED : 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) => 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); } - /// - /// Local east, north, up (ENU) coordinates for short-range Cartesian representations of geographic locations (REP-103) - /// - /// X axis: East - /// Y axis: North - /// Z axis: Up - /// - /// - public class ENU : FLU { } + 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 enum CoordinateSpaceSelection { diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/HUDPanel.cs b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/HUDPanel.cs index addb5f52..3725620e 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/HUDPanel.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/HUDPanel.cs @@ -1,324 +1,227 @@ using System; using System.Collections.Generic; +using System.IO; using Unity.Robotics.ROSTCPConnector.MessageGeneration; using UnityEngine; namespace Unity.Robotics.ROSTCPConnector { - public class HUDPanel : MonoBehaviour + public interface IHudTab { - // GUI variables - GUIStyle m_LabelStyle; - GUIStyle m_ConnectionArrowStyle; - GUIStyle m_ContentStyle; - GUIStyle m_MessageStyle; - bool m_ViewSent = false; - bool m_ViewRecv = false; - bool m_ViewSrvs = false; - Rect m_ScrollRect; + string Label { get; } + void OnGUI(HudPanel hud); + void OnSelected(); + void OnDeselected(); + } + + public class HudPanel : MonoBehaviour + { + public static GUIStyle s_BoldStyle; + + // these are static so that anyone can register tabs and windows without needing to worry about whether the hud has been initialized + static SortedList s_HUDTabs = new SortedList(); + static SortedList s_HeaderContents = new SortedList(); + static List s_ActiveWindows = new List(); + static int s_NextWindowID = 101; // ROS Message variables internal bool isEnabled; - internal string host; - MessageViewState m_LastMessageSent; - string m_LastMessageSentMeta = "None"; - float m_LastMessageSentRealtime; + HudWindow m_DraggingWindow; - // For the Hud's IP address field, we store the IP address and port in PlayerPrefs. - // This is used to remember the last IP address the player typed into the HUD, in builds where ConnectOnStart is not checked - public const string PlayerPrefsKey_ROS_IP = "ROS_IP"; - public const string PlayerPrefsKey_ROS_TCP_PORT = "ROS_TCP_PORT"; + IHudTab m_SelectedTab; - public static string RosIPAddressPref + void Awake() { - get => PlayerPrefs.GetString(PlayerPrefsKey_ROS_IP, "127.0.0.1"); + InitStyles(); } - public static int RosPortPref + void OnGUI() { - get => PlayerPrefs.GetInt(PlayerPrefsKey_ROS_TCP_PORT, 10000); - } + if (!isEnabled) + return; - public static void SetIPPref(string ipAddress) - { - PlayerPrefs.SetString(PlayerPrefsKey_ROS_IP, ipAddress); - } + // Initialize main HUD + GUILayout.BeginVertical(GUI.skin.box, GUILayout.Width(300)); - public static void SetPortPref(int port) - { - PlayerPrefs.SetInt(PlayerPrefsKey_ROS_TCP_PORT, port); - } + foreach (Action element in s_HeaderContents.Values) + { + element(); + } + GUILayout.BeginHorizontal(); - public void SetLastMessageSent(string topic, Message message) - { - m_LastMessageSent = new MessageViewState() { label = "Last Message Sent:", message = message }; - m_LastMessageSentMeta = $"{topic} (time: {System.DateTime.Now.TimeOfDay})"; - m_LastMessageSentRealtime = ROSConnection.s_RealTimeSinceStartup; - } + foreach (IHudTab tab in s_HUDTabs.Values) + { + var wasSelected = tab == m_SelectedTab; + var selected = GUILayout.Toggle(wasSelected, tab.Label, GUI.skin.button); + if (selected != wasSelected) + { + if (m_SelectedTab != null) + m_SelectedTab.OnDeselected(); - MessageViewState m_LastMessageReceived; - string m_LastMessageReceivedMeta = "None"; - float m_LastMessageReceivedRealtime; + m_SelectedTab = selected ? tab : null; - public void SetLastMessageReceived(string topic, Message message) - { - m_LastMessageReceived = new MessageViewState() { label = "Last Message Received:", message = message }; - m_LastMessageReceivedMeta = $"{topic} (time: {System.DateTime.Now.TimeOfDay})"; - m_LastMessageReceivedRealtime = Time.realtimeSinceStartup; - } + if (m_SelectedTab != null) + m_SelectedTab.OnSelected(); + } + } - List activeServices = new List(); - MessageViewState lastCompletedServiceRequest = null; - MessageViewState lastCompletedServiceResponse = null; - int nextServiceID = 101; + GUILayout.EndHorizontal(); - public int AddServiceRequest(string topic, Message request) - { - int serviceID = nextServiceID; - nextServiceID++; + if (m_SelectedTab != null) + m_SelectedTab.OnGUI(this); + + GUILayout.EndVertical(); - activeServices.Add(new MessageViewState() + // Draggable windows + var current = Event.current; + if (current.type == EventType.MouseDown) + { + for (var Idx = s_ActiveWindows.Count - 1; Idx >= 0; --Idx) + { + var window = s_ActiveWindows[Idx]; + if (s_ActiveWindows[Idx].TryDragWindow(current)) + { + m_DraggingWindow = s_ActiveWindows[Idx]; + break; + } + } + } + else if (current.type == EventType.MouseDrag && m_DraggingWindow != null) + { + m_DraggingWindow.UpdateDragWindow(current); + } + else if (current.type == EventType.MouseUp && m_DraggingWindow != null) { - serviceID = serviceID, - timestamp = Time.time, - topic = topic, - message = request, - label = $"{topic} Service Requested", - }); - - return serviceID; + m_DraggingWindow.EndDragWindow(); + m_DraggingWindow = null; + } + + foreach (var window in s_ActiveWindows) + window.DrawWindow(this); } - public void AddServiceResponse(int serviceID, Message response) + public static void RegisterTab(IHudTab tab, int index = 0) { - lastCompletedServiceRequest = activeServices.Find(s => s.serviceID == serviceID); - activeServices.Remove(lastCompletedServiceRequest); - - lastCompletedServiceResponse = new MessageViewState() + if (s_HUDTabs.ContainsKey(index)) { - serviceID = serviceID, - timestamp = Time.time, - topic = lastCompletedServiceRequest.topic, - message = response, - label = $"{lastCompletedServiceRequest.topic} Service Response", - }; + Debug.LogWarning($"HUDPanel already contains a tab registered at index {index}! Registering at index {s_HUDTabs.Count} instead."); + index = s_HUDTabs.Count; + } + + s_HUDTabs.Add(index, tab); } - void Awake() + public static void RegisterHeader(Action headerContent, int index = 0) { - // Define font styles - m_LabelStyle = new GUIStyle + if (s_HeaderContents.ContainsKey(index)) { - alignment = TextAnchor.MiddleLeft, - normal = { textColor = Color.white }, - fontStyle = FontStyle.Bold, - fixedWidth = 250 - }; + Debug.LogWarning($"HUDPanel already contains a header registered at index {index}! Registering at index {s_HeaderContents.Count} instead."); + index = s_HeaderContents.Count; + } - m_ConnectionArrowStyle = new GUIStyle - { - alignment = TextAnchor.MiddleLeft, - normal = { textColor = Color.white }, - fontSize = 22, - fontStyle = FontStyle.Bold, - fixedWidth = 250 - }; + s_HeaderContents.Add(index, headerContent); + } - m_ContentStyle = new GUIStyle - { - alignment = TextAnchor.MiddleLeft, - padding = new RectOffset(10, 0, 0, 5), - normal = { textColor = Color.white }, - fixedWidth = 300 - }; + public static void AddWindow(HudWindow window) + { + s_ActiveWindows.Add(window); + } + + public static void RemoveWindow(HudWindow window) + { + s_ActiveWindows.Remove(window); + } - m_MessageStyle = new GUIStyle + public static int GetNextWindowID() + { + var result = s_NextWindowID; + s_NextWindowID++; + return result; + } + + void InitStyles() + { + // Define font styles + s_BoldStyle = new GUIStyle { alignment = TextAnchor.MiddleLeft, - padding = new RectOffset(10, 0, 5, 5), normal = { textColor = Color.white }, - fixedWidth = 300, - wordWrap = true + fontStyle = FontStyle.Bold, }; - - m_ScrollRect = new Rect(); } - - Color GetConnectionColor(float elapsedTime) + public static Rect GetDefaultWindowRect() { - var bright = new Color(1, 1, 0.5f); - var mid = new Color(0, 1, 1); - var dark = new Color(0, 0.5f, 1); - const float brightDuration = 0.03f; - const float fadeToDarkDuration = 1.0f; - - if (!ROSConnection.instance.HasConnectionThread) - return Color.gray; - if (ROSConnection.instance.HasConnectionError) - return Color.red; - if (elapsedTime <= brightDuration) - return bright; - else - return Color.Lerp(mid, dark, elapsedTime / fadeToDarkDuration); + return new Rect(450, 0, 300, 200); } - void OnGUI() + public Rect GetFreeWindowRect() { - if (!isEnabled) - return; - - // Initialize main HUD - GUILayout.BeginVertical(GUI.skin.box, GUILayout.Width(300)); + var xQueue = new Queue(); + var yQueue = new Queue(); + yQueue.Enqueue(GetDefaultWindowRect()); - // ROS IP Setup - GUILayout.BeginHorizontal(); - Color baseColor = GUI.color; - GUI.color = Color.white; - GUI.Label(new Rect(4, 5, 25, 15), "I", m_ConnectionArrowStyle); - GUI.color = GetConnectionColor(Time.realtimeSinceStartup - m_LastMessageReceivedRealtime); - GUI.Label(new Rect(8, 6, 25, 15), "\u2190", m_ConnectionArrowStyle); - GUI.color = GetConnectionColor(Time.realtimeSinceStartup - m_LastMessageSentRealtime); - GUI.Label(new Rect(8, 0, 25, 15), "\u2192", m_ConnectionArrowStyle); - GUI.color = baseColor; - -#if ROS2 - string protocolName = "ROS2"; -#else - string protocolName = "ROS"; -#endif - - GUILayout.Space(30); - GUILayout.Label($"{protocolName} IP: ", m_LabelStyle, GUILayout.Width(100)); - - if (!ROSConnection.instance.HasConnectionThread) + while (yQueue.Count > 0 || xQueue.Count > 0) { - // if you've never run a build on this machine before, initialize the playerpref settings to the ones from the RosConnection - if (!PlayerPrefs.HasKey(PlayerPrefsKey_ROS_IP)) - SetIPPref(ROSConnection.instance.RosIPAddress); - if (!PlayerPrefs.HasKey(PlayerPrefsKey_ROS_TCP_PORT)) - SetPortPref(ROSConnection.instance.RosPort); - - // NB, here the user is editing the PlayerPrefs values, not the ones in the RosConnection. - // (So that the hud remembers what IP you used last time you ran this build.) - // The RosConnection receives the edited values when you click Connect. - SetIPPref(GUILayout.TextField(RosIPAddressPref)); - SetPortPref(Convert.ToInt32(GUILayout.TextField(RosPortPref.ToString()))); - - GUILayout.EndHorizontal(); - GUILayout.Label("(Not connected)"); - if (GUILayout.Button("Connect")) - ROSConnection.instance.Connect(RosIPAddressPref, RosPortPref); - } - else - { - GUILayout.Label(host, m_ContentStyle); - GUILayout.EndHorizontal(); - } + var testRect = xQueue.Count > 0 ? xQueue.Dequeue() : yQueue.Dequeue(); + if (testRect.xMax > Screen.width || testRect.yMax > Screen.height) + continue; - // Last message sent - GUILayout.Label("Last Message Sent:", m_LabelStyle); - GUILayout.Label(m_LastMessageSentMeta, m_ContentStyle); - if (m_LastMessageSent != null) - m_ViewSent = GUILayout.Toggle(m_ViewSent, "View contents"); + float maxX, maxY; + if (IsFreeWindowRect(testRect, out maxX, out maxY)) + return testRect; - // Last message received - GUILayout.Label("Last Message Received:", m_LabelStyle); - GUILayout.Label(m_LastMessageReceivedMeta, m_ContentStyle); - if (m_LastMessageReceived != null) - m_ViewRecv = GUILayout.Toggle(m_ViewRecv, "View contents"); - - GUILayout.Label($"{activeServices.Count} Active Service Requests:", m_LabelStyle); - if (activeServices.Count > 0) - { - var dots = new String('.', (int)Time.time % 4); - GUILayout.Label($"Waiting for service response{dots}", m_ContentStyle); + xQueue.Enqueue(new Rect(maxX, testRect.y, testRect.width, testRect.height)); + yQueue.Enqueue(new Rect(testRect.x, maxY, testRect.width, testRect.height)); } - m_ViewSrvs = GUILayout.Toggle(m_ViewSrvs, "View services status"); - - GUILayout.EndVertical(); - - // Update length of scroll - if (GUILayoutUtility.GetLastRect().height > 1 && GUILayoutUtility.GetLastRect().width > 1) - m_ScrollRect = GUILayoutUtility.GetLastRect(); - - // Optionally show message contents - float y = m_ScrollRect.yMax; - if (m_ViewSent) - { - y = ShowMessage(m_LastMessageSent, y); - } + return GetDefaultWindowRect(); + } - if (m_ViewRecv) - { - y = ShowMessage(m_LastMessageReceived, y); - } + public bool IsFreeWindowRect(Rect rect) + { + foreach (var window in s_ActiveWindows) + if (window.WindowRect.Overlaps(rect)) + return false; + return true; + } - if (m_ViewSrvs) + public bool IsFreeWindowRect(Rect rect, out float maxX, out float maxY) + { + maxX = 0; + maxY = 0; + var result = true; + foreach (var window in s_ActiveWindows) { - foreach (MessageViewState service in activeServices) + if (window.WindowRect.Overlaps(rect)) { - y = ShowMessage(service, y, showElapsedTime: true); - } - - if (lastCompletedServiceRequest != null && lastCompletedServiceResponse != null) - { - y = ShowMessage(lastCompletedServiceRequest, y); - y = ShowMessage(lastCompletedServiceResponse, y); + maxX = Mathf.Max(maxX, window.WindowRect.xMax); + maxY = Mathf.Max(maxY, window.WindowRect.yMax); + result = false; } } + + return result; } - /// - /// All the information necessary to display a message and remember its scroll position - /// - class MessageViewState +#if UNITY_EDITOR + // automatically clear all static content on pressing play + [UnityEditor.InitializeOnLoadMethod] + static void InitializeOnLoad() { - public string label; - public int serviceID; - public float timestamp; - public string topic; - public Message message; - public Rect contentRect; - public Vector2 scrollPosition; + UnityEditor.EditorApplication.playModeStateChanged += OnPlayMode; } - /// - /// Displays a MessageViewState - /// - /// The message view to draw - /// The Y position to draw at - /// Whether to add elapsed time to the title - /// The new Y position to draw at - float ShowMessage(MessageViewState msgView, float y, bool showElapsedTime = false) + static void OnPlayMode(UnityEditor.PlayModeStateChange change) { - if (msgView == null) - return y; - - // Start scrollviews - float height = msgView.contentRect.height > 0 ? Mathf.Min(msgView.contentRect.height, 200) : 200; - Rect panelRect = new Rect(0, y + 5, 325, height); - msgView.scrollPosition = GUI.BeginScrollView(panelRect, msgView.scrollPosition, msgView.contentRect); - - GUILayout.BeginVertical("box"); - - // Paste contents of message - if (showElapsedTime) - GUILayout.Label($"{msgView.label} ({Time.time - msgView.timestamp})", m_LabelStyle); - else - GUILayout.Label(msgView.label, m_LabelStyle); - GUILayout.Label(msgView.message.ToString(), m_MessageStyle); - - GUILayout.EndVertical(); - GUI.EndScrollView(); - - // Update size of internal rect view - if (GUILayoutUtility.GetLastRect().height > 1 && GUILayoutUtility.GetLastRect().width > 1) - msgView.contentRect = GUILayoutUtility.GetLastRect(); - - return panelRect.yMax; + if (change == UnityEditor.PlayModeStateChange.ExitingEditMode) + { + s_HUDTabs.Clear(); + s_HeaderContents.Clear(); + s_ActiveWindows.Clear(); + } } +#endif } } diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/HudWindow.cs b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/HudWindow.cs new file mode 100644 index 00000000..38c8c26e --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/HudWindow.cs @@ -0,0 +1,159 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using UnityEngine; + +namespace Unity.Robotics.ROSTCPConnector +{ + public interface IMenuFiller + { + void AddItem(GUIContent name, bool selected, System.Action callback); + } + + public class HudWindow + { + const float c_DraggableSize = 8; + Rect m_WindowRect; + public Rect WindowRect => m_WindowRect; + string m_Title; + bool m_IsActive; + public bool IsActive => m_IsActive; + bool m_AutoLayout; + bool m_DraggingBottom; + bool m_DraggingLeft; + bool m_DraggingRight; + bool m_DraggingTitle; + Vector2 m_DragMouseOffset; + Vector2 m_ContentScrollPosition; + System.Action m_GuiDrawer; + System.Action m_GuiMenu; + int m_WindowID; + + public HudWindow(string title, Rect rect) + { + m_Title = title; + m_WindowID = HudPanel.GetNextWindowID(); + m_IsActive = true; + m_AutoLayout = false; + m_WindowRect = rect; + } + + public HudWindow(string title) + { + m_Title = title; + m_WindowID = HudPanel.GetNextWindowID(); + m_IsActive = true; + m_AutoLayout = true; + } + + public void SetActive(bool active) + { + m_IsActive = active; + } + + public bool HasActiveRect => m_IsActive && !m_AutoLayout; + + public void SetOnGUI(Action guiDrawer) + { + m_GuiDrawer = guiDrawer; + } + + public void SetGUIMenu(Action guiMenu) + { + m_GuiMenu = guiMenu; + } + + public void DrawWindow(HudPanel hud) + { + if (m_IsActive && m_GuiDrawer != null) + { + if (m_AutoLayout) + { + m_WindowRect = hud.GetFreeWindowRect(); + m_AutoLayout = false; + } + GUI.Window(m_WindowID, m_WindowRect, DrawWindowContents, m_Title); + } + } + + +#if UNITY_EDITOR + class EditorMenuBuilder : IMenuFiller + { + UnityEditor.GenericMenu m_Menu; + public EditorMenuBuilder() + { + m_Menu = new UnityEditor.GenericMenu(); + } + + public void AddItem(GUIContent name, bool selected, Action callback) + { + m_Menu.AddItem(name, selected, ()=>callback()); + } + + public void Show(Vector2 position) + { + m_Menu.DropDown(new Rect(position.x, position.y, 0f, 0f)); + } + } +#endif + + void DrawWindowContents(int id) + { + m_ContentScrollPosition = GUILayout.BeginScrollView(m_ContentScrollPosition); + m_GuiDrawer(); + GUILayout.EndScrollView(); + if (m_GuiMenu != null) + { + Rect scrollRect = GUILayoutUtility.GetLastRect(); + if (GUI.Button(new Rect(scrollRect.xMin - 5, scrollRect.yMin - 17, 20, 20), "\u2630")) + { +#if UNITY_EDITOR + EditorMenuBuilder menuBuilder = new EditorMenuBuilder(); + m_GuiMenu(menuBuilder); + menuBuilder.Show(new Vector2(scrollRect.xMin, scrollRect.yMin+70)); +#endif + } + } + } + + public bool TryDragWindow(Event current) + { + var expandedWindowMenu = new Rect(m_WindowRect.x - c_DraggableSize, m_WindowRect.y, m_WindowRect.width + c_DraggableSize * 2, m_WindowRect.height + c_DraggableSize); + if (expandedWindowMenu.Contains(current.mousePosition)) + { + m_DraggingTitle = current.mousePosition.y < m_WindowRect.yMin + c_DraggableSize * 2; + m_DraggingLeft = current.mousePosition.x < m_WindowRect.xMin + c_DraggableSize; + m_DraggingRight = current.mousePosition.x > m_WindowRect.xMax - c_DraggableSize; + m_DraggingBottom = current.mousePosition.y > m_WindowRect.yMax - c_DraggableSize; + } + + m_DragMouseOffset = current.mousePosition - new Vector2(m_WindowRect.xMin, m_WindowRect.yMin); + return m_DraggingTitle || m_DraggingLeft || m_DraggingRight || m_DraggingBottom; + } + + public void UpdateDragWindow(Event current) + { + if (m_DraggingTitle) + { + m_WindowRect.x = current.mousePosition.x - m_DragMouseOffset.x; + m_WindowRect.y = current.mousePosition.y - m_DragMouseOffset.y; + } + else + { + if (m_DraggingLeft) + m_WindowRect.xMin = current.mousePosition.x; + if (m_DraggingBottom) + m_WindowRect.yMax = current.mousePosition.y; + if (m_DraggingRight) + m_WindowRect.xMax = current.mousePosition.x; + } + } + + public void EndDragWindow() + { + m_DraggingTitle = m_DraggingLeft = m_DraggingRight = m_DraggingBottom = false; + } + + } +} diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/HudWindow.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/HudWindow.cs.meta new file mode 100644 index 00000000..6bc12a49 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/HudWindow.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 079abb065905cb142b6daf13e36a8c5b +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/OutgoingMessageSender.cs b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/OutgoingMessageSender.cs index 8e66bed0..68d3f523 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/OutgoingMessageSender.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/OutgoingMessageSender.cs @@ -25,11 +25,11 @@ public enum SendToState */ public class SysCommandSender : OutgoingMessageSender { - private List m_ListOfSerializations; + List m_ListOfSerializations; - public SysCommandSender(List m_ListOfSerializations) + public SysCommandSender(List listOfSerializations) { - this.m_ListOfSerializations = m_ListOfSerializations; + m_ListOfSerializations = listOfSerializations; } public override SendToState SendInternal(MessageSerializer m_MessageSerializer, Stream stream) 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 b47991a5..9f65704a 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSConnection.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSConnection.cs @@ -9,6 +9,7 @@ using UnityEngine.Serialization; using System.Collections.Concurrent; using System.Threading; +using System.Linq; namespace Unity.Robotics.ROSTCPConnector { @@ -44,20 +45,26 @@ public class ROSConnection : MonoBehaviour float m_SleepTimeSeconds = 0.01f; public float SleepTimeSeconds { get => m_SleepTimeSeconds; set => m_SleepTimeSeconds = value; } + [SerializeField] [FormerlySerializedAs("showHUD")] bool m_ShowHUD = true; public bool ShowHud { get => m_ShowHUD; set => m_ShowHUD = value; } + [SerializeField] + string[] m_TFTopics = { "/tf" }; + public string[] TFTopics { get => m_TFTopics; set => m_TFTopics = value; } + const int k_DefaultPublisherQueueSize = 10; const bool k_DefaultPublisherLatch = false; // GUI window variables - internal HUDPanel m_HudPanel = null; + internal HudPanel m_HudPanel = null; + public HudPanel HUDPanel => m_HudPanel; class OutgoingMessageQueue { - private ConcurrentQueue m_OutgoingMessageQueue; + ConcurrentQueue m_OutgoingMessageQueue; public readonly ManualResetEvent NewMessageReadyToSendEvent; public OutgoingMessageQueue() @@ -78,11 +85,10 @@ public bool TryDequeue(out OutgoingMessageSender outgoingMessageSender) } } - private OutgoingMessageQueue m_OutgoingMessageQueue = new OutgoingMessageQueue(); + OutgoingMessageQueue m_OutgoingMessageQueue = new OutgoingMessageQueue(); ConcurrentQueue> m_IncomingMessages = new ConcurrentQueue>(); CancellationTokenSource m_ConnectionThreadCancellation; - public bool HasConnectionThread => m_ConnectionThreadCancellation != null; static bool m_HasConnectionError = false; @@ -92,77 +98,163 @@ public bool TryDequeue(out OutgoingMessageSender outgoingMessageSender) public static float s_RealTimeSinceStartup = 0.0f; readonly object m_ServiceRequestLock = new object(); + int m_NextSrvID = 101; Dictionary m_ServicesWaiting = new Dictionary(); - struct SubscriberCallback - { - public Func deserialize; - public string rosMessageName; - public List> callbacks; - } + public bool listenForTFMessages = true; + + float m_LastMessageReceivedRealtime; + float m_LastMessageSentRealtime; + public float LastMessageReceivedRealtime => m_LastMessageReceivedRealtime; + public float LastMessageSentRealtime => m_LastMessageSentRealtime; - Dictionary m_Subscribers = new Dictionary(); + // For the IP address field we show in the hud, we store the IP address and port in PlayerPrefs. + // This is used to remember the last IP address the player typed into the HUD, in builds where ConnectOnStart is not checked + public const string PlayerPrefsKey_ROS_IP = "ROS_IP"; + public const string PlayerPrefsKey_ROS_TCP_PORT = "ROS_TCP_PORT"; + public static string RosIPAddressPref => PlayerPrefs.GetString(PlayerPrefsKey_ROS_IP, "127.0.0.1"); + public static int RosPortPref => PlayerPrefs.GetInt(PlayerPrefsKey_ROS_TCP_PORT, 10000); - struct UnityServiceImplementation + public bool HasSubscriber(string topic) { - public Func deserialize; - public string rosMessageName; - public Func callback; + RosTopicState info; + return m_Topics.TryGetValue(topic, out info) && info.HasSubscriberCallback; } - object m_DictionaryLock = new object(); - Dictionary m_UnityServices = new Dictionary(); - Dictionary m_Publishers = new Dictionary(); - Dictionary m_RosServices = new Dictionary(); MessageSerializer m_MessageSerializer = new MessageSerializer(); MessageDeserializer m_MessageDeserializer = new MessageDeserializer(); List> m_TopicsListCallbacks = new List>(); List>> m_TopicsAndTypesListCallbacks = new List>>(); + List> m_NewTopicCallbacks = new List>(); - public void Subscribe(string topic, Action callback) where T : Message + Dictionary m_Topics = new Dictionary(); + + public void ListenForTopics(Action callback, bool notifyAllExistingTopics = false) + { + m_NewTopicCallbacks.Add(callback); + if (notifyAllExistingTopics) + { + foreach (RosTopicState state in AllTopics) + { + callback(state); + } + } + } + + RosTopicState AddTopic(string topic, string rosMessageName) { - string rosMessageName = rosMessageName = MessageRegistry.GetRosMessageName(); - AddSubscriberInternal(topic, rosMessageName, callback); + RosTopicState newTopic = new RosTopicState(topic, rosMessageName, this, new InternalAPI(this)); + lock (m_Topics) + { + m_Topics.Add(topic, newTopic); + } + return newTopic; + } - if (HasConnectionThread) - SendSubscriberRegistration(topic, rosMessageName); + public RosTopicState GetTopic(string topic) + { + RosTopicState info; + m_Topics.TryGetValue(topic, out info); + return info; } - void AddSubscriberInternal(string topic, string rosMessageName, Action callback) where T : Message + public IEnumerable AllTopics => m_Topics.Values; + + public RosTopicState GetOrCreateTopic(string topic, string rosMessageName) { - SubscriberCallback subCallbacks; - if (!m_Subscribers.TryGetValue(topic, out subCallbacks)) + RosTopicState state = GetTopic(topic); + if (state != null) { - subCallbacks = new SubscriberCallback + if (state.RosMessageName != rosMessageName) { - deserialize = MessageRegistry.GetDeserializeFunction(), - rosMessageName = rosMessageName, - callbacks = new List> { } - }; - m_Subscribers.Add(topic, subCallbacks); + state.ChangeRosMessageName(rosMessageName); + } + return state; } - subCallbacks.callbacks.Add((Message msg) => + RosTopicState result = AddTopic(topic, rosMessageName); + foreach (Action callback in m_NewTopicCallbacks) { - callback((T)msg); + callback(result); + } + return result; + } + + public void Subscribe(string topic, Action callback) where T : Message + { + string rosMessageName = MessageRegistry.GetRosMessageName(); + AddSubscriberInternal(topic, rosMessageName, (Message msg) => + { + if (msg.RosMessageName == rosMessageName) + { + callback((T)msg); + } + else + { + Debug.LogError($"Subscriber to '{topic}' expected '{rosMessageName}' but received '{msg.RosMessageName}'!?"); + } }); } - public void ImplementService(string topic, Func callback) where REQUEST : Message + public void Unsubscribe(string topic) { - string rosMessageName = rosMessageName = MessageRegistry.GetRosMessageName(); - m_UnityServices[topic] = new UnityServiceImplementation + RosTopicState info = GetTopic(topic); + if (info != null) + info.UnsubscribeAll(); + } + + // Version for when the message type is unknown at compile time + public void SubscribeByMessageName(string topic, string rosMessageName, Action callback) + { + var constructor = MessageRegistry.GetDeserializeFunction(rosMessageName); + if (constructor == null) { - deserialize = MessageRegistry.GetDeserializeFunction(), - rosMessageName = rosMessageName, - callback = (Message msg) => callback((REQUEST)msg) - }; + Debug.LogError($"Failed to subscribe to topic {topic} - no class has RosMessageName \"{rosMessageName}\"!"); + return; + } - if (HasConnectionThread) - SendUnityServiceRegistration(topic, rosMessageName); + AddSubscriberInternal(topic, rosMessageName, callback); } + void AddSubscriberInternal(string topic, string rosMessageName, Action callback) + { + RosTopicState info; + if (!m_Topics.TryGetValue(topic, out info)) + { + info = AddTopic(topic, rosMessageName); + } + + info.AddSubscriber(callback); + + foreach (Action topicCallback in m_NewTopicCallbacks) + { + topicCallback(info); + } + } + + // Implement a service in Unity + public void ImplementService(string topic, Func callback, int? queueSize = null) + where TRequest : Message + { + string rosMessageName = rosMessageName = MessageRegistry.GetRosMessageName(); + + RosTopicState info; + if (!m_Topics.TryGetValue(topic, out info)) + { + info = AddTopic(topic, rosMessageName); + } + + int resolvedQueueSize = queueSize.GetValueOrDefault(k_DefaultPublisherQueueSize); + info.ImplementService((Message msg) => callback((TRequest)msg), resolvedQueueSize); + + foreach (Action topicCallback in m_NewTopicCallbacks) + { + topicCallback(info); + } + } + + // Send a request to a ros service public async void SendServiceMessage(string rosServiceName, Message serviceRequest, Action callback) where RESPONSE : Message, new() { RESPONSE response = await SendServiceMessage(rosServiceName, serviceRequest); @@ -176,6 +268,7 @@ public void ImplementService(string topic, Func callb } } + // Send a request to a ros service public async Task SendServiceMessage(string rosServiceName, Message serviceRequest) where RESPONSE : Message, new() { m_MessageSerializer.Clear(); @@ -190,8 +283,8 @@ public void ImplementService(string topic, Func callb m_ServicesWaiting.Add(srvID, pauser); } - SendSysCommand(SysCommand.k_SysCommand_ServiceRequest, new SysCommand_Service { srv_id = srvID }); - Publish(rosServiceName, serviceRequest); + RosTopicState topicState = GetOrCreateTopic(rosServiceName, serviceRequest.RosMessageName); + topicState.SendServiceRequest(serviceRequest, srvID); byte[] rawResponse = (byte[])await pauser.PauseUntilResumed(); @@ -216,82 +309,33 @@ public void RegisterSubscriber(string topic, string rosMessageName) { } - public bool TryGetPublisher(out ROSPublisher result, string rosTopicName, + public RosTopicState RegisterPublisher(string rosTopicName, int? queue_size = null, bool? latch = null) where T : Message { - ROSPublisherBase publisher; - lock (m_DictionaryLock) - { - if (m_Publishers.TryGetValue(rosTopicName, out publisher)) - { - - result = (ROSPublisher)publisher; - if (result == null) - { - Debug.LogError($"Existing publisher with topic {rosTopicName} is null!"); - return false; - } - - string messageName = MessageRegistry.GetRosMessageName(); - if (publisher.EquivalentTo(rosTopicName, messageName, queue_size, latch)) - { - //We already have a valid existing publisher of the correct type. - return true; - } - else - { - string errorMessage = $"Publisher on topic {rosTopicName} has changed type! " + - $"Do you have multiple publishers on the same topic?"; - Debug.LogError(errorMessage); - return false; - } - } - else - { - //No publisher already exists. - result = null; - return false; - } - } + return RegisterPublisher(rosTopicName, MessageRegistry.GetRosMessageName(), queue_size, latch); } - public ROSPublisher RegisterPublisher(string rosTopicName, - int? queue_size = null, bool? latch = null) where T : Message + public RosTopicState RegisterPublisher(string rosTopicName, string rosMessageName, + int? queueSize = null, bool? latch = null) { - bool correctPublisherAlreadyExists = TryGetPublisher(out ROSPublisher result, rosTopicName, queue_size, latch); - if (correctPublisherAlreadyExists) - { - Debug.LogWarning($"Publisher for topic {rosTopicName} registered twice!"); - return result; - } - else - { - if (result == null) - { - //Create a new publisher. - int resolvedQueueSize = queue_size.GetValueOrDefault(k_DefaultPublisherQueueSize); - bool resolvedLatch = latch.GetValueOrDefault(k_DefaultPublisherLatch); - result = new ROSPublisher(rosTopicName, resolvedQueueSize, resolvedLatch); - m_Publishers[rosTopicName] = result; - } - else - { - throw new Exception("Failed to register publisher!"); - } - } - return result; + RosTopicState topicState = GetOrCreateTopic(rosTopicName, rosMessageName); + //Create a new publisher. + int resolvedQueueSize = queueSize.GetValueOrDefault(k_DefaultPublisherQueueSize); + bool resolvedLatch = latch.GetValueOrDefault(k_DefaultPublisherLatch); + topicState.RegisterPublisher(resolvedQueueSize, resolvedLatch); + return topicState; } - public void RegisterRosService(string topic) where T : Message + public void RegisterRosService(string topic) where TRequest : Message where TResponse : Message { - RegisterRosService(topic, MessageRegistry.GetRosMessageName()); + RegisterRosService(topic, MessageRegistry.GetRosMessageName(), MessageRegistry.GetRosMessageName()); } - public void RegisterRosService(string topic, string rosMessageName) + public void RegisterRosService(string topic, string requestMessageName, string responseMessageName, int? queueSize = null) { - m_RosServices[topic] = rosMessageName; - if (HasConnectionThread) - SendRosServiceRegistration(topic, rosMessageName); + RosTopicState info = GetOrCreateTopic(topic, requestMessageName); + int resolvedQueueSize = queueSize.GetValueOrDefault(k_DefaultPublisherQueueSize); + info.RegisterRosService(responseMessageName, resolvedQueueSize); } [Obsolete("Calling ImplementUnityService now implicitly registers it")] @@ -299,48 +343,103 @@ public void RegisterUnityService(string topic, string rosMessageName) { } - - void SendSubscriberRegistration(string topic, string rosMessageName, NetworkStream stream = null) + internal struct InternalAPI { - SendSysCommand(SysCommand.k_SysCommand_Subscribe, new SysCommand_TopicAndType { topic = topic, message_name = rosMessageName }, stream); - } + ROSConnection m_Self; - void SendRosServiceRegistration(string topic, string rosMessageName, NetworkStream stream = null) - { - SendSysCommand(SysCommand.k_SysCommand_RosService, new SysCommand_TopicAndType { topic = topic, message_name = rosMessageName }, stream); + public InternalAPI(ROSConnection self) { m_Self = self; } + + public MessageDeserializer Deserializer => m_Self.m_MessageDeserializer; + + public void SendSubscriberRegistration(string topic, string rosMessageName, NetworkStream stream = null) + { + m_Self.SendSysCommand(SysCommand.k_SysCommand_Subscribe, new SysCommand_TopicAndType { topic = topic, message_name = rosMessageName }, stream); + } + + public void SendRosServiceRegistration(string topic, string rosMessageName, NetworkStream stream = null) + { + m_Self.SendSysCommand(SysCommand.k_SysCommand_RosService, new SysCommand_TopicAndType { topic = topic, message_name = rosMessageName }, stream); + } + + public void SendUnityServiceRegistration(string topic, string rosMessageName, NetworkStream stream = null) + { + m_Self.SendSysCommand(SysCommand.k_SysCommand_UnityService, new SysCommand_TopicAndType { topic = topic, message_name = rosMessageName }, stream); + } + + public void SendSubscriberUnregistration(string topic, NetworkStream stream = null) + { + m_Self.SendSysCommand(SysCommand.k_SysCommand_RemoveSubscriber, new SysCommand_Topic { topic = topic }, stream); + } + + public void SendPublisherUnregistration(string topic, NetworkStream stream = null) + { + m_Self.SendSysCommand(SysCommand.k_SysCommand_RemovePublisher, new SysCommand_Topic { topic = topic }, stream); + } + + public void SendRosServiceUnregistration(string topic, NetworkStream stream = null) + { + m_Self.SendSysCommand(SysCommand.k_SysCommand_RemoveRosService, new SysCommand_Topic { topic = topic }, stream); + } + + public void SendUnityServiceUnregistration(string topic, NetworkStream stream = null) + { + m_Self.SendSysCommand(SysCommand.k_SysCommand_RemoveUnityService, new SysCommand_Topic { topic = topic }, stream); + } + + public void SendUnityServiceResponse(int serviceId, NetworkStream stream = null) + { + m_Self.SendSysCommand(SysCommand.k_SysCommand_ServiceResponse, new SysCommand_Service { srv_id = serviceId }, stream); + } + + public void SendPublisherRegistration(string topic, string message_name, int queueSize, bool latch, NetworkStream stream = null) + { + m_Self.SendSysCommand(SysCommand.k_SysCommand_Publish, + new SysCommand_PublisherRegistration { topic = topic, message_name = message_name, queue_size = queueSize, latch = latch } + ); + } + + public void SendServiceRequest(int serviceId) + { + m_Self.SendSysCommand(SysCommand.k_SysCommand_ServiceRequest, new SysCommand_Service { srv_id = serviceId }); + } + + public void AddSenderToQueue(OutgoingMessageSender sender) + { + m_Self.m_OutgoingMessageQueue.Enqueue(sender); + } } - void SendUnityServiceRegistration(string topic, string rosMessageName, NetworkStream stream = null) + static ROSConnection _instance; + + public static ROSConnection GetOrCreateInstance() { - SendSysCommand(SysCommand.k_SysCommand_UnityService, new SysCommand_TopicAndType { topic = topic, message_name = rosMessageName }, stream); + if (_instance == null) + { + GameObject prefab = Resources.Load("ROSConnectionPrefab"); + if (prefab == null) + { + Debug.LogWarning("No settings for ROSConnection.instance! Open \"ROS Settings\" from the Robotics menu to configure it."); + GameObject instance = new GameObject("ROSConnection"); + _instance = instance.AddComponent(); + } + else + { + _instance = Instantiate(prefab).GetComponent(); + } + } + return _instance; } - private static ROSConnection _instance; + [Obsolete("Please call ROSConnection.GetOrCreateInstance()")] public static ROSConnection instance { get { - if (_instance == null) - { - GameObject prefab = Resources.Load("ROSConnectionPrefab"); - if (prefab == null) - { - Debug.LogWarning( - "No settings for ROSConnection.instance! Open \"ROS Settings\" from the Robotics menu to configure it."); - GameObject instance = new GameObject("ROSConnection"); - _instance = instance.AddComponent(); - } - else - { - _instance = Instantiate(prefab).GetComponent(); - } - } - - return _instance; + return GetOrCreateInstance(); } } - void OnEnable() + void Awake() { if (_instance == null) _instance = this; @@ -350,10 +449,13 @@ void Start() { InitializeHUD(); + HudPanel.RegisterHeader(DrawHeaderGUI); + + if (listenForTFMessages) + TFSystem.GetOrCreateInstance(); + if (ConnectOnStart) - { Connect(); - } } public void Connect(string ipAddress, int port) @@ -368,9 +470,6 @@ public void Connect() if (!IPFormatIsCorrect(RosIPAddress)) Debug.LogWarning("Invalid ROS IP address: " + RosIPAddress); - if (m_HudPanel != null) - m_HudPanel.host = $"{RosIPAddress}:{RosPort}"; - m_ConnectionThreadCancellation = new CancellationTokenSource(); Task.Run(() => ConnectionThread( @@ -379,62 +478,41 @@ public void Connect() m_NetworkTimeoutSeconds, m_KeepaliveTime, (int)(m_SleepTimeSeconds * 1000.0f), - RegisterAll, - DeregisterAll, + OnConnectionStartedCallback, + OnConnectionLostCallback, m_OutgoingMessageQueue, m_IncomingMessages, m_ConnectionThreadCancellation.Token )); } - void RegisterAll(NetworkStream stream) + // NB this callback is not running on the main thread, be cautious about modifying data here + void OnConnectionStartedCallback(NetworkStream stream) { - lock (m_DictionaryLock) + RosTopicState[] topics; + lock (m_Topics) { - foreach (var keyValue in m_Subscribers) - { - if (keyValue.Value.rosMessageName != null) - { - SendSubscriberRegistration(keyValue.Key, keyValue.Value.rosMessageName, stream); - } - } - - foreach (var keyValue in m_UnityServices) - { - if (keyValue.Value.rosMessageName != null) - { - SendUnityServiceRegistration(keyValue.Key, keyValue.Value.rosMessageName, stream); - } - } + topics = AllTopics.ToArray(); + } - foreach (var keyValue in m_Publishers) - { - if (keyValue.Value != null) - { - keyValue.Value.OnConnectionEstablished(m_MessageSerializer, stream); - } - } + foreach (RosTopicState topicInfo in m_Topics.Values.ToArray()) + topicInfo.OnConnectionEstablished(stream); - foreach (var keyValue in m_RosServices) - { - if (keyValue.Value != null) - { - SendRosServiceRegistration(keyValue.Key, keyValue.Value, stream); - } - } - } + RefreshTopicsList(); } - void DeregisterAll() + void OnConnectionLostCallback() { - lock (m_DictionaryLock) + RosTopicState[] topics; + lock (m_Topics) { - foreach (var keyValue in m_Publishers) - { - //For all publishers, notify that they need to re-register. - if (keyValue.Value != null) - keyValue.Value.PublisherRegistered = false; - } + topics = AllTopics.ToArray(); + } + + foreach (RosTopicState topicInfo in topics) + { + //For all publishers, notify that they need to re-register. + topicInfo.OnConnectionLost(); } } @@ -448,21 +526,12 @@ public void Disconnect() void OnValidate() { - InitializeHUD(); - } - - private void InitializeHUD() - { - if (!Application.isPlaying || (!m_ShowHUD && m_HudPanel == null)) + // the prefab is not the instance! + if (gameObject.scene.name == null) return; - if (m_HudPanel == null) - { - m_HudPanel = gameObject.AddComponent(); - m_HudPanel.host = $"{RosIPAddress}:{RosPort}"; - } - - m_HudPanel.isEnabled = m_ShowHUD; + if (_instance == null) + _instance = this; } Action m_SpecialIncomingMessageHandler; @@ -475,6 +544,7 @@ void Update() while (m_IncomingMessages.TryDequeue(out data)) { (string topic, byte[] contents) = data; + m_LastMessageReceivedRealtime = Time.realtimeSinceStartup; if (m_SpecialIncomingMessageHandler != null) { @@ -486,20 +556,36 @@ void Update() } else { - // notify whatever is interested in this incoming message - SubscriberCallback callback; - if (m_Subscribers.TryGetValue(topic, out callback)) - { - m_MessageDeserializer.InitWithBuffer(contents); - Message message = callback.deserialize(m_MessageDeserializer); + RosTopicState topicInfo = GetTopic(topic); + // 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); + } + } + } - if (m_HudPanel != null && !topic.StartsWith("__")) - m_HudPanel.SetLastMessageReceived(topic, message); + float m_LastTopicsRequestRealtime = -1; + const float k_TimeBetweenTopicsUpdates = 5.0f; - callback.callbacks.ForEach(item => item(message)); + public void RefreshTopicsList() + { + // cap the rate of requests + if (m_LastTopicsRequestRealtime != -1 && s_RealTimeSinceStartup - m_LastTopicsRequestRealtime <= k_TimeBetweenTopicsUpdates) + return; + + m_LastTopicsRequestRealtime = s_RealTimeSinceStartup; + GetTopicAndTypeList((data) => + { + foreach (KeyValuePair kv in data) + { + RosTopicState state = GetOrCreateTopic(kv.Key, kv.Value); + if (state.RosMessageName != kv.Value) + { + state.ChangeRosMessageName(kv.Value); } } - } + }); } void ReceiveSysCommand(string topic, string json) @@ -528,36 +614,26 @@ void ReceiveSysCommand(string topic, string json) { var serviceCommand = JsonUtility.FromJson(json); - // the next incoming message will be a service request, so set a special callback to process it + // the next incoming message will be a request for a Unity service, so set a special callback to process it m_SpecialIncomingMessageHandler = (string serviceTopic, byte[] requestBytes) => { m_SpecialIncomingMessageHandler = null; - // find the service implementation - UnityServiceImplementation service; - if (!m_UnityServices.TryGetValue(serviceTopic, out service)) + RosTopicState topicState = GetTopic(serviceTopic); + if (topicState == null) { Debug.LogError($"Unity service {serviceTopic} has not been implemented!"); return; } - // deserialize the request message - m_MessageDeserializer.InitWithBuffer(requestBytes); - Message requestMessage = service.deserialize(m_MessageDeserializer); - - // run the actual service - Message responseMessage = service.callback(requestMessage); - - // send the response message back - SendSysCommand(SysCommand.k_SysCommand_ServiceResponse, new SysCommand_Service { srv_id = serviceCommand.srv_id }); - Publish(serviceTopic, responseMessage); + topicState.HandleUnityServiceRequest(requestBytes, serviceCommand.srv_id); }; } break; case SysCommand.k_SysCommand_ServiceResponse: { - // it's a response from a ros service + // the next incoming message will be a response from a ros service var serviceCommand = JsonUtility.FromJson(json); m_SpecialIncomingMessageHandler = (string serviceTopic, byte[] requestBytes) => { @@ -585,8 +661,8 @@ void ReceiveSysCommand(string topic, string json) if (m_TopicsAndTypesListCallbacks.Count > 0) { Dictionary callbackParam = new Dictionary(); - for (int Idx = 0; Idx < topicsResponse.topics.Length; ++Idx) - callbackParam[topicsResponse.topics[Idx]] = topicsResponse.types[Idx]; + for (int idx = 0; idx < topicsResponse.topics.Length; ++idx) + callbackParam[topicsResponse.topics[idx]] = topicsResponse.types[idx]; m_TopicsAndTypesListCallbacks.ForEach(a => a(callbackParam)); m_TopicsAndTypesListCallbacks.Clear(); } @@ -620,7 +696,7 @@ static async Task ConnectionThread( float networkTimeoutSeconds, float keepaliveTime, int sleepMilliseconds, - Action RegisterAll, + Action OnConnectionStartedCallback, Action DeregisterAll, OutgoingMessageQueue outgoingQueue, ConcurrentQueue> incomingQueue, @@ -647,7 +723,7 @@ static async Task ConnectionThread( networkStream.ReadTimeout = (int)(networkTimeoutSeconds * 1000); SendKeepalive(networkStream); - RegisterAll(networkStream); + OnConnectionStartedCallback(networkStream); readerCancellation = new CancellationTokenSource(); _ = Task.Run(() => ReaderThread(nextReaderIdx, networkStream, incomingQueue, sleepMilliseconds, readerCancellation.Token)); @@ -657,7 +733,6 @@ static async Task ConnectionThread( float waitingSinceRealTime = s_RealTimeSinceStartup; while (true) { - bool messageReadyEventWasSet = outgoingQueue.NewMessageReadyToSendEvent.WaitOne(sleepMilliseconds); token.ThrowIfCancellationRequested(); @@ -676,7 +751,6 @@ static async Task ConnectionThread( while (outgoingQueue.TryDequeue(out OutgoingMessageSender sendsOutgoingMessages)) { - OutgoingMessageSender.SendToState sendToState = sendsOutgoingMessages.SendInternal(messageSerializer, networkStream); switch (sendToState) { @@ -733,7 +807,7 @@ static async Task ReaderThread(int readerIdx, NetworkStream networkStream, Concu try { Tuple content = await ReadMessageContents(networkStream, sleepMilliseconds, token); - //Debug.Log($"Message {content.Item1} received"); + // Debug.Log($"Message {content.Item1} received"); ROSConnection.m_HasConnectionError = false; if (content.Item1 != "") // ignore keepalive messages @@ -797,8 +871,6 @@ void OnApplicationQuit() Disconnect(); } - - void SendSysCommand(string command, object param, NetworkStream stream = null) { if (stream != null) @@ -807,7 +879,7 @@ void SendSysCommand(string command, object param, NetworkStream stream = null) QueueSysCommand(command, param); } - private static void PopulateSysCommand(MessageSerializer messageSerializer, string command, object param) + static void PopulateSysCommand(MessageSerializer messageSerializer, string command, object param) { messageSerializer.Clear(); // syscommands are sent as: @@ -836,13 +908,13 @@ public void QueueSysCommand(string command, object param) m_OutgoingMessageQueue.Enqueue(new SysCommandSender(m_MessageSerializer.GetBytesSequence())); } - [ObsoleteAttribute("Use Publish instead of Send", false)] - public void Send(string rosTopicName, T message) where T : Message + [Obsolete("Use Publish instead of Send", false)] + public void Send(string rosTopicName, Message message) { Publish(rosTopicName, message); } - public void Publish(string rosTopicName, T message) where T : Message + public void Publish(string rosTopicName, Message message) { if (rosTopicName.StartsWith("__")) { @@ -850,31 +922,161 @@ public void Publish(string rosTopicName, T message) where T : Message } else { - //Find the publisher and queue the message for sending. - if (TryGetPublisher(out ROSPublisher existingPublisher, rosTopicName)) - { - existingPublisher.PublishInternal(message); - m_OutgoingMessageQueue.Enqueue(existingPublisher); - - if (m_HudPanel != null) - m_HudPanel.SetLastMessageSent(rosTopicName, message); - } - else + RosTopicState rosTopic = GetTopic(rosTopicName); + if (rosTopic == null || !rosTopic.IsPublisher) { - throw new Exception($"No registered publisher on topic {rosTopicName} of type {MessageRegistry.GetRosMessageName()}!"); + throw new Exception($"No registered publisher on topic {rosTopicName} for type {message.RosMessageName}!"); } + + rosTopic.Publish(message); } } - public static T GetFromPool(string rosTopicName) where T : Message + public T GetFromPool(string rosTopicName) where T : Message { - if (instance.TryGetPublisher(out ROSPublisher rosPublisher, rosTopicName)) + RosTopicState topicState = GetTopic(rosTopicName); + if (topicState != null) { - return rosPublisher.GetMessageFromPool(); + 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)) + return; + + if (m_HudPanel == null) + { + m_HudPanel = gameObject.AddComponent(); + } + + m_HudPanel.isEnabled = m_ShowHUD; + } + + void DrawHeaderGUI() + { + GUIStyle labelStyle = new GUIStyle + { + alignment = TextAnchor.MiddleLeft, + normal = { textColor = Color.white }, + fontStyle = FontStyle.Bold, + fixedWidth = 250 + }; + + GUIStyle contentStyle = new GUIStyle + { + alignment = TextAnchor.MiddleLeft, + padding = new RectOffset(10, 0, 0, 5), + normal = { textColor = Color.white }, + fixedWidth = 300 + }; + + + // ROS IP Setup + GUILayout.BeginHorizontal(); + DrawConnectionArrows( + true, + 0, + 0, + Time.realtimeSinceStartup - LastMessageReceivedRealtime, + Time.realtimeSinceStartup - LastMessageSentRealtime, + HasConnectionThread, + HasConnectionThread, + HasConnectionError + ); + +#if ROS2 + string protocolName = "ROS2"; +#else + string protocolName = "ROS"; +#endif + + GUILayout.Space(30); + GUILayout.Label($"{protocolName} IP: ", labelStyle, GUILayout.Width(100)); + + if (!HasConnectionThread) + { + // if you've never run a build on this machine before, initialize the playerpref settings to the ones from the RosConnection + if (!PlayerPrefs.HasKey(PlayerPrefsKey_ROS_IP)) + SetIPPref(RosIPAddress); + if (!PlayerPrefs.HasKey(PlayerPrefsKey_ROS_TCP_PORT)) + SetPortPref(RosPort); + + // NB, here the user is editing the PlayerPrefs values, not the ones in the RosConnection. + // (So that the hud remembers what IP you used last time you ran this build.) + // The RosConnection receives the edited values when you click Connect. + SetIPPref(GUILayout.TextField(RosIPAddressPref)); + SetPortPref(Convert.ToInt32(GUILayout.TextField(RosPortPref.ToString()))); + + GUILayout.EndHorizontal(); + GUILayout.Label("(Not connected)"); + if (GUILayout.Button("Connect")) + Connect(RosIPAddressPref, RosPortPref); + } + else + { + GUILayout.Label($"{RosIPAddress}:{RosPort}", contentStyle); + GUILayout.EndHorizontal(); + } + } + + static GUIStyle s_ConnectionArrowStyle; + + public static void DrawConnectionArrows(bool withBar, float x, float y, float receivedTime, float sentTime, bool isPublisher, bool isSubscriber, bool hasError) + { + if (s_ConnectionArrowStyle == null) + { + s_ConnectionArrowStyle = new GUIStyle + { + alignment = TextAnchor.MiddleLeft, + normal = { textColor = Color.white }, + fontSize = 22, + fontStyle = FontStyle.Bold, + fixedWidth = 250 + }; + } + + var baseColor = GUI.color; + GUI.color = Color.white; + if (withBar) + GUI.Label(new Rect(x + 4, y + 5, 25, 15), "I", s_ConnectionArrowStyle); + GUI.color = GetConnectionColor(receivedTime, isSubscriber, hasError); + GUI.Label(new Rect(x + 8, y + 6, 25, 15), "\u2190", s_ConnectionArrowStyle); + GUI.color = GetConnectionColor(sentTime, isPublisher, hasError); + GUI.Label(new Rect(x + 8, y + 0, 25, 15), "\u2192", s_ConnectionArrowStyle); + GUI.color = baseColor; + } + + public static void SetIPPref(string ipAddress) + { + PlayerPrefs.SetString(PlayerPrefsKey_ROS_IP, ipAddress); + } + + public static void SetPortPref(int port) + { + PlayerPrefs.SetInt(PlayerPrefsKey_ROS_TCP_PORT, port); + } + + public static Color GetConnectionColor(float elapsedTime, bool hasConnection, bool hasError) + { + var bright = new Color(1, 1, 0.5f); + var mid = new Color(0, 1, 1); + var dark = new Color(0, 0.5f, 1); + const float brightDuration = 0.03f; + const float fadeToDarkDuration = 1.0f; + + if (!hasConnection) + return Color.gray; + if (hasError) + return Color.red; + + if (elapsedTime <= brightDuration) + return bright; + return Color.Lerp(mid, dark, elapsedTime / fadeToDarkDuration); + } + public static bool IPFormatIsCorrect(string ipAddress) { if (ipAddress == null || ipAddress == "") diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSPublisher.cs b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSPublisher.cs deleted file mode 100644 index 6736e552..00000000 --- a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSPublisher.cs +++ /dev/null @@ -1,290 +0,0 @@ -using System; -using System.Collections.Generic; -using System.IO; -using System.Threading; -using Unity.Robotics.ROSTCPConnector.MessageGeneration; -using UnityEngine; - -namespace Unity.Robotics.ROSTCPConnector -{ - public abstract class ROSPublisherBase : OutgoingMessageSender - { - public abstract string RosMessageName { get; } - - public string TopicName { get; } - - public int QueueSize { get; } - - public bool Latch { get; } - - public bool PublisherRegistered { get; set; } - - protected ROSPublisherBase(string topicName, int queueSize, bool latch) - { - if (queueSize < 1) - { - throw new Exception("Queue size must be greater than or equal to 1."); - } - - TopicName = topicName; - QueueSize = queueSize; - Latch = latch; - PublisherRegistered = false; - } - - public bool EquivalentTo(string topicName, string messageName, int? queueSize, bool? latch) - { - return TopicName == topicName && RosMessageName == messageName && - (!queueSize.HasValue || QueueSize == queueSize) && - (!latch.HasValue || Latch == latch); - } - - public abstract void OnConnectionEstablished(MessageSerializer m_MessageSerializer, Stream stream); - } - - public class ROSPublisher : ROSPublisherBase where T : Message - { - public override string RosMessageName { get; } - - //Messages waiting to be sent queue. - private LinkedList outgoingMessages = new LinkedList(); - - //Keeps track of how many outgoing messages were removed due to queue overflow. - //If a message is published but the queue is full, the counter is incremented, the - //first message in the queue is recycled and the message to publish is put on the end of the queue. - //If this is non 0, a SendTo call will decrement the counter instead of sending data. - private int queueOverflowUnsentCounter = 0; - - //Latching - This message will be set if latching is enabled, and on reconnection, it will be sent again. - private T lastMessageSent = null; - - //Whether you want to pool messages for reuse (Used to reduce GC calls). - private volatile bool _messagePoolEnabled; - - //Optional, used if you want to pool messages and reuse them when they are no longer in use. - private Queue inactiveMessagePool = new Queue(); - - public ROSPublisher(string topicName, int queueSize, bool latch) : base(topicName, queueSize, latch) - { - this.RosMessageName = MessageRegistry.GetRosMessageName(); - } - - internal void PublishInternal(T message) - { - lock (outgoingMessages) - { - if (outgoingMessages.Count >= QueueSize) - { - //Remove outgoing messages that don't fit in the queue. - //Recycle the message if applicable - RecycleMessageIfApplicable(outgoingMessages.First.Value); - //Update the overflow counter. - queueOverflowUnsentCounter++; - outgoingMessages.RemoveFirst(); - } - - //Add a new valid message to the end. - outgoingMessages.AddLast(message); - } - } - - public override void OnConnectionEstablished(MessageSerializer m_MessageSerializer, Stream stream) - { - //Register the publisher with the ROS Endpoint. - RegisterPublisherIfApplicable(m_MessageSerializer, stream); - } - - private SendToState RemoveMessageToSend(out T messageToSend) - { - SendToState result = SendToState.NoMessageToSendError; - messageToSend = null; - lock (outgoingMessages) - { - if (queueOverflowUnsentCounter > 0) - { - //This means that we can't send message to ROS as fast as we're generating them. - //This could potentially be bad as it means that we are dropping messages! - queueOverflowUnsentCounter--; - messageToSend = null; - result = SendToState.QueueFullWarning; - } - else if (outgoingMessages.Count > 0) - { - //Retrieve the next message and populate messageToSend. - messageToSend = outgoingMessages.First.Value; - outgoingMessages.RemoveFirst(); - result = SendToState.Normal; - } - } - - return result; - } - - public bool PeekNextMessageToSend(out T messageToSend) - { - bool result = false; - messageToSend = null; - lock (outgoingMessages) - { - if (outgoingMessages.Count > 0) - { - messageToSend = outgoingMessages.First.Value; - result = true; - } - } - - - return result; - } - - private void SendMessageWithStream(MessageSerializer m_MessageSerializer, Stream stream, Message message) - { - RegisterPublisherIfApplicable(m_MessageSerializer, stream); - - //Clear the serializer - m_MessageSerializer.Clear(); - //Prepare the data to send. - m_MessageSerializer.Write(TopicName); - m_MessageSerializer.SerializeMessageWithLength(message); - //Send via the stream. - m_MessageSerializer.SendTo(stream); - } - - private void RegisterPublisherIfApplicable(MessageSerializer m_MessageSerializer, Stream stream) - { - if (PublisherRegistered) - { - return; - } - - //Register the publisher before sending anything. - SysCommandPublisherRegistration publisherRegistration = - new SysCommandPublisherRegistration(this); - publisherRegistration.SendTo(stream, m_MessageSerializer); - PublisherRegistered = true; - - if (Latch && lastMessageSent != null) - { - //This topic is latching, so to mimic that functionality, - //here the last sent message is sent again with the new connection. - SendMessageWithStream(m_MessageSerializer, stream, lastMessageSent); - } - } - - public override SendToState SendInternal(MessageSerializer m_MessageSerializer, Stream stream) - { - RegisterPublisherIfApplicable(m_MessageSerializer, stream); - SendToState sendToState = RemoveMessageToSend(out T toSend); - if (sendToState == SendToState.Normal) - { - SendMessageWithStream(m_MessageSerializer, stream, toSend); - - //Recycle the message (if applicable). - if (Latch) - { - if (lastMessageSent != null && lastMessageSent != toSend) - { - RecycleMessageIfApplicable(lastMessageSent); - } - - lastMessageSent = toSend; - } - else - { - RecycleMessageIfApplicable(toSend); - } - } - - return sendToState; - } - - public override void ClearAllQueuedData() - { - List toRecycle; - lock (outgoingMessages) - { - toRecycle = new List(outgoingMessages); - outgoingMessages.Clear(); - queueOverflowUnsentCounter = 0; - } - - foreach (T messageToRecycle in toRecycle) - { - RecycleMessageIfApplicable(messageToRecycle); - } - } - - private void RecycleMessageIfApplicable(T toRecycle) - { - if (!MessagePoolEnabled) - { - return; - } - - //Add the message back to the pool. - AddMessageToPool(toRecycle); - } - - #region Message Pooling - - public bool MessagePoolEnabled - { - get => _messagePoolEnabled; - set - { - if (_messagePoolEnabled != value) - { - _messagePoolEnabled = value; - if (!_messagePoolEnabled) - { - lock (inactiveMessagePool) - { - inactiveMessagePool.Clear(); - } - } - } - } - } - - public void AddMessageToPool(T messageToRecycle) - { - Debug.Assert(MessagePoolEnabled, - "Adding a message to a message pool that is not enabled, please set MessagePoolEnabled to true."); - - lock (inactiveMessagePool) - { - if (MessagePoolEnabled && 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. - inactiveMessagePool.Enqueue(messageToRecycle); - } - } - } - - /** - * @return a message of type T full of garbage data, be sure to update it accordingly. - */ - public T GetMessageFromPool() - { - T result = null; - MessagePoolEnabled = true; - lock (inactiveMessagePool) - { - if (inactiveMessagePool.Count > 0) - { - result = inactiveMessagePool.Dequeue(); - } - } - - if (result == null) - { - result = Activator.CreateInstance(); - } - - return result; - } - - #endregion - } -} diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/RosTopicState.cs b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/RosTopicState.cs new file mode 100644 index 00000000..a3c41639 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/RosTopicState.cs @@ -0,0 +1,262 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using System.Net.Sockets; +using System.Threading.Tasks; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using UnityEngine; + +namespace Unity.Robotics.ROSTCPConnector +{ + public class RosTopicState + { + string m_Topic; + public string Topic => m_Topic; + + MessageSubtopic m_Subtopic; + public MessageSubtopic Subtopic => m_Subtopic; + + string m_RosMessageName; + public string RosMessageName => m_RosMessageName; + + TopicMessageSender m_MessageSender; + public TopicMessageSender MessageSender; + public bool IsPublisher { get; private set; } + 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; + RosTopicState m_ServiceResponseTopic; + public RosTopicState ServiceResponseTopic => m_ServiceResponseTopic; + + public bool IsUnityService => m_ServiceImplementation != null || m_ServiceImplementationAsync != null; + + List> m_SubscriberCallbacks = new List>(); + public bool HasSubscriberCallback => m_SubscriberCallbacks.Count > 0; + public bool SentSubscriberRegistration { get; private set; } + + float m_LastMessageReceivedRealtime; + float m_LastMessageSentRealtime; + 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) + { + m_Topic = topic; + m_Subtopic = subtopic; + m_RosMessageName = rosMessageName; + m_Connection = connection; + m_ConnectionInternal = connectionInternal; + } + + internal void ChangeRosMessageName(string rosMessageName) + { + if (m_RosMessageName != null) + Debug.LogWarning($"Inconsistent declaration of topic '{Topic}': was '{m_RosMessageName}', switching to '{rosMessageName}'."); + m_RosMessageName = rosMessageName; + // force deserializer to be refreshed + m_Deserializer = null; + } + + internal void OnMessageReceived(byte[] data) + { + m_LastMessageReceivedRealtime = Time.realtimeSinceStartup; + if (m_IsRosService && m_ServiceResponseTopic != null) + { + // For a service, incoming messages are a different type from outgoing messages. + // We process them using a separate RosTopicState. + m_ServiceResponseTopic.OnMessageReceived(data); + return; + } + + // don't bother deserializing this message if nobody cares + if (m_SubscriberCallbacks.Count == 0) + { + return; + } + + Message message = Deserialize(data); + + m_SubscriberCallbacks.ForEach(item => item(message)); + } + + void OnMessageSent(Message message) + { + m_LastMessageSentRealtime = Time.realtimeSinceStartup; + if (m_RosMessageName == null) + { + ChangeRosMessageName(message.RosMessageName); + } + + m_SubscriberCallbacks.ForEach(item => item(message)); + } + + internal async void HandleUnityServiceRequest(byte[] data, int serviceId) + { + if (!IsUnityService) + { + Debug.LogError($"Unity service '{m_Topic}' has not been implemented!"); + return; + } + + OnMessageReceived(data); + + // deserialize the request message + Message requestMessage = Deserialize(data); + + // run the actual service + Message response; + + if (m_ServiceImplementationAsync != null) + { + response = await m_ServiceImplementationAsync(requestMessage); + } + else + { + response = m_ServiceImplementation(requestMessage); + } + + m_ServiceResponseTopic.OnMessageSent(response); + + // send the response message back + m_ConnectionInternal.SendUnityServiceResponse(serviceId); + m_MessageSender.Queue(response); + m_ConnectionInternal.AddSenderToQueue(m_MessageSender); + } + + Message Deserialize(byte[] data) + { + if (m_Deserializer == null) + m_Deserializer = MessageRegistry.GetDeserializeFunction(m_RosMessageName, m_Subtopic); + + m_ConnectionInternal.Deserializer.InitWithBuffer(data); + return m_Deserializer(m_ConnectionInternal.Deserializer); + } + + public void AddSubscriber(Action callback) + { + m_SubscriberCallbacks.Add(callback); + + RegisterSubscriber(); + } + + void RegisterSubscriber(NetworkStream stream = null) + { + if (m_Connection.HasConnectionThread && !SentSubscriberRegistration) + { + m_ConnectionInternal.SendSubscriberRegistration(m_Topic, m_RosMessageName, stream); + SentSubscriberRegistration = true; + } + } + + public void UnsubscribeAll() + { + m_SubscriberCallbacks.Clear(); + m_ConnectionInternal.SendSubscriberUnregistration(m_Topic); + SentSubscriberRegistration = false; + } + + public void ImplementService(Func implementation, int queueSize) + { + m_ServiceImplementation = implementation; + m_ConnectionInternal.SendUnityServiceRegistration(m_Topic, m_RosMessageName); + m_ServiceResponseTopic = new RosTopicState(m_Topic, m_RosMessageName, m_Connection, m_ConnectionInternal, MessageSubtopic.Response); + CreateMessageSender(queueSize); + } + + public void ImplementService(Func> implementation, int queueSize) + { + m_ServiceImplementationAsync = implementation; + m_ConnectionInternal.SendUnityServiceRegistration(m_Topic, m_RosMessageName); + m_ServiceResponseTopic = new RosTopicState(m_Topic, m_RosMessageName, m_Connection, m_ConnectionInternal, MessageSubtopic.Response); + CreateMessageSender(queueSize); + } + + public void RegisterPublisher(int queueSize, bool latch) + { + if (IsPublisher) + { + Debug.LogWarning($"Publisher for topic {m_Topic} registered twice!"); + return; + } + IsPublisher = true; + IsPublisherLatched = latch; + m_ConnectionInternal.SendPublisherRegistration(m_Topic, m_RosMessageName, queueSize, latch); + CreateMessageSender(queueSize); + } + + public void Publish(Message message) + { + m_LastMessageSentRealtime = Time.realtimeSinceStartup; + OnMessageSent(message); + m_MessageSender.Queue(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 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); + } + + internal void SendServiceRequest(Message requestMessage, int serviceId) + { + m_ConnectionInternal.SendServiceRequest(serviceId); + m_MessageSender.Queue(requestMessage); + m_ConnectionInternal.AddSenderToQueue(m_MessageSender); + } + + internal void OnConnectionEstablished(NetworkStream stream) + { + if (m_SubscriberCallbacks.Count > 0 && !SentSubscriberRegistration) + { + m_ConnectionInternal.SendSubscriberRegistration(m_Topic, m_RosMessageName, stream); + SentSubscriberRegistration = true; + } + + if (IsUnityService) + { + m_ConnectionInternal.SendUnityServiceRegistration(m_Topic, m_RosMessageName, stream); + } + + if (IsPublisher) + { + //Register the publisher before sending anything. + m_ConnectionInternal.SendPublisherRegistration(m_Topic, m_RosMessageName, m_MessageSender.QueueSize, IsPublisherLatched, stream); + if (IsPublisherLatched) + { + m_MessageSender.PrepareLatchMessage(); + m_ConnectionInternal.AddSenderToQueue(m_MessageSender); + } + } + + if (m_IsRosService) + { + m_ConnectionInternal.SendRosServiceRegistration(m_Topic, m_RosMessageName, stream); + } + } + + internal void OnConnectionLost() + { + SentSubscriberRegistration = false; + } + } +} diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/RosTopicState.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/RosTopicState.cs.meta new file mode 100644 index 00000000..264bdac2 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/RosTopicState.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 478c335af85accc499b75e7883e649fc +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: 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 33a96946..0518fd53 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/SysCommand.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/SysCommand.cs @@ -7,7 +7,6 @@ namespace Unity.Robotics.ROSTCPConnector { public abstract class SysCommand { - public const string k_SysCommand_Log = "__log"; public const string k_SysCommand_Warning = "__warn"; public const string k_SysCommand_Error = "__error"; @@ -18,6 +17,10 @@ public abstract class SysCommand public const string k_SysCommand_RosService = "__ros_service"; public const string k_SysCommand_UnityService = "__unity_service"; public const string k_SysCommand_TopicList = "__topic_list"; + public const string k_SysCommand_RemoveSubscriber = "__remove_subscriber"; + public const string k_SysCommand_RemovePublisher = "__remove_publisher"; + public const string k_SysCommand_RemoveRosService = "__remove_ros_service"; + public const string k_SysCommand_RemoveUnityService = "__remove_unity_service"; public abstract string Command { @@ -53,6 +56,11 @@ public void SendTo([NotNull] Stream stream, MessageSerializer messageSerializer } } + public struct SysCommand_Topic + { + public string topic; + } + public struct SysCommand_TopicAndType { public string topic; @@ -79,27 +87,11 @@ public struct SysCommand_TopicsResponse public string[] types; } - public class SysCommandPublisherRegistration : SysCommand + public struct SysCommand_PublisherRegistration { - [SerializeField] public string topic; - [SerializeField] public string message_name; - [SerializeField] public int queue_size; - [SerializeField] public bool latch; - - public SysCommandPublisherRegistration(ROSPublisherBase rosPublisher) : this( - rosPublisher.TopicName, rosPublisher.RosMessageName, rosPublisher.QueueSize, rosPublisher.Latch) - { - } - - public SysCommandPublisherRegistration(string topic, string messageName, int queueSize, bool latch) - { - this.topic = topic; - message_name = messageName; - queue_size = queueSize; - this.latch = latch; - } - - public override string Command => k_SysCommand_Publish; + public string topic; + public string message_name; + public int queue_size; + public bool latch; } - } diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TFAttachment.cs b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TFAttachment.cs new file mode 100644 index 00000000..4ab7c497 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TFAttachment.cs @@ -0,0 +1,23 @@ +using System.Collections; +using System.Collections.Generic; +using UnityEngine; + +namespace Unity.Robotics.ROSTCPConnector +{ + public class TFAttachment : MonoBehaviour + { + [SerializeField] + string m_FrameID; + public string FrameID { get => m_FrameID; set => m_FrameID = value; } + [SerializeField] + string m_TFTopic = "/tf"; + public string TFTopic { get => m_TFTopic; set => m_TFTopic = value; } + + void Start() + { + transform.parent = TFSystem.GetOrCreateInstance().GetTransformObject(m_FrameID, m_TFTopic).transform; + transform.localPosition = Vector3.zero; + transform.localRotation = Quaternion.identity; + } + } +} diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TFAttachment.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TFAttachment.cs.meta new file mode 100644 index 00000000..16b8e284 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TFAttachment.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f9ca0c4d88f90d140ab1d4dd9e4e8e1c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TFStream.cs b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TFStream.cs new file mode 100644 index 00000000..d0385e5c --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TFStream.cs @@ -0,0 +1,187 @@ +using System.Collections.Generic; +using UnityEngine; + +// Represents a transform - position and rotation. +//(Like the Unity Transform class, but without the GameObject baggage that comes with it.) +public struct TFFrame +{ + public Vector3 translation; + public Quaternion rotation; + public static TFFrame identity = new TFFrame(Vector3.zero, Quaternion.identity); + + public TFFrame(Vector3 translation, Quaternion rotation) + { + this.translation = translation; + this.rotation = rotation; + } + + public Vector3 TransformPoint(Vector3 point) + { + return translation + rotation * point; + } + + public Vector3 InverseTransformPoint(Vector3 point) + { + return Quaternion.Inverse(rotation) * (point - translation); + } + + public TFFrame Compose(TFFrame child) + { + return new TFFrame(TransformPoint(child.translation), rotation * child.rotation); + } + + public static TFFrame Lerp(TFFrame a, TFFrame b, float lerp) + { + return new TFFrame + { + translation = Vector3.Lerp(a.translation, b.translation, lerp), + rotation = Quaternion.Lerp(a.rotation, b.rotation, lerp) + }; + } +} + +// Represents a transform frame changing over time. +public class TFStream +{ + public string Name { get; private set; } + public string TFTopic { get; private set; } + public TFStream Parent { get; private set; } + public IEnumerable Children => m_Children; + + // oldest first + List m_Timestamps = new List(); + // same order as m_Timestamps + List m_Frames = new List(); + List m_Children = new List(); + + // a gameobject at the last known position of this tfstream + GameObject m_GameObject; + public GameObject GameObject => m_GameObject; + + public TFStream(TFStream parent, string name, string tfTopic) + { + Name = name; + TFTopic = tfTopic; + m_GameObject = new GameObject(name); + SetParent(parent); + } + + public void SetParent(TFStream newParent) + { + if (Parent == newParent) + return; + + if (Parent != null) + { + Parent.m_Children.Remove(this); + } + + if (newParent != null) + { + m_GameObject.transform.parent = newParent.m_GameObject.transform; + newParent.m_Children.Add(this); + } + else + { + m_GameObject.transform.parent = null; + } + Parent = newParent; + } + + public void Add(long timestamp, Vector3 translation, Quaternion rotation) + { + TFFrame newEntry = new TFFrame(translation, rotation); + // most likely case: we're just adding a newer transform to the end of the list + if (m_Timestamps.Count == 0 || m_Timestamps[m_Timestamps.Count - 1] < timestamp) + { + m_Timestamps.Add(timestamp); + m_Frames.Add(newEntry); + m_GameObject.transform.localPosition = translation; + m_GameObject.transform.localRotation = rotation; + } + else + { + int index = m_Timestamps.BinarySearch(timestamp); + if (index < 0) + { + // no preexisting entry, but ~index gives us the position to insert the new entry + m_Timestamps.Insert(~index, timestamp); + m_Frames.Insert(~index, newEntry); + } + else + { + // we found an existing entry at the same timestamp!? Just replace the old one, I guess. + m_Frames[index] = newEntry; + } + } + + // for now, just a lazy way to keep the buffer from growing infinitely: every 50 updates, discard the oldest 50 + if (m_Timestamps.Count > 100) + { + m_Timestamps.RemoveRange(0, 50); + m_Frames.RemoveRange(0, 50); + } + } + + public TFFrame GetLocalTF(long time = 0) + { + // this stream has no data at all, so just report identity. + if (m_Frames.Count == 0) + return TFFrame.identity; + + // if time is 0, just get the newest position + if (time == 0) + return m_Frames[m_Frames.Count - 1]; + + int index = m_Timestamps.BinarySearch(time); + if (index >= 0) + { + // no problem, we have an entry at this time + return m_Frames[index]; + } + + index = ~index; + if (index == 0) + { + // older than our first entry: just use the first one + return m_Frames[0]; + } + else if (index == m_Frames.Count) + { + // newer than our last entry: just use the last one + return m_Frames[m_Frames.Count - 1]; + } + else + { + // between two entries: interpolate + float lerpValue = (time - m_Timestamps[index - 1]) / (float)(m_Timestamps[index] - m_Timestamps[index - 1]); + return TFFrame.Lerp(m_Frames[index - 1], m_Frames[index], lerpValue); + } + } + + public TFFrame GetWorldTF(long time = 0) + { + TFFrame parent; + if (Parent != null) + parent = Parent.GetWorldTF(time); + else + parent = TFFrame.identity; + + return parent.Compose(GetLocalTF(time)); + } + + // Can we safely stop polling for updates to a transform at this time? + public bool IsTimeStable(long time) + { + if (time == 0) // time 0 ("use the newest data") is never stable + return false; + + if (m_Timestamps.Count == 0 || m_Timestamps[0] > time || m_Timestamps[m_Timestamps.Count - 1] < time) + return false; + + if (Parent != null && !Parent.IsTimeStable(time)) + return false; + + return true; + } +} diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TFStream.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TFStream.cs.meta new file mode 100644 index 00000000..1c3d94ce --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TFStream.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 2c8edcde9e9616e4e8d671e1c7a642bb +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TFSystem.cs b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TFSystem.cs new file mode 100644 index 00000000..03aeca08 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TFSystem.cs @@ -0,0 +1,194 @@ +using System; +using System.Collections.Generic; +using RosMessageTypes.BuiltinInterfaces; +using RosMessageTypes.Std; +using RosMessageTypes.Tf2; +using Unity.Robotics.ROSTCPConnector; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +public class TFSystem +{ + public static TFSystem instance { get; private set; } + Dictionary m_TFTopics = new Dictionary(); + + class TFTopicState + { + string m_TFTopic; + Dictionary m_TransformTable = new Dictionary(); + List> m_Listeners = new List>(); + + public TFTopicState(string tfTopic = "/tf") + { + m_TFTopic = tfTopic; + ROSConnection.GetOrCreateInstance().Subscribe(tfTopic, ReceiveTF); + } + + public TFStream GetOrCreateTFStream(string frame_id) + { + TFStream tf; + while (frame_id.EndsWith("/")) + frame_id = frame_id.Substring(0, frame_id.Length - 1); + + var slash = frame_id.LastIndexOf('/'); + var singleName = slash == -1 ? frame_id : frame_id.Substring(slash + 1); + if (!m_TransformTable.TryGetValue(singleName, out tf) || tf == null) + { + if (slash <= 0) + { + // there's no slash, or only an initial slash - just create a new root object + // (set the parent later if and when we learn more) + tf = new TFStream(null, singleName, m_TFTopic); + } + else + { + var parent = GetOrCreateTFStream(frame_id.Substring(0, slash)); + tf = new TFStream(parent, singleName, m_TFTopic); + } + + m_TransformTable[singleName] = tf; + NotifyChanged(tf); + } + else if (slash > 0 && tf.Parent == null) + { + tf.SetParent(GetOrCreateTFStream(frame_id.Substring(0, slash))); + } + + return tf; + } + + 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); + tf.Add( + tf_message.header.stamp.ToLongTime(), + tf_message.transform.translation.From(), + tf_message.transform.rotation.From() + ); + NotifyChanged(tf); + } + } + + public IEnumerable GetTransformNames() + { + return m_TransformTable.Keys; + } + + public IEnumerable GetTransforms() + { + return m_TransformTable.Values; + } + + public TFStream GetTransformStream(string frame_id) + { + TFStream result = null; + m_TransformTable.TryGetValue(frame_id, out result); + return result; + } + + public void AddListener(Action callback) + { + m_Listeners.Add(callback); + } + + public void NotifyChanged(TFStream stream) + { + foreach (Action callback in m_Listeners) + { + callback(stream); + } + } + + public void NotifyAllChanged() + { + foreach (var stream in m_TransformTable.Values) + NotifyChanged(stream); + } + } + + private TFSystem() + { + + } + + public static TFSystem GetOrCreateInstance() + { + if (instance != null) + return instance; + + ROSConnection ros = ROSConnection.GetOrCreateInstance(); + instance = new TFSystem(); + foreach (string s in ros.TFTopics) + { + instance.GetOrCreateTFTopic(s); + } + return instance; + } + + public IEnumerable GetTransformNames(string tfTopic = "/tf") + { + return GetOrCreateTFTopic(tfTopic).GetTransformNames(); + } + + public IEnumerable GetTransforms(string tfTopic = "/tf") + { + return GetOrCreateTFTopic(tfTopic).GetTransforms(); + } + + public void AddListener(Action callback, bool notifyAllStreamsNow = true, string tfTopic = "/tf") + { + TFTopicState state = GetOrCreateTFTopic(tfTopic); + state.AddListener(callback); + if (notifyAllStreamsNow) + state.NotifyAllChanged(); + } + + public void NotifyAllChanged(TFStream stream) + { + GetOrCreateTFTopic(stream.TFTopic).NotifyAllChanged(); + } + + public TFFrame GetTransform(HeaderMsg header, string tfTopic = "/tf") + { + return GetTransform(header.frame_id, header.stamp.ToLongTime(), tfTopic); + } + + public TFFrame GetTransform(string frame_id, long time, string tfTopic = "/tf") + { + var stream = GetTransformStream(frame_id, tfTopic); + if (stream != null) + return stream.GetWorldTF(time); + return TFFrame.identity; + } + + public TFFrame GetTransform(string frame_id, TimeMsg time, string tfTopic = "/tf") + { + return GetTransform(frame_id, time.ToLongTime(), tfTopic); + } + + public TFStream GetTransformStream(string frame_id, string tfTopic = "/tf") + { + return GetOrCreateTFTopic(tfTopic).GetTransformStream(frame_id); + } + + public GameObject GetTransformObject(string frame_id, string tfTopic = "/tf") + { + TFStream stream = GetOrCreateTFTopic(tfTopic).GetOrCreateTFStream(frame_id); + return stream.GameObject; + } + + TFTopicState GetOrCreateTFTopic(string tfTopic) + { + TFTopicState tfTopicState; + if (!m_TFTopics.TryGetValue(tfTopic, out tfTopicState)) + { + tfTopicState = new TFTopicState(tfTopic); + m_TFTopics[tfTopic] = tfTopicState; + } + return tfTopicState; + } +} diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TFSystem.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TFSystem.cs.meta new file mode 100644 index 00000000..eeca5047 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TFSystem.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 5fcaaf7eeccf1674c9b78fa6cfba67c8 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TopicMessageSender.cs b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TopicMessageSender.cs new file mode 100644 index 00000000..e49abed7 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TopicMessageSender.cs @@ -0,0 +1,238 @@ +using System; +using System.Collections.Generic; +using System.IO; +using System.Linq; +using System.Threading; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using UnityEngine; + +namespace Unity.Robotics.ROSTCPConnector +{ + public class TopicMessageSender : OutgoingMessageSender + { + public string RosMessageName { get; private set; } + + public string TopicName { get; private set; } + + public int QueueSize { get; private set; } + + //Messages waiting to be sent queue. + LinkedList m_OutgoingMessages = new LinkedList(); + + //Keeps track of how many outgoing messages were removed due to queue overflow. + //If a message is published but the queue is full, the counter is incremented, the + //first message in the queue is recycled and the message to publish is put on the end of the queue. + //If this is non 0, a SendTo call will decrement the counter instead of sending data. + int m_QueueOverflowUnsentCounter = 0; + + //Latching - This message will be set if latching is enabled, and on reconnection, it will be sent again. + 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(); + + public TopicMessageSender(string topicName, string rosMessageName, int queueSize) + { + if (queueSize < 1) + { + throw new Exception("Queue size must be greater than or equal to 1."); + } + + TopicName = topicName; + RosMessageName = rosMessageName; + QueueSize = queueSize; + } + + internal void Queue(Message message) + { + lock (m_OutgoingMessages) + { + if (m_OutgoingMessages.Count >= QueueSize) + { + //Remove outgoing messages that don't fit in the queue. + //Recycle the message if applicable + TryRecycleMessage(m_OutgoingMessages.First.Value); + //Update the overflow counter. + m_QueueOverflowUnsentCounter++; + m_OutgoingMessages.RemoveFirst(); + } + + //Add a new valid message to the end. + m_OutgoingMessages.AddLast(message); + } + } + + SendToState GetMessageToSend(out Message messageToSend) + { + SendToState result = SendToState.NoMessageToSendError; + messageToSend = null; + lock (m_OutgoingMessages) + { + if (m_QueueOverflowUnsentCounter > 0) + { + //This means that we can't send message to ROS as fast as we're generating them. + //This could potentially be bad as it means that we are dropping messages! + m_QueueOverflowUnsentCounter--; + messageToSend = null; + result = SendToState.QueueFullWarning; + } + else if (m_OutgoingMessages.Count > 0) + { + //Retrieve the next message and populate messageToSend. + messageToSend = m_OutgoingMessages.First.Value; + m_OutgoingMessages.RemoveFirst(); + result = SendToState.Normal; + } + } + + return result; + } + + public bool PeekNextMessageToSend(out Message messageToSend) + { + bool result = false; + messageToSend = null; + lock (m_OutgoingMessages) + { + if (m_OutgoingMessages.Count > 0) + { + messageToSend = m_OutgoingMessages.First.Value; + result = true; + } + } + + + return result; + } + + void SendMessageWithStream(MessageSerializer messageSerializer, Stream stream, Message message) + { + //Clear the serializer + messageSerializer.Clear(); + //Prepare the data to send. + messageSerializer.Write(TopicName); + messageSerializer.SerializeMessageWithLength(message); + //Send via the stream. + messageSerializer.SendTo(stream); + } + + public void PrepareLatchMessage() + { + if (m_LastMessageSent != null && !m_OutgoingMessages.Any()) + { + //This topic is latching, so to mimic that functionality, + // the last sent message is sent again with the new connection. + m_OutgoingMessages.AddFirst(m_LastMessageSent); + } + } + + public override SendToState SendInternal(MessageSerializer messageSerializer, Stream stream) + { + SendToState sendToState = GetMessageToSend(out Message toSend); + if (sendToState == SendToState.Normal) + { + SendMessageWithStream(messageSerializer, stream, toSend); + + //Recycle the message (if applicable). + if (m_LastMessageSent != null && m_LastMessageSent != toSend) + { + TryRecycleMessage(m_LastMessageSent); + } + + m_LastMessageSent = toSend; + } + + return sendToState; + } + + public override void ClearAllQueuedData() + { + List toRecycle; + lock (m_OutgoingMessages) + { + toRecycle = new List(m_OutgoingMessages); + m_OutgoingMessages.Clear(); + m_QueueOverflowUnsentCounter = 0; + } + + foreach (Message messageToRecycle in toRecycle) + { + TryRecycleMessage(messageToRecycle); + } + } + + void TryRecycleMessage(Message toRecycle) + { + if (!MessagePoolEnabled) + { + 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); + } + } + } + + /** + * @return a message of type T full of garbage data, be sure to update it accordingly. + */ + public Message GetMessageFromPool() + { + 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; + } + + #endregion + } +} diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSPublisher.cs.meta b/com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TopicMessageSender.cs.meta similarity index 100% rename from com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSPublisher.cs.meta rename to com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TopicMessageSender.cs.meta diff --git a/com.unity.robotics.ros-tcp-connector/Tools.meta b/com.unity.robotics.ros-tcp-connector/Tools.meta new file mode 100644 index 00000000..cce9f940 --- /dev/null +++ b/com.unity.robotics.ros-tcp-connector/Tools.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: bea5d2e440e289646985b5afccde8e04 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.ros-tcp-connector/package.json b/com.unity.robotics.ros-tcp-connector/package.json index 75dcaaa0..4f2768f7 100644 --- a/com.unity.robotics.ros-tcp-connector/package.json +++ b/com.unity.robotics.ros-tcp-connector/package.json @@ -1,6 +1,6 @@ { "name": "com.unity.robotics.ros-tcp-connector", - "version": "0.5.0-preview", + "version": "0.6.0-preview", "displayName": "ROS TCP Connector", "description": "Bridge components and message generation allowing Unity to communicate with ROS and ROS2 services", "unity": "2020.2", diff --git a/com.unity.robotics.visualizations/DefaultVisualizationSuite.prefab b/com.unity.robotics.visualizations/DefaultVisualizationSuite.prefab new file mode 100644 index 00000000..e45e6d33 --- /dev/null +++ b/com.unity.robotics.visualizations/DefaultVisualizationSuite.prefab @@ -0,0 +1,4078 @@ +%YAML 1.1 +%TAG !u! tag:unity3d.com,2011: +--- !u!1 &8220441480183119 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 5986467313172432323} + - component: {fileID: 7667896435854312411} + m_Layer: 0 + m_Name: GoalStatusArray + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &5986467313172432323 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 8220441480183119} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 148721012047438994} + m_RootOrder: 2 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &7667896435854312411 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 8220441480183119} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 9dc27fe6fcdc64cef82392ec66c6ffdf, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 +--- !u!1 &29758807878552846 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 3198325951128121373} + - component: {fileID: 143983801561906140} + m_Layer: 0 + m_Name: RelativeHumidity + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &3198325951128121373 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 29758807878552846} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 1464881800879191381} + m_RootOrder: 21 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &143983801561906140 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 29758807878552846} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 6fa4c01962880432ba05b0bbcceeb680, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 +--- !u!1 &32108550230696561 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 2705058716118802165} + - component: {fileID: 4544822530810619555} + m_Layer: 0 + m_Name: diagnostic_msgs + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &2705058716118802165 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 32108550230696561} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: + - {fileID: 4371458951327426256} + - {fileID: 8992427609967968221} + m_Father: {fileID: 3885009520023495739} + m_RootOrder: 1 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &4544822530810619555 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 32108550230696561} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: d52ab7220652a3a4bb29ca9a1d4b91c8, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Priority: -1 +--- !u!1 &190290393943517930 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 3650867935625600996} + - component: {fileID: 231332887494367807} + m_Layer: 0 + m_Name: Image + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &3650867935625600996 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 190290393943517930} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 1464881800879191381} + m_RootOrder: 5 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &231332887494367807 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 190290393943517930} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 31dd4d69a4ee14d7fa748be4177b1fa6, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Debayer: 1 +--- !u!1 &282174558860478410 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 119694331486013991} + - component: {fileID: 7394996294379088659} + m_Layer: 0 + m_Name: Point32 + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &119694331486013991 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 282174558860478410} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 5960358800852583846} + m_RootOrder: 4 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &7394996294379088659 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 282174558860478410} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: ca06d2332e51924479738a34919115b6, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Radius: 0.01 + m_Color: {r: 0, g: 0, b: 0, a: 0} + m_Label: +--- !u!1 &479395198343907065 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 5945001988879825757} + - component: {fileID: 4625238146667055578} + m_Layer: 0 + m_Name: Range + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &5945001988879825757 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 479395198343907065} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 1464881800879191381} + m_RootOrder: 20 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &4625238146667055578 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 479395198343907065} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 0b206120c451245dd997d68b0605e7d0, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Color: {r: 0, g: 0, b: 0, a: 0} + m_TFTrackingSettings: + type: 1 + tfTopic: /tf +--- !u!1 &497132246027778600 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 7654696011896559656} + - component: {fileID: 2446387845571783168} + m_Layer: 0 + m_Name: PoseArray + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &7654696011896559656 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 497132246027778600} + m_LocalRotation: {x: -0, y: -0, z: -0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 5960358800852583846} + m_RootOrder: 7 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &2446387845571783168 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 497132246027778600} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: e377c0825a3a9084a8055a7edef5215f, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Size: 0.1 + m_DrawUnityAxes: 0 + m_TFTrackingSettings: + type: 1 + tfTopic: /tf +--- !u!1 &655416681228388073 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 4710520166345229624} + - component: {fileID: 705645579276828629} + m_Layer: 0 + m_Name: CameraInfo + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &4710520166345229624 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 655416681228388073} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 1464881800879191381} + m_RootOrder: 1 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &705645579276828629 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 655416681228388073} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 47b40ad0f07784a3eb9b79e59b2b08f6, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + ImageTopic: +--- !u!1 &1030431951597290439 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 8141636989055293894} + - component: {fileID: 5388577346585104309} + m_Layer: 0 + m_Name: BatteryState + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &8141636989055293894 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 1030431951597290439} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 1464881800879191381} + m_RootOrder: 0 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &5388577346585104309 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 1030431951597290439} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 2444f926fed994054b751fb88c0cedd6, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 +--- !u!1 &1093120864514574883 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 4588628074772272209} + - component: {fileID: 3972522128722269404} + m_Layer: 0 + m_Name: ColorRGBA + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &4588628074772272209 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 1093120864514574883} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 8625932825378670584} + m_RootOrder: 1 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &3972522128722269404 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 1093120864514574883} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 7dffbc3f3164cbd428d12e169f45bd6e, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 +--- !u!1 &1144161317836883857 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 5004252178247847185} + - component: {fileID: 8211794395676394385} + m_Layer: 0 + m_Name: Joy + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &5004252178247847185 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 1144161317836883857} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 1464881800879191381} + m_RootOrder: 8 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &8211794395676394385 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 1144161317836883857} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: a4c69f0a459af400c8c9acceacad8be7, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Settings: {fileID: 0} +--- !u!1 &1160754893935406713 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 3924764736329300096} + - component: {fileID: 7747845015475638968} + m_Layer: 0 + m_Name: Solid + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &3924764736329300096 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 1160754893935406713} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 2863337946553629617} + m_RootOrder: 2 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &7747845015475638968 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 1160754893935406713} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 50fba5bdff765464e9ad375003b5267f, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Origin: {fileID: 0} + m_Color: {r: 0, g: 0, b: 0, a: 0} +--- !u!1 &1167834518159940086 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 6296874268567447714} + - component: {fileID: 6818012456600889604} + - component: {fileID: 5714553414960223763} + m_Layer: 0 + m_Name: Twist + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &6296874268567447714 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 1167834518159940086} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 5960358800852583846} + m_RootOrder: 11 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &6818012456600889604 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 1167834518159940086} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: b6a9055543d8340b2900ae15dd34317b, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + thickness: 0.01 + lengthScale: 1 + sphereRadius: 1 + origin: {fileID: 0} + m_Color: {r: 0, g: 0, b: 0, a: 0} +--- !u!114 &5714553414960223763 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 1167834518159940086} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: c318843ad54314ff1b015df116ce0976, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + thickness: 0.01 + lengthScale: 1 + sphereRadius: 1 + origin: {fileID: 0} + m_Color: {r: 0, g: 0, b: 0, a: 0} + m_TFTrackingSettings: + type: 1 + tfTopic: /tf +--- !u!1 &1278862467777487029 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 5777239018193456623} + - component: {fileID: 4155731808898009650} + m_Layer: 0 + m_Name: TimeReference + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &5777239018193456623 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 1278862467777487029} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 1464881800879191381} + m_RootOrder: 23 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &4155731808898009650 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 1278862467777487029} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: d7207899898b44893b24d167f92ebf76, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 +--- !u!1 &1283768819622691844 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 4267746868194099099} + - component: {fileID: 8185104009475192630} + m_Layer: 0 + m_Name: JointState + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &4267746868194099099 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 1283768819622691844} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 1464881800879191381} + m_RootOrder: 7 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &8185104009475192630 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 1283768819622691844} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: dc4755a1cc0214b92af68e89a25b09f3, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_ShowEffort: 0 + m_UrdfRobot: {fileID: 0} + m_Color: {r: 0, g: 0, b: 0, a: 0} + m_TFTrackingSettings: + type: 1 + tfTopic: /tf +--- !u!1 &1345203064544116623 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 1268297187464773281} + - component: {fileID: 4698280316413796283} + - component: {fileID: 8492896804456040599} + m_Layer: 0 + m_Name: TwistWithCovariance + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &1268297187464773281 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 1345203064544116623} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 5960358800852583846} + m_RootOrder: 12 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &4698280316413796283 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 1345203064544116623} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 9f5e703fee4774175ad7e40b70d5b17b, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + thickness: 0.01 + lengthScale: 1 + sphereRadius: 1 + origin: {fileID: 0} + m_Color: {r: 0, g: 0, b: 0, a: 0} +--- !u!114 &8492896804456040599 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 1345203064544116623} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 459e206b765d9491689eee30dc54bddd, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + thickness: 0.01 + lengthScale: 1 + sphereRadius: 1 + origin: {fileID: 0} + m_Color: {r: 0, g: 0, b: 0, a: 0} + m_TFTrackingSettings: + type: 1 + tfTopic: /tf +--- !u!1 &1578550911891748522 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 6018911647020148077} + - component: {fileID: 1538271336266211638} + m_Layer: 0 + m_Name: Int64MultiArray + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &6018911647020148077 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 1578550911891748522} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 8625932825378670584} + m_RootOrder: 7 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &1538271336266211638 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 1578550911891748522} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 7367dea8b0ce5fa4e90214177ca45079, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Tabulate: 1 +--- !u!1 &1815730348320824844 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 3700627113227026602} + - component: {fileID: 3324874550881737244} + m_Layer: 0 + m_Name: Marker + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &3700627113227026602 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 1815730348320824844} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 9093984623055348717} + m_RootOrder: 0 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &3324874550881737244 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 1815730348320824844} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 90600d3790e079d49997a5f9f09d01d3, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 +--- !u!1 &1847056819877925309 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 2285070536742860452} + - component: {fileID: 3489572002153421434} + m_Layer: 0 + m_Name: MultiDOFJointState + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &2285070536742860452 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 1847056819877925309} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 1464881800879191381} + m_RootOrder: 13 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &3489572002153421434 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 1847056819877925309} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: effc77ba9c5ea483792df0ccd656fc7f, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_UrdfRobot: {fileID: 0} + m_Color: {r: 0, g: 0, b: 0, a: 0} + m_TFTrackingSettings: + type: 1 + tfTopic: /tf +--- !u!1 &2097095153000774029 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 9147044056431671843} + - component: {fileID: 1371375351981416475} + m_Layer: 0 + m_Name: Plane + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &9147044056431671843 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 2097095153000774029} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 2863337946553629617} + m_RootOrder: 1 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &1371375351981416475 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 2097095153000774029} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: e6257160834256b4b861ad5458dd4050, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Color: {r: 0, g: 0, b: 0, a: 0} +--- !u!1 &2133705848873659408 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 1667955411476894290} + - component: {fileID: 464542745395013059} + m_Layer: 0 + m_Name: PointField + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &1667955411476894290 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 2133705848873659408} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 1464881800879191381} + m_RootOrder: 19 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &464542745395013059 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 2133705848873659408} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: aa0b8ae6bccec42b389ffce51057e81d, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 +--- !u!1 &2342561914306062115 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 5543601553466399134} + - component: {fileID: 4537657291303860418} + m_Layer: 0 + m_Name: UInt16MultiArray + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &5543601553466399134 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 2342561914306062115} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 8625932825378670584} + m_RootOrder: 10 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &4537657291303860418 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 2342561914306062115} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 7cade9fa5fce2fb4c86bfb960650bc19, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Tabulate: 1 +--- !u!1 &2472986299860318132 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 7911503663522063850} + - component: {fileID: 2816685128873445351} + - component: {fileID: 4252262176058916798} + m_Layer: 0 + m_Name: Point + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &7911503663522063850 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 2472986299860318132} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 5960358800852583846} + m_RootOrder: 3 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &2816685128873445351 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 2472986299860318132} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 411f6a469899d8c4691994218731ae87, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Radius: 0.01 + m_Color: {r: 0, g: 0, b: 0, a: 0} + m_Label: +--- !u!114 &4252262176058916798 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 2472986299860318132} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 59143f68e58e4d6449372a89c6b2ebd8, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Radius: 0.01 + m_Color: {r: 0, g: 0, b: 0, a: 0} + m_Label: + m_TFTrackingSettings: + type: 1 + tfTopic: /tf +--- !u!1 &2473740400490751114 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 7131592569726091290} + - component: {fileID: 3432624721861520032} + - component: {fileID: 7305108287282068823} + m_Layer: 0 + m_Name: Transform + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &7131592569726091290 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 2473740400490751114} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 5960358800852583846} + m_RootOrder: 10 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &3432624721861520032 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 2473740400490751114} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 7d4f4c2adcaad6b4799f59c76f040eee, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Size: 0.1 + m_DrawUnityAxes: 0 + m_Color: {r: 0, g: 0, b: 0, a: 0} + m_Label: +--- !u!114 &7305108287282068823 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 2473740400490751114} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 34006ea0674a568409a63b1695197f8b, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Size: 0.1 + m_DrawUnityAxes: 0 + m_Color: {r: 0, g: 0, b: 0, a: 0} + m_Label: + m_TFTrackingSettings: + type: 1 + tfTopic: /tf +--- !u!1 &2490482372229701612 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 6438524342332312404} + - component: {fileID: 6744919956612864540} + m_Layer: 0 + m_Name: FluidPressure + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &6438524342332312404 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 2490482372229701612} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 1464881800879191381} + m_RootOrder: 3 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &6744919956612864540 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 2490482372229701612} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 154673feadb3d41868cfa7f6c127ade4, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 +--- !u!1 &2681928128229103690 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 8035088012049353340} + - component: {fileID: 4828607230927912104} + m_Layer: 0 + m_Name: Float64MultiArray + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &8035088012049353340 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 2681928128229103690} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 8625932825378670584} + m_RootOrder: 3 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &4828607230927912104 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 2681928128229103690} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 988ec541b83a2e048be71cbd080cd871, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Tabulate: 1 +--- !u!1 &2779277618506728284 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 8992427609967968221} + - component: {fileID: 5871917153454302751} + m_Layer: 0 + m_Name: DiagnosticArray + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &8992427609967968221 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 2779277618506728284} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 2705058716118802165} + m_RootOrder: 1 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &5871917153454302751 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 2779277618506728284} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: ebac18b3d35d04a7ebf8099e7f11e01f, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 +--- !u!1 &2781538271582551566 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 3915600484724281225} + - component: {fileID: 5315367504069929536} + m_Layer: 0 + m_Name: GoalStatus + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &3915600484724281225 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 2781538271582551566} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 148721012047438994} + m_RootOrder: 1 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &5315367504069929536 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 2781538271582551566} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 0a5b3782d48ba46d485890cf446ad3e2, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 +--- !u!1 &2917204818746005398 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 6830301937958890614} + - component: {fileID: 2291704969931532978} + m_Layer: 0 + m_Name: GoalID + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &6830301937958890614 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 2917204818746005398} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 148721012047438994} + m_RootOrder: 0 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &2291704969931532978 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 2917204818746005398} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 07c239177cd9749e0865f008864d28b7, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 +--- !u!1 &2932710031339968934 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 8625932825378670584} + - component: {fileID: 6688221814331890443} + m_Layer: 0 + m_Name: std_msgs + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &8625932825378670584 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 2932710031339968934} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: + - {fileID: 8801515403862736623} + - {fileID: 4588628074772272209} + - {fileID: 4872493303817866288} + - {fileID: 8035088012049353340} + - {fileID: 5569118112003356481} + - {fileID: 1456749820322419521} + - {fileID: 2795885633677098601} + - {fileID: 6018911647020148077} + - {fileID: 2052082919037097433} + - {fileID: 5229810508948184452} + - {fileID: 5543601553466399134} + - {fileID: 3289027219588273902} + - {fileID: 9137994588492629459} + m_Father: {fileID: 3885009520023495739} + m_RootOrder: 7 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &6688221814331890443 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 2932710031339968934} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: d52ab7220652a3a4bb29ca9a1d4b91c8, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Priority: -1 +--- !u!1 &3007199531574625661 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 736020056192451353} + - component: {fileID: 1657468168323334076} + m_Layer: 0 + m_Name: CompressedImage + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &736020056192451353 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 3007199531574625661} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 1464881800879191381} + m_RootOrder: 2 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &1657468168323334076 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 3007199531574625661} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: c86bacc4e290e423ab7ac8f9dfde38cb, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 +--- !u!1 &3275889354970527495 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 4949125247766497678} + - component: {fileID: 5156877151121463008} + m_Layer: 0 + m_Name: MagneticField + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &4949125247766497678 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 3275889354970527495} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 1464881800879191381} + m_RootOrder: 12 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &5156877151121463008 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 3275889354970527495} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 695bf78f0b1e4437db2da726dfd5f785, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Color: {r: 0, g: 0, b: 0, a: 0} + m_TFTrackingSettings: + type: 1 + tfTopic: /tf +--- !u!1 &3295165414092385149 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 9137994588492629459} + - component: {fileID: 692807264381653956} + m_Layer: 0 + m_Name: UInt64MultiArray + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &9137994588492629459 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 3295165414092385149} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 8625932825378670584} + m_RootOrder: 12 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &692807264381653956 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 3295165414092385149} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 7a17e46fd53d29440bbd888cdd5be496, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Tabulate: 1 +--- !u!1 &3513890383435784659 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 1714386207299109949} + - component: {fileID: 1305978815096875101} + m_Layer: 0 + m_Name: OccupancyGrid + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &1714386207299109949 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 3513890383435784659} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 8087627403559164820} + m_RootOrder: 3 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &1305978815096875101 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 3513890383435784659} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 32ee8f2011c7b2b4ea58f0b05d0b1838, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Offset: {x: 0, y: 0, z: 0} + m_Material: {fileID: 2100000, guid: 8e30ea50d6b692349a46d177ded15435, type: 2} + m_TFTrackingSettings: + type: 1 + tfTopic: /tf + m_Unoccupied: {r: 1, g: 1, b: 1, a: 1} + m_Occupied: {r: 0, g: 0, b: 0, a: 1} + m_Unknown: {r: 0, g: 0, b: 0, a: 0} +--- !u!1 &3533844475292599465 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 1188987890863252038} + - component: {fileID: 3385951721461172776} + m_Layer: 0 + m_Name: PointCloud + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &1188987890863252038 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 3533844475292599465} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 1464881800879191381} + m_RootOrder: 17 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &3385951721461172776 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 3533844475292599465} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: cc3913d621dc7481b924170b46950dfc, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: + m_Settings: {fileID: 11400000, guid: 1b7359921b91a4830ae57b1c5b22caeb, type: 2} +--- !u!1 &3625111931293026982 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 1746080842249086020} + - component: {fileID: 5762556200703979462} + m_Layer: 0 + m_Name: JoyFeedbackArray + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &1746080842249086020 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 3625111931293026982} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 1464881800879191381} + m_RootOrder: 10 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &5762556200703979462 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 3625111931293026982} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 4954b8850f3f04096a5ab8ceb6e51fea, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 +--- !u!1 &3663615855669088657 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 148721012047438994} + - component: {fileID: 3944596388181530883} + m_Layer: 0 + m_Name: actionlib_msgs + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &148721012047438994 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 3663615855669088657} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: + - {fileID: 6830301937958890614} + - {fileID: 3915600484724281225} + - {fileID: 5986467313172432323} + m_Father: {fileID: 3885009520023495739} + m_RootOrder: 0 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &3944596388181530883 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 3663615855669088657} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: d52ab7220652a3a4bb29ca9a1d4b91c8, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Priority: -1 +--- !u!1 &3862572191853107957 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 3077093189101622154} + - component: {fileID: 3641201344362751560} + m_Layer: 0 + m_Name: Illuminance + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &3077093189101622154 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 3862572191853107957} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 1464881800879191381} + m_RootOrder: 4 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &3641201344362751560 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 3862572191853107957} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 6cfe585cdd95e41d381711160acf4c02, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 +--- !u!1 &3885009520023495737 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 3885009520023495739} + - component: {fileID: 8652144247881302430} + - component: {fileID: 4180636070010704716} + m_Layer: 0 + m_Name: DefaultVisualizationSuite + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &3885009520023495739 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 3885009520023495737} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: + - {fileID: 148721012047438994} + - {fileID: 2705058716118802165} + - {fileID: 5960358800852583846} + - {fileID: 8087627403559164820} + - {fileID: 1464881800879191381} + - {fileID: 2863337946553629617} + - {fileID: 892830402226269397} + - {fileID: 8625932825378670584} + - {fileID: 8632442061405313714} + - {fileID: 9093984623055348717} + - {fileID: 2742884835330475003} + m_Father: {fileID: 0} + m_RootOrder: 0 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &8652144247881302430 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 3885009520023495737} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 89775f84ec1b05048997be0d603cca35, type: 3} + m_Name: + m_EditorClassIdentifier: + axesScale: 0.1 + lineThickness: 0.01 + color: {r: 1, g: 1, b: 1, a: 1} +--- !u!114 &4180636070010704716 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 3885009520023495737} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: e120188e851b6f44b85f38aecc703ba6, type: 3} + m_Name: + m_EditorClassIdentifier: +--- !u!1 &4624789586922521961 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 4212352431820866892} + - component: {fileID: 2422501112789811223} + - component: {fileID: 3439158116004506186} + m_Layer: 0 + m_Name: Quaternion + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &4212352431820866892 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 4624789586922521961} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 5960358800852583846} + m_RootOrder: 9 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &2422501112789811223 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 4624789586922521961} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 0f651df5ec61c734f8083a79205badd1, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Size: 0.1 + m_DrawAtPosition: {fileID: 0} + m_DrawUnityAxes: 0 + m_Color: {r: 0, g: 0, b: 0, a: 0} + m_Label: +--- !u!114 &3439158116004506186 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 4624789586922521961} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 0881ebe31dc31444f913ca60e3b34783, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Size: 0.01 + m_DrawAtPosition: {fileID: 0} + m_DrawUnityAxes: 0 + m_Color: {r: 0, g: 0, b: 0, a: 0} + m_Label: + m_TFTrackingSettings: + type: 1 + tfTopic: /tf +--- !u!1 &4688618687723766012 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 3978828494085976104} + - component: {fileID: 3340153847375972651} + m_Layer: 0 + m_Name: MultiEchoLaserScan + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &3978828494085976104 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 4688618687723766012} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 1464881800879191381} + m_RootOrder: 14 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &3340153847375972651 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 4688618687723766012} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 0bde06f4fcf854104b7d4970fe09bc99, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: + m_Settings: {fileID: 11400000, guid: 1d15599f177254e03b3698260c33d600, type: 2} +--- !u!1 &4865483026275294205 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 4891852587693589216} + - component: {fileID: 3052286054258679401} + m_Layer: 0 + m_Name: Temperature + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &4891852587693589216 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 4865483026275294205} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 1464881800879191381} + m_RootOrder: 22 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &3052286054258679401 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 4865483026275294205} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 6025e3d3cb1d74c58ba9bf04d7417f22, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 +--- !u!1 &4947339100352685114 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 7065181137189497651} + - component: {fileID: 5222529248919960493} + m_Layer: 0 + m_Name: LaserScan + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &7065181137189497651 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 4947339100352685114} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 1464881800879191381} + m_RootOrder: 11 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &5222529248919960493 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 4947339100352685114} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 16302429be2008e47bd0a636c03d05f1, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: + m_Settings: {fileID: 11400000, guid: f53165fb8c4d340e3ad973ec7784242e, type: 2} +--- !u!1 &5021183695555063441 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 8087627403559164820} + - component: {fileID: 5300212233609044834} + m_Layer: 0 + m_Name: nav_msgs + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &8087627403559164820 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 5021183695555063441} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: + - {fileID: 3140499478078044468} + - {fileID: 8663488969228085318} + - {fileID: 5143174323963746430} + - {fileID: 1714386207299109949} + m_Father: {fileID: 3885009520023495739} + m_RootOrder: 3 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &5300212233609044834 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 5021183695555063441} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: d52ab7220652a3a4bb29ca9a1d4b91c8, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Priority: -1 +--- !u!1 &5125242539905964723 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 3140499478078044468} + - component: {fileID: 5546178826512412313} + m_Layer: 0 + m_Name: GridCells + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &3140499478078044468 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 5125242539905964723} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 8087627403559164820} + m_RootOrder: 0 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &5546178826512412313 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 5125242539905964723} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 5c28a0753db7113489cb49a68cb713f3, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Radius: 0.1 + m_Color: {r: 0, g: 0, b: 0, a: 0} + m_TFTrackingSettings: + type: 1 + tfTopic: /tf +--- !u!1 &5240387698711624104 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 7075564215015182945} + - component: {fileID: 8258684177818339835} + m_Layer: 0 + m_Name: PointCloud2 + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &7075564215015182945 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 5240387698711624104} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 1464881800879191381} + m_RootOrder: 18 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &8258684177818339835 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 5240387698711624104} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: f71dde58eb4384a648d556f826fd8b02, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: + m_Settings: {fileID: 11400000, guid: f1d2df8bcd83d4410b03e8a6ddd5eac4, type: 2} +--- !u!1 &5308612252404907650 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 5229810508948184452} + - component: {fileID: 8205076175141463216} + m_Layer: 0 + m_Name: UInt8MultiArray + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &5229810508948184452 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 5308612252404907650} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 8625932825378670584} + m_RootOrder: 9 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &8205076175141463216 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 5308612252404907650} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: cbf496ff53d39fb4f95457addd3626a9, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Tabulate: 1 +--- !u!1 &5531956128861424357 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 2407673968954180435} + - component: {fileID: 2197522215027751259} + - component: {fileID: 6015196953607691592} + m_Layer: 0 + m_Name: AccelWithCovariance + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &2407673968954180435 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 5531956128861424357} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 5960358800852583846} + m_RootOrder: 1 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &2197522215027751259 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 5531956128861424357} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: c08ec320a78e1b84594eabd50dd18859, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Thickness: 0.01 + m_LengthScale: 1 + m_SphereRadius: 1 + m_Origin: {fileID: 0} + m_Color: {r: 0, g: 0, b: 0, a: 0} +--- !u!114 &6015196953607691592 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 5531956128861424357} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 19a534f8e39de6d4aab8572469822b4b, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Thickness: 0.01 + m_LengthScale: 1 + m_SphereRadius: 1 + m_Origin: {fileID: 0} + m_Color: {r: 0, g: 0, b: 0, a: 0} + m_TFTrackingSettings: + type: 1 + tfTopic: /tf +--- !u!1 &5709579207413330996 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 2795885633677098601} + - component: {fileID: 1341703867306652306} + m_Layer: 0 + m_Name: Int32MultiArray + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &2795885633677098601 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 5709579207413330996} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 8625932825378670584} + m_RootOrder: 6 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &1341703867306652306 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 5709579207413330996} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 6ed9ee96944ecbc4fafaa9e6c75a8183, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Tabulate: 1 +--- !u!1 &5720674425784660787 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 2587541808226688938} + - component: {fileID: 9111314266988519193} + m_Layer: 0 + m_Name: NavSatStatus + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &2587541808226688938 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 5720674425784660787} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 1464881800879191381} + m_RootOrder: 16 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &9111314266988519193 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 5720674425784660787} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: c67a6c59d9f1c42eab47e5ca9d8a8516, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 +--- !u!1 &5841521771613453888 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 8663488969228085318} + - component: {fileID: 5411086277896228073} + m_Layer: 0 + m_Name: Odometry + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &8663488969228085318 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 5841521771613453888} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 8087627403559164820} + m_RootOrder: 1 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &5411086277896228073 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 5841521771613453888} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: aa5e309a73db1427387873581691511a, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + thickness: 0.01 + lengthScale: 1 + sphereRadius: 1 + m_Color: {r: 0, g: 0, b: 0, a: 0} + m_TFTrackingSettings: + type: 1 + tfTopic: /tf +--- !u!1 &6010295057015668764 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 955798865801369551} + - component: {fileID: 86254994656283680} + m_Layer: 0 + m_Name: JoyFeedback + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &955798865801369551 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 6010295057015668764} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 1464881800879191381} + m_RootOrder: 9 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &86254994656283680 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 6010295057015668764} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 71673c9c395324bf6be9680ae8210191, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 +--- !u!1 &6040586003059455330 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 6794178213995200361} + - component: {fileID: 2237346254655339778} + - component: {fileID: 8281281119776082372} + m_Layer: 0 + m_Name: Inertia + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &6794178213995200361 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 6040586003059455330} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 5960358800852583846} + m_RootOrder: 2 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &2237346254655339778 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 6040586003059455330} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 23204335991390b47ada035c24385383, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Origin: {fileID: 0} + m_Radius: 0 + m_Color: {r: 0, g: 0, b: 0, a: 0} +--- !u!114 &8281281119776082372 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 6040586003059455330} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 40878073c7b2b0a409b71a34633bdfcb, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Origin: {fileID: 0} + m_Radius: 0 + m_Color: {r: 0, g: 0, b: 0, a: 0} + m_Label: Center of mass + m_TFTrackingType: + type: 1 + tfTopic: /tf +--- !u!1 &6091842005400080552 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 5143174323963746430} + - component: {fileID: 5556785160914383011} + m_Layer: 0 + m_Name: Path + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &5143174323963746430 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 6091842005400080552} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 8087627403559164820} + m_RootOrder: 2 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &5556785160914383011 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 6091842005400080552} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 699093dabce49664bb68bd7011175b6c, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Thickness: 0.1 + m_Color: {r: 0, g: 0, b: 0, a: 0} + m_Offset: {x: 0, y: 0, z: 0} + m_TFTrackingSettings: + type: 1 + tfTopic: /tf +--- !u!1 &6176169723169673308 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 3655933225424945218} + - component: {fileID: 8304216471446773222} + - component: {fileID: 3894813411877443201} + m_Layer: 0 + m_Name: Accel + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &3655933225424945218 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 6176169723169673308} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 5960358800852583846} + m_RootOrder: 0 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &8304216471446773222 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 6176169723169673308} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 7d5d9a44a1314c64bb8c0ef2cc5bb27f, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Thickness: 0.01 + m_LengthScale: 1 + m_SphereRadius: 1 + m_Origin: {fileID: 0} + m_Color: {r: 0, g: 0, b: 0, a: 0} +--- !u!114 &3894813411877443201 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 6176169723169673308} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 67c032e5682c70440a684a13df04044e, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Thickness: 0.01 + m_LengthScale: 1 + m_SphereRadius: 1 + m_Origin: {fileID: 0} + m_Color: {r: 0, g: 0, b: 0, a: 0} + m_TFTrackingSettings: + type: 1 + tfTopic: /tf +--- !u!1 &6239934629084361084 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 4872493303817866288} + - component: {fileID: 7707798677355994390} + m_Layer: 0 + m_Name: Float32MultiArray + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &4872493303817866288 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 6239934629084361084} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 8625932825378670584} + m_RootOrder: 2 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &7707798677355994390 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 6239934629084361084} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: ddb07ee5b24bed04888df0c97561fe0a, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Tabulate: 1 +--- !u!1 &6488346027097204889 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 7591019540803253939} + - component: {fileID: 3755156803250151580} + - component: {fileID: 1875245268313931979} + m_Layer: 0 + m_Name: Wrench + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &7591019540803253939 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 6488346027097204889} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 5960358800852583846} + m_RootOrder: 14 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &3755156803250151580 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 6488346027097204889} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: b7112e062888947a9a1933a38520b7fa, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + thickness: 0.01 + lengthScale: 1 + sphereRadius: 1 + origin: {fileID: 0} + m_Color: {r: 0, g: 0, b: 0, a: 0} +--- !u!114 &1875245268313931979 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 6488346027097204889} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: f7e131484650e42dcaf443e03308edd6, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + thickness: 0.01 + lengthScale: 1 + sphereRadius: 1 + origin: {fileID: 0} + m_Color: {r: 0, g: 0, b: 0, a: 0} + m_TFTrackingSettings: + type: 1 + tfTopic: /tf +--- !u!1 &6586949068025881174 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 8786014348878986211} + - component: {fileID: 5180805759586010064} + - component: {fileID: 2949027852674821604} + m_Layer: 0 + m_Name: Polygon + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &8786014348878986211 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 6586949068025881174} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 5960358800852583846} + m_RootOrder: 5 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &5180805759586010064 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 6586949068025881174} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 98e9789d1b217d0479bddfe4e5ac15af, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Thickness: 0.1 + m_Color: {r: 0, g: 0, b: 0, a: 0} +--- !u!114 &2949027852674821604 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 6586949068025881174} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 2ff0d98070786c64981a74ca2ef65439, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Thickness: 0.01 + m_Color: {r: 0, g: 0, b: 0, a: 0} + m_TFTrackingSettings: + type: 1 + tfTopic: /tf +--- !u!1 &6596066656845606974 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 7621306614409298851} + - component: {fileID: 8985211858251766299} + - component: {fileID: 3188972202244487804} + m_Layer: 0 + m_Name: PoseWithCovariance + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &7621306614409298851 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 6596066656845606974} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 5960358800852583846} + m_RootOrder: 8 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &8985211858251766299 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 6596066656845606974} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: c0acf581c8a13f3448483853151ef326, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Size: 0.1 + m_DrawUnityAxes: 0 +--- !u!114 &3188972202244487804 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 6596066656845606974} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 6ce850bfe5a7ae641ae1e0005662788e, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Size: 0.1 + m_DrawUnityAxes: 0 + m_TFTrackingSettings: + type: 1 + tfTopic: /tf +--- !u!1 &7046769544666923959 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 2863337946553629617} + - component: {fileID: 6414414336950911228} + m_Layer: 0 + m_Name: shape_msgs + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &2863337946553629617 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 7046769544666923959} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: + - {fileID: 7014029237715275446} + - {fileID: 9147044056431671843} + - {fileID: 3924764736329300096} + m_Father: {fileID: 3885009520023495739} + m_RootOrder: 5 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &6414414336950911228 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 7046769544666923959} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: d52ab7220652a3a4bb29ca9a1d4b91c8, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Priority: -1 +--- !u!1 &7121277926068496737 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 5285725338617380350} + - component: {fileID: 3206162563004479801} + m_Layer: 0 + m_Name: Imu + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &5285725338617380350 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 7121277926068496737} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 1464881800879191381} + m_RootOrder: 6 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &3206162563004479801 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 7121277926068496737} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: d4b553ef5da854fa281147ab847e1c03, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Color: {r: 0, g: 0, b: 0, a: 0} + m_LengthScale: 1 + m_SphereRadius: 1 + m_Thickness: 0.01 + m_TFTrackingSettings: + type: 1 + tfTopic: /tf +--- !u!1 &7418906758068126324 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 6484321262814106970} + - component: {fileID: 6915640277351407469} + m_Layer: 0 + m_Name: NavSatFix + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &6484321262814106970 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 7418906758068126324} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 1464881800879191381} + m_RootOrder: 15 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &6915640277351407469 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 7418906758068126324} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 2458f4dffd26746e39c22d7233462b49, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 +--- !u!1 &7617470554793199405 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 7014029237715275446} + - component: {fileID: 171242428705884603} + m_Layer: 0 + m_Name: Mesh + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &7014029237715275446 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 7617470554793199405} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 2863337946553629617} + m_RootOrder: 0 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &171242428705884603 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 7617470554793199405} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: a55d9039eb2184d4683f569bb1463786, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Origin: {fileID: 0} + m_Color: {r: 0, g: 0, b: 0, a: 0} +--- !u!1 &7621486560234199926 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 780982761275414569} + - component: {fileID: 7330358251251071878} + - component: {fileID: 2172106520665917229} + m_Layer: 0 + m_Name: Vector3 + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &780982761275414569 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 7621486560234199926} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 5960358800852583846} + m_RootOrder: 13 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &7330358251251071878 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 7621486560234199926} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: c5f79b92768699c4c83e62bc7df9a7c5, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Radius: 0.01 + m_Color: {r: 0, g: 0, b: 0, a: 0} + m_Label: +--- !u!114 &2172106520665917229 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 7621486560234199926} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: aeaeab39bc34bec4fb88e7a71fd68b18, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Radius: 0.01 + m_Color: {r: 0, g: 0, b: 0, a: 0} + m_Label: + m_TFTrackingSettings: + type: 1 + tfTopic: /tf +--- !u!1 &7636441891634033707 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 1464881800879191381} + - component: {fileID: 7735061903003887872} + m_Layer: 0 + m_Name: sensor_msgs + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &1464881800879191381 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 7636441891634033707} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: + - {fileID: 8141636989055293894} + - {fileID: 4710520166345229624} + - {fileID: 736020056192451353} + - {fileID: 6438524342332312404} + - {fileID: 3077093189101622154} + - {fileID: 3650867935625600996} + - {fileID: 5285725338617380350} + - {fileID: 4267746868194099099} + - {fileID: 5004252178247847185} + - {fileID: 955798865801369551} + - {fileID: 1746080842249086020} + - {fileID: 7065181137189497651} + - {fileID: 4949125247766497678} + - {fileID: 2285070536742860452} + - {fileID: 3978828494085976104} + - {fileID: 6484321262814106970} + - {fileID: 2587541808226688938} + - {fileID: 1188987890863252038} + - {fileID: 7075564215015182945} + - {fileID: 1667955411476894290} + - {fileID: 5945001988879825757} + - {fileID: 3198325951128121373} + - {fileID: 4891852587693589216} + - {fileID: 5777239018193456623} + m_Father: {fileID: 3885009520023495739} + m_RootOrder: 4 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &7735061903003887872 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 7636441891634033707} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: d52ab7220652a3a4bb29ca9a1d4b91c8, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Priority: -1 +--- !u!1 &7744532052838294221 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 892830402226269397} + - component: {fileID: 1513170255868238818} + m_Layer: 0 + m_Name: stereo_msgs + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &892830402226269397 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 7744532052838294221} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: + - {fileID: 7418879579174395619} + m_Father: {fileID: 3885009520023495739} + m_RootOrder: 6 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &1513170255868238818 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 7744532052838294221} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: d52ab7220652a3a4bb29ca9a1d4b91c8, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Priority: -1 +--- !u!1 &7811661128855245756 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 3289027219588273902} + - component: {fileID: 261699424676030690} + m_Layer: 0 + m_Name: UInt32MultiArray + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &3289027219588273902 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 7811661128855245756} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 8625932825378670584} + m_RootOrder: 11 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &261699424676030690 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 7811661128855245756} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: ec0af1c42844d604c91f24201ee8d301, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Tabulate: 1 +--- !u!1 &7892209135603776310 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 5569118112003356481} + - component: {fileID: 1405831720783885904} + m_Layer: 0 + m_Name: Int8MultiArray + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &5569118112003356481 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 7892209135603776310} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 8625932825378670584} + m_RootOrder: 4 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &1405831720783885904 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 7892209135603776310} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 9f4e04d09fa3452489c3652d848172f3, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Tabulate: 1 +--- !u!1 &8068246987360747856 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 4371458951327426256} + - component: {fileID: 7828607638577585239} + m_Layer: 0 + m_Name: DiagnosticStatus + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &4371458951327426256 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 8068246987360747856} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 2705058716118802165} + m_RootOrder: 0 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &7828607638577585239 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 8068246987360747856} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: a8fd8da85002c46c0ac5f35abebe2224, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 +--- !u!1 &8079308096200974559 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 5960358800852583846} + - component: {fileID: 9009484810019029931} + m_Layer: 0 + m_Name: geometry_msgs + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &5960358800852583846 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 8079308096200974559} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: + - {fileID: 3655933225424945218} + - {fileID: 2407673968954180435} + - {fileID: 6794178213995200361} + - {fileID: 7911503663522063850} + - {fileID: 119694331486013991} + - {fileID: 8786014348878986211} + - {fileID: 6223847722036017341} + - {fileID: 7654696011896559656} + - {fileID: 7621306614409298851} + - {fileID: 4212352431820866892} + - {fileID: 7131592569726091290} + - {fileID: 6296874268567447714} + - {fileID: 1268297187464773281} + - {fileID: 780982761275414569} + - {fileID: 7591019540803253939} + m_Father: {fileID: 3885009520023495739} + m_RootOrder: 2 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &9009484810019029931 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 8079308096200974559} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: d52ab7220652a3a4bb29ca9a1d4b91c8, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Priority: -1 +--- !u!1 &8081225200330002091 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 1456749820322419521} + - component: {fileID: 3194210823399973676} + m_Layer: 0 + m_Name: Int16MultiArray + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &1456749820322419521 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 8081225200330002091} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 8625932825378670584} + m_RootOrder: 5 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &3194210823399973676 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 8081225200330002091} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 595224681c0c2a14aa4bf87ee554f490, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Tabulate: 1 +--- !u!1 &8088604883153766458 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 3642796221149464684} + - component: {fileID: 8917307240532856020} + m_Layer: 0 + m_Name: JointTrajectory + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &3642796221149464684 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 8088604883153766458} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 8632442061405313714} + m_RootOrder: 0 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &8917307240532856020 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 8088604883153766458} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: f99e8ef135fd8d14194efcb02e2b240a, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_UrdfRobot: {fileID: 0} + m_PathThickness: 0.01 + m_Color: {r: 0, g: 0, b: 0, a: 1} + m_TFTrackingSettings: + type: 1 + tfTopic: /tf +--- !u!1 &8221533276542151489 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 8801515403862736623} + - component: {fileID: 3712213314885987348} + m_Layer: 0 + m_Name: ByteMultiArray + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &8801515403862736623 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 8221533276542151489} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 8625932825378670584} + m_RootOrder: 0 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &3712213314885987348 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 8221533276542151489} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: d7d806713ecf02040a67c8e91540b052, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Tabulate: 1 +--- !u!1 &8334941386756716733 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 2052082919037097433} + - component: {fileID: 4632704923410846267} + m_Layer: 0 + m_Name: Time + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &2052082919037097433 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 8334941386756716733} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 8625932825378670584} + m_RootOrder: 8 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &4632704923410846267 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 8334941386756716733} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 1b78264b4fe81a446bffc3cd3f65be67, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 +--- !u!1 &8582696812179479240 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 6223847722036017341} + - component: {fileID: 4194812233120754391} + - component: {fileID: 517002158194994940} + m_Layer: 0 + m_Name: Pose + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &6223847722036017341 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 8582696812179479240} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 5960358800852583846} + m_RootOrder: 6 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &4194812233120754391 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 8582696812179479240} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 9bdfb211949793b4d8196408d5ac551a, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Size: 0.1 + m_DrawUnityAxes: 0 +--- !u!114 &517002158194994940 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 8582696812179479240} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 517ff03e8ebf1c0478a6b77647f1f439, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 + m_Size: 0.1 + m_DrawUnityAxes: 0 + m_TFTrackingSettings: + type: 1 + tfTopic: /tf +--- !u!1 &8698263691841410378 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 9093984623055348717} + - component: {fileID: 5186909126573296809} + m_Layer: 0 + m_Name: visualization_msgs + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &9093984623055348717 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 8698263691841410378} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: + - {fileID: 3700627113227026602} + m_Father: {fileID: 3885009520023495739} + m_RootOrder: 9 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &5186909126573296809 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 8698263691841410378} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: d52ab7220652a3a4bb29ca9a1d4b91c8, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Priority: -1 +--- !u!1 &8715484863927459791 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 7418879579174395619} + - component: {fileID: 4143190304998066172} + m_Layer: 0 + m_Name: DisparityImage + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &7418879579174395619 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 8715484863927459791} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 892830402226269397} + m_RootOrder: 0 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &4143190304998066172 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 8715484863927459791} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 64f41ca8e53c67c4b8ae0a8f503acec6, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Topic: + m_ID: 0-0 +--- !u!1 &8908437654478296370 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 8632442061405313714} + - component: {fileID: 302348017695905028} + m_Layer: 0 + m_Name: trajectory_msgs + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &8632442061405313714 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 8908437654478296370} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: + - {fileID: 3642796221149464684} + m_Father: {fileID: 3885009520023495739} + m_RootOrder: 8 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &302348017695905028 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 8908437654478296370} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: d52ab7220652a3a4bb29ca9a1d4b91c8, type: 3} + m_Name: + m_EditorClassIdentifier: + m_Priority: -1 +--- !u!1001 &1914626938635344595 +PrefabInstance: + m_ObjectHideFlags: 0 + serializedVersion: 2 + m_Modification: + m_TransformParent: {fileID: 3885009520023495739} + m_Modifications: + - target: {fileID: 863246206611208022, guid: 57cef4e772232da4b92e8f8931f3cc75, type: 3} + propertyPath: m_Name + value: BasicDrawingManager + objectReference: {fileID: 0} + - target: {fileID: 4360241143132002600, guid: 57cef4e772232da4b92e8f8931f3cc75, type: 3} + propertyPath: m_RootOrder + value: 10 + objectReference: {fileID: 0} + - target: {fileID: 4360241143132002600, guid: 57cef4e772232da4b92e8f8931f3cc75, type: 3} + propertyPath: m_LocalPosition.x + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 4360241143132002600, guid: 57cef4e772232da4b92e8f8931f3cc75, type: 3} + propertyPath: m_LocalPosition.y + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 4360241143132002600, guid: 57cef4e772232da4b92e8f8931f3cc75, type: 3} + propertyPath: m_LocalPosition.z + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 4360241143132002600, guid: 57cef4e772232da4b92e8f8931f3cc75, type: 3} + propertyPath: m_LocalRotation.w + value: 1 + objectReference: {fileID: 0} + - target: {fileID: 4360241143132002600, guid: 57cef4e772232da4b92e8f8931f3cc75, type: 3} + propertyPath: m_LocalRotation.x + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 4360241143132002600, guid: 57cef4e772232da4b92e8f8931f3cc75, type: 3} + propertyPath: m_LocalRotation.y + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 4360241143132002600, guid: 57cef4e772232da4b92e8f8931f3cc75, type: 3} + propertyPath: m_LocalRotation.z + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 4360241143132002600, guid: 57cef4e772232da4b92e8f8931f3cc75, type: 3} + propertyPath: m_LocalEulerAnglesHint.x + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 4360241143132002600, guid: 57cef4e772232da4b92e8f8931f3cc75, type: 3} + propertyPath: m_LocalEulerAnglesHint.y + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 4360241143132002600, guid: 57cef4e772232da4b92e8f8931f3cc75, type: 3} + propertyPath: m_LocalEulerAnglesHint.z + value: 0 + objectReference: {fileID: 0} + m_RemovedComponents: [] + m_SourcePrefab: {fileID: 100100000, guid: 57cef4e772232da4b92e8f8931f3cc75, type: 3} +--- !u!4 &2742884835330475003 stripped +Transform: + m_CorrespondingSourceObject: {fileID: 4360241143132002600, guid: 57cef4e772232da4b92e8f8931f3cc75, type: 3} + m_PrefabInstance: {fileID: 1914626938635344595} + m_PrefabAsset: {fileID: 0} diff --git a/com.unity.robotics.visualizations/DefaultVisualizationSuite.prefab.meta b/com.unity.robotics.visualizations/DefaultVisualizationSuite.prefab.meta new file mode 100644 index 00000000..2fcdad2b --- /dev/null +++ b/com.unity.robotics.visualizations/DefaultVisualizationSuite.prefab.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 7e8ecfd17cd58c247916864b74797f3f +PrefabImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Documentation~/README.md b/com.unity.robotics.visualizations/Documentation~/README.md new file mode 100644 index 00000000..b06ca516 --- /dev/null +++ b/com.unity.robotics.visualizations/Documentation~/README.md @@ -0,0 +1,132 @@ +# Robotics Visualization 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. + +Get started with the Visualizations Package with our [Nav2-SLAM tutorial](https://github.com/Unity-Technologies/Robotics-Nav2-SLAM-Example)! + +> This package is compatible with ROS 1 and ROS 2, and Unity versions 2020.2+. + +**Table of Contents** +- [Installation](#installation) +- [Configuring a Visualization Suite](#configuring-a-visualization-suite) + - [Priority Setter](#priority-setter) +- [The HUD](#the-hud) +- [Visualization Base Classes](#visualization-base-classes) +- [Using the Inspector](#using-the-inspector) + - [Message Topics](#message-topics) + - [TF Topics and Tracking](#tf-topics-and-tracking) + - [Visualization Settings](#visualization-settings) + - [Joy Messages](#joy-messages) + - [Point Clouds](#point-clouds) + +--- + +## Installation + +1. Using Unity 2020.2 or later, open the Package Manager from `Window` -> `Package Manager`. +2. In the Package Manager window, find and click the + button in the upper lefthand corner of the window. Select `Add package from git URL....` + + ![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`. + + > 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`. + +## Configuring a Visualization Suite + +This package contains a `DefaultVisualizationSuite` prefab, located in the [root of this package](../DefaultVisualizationSuite.prefab) that provides visualizer components for many common ROS message types, organized in the hierarchy by package. These components control how messages are displayed in the Unity scene. You may also create your own visualization suite by creating a GameObject with only the desired default or custom visualizer components for your project. + +The package also contains an `EmptyVisualizationSuite` prefab (also located at the [root of this package](../EmptyVisualizationSuite.prefab)), which contains all necessary components to add visualizations, but none of the default visualizers so you may pick and choose which visualizers you want in the scene. + +The UI windows for visualizations will automatically be laid out as they are turned on, but they can also be dragged and resized. The visualizations in the scene can be customized as described in the [Inspector](#using-the-inspector) section. The topics being visualized and the window configurations are saved between sessions and can be exported and loaded via the HUD's `Layout > Export/Import layout` buttons. + +### Priority Setter + +The [Priority Setter](../Runtime/Scripts/PrioritySetter.cs) allows users to modify which visualization is preferred, in case of multiple visualizers per ROS message type. All default visualizers default to priority `-1`, and custom visualizers will default to `0`, making custom visualizers a higher priority. This means that, when toggling on visualizations in the scene, the higher priority visualizers will be turned on. + +To use the Priority Setter, simply add the PrioritySetter component to the GameObject that holds the visualization you want to modify, and set its `Priority` field value. + +## The HUD + +![](../images~/hud.png) + +The top-left panel in the Game view provides a GUI system that offers tabs to toggle additional information about the state of the ROS communication and visualizations. + +The default tabs on the HUD panel includes: + +- **Topics**: Contains a list of all ROS topics on which this current session has sent or received a message. The `2D` toggle enables a window that shows the last message sent or received on that topic. The `3D` toggle enables an in-scene drawing that represents the last message sent or received on that topic. If no toggle is available, that topic does not have a default visualizer enabled in the Unity scene. + - The Topics tab also contains a search bar that allows you to search for topics. +- **Transforms**: Contains [`tf`](http://wiki.ros.org/tf) visualization options, including displaying the axes, links, and labels for each frame. +- **Layout**: Contains options to save and load this visualization configuration. While the visualization components are by default saved via the scene or the prefab, the window layout and visualized message list is saved as a JSON file. By default, this file is saved to a `RosHudLayout.json` file on your machine's [`Application.persistentDataPath`](https://docs.unity3d.com/ScriptReference/Application-persistentDataPath.html) and loaded on each session. In this Layout tab, you can choose to `Export` this JSON file with a custom name to a chosen location on your device, as well as `Import` a layout JSON file to begin using that saved visualization configuration. + +The HUD is also designed to be customizable; you may add custom tabs or headers to the HUD. You can write a custom script similar to the [VisualizationLayoutTab](../Runtime/Scripts/VisualizationLayoutTab.cs) to extend the HUD. + +> Get started with custom visualization scripts with the TEMP LINK [Nav2: Making a Custom Visualizer](https://github.com/Unity-Technologies/Robotics-Nav2-SLAM-Example/blob/amanda/custom-viz/readmes/custom_viz.md) tutorial! + +## Visualization Base Classes + +TODO + +## Using the Inspector + +The visualizers for each message type are implemented as Unity MonoBehaviours that are added as components to a GameObject in the scene. This is provided via the `DefaultVisualizationSuite` prefab, or in any custom visualization suite. In the `DefaultVisualizationSuite`, each individual default visualizer can be found by expanding the GameObject in the hierarchy and selecting the GameObject corresponding to the message type's package, e.g. `Geometry` for geometry_msgs. + +### Message Topics + +Visualizations, by default, are created based on ROS message types. However, you can also directly assign a topic in the visualizer component's Inspector--you can find the ` Default Visualizer` component by expanding the `DefaultVisualizationSuite` GameObject in the Hierarchy and selecting the child object for the package. In the Inspector window, you will see all the default visualizers provided for this package (you may need to scroll down to see all added components). + +The **Topic** field can be specifically assigned to customize visualizations for only that topic. This is particularly useful for adding multiple default visualizers of the same ROS message type, customized for different topics. + +![](../images~/point_inspector.png) + +### TF Topics and Tracking + +For messages with stamped headers, there is an option to customize the coordinate frame tracking per visualization. This is provided in the applicable default visualizers via the `TF Tracking Settings`, which contains options for a topic string and a type. + +**TF Topic:** It is important to render 3D visualizations in the proper coordinate frame. By default, the `TF Topic` is assigned to `/tf`, but this can be replaced with a different or namespaced TF topic. + +**Tracking Type - Exact:** This setting adds the visualization drawing as a child of the `BasicDrawingManager`. The drawing's transform will be modified directly by looking back in time and using the *exact* transform corresponding to the header's timestamp. + +**Tracking Type - Track Latest:** This setting places the visualization drawing as a child GameObject corresponding to the proper `frame_id`. The drawing will have a zeroed local position and rotation, and the *frame* GameObject will be transformed based on the *latest* transform information. + +**Tracking Type - None:** This setting will set the local position of the drawing to `Vector3.zero` and the local rotation to be `Quaternion.identity`. + +### Visualization Settings + +The 3D visualizations offer customizations such as `label` and `color` fields, which will modify the drawing in the scene. Visualizations including lines or arrows (e.g. `sensor_msgs/Imu`) provide options for the length and thickness of the arrow as well as the radius around which any curved arrows are drawn. These customizations will be specific to each message type. Please note that changes to these settings will not be saved during runtime, and you will have to exit Play mode to save these modifications. + +> Note: Size-related fields are in Unity coordinates, where 1 unit = 1 meter. + +### Joy Messages + +This package contains preconfigured button maps for the Xbox 360 wired and wireless controllers for Windows and Linux mappings, provided as ScriptableObjects in the [TEMP link] [`Resources/VisualizerSettings`](https://github.com/Unity-Technologies/ROS-TCP-Connector/tree/laurie/JoySettings/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ScriptableObjects) directory. + +You can create your own custom mapping for the Joy Default Visualizer by right-clicking in the Project window under `Create > Robotics > Sensor Visualizers > Joy`. Once the file is made, you can click into the asset and manually assign the button or axis index appropriate for your custom controller. + +Once the mapping is done, in your Joy Default Visualizer component (e.g. `DefaultVisualizationSuite/Sensor/JoyDefaultVisualizer`), assign the `Settings` field to your newly made button map. + +### Point Clouds + +Similar to the Visualization Settings, point cloud visualizations are highly customizable. Settings for these visualizers (PointCloud, LaserScan, etc.) will be saved during runtime. For more information on this, you can check out the [TEMP link] [base SettingsBasedVisualizer](../Editor/SettingsBasedVisualizerEditor.cs) class, as well as read more about Unity's [ScriptableObjects](https://docs.unity3d.com/Manual/class-ScriptableObject.html). + +The standard settings are provided in ScriptableObjects. Default settings are provided in the [TEMP link] [`Resources/VisualizerSettings`](https://github.com/Unity-Technologies/ROS-TCP-Connector/tree/laurie/JoySettings/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ScriptableObjects) directory, and can be created by right-clicking in the Project window under `Create > Robotics > Sensor Visualizers`. After being created, this configuration can be dragged and dropped into the component's Inspector field `Visualizer settings,` or selected by clicking on the small circle to the right of the field. + +![](../images~/pcl2.png) + +The settings available will depend on the ROS message type. + +**Channel Name**: These settings allow you to choose which channel name corresponds to X, Y, Z, and color channels. + +**Size**: Each point is by default a uniform size. This toggle allows you to select a channel that defines the size of each drawn point, e.g. using `intensity` to assign scale based on each point's intensity reading. + +**Color**: The color options enable the point clouds to be drawn with configurable colors. + + - The `HSV` option allows you to choose a channel that will be automatically converted to colored points. This can be useful for visualizing individual lasers like a lidar's `ring` channel, for example. + + - The `Combined RGB` option is used for channels that should specifically be parsed into RGB data, e.g. `rgb` channels. + + - The `Separate RGB` is similar to the HSV option, but assigns a different channel to each R, G, and B color channel, which may be used for visualizing X, Y, and Z axes, for example. + +**Range**: The min and max value fields configure the ranges for the sliding bar provided. This setting is applied to the respective range field. \ No newline at end of file diff --git a/com.unity.robotics.visualizations/Editor.meta b/com.unity.robotics.visualizations/Editor.meta new file mode 100644 index 00000000..b6f3e16b --- /dev/null +++ b/com.unity.robotics.visualizations/Editor.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 7d9fe57c3e129dd4e8988e49a3bb96cb +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Editor/LaserScanVisualizerSettingsEditor.cs b/com.unity.robotics.visualizations/Editor/LaserScanVisualizerSettingsEditor.cs new file mode 100644 index 00000000..52cc854a --- /dev/null +++ b/com.unity.robotics.visualizations/Editor/LaserScanVisualizerSettingsEditor.cs @@ -0,0 +1,23 @@ +using RosMessageTypes.Sensor; +using System; +using Unity.Robotics.Visualizations; +using UnityEditor; +using UnityEngine; + +[CustomEditor(typeof(LaserScanDefaultVisualizer))] +public class LaserScanEditor : SettingsBasedVisualizerEditor +{ + public override void OnInspectorGUI() + { + base.OnInspectorGUI(); + + if (m_Config.UseIntensitySize) + { + m_Config.MaxIntensity = float.Parse(EditorGUILayout.TextField("Max Intensity", m_Config.MaxIntensity.ToString())); + } + else + { + m_Config.PointRadius = float.Parse(EditorGUILayout.TextField("Point Radius", m_Config.PointRadius.ToString())); + } + } +} diff --git a/com.unity.robotics.visualizations/Editor/LaserScanVisualizerSettingsEditor.cs.meta b/com.unity.robotics.visualizations/Editor/LaserScanVisualizerSettingsEditor.cs.meta new file mode 100644 index 00000000..5fa67cf4 --- /dev/null +++ b/com.unity.robotics.visualizations/Editor/LaserScanVisualizerSettingsEditor.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c6adbe8a764340b4f93ff3660240f39b +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Editor/MultiEchoLaserScanVisualizerSettingsEditor.cs b/com.unity.robotics.visualizations/Editor/MultiEchoLaserScanVisualizerSettingsEditor.cs new file mode 100644 index 00000000..cb10893d --- /dev/null +++ b/com.unity.robotics.visualizations/Editor/MultiEchoLaserScanVisualizerSettingsEditor.cs @@ -0,0 +1,45 @@ +using RosMessageTypes.Sensor; +using System; +using UnityEditor; +using UnityEngine; + +[CustomEditor(typeof(MultiEchoLaserScanDefaultVisualizer))] +public class MultiEchoLaserScanEditor : SettingsBasedVisualizerEditor +{ + string m_SizeMax = "1000"; + float m_SizeMaxVal = 1000; + string m_SizeMin = "0"; + float m_SizeMinVal; + + void CreateMinMaxSlider(ref float[] range, float min, float max) + { + GUILayout.BeginHorizontal(); + GUILayout.Label(range[0].ToString()); + EditorGUILayout.MinMaxSlider(ref range[0], ref range[1], min, max); + GUILayout.Label(range[1].ToString()); + GUILayout.EndHorizontal(); + } + + void MinMaxText(string label, ref float minVal, ref string minS, ref float maxVal, ref string maxS) + { + minVal = float.Parse(EditorGUILayout.TextField($"Min {label} range:", minS)); + minS = minVal.ToString(); + + maxVal = float.Parse(EditorGUILayout.TextField($"Max {label} range:", maxS)); + maxS = maxVal.ToString(); + } + + public override void OnInspectorGUI() + { + base.OnInspectorGUI(); + + if (m_Config.UseIntensitySize) + { + CreateMinMaxEditor("Size channel min", "Max", m_Config.SizeRange); + } + else + { + m_Config.PointRadius = float.Parse(EditorGUILayout.TextField("Point Radius", m_Config.PointRadius.ToString())); + } + } +} diff --git a/com.unity.robotics.visualizations/Editor/MultiEchoLaserScanVisualizerSettingsEditor.cs.meta b/com.unity.robotics.visualizations/Editor/MultiEchoLaserScanVisualizerSettingsEditor.cs.meta new file mode 100644 index 00000000..dcc3219e --- /dev/null +++ b/com.unity.robotics.visualizations/Editor/MultiEchoLaserScanVisualizerSettingsEditor.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 302833860c33efc4385f3bc07e7ceacc +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Editor/PointCloud2VisualizerSettingsEditor.cs b/com.unity.robotics.visualizations/Editor/PointCloud2VisualizerSettingsEditor.cs new file mode 100644 index 00000000..ab8c7ebd --- /dev/null +++ b/com.unity.robotics.visualizations/Editor/PointCloud2VisualizerSettingsEditor.cs @@ -0,0 +1,67 @@ +using RosMessageTypes.Sensor; +using System; +using Unity.Robotics.Visualizations; +using UnityEditor; +using UnityEngine; +using System.Globalization; +using System.Collections.Generic; + +[CustomEditor(typeof(PointCloud2DefaultVisualizer))] +public class PointCloud2Editor : SettingsBasedVisualizerEditor +{ + public override void OnInspectorGUI() + { + base.OnInspectorGUI(); + + GUI.changed = false; + CreateNewDropdown(m_Config.Channels, "X channel name:", m_Config.XChannel, newChannel => { m_Config.XChannel = newChannel; }); + CreateNewDropdown(m_Config.Channels, "Y channel name:", m_Config.YChannel, newChannel => { m_Config.YChannel = newChannel; }); + CreateNewDropdown(m_Config.Channels, "Z channel name:", m_Config.ZChannel, newChannel => { m_Config.ZChannel = newChannel; }); + + m_Config.UseSizeChannel = EditorGUILayout.ToggleLeft("Use size channel", m_Config.UseSizeChannel); + + if (m_Config.UseSizeChannel) + { + m_Config.Size = EditorGUILayout.FloatField("Max Size:", m_Config.Size); + CreateNewDropdown(m_Config.Channels, "Size channel name:", m_Config.SizeChannel, newChannel => { m_Config.SizeChannel = newChannel; }); + CreateMinMaxEditor("Size channel min", "Max", m_Config.SizeRange); + } + else + { + m_Config.Size = EditorGUILayout.FloatField("Size:", m_Config.Size); + } + + m_Config.UseRgbChannel = EditorGUILayout.ToggleLeft("Use color channel", m_Config.UseRgbChannel); + + if (m_Config.UseRgbChannel) + { + m_Config.ColorModeSetting = (PointCloud2VisualizerSettings.ColorMode)EditorGUILayout.EnumPopup("Color mode", m_Config.ColorModeSetting); + + switch (m_Config.ColorModeSetting) + { + case PointCloud2VisualizerSettings.ColorMode.HSV: + CreateNewDropdown(m_Config.Channels, "Hue channel name:", m_Config.HueChannel, newChannel => { m_Config.HueChannel = newChannel; }); + CreateMinMaxEditor("Hue channel min", "Max", m_Config.HueRange); + break; + case PointCloud2VisualizerSettings.ColorMode.CombinedRGB: + CreateNewDropdown(m_Config.Channels, "Rgb channel name:", m_Config.RgbChannel, newChannel => { m_Config.RgbChannel = newChannel; }); + break; + case PointCloud2VisualizerSettings.ColorMode.SeparateRGB: + CreateNewDropdown(m_Config.Channels, "R channel name:", m_Config.RChannel, newChannel => { m_Config.RChannel = newChannel; }); + CreateMinMaxEditor("R channel min", "Max", m_Config.RRange); + + CreateNewDropdown(m_Config.Channels, "G channel name:", m_Config.GChannel, newChannel => { m_Config.GChannel = newChannel; }); + CreateMinMaxEditor("G channel min", "Max", m_Config.GRange); + + CreateNewDropdown(m_Config.Channels, "B channel name:", m_Config.BChannel, newChannel => { m_Config.BChannel = newChannel; }); + CreateMinMaxEditor("B channel min", "Max", m_Config.BRange); + break; + } + } + + if (GUI.changed) + { + VisualizerRedraw(); + } + } +} diff --git a/com.unity.robotics.visualizations/Editor/PointCloud2VisualizerSettingsEditor.cs.meta b/com.unity.robotics.visualizations/Editor/PointCloud2VisualizerSettingsEditor.cs.meta new file mode 100644 index 00000000..4e96fc70 --- /dev/null +++ b/com.unity.robotics.visualizations/Editor/PointCloud2VisualizerSettingsEditor.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 1e44c9bb80faa2441ae433cf919fb6f6 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Editor/PointCloudVisualizerSettingsEditor.cs b/com.unity.robotics.visualizations/Editor/PointCloudVisualizerSettingsEditor.cs new file mode 100644 index 00000000..a79c3b8c --- /dev/null +++ b/com.unity.robotics.visualizations/Editor/PointCloudVisualizerSettingsEditor.cs @@ -0,0 +1,55 @@ +using System; +using RosMessageTypes.Sensor; +using UnityEditor; +using UnityEngine; + +[CustomEditor(typeof(PointCloudDefaultVisualizer))] +public class PointCloudVisualizerEditor : SettingsBasedVisualizerEditor +{ + public override void OnInspectorGUI() + { + base.OnInspectorGUI(); + m_Config.UseSizeChannel = EditorGUILayout.ToggleLeft("Use size channel", m_Config.UseSizeChannel); + + if (m_Config.UseSizeChannel) + { + m_Config.Size = EditorGUILayout.FloatField("Max Size:", m_Config.Size); + CreateNewDropdown(m_Config.Channels, "Size channel name:", m_Config.SizeChannel, + newChannel => { m_Config.SizeChannel = newChannel; }); + CreateMinMaxEditor("Size channel min", "Max", m_Config.SizeRange); + } + else + { + m_Config.Size = EditorGUILayout.FloatField("Size:", m_Config.Size); + } + + m_Config.UseRgbChannel = EditorGUILayout.ToggleLeft("Use color channel", m_Config.UseRgbChannel); + + if (m_Config.UseRgbChannel) + { + m_Config.ColorMode = + (PointCloud2VisualizerSettings.ColorMode)EditorGUILayout.EnumPopup("Color mode", m_Config.ColorMode); + + switch (m_Config.ColorMode) + { + case PointCloud2VisualizerSettings.ColorMode.HSV: + CreateNewDropdown(m_Config.Channels, "Hue channel name:", m_Config.HueChannel, newChannel => { m_Config.HueChannel = newChannel; }); + CreateMinMaxEditor("Hue channel min", "Max", m_Config.HueRange); + break; + case PointCloud2VisualizerSettings.ColorMode.CombinedRGB: + CreateNewDropdown(m_Config.Channels, "RGB channel name:", m_Config.RgbChannel, newChannel => { m_Config.RgbChannel = newChannel; }); + break; + case PointCloud2VisualizerSettings.ColorMode.SeparateRGB: + CreateNewDropdown(m_Config.Channels, "R channel name:", m_Config.RChannel, newChannel => { m_Config.RChannel = newChannel; }); + CreateMinMaxEditor("R channel min", "Max", m_Config.RRange); + + CreateNewDropdown(m_Config.Channels, "G channel name:", m_Config.GChannel, newChannel => { m_Config.GChannel = newChannel; }); + CreateMinMaxEditor("G channel min", "Max", m_Config.GRange); + + CreateNewDropdown(m_Config.Channels, "B channel name:", m_Config.BChannel, newChannel => { m_Config.BChannel = newChannel; }); + CreateMinMaxEditor("B channel min", "Max", m_Config.BRange); + break; + } + } + } +} diff --git a/com.unity.robotics.visualizations/Editor/PointCloudVisualizerSettingsEditor.cs.meta b/com.unity.robotics.visualizations/Editor/PointCloudVisualizerSettingsEditor.cs.meta new file mode 100644 index 00000000..1e1637f9 --- /dev/null +++ b/com.unity.robotics.visualizations/Editor/PointCloudVisualizerSettingsEditor.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 35715219bbb33ce45882b5554f07a4fd +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Editor/SettingsBasedVisualizerEditor.cs b/com.unity.robotics.visualizations/Editor/SettingsBasedVisualizerEditor.cs new file mode 100644 index 00000000..86ce2e5d --- /dev/null +++ b/com.unity.robotics.visualizations/Editor/SettingsBasedVisualizerEditor.cs @@ -0,0 +1,89 @@ +using System.Collections; +using System.Collections.Generic; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using UnityEditor; +using UnityEngine; + +public abstract class SettingsBasedVisualizerEditor : Editor + where TMessageType : Message + where TVisualizerSettings : VisualizerSettingsGeneric +{ + protected TVisualizerSettings m_Config; + Editor m_Editor; + + public override void OnInspectorGUI() + { + DrawingVisualizerWithSettings visualizer = (DrawingVisualizerWithSettings)target; + visualizer.Topic = EditorGUILayout.TextField("Topic", visualizer.Topic); + + m_Config = visualizer.Settings; + m_Config = (TVisualizerSettings)EditorGUILayout.ObjectField("Visualizer settings", m_Config, typeof(TVisualizerSettings), false); + visualizer.Settings = m_Config; + + if (GUI.changed) + { + EditorUtility.SetDirty(target); + if (visualizer.gameObject.scene != null) + { + UnityEditor.SceneManagement.EditorSceneManager.MarkSceneDirty(visualizer.gameObject.scene); + } + GUI.changed = false; + } + + EditorGUI.indentLevel++; + OnInspectorGUIForSettings(visualizer); + EditorGUI.indentLevel--; + + if (GUI.changed) + { + visualizer.Redraw(); + } + } + + public void VisualizerRedraw() + { + DrawingVisualizerWithSettings visualizer = (DrawingVisualizerWithSettings)target; + visualizer.Redraw(); + } + + protected virtual void OnInspectorGUIForSettings(DrawingVisualizerWithSettings visualizer) + { + CreateCachedEditor(m_Config, null, ref m_Editor); + m_Editor.OnInspectorGUI(); + } + + public void CreateNewDropdown(string[] channels, string label, string channel, System.Action action) + { + if (channels == null) + return; + + GUILayout.BeginHorizontal(); + GUILayout.Label(label); + if (EditorGUILayout.DropdownButton(new GUIContent(channel), FocusType.Keyboard)) + { + var menu = new GenericMenu(); + foreach (var c in channels) + menu.AddItem(new GUIContent(c), c == channel, () => + { + action(c); + VisualizerRedraw(); + }); + menu.DropDown(new Rect(Event.current.mousePosition.x, Event.current.mousePosition.y, 0f, 0f)); + } + + GUILayout.EndHorizontal(); + } + + // NB, this modifies the 'range' array in place + public void CreateMinMaxEditor(string label1, string label2, float[] range) + { + GUILayout.BeginHorizontal(); + range[0] = EditorGUILayout.FloatField(label1, range[0]); + //GUILayout.Space(30); + GUILayout.Label(label2); + range[1] = EditorGUILayout.FloatField(range[1], GUILayout.Width(70)); + GUILayout.EndHorizontal(); + } + +} diff --git a/com.unity.robotics.visualizations/Editor/SettingsBasedVisualizerEditor.cs.meta b/com.unity.robotics.visualizations/Editor/SettingsBasedVisualizerEditor.cs.meta new file mode 100644 index 00000000..f581ddb6 --- /dev/null +++ b/com.unity.robotics.visualizations/Editor/SettingsBasedVisualizerEditor.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 5cabae8e7d7996349b2df8c3b779682b +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Editor/Unity.Robotics.Visualizations.Editor.asmdef b/com.unity.robotics.visualizations/Editor/Unity.Robotics.Visualizations.Editor.asmdef new file mode 100644 index 00000000..8f9a11d8 --- /dev/null +++ b/com.unity.robotics.visualizations/Editor/Unity.Robotics.Visualizations.Editor.asmdef @@ -0,0 +1,20 @@ +{ + "name": "Unity.Robotics.Visualizations.Editor", + "rootNamespace": "", + "references": [ + "GUID:625bfc588fb96c74696858f2c467e978", + "GUID:b1ef917f7a8a86a4eb639ec2352edbf8", + "GUID:3f8053c1a58fb7f47b2af9d34d2e4b1e" + ], + "includePlatforms": [ + "Editor" + ], + "excludePlatforms": [], + "allowUnsafeCode": false, + "overrideReferences": false, + "precompiledReferences": [], + "autoReferenced": true, + "defineConstraints": [], + "versionDefines": [], + "noEngineReferences": false +} diff --git a/com.unity.robotics.visualizations/Editor/Unity.Robotics.Visualizations.Editor.asmdef.meta b/com.unity.robotics.visualizations/Editor/Unity.Robotics.Visualizations.Editor.asmdef.meta new file mode 100644 index 00000000..5362c1a2 --- /dev/null +++ b/com.unity.robotics.visualizations/Editor/Unity.Robotics.Visualizations.Editor.asmdef.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 4743715d909f1d248997c091ead581ec +AssemblyDefinitionImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Editor/VisualizationEditorWindow.cs b/com.unity.robotics.visualizations/Editor/VisualizationEditorWindow.cs new file mode 100644 index 00000000..459aed5f --- /dev/null +++ b/com.unity.robotics.visualizations/Editor/VisualizationEditorWindow.cs @@ -0,0 +1,122 @@ +using System.Collections; +using System.Collections.Generic; +using UnityEditor; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public class VisualizationEditorWindow : EditorWindow + { + IVisualFactory m_VisualFactory; + [SerializeField] + string m_Topic; + [SerializeField] + string m_RosMessageName; + [SerializeField] + string m_FactoryID; + IVisual m_Visual; + Vector2 m_ScrollPos; + + public VisualizationEditorWindow(IVisualFactory factory, string topic, string rosMessageName) + { + m_VisualFactory = factory; + m_Topic = topic; + m_RosMessageName = rosMessageName; + SetFactory(factory); + EditorApplication.playModeStateChanged += OnPlayModeState; + titleContent = new GUIContent(m_Topic); + } + + void OnPlayModeState(PlayModeStateChange state) + { + if (state == PlayModeStateChange.EnteredEditMode) + { + m_VisualFactory = null; + m_Visual = null; + } + } + + public void SetFactory(IVisualFactory factory) + { + m_VisualFactory = factory; + m_FactoryID = factory.ID; + m_Visual = null; + } + + private void OnGUI() + { + if (!EditorApplication.isPlaying) + { + GUILayout.Label("Waiting for play mode..."); + return; + } + + if (m_Visual == null && EditorApplication.isPlaying) + { + if (m_FactoryID == null) + { + GUIContent buttonContent = new GUIContent("Select Visualizer"); + Rect selectBtnRect = GUILayoutUtility.GetRect(buttonContent, EditorStyles.toolbarDropDown, GUILayout.ExpandWidth(false)); + if (EditorGUI.DropdownButton(selectBtnRect, buttonContent, FocusType.Keyboard)) + { + GenericMenu menu = new GenericMenu(); + foreach (IVisualFactory factory in VisualFactoryRegistry.GetAllVisualFactories(m_Topic, m_RosMessageName)) + { + menu.AddItem(new GUIContent(factory.Name), false, () => SetFactory(factory)); + } + menu.DropDown(selectBtnRect); + } + } + else if (m_VisualFactory == null) + { + foreach (IVisualFactory factory in VisualFactoryRegistry.GetAllVisualFactories(m_Topic, m_RosMessageName)) + { + if (factory.ID == m_FactoryID) + { + m_VisualFactory = factory; + } + } + if (m_VisualFactory == null) + m_FactoryID = null; + } + + if (m_VisualFactory != null) + m_Visual = m_VisualFactory.GetOrCreateVisual(m_Topic); + } + + if (m_Visual != null) + { + GUILayout.BeginHorizontal(GUI.skin.box); + EditorGUI.BeginDisabledGroup(m_VisualFactory == null || !m_VisualFactory.CanShowDrawing); + bool drawingEnable = EditorGUILayout.ToggleLeft("Show 3d drawings", m_Visual.IsDrawingEnabled); + EditorGUI.EndDisabledGroup(); + EditorGUI.BeginDisabledGroup(!(m_VisualFactory is Object)); + if (GUILayout.Button("Select in Editor")) + { + Object factoryObject = (Object)m_VisualFactory; + Selection.activeObject = factoryObject; + } + EditorGUI.EndDisabledGroup(); + GUILayout.EndHorizontal(); + if (m_Visual.IsDrawingEnabled != drawingEnable) + m_Visual.SetDrawingEnabled(drawingEnable); + + m_ScrollPos = GUILayout.BeginScrollView(m_ScrollPos); + m_Visual.OnGUI(); + GUILayout.EndScrollView(); + } + } + + [InitializeOnLoadMethod] + static void Initialize() + { + VisualizationTopicsTabEntry.SetOpenWindowCallback(OpenWindow); + } + + static void OpenWindow(IVisualFactory visualizer, string topic, string rosMessageName) + { + VisualizationEditorWindow window = new VisualizationEditorWindow(visualizer, topic, rosMessageName); + window.Show(); + } + } +} diff --git a/com.unity.robotics.visualizations/Editor/VisualizationEditorWindow.cs.meta b/com.unity.robotics.visualizations/Editor/VisualizationEditorWindow.cs.meta new file mode 100644 index 00000000..bcc203f2 --- /dev/null +++ b/com.unity.robotics.visualizations/Editor/VisualizationEditorWindow.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: add5fb33ad64f3f40a87b97da181458b +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/EmptyVisualizationSuite.prefab b/com.unity.robotics.visualizations/EmptyVisualizationSuite.prefab new file mode 100644 index 00000000..f9273f5b --- /dev/null +++ b/com.unity.robotics.visualizations/EmptyVisualizationSuite.prefab @@ -0,0 +1,124 @@ +%YAML 1.1 +%TAG !u! tag:unity3d.com,2011: +--- !u!1 &3885009520023495737 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 3885009520023495739} + - component: {fileID: 8652144247881302430} + - component: {fileID: 4180636070010704716} + m_Layer: 0 + m_Name: BlankVisualizationSuite + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &3885009520023495739 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 3885009520023495737} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: + - {fileID: 2742884835330475003} + m_Father: {fileID: 0} + m_RootOrder: 0 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &8652144247881302430 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 3885009520023495737} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 89775f84ec1b05048997be0d603cca35, type: 3} + m_Name: + m_EditorClassIdentifier: + axesScale: 0.1 + lineThickness: 0.01 + color: {r: 1, g: 1, b: 1, a: 1} +--- !u!114 &4180636070010704716 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 3885009520023495737} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: e120188e851b6f44b85f38aecc703ba6, type: 3} + m_Name: + m_EditorClassIdentifier: +--- !u!1001 &1914626938635344595 +PrefabInstance: + m_ObjectHideFlags: 0 + serializedVersion: 2 + m_Modification: + m_TransformParent: {fileID: 3885009520023495739} + m_Modifications: + - target: {fileID: 863246206611208022, guid: 57cef4e772232da4b92e8f8931f3cc75, type: 3} + propertyPath: m_Name + value: BasicDrawingManager + objectReference: {fileID: 0} + - target: {fileID: 4360241143132002600, guid: 57cef4e772232da4b92e8f8931f3cc75, type: 3} + propertyPath: m_RootOrder + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 4360241143132002600, guid: 57cef4e772232da4b92e8f8931f3cc75, type: 3} + propertyPath: m_LocalPosition.x + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 4360241143132002600, guid: 57cef4e772232da4b92e8f8931f3cc75, type: 3} + propertyPath: m_LocalPosition.y + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 4360241143132002600, guid: 57cef4e772232da4b92e8f8931f3cc75, type: 3} + propertyPath: m_LocalPosition.z + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 4360241143132002600, guid: 57cef4e772232da4b92e8f8931f3cc75, type: 3} + propertyPath: m_LocalRotation.w + value: 1 + objectReference: {fileID: 0} + - target: {fileID: 4360241143132002600, guid: 57cef4e772232da4b92e8f8931f3cc75, type: 3} + propertyPath: m_LocalRotation.x + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 4360241143132002600, guid: 57cef4e772232da4b92e8f8931f3cc75, type: 3} + propertyPath: m_LocalRotation.y + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 4360241143132002600, guid: 57cef4e772232da4b92e8f8931f3cc75, type: 3} + propertyPath: m_LocalRotation.z + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 4360241143132002600, guid: 57cef4e772232da4b92e8f8931f3cc75, type: 3} + propertyPath: m_LocalEulerAnglesHint.x + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 4360241143132002600, guid: 57cef4e772232da4b92e8f8931f3cc75, type: 3} + propertyPath: m_LocalEulerAnglesHint.y + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 4360241143132002600, guid: 57cef4e772232da4b92e8f8931f3cc75, type: 3} + propertyPath: m_LocalEulerAnglesHint.z + value: 0 + objectReference: {fileID: 0} + m_RemovedComponents: [] + m_SourcePrefab: {fileID: 100100000, guid: 57cef4e772232da4b92e8f8931f3cc75, type: 3} +--- !u!4 &2742884835330475003 stripped +Transform: + m_CorrespondingSourceObject: {fileID: 4360241143132002600, guid: 57cef4e772232da4b92e8f8931f3cc75, type: 3} + m_PrefabInstance: {fileID: 1914626938635344595} + m_PrefabAsset: {fileID: 0} diff --git a/com.unity.robotics.visualizations/EmptyVisualizationSuite.prefab.meta b/com.unity.robotics.visualizations/EmptyVisualizationSuite.prefab.meta new file mode 100644 index 00000000..cc588407 --- /dev/null +++ b/com.unity.robotics.visualizations/EmptyVisualizationSuite.prefab.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 8968dd66bff86df47bc992562d7a9082 +PrefabImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Resources.meta b/com.unity.robotics.visualizations/Resources.meta new file mode 100644 index 00000000..41302c48 --- /dev/null +++ b/com.unity.robotics.visualizations/Resources.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: a6da88139e73b404a83fe719fc82af90 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Resources/VisualizerSettings.meta b/com.unity.robotics.visualizations/Resources/VisualizerSettings.meta new file mode 100644 index 00000000..f7eee668 --- /dev/null +++ b/com.unity.robotics.visualizations/Resources/VisualizerSettings.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 446495620753543beaf7487d9451c1bd +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Resources/VisualizerSettings/JoyVisualizerSettings_Xbox360LinuxWired.asset b/com.unity.robotics.visualizations/Resources/VisualizerSettings/JoyVisualizerSettings_Xbox360LinuxWired.asset new file mode 100644 index 00000000..610456fc --- /dev/null +++ b/com.unity.robotics.visualizations/Resources/VisualizerSettings/JoyVisualizerSettings_Xbox360LinuxWired.asset @@ -0,0 +1,37 @@ +%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: 3e64381bafaf07f41b8194aec986fe7e, type: 3} + m_Name: JoyVisualizerSettings_Xbox360LinuxWired + m_EditorClassIdentifier: + m_Button_South: 12 + m_Button_East: 13 + m_Button_West: 14 + m_Button_North: 15 + m_Button_Back: 18 + m_Button_Start: 19 + m_Button_Power: 20 + m_DPad_Up: 0 + m_DPad_Down: 0 + m_DPad_Left: 0 + m_DPad_Right: 0 + m_DPad_XAxis: 7 + m_DPad_YAxis: 8 + m_LStick_X: 1 + m_LStick_Y: 2 + m_LStick_Click: 21 + m_LShoulder: 16 + m_LTrigger: 6 + m_RStick_X: 3 + m_RStick_Y: 4 + m_RStick_Click: 22 + m_RShoulder: 17 + m_RTrigger: 5 diff --git a/com.unity.robotics.visualizations/Resources/VisualizerSettings/JoyVisualizerSettings_Xbox360LinuxWired.asset.meta b/com.unity.robotics.visualizations/Resources/VisualizerSettings/JoyVisualizerSettings_Xbox360LinuxWired.asset.meta new file mode 100644 index 00000000..80711fe3 --- /dev/null +++ b/com.unity.robotics.visualizations/Resources/VisualizerSettings/JoyVisualizerSettings_Xbox360LinuxWired.asset.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 44f092bafc464034fa6e4e64241e7d85 +NativeFormatImporter: + externalObjects: {} + mainObjectFileID: 11400000 + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Resources/VisualizerSettings/JoyVisualizerSettings_Xbox360LinuxWireless.asset b/com.unity.robotics.visualizations/Resources/VisualizerSettings/JoyVisualizerSettings_Xbox360LinuxWireless.asset new file mode 100644 index 00000000..ebb4532d --- /dev/null +++ b/com.unity.robotics.visualizations/Resources/VisualizerSettings/JoyVisualizerSettings_Xbox360LinuxWireless.asset @@ -0,0 +1,37 @@ +%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: 3e64381bafaf07f41b8194aec986fe7e, type: 3} + m_Name: JoyVisualizerSettings_Xbox360LinuxWireless + m_EditorClassIdentifier: + m_Button_South: 12 + m_Button_East: 13 + m_Button_West: 14 + m_Button_North: 15 + m_Button_Back: 18 + m_Button_Start: 19 + m_Button_Power: 20 + m_DPad_Up: 0 + m_DPad_Down: 0 + m_DPad_Left: 0 + m_DPad_Right: 0 + m_DPad_XAxis: 7 + m_DPad_YAxis: 8 + m_LStick_X: 1 + m_LStick_Y: 2 + m_LStick_Click: 21 + m_LShoulder: 16 + m_LTrigger: 6 + m_RStick_X: 3 + m_RStick_Y: 4 + m_RStick_Click: 22 + m_RShoulder: 17 + m_RTrigger: 5 diff --git a/com.unity.robotics.visualizations/Resources/VisualizerSettings/JoyVisualizerSettings_Xbox360LinuxWireless.asset.meta b/com.unity.robotics.visualizations/Resources/VisualizerSettings/JoyVisualizerSettings_Xbox360LinuxWireless.asset.meta new file mode 100644 index 00000000..15dda23a --- /dev/null +++ b/com.unity.robotics.visualizations/Resources/VisualizerSettings/JoyVisualizerSettings_Xbox360LinuxWireless.asset.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 39362f79ba2ad754a889a171562c7eea +NativeFormatImporter: + externalObjects: {} + mainObjectFileID: 11400000 + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Resources/VisualizerSettings/JoyVisualizerSettings_Xbox360Windows.asset b/com.unity.robotics.visualizations/Resources/VisualizerSettings/JoyVisualizerSettings_Xbox360Windows.asset new file mode 100644 index 00000000..af175314 --- /dev/null +++ b/com.unity.robotics.visualizations/Resources/VisualizerSettings/JoyVisualizerSettings_Xbox360Windows.asset @@ -0,0 +1,40 @@ +%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: 3e64381bafaf07f41b8194aec986fe7e, type: 3} + m_Name: JoyVisualizerSettings_Xbox360Windows + m_EditorClassIdentifier: + m_TFTrackingSettings: + type: 1 + tfTopic: /tf + m_Button_South: 12 + m_Button_East: 13 + m_Button_West: 14 + m_Button_North: 15 + m_Button_Back: 0 + m_Button_Start: 19 + m_Button_Power: 20 + m_DPad_Up: 22 + m_DPad_Down: 23 + m_DPad_Left: 24 + m_DPad_Right: 25 + m_DPad_XAxis: 0 + m_DPad_YAxis: 0 + m_LStick_X: 1 + m_LStick_Y: 2 + m_LStick_Click: 0 + m_LShoulder: 16 + m_LTrigger: 6 + m_RStick_X: 3 + m_RStick_Y: 4 + m_RStick_Click: 0 + m_RShoulder: 17 + m_RTrigger: 5 diff --git a/com.unity.robotics.visualizations/Resources/VisualizerSettings/JoyVisualizerSettings_Xbox360Windows.asset.meta b/com.unity.robotics.visualizations/Resources/VisualizerSettings/JoyVisualizerSettings_Xbox360Windows.asset.meta new file mode 100644 index 00000000..90fcca16 --- /dev/null +++ b/com.unity.robotics.visualizations/Resources/VisualizerSettings/JoyVisualizerSettings_Xbox360Windows.asset.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 7502d623bf6f41343b1e21d0160765cc +NativeFormatImporter: + externalObjects: {} + mainObjectFileID: 11400000 + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Resources/VisualizerSettings/LaserScanFerrybotVisualizerSettings.asset b/com.unity.robotics.visualizations/Resources/VisualizerSettings/LaserScanFerrybotVisualizerSettings.asset new file mode 100644 index 00000000..badde39d --- /dev/null +++ b/com.unity.robotics.visualizations/Resources/VisualizerSettings/LaserScanFerrybotVisualizerSettings.asset @@ -0,0 +1,20 @@ +%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: b7a16d0bc53a1492bb43ad71301a4a2e, type: 3} + m_Name: LaserScanFerrybotVisualizerSettings + m_EditorClassIdentifier: + m_TFTrackingType: 1 + m_UseIntensitySize: 0 + m_PointRadius: 0.1 + m_MaxIntensity: 100 + m_TFTopic: /ferrybot/tf + m_ColorMode: 2 diff --git a/com.unity.robotics.visualizations/Resources/VisualizerSettings/LaserScanFerrybotVisualizerSettings.asset.meta b/com.unity.robotics.visualizations/Resources/VisualizerSettings/LaserScanFerrybotVisualizerSettings.asset.meta new file mode 100644 index 00000000..42b89507 --- /dev/null +++ b/com.unity.robotics.visualizations/Resources/VisualizerSettings/LaserScanFerrybotVisualizerSettings.asset.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 61008a554349cfb439d80db19c3e6f9c +NativeFormatImporter: + externalObjects: {} + mainObjectFileID: 0 + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Resources/VisualizerSettings/LaserScanFindbot2VisualizerSettings.asset b/com.unity.robotics.visualizations/Resources/VisualizerSettings/LaserScanFindbot2VisualizerSettings.asset new file mode 100644 index 00000000..a1cea9c6 --- /dev/null +++ b/com.unity.robotics.visualizations/Resources/VisualizerSettings/LaserScanFindbot2VisualizerSettings.asset @@ -0,0 +1,20 @@ +%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: b7a16d0bc53a1492bb43ad71301a4a2e, type: 3} + m_Name: LaserScanFindbot2VisualizerSettings + m_EditorClassIdentifier: + m_TFTrackingType: 1 + m_UseIntensitySize: 0 + m_PointRadius: 0.1 + m_MaxIntensity: 100 + m_TFTopic: /findbot2/tf + m_ColorMode: 2 diff --git a/com.unity.robotics.visualizations/Resources/VisualizerSettings/LaserScanFindbot2VisualizerSettings.asset.meta b/com.unity.robotics.visualizations/Resources/VisualizerSettings/LaserScanFindbot2VisualizerSettings.asset.meta new file mode 100644 index 00000000..d0da7cc0 --- /dev/null +++ b/com.unity.robotics.visualizations/Resources/VisualizerSettings/LaserScanFindbot2VisualizerSettings.asset.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 2820c2f76e52c9e418bb8ed03b111b20 +NativeFormatImporter: + externalObjects: {} + mainObjectFileID: 0 + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Resources/VisualizerSettings/LaserScanFindbotVisualizerSettings.asset b/com.unity.robotics.visualizations/Resources/VisualizerSettings/LaserScanFindbotVisualizerSettings.asset new file mode 100644 index 00000000..69fd92fd --- /dev/null +++ b/com.unity.robotics.visualizations/Resources/VisualizerSettings/LaserScanFindbotVisualizerSettings.asset @@ -0,0 +1,20 @@ +%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: b7a16d0bc53a1492bb43ad71301a4a2e, type: 3} + m_Name: LaserScanFindbotVisualizerSettings + m_EditorClassIdentifier: + m_TFTrackingType: 1 + m_UseIntensitySize: 0 + m_PointRadius: 0.1 + m_MaxIntensity: 100 + m_TFTopic: /findbot/tf + m_ColorMode: 2 diff --git a/com.unity.robotics.visualizations/Resources/VisualizerSettings/LaserScanFindbotVisualizerSettings.asset.meta b/com.unity.robotics.visualizations/Resources/VisualizerSettings/LaserScanFindbotVisualizerSettings.asset.meta new file mode 100644 index 00000000..fbc28500 --- /dev/null +++ b/com.unity.robotics.visualizations/Resources/VisualizerSettings/LaserScanFindbotVisualizerSettings.asset.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 09d3908a6e6e7bc42a2ade90bd55175b +NativeFormatImporter: + externalObjects: {} + mainObjectFileID: 0 + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Resources/VisualizerSettings/LaserScanVisualizerSettings.asset b/com.unity.robotics.visualizations/Resources/VisualizerSettings/LaserScanVisualizerSettings.asset new file mode 100644 index 00000000..78560764 --- /dev/null +++ b/com.unity.robotics.visualizations/Resources/VisualizerSettings/LaserScanVisualizerSettings.asset @@ -0,0 +1,20 @@ +%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: b7a16d0bc53a1492bb43ad71301a4a2e, type: 3} + m_Name: LaserScanVisualizerSettings + m_EditorClassIdentifier: + m_TFTrackingType: 1 + m_UseIntensitySize: 0 + m_PointRadius: 0.1 + m_MaxIntensity: 100 + m_TFTopic: /tf + m_ColorMode: 2 diff --git a/com.unity.robotics.visualizations/Resources/VisualizerSettings/LaserScanVisualizerSettings.asset.meta b/com.unity.robotics.visualizations/Resources/VisualizerSettings/LaserScanVisualizerSettings.asset.meta new file mode 100644 index 00000000..094385c8 --- /dev/null +++ b/com.unity.robotics.visualizations/Resources/VisualizerSettings/LaserScanVisualizerSettings.asset.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: f53165fb8c4d340e3ad973ec7784242e +NativeFormatImporter: + externalObjects: {} + mainObjectFileID: 0 + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Resources/VisualizerSettings/MultiEchoLaserScanVisualizerSettings.asset b/com.unity.robotics.visualizations/Resources/VisualizerSettings/MultiEchoLaserScanVisualizerSettings.asset new file mode 100644 index 00000000..83cbdc6f --- /dev/null +++ b/com.unity.robotics.visualizations/Resources/VisualizerSettings/MultiEchoLaserScanVisualizerSettings.asset @@ -0,0 +1,19 @@ +%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: 0234f04e5173b4e9e8a3bcae2afcde80, type: 3} + m_Name: MultiEchoLaserScanVisualizerSettings + m_EditorClassIdentifier: + m_UseIntensitySize: 0 + m_PointRadius: 0.05 + m_SizeRange: + - 0 + - 100 diff --git a/com.unity.robotics.visualizations/Resources/VisualizerSettings/MultiEchoLaserScanVisualizerSettings.asset.meta b/com.unity.robotics.visualizations/Resources/VisualizerSettings/MultiEchoLaserScanVisualizerSettings.asset.meta new file mode 100644 index 00000000..7f8e7b2f --- /dev/null +++ b/com.unity.robotics.visualizations/Resources/VisualizerSettings/MultiEchoLaserScanVisualizerSettings.asset.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 1d15599f177254e03b3698260c33d600 +NativeFormatImporter: + externalObjects: {} + mainObjectFileID: 0 + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Resources/VisualizerSettings/PointCloud2VisualizerSettings.asset b/com.unity.robotics.visualizations/Resources/VisualizerSettings/PointCloud2VisualizerSettings.asset new file mode 100644 index 00000000..92fec9d7 --- /dev/null +++ b/com.unity.robotics.visualizations/Resources/VisualizerSettings/PointCloud2VisualizerSettings.asset @@ -0,0 +1,41 @@ +%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: 9869b969f021440789a37afecf3ffca4, type: 3} + m_Name: PointCloud2VisualizerSettings + m_EditorClassIdentifier: + colorMode: 0 + m_XChannel: x + m_YChannel: y + m_ZChannel: z + m_RgbChannel: x + m_RChannel: x + m_GChannel: y + m_BChannel: z + m_SizeChannel: x + m_RgbRange: + - 0 + - 31 + m_RRange: + - -100 + - 100 + m_GRange: + - -100 + - 100 + m_BRange: + - -100 + - 100 + m_SizeRange: + - 0 + - 100 + m_Size: 0.01 + m_UseRgbChannel: 0 + m_UseSizeChannel: 0 diff --git a/com.unity.robotics.visualizations/Resources/VisualizerSettings/PointCloud2VisualizerSettings.asset.meta b/com.unity.robotics.visualizations/Resources/VisualizerSettings/PointCloud2VisualizerSettings.asset.meta new file mode 100644 index 00000000..754eb7ec --- /dev/null +++ b/com.unity.robotics.visualizations/Resources/VisualizerSettings/PointCloud2VisualizerSettings.asset.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: f1d2df8bcd83d4410b03e8a6ddd5eac4 +NativeFormatImporter: + externalObjects: {} + mainObjectFileID: 0 + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Resources/VisualizerSettings/PointCloudVisualizerSettings.asset b/com.unity.robotics.visualizations/Resources/VisualizerSettings/PointCloudVisualizerSettings.asset new file mode 100644 index 00000000..0abe3366 --- /dev/null +++ b/com.unity.robotics.visualizations/Resources/VisualizerSettings/PointCloudVisualizerSettings.asset @@ -0,0 +1,38 @@ +%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: 5116417a614f44321b9dd939cff73583, type: 3} + m_Name: PointCloudVisualizerSettings + m_EditorClassIdentifier: + colorMode: 0 + m_RgbChannel: x + m_RChannel: x + m_GChannel: y + m_BChannel: z + m_SizeChannel: x + m_RgbRange: + - 0 + - 31 + m_RRange: + - -100 + - 100 + m_GRange: + - -100 + - 100 + m_BRange: + - -100 + - 100 + m_SizeRange: + - 0 + - 100 + m_Size: 0.05 + m_UseRgbChannel: 0 + m_UseSizeChannel: 0 diff --git a/com.unity.robotics.visualizations/Resources/VisualizerSettings/PointCloudVisualizerSettings.asset.meta b/com.unity.robotics.visualizations/Resources/VisualizerSettings/PointCloudVisualizerSettings.asset.meta new file mode 100644 index 00000000..8df2e272 --- /dev/null +++ b/com.unity.robotics.visualizations/Resources/VisualizerSettings/PointCloudVisualizerSettings.asset.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 1b7359921b91a4830ae57b1c5b22caeb +NativeFormatImporter: + externalObjects: {} + mainObjectFileID: 0 + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime.meta b/com.unity.robotics.visualizations/Runtime.meta new file mode 100644 index 00000000..6d7f9a76 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 93a2f4eec24ea4242acc343ee1da42d5 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers.meta new file mode 100644 index 00000000..38dcbada --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 662f22594ed391c4d9361b2e598b0835 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Actionlib.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Actionlib.meta new file mode 100644 index 00000000..8403fb6a --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Actionlib.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: e88249d46ec684f44ab881fcb9198fc1 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Actionlib/GoalIDDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Actionlib/GoalIDDefaultVisualizer.cs new file mode 100644 index 00000000..fa8140f2 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Actionlib/GoalIDDefaultVisualizer.cs @@ -0,0 +1,20 @@ +using System; +using RosMessageTypes.Actionlib; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public class GoalIDDefaultVisualizer : GuiVisualizer + { + public override Action CreateGUI(GoalIDMsg message, MessageMetadata meta) => () => + { + GUI(message); + }; + + public static void GUI(GoalIDMsg message) + { + message.stamp.GUI(); + GUILayout.Label($"ID: {message.id}"); + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Actionlib/GoalIDDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Actionlib/GoalIDDefaultVisualizer.cs.meta new file mode 100644 index 00000000..a74fa5bb --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Actionlib/GoalIDDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 07c239177cd9749e0865f008864d28b7 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Actionlib/GoalStatusArrayDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Actionlib/GoalStatusArrayDefaultVisualizer.cs new file mode 100644 index 00000000..e6baf88c --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Actionlib/GoalStatusArrayDefaultVisualizer.cs @@ -0,0 +1,21 @@ +using System; +using RosMessageTypes.Actionlib; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public class GoalStatusArrayDefaultVisualizer : GuiVisualizer + { + public override Action CreateGUI(GoalStatusArrayMsg message, MessageMetadata meta) => () => + { + GUI(message); + }; + + public static void GUI(GoalStatusArrayMsg message) + { + message.header.GUI(); + foreach (GoalStatusMsg status in message.status_list) + GoalStatusDefaultVisualizer.GUI(status); + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Actionlib/GoalStatusArrayDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Actionlib/GoalStatusArrayDefaultVisualizer.cs.meta new file mode 100644 index 00000000..8890544a --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Actionlib/GoalStatusArrayDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 9dc27fe6fcdc64cef82392ec66c6ffdf +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Actionlib/GoalStatusDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Actionlib/GoalStatusDefaultVisualizer.cs new file mode 100644 index 00000000..844f35d5 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Actionlib/GoalStatusDefaultVisualizer.cs @@ -0,0 +1,26 @@ +using System; +using RosMessageTypes.Actionlib; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public class GoalStatusDefaultVisualizer : GuiVisualizer + { + public override Action CreateGUI(GoalStatusMsg message, MessageMetadata meta) => () => + { + GUI(message); + }; + + public static string[] s_GoalStatusTable = new string[] + { + "PENDING","ACTIVE","PREEMPTED","SUCCEEDED","ABORTED","REJECTED","PREEMPTING","RECALLING","RECALLED","LOST" + }; + + public static void GUI(GoalStatusMsg message) + { + string status = (message.status >= 0 && message.status < s_GoalStatusTable.Length) ? s_GoalStatusTable[message.status] : $"INVALID({message.status})"; + GUILayout.Label($"Status: {message.goal_id} = {status}"); + GUILayout.Label(message.text); + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Actionlib/GoalStatusDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Actionlib/GoalStatusDefaultVisualizer.cs.meta new file mode 100644 index 00000000..4ea00563 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Actionlib/GoalStatusDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0a5b3782d48ba46d485890cf446ad3e2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Diagnostic.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Diagnostic.meta new file mode 100644 index 00000000..591d5900 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Diagnostic.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: d3f86c69175b74df8ab49616226a3d41 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Diagnostic/DiagnosticArrayDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Diagnostic/DiagnosticArrayDefaultVisualizer.cs new file mode 100644 index 00000000..f14cf5e2 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Diagnostic/DiagnosticArrayDefaultVisualizer.cs @@ -0,0 +1,21 @@ +using System; +using RosMessageTypes.Diagnostic; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public class DiagnosticArrayDefaultVisualizer : GuiVisualizer + { + public override Action CreateGUI(DiagnosticArrayMsg message, MessageMetadata meta) => () => + { + GUI(message); + }; + + public static void GUI(DiagnosticArrayMsg message) + { + message.header.GUI(); + foreach (DiagnosticStatusMsg status in message.status) + DiagnosticStatusDefaultVisualizer.GUI(status); + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Diagnostic/DiagnosticArrayDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Diagnostic/DiagnosticArrayDefaultVisualizer.cs.meta new file mode 100644 index 00000000..91348f22 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Diagnostic/DiagnosticArrayDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: ebac18b3d35d04a7ebf8099e7f11e01f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Diagnostic/DiagnosticStatusDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Diagnostic/DiagnosticStatusDefaultVisualizer.cs new file mode 100644 index 00000000..0234f110 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Diagnostic/DiagnosticStatusDefaultVisualizer.cs @@ -0,0 +1,30 @@ +using System; +using RosMessageTypes.Diagnostic; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public class DiagnosticStatusDefaultVisualizer : GuiVisualizer + { + public override Action CreateGUI(DiagnosticStatusMsg message, MessageMetadata meta) => () => + { + GUI(message); + }; + + public static string[] s_DiagnosticLevelTable = new string[] + { + "OK","WARN","ERROR","STALE" + }; + + public static void GUI(DiagnosticStatusMsg message) + { + string status = (message.level >= 0 && message.level < s_DiagnosticLevelTable.Length) ? s_DiagnosticLevelTable[message.level] : "INVALID"; + GUILayout.Label(message.hardware_id.Length > 0 ? $"Status of {message.name}|{message.hardware_id}: {status}" : $"Status of {message.name}: {status}"); + GUILayout.Label(message.message); + foreach (KeyValueMsg keyValue in message.values) + { + GUILayout.Label($" {keyValue.key}: {keyValue.value}"); + } + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Diagnostic/DiagnosticStatusDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Diagnostic/DiagnosticStatusDefaultVisualizer.cs.meta new file mode 100644 index 00000000..1ea753ac --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Diagnostic/DiagnosticStatusDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: a8fd8da85002c46c0ac5f35abebe2224 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry.meta new file mode 100644 index 00000000..85776265 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: bf1d313b525b55145991883abb467ca3 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/AccelDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/AccelDefaultVisualizer.cs new file mode 100644 index 00000000..f58bfbbc --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/AccelDefaultVisualizer.cs @@ -0,0 +1,35 @@ +using System; +using RosMessageTypes.Geometry; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +public class AccelDefaultVisualizer : DrawingVisualizer +{ + public float m_Thickness = 0.01f; + public float m_LengthScale = 1.0f; + public float m_SphereRadius = 1.0f; + public GameObject m_Origin; + [SerializeField] + Color m_Color; + + public override void Draw(Drawing3d drawing, AccelMsg message, MessageMetadata meta) + { + Draw(message, drawing, SelectColor(m_Color, meta), m_Origin, m_LengthScale, m_SphereRadius, m_Thickness); + } + + public static void Draw(AccelMsg message, Drawing3d drawing, Color color, GameObject origin, float lengthScale = 1, float sphereRadius = 1, float thickness = 0.01f) where C : ICoordinateSpace, new() + { + Vector3 originPos = (origin == null) ? Vector3.zero : origin.transform.position; + drawing.DrawArrow(originPos, originPos + message.linear.From() * lengthScale, color, thickness); + VisualizationUtils.DrawAngularVelocityArrow(drawing, message.angular.From(), originPos, color, sphereRadius, thickness); + } + + public override Action CreateGUI(AccelMsg message, MessageMetadata meta) + { + return () => + { + message.GUI(); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/AccelDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/AccelDefaultVisualizer.cs.meta new file mode 100644 index 00000000..6217bf12 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/AccelDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 7d5d9a44a1314c64bb8c0ef2cc5bb27f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/AccelStampedDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/AccelStampedDefaultVisualizer.cs new file mode 100644 index 00000000..c6d03eb8 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/AccelStampedDefaultVisualizer.cs @@ -0,0 +1,32 @@ +using System; +using RosMessageTypes.Geometry; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +public class AccelStampedDefaultVisualizer : DrawingVisualizer +{ + public float m_Thickness = 0.01f; + public float m_LengthScale = 1.0f; + public float m_SphereRadius = 1.0f; + public GameObject m_Origin; + [SerializeField] + Color m_Color; + [SerializeField] + TFTrackingSettings m_TFTrackingSettings; + + public override void Draw(Drawing3d drawing, AccelStampedMsg message, MessageMetadata meta) + { + drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header); + AccelDefaultVisualizer.Draw(message.accel, drawing, SelectColor(m_Color, meta), m_Origin, m_LengthScale, m_SphereRadius, m_Thickness); + } + + public override Action CreateGUI(AccelStampedMsg message, MessageMetadata meta) + { + return () => + { + message.header.GUI(); + message.accel.GUI(); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/AccelStampedDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/AccelStampedDefaultVisualizer.cs.meta new file mode 100644 index 00000000..7241931c --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/AccelStampedDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 67c032e5682c70440a684a13df04044e +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/AccelWithCovarianceDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/AccelWithCovarianceDefaultVisualizer.cs new file mode 100644 index 00000000..12b2a5f7 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/AccelWithCovarianceDefaultVisualizer.cs @@ -0,0 +1,35 @@ +using System; +using RosMessageTypes.Geometry; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +public class AccelWithCovarianceDefaultVisualizer : DrawingVisualizer +{ + public float m_Thickness = 0.01f; + public float m_LengthScale = 1.0f; + public float m_SphereRadius = 1.0f; + public GameObject m_Origin; + [SerializeField] + Color m_Color; + bool m_ViewCovariance; + + public override void Draw(Drawing3d drawing, AccelWithCovarianceMsg message, MessageMetadata meta) + { + Draw(message, drawing, SelectColor(m_Color, meta), m_Origin, m_LengthScale, m_SphereRadius, m_Thickness); + } + + public static void Draw(AccelWithCovarianceMsg message, Drawing3d drawing, Color color, GameObject origin, float lengthScale = 1, float sphereRadius = 1, float thickness = 0.01f) where C : ICoordinateSpace, new() + { + AccelDefaultVisualizer.Draw(message.accel, drawing, color, origin, lengthScale, sphereRadius, thickness); + } + + public override Action CreateGUI(AccelWithCovarianceMsg message, MessageMetadata meta) + { + return () => + { + message.accel.GUI(); + VisualizationUtils.GUIGrid(message.covariance, 6, ref m_ViewCovariance); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/AccelWithCovarianceDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/AccelWithCovarianceDefaultVisualizer.cs.meta new file mode 100644 index 00000000..bf72a400 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/AccelWithCovarianceDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c08ec320a78e1b84594eabd50dd18859 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/AccelWithCovarianceStampedDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/AccelWithCovarianceStampedDefaultVisualizer.cs new file mode 100644 index 00000000..0712cbcb --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/AccelWithCovarianceStampedDefaultVisualizer.cs @@ -0,0 +1,34 @@ +using System; +using RosMessageTypes.Geometry; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +public class AccelWithCovarianceStampedDefaultVisualizer : DrawingVisualizer +{ + public float m_Thickness = 0.01f; + public float m_LengthScale = 1.0f; + public float m_SphereRadius = 1.0f; + public GameObject m_Origin; + [SerializeField] + Color m_Color; + bool m_ViewCovariance; + [SerializeField] + TFTrackingSettings m_TFTrackingSettings; + + public override void Draw(Drawing3d drawing, AccelWithCovarianceStampedMsg message, MessageMetadata meta) + { + drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header); + AccelWithCovarianceDefaultVisualizer.Draw(message.accel, drawing, SelectColor(m_Color, meta), m_Origin, m_LengthScale, m_SphereRadius, m_Thickness); + } + + public override Action CreateGUI(AccelWithCovarianceStampedMsg message, MessageMetadata meta) + { + return () => + { + message.header.GUI(); + message.accel.accel.GUI(); + VisualizationUtils.GUIGrid(message.accel.covariance, 6, ref m_ViewCovariance); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/AccelWithCovarianceStampedDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/AccelWithCovarianceStampedDefaultVisualizer.cs.meta new file mode 100644 index 00000000..74087b1e --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/AccelWithCovarianceStampedDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 19a534f8e39de6d4aab8572469822b4b +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/InertiaDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/InertiaDefaultVisualizer.cs new file mode 100644 index 00000000..3ceafcf5 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/InertiaDefaultVisualizer.cs @@ -0,0 +1,26 @@ +using System; +using RosMessageTypes.Geometry; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +public class InertiaDefaultVisualizer : DrawingVisualizer +{ + public GameObject m_Origin; + public float m_Radius; + [SerializeField] + Color m_Color; + + public override void Draw(Drawing3d drawing, InertiaMsg message, MessageMetadata meta) + { + Vector3DefaultVisualizer.Draw(message.com, drawing, m_Origin, SelectColor(m_Color, meta), "Center of mass", m_Radius); + } + + public override Action CreateGUI(InertiaMsg message, MessageMetadata meta) + { + return () => + { + message.GUI(); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/InertiaDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/InertiaDefaultVisualizer.cs.meta new file mode 100644 index 00000000..e048c1d7 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/InertiaDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 23204335991390b47ada035c24385383 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/InertiaStampedDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/InertiaStampedDefaultVisualizer.cs new file mode 100644 index 00000000..5dcb29a4 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/InertiaStampedDefaultVisualizer.cs @@ -0,0 +1,32 @@ +using System; +using RosMessageTypes.Geometry; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +public class InertiaStampedDefaultVisualizer : DrawingVisualizer +{ + public GameObject m_Origin; + public float m_Radius; + [SerializeField] + Color m_Color; + [SerializeField] + string m_Label = "Center of mass"; + [SerializeField] + TFTrackingSettings m_TFTrackingType; + + public override void Draw(Drawing3d drawing, InertiaStampedMsg message, MessageMetadata meta) + { + drawing.SetTFTrackingSettings(m_TFTrackingType, message.header); + Vector3DefaultVisualizer.Draw(message.inertia.com, drawing, m_Origin, SelectColor(m_Color, meta), SelectLabel(m_Label, meta), m_Radius); + } + + public override Action CreateGUI(InertiaStampedMsg message, MessageMetadata meta) + { + return () => + { + message.header.GUI(); + message.inertia.GUI(); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/InertiaStampedDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/InertiaStampedDefaultVisualizer.cs.meta new file mode 100644 index 00000000..32b7e9a3 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/InertiaStampedDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 40878073c7b2b0a409b71a34633bdfcb +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/Point32DefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/Point32DefaultVisualizer.cs new file mode 100644 index 00000000..6d8be273 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/Point32DefaultVisualizer.cs @@ -0,0 +1,41 @@ +using System; +using RosMessageTypes.Geometry; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public class Point32DefaultVisualizer : DrawingVisualizer + { + [SerializeField] + float m_Radius = 0.01f; + [SerializeField] + Color m_Color; + [SerializeField] + string m_Label; + + public override void Draw(Drawing3d drawing, Point32Msg message, MessageMetadata meta) + { + Draw(message, drawing, SelectColor(m_Color, meta), SelectLabel(m_Label, meta), m_Radius); + } + + public static void Draw(Point32Msg message, Drawing3d drawing, Color color, string label, float size = 0.01f) where C : ICoordinateSpace, new() + { + drawing.DrawPoint(message.From(), color, size); + drawing.DrawLabel(label, message.From(), color, size * 1.5f); + } + + public static void Draw(Point32Msg message, Drawing3d drawing, Color color, float size = 0.01f) where C : ICoordinateSpace, new() + { + drawing.DrawPoint(message.From(), color, size); + } + + public override Action CreateGUI(Point32Msg message, MessageMetadata meta) + { + return () => + { + message.GUI(); + }; + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/Point32DefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/Point32DefaultVisualizer.cs.meta new file mode 100644 index 00000000..5c2c79ae --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/Point32DefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: ca06d2332e51924479738a34919115b6 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PointDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PointDefaultVisualizer.cs new file mode 100644 index 00000000..9d847a79 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PointDefaultVisualizer.cs @@ -0,0 +1,42 @@ +using System; +using RosMessageTypes.Geometry; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public class PointDefaultVisualizer : DrawingVisualizer + { + [SerializeField] + float m_Radius = 0.01f; + [SerializeField] + Color m_Color; + [SerializeField] + string m_Label; + + public override void Draw(Drawing3d drawing, PointMsg message, MessageMetadata meta) + { + Draw(message, drawing, SelectColor(m_Color, meta), SelectLabel(m_Label, meta), m_Radius); + } + + public static void Draw(PointMsg message, Drawing3d drawing, Color color, string label, float size = 0.01f) where C : ICoordinateSpace, new() + { + drawing.DrawPoint(message.From(), color, size); + drawing.DrawLabel(label, message.From(), color, size * 1.5f); + } + + public static void Draw(PointMsg message, Drawing3d drawing, Color color, float size = 0.01f) where C : ICoordinateSpace, new() + { + drawing.DrawPoint(message.From(), color, size); + } + + public override Action CreateGUI(PointMsg message, MessageMetadata meta) + { + return () => + { + message.GUI(); + }; + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PointDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PointDefaultVisualizer.cs.meta new file mode 100644 index 00000000..cced765e --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PointDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 411f6a469899d8c4691994218731ae87 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PointStampedDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PointStampedDefaultVisualizer.cs new file mode 100644 index 00000000..2c3d8b53 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PointStampedDefaultVisualizer.cs @@ -0,0 +1,34 @@ +using System; +using RosMessageTypes.Geometry; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public class PointStampedDefaultVisualizer : DrawingVisualizer + { + [SerializeField] + float m_Radius = 0.01f; + [SerializeField] + Color m_Color; + [SerializeField] + string m_Label; + [SerializeField] + TFTrackingSettings m_TFTrackingSettings; + + public override void Draw(Drawing3d drawing, PointStampedMsg message, MessageMetadata meta) + { + drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header); + PointDefaultVisualizer.Draw(message.point, drawing, SelectColor(m_Color, meta), SelectLabel(m_Label, meta), m_Radius); + } + + public override Action CreateGUI(PointStampedMsg message, MessageMetadata meta) + { + return () => + { + message.header.GUI(); + message.point.GUI(); + }; + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PointStampedDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PointStampedDefaultVisualizer.cs.meta new file mode 100644 index 00000000..fcd6c542 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PointStampedDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 59143f68e58e4d6449372a89c6b2ebd8 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PolygonDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PolygonDefaultVisualizer.cs new file mode 100644 index 00000000..87593d94 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PolygonDefaultVisualizer.cs @@ -0,0 +1,39 @@ +using System; +using RosMessageTypes.Geometry; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public class PolygonDefaultVisualizer : DrawingVisualizer + { + [SerializeField] + float m_Thickness = 0.1f; + [SerializeField] + Color m_Color; + + public override void Draw(Drawing3d drawing, PolygonMsg message, MessageMetadata meta) + { + Draw(message, drawing, SelectColor(m_Color, meta), m_Thickness); + } + + public static void Draw(PolygonMsg message, Drawing3d drawing, Color color, float thickness = 0.01f) where C : ICoordinateSpace, new() + { + Vector3 prevPos = message.points[message.points.Length - 1].From(); + foreach (Point32Msg p in message.points) + { + Vector3 curPos = p.From(); + drawing.DrawLine(prevPos, curPos, color, thickness); + prevPos = curPos; + } + } + + public override Action CreateGUI(PolygonMsg message, MessageMetadata meta) + { + return () => + { + message.GUI(); + }; + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PolygonDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PolygonDefaultVisualizer.cs.meta new file mode 100644 index 00000000..02d85b78 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PolygonDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 98e9789d1b217d0479bddfe4e5ac15af +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PolygonStampedDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PolygonStampedDefaultVisualizer.cs new file mode 100644 index 00000000..4d7e9a90 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PolygonStampedDefaultVisualizer.cs @@ -0,0 +1,32 @@ +using System; +using RosMessageTypes.Geometry; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public class PolygonStampedDefaultVisualizer : DrawingVisualizer + { + [SerializeField] + float m_Thickness = 0.01f; + [SerializeField] + Color m_Color; + [SerializeField] + TFTrackingSettings m_TFTrackingSettings; + + public override void Draw(Drawing3d drawing, PolygonStampedMsg message, MessageMetadata meta) + { + drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header); + PolygonDefaultVisualizer.Draw(message.polygon, drawing, SelectColor(m_Color, meta), m_Thickness); + } + + public override Action CreateGUI(PolygonStampedMsg message, MessageMetadata meta) + { + return () => + { + message.header.GUI(); + message.polygon.GUI(); + }; + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PolygonStampedDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PolygonStampedDefaultVisualizer.cs.meta new file mode 100644 index 00000000..c8f9ad9a --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PolygonStampedDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 2ff0d98070786c64981a74ca2ef65439 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PoseArrayDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PoseArrayDefaultVisualizer.cs new file mode 100644 index 00000000..65b5101f --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PoseArrayDefaultVisualizer.cs @@ -0,0 +1,38 @@ +using System; +using RosMessageTypes.Geometry; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +public class PoseArrayDefaultVisualizer : DrawingVisualizer +{ + [SerializeField] + float m_Size = 0.1f; + [SerializeField] + [Tooltip("If ticked, draw the axis lines for Unity coordinates. Otherwise, draw the axis lines for ROS coordinates (FLU).")] + bool m_DrawUnityAxes; + [SerializeField] + TFTrackingSettings m_TFTrackingSettings; + + public override void Draw(Drawing3d drawing, PoseArrayMsg message, MessageMetadata meta) + { + drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header); + Draw(message, drawing, m_Size, m_DrawUnityAxes); + } + + public static void Draw(PoseArrayMsg message, Drawing3d drawing, float size = 0.1f, bool drawUnityAxes = false) where C : ICoordinateSpace, new() + { + foreach (PoseMsg pose in message.poses) + { + PoseDefaultVisualizer.Draw(pose, drawing, size, drawUnityAxes); + } + } + + public override Action CreateGUI(PoseArrayMsg message, MessageMetadata meta) + { + return () => + { + message.GUI(); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PoseArrayDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PoseArrayDefaultVisualizer.cs.meta new file mode 100644 index 00000000..2c8504b5 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PoseArrayDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: e377c0825a3a9084a8055a7edef5215f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PoseDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PoseDefaultVisualizer.cs new file mode 100644 index 00000000..562a07e1 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PoseDefaultVisualizer.cs @@ -0,0 +1,39 @@ +using System; +using RosMessageTypes.Geometry; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +public class PoseDefaultVisualizer : DrawingVisualizer +{ + [SerializeField] + float m_Size = 0.1f; + [SerializeField] + [Tooltip("If ticked, draw the axis lines for Unity coordinates. Otherwise, draw the axis lines for ROS coordinates (FLU).")] + bool m_DrawUnityAxes; + + public override void Draw(Drawing3d drawing, PoseMsg message, MessageMetadata meta) + { + Draw(message, drawing, m_Size, m_DrawUnityAxes); + } + + public static void Draw(PoseMsg message, Drawing3d drawing, float size = 0.1f, bool drawUnityAxes = false) + where C : ICoordinateSpace, new() + { + VisualizationUtils.DrawAxisVectors( + drawing, + new Vector3Msg(message.position.x, message.position.y, message.position.z), + message.orientation, + size, + drawUnityAxes + ); + } + + public override Action CreateGUI(PoseMsg message, MessageMetadata meta) + { + return () => + { + message.GUI(); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PoseDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PoseDefaultVisualizer.cs.meta new file mode 100644 index 00000000..0717cb2d --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PoseDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 9bdfb211949793b4d8196408d5ac551a +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PoseStampedDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PoseStampedDefaultVisualizer.cs new file mode 100644 index 00000000..bf5e1124 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PoseStampedDefaultVisualizer.cs @@ -0,0 +1,31 @@ +using System; +using RosMessageTypes.Geometry; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +public class PoseStampedDefaultVisualizer : DrawingVisualizer +{ + [SerializeField] + float m_Size = 0.1f; + [SerializeField] + [Tooltip("If ticked, draw the axis lines for Unity coordinates. Otherwise, draw the axis lines for ROS coordinates (FLU).")] + bool m_DrawUnityAxes; + [SerializeField] + TFTrackingSettings m_TFTrackingSettings; + + public override void Draw(Drawing3d drawing, PoseStampedMsg message, MessageMetadata meta) + { + drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header); + PoseDefaultVisualizer.Draw(message.pose, drawing, m_Size, m_DrawUnityAxes); + } + + public override Action CreateGUI(PoseStampedMsg message, MessageMetadata meta) + { + return () => + { + message.header.GUI(); + message.pose.GUI(); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PoseStampedDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PoseStampedDefaultVisualizer.cs.meta new file mode 100644 index 00000000..fab388d8 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PoseStampedDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 517ff03e8ebf1c0478a6b77647f1f439 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PoseWithCovarianceDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PoseWithCovarianceDefaultVisualizer.cs new file mode 100644 index 00000000..161cd78c --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PoseWithCovarianceDefaultVisualizer.cs @@ -0,0 +1,29 @@ +using System; +using RosMessageTypes.Geometry; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +public class PoseWithCovarianceDefaultVisualizer : DrawingVisualizer +{ + [SerializeField] + float m_Size = 0.1f; + [SerializeField] + [Tooltip("If ticked, draw the axis lines for Unity coordinates. Otherwise, draw the axis lines for ROS coordinates (FLU).")] + bool m_DrawUnityAxes; + bool m_ViewCovariance; + + public override void Draw(Drawing3d drawing, PoseWithCovarianceMsg message, MessageMetadata meta) + { + PoseDefaultVisualizer.Draw(message.pose, drawing, m_Size, m_DrawUnityAxes); + } + + public override Action CreateGUI(PoseWithCovarianceMsg message, MessageMetadata meta) + { + return () => + { + message.pose.GUI(); + VisualizationUtils.GUIGrid(message.covariance, 6, ref m_ViewCovariance); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PoseWithCovarianceDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PoseWithCovarianceDefaultVisualizer.cs.meta new file mode 100644 index 00000000..3635f94f --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PoseWithCovarianceDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c0acf581c8a13f3448483853151ef326 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PoseWithCovarianceStampedDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PoseWithCovarianceStampedDefaultVisualizer.cs new file mode 100644 index 00000000..abb1492a --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PoseWithCovarianceStampedDefaultVisualizer.cs @@ -0,0 +1,33 @@ +using System; +using RosMessageTypes.Geometry; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +public class PoseWithCovarianceStampedDefaultVisualizer : DrawingVisualizer +{ + [SerializeField] + float m_Size = 0.1f; + [SerializeField] + [Tooltip("If ticked, draw the axis lines for Unity coordinates. Otherwise, draw the axis lines for ROS coordinates (FLU).")] + bool m_DrawUnityAxes; + bool m_ViewCovariance; + [SerializeField] + TFTrackingSettings m_TFTrackingSettings; + + public override void Draw(Drawing3d drawing, PoseWithCovarianceStampedMsg message, MessageMetadata meta) + { + drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header); + PoseDefaultVisualizer.Draw(message.pose.pose, drawing, m_Size, m_DrawUnityAxes); + } + + public override Action CreateGUI(PoseWithCovarianceStampedMsg message, MessageMetadata meta) + { + return () => + { + message.header.GUI(); + message.pose.pose.GUI(); + VisualizationUtils.GUIGrid(message.pose.covariance, 6, ref m_ViewCovariance); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PoseWithCovarianceStampedDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PoseWithCovarianceStampedDefaultVisualizer.cs.meta new file mode 100644 index 00000000..d009de96 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/PoseWithCovarianceStampedDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6ce850bfe5a7ae641ae1e0005662788e +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/QuaternionDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/QuaternionDefaultVisualizer.cs new file mode 100644 index 00000000..01e51003 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/QuaternionDefaultVisualizer.cs @@ -0,0 +1,49 @@ +using System; +using RosMessageTypes.Geometry; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public class QuaternionDefaultVisualizer : DrawingVisualizer + { + [SerializeField] + float m_Size = 0.1f; + [SerializeField] + GameObject m_DrawAtPosition; + [SerializeField] + [Tooltip("If ticked, draw the axis lines for Unity coordinates. Otherwise, draw the axis lines for ROS coordinates (FLU).")] + bool m_DrawUnityAxes; + [SerializeField] + Color m_Color; + [SerializeField] + string m_Label; + + public override void Draw(Drawing3d drawing, QuaternionMsg message, MessageMetadata meta) + { + Draw(message, drawing, m_DrawAtPosition, m_Size, m_DrawUnityAxes); + drawing.DrawLabel(SelectLabel(m_Label, meta), transform.position, SelectColor(m_Color, meta), m_Size); + } + + public static void Draw(QuaternionMsg message, Drawing3d drawing, GameObject drawAtPosition = null, float size = 0.1f, bool drawUnityAxes = false) +where C : ICoordinateSpace, new() + { + Vector3 position = drawAtPosition != null ? drawAtPosition.transform.position : Vector3.zero; + VisualizationUtils.DrawAxisVectors(drawing, position.To(), message, size, drawUnityAxes); + } + + public static void Draw(QuaternionMsg message, Drawing3d drawing, Vector3 position, float size = 0.1f, bool drawUnityAxes = false) + where C : ICoordinateSpace, new() + { + VisualizationUtils.DrawAxisVectors(drawing, position.To(), message, size, drawUnityAxes); + } + + public override Action CreateGUI(QuaternionMsg message, MessageMetadata meta) + { + return () => + { + message.GUI(); + }; + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/QuaternionDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/QuaternionDefaultVisualizer.cs.meta new file mode 100644 index 00000000..a58df482 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/QuaternionDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0f651df5ec61c734f8083a79205badd1 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/QuaternionStampedDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/QuaternionStampedDefaultVisualizer.cs new file mode 100644 index 00000000..b393edba --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/QuaternionStampedDefaultVisualizer.cs @@ -0,0 +1,40 @@ +using System; +using RosMessageTypes.Geometry; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public class QuaternionStampedDefaultVisualizer : DrawingVisualizer + { + [SerializeField] + float m_Size = 0.01f; + [SerializeField] + GameObject m_DrawAtPosition; + [SerializeField] + [Tooltip("If ticked, draw the axis lines for Unity coordinates. Otherwise, draw the axis lines for ROS coordinates (FLU).")] + bool m_DrawUnityAxes; + [SerializeField] + Color m_Color; + [SerializeField] + string m_Label; + [SerializeField] + TFTrackingSettings m_TFTrackingSettings; + + public override void Draw(Drawing3d drawing, QuaternionStampedMsg message, MessageMetadata meta) + { + drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header); + QuaternionDefaultVisualizer.Draw(message.quaternion, drawing, m_DrawAtPosition, m_Size, m_DrawUnityAxes); + drawing.DrawLabel(SelectLabel(m_Label, meta), transform.position, SelectColor(m_Color, meta), m_Size); + } + + public override Action CreateGUI(QuaternionStampedMsg message, MessageMetadata meta) + { + return () => + { + message.header.GUI(); + message.quaternion.GUI(); + }; + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/QuaternionStampedDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/QuaternionStampedDefaultVisualizer.cs.meta new file mode 100644 index 00000000..139ce36e --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/QuaternionStampedDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0881ebe31dc31444f913ca60e3b34783 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/TransformDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/TransformDefaultVisualizer.cs new file mode 100644 index 00000000..e136fb47 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/TransformDefaultVisualizer.cs @@ -0,0 +1,39 @@ +using System; +using RosMessageTypes.Geometry; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public class TransformDefaultVisualizer : DrawingVisualizer + { + [SerializeField] + float m_Size = 0.1f; + [SerializeField] + [Tooltip("If ticked, draw the axis lines for Unity coordinates. Otherwise, draw the axis lines for ROS coordinates (FLU).")] + bool m_DrawUnityAxes; + [SerializeField] + Color m_Color; + [SerializeField] + string m_Label; + + public override void Draw(Drawing3d drawing, TransformMsg message, MessageMetadata meta) + { + Draw(message, drawing, m_Size, m_DrawUnityAxes); + drawing.DrawLabel(SelectLabel(m_Label, meta), message.translation.From(), SelectColor(m_Color, meta), m_Size); + } + + public static void Draw(TransformMsg transform, Drawing3d drawing, float size = 0.01f, bool drawUnityAxes = false) where C : ICoordinateSpace, new() + { + QuaternionDefaultVisualizer.Draw(transform.rotation, drawing, transform.translation.From(), size, drawUnityAxes); + } + + public override Action CreateGUI(TransformMsg message, MessageMetadata meta) + { + return () => + { + message.GUI(); + }; + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/TransformDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/TransformDefaultVisualizer.cs.meta new file mode 100644 index 00000000..5c099eee --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/TransformDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 7d4f4c2adcaad6b4799f59c76f040eee +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/TransformStampedDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/TransformStampedDefaultVisualizer.cs new file mode 100644 index 00000000..9a3e7e3a --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/TransformStampedDefaultVisualizer.cs @@ -0,0 +1,38 @@ +using System; +using RosMessageTypes.Geometry; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public class TransformStampedDefaultVisualizer : DrawingVisualizer + { + [SerializeField] + float m_Size = 0.1f; + [SerializeField] + [Tooltip("If ticked, draw the axis lines for Unity coordinates. Otherwise, draw the axis lines for ROS coordinates (FLU).")] + bool m_DrawUnityAxes; + [SerializeField] + Color m_Color; + [SerializeField] + string m_Label; + [SerializeField] + TFTrackingSettings m_TFTrackingSettings; + + public override void Draw(Drawing3d drawing, TransformStampedMsg message, MessageMetadata meta) + { + drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header); + TransformDefaultVisualizer.Draw(message.transform, drawing, m_Size, m_DrawUnityAxes); + drawing.DrawLabel(SelectLabel(m_Label, meta), message.transform.translation.From(), SelectColor(m_Color, meta), m_Size); + } + + public override Action CreateGUI(TransformStampedMsg message, MessageMetadata meta) + { + return () => + { + message.header.GUI(); + message.transform.GUI(); + }; + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/TransformStampedDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/TransformStampedDefaultVisualizer.cs.meta new file mode 100644 index 00000000..3002b0e8 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/TransformStampedDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 34006ea0674a568409a63b1695197f8b +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/TwistDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/TwistDefaultVisualizer.cs new file mode 100644 index 00000000..2336611f --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/TwistDefaultVisualizer.cs @@ -0,0 +1,34 @@ +using System; +using RosMessageTypes.Geometry; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public class TwistDefaultVisualizer : DrawingVisualizer + { + public float thickness = 0.01f; + public float lengthScale = 1.0f; + public float sphereRadius = 1.0f; + public GameObject origin; + [SerializeField] + Color m_Color; + + public override void Draw(Drawing3d drawing, TwistMsg message, MessageMetadata meta) + { + var orig = origin == null ? Vector3.zero : origin.transform.position; + Draw(message, drawing, SelectColor(m_Color, meta), orig, lengthScale, sphereRadius, thickness); + } + + public static void Draw(TwistMsg message, Drawing3d drawing, Color color, Vector3 origin, float lengthScale = 1, float sphereRadius = 1, float thickness = 0.01f) where C : ICoordinateSpace, new() + { + drawing.DrawArrow(origin, origin + message.linear.From() * lengthScale, color, thickness); + VisualizationUtils.DrawAngularVelocityArrow(drawing, message.angular.From(), origin, color, sphereRadius, thickness); + } + + public override Action CreateGUI(TwistMsg message, MessageMetadata meta) => () => + { + message.GUI(); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/TwistDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/TwistDefaultVisualizer.cs.meta new file mode 100644 index 00000000..fac23a68 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/TwistDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b6a9055543d8340b2900ae15dd34317b +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/TwistStampedDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/TwistStampedDefaultVisualizer.cs new file mode 100644 index 00000000..3fe958c6 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/TwistStampedDefaultVisualizer.cs @@ -0,0 +1,32 @@ +using System; +using RosMessageTypes.Geometry; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public class TwistStampedDefaultVisualizer : DrawingVisualizer + { + public float thickness = 0.01f; + public float lengthScale = 1.0f; + public float sphereRadius = 1.0f; + public GameObject origin; + [SerializeField] + Color m_Color; + [SerializeField] + TFTrackingSettings m_TFTrackingSettings; + + public override void Draw(Drawing3d drawing, TwistStampedMsg message, MessageMetadata meta) + { + drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header); + var orig = origin == null ? Vector3.zero : origin.transform.position; + TwistDefaultVisualizer.Draw(message.twist, drawing, SelectColor(m_Color, meta), orig, lengthScale, sphereRadius, thickness); + } + + public override Action CreateGUI(TwistStampedMsg message, MessageMetadata meta) => () => + { + message.header.GUI(); + message.twist.GUI(); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/TwistStampedDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/TwistStampedDefaultVisualizer.cs.meta new file mode 100644 index 00000000..d4ac516b --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/TwistStampedDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c318843ad54314ff1b015df116ce0976 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/TwistWithCovarianceDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/TwistWithCovarianceDefaultVisualizer.cs new file mode 100644 index 00000000..dcb92131 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/TwistWithCovarianceDefaultVisualizer.cs @@ -0,0 +1,35 @@ +using System; +using RosMessageTypes.Geometry; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public class TwistWithCovarianceDefaultVisualizer : DrawingVisualizer + { + public float thickness = 0.01f; + public float lengthScale = 1.0f; + public float sphereRadius = 1.0f; + public GameObject origin; + [SerializeField] + Color m_Color; + bool m_ViewCovariance; + + public override void Draw(Drawing3d drawing, TwistWithCovarianceMsg message, MessageMetadata meta) + { + var orig = origin == null ? Vector3.zero : origin.transform.position; + Draw(message, drawing, SelectColor(m_Color, meta), orig, lengthScale, sphereRadius, thickness); + } + + public static void Draw(TwistWithCovarianceMsg message, Drawing3d drawing, Color color, Vector3 origin, float lengthScale = 1, float sphereRadius = 1, float thickness = 0.01f) where C : ICoordinateSpace, new() + { + TwistDefaultVisualizer.Draw(message.twist, drawing, color, origin, lengthScale, sphereRadius, thickness); + } + + public override Action CreateGUI(TwistWithCovarianceMsg message, MessageMetadata meta) => () => + { + message.twist.GUI(); + VisualizationUtils.GUIGrid(message.covariance, 6, ref m_ViewCovariance); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/TwistWithCovarianceDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/TwistWithCovarianceDefaultVisualizer.cs.meta new file mode 100644 index 00000000..43937772 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/TwistWithCovarianceDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 9f5e703fee4774175ad7e40b70d5b17b +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/TwistWithCovarianceStampedDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/TwistWithCovarianceStampedDefaultVisualizer.cs new file mode 100644 index 00000000..b178f4a2 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/TwistWithCovarianceStampedDefaultVisualizer.cs @@ -0,0 +1,34 @@ +using System; +using RosMessageTypes.Geometry; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public class TwistWithCovarianceStampedDefaultVisualizer : DrawingVisualizer + { + public float thickness = 0.01f; + public float lengthScale = 1.0f; + public float sphereRadius = 1.0f; + public GameObject origin; + [SerializeField] + Color m_Color; + bool m_ViewCovariance; + [SerializeField] + TFTrackingSettings m_TFTrackingSettings; + + public override void Draw(Drawing3d drawing, TwistWithCovarianceStampedMsg message, MessageMetadata meta) + { + drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header); + var orig = origin == null ? Vector3.zero : origin.transform.position; + TwistWithCovarianceDefaultVisualizer.Draw(message.twist, drawing, SelectColor(m_Color, meta), orig, lengthScale, sphereRadius, thickness); + } + + public override Action CreateGUI(TwistWithCovarianceStampedMsg message, MessageMetadata meta) => () => + { + message.header.GUI(); + message.twist.twist.GUI(); + VisualizationUtils.GUIGrid(message.twist.covariance, 6, ref m_ViewCovariance); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/TwistWithCovarianceStampedDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/TwistWithCovarianceStampedDefaultVisualizer.cs.meta new file mode 100644 index 00000000..0b585d2e --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/TwistWithCovarianceStampedDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 459e206b765d9491689eee30dc54bddd +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/Vector3DefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/Vector3DefaultVisualizer.cs new file mode 100644 index 00000000..da2494da --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/Vector3DefaultVisualizer.cs @@ -0,0 +1,52 @@ +using System; +using RosMessageTypes.Geometry; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public class Vector3DefaultVisualizer : DrawingVisualizer + { + [SerializeField] + float m_Radius = 0.01f; + [SerializeField] + Color m_Color; + [SerializeField] + string m_Label; + + public override void Draw(Drawing3d drawing, Vector3Msg message, MessageMetadata meta) + { + Draw(message, drawing, SelectColor(m_Color, meta), SelectLabel(m_Label, meta), m_Radius); + } + + public static void Draw(Vector3Msg message, Drawing3d drawing, Color color, string label, float size = 0.01f) where C : ICoordinateSpace, new() + { + Vector3 point = message.From(); + drawing.DrawPoint(point, color, size); + drawing.DrawLabel(label, point, color, size * 1.5f); + } + + public static void Draw(Vector3Msg message, Drawing3d drawing, Color color, float size = 0.01f) where C : ICoordinateSpace, new() + { + drawing.DrawPoint(message.From(), color, size); + } + + public static void Draw(Vector3Msg message, Drawing3d drawing, GameObject origin, Color color, string label, float size = 0.01f) where C : ICoordinateSpace, new() + { + Vector3 point = message.From(); + if (origin != null) + point = origin.transform.TransformPoint(point); + drawing.DrawPoint(point, color, size); + drawing.DrawLabel(label, point, color, size * 1.5f); + } + + + public override Action CreateGUI(Vector3Msg message, MessageMetadata meta) + { + return () => + { + message.GUI(); + }; + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/Vector3DefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/Vector3DefaultVisualizer.cs.meta new file mode 100644 index 00000000..a87c0c28 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/Vector3DefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c5f79b92768699c4c83e62bc7df9a7c5 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/Vector3StampedDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/Vector3StampedDefaultVisualizer.cs new file mode 100644 index 00000000..1bd827d8 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/Vector3StampedDefaultVisualizer.cs @@ -0,0 +1,34 @@ +using System; +using RosMessageTypes.Geometry; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public class Vector3StampedDefaultVisualizer : DrawingVisualizer + { + [SerializeField] + float m_Radius = 0.01f; + [SerializeField] + Color m_Color; + [SerializeField] + string m_Label; + [SerializeField] + TFTrackingSettings m_TFTrackingSettings; + + public override void Draw(Drawing3d drawing, Vector3StampedMsg message, MessageMetadata meta) + { + drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header); + Vector3DefaultVisualizer.Draw(message.vector, drawing, SelectColor(m_Color, meta), SelectLabel(m_Label, meta), m_Radius); + } + + public override Action CreateGUI(Vector3StampedMsg message, MessageMetadata meta) + { + return () => + { + message.header.GUI(); + message.vector.GUI(); + }; + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/Vector3StampedDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/Vector3StampedDefaultVisualizer.cs.meta new file mode 100644 index 00000000..314de8b3 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/Vector3StampedDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: aeaeab39bc34bec4fb88e7a71fd68b18 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/WrenchDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/WrenchDefaultVisualizer.cs new file mode 100644 index 00000000..8b81a428 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/WrenchDefaultVisualizer.cs @@ -0,0 +1,34 @@ +using System; +using RosMessageTypes.Geometry; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public class WrenchDefaultVisualizer : DrawingVisualizer + { + public float thickness = 0.01f; + public float lengthScale = 1.0f; + public float sphereRadius = 1.0f; + public GameObject origin; + [SerializeField] + Color m_Color; + + public override void Draw(Drawing3d drawing, WrenchMsg message, MessageMetadata meta) + { + Draw(message, drawing, SelectColor(m_Color, meta), origin.transform.position, lengthScale, sphereRadius, thickness); + } + + public override Action CreateGUI(WrenchMsg message, MessageMetadata meta) => () => + { + message.GUI(); + }; + + public static void Draw(WrenchMsg message, Drawing3d drawing, Color color, Vector3 origin, float lengthScale = 1, float sphereRadius = 1, float thickness = 0.01f) where C : ICoordinateSpace, new() + { + drawing.DrawArrow(origin, origin + message.force.From() * lengthScale, color, thickness); + VisualizationUtils.DrawAngularVelocityArrow(drawing, message.torque.From(), origin, color, sphereRadius, thickness); + } + + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/WrenchDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/WrenchDefaultVisualizer.cs.meta new file mode 100644 index 00000000..9ea39034 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/WrenchDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b7112e062888947a9a1933a38520b7fa +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/WrenchStampedDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/WrenchStampedDefaultVisualizer.cs new file mode 100644 index 00000000..3b5358a2 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/WrenchStampedDefaultVisualizer.cs @@ -0,0 +1,31 @@ +using System; +using RosMessageTypes.Geometry; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public class WrenchStampedDefaultVisualizer : DrawingVisualizer + { + public float thickness = 0.01f; + public float lengthScale = 1.0f; + public float sphereRadius = 1.0f; + public GameObject origin; + [SerializeField] + Color m_Color; + [SerializeField] + TFTrackingSettings m_TFTrackingSettings; + + public override void Draw(Drawing3d drawing, WrenchStampedMsg message, MessageMetadata meta) + { + drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header); + WrenchDefaultVisualizer.Draw(message.wrench, drawing, SelectColor(m_Color, meta), origin.transform.position, lengthScale, sphereRadius, thickness); + } + + public override Action CreateGUI(WrenchStampedMsg message, MessageMetadata meta) => () => + { + message.header.GUI(); + message.wrench.GUI(); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/WrenchStampedDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/WrenchStampedDefaultVisualizer.cs.meta new file mode 100644 index 00000000..74ce03ad --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Geometry/WrenchStampedDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f7e131484650e42dcaf443e03308edd6 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Nav.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Nav.meta new file mode 100644 index 00000000..1b037dc7 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Nav.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: c9ff55896c67a3e4398bd90bb1aa2b59 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Nav/GridCellsDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Nav/GridCellsDefaultVisualizer.cs new file mode 100644 index 00000000..f8741439 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Nav/GridCellsDefaultVisualizer.cs @@ -0,0 +1,32 @@ +using System; +using RosMessageTypes.Nav; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +public class GridCellsDefaultVisualizer : DrawingVisualizer +{ + [SerializeField] + float m_Radius = 0.1f; + [SerializeField] + Color m_Color; + [SerializeField] + TFTrackingSettings m_TFTrackingSettings; + public override void Draw(Drawing3d drawing, GridCellsMsg message, MessageMetadata meta) + { + drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header); + Draw(message, drawing, SelectColor(m_Color, meta), m_Radius); + } + + public static void Draw(GridCellsMsg message, Drawing3d drawing, Color color, float radius = 0.01f) + where C : ICoordinateSpace, new() + { + VisualizationUtils.DrawPointCloud(message.cells, drawing, color, radius); + } + + public override Action CreateGUI(GridCellsMsg message, MessageMetadata meta) => () => + { + message.header.GUI(); + GUILayout.Label($"Cell width x height: {message.cell_width} x {message.cell_height}"); + }; +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Nav/GridCellsDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Nav/GridCellsDefaultVisualizer.cs.meta new file mode 100644 index 00000000..1c04774f --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Nav/GridCellsDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 5c28a0753db7113489cb49a68cb713f3 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Nav/OccupancyGridDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Nav/OccupancyGridDefaultVisualizer.cs new file mode 100644 index 00000000..b8b24734 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Nav/OccupancyGridDefaultVisualizer.cs @@ -0,0 +1,190 @@ +using System; +using System.Collections.Generic; +//using RosMessageTypes.Map; +using RosMessageTypes.Nav; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +public class OccupancyGridDefaultVisualizer : BaseVisualFactory +{ + static readonly int k_Color0 = Shader.PropertyToID("_Color0"); + static readonly int k_Color100 = Shader.PropertyToID("_Color100"); + static readonly int k_ColorUnknown = Shader.PropertyToID("_ColorUnknown"); + [SerializeField] + Vector3 m_Offset = Vector3.zero; + [SerializeField] + Material m_Material; + [SerializeField] + TFTrackingSettings m_TFTrackingSettings; + [Header("Cell Colors")] + [SerializeField] + Color m_Unoccupied = Color.white; + [SerializeField] + Color m_Occupied = Color.black; + [SerializeField] + Color m_Unknown = Color.clear; + + Dictionary m_BaseVisuals = new Dictionary(); + + public override bool CanShowDrawing => true; + + public override IVisual GetOrCreateVisual(string topic) + { + OccupancyGridVisual baseVisual; + if (m_BaseVisuals.TryGetValue(topic, out baseVisual)) + return baseVisual; + + baseVisual = new OccupancyGridVisual(topic, this); + m_BaseVisuals.Add(topic, baseVisual); + return baseVisual; + } + + protected override IVisual CreateVisual(string topic) => throw new NotImplementedException(); + + public class OccupancyGridVisual : IVisual + { + string m_Topic; + Mesh m_Mesh; + Material m_Material; + Texture2D m_Texture; + bool m_TextureIsDirty = true; + bool m_IsDrawingEnabled; + public bool IsDrawingEnabled => m_IsDrawingEnabled; + float m_LastDrawingFrameTime = -1; + + Drawing3d m_Drawing; + OccupancyGridDefaultVisualizer m_Settings; + OccupancyGridMsg m_Message; + + public uint Width => m_Message.info.width; + public uint Height => m_Message.info.height; + + public OccupancyGridVisual(string topic, OccupancyGridDefaultVisualizer settings) + { + m_Topic = topic; + m_Settings = settings; + + ROSConnection.GetOrCreateInstance().Subscribe(m_Topic, AddMessage); + } + + public void AddMessage(Message message) + { + if (!VisualizationUtils.AssertMessageType(message, m_Topic)) + return; + + m_Message = (OccupancyGridMsg)message; + m_TextureIsDirty = true; + + if (m_IsDrawingEnabled && Time.time > m_LastDrawingFrameTime) + Redraw(); + + m_LastDrawingFrameTime = Time.time; + } + + public void Redraw() + { + if (m_Mesh == null) + { + m_Mesh = new Mesh(); + m_Mesh.vertices = new[] + { Vector3.zero, new Vector3(0, 0, 1), new Vector3(1, 0, 1), new Vector3(1, 0, 0) }; + m_Mesh.uv = new[] { Vector2.zero, Vector2.up, Vector2.one, Vector2.right }; + m_Mesh.triangles = new[] { 0, 1, 2, 2, 3, 0 }; + } + + if (m_Material == null) + { + m_Material = (m_Settings.m_Material != null) ? new Material(m_Settings.m_Material) : new Material(Shader.Find("Unlit/OccupancyGrid")); + } + m_Material.mainTexture = GetTexture(); + m_Material.SetColor(k_Color0, m_Settings.m_Unoccupied); + m_Material.SetColor(k_Color100, m_Settings.m_Occupied); + m_Material.SetColor(k_ColorUnknown, m_Settings.m_Unknown); + + var origin = m_Message.info.origin.position.From(); + var rotation = m_Message.info.origin.orientation.From(); + rotation.eulerAngles += new Vector3(0, -90, 0); // TODO: Account for differing texture origin + var scale = m_Message.info.resolution; + + if (m_Drawing == null) + { + m_Drawing = Drawing3dManager.CreateDrawing(); + } + else + { + m_Drawing.Clear(); + } + + m_Drawing.SetTFTrackingSettings(m_Settings.m_TFTrackingSettings, m_Message.header); + // offset the mesh by half a grid square, because the message's position defines the CENTER of grid square 0,0 + Vector3 drawOrigin = origin - rotation * new Vector3(scale * 0.5f, 0, scale * 0.5f) + m_Settings.m_Offset; + m_Drawing.DrawMesh(m_Mesh, drawOrigin, rotation, + new Vector3(m_Message.info.width * scale, 1, m_Message.info.height * scale), m_Material); + } + + public void DeleteDrawing() + { + if (m_Drawing != null) + { + m_Drawing.Destroy(); + } + + m_Drawing = null; + } + + public Texture2D GetTexture() + { + if (!m_TextureIsDirty) + return m_Texture; + + if (m_Texture == null) + { + m_Texture = new Texture2D((int)m_Message.info.width, (int)m_Message.info.height, TextureFormat.R8, true); + m_Texture.wrapMode = TextureWrapMode.Clamp; + m_Texture.filterMode = FilterMode.Point; + } + else if (m_Message.info.width != m_Texture.width || m_Message.info.height != m_Texture.height) + { + m_Texture.Resize((int)m_Message.info.width, (int)m_Message.info.height); + } + + m_Texture.SetPixelData(m_Message.data, 0); + m_Texture.Apply(); + m_TextureIsDirty = false; + return m_Texture; + } + + public void OnGUI() + { + if (m_Message == null) + { + GUILayout.Label("Waiting for message..."); + return; + } + + m_Message.header.GUI(); + m_Message.info.GUI(); + } + + public void SetDrawingEnabled(bool enabled) + { + if (m_IsDrawingEnabled == enabled) + return; + + m_IsDrawingEnabled = enabled; + + if (!enabled && m_Drawing != null) + { + m_Drawing.Clear(); + } + + if (enabled && m_Message != null) + { + Redraw(); + } + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Nav/OccupancyGridDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Nav/OccupancyGridDefaultVisualizer.cs.meta new file mode 100644 index 00000000..de6ccb27 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Nav/OccupancyGridDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 32ee8f2011c7b2b4ea58f0b05d0b1838 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Nav/OdometryDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Nav/OdometryDefaultVisualizer.cs new file mode 100644 index 00000000..48f37190 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Nav/OdometryDefaultVisualizer.cs @@ -0,0 +1,36 @@ +using System; +using RosMessageTypes.Nav; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +public class OdometryDefaultVisualizer : DrawingVisualizer +{ + public float thickness = 0.01f; + public float lengthScale = 1.0f; + public float sphereRadius = 1.0f; + [SerializeField] + Color m_Color; + [SerializeField] + TFTrackingSettings m_TFTrackingSettings; + + public override void Draw(Drawing3d drawing, OdometryMsg message, MessageMetadata meta) + { + drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header); + Draw(message, drawing, SelectColor(m_Color, meta), lengthScale, sphereRadius, thickness); + } + + public static void Draw(OdometryMsg message, Drawing3d drawing, Color color, float lengthScale = 1, float sphereRadius = 1, float thickness = 0.01f) where C : ICoordinateSpace, new() + { + PoseDefaultVisualizer.Draw(message.pose.pose, drawing); + TwistDefaultVisualizer.Draw(message.twist.twist, drawing, color, message.pose.pose.position.From(), lengthScale, sphereRadius, thickness); + } + + public override Action CreateGUI(OdometryMsg message, MessageMetadata meta) => () => + { + message.header.GUI(); + GUILayout.Label($"Child frame ID: {message.child_frame_id}"); + message.pose.pose.GUI("Pose:"); + message.twist.twist.GUI("Twist:"); + }; +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Nav/OdometryDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Nav/OdometryDefaultVisualizer.cs.meta new file mode 100644 index 00000000..805b4a61 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Nav/OdometryDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: aa5e309a73db1427387873581691511a +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Nav/PathDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Nav/PathDefaultVisualizer.cs new file mode 100644 index 00000000..c8be78be --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Nav/PathDefaultVisualizer.cs @@ -0,0 +1,44 @@ +using System; +using System.Linq; +using RosMessageTypes.Geometry; +using RosMessageTypes.Nav; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +public class PathDefaultVisualizer : DrawingVisualizer +{ + [SerializeField] + float m_Thickness = 0.1f; + [SerializeField] + Color m_Color; + [SerializeField] + Vector3 m_Offset = Vector3.zero; + [SerializeField] + TFTrackingSettings m_TFTrackingSettings; + + public override void Draw(Drawing3d drawing, PathMsg message, MessageMetadata meta) + { + drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header); + Draw(message, drawing, SelectColor(m_Color, meta), m_Thickness, m_Offset); + } + + public static void Draw(PathMsg message, Drawing3d drawing, Color color, float thickness = 0.1f, Vector3 offset = default(Vector3)) + where C : ICoordinateSpace, new() + { + drawing.DrawPath(message.poses.Select(pose => pose.pose.position.From() + offset), color, thickness); + } + + public override Action CreateGUI(PathMsg message, MessageMetadata meta) + { + return () => + { + message.header.GUI(); + foreach (PoseStampedMsg pose in message.poses) + { + pose.header.GUI(); + pose.pose.GUI(); + } + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Nav/PathDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Nav/PathDefaultVisualizer.cs.meta new file mode 100644 index 00000000..02590d49 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Nav/PathDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 699093dabce49664bb68bd7011175b6c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor.meta new file mode 100644 index 00000000..5de980a9 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 64afce7af25931e479256f33d6d11caf +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/BatteryStateDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/BatteryStateDefaultVisualizer.cs new file mode 100644 index 00000000..c1df3856 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/BatteryStateDefaultVisualizer.cs @@ -0,0 +1,27 @@ +using System; +using RosMessageTypes.Sensor; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using UnityEngine; + +public class BatteryStateDefaultVisualizer : GuiVisualizer +{ + public override Action CreateGUI(BatteryStateMsg message, MessageMetadata meta) + { + var voltage = string.Join(", ", message.cell_voltage); + var temp = string.Join(", ", message.cell_temperature); + + return () => + { + message.header.GUI(); + GUILayout.Label($"Voltage: {message.voltage} (V)\nTemperature: {message.temperature} (ºC)\nCurrent: {message.current} (A)\nCharge: {message.charge} (Ah)\nCapacity: {message.capacity} (Ah)\nDesign Capacity: {message.design_capacity} (Ah)\nPercentage: {message.percentage}"); + GUILayout.Label($"Power supply status: {(BatteryState_Status_Constants)message.power_supply_status}"); + GUILayout.Label($"Power supply health: {(BatteryState_Health_Constants)message.power_supply_health}"); + GUILayout.Label($"Power supply technology: {(BatteryState_Technology_Constants)message.power_supply_technology}"); + GUILayout.Label($"Present: {message.present}"); + GUILayout.Label($"Cell voltage: {voltage}"); + GUILayout.Label($"Cell temperature: {temp}"); + GUILayout.Label($"Location: {message.location}\nSerial number: {message.serial_number}"); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/BatteryStateDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/BatteryStateDefaultVisualizer.cs.meta new file mode 100644 index 00000000..82b1be26 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/BatteryStateDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 2444f926fed994054b751fb88c0cedd6 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/CameraInfoDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/CameraInfoDefaultVisualizer.cs new file mode 100644 index 00000000..610b9e64 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/CameraInfoDefaultVisualizer.cs @@ -0,0 +1,43 @@ +using System; +using RosMessageTypes.Sensor; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using UnityEngine; + +public class CameraInfoDefaultVisualizer : TextureVisualizer +{ + [SerializeField] + public string ImageTopic; + bool m_ViewK; + bool m_ViewP; + bool m_ViewR; + + public override Texture2D CreateTexture(CameraInfoMsg message) + { + // False if ROI not used, true if subwindow captured + if (!message.roi.do_rectify) + return null; + + ITextureVisual imageVisual = VisualizationUtils.GetVisual(ImageTopic) as ITextureVisual; + if (imageVisual == null) + return null; + + return message.roi.RegionOfInterestTexture(imageVisual.GetTexture()); + } + + public override Action CreateGUI(CameraInfoMsg message, MessageMetadata meta, Texture2D tex) + { + return () => + { + message.header.GUI(); + GUILayout.Label($"Height x Width: {message.height}x{message.width}\nDistortion model: {message.distortion_model}"); + GUILayout.Label($"Distortion parameters: {string.Join(", ", message.d)}"); + VisualizationUtils.GUIGrid(message.k, 3, "K", ref m_ViewK); + VisualizationUtils.GUIGrid(message.r, 3, "R", ref m_ViewR); + VisualizationUtils.GUIGrid(message.p, 3, "P", ref m_ViewP); + GUILayout.Label($"Binning X: {message.binning_x}\nBinning Y: {message.binning_y}"); + message.roi.GUI(tex); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/CameraInfoDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/CameraInfoDefaultVisualizer.cs.meta new file mode 100644 index 00000000..b0105b2c --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/CameraInfoDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 47b40ad0f07784a3eb9b79e59b2b08f6 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/CompressedImageDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/CompressedImageDefaultVisualizer.cs new file mode 100644 index 00000000..da2080ee --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/CompressedImageDefaultVisualizer.cs @@ -0,0 +1,23 @@ +using System; +using RosMessageTypes.Sensor; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using UnityEngine; + +public class CompressedImageDefaultVisualizer : TextureVisualizer +{ + public override Texture2D CreateTexture(CompressedImageMsg message) + { + return message.ToTexture2D(); + } + + public override Action CreateGUI(CompressedImageMsg message, MessageMetadata meta, Texture2D tex) + { + return () => + { + message.header.GUI(); + tex.GUITexture(); + GUILayout.Label($"Format: {message.format}"); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/CompressedImageDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/CompressedImageDefaultVisualizer.cs.meta new file mode 100644 index 00000000..021d9b64 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/CompressedImageDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c86bacc4e290e423ab7ac8f9dfde38cb +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/FluidPressureDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/FluidPressureDefaultVisualizer.cs new file mode 100644 index 00000000..e758376a --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/FluidPressureDefaultVisualizer.cs @@ -0,0 +1,16 @@ +using System; +using RosMessageTypes.Sensor; +using Unity.Robotics.Visualizations; +using UnityEngine; + +public class FluidPressureDefaultVisualizer : GuiVisualizer +{ + public override Action CreateGUI(FluidPressureMsg message, MessageMetadata meta) + { + return () => + { + message.header.GUI(); + GUILayout.Label($"Fluid Pressure: {message.fluid_pressure} (Pascals)\nVariance: {message.variance}"); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/FluidPressureDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/FluidPressureDefaultVisualizer.cs.meta new file mode 100644 index 00000000..3b993dcd --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/FluidPressureDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 154673feadb3d41868cfa7f6c127ade4 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/IlluminanceDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/IlluminanceDefaultVisualizer.cs new file mode 100644 index 00000000..c59abd1f --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/IlluminanceDefaultVisualizer.cs @@ -0,0 +1,16 @@ +using System; +using RosMessageTypes.Sensor; +using Unity.Robotics.Visualizations; +using UnityEngine; + +public class IlluminanceDefaultVisualizer : GuiVisualizer +{ + public override Action CreateGUI(IlluminanceMsg message, MessageMetadata meta) + { + return () => + { + message.header.GUI(); + GUILayout.Label($"Illuminance: {message.illuminance} (Lux)\nVariance: {message.variance}"); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/IlluminanceDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/IlluminanceDefaultVisualizer.cs.meta new file mode 100644 index 00000000..98f90bbe --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/IlluminanceDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6cfe585cdd95e41d381711160acf4c02 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ImageDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ImageDefaultVisualizer.cs new file mode 100644 index 00000000..9f34b6e6 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ImageDefaultVisualizer.cs @@ -0,0 +1,127 @@ +using System; +using System.Collections.Generic; +using RosMessageTypes.Sensor; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using UnityEngine; + +public class ImageDefaultVisualizer : BaseVisualFactory +{ + [SerializeField] + bool m_Debayer = true; + + public override bool CanShowDrawing => false; + + protected override IVisual CreateVisual(string topic) + { + return new ImageVisual(topic, this); + } + + public class ImageVisual : ITextureVisual + { + string m_Topic; + + // cache the original image height, width and encoding here so that we if we have to debayer the image (which resizes it) we still show the real values + int m_Width; + int m_Height; + string m_Encoding; + bool m_Debayer; + + ImageDefaultVisualizer m_Factory; + + // after anyone asks for it, we cache the properly processed texture here + Texture2D m_Texture2D; + // if nobody asks for the proper texture, we generate CheapTexture2D: this is just + // the raw bytes from the message, dumped into a texture. + // cheap to make but will look wrong without a special shader + Texture2D m_CheapTexture2D; + Material m_CheapTextureMaterial; + List> m_OnChangeCallbacks = new List>(); + + public void ListenForTextureChange(Action callback) + { + m_OnChangeCallbacks.Add(callback); + } + + public ImageVisual(string topic, ImageDefaultVisualizer factory) + { + m_Topic = topic; + m_Factory = factory; + m_Debayer = m_Factory.m_Debayer; + m_CheapTextureMaterial = new Material(Shader.Find("Unlit/ImageMsg")); + + ROSConnection.GetOrCreateInstance().Subscribe(m_Topic, AddMessage); + } + + public virtual void AddMessage(Message message) + { + if (!VisualizationUtils.AssertMessageType(message, m_Topic)) + return; + + this.message = (ImageMsg)message; + m_Texture2D = null; + m_CheapTexture2D = null; + m_Width = (int)this.message.width; + m_Height = (int)this.message.height; + m_Encoding = this.message.encoding; + //m_CheapTextureMaterial.SetFloat("_gray", this.message.GetNumChannels() == 1 ? 1.0f : 0.0f); + m_CheapTextureMaterial.SetFloat("_convertBGR", this.message.EncodingRequiresBGRConversion() ? 1.0f : 0.0f); + + // if anyone wants to know about the texture, notify them + if (m_OnChangeCallbacks.Count > 0) + GetTexture(); + } + + public ImageMsg message { get; private set; } + + public void OnGUI() + { + if (message == null) + { + GUILayout.Label("Waiting for message..."); + return; + } + + message.header.GUI(); + GUILayout.Label($"{m_Height}x{m_Width}, encoding: {m_Encoding}"); + if (message.data.Length > 0) + { + GUILayout.BeginHorizontal(); + if (m_Encoding.StartsWith("bayer")) + m_Debayer = GUILayout.Toggle(m_Debayer, "Debayer"); + GUILayout.EndHorizontal(); + + if (m_Texture2D != null || m_Debayer) + { + // if we already generated the "real" texture, just use that + GetTexture().GUITexture(); + } + else + { + if (m_CheapTexture2D == null) + { + m_CheapTexture2D = message.ToTexture2D(m_Debayer, convertBGR: false, flipY: false); + } + m_CheapTexture2D.GUITexture(m_CheapTextureMaterial); + } + } + } + + public Texture2D GetTexture() + { + if (m_Texture2D == null) + { + m_Texture2D = (message != null && message.data.Length > 0) ? message.ToTexture2D(m_Debayer) : null; + + foreach (Action callback in m_OnChangeCallbacks) + callback(m_Texture2D); + } + return m_Texture2D; + } + + public bool IsDrawingEnabled => false; + public void SetDrawingEnabled(bool enabled) { } + public void Redraw() { } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ImageDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ImageDefaultVisualizer.cs.meta new file mode 100644 index 00000000..04e5b45f --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ImageDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 31dd4d69a4ee14d7fa748be4177b1fa6 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ImuDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ImuDefaultVisualizer.cs new file mode 100644 index 00000000..ffc0cda5 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ImuDefaultVisualizer.cs @@ -0,0 +1,49 @@ +using System; +using RosMessageTypes.Sensor; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +public class ImuDefaultVisualizer : DrawingVisualizer +{ + [SerializeField] + public Color m_Color; + [SerializeField] + public float m_LengthScale = 1; + [SerializeField] + public float m_SphereRadius = 1; + [SerializeField] + public float m_Thickness = 0.01f; + bool m_ViewAccel; + bool m_ViewAngular; + bool m_ViewOrientation; + [SerializeField] + TFTrackingSettings m_TFTrackingSettings; + + public override void Draw(Drawing3d drawing, ImuMsg message, MessageMetadata meta) + { + drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header); + Draw(message, drawing, SelectColor(m_Color, meta), m_LengthScale, m_SphereRadius, m_Thickness); + } + + public static void Draw(ImuMsg message, Drawing3d drawing, Color color, float lengthScale = 1, float sphereRadius = 1, float thickness = 0.01f) where C : ICoordinateSpace, new() + { + QuaternionDefaultVisualizer.Draw(message.orientation, drawing); + drawing.DrawArrow(Vector3.zero, message.linear_acceleration.From() * lengthScale, color, thickness); + VisualizationUtils.DrawAngularVelocityArrow(drawing, message.angular_velocity.From(), Vector3.zero, color, sphereRadius, thickness); + } + + public override Action CreateGUI(ImuMsg message, MessageMetadata meta) + { + return () => + { + message.header.GUI(); + message.orientation.GUI("Orientation"); + message.angular_velocity.GUI("Angular velocity"); + message.linear_acceleration.GUI("Linear acceleration"); + VisualizationUtils.GUIGrid(message.orientation_covariance, 3, "Orientation covariance", ref m_ViewOrientation); + VisualizationUtils.GUIGrid(message.angular_velocity_covariance, 3, "Angular velocity covariance", ref m_ViewAngular); + VisualizationUtils.GUIGrid(message.linear_acceleration_covariance, 3, "Linear acceleration covariance", ref m_ViewAccel); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ImuDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ImuDefaultVisualizer.cs.meta new file mode 100644 index 00000000..b393d9e0 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ImuDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: d4b553ef5da854fa281147ab847e1c03 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/JointStateDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/JointStateDefaultVisualizer.cs new file mode 100644 index 00000000..54c5d3fd --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/JointStateDefaultVisualizer.cs @@ -0,0 +1,73 @@ +using System; +using RosMessageTypes.Sensor; +using Unity.Robotics.Visualizations; +using UnityEngine; +#if URDF_IMPORTER +using Unity.Robotics.UrdfImporter; +#endif + +public class JointStateDefaultVisualizer : DrawingVisualizer +{ + [SerializeField] + bool m_ShowEffort; +#if URDF_IMPORTER + [SerializeField] + UrdfRobot m_UrdfRobot; + RobotVisualization m_RobotData; +#endif + [SerializeField] + Color m_Color; + [SerializeField] + TFTrackingSettings m_TFTrackingSettings; + + public override void Start() + { + base.Start(); +#if URDF_IMPORTER + if (m_UrdfRobot != null) + m_RobotData = new RobotVisualization(m_UrdfRobot); +#endif + } + + public override void Draw(Drawing3d drawing, JointStateMsg message, MessageMetadata meta) + { +#if URDF_IMPORTER + drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header); + var color = SelectColor(m_Color, meta); + m_RobotData.DrawGhost(drawing, message, color); + if (m_ShowEffort) + m_RobotData.DrawEffort(drawing, message, color); +#endif + } + + public override Action CreateGUI(JointStateMsg message, MessageMetadata meta) + { +#if URDF_IMPORTER + var pos = message.position.Length > 0; + var vel = message.velocity.Length > 0; + var eff = message.effort.Length > 0; + var s = ""; + + for (var i = 0; i < message.name.Length; i++) + { + if (s.Length > 0) s += "\n"; + s += $"{message.name[i]}:"; + if (pos) s += $"\nPosition: {message.position[i]}"; + if (vel) s += $"\nVelocity: {message.velocity[i]}"; + if (eff) s += $"\nEffort: {message.effort[i]}"; + } + + return () => + { + message.header.GUI(); + GUILayout.Label(s); + }; +#else + return () => + { + GUILayout.Label("To use the default visualizer for JointState, please add urdf-importer to your project."); + GUILayout.Label("https://github.com/Unity-Technologies/URDF-Importer"); + }; +#endif + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/JointStateDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/JointStateDefaultVisualizer.cs.meta new file mode 100644 index 00000000..7962bf42 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/JointStateDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: dc4755a1cc0214b92af68e89a25b09f3 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/JoyDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/JoyDefaultVisualizer.cs new file mode 100644 index 00000000..715e0369 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/JoyDefaultVisualizer.cs @@ -0,0 +1,16 @@ +using System; +using RosMessageTypes.Sensor; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using UnityEngine; + +public class JoyDefaultVisualizer : GuiVisualizer +{ + [SerializeField] + public JoyVisualizerSettings m_Settings; + + public override Action CreateGUI(JoyMsg message, MessageMetadata meta) + { + return m_Settings.CreateGUI(message, meta); + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/JoyDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/JoyDefaultVisualizer.cs.meta new file mode 100644 index 00000000..5a6f7325 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/JoyDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: a4c69f0a459af400c8c9acceacad8be7 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/JoyFeedbackArrayDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/JoyFeedbackArrayDefaultVisualizer.cs new file mode 100644 index 00000000..35d7e005 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/JoyFeedbackArrayDefaultVisualizer.cs @@ -0,0 +1,15 @@ +using System; +using RosMessageTypes.Sensor; +using Unity.Robotics.Visualizations; +using UnityEngine; + +public class JoyFeedbackArrayDefaultVisualizer : GuiVisualizer +{ + public override Action CreateGUI(JoyFeedbackArrayMsg message, MessageMetadata meta) + { + return () => + { + foreach (JoyFeedbackMsg m in message.array) m.GUI(); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/JoyFeedbackArrayDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/JoyFeedbackArrayDefaultVisualizer.cs.meta new file mode 100644 index 00000000..ea92ae05 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/JoyFeedbackArrayDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 4954b8850f3f04096a5ab8ceb6e51fea +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/JoyFeedbackDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/JoyFeedbackDefaultVisualizer.cs new file mode 100644 index 00000000..dbe9e230 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/JoyFeedbackDefaultVisualizer.cs @@ -0,0 +1,14 @@ +using RosMessageTypes.Sensor; +using System; +using Unity.Robotics.Visualizations; + +public class JoyFeedbackDefaultVisualizer : GuiVisualizer +{ + public override Action CreateGUI(JoyFeedbackMsg message, MessageMetadata meta) + { + return () => + { + message.GUI(); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/JoyFeedbackDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/JoyFeedbackDefaultVisualizer.cs.meta new file mode 100644 index 00000000..02eca39d --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/JoyFeedbackDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 71673c9c395324bf6be9680ae8210191 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/LaserScanDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/LaserScanDefaultVisualizer.cs new file mode 100644 index 00000000..80062cb5 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/LaserScanDefaultVisualizer.cs @@ -0,0 +1,10 @@ +using System; +using RosMessageTypes.Sensor; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +public class LaserScanDefaultVisualizer : DrawingVisualizerWithSettings +{ + public override string DefaultScriptableObjectPath => "VisualizerSettings/LaserScanVisualizerSettings"; +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/LaserScanDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/LaserScanDefaultVisualizer.cs.meta new file mode 100644 index 00000000..06aa4458 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/LaserScanDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 16302429be2008e47bd0a636c03d05f1 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/MagneticFieldDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/MagneticFieldDefaultVisualizer.cs new file mode 100644 index 00000000..9239e26c --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/MagneticFieldDefaultVisualizer.cs @@ -0,0 +1,35 @@ +using System; +using RosMessageTypes.Sensor; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +public class MagneticFieldDefaultVisualizer : DrawingVisualizer +{ + [SerializeField] + Color m_Color; + bool m_ViewCovariance; + [SerializeField] + TFTrackingSettings m_TFTrackingSettings; + + public override void Draw(Drawing3d drawing, MagneticFieldMsg message, MessageMetadata meta) + { + drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header); + Draw(message, drawing, SelectColor(m_Color, meta)); + } + + public static void Draw(MagneticFieldMsg message, Drawing3d drawing, Color color, float lengthScale = 1) where C : ICoordinateSpace, new() + { + drawing.DrawArrow(Vector3.zero, message.magnetic_field.From() * lengthScale, color); + } + + public override Action CreateGUI(MagneticFieldMsg message, MessageMetadata meta) + { + return () => + { + message.header.GUI(); + message.magnetic_field.GUI("Magnetic field (Tesla)"); + VisualizationUtils.GUIGrid(message.magnetic_field_covariance, 3, "Covariance", ref m_ViewCovariance); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/MagneticFieldDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/MagneticFieldDefaultVisualizer.cs.meta new file mode 100644 index 00000000..75eb5c74 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/MagneticFieldDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 695bf78f0b1e4437db2da726dfd5f785 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/MultiDOFJointStateDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/MultiDOFJointStateDefaultVisualizer.cs new file mode 100644 index 00000000..3de8ed9e --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/MultiDOFJointStateDefaultVisualizer.cs @@ -0,0 +1,64 @@ +using System; +using RosMessageTypes.Sensor; +using Unity.Robotics.Visualizations; +using UnityEngine; +#if URDF_IMPORTER +using Unity.Robotics.UrdfImporter; +#endif + +public class MultiDOFJointStateDefaultVisualizer : DrawingVisualizer +{ +#if URDF_IMPORTER + [SerializeField] + UrdfRobot m_UrdfRobot; + RobotVisualization m_RobotData; +#endif + [SerializeField] + Color m_Color; + [SerializeField] + TFTrackingSettings m_TFTrackingSettings; + + public override void Start() + { + base.Start(); +#if URDF_IMPORTER + if (m_UrdfRobot != null) + m_RobotData = new RobotVisualization(m_UrdfRobot); +#endif + } + + public override void Draw(Drawing3d drawing, MultiDOFJointStateMsg message, MessageMetadata meta) + { +#if URDF_IMPORTER + drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header); + m_RobotData.DrawGhost(drawing, message, SelectColor(m_Color, meta)); +#endif + } + + public override Action CreateGUI(MultiDOFJointStateMsg message, MessageMetadata meta) + { +#if URDF_IMPORTER + var tr = message.transforms.Length > 0; + var tw = message.twist.Length > 0; + var wr = message.wrench.Length > 0; + + return () => + { + message.header.GUI(); + for (var i = 0; i < message.joint_names.Length; i++) + { + GUILayout.Label($"{message.joint_names[i]}"); + if (tr) message.transforms[i].GUI(); + if (tw) message.twist[i].GUI(); + if (wr) message.wrench[i].GUI(); + } + }; +#else + return () => + { + GUILayout.Label("To use the default visualizer for MultiDOFJointState, please add urdf-importer to your project."); + GUILayout.Label("https://github.com/Unity-Technologies/URDF-Importer"); + }; +#endif + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/MultiDOFJointStateDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/MultiDOFJointStateDefaultVisualizer.cs.meta new file mode 100644 index 00000000..a58a811c --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/MultiDOFJointStateDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: effc77ba9c5ea483792df0ccd656fc7f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/MultiEchoLaserScanDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/MultiEchoLaserScanDefaultVisualizer.cs new file mode 100644 index 00000000..b31bd4dc --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/MultiEchoLaserScanDefaultVisualizer.cs @@ -0,0 +1,50 @@ +using System; +using RosMessageTypes.Sensor; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +public class MultiEchoLaserScanDefaultVisualizer : DrawingVisualizerWithSettings +{ + public override string DefaultScriptableObjectPath => "VisualizerSettings/MultiEchoLaserScanVisualizerSettings"; + + public override void Draw(Drawing3d drawing, MultiEchoLaserScanMsg message, MessageMetadata meta) + { + Draw(message, drawing, Settings); + } + + public static void Draw(MultiEchoLaserScanMsg message, Drawing3d drawing, MultiEchoLaserScanVisualizerSettings settings) + where C : ICoordinateSpace, new() + { + Draw(message, drawing.AddPointCloud(message.ranges.Length), settings); + } + + public static void Draw(MultiEchoLaserScanMsg message, PointCloudDrawing pointCloud, MultiEchoLaserScanVisualizerSettings settings) where C : ICoordinateSpace, new() + { + pointCloud.SetCapacity(message.ranges.Length * message.ranges[0].echoes.Length); + // negate the angle because ROS coordinates are right-handed, unity coordinates are left-handed + float angle = -message.angle_min; + // foreach(MLaserEcho echo in message.ranges) + for (int i = 0; i < message.ranges.Length; i++) + { + var echoes = message.ranges[i].echoes; + // foreach (float range in echo.echoes) + for (int j = 0; j < echoes.Length; j++) + { + Vector3 point = Quaternion.Euler(0, Mathf.Rad2Deg * angle, 0) * Vector3.forward * echoes[j]; + Color c = Color.HSVToRGB(Mathf.InverseLerp(message.range_min, message.range_max, echoes[j]), 1, 1); + + var radius = settings.PointRadius; + + if (message.intensities.Length > 0 && settings.UseIntensitySize) + { + radius = Mathf.InverseLerp(settings.SizeRange[0], settings.SizeRange[1], message.intensities[i].echoes[j]); + } + + pointCloud.AddPoint(point, c, radius); + } + angle -= message.angle_increment; + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/MultiEchoLaserScanDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/MultiEchoLaserScanDefaultVisualizer.cs.meta new file mode 100644 index 00000000..f885d22e --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/MultiEchoLaserScanDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0bde06f4fcf854104b7d4970fe09bc99 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/NavSatFixDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/NavSatFixDefaultVisualizer.cs new file mode 100644 index 00000000..fb08e15c --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/NavSatFixDefaultVisualizer.cs @@ -0,0 +1,18 @@ +using System; +using RosMessageTypes.Sensor; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using UnityEngine; + +public class NavSatFixDefaultVisualizer : GuiVisualizer +{ + public override Action CreateGUI(NavSatFixMsg message, MessageMetadata meta) + { + return () => + { + message.header.GUI(); + message.status.GUI(); + GUILayout.Label($"Coordinates: {message.ToLatLongString()}\nAltitude: {message.altitude} (m)\nPosition covariance: {string.Join(", ", message.position_covariance)} (m^2)\nPosition covariance type: {(NavSatFix_Covariance_Constants)message.position_covariance_type}"); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/NavSatFixDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/NavSatFixDefaultVisualizer.cs.meta new file mode 100644 index 00000000..4c266f9e --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/NavSatFixDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 2458f4dffd26746e39c22d7233462b49 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/NavSatStatusDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/NavSatStatusDefaultVisualizer.cs new file mode 100644 index 00000000..f3773e3b --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/NavSatStatusDefaultVisualizer.cs @@ -0,0 +1,15 @@ +using System; +using RosMessageTypes.Sensor; +using Unity.Robotics.Visualizations; +using UnityEngine; + +public class NavSatStatusDefaultVisualizer : GuiVisualizer +{ + public override Action CreateGUI(NavSatStatusMsg message, MessageMetadata meta) + { + return () => + { + message.GUI(); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/NavSatStatusDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/NavSatStatusDefaultVisualizer.cs.meta new file mode 100644 index 00000000..adcd61a6 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/NavSatStatusDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c67a6c59d9f1c42eab47e5ca9d8a8516 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/PointCloud2DefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/PointCloud2DefaultVisualizer.cs new file mode 100644 index 00000000..ba112c7c --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/PointCloud2DefaultVisualizer.cs @@ -0,0 +1,12 @@ +using System; +using System.Collections.Generic; +using RosMessageTypes.Sensor; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +public class PointCloud2DefaultVisualizer : DrawingVisualizerWithSettings +{ + public override string DefaultScriptableObjectPath => "VisualizerSettings/PointCloud2VisualizerSettings"; +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/PointCloud2DefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/PointCloud2DefaultVisualizer.cs.meta new file mode 100644 index 00000000..47ab9b2f --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/PointCloud2DefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f71dde58eb4384a648d556f826fd8b02 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/PointCloudDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/PointCloudDefaultVisualizer.cs new file mode 100644 index 00000000..6de5e821 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/PointCloudDefaultVisualizer.cs @@ -0,0 +1,11 @@ +using System; +using System.Linq; +using RosMessageTypes.Sensor; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +public class PointCloudDefaultVisualizer : DrawingVisualizerWithSettings +{ + public override string DefaultScriptableObjectPath => "VisualizerSettings/PointCloudVisualizerSettings"; +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/PointCloudDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/PointCloudDefaultVisualizer.cs.meta new file mode 100644 index 00000000..025d1fe9 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/PointCloudDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: cc3913d621dc7481b924170b46950dfc +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/PointFieldDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/PointFieldDefaultVisualizer.cs new file mode 100644 index 00000000..0d1ae0af --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/PointFieldDefaultVisualizer.cs @@ -0,0 +1,15 @@ +using System; +using RosMessageTypes.Sensor; +using Unity.Robotics.Visualizations; +using UnityEngine; + +public class PointFieldDefaultVisualizer : DrawingVisualizer +{ + public override Action CreateGUI(PointFieldMsg message, MessageMetadata meta) + { + return () => + { + message.GUI(); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/PointFieldDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/PointFieldDefaultVisualizer.cs.meta new file mode 100644 index 00000000..0cef1180 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/PointFieldDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: aa0b8ae6bccec42b389ffce51057e81d +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/RangeDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/RangeDefaultVisualizer.cs new file mode 100644 index 00000000..2b747e50 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/RangeDefaultVisualizer.cs @@ -0,0 +1,40 @@ +using System; +using RosMessageTypes.Sensor; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +public class RangeDefaultVisualizer : DrawingVisualizer +{ + [SerializeField] + Color m_Color; + [SerializeField] + TFTrackingSettings m_TFTrackingSettings; + + public override void Draw(Drawing3d drawing, RangeMsg message, MessageMetadata meta) + { + drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header); + Draw(message, drawing, SelectColor(m_Color, meta)); + } + + public static void Draw(RangeMsg message, Drawing3d drawing, Color color, float size = 0.1f, bool drawUnityAxes = false) + where C : ICoordinateSpace, new() + { + var asin = Mathf.Asin(message.field_of_view); + var acos = Mathf.Acos(message.field_of_view); + Color col = Color.HSVToRGB(Mathf.InverseLerp(message.min_range, message.max_range, message.range), 1, 1); + Vector3 end = new Vector3(message.range * acos, 0, message.range * asin).toUnity; + + drawing.DrawCone(end, Vector3.zero, col, Mathf.Rad2Deg * message.field_of_view / 2); + } + + public override Action CreateGUI(RangeMsg message, MessageMetadata meta) + { + return () => + { + message.header.GUI(); + GUILayout.Label($"Radiation type: {(Range_RadiationType_Constants)message.radiation_type}\nFOV: {message.field_of_view} (rad)\nMin range: {message.min_range} (m)\nMax range: {message.max_range} (m)\nRange: {message.range} (m)"); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/RangeDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/RangeDefaultVisualizer.cs.meta new file mode 100644 index 00000000..d3fc3e90 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/RangeDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0b206120c451245dd997d68b0605e7d0 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/RelativeHumidityDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/RelativeHumidityDefaultVisualizer.cs new file mode 100644 index 00000000..286f223f --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/RelativeHumidityDefaultVisualizer.cs @@ -0,0 +1,16 @@ +using System; +using RosMessageTypes.Sensor; +using Unity.Robotics.Visualizations; +using UnityEngine; + +public class RelativeHumidityDefaultVisualizer : GuiVisualizer +{ + public override Action CreateGUI(RelativeHumidityMsg message, MessageMetadata meta) + { + return () => + { + message.header.GUI(); + GUILayout.Label($"Relative Humidity: {message.relative_humidity}\nVariance: {message.variance}"); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/RelativeHumidityDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/RelativeHumidityDefaultVisualizer.cs.meta new file mode 100644 index 00000000..eb39b218 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/RelativeHumidityDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6fa4c01962880432ba05b0bbcceeb680 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ScriptableObjects.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ScriptableObjects.meta new file mode 100644 index 00000000..838c566f --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ScriptableObjects.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: af6374b98e6603f469c2c15134ef6d6c +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ScriptableObjects/JoyVisualizerSettings.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ScriptableObjects/JoyVisualizerSettings.cs new file mode 100644 index 00000000..aa71b530 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ScriptableObjects/JoyVisualizerSettings.cs @@ -0,0 +1,237 @@ +using RosMessageTypes.Sensor; +using System; +using System.Collections; +using System.Collections.Generic; +using Unity.Robotics.Visualizations; +using UnityEngine; + +[CreateAssetMenu(fileName = "JoyVisualizerSettings", menuName = "Robotics/Sensor Visualizers/Joy", order = 1)] +public class JoyVisualizerSettings : ScriptableObject +{ + public enum JoyInputType + { + None, + Axis_0, + Axis_1, + Axis_2, + Axis_3, + Axis_4, + Axis_5, + Axis_6, + Axis_7, + Axis_8, + Axis_9, + Axis_10, + Button_0, + Button_1, + Button_2, + Button_3, + Button_4, + Button_5, + Button_6, + Button_7, + Button_8, + Button_9, + Button_10, + Button_11, + Button_12, + Button_13, + Button_14, + Button_15, + } + public JoyInputType m_Button_South; + public JoyInputType m_Button_East; + public JoyInputType m_Button_West; + public JoyInputType m_Button_North; + public JoyInputType m_Button_Back; + public JoyInputType m_Button_Start; + public JoyInputType m_Button_Power; + public JoyInputType m_DPad_Up; + public JoyInputType m_DPad_Down; + public JoyInputType m_DPad_Left; + public JoyInputType m_DPad_Right; + public JoyInputType m_DPad_XAxis; + public JoyInputType m_DPad_YAxis; + public JoyInputType m_LStick_X; + public JoyInputType m_LStick_Y; + public JoyInputType m_LStick_Click; + public JoyInputType m_LShoulder; + public JoyInputType m_LTrigger; + public JoyInputType m_RStick_X; + public JoyInputType m_RStick_Y; + public JoyInputType m_RStick_Click; + public JoyInputType m_RShoulder; + public JoyInputType m_RTrigger; + + public Action CreateGUI(JoyMsg message, MessageMetadata meta) + { + return () => + { + message.header.GUI(); + + Rect layout = GUILayoutUtility.GetAspectRect(2); + float minX = layout.xMin; + float maxX = layout.xMax; + float midX = (minX + maxX) / 2; + float minY = layout.yMin; + float maxY = layout.yMax; + float midY = (minY + maxY) / 2; + float w = layout.width; + float h = layout.height; + float sw = w * 0.125f; + float sh = h * 0.25f; + float bw = w * 0.0625f; + float bh = h * 0.125f; + + // Triggers + GUI.Box(new Rect(minX + sw, minY, bw, sh), TriggerTexture(GetInputFloat(message, m_LTrigger))); + GUI.Box(new Rect(maxX - sw * 1.5f, minY, bw, sh), TriggerTexture(GetInputFloat(message, m_RTrigger))); + + // Shoulders + GUI.Box(new Rect(minX + sw * 1.5f, minY + bh, bw, bh), ButtonTexture(GetInputBool(message, m_LShoulder))); + GUI.Box(new Rect(maxX - sw * 2.0f, minY + bh, bw, bh), ButtonTexture(GetInputBool(message, m_RShoulder))); + + // Dpad, central buttons + GUI.Box(new Rect(midX - sw * 2.5f, maxY - sh, sw, sh), + StickTexture( + GetInputAxisOrButtons(message, m_DPad_XAxis, m_DPad_Left, m_DPad_Right), + GetInputAxisOrButtons(message, m_DPad_YAxis, m_DPad_Up, m_DPad_Down), + false) + ); + + if (m_Button_Back != JoyInputType.None) + GUI.Box(new Rect(midX - bw * 2.5f, midY - bh, bw, bh), ButtonTexture(GetInputBool(message, m_Button_Back))); + if (m_Button_Power != JoyInputType.None) + GUI.Box(new Rect(midX - bw * 0.5f, midY - bh, bw, bh), ButtonTexture(GetInputBool(message, m_Button_Power))); + if (m_Button_Start != JoyInputType.None) + GUI.Box(new Rect(midX + bw * 1.5f, midY - bh, bw, bh), ButtonTexture(GetInputBool(message, m_Button_Start))); + + // N/E/S/W buttons + float btnX = maxX - sw * 1.25f; + float btnY = midY - bh * 0.5f; + GUI.Box(new Rect(btnX - bw, btnY, bw, bh), ButtonTexture(GetInputBool(message, m_Button_West))); + GUI.Box(new Rect(btnX, btnY - bh, bw, bh), ButtonTexture(GetInputBool(message, m_Button_North))); + GUI.Box(new Rect(btnX, btnY + bh, bw, bh), ButtonTexture(GetInputBool(message, m_Button_South))); + GUI.Box(new Rect(btnX + bw, btnY, bw, bh), ButtonTexture(GetInputBool(message, m_Button_East))); + + // Joysticks + GUI.Box(new Rect(minX + sw * 0.5f, midY - sh * 0.5f, sw, sh), StickTexture(GetInputFloat(message, m_LStick_X), GetInputFloat(message, m_LStick_Y), GetInputBool(message, m_LStick_Click))); + GUI.Box(new Rect(midX + sw * 1.5f, maxY - sh, sw, sh), StickTexture(GetInputFloat(message, m_RStick_X), GetInputFloat(message, m_RStick_Y), GetInputBool(message, m_RStick_Click))); + }; + } + + public float GetInputFloat(JoyMsg message, JoyInputType inputType) + { + if (inputType >= JoyInputType.Button_0) + { + int buttonIndex = inputType - JoyInputType.Button_0; + if (buttonIndex < 0 || buttonIndex >= message.buttons.Length) + return 0.0f; + return message.buttons[buttonIndex] != 0 ? 1.0f : 0.0f; + } + + int axisIndex = inputType - JoyInputType.Axis_0; + if (axisIndex < 0 || axisIndex >= message.axes.Length) + return 0.0f; + return message.axes[axisIndex]; + } + + public float GetInputAxisOrButtons(JoyMsg message, JoyInputType axis, JoyInputType upButton, JoyInputType downButton) + { + if (axis != JoyInputType.None) + return GetInputFloat(message, axis); + + return GetInputFloat(message, upButton) - GetInputFloat(message, downButton); + } + + public bool GetInputBool(JoyMsg message, JoyInputType inputType) + { + if (inputType >= JoyInputType.Button_0) + { + int buttonIndex = inputType - JoyInputType.Button_0; + if (buttonIndex < 0 || buttonIndex >= message.buttons.Length) + return false; + return message.buttons[buttonIndex] != 0; + } + + int axisIndex = inputType - JoyInputType.Axis_0; + if (axisIndex < 0 || axisIndex >= message.axes.Length) + return false; + return message.axes[axisIndex] != 0; + } + + static Color32[] s_ColorHighlight; + static Color32[] GetColorHighlight() + { + if (s_ColorHighlight == null) + { + s_ColorHighlight = new Color32[100]; + for (int i = 0; i < s_ColorHighlight.Length; i++) + { + s_ColorHighlight[i] = Color.red; + } + } + return s_ColorHighlight; + } + + static Color32[] s_ColorPress; + static Color32[] GetColorPress() + { + if (s_ColorPress == null) + { + s_ColorPress = new Color32[2500]; + for (int i = 0; i < s_ColorPress.Length; i++) + { + s_ColorPress[i] = Color.blue; + } + } + return s_ColorPress; + } + + public Texture2D TriggerTexture(float fraction) + { + Texture2D tex = new Texture2D(25, 50); + int y = Mathf.FloorToInt(fraction * 20) + tex.height / 2; + tex.SetPixels32(0, y - 2, 25, 4, GetColorHighlight()); + tex.Apply(); + return tex; + } + + static Texture2D s_ButtonPressed; + static Texture2D s_ButtonUnpressed; + + public Texture2D ButtonTexture(bool pressed) + { + if (pressed) + { + if (s_ButtonPressed == null) + { + s_ButtonPressed = new Texture2D(10, 10); + s_ButtonPressed.SetPixels32(GetColorHighlight()); + s_ButtonPressed.Apply(); + } + return s_ButtonPressed; + } + else + { + if (s_ButtonUnpressed == null) + { + s_ButtonUnpressed = new Texture2D(10, 10); + s_ButtonUnpressed.Apply(); + } + return s_ButtonUnpressed; + } + } + + public Texture2D StickTexture(float xAxis, float yAxis, bool clicked) + { + Texture2D tex = new Texture2D(50, 50); + if (clicked) + tex.SetPixels32(GetColorPress()); + int x = Mathf.FloorToInt(xAxis * -20) + tex.width / 2; + int y = Mathf.FloorToInt(yAxis * 20) + tex.height / 2; + tex.SetPixels32(x - 5, y - 5, 10, 10, GetColorHighlight()); + tex.Apply(); + return tex; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ScriptableObjects/JoyVisualizerSettings.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ScriptableObjects/JoyVisualizerSettings.cs.meta new file mode 100644 index 00000000..f4bde33e --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ScriptableObjects/JoyVisualizerSettings.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 3e64381bafaf07f41b8194aec986fe7e +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ScriptableObjects/LaserScanVisualizerSettings.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ScriptableObjects/LaserScanVisualizerSettings.cs new file mode 100644 index 00000000..b9814d04 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ScriptableObjects/LaserScanVisualizerSettings.cs @@ -0,0 +1,82 @@ +using RosMessageTypes.Sensor; +using System; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +[CreateAssetMenu(fileName = "LaserScanVisualizerSettings", menuName = "Robotics/Sensor Visualizers/LaserScan", order = 1)] +public class LaserScanVisualizerSettings : VisualizerSettingsGeneric +{ + [SerializeField] + bool m_UseIntensitySize; + public bool UseIntensitySize { get => m_UseIntensitySize; set => m_UseIntensitySize = value; } + [HideInInspector, SerializeField] + float m_PointRadius = 0.05f; + public float PointRadius { get => m_PointRadius; set => m_PointRadius = value; } + [HideInInspector, SerializeField] + float m_MaxIntensity = 100.0f; + public float MaxIntensity { get => m_MaxIntensity; set => m_MaxIntensity = value; } + + public enum ColorModeType + { + Distance, + Intensity, + Angle, + } + + [SerializeField] + ColorModeType m_ColorMode; + public ColorModeType ColorMode { get => m_ColorMode; set => m_ColorMode = value; } + + public override void Draw(Drawing3d drawing, LaserScanMsg message, MessageMetadata meta) + { + drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header); + + PointCloudDrawing pointCloud = drawing.AddPointCloud(message.ranges.Length); + // negate the angle because ROS coordinates are right-handed, unity coordinates are left-handed + float angle = -message.angle_min; + ColorModeType mode = m_ColorMode; + if (mode == ColorModeType.Intensity && message.intensities.Length != message.ranges.Length) + mode = ColorModeType.Distance; + for (int i = 0; i < message.ranges.Length; i++) + { + Vector3 point = Quaternion.Euler(0, Mathf.Rad2Deg * angle, 0) * Vector3.forward * message.ranges[i]; + + Color32 c = Color.white; + switch (mode) + { + case ColorModeType.Distance: + c = Color.HSVToRGB(Mathf.InverseLerp(message.range_min, message.range_max, message.ranges[i]), 1, 1); + break; + case ColorModeType.Intensity: + c = new Color(1, message.intensities[i] / m_MaxIntensity, 0, 1); + break; + case ColorModeType.Angle: + c = Color.HSVToRGB((1 + angle / (Mathf.PI * 2)) % 1, 1, 1); + break; + } + + float radius = m_PointRadius; + if (m_UseIntensitySize && message.intensities.Length > 0) + { + radius = Mathf.InverseLerp(0, m_MaxIntensity, message.intensities[i]); + } + pointCloud.AddPoint(point, c, radius); + + angle -= message.angle_increment; + } + pointCloud.Bake(); + } + + public override Action CreateGUI(LaserScanMsg message, MessageMetadata meta) + { + return () => + { + message.header.GUI(); + GUILayout.Label($"Angle min {message.angle_min}, max {message.angle_max}, increment {message.angle_increment}"); + GUILayout.Label($"Time between measurements {message.time_increment}; time between scans {message.scan_time}"); + GUILayout.Label($"Range min {message.range_min}, max {message.range_max}"); + GUILayout.Label(message.intensities.Length == 0 ? $"{message.ranges.Length} range entries (no intensity data)" : $"{message.ranges.Length} range and intensity entries"); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ScriptableObjects/LaserScanVisualizerSettings.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ScriptableObjects/LaserScanVisualizerSettings.cs.meta new file mode 100644 index 00000000..52683991 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ScriptableObjects/LaserScanVisualizerSettings.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b7a16d0bc53a1492bb43ad71301a4a2e +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ScriptableObjects/MultiEchoLaserScanVisualizerSettings.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ScriptableObjects/MultiEchoLaserScanVisualizerSettings.cs new file mode 100644 index 00000000..1932db5b --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ScriptableObjects/MultiEchoLaserScanVisualizerSettings.cs @@ -0,0 +1,37 @@ +using RosMessageTypes.Sensor; +using System; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +[CreateAssetMenu(fileName = "MultiEchoLaserScanVisualizerSettings", menuName = "Robotics/Sensor Visualizers/MultiEchoLaserScan", order = 1)] +public class MultiEchoLaserScanVisualizerSettings : VisualizerSettingsGeneric +{ + [SerializeField] + bool m_UseIntensitySize; + public bool UseIntensitySize { get => m_UseIntensitySize; set => m_UseIntensitySize = value; } + [HideInInspector, SerializeField] + float m_PointRadius = 0.05f; + public float PointRadius { get => m_PointRadius; set => m_PointRadius = value; } + [HideInInspector, SerializeField] + float[] m_SizeRange = { 0, 100 }; + public float[] SizeRange { get => m_SizeRange; set => m_SizeRange = value; } + + public override void Draw(Drawing3d drawing, MultiEchoLaserScanMsg message, MessageMetadata meta) + { + drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header); + MultiEchoLaserScanDefaultVisualizer.Draw(message, drawing, this); + } + + public override Action CreateGUI(MultiEchoLaserScanMsg message, MessageMetadata meta) + { + return () => + { + message.header.GUI(); + GUILayout.Label($"Angle min, max: [{message.angle_min}, {message.angle_max}] (rad)\nIncrement: {message.angle_increment} (rad)"); + GUILayout.Label($"Time between measurements: {message.time_increment} (s)\nTime between scans: {message.scan_time} (s)"); + GUILayout.Label($"Range min, max: [{message.range_min}, {message.range_max}] (m)"); + }; + } + +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ScriptableObjects/MultiEchoLaserScanVisualizerSettings.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ScriptableObjects/MultiEchoLaserScanVisualizerSettings.cs.meta new file mode 100644 index 00000000..57b529be --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ScriptableObjects/MultiEchoLaserScanVisualizerSettings.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0234f04e5173b4e9e8a3bcae2afcde80 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ScriptableObjects/PointCloud2VisualizerSettings.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ScriptableObjects/PointCloud2VisualizerSettings.cs new file mode 100644 index 00000000..9e34bc75 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ScriptableObjects/PointCloud2VisualizerSettings.cs @@ -0,0 +1,182 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using RosMessageTypes.Sensor; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +[CreateAssetMenu(fileName = "PointCloud2VisualizerSettings", menuName = "Robotics/Sensor Visualizers/PointCloud2", order = 1)] +public class PointCloud2VisualizerSettings : VisualizerSettingsGeneric +{ + public enum ColorMode + { + HSV, + SeparateRGB, + CombinedRGB, + } + + [HideInInspector, SerializeField] + ColorMode m_ColorModeSetting; + public ColorMode ColorModeSetting { get => m_ColorModeSetting; set => m_ColorModeSetting = value; } + public string[] Channels { get => m_Channels; set => m_Channels = value; } + string[] m_Channels; + + public string XChannel { get => m_XChannel; set => m_XChannel = value; } + string m_XChannel = "x"; + public string YChannel { get => m_YChannel; set => m_YChannel = value; } + string m_YChannel = "y"; + public string ZChannel { get => m_ZChannel; set => m_ZChannel = value; } + string m_ZChannel = "z"; + public string HueChannel { get => m_HueChannel; set => m_HueChannel = value; } + string m_HueChannel = ""; + public string RgbChannel { get => m_RgbChannel; set => m_RgbChannel = value; } + string m_RgbChannel = "rgb"; + public string RChannel { get => m_RChannel; set => m_RChannel = value; } + string m_RChannel = ""; + public string GChannel { get => m_GChannel; set => m_GChannel = value; } + string m_GChannel = ""; + public string BChannel { get => m_BChannel; set => m_BChannel = value; } + string m_BChannel = ""; + public string SizeChannel { get => m_SizeChannel; set => m_SizeChannel = value; } + string m_SizeChannel = ""; + + public float[] HueRange { get => m_HueRange; set => m_HueRange = value; } + float[] m_HueRange = { 0, 100 }; + public float[] RRange { get => m_RRange; set => m_RRange = value; } + float[] m_RRange = { 0, 100 }; + public float[] GRange { get => m_GRange; set => m_GRange = value; } + float[] m_GRange = { 0, 100 }; + public float[] BRange { get => m_BRange; set => m_BRange = value; } + float[] m_BRange = { 0, 100 }; + public float[] SizeRange { get => m_SizeRange; set => m_SizeRange = value; } + float[] m_SizeRange = { 0, 100 }; + public float Size { get => m_Size; set => m_Size = value; } + float m_Size = 0.05f; + + public bool UseRgbChannel { get => m_UseRgbChannel; set => m_UseRgbChannel = value; } + bool m_UseRgbChannel = true; + public bool UseSizeChannel { get => m_UseSizeChannel; set => m_UseSizeChannel = value; } + bool m_UseSizeChannel = true; + + public override void Draw(Drawing3d drawing, PointCloud2Msg message, MessageMetadata meta) + { + drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header); + var pointCloud = drawing.AddPointCloud((int)(message.data.Length / message.point_step)); + + Channels = message.fields.Select(field => field.name).ToArray(); + + Dictionary channelToIdx = new Dictionary(); + for (int i = 0; i < message.fields.Length; i++) + { + channelToIdx.Add(message.fields[i].name, i); + } + + TFFrame frame = TFSystem.instance.GetTransform(message.header); + + Func colorGenerator = (int iPointStep) => Color.white; + + if (m_UseRgbChannel) + { + switch (ColorModeSetting) + { + case ColorMode.HSV: + if (m_HueChannel.Length > 0) + { + int hueChannelOffset = (int)message.fields[channelToIdx[m_HueChannel]].offset; + colorGenerator = (int iPointStep) => + { + int colC = BitConverter.ToInt16(message.data, (iPointStep + hueChannelOffset)); + return Color.HSVToRGB(Mathf.InverseLerp(m_HueRange[0], m_HueRange[1], colC), 1, 1); + }; + } + break; + case ColorMode.SeparateRGB: + if (m_RChannel.Length > 0 && m_GChannel.Length > 0 && m_BChannel.Length > 0) + { + int rChannelOffset = (int)message.fields[channelToIdx[m_RChannel]].offset; + int gChannelOffset = (int)message.fields[channelToIdx[m_GChannel]].offset; + int bChannelOffset = (int)message.fields[channelToIdx[m_BChannel]].offset; + colorGenerator = (int iPointStep) => + { + var colR = Mathf.InverseLerp(m_RRange[0], m_RRange[1], BitConverter.ToSingle(message.data, iPointStep + rChannelOffset)); + var colG = Mathf.InverseLerp(m_GRange[0], m_GRange[1], BitConverter.ToSingle(message.data, iPointStep + gChannelOffset)); + var colB = Mathf.InverseLerp(m_BRange[0], m_BRange[1], BitConverter.ToSingle(message.data, iPointStep + bChannelOffset)); + return new Color(colR, colG, colB, 1); + }; + } + break; + case ColorMode.CombinedRGB: + if (m_RgbChannel.Length > 0) + { + int rgbChannelOffset = (int)message.fields[channelToIdx[m_RgbChannel]].offset; + colorGenerator = (int iPointStep) => new Color32 + ( + message.data[iPointStep + rgbChannelOffset + 2], + message.data[iPointStep + rgbChannelOffset + 1], + message.data[iPointStep + rgbChannelOffset], + 255 + ); + } + break; + } + } + + int xChannelOffset = (int)message.fields[channelToIdx[m_XChannel]].offset; + int yChannelOffset = (int)message.fields[channelToIdx[m_YChannel]].offset; + int zChannelOffset = (int)message.fields[channelToIdx[m_ZChannel]].offset; + int sizeChannelOffset = 0; + bool useSizeChannel = m_UseSizeChannel && m_SizeChannel != ""; + if (useSizeChannel) + sizeChannelOffset = (int)message.fields[channelToIdx[m_SizeChannel]].offset; + int maxI = message.data.Length / (int)message.point_step; + for (int i = 0; i < maxI; i++) + { + int iPointStep = i * (int)message.point_step; + var x = BitConverter.ToSingle(message.data, iPointStep + xChannelOffset); + var y = BitConverter.ToSingle(message.data, iPointStep + yChannelOffset); + var z = BitConverter.ToSingle(message.data, iPointStep + zChannelOffset); + Vector3 rosPoint = new Vector3(x, y, z); + Vector3 unityPoint = rosPoint.toUnity; + + Color color = colorGenerator(iPointStep); + + float radius; + if (useSizeChannel) + { + var size = BitConverter.ToSingle(message.data, iPointStep + sizeChannelOffset); + radius = Mathf.InverseLerp(m_SizeRange[0], m_SizeRange[1], size) * m_Size; + } + else + { + radius = m_Size; + } + + pointCloud.AddPoint(unityPoint, color, radius); + } + } + + public override Action CreateGUI(PointCloud2Msg message, MessageMetadata meta) + { + var formatDict = new Dictionary>(); + + foreach (var field in message.fields) + if (formatDict.ContainsKey((PointField_Format_Constants)field.datatype)) + formatDict[(PointField_Format_Constants)field.datatype].Add(field.name); + else + formatDict.Add((PointField_Format_Constants)field.datatype, new List { field.name }); + + var formats = ""; + foreach (var f in formatDict) + if (f.Value.Count > 0) + formats += $"{f.Key}: {string.Join(", ", f.Value)}\n"; + + return () => + { + message.header.GUI(); + GUILayout.Label($"Height x Width: {message.height}x{message.width}\nData length: {message.data.Length}\nPoint step: {message.point_step}\nRow step: {message.row_step}\nIs dense: {message.is_dense}"); + GUILayout.Label($"Channels:\n{formats}"); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ScriptableObjects/PointCloud2VisualizerSettings.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ScriptableObjects/PointCloud2VisualizerSettings.cs.meta new file mode 100644 index 00000000..c28ca486 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ScriptableObjects/PointCloud2VisualizerSettings.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 9869b969f021440789a37afecf3ffca4 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ScriptableObjects/PointCloudVisualizerSettings.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ScriptableObjects/PointCloudVisualizerSettings.cs new file mode 100644 index 00000000..2b798722 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ScriptableObjects/PointCloudVisualizerSettings.cs @@ -0,0 +1,131 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using RosMessageTypes.Sensor; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +[CreateAssetMenu(fileName = "PointCloudVisualizerSettings", menuName = "Robotics/Sensor Visualizers/PointCloud", order = 1)] +public class PointCloudVisualizerSettings : VisualizerSettingsGeneric +{ + [HideInInspector, SerializeField] + PointCloud2VisualizerSettings.ColorMode m_ColorMode; + public PointCloud2VisualizerSettings.ColorMode ColorMode { get => m_ColorMode; set => m_ColorMode = value; } + + public string[] Channels { get => m_Channels; set => m_Channels = value; } + string[] m_Channels; + public string HueChannel { get => m_HueChannel; set => m_HueChannel = value; } + string m_HueChannel = ""; + public string RgbChannel { get => m_RgbChannel; set => m_RgbChannel = value; } + string m_RgbChannel = ""; + public string RChannel { get => m_RChannel; set => m_RChannel = value; } + string m_RChannel = ""; + public string GChannel { get => m_GChannel; set => m_GChannel = value; } + string m_GChannel = ""; + public string BChannel { get => m_BChannel; set => m_BChannel = value; } + string m_BChannel = ""; + public string SizeChannel { get => m_SizeChannel; set => m_SizeChannel = value; } + string m_SizeChannel = ""; + + public float[] HueRange { get => m_HueRange; set => m_HueRange = value; } + float[] m_HueRange = { 0, 100 }; + public float[] RRange { get => m_RRange; set => m_RRange = value; } + float[] m_RRange = { 0, 100 }; + public float[] GRange { get => m_GRange; set => m_GRange = value; } + float[] m_GRange = { 0, 100 }; + public float[] BRange { get => m_BRange; set => m_BRange = value; } + float[] m_BRange = { 0, 100 }; + public float[] SizeRange { get => m_SizeRange; set => m_SizeRange = value; } + float[] m_SizeRange = { 0, 100 }; + public float Size { get => m_Size; set => m_Size = value; } + float m_Size = 0.05f; + + public bool UseRgbChannel { get => m_UseRgbChannel; set => m_UseRgbChannel = value; } + bool m_UseRgbChannel = true; + public bool UseSeparateRgb { get => m_UseSeparateRgb; set => m_UseSeparateRgb = value; } + bool m_UseSeparateRgb = true; + public bool UseSizeChannel { get => m_UseSizeChannel; set => m_UseSizeChannel = value; } + bool m_UseSizeChannel = true; + + public override Action CreateGUI(PointCloudMsg message, MessageMetadata meta) + { + var channelNames = string.Join(", ", message.channels.Select(i => i.name)); + + return () => + { + message.header.GUI(); + GUILayout.Label($"Length of points: {message.points.Length}\nChannel names: {channelNames}"); + }; + } + + public override void Draw(Drawing3d drawing, PointCloudMsg message, MessageMetadata meta) + { + drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header); + PointCloudDrawing pointCloud = drawing.AddPointCloud(); + Channels = message.channels.Select(field => field.name).ToArray(); + + pointCloud.SetCapacity(message.points.Length); + + Dictionary channelToIdx = new Dictionary(); + for (int i = 0; i < message.channels.Length; i++) + { + channelToIdx.Add(message.channels[i].name, i); + } + + Color color = Color.white; + for (int i = 0; i < message.points.Length; i++) + { + Vector3 point = message.points[i].From(); + + if (m_UseRgbChannel) + { + switch (m_ColorMode) + { + case PointCloud2VisualizerSettings.ColorMode.HSV: + if (m_HueChannel.Length > 0) + { + float colC = message.channels[channelToIdx[m_HueChannel]].values[i]; + color = Color.HSVToRGB(Mathf.InverseLerp(m_HueRange[0], m_HueRange[1], colC), 1, 1); + } + break; + case PointCloud2VisualizerSettings.ColorMode.SeparateRGB: + if (m_RChannel.Length > 0 && m_GChannel.Length > 0 && m_BChannel.Length > 0) + { + var colR = Mathf.InverseLerp(m_RRange[0], m_RRange[1], message.channels[channelToIdx[m_RChannel]].values[i]); + var r = Mathf.InverseLerp(0, 1, colR); + + var colG = Mathf.InverseLerp(m_GRange[0], m_GRange[1], message.channels[channelToIdx[m_GChannel]].values[i]); + var g = Mathf.InverseLerp(0, 1, colG); + + var colB = Mathf.InverseLerp(m_BRange[0], m_BRange[1], message.channels[channelToIdx[m_BChannel]].values[i]); + var b = Mathf.InverseLerp(0, 1, colB); + color = new Color(r, g, b, 1); + } + break; + case PointCloud2VisualizerSettings.ColorMode.CombinedRGB: + // uint8 (R,G,B) values packed into the least significant 24 bits, in order. + { + byte[] rgb = BitConverter.GetBytes(message.channels[channelToIdx[m_RgbChannel]].values[i]); + + var r = Mathf.InverseLerp(0, 1, BitConverter.ToSingle(rgb, 0)); + var g = Mathf.InverseLerp(0, 1, BitConverter.ToSingle(rgb, 8)); + var b = Mathf.InverseLerp(0, 1, BitConverter.ToSingle(rgb, 16)); + color = new Color(r, g, b, 1); + } + break; + } + } + + var radius = m_Size; + if (m_UseSizeChannel && m_SizeChannel.Length > 0) + { + var size = message.channels[channelToIdx[m_SizeChannel]].values[i]; + radius = Mathf.InverseLerp(m_SizeRange[0], m_SizeRange[1], size); + } + + pointCloud.AddPoint(point, color, radius); + } + pointCloud.Bake(); + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ScriptableObjects/PointCloudVisualizerSettings.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ScriptableObjects/PointCloudVisualizerSettings.cs.meta new file mode 100644 index 00000000..fba5e473 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/ScriptableObjects/PointCloudVisualizerSettings.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 5116417a614f44321b9dd939cff73583 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/TemperatureDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/TemperatureDefaultVisualizer.cs new file mode 100644 index 00000000..e3651a6d --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/TemperatureDefaultVisualizer.cs @@ -0,0 +1,16 @@ +using System; +using RosMessageTypes.Sensor; +using Unity.Robotics.Visualizations; +using UnityEngine; + +public class TemperatureDefaultVisualizer : GuiVisualizer +{ + public override Action CreateGUI(TemperatureMsg message, MessageMetadata meta) + { + return () => + { + message.header.GUI(); + GUILayout.Label($"Temperature: {message.temperature} (ºC)\nVariance: {message.variance}"); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/TemperatureDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/TemperatureDefaultVisualizer.cs.meta new file mode 100644 index 00000000..9a14a60f --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/TemperatureDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6025e3d3cb1d74c58ba9bf04d7417f22 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/TimeReferenceDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/TimeReferenceDefaultVisualizer.cs new file mode 100644 index 00000000..249060d5 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/TimeReferenceDefaultVisualizer.cs @@ -0,0 +1,17 @@ +using System; +using RosMessageTypes.Sensor; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using UnityEngine; + +public class TimeReferenceDefaultVisualizer : GuiVisualizer +{ + public override Action CreateGUI(TimeReferenceMsg message, MessageMetadata meta) + { + return () => + { + message.header.GUI(); + GUILayout.Label($"Time reference:{message.time_ref.ToTimestampString()}\nSource: {message.source}"); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/TimeReferenceDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/TimeReferenceDefaultVisualizer.cs.meta new file mode 100644 index 00000000..1d68575c --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Sensor/TimeReferenceDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: d7207899898b44893b24d167f92ebf76 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Shape.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Shape.meta new file mode 100644 index 00000000..064fbc2d --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Shape.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: c67a5961210ebe44a9d7eedd18ae35fe +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Shape/MeshDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Shape/MeshDefaultVisualizer.cs new file mode 100644 index 00000000..c0b9ca65 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Shape/MeshDefaultVisualizer.cs @@ -0,0 +1,41 @@ +using System; +using System.Linq; +using RosMessageTypes.Shape; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +public class MeshDefaultVisualizer : DrawingVisualizer +{ + [SerializeField] + GameObject m_Origin; + [SerializeField] + Color m_Color; + + public override void Draw(Drawing3d drawing, MeshMsg message, MessageMetadata meta) + { + Draw(message, drawing, SelectColor(m_Color, meta), m_Origin); + } + + public static void Draw(MeshMsg message, Drawing3d drawing, Color color, GameObject origin = null) where C : ICoordinateSpace, new() + { + Mesh mesh = new Mesh(); + mesh.vertices = message.vertices.Select(v => v.From()).ToArray(); + mesh.triangles = message.triangles.SelectMany(tri => tri.vertex_indices.Select(i => (int)i)).ToArray(); + if (origin != null) + drawing.DrawMesh(mesh, origin.transform, color); + else + drawing.DrawMesh(mesh, Vector3.zero, Quaternion.identity, Vector3.one, color); + } + + public override Action CreateGUI(MeshMsg message, MessageMetadata meta) + { + var showTriangles = false; + return () => + { + showTriangles = GUILayout.Toggle(showTriangles, $"Show {message.vertices.Length} vertices"); + if (showTriangles) + message.GUI(); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Shape/MeshDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Shape/MeshDefaultVisualizer.cs.meta new file mode 100644 index 00000000..3c375ce2 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Shape/MeshDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: a55d9039eb2184d4683f569bb1463786 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Shape/PlaneDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Shape/PlaneDefaultVisualizer.cs new file mode 100644 index 00000000..c4dfb198 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Shape/PlaneDefaultVisualizer.cs @@ -0,0 +1,45 @@ +using System; +using RosMessageTypes.Shape; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +public class PlaneDefaultVisualizer : DrawingVisualizer +{ + [SerializeField] + Color m_Color; + + public override void Draw(Drawing3d drawing, PlaneMsg message, MessageMetadata meta) + { + Draw(message, drawing, SelectColor(m_Color, meta)); + } + + public static void Draw(PlaneMsg message, Drawing3d drawing, Color color, GameObject center = null, float size = 10.0f) + where C : ICoordinateSpace, new() + { + Draw(message, drawing, color, (center != null) ? center.transform.position : Vector3.zero, size); + } + + public static void Draw(PlaneMsg message, Drawing3d drawing, Color color, Vector3 origin, float size = 10.0f) where C : ICoordinateSpace, new() + { + Vector3 normal = new Vector3((float)message.coef[0], (float)message.coef[1], (float)message.coef[2]).toUnity; + float d = (float)message.coef[3]; + + float normalScale = (Vector3.Dot(normal, origin) + d) / normal.sqrMagnitude; + Vector3 center = origin - normal * normalScale; + + Vector3 forward = (Mathf.Abs(normal.x) > Mathf.Abs(normal.y)) ? Vector3.Cross(normal, Vector3.up).normalized : Vector3.Cross(normal, Vector3.right).normalized; + Vector3 side = Vector3.Cross(normal, forward).normalized; + Vector3 diagonalA = (forward + side) * size; + Vector3 diagonalB = (forward - side) * size; + drawing.DrawQuad(center - diagonalA, center + diagonalB, center + diagonalA, center - diagonalB, color, true); + } + + public override Action CreateGUI(PlaneMsg message, MessageMetadata meta) + { + return () => + { + GUILayout.Label($"[{message.coef[0]}, {message.coef[1]}, {message.coef[2]}, {message.coef[3]}]"); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Shape/PlaneDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Shape/PlaneDefaultVisualizer.cs.meta new file mode 100644 index 00000000..020d78f5 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Shape/PlaneDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: e6257160834256b4b861ad5458dd4050 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Shape/SolidPrimitiveDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Shape/SolidPrimitiveDefaultVisualizer.cs new file mode 100644 index 00000000..030a6933 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Shape/SolidPrimitiveDefaultVisualizer.cs @@ -0,0 +1,58 @@ +using System; +using RosMessageTypes.Shape; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +public class SolidPrimitiveDefaultVisualizer : DrawingVisualizer +{ + [SerializeField] + GameObject m_Origin; + [SerializeField] + Color m_Color; + + public override void Draw(Drawing3d drawing, SolidPrimitiveMsg message, MessageMetadata meta) + { + Draw(message, drawing, SelectColor(m_Color, meta), m_Origin); + } + + public static void Draw(SolidPrimitiveMsg message, Drawing3d drawing, Color color, GameObject origin = null) + where C : ICoordinateSpace, new() + { + Vector3 originPosition = origin != null ? origin.transform.position : Vector3.zero; + Quaternion originRotation = origin != null ? origin.transform.rotation : Quaternion.identity; + switch (message.type) + { + case SolidPrimitiveMsg.BOX: + drawing.DrawCuboid( + originPosition, + new Vector3( + (float)message.dimensions[SolidPrimitiveMsg.BOX_X] * 0.5f, + (float)message.dimensions[SolidPrimitiveMsg.BOX_Y] * 0.5f, + (float)message.dimensions[SolidPrimitiveMsg.BOX_Z] * 0.5f).toUnity, + originRotation, + color + ); + break; + case SolidPrimitiveMsg.SPHERE: + drawing.DrawSphere(originPosition, color, (float)message.dimensions[SolidPrimitiveMsg.SPHERE_RADIUS]); + break; + case SolidPrimitiveMsg.CYLINDER: + Vector3 cylinderAxis = originRotation * Vector3.up * (float)message.dimensions[SolidPrimitiveMsg.CYLINDER_HEIGHT] * 0.5f; + drawing.DrawCylinder(originPosition - cylinderAxis, originPosition + cylinderAxis, color, (float)message.dimensions[SolidPrimitiveMsg.CYLINDER_RADIUS]); + break; + case SolidPrimitiveMsg.CONE: + Vector3 coneAxis = originRotation * Vector3.up * (float)message.dimensions[SolidPrimitiveMsg.CONE_HEIGHT] * 0.5f; + drawing.DrawCone(originPosition - coneAxis, originPosition + coneAxis, color, (float)message.dimensions[SolidPrimitiveMsg.CONE_RADIUS]); + break; + } + } + + public override Action CreateGUI(SolidPrimitiveMsg message, MessageMetadata meta) + { + return () => + { + message.GUI(); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Shape/SolidPrimitiveDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Shape/SolidPrimitiveDefaultVisualizer.cs.meta new file mode 100644 index 00000000..254cab17 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Shape/SolidPrimitiveDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 50fba5bdff765464e9ad375003b5267f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std.meta new file mode 100644 index 00000000..c2b86382 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: e63c2830a85631041a4fc85077fc7f3d +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/ByteMultiArrayDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/ByteMultiArrayDefaultVisualizer.cs new file mode 100644 index 00000000..36dedf03 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/ByteMultiArrayDefaultVisualizer.cs @@ -0,0 +1,18 @@ +using System; +using RosMessageTypes.Std; +using Unity.Robotics.Visualizations; +using UnityEngine; + +public class ByteMultiArrayDefaultVisualizer : GuiVisualizer +{ + [SerializeField] + bool m_Tabulate = true; + + public override Action CreateGUI(ByteMultiArrayMsg message, MessageMetadata meta) + { + return () => + { + message.layout.GUIMultiArray(message.data, ref m_Tabulate); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/ByteMultiArrayDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/ByteMultiArrayDefaultVisualizer.cs.meta new file mode 100644 index 00000000..158ae00c --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/ByteMultiArrayDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: d7d806713ecf02040a67c8e91540b052 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/ColorRGBADefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/ColorRGBADefaultVisualizer.cs new file mode 100644 index 00000000..9067618e --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/ColorRGBADefaultVisualizer.cs @@ -0,0 +1,15 @@ +using System; +using RosMessageTypes.Std; +using Unity.Robotics.Visualizations; +using UnityEngine; + +public class ColorRGBADefaultVisualizer : GuiVisualizer +{ + public override Action CreateGUI(ColorRGBAMsg message, MessageMetadata meta) + { + return () => + { + message.GUI(); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/ColorRGBADefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/ColorRGBADefaultVisualizer.cs.meta new file mode 100644 index 00000000..418d3e05 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/ColorRGBADefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 7dffbc3f3164cbd428d12e169f45bd6e +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/Float32MultiArrayDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/Float32MultiArrayDefaultVisualizer.cs new file mode 100644 index 00000000..8ae8fdbf --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/Float32MultiArrayDefaultVisualizer.cs @@ -0,0 +1,18 @@ +using System; +using RosMessageTypes.Std; +using Unity.Robotics.Visualizations; +using UnityEngine; + +public class Float32MultiArrayDefaultVisualizer : GuiVisualizer +{ + [SerializeField] + bool m_Tabulate = true; + + public override Action CreateGUI(Float32MultiArrayMsg message, MessageMetadata meta) + { + return () => + { + message.layout.GUIMultiArray(message.data, ref m_Tabulate); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/Float32MultiArrayDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/Float32MultiArrayDefaultVisualizer.cs.meta new file mode 100644 index 00000000..7986258a --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/Float32MultiArrayDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: ddb07ee5b24bed04888df0c97561fe0a +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/Float64MultiArrayDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/Float64MultiArrayDefaultVisualizer.cs new file mode 100644 index 00000000..05ee6b11 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/Float64MultiArrayDefaultVisualizer.cs @@ -0,0 +1,18 @@ +using System; +using RosMessageTypes.Std; +using Unity.Robotics.Visualizations; +using UnityEngine; + +public class Float64MultiArrayDefaultVisualizer : GuiVisualizer +{ + [SerializeField] + bool m_Tabulate = true; + + public override Action CreateGUI(Float64MultiArrayMsg message, MessageMetadata meta) + { + return () => + { + message.layout.GUIMultiArray(message.data, ref m_Tabulate); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/Float64MultiArrayDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/Float64MultiArrayDefaultVisualizer.cs.meta new file mode 100644 index 00000000..96734894 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/Float64MultiArrayDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 988ec541b83a2e048be71cbd080cd871 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/Int16MultiArrayDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/Int16MultiArrayDefaultVisualizer.cs new file mode 100644 index 00000000..296962e8 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/Int16MultiArrayDefaultVisualizer.cs @@ -0,0 +1,18 @@ +using System; +using RosMessageTypes.Std; +using Unity.Robotics.Visualizations; +using UnityEngine; + +public class Int16MultiArrayDefaultVisualizer : GuiVisualizer +{ + [SerializeField] + bool m_Tabulate = true; + + public override Action CreateGUI(Int16MultiArrayMsg message, MessageMetadata meta) + { + return () => + { + message.layout.GUIMultiArray(message.data, ref m_Tabulate); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/Int16MultiArrayDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/Int16MultiArrayDefaultVisualizer.cs.meta new file mode 100644 index 00000000..120f837a --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/Int16MultiArrayDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 595224681c0c2a14aa4bf87ee554f490 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/Int32MultiArrayDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/Int32MultiArrayDefaultVisualizer.cs new file mode 100644 index 00000000..ed84b0af --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/Int32MultiArrayDefaultVisualizer.cs @@ -0,0 +1,18 @@ +using System; +using RosMessageTypes.Std; +using Unity.Robotics.Visualizations; +using UnityEngine; + +public class Int32MultiArrayDefaultVisualizer : GuiVisualizer +{ + [SerializeField] + bool m_Tabulate = true; + + public override Action CreateGUI(Int32MultiArrayMsg message, MessageMetadata meta) + { + return () => + { + message.layout.GUIMultiArray(message.data, ref m_Tabulate); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/Int32MultiArrayDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/Int32MultiArrayDefaultVisualizer.cs.meta new file mode 100644 index 00000000..50516f62 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/Int32MultiArrayDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6ed9ee96944ecbc4fafaa9e6c75a8183 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/Int64MultiArrayDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/Int64MultiArrayDefaultVisualizer.cs new file mode 100644 index 00000000..29ca6aed --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/Int64MultiArrayDefaultVisualizer.cs @@ -0,0 +1,18 @@ +using System; +using RosMessageTypes.Std; +using Unity.Robotics.Visualizations; +using UnityEngine; + +public class Int64MultiArrayDefaultVisualizer : GuiVisualizer +{ + [SerializeField] + bool m_Tabulate = true; + + public override Action CreateGUI(Int64MultiArrayMsg message, MessageMetadata meta) + { + return () => + { + message.layout.GUIMultiArray(message.data, ref m_Tabulate); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/Int64MultiArrayDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/Int64MultiArrayDefaultVisualizer.cs.meta new file mode 100644 index 00000000..05f60ab7 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/Int64MultiArrayDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 7367dea8b0ce5fa4e90214177ca45079 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/Int8MultiArrayDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/Int8MultiArrayDefaultVisualizer.cs new file mode 100644 index 00000000..4b2ba3b2 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/Int8MultiArrayDefaultVisualizer.cs @@ -0,0 +1,18 @@ +using System; +using RosMessageTypes.Std; +using Unity.Robotics.Visualizations; +using UnityEngine; + +public class Int8MultiArrayDefaultVisualizer : GuiVisualizer +{ + [SerializeField] + bool m_Tabulate = true; + + public override Action CreateGUI(Int8MultiArrayMsg message, MessageMetadata meta) + { + return () => + { + message.layout.GUIMultiArray(message.data, ref m_Tabulate); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/Int8MultiArrayDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/Int8MultiArrayDefaultVisualizer.cs.meta new file mode 100644 index 00000000..e3ac0cac --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/Int8MultiArrayDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 9f4e04d09fa3452489c3652d848172f3 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/TimeDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/TimeDefaultVisualizer.cs new file mode 100644 index 00000000..c0c90cb2 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/TimeDefaultVisualizer.cs @@ -0,0 +1,15 @@ +using System; +using RosMessageTypes.BuiltinInterfaces; +using Unity.Robotics.Visualizations; +using UnityEngine; + +public class TimeDefaultVisualizer : GuiVisualizer +{ + public override Action CreateGUI(TimeMsg message, MessageMetadata meta) + { + return () => + { + message.GUI(); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/TimeDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/TimeDefaultVisualizer.cs.meta new file mode 100644 index 00000000..ac42d101 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/TimeDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 1b78264b4fe81a446bffc3cd3f65be67 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/UInt16MultiArrayDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/UInt16MultiArrayDefaultVisualizer.cs new file mode 100644 index 00000000..75ee422d --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/UInt16MultiArrayDefaultVisualizer.cs @@ -0,0 +1,18 @@ +using System; +using RosMessageTypes.Std; +using Unity.Robotics.Visualizations; +using UnityEngine; + +public class UInt16MultiArrayDefaultVisualizer : GuiVisualizer +{ + [SerializeField] + bool m_Tabulate = true; + + public override Action CreateGUI(UInt16MultiArrayMsg message, MessageMetadata meta) + { + return () => + { + message.layout.GUIMultiArray(message.data, ref m_Tabulate); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/UInt16MultiArrayDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/UInt16MultiArrayDefaultVisualizer.cs.meta new file mode 100644 index 00000000..154b5815 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/UInt16MultiArrayDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 7cade9fa5fce2fb4c86bfb960650bc19 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/UInt32MultiArrayDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/UInt32MultiArrayDefaultVisualizer.cs new file mode 100644 index 00000000..ec0f8e5d --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/UInt32MultiArrayDefaultVisualizer.cs @@ -0,0 +1,18 @@ +using System; +using RosMessageTypes.Std; +using Unity.Robotics.Visualizations; +using UnityEngine; + +public class UInt32MultiArrayDefaultVisualizer : GuiVisualizer +{ + [SerializeField] + bool m_Tabulate = true; + + public override Action CreateGUI(UInt32MultiArrayMsg message, MessageMetadata meta) + { + return () => + { + message.layout.GUIMultiArray(message.data, ref m_Tabulate); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/UInt32MultiArrayDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/UInt32MultiArrayDefaultVisualizer.cs.meta new file mode 100644 index 00000000..7e07f064 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/UInt32MultiArrayDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: ec0af1c42844d604c91f24201ee8d301 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/UInt64MultiArrayDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/UInt64MultiArrayDefaultVisualizer.cs new file mode 100644 index 00000000..c8801b16 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/UInt64MultiArrayDefaultVisualizer.cs @@ -0,0 +1,18 @@ +using System; +using RosMessageTypes.Std; +using Unity.Robotics.Visualizations; +using UnityEngine; + +public class UInt64MultiArrayDefaultVisualizer : GuiVisualizer +{ + [SerializeField] + bool m_Tabulate = true; + + public override Action CreateGUI(UInt64MultiArrayMsg message, MessageMetadata meta) + { + return () => + { + message.layout.GUIMultiArray(message.data, ref m_Tabulate); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/UInt64MultiArrayDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/UInt64MultiArrayDefaultVisualizer.cs.meta new file mode 100644 index 00000000..88f17623 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/UInt64MultiArrayDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 7a17e46fd53d29440bbd888cdd5be496 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/UInt8MultiArrayDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/UInt8MultiArrayDefaultVisualizer.cs new file mode 100644 index 00000000..49308f16 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/UInt8MultiArrayDefaultVisualizer.cs @@ -0,0 +1,18 @@ +using System; +using RosMessageTypes.Std; +using Unity.Robotics.Visualizations; +using UnityEngine; + +public class UInt8MultiArrayDefaultVisualizer : GuiVisualizer +{ + [SerializeField] + bool m_Tabulate = true; + + public override Action CreateGUI(UInt8MultiArrayMsg message, MessageMetadata meta) + { + return () => + { + message.layout.GUIMultiArray(message.data, ref m_Tabulate); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/UInt8MultiArrayDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/UInt8MultiArrayDefaultVisualizer.cs.meta new file mode 100644 index 00000000..08fea458 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Std/UInt8MultiArrayDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: cbf496ff53d39fb4f95457addd3626a9 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Stereo.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Stereo.meta new file mode 100644 index 00000000..47a05e26 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Stereo.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 278c048983c16014a864797223a772d6 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Stereo/DisparityImageDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Stereo/DisparityImageDefaultVisualizer.cs new file mode 100644 index 00000000..31de4387 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Stereo/DisparityImageDefaultVisualizer.cs @@ -0,0 +1,38 @@ +using RosMessageTypes.Stereo; +using System; +using System.Collections; +using System.Collections.Generic; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public class DisparityImageDefaultVisualizer : TextureVisualizer + { + static Material s_Material; + + public override Action CreateGUI(DisparityImageMsg message, MessageMetadata meta, Texture2D tex) + { + if (s_Material == null) + s_Material = new Material(Shader.Find("Unlit/DisparityImage")); + Material material = new Material(s_Material); + material.SetFloat("_MinDisparity", message.min_disparity); + material.SetFloat("_MaxDisparity", message.max_disparity); + + return () => + { + message.image.header.GUI(); + GUILayout.Label($"f: {message.f}, T: {message.t}, delta_d:{message.delta_d}"); + GUILayout.Label($"Disparity: [{message.min_disparity}..{message.max_disparity}]"); + GUILayout.Label($"Region of interest: "); + message.valid_window.GUI(); + tex.GUITexture(material); + }; + } + + public override Texture2D CreateTexture(DisparityImageMsg message) + { + return message.image.ToTexture2D(false); + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Stereo/DisparityImageDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Stereo/DisparityImageDefaultVisualizer.cs.meta new file mode 100644 index 00000000..67758ed0 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Stereo/DisparityImageDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 64f41ca8e53c67c4b8ae0a8f503acec6 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/TFSystemVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/TFSystemVisualizer.cs new file mode 100644 index 00000000..7b5d201f --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/TFSystemVisualizer.cs @@ -0,0 +1,179 @@ +using System; +using System.Collections.Generic; +using Unity.Robotics.ROSTCPConnector; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public class TFSystemVisualizer : MonoBehaviour, IHudTab + { + public float axesScale = 0.1f; + public float lineThickness = 0.01f; + public Color color = Color.white; + Dictionary drawings = new Dictionary(); + + public void Start() + { + TFSystem.GetOrCreateInstance().AddListener(OnChanged); + HudPanel.RegisterTab(this, (int)HudTabOrdering.TF); + if (color.a == 0) + color.a = 1; + } + + void EnsureSettings(TFStream stream) + { + if (!m_ShowExpanded.ContainsKey(stream)) + { + m_ShowExpanded.Add(stream, true); + m_ShowAxes.Add(stream, ShowTFAxesDefault); + m_ShowLinks.Add(stream, ShowTFLinksDefault); + m_ShowNames.Add(stream, ShowTFNamesDefault); + } + } + + public void OnChanged(TFStream stream) + { + Drawing3d drawing; + if (!drawings.TryGetValue(stream.Name, out drawing)) + { + drawing = Drawing3d.Create(); + drawings[stream.Name] = drawing; + if (stream.Parent != null) + { + OnChanged(stream.Parent); + Drawing3d parentStream; + if (drawings.TryGetValue(stream.Parent.Name, out parentStream)) + { + drawing.transform.parent = parentStream.transform; + } + } + } + + TFFrame frame = stream.GetLocalTF(); + + drawing.transform.localPosition = frame.translation; + drawing.transform.localRotation = frame.rotation; + drawing.Clear(); + EnsureSettings(stream); + if (m_ShowAxes[stream]) + VisualizationUtils.DrawAxisVectors(drawing, Vector3.zero.To(), Quaternion.identity.To(), axesScale, false); + + if (m_ShowLinks[stream]) + drawing.DrawLine(Quaternion.Inverse(frame.rotation) * -frame.translation, Vector3.zero, color, lineThickness); + + if (m_ShowNames[stream]) + drawing.DrawLabel(stream.Name, Vector3.zero, color); + } + + // === Code for the "Transforms" hud tab === + + const float k_IndentWidth = 10; + const float k_TFNameWidth = 136; + const float k_CheckboxWidth = 35; + Vector2 m_TransformMenuScrollPosition; + + // Default visualization settings + Dictionary m_ShowExpanded = new Dictionary(); + public bool ShowTFAxesDefault { get; set; } + Dictionary m_ShowAxes = new Dictionary(); + public bool ShowTFLinksDefault { get; set; } + Dictionary m_ShowLinks = new Dictionary(); + public bool ShowTFNamesDefault { get; set; } + Dictionary m_ShowNames = new Dictionary(); + + string IHudTab.Label => "Transforms"; + + void IHudTab.OnSelected() + { + } + + void IHudTab.OnDeselected() + { + } + + void IHudTab.OnGUI(HudPanel hud) + { + m_TransformMenuScrollPosition = GUILayout.BeginScrollView(m_TransformMenuScrollPosition); + + GUI.changed = false; + GUILayout.BeginHorizontal(GUILayout.Height(20)); + GUILayout.Label("", GUILayout.Width(k_TFNameWidth)); + ShowTFAxesDefault = DrawTFHeaderCheckbox(ShowTFAxesDefault, "Axes", (stream, check) => m_ShowAxes[stream] = check); + ShowTFLinksDefault = DrawTFHeaderCheckbox(ShowTFLinksDefault, "Link", (stream, check) => m_ShowLinks[stream] = check); + ShowTFNamesDefault = DrawTFHeaderCheckbox(ShowTFNamesDefault, "Lbl", (stream, check) => m_ShowNames[stream] = check); + GUILayout.EndHorizontal(); + bool globalChange = GUI.changed; + + // draw the root objects + int numDrawn = 0; + foreach (TFStream stream in TFSystem.instance.GetTransforms()) + { + if (stream.Parent == null) + { + numDrawn++; + DrawTFStreamHierarchy(stream, 0, globalChange); + } + } + + if (numDrawn == 0) + { + GUILayout.Label("(No transform data received yet)"); + } + + GUILayout.EndScrollView(); + } + + void DrawTFStreamHierarchy(TFStream stream, int indent, bool globalChange) + { + GUI.changed = false; + EnsureSettings(stream); + + GUILayout.BeginHorizontal(); + GUILayout.Space(indent * k_IndentWidth); + + /*#if UNITY_EDITOR + GUIStyle style = new GUIStyle(UnityEditor.EditorStyles.foldout); + style.fixedWidth = k_TFNameWidth;// - indent * k_IndentWidth; + style.stretchWidth = false; + m_ShowExpanded[stream] = UnityEditor.EditorGUILayout.Foldout(m_ShowExpanded[stream], stream.Name, true, style);// GUILayout.Width(k_TFNameWidth - indent * k_IndentWidth)); + #else*/ + if (GUILayout.Button(stream.Name, GUI.skin.label, GUILayout.Width(k_TFNameWidth - indent * k_IndentWidth))) + m_ShowExpanded[stream] = !m_ShowExpanded[stream]; + //#endif + + m_ShowAxes[stream] = GUILayout.Toggle(m_ShowAxes[stream], "", GUILayout.Width(k_CheckboxWidth)); + m_ShowLinks[stream] = GUILayout.Toggle(m_ShowLinks[stream], "", GUILayout.Width(k_CheckboxWidth)); + m_ShowNames[stream] = GUILayout.Toggle(m_ShowNames[stream], "", GUILayout.Width(k_CheckboxWidth)); + GUILayout.EndHorizontal(); + + if (GUI.changed || globalChange) + { + TFSystem.instance.NotifyAllChanged(stream); + } + + if (m_ShowExpanded[stream]) + foreach (TFStream child in stream.Children) + DrawTFStreamHierarchy(child, indent + 1, globalChange); + } + + bool DrawTFHeaderCheckbox(bool wasChecked, string label, Action setter) + { + bool result = GUILayout.Toggle(wasChecked, "", GUILayout.Width(k_CheckboxWidth)); + + Rect checkbox = GUILayoutUtility.GetLastRect(); + Vector2 textSize = GUI.skin.label.CalcSize(new GUIContent(label)); + Rect labelRect = new Rect(checkbox.xMin - textSize.x, checkbox.yMin, textSize.x, checkbox.height); + GUI.Label(labelRect, label); + + if (wasChecked != result) + { + foreach (TFStream stream in TFSystem.instance.GetTransforms()) + { + setter(stream, result); + } + } + return result; + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/TFSystemVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/TFSystemVisualizer.cs.meta new file mode 100644 index 00000000..fba0670f --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/TFSystemVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 89775f84ec1b05048997be0d603cca35 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Trajectory.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Trajectory.meta new file mode 100644 index 00000000..3fa4e48c --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Trajectory.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 8757b954cdf62834cb5b49da10ca8e19 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Trajectory/JointTrajectoryDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Trajectory/JointTrajectoryDefaultVisualizer.cs new file mode 100644 index 00000000..c4c31eb8 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Trajectory/JointTrajectoryDefaultVisualizer.cs @@ -0,0 +1,53 @@ +using System; +using RosMessageTypes.Trajectory; +using Unity.Robotics.Visualizations; +using UnityEngine; +#if URDF_IMPORTER +using Unity.Robotics.UrdfImporter; +#endif + +public class JointTrajectoryDefaultVisualizer : DrawingVisualizer +{ +#if URDF_IMPORTER + [SerializeField] + UrdfRobot m_UrdfRobot; + RobotVisualization m_RobotData; +#endif + [SerializeField] + float m_PathThickness = 0.01f; + [SerializeField] + Color m_Color; + [SerializeField] + TFTrackingSettings m_TFTrackingSettings; + + + public override void Start() + { + base.Start(); +#if URDF_IMPORTER + if (m_UrdfRobot != null) + { + m_RobotData = new RobotVisualization(m_UrdfRobot); + } +#endif + } + + public override void Draw(Drawing3d drawing, JointTrajectoryMsg message, MessageMetadata meta) + { +#if URDF_IMPORTER + drawing.SetTFTrackingSettings(m_TFTrackingSettings, message.header); + m_RobotData.DrawJointPaths(drawing, message, SelectColor(m_Color, meta), m_PathThickness); +#endif + } + +#if !URDF_IMPORTER + public override Action CreateGUI(JointTrajectoryMsg message, MessageMetadata meta) + { + return () => + { + GUILayout.Label("To use the default visualizer for JointTrajectory, please add urdf-importer to your project."); + GUILayout.Label("https://github.com/Unity-Technologies/URDF-Importer"); + }; + } +#endif +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Trajectory/JointTrajectoryDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Trajectory/JointTrajectoryDefaultVisualizer.cs.meta new file mode 100644 index 00000000..825d7e39 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Trajectory/JointTrajectoryDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f99e8ef135fd8d14194efcb02e2b240a +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Visualization.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Visualization.meta new file mode 100644 index 00000000..85a48f8e --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Visualization.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: fe65b1af9983acb4fb5713ed2d0416b5 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Visualization/MarkerDefaultVisualizer.cs b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Visualization/MarkerDefaultVisualizer.cs new file mode 100644 index 00000000..16a7c587 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Visualization/MarkerDefaultVisualizer.cs @@ -0,0 +1,330 @@ +using RosMessageTypes.Visualization; +using System.Collections; +using System.Collections.Generic; +using System.Linq; +using Unity.Robotics.ROSTCPConnector; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public class MarkerDefaultVisualizer : BaseVisualFactory + { + public override bool CanShowDrawing => true; + + public override void Start() + { + base.Start(); + + if (m_Topic == "") + { + VisualFactoryRegistry.RegisterTypeVisualizer(this, Priority); + } + } + + protected override IVisual CreateVisual(string topic) + { + return new MarkersVisual(topic); + } + + public class MarkersVisual : IVisual + { + public string Label => $"Markers ({m_DrawingNamespaces.Count})"; + Dictionary> m_DrawingNamespaces = new Dictionary>(); + HashSet m_HiddenNamespaces = new HashSet(); + bool m_IsDrawingEnabled; + public bool IsDrawingEnabled => m_IsDrawingEnabled; + + public MarkersVisual(string topic) + { + ROSConnection ros = ROSConnection.GetOrCreateInstance(); + RosTopicState state = ros.GetTopic(topic); + if (state.RosMessageName == MarkerArrayMsg.k_RosMessageName) + ros.Subscribe(topic, OnMarkerArray); + else if (state.RosMessageName == MarkerMsg.k_RosMessageName) + ros.Subscribe(topic, OnMarker); + } + + public void Destroy() + { + + } + + public void OnGUI() + { + if (m_DrawingNamespaces.Count == 0) + { + GUILayout.Label("Waiting for messages..."); + } + + foreach (KeyValuePair> element in m_DrawingNamespaces) + { + string key = element.Key; + bool hidden = m_HiddenNamespaces.Contains(key); + bool newHidden = GUILayout.Toggle(hidden, $"{key} ({element.Value.Count})"); + if (newHidden != hidden) + { + if (newHidden) + { + m_HiddenNamespaces.Remove(key); + foreach (Drawing3d drawing in element.Value.Values) + { + drawing.gameObject.SetActive(false); + } + } + else + { + m_HiddenNamespaces.Add(key); + foreach (Drawing3d drawing in element.Value.Values) + { + drawing.gameObject.SetActive(true); + } + } + } + } + } + + void OnMarkerArray(MarkerArrayMsg array) + { + foreach (MarkerMsg marker in array.markers) + OnMarker(marker); + } + + void OnMarker(MarkerMsg marker) + { + Dictionary ns; + Drawing3d drawing; + switch (marker.action) + { + case MarkerMsg.DELETEALL: + foreach (Dictionary namespaceToDestroy in m_DrawingNamespaces.Values) + foreach (Drawing3d drawingToDestroy in namespaceToDestroy.Values) + drawingToDestroy.Destroy(); + m_DrawingNamespaces.Clear(); + break; + case MarkerMsg.ADD: + if (!m_DrawingNamespaces.TryGetValue(marker.ns, out ns)) + { + ns = new Dictionary(); + m_DrawingNamespaces.Add(marker.ns, ns); + } + if (!ns.TryGetValue(marker.id, out drawing)) + { + drawing = Drawing3d.Create(); + ns.Add(marker.id, drawing); + } + if (marker.lifetime.sec == 0 && marker.lifetime.nanosec == 0) + drawing.ClearDuration(); + else + drawing.SetDuration(marker.lifetime.sec + marker.lifetime.nanosec / 1E9f); + drawing.Clear(); + Draw(marker, drawing); + break; + case MarkerMsg.DELETE: + if (!m_DrawingNamespaces.TryGetValue(marker.ns, out ns)) + { + ns = new Dictionary(); + m_DrawingNamespaces.Add(marker.ns, ns); + } + if (ns.TryGetValue(marker.id, out drawing)) + { + drawing.Destroy(); + ns.Remove(marker.id); + } + break; + } + } + + public static void Draw(MarkerMsg marker, Drawing3d drawing) + where C : ICoordinateSpace, new() + { + switch (marker.type) + { + case MarkerMsg.ARROW: + Vector3 startPoint; + Vector3 endPoint; + if (marker.points.Length >= 2) + { + startPoint = marker.points[0].From(); + endPoint = marker.points[1].From(); + + float arrowheadGradient = 0.5f; + if (marker.scale.z != 0) + arrowheadGradient = (float)(marker.scale.y / marker.scale.z); + + drawing.DrawArrow(startPoint, endPoint, marker.color.ToUnityColor(), + (float)marker.scale.x, (float)(marker.scale.y / marker.scale.x), arrowheadGradient); + } + else + { + startPoint = marker.pose.position.From(); + endPoint = startPoint + marker.pose.orientation.From() * Vector3.forward * (float)marker.scale.x; + + drawing.DrawArrow(startPoint, endPoint, marker.color.ToUnityColor(), (float)marker.scale.y); + } + break; + case MarkerMsg.CUBE: + drawing.DrawCuboid(marker.pose.position.From(), marker.scale.From() * 0.5f, marker.pose.orientation.From(), marker.color.ToUnityColor()); + break; + case MarkerMsg.SPHERE: + drawing.DrawSpheroid(marker.pose.position.From(), marker.scale.From() * 0.5f, marker.pose.orientation.From(), marker.color.ToUnityColor()); + break; + case MarkerMsg.CYLINDER: + drawing.transform.position = marker.pose.position.From(); + drawing.transform.rotation = marker.pose.orientation.From(); + drawing.transform.localScale = marker.scale.From(); + drawing.DrawCylinder(new Vector3(0, -0.5f, 0), new Vector3(0, 0.5f, 0), marker.color.ToUnityColor(), 0.5f); + break; + case MarkerMsg.LINE_STRIP: + drawing.transform.position = marker.pose.position.From(); + drawing.transform.rotation = marker.pose.orientation.From(); + if (marker.colors.Length == marker.points.Length) + { + drawing.DrawLineStrip(marker.points.Select(p => p.From()).ToArray(), marker.colors.Select(c => (Color32)c.ToUnityColor()).ToArray(), (float)marker.scale.x); + } + else + { + drawing.DrawLineStrip(marker.points.Select(p => p.From()).ToArray(), marker.color.ToUnityColor(), (float)marker.scale.x); + } + break; + case MarkerMsg.LINE_LIST: + drawing.transform.position = marker.pose.position.From(); + drawing.transform.rotation = marker.pose.orientation.From(); + if (marker.colors.Length == marker.points.Length) + { + drawing.DrawLines(marker.points.Select(p => p.From()).ToArray(), marker.colors.Select(c => (Color32)c.ToUnityColor()).ToArray(), (float)marker.scale.x); + } + else + { + drawing.DrawLines(marker.points.Select(p => p.From()).ToArray(), marker.color.ToUnityColor(), (float)marker.scale.x); + } + break; + case MarkerMsg.CUBE_LIST: + { + drawing.transform.position = marker.pose.position.From(); + drawing.transform.rotation = marker.pose.orientation.From(); + Vector3 cubeScale = marker.scale.From() * 0.5f; + if (marker.colors.Length == marker.points.Length) + { + for (int Idx = 0; Idx < marker.points.Length; ++Idx) + { + drawing.DrawCuboid(marker.points[Idx].From(), cubeScale, marker.colors[Idx].ToUnityColor()); + } + } + else + { + Color32 color = marker.color.ToUnityColor(); + for (int Idx = 0; Idx < marker.points.Length; ++Idx) + { + drawing.DrawCuboid(marker.points[Idx].From(), cubeScale, color); + } + } + } + break; + case MarkerMsg.SPHERE_LIST: + { + drawing.transform.position = marker.pose.position.From(); + drawing.transform.rotation = marker.pose.orientation.From(); + Vector3 radii = marker.scale.From() * 0.5f; + if (marker.colors.Length == marker.points.Length) + { + for (int Idx = 0; Idx < marker.points.Length; ++Idx) + { + drawing.DrawSpheroid(marker.points[Idx].From(), radii, Quaternion.identity, marker.colors[Idx].ToUnityColor()); + } + } + else + { + Color32 color = marker.color.ToUnityColor(); + for (int Idx = 0; Idx < marker.points.Length; ++Idx) + { + drawing.DrawSpheroid(marker.points[Idx].From(), radii, Quaternion.identity, color); + } + } + } + break; + case MarkerMsg.POINTS: + { + PointCloudDrawing cloud = drawing.AddPointCloud(marker.points.Length); + cloud.transform.position = marker.pose.position.From(); + cloud.transform.rotation = marker.pose.orientation.From(); + float radius = (float)marker.scale.x; + if (marker.colors.Length == marker.points.Length) + { + for (int Idx = 0; Idx < marker.points.Length; ++Idx) + { + cloud.AddPoint(marker.points[Idx].From(), marker.colors[Idx].ToUnityColor(), radius); + } + } + else + { + Color32 color = marker.color.ToUnityColor(); + for (int Idx = 0; Idx < marker.points.Length; ++Idx) + { + cloud.AddPoint(marker.points[Idx].From(), color, radius); + } + } + cloud.Bake(); + } + break; + case MarkerMsg.TEXT_VIEW_FACING: + drawing.DrawLabel(marker.text, marker.pose.position.From(), marker.color.ToUnityColor()); + break; + case MarkerMsg.MESH_RESOURCE: + break; + case MarkerMsg.TRIANGLE_LIST: + { + drawing.transform.position = marker.pose.position.From(); + drawing.transform.rotation = marker.pose.orientation.From(); + float radius = (float)marker.scale.x; + if (marker.colors.Length == marker.points.Length) + { + for (int Idx = 2; Idx < marker.points.Length; Idx += 3) + { + drawing.DrawTriangle( + marker.points[Idx - 2].From(), + marker.points[Idx - 1].From(), + marker.points[Idx].From(), + marker.colors[Idx - 2].ToUnityColor(), + marker.colors[Idx - 1].ToUnityColor(), + marker.colors[Idx].ToUnityColor()); + } + } + else + { + Color32 color = marker.color.ToUnityColor(); + for (int Idx = 2; Idx < marker.points.Length; Idx += 3) + { + drawing.DrawTriangle( + marker.points[Idx - 2].From(), + marker.points[Idx - 1].From(), + marker.points[Idx].From(), + color); + } + } + } + break; + } + } + + public void SetDrawingEnabled(bool enabled) + { + if (m_IsDrawingEnabled == enabled) + return; + + foreach (var ns in m_DrawingNamespaces.Values) + { + foreach (Drawing3d drawing in ns.Values) + drawing.enabled = enabled; + } + m_IsDrawingEnabled = enabled; + } + + public void Redraw() + { + SetDrawingEnabled(true); + } + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Visualization/MarkerDefaultVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Visualization/MarkerDefaultVisualizer.cs.meta new file mode 100644 index 00000000..1d3afeb0 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/DefaultVisualizers/Visualization/MarkerDefaultVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 90600d3790e079d49997a5f9f09d01d3 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d.meta b/com.unity.robotics.visualizations/Runtime/Drawing3d.meta new file mode 100644 index 00000000..65120cba --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 2beeb7bf6dfe68949b9fe67a0e97a36b +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Drawing3dManager.prefab b/com.unity.robotics.visualizations/Runtime/Drawing3d/Drawing3dManager.prefab new file mode 100644 index 00000000..bb108362 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Drawing3dManager.prefab @@ -0,0 +1,49 @@ +%YAML 1.1 +%TAG !u! tag:unity3d.com,2011: +--- !u!1 &863246206611208022 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 4360241143132002600} + - component: {fileID: 8942715124792280059} + m_Layer: 0 + m_Name: Drawing3dManager + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &4360241143132002600 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 863246206611208022} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 0} + m_RootOrder: 0 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &8942715124792280059 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 863246206611208022} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 9b11ef521a4b61a46bb4f858ab62c6d1, type: 3} + m_Name: + m_EditorClassIdentifier: + m_UnlitVertexColorMaterial: {fileID: 2100000, guid: 8acbae034b3d2ca42bbca7e21e3f964f, type: 2} + m_UnlitColorMaterial: {fileID: 2100000, guid: 51187810e7114ee4687016c7fa476970, type: 2} + m_UnlitColorAlphaMaterial: {fileID: 2100000, guid: b5ff7d4831357f1438fa8c8adb0cd5dc, type: 2} + m_UnlitPointCloudMaterial: {fileID: 2100000, guid: 0a328b723b7201142986276d292502c4, type: 2} diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Drawing3dManager.prefab.meta b/com.unity.robotics.visualizations/Runtime/Drawing3d/Drawing3dManager.prefab.meta new file mode 100644 index 00000000..a6eb74ad --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Drawing3dManager.prefab.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 57cef4e772232da4b92e8f8931f3cc75 +PrefabImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials.meta b/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials.meta new file mode 100644 index 00000000..62379071 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: eb202580e419a3d499d4487e77e02f81 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials/UnlitColor.mat b/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials/UnlitColor.mat new file mode 100644 index 00000000..48bb01e5 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials/UnlitColor.mat @@ -0,0 +1,78 @@ +%YAML 1.1 +%TAG !u! tag:unity3d.com,2011: +--- !u!21 &2100000 +Material: + serializedVersion: 6 + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_Name: UnlitColor + m_Shader: {fileID: 10755, guid: 0000000000000000f000000000000000, type: 0} + m_ShaderKeywords: + m_LightmapFlags: 4 + m_EnableInstancingVariants: 0 + m_DoubleSidedGI: 0 + m_CustomRenderQueue: -1 + stringTagMap: {} + disabledShaderPasses: [] + m_SavedProperties: + serializedVersion: 3 + m_TexEnvs: + - _BumpMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _DetailAlbedoMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _DetailMask: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _DetailNormalMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _EmissionMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _MainTex: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _MetallicGlossMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _OcclusionMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _ParallaxMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + m_Floats: + - _BumpScale: 1 + - _Cutoff: 0.5 + - _DetailNormalMapScale: 1 + - _DstBlend: 0 + - _GlossMapScale: 1 + - _Glossiness: 0.5 + - _GlossyReflections: 1 + - _Metallic: 0 + - _Mode: 0 + - _OcclusionStrength: 1 + - _Parallax: 0.02 + - _SmoothnessTextureChannel: 0 + - _SpecularHighlights: 1 + - _SrcBlend: 1 + - _UVSec: 0 + - _ZWrite: 1 + m_Colors: + - _Color: {r: 1, g: 1, b: 1, a: 1} + - _EmissionColor: {r: 0, g: 0, b: 0, a: 1} + m_BuildTextureStacks: [] diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials/UnlitColor.mat.meta b/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials/UnlitColor.mat.meta new file mode 100644 index 00000000..48fa3415 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials/UnlitColor.mat.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 51187810e7114ee4687016c7fa476970 +NativeFormatImporter: + externalObjects: {} + mainObjectFileID: 0 + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials/UnlitColorAlpha.mat b/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials/UnlitColorAlpha.mat new file mode 100644 index 00000000..291d0792 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials/UnlitColorAlpha.mat @@ -0,0 +1,78 @@ +%YAML 1.1 +%TAG !u! tag:unity3d.com,2011: +--- !u!21 &2100000 +Material: + serializedVersion: 6 + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_Name: UnlitColorAlpha + m_Shader: {fileID: 4800000, guid: 460101c20107a2140b780865ef33091c, type: 3} + m_ShaderKeywords: + m_LightmapFlags: 4 + m_EnableInstancingVariants: 0 + m_DoubleSidedGI: 0 + m_CustomRenderQueue: -1 + stringTagMap: {} + disabledShaderPasses: [] + m_SavedProperties: + serializedVersion: 3 + m_TexEnvs: + - _BumpMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _DetailAlbedoMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _DetailMask: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _DetailNormalMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _EmissionMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _MainTex: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _MetallicGlossMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _OcclusionMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _ParallaxMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + m_Floats: + - _BumpScale: 1 + - _Cutoff: 0.5 + - _DetailNormalMapScale: 1 + - _DstBlend: 0 + - _GlossMapScale: 1 + - _Glossiness: 0.5 + - _GlossyReflections: 1 + - _Metallic: 0 + - _Mode: 0 + - _OcclusionStrength: 1 + - _Parallax: 0.02 + - _SmoothnessTextureChannel: 0 + - _SpecularHighlights: 1 + - _SrcBlend: 1 + - _UVSec: 0 + - _ZWrite: 1 + m_Colors: + - _Color: {r: 1, g: 1, b: 1, a: 1} + - _EmissionColor: {r: 0, g: 0, b: 0, a: 1} + m_BuildTextureStacks: [] diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials/UnlitColorAlpha.mat.meta b/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials/UnlitColorAlpha.mat.meta new file mode 100644 index 00000000..bbbd1b72 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials/UnlitColorAlpha.mat.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: b5ff7d4831357f1438fa8c8adb0cd5dc +NativeFormatImporter: + externalObjects: {} + mainObjectFileID: 0 + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials/UnlitPointCloudAdditive.mat b/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials/UnlitPointCloudAdditive.mat new file mode 100644 index 00000000..97818218 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials/UnlitPointCloudAdditive.mat @@ -0,0 +1,79 @@ +%YAML 1.1 +%TAG !u! tag:unity3d.com,2011: +--- !u!21 &2100000 +Material: + serializedVersion: 6 + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_Name: UnlitPointCloudAdditive + m_Shader: {fileID: 4800000, guid: 4e4553124a2cf424bb79e020815b3dcf, type: 3} + m_ShaderKeywords: + m_LightmapFlags: 4 + m_EnableInstancingVariants: 0 + m_DoubleSidedGI: 0 + m_CustomRenderQueue: -1 + stringTagMap: {} + disabledShaderPasses: [] + m_SavedProperties: + serializedVersion: 3 + m_TexEnvs: + - _BumpMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _DetailAlbedoMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _DetailMask: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _DetailNormalMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _EmissionMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _MainTex: + m_Texture: {fileID: 2800000, guid: 4b8089fd78123c643998ee91beb76df7, type: 3} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _MetallicGlossMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _OcclusionMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _ParallaxMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + m_Floats: + - _BumpScale: 1 + - _Cutoff: 0.5 + - _DetailNormalMapScale: 1 + - _DstBlend: 0 + - _GlossMapScale: 1 + - _Glossiness: 0.5 + - _GlossyReflections: 1 + - _Metallic: 0 + - _Mode: 0 + - _OcclusionStrength: 1 + - _Parallax: 0.02 + - _Radius: 0.1 + - _SmoothnessTextureChannel: 0 + - _SpecularHighlights: 1 + - _SrcBlend: 1 + - _UVSec: 0 + - _ZWrite: 1 + m_Colors: + - _Color: {r: 1, g: 1, b: 1, a: 1} + - _EmissionColor: {r: 0, g: 0, b: 0, a: 1} + m_BuildTextureStacks: [] diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials/UnlitPointCloudAdditive.mat.meta b/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials/UnlitPointCloudAdditive.mat.meta new file mode 100644 index 00000000..038ebb14 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials/UnlitPointCloudAdditive.mat.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 88a5198f9a6e057478116eb7e1e4c2f1 +NativeFormatImporter: + externalObjects: {} + mainObjectFileID: 0 + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials/UnlitPointCloudCutout.mat b/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials/UnlitPointCloudCutout.mat new file mode 100644 index 00000000..9d26e4ea --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials/UnlitPointCloudCutout.mat @@ -0,0 +1,79 @@ +%YAML 1.1 +%TAG !u! tag:unity3d.com,2011: +--- !u!21 &2100000 +Material: + serializedVersion: 6 + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_Name: UnlitPointCloudCutout + m_Shader: {fileID: 4800000, guid: 90952ee016b305a4a9886b663e5aa6bc, type: 3} + m_ShaderKeywords: + m_LightmapFlags: 4 + m_EnableInstancingVariants: 0 + m_DoubleSidedGI: 0 + m_CustomRenderQueue: -1 + stringTagMap: {} + disabledShaderPasses: [] + m_SavedProperties: + serializedVersion: 3 + m_TexEnvs: + - _BumpMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _DetailAlbedoMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _DetailMask: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _DetailNormalMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _EmissionMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _MainTex: + m_Texture: {fileID: 2800000, guid: 4b8089fd78123c643998ee91beb76df7, type: 3} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _MetallicGlossMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _OcclusionMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _ParallaxMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + m_Floats: + - _BumpScale: 1 + - _Cutoff: 0.5 + - _DetailNormalMapScale: 1 + - _DstBlend: 0 + - _GlossMapScale: 1 + - _Glossiness: 0.5 + - _GlossyReflections: 1 + - _Metallic: 0 + - _Mode: 0 + - _OcclusionStrength: 1 + - _Parallax: 0.02 + - _Radius: 0.1 + - _SmoothnessTextureChannel: 0 + - _SpecularHighlights: 1 + - _SrcBlend: 1 + - _UVSec: 0 + - _ZWrite: 1 + m_Colors: + - _Color: {r: 1, g: 1, b: 1, a: 1} + - _EmissionColor: {r: 0, g: 0, b: 0, a: 1} + m_BuildTextureStacks: [] diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials/UnlitPointCloudCutout.mat.meta b/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials/UnlitPointCloudCutout.mat.meta new file mode 100644 index 00000000..e639a793 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials/UnlitPointCloudCutout.mat.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 0a328b723b7201142986276d292502c4 +NativeFormatImporter: + externalObjects: {} + mainObjectFileID: 0 + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials/UnlitVertexColor.mat b/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials/UnlitVertexColor.mat new file mode 100644 index 00000000..f32c0143 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials/UnlitVertexColor.mat @@ -0,0 +1,78 @@ +%YAML 1.1 +%TAG !u! tag:unity3d.com,2011: +--- !u!21 &2100000 +Material: + serializedVersion: 6 + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_Name: UnlitVertexColor + m_Shader: {fileID: 4800000, guid: b1651fbb071c06b409fe12536561208a, type: 3} + m_ShaderKeywords: + m_LightmapFlags: 4 + m_EnableInstancingVariants: 0 + m_DoubleSidedGI: 0 + m_CustomRenderQueue: -1 + stringTagMap: {} + disabledShaderPasses: [] + m_SavedProperties: + serializedVersion: 3 + m_TexEnvs: + - _BumpMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _DetailAlbedoMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _DetailMask: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _DetailNormalMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _EmissionMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _MainTex: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _MetallicGlossMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _OcclusionMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _ParallaxMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + m_Floats: + - _BumpScale: 1 + - _Cutoff: 0.5 + - _DetailNormalMapScale: 1 + - _DstBlend: 0 + - _GlossMapScale: 1 + - _Glossiness: 0.5 + - _GlossyReflections: 1 + - _Metallic: 0 + - _Mode: 0 + - _OcclusionStrength: 1 + - _Parallax: 0.02 + - _SmoothnessTextureChannel: 0 + - _SpecularHighlights: 1 + - _SrcBlend: 1 + - _UVSec: 0 + - _ZWrite: 1 + m_Colors: + - _Color: {r: 1, g: 1, b: 1, a: 1} + - _EmissionColor: {r: 0, g: 0, b: 0, a: 1} + m_BuildTextureStacks: [] diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials/UnlitVertexColor.mat.meta b/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials/UnlitVertexColor.mat.meta new file mode 100644 index 00000000..c69d5a4b --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials/UnlitVertexColor.mat.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 8acbae034b3d2ca42bbca7e21e3f964f +NativeFormatImporter: + externalObjects: {} + mainObjectFileID: 0 + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials/UnlitVertexColorAlpha.mat b/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials/UnlitVertexColorAlpha.mat new file mode 100644 index 00000000..3b7acdea --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials/UnlitVertexColorAlpha.mat @@ -0,0 +1,79 @@ +%YAML 1.1 +%TAG !u! tag:unity3d.com,2011: +--- !u!21 &2100000 +Material: + serializedVersion: 6 + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_Name: UnlitVertexColorAlpha + m_Shader: {fileID: 4800000, guid: af241396b3cfedd4a8d92e4203607847, type: 3} + m_ShaderKeywords: + m_LightmapFlags: 4 + m_EnableInstancingVariants: 0 + m_DoubleSidedGI: 0 + m_CustomRenderQueue: -1 + stringTagMap: {} + disabledShaderPasses: [] + m_SavedProperties: + serializedVersion: 3 + m_TexEnvs: + - _BumpMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _DetailAlbedoMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _DetailMask: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _DetailNormalMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _EmissionMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _MainTex: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _MetallicGlossMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _OcclusionMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _ParallaxMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + m_Floats: + - _BumpScale: 1 + - _Cutoff: 0.5 + - _DetailNormalMapScale: 1 + - _DstBlend: 0 + - _Fade: 1 + - _GlossMapScale: 1 + - _Glossiness: 0.5 + - _GlossyReflections: 1 + - _Metallic: 0 + - _Mode: 0 + - _OcclusionStrength: 1 + - _Parallax: 0.02 + - _SmoothnessTextureChannel: 0 + - _SpecularHighlights: 1 + - _SrcBlend: 1 + - _UVSec: 0 + - _ZWrite: 1 + m_Colors: + - _Color: {r: 1, g: 1, b: 1, a: 1} + - _EmissionColor: {r: 0, g: 0, b: 0, a: 1} + m_BuildTextureStacks: [] diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials/UnlitVertexColorAlpha.mat.meta b/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials/UnlitVertexColorAlpha.mat.meta new file mode 100644 index 00000000..b6992e1f --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Materials/UnlitVertexColorAlpha.mat.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: bb6ff821de64c7b4cbf532af80900315 +NativeFormatImporter: + externalObjects: {} + mainObjectFileID: 0 + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Scripts.meta b/com.unity.robotics.visualizations/Runtime/Drawing3d/Scripts.meta new file mode 100644 index 00000000..796e5a3d --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Scripts.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 7cd22e1ed4e15034b9c7a450452aaa3a +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Scripts/Drawing3d.cs b/com.unity.robotics.visualizations/Runtime/Drawing3d/Scripts/Drawing3d.cs new file mode 100644 index 00000000..86d59caf --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Scripts/Drawing3d.cs @@ -0,0 +1,820 @@ +using RosMessageTypes.Std; +using System.Collections; +using System.Collections.Generic; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public enum TFTrackingType + { + None, + Exact, + TrackLatest, + } + + [System.Serializable] + public class TFTrackingSettings + { + public TFTrackingType type = TFTrackingType.Exact; + public string tfTopic = "/tf"; + } + + public class Drawing3d : MonoBehaviour + { + static GUIStyle s_LabelStyle; + + struct LabelInfo3d + { + public Vector3 position; + public string text; + public Color color; + public float worldSpacing; + } + + Mesh m_Mesh; + List m_Vertices = new List(); + List m_Colors32 = new List(); + // When drawing a mesh, it's easier and faster to create a separate object to represent the mesh, than copy its vertices into our drawing + List m_SupplementalMeshes = new List(); + // For efficiency, when a BasicDrawing is cleared, its supplemental meshes go dormant instead of being deleted. They get reawakened by calling AddMesh. + Queue m_DormantMeshes = new Queue(); + List m_PointClouds = new List(); + Queue m_DormantPointClouds = new Queue(); + List m_Triangles = new List(); + List m_Labels = new List(); + bool m_isDirty = false; + Coroutine m_DestroyAfterDelay; + + public static Drawing3d Create(float duration = -1, Material material = null) + { + return Drawing3dManager.CreateDrawing(duration, material); + } + + public void Init(Drawing3dManager parent, Material material, float duration = -1) + { + m_Mesh = new Mesh(); + + transform.parent = parent.transform; + + MeshFilter mfilter = gameObject.AddComponent(); + mfilter.sharedMesh = m_Mesh; + MeshRenderer mrenderer = gameObject.AddComponent(); + mrenderer.sharedMaterial = material; + + if (duration >= 0) + { + SetDuration(duration); + } + + if (s_LabelStyle == null) + { + s_LabelStyle = new GUIStyle() + { + alignment = TextAnchor.LowerLeft, + wordWrap = false, + }; + } + } + + public void SetTFTrackingSettings(TFTrackingSettings tfTrackingType, HeaderMsg headerMsg) + { + switch (tfTrackingType.type) + { + case TFTrackingType.Exact: + { + TFFrame frame = TFSystem.instance.GetTransform(headerMsg, tfTrackingType.tfTopic); + transform.position = frame.translation; + transform.rotation = frame.rotation; + } + break; + case TFTrackingType.TrackLatest: + { + transform.parent = TFSystem.instance.GetTransformObject(headerMsg.frame_id, tfTrackingType.tfTopic).transform; + transform.localPosition = Vector3.zero; + transform.localRotation = Quaternion.identity; + } + break; + case TFTrackingType.None: + transform.localPosition = Vector3.zero; + transform.localRotation = Quaternion.identity; + break; + } + } + + public void SetDuration(float duration) + { + if (m_DestroyAfterDelay != null) + StopCoroutine(m_DestroyAfterDelay); + m_DestroyAfterDelay = StartCoroutine(DestroyAfterDelay(duration)); + } + + public void ClearDuration() + { + if (m_DestroyAfterDelay != null) + StopCoroutine(m_DestroyAfterDelay); + } + + public IEnumerator DestroyAfterDelay(float duration) + { + yield return new WaitForSeconds(duration); + Destroy(); + } + + public void DrawLine(Vector3 from, Vector3 to, Color32 color, float thickness = 0.1f) + { + DrawLine(from, to, color, color, thickness); + } + + public void DrawLine(Vector3 from, Vector3 to, Color32 fromColor, Color32 toColor, float thickness = 0.1f) + { + // Could do this by calling DrawCylinder - but hardcoding it is more efficient + Vector3 forwardVector = (to - from).normalized; + Vector3 sideVector; + if (Vector3.Dot(forwardVector, Vector3.up) > 0.9f) // just want any vector perpendicular to forwardVector + sideVector = Vector3.Cross(forwardVector, Vector3.forward).normalized * thickness; + else + sideVector = Vector3.Cross(forwardVector, Vector3.up).normalized * thickness; + Vector3 upVector = Vector3.Cross(forwardVector, sideVector).normalized * thickness; + int start = m_Vertices.Count; + m_Vertices.Add(from + sideVector); //0 + m_Vertices.Add(from - sideVector); + m_Vertices.Add(from + upVector); //2 + m_Vertices.Add(from - upVector); + m_Vertices.Add(to + sideVector);//4 + m_Vertices.Add(to - sideVector); + m_Vertices.Add(to + upVector);// 6 + m_Vertices.Add(to - upVector); + + for (int Idx = 0; Idx < 4; ++Idx) + m_Colors32.Add(fromColor); + for (int Idx = 0; Idx < 4; ++Idx) + m_Colors32.Add(toColor); + + AddTriangles(start, 0, 2, 4, 4, 2, 6, 1, 5, 2, 2, 5, 6); + AddTriangles(start, 0, 4, 3, 3, 4, 7, 1, 3, 5, 5, 3, 7); + AddTriangles(start, 1, 2, 3, 3, 2, 0, 5, 7, 6, 6, 7, 4); + SetDirty(); + } + + static int[] simpleRingIndices = new int[4] { 3, 2, 1, 0 }; + static int[] pathCornerRingIndices = new int[4] { 0, 3, 2, -2 }; + + public void DrawPath(IEnumerable path, Color32 color, float thickness = 0.1f) + { + IEnumerator enumerator = path.GetEnumerator(); + if (!enumerator.MoveNext()) + return; // empty path + + Vector3 pointA = enumerator.Current; + if (!enumerator.MoveNext()) + { + // only one point in path + DrawPoint(pointA, color, thickness); + return; + } + + Vector3 pointB = enumerator.Current; + Vector3 lineABDirection = (pointB - pointA).normalized; + Vector3 oldOutVector = Vector3.Cross(Vector3.up, lineABDirection).normalized * thickness; + Vector3 oldSideVector = Vector3.Cross(oldOutVector, lineABDirection).normalized * thickness; + int oldVertexIndex = m_Vertices.Count; + m_Vertices.Add(pointA + oldOutVector); // 0 + m_Colors32.Add(color); + m_Vertices.Add(pointA - oldSideVector); // 1 + m_Colors32.Add(color); + m_Vertices.Add(pointA - oldOutVector); // 2 + m_Colors32.Add(color); + m_Vertices.Add(pointA + oldSideVector); // 3 + m_Colors32.Add(color); + AddQuad(oldVertexIndex, 3, 2, 1, 0); + + while (enumerator.MoveNext()) + { + Vector3 pointC = enumerator.Current; + // create new vertices around pointB. + Vector3 lineBCDirection = (pointC - pointB).normalized; + Vector3 sideVector = Vector3.Cross(lineABDirection, lineBCDirection).normalized * thickness; + Vector3 abOutVector = Vector3.Cross(lineABDirection, sideVector).normalized * thickness; + Vector3 bcOutVector = Vector3.Cross(lineBCDirection, sideVector).normalized * thickness; + Vector3 midOutVector = (abOutVector + bcOutVector).normalized * thickness; + Vector3 inVector = -midOutVector; // TO DO: make this smarter + + // we draw each corner with 6 vertices: the two perpendiculars, the inside, the outside, + // and two extra outside points perpendicular to line AB and line BC respectively. + // We set newVertexIndex in the middle of this set of vertices, because next time around this will become the oldVertexIndex + // and we want to treat it the same as the initial four vertices we created above + m_Vertices.Add(pointB + abOutVector); // -2 + m_Colors32.Add(color); + m_Vertices.Add(pointB + midOutVector); // -1 + m_Colors32.Add(color); + int newVertexIndex = m_Vertices.Count; + m_Vertices.Add(pointB + sideVector); // 0 + m_Colors32.Add(color); + m_Vertices.Add(pointB + bcOutVector); // 1 + m_Colors32.Add(color); + m_Vertices.Add(pointB - sideVector); // 2 + m_Colors32.Add(color); + m_Vertices.Add(pointB + inVector); // 3 + m_Colors32.Add(color); + AddTriangles(newVertexIndex, -2, -1, 0, -2, 2, -1, -1, 2, 1, -1, 1, 0); + + // to avoid having too much twist in the connecting polygons, figure out axis is closest to the old side vector + float sideDot = Vector3.Dot(oldSideVector, sideVector); + float outDot = Vector3.Dot(oldSideVector, abOutVector); + + int alignmentIndex; + if (Mathf.Abs(sideDot) > Mathf.Abs(outDot)) + { + alignmentIndex = sideDot > 0 ? 0 : 2; + } + else + { + alignmentIndex = outDot > 0 ? 1 : 3; + } + ConnectPathRings(oldVertexIndex, newVertexIndex, simpleRingIndices, pathCornerRingIndices, alignmentIndex); + + pointA = pointB; + pointB = pointC; + lineABDirection = lineBCDirection; + oldVertexIndex = newVertexIndex; + oldSideVector = sideVector; + oldOutVector = bcOutVector; + } + + // for the last point, we make another ring + int finalVertexIndex = m_Vertices.Count; + m_Vertices.Add(pointB + oldOutVector); // 0 + m_Colors32.Add(color); + m_Vertices.Add(pointB - oldSideVector); // 1 + m_Colors32.Add(color); + m_Vertices.Add(pointB - oldOutVector); // 2 + m_Colors32.Add(color); + m_Vertices.Add(pointB + oldSideVector); // 3 + m_Colors32.Add(color); + AddQuad(finalVertexIndex, 0, 1, 2, 3); + ConnectPathRings(oldVertexIndex, finalVertexIndex, simpleRingIndices, simpleRingIndices, 0); + SetDirty(); + } + + void ConnectPathRings(int fromIndex, int toIndex, int[] fromOffsets, int[] toOffsets, int alignment) + { + // connect this line + for (int Idx = 0; Idx < fromOffsets.Length; ++Idx) + { + AddQuad( + fromIndex + fromOffsets[((alignment + Idx) % 4)], + toIndex + toOffsets[Idx], + toIndex + toOffsets[(Idx + 1) % 4], + fromIndex + fromOffsets[((alignment + Idx + 1) % 4)] + ); + } + } + + public void DrawPoint(Vector3 point, Color32 color, float radius = 0.1f) + { + // draw a point as an octahedron + int start = m_Vertices.Count; + m_Vertices.Add(point + new Vector3(radius, 0, 0)); + m_Vertices.Add(point + new Vector3(-radius, 0, 0)); + m_Vertices.Add(point + new Vector3(0, radius, 0)); + m_Vertices.Add(point + new Vector3(0, -radius, 0)); + m_Vertices.Add(point + new Vector3(0, 0, radius)); + m_Vertices.Add(point + new Vector3(0, 0, -radius)); + + for (int Idx = 0; Idx < 6; ++Idx) + m_Colors32.Add(color); + + AddTriangles(start, 0, 2, 4, 0, 5, 2, 0, 4, 3, 0, 3, 5); + AddTriangles(start, 1, 4, 2, 1, 2, 5, 1, 3, 4, 1, 5, 3); + SetDirty(); + } + + public void DrawPoint(Vector3 point, string label, Color32 color, float radius = 0.1f) + { + DrawPoint(point, color, radius); + DrawLabel(label, point, color, radius * 1.5f); + } + + public void DrawQuad(Vector3 a, Vector3 b, Vector3 c, Vector3 d, Color32 color, bool doubleSided = false) + { + int start = m_Vertices.Count; + m_Vertices.Add(a); + m_Colors32.Add(color); + m_Vertices.Add(b); + m_Colors32.Add(color); + m_Vertices.Add(c); + m_Colors32.Add(color); + m_Vertices.Add(d); + m_Colors32.Add(color); + AddQuad(start, 0, 1, 2, 3); + if (doubleSided) + AddQuad(start, 3, 2, 1, 0); + } + + public void DrawArrow(Vector3 from, Vector3 to, Color32 color, float thickness = 0.1f, float arrowheadScale = 3, float arrowheadGradient = 0.5f) + { + float arrowheadRadius = thickness * arrowheadScale; + float arrowheadLength = arrowheadRadius / arrowheadGradient; + Vector3 direction = (to - from).normalized; + Vector3 arrowheadJoint = to - direction * arrowheadLength; + DrawLine(from, arrowheadJoint, color, thickness); + DrawCone(arrowheadJoint, to, color, arrowheadRadius); + } + + public void DrawCuboid(Vector3 center, Vector3 halfSize, Color32 color) + { + DrawParallelepiped(center, new Vector3(halfSize.x, 0, 0), new Vector3(0, halfSize.y, 0), new Vector3(0, 0, halfSize.z), color); + } + + public void DrawCuboid(Vector3 center, Vector3 halfSize, Quaternion rotation, Color32 color) + { + DrawParallelepiped(center, + rotation * new Vector3(halfSize.x, 0, 0), + rotation * new Vector3(0, halfSize.y, 0), + rotation * new Vector3(0, 0, halfSize.z), + color); + } + + public void DrawParallelepiped(Vector3 center, Vector3 x, Vector3 y, Vector3 z, Color32 color) + { + int start = m_Vertices.Count; + m_Vertices.Add(center - x - y - z); // 0 + m_Vertices.Add(center - x - y + z); // 1 + m_Vertices.Add(center - x + y - z); // 2 + m_Vertices.Add(center - x + y + z); // 3 + m_Vertices.Add(center + x - y - z); // 4 + m_Vertices.Add(center + x - y + z); // 5 + m_Vertices.Add(center + x + y - z); // 6 + m_Vertices.Add(center + x + y + z); // 7 + for (int Idx = 0; Idx < 8; ++Idx) + m_Colors32.Add(color); + AddQuad(start, 0, 1, 3, 2); // left face + AddQuad(start, 4, 6, 7, 5); // right face + AddQuad(start, 2, 3, 7, 6); // top face + AddQuad(start, 0, 4, 5, 1); // bottom face + AddQuad(start, 0, 2, 6, 4); // back face + AddQuad(start, 1, 5, 7, 3); // front face + SetDirty(); + } + + public void DrawSphere(Vector3 center, Color32 color, float radius, int numDivisions = 16) + { + DrawSpheroid(center, new Vector3(radius, radius, radius), Quaternion.identity, color, numDivisions); + } + + public void DrawSpheroid(Vector3 center, Vector3 radii, Quaternion rotation, Color32 color, int numDivisions = 16) + { + int start = m_Vertices.Count; + Vector3 upVector = rotation * Vector3.up * radii.y; + m_Vertices.Add(center + upVector); + m_Colors32.Add(color); + m_Vertices.Add(center - upVector); + m_Colors32.Add(color); + + int numRings = (numDivisions / 2) - 1; + float yawStep = Mathf.PI * 2 / numDivisions; + float pitchStep = Mathf.PI / (numRings + 1); + int lastRingStart = 2 + numRings - 1; + + for (int vertexIdx = 0; vertexIdx < numDivisions; ++vertexIdx) + { + float yaw = vertexIdx * yawStep; + Vector3 sideVector = rotation * new Vector3(Mathf.Sin(yaw) * radii.x, 0, Mathf.Cos(yaw) * radii.z); + for (int ringIdx = 0; ringIdx < numRings; ++ringIdx) + { + float pitch = Mathf.PI * 0.5f - (ringIdx + 1) * pitchStep; + m_Vertices.Add(center + sideVector * Mathf.Cos(pitch) + upVector * Mathf.Sin(pitch)); + m_Colors32.Add(color); + + if (ringIdx + 1 < numRings) + { + AddQuad(start + 2, + ((vertexIdx + 1) % numDivisions) * numRings + ringIdx, + ((vertexIdx + 1) % numDivisions) * numRings + ringIdx + 1, + vertexIdx * numRings + ringIdx + 1, + vertexIdx * numRings + ringIdx + ); + } + } + + AddTriangles(start, + 0, + 2 + ((vertexIdx + 1) % numDivisions) * numRings, + 2 + vertexIdx * numRings, + + 1, + lastRingStart + vertexIdx * numRings, + lastRingStart + ((vertexIdx + 1) % numDivisions) * numRings + ); + } + SetDirty(); + } + + public void DrawCircle(Vector3 center, Vector3 normal, Color32 color, float radius, int numRingVertices = 8) + { + DrawCircleOneSided(center, normal, color, radius, numRingVertices); + DrawCircleOneSided(center, -normal, color, radius, numRingVertices); + } + + public void DrawCircleOneSided(Vector3 center, Vector3 normal, Color32 color, float radius, int numRingVertices = 8) + { + Vector3 forwardVector = ((Mathf.Abs(normal.z) < Mathf.Abs(normal.x)) ? new Vector3(normal.y, -normal.x, 0) : new Vector3(0, -normal.z, normal.y)).normalized * radius; + Vector3 sideVector = Vector3.Cross(forwardVector, normal).normalized * radius; + + float angleScale = Mathf.PI * 2.0f / numRingVertices; + + int start = m_Vertices.Count; + m_Vertices.Add(center); + m_Colors32.Add(color); + + m_Vertices.Add(center + forwardVector); + m_Colors32.Add(color); + for (int step = 1; step < numRingVertices; ++step) + { + float angle = step * angleScale; + m_Vertices.Add(center + forwardVector * Mathf.Cos(angle) + sideVector * Mathf.Sin(angle)); + m_Colors32.Add(color); + + m_Triangles.Add(start); // center + m_Triangles.Add(start + step + 1); // new ring vertex + m_Triangles.Add(start + step); // previous ring vertex + } + + m_Triangles.Add(start); // center + m_Triangles.Add(start + numRingVertices); // last ring vertex + m_Triangles.Add(start + 1); // first ring vertex + SetDirty(); + } + + public void DrawCylinder(Vector3 bottom, Vector3 top, Color32 color, float radius, int numRingVertices = 8) + { + DrawCylinder(bottom, top, color, color, radius, numRingVertices); + } + + public void DrawCylinder(Vector3 bottom, Vector3 top, Color32 bottomColor, Color32 topColor, float radius, int numRingVertices = 8) + { + int start = m_Vertices.Count; + Vector3 upVector = top - bottom; + Vector3 forwardVector = ((Mathf.Abs(upVector.z) < Mathf.Abs(upVector.x)) ? new Vector3(upVector.y, -upVector.x, 0) : new Vector3(0, -upVector.z, upVector.y)).normalized * radius; + Vector3 sideVector = Vector3.Cross(forwardVector, upVector).normalized * radius; + float angleScale = Mathf.PI * 2.0f / numRingVertices; + + m_Vertices.Add(bottom); + m_Colors32.Add(bottomColor); + + m_Vertices.Add(top); + m_Colors32.Add(topColor); + + m_Vertices.Add(bottom + forwardVector); + m_Colors32.Add(bottomColor); + + m_Vertices.Add(top + forwardVector); + m_Colors32.Add(topColor); + + for (int step = 1; step < numRingVertices; ++step) + { + float angle = step * angleScale; + m_Vertices.Add(bottom + forwardVector * Mathf.Cos(angle) + sideVector * Mathf.Sin(angle)); + m_Colors32.Add(bottomColor); + + m_Vertices.Add(top + forwardVector * Mathf.Cos(angle) + sideVector * Mathf.Sin(angle)); + m_Colors32.Add(topColor); + + // bottom circle + m_Triangles.Add(start); // bottom + m_Triangles.Add(start + step * 2); // previous bottom ring vertex + m_Triangles.Add(start + step * 2 + 2); // new bottom ring vertex + + // top circle + m_Triangles.Add(start + 1); // top + m_Triangles.Add(start + step * 2 + 3); // new top ring vertex + m_Triangles.Add(start + step * 2 + 1); // previous top ring vertex + + // cylinder wall + AddQuad(start + step * 2, 0, 1, 3, 2); + } + + // bottom circle + m_Triangles.Add(start); // bottom + m_Triangles.Add(start + numRingVertices * 2); // last bottom ring vertex + m_Triangles.Add(start + 2); // first bottom ring vertex + + // top circle + m_Triangles.Add(start + 1); // top + m_Triangles.Add(start + 3); // first top ring vertex + m_Triangles.Add(start + numRingVertices * 2 + 1); // last top ring vertex + + // cylinder wall + AddQuad(start, 3, 2, numRingVertices * 2, numRingVertices * 2 + 1); + SetDirty(); + } + + public void DrawCone(Vector3 basePosition, Vector3 tipPosition, Color32 color, float radius, int numRingVertices = 8) + { + int start = m_Vertices.Count; + m_Vertices.Add(basePosition); + m_Colors32.Add(new Color32((byte)(color.r / 2), (byte)(color.g / 2), (byte)(color.b / 2), 255)); + m_Vertices.Add(tipPosition); + m_Colors32.Add(color); + Vector3 heightVector = tipPosition - basePosition; + Vector3 forwardVector = ((Mathf.Abs(heightVector.z) < Mathf.Abs(heightVector.x)) ? new Vector3(heightVector.y, -heightVector.x, 0) : new Vector3(0, -heightVector.z, heightVector.y)).normalized * radius; + Vector3 sideVector = Vector3.Cross(forwardVector, heightVector).normalized * radius; + float angleScale = Mathf.PI * 2.0f / numRingVertices; + + m_Vertices.Add(basePosition + forwardVector); + m_Colors32.Add(color); + + for (int step = 1; step < numRingVertices; ++step) + { + float angle = step * angleScale; + m_Vertices.Add(basePosition + forwardVector * Mathf.Cos(angle) + sideVector * Mathf.Sin(angle)); + m_Colors32.Add(color); + + m_Triangles.Add(start + 1); // tip + m_Triangles.Add(start + step + 2); // new ring vertex + m_Triangles.Add(start + step + 1); // previous ring vertex + + m_Triangles.Add(start); // base + m_Triangles.Add(start + step + 1); // previous ring vertex + m_Triangles.Add(start + step + 2); // new ring vertex + } + + m_Triangles.Add(start + 1); // tip + m_Triangles.Add(start + 2); // first ring vertex + m_Triangles.Add(start + numRingVertices + 1); // last ring vertex + + m_Triangles.Add(start); // base + m_Triangles.Add(start + numRingVertices + 1); // last ring vertex + m_Triangles.Add(start + 2); // first ring vertex + SetDirty(); + } + + public void DrawTriangle(Vector3 a, Vector3 b, Vector3 c, Color32 color) + { + int start = m_Vertices.Count; + m_Vertices.Add(a); + m_Vertices.Add(b); + m_Vertices.Add(c); + for (int Idx = 0; Idx < 3; ++Idx) + m_Colors32.Add(color); + AddTriangles(start, 0, 1, 2); + SetDirty(); + } + + public void DrawTriangle(Vector3 a, Vector3 b, Vector3 c, Color32 colorA, Color32 colorB, Color32 colorC) + { + int start = m_Vertices.Count; + m_Vertices.Add(a); + m_Colors32.Add(colorA); + m_Vertices.Add(b); + m_Colors32.Add(colorB); + m_Vertices.Add(c); + m_Colors32.Add(colorC); + AddTriangles(start, 0, 1, 2); + SetDirty(); + } + + public void DrawTriangleFan(Color32 color, Vector3 center, Vector3 first, params Vector3[] fanPoints) + { + int centerIdx = m_Vertices.Count; + m_Vertices.Add(center); + m_Colors32.Add(color); + int currentIdx = m_Vertices.Count; + m_Vertices.Add(first); + m_Colors32.Add(color); + foreach (Vector3 point in fanPoints) + { + m_Vertices.Add(point); + m_Colors32.Add(color); + m_Triangles.Add(centerIdx); + m_Triangles.Add(currentIdx); + m_Triangles.Add(currentIdx + 1); + currentIdx++; + } + SetDirty(); + } + + public void DrawTriangleStrip(Color32 color, Vector3 first, Vector3 second, params Vector3[] otherPoints) + { + int currentIdx = m_Vertices.Count; + m_Vertices.Add(first); + m_Colors32.Add(color); + m_Vertices.Add(second); + m_Colors32.Add(color); + foreach (Vector3 point in otherPoints) + { + m_Vertices.Add(point); + m_Colors32.Add(color); + AddTriangles(currentIdx, 0, 1, 2); + currentIdx++; + } + SetDirty(); + } + + public void DrawLines(Vector3[] pointPairs, Color32 color, float thickness) + { + for (int Idx = 1; Idx < pointPairs.Length; Idx += 2) + { + DrawLine(pointPairs[Idx - 1], pointPairs[Idx], color, thickness); + } + } + + public void DrawLines(Vector3[] pointPairs, Color32[] colorPairs, float thickness) + { + for (int Idx = 1; Idx < pointPairs.Length; Idx += 2) + { + DrawLine(pointPairs[Idx - 1], pointPairs[Idx], colorPairs[Idx - 1], colorPairs[Idx], thickness); + } + } + + public void DrawLineStrip(Vector3[] stripPoints, Color32 color, float thickness) + { + for (int Idx = 1; Idx < stripPoints.Length; ++Idx) + { + DrawLine(stripPoints[Idx - 1], stripPoints[Idx], color, color, thickness); + } + } + + public void DrawLineStrip(Vector3[] stripPoints, Color32[] stripColors, float thickness) + { + for (int Idx = 1; Idx < stripPoints.Length; ++Idx) + { + DrawLine(stripPoints[Idx - 1], stripPoints[Idx], stripColors[Idx - 1], stripColors[Idx], thickness); + } + } + + void AddTriangles(int firstIdx, params int[] offsets) + { + foreach (int offset in offsets) + { + m_Triangles.Add(firstIdx + offset); + } + } + + void AddTriangle(int firstIdx, int a, int b, int c) + { + m_Triangles.Add(firstIdx + a); + m_Triangles.Add(firstIdx + b); + m_Triangles.Add(firstIdx + c); + } + + // indices in clockwise order + void AddQuad(int offset, int a, int b, int c, int d) + { + AddQuad(offset + a, offset + b, offset + c, offset + d); + } + + // indices in clockwise order + void AddQuad(int a, int b, int c, int d) + { + m_Triangles.Add(a); + m_Triangles.Add(b); + m_Triangles.Add(c); + + m_Triangles.Add(a); + m_Triangles.Add(c); + m_Triangles.Add(d); + } + + public void DrawLabel(string text, Vector3 position, Color color, float worldSpacing = 0) + { + m_Labels.Add(new LabelInfo3d { text = text, position = position, color = color, worldSpacing = worldSpacing }); + } + + public void DrawMesh(Mesh source, Transform transform, Color32 color) + { + DrawMesh(source, transform.position, transform.rotation, transform.localScale, color); + } + + public void DrawMesh(Mesh source, Vector3 position, Quaternion rotation, Vector3 scale, Color32 color) + { + Material colorMaterial = (color.a < 255) ? + new Material(Drawing3dManager.instance.UnlitColorAlphaMaterial) : + new Material(Drawing3dManager.instance.UnlitColorMaterial); + colorMaterial.color = color; + DrawMesh(source, position, rotation, scale, colorMaterial); + } + + public GameObject DrawMesh(Mesh source, Vector3 position, Quaternion rotation, Vector3 scale, Material material) + { + GameObject meshObject; + MeshRenderer mrenderer; + if (m_DormantMeshes.Count > 0) + { + mrenderer = m_DormantMeshes.Dequeue(); + mrenderer.enabled = true; + meshObject = mrenderer.gameObject; + } + else + { + meshObject = new GameObject(source.name); + MeshFilter mfilter = meshObject.AddComponent(); + mfilter.sharedMesh = source; + mrenderer = meshObject.AddComponent(); + } + meshObject.transform.parent = transform; + meshObject.transform.position = position; + meshObject.transform.rotation = rotation; + meshObject.transform.localScale = scale; + mrenderer.sharedMaterial = material; + m_SupplementalMeshes.Add(mrenderer); + + return meshObject; + } + + public PointCloudDrawing AddPointCloud(int numPoints = 0, Material material = null) + { + PointCloudDrawing result; + if (m_DormantPointClouds.Count > 0) + { + result = m_DormantPointClouds.Dequeue(); + result.SetCapacity(numPoints); + result.SetMaterial(material); + } + else + { + result = PointCloudDrawing.Create(gameObject, numPoints, material); + } + m_PointClouds.Add(result); + return result; + } + + public void Clear() + { + m_Vertices.Clear(); + m_Colors32.Clear(); + m_Triangles.Clear(); + m_Labels.Clear(); + foreach (MeshRenderer obj in m_SupplementalMeshes) + { + obj.enabled = false; + m_DormantMeshes.Enqueue(obj); + } + m_SupplementalMeshes.Clear(); + foreach (PointCloudDrawing pointCloud in m_PointClouds) + { + pointCloud.Clear(); + m_DormantPointClouds.Enqueue(pointCloud); + } + m_PointClouds.Clear(); + SetDirty(); + } + + public void Destroy() + { + GameObject.Destroy(gameObject); + } + + internal void OnDrawingGUI(Camera cam) + { + foreach (LabelInfo3d label in m_Labels) + { + Vector3 screenPos = cam.WorldToScreenPoint(transform.TransformPoint(label.position) + cam.transform.right * label.worldSpacing); + Vector3 guiPos = GUIUtility.ScreenToGUIPoint(screenPos); + GUI.color = label.color; + GUIContent labelContent = new GUIContent(label.text); + Vector2 labelSize = s_LabelStyle.CalcSize(labelContent); + labelSize.y *= 2;// no idea why we get bad answers for height + + guiPos.y += labelSize.y * 0.35f; + GUI.Label(new Rect(guiPos.x, Screen.height - guiPos.y, labelSize.x, labelSize.y), label.text); + } + } + +#if UNITY_EDITOR + void OnDrawGizmos() + { + if (m_Labels.Count == 0) + return; + Camera cam = UnityEditor.SceneView.currentDrawingSceneView.camera; + GUIStyle style = new GUIStyle(); + foreach (LabelInfo3d label in m_Labels) + { + style.normal.textColor = label.color; + Vector3 worldPos = transform.TransformPoint(label.position + cam.transform.right * label.worldSpacing); + UnityEditor.Handles.Label(worldPos, label.text, style); + } + } + +#endif + + public void Refresh() + { + m_isDirty = false; + m_Mesh.Clear(); + m_Mesh.indexFormat = m_Vertices.Count < 65536 ? UnityEngine.Rendering.IndexFormat.UInt16 : UnityEngine.Rendering.IndexFormat.UInt32; + m_Mesh.vertices = m_Vertices.ToArray(); + m_Mesh.colors32 = m_Colors32.ToArray(); + m_Mesh.triangles = m_Triangles.ToArray(); + } + + void SetDirty() + { + if (!m_isDirty) + { + m_isDirty = true; + Drawing3dManager.instance.AddDirty(this); + } + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Scripts/Drawing3d.cs.meta b/com.unity.robotics.visualizations/Runtime/Drawing3d/Scripts/Drawing3d.cs.meta new file mode 100644 index 00000000..fe1ff5a2 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Scripts/Drawing3d.cs.meta @@ -0,0 +1,12 @@ +fileFormatVersion: 2 +guid: 54795ada29ad01147a2c983c42cb08cb +dev:com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/PregeneratedMessages/Actionlib/msg/MGoalStatus.cs.meta +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Scripts/Drawing3dManager.cs b/com.unity.robotics.visualizations/Runtime/Drawing3d/Scripts/Drawing3dManager.cs new file mode 100644 index 00000000..25947286 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Scripts/Drawing3dManager.cs @@ -0,0 +1,107 @@ +using System.Collections; +using System.Collections.Generic; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public class Drawing3dManager : MonoBehaviour + { + static Drawing3dManager s_Instance; + public static Drawing3dManager instance + { + get + { +#if UNITY_EDITOR + if (s_Instance == null) + { + s_Instance = FindObjectOfType(); + if (s_Instance == null) + { + GameObject newDebugDrawObj = new GameObject("DrawingManager"); + s_Instance = newDebugDrawObj.AddComponent(); + s_Instance.m_UnlitVertexColorMaterial = new Material(Shader.Find("Unlit/VertexColor")); + s_Instance.m_UnlitColorMaterial = new Material(Shader.Find("Unlit/Color")); + s_Instance.m_UnlitColorAlphaMaterial = new Material(Shader.Find("Unlit/ColorAlpha")); + s_Instance.m_UnlitPointCloudMaterial = new Material(Shader.Find("Unlit/PointCloudCutout")); + } + } +#endif + return s_Instance; + } + } + + Camera m_Camera; + List m_Drawings = new List(); + List m_Dirty = new List(); + + [SerializeField] + Material m_UnlitVertexColorMaterial; + [SerializeField] + Material m_UnlitColorMaterial; + [SerializeField] + Material m_UnlitColorAlphaMaterial; + [SerializeField] + Material m_UnlitPointCloudMaterial; + + public Material UnlitVertexColorMaterial => m_UnlitVertexColorMaterial; + public Material UnlitColorMaterial => m_UnlitColorMaterial; + public Material UnlitColorAlphaMaterial => m_UnlitColorAlphaMaterial; + public Material UnlitPointCloudMaterial => m_UnlitPointCloudMaterial; + + void Awake() + { + s_Instance = this; + m_Camera = Camera.main; + } + + public void AddDirty(Drawing3d drawing) + { + m_Dirty.Add(drawing); + } + + public void DestroyDrawing(Drawing3d drawing) + { + m_Drawings.Remove(drawing); + GameObject.Destroy(drawing.gameObject); + } + + public static Drawing3d CreateDrawing(float duration = -1, Material material = null) + { + GameObject newDrawingObj = new GameObject("Drawing"); + Drawing3d newDrawing = newDrawingObj.AddComponent(); + newDrawing.Init(instance, material != null ? material : instance.UnlitVertexColorMaterial, duration); + instance.m_Drawings.Add(newDrawing); + return newDrawing; + } + + + void LateUpdate() + { + foreach (Drawing3d drawing in m_Dirty) + { + if (drawing != null) + drawing.Refresh(); + } + m_Dirty.Clear(); + } + + public void OnGUI() + { + Color oldColor = GUI.color; + + int Idx = 0; + while (Idx < m_Drawings.Count) + { + if (m_Drawings[Idx] == null) + m_Drawings.RemoveAt(Idx); + else + { + m_Drawings[Idx].OnDrawingGUI(m_Camera); + ++Idx; + } + } + + GUI.color = oldColor; + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Scripts/Drawing3dManager.cs.meta b/com.unity.robotics.visualizations/Runtime/Drawing3d/Scripts/Drawing3dManager.cs.meta new file mode 100644 index 00000000..efbbcf2a --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Scripts/Drawing3dManager.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 9b11ef521a4b61a46bb4f858ab62c6d1 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Scripts/PointCloudDrawing.cs b/com.unity.robotics.visualizations/Runtime/Drawing3d/Scripts/PointCloudDrawing.cs new file mode 100644 index 00000000..b9ab2e68 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Scripts/PointCloudDrawing.cs @@ -0,0 +1,147 @@ +using System.Collections; +using System.Collections.Generic; + +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public class PointCloudDrawing : MonoBehaviour + { + static readonly float k_SqrtPointFive = Mathf.Sqrt(0.5f); + + Mesh m_Mesh; + List m_Vertices = new List(); + List m_UVRs = new List(); // texture UV and point radius + List m_Colors32 = new List(); + List m_Triangles = new List(); + MeshRenderer m_MeshRenderer; + + public static PointCloudDrawing Create(GameObject parent = null, int numPoints = 0, Material material = null) + { + GameObject newDrawingObj = new GameObject("PointCloud"); + if (parent != null) + { + newDrawingObj.transform.parent = parent.transform; + newDrawingObj.transform.localPosition = Vector3.zero; + newDrawingObj.transform.localRotation = Quaternion.identity; + } + PointCloudDrawing newDrawing = newDrawingObj.AddComponent(); + newDrawing.SetCapacity(numPoints); + newDrawing.SetMaterial(material); + return newDrawing; + } + + public void Awake() + { + m_Mesh = new Mesh(); + + MeshFilter mfilter = gameObject.AddComponent(); + mfilter.sharedMesh = m_Mesh; + + m_MeshRenderer = gameObject.AddComponent(); + } + + public void SetCapacity(int numPoints) + { + m_Vertices.Capacity = numPoints * 4; + m_UVRs.Capacity = numPoints * 4; + m_Colors32.Capacity = numPoints * 4; + } + + public void SetMaterial(Material material) + { + if (material == null) + material = Drawing3dManager.instance.UnlitPointCloudMaterial; + + m_MeshRenderer.sharedMaterial = material; + } + + public void AddPoint(Vector3 point, Color32 color, float radius) + { + int start = m_Vertices.Count; + + for (int Idx = 0; Idx < 4; ++Idx) + { + m_Vertices.Add(point); + m_Colors32.Add(color); + } + + m_UVRs.Add(new Vector3(0, 0, radius)); + m_UVRs.Add(new Vector3(0, 1, radius)); + m_UVRs.Add(new Vector3(1, 0, radius)); + m_UVRs.Add(new Vector3(1, 1, radius)); + + m_Triangles.Add(start + 0); + m_Triangles.Add(start + 1); + m_Triangles.Add(start + 2); + m_Triangles.Add(start + 3); + m_Triangles.Add(start + 2); + m_Triangles.Add(start + 1); + SetDirty(); + } + + public void AddTriangle(Vector3 point, Color32 color, float radius) + { + int start = m_Vertices.Count; + + for (int Idx = 0; Idx < 3; ++Idx) + { + m_Vertices.Add(point); + m_Colors32.Add(color); + } + + m_UVRs.Add(new Vector3(0.5f - k_SqrtPointFive, 0.5f, radius)); + m_UVRs.Add(new Vector3(1, 1.5f + k_SqrtPointFive, radius)); + m_UVRs.Add(new Vector3(1, -0.5f - k_SqrtPointFive, radius)); + + m_Triangles.Add(start + 0); + m_Triangles.Add(start + 1); + m_Triangles.Add(start + 2); + SetDirty(); + } + + void ClearBuffers() + { + m_Vertices.Clear(); + m_Colors32.Clear(); + m_UVRs.Clear(); + m_Triangles.Clear(); + } + + public void Clear() + { + ClearBuffers(); + SetDirty(); + } + + // Bake all buffered data into a mesh. Clear the buffers. + // In most cases you don't need to call this - the mesh will be generated for you in the Update function. + public void Bake() + { + GenerateMesh(); + ClearBuffers(); + enabled = false; + } + + void GenerateMesh() + { + m_Mesh.Clear(); + m_Mesh.indexFormat = m_Vertices.Count < 65536 ? UnityEngine.Rendering.IndexFormat.UInt16 : UnityEngine.Rendering.IndexFormat.UInt32; + m_Mesh.vertices = m_Vertices.ToArray(); + m_Mesh.colors32 = m_Colors32.ToArray(); + m_Mesh.SetUVs(0, m_UVRs.ToArray()); + m_Mesh.triangles = m_Triangles.ToArray(); + } + + void SetDirty() + { + enabled = true; + } + + public void Update() + { + GenerateMesh(); + enabled = false; + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Scripts/PointCloudDrawing.cs.meta b/com.unity.robotics.visualizations/Runtime/Drawing3d/Scripts/PointCloudDrawing.cs.meta new file mode 100644 index 00000000..6da753d1 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Scripts/PointCloudDrawing.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0690fdd11313ac548be10896bcecb5de +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders.meta b/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders.meta new file mode 100644 index 00000000..74615212 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 35bef5269b6d44c4ea14f2c84d52e308 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitColorAlpha.shader b/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitColorAlpha.shader new file mode 100644 index 00000000..e165753d --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitColorAlpha.shader @@ -0,0 +1,48 @@ +Shader "Unlit/ColorAlpha" +{ + Properties + { + _Color ("Main Color", Color) = (1,1,1,1) + } + SubShader + { + Tags {"Queue" = "Transparent" "IgnoreProjector" = "True" "RenderType"="Transparent" } + LOD 100 + ZWrite Off + Blend SrcAlpha OneMinusSrcAlpha + + Pass + { + CGPROGRAM + #pragma vertex vert + #pragma fragment frag + + #include "UnityCG.cginc" + + fixed4 _Color; + + struct appdata + { + float4 vertex : POSITION; + }; + + struct v2f + { + float4 vertex : SV_POSITION; + }; + + v2f vert (appdata v) + { + v2f o; + o.vertex = UnityObjectToClipPos(v.vertex); + return o; + } + + fixed4 frag (v2f i) : SV_Target + { + return _Color; + } + ENDCG + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitColorAlpha.shader.meta b/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitColorAlpha.shader.meta new file mode 100644 index 00000000..6b66e409 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitColorAlpha.shader.meta @@ -0,0 +1,9 @@ +fileFormatVersion: 2 +guid: 460101c20107a2140b780865ef33091c +ShaderImporter: + externalObjects: {} + defaultTextures: [] + nonModifiableTextures: [] + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitPointCloudAdditive.shader b/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitPointCloudAdditive.shader new file mode 100644 index 00000000..64b158fb --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitPointCloudAdditive.shader @@ -0,0 +1,65 @@ +Shader "Unlit/PointCloudAdditive" +{ + Properties + { + _MainTex("Texture", 2D) = "white" {} + _Radius("Radius", float) = 0.1 + } + + SubShader + { + Tags {"Queue" = "Transparent" "IgnoreProjector" = "True" "RenderType" = "Transparent" } + LOD 100 + ZWrite Off + Blend SrcAlpha One + + Pass + { + CGPROGRAM + #pragma vertex vert + #pragma fragment frag + + #include "UnityCG.cginc" + + float _Radius; + sampler2D _MainTex; + float4 _MainTex_ST; + + struct appdata + { + float4 vertex : POSITION; + float2 uv : TEXCOORD0; + float4 color : COLOR; + }; + + struct v2f + { + float4 vertex : SV_POSITION; + float2 uv : TEXCOORD0; + float4 color : COLOR; + }; + + v2f vert (appdata v) + { + v2f o; + o.vertex = UnityObjectToClipPos( + float4(v.vertex.x, v.vertex.y, v.vertex.z, 0) + ); + + 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)); + o.vertex.x += (uv.x - 0.5) * 2 * _Radius * _ScreenParams.y / _ScreenParams.x; + o.vertex.y -= (uv.y - 0.5) * 2 * _Radius; + o.uv = uv; + o.color = v.color; + return o; + } + + fixed4 frag (v2f i) : SV_Target + { + return tex2D(_MainTex, i.uv) * i.color; + } + ENDCG + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitPointCloudAdditive.shader.meta b/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitPointCloudAdditive.shader.meta new file mode 100644 index 00000000..fbdc3a57 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitPointCloudAdditive.shader.meta @@ -0,0 +1,9 @@ +fileFormatVersion: 2 +guid: 4e4553124a2cf424bb79e020815b3dcf +ShaderImporter: + externalObjects: {} + defaultTextures: [] + nonModifiableTextures: [] + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitPointCloudCutout.shader b/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitPointCloudCutout.shader new file mode 100644 index 00000000..e2f7543f --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitPointCloudCutout.shader @@ -0,0 +1,68 @@ +Shader "Unlit/PointCloudCutout" +{ + Properties + { + _MainTex("Texture", 2D) = "white" {} + } + + SubShader + { + Tags {"Queue" = "AlphaTest" "IgnoreProjector" = "True" "RenderType" = "TransparentCutout" } + LOD 100 + ZWrite On + Blend SrcAlpha OneMinusSrcAlpha + + Pass + { + CGPROGRAM + #pragma vertex vert + #pragma fragment frag + + #include "UnityCG.cginc" + + sampler2D _MainTex; + float4 _MainTex_ST; + + struct appdata + { + float4 vertex : POSITION; + float3 uvr : TEXCOORD0; + float4 color : COLOR; + }; + + struct v2f + { + float4 vertex : SV_POSITION; + float2 uv : TEXCOORD0; + float4 color : COLOR; + }; + + v2f vert(appdata v) + { + v2f o; + o.vertex = UnityObjectToClipPos( + float4(v.vertex.x, v.vertex.y, v.vertex.z, 0) + ); + + 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)); + o.vertex.x += (uv.x - 0.5) * 2 * radius * _ScreenParams.y / _ScreenParams.x; + o.vertex.y -= (uv.y - 0.5) * 2 * radius; + o.uv = uv; + o.color = v.color; + o.color.a = 1; + return o; + } + + fixed4 frag (v2f i) : SV_Target + { + fixed4 color = tex2D(_MainTex, i.uv); + clip(color.a-0.1); + color.a = 1; + return color * i.color; + } + ENDCG + } + } +} \ No newline at end of file diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitPointCloudCutout.shader.meta b/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitPointCloudCutout.shader.meta new file mode 100644 index 00000000..063eb1eb --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitPointCloudCutout.shader.meta @@ -0,0 +1,9 @@ +fileFormatVersion: 2 +guid: 90952ee016b305a4a9886b663e5aa6bc +ShaderImporter: + externalObjects: {} + defaultTextures: [] + nonModifiableTextures: [] + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitVertexColor.shader b/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitVertexColor.shader new file mode 100644 index 00000000..cc777eff --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitVertexColor.shader @@ -0,0 +1,46 @@ +Shader "Unlit/VertexColor" +{ + Properties + { + } + SubShader + { + Tags { "RenderType"="Opaque" } + LOD 100 + + Pass + { + CGPROGRAM + #pragma vertex vert + #pragma fragment frag + + #include "UnityCG.cginc" + + struct appdata + { + float4 vertex : POSITION; + fixed4 color : COLOR; + }; + + struct v2f + { + float4 vertex : SV_POSITION; + fixed4 vertexColor : COLOR; + }; + + v2f vert (appdata v) + { + v2f o; + o.vertex = UnityObjectToClipPos(v.vertex); + o.vertexColor = v.color; + return o; + } + + fixed4 frag (v2f i) : SV_Target + { + return i.vertexColor; + } + ENDCG + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitVertexColor.shader.meta b/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitVertexColor.shader.meta new file mode 100644 index 00000000..c1aa40b8 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitVertexColor.shader.meta @@ -0,0 +1,10 @@ +fileFormatVersion: 2 +guid: b1651fbb071c06b409fe12536561208a +ShaderImporter: + externalObjects: {} + defaultTextures: [] + nonModifiableTextures: [] + preprocessorOverride: 0 + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitVertexColorAlpha.shader b/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitVertexColorAlpha.shader new file mode 100644 index 00000000..ebd138f0 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitVertexColorAlpha.shader @@ -0,0 +1,52 @@ +Shader "Unlit/VertexColorAlpha" +{ + Properties + { + _Fade ("Fade", Range(0.0,1.0)) = 1 + } + SubShader + { + Tags {"Queue" = "Transparent" "IgnoreProjector" = "True" "RenderType"="Transparent" } + LOD 100 + ZWrite Off + Blend SrcAlpha OneMinusSrcAlpha + + Pass + { + CGPROGRAM + #pragma vertex vert + #pragma fragment frag + + #include "UnityCG.cginc" + + float _Fade; + + struct appdata + { + float4 vertex : POSITION; + fixed4 color : COLOR; + }; + + struct v2f + { + float4 vertex : SV_POSITION; + fixed4 vertexColor : COLOR; + }; + + v2f vert (appdata v) + { + v2f o; + o.vertex = UnityObjectToClipPos(v.vertex); + o.vertexColor = v.color; + o.vertexColor.a *= _Fade; + return o; + } + + fixed4 frag (v2f i) : SV_Target + { + return i.vertexColor; + } + ENDCG + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitVertexColorAlpha.shader.meta b/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitVertexColorAlpha.shader.meta new file mode 100644 index 00000000..46b3f06c --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Shaders/UnlitVertexColorAlpha.shader.meta @@ -0,0 +1,9 @@ +fileFormatVersion: 2 +guid: af241396b3cfedd4a8d92e4203607847 +ShaderImporter: + externalObjects: {} + defaultTextures: [] + nonModifiableTextures: [] + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Textures.meta b/com.unity.robotics.visualizations/Runtime/Drawing3d/Textures.meta new file mode 100644 index 00000000..979163ad --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Textures.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 132f12e4139b83442bf83ebe46bbdfd2 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Textures/PointSpriteCross.png b/com.unity.robotics.visualizations/Runtime/Drawing3d/Textures/PointSpriteCross.png new file mode 100644 index 00000000..5df8af98 Binary files /dev/null and b/com.unity.robotics.visualizations/Runtime/Drawing3d/Textures/PointSpriteCross.png differ diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Textures/PointSpriteCross.png.meta b/com.unity.robotics.visualizations/Runtime/Drawing3d/Textures/PointSpriteCross.png.meta new file mode 100644 index 00000000..3e548fde --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Textures/PointSpriteCross.png.meta @@ -0,0 +1,106 @@ +fileFormatVersion: 2 +guid: 377c8e589c4618e4a9950e8c3a434634 +TextureImporter: + internalIDToNameTable: [] + externalObjects: {} + serializedVersion: 11 + mipmaps: + mipMapMode: 0 + enableMipMap: 1 + sRGBTexture: 1 + linearTexture: 0 + fadeOut: 0 + borderMipMap: 0 + mipMapsPreserveCoverage: 0 + alphaTestReferenceValue: 0.5 + mipMapFadeDistanceStart: 1 + mipMapFadeDistanceEnd: 3 + bumpmap: + convertToNormalMap: 0 + externalNormalMap: 0 + heightScale: 0.25 + normalMapFilter: 0 + isReadable: 0 + streamingMipmaps: 0 + streamingMipmapsPriority: 0 + vTOnly: 0 + grayScaleToAlpha: 0 + generateCubemap: 6 + cubemapConvolution: 0 + seamlessCubemap: 0 + textureFormat: 1 + maxTextureSize: 2048 + textureSettings: + serializedVersion: 2 + filterMode: -1 + aniso: -1 + mipBias: -100 + wrapU: 1 + wrapV: 1 + wrapW: 1 + nPOTScale: 1 + lightmap: 0 + compressionQuality: 50 + spriteMode: 0 + spriteExtrude: 1 + spriteMeshType: 1 + alignment: 0 + spritePivot: {x: 0.5, y: 0.5} + spritePixelsToUnits: 100 + spriteBorder: {x: 0, y: 0, z: 0, w: 0} + spriteGenerateFallbackPhysicsShape: 1 + alphaUsage: 1 + alphaIsTransparency: 0 + spriteTessellationDetail: -1 + textureType: 0 + textureShape: 1 + singleChannelComponent: 0 + maxTextureSizeSet: 0 + compressionQualitySet: 0 + textureFormatSet: 0 + ignorePngGamma: 0 + applyGammaDecoding: 0 + platformSettings: + - serializedVersion: 3 + buildTarget: DefaultTexturePlatform + maxTextureSize: 2048 + resizeAlgorithm: 0 + textureFormat: -1 + textureCompression: 1 + compressionQuality: 50 + crunchedCompression: 0 + allowsAlphaSplitting: 0 + overridden: 0 + androidETC2FallbackOverride: 0 + forceMaximumCompressionQuality_BC6H_BC7: 0 + - serializedVersion: 3 + buildTarget: Standalone + maxTextureSize: 2048 + resizeAlgorithm: 0 + textureFormat: -1 + textureCompression: 1 + compressionQuality: 50 + crunchedCompression: 0 + allowsAlphaSplitting: 0 + overridden: 0 + androidETC2FallbackOverride: 0 + forceMaximumCompressionQuality_BC6H_BC7: 0 + spriteSheet: + serializedVersion: 2 + sprites: [] + outline: [] + physicsShape: [] + bones: [] + spriteID: + internalID: 0 + vertices: [] + indices: + edges: [] + weights: [] + secondaryTextures: [] + spritePackingTag: + pSDRemoveMatte: 0 + pSDShowRemoveMatteOption: 0 + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Textures/PointSpriteOutlined.dds b/com.unity.robotics.visualizations/Runtime/Drawing3d/Textures/PointSpriteOutlined.dds new file mode 100644 index 00000000..f18df68d Binary files /dev/null and b/com.unity.robotics.visualizations/Runtime/Drawing3d/Textures/PointSpriteOutlined.dds differ diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Textures/PointSpriteOutlined.dds.meta b/com.unity.robotics.visualizations/Runtime/Drawing3d/Textures/PointSpriteOutlined.dds.meta new file mode 100644 index 00000000..5cd2ef64 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Textures/PointSpriteOutlined.dds.meta @@ -0,0 +1,19 @@ +fileFormatVersion: 2 +guid: feb6b5c793f9a3344b5332de80f478f5 +IHVImageFormatImporter: + externalObjects: {} + textureSettings: + serializedVersion: 2 + filterMode: 1 + aniso: 1 + mipBias: 0 + wrapU: 1 + wrapV: 1 + wrapW: 1 + isReadable: 0 + sRGBTexture: 1 + streamingMipmaps: 0 + streamingMipmapsPriority: 0 + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Textures/PointSpriteWhite.png b/com.unity.robotics.visualizations/Runtime/Drawing3d/Textures/PointSpriteWhite.png new file mode 100644 index 00000000..11844ffb Binary files /dev/null and b/com.unity.robotics.visualizations/Runtime/Drawing3d/Textures/PointSpriteWhite.png differ diff --git a/com.unity.robotics.visualizations/Runtime/Drawing3d/Textures/PointSpriteWhite.png.meta b/com.unity.robotics.visualizations/Runtime/Drawing3d/Textures/PointSpriteWhite.png.meta new file mode 100644 index 00000000..1e16bb5a --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Drawing3d/Textures/PointSpriteWhite.png.meta @@ -0,0 +1,106 @@ +fileFormatVersion: 2 +guid: 4b8089fd78123c643998ee91beb76df7 +TextureImporter: + internalIDToNameTable: [] + externalObjects: {} + serializedVersion: 11 + mipmaps: + mipMapMode: 0 + enableMipMap: 1 + sRGBTexture: 1 + linearTexture: 0 + fadeOut: 0 + borderMipMap: 0 + mipMapsPreserveCoverage: 0 + alphaTestReferenceValue: 0.5 + mipMapFadeDistanceStart: 1 + mipMapFadeDistanceEnd: 3 + bumpmap: + convertToNormalMap: 0 + externalNormalMap: 0 + heightScale: 0.25 + normalMapFilter: 0 + isReadable: 0 + streamingMipmaps: 0 + streamingMipmapsPriority: 0 + vTOnly: 0 + grayScaleToAlpha: 0 + generateCubemap: 6 + cubemapConvolution: 0 + seamlessCubemap: 0 + textureFormat: 1 + maxTextureSize: 2048 + textureSettings: + serializedVersion: 2 + filterMode: 1 + aniso: -1 + mipBias: -100 + wrapU: 1 + wrapV: 1 + wrapW: 1 + nPOTScale: 1 + lightmap: 0 + compressionQuality: 50 + spriteMode: 0 + spriteExtrude: 1 + spriteMeshType: 1 + alignment: 0 + spritePivot: {x: 0.5, y: 0.5} + spritePixelsToUnits: 100 + spriteBorder: {x: 0, y: 0, z: 0, w: 0} + spriteGenerateFallbackPhysicsShape: 1 + alphaUsage: 1 + alphaIsTransparency: 1 + spriteTessellationDetail: -1 + textureType: 0 + textureShape: 1 + singleChannelComponent: 0 + maxTextureSizeSet: 0 + compressionQualitySet: 0 + textureFormatSet: 0 + ignorePngGamma: 0 + applyGammaDecoding: 0 + platformSettings: + - serializedVersion: 3 + buildTarget: DefaultTexturePlatform + maxTextureSize: 2048 + resizeAlgorithm: 0 + textureFormat: -1 + textureCompression: 1 + compressionQuality: 50 + crunchedCompression: 0 + allowsAlphaSplitting: 0 + overridden: 0 + androidETC2FallbackOverride: 0 + forceMaximumCompressionQuality_BC6H_BC7: 0 + - serializedVersion: 3 + buildTarget: Standalone + maxTextureSize: 2048 + resizeAlgorithm: 0 + textureFormat: -1 + textureCompression: 1 + compressionQuality: 50 + crunchedCompression: 0 + allowsAlphaSplitting: 0 + overridden: 0 + androidETC2FallbackOverride: 0 + forceMaximumCompressionQuality_BC6H_BC7: 0 + spriteSheet: + serializedVersion: 2 + sprites: [] + outline: [] + physicsShape: [] + bones: [] + spriteID: + internalID: 0 + vertices: [] + indices: + edges: [] + weights: [] + secondaryTextures: [] + spritePackingTag: + pSDRemoveMatte: 0 + pSDShowRemoveMatteOption: 0 + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/ExampleVisualizers.meta b/com.unity.robotics.visualizations/Runtime/ExampleVisualizers.meta new file mode 100644 index 00000000..89642377 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/ExampleVisualizers.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 4086b8b41fb0f8d47946ff857eb55bc5 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/ExampleVisualizers/PointTracingVisualizerExample.cs b/com.unity.robotics.visualizations/Runtime/ExampleVisualizers/PointTracingVisualizerExample.cs new file mode 100644 index 00000000..1eade29d --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/ExampleVisualizers/PointTracingVisualizerExample.cs @@ -0,0 +1,33 @@ +using RosMessageTypes.Geometry; +using System; +using System.Collections; +using System.Collections.Generic; +using System.Linq; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public class PointTracingVisualizerExample : HistoryDrawingVisualizer + { + [SerializeField] + Color m_Color; + [SerializeField] + float m_Thickness = 0.1f; + [SerializeField] + string m_Label; + + public override void Draw(Drawing3d drawing, IEnumerable> messages) + { + Vector3 prevPoint = Vector3.zero; + Color color = Color.white; + string label = ""; + + MessageMetadata meta = messages.FirstOrDefault().Item2; + color = VisualizationUtils.SelectColor(m_Color, meta); + label = VisualizationUtils.SelectLabel(m_Label, meta); + drawing.DrawPath(messages.Select(tuple => tuple.Item1.From()), color, m_Thickness); + drawing.DrawLabel(label, prevPoint, color); + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/ExampleVisualizers/PointTracingVisualizerExample.cs.meta b/com.unity.robotics.visualizations/Runtime/ExampleVisualizers/PointTracingVisualizerExample.cs.meta new file mode 100644 index 00000000..6433c9a2 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/ExampleVisualizers/PointTracingVisualizerExample.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: e0b7d853d9445db4eb15e4113a720594 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/ExampleVisualizers/PointVisualizerExample.cs b/com.unity.robotics.visualizations/Runtime/ExampleVisualizers/PointVisualizerExample.cs new file mode 100644 index 00000000..794b7213 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/ExampleVisualizers/PointVisualizerExample.cs @@ -0,0 +1,40 @@ +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; +using RosMessageTypes.Geometry; + +public class PointVisualizerExample : DrawingVisualizer +{ + // these settings will appear as configurable parameters in the Unity editor. + public float m_Size = 0.1f; + public Color m_Color; + public string m_Label; + + public override void Draw(Drawing3d drawing, PointMsg msg, MessageMetadata meta) + { + // If the user doesn't specify a color, SelectColor helpfully picks one + // based on the message topic. + Color finalColor = VisualizationUtils.SelectColor(m_Color, meta); + + // Most of the default visualizers offer static drawing functions + // so that your own visualizers can easily send work to them. + PointDefaultVisualizer.Draw(msg, drawing, finalColor, m_Size); + + // You can also directly use the drawing functions provided by the Drawing class + drawing.DrawLabel(m_Label, msg.From(), finalColor, m_Size); + } + + public override System.Action CreateGUI(PointMsg msg, MessageMetadata meta) + { + // this code runs each time a new message is received. + // If you want to preprocess the message or declare any state variables for + // the GUI to use, you can do that here. + string text = $"[{msg.x}, {msg.y}, {msg.z}]"; + + return () => + { + // this code runs once per UI event, like a normal Unity OnGUI function. + GUILayout.Label(text); + }; + } +} diff --git a/com.unity.robotics.visualizations/Runtime/ExampleVisualizers/PointVisualizerExample.cs.meta b/com.unity.robotics.visualizations/Runtime/ExampleVisualizers/PointVisualizerExample.cs.meta new file mode 100644 index 00000000..d418a2f3 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/ExampleVisualizers/PointVisualizerExample.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 58030c95e599daf4caef5b66f97ce954 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/ExampleVisualizers/PrefabVisualizerExample.cs b/com.unity.robotics.visualizations/Runtime/ExampleVisualizers/PrefabVisualizerExample.cs new file mode 100644 index 00000000..47f4e35e --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/ExampleVisualizers/PrefabVisualizerExample.cs @@ -0,0 +1,78 @@ +using UnityEngine; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using RosMessageTypes.Geometry; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector; + +// A simple visualizer that places a (user configured) prefab to show the position and +// orientation of a Pose message +public class PrefabVisualizerExample : BaseVisualFactory +{ + // this setting will appear as a configurable parameter in the Unity editor. + public GameObject prefab; + + // The BaseVisualFactory's job is just to create visuals for topics as appropriate. + protected override IVisual CreateVisual(string topic) + { + return new PrefabVisual(topic, prefab); + } + + // The job of the visual itself is to subscribe to a topic, and draw + // representations of the messages it receives. + class PrefabVisual : IVisual + { + GameObject m_Prefab; + GameObject m_PrefabInstance; + PoseMsg m_LastMessage; + bool m_IsDrawingEnabled; + public bool IsDrawingEnabled => m_IsDrawingEnabled; + + public PrefabVisual(string topic, GameObject prefab) + { + m_Prefab = prefab; + + ROSConnection.GetOrCreateInstance().Subscribe(topic, AddMessage); + } + + void AddMessage(PoseMsg message) + { + m_LastMessage = message; + + if (m_IsDrawingEnabled) + Redraw(); + } + + public void SetDrawingEnabled(bool enabled) + { + m_IsDrawingEnabled = enabled; + + if (enabled) + Redraw(); + else + GameObject.Destroy(m_PrefabInstance); + } + + public void Redraw() + { + if (m_LastMessage == null) + { + return; + } + + GameObject.Destroy(m_PrefabInstance); + m_PrefabInstance = GameObject.Instantiate(m_Prefab); + m_PrefabInstance.transform.position = m_LastMessage.position.From(); + m_PrefabInstance.transform.rotation = m_LastMessage.orientation.From(); + } + + public void OnGUI() + { + // Draw the default GUI for a Pose message. + m_LastMessage.GUI(); + } + } + + // Indicates that this visualizer should have a "3d" drawing checkbox in the topics list + public override bool CanShowDrawing => true; +} diff --git a/com.unity.robotics.visualizations/Runtime/ExampleVisualizers/PrefabVisualizerExample.cs.meta b/com.unity.robotics.visualizations/Runtime/ExampleVisualizers/PrefabVisualizerExample.cs.meta new file mode 100644 index 00000000..5922d8f5 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/ExampleVisualizers/PrefabVisualizerExample.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: d68dfc0c4f1923646aad80db05b8caf1 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Materials.meta b/com.unity.robotics.visualizations/Runtime/Materials.meta new file mode 100644 index 00000000..f40f4a20 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Materials.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 70b411c992aa4c746bafeb62d89f0fc5 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Materials/OccupancyGrid.mat b/com.unity.robotics.visualizations/Runtime/Materials/OccupancyGrid.mat new file mode 100644 index 00000000..98169da1 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Materials/OccupancyGrid.mat @@ -0,0 +1,81 @@ +%YAML 1.1 +%TAG !u! tag:unity3d.com,2011: +--- !u!21 &2100000 +Material: + serializedVersion: 6 + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_Name: OccupancyGrid + m_Shader: {fileID: 4800000, guid: 9ac3975a359483f4d828effd03fedb33, type: 3} + m_ShaderKeywords: + m_LightmapFlags: 4 + m_EnableInstancingVariants: 0 + m_DoubleSidedGI: 0 + m_CustomRenderQueue: -1 + stringTagMap: {} + disabledShaderPasses: [] + m_SavedProperties: + serializedVersion: 3 + m_TexEnvs: + - _BumpMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _DetailAlbedoMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _DetailMask: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _DetailNormalMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _EmissionMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _MainTex: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _MetallicGlossMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _OcclusionMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + - _ParallaxMap: + m_Texture: {fileID: 0} + m_Scale: {x: 1, y: 1} + m_Offset: {x: 0, y: 0} + m_Floats: + - _BumpScale: 1 + - _Cutoff: 0.5 + - _DetailNormalMapScale: 1 + - _DstBlend: 0 + - _GlossMapScale: 1 + - _Glossiness: 0.5 + - _GlossyReflections: 1 + - _Metallic: 0 + - _Mode: 0 + - _OcclusionStrength: 1 + - _Parallax: 0.02 + - _SmoothnessTextureChannel: 0 + - _SpecularHighlights: 1 + - _SrcBlend: 1 + - _UVSec: 0 + - _ZWrite: 1 + m_Colors: + - _Color: {r: 1, g: 1, b: 1, a: 1} + - _Color0: {r: 1, g: 1, b: 1, a: 1} + - _Color100: {r: 0, g: 0, b: 0, a: 1} + - _ColorUnknown: {r: 0, g: 0, b: 0, a: 0} + - _EmissionColor: {r: 0, g: 0, b: 0, a: 1} + m_BuildTextureStacks: [] diff --git a/com.unity.robotics.visualizations/Runtime/Materials/OccupancyGrid.mat.meta b/com.unity.robotics.visualizations/Runtime/Materials/OccupancyGrid.mat.meta new file mode 100644 index 00000000..5952cf00 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Materials/OccupancyGrid.mat.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 8e30ea50d6b692349a46d177ded15435 +NativeFormatImporter: + externalObjects: {} + mainObjectFileID: 2100000 + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Scripts.meta b/com.unity.robotics.visualizations/Runtime/Scripts.meta new file mode 100644 index 00000000..34f72a8a --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: be5ddce769b486e4a8683d7bd1d3786a +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/BaseVisualFactory.cs b/com.unity.robotics.visualizations/Runtime/Scripts/BaseVisualFactory.cs new file mode 100644 index 00000000..ebd56e6f --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/BaseVisualFactory.cs @@ -0,0 +1,75 @@ +using System.Collections; +using System.Collections.Generic; +using System.Linq; +using Unity.Robotics.Visualizations; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using UnityEngine; + + +namespace Unity.Robotics.Visualizations +{ + public abstract class BaseVisualFactory : MonoBehaviour, IVisualFactory, IPriority + where T : Message + { + [SerializeField] + protected string m_Topic; + public string Topic { get => m_Topic; set => m_Topic = value; } + + [SerializeField] + [HideInInspector] + string m_ID; + public string ID => m_ID; + +#if UNITY_EDITOR + // this could be any string that's not blank or null; it just needs to not test equal to any "real" deserialized ID + string m_LastSetID = "~~invalid"; + + void OnValidate() + { + if (m_LastSetID != m_ID) + { + // Get a locally unique, scene non-specific, persistent id for this component + string fullID = UnityEditor.GlobalObjectId.GetGlobalObjectIdSlow(this).ToString(); + string[] idParts = fullID.Split('-'); + if (idParts.Length < 5) // not sure how this would happen, but just in case + m_ID = fullID; + else + m_ID = idParts[3]+"-"+idParts[4]; + m_LastSetID = m_ID; + } + } +#endif + public virtual string Name => (string.IsNullOrEmpty(m_Topic) ? "" : $"({m_Topic}) ") + GetType().ToString().Split('.').Last(); + + public int Priority { get; set; } + public abstract bool CanShowDrawing { get; } + + Dictionary m_Visuals = new Dictionary(); + public IEnumerable AllVisuals => m_Visuals.Values; + + public virtual IVisual GetOrCreateVisual(string topic) + { + IVisual visual; + if (m_Visuals.TryGetValue(topic, out visual)) + return visual; + + visual = CreateVisual(topic); + m_Visuals.Add(topic, visual); + return visual; + } + + protected abstract IVisual CreateVisual(string topic); + + public virtual void Start() + { + if (m_Topic == "") + { + VisualFactoryRegistry.RegisterTypeVisualizer(this, Priority); + } + else + { + VisualFactoryRegistry.RegisterTopicVisualizer(m_Topic, this, Priority); + } + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/BaseVisualFactory.cs.meta b/com.unity.robotics.visualizations/Runtime/Scripts/BaseVisualFactory.cs.meta new file mode 100644 index 00000000..0b1ca957 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/BaseVisualFactory.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 7e494c6f903a1d344a0f0e734217e75d +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/DrawingVisualizer.cs b/com.unity.robotics.visualizations/Runtime/Scripts/DrawingVisualizer.cs new file mode 100644 index 00000000..a8eb2b0b --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/DrawingVisualizer.cs @@ -0,0 +1,143 @@ +using RosMessageTypes.Std; +using System; +using Unity.Robotics.ROSTCPConnector; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public abstract class DrawingVisualizer : BaseVisualFactory + where T : Message + { + public override bool CanShowDrawing => true; + + protected override IVisual CreateVisual(string topic) + { + return new DrawingVisual(topic, this); + } + + public Color SelectColor(Color userColor, MessageMetadata meta) + { + return VisualizationUtils.SelectColor(userColor, meta); + } + + public string SelectLabel(string userLabel, MessageMetadata meta) + { + return VisualizationUtils.SelectLabel(userLabel, meta); + } + + public virtual void Draw(DrawingVisual drawing, T message, MessageMetadata meta) + { + Draw(drawing.Drawing, message, meta); + } + + public virtual void Draw(Drawing3d drawing, T message, MessageMetadata meta) { } + + public virtual Action CreateGUI(T message, MessageMetadata meta) + { + return VisualizationUtils.CreateDefaultGUI(message, meta); + } + + public class DrawingVisual : IVisual + { + T m_Message; + MessageMetadata m_Meta; + + Drawing3d m_Drawing; + public Drawing3d Drawing => m_Drawing; + Action m_GUIAction; + DrawingVisualizer m_Factory; + string m_Topic; + bool m_IsDrawingEnabled; + public bool IsDrawingEnabled => m_IsDrawingEnabled; + float m_LastDrawingFrameTime = -1; + + public DrawingVisual(string topic, DrawingVisualizer factory) + { + m_Topic = topic; + m_Factory = factory; + + ROSConnection.GetOrCreateInstance().Subscribe(topic, AddMessage); + } + + public virtual void AddMessage(Message message) + { + MessageMetadata meta = new MessageMetadata(m_Topic, Time.time, DateTime.Now); + + if (!VisualizationUtils.AssertMessageType(message, m_Topic)) + return; + + m_Message = (T)message; + m_Meta = meta; + m_GUIAction = null; + + // If messages are coming in faster than 1 per frame, we only update the drawing once per frame + if (m_IsDrawingEnabled && Time.time > m_LastDrawingFrameTime) + { + Redraw(); + } + + m_LastDrawingFrameTime = Time.time; + } + + public void SetDrawingEnabled(bool enabled) + { + if (m_IsDrawingEnabled == enabled) + return; + + m_IsDrawingEnabled = enabled; + + if (!enabled && m_Drawing != null) + { + m_Drawing.Clear(); + } + + if (enabled && m_Message != null) + { + Redraw(); + } + } + + public bool hasDrawing => m_Drawing != null; + + public void OnGUI() + { + if (m_Message == null) + { + GUILayout.Label("Waiting for message..."); + return; + } + + if (m_GUIAction == null) + { + m_GUIAction = m_Factory.CreateGUI(m_Message, m_Meta); + } + m_GUIAction(); + } + + public void DeleteDrawing() + { + if (m_Drawing != null) + { + m_Drawing.Destroy(); + } + + m_Drawing = null; + } + + public void Redraw() + { + if (m_Drawing == null) + { + m_Drawing = Drawing3dManager.CreateDrawing(); + } + else + { + m_Drawing.Clear(); + } + + m_Factory.Draw(this, m_Message, m_Meta); + } + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/DrawingVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/Scripts/DrawingVisualizer.cs.meta new file mode 100644 index 00000000..e92b0c77 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/DrawingVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 9fdad56ecb3564d98804c1e733c81a2c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/DrawingVisualizerWithSettings.cs b/com.unity.robotics.visualizations/Runtime/Scripts/DrawingVisualizerWithSettings.cs new file mode 100644 index 00000000..1c7d327e --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/DrawingVisualizerWithSettings.cs @@ -0,0 +1,57 @@ +using RosMessageTypes.Std; +using System; +using Unity.Robotics.ROSTCPConnector; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public abstract class DrawingVisualizerWithSettings : DrawingVisualizer + where TMessage : Message + where TDrawingSettings : VisualizerSettingsGeneric + { + public const string ScriptableObjectsSettingsPath = "ScriptableObjects/"; + public abstract string DefaultScriptableObjectPath { get; } + + [SerializeField] + TDrawingSettings m_Settings; + public TDrawingSettings Settings { get => m_Settings; set => m_Settings = value; } + + public override string Name => (string.IsNullOrEmpty(m_Topic) ? "" : $"({m_Topic}) ") + Settings.name; + + void Awake() + { + if (m_Settings == null) + { + m_Settings = Resources.Load(DefaultScriptableObjectPath); + } + } + + void OnValidate() + { + if (m_Settings == null) + { + m_Settings = (TDrawingSettings)Resources.Load(DefaultScriptableObjectPath); + } + } + + public override void Draw(Drawing3d drawing, TMessage message, MessageMetadata meta) + { + m_Settings.Draw(drawing, message, meta); + } + + public override Action CreateGUI(TMessage message, MessageMetadata meta) + { + return m_Settings.CreateGUI(message, meta); + } + + public void Redraw() + { + // settings have changed - update the visualization + foreach (IVisual visual in AllVisuals) + { + visual.Redraw(); + } + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/DrawingVisualizerWithSettings.cs.meta b/com.unity.robotics.visualizations/Runtime/Scripts/DrawingVisualizerWithSettings.cs.meta new file mode 100644 index 00000000..3ea8e4a8 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/DrawingVisualizerWithSettings.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: cb6866ca1143c8d4faa051e321228ed9 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/GuiVisualizer.cs b/com.unity.robotics.visualizations/Runtime/Scripts/GuiVisualizer.cs new file mode 100644 index 00000000..3b1b997e --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/GuiVisualizer.cs @@ -0,0 +1,75 @@ +using RosMessageTypes.Std; +using System; +using System.Collections.Generic; +using Unity.Robotics.ROSTCPConnector; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public abstract class GuiVisualizer : BaseVisualFactory, IPriority + where T : Message + { + protected override IVisual CreateVisual(string topic) + { + return new GuiVisual(topic, this); + } + + public override bool CanShowDrawing => false; + + public virtual Action CreateGUI(T message, MessageMetadata meta) + { + return VisualizationUtils.CreateDefaultGUI(message, meta); + } + + public class GuiVisual : IVisual + { + string m_Topic; + GuiVisualizer m_Factory; + + Action m_GUIAction; + + public GuiVisual(string topic, GuiVisualizer factory) + { + m_Topic = topic; + m_Factory = factory; + ROSConnection.GetOrCreateInstance().Subscribe(m_Topic, AddMessage); + } + + public void AddMessage(Message message) + { + if (!VisualizationUtils.AssertMessageType(message, m_Topic)) + return; + + this.message = (T)message; + this.meta = new MessageMetadata(m_Topic, Time.time, DateTime.Now); + m_GUIAction = null; + } + + public T message { get; private set; } + public MessageMetadata meta { get; private set; } + + public bool hasDrawing => false; + public bool hasAction => m_GUIAction != null; + + public void OnGUI() + { + if (message == null) + { + GUILayout.Label("Waiting for message..."); + return; + } + + if (m_GUIAction == null) + { + m_GUIAction = m_Factory.CreateGUI(message, meta); + } + m_GUIAction(); + } + + public bool IsDrawingEnabled => false; + public void SetDrawingEnabled(bool enabled) { } + public void Redraw() { } + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/GuiVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/Scripts/GuiVisualizer.cs.meta new file mode 100644 index 00000000..0ad0eae3 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/GuiVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 1a335f2cbc85f406a834878ea0ba7cf0 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/HistoryDrawingVisualizer.cs b/com.unity.robotics.visualizations/Runtime/Scripts/HistoryDrawingVisualizer.cs new file mode 100644 index 00000000..e258f17a --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/HistoryDrawingVisualizer.cs @@ -0,0 +1,136 @@ +using RosMessageTypes.Std; +using System; +using System.Collections.Generic; +using System.Linq; +using Unity.Robotics.ROSTCPConnector; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public abstract class HistoryDrawingVisualizer : BaseVisualFactory + where T : Message + { + [SerializeField] + int m_HistoryLength; + public int HistoryLength { get => m_HistoryLength; set => m_HistoryLength = value; } + + public override bool CanShowDrawing => true; + + protected override IVisual CreateVisual(string topic) + { + return new HistoryDrawingVisual(topic, this, m_HistoryLength); + } + + public virtual void Draw(Drawing3d drawing, IEnumerable> messages) + { + Draw(drawing, messages.Select(Tuple => Tuple.Item1)); + } + + public virtual void Draw(Drawing3d drawing, IEnumerable messages) { } + + public virtual Action CreateGUI(IEnumerable> messages) + { + List actions = new List(); + foreach ((T message, MessageMetadata meta) in messages) + { + actions.Add(CreateGUI(message, meta)); + } + return () => actions.ForEach(a => a()); + } + + public virtual Action CreateGUI(T message, MessageMetadata meta) + { + return VisualizationUtils.CreateDefaultGUI(message, meta); + } + + public class HistoryDrawingVisual : IVisual + { + string m_Topic; + Queue> m_History = new Queue>(); + + Drawing3d m_BasicDrawing; + Action m_GUIAction; + HistoryDrawingVisualizer m_Factory; + int m_HistoryLength; + bool m_IsDrawingEnabled; + public bool IsDrawingEnabled => m_IsDrawingEnabled; + float m_LastDrawingFrameTime = -1; + + public HistoryDrawingVisual(string topic, HistoryDrawingVisualizer factory, int historyLength) + { + m_Topic = topic; + m_Factory = factory; + m_HistoryLength = historyLength; + + ROSConnection.GetOrCreateInstance().Subscribe(m_Topic, AddMessage); + } + + public void AddMessage(Message message) + { + if (!VisualizationUtils.AssertMessageType(message, m_Topic)) + return; + + m_History.Enqueue(new Tuple( + (T)message, + new MessageMetadata(m_Topic, Time.time, DateTime.Now) + )); + if (m_History.Count > m_HistoryLength) + m_History.Dequeue(); + m_GUIAction = null; + + if (m_IsDrawingEnabled && Time.time > m_LastDrawingFrameTime) + { + Redraw(); + } + + m_LastDrawingFrameTime = Time.time; + } + + public void SetDrawingEnabled(bool enabled) + { + if (m_IsDrawingEnabled == enabled) + return; + + if (!enabled && m_BasicDrawing != null) + { + m_BasicDrawing.Clear(); + } + m_IsDrawingEnabled = enabled; + } + + public void OnGUI() + { + if (m_GUIAction == null) + { + m_GUIAction = m_Factory.CreateGUI(m_History); + } + m_GUIAction(); + } + + public void DeleteDrawing() + { + if (m_BasicDrawing != null) + { + m_BasicDrawing.Destroy(); + } + + m_BasicDrawing = null; + } + + public void Redraw() + { + if (m_BasicDrawing == null) + { + m_BasicDrawing = Drawing3dManager.CreateDrawing(); + } + else + { + m_BasicDrawing.Clear(); + } + + m_Factory.Draw(m_BasicDrawing, m_History); + } + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/HistoryDrawingVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/Scripts/HistoryDrawingVisualizer.cs.meta new file mode 100644 index 00000000..e051d405 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/HistoryDrawingVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 3cb9a319feaafdd418ff9c3a8156add6 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/HudTabOrdering.cs b/com.unity.robotics.visualizations/Runtime/Scripts/HudTabOrdering.cs new file mode 100644 index 00000000..576d7a26 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/HudTabOrdering.cs @@ -0,0 +1,16 @@ +using System.Collections; +using System.Collections.Generic; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + // By default a user-created tab is assigned a positive index. + // The built in tabs have a negative index so that they always come first. + public enum HudTabOrdering + { + Topics = -4, + TF = -3, + Layout = -2, + Markers = -1, + } +} diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/HudTabOrdering.cs.meta b/com.unity.robotics.visualizations/Runtime/Scripts/HudTabOrdering.cs.meta new file mode 100644 index 00000000..022ab0cf --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/HudTabOrdering.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 404ec7c3c6fa0554bab9b601e0f21883 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/ITextureVisual.cs b/com.unity.robotics.visualizations/Runtime/Scripts/ITextureVisual.cs new file mode 100644 index 00000000..214778c8 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/ITextureVisual.cs @@ -0,0 +1,13 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public interface ITextureVisual : IVisual + { + Texture2D GetTexture(); + void ListenForTextureChange(Action callback); + } +} diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/ITextureVisual.cs.meta b/com.unity.robotics.visualizations/Runtime/Scripts/ITextureVisual.cs.meta new file mode 100644 index 00000000..483b6418 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/ITextureVisual.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 3efe57f4d81545242b6956d318d18c75 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/IVisual.cs b/com.unity.robotics.visualizations/Runtime/Scripts/IVisual.cs new file mode 100644 index 00000000..5ee62f31 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/IVisual.cs @@ -0,0 +1,14 @@ +using System.Collections; +using System.Collections.Generic; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public interface IVisual + { + bool IsDrawingEnabled { get; } + void SetDrawingEnabled(bool enabled); + void Redraw(); + void OnGUI(); + } +} diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/IVisual.cs.meta b/com.unity.robotics.visualizations/Runtime/Scripts/IVisual.cs.meta new file mode 100644 index 00000000..8096b8b4 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/IVisual.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: e00ffde8b00d2694fa38599d02923475 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/IVisualFactory.cs b/com.unity.robotics.visualizations/Runtime/Scripts/IVisualFactory.cs new file mode 100644 index 00000000..e55a1b6c --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/IVisualFactory.cs @@ -0,0 +1,14 @@ +using System.Collections; +using System.Collections.Generic; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public interface IVisualFactory + { + string Name { get; } + string ID { get; } + bool CanShowDrawing { get; } + IVisual GetOrCreateVisual(string topic); + } +} diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/IVisualFactory.cs.meta b/com.unity.robotics.visualizations/Runtime/Scripts/IVisualFactory.cs.meta new file mode 100644 index 00000000..f88fbcf4 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/IVisualFactory.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b50b363db9a1c3443ae61cbc685dfbf1 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/PrioritySetter.cs b/com.unity.robotics.visualizations/Runtime/Scripts/PrioritySetter.cs new file mode 100644 index 00000000..7c7f9013 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/PrioritySetter.cs @@ -0,0 +1,23 @@ +using System.Collections; +using System.Collections.Generic; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public interface IPriority + { + int Priority { get; set; } + } + + public class PrioritySetter : MonoBehaviour + { + [SerializeField] + int m_Priority; + + private void Awake() + { + foreach (IPriority p in GetComponentsInChildren()) + p.Priority = m_Priority; + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/PrioritySetter.cs.meta b/com.unity.robotics.visualizations/Runtime/Scripts/PrioritySetter.cs.meta new file mode 100644 index 00000000..df291806 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/PrioritySetter.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: d52ab7220652a3a4bb29ca9a1d4b91c8 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/RobotVisualization.cs b/com.unity.robotics.visualizations/Runtime/Scripts/RobotVisualization.cs new file mode 100644 index 00000000..2b10352a --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/RobotVisualization.cs @@ -0,0 +1,263 @@ +#if URDF_IMPORTER +using RosMessageTypes.Sensor; +using RosMessageTypes.Trajectory; +using System; +using System.Collections; +using System.Collections.Generic; +using System.Linq; +using UnityEngine; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; + +using Unity.Robotics.UrdfImporter; + +namespace Unity.Robotics.Visualizations +{ + public class RobotVisualization + { + public struct JointPlacement + { + public UrdfJoint Joint; + public Vector3 Position; + public Quaternion Rotation; + } + + Dictionary m_JointsByName = new Dictionary(); + UrdfRobot m_Robot; + + public RobotVisualization(UrdfRobot robot) + { + this.m_Robot = robot; + foreach (UrdfJoint joint in robot.gameObject.GetComponentsInChildren()) + { + if (!m_JointsByName.ContainsKey(joint.jointName)) + { + m_JointsByName.Add( + joint.jointName, + new JointPlacement { Joint = joint, Position = joint.transform.localPosition, Rotation = joint.transform.localRotation } + ); + } + } + } + + // Returns the positions of the selected joint across the whole trajectory + public Vector3[] GetJointPath(int jointIndex, string[] jointNames, JointTrajectoryPointMsg[] point) + { + Vector3[] result = new Vector3[point.Length]; + for (int Idx = 0; Idx < point.Length; ++Idx) + { + JointPlacement[] placements = GetJointPlacements(point[Idx], jointNames); + result[Idx] = placements[jointIndex].Position; + } + return result; + } + + // Returns a 2d array: JointPlacement[index of JointTrajectoryPoint][index of joint] + public JointPlacement[][] GetJointPlacements(JointTrajectoryMsg trajectory) + { + JointPlacement[][] result = new JointPlacement[trajectory.points.Length][]; + for (int Idx = 0; Idx < trajectory.points.Length; ++Idx) + { + result[Idx] = GetJointPlacements(trajectory.points[Idx], trajectory.joint_names); + } + return result; + } + + public JointPlacement[] GetJointPlacements(JointStateMsg jointState) + { + Quaternion lastWorldRotation = m_Robot.transform.rotation; + Vector3 lastWorldPosition = m_Robot.transform.position; + GameObject lastJoint = m_Robot.gameObject; + JointPlacement[] result = new JointPlacement[jointState.name.Length]; + + for (int i = 0; i < jointState.name.Length; ++i) + { + JointPlacement jointData = m_JointsByName[jointState.name[i]]; + float rotationDegrees = (float)(jointState.position[i] * Mathf.Rad2Deg); + + ArticulationBody body = jointData.Joint.GetComponent(); + Quaternion jointRotation = body.anchorRotation * Quaternion.Euler(rotationDegrees, 0, 0) * Quaternion.Inverse(body.anchorRotation); + Quaternion localRotation = jointData.Rotation * jointRotation; + Vector3 localPosition = lastJoint.transform.InverseTransformPoint(body.transform.position); + Vector3 worldPosition = lastWorldPosition + lastWorldRotation * localPosition; + Quaternion worldRotation = lastWorldRotation * localRotation; + + result[i] = new JointPlacement { Joint = jointData.Joint, Position = worldPosition, Rotation = worldRotation }; + + lastWorldPosition = worldPosition; + lastWorldRotation = worldRotation; + lastJoint = body.gameObject; + } + + return result; + } + + public JointPlacement[] GetJointPlacements(MultiDOFJointStateMsg jointState) + { + Quaternion lastWorldRotation = m_Robot.transform.rotation; + Vector3 lastWorldPosition = m_Robot.transform.position; + GameObject lastJoint = m_Robot.gameObject; + JointPlacement[] result = new JointPlacement[jointState.joint_names.Length]; + + for (int i = 0; i < jointState.joint_names.Length; ++i) + { + JointPlacement jointData = m_JointsByName[jointState.joint_names[i]]; + + ArticulationBody body = jointData.Joint.GetComponent(); + Quaternion jointRotation = body.anchorRotation * jointState.transforms[i].rotation.From() * Quaternion.Inverse(body.anchorRotation); + Quaternion localRotation = jointData.Rotation * jointRotation; + Vector3 localPosition = Vector3.Scale(lastJoint.transform.InverseTransformPoint(body.transform.position), jointState.transforms[i].translation.From()); + Vector3 worldPosition = lastWorldPosition + lastWorldRotation * (lastJoint.transform.InverseTransformPoint(body.transform.position) + localPosition); + Quaternion worldRotation = lastWorldRotation * localRotation; + + result[i] = new JointPlacement { Joint = jointData.Joint, Position = worldPosition, Rotation = worldRotation }; + + lastWorldPosition = worldPosition; + lastWorldRotation = worldRotation; + lastJoint = body.gameObject; + } + + return result; + } + + public JointPlacement[] GetJointPlacements(JointTrajectoryPointMsg point, string[] jointNames) + { + Quaternion lastWorldRotation = m_Robot.transform.rotation; + Vector3 lastWorldPosition = m_Robot.transform.position; + GameObject lastJoint = m_Robot.gameObject; + JointPlacement[] result = new JointPlacement[jointNames.Length]; + + for (int Idx = 0; Idx < point.positions.Length; ++Idx) + { + JointPlacement jointData = m_JointsByName[jointNames[Idx]]; + float rotationDegrees = (float)(point.positions[Idx] * Mathf.Rad2Deg); + + ArticulationBody body = jointData.Joint.GetComponent(); + Quaternion jointRotation = body.anchorRotation * Quaternion.Euler(rotationDegrees, 0, 0) * Quaternion.Inverse(body.anchorRotation); + Quaternion localRotation = jointData.Rotation * jointRotation; + Vector3 localPosition = lastJoint.transform.InverseTransformPoint(body.transform.position); + Vector3 worldPosition = lastWorldPosition + lastWorldRotation * localPosition; + Quaternion worldRotation = lastWorldRotation * localRotation; + + result[Idx] = new JointPlacement { Joint = jointData.Joint, Position = worldPosition, Rotation = worldRotation }; + + lastWorldPosition = worldPosition; + lastWorldRotation = worldRotation; + lastJoint = body.gameObject; + } + + return result; + } + + public UrdfJoint GetJointByName(string joint) + { + JointPlacement result; + if (!m_JointsByName.TryGetValue(joint, out result)) + { + return null; + } + + return result.Joint; + } + + public void DrawJointPaths(Drawing3d drawing, JointTrajectoryMsg message, Color color, float pathThickness) + { + JointPlacement[][] jointPlacements = GetJointPlacements(message); + for (int Idx = 0; Idx < message.joint_names.Length; ++Idx) + { + DrawJointPath(drawing, jointPlacements, Idx, color, pathThickness); + } + } + + public void DrawJointPaths(Drawing3d drawing, JointPlacement[][] jointPlacements, Color color, float pathThickness) + { + for (int pathIdx = 1; pathIdx < jointPlacements.Length; ++pathIdx) + { + JointPlacement[] pose1 = jointPlacements[pathIdx - 1]; + JointPlacement[] pose2 = jointPlacements[pathIdx]; + for (int jointIdx = 0; jointIdx < pose1.Length; ++jointIdx) + { + drawing.DrawLine(pose1[jointIdx].Position, pose2[jointIdx].Position, color, pathThickness); + } + } + } + + public void DrawJointPath(Drawing3d drawing, JointTrajectoryMsg message, int jointIndex, Color color, float pathThickness) + { + DrawJointPath(drawing, GetJointPlacements(message), jointIndex, color, pathThickness); + } + + public void DrawEffort(Drawing3d drawing, JointStateMsg message, Color color) + { + if (message.effort.Length > 0) + { + DrawEffort(drawing, GetJointPlacements(message), color, message.effort); + } + else + { + Debug.Log("This JointState message contains no Effort data!"); + return; + } + } + + public void DrawEffort(Drawing3d drawing, JointPlacement[] placements, Color color, double[] radii) + { + for (int i = 0; i < placements.Length; i++) + { + VisualizationUtils.DrawRotationArrow(drawing, placements[i].Rotation, placements[i].Position, color, (float)radii[i]); + } + } + + public void DrawGhost(Drawing3d drawing, JointTrajectoryMsg message, int pointIndex, Color color) + { + DrawGhost(drawing, GetJointPlacements(message.points[pointIndex], message.joint_names), color); + } + + public void DrawGhost(Drawing3d drawing, JointTrajectoryPointMsg message, string[] jointNames, Color color) + { + DrawGhost(drawing, GetJointPlacements(message, jointNames), color); + } + + public void DrawGhost(Drawing3d drawing, JointStateMsg message, Color color) + { + DrawGhost(drawing, GetJointPlacements(message), color); + } + + public void DrawGhost(Drawing3d drawing, MultiDOFJointStateMsg message, Color color) + { + DrawGhost(drawing, GetJointPlacements(message), color); + } + + public void DrawJointPath(Drawing3d drawing, JointPlacement[][] jointPlacements, int jointIndex, Color color, float pathThickness) + { + drawing.DrawLineStrip(jointPlacements.Select(p => p[jointIndex].Position).ToArray(), color, pathThickness); + } + + public void DrawGhost(Drawing3d drawing, JointPlacement[] placements, Color color) + { + foreach (JointPlacement jointPlacement in placements) + { + UrdfJoint joint = jointPlacement.Joint; + int numChildren = joint.transform.childCount; + for (int Idx = 0; Idx < numChildren; ++Idx) + { + Transform child = joint.transform.GetChild(Idx); + if (child.GetComponent()) // don't want to draw the other joints with this jointPlacement + continue; + foreach (MeshFilter mfilter in child.GetComponentsInChildren()) + { + Vector3 localMeshOffset = jointPlacement.Rotation * joint.transform.InverseTransformPoint(mfilter.transform.position); + Quaternion localMeshRotation = Quaternion.Inverse(joint.transform.rotation) * mfilter.transform.rotation; + drawing.DrawMesh( + mfilter.sharedMesh, + jointPlacement.Position + localMeshOffset, + jointPlacement.Rotation * localMeshRotation, + Vector3.one, + color + ); + } + } + } + } + } +} +#endif diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/RobotVisualization.cs.meta b/com.unity.robotics.visualizations/Runtime/Scripts/RobotVisualization.cs.meta new file mode 100644 index 00000000..3cf23b9b --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/RobotVisualization.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 3802e45750af17d4286a4418631ce4e6 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/TextureVisualizer.cs b/com.unity.robotics.visualizations/Runtime/Scripts/TextureVisualizer.cs new file mode 100644 index 00000000..8be4ac7b --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/TextureVisualizer.cs @@ -0,0 +1,99 @@ +using RosMessageTypes.Std; +using System; +using System.Collections.Generic; +using Unity.Robotics.ROSTCPConnector; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public abstract class TextureVisualizer : BaseVisualFactory + where T : Message + { + public override bool CanShowDrawing => false; + + protected override IVisual CreateVisual(string topic) + { + return new TextureVisual(topic, this); + } + + public abstract Texture2D CreateTexture(T message); + + public virtual Action CreateGUI(T message, MessageMetadata meta, Texture2D tex) + { + return VisualizationUtils.CreateDefaultGUI(message, meta); + } + + public class TextureVisual : ITextureVisual + { + string m_Topic; + TextureVisualizer m_Factory; + + Action m_GUIAction; + Texture2D m_Texture2D; + List> m_OnChangeCallbacks = new List>(); + + public void ListenForTextureChange(Action callback) + { + m_OnChangeCallbacks.Add(callback); + } + + public TextureVisual(string topic, TextureVisualizer factory) + { + m_Topic = topic; + m_Factory = factory; + + ROSConnection.GetOrCreateInstance().Subscribe(topic, AddMessage); + } + + public void AddMessage(Message message) + { + if (!VisualizationUtils.AssertMessageType(message, m_Topic)) + return; + + this.message = (T)message; + this.meta = meta; + m_Texture2D = null; + m_GUIAction = null; + + // notify anyone who requested updates when the texture changes + foreach (Action callback in m_OnChangeCallbacks) + { + callback(GetTexture()); + } + } + + public T message { get; private set; } + + public MessageMetadata meta { get; private set; } + + public void OnGUI() + { + if (message == null) + { + GUILayout.Label("Waiting for message..."); + return; + } + + if (m_GUIAction == null) + { + m_GUIAction = m_Factory.CreateGUI(message, meta, GetTexture()); + } + m_GUIAction(); + } + + public Texture2D GetTexture() + { + if (m_Texture2D == null && message != null) + { + m_Texture2D = m_Factory.CreateTexture(message); + } + return m_Texture2D; + } + + public bool IsDrawingEnabled => false; + public void SetDrawingEnabled(bool enabled) { } + public void Redraw() { } + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/TextureVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/Scripts/TextureVisualizer.cs.meta new file mode 100644 index 00000000..7533f73c --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/TextureVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f8cd0bdef3c054967b3372bf4092ec72 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/ToStringVisualizer.cs b/com.unity.robotics.visualizations/Runtime/Scripts/ToStringVisualizer.cs new file mode 100644 index 00000000..b06b6696 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/ToStringVisualizer.cs @@ -0,0 +1,74 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using Unity.Robotics.ROSTCPConnector; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + class ToStringVisualizer : IVisualFactory + { + public string Name => "ToString"; + public string ID => "ToString"; + MessageSubtopic m_Subtopic; + + public ToStringVisualizer(MessageSubtopic subtopic) + { + m_Subtopic = subtopic; + } + + Dictionary m_Visuals = new Dictionary(); + + public IVisual GetOrCreateVisual(string topic) + { + IVisual visual; + if (m_Visuals.TryGetValue(topic, out visual)) + return visual; + + visual = new ToStringVisual(topic, m_Subtopic); + m_Visuals.Add(topic, visual); + return visual; + } + + // The ToString visualizer is the default visualizer. If you're trying to register it, something has gone extremely wrong... + public void Register(int priority) { throw new NotImplementedException(); } + + public bool CanShowDrawing => false; + + class ToStringVisual : IVisual + { + public Message message { get; private set; } + + public ToStringVisual(string topic, MessageSubtopic subtopic) + { + ROSConnection ros = ROSConnection.GetOrCreateInstance(); + RosTopicState state = ros.GetTopic(topic); + if (subtopic == MessageSubtopic.Response) + state = state.ServiceResponseTopic; + state.AddSubscriber(AddMessage); + } + + public void AddMessage(Message newMessage) + { + message = newMessage; + } + + public void OnGUI() + { + if (message == null) + { + GUILayout.Label("Waiting for message..."); + return; + } + + string text = message.ToString(); + GUILayout.Label(text); + } + + public bool IsDrawingEnabled => false; + public void SetDrawingEnabled(bool enabled) { } + public void Redraw() { } + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/ToStringVisualizer.cs.meta b/com.unity.robotics.visualizations/Runtime/Scripts/ToStringVisualizer.cs.meta new file mode 100644 index 00000000..cacab8bf --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/ToStringVisualizer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8f7c98995a451ab4d989cb37b220ed64 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/VisualFactoryRegistry.cs b/com.unity.robotics.visualizations/Runtime/Scripts/VisualFactoryRegistry.cs new file mode 100644 index 00000000..423f2e1e --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/VisualFactoryRegistry.cs @@ -0,0 +1,140 @@ +using RosMessageTypes.Std; +using System; +using System.Collections.Generic; +using System.Linq; +using Unity.Robotics.ROSTCPConnector; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public readonly struct MessageMetadata + { + public readonly string Topic; + public readonly float FrameTime; + public readonly DateTime Timestamp; + + public MessageMetadata(string topic, float frameTime, DateTime timestamp) + { + Topic = topic; + FrameTime = frameTime; + Timestamp = timestamp; + } + } + + public static class VisualFactoryRegistry + { + class PrioritizedList + { + public T Best { get; private set; } + int m_BestPriority = int.MinValue; + List m_All = new List(); + public IEnumerable All => m_All; + + public void Add(T value, int priority) + { + m_All.Add(value); + if (m_BestPriority <= priority) + { + m_BestPriority = priority; + Best = value; + } + } + } + + static Dictionary> s_TopicVisualFactories = new Dictionary>(); + static Dictionary> s_TypeVisualFactories = new Dictionary>(); + + static Dictionary> s_TopicResponseVisualFactories = new Dictionary>(); + static Dictionary> s_TypeResponseVisualFactories = new Dictionary>(); + + 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 + { + RegisterTypeVisualizer(MessageRegistry.GetRosMessageName(), visualFactory, priority, subtopic); + } + + public static void RegisterTypeVisualizer(string rosMessageName, IVisualFactory visualFactory, int priority = 0, MessageSubtopic subtopic = MessageSubtopic.Default) + { + Dictionary> factoriesTable = (subtopic != MessageSubtopic.Response) ? s_TypeVisualFactories : s_TypeResponseVisualFactories; + PrioritizedList currentEntry; + if (!factoriesTable.TryGetValue(rosMessageName, out currentEntry)) + { + currentEntry = new PrioritizedList(); + currentEntry.Add(s_DefaultVisualFactory, int.MinValue); + factoriesTable[rosMessageName] = currentEntry; + } + currentEntry.Add(visualFactory, priority); + } + + public static void RegisterTopicVisualizer(string topic, IVisualFactory visualFactory, int priority = 0, MessageSubtopic subtopic = MessageSubtopic.Default) + { + if (topic == null) + Debug.Log("Registered null topic!"); + Dictionary> factoriesTable = (subtopic != MessageSubtopic.Response) ? s_TopicVisualFactories : s_TopicResponseVisualFactories; + PrioritizedList currentEntry; + if (!factoriesTable.TryGetValue(topic, out currentEntry)) + { + currentEntry = new PrioritizedList(); + factoriesTable[topic] = currentEntry; + } + currentEntry.Add(visualFactory, priority); + } + + public static IVisualFactory GetVisualFactory(string topic, string rosMessageName = null, MessageSubtopic subtopic = MessageSubtopic.Default) + { + PrioritizedList result; + Dictionary> topicsTable = (subtopic != MessageSubtopic.Response) ? s_TopicVisualFactories : s_TopicResponseVisualFactories; + topicsTable.TryGetValue(topic, out result); + if (result != null) + return result.Best; + + if (rosMessageName != null) + { + Dictionary> typesTable = (subtopic != MessageSubtopic.Response) ? s_TypeVisualFactories : s_TypeResponseVisualFactories; + typesTable.TryGetValue(rosMessageName, out result); + if (result != null) + return result.Best; + + if (MessageRegistry.GetDeserializeFunction(rosMessageName) != null) + return (subtopic != MessageSubtopic.Response) ? s_DefaultVisualFactory : s_DefaultResponseVisualFactory; + } + + return null; + } + + public static IEnumerable GetAllVisualFactories(string topic, string rosMessageName) + { + PrioritizedList result; + IEnumerable topicVisualizers = null; + if (topic != null) + { + s_TopicVisualFactories.TryGetValue(topic, out result); + if (result != null) + topicVisualizers = result.All; + } + + IEnumerable typeVisualizers = null; + if (rosMessageName != null) + { + s_TypeVisualFactories.TryGetValue(rosMessageName, out result); + if (result != null) + typeVisualizers = result.All; + else if (MessageRegistry.GetDeserializeFunction(rosMessageName) != null) + typeVisualizers = new IVisualFactory[] { s_DefaultVisualFactory }; + } + + if (topicVisualizers == null) + { + return typeVisualizers; + } + + if (typeVisualizers != null) + return topicVisualizers.Concat(typeVisualizers); + else + return topicVisualizers; + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/VisualFactoryRegistry.cs.meta b/com.unity.robotics.visualizations/Runtime/Scripts/VisualFactoryRegistry.cs.meta new file mode 100644 index 00000000..f9b5e170 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/VisualFactoryRegistry.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 498990d9703394d7890604669ce2cf4d +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/VisualizationLayoutTab.cs b/com.unity.robotics.visualizations/Runtime/Scripts/VisualizationLayoutTab.cs new file mode 100644 index 00000000..8b51a197 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/VisualizationLayoutTab.cs @@ -0,0 +1,44 @@ +using System.IO; +using System.Collections; +using Unity.Robotics.ROSTCPConnector; +using UnityEngine; +using UnityEditor; + +namespace Unity.Robotics.Visualizations +{ + public class VisualizationLayoutTab : IHudTab + { + string IHudTab.Label => "Layout"; + string m_LayoutPath; + VisualizationTopicsTab m_Topics; + + public VisualizationLayoutTab(VisualizationTopicsTab topics) + { + m_Topics = topics; + } + + void IHudTab.OnGUI(HudPanel hud) + { + // Save/Load layout files + GUILayout.BeginHorizontal(); + if (GUILayout.Button("Export layout")) + { +#if UNITY_EDITOR + m_LayoutPath = EditorUtility.SaveFilePanel("Save Visualizations Settings", "", "RosHudLayout", "json"); +#endif + m_Topics.SaveLayout(m_LayoutPath); + } + if (GUILayout.Button("Import layout")) + { +#if UNITY_EDITOR + m_LayoutPath = EditorUtility.OpenFilePanel("Select Visualizations Settings", "", "json"); +#endif + m_Topics.LoadLayout(m_LayoutPath); + } + GUILayout.EndHorizontal(); + } + + void IHudTab.OnSelected() { } + void IHudTab.OnDeselected() { } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/VisualizationLayoutTab.cs.meta b/com.unity.robotics.visualizations/Runtime/Scripts/VisualizationLayoutTab.cs.meta new file mode 100644 index 00000000..1987f6f7 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/VisualizationLayoutTab.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: aff7c974a44ec46aba7f7125ea52977b +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/VisualizationTopicsTab.cs b/com.unity.robotics.visualizations/Runtime/Scripts/VisualizationTopicsTab.cs new file mode 100644 index 00000000..1689389c --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/VisualizationTopicsTab.cs @@ -0,0 +1,254 @@ +using System.Collections; +using System.Collections.Generic; +using System.Linq; +using Unity.Robotics.ROSTCPConnector; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public class VisualizationTopicsTab : MonoBehaviour, IHudTab + { + ROSConnection m_Connection; + Dictionary m_Topics = new Dictionary(); + List m_TopicsSorted; + + string IHudTab.Label => "Topics"; + + string m_TopicFilter = ""; + + Vector2 m_TopicMenuScrollPosition; + + string LayoutFilePath => System.IO.Path.Combine(Application.persistentDataPath, "RosHudLayout.json"); + + enum SortMode + { + Normal, + v2D, + v3D, + Topic, + TopicDescending, + } + SortMode m_SortMode; + Texture2D m_FillTexture; + + public void Start() + { + m_FillTexture = VisualizationUtils.MakeTexture(16, 16, new Color(0.125f, 0.19f, 0.25f)); + + m_Connection = ROSConnection.GetOrCreateInstance(); + HudPanel.RegisterTab(this, (int)HudTabOrdering.Topics); + HudPanel.RegisterTab(new VisualizationLayoutTab(this), (int)HudTabOrdering.Layout); + LoadLayout(); + m_Connection.ListenForTopics(OnNewTopic, notifyAllExistingTopics: true); + } + + void OnNewTopic(RosTopicState state) + { + VisualizationTopicsTabEntry vis; + if (!m_Topics.TryGetValue(state.Topic, out vis)) + { + vis = new VisualizationTopicsTabEntry(state, m_FillTexture); + m_Topics.Add(state.Topic, vis); + m_TopicsSorted = null; + } + } + + + void IHudTab.OnGUI(HudPanel hud) + { + m_Connection.RefreshTopicsList(); + + GUILayout.BeginHorizontal(); + bool showPrompt = (GUI.GetNameOfFocusedControl() != "topic_filter" && m_TopicFilter == ""); + GUI.SetNextControlName("topic_filter"); + if (showPrompt) + { + Color oldCol = GUI.color; + GUI.color = new Color(oldCol.r, oldCol.g, oldCol.b, 0.5f); + GUILayout.TextField("(Type here to filter topics)"); + GUI.color = oldCol; + } + else + { + m_TopicFilter = GUILayout.TextField(m_TopicFilter).ToLower(); + } + + GUILayout.EndHorizontal(); + + GUILayout.BeginHorizontal(); + if (m_SortMode != SortMode.v2D) + GUILayout.Space(5); + string label2D = m_SortMode == SortMode.v2D ? "\u25BC2D" : "2D"; + if (GUILayout.Button(label2D, HudPanel.s_BoldStyle, GUILayout.Width(20))) + { + SetSortMode(m_SortMode == SortMode.v2D ? SortMode.Normal : SortMode.v2D); + } + if (m_SortMode == SortMode.v2D) + GUILayout.Space(5); + if (m_SortMode != SortMode.v3D) + GUILayout.Space(5); + string label3D = m_SortMode == SortMode.v3D ? "\u25BC3D" : "3D"; + if (GUILayout.Button(label3D, HudPanel.s_BoldStyle, GUILayout.Width(25))) + { + SetSortMode(m_SortMode == SortMode.v3D ? SortMode.Normal : SortMode.v3D); + } + if (m_SortMode == SortMode.v3D) + GUILayout.Space(5); + string labelTopic = m_SortMode == SortMode.Topic ? "\u25BCTopic" : m_SortMode == SortMode.TopicDescending ? "\u25B2Topic" : "Topic"; + if (GUILayout.Button(labelTopic, HudPanel.s_BoldStyle)) + { + if (m_SortMode == SortMode.TopicDescending) + SetSortMode(SortMode.Normal); + else if (m_SortMode == SortMode.Topic) + SetSortMode(SortMode.TopicDescending); + else + SetSortMode(SortMode.Topic); + } + GUILayout.EndHorizontal(); + + m_TopicMenuScrollPosition = GUILayout.BeginScrollView(m_TopicMenuScrollPosition); + var numTopicsShown = 0; + + if (m_TopicsSorted == null) + SortTopics(); + + foreach (VisualizationTopicsTabEntry topicState in m_TopicsSorted) + { + if (m_TopicFilter != "" && !topicState.Topic.ToLower().Contains(m_TopicFilter) && !topicState.RosMessageName.ToLower().Contains(m_TopicFilter)) + continue; + + numTopicsShown++; + topicState.DrawGUI(); + } + + GUILayout.EndScrollView(); + + if (numTopicsShown == 0) + { + if (!m_Connection.AllTopics.Any()) + GUILayout.Label("No topics registered"); + else + GUILayout.Label($"No topics named \"{m_TopicFilter}\"!"); + } + } + + void SetSortMode(SortMode sortMode) + { + m_SortMode = sortMode; + m_TopicsSorted = null; + } + + void SortTopics() + { + m_TopicsSorted = m_Topics.Values.ToList(); + switch (m_SortMode) + { + case SortMode.v2D: + m_TopicsSorted.Sort( + (VisualizationTopicsTabEntry a, VisualizationTopicsTabEntry b) => + { + int primary = b.CanShowWindow.CompareTo(a.CanShowWindow); + return (primary != 0) ? primary : b.IsVisualizingUI.CompareTo(a.IsVisualizingUI); + } + ); + break; + case SortMode.v3D: + m_TopicsSorted.Sort( + (VisualizationTopicsTabEntry a, VisualizationTopicsTabEntry b) => + { + int primary = b.CanShowWindow.CompareTo(a.CanShowWindow); + if (primary != 0) + return primary; + int secondary = b.CanShowDrawing.CompareTo(a.CanShowDrawing); + return (secondary != 0) ? secondary : b.IsVisualizingDrawing.CompareTo(a.IsVisualizingDrawing); + } + ); + break; + case SortMode.Topic: + m_TopicsSorted.Sort( + (VisualizationTopicsTabEntry a, VisualizationTopicsTabEntry b) => a.Topic.CompareTo(b.Topic) + ); + break; + case SortMode.TopicDescending: + m_TopicsSorted.Sort( + (VisualizationTopicsTabEntry a, VisualizationTopicsTabEntry b) => b.Topic.CompareTo(a.Topic) + ); + break; + default: + break; + } + } + + public void OnSelected() { } + public void OnDeselected() { } + + class HUDLayoutSave + { + public VisualizationTopicsTabEntry.SaveState[] Rules; + + public void AddRules(IEnumerable rules) + { + var topicRuleSaves = new List(); + foreach (var rule in rules) + { + if (rule == null) + continue; + var save = rule.CreateSaveState(); + if (save != null) + topicRuleSaves.Add(save); + } + + Rules = topicRuleSaves.ToArray(); + } + } + + public void SaveLayout(string path = "") + { + // Print filepath if saving to user-input path; default to persistentDataPath + if (path.Length > 0) + { + Debug.Log($"Saved visualizations layout to {path}"); + } + else + { + path = LayoutFilePath; + } + + HUDLayoutSave saveState = new HUDLayoutSave { }; + saveState.AddRules(m_Topics.Values); + System.IO.File.WriteAllText(path, JsonUtility.ToJson(saveState)); + } + + public void LoadLayout(string path = "") + { + if (path.Length > 0) + { + Debug.Log($"Loaded visualizations layout from {path}"); + } + else + { + path = LayoutFilePath; + } + + if (System.IO.File.Exists(path)) + { + LoadLayout(JsonUtility.FromJson(System.IO.File.ReadAllText(path))); + } + } + + 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); + } + } + + void OnApplicationQuit() + { + SaveLayout(); + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/VisualizationTopicsTab.cs.meta b/com.unity.robotics.visualizations/Runtime/Scripts/VisualizationTopicsTab.cs.meta new file mode 100644 index 00000000..9f27f1b7 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/VisualizationTopicsTab.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: e120188e851b6f44b85f38aecc703ba6 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/VisualizationTopicsTabEntry.cs b/com.unity.robotics.visualizations/Runtime/Scripts/VisualizationTopicsTabEntry.cs new file mode 100644 index 00000000..34f6de55 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/VisualizationTopicsTabEntry.cs @@ -0,0 +1,314 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using System.Linq; +using Unity.Robotics.ROSTCPConnector; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + // Represents a single line in the VisualizationTopicsTab + // and saves and loads the options for that line, plus the associated hud windows etc. + public class VisualizationTopicsTabEntry + { + RosTopicState m_TopicState; + public RosTopicState TopicState => m_TopicState; + public string Topic => m_TopicState.Topic; + public MessageSubtopic Subtopic => m_TopicState.Subtopic; + public string RosMessageName => m_TopicState.RosMessageName; + public string Title => Topic + (Subtopic == MessageSubtopic.Response ? " (response)" : ""); + + // a service topic is represented by two lines, one for the request and one for the response. m_ServiceResponseTopic is the response. + VisualizationTopicsTabEntry m_ServiceResponseTopic; + Texture2D m_Background; + GUIStyle m_LineStyle; + GUIStyle m_HoverStyle; + + bool m_IsVisualizingUI; + public bool IsVisualizingUI => m_IsVisualizingUI; + + bool m_IsVisualizingDrawing; + public bool IsVisualizingDrawing => m_IsVisualizingDrawing; + + string m_CachedRosMessageName; + + IVisualFactory m_VisualFactory; + bool m_NoVisualFactoryAvailable; + + IVisual m_Visual; + public IVisual Visual => m_Visual; + HudWindow m_VisualWindow; + bool m_IsHovering; + + public IVisualFactory GetVisualFactory() + { + if (m_CachedRosMessageName != RosMessageName) + { + // if the topic has changed, discard our cached data + m_VisualFactory = null; + m_NoVisualFactoryAvailable = false; + } + + if (m_VisualFactory == null && !m_NoVisualFactoryAvailable) + { + SetVisualFactory(VisualFactoryRegistry.GetVisualFactory(Topic, RosMessageName, Subtopic)); + } + return m_VisualFactory; + } + + public void SetVisualFactory(IVisualFactory visualFactory) + { + if (m_Visual != null) + m_Visual.SetDrawingEnabled(false); + m_Visual = null; + + m_VisualFactory = visualFactory; + m_CachedRosMessageName = RosMessageName; + if (m_VisualFactory == null) + m_NoVisualFactoryAvailable = true; + + SetVisualizing(m_IsVisualizingUI, m_IsVisualizingDrawing); + } + + public bool CanShowWindow => GetVisualFactory() != null; + public bool CanShowDrawing => GetVisualFactory() != null && m_VisualFactory.CanShowDrawing; + + public void DrawGUI() + { + DrawGUILine(); + + if (m_ServiceResponseTopic != null) + { + m_ServiceResponseTopic.DrawGUILine(); + } + } + + public void DrawGUILine() + { + bool showWindow = IsVisualizingUI; + bool showDrawing = IsVisualizingDrawing; + + bool canShowWindow = CanShowWindow; + bool canShowDrawing = CanShowDrawing; + + var hasWindow = showWindow; + var hasDrawing = showDrawing; + + if (m_LineStyle == null) + InitLineStyle(); + + GUILayout.BeginHorizontal(m_IsHovering ? m_HoverStyle : m_LineStyle); + if (hasWindow || canShowWindow) + showWindow = GUILayout.Toggle(showWindow, "", GUILayout.Width(15)); + else + GUILayout.Label("", GUILayout.Width(15)); + + if (hasDrawing || canShowDrawing) + showDrawing = GUILayout.Toggle(showDrawing, "", GUILayout.Width(15)); + else + GUILayout.Label("", GUILayout.Width(15)); + + var baseColor = GUI.color; + GUI.color = canShowWindow ? baseColor : Color.grey; + + GUILayout.Space(40); + Rect space = GUILayoutUtility.GetLastRect(); + ROSConnection.DrawConnectionArrows(false, + space.x + 5, space.y, + Time.realtimeSinceStartup - TopicState.LastMessageReceivedRealtime, + Time.realtimeSinceStartup - TopicState.LastMessageSentRealtime, + TopicState.IsPublisher, + TopicState.SentSubscriberRegistration, + m_TopicState.Connection.HasConnectionError); + + if (GUILayout.Button(Title, m_LineStyle, GUILayout.Width(240))) + { + if (!canShowWindow) + { + Debug.LogError($"No message class registered for type {RosMessageName}"); + } + else if (!canShowDrawing) + { + showWindow = !showWindow; + } + else + { + var toggleOn = !showWindow || !showDrawing; + showWindow = toggleOn; + showDrawing = toggleOn; + } + } + + GUI.color = baseColor; + GUILayout.EndHorizontal(); + + Rect horizontalRect = GUILayoutUtility.GetLastRect(); + +#if UNITY_EDITOR + Rect buttonRect = new Rect(horizontalRect.xMax - 20, horizontalRect.center.y - 10, 20, 20); + if (GUI.Button(buttonRect, "\u2630")) + ShowOptionsMenu(buttonRect); +#endif + + if (m_IsHovering) + { + string labelText = RosMessageName; + Vector2 labelSize = GUI.skin.box.CalcSize(new GUIContent(labelText)); + + GUI.Box(new Rect(horizontalRect.xMax - labelSize.x, horizontalRect.yMax - 10, labelSize.x, horizontalRect.height), labelText); + } + + if (Event.current.type == EventType.Repaint) + m_IsHovering = horizontalRect.Contains(Event.current.mousePosition); + + if (showDrawing != m_IsVisualizingDrawing || showWindow != m_IsVisualizingUI) + { + SetVisualizing(showWindow, showDrawing); + } + } + + public void SetVisualizing(bool ui, bool drawing) + { + if (m_VisualWindow != null) + { + m_VisualWindow.SetActive(ui); + } + else if (ui) + { + m_VisualWindow = new HudWindow(Title); + HudPanel.AddWindow(m_VisualWindow); + } + + if ((ui || drawing) && m_Visual == null) + { + m_Visual = GetVisualFactory().GetOrCreateVisual(Topic); + } + + if (m_Visual != null) + { + m_Visual.SetDrawingEnabled(drawing); + + if (m_VisualWindow != null) + { + m_VisualWindow.SetOnGUI(m_Visual.OnGUI); + } + } + + m_IsVisualizingUI = ui; + m_IsVisualizingDrawing = drawing; + } + + [Serializable] + public class SaveState + { + public string Topic; + public string RosMessageName; + public string VisualizerID; + public Rect Rect; + public bool HasRect; + public bool ShowWindow; + public bool ShowDrawing; + } + + public VisualizationTopicsTabEntry(RosTopicState baseState, Texture2D background) + { + m_TopicState = baseState; + m_Background = background; + + if (baseState.ServiceResponseTopic != null) + { + m_ServiceResponseTopic = new VisualizationTopicsTabEntry(baseState.ServiceResponseTopic, background); + } + + m_CachedRosMessageName = RosMessageName; + } + + internal VisualizationTopicsTabEntry(SaveState save, RosTopicState topicState, Texture2D background) + { + m_TopicState = topicState; + m_Background = background; + + if (save != null) + { + if (save.VisualizerID != null) + { + m_VisualFactory = VisualFactoryRegistry.GetAllVisualFactories(Topic, RosMessageName).FirstOrDefault(v => v.ID == save.VisualizerID); + m_CachedRosMessageName = RosMessageName; + } + + 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); + } + + if (m_VisualWindow != null) + { + HudPanel.AddWindow(m_VisualWindow); + } + + SetVisualizing(save.ShowWindow, save.ShowDrawing); + } + } + + void InitLineStyle() + { + m_LineStyle = new GUIStyle(GUI.skin.label); + m_LineStyle.hover.textColor = Color.white; + + m_HoverStyle = new GUIStyle(GUI.skin.label); + m_HoverStyle.hover.textColor = Color.white; + m_HoverStyle.hover.background = m_Background; + m_HoverStyle.normal.background = m_Background; + } + + public SaveState CreateSaveState() + { + if (!IsVisualizingUI && !IsVisualizingDrawing) + { + return null; + } + + return new SaveState + { + 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, + VisualizerID = m_VisualFactory.ID, + ShowWindow = m_IsVisualizingUI, + ShowDrawing = m_IsVisualizingDrawing, + }; + } + +#if UNITY_EDITOR + void ShowOptionsMenu(Rect position) + { + UnityEditor.GenericMenu menu = new UnityEditor.GenericMenu(); + foreach(IVisualFactory factory in VisualFactoryRegistry.GetAllVisualFactories(Topic, RosMessageName)) + { + bool isSelected = GetVisualFactory() == factory; + menu.AddItem(new GUIContent(isSelected? factory.Name+" (main)": factory.Name), false, () => OnSelect(factory)); + } + menu.DropDown(position); + } + + static Action s_OpenWindowCallback; + + public static void SetOpenWindowCallback(Action callback) + { + s_OpenWindowCallback = callback; + } + + void OnSelect(IVisualFactory factory) + { + if (s_OpenWindowCallback != null) + s_OpenWindowCallback(factory, Topic, RosMessageName); + } +#endif + } +} diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/VisualizationTopicsTabEntry.cs.meta b/com.unity.robotics.visualizations/Runtime/Scripts/VisualizationTopicsTabEntry.cs.meta new file mode 100644 index 00000000..37ff261e --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/VisualizationTopicsTabEntry.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 1c62852de58cdea4185cd230fbe8775b +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/VisualizationUtils.cs b/com.unity.robotics.visualizations/Runtime/Scripts/VisualizationUtils.cs new file mode 100644 index 00000000..78bc225e --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/VisualizationUtils.cs @@ -0,0 +1,551 @@ +using RosMessageTypes.Actionlib; +using RosMessageTypes.Diagnostic; +using RosMessageTypes.Geometry; +using RosMessageTypes.Nav; +using RosMessageTypes.Sensor; +using RosMessageTypes.Shape; +using RosMessageTypes.Std; +using RosMessageTypes.Visualization; +using System; +using System.Collections.Generic; +using System.Linq; +using RosMessageTypes.BuiltinInterfaces; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; +using Unity.Robotics.ROSTCPConnector; + +namespace Unity.Robotics.Visualizations +{ + public static class VisualizationUtils + { + public static Color SelectColor(Color userColor, MessageMetadata meta) + { + if (userColor.r == 0 && userColor.g == 0 && userColor.b == 0) + return PickColorForTopic(meta.Topic); + + if (userColor.a == 0) + return new Color(userColor.r, userColor.g, userColor.b, 1); + + return userColor; + } + + public static string SelectLabel(string userLabel, MessageMetadata meta) + { + if (string.IsNullOrEmpty(userLabel)) + return meta.Topic; + + return userLabel; + } + + public static Action CreateDefaultGUI(Message message, MessageMetadata meta) + { + string text = message.ToString(); + return () => { GUILayout.Label(text); }; + } + + public static Color32 PickColorForTopic(string topic) + { + if (topic == "") + return Color.black; + + byte[] bytes = BitConverter.GetBytes(topic.GetHashCode()); + return new Color32(bytes[0], bytes[1], bytes[2], 255); + } + + public static Texture2D MakeTexture(int width, int height, Color color) + { + Color[] pix = new Color[16 * 16]; + + for (int i = 0; i < pix.Length; i++) + pix[i] = color; + + Texture2D result = new Texture2D(width, height); + result.SetPixels(pix); + result.Apply(); + return result; + } + + public static bool AssertMessageType(Message message, string topic) + { + if (!(message is T)) + { + Debug.LogError($"Topic \"{topic}\": Can't visualize a message of type {message.GetType()}! (expected {typeof(T)})."); + return false; + } + + return true; + } + + public static IVisual GetVisual(string topic, MessageSubtopic subtopic = MessageSubtopic.Default) + { + RosTopicState topicState = ROSConnection.GetOrCreateInstance().GetTopic(topic); + if (topicState != null && subtopic == MessageSubtopic.Response) + topicState = topicState.ServiceResponseTopic; + + if (topicState == null) + return null; + + IVisualFactory factory = VisualFactoryRegistry.GetVisualFactory(topic, topicState.RosMessageName, subtopic); + if (factory == null) + return null; + + return factory.GetOrCreateVisual(topic); + } + + public static IVisual GetVisual(string topic, string rosMessageName, MessageSubtopic subtopic) + { + IVisualFactory factory = VisualFactoryRegistry.GetVisualFactory(topic, rosMessageName, subtopic); + if (factory == null) + return null; + + return factory.GetOrCreateVisual(topic); + } + + public static void DrawAxisVectors(Drawing3d drawing, Vector3Msg position, QuaternionMsg rotation, float size, bool drawUnityAxes) where C : ICoordinateSpace, new() + { + Vector3 unityPosition = position.From(); + Quaternion unityRotation = rotation.From(); + Vector3 x, y, z; + if (drawUnityAxes) + { + x = unityRotation * new Vector3(1, 0, 0) * size; + y = unityRotation * new Vector3(0, 1, 0) * size; + z = unityRotation * new Vector3(0, 0, 1) * size; + } + else + { + x = unityRotation * new Vector3(1, 0, 0).toUnity * size; + y = unityRotation * new Vector3(0, 1, 0).toUnity * size; + z = unityRotation * new Vector3(0, 0, 1).toUnity * size; + } + drawing.DrawLine(unityPosition, unityPosition + x, Color.red, size * 0.1f); + drawing.DrawLine(unityPosition, unityPosition + y, Color.green, size * 0.1f); + drawing.DrawLine(unityPosition, unityPosition + z, Color.blue, size * 0.1f); + } + + public static void DrawPointCloud(PointMsg[] points, Drawing3d drawing, Color color, float radius = 0.01f) + where C : ICoordinateSpace, new() + { + PointCloudDrawing pointCloud = drawing.AddPointCloud(points.Length); + foreach (PointMsg p in points) + pointCloud.AddPoint(p.From(), color, radius); + pointCloud.Bake(); + } + + public static void DrawPointCloud(Point32Msg[] points, Drawing3d drawing, Color color, float radius = 0.01f) + where C : ICoordinateSpace, new() + { + PointCloudDrawing pointCloud = drawing.AddPointCloud(points.Length); + foreach (Point32Msg p in points) + pointCloud.AddPoint(p.From(), color, radius); + pointCloud.Bake(); + } + + public static void DrawAngularVelocityArrow(Drawing3d drawing, Vector3 angularVelocity, Vector3 sphereCenter, Color32 color, float sphereRadius = 1.0f, float arrowThickness = 0.01f) + { + DrawRotationArrow(drawing, angularVelocity.normalized, angularVelocity.magnitude * Mathf.Rad2Deg, sphereCenter, color, sphereRadius, arrowThickness); + } + + public static void DrawRotationArrow(Drawing3d drawing, Quaternion rotation, Vector3 sphereCenter, Color32 color, float sphereRadius = 1.0f, float arrowThickness = 0.01f) + { + Vector3 axis; + float angleDegrees; + rotation.ToAngleAxis(out angleDegrees, out axis); + DrawRotationArrow(drawing, axis, angleDegrees, sphereCenter, color, sphereRadius, arrowThickness); + } + + public static void DrawRotationArrow(Drawing3d drawing, Vector3 rotationAxis, float rotationDegrees, Vector3 sphereCenter, Color32 color, float sphereRadius = 1.0f, float arrowThickness = 0.01f) + { + Vector3 startVector = Vector3.Cross(Vector3.up, rotationAxis); + if (startVector.sqrMagnitude < 0.01f) + startVector = Vector3.forward; + + float degreesPerStep = 10.0f; // approximately + int numSteps = Mathf.Max((int)(rotationDegrees / degreesPerStep), 2); + float pushOutPerStep = 0; + if (rotationDegrees > 360) + { + float pushOutFinal = arrowThickness * 6 * rotationDegrees / 360; + pushOutPerStep = pushOutFinal / numSteps; + } + + Quaternion deltaRotation = Quaternion.AngleAxis(rotationDegrees / (float)numSteps, rotationAxis); + List points = new List(); + + Quaternion currentRotation = Quaternion.LookRotation(startVector); + for (int step = 0; step < numSteps; ++step) + { + points.Add(sphereCenter + currentRotation * Vector3.forward * sphereRadius); + currentRotation = deltaRotation * currentRotation; + sphereRadius += pushOutPerStep; + } + + drawing.DrawLineStrip(points.ToArray(), color, arrowThickness); + drawing.DrawArrow(points[points.Count - 1], sphereCenter + currentRotation * Vector3.forward * sphereRadius, color, arrowThickness); + } + + public static void GUI(this AccelMsg message) + { + message.linear.GUI("Linear"); + message.angular.GUI("Angular"); + } + + public static void GUI(this ColorRGBAMsg message, bool withText = true) + { + Color oldBackgroundColor = UnityEngine.GUI.color; + + GUILayout.BeginHorizontal(); + UnityEngine.GUI.backgroundColor = message.ToUnityColor(); + GUILayout.Box("", s_ColorSwatchStyle); + UnityEngine.GUI.backgroundColor = oldBackgroundColor; + if (withText) + GUILayout.Label($"R{message.r} G{message.g} B{message.b} A{message.a}"); + GUILayout.EndHorizontal(); + } + + public static void GUI(this HeaderMsg message) + { +#if !ROS2 + GUILayout.Label($"<{message.seq} {message.frame_id} {message.stamp.ToTimestampString()}>"); +#else + GUILayout.Label($"<{message.frame_id} {message.stamp.ToTimestampString()}>"); +#endif + + } + + public static void GUITexture(this Texture2D tex) + { + // TODO: Rescale/recenter image based on window height/width + if (tex == null) + return; + + var origRatio = tex.width / (float)tex.height; + UnityEngine.GUI.Box(GUILayoutUtility.GetAspectRect(origRatio), tex); + } + + public static void GUITexture(this Texture2D tex, Material material) + { + if (tex == null) + return; + var origRatio = tex.width / (float)tex.height; + Rect textureRect = GUILayoutUtility.GetAspectRect(origRatio); + if (Event.current.type == EventType.Repaint) + { + Graphics.DrawTexture(textureRect, tex, material); + } + else + { + UnityEngine.GUI.Box(textureRect, tex); + } + } + + public static void GUI(this InertiaMsg message) + { + GUILayout.Label($"Mass: {message.m}kg"); + message.com.GUI(); + GUILayout.BeginHorizontal(); + GUILayout.Label(message.ixx.ToString()); + GUILayout.Label(message.ixy.ToString()); + GUILayout.Label(message.ixz.ToString()); + GUILayout.EndHorizontal(); + GUILayout.BeginHorizontal(); + GUILayout.Label(message.ixy.ToString()); + GUILayout.Label(message.iyy.ToString()); + GUILayout.Label(message.iyz.ToString()); + GUILayout.EndHorizontal(); + GUILayout.BeginHorizontal(); + GUILayout.Label(message.ixz.ToString()); + GUILayout.Label(message.iyz.ToString()); + GUILayout.Label(message.izz.ToString()); + GUILayout.EndHorizontal(); + } + + public static void GUI(this JoyFeedbackMsg message) + { + GUILayout.Label($"Type: {(JoyFeedback_Type_Constants)message.type}\nID: {message.id}\nIntensity: {message.intensity}"); + } + + public static void GUI(this MapMetaDataMsg message) + { + GUILayout.Label($"Load time: {message.map_load_time.ToTimestampString()}"); + GUILayout.Label($"Resolution: {message.resolution}"); + GUILayout.Label($"Size: {message.width}x{message.height}"); + message.origin.GUI(); + } + + public static void GUI(this MeshMsg message) + { + foreach (PointMsg p in message.vertices) + p.GUI(); + foreach (MeshTriangleMsg tri in message.triangles) + tri.GUI(); + } + + public static void GUI(this MeshTriangleMsg message) + { + string text = "[" + String.Join(", ", message.vertex_indices.Select(i => i.ToString()).ToArray()) + "]"; + GUILayout.Label(text); + } + + public static void GUI(this NavSatStatusMsg message) + { + GUILayout.Label($"Status: {(NavSatStatus_Type_Constants)message.status}\nService: {(NavSatStatus_Service_Constants)message.service}"); + } + + public static void GUI(this PointMsg message, string name) + { + string body = $"[{message.x:F2}, {message.y:F2}, {message.z:F2}]"; + if (name == null || name == "") + GUILayout.Label(body); + else + GUILayout.Label($"{name}: {body}"); + } + + public static void GUI(this PointMsg message) + { + GUILayout.Label($"[{message.x:F2}, {message.y:F2}, {message.z:F2}]"); + } + + public static void GUI(this Point32Msg message, string name) + { + string body = $"[{message.x:F2}, {message.y:F2}, {message.z:F2}]"; + if (name == null || name == "") + GUILayout.Label(body); + else + GUILayout.Label($"{name}: {body}"); + } + + public static void GUI(this Point32Msg message) + { + GUILayout.Label($"[{message.x:F2}, {message.y:F2}, {message.z:F2}]"); + } + + public static void GUI(this PointFieldMsg message) + { + GUILayout.Label($"Name: {message.name}\nDatatype: {(PointField_Format_Constants)message.datatype}"); + if (message.count > 1) + GUILayout.Label($"Count: {message.count}"); + } + + public static void GUI(this PolygonMsg message) + { + GUILayout.Label($"({message.points.Length} points):"); + foreach (Point32Msg p in message.points) + GUI(p); + } + + public static void GUI(this PoseMsg message, string name = "") + { + if (name.Length > 0) + { + GUILayout.Label(name); + } + message.position.GUI("Position"); + message.orientation.GUI("Orientation"); + } + + public static void GUI(this PoseArrayMsg message) + { + GUI(message.header); + for (int Idx = 0; Idx < message.poses.Length; ++Idx) + { + PoseMsg pose = message.poses[Idx]; + pose.position.GUI($"[{Idx}] Position"); + pose.orientation.GUI("Orientation"); + } + } + + public static void GUI(this QuaternionMsg message, string label) + { + if (label != "" && label != null) + label += ": "; + GUILayout.Label($"{label}[{message.x:F2}, {message.y:F2}, {message.z:F2}, {message.w:F2}]"); + } + + public static void GUI(this QuaternionMsg message) + { + GUILayout.Label($"[{message.x:F2}, {message.y:F2}, {message.z:F2}, {message.w:F2}]"); + } + + public static void GUI(this RegionOfInterestMsg message, Texture2D tex = null) + { + if (tex != null) + { + var ratio = (float)tex.width / (float)tex.height; + UnityEngine.GUI.Box(GUILayoutUtility.GetAspectRect(ratio), tex); + } + GUILayout.Label($"x_offset: {message.x_offset}\ny_offset: {message.y_offset}\nHeight: {message.height}\nWidth: {message.width}\nDo rectify: {message.do_rectify}"); + } + + public static void GUI(this SelfTestResponse message) + { + string pass = message.passed != 0 ? "OK" : "FAIL"; + GUILayout.Label($"Self test {message.id}: {pass}"); + foreach (DiagnosticStatusMsg status in message.status) + DiagnosticStatusDefaultVisualizer.GUI(status); + } + + public static void GUI(this SolidPrimitiveMsg message) + { + switch (message.type) + { + case SolidPrimitiveMsg.BOX: + GUILayout.Label($"SolidPrimitive BOX\n[X:{message.dimensions[SolidPrimitiveMsg.BOX_X]}, Y:{message.dimensions[SolidPrimitiveMsg.BOX_Y]}, Z:{message.dimensions[SolidPrimitiveMsg.BOX_Z]}]"); + break; + case SolidPrimitiveMsg.SPHERE: + GUILayout.Label($"SolidPrimitive SPHERE\nRadius: {message.dimensions[SolidPrimitiveMsg.SPHERE_RADIUS]}"); + break; + case SolidPrimitiveMsg.CYLINDER: + GUILayout.Label($"SolidPrimitive CYLINDER\nHeight: {message.dimensions[SolidPrimitiveMsg.CYLINDER_HEIGHT]}\nRadius: {message.dimensions[SolidPrimitiveMsg.CYLINDER_RADIUS]}"); + break; + case SolidPrimitiveMsg.CONE: + GUILayout.Label($"SolidPrimitive CONE\nHeight: {message.dimensions[SolidPrimitiveMsg.CONE_HEIGHT]}\nRadius: {message.dimensions[SolidPrimitiveMsg.CONE_RADIUS]}"); + break; + default: + GUILayout.Label($"INVALID shape {message.type}!?"); + break; + } + } + + public static void GUI(this TimeMsg message) + { + GUILayout.Label(message.ToTimestampString()); + } + + public static void GUI(this TransformMsg message) + { + message.translation.GUI("Translation"); + message.rotation.GUI("Rotation"); + } + + public static void GUI(this TwistMsg message, string name = "") + { + if (name.Length > 0) + { + GUILayout.Label(name); + } + message.linear.GUI("Linear"); + message.angular.GUI("Angular"); + } + + public static void GUI(this Vector3Msg message, string name) + { + string body = $"[{message.x:F2}, {message.y:F2}, {message.z:F2}]"; + if (name == null || name == "") + GUILayout.Label(body); + else + GUILayout.Label($"{name}: {body}"); + } + + public static void GUI(this Vector3Msg message) + { + GUILayout.Label($"[{message.x:F2}, {message.y:F2}, {message.z:F2}]"); + } + + public static void GUI(this WrenchMsg message) + { + message.force.GUI("Force"); + message.torque.GUI("Torque"); + } + + public static void GUIGrid(T[] data, int width, ref bool view) + { + view = GUILayout.Toggle(view, "View matrix"); + if (!view) return; + + int dataIndex = 0; + while (dataIndex < data.Length) + { + GUILayout.BeginHorizontal(); + for (int Idx = 0; Idx < width && dataIndex < data.Length; ++Idx) + { + GUILayout.Label(data[dataIndex].ToString()); + dataIndex++; + } + GUILayout.EndHorizontal(); + } + } + + public static void GUIGrid(T[] data, int width, string name, ref bool view) + { + view = GUILayout.Toggle(view, $"View {name} matrix"); + if (!view) return; + + int dataIndex = 0; + GUILayout.Label(name); + while (dataIndex < data.Length) + { + GUILayout.BeginHorizontal(); + for (int Idx = 0; Idx < width && dataIndex < data.Length; ++Idx) + { + GUILayout.Label(data[dataIndex].ToString()); + dataIndex++; + } + GUILayout.EndHorizontal(); + } + } + + static readonly GUIStyle s_ColorSwatchStyle = new GUIStyle + { + normal = new GUIStyleState { background = Texture2D.whiteTexture }, + fixedWidth = 25, + fixedHeight = 25 + }; + + public static void GUIMultiArray(this MultiArrayLayoutMsg layout, T[] data, ref bool tabulate) + { + tabulate = GUILayout.Toggle(tabulate, "Table view"); + GUIMultiArray(layout, data, tabulate); + } + + static GUIStyle s_ArrayContainerStyle; + + public static void GUIMultiArray(this MultiArrayLayoutMsg layout, T[] data, bool tabulate = true) + { + if (s_ArrayContainerStyle == null) + { + s_ArrayContainerStyle = UnityEngine.GUI.skin.GetStyle("box"); + } + GUIMultiArrayLevel(data, layout, layout.data_offset, 0, tabulate); + } + + static void GUIMultiArrayLevel(T[] data, MultiArrayLayoutMsg layout, uint dataIndex, int depth, bool tabulate) + { + uint size = layout.dim[depth].size; + if (layout.dim.Length > depth + 1) + { + uint stride = layout.dim[depth + 1].stride; + if (depth > 0) + GUILayout.BeginVertical(s_ArrayContainerStyle); + for (int step = 0; step < size; ++step) + { + GUIMultiArrayLevel(data, layout, dataIndex, depth + 1, tabulate); + dataIndex += stride; + } + if (depth > 0) + GUILayout.EndVertical(); + } + else if (tabulate) + { + GUILayout.BeginHorizontal(); + for (int step = 0; step < size; ++step) + { + GUILayout.Label(data[dataIndex].ToString()); + dataIndex++; + } + GUILayout.EndHorizontal(); + } + else + { + GUILayout.BeginVertical(s_ArrayContainerStyle); + for (int step = 0; step < size; ++step) + { + GUILayout.Label(data[dataIndex].ToString()); + dataIndex++; + } + GUILayout.EndVertical(); + } + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/VisualizationUtils.cs.meta b/com.unity.robotics.visualizations/Runtime/Scripts/VisualizationUtils.cs.meta new file mode 100644 index 00000000..bc3848b2 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/VisualizationUtils.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 9959da3268082486084331c1cae4afca +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/VisualizerSettingsGeneric.cs b/com.unity.robotics.visualizations/Runtime/Scripts/VisualizerSettingsGeneric.cs new file mode 100644 index 00000000..38027cbd --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/VisualizerSettingsGeneric.cs @@ -0,0 +1,25 @@ +using System; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using UnityEngine; + +namespace Unity.Robotics.Visualizations +{ + public class VisualizerSettingsGeneric : ScriptableObject + where T : Message + { + public string RosMessageName => MessageRegistry.GetRosMessageName(); + + [SerializeField] + protected TFTrackingSettings m_TFTrackingSettings = new TFTrackingSettings { type = TFTrackingType.Exact, tfTopic = "/tf" }; + public TFTrackingSettings TFTrackingSettings { get => m_TFTrackingSettings; set => m_TFTrackingSettings = value; } + + public virtual Action CreateGUI(T message, MessageMetadata meta) + { + return null; + } + + public virtual void Draw(Drawing3d drawing, T message, MessageMetadata meta) + { + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/Scripts/VisualizerSettingsGeneric.cs.meta b/com.unity.robotics.visualizations/Runtime/Scripts/VisualizerSettingsGeneric.cs.meta new file mode 100644 index 00000000..50a6acc1 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Scripts/VisualizerSettingsGeneric.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 1d15a326082f3ac4d87807c14116e32e +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Shaders.meta b/com.unity.robotics.visualizations/Runtime/Shaders.meta new file mode 100644 index 00000000..6572b072 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Shaders.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 97856a5dc6c298445abfcf7b6ba701a0 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Shaders/ImageMsg.shader b/com.unity.robotics.visualizations/Runtime/Shaders/ImageMsg.shader new file mode 100644 index 00000000..6e72c2bc --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Shaders/ImageMsg.shader @@ -0,0 +1,71 @@ +Shader "Unlit/ImageMsg" +{ + Properties + { + _MainTex("Texture", 2D) = "white" {} + [Toggle] _convertBGR("convert BGR", Float) = 0 + [Toggle] _flipY("Flip Y", Float) = 1 + [Toggle] _gray("Gray", Float) = 0 + } + + SubShader + { + Tags { "Queue"="Overlay" "IgnoreProjector"="True" "RenderType"="Transparent" } + Lighting Off + Cull Off + ZTest Always + ZWrite Off + Fog { Mode Off } + + Pass + { + CGPROGRAM + #pragma vertex vert + #pragma fragment frag + + #include "UnityCG.cginc" + + sampler2D _MainTex; + float4 _MainTex_ST; + float _convertBGR; + float _flipY; + float _gray; + + struct appdata + { + float4 vertex : POSITION; + float2 uv : TEXCOORD0; + }; + + struct v2f + { + float4 vertex : SV_POSITION; + float2 uv : TEXCOORD0; + }; + + v2f vert(appdata v) + { + v2f o; + o.vertex = UnityObjectToClipPos( + float4(v.vertex.x, v.vertex.y, v.vertex.z, 0) + ); + o.uv = _flipY == 0? v.uv: float2(v.uv.x, 1-v.uv.y); + return o; + } + + fixed4 frag (v2f i) : SV_Target + { + fixed4 color = tex2D(_MainTex, i.uv); + if(_gray > 0.5) + color = color.rrra; + else if(_convertBGR > 0.5) + color = color.bgra; + color.a = 1; + return color; + } + + ENDCG + } + + } +} diff --git a/com.unity.robotics.visualizations/Runtime/Shaders/ImageMsg.shader.meta b/com.unity.robotics.visualizations/Runtime/Shaders/ImageMsg.shader.meta new file mode 100644 index 00000000..caa52843 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Shaders/ImageMsg.shader.meta @@ -0,0 +1,10 @@ +fileFormatVersion: 2 +guid: 79210c8cd34c373409ebb5855746ef15 +ShaderImporter: + externalObjects: {} + defaultTextures: [] + nonModifiableTextures: [] + preprocessorOverride: 0 + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Shaders/UnlitDisparityImage.shader b/com.unity.robotics.visualizations/Runtime/Shaders/UnlitDisparityImage.shader new file mode 100644 index 00000000..19061c1f --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Shaders/UnlitDisparityImage.shader @@ -0,0 +1,63 @@ +Shader "Unlit/DisparityImage" +{ + Properties + { + _MainTex("Texture", 2D) = "white" {} + _MinDisparity("Min Disparity", float) = 1 + _MaxDisparity("Max Disparity", float) = 10 + _Color0 ("Color 0", Color) = (1,1,1,1) + _Color100 ("Color 100", Color) = (0,0,0,1) + } + SubShader + { + Tags {"Queue" = "Transparent" "IgnoreProjector" = "True" "RenderType"="Transparent" } + LOD 100 + ZWrite Off + Blend SrcAlpha OneMinusSrcAlpha + + Pass + { + CGPROGRAM + #pragma vertex vert + #pragma fragment frag + + #include "UnityCG.cginc" + + float _MinDisparity; + float _MaxDisparity; + fixed4 _Color0; + fixed4 _Color100; + + sampler2D _MainTex; + float4 _MainTex_ST; + + struct appdata + { + float4 vertex : POSITION; + float2 uv : TEXCOORD0; + }; + + struct v2f + { + float4 vertex : SV_POSITION; + float2 uv : TEXCOORD0; + }; + + v2f vert (appdata v) + { + v2f o; + o.vertex = UnityObjectToClipPos(v.vertex); + o.uv = TRANSFORM_TEX(v.uv, _MainTex); + return o; + } + + fixed4 frag (v2f i) : SV_Target + { + float raw = tex2D(_MainTex, i.uv).r; + float scaled = (raw - _MinDisparity) / (_MaxDisparity - _MinDisparity); + return lerp(_Color0, _Color100, scaled); + } + ENDCG + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/Shaders/UnlitDisparityImage.shader.meta b/com.unity.robotics.visualizations/Runtime/Shaders/UnlitDisparityImage.shader.meta new file mode 100644 index 00000000..93aeb7c9 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Shaders/UnlitDisparityImage.shader.meta @@ -0,0 +1,9 @@ +fileFormatVersion: 2 +guid: fed2755da54e7104993dbb6f30b7dc90 +ShaderImporter: + externalObjects: {} + defaultTextures: [] + nonModifiableTextures: [] + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Shaders/UnlitOccupancyGrid.shader b/com.unity.robotics.visualizations/Runtime/Shaders/UnlitOccupancyGrid.shader new file mode 100644 index 00000000..a785a484 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Shaders/UnlitOccupancyGrid.shader @@ -0,0 +1,66 @@ +Shader "Unlit/OccupancyGrid" +{ + Properties + { + _MainTex("Texture", 2D) = "white" {} + _ColorUnknown ("Unknown Color", Color) = (0,0,0,0) + _Color0 ("Color 0", Color) = (1,1,1,1) + _Color100 ("Color 100", Color) = (0,0,0,1) + } + SubShader + { + Tags {"Queue" = "Transparent" "IgnoreProjector" = "True" "RenderType"="Transparent" } + LOD 100 + ZWrite Off + Blend SrcAlpha OneMinusSrcAlpha + + Pass + { + Offset -1, -1 + + CGPROGRAM + #pragma vertex vert + #pragma fragment frag + + #include "UnityCG.cginc" + + fixed4 _ColorUnknown; + fixed4 _Color0; + fixed4 _Color100; + + sampler2D _MainTex; + float4 _MainTex_ST; + + struct appdata + { + float4 vertex : POSITION; + float2 uv : TEXCOORD0; + }; + + struct v2f + { + float4 vertex : SV_POSITION; + float2 uv : TEXCOORD0; + }; + + v2f vert (appdata v) + { + v2f o; + o.vertex = UnityObjectToClipPos(v.vertex); + o.uv = TRANSFORM_TEX(v.uv, _MainTex); + return o; + } + + fixed4 frag (v2f i) : SV_Target + { + // byte range 0-255 is rescaled down to 0-1 here; for an occupancy grid we want 100 to rescale to 1, so we need a multiplier. + float frac = tex2D(_MainTex, i.uv).r * 255.0/100.0; + if (frac > 1) + return _ColorUnknown; + else + return lerp(_Color0, _Color100, frac); + } + ENDCG + } + } +} diff --git a/com.unity.robotics.visualizations/Runtime/Shaders/UnlitOccupancyGrid.shader.meta b/com.unity.robotics.visualizations/Runtime/Shaders/UnlitOccupancyGrid.shader.meta new file mode 100644 index 00000000..8c3e8242 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Shaders/UnlitOccupancyGrid.shader.meta @@ -0,0 +1,9 @@ +fileFormatVersion: 2 +guid: 9ac3975a359483f4d828effd03fedb33 +ShaderImporter: + externalObjects: {} + defaultTextures: [] + nonModifiableTextures: [] + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Runtime/Unity.Robotics.Visualizations.asmdef b/com.unity.robotics.visualizations/Runtime/Unity.Robotics.Visualizations.asmdef new file mode 100644 index 00000000..fe08f88b --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Unity.Robotics.Visualizations.asmdef @@ -0,0 +1,29 @@ +{ + "name": "Unity.Robotics.Visualizations", + "rootNamespace": "", + "references": [ + "GUID:625bfc588fb96c74696858f2c467e978", + "GUID:b1ef917f7a8a86a4eb639ec2352edbf8" + ], + "includePlatforms": [ + "Editor", + "LinuxStandalone64", + "macOSStandalone", + "WindowsStandalone32", + "WindowsStandalone64" + ], + "excludePlatforms": [], + "allowUnsafeCode": false, + "overrideReferences": false, + "precompiledReferences": [], + "autoReferenced": true, + "defineConstraints": [], + "versionDefines": [ + { + "name": "com.unity.robotics.urdf-importer", + "expression": "0.1.0", + "define": "URDF_IMPORTER" + } + ], + "noEngineReferences": false +} diff --git a/com.unity.robotics.visualizations/Runtime/Unity.Robotics.Visualizations.asmdef.meta b/com.unity.robotics.visualizations/Runtime/Unity.Robotics.Visualizations.asmdef.meta new file mode 100644 index 00000000..8a0b0f14 --- /dev/null +++ b/com.unity.robotics.visualizations/Runtime/Unity.Robotics.Visualizations.asmdef.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 3f8053c1a58fb7f47b2af9d34d2e4b1e +AssemblyDefinitionImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/Visualizations.md b/com.unity.robotics.visualizations/Visualizations.md new file mode 100644 index 00000000..e768d2d4 --- /dev/null +++ b/com.unity.robotics.visualizations/Visualizations.md @@ -0,0 +1,193 @@ +# Unity Visualization 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: + "https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.visualizations" + +Now, to get started with visualizations, navigate to Packages/com.unity.robotics.visualizations in your Project window, and drag the DefaultVisualizationSuite prefab into your Unity scene. +When you press play, in your ROSConnection HUD you should find three new tabs: Topics, Transforms and Layout. + +![](images~/VisualizationsHUD.PNG) + +(The HUD should appear in the top left corner of the Game window as long as you're in Play mode and a ROSConnection is active. If you don't see it, ensure "Show HUD" is ticked in your Robotics/ROS Settings window.) + +When you click the Topics tab, it opens a list of all ROS topics published by any ROS node. You can see the actual ROS message type name by hovering over the topic. On the left you'll see "2D" and "3D" checkboxes to toggle visualizations for each topic. (For some messages, such as std_msgs/String for example, a 3d visualization doesn't make sense - you'll only see a 2D checkbox in those cases). Click on the appropriate checkbox to toggle that visualization for the given topic. + +![](images~/TopicsMenu.PNG) + +3D visualizations are simply drawn in your Unity scene, whereas 2D visualizations open in a window; You can move these windows around by dragging their title bar, or resize them by dragging the corners. The set of windows you have open, and their positions, are saved between sessions. (If necessary, you can export and import this layout file from the Layout tab.) + +# Configuring your visualization suite + +The DefaultVisualizationSuite prefab used above contains visualizations for most of the ROS common message types; if you expand the prefab in your hierarchy, you can see the individual components that provide the visualizations, and adjust their settings if you want. + +For more fine grained control over your visualizations, you can add or remove your own visualizer components to the scene. The default visualizers should be easy to find: they're all named *MessageType*DefaultVisualizer - for example, `Vector3DefaultVisualizer`. Each visualizer can be set up to apply only to messages on a specific topic (just set the "topic" field - or leave it blank to apply to all messages of the appropriate type). + +So, for example, let's assume you want to call out Point messages on the "important_points" topic, and you'd like them to appear extra large and in red. So maybe you'd add a DefaultVisualizerPoint script to your suite, and set it to draw large red points. + +![](images~/VisualizationSuiteExample.PNG) + +(Note that in Unity, "radius" is in Unity coordinates, so a radius of 1 means the points have a radius of 1 meter.) + +On the right hand side of the Topics tab, you'll find a dropdown menu for selecting which visualizer to use for that topic. + +# Writing a basic visualizer + +Although there are many visualizers for common message types included the visualization package, no doubt at some point you'll have your own unique messages you want to visualize, or you'll want to customize how a message is displayed for your project's specific needs. The simplest way to create a new visualizer is to write a script that inherits from DrawingVisualizer. + +Here's a simple example. (You can find this code [in the ExampleVisualizers folder](Runtime/ExampleVisualizers/PointVisualizerExample.cs)) + +```csharp +using Unity.Robotics.MessageVisualizers; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using UnityEngine; +using RosMessageTypes.Geometry; + +public class PointVisualizerExample : DrawingVisualizer +{ + // these settings will appear as configurable parameters in the Unity editor. + public float m_Size = 0.1f; + public Color m_Color; + public string m_Label; + + public override void Draw(Drawing3d drawing, PointMsg msg, MessageMetadata meta) + { + // If the user hasn't specified a color, SelectColor helpfully picks one + // based on the message topic. + Color finalColor = MessageVisualizationUtils.SelectColor(m_Color, meta); + + // Similarly, if the user leaves the label blank, SelectLabel will use the + // topic name as a label. + string finalLabel = MessageVisualizationUtils.SelectLabel(m_Label, meta); + + // Most of the default visualizers offer static drawing functions + // so that your own visualizers can easily send work to them. + PointDefaultVisualizer.Draw(msg, drawing, finalColor, m_Size); + + // You can also directly use the drawing functions provided by the Drawing class + drawing.DrawLabel(finalLabel, msg.From(), finalColor, m_Size); + } + + public override System.Action CreateGUI(PointMsg msg, MessageMetadata meta) + { + // this code runs each time a new message is received. + // If you want to preprocess the message or declare any state variables for + // the GUI to use, you can do that here. + string text = $"[{msg.x}, {msg.y}, {msg.z}]"; + + return () => + { + // this code runs once per UI event, like a normal Unity OnGUI function. + GUILayout.Label(text); + }; + } +} +``` + +- When the system is ready to display the graphics for a message, it will call your Draw() function, providing all the information you need about what drawing to create. The first parameter, `drawing`, is a convenient 3d drawing class that can display text, 3d lines, points, and other geometric shapes. This drawing will automatically be cleared before your Draw function is called, so all you have to do is call the drawing functions you want. +- When the HUD needs to display the GUI for your message, it will call CreateGUI on your class. CreateGUI should return a function that behaves like a normal Unity OnGUI callback: in other words, it will be invoked once per UI event, for as long as your visualizer is active, and any GUI elements you draw in this function will appear in the HUD. Note this is a runtime GUI, not an editor GUI; if you want your code to work in a build, the GUI functions you call should come from GUI or GUILayout, not EditorGUILayout. + +# Writing a custom visualizer + +If you want to create a visualization more complex than the simple DrawingVisualizer class can support, consider using its baseclass, the BaseVisualFactory. + +The most obvious way that BaseVisualFactory is different from DrawingVisualizer is that it doesn't conceal the distinction between a Visualizer and a Visual. +- A Visualizer is a MonoBehaviour that's placed in your Unity scene. It has settings the user can adjust. It's responsible for creating Visuals. +- A Visual is an object created at runtime, associated with a specific topic. It subscribes to the given topic and displays the messages it receives. +- NB: When writing a visualizer, do not assume there's a one-to-one relationship between the visualizer and the message being displayed; a visualizer can create more than one visual at a time. In other words, do not store information or metadata about the message being visualized in the visualizer. That kind of information should be stored in the Visual. + +Here's a simple example of a visualizer that instantiates a prefab to represent a Pose message. (You can also find this code [in the ExampleVisualizers folder](Runtime/ExampleVisualizers/PrefabVisualizerExample.cs)) + +```csharp +using UnityEngine; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using Unity.Robotics.ROSTCPConnector.MessageGeneration; +using RosMessageTypes.Geometry; +using Unity.Robotics.MessageVisualizers; +using Unity.Robotics.ROSTCPConnector; + +// A simple visualizer that places a (user configured) prefab to show the position and +// orientation of a Pose message +public class PrefabVisualizerExample : BaseVisualFactory +{ + // this setting will appear as a configurable parameter in the Unity editor. + public GameObject prefab; + + // The BaseVisualFactory's job is just to create visuals for topics as appropriate. + protected override IVisual CreateVisual(string topic) + { + return new PrefabVisual(topic, prefab); + } + + // The job of the visual itself is to subscribe to a topic, and draw + // representations of the messages it receives. + class PrefabVisual : IVisual + { + GameObject m_Prefab; + GameObject m_PrefabInstance; + PoseMsg m_LastMessage; + bool m_DrawingEnabled; + + public PrefabVisual(string topic, GameObject prefab) + { + m_Prefab = prefab; + + ROSConnection.GetOrCreateInstance().Subscribe(topic, AddMessage); + } + + void AddMessage(PoseMsg message) + { + // In this example we only store one message to visualize, but in principle + // you could store all the messages in a list and do whatever you want with them. + m_LastMessage = message; + + if (m_DrawingEnabled) + { + Redraw(); + } + } + + public void SetDrawingEnabled(bool enabled) + { + m_DrawingEnabled = enabled; + + if (enabled) + { + Redraw(); + } + else + { + GameObject.Destroy(m_PrefabInstance); + } + } + + public void Redraw() + { + GameObject.Destroy(m_PrefabInstance); + + if (m_LastMessage == null) + { + return; + } + + m_PrefabInstance = GameObject.Instantiate(m_Prefab); + m_PrefabInstance.transform.position = m_LastMessage.position.From(); + m_PrefabInstance.transform.rotation = m_LastMessage.orientation.From(); + } + + public void OnGUI() + { + // This is just a normal Unity OnGUI function. + // Draw the default GUI for a Pose message. + m_LastMessage.GUI(); + } + } + + // true = this visualizer should have a "3d" drawing checkbox in the topics list + public override bool CanShowDrawing => true; +} +``` + +- The CreateVisual function will only be called once for each topic, the first time visualizations are turned on for that topic. +- In order for your visual to actually receive messages, remember to call ROSConnection.GetOrCreateInstance().Subscribe to subscribe to messages on the appropriate topic. Feel free to subscribe to more than one topic if necessary. +- If you still want to use the Drawing3d class to display your visualization, you can create one by calling Drawing3dManager.CreateDrawing(). For efficiency, you should not repeatedly create and destroy drawings; prefer to create a drawing once and call its Clear function when you need to update it. diff --git a/com.unity.robotics.visualizations/Visualizations.md.meta b/com.unity.robotics.visualizations/Visualizations.md.meta new file mode 100644 index 00000000..96141cd5 --- /dev/null +++ b/com.unity.robotics.visualizations/Visualizations.md.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: d6e7b80d09162324cbc83e645f55635c +TextScriptImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.visualizations/images~/NiryoWithGhosts.PNG b/com.unity.robotics.visualizations/images~/NiryoWithGhosts.PNG new file mode 100644 index 00000000..e4d84fb1 Binary files /dev/null and b/com.unity.robotics.visualizations/images~/NiryoWithGhosts.PNG differ diff --git a/com.unity.robotics.visualizations/images~/TopicsMenu.PNG b/com.unity.robotics.visualizations/images~/TopicsMenu.PNG new file mode 100644 index 00000000..91c41792 Binary files /dev/null and b/com.unity.robotics.visualizations/images~/TopicsMenu.PNG differ diff --git a/com.unity.robotics.visualizations/images~/VisualizationSuiteExample.PNG b/com.unity.robotics.visualizations/images~/VisualizationSuiteExample.PNG new file mode 100644 index 00000000..0bdf0914 Binary files /dev/null and b/com.unity.robotics.visualizations/images~/VisualizationSuiteExample.PNG differ diff --git a/com.unity.robotics.visualizations/images~/VisualizationsHUD.PNG b/com.unity.robotics.visualizations/images~/VisualizationsHUD.PNG new file mode 100644 index 00000000..52638d5b Binary files /dev/null and b/com.unity.robotics.visualizations/images~/VisualizationsHUD.PNG differ diff --git a/com.unity.robotics.visualizations/images~/hud.png b/com.unity.robotics.visualizations/images~/hud.png new file mode 100644 index 00000000..e74a8fcb Binary files /dev/null and b/com.unity.robotics.visualizations/images~/hud.png differ diff --git a/com.unity.robotics.visualizations/images~/pcl2.png b/com.unity.robotics.visualizations/images~/pcl2.png new file mode 100644 index 00000000..30e90e71 Binary files /dev/null and b/com.unity.robotics.visualizations/images~/pcl2.png differ diff --git a/com.unity.robotics.visualizations/images~/point_inspector.png b/com.unity.robotics.visualizations/images~/point_inspector.png new file mode 100644 index 00000000..a356fc78 Binary files /dev/null and b/com.unity.robotics.visualizations/images~/point_inspector.png differ diff --git a/com.unity.robotics.visualizations/package.json b/com.unity.robotics.visualizations/package.json new file mode 100644 index 00000000..cbec7f32 --- /dev/null +++ b/com.unity.robotics.visualizations/package.json @@ -0,0 +1,10 @@ +{ + "name": "com.unity.robotics.visualizations", + "version": "0.1.0-preview", + "displayName": "Robotics Visualization", + "description": "Components for displaying specific ROS messages", + "unity": "2020.2", + "unityRelease": "0b9", + "dependencies": { + } +} diff --git a/com.unity.robotics.visualizations/package.json.meta b/com.unity.robotics.visualizations/package.json.meta new file mode 100644 index 00000000..f2c12f2d --- /dev/null +++ b/com.unity.robotics.visualizations/package.json.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 0b2a1e8df19eb2e4a9bc2fac53c4a300 +TextScriptImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: