Skip to content

GitLab

Commit 851182e7 authored by czb5793's avatar czb5793
Browse files

Small changes

parent 45ad4a04
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 1919 additions and 2619 deletions
config.yaml 100755 → 100644
View file @ 851182e7
......@@ -34,9 +34,9 @@ robot:
motor:
noise:
# Standard deviation of the left wheel's velocity in meters/second while executing a motion command.
left_velocity: 0.005 # 0.005
left_velocity: 0.05 # 0.005, 0.05
# Standard deviation of the right wheel's velocity in meters/second while executing a motion command.
right_velocity: 0.005
right_velocity: 0.05
# Configures the sensors used by the robot
sensor:
......@@ -48,8 +48,8 @@ robot:
min_read_value: 18
# Value that is read at minimum range
max_read_value: 3960
# Standard deviation of the distance from the sensors to the detected landmark.
noise: 0.0001 # 0.0001
# Standard deviation of the distance from the sensors to the detected landmark in meters.
noise: 0.005 # 0.0001 0.005
# Specificies the poses and number of sensors
# Each pose is composed of x position, y position and angle in degrees
poses: [[-0.038, 0.048, 128],
......@@ -78,9 +78,9 @@ robot:
# Configures the GUI
viewer:
# Number of pixels per row of a frame
pixels_width: 500
pixels_width: 800
# Number of pixels per column of a frame
pixels_height: 600
pixels_height: 800
# Pixels per meter
zoom: 100
# Distance in meters of the major gridlines
......@@ -92,20 +92,24 @@ viewer:
map:
# Configures the generated obstacles
obstacle:
# Configures octagon obstacles
octagon:
feature:
# Determines whether octagon obstacles shall be generated
enabled: true
# Radius of obstacles 0.04
radius: 0.7
# Minimum amount of generated obstacles 25
min_count: 4
# Maximum amount of generated obstacles 30
max_count: 6
# Radius of feature points
radius: 0.04
# Density of features
density: 0.15
# The following attributes are used if rectangles are disable
# Minimum amount of generated obstacles
min_count: 80
# Maximum amount of generated obstacles
max_count: 150
# Minimum distance to origin
min_distance: 0.2
# Maximum distance to origin
max_distance: 3.5
max_distance: 2.5
# Configures rectangle obstacles
# Configures rectangle obstacles
rectangle:
# Determines whether rectangle obstacles shall be generated
......@@ -113,17 +117,17 @@ map:
# Minimum side length of a rectangle
min_dim: 0.4
# Maximum side length of a rectangle
max_dim: 2.5
max_dim: 4.5
# Maximum sum of the two side lengths of a rectangle
max_combined_dim: 2.9
max_combined_dim: 4.9
# Minimum amount of generated obstacles
min_count: 4
min_count: 5
# Maximum amount of generated obstacles
max_count: 7
max_count: 15
# Minimum distance to origin
min_distance: 0.4
min_distance: 0.5
# Maximum distance to origin
max_distance: 3
max_distance: 4
# Configures the goal generation
goal:
# Determines if a new goal is generated once the robot reaches its current goal
......@@ -131,7 +135,7 @@ map:
# Minimum distance to origin
min_distance: 0
# Maximum distance to origin
max_distance: 2
max_distance: 3.5
# Minimum distance to all obstacles
min_clearance: 0.2
......@@ -140,14 +144,13 @@ control:
# If robot is closer than this distance to the goal, it is considered as reached
goal_reached_distance: 0.05
# If a sensor measures a distance smaller than the danger distance, the robot immediately starts moving into the opposite direction
danger_distance: 0.04 # 0.06
danger_distance: 0.06 # 0.06
# If a sensor measures a distance smaller than the caution distance, the robot will follow the wall of the obstacle
# Set to danger_distance to disable wall following, since the map contains small, circle-like objects, where wall following can lead to looping around an object
caution_distance: 0.15 # 0.06
caution_distance: 0.06 # a. 0.06; b. 0.15
# Criterion for stopping the following of a wall
progress_epsilon: 0.05 # 0.05
progress_epsilon: 0.05 # a. 0.05; b. 0.2
# Configures the SLAM system
slam:
# The amount of variables that describe the robot's state
# These are x position, y position and current angle theta
......@@ -161,28 +164,28 @@ slam:
# It determines whether correspondences of landmarks are given, i.e. identifies of landmarks are given
feature_detector: true
# Configures the sensor noise. The values are currently empirically chosen.
sensor_noise:
# Standard deviation of the detected distance in meters 0.2
detected_distance: 0.01
# Standard deviation of the detected angle in degrees
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.
motion_noise:
# Standard deviation of the robots x-coordinate in meters after executing a motion command.
x: 0.005
x: 0.0002 # 0.0005
# Standard deviation of the robots y-coordinate in meters after executing a motion command.
y: 0.005
y: 0.0002 # 0.0005
# Standard deviation of the robots angle in degrees after executing a motion command.
theta: 1
theta: 0.1
# Configures the sensor noise. The values are currently empirically chosen.
sensor_noise:
# Standard deviation of the detected distance in meters 0.2 0.01
detected_distance: 0.2 # 0.008
# Standard deviation of the detected angle in degrees
detected_angle: 30
fast_slam:
# Determines whether the FastSLAM algorithm shall be executed
enabled: true
enabled: false
# The mahalanobis distance threshold used in data association
distance_threshold: 0.125
# The number of used particles
......@@ -190,37 +193,46 @@ slam:
# Configures the motion noise. The values are currently empirically chosen.
motion_noise:
# Standard deviation of the motion command's translational velocity in m/s.
translational_velocity: 0.005 #0.005
translational_velocity: 0.01 #0.005
# Standard deviation of the motion command's rotational velocity in rad/s.
rotational_velocity: 0.005 #0.005
rotational_velocity: 0.0001 #0.005
# Configures the sensor noise. The values are currently empirically chosen.
sensor_noise:
# Standard deviation of the detected distance in meters 0.2 0.01
detected_distance: 0.2 # 0.008
# Standard deviation of the detected angle in degrees
detected_angle: 30
graph_based_slam:
# Determines whether the Graph-based SLAM algorithm shall be executed
enabled: true
# The euclidean distance threshold [m] used in data association.
# If the distance between the estimated landmarks via slam and via actual measurement
# larger than the threshold, start backend
distance_threshold: 0.2
# The number interval of pose-vertices added that the graph optimization is executed.
optimization_interval: 50
distance_threshold: 0.1
# The timestep interval of executing the frontend part.
frontend_interval: 5
# Pose density of frontend, meaning the minimum distance [m] between the current pose and the last pose.
frontend_pose_density: 0.1
# number of fixed vertexes while the graph optimization
num_fixed_vertexes: 20
# draw trajectory
draw_trajectory: true
# sparse solver: cholesky or spsolve
solver: "cholesky"
# Configures the motion noise. The values are currently empirically chosen.
motion_noise:
# Standard deviation of the robots x-coordinate in meters after executing a motion command.
x: 0.004 # 0.005
x: 0.01 # 0.005 0.01
# Standard deviation of the robots y-coordinate in meters after executing a motion command.
y: 0.004 # 0.005
y: 0.01 # 0.005
# Standard deviation of the robots angle in degrees after executing a motion command in degrees.
theta: 1.0 #0.09
# Configures the sensor noise. The values are currently empirically chosen.
sensor_noise:
#x: 0.02 # 0.02 constraint range-bearing
#y: 0.5236 # 0.02
#y: 0.5236 # rad/s
x: 0.02 # 0.02 constraint x-y
y: 0.02 # 0.02 # rad/s
y: 0.02 # 0.02
# Configures the evaluation of the SLAM algorithms
evaluation:
......@@ -230,8 +242,8 @@ slam:
# A low interval (for example 1) causes performance problems
interval: 18
# Determine the method of data association,
# if true, data wil be associated by landmark identifiers, otherwise by minimum distance
associate_by_id: true
# if true, landmark identifiers will be associated, otherwise by minimum distance
associate_id: true
# Configures the 2D grid map of the occupancy grid mapping algorithm
mapping:
......@@ -240,9 +252,9 @@ slam:
# Width of the map in meters
gridmap:
# Width of the map in meters
width: 5
width: 8
# Height of the map in meters
height: 6
height: 8
# Number of grids per meter. Note that high resolution will lead to performance issues.
resolution: 20
......@@ -250,9 +262,4 @@ slam:
# Determines whether the path planning algorithm shall be executed
enabled: true
# Determines how importance the heuristic term is.
heuristic_weight: 1.0
heuristic_weight: 1.0
\ No newline at end of file
config_graph_based_slam.yaml → config01.yaml 100644 → 100755
View file @ 851182e7
......@@ -34,9 +34,9 @@ robot:
motor:
noise:
# Standard deviation of the left wheel's velocity in meters/second while executing a motion command.
left_velocity: 0.00005 # 0.005, 0.05
left_velocity: 0.005 # 0.005
# Standard deviation of the right wheel's velocity in meters/second while executing a motion command.
right_velocity: 0.00005
right_velocity: 0.005
# Configures the sensors used by the robot
sensor:
......@@ -48,8 +48,8 @@ robot:
min_read_value: 18
# Value that is read at minimum range
max_read_value: 3960
# Standard deviation of the distance from the sensors to the detected landmark in meters.
noise: 0.000005 # 0.0001 0.005
# Standard deviation of the distance from the sensors to the detected landmark.
noise: 0.0001 # 0.0001
# Specificies the poses and number of sensors
# Each pose is composed of x position, y position and angle in degrees
poses: [[-0.038, 0.048, 128],
......@@ -78,9 +78,9 @@ robot:
# Configures the GUI
viewer:
# Number of pixels per row of a frame
pixels_width: 800
pixels_width: 500
# Number of pixels per column of a frame
pixels_height: 800
pixels_height: 600
# Pixels per meter
zoom: 100
# Distance in meters of the major gridlines
......@@ -92,6 +92,7 @@ viewer:
map:
# Configures the generated obstacles
obstacle:
# Configures features
feature:
# Determines whether octagon obstacles shall be generated
enabled: true
......@@ -99,38 +100,34 @@ map:
radius: 0.04
# Density of features
density: 0.15
# Configures octagon obstacles
octagon:
# Determines whether octagon obstacles shall be generated
enabled: false
# Radius of obstacles
radius: 1
# The following attributes are used if rectangles are disable
# Minimum amount of generated obstacles
min_count: 4
min_count: 40
# Maximum amount of generated obstacles
max_count: 6
max_count: 60
# Minimum distance to origin
min_distance: 0.2
# Maximum distance to origin
max_distance: 3.5
max_distance: 2.5
# Configures rectangle obstacles
# Configures rectangle obstacles
rectangle:
# Determines whether rectangle obstacles shall be generated
enabled: true
enabled: false
# Minimum side length of a rectangle
min_dim: 0.4
# Maximum side length of a rectangle
max_dim: 4.5
max_dim: 2.5
# Maximum sum of the two side lengths of a rectangle
max_combined_dim: 4.9
max_combined_dim: 2.9
# Minimum amount of generated obstacles
min_count: 5
min_count: 4
# Maximum amount of generated obstacles
max_count: 15
max_count: 7
# Minimum distance to origin
min_distance: 1.4
min_distance: 0.4
# Maximum distance to origin
max_distance: 4
max_distance: 3
# Configures the goal generation
goal:
# Determines if a new goal is generated once the robot reaches its current goal
......@@ -138,7 +135,7 @@ map:
# Minimum distance to origin
min_distance: 0
# Maximum distance to origin
max_distance: 3.5
max_distance: 2
# Minimum distance to all obstacles
min_clearance: 0.2
......@@ -147,10 +144,10 @@ control:
# If robot is closer than this distance to the goal, it is considered as reached
goal_reached_distance: 0.05
# If a sensor measures a distance smaller than the danger distance, the robot immediately starts moving into the opposite direction
danger_distance: 0.04 # 0.06
danger_distance: 0.06 # 0.06
# If a sensor measures a distance smaller than the caution distance, the robot will follow the wall of the obstacle
# Set to danger_distance to disable wall following, since the map contains small, circle-like objects, where wall following can lead to looping around an object
caution_distance: 0.15 # 0.06
caution_distance: 0.06 # 0.06
# Criterion for stopping the following of a wall
progress_epsilon: 0.05 # 0.05
......@@ -164,73 +161,77 @@ slam:
# These are x position and y position
# Currently only supports 2
landmark_state_size: 2
# Determines whether landmark-identifiers is used for slam estimate
# It determines whether correspondences are used,
# i.e. true for known data association, false for unknown data association.
# Determines whether landmark-identifiers is used
# It determines whether correspondences of landmarks are given, i.e. identifies of landmarks are given
feature_detector: true
# Configures the sensor noise. The values are currently empirically chosen.
sensor_noise:
# Standard deviation of the detected distance in meters 0.2
detected_distance: 0.01
# Standard deviation of the detected angle in degrees
detected_angle: 30
ekf_slam:
# Determines whether the EKF SLAM algorithm shall be executed
enabled: false
enabled: true
# The mahalanobis distance threshold used in data association
distance_threshold: 1
# Configures the motion noise. The values are currently empirically chosen.
motion_noise:
# Standard deviation of the robots x-coordinate in meters after executing a motion command.
x: 0.005
x: 0.01 # 0.0005
# Standard deviation of the robots y-coordinate in meters after executing a motion command.
y: 0.005
y: 0.01 # 0.0005
# Standard deviation of the robots angle in degrees after executing a motion command.
theta: 1
# Configures the sensor noise. The values are currently empirically chosen.
sensor_noise:
# Standard deviation of the detected distance in meters 0.2 0.01
detected_distance: 0.2
# Standard deviation of the detected angle in degrees
detected_angle: 30
fast_slam:
# Determines whether the FastSLAM algorithm shall be executed
enabled: false
enabled: true
# The mahalanobis distance threshold used in data association
distance_threshold: 0.125
# The number of used particles
n_particles: 150 # 80
n_particles: 200 # 80
# Configures the motion noise. The values are currently empirically chosen.
motion_noise:
# Standard deviation of the motion command's translational velocity in m/s.
# Standard deviation of the motion command's translational velocity in m/s. # 0.02,0.015
translational_velocity: 0.005 #0.005
# Standard deviation of the motion command's rotational velocity in rad/s.
rotational_velocity: 0.005 #0.005
graph_based_slam:
# Determines whether the Graph-based SLAM algorithm shall be executed
enabled: true
# The euclidean distance threshold [m] used in data association.
# If the distance between the estimated landmarks via slam and via actual measurement
# larger than the threshold, start backend
distance_threshold: 0.2
# The timestep interval of executing the frontend part.
frontend_interval: 5
# Pose density of frontend, meaning the minimum distance [m] between the current pose and the last pose.
frontend_pose_density: 0.1
# number of fixed vertexes while the graph optimization
num_fixed_vertexes: 20
# draw trajectory
draw_trajectory: false
# Configures the motion noise. The values are currently empirically chosen.
motion_noise:
# Standard deviation of the robots x-coordinate in meters after executing a motion command.
x: 0.01 # 0.005
# Standard deviation of the robots y-coordinate in meters after executing a motion command.
y: 0.01 # 0.005
# Standard deviation of the robots angle in degrees after executing a motion command in degrees.
theta: 1.0 #0.09
rotational_velocity: 0.003 #0.005 0.001
# Configures the sensor noise. The values are currently empirically chosen.
sensor_noise:
#x: 0.02 # 0.02 constraint range-bearing
#y: 0.5236 # rad/s
x: 0.02 # 0.02 constraint x-y
y: 0.02 # 0.02
# Standard deviation of the detected distance in meters 0.2 0.01
detected_distance: 0.2 # 0.008
# Standard deviation of the detected angle in degrees
detected_angle: 30
graph_based_slam:
# Determines whether the Graph-based SLAM algorithm shall be executed
enabled: true
# The euclidean distance threshold [m] used in data association.
# If the distance between the estimated landmarks via slam and via actual measurement
# larger than the threshold, start backend
distance_threshold: 0.1 #0.2
# The timestep interval of executing the frontend part in simulation cycles.
frontend_interval: 5
# Pose density of frontend, meaning the minimum distance [m] between the current pose and the last pose.
frontend_pose_density: 0.1
# number of fixed vertexes while the graph optimization.
num_fixed_vertexes: 20
# draw trajectory on the frame
draw_trajectory: false
# sparse solver: cholesky or spsolve
solver: "cholesky"
# Configures the motion noise. The values are currently empirically chosen.
motion_noise:
# Standard deviation of the robots x-coordinate in meters after executing a motion command.
x: 0.005
# Standard deviation of the robots y-coordinate in meters after executing a motion command.
y: 0.005
# Standard deviation of the robots angle in degrees after executing a motion command in degrees.
theta: 1.0 #0.09
# Configures the sensor noise. The values are currently empirically chosen.
sensor_noise:
x: 0.02
y: 0.02
# Configures the evaluation of the SLAM algorithms
evaluation:
......@@ -240,7 +241,7 @@ slam:
# A low interval (for example 1) causes performance problems
interval: 18
# Determine the method of data association,
# if true, landmark identifiers will be associated, otherwise by minimum distance
# if true, data wil be associated by landmark identifiers, otherwise by minimum distance
associate_id: true
# Configures the 2D grid map of the occupancy grid mapping algorithm
......@@ -250,9 +251,9 @@ slam:
# Width of the map in meters
gridmap:
# Width of the map in meters
width: 8
width: 5
# Height of the map in meters
height: 8
height: 6
# Number of grids per meter. Note that high resolution will lead to performance issues.
resolution: 20
......@@ -260,4 +261,9 @@ slam:
# Determines whether the path planning algorithm shall be executed
enabled: true
# Determines how importance the heuristic term is.
heuristic_weight: 1.0
\ No newline at end of file
heuristic_weight: 1.0
maps/slam_example_1
View file @ 851182e7
No preview for this file type
maps/slam_example_3
View file @ 851182e7
No preview for this file type
readme.md
View file @ 851182e7
......@@ -36,7 +36,7 @@ which uses the default `config.yaml` configuration file. A specific configuratio
specified as an additional program parameter:
python rimulator.py original_config.yaml
python rimulator.py config_ekf_fastslam.yaml
python rimulator.py config01.yaml
Alternatively, the simulator can be run using `docker`, as described in [documentation/docker.md](documentation/docker.md).
......@@ -82,9 +82,9 @@ over the course of the simulation. Only displayed if the SLAM evaluation is enab
representing the robot poses and landmark-vertices representing the landmark positions.
## Configuration
The simulator can be configured by a variety of parameters. The default configuration file is [config.yaml](config.yaml)
The simulator can be configured by a variety of parameters. The default configuration file is [config.yaml](config01.yaml)
where all parameters are documented. This file includes the EKF Slam, FastSlam, Graph based Slam, Occupancy gird mapping and A* path planning that can be enabled or
disabled. The configuration file [config_ekf_fastslam.yaml](config_ekf_fastslam.yaml) includes
disabled. The configuration file [config01.yaml](config01.yaml) includes
an extension performing completely identical to the [sobot rimulator](https://collaborating.tuhh.de/cva9931/sobot-rimulator) of Michael Dobler. The configuration file [original_config.yaml](original_config.yaml) does not include
any of the extensions made and performs completely identical to the original sobot rimulator.
......
rimulator.py
View file @ 851182e7
......@@ -295,8 +295,8 @@ class Simulator:
if __name__ == "__main__":
#filename = "config_graph_based_slam.yaml" if len(sys.argv) == 1 else sys.argv[1]
filename = "config.yaml" if len(sys.argv) == 1 else sys.argv[1]
#filename = "config.yaml" if len(sys.argv) == 1 else sys.argv[1]
filename = "config01.yaml" if len(sys.argv) == 1 else sys.argv[1]
with open(filename, 'r') as ymlfile:
cfg = yaml.safe_load(ymlfile)
......
scripts/Analysis.ipynb
View file @ 851182e7
This diff is collapsed.
scripts/test_graph_performance.ipynb
View file @ 851182e7
This diff is collapsed.
supervisor/slam/mapping.py deleted 100644 → 0
View file @ 45ad4a04
"""
This file includes
- occupancy grid mapping algorithm
- path planning algorithm
"""
import numpy as np
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
"""
def update(self, z):
"""
Update the map and the path using mapping and path planning algorithms
:param z: Measurements data from sensors
"""
raise NotImplementedError()
def get_map(self):
"""
Returns the estimated occupancy map
"""
raise NotImplementedError()
def get_path(self):
"""
Returns the estimated path of planning
"""
raise NotImplementedError()
class OccupancyGridMapping2d(Mapping):
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 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 = (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.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
"""
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