Commit 99e6a55f authored by czb5793's avatar czb5793
Browse files

Add a "start mapping" button to control whether grid mappings should be executed.

parent d8ea16a9
......@@ -169,7 +169,7 @@ slam:
detected_angle: 30
ekf_slam:
# Determines whether the EKF SLAM algorithm shall be executed
enabled: true
enabled: false
# The mahalanobis distance threshold used in data association
distance_threshold: 1
# Configures the motion noise. The values are currently empirically chosen.
......@@ -224,7 +224,7 @@ slam:
enabled: true
# Determines the interval of when the accuracy of the generated maps is calculated
# A low interval (for example 1) causes performance problems
interval: 20
interval: 18
# Configures the 2D grid map of the occupancy grid mapping algorithm
mapping:
......@@ -237,7 +237,7 @@ slam:
# Height of the map in meters
height: 6
# Number of pixels per meter. The GUI has problem while using grid map of high resolution.
resolution: 10
resolution: 20
# offset the origin in the grid map
offset:
# offset in meters in horizontal direction
......
......@@ -49,6 +49,7 @@ class Viewer:
self.fastslam_enabled = slam_cfg["fast_slam"]["enabled"]
self.graphslambased_enabled = slam_cfg["graph_based_slam"]["enabled"]
self.use_slam_evaluation = slam_cfg["evaluation"]["enabled"]
self.mapping_enabled = slam_cfg["mapping"]["enabled"]
# initialize camera parameters
self.num_frames = num_frames
......@@ -174,6 +175,13 @@ class Viewer:
self.button_plot_graph.set_image_position(gtk.PositionType.LEFT)
self.button_plot_graph.connect('clicked', self.on_plot_graph)
# build the start-mapping button
self.start_mapping = False # controls whether mapping is executed.
self.button_start_mapping = gtk.Button("Start Mapping")
self._decorate_start_mapping_button_inactive()
self.button_start_mapping.set_image_position(gtk.PositionType.LEFT)
self.button_start_mapping.connect('clicked', self.on_start_mapping)
# == lay out the window
labels_box = gtk.HBox(spacing=self.view_width_pixels - 52) # Subtract number of pixels that the text of the labels roughly need
......@@ -210,6 +218,9 @@ class Viewer:
if self.graphslambased_enabled:
information_box.pack_start(self.button_plot_graph, False, False, 0)
if self.mapping_enabled:
information_box.pack_start(self.button_start_mapping, False, False, 0)
if num_frames > 1 and self.use_slam_evaluation:
information_box.pack_start(self.button_slam_evaluation, False, False, 0)
......@@ -412,6 +423,20 @@ class Viewer:
"""
self.simulator.graphbasedslam_plotter.plot_graph()
def on_start_mapping(self, widget):
"""
Callback function that handles a click on the "Start Mapping" button
:param widget: The corresponding widget
:return:
"""
self.start_mapping = not self.start_mapping
if self.start_mapping:
self._decorate_start_mapping_button_active()
self.simulator.draw_world()
else:
self._decorate_start_mapping_button_inactive()
self.simulator.reset_grid_map()
def on_expose1(self, widget, context):
"""
......@@ -472,3 +497,21 @@ class Viewer:
draw_invisibles_image.set_from_stock(gtk.STOCK_ADD, gtk.IconSize.BUTTON)
self.button_draw_invisibles.set_image(draw_invisibles_image)
self.button_draw_invisibles.set_label('Show Invisibles')
def _decorate_start_mapping_button_inactive(self):
"""
Specifies the "Start Mapping" button while it is disabled
"""
draw_invisibles_image = gtk.Image()
draw_invisibles_image.set_from_stock(gtk.STOCK_ADD, gtk.IconSize.BUTTON)
self.button_start_mapping.set_image(draw_invisibles_image)
self.button_start_mapping.set_label('Start Mapping')
def _decorate_start_mapping_button_active(self):
"""
Specifies the "Start Mapping" button while it is enabled
"""
draw_invisibles_image = gtk.Image()
draw_invisibles_image.set_from_stock(gtk.STOCK_ADD, gtk.IconSize.BUTTON)
self.button_start_mapping.set_image(draw_invisibles_image)
self.button_start_mapping.set_label('Reset Mapping')
\ No newline at end of file
......@@ -18,31 +18,31 @@ class MappingPlotter:
"""
Draw the occupancy mapping estimated by the mapping algorithm to the frame
"""
if self.viewer.draw_invisibles == 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 val >= 0.5:
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.5)
self.draw_path_planning_to_frame(frame)
self._draw_goal_to_frame(frame)
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 val >= 0.5:
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.5)
self.draw_path_planning_to_frame(frame)
self._draw_goal_to_frame(frame)
def draw_path_planning_to_frame(self, frame):
"""
Draw the path to the frame
:param frame: The frame to be used
"""
if self.viewer.draw_invisibles == True:
path = self.slam_mapping.get_path()
if len(path) > 1:
frame.add_lines([path],
linewidth=0.010,
color="red1",
alpha=0.9)
path = self.slam_mapping.get_path()
if len(path) > 1:
frame.add_lines([path],
linewidth=0.010,
color="red1",
alpha=0.9)
def _draw_goal_to_frame(self, frame):
"""
......
......@@ -59,6 +59,16 @@ class OccupancyGridMapping2d(Mapping):
self.prob_unknown = 0.5 # prior
self.prob_occ = 0.99 # probability perceptual a grid is occupied
self.prob_free = 0.01 # probability perceptual a grid is free
self.reset() # initialize the algorithm
def reset(self):
"""
reset the map
"""
self.update_enabled = False
self.map = np.full((self.H, self.W), self.prob_unknown, dtype=np.float32)
self.L = self.__prob2log(np.full_like(self.map, self.prob_unknown, dtype=np.float32)) # log recursive term of the map
self.L0 = self.__prob2log(np.full_like(self.map, self.prob_unknown, dtype=np.float32)) # log prior term of the map
......@@ -72,6 +82,9 @@ class OccupancyGridMapping2d(Mapping):
Update the path planning
:param z: Measurement, represented as tuple of measured distance and measured angle
"""
if not self.update_enabled:
return
observed_pixs = [] # a list of grid positions and its occupancy probabilities
lines = self.__calc_lines(z)
for x0, y0, x1, y1 in lines:
......
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