### Fix the problem that the robot moves around a circle endlessly, not being able...

`Fix the problem that the robot moves around a circle endlessly, not being able to reach the goal. The problem was solved by a Timer (Timer.py).`
parent 5a937b10
 ... ... @@ -26,6 +26,7 @@ from models.Pose import Pose 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) ... ... @@ -60,16 +61,28 @@ class MapManager: # apply the new obstacles and goal to the world self.apply_to_world(world) def add_new_goal(self): def add_new_goal(self, world = None): """ Adds a new goal """ i = 100000 max_dist = self.cfg["goal"]["max_distance"] while True: i -= 1 goal = self.__generate_new_goal() intersects = self.__check_obstacle_intersections(goal) if not intersects: if not intersects and world is None: self.current_goal = goal break elif not intersects and world is not None: # add a new goal not far from the robot rob_x, rob_y = world.robots.supervisor.estimated_pose.vposition() distance_to_robot = linalg.distance([rob_x, rob_y], goal) if distance_to_robot < 1 and distance_to_robot > 0.5: # being able to a new goal not far from the robot self.current_goal = goal break if rob_x**2 + rob_y**2 > max_dist**2: # not being able to a new goal near the robot self.current_goal = goal break def __generate_octagon_obstacles(self, world): """ ... ...
 ... ... @@ -30,7 +30,7 @@ class GraphBasedSLAM(Slam): slam_cfg["graph_based_slam"]["motion_noise"]["y"], np.deg2rad(slam_cfg["graph_based_slam"]["motion_noise"]["theta"])]) ** 2 self.min_distance_threshold = slam_cfg["graph_based_slam"]["distance_threshold"] # minimum distance for data assosiation self.frontend_density = slam_cfg["graph_based_slam"]["frontend_density"] self.frontend_pose_density = slam_cfg["graph_based_slam"]["frontend_pose_density"] self.optimization_interval = slam_cfg["graph_based_slam"]['optimization_interval'] # the number interval of pose-vertices added that the graph optimization is executed. self.frontend_interval = slam_cfg["graph_based_slam"]['frontend_interval'] # the timestep interval of executing the frontend part. ... ... @@ -101,9 +101,9 @@ class GraphBasedSLAM(Slam): self.step_counter += 1 """ Update the Graph """ if self.step_counter % self.frontend_interval == 0: # create a vertex each n steps if self.step_counter % self.frontend_interval == 0: # create a vertex every n steps self.__front_end(z) num_vertices = len(self.graph.vertices) num_vertices = len(self.graph.vertices) # #vertices = #poses + #landmarks if num_vertices > 0 and self.flg_optim == True: print ("Graph-based SLAM starts the backend-optimization with " + str(num_vertices) + " vertices") ... ... @@ -111,7 +111,7 @@ class GraphBasedSLAM(Slam): self.__back_end() if self.callback is not None: self.callback(str(self), time.time() - start_time) # time used for updating self.callback(str(self), time.time() - start_time) # record time used for updating def __front_end(self, z): """ ... ... @@ -123,7 +123,7 @@ class GraphBasedSLAM(Slam): vertex1 = self.graph.get_last_pose_vertex() # a previous vertex vertex2 = PoseVertex(self.mu, self.Sigma) # a current vertex distance = (vertex1.pose[0, 0] - vertex2.pose[0, 0])**2 + (vertex1.pose[1, 0] - vertex2.pose[1, 0])**2 if distance < self.frontend_density**2: # keep the vertex density not high if distance < self.frontend_pose_density**2: # keep the vertex density not high return self.graph.add_vertex(vertex2) ... ... @@ -142,17 +142,20 @@ class GraphBasedSLAM(Slam): vertex3 = LandmarkVertex(pos_lm, self.sensor_noise, lm_id) # create a new vertex representing a landmark self.graph.add_vertex(vertex3) # add the vertex to the graph else: # Determine whether robot is revisiting a landmark, if yes, start backend. # Calculate the distance between the estimated landmarks via slam and via actual measurement, # if the distance is larger than the threshold, start backend # 1. Known that the robot is revisiting a previous seen landmark, # 2. Calculate the distance between the estimated landmarks via slam and via actual measurement, # if the robot is revisiting a landmark, and the distance is larger than the threshold, # start backend to correct the consistancy between the estimate and the measurments. distance2 = (pos_lm[0, 0] - vertex3.pose[0, 0])**2 + (pos_lm[1, 0] - vertex3.pose[1, 0])**2 if distance2 > self.min_distance_threshold**2 and self.flg_optim == False: self.flg_optim = True # Create an edge connecting a pose and a landmark. measurement, info = PoseLandmarkEdge.encode_measurement(zi, self.sensor_noise) self.graph.add_edge(vertex2, vertex3, measurement, info) # add an PoseLandmarkEdge edge """ calculate pose-pose edge """ # Create an edge connecting two poses measurement, info = PosePoseEdge.encode_measurement(vertex1.pose, vertex2.pose, self.motion_noise) self.graph.add_edge(vertex1, vertex2, measurement, info) ... ...
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!