[AUTO-7325] [AUTO-7340] Add python API commands for Destination sensor

Author: daviduhm

Assets/Scripts/Api/ApiManager.cs

@@ -74,6 +74,7 @@ private enum MessageType
         public HashSet<GameObject> Waypoints = new HashSet<GameObject>();
         public HashSet<GameObject> StopLine = new HashSet<GameObject>();
         public HashSet<GameObject> LaneChange = new HashSet<GameObject>();
+        public HashSet<GameObject> Destinations = new HashSet<GameObject>();
         public List<JSONObject> Events = new List<JSONObject>();
 
         public string Key { get; } = "ApiManager";
@@ -316,6 +317,7 @@ public async Task Reset()
             Waypoints.Clear();
             StopLine.Clear();
             LaneChange.Clear();
+            Destinations.Clear();
 
             StopAllCoroutines();
 
@@ -412,6 +414,24 @@ public void AddWaypointReached(GameObject obj, int index)
             }
         }
 
+        public void AddDestinationReached(GameObject obj)
+        {
+            if (!Destinations.Contains(obj))
+            {
+                return;
+            }
+
+            string uid;
+            if (AgentUID.TryGetValue(obj, out uid))
+            {
+                var j = new JSONObject();
+                j.Add("type", new JSONString("destination_reached"));
+                j.Add("agent", new JSONString(uid));
+
+                Events.Add(j);
+            }
+        }
+
         public void RegisterAgentWithWaypoints(GameObject obj)
         {
             if (AgentFollowingWaypoints.Contains(obj))

Assets/Scripts/Api/Commands/AgentOnDestinationReached.cs

@@ -0,0 +1,33 @@
+/**
+ * Copyright (c) 2021 LG Electronics, Inc.
+ *
+ * This software contains code licensed as described in LICENSE.
+ *
+ */
+
+using SimpleJSON;
+using UnityEngine;
+
+namespace Simulator.Api.Commands
+{
+    class AgentOnDestinationReached : ICommand
+    {
+        public string Name => "agent/on_destination_reached";
+
+        public void Execute(JSONNode args)
+        {
+            var api = ApiManager.Instance;
+            var uid = args["uid"].Value;
+
+            if (api.Agents.TryGetValue(uid, out GameObject obj))
+            {
+                api.Destinations.Add(obj);
+                api.SendResult(this);
+            }
+            else
+            {
+                api.SendError(this, $"Agent '{uid}' not found");
+            }
+        }
+    }
+}

Assets/Scripts/Api/Commands/AgentOnDestinationReached.cs.meta

@@ -0,0 +1,41 @@
+/**
+ * Copyright (c) 2021 LG Electronics, Inc.
+ *
+ * This software contains code licensed as described in LICENSE.
+ *
+ */
+
+using SimpleJSON;
+using UnityEngine;
+using Simulator.Map;
+
+namespace Simulator.Api.Commands
+{
+    class MapFromNav : ICommand
+    {
+        public string Name => "map/from_nav";
+
+        public void Execute(JSONNode args)
+        {
+            var api = ApiManager.Instance;
+
+            var position = args["position"].ReadVector3();
+            var orientation = args["orientation"].ReadQuaternion();
+
+            var nav = NavOrigin.Find();
+            if (nav == null)
+            {
+                api.SendError(this, "NavOrigin not found");
+                return;
+            }
+
+            var point = nav.FromNavPose(position, orientation);
+
+            var res = new JSONObject();
+            res.Add("position", point.position);
+            res.Add("rotation", point.rotation.eulerAngles);
+
+            api.SendResult(this, res);
+        }
+    }
+}

Assets/Scripts/Api/Commands/MapFromNav.cs

@@ -0,0 +1,48 @@
+/**
+ * Copyright (c) 2021 LG Electronics, Inc.
+ *
+ * This software contains code licensed as described in LICENSE.
+ *
+ */
+
+using SimpleJSON;
+using UnityEngine;
+using System.Linq;
+using System.Reflection;
+using Simulator.Utilities;
+using Simulator.Sensors;
+
+namespace Simulator.Api.Commands
+{
+    class VehicleSetDestination : ICommand
+    {
+        public string Name => "vehicle/set_destination";
+
+        public void Execute(JSONNode args)
+        {
+            var uid = args["uid"].Value;
+            var position = args["transform"]["position"].ReadVector3();
+            var rotation = args["transform"]["rotation"].ReadVector3();
+
+            var api = ApiManager.Instance;
+            if (api.Agents.TryGetValue(uid, out GameObject obj))
+            {
+                var sensors = obj.GetComponentsInChildren<SensorBase>();
+                var destination_sensor = sensors.FirstOrDefault(s => s.GetType().GetCustomAttribute<SensorType>().Name == "Destination");
+
+                if (destination_sensor == null)
+                {
+                    api.SendError(this, $"Agent '{uid}' does not have Destination Sensor");
+                    return;
+                }
+
+                destination_sensor.GetType().GetMethod("SetDestination").Invoke(destination_sensor, new object[] { position, rotation });
+                api.SendResult(this);
+            }
+            else
+            {
+                api.SendError(this, $"Agent '{uid}' not found");
+            }
+        }
+    }
+}

Assets/Scripts/Api/Commands/MapFromNav.cs.meta

@@ -0,0 +1,46 @@
+/**
+ * Copyright (c) 2021 LG Electronics, Inc.
+ *
+ * This software contains code licensed as described in LICENSE.
+ *
+ */
+
+using SimpleJSON;
+using UnityEngine;
+using System.Linq;
+using System.Reflection;
+using Simulator.Utilities;
+using Simulator.Sensors;
+
+namespace Simulator.Api.Commands
+{
+    class VehicleSetInitialPose : ICommand
+    {
+        public string Name => "vehicle/set_initial_pose";
+
+        public void Execute(JSONNode args)
+        {
+            var uid = args["uid"].Value;
+
+            var api = ApiManager.Instance;
+            if (api.Agents.TryGetValue(uid, out GameObject obj))
+            {
+                var sensors = obj.GetComponentsInChildren<SensorBase>();
+                var destination_sensor = sensors.FirstOrDefault(s => s.GetType().GetCustomAttribute<SensorType>().Name == "Destination");
+
+                if (destination_sensor == null)
+                {
+                    api.SendError(this, $"Agent '{uid}' does not have Destination Sensor");
+                    return;
+                }
+
+                destination_sensor.GetType().GetMethod("SetInitialPose").Invoke(destination_sensor, new object[] {});
+                api.SendResult(this);
+            }
+            else
+            {
+                api.SendError(this, $"Agent '{uid}' not found");
+            }
+        }
+    }
+}

Assets/Scripts/Api/Commands/VehicleSetDestination.cs

@@ -214,13 +214,28 @@ public struct Pose
         public Quaternion orientation;
     }
 
