Commit 5c0bd97c authored by czb5793's avatar czb5793
Browse files

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[0].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!
Please register or to comment