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

Removed all elements of OpenCV from file, removed the wait key function that allowed for graceful exit from OpenCV

Browse Source 
Removed elements of OpenCV from files
This commit is contained in:
Matt Lubas
2016-08-15 11:46:53 -04:00
parent 92a45c38ae
commit 2085647748
2 changed files with 9 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
examples/logdemo.py
View File

@@ -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)

126
examples/pltslamshow.py
View File

@@ -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