def __generate_feature_line(self, x0, y0, x1, y1, radius, density):
"""
Generate features along a line.
:param x0: coordinate of the starting point.
:param y0: coordinate of the end
:param x1: coordinate of the starting point
:param y1: coordinate of the end
:param radius: the radius of feature-point.
:param density: density of the features on the line.
:return:
"""
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(radius, Pose(x + nx, y + ny, theta), 0)
obstacles.append(feature)
return obstacles
def get_estimated_pose(self):
"""
Returns the estimated robot pose by only considering the particle with the highest importance factor
:return: Estimated robot pose consisting of position and angle
"""
particle = self.get_best_particle()
return Pose(particle.x, particle.y, particle.theta)
def draw_slam_to_frame(self):
"""
Draws a SLAM visualization to the frame
"""
frame = self.viewer.current_frames[self.frame_number]
self.__draw_robot_to_frame(frame, self.slam.get_estimated_pose())
# draw all the obstacles
for landmark in self.slam.get_landmarks():
obstacle = OctagonObstacle(self.radius,
Pose(landmark[0], landmark[1], 0))
obstacle_plotter = ObstaclePlotter(obstacle)
obstacle_plotter.draw_obstacle_to_frame(frame, "black", alpha=0.6)
if self.viewer.draw_invisibles and isinstance(self.slam, EKFSlam):
self.__draw_confidence_ellipse(frame)
def __generate_rectangle_obstacles(self, world):
"""
Generate random rectangle obstacles
:param world: The world for which they are generated
:return: List of generated rectangle obstacles
"""
obs_min_dim = self.cfg["obstacle"]["rectangle"]["min_dim"]
obs_max_dim = self.cfg["obstacle"]["rectangle"]["max_dim"]
obs_max_combined_dim = self.cfg["obstacle"]["rectangle"][
"max_combined_dim"]
obs_min_count = self.cfg["obstacle"]["rectangle"]["min_count"]
obs_max_count = self.cfg["obstacle"]["rectangle"]["max_count"]
obs_min_dist = self.cfg["obstacle"]["rectangle"]["min_distance"]
obs_max_dist = self.cfg["obstacle"]["rectangle"]["max_distance"]
# generate the obstacles
obstacles = []
obs_dim_range = obs_max_dim - obs_min_dim
obs_dist_range = obs_max_dist - obs_min_dist
num_obstacles = randrange(obs_min_count, obs_max_count + 1)
test_geometries = [r.global_geometry for r in world.robots]
while len(obstacles) < num_obstacles:
# generate dimensions
width = obs_min_dim + (random() * obs_dim_range)
height = obs_min_dim + (random() * obs_dim_range)
while width + height > obs_max_combined_dim:
height = obs_min_dim + (random() * obs_dim_range)
# generate position
dist = obs_min_dist + (random() * obs_dist_range)
phi = -pi + (random() * 2 * pi)
x = dist * sin(phi)
y = dist * cos(phi)
# generate orientation
theta = -pi + (random() * 2 * pi)
# test if the obstacle overlaps the robots or the goal
obstacle = RectangleObstacle(width, height, Pose(x, y, theta))
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_random_features(self, world):
"""
Generate random features represented by octagon obstacles
:param world: The world for which they are generated
:return: List of generated features
"""
obs_radius = self.cfg["obstacle"]["feature"]["radius"]
obs_min_count = self.cfg["obstacle"]["feature"]["min_count"]
obs_max_count = self.cfg["obstacle"]["feature"]["max_count"]
obs_min_dist = self.cfg["obstacle"]["feature"]["min_distance"]
obs_max_dist = self.cfg["obstacle"]["feature"]["max_distance"]
# generate the obstacles
obstacles = []
obs_dist_range = obs_max_dist - obs_min_dist
num_obstacles = randrange(obs_min_count, obs_max_count + 1)
test_geometries = [r.global_geometry for r in world.robots]
while len(obstacles) < num_obstacles:
# generate position
dist = obs_min_dist + (random() * obs_dist_range)
phi = -pi + (random() * 2 * pi)
x = dist * sin(phi)
y = dist * cos(phi)
# generate orientation
theta = -pi + (random() * 2 * pi)
# test if the obstacle overlaps the robots or the goal
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)
for i, feature in enumerate(obstacles):
feature.id = i
return obstacles
def get_estimated_pose(self):
"""
Returns the estimated robot pose by retrieving the first three elements of the combined state vector
:return: Estimated robot pose consisting of position and angle
"""
return Pose(self.mu[0, 0], self.mu[1, 0], self.mu[2, 0])
def get_estimated_pose(self):
"""
Returns the estimated pose of the robot
"""
return Pose(self.mu[0, 0], self.mu[1, 0], self.mu[2, 0])