+    [MessageType("geometry_msgs/PoseStamped")]
+    public struct PoseStamped
+    {
+        public Header header;
+        public Pose pose;
+    }
+
     [MessageType("geometry_msgs/PoseWithCovariance")]
     public struct PoseWithCovariance
     {
         public Pose pose;
         public double[] covariance;  // float64[36] covariance
     }
 
+    [MessageType("geometry_msgs/PoseWithCovarianceStamped")]
+    public struct PoseWithCovarianceStamped
+    {
+        public Header header;
+        public Pose pose;
+        public double[] covariance;  // float64[36] covariance
+    }
+
     [MessageType("geometry_msgs/Quaternion")]
     public struct Quaternion
     {

Assets/Scripts/Api/Commands/VehicleSetDestination.cs.meta

@@ -0,0 +1,80 @@
+/**
+ * Copyright (c) 2021 LG Electronics, Inc.
+ *
+ * This software contains code licensed as described in LICENSE.
+ *
+ */
+
+using UnityEngine;
+using System.Collections.Generic;
+
+namespace Simulator.Map
+{
+    public struct NavPose
+    {
+        // Unity: Right/Up/Forward
+        // ROS: Forward/Left/Up
+
+        public Vector3 position;
+        public Quaternion orientation;
+    }
+
+    public class NavOrigin : MonoBehaviour
+    {
+        public float OriginX;
+        public float OriginY;
+        public float Rotation;  // Not supported yet
+
+        public static NavOrigin Find()
+        {
+            var origin = FindObjectOfType<NavOrigin>();
+            if (origin == null)
+            {
+                Debug.LogWarning("Map is missing NavOrigin component! Adding temporary NavOrigin. Please add to scene and set origin for Navigation2 stack");
+                origin = new GameObject("NavOrigin").AddComponent<NavOrigin>();
+                origin.transform.SetPositionAndRotation(Vector3.zero, Quaternion.identity);
+                origin.OriginX = 0;
+                origin.OriginY = 0;
+                origin.Rotation = 0;
+            }
+
+            return origin;
+        }
+
+        public NavPose GetNavPose(Transform pose)
+        {
+            var nav_pose = new NavPose();
+
+            var position = transform.InverseTransformPoint(pose.position);
+            position.Set(position.z, -position.x, position.y);
+            position.x += OriginX;
+            position.y += OriginY;
+            nav_pose.position = position;
+
+            var heading = Vector3.SignedAngle(transform.forward, pose.transform.forward, -transform.up);
+            Quaternion orientation = Quaternion.Euler(0, 0, heading);
+            nav_pose.orientation = orientation;
+
+            return nav_pose;
+        }
+
+        public Transform FromNavPose(Vector3 position, Quaternion orientation)
+        {
+            var point = new GameObject();
+
+            position.x -= OriginX;
+            position.y -= OriginY;
+            position.Set(-position.y, position.z, position.x);
+            point.transform.position = transform.TransformPoint(position);
+
+            orientation.Set(orientation.y, -orientation.z, -orientation.x, orientation.w);
+            var heading = orientation.eulerAngles.y - transform.rotation.eulerAngles.y;
+            point.transform.rotation = Quaternion.Euler(0, heading, 0);
+
+            var pose = point.transform;
+            Destroy(point);
+
+            return pose;
+        }
+    }
+}