From 3768dee245803d416811b1ab31e04dd9e9cb40b7 Mon Sep 17 00:00:00 2001 From: Jiri Hubacek Date: Fri, 9 Nov 2018 14:39:25 +0100 Subject: [PATCH] Fix frame plot --- plot.py | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/plot.py b/plot.py index 4a544f5..43f7525 100644 --- a/plot.py +++ b/plot.py @@ -43,40 +43,47 @@ def car_frame(pose): Keyword arguments: pose -- The pose of a car. """ + dr = (LENGTH - WHEEL_BASE) / 2 + df = LENGTH - dr + lfx = pose[0] lfx += (WIDTH / 2.0) * cos(pose[2] + pi / 2.0) - lfx += WHEEL_BASE * cos(pose[2]) + lfx += df * cos(pose[2]) lfx += SAFETY_DIST * cos(pose[2]) lrx = pose[0] lrx += (WIDTH / 2.0) * cos(pose[2] + pi / 2.0) + lrx += -dr * cos(pose[2]) lrx += -SAFETY_DIST * cos(pose[2]) rrx = pose[0] rrx += (WIDTH / 2.0) * cos(pose[2] - pi / 2.0) + rrx += -dr * cos(pose[2]) rrx += -SAFETY_DIST * cos(pose[2]) rfx = pose[0] rfx += (WIDTH / 2.0) * cos(pose[2] - pi / 2.0) - rfx += WHEEL_BASE * cos(pose[2]) + rfx += df * cos(pose[2]) rfx += SAFETY_DIST * cos(pose[2]) lfy = pose[1] lfy += (WIDTH / 2.0) * sin(pose[2] + pi / 2.0) - lfy += WHEEL_BASE * sin(pose[2]) + lfy += df * sin(pose[2]) lfy += SAFETY_DIST * sin(pose[2]) lry = pose[1] lry += (WIDTH / 2.0) * sin(pose[2] + pi / 2.0) + lry += -dr * sin(pose[2]) lry += -SAFETY_DIST * sin(pose[2]) rry = pose[1] rry += (WIDTH / 2.0) * sin(pose[2] - pi / 2.0) + rry += -dr * sin(pose[2]) rry += -SAFETY_DIST * sin(pose[2]) rfy = pose[1] rfy += (WIDTH / 2.0) * sin(pose[2] - pi / 2.0) - rfy += WHEEL_BASE * sin(pose[2]) + rfy += df * sin(pose[2]) rfy += SAFETY_DIST * sin(pose[2]) xcoords = (lfx, lrx, rrx, rfx) -- 2.39.2