Update car plot according to common usage

diff --git a/scripts/plot_json_objects_scenario.py b/scripts/plot_json_objects_scenario.py
index 878561cb213f96099c2542ce3a4b60fddf53c06d..7705c4532b494827e412afb0e419c1c53addb1ff 100644 (file)
--- a/scripts/plot_json_objects_scenario.py
+++ b/scripts/plot_json_objects_scenario.py
@@ -48,6 +48,11 @@ def plot_car(pose):
     lfx += BCAR_DF * cos(pose[2])
     lfx += BCAR_SD * cos(pose[2])
 
+    lf3x = pose[0]
+    lf3x += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
+    lf3x += 2/3 * BCAR_DF * cos(pose[2])
+    lf3x += BCAR_SD * cos(pose[2])
+
     lrx = pose[0]
     lrx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
     lrx += -BCAR_DR * cos(pose[2])
@@ -63,11 +68,21 @@ def plot_car(pose):
     rfx += BCAR_DF * cos(pose[2])
     rfx += BCAR_SD * cos(pose[2])
 
+    rf3x = pose[0]
+    rf3x += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
+    rf3x += 2/3 * BCAR_DF * cos(pose[2])
+    rf3x += BCAR_SD * cos(pose[2])
+
     lfy = pose[1]
     lfy += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
     lfy += BCAR_DF * sin(pose[2])
     lfy += BCAR_SD * sin(pose[2])
 
+    lf3y = pose[1]
+    lf3y += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
+    lf3y += 2/3 * BCAR_DF * sin(pose[2])
+    lf3y += BCAR_SD * sin(pose[2])
+
     lry = pose[1]
     lry += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
     lry += -BCAR_DR * sin(pose[2])
@@ -83,9 +98,22 @@ def plot_car(pose):
     rfy += BCAR_DF * sin(pose[2])
     rfy += BCAR_SD * sin(pose[2])
 
-    xcoords = (lfx - MINX, lrx - MINX, rrx - MINX, rfx - MINX)
-    ycoords = (lfy - MINY, lry - MINY, rry - MINY, rfy - MINY)
-    return (xcoords, ycoords)
+    rf3y = pose[1]
+    rf3y += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
+    rf3y += 2/3 * BCAR_DF * sin(pose[2])
+    rf3y += BCAR_SD * sin(pose[2])
+
+    cfx = pose[0]
+    cfx += BCAR_DF * cos(pose[2])
+    cfx += BCAR_SD * cos(pose[2])
+
+    cfy = pose[1]
+    cfy += BCAR_DF * sin(pose[2])
+    cfy += BCAR_SD * sin(pose[2])
+
+    xcoords = (lfx, lrx, rrx, rfx, cfx, rf3x, lf3x, cfx, lfx)
+    ycoords = (lfy, lry, rry, rfy, cfy, rf3y, lf3y, cfy, lfy)
+    return ([x - MINX for x in xcoords], [y - MINY for y in ycoords])
 
 if __name__ == "__main__":
     if (len(argv) == 2):