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])