vato007/breezyslam
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Cleaned up some code

Browse Source
This commit is contained in:
Matt Lubas
2016-08-11 16:30:31 -04:00
parent e8cf694ce0
commit 92a45c38ae
1 changed files with 11 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
examples/pltslamshow.py
View File

@@ -54,12 +54,6 @@ FONT_FACE = cv.CV_FONT_HERSHEY_COMPLEX
from math import sin, cos, radians from math import sin, cos, radians
def _rotate(x, y, r, theta_deg):
theta = radians(theta_deg)
dx = r * cos(theta)
dy = r * sin(theta)
return x+dx, y+dy
class SlamShow(object): class SlamShow(object):
def __init__(self, map_size_pixels, map_scale_mm_per_pixel, window_name): def __init__(self, map_size_pixels, map_scale_mm_per_pixel, window_name):
@@ -182,8 +176,8 @@ class SlamShow(object):
def _add_vehicle(self, x_mm, y_mm, theta_deg): def _add_vehicle(self, x_mm, y_mm, theta_deg):
# Use a very short arrow shaft to orient the head of the arrow #XXX Use a very short arrow shaft to orient the head of the arrow
dx, dy = _rotate(0, 0, .1, theta_deg) dx, dy = plt_rotate(0, 0, 1, theta_deg)
self.vehicle=self.ax.arrow(x_mm, y_mm, dx, dy, head_width=ROBOT_WIDTH_MM, head_length=ROBOT_HEIGHT_MM, fc='r', ec='r') self.vehicle=self.ax.arrow(x_mm, y_mm, dx, dy, head_width=ROBOT_WIDTH_MM, head_length=ROBOT_HEIGHT_MM, fc='r', ec='r')
@@ -265,4 +259,13 @@ def rotate(pt, deg):
s = sin(rad) s = sin(rad)
x,y = pt x,y = pt
return int(x*c - y*s), int(x*s + y*c) return int(x*c - y*s), int(x*s + y*c)
def plt_rotate(x, y, r, deg):
rad = radians(deg)
c = cos(rad)
s = sin(rad)
dx = r * c
dy = r * s
return x+dx, y+dy