Commit 9485b8da authored by czb5793's avatar czb5793
Browse files

Add a new FeaturePoint.py class to represent features of the environment.

parent 54d6b4cf
......@@ -96,12 +96,12 @@ map:
octagon:
# Determines whether octagon obstacles shall be generated
enabled: true
# Radius of obstacles
radius: 0.04
# Minimum amount of generated obstacles
min_count: 25
# Maximum amount of generated obstacles
max_count: 50
# Radius of obstacles 0.04
radius: 1
# Minimum amount of generated obstacles 25
min_count: 2
# Maximum amount of generated obstacles 30
max_count: 4
# Minimum distance to origin
min_distance: 0.2
# Maximum distance to origin
......@@ -109,17 +109,17 @@ map:
# Configures rectangle obstacles
rectangle:
# Determines whether rectangle obstacles shall be generated
enabled: false
enabled: true
# Minimum side length of a rectangle
min_dim: 0.1
min_dim: 0.5
# Maximum side length of a rectangle
max_dim: 2.5
# Maximum sum of the two side lengths of a rectangle
max_combined_dim: 2.6
max_combined_dim: 3.0
# Minimum amount of generated obstacles
min_count: 10
min_count: 2
# Maximum amount of generated obstacles
max_count: 30
max_count: 4
# Minimum distance to origin
min_distance: 0.4
# Maximum distance to origin
......@@ -213,8 +213,10 @@ slam:
theta: 1.0 #0.09
# Configures the sensor noise. The values are currently empirically chosen.
sensor_noise:
x: 0.01 # 0.02
y: 0.5236 # 0.02
#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
# Configures the evaluation of the SLAM algorithms
evaluation:
......
......@@ -156,6 +156,9 @@ slam:
# These are x position and y position
# Currently only supports 2
landmark_state_size: 2
# Determines whether landmark-matcher is used
# It determines whether correspondences of landmarks are given, i.e. identifies of landmarks are given
landmark_matcher: true
# Configures the sensor noise. The values are currently empirically chosen.
sensor_noise:
# Standard deviation of the detected distance in meters
......
......@@ -18,7 +18,7 @@
import utils.geometrics_util as geometrics
from models.obstacles.FeaturePoint import FeaturePoint
from simulation.exceptions import CollisionException
class Physics:
......@@ -72,7 +72,7 @@ class Physics:
dmin = float('inf')
solid_id = -1
detector_line = sensor.detector_line
solid_object = None
for i, solid in enumerate(solids):
if solid is not robot: # assume that the sensor does not detect it's own robot
......@@ -86,11 +86,16 @@ class Physics:
if intersection_exists and d < dmin:
dmin = d
solid_id = i
solid_object = solid
# if there is an intersection, update the sensor with the new delta value
if dmin != float('inf'):
sensor.detect(dmin)
robot.landmark_matcher.detect(sensor.id, solid_id)
sensor.detect(dmin) # distance to the nearest solid
else:
sensor.detect(None)
# if a feature has been extracted from the environment, update the matcher with the feature identifier
if dmin != float('inf') and type(solid_object) == FeaturePoint:
robot.landmark_matcher.detect(sensor.id, solid_object.id)
else:
robot.landmark_matcher.detect(sensor.id, -1)
from models.obstacles.OctagonObstacle import OctagonObstacle
class FeaturePoint(OctagonObstacle):
def __init__(self, radius, pose, id):
super(FeaturePoint, self).__init__(radius, pose)
self.id = id
\ No newline at end of file
......@@ -25,7 +25,7 @@ from models.obstacles.OctagonObstacle import OctagonObstacle
from models.Pose import Pose
from models.Polygon import Polygon
from models.obstacles.RectangleObstacle import RectangleObstacle
from models.obstacles.FeaturePoint import FeaturePoint
seed(42)
......@@ -51,6 +51,8 @@ class MapManager:
if self.cfg["obstacle"]["rectangle"]["enabled"]:
obstacles += self.__generate_rectangle_obstacles(world)
obstacles += self.__generate_features(world, obstacles)
# update the current obstacles and goal
self.current_obstacles = obstacles
self.add_new_goal()
......@@ -153,6 +155,49 @@ class MapManager:
obstacles.append(obstacle)
return obstacles
def __generate_feature_line(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 = False
for test_geometry in test_geometries:
intersects |= geometrics.convex_polygon_intersect_test(test_geometry, obstacle.global_geometry)
if not intersects:
obstacles.append(obstacle)
return obstacles
def __generate_feature_obstacle(self, world, vertexes):
num_vertexes = len(vertexes)
obstacles = []
for i in range(num_vertexes):
j = 0 if i+1 >= num_vertexes else i+1
x0 = vertexes[i][0]
y0 = vertexes[i][1]
x1 = vertexes[j][0]
y1 = vertexes[j][1]
obstacles += self.__generate_feature_line(world, x0, y0, x1, y1)
obstacles.pop()
return obstacles
def __generate_features(self, world, obstacles):
features = []
for obstacle in obstacles:
if type(obstacle) == FeaturePoint:
continue
features += self.__generate_feature_obstacle(world, obstacle.global_geometry.vertexes)
# add identifier for each feature.
for i, f in enumerate(features):
f.id = i
return features
def __generate_new_goal(self):
"""
Generate a new random goal
......
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