Radar does not detect NPCs with waypoints

[pantonante]

Hi, when running a scenario created with the Visual Scenario Editor containing NPCs following waypoints, the radar does not detect the vehicles. If I create a scenario with a static vehicle, the radar works fine. The radar works fine also with the Random Traffic template.

I get the same behavior with SVL version 2021.1/2.2/3.

[EricBoiseLGSVL]

We have fixes that we are pushing soon to master to address this

[pantonante]

That's great! Thank you!

[pantonante]

@EricBoiseLGSVL do you have an estimate on when this will happen? I'm working on a project where this feature is very important and I would like to plan around it.

[EricBoiseLGSVL]

We are hoping to push changes to master within the next few weeks.

[pantonante]

In light of LG sunsetting SVL, do you still plan to push the fix? If not, can you suggest what is likely causing the problem so I can try to contribute? (I'm not an experienced Unity developer)

[EricBoiseLGSVL]

Latest radar sensor.cs

/**
 * Copyright (c) 2019-2021 LG Electronics, Inc.
 *
 * This software contains code licensed as described in LICENSE.
 *
 */

using System.Linq;
using System.Collections.Generic;
using Simulator.Bridge;
using Simulator.Bridge.Data;
using Simulator.Utilities;
using UnityEngine;
using UnityEngine.AI;
using Simulator.Sensors.UI;

namespace Simulator.Sensors
{
    using System;

    [SensorType("Radar", new[] { typeof(DetectedRadarObjectData) })]
    public class RadarSensor : SensorBase
    {
        [SensorParameter]
        [Range(1.0f, 100f)]
        public float Frequency = 13.4f;
        public LayerMask RadarBlockers;

        private List<RadarMesh> radars = new List<RadarMesh>();
        private WireframeBoxes WireframeBoxes;

        private uint seqId;
        private float nextPublish;

        private BridgeInstance Bridge;
        private Publisher<DetectedRadarObjectData> Publish;
        private Rigidbody SelfRigidBody;

        /// <summary>
        /// Dictionary of all agents in the range and their semaphore locks
        /// Semaphore is required because radar can use multiple trigger colliders
        /// Agent has to be cached until any trigger containing it is in the range
        /// </summary>
        private Dictionary<ITriggerAgent, int> AgentsInRange = new Dictionary<ITriggerAgent, int>();
        private Dictionary<ITriggerAgent, DetectedRadarObject> Detected = new Dictionary<ITriggerAgent, DetectedRadarObject>();
        private Dictionary<ITriggerAgent, Box> Visualized = new Dictionary<ITriggerAgent, Box>();

        [AnalysisMeasurement(MeasurementType.Count)]
        public int MaxTracked = -1;

        public override SensorDistributionType DistributionType => SensorDistributionType.MainOrClient;
        public override float PerformanceLoad { get; } = 0.2f;

        struct Box
        {
            public Vector3 Size;
            public Color Color;
        }

        private void Awake()
        {
            radars.AddRange(GetComponentsInChildren<RadarMesh>());
            foreach (var radar in radars)
            {
                radar.Init();
            }
            SimulatorManager.Instance.NPCManager.RegisterDespawnCallback(OnExitRange);
        }

        protected override void Initialize()
        {
            Debug.Assert(SimulatorManager.Instance != null);
            WireframeBoxes = SimulatorManager.Instance.WireframeBoxes;
            foreach (var radar in radars)
            {
                radar.SetCallbacks(OnEnterRange, OnExitRange);
            }
            nextPublish = Time.time + 1.0f / Frequency;

            SelfRigidBody = gameObject.GetComponentInParent<Rigidbody>();    
            SimulatorManager.Instance.TriggersManager.AgentUnregistered += TriggersManagerOnAgentUnregistered;
        }

        protected override void Deinitialize()
        {
            var agents = AgentsInRange.Keys.ToList();
            for (var i = agents.Count - 1; i >= 0; i--)
            {
                var triggerAgent = agents[i];
                OnExitRange(triggerAgent, true);
            }
            SimulatorManager.Instance.TriggersManager.AgentUnregistered -= TriggersManagerOnAgentUnregistered;
        }

        private void Update()
        {
            if (Bridge == null || Bridge.Status != Status.Connected)
            {
                return;
            }

            if (Time.time < nextPublish)
            {
                return;
            }

            nextPublish = Time.time + 1.0f / Frequency;
            MaxTracked = Mathf.Max(MaxTracked, Detected.Count);
            Publish(new DetectedRadarObjectData()
            {
                Name = Name,
                Frame = Frame,
                Time = SimulatorManager.Instance.CurrentTime,
                Sequence = seqId++,
                Data = Detected.Values.ToArray(),
            });
        }

        private void FixedUpdate()
        {
            foreach (var detectedAgent in AgentsInRange)
            {
                WhileInRange(detectedAgent.Key);
            }
        }

        private void TriggersManagerOnAgentUnregistered(ITriggerAgent iTriggerAgent)
        {
            OnExitRange(iTriggerAgent, true);
        }

        public override void OnBridgeSetup(BridgeInstance bridge)
        {
            Bridge = bridge;
            Publish = Bridge.AddPublisher<DetectedRadarObjectData>(Topic);
        }

        void WhileInRange(ITriggerAgent triggerAgent)
        {
            if (CheckBlocked(triggerAgent))
            {
                if (Detected.ContainsKey(triggerAgent))
                {
                    Detected.Remove(triggerAgent);
                }
                if (Visualized.ContainsKey(triggerAgent))
                {
                    Visualized.Remove(triggerAgent);
                }
                return;
            }

            if (Detected.TryGetValue(triggerAgent, out var detected)) // update existing data
            {
                Vector3 objectVelocity = GetObjectVelocity(triggerAgent);

                var otherPosition = triggerAgent.AgentTransform.position;
                var thisTransform = transform;
                var thisPosition = thisTransform.position;
                detected.SensorPosition = thisPosition;
                detected.SensorAim = thisTransform.forward;
                detected.SensorRight = thisTransform.right;
                detected.SensorVelocity = GetSensorVelocity();
                detected.SensorAngle = GetSensorAngle(triggerAgent);
                detected.Position = otherPosition;
                detected.Velocity = objectVelocity;
                detected.RelativePosition = otherPosition - thisPosition;
                detected.RelativeVelocity = GetSensorVelocity() - objectVelocity;
                detected.ColliderSize = triggerAgent.AgentBounds.size;
                detected.State = GetAgentState(triggerAgent);
                detected.NewDetection = false;
            }
            else
            {

                Box box = GetVisualizationBox(triggerAgent);
                if (box.Size == Vector3.zero) // Empty box returned if tag is not right
                {
                    return;
                }

                Vector3 objectVelocity = GetObjectVelocity(triggerAgent);

                Transform thisTransform;
                Bounds otherBounds;
                Visualized.Add(triggerAgent, box);
                Detected.Add(triggerAgent, new DetectedRadarObject()
                {
                    Id = triggerAgent.AgentGameObject.GetInstanceID(),
                    SensorPosition = (thisTransform = transform).position,
                    SensorAim = thisTransform.forward,
                    SensorRight = thisTransform.right,
                    SensorVelocity = GetSensorVelocity(),
                    SensorAngle = GetSensorAngle(triggerAgent),
                    Position = (otherBounds = triggerAgent.AgentBounds).center,
                    Velocity = objectVelocity,
                    RelativePosition = otherBounds.center - transform.position,
                    RelativeVelocity = GetSensorVelocity() - objectVelocity,
                    ColliderSize = triggerAgent.AgentBounds.size,
                    State = GetAgentState(triggerAgent),
                    NewDetection = true,
                });
            }
        }

        void OnEnterRange(Collider other, RadarMesh radar)
        {
            if (other.isTrigger)
                return;

            if (!other.enabled)
                return;

            var triggerAgent = SimulatorManager.Instance.TriggersManager.TryGetTriggerAgent(other);
            if (triggerAgent == null)
                return;

            if (AgentsInRange.TryGetValue(triggerAgent, out var locks))
            {
                AgentsInRange[triggerAgent] = locks + 1;
            }
            else
            {
                AgentsInRange.Add(triggerAgent, 1);
            }
        }

        void OnExitRange(NPCController controller)
        {
            OnExitRange(controller.MainCollider);
        }

        void OnExitRange(Collider other, RadarMesh radar)
        {
            OnExitRange(other);
        }

        void OnExitRange(Collider other)
        {
            if (other.isTrigger)
                return;

            if (!other.enabled)
                return;

            OnExitRange(SimulatorManager.Instance.TriggersManager.TryGetTriggerAgent(other));
        }

        void OnExitRange(ITriggerAgent triggerAgent, bool forceRemove = false)
        {
            if (triggerAgent == null)
                return;

            if (!AgentsInRange.TryGetValue(triggerAgent, out var locks))
            {
                return;
            }

            AgentsInRange[triggerAgent] = --locks;
            if (!forceRemove && locks > 0)
            {
                return;
            }

            // Remove agent from all cache collections if no radar collider overlaps them
            AgentsInRange.Remove(triggerAgent);
            if (Detected.ContainsKey(triggerAgent))
            {
                Detected.Remove(triggerAgent);
            }
            if (Visualized.ContainsKey(triggerAgent))
            {
                Visualized.Remove(triggerAgent);
            }

        }

        private bool CheckBlocked(ITriggerAgent triggerAgent)
        {
            bool isBlocked = false;
            var orig = transform.position;
            var end = triggerAgent.AgentBounds.center;
            var dir = end - orig;
            var dist = (end - orig).magnitude;
            if (Physics.Raycast(orig, dir, out RaycastHit hit, dist, RadarBlockers)) // ignore if blocked
            {
                var hitAgent = SimulatorManager.Instance.TriggersManager.TryGetTriggerAgent(hit.collider);
                if (hitAgent != triggerAgent)
                {
                    isBlocked = true;
                }
            }
            return isBlocked;
        }

        private Box GetVisualizationBox(ITriggerAgent triggerAgent)
        {
            var bbox = new Box();

            if (triggerAgent.AgentGameObject.layer == LayerMask.NameToLayer("NPC"))
            {
                bbox.Color = Color.green;
            }
            else if (triggerAgent.AgentGameObject.layer == LayerMask.NameToLayer("Pedestrian"))
            {
                bbox.Color = Color.yellow;
            }
            else if (triggerAgent.AgentGameObject.layer == LayerMask.NameToLayer("Bicycle"))
            {
                bbox.Color = Color.cyan;
            }
            else if (triggerAgent.AgentGameObject.layer == LayerMask.NameToLayer("Agent"))
            {
                bbox.Color = Color.magenta;
            }
            else
            {
                return bbox;
            }

            bbox.Size = triggerAgent.AgentBounds.size;

            return bbox;
        }

        private Vector3 GetSensorVelocity()
        {
            return SelfRigidBody == null ? Vector3.zero : SelfRigidBody.velocity;
        }

        private double GetSensorAngle(ITriggerAgent triggerAgent)
        {
            // angle is orientation of the obstacle in degrees as seen by radar, counterclockwise is positive
            double angle = -Vector3.SignedAngle(transform.forward, triggerAgent.AgentTransform.forward, transform.up);
            if (angle > 90)
            {
                angle -= 180;
            }
            else if (angle < -90)
            {
                angle += 180;
            }
            return angle;
        }

        private Vector3 GetObjectVelocity(ITriggerAgent triggerAgent)
        {
            if (triggerAgent == null)
                return Vector3.zero;

            switch (triggerAgent.AgentType)
            {
                case AgentType.Unknown:
                    throw new ArgumentException();
                case AgentType.Ego:
                    if (triggerAgent is IAgentController agentController)
                    {
                        return agentController.Velocity;
                    }
                    break;
                case AgentType.Npc:
                    if (triggerAgent is NPCController npc)
                    {
                        return npc.simpleVelocity;
                    }
                    break;
                case AgentType.Pedestrian:
                    if (triggerAgent is PedestrianController ped)
                    {
                        if (ped.ActiveBehaviour is PedestrianAutomaticBehaviour pedActiveBehaviour)
                        {
                            return pedActiveBehaviour.Agent.desiredVelocity;
                        }

                        var rb = ped.RB;
                        if (rb != null)
                        {
                            return rb.velocity;
                        }
                    }

                    break;
                default:
                    throw new ArgumentOutOfRangeException();
            }

            return Vector3.zero;
        }

        private int GetAgentState(ITriggerAgent triggerAgent)
        {
            int state = 1;
            if (triggerAgent != null && triggerAgent.MovementSpeed > 1f)
            {
                state = 0;
            }
            return state;
        }

        public override void OnVisualize(Visualizer visualizer)
        {
            foreach (var v in Visualized)
            {
                var triggerAgent = v.Key;
                var box = v.Value;
                if (triggerAgent.AgentGameObject.activeInHierarchy)
                {
                    WireframeBoxes.Draw(triggerAgent.AgentTransform.localToWorldMatrix, new Vector3(0f, triggerAgent.AgentBounds.extents.y, 0f), box.Size, box.Color);
                }
            }

            foreach (var radar in radars)
            {
                Graphics.DrawMesh(radar.RadarMeshFilter.sharedMesh, transform.localToWorldMatrix, radar.RadarMeshRenderer.sharedMaterial, LayerMask.NameToLayer("Sensor"));
            }
        }

        public override void OnVisualizeToggle(bool state) {}
    }
}

This may help if the sensor changes don't solve the issue. It was the change needed for GT2D detection of add agent in api mode. PedestrianManager.cs (NPCManager.cs might need the same changes.)

298 SimulatorManager.Instance.UpdateSegmentationColors(pedController.gameObject, pedController.GTID);
before
return pedController

and 
322
// Segmentation ID does not change on re-pool, so segmentation colors don't have to be updated
// Just make sure GTID is tracked again after re-spawning from pool
SimulatorManager.Instance.SegmentationIdMapping.AddOrGetSegmentationId(CurrentPooledPeds[i].gameObject, CurrentPooledPeds[i].GTID);

before
foreach (var callback in SpawnCallbacks)

[pantonante]

Is it possible that you also changed the ITriggerAgent interface and the SimulatorManager?

I'm getting these errors when I try to compile:

'ITriggerAgent' does not contain a definition for 'AgentGameObject' and no accessible extension method 'AgentGameObject' accepting a first argument of type 'ITriggerAgent' could be found (are you missing a using directive or an assembly reference?)
'ITriggerAgent' does not contain a definition for 'AgentBounds' and no accessible extension method 'AgentBounds' accepting a first argument of type 'ITriggerAgent' could be found (are you missing a using directive or an assembly reference?)
'SimulatorManager' does not contain a definition for 'TriggersManager' and no accessible extension method 'TriggersManager' accepting a first argument of type 'SimulatorManager' could be found (are you missing a using directive or an assembly reference?)

[EricBoiseLGSVL]

Yes, since our current repo is ahead of latest release we have quite a few changes in simulator that will be missing.

[pantonante]

Any chance you can share these changes in some branch or here?

[EricBoiseLGSVL]

Sorry, with simulator being sunset we will not be pushing any changes. With the current state internally, it will cause too many issues if we share or push latest.

[alperenbayraktar]

@pantonante did you manage to send the radar data after all?

[pantonante]

No, I decided to generate the radar from ground truth data outside SVL because it was easier in my project. You can read the code here https://github.com/pantonante/apollo/blob/master/verification/bridge/fake_radar.py

[alperenbayraktar]

I see, deleting the isTrigger check at the start of the WhileInRange method also fixes the problem.