Commit ef180e17 authored by czb5793's avatar czb5793
Browse files

1. Add a callback function for evaluating time used for updates.

2. Record slam data and save as csv files
parent 3df80973
......@@ -118,6 +118,19 @@ class Frame:
'alpha': alpha
})
def add_background_image(self, image, translate):
"""
Adds a background image to the list of objects to be drawn
:param pixbuf: image in RGBA format, (numpy array)
:param translate: (tx, ty), tx: amount to translate in the X direction, ty: amount to translate in the Y direction
"""
self.draw_list.append({
'type': 'bg_image',
'image': image,
'translate': translate
})
......@@ -20,7 +20,9 @@
from math import *
from gui.ColorPalette import *
import gi
gi.require_version('Gtk', '3.0')
from gi.repository import Gdk, GdkPixbuf, GLib
class Painter:
......@@ -38,11 +40,11 @@ class Painter:
:param widget: The widget onto which the frame is drawn
:param context: The cairo context to be used
"""
width_pixels = widget.get_allocated_width()
height_pixels = widget.get_allocated_height()
self.width_pixels = widget.get_allocated_width()
self.height_pixels = widget.get_allocated_height()
# transform the the view to metric coordinates
context.translate(width_pixels / 2.0, height_pixels / 2.0) # move origin to center of window
context.translate(self.width_pixels / 2.0, self.height_pixels / 2.0) # move origin to center of window
context.scale(self.pixels_per_meter,
-self.pixels_per_meter) # pull view to edges of window ( also flips y-axis )
......@@ -87,6 +89,10 @@ class Painter:
component['height'],
component['color'],
component['alpha'])
elif component['type'] == 'bg_image':
self.draw_background_image(context,
component['image'],
component['translate'])
def draw_ellipse(self, context,
pos, angle,
......@@ -181,6 +187,24 @@ class Painter:
context.rectangle(pos[0], pos[1], width, height)
context.fill()
def draw_background_image(self, context, image, translate):
"""
Draws an background_image
:param context:
:param image: Image in RGBA format, (numpy array)
:param alpha: Alpha value of the color
: param translate: (tx, ty), tx: amount to translate in the X direction, ty: amount to translate in the Y direction
"""
context.scale(1/self.pixels_per_meter, 1/self.pixels_per_meter)
h, w, c = image.shape
assert c == 3 or c == 4
pixbuf = self.array_to_pixbuf(image)
tx, ty = translate
Gdk.cairo_set_source_pixbuf(context, pixbuf, -tx, -ty)
context.paint()
context.scale(self.pixels_per_meter,
self.pixels_per_meter)
def set_color(self, cairo_context, color_string, alpha):
"""
Sets a color of the cairo context
......@@ -190,5 +214,21 @@ class Painter:
"""
ColorPalette.dab(cairo_context, color_string, alpha)
def array_to_pixbuf(self, arr):
"""
Convert from numpy array to GdkPixbuf
:param arr: an numpy array of image in RGBA format
:return: GdkPixbuf object
"""
arr = arr.astype('uint8')
h, w, c = arr.shape
assert c == 3 or c == 4
if hasattr(GdkPixbuf.Pixbuf, 'new_from_bytes'):
Z = GLib.Bytes.new(arr.tobytes())
return GdkPixbuf.Pixbuf.new_from_bytes(Z, GdkPixbuf.Colorspace.RGB, c == 4, 8, w, h, w * c)
return GdkPixbuf.Pixbuf.new_from_data(arr.tobytes(), GdkPixbuf.Colorspace.RGB, c == 4, 8, w, h, w * c, None, None)
......@@ -401,8 +401,8 @@ class Viewer:
:param widget: The corresponding widget
"""
if self.simulator.slam_evaluations is not None:
self.simulator.slam_evaluations.plot()
if self.simulator.slam_evaluation is not None:
self.simulator.slam_evaluation.plot()
def on_plot_covariances(self, widget):
......
from math import log
import numpy as np
class MappingPlotter:
def __init__(self, slam_mapping, viewer, frame_number):
......@@ -11,9 +12,10 @@ class MappingPlotter:
self.slam_mapping = slam_mapping
self.viewer = viewer
self.frame_number = frame_number
self.resolution = slam_mapping.resolution # number of pixels per meter
self.pixel_size = 1/self.resolution # pixel size in meters
self.grid_resolution = slam_mapping.resolution # number of grids per meter
self.offset = slam_mapping.offset # offset in meters
self.pixels_per_meter = viewer.pixels_per_meter
self.alpha = 200 # alpha value of color
def draw_mapping_to_frame(self):
"""
......@@ -21,22 +23,18 @@ class MappingPlotter:
"""
self.slam_mapping.update_enabled = True
frame = self.viewer.current_frames[self.frame_number]
H, W = self.slam_mapping.map_shape()
map = self.slam_mapping.get_map()
for j in range(H):
for i in range(W):
val = map[j, i] # the occupancy probability
if abs(val - 0.5) < 0.0001: # unknown area
x, y = self.__to_meter(i, j)
frame.add_rectangle([x, y], self.pixel_size, self.pixel_size, color=(0.5, 0.5, 0.5), alpha=0.3)
elif val > 0.5: # occupied area
x, y = self.__to_meter(i, j)
val = 1-val
frame.add_rectangle([x, y], self.pixel_size, self.pixel_size, color=(val, val, val), alpha=0.8)
map = 255 - map * 255
map.astype(int)
image = self.matrix_to_image(map, self.pixels_per_meter/self.grid_resolution, self.alpha)
tx, ty = self.offset
frame.add_background_image(image, (tx*self.pixels_per_meter, ty*self.pixels_per_meter))
self.draw_path_planning_to_frame(frame)
self._draw_goal_to_frame(frame)
self.draw_path_planning_to_frame(frame)
def draw_path_planning_to_frame(self, frame):
"""
Draw the path to the frame
......@@ -50,6 +48,39 @@ class MappingPlotter:
color="red1",
alpha=0.9)
@staticmethod
def matrix_to_image(m, resolution, alpha):
"""
Convert the grid map to an image
:param m: numpy array in size of H x W with value from 0 - 255
:param resolution: pixels per grid
:param alpha: Alpha value of the color
:return: an image of map in RGBA format
"""
H, W = m.shape
#resolution = resolution
img = np.zeros(( round(H * resolution),
round(W * resolution), 4))
img[:, :, 3] = alpha
for j in range(0, H):
for i in range(0, W):
ii1 = round(i * resolution)
ii2 = round((i+1) * resolution)
jj1 = round(j * resolution)
jj2 = round((j+1) * resolution)
img[jj1:jj2, ii1:ii2, 0:3] = m[j, i]
return img
def pixbuf_from_array(z):
" Convert from numpy array to GdkPixbuf "
z = z.astype('uint8')
h, w, c = z.shape
assert c == 3 or c == 4
if hasattr(GdkPixbuf.Pixbuf, 'new_from_bytes'):
Z = GLib.Bytes.new(z.tobytes())
return GdkPixbuf.Pixbuf.new_from_bytes(Z, GdkPixbuf.Colorspace.RGB, c == 4, 8, w, h, w * c)
return GdkPixbuf.Pixbuf.new_from_data(z.tobytes(), GdkPixbuf.Colorspace.RGB, c == 4, 8, w, h, w * c, None, None)
def _draw_goal_to_frame(self, frame):
"""
Draw the current goal to the frame
......@@ -58,7 +89,7 @@ class MappingPlotter:
goal = self.slam_mapping.supervisor.goal()
frame.add_circle(pos=goal,
radius=0.05,
color="dark green",
color="dark red",
alpha=0.65)
frame.add_circle(pos=goal,
radius=0.01,
......@@ -73,6 +104,6 @@ class MappingPlotter:
:return:
x, y: the position of a point in meters
"""
x = x/self.resolution - self.offset[0]
y = y/self.resolution - self.offset[1]
x = x/self.grid_resolution - self.offset[0]
y = y/self.grid_resolution - self.offset[1]
return x, y
\ No newline at end of file
......@@ -49,7 +49,8 @@ class AStarPlanner(PathPlanner):
goal_node = self.Node(gx, gy, 0, -1, 0)
if self.__vertify_node(start_node) == False or self.__vertify_node(goal_node) == False:
raise ValueError() # start or goal is not valid
# raise ValueError() # start or goal is not valid
return list()
open_set = dict() # open set, i.e. a set of nodes to be evaluated
closed_set = dict() # closed set, i.e. a set of nodes already evaluated
......
......@@ -10,7 +10,7 @@ import math
from scipy.ndimage import gaussian_filter
from math import cos, sin, sqrt
from utils.geometrics_util import bresenham_line
import time
class Mapping:
"""
An abstract class for a mapping algorithm
......@@ -37,33 +37,33 @@ class Mapping:
class OccupancyGridMapping2d(Mapping):
def __init__ (self, slam, slam_cfg, supervisor_interface, path_planner = None):
def __init__ (self, slam, slam_cfg, supervisor_interface, path_planner = None, callback = None):
"""
Initialize the OccupancyGridMapping2d object
:param slam: The underlying slam algorithm object.
:param slam_cfg: The slam configuration.
:param path_planner: An object of PathPlanner
:param viewer_resolution: viewer_resolution, pixels per meters
:param callback: callback function
"""
self.supervisor = supervisor_interface
self.slam = slam
self.path_planner = path_planner
self.width = slam_cfg['mapping']['gridmap']['width'] # width of the map in meters
self.height = slam_cfg['mapping']['gridmap']['height'] # height of the map in meters
self.resolution = slam_cfg['mapping']['gridmap']['resolution'] # resolution of the map, i.e. number of pixels per meter
self.resolution = slam_cfg['mapping']['gridmap']['resolution'] # resolution of the map, i.e. number of grids per meter
self.W = int(self.width * self.resolution) # width of the map in pixels
self.H = int(self.height * self.resolution) # height of the map in pixels
self.offset = (slam_cfg['mapping']['gridmap']['offset']['x'], # offset in meters in horizontal direction
slam_cfg['mapping']['gridmap']['offset']['y']) # offset in meters in vertical directionss
self.offset = (self.width/2, self.height/2)
self.max_range = supervisor_interface.proximity_sensor_max_range()
self.prob_unknown = 0.5 # prior
self.prob_occ = 0.9 # probability perceptual a grid is occupied
self.prob_free = 0.1 # probability perceptual a grid is free
self.prob_occ = 0.8 # probability perceptual a grid is occupied
self.prob_free = 0.2 # probability perceptual a grid is free
self.callback = callback
self.reset() # initialize the algorithm
def reset(self):
"""
reset the map
......@@ -85,6 +85,8 @@ class OccupancyGridMapping2d(Mapping):
if not self.update_enabled:
return
start_time = time.time()
observed_pixs = [] # a list of grid positions and its occupancy probabilities
lines = self.__calc_lines(z)
for x0, y0, x1, y1 in lines:
......@@ -102,7 +104,15 @@ class OccupancyGridMapping2d(Mapping):
self.L = self.L + inverse_sensor - self.L0 # update the recursive term
self.L = np.clip(self.L, -5, 5)
self.update_counter += 1
mapping_time = time.time() - start_time
start_time = time.time()
self.__update_path_planning() # update path planning
path_planning_time = time.time() - start_time
if self.callback is not None:
self.callback(str(self), mapping_time)
self.callback("A Star planning", path_planning_time)
def get_path(self):
"""
......@@ -131,24 +141,28 @@ class OccupancyGridMapping2d(Mapping):
return self.map.shape
def __update_path_planning(self):
occ_threshold = 0.2
if self.path_planner is not None and self.update_counter % 5 ==0 and self.update_counter > 5:
goal = self.supervisor.goal() # get the goal
start = self.slam.get_estimated_pose().sunpack() # get the estimated pose from slam
gx, gy = self.__to_gridmap_position(goal[0], goal[1])
sx, sy = self.__to_gridmap_position(start[0], start[1])
self.map[sy, sx] = 0
if self.map[gy, gx] < occ_threshold:
bool_map = self.blur(self.map)
# bool_map = np.copy(self.map) # calculate a boolean map
bool_map[bool_map >= occ_threshold] = True
bool_map[bool_map < occ_threshold] = False
bool_map = bool_map.astype(np.bool)
bool_map[sy, sx] = False
bool_map[gy, gx] = False
self.path = self.path_planner.planning(sx, sy, gx, gy, bool_map, type='euclidean')
else:
self.path = list()
occ_threshold = 0.1
try:
if self.path_planner is not None and self.update_counter % 5 ==0 and self.update_counter > 5:
goal = self.supervisor.goal() # get the goal
start = self.slam.get_estimated_pose().sunpack() # get the estimated pose from slam
gx, gy = self.__to_gridmap_position(goal[0], goal[1])
sx, sy = self.__to_gridmap_position(start[0], start[1])
self.map[sy, sx] = 0
if self.map[gy, gx] < occ_threshold:
bool_map = self.blur(self.map)
# bool_map = np.copy(self.map) # calculate a boolean map
bool_map[bool_map >= occ_threshold] = True
bool_map[bool_map < occ_threshold] = False
bool_map = bool_map.astype(np.bool)
bool_map[sy, sx] = False
bool_map[gy, gx] = False
self.path = self.path_planner.planning(sx, sy, gx, gy, bool_map, type='euclidean')
else:
self.path = list()
except IndexError:
pass
def __inverse_sensor_model(self, points):
"""
......@@ -182,7 +196,7 @@ class OccupancyGridMapping2d(Mapping):
Blur the map
:return:
"""
return gaussian_filter(image, sigma=1)
return gaussian_filter(image, sigma=2)
def __calc_lines(self, z):
"""
......@@ -250,4 +264,7 @@ class OccupancyGridMapping2d(Mapping):
lm = [0, 0]
lm[0] = x[0] + z[0] * cos(z[1] + x[2])
lm[1] = x[1] + z[0] * sin(z[1] + x[2])
return lm
\ No newline at end of file
return lm
def __str__(self):
return "OccupancyGridMapping2d"
\ No newline at end of file
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment