Commit 15e3df18 authored by czb5793's avatar czb5793
Browse files

Small changes

parent f16d822f
......@@ -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.1 # 0.0001
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.1 # 0.0001
right_velocity: 0.005
# Configures the sensors used by the robot
sensor:
......@@ -97,33 +97,33 @@ map:
# Determines whether octagon obstacles shall be generated
enabled: true
# Radius of obstacles 0.04
radius: 1
radius: 0.7
# Minimum amount of generated obstacles 25
min_count: 2
min_count: 4
# Maximum amount of generated obstacles 30
max_count: 2
max_count: 6
# Minimum distance to origin
min_distance: 0.2
# Maximum distance to origin
max_distance: 2
max_distance: 3.5
# Configures rectangle obstacles
rectangle:
# Determines whether rectangle obstacles shall be generated
enabled: true
# Minimum side length of a rectangle
min_dim: 0.5
min_dim: 0.4
# Maximum side length of a rectangle
max_dim: 2.5
# Maximum sum of the two side lengths of a rectangle
max_combined_dim: 3.0
max_combined_dim: 2.9
# Minimum amount of generated obstacles
min_count: 2
min_count: 4
# Maximum amount of generated obstacles
max_count: 4
max_count: 7
# Minimum distance to origin
min_distance: 0.4
# Maximum distance to origin
max_distance: 6
max_distance: 3
# Configures the goal generation
goal:
# Determines if a new goal is generated once the robot reaches its current goal
......@@ -169,7 +169,7 @@ slam:
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.
......@@ -182,17 +182,17 @@ slam:
theta: 1
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: 80
n_particles: 150 # 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.
translational_velocity: 0.005
translational_velocity: 0.005 #0.005
# Standard deviation of the motion command's rotational velocity in rad/s.
rotational_velocity: 0.005
rotational_velocity: 0.005 #0.005
graph_based_slam:
# Determines whether the Graph-based SLAM algorithm shall be executed
......@@ -213,19 +213,19 @@ slam:
x: 0.004 # 0.005
# Standard deviation of the robots y-coordinate in meters after executing a motion command.
y: 0.004 # 0.005
# Standard deviation of the robots angle in degrees after executing a motion command.
# 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
x: 0.02 # 0.02 constraint x-y
y: 0.02 # 0.02
y: 0.02 # 0.02 # rad/s
# Configures the evaluation of the SLAM algorithms
evaluation:
# Determines whether the accuracy of the generated maps shall be evaluated
enabled: True
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: 18
......
......@@ -200,6 +200,8 @@ slam:
optimization_interval: 100
# The timestep interval of executing the frontend part.
frontend_interval: 5
# number of fixed vertexes while the graph optimization
num_fixed_vertexes: 20
# 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.
......
# Disrecte time that elapses during one simulation cycle
period: 0.1
# Configures the robot
# These values are inspired by the Khepera III robot
robot:
# Maximum translational velocity in m/s
max_transl_vel: 0.3148
# Maximum angular velocity in rad/s
max_ang_vel: 2.2763
# Polygon specifying the shape of the robot
bottom_plate: [[-0.024, 0.064],
[0.033, 0.064],
[0.057, 0.043],
[0.074, 0.010],
[0.074, -0.010],
[0.057, -0.043],
[0.033, -0.064],
[-0.025, -0.064],
[-0.042, -0.043],
[-0.048, -0.010],
[-0.048, 0.010],
[-0.042, 0.043]]
# Polygon specifying the top view of the robot, used solely for plotting
top_plate: [[-0.031, 0.043],
[-0.031, -0.043],
[0.033, -0.043],
[0.052, -0.021],
[0.057, 0.000],
[0.052, 0.021],
[0.033, 0.043]]
# Configures the motor used by the 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
# Standard deviation of the right wheel's velocity in meters/second while executing a motion command.
right_velocity: 0.00005
# Configures the sensors used by the robot
sensor:
# Minimum detection range of the sensor in meters
min_range: 0.02
# Maximum detection range of the sensor in meters
max_range: 0.2
# Value that is read at maximum range
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
# 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],
[0.019, 0.064, 75],
[0.050, 0.050, 42],
[0.070, 0.017, 13],
[0.070, -0.017, -13],
[0.050, -0.050, -42],
[0.019, -0.064, -75],
[-0.038, -0.048, -128],
[-0.048, 0.000, 180]]
# Degree of the plotted cone angle of the sensor
# Careful: the sensor only has a single detection line! The cone is purely for plotting.
cone_angle: 10
# Configures the wheels of the robot
wheel:
# Wheel radius
radius: 0.021
# Distance between the two centers of the wheels
base_length: 0.0885
# Value returned by wheel odometry per revolution
ticks_per_rev: 2765
# Maximum speed of the wheel in rad/s
max_speed: 15.0
# Configures the GUI
viewer:
# Number of pixels per row of a frame
pixels_width: 800
# Number of pixels per column of a frame
pixels_height: 800
# Pixels per meter
zoom: 100
# Distance in meters of the major gridlines
major_gridline_interval: 1
# Between the major gridline there are this many minor divisions
major_gridline_subdivisions: 5
# Configures the random map generation
map:
# Configures the generated obstacles
obstacle:
feature:
# Determines whether octagon obstacles shall be generated
enabled: true
# Radius of feature points
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
# Minimum amount of generated obstacles
min_count: 4
# Maximum amount of generated obstacles
max_count: 6
# Minimum distance to origin
min_distance: 0.2
# Maximum distance to origin
max_distance: 3.5
# Configures rectangle obstacles
rectangle:
# Determines whether rectangle obstacles shall be generated
enabled: true
# Minimum side length of a rectangle
min_dim: 0.4
# Maximum side length of a rectangle
max_dim: 4.5
# Maximum sum of the two side lengths of a rectangle
max_combined_dim: 4.9
# Minimum amount of generated obstacles
min_count: 5
# Maximum amount of generated obstacles
max_count: 15
# Minimum distance to origin
min_distance: 1.4
# Maximum distance to origin
max_distance: 4
# Configures the goal generation
goal:
# Determines if a new goal is generated once the robot reaches its current goal
endless: true
# Minimum distance to origin
min_distance: 0
# Maximum distance to origin
max_distance: 3.5
# Minimum distance to all obstacles
min_clearance: 0.2
# Configures the control of the robot
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
# 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
# Criterion for stopping the following of a wall
progress_epsilon: 0.05 # 0.05
# 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
# Currently only supports 3
robot_state_size: 3
# The amount of variables that describe a landmark's state
# 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.
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
# 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
# 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.
theta: 1
fast_slam:
# Determines whether the FastSLAM algorithm shall be executed
enabled: false
# The mahalanobis distance threshold used in data association
distance_threshold: 0.125
# The number of used particles
n_particles: 150 # 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.
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
# 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
# Configures the evaluation of the SLAM algorithms
evaluation:
# Determines whether the accuracy of the generated maps shall be evaluated
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: 18
# Determine the method of data association,
# 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:
# Determines whether the mapping algorithm shall be executed
enabled: true
# Width of the map in meters
gridmap:
# Width of the map in meters
width: 8
# Height of the map in meters
height: 8
# Number of grids per meter. Note that high resolution will lead to performance issues.
resolution: 20
path_planning:
# 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
......@@ -27,7 +27,7 @@ from models.Polygon import Polygon
from models.obstacles.RectangleObstacle import RectangleObstacle
from models.obstacles.FeaturePoint import FeaturePoint
from utils import linalg2_util as linalg
seed(42)
#seed(42)
class MapManager:
......@@ -51,8 +51,8 @@ class MapManager:
obstacles += self.__generate_octagon_obstacles(world)
if self.cfg["obstacle"]["rectangle"]["enabled"]:
obstacles += self.__generate_rectangle_obstacles(world)
obstacles += self.__generate_features(world, obstacles)
if self.cfg["obstacle"]["feature"]["enabled"]:
obstacles += self.__generate_features(world, obstacles)
# update the current obstacles and goal
self.current_obstacles = obstacles
......@@ -65,9 +65,9 @@ class MapManager:
"""
Adds a new goal
"""
i = 100000
i = 3000
max_dist = self.cfg["goal"]["max_distance"]
while True:
while i>0:
i -= 1
goal = self.__generate_new_goal()
intersects = self.__check_obstacle_intersections(goal)
......@@ -168,17 +168,22 @@ class MapManager:
obstacles.append(obstacle)
return obstacles
def __generate_feature_line(self, world, x0, y0, x1, y1):
def __generate_feature_line2(self, world, x0, y0, x1, y1):
r = 8 # resolution: pixs/meter
obs_radius = 0.04
line = geometrics.bresenham_line(x0*r, y0*r, x1*r, y1*r)
test_geometries = [r.global_geometry for r in world.robots]
obstacles = []
for x, y in line:
x = x/r
y = y/r
theta = -pi + (random() * 2 * pi)
obstacle = FeaturePoint(obs_radius, Pose(x, y, theta), 0)
# intersects = self.__check_obstacle_intersections([x,y])
# if not intersects:
# obstacles.append(obstacle)
intersects = False
for test_geometry in test_geometries:
intersects |= geometrics.convex_polygon_intersect_test(test_geometry, obstacle.global_geometry)
......@@ -186,7 +191,41 @@ class MapManager:
obstacles.append(obstacle)
return obstacles
def feature_test_geometry(self, x,y):
n = 6
r = 0.3
goal_test_geometry = []
for i in range(n):
goal_test_geometry.append(
[x + r * cos(i * 2 * pi / n), y + r * sin(i * 2 * pi / n)]
)
test_geometry = Polygon(goal_test_geometry)
return test_geometry
def __generate_feature_line(self, x0, y0, x1, y1, obs_radius, density):
c = density # feature density
a = atan2((y1-y0), (x1-x0))
obstacles = []
x = x0; y = y0
dx = copysign(c * cos(a), x1 - x0)
dy = copysign(c * sin(a), y1 - y0)
length = int((x1 - x0) // dx)
for i in range(length):
x += dx
y += dy
theta = -pi + (random() * 2 * pi)
nx = dx*(random()-1)*2
ny = dy*(random()-1)*2
feature = FeaturePoint(obs_radius, Pose(x + nx, y + ny, theta), 0)
obstacles.append(feature)
return obstacles
def __generate_feature_obstacle(self, world, vertexes):
radius = self.cfg["obstacle"]["feature"]["radius"]
density = self.cfg["obstacle"]["feature"]["density"]
num_vertexes = len(vertexes)
obstacles = []
for i in range(num_vertexes):
......@@ -195,8 +234,8 @@ class MapManager:
y0 = vertexes[i][1]
x1 = vertexes[j][0]
y1 = vertexes[j][1]
obstacles += self.__generate_feature_line(world, x0, y0, x1, y1)
obstacles.pop()
obstacles += self.__generate_feature_line(x0, y0, x1, y1, radius, density)
#obstacles.pop()
return obstacles
def __generate_features(self, world, obstacles):
......@@ -209,6 +248,7 @@ class MapManager:
for i, f in enumerate(features):
f.id = i
print ("#Feature: ", len(features))
return features
def __generate_new_goal(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