Commit 0c2ea0b5 authored by czb5793's avatar czb5793
Browse files

Small changes in order to make the code more manageable.

parent 99e6a55f
......@@ -143,6 +143,12 @@ class Simulator:
self.cfg["slam"]["evaluation"], self.world.robots[0])
n_frame += 1
# register mapping plotters to the system
self.reg_mapping_plotters = [self.ekfslam_mapping_plotter, self.fastslam_mapping_plotter,
self.graphbasedslam_mapping_plotter]
# register slam plotters to the system
self.reg_slam_plotters = [self.ekfslam_plotter, self.fastslam_plotter, self.graphbasedslam_plotter]
# render the initial world
self.draw_world()
......@@ -203,23 +209,22 @@ class Simulator:
self.pause_sim()
self.initialize_sim(random=True)
def reset_grid_map(self):
for robot in self.world.robots:
robot.supervisor.reset_grid_mapping()
def draw_world(self):
self.viewer.new_frame() # start a fresh frame
self.world_plotter.draw_world_to_frame() # draw the world onto the frame
if self.ekfslam_mapping_plotter is not None:
self.ekfslam_mapping_plotter.draw_mapping_to_frame()
if self.fastslam_mapping_plotter is not None:
self.fastslam_mapping_plotter.draw_mapping_to_frame()
if self.graphbasedslam_mapping_plotter is not None:
self.graphbasedslam_mapping_plotter.draw_mapping_to_frame()
if self.ekfslam_plotter is not None:
self.ekfslam_plotter.draw_slam_to_frame()
if self.fastslam_plotter is not None:
self.fastslam_plotter.draw_slam_to_frame()
if self.graphbasedslam_plotter is not None:
self.graphbasedslam_plotter.draw_slam_to_frame()
# draw occupancy grid maps to the frame
if self.viewer.start_mapping:
for plotter in self.reg_mapping_plotters:
if plotter is not None:
plotter.draw_mapping_to_frame()
# draw the slam estimations to the frame
for plotter in self.reg_slam_plotters:
if plotter is not None:
plotter.draw_slam_to_frame()
self.viewer.draw_frame() # render the frame
......
......@@ -105,6 +105,10 @@ class Supervisor:
path_planner = AStarPlanner() if cfg["slam"]["mapping"]["path_planning"]["enabled"] else None
self.graphbasedslam_mapping = OccupancyGridMapping2d(self.graphbasedslam, cfg["slam"], controller_interface, path_planner)
# register slam objects and mapping objects to the system
self.reg_slam = [self.ekfslam, self.fastslam, self.graphbasedslam]
self.reg_mapping = [self.ekfslam_mapping, self.fastslam_mapping, self.graphbasedslam_mapping]
# state machine
self.state_machine = SupervisorStateMachine(self, self.control_cfg)
......@@ -229,20 +233,20 @@ class Supervisor:
sensor_angles = [pose.theta for pose in self.proximity_sensor_placements]
landmark_ids = self.robot.read_landmark_matcher()
# update LAM estimations
if self.ekfslam is not None:
self.ekfslam.update(motion_command, zip(measured_distances, sensor_angles, landmark_ids))
if self.fastslam is not None:
self.fastslam.update(motion_command, zip(measured_distances, sensor_angles, landmark_ids))
if self.graphbasedslam is not None:
self.graphbasedslam.update(motion_command, zip(measured_distances, sensor_angles, landmark_ids))
# update mappings
if self.time > 500:
if self.ekfslam_mapping is not None:
self.ekfslam_mapping.update(zip(measured_distances, sensor_angles))
if self.fastslam_mapping is not None:
self.fastslam_mapping.update(zip(measured_distances, sensor_angles))
if self.graphbasedslam_mapping is not None:
self.graphbasedslam_mapping.update(zip(measured_distances, sensor_angles))
for slam in self.reg_slam:
if slam is not None:
slam.update(motion_command, zip(measured_distances, sensor_angles, landmark_ids))
# update mapping estimations
for mapping in self.reg_mapping:
if mapping is not None:
mapping.update(zip(measured_distances, sensor_angles))
def reset_grid_mapping(self):
# reset mappings
for mapping in self.reg_mapping:
if mapping is not None:
mapping.reset()
def _send_robot_commands(self):
"""
......
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