Update WaypointUpdater, Revamp Fake TLdetector

[mithi]

IMPORTANT NOTE: please test first on your machine before approving/merging! Not sure if it works for all machines!

• May need to tweak ros rate for both nodes also.

• I’m considering deleting the pull request.

WaypointUpdater

• Move functionality from pose callback to loop
• This fixed some of my lag issues, i think this is a more efficient implementation

TLDetector

• Use ground true data from topic Vehice/TrafficLight
• There is a latency of about 2-4 seconds in my current machine setup so it takes about 3 seconds after the I see the simulator's traffic light turn green before the WaypointUpdater prints that it chooses to mode CRUISE from mode BRAKE, I suspect that in more powerful machines this shouldn't be an issue, I hope...
• I think the reason for this is because how we get the closest way point for each traffic light is very inefficient since it is not hard coded. To get the closest waypoint we have to go through all 10000+ waypoints in base waypoints for each traffic light and each time the callback is executed we do this eight times since there are eight traffic lights to get the nearest traffic light ahead. There must be a better way for this. A more optimized solution.

[buffos]

I think the reason for this is because how we get the closest way point for each traffic light is very inefficient since it is not hard coded.

Why don't you do that once in your init. function And you can do that in one loop, instead of passing a single pose, pass an array and return what ever suits you. So it can be done in 1 pass. Or, since this is just temporary code, just let it be, and run it 8 times for the 8 lights once. Store them in a dictionary with poses as keys and you are done.

[mithi]

@buffos Okay I will do that, I wasn't able to think of that last time (maybe because I was already sleepy haha).

[mithi]

Updated pull request: https://github.com/kung-fu-panda-automotive/carla-driver/pull/41