diff --git a/examples/logdemo.py b/examples/logdemo.py index 6a6fb27..4c980e9 100755 --- a/examples/logdemo.py +++ b/examples/logdemo.py @@ -102,7 +102,6 @@ def main(): # Display map and robot pose display.displayMap(mapbytes) - display.displayRobot((x_mm, y_mm, theta_degrees)) display.setPose(x_mm, y_mm, theta_degrees) diff --git a/examples/pltslamshow.py b/examples/pltslamshow.py index 74d9218..91a455e 100644 --- a/examples/pltslamshow.py +++ b/examples/pltslamshow.py @@ -46,12 +46,7 @@ SENSOR_NEGATIVE_COLOR_BGR = (0,0,255) # Trajectory display params TRAJECTORY_COLOR_BGR = (255, 0, 0) -import cv import matplotlib.pyplot as plt - -# Arbitrary font for OpenCV -FONT_FACE = cv.CV_FONT_HERSHEY_COMPLEX - from math import sin, cos, radians class SlamShow(object): @@ -66,19 +61,6 @@ class SlamShow(object): # Create a byte array to display the map with a color overlay self.bgrbytes = bytearray(map_size_pixels * map_size_pixels * 3) - # Create an empty OpenCV image to be filled with map bytes - self.image = cv.CreateImageHeader((map_size_pixels,map_size_pixels), cv.IPL_DEPTH_8U, 3) - - # Create an OpenCV window for displaying the map - cv.NamedWindow(window_name) - - # Set up font for displaying velocities - self.font = cv.InitFont(FONT_FACE, 1, 1) - - # Display initial empty image - cv.SetData(self.image, self.bgrbytes, self.map_size_pixels*3) - cv.ShowImage(self.window_name, self.image) - # Make a nice big (10"x10") figure fig = plt.figure(figsize=(10,10)) @@ -101,7 +83,7 @@ class SlamShow(object): self.ax.grid(False) - #Starting vehicle at Center, 1600mm, 1600mm + #Starting vehicle at Center, 16000mm, 16000mm self._add_vehicle(16000,16000,0) def displayMap(self, mapbytes): @@ -111,35 +93,7 @@ class SlamShow(object): self.bgrbytes[1::3] = mapbytes self.bgrbytes[2::3] = mapbytes - - # Put color bytes into image - cv.SetData(self.image, self.bgrbytes, self.map_size_pixels*3) - - - def displayRobot(self, (x_mm, y_mm, theta_deg), color=ROBOT_COLOR_BGR, scale=1, line_thickness=1): - - # Get a polyline (e.g. triangle) to represent the robot icon - robot_points = self.robot_polyline(scale) - - # Rotate the polyline by the current angle... would use rotate instead of this - robot_points = map(lambda pt: rotate(pt, theta_deg), robot_points) - - # Convert the robot position from meters to pixels - x_pix, y_pix = self.mm2pix(x_mm), self.mm2pix(y_mm) - - # Move the polyline to the current robot position ... would use rotate instead of this? - robot_points = map(lambda pt: (x_pix+pt[0], y_pix+pt[1]), robot_points) - - # Add an icon for the robot - cv.PolyLine(self.image, [robot_points], True, color, line_thickness) - - def displayScan(self, scan, offset_mm = (0,0), color=SCANPOINT_COLOR_BGR): - - for point in scan: - cv.Circle(self.image, (self.mm2pix(point[0]+offset_mm[0]), self.mm2pix(point[1]+offset_mm[1])), \ - SCANPOINT_RADIUS, color) - def displayVelocities(self, dxy_mm, dtheta_deg): @@ -153,12 +107,6 @@ class SlamShow(object): x1_mm, y1_mm = trajectory[k-1] x2_mm, y2_mm = trajectory[k] - - cv.Line(self.image, - (self.mm2pix(x1_mm), self.mm2pix(y1_mm)), \ - (self.mm2pix(x2_mm), self.mm2pix(y2_mm)), \ - TRAJECTORY_COLOR_BGR) - def setPose(self, x_mm, y_mm, theta_deg): ''' @@ -167,7 +115,6 @@ class SlamShow(object): Y: forward/back (cm) theta: rotation (degrees) ''' - #remove old arrow self.vehicle.remove() @@ -176,8 +123,8 @@ class SlamShow(object): def _add_vehicle(self, x_mm, y_mm, theta_deg): - #XXX Use a very short arrow shaft to orient the head of the arrow - dx, dy = plt_rotate(0, 0, 1, theta_deg) + #Use a very short arrow shaft to orient the head of the arrow + dx, dy = plt_rotate(0, 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') @@ -193,72 +140,17 @@ class SlamShow(object): # Refresh display, setting flag on window close or keyboard interrupt try: - plt.pause(.01) + plt.pause(.0001) + + return True except: return False - - # Display image - cv.ShowImage(self.window_name, self.image) - - # Force image display, returning False if user hit ESC, True otherwise - key = cvdisplay() - return False if key==27 else True - - def waitkey(self, action): - - print('Hit any key to %s ...' % action) - - key = -1 - - while True: - - key = cvdisplay() - if key > -1: - break - - return key - - - # Puts text in the image to label the velocity display - def show_velocity(self, value, valspan, label, y): - cv.PutText(self.image, label+':', (SENSOR_TEXT_X, y), self.font, SENSOR_LABEL_COLOR_BGR) - bar_x1 = SENSOR_BAR_X + SENSOR_BAR_MAX_HEIGHT - bar_y1 = y + SENSOR_BAR_Y_OFFSET - bar_x2 = bar_x1 + int(value / valspan * SENSOR_BAR_MAX_HEIGHT) - bar_y2 = y - SENSOR_BAR_WIDTH + SENSOR_BAR_Y_OFFSET - bar_color = SENSOR_NEGATIVE_COLOR_BGR if value < 0 else SENSOR_POSITIVE_COLOR_BGR - cv.Rectangle(self.image, (bar_x1, bar_y1), (bar_x2, bar_y2), bar_color, cv.CV_FILLED) - - - # Builds an array of points for a polyline representing the robot, pointing - # rightward and centered at (0,0). - # Currently builds an isoceles triangle pointing rightward - def robot_polyline(self, scale): - - xlft = -ROBOT_HEIGHT / 2 * scale - xrgt = ROBOT_HEIGHT / 2 * scale - ybot = ROBOT_WIDTH / 2 * scale - ytop = -ROBOT_HEIGHT / 2 * scale - return [(xlft,ybot), (xrgt,0), (xlft,ytop)] - - + # Converts millimeters to pixels def mm2pix(self, mm): return int(mm / float(self.map_scale_mm_per_pixel)) # Helpers ------------------------------------------------------------- - -# Forces OpenCV image display, returning id of key it or -1 if none -def cvdisplay(): - return cv.WaitKey(1) - -# Rotates a point by a specified number of degrees -def rotate(pt, deg): - rad = radians(deg) - c = cos(rad) - s = sin(rad) - x,y = pt - return int(x*c - y*s), int(x*s + y*c) def plt_rotate(x, y, r, deg): rad = radians(deg) @@ -266,6 +158,6 @@ def plt_rotate(x, y, r, deg): s = sin(rad) dx = r * c dy = r * s - return x+dx, y+dy - + +