Commit 5a937b10 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 abc03417
......@@ -34,9 +34,9 @@ robot:
motor:
noise:
# Standard deviation of the left wheel's velocity in meters/second while executing a motion command.
left_velocity: 0.01 # 0.0001
left_velocity: 0.1 # 0.0001
# Standard deviation of the right wheel's velocity in meters/second while executing a motion command.
right_velocity: 0.01 # 0.0001
right_velocity: 0.1 # 0.0001
# Configures the sensors used by the robot
sensor:
......@@ -101,7 +101,7 @@ map:
# Minimum amount of generated obstacles 25
min_count: 2
# Maximum amount of generated obstacles 30
max_count: 4
max_count: 2
# Minimum distance to origin
min_distance: 0.2
# Maximum distance to origin
......@@ -196,7 +196,7 @@ slam:
graph_based_slam:
# Determines whether the Graph-based SLAM algorithm shall be executed
enabled: false
enabled: true
# The euclidean distance threshold [m] used in data association.
# If the distance between the estimated landmarks via slam and via actual measurement
# larger than the threshold, start backend
......@@ -205,8 +205,8 @@ slam:
optimization_interval: 50
# The timestep interval of executing the frontend part.
frontend_interval: 5
# Vertex density of frontend. minimum distance [m] between the current pose and the last pose.
frontend_density: 0.1
# Pose density of frontend, meaning the minimum distance [m] between the current pose and the last pose.
frontend_pose_density: 0.1
# Configures the motion noise. The values are currently empirically chosen.
motion_noise:
# Standard deviation of the robots x-coordinate in meters after executing a motion command.
......
......@@ -41,7 +41,7 @@ from robot.RobotSupervisorInterface import *
from simulation.World import *
from plotters.WorldPlotter import *
from simulation.exceptions import CollisionException
from simulation.exceptions import CollisionException, GoalNotReachedException
class Simulator:
......@@ -268,6 +268,12 @@ class Simulator:
self.map_manager.apply_to_world(self.world)
else:
self.end_sim("Goal Reached!")
except GoalNotReachedException:
if self.cfg["map"]["goal"]["endless"]:
self.map_manager.add_new_goal(self.world) # add a new goal not far from the robot
self.map_manager.apply_to_world(self.world)
else:
self.end_sim("Goal Reached!")
# Evaluate accuracies of slam
self._update_slam_accuracies()
......
......@@ -23,3 +23,6 @@ class CollisionException(Exception):
class GoalReachedException(Exception):
pass
class GoalNotReachedException(Exception):
pass
\ No newline at end of file
......@@ -18,9 +18,9 @@
from supervisor.ControlState import *
from simulation.exceptions import GoalReachedException
from simulation.exceptions import GoalReachedException, GoalNotReachedException
from utils import linalg2_util as linalg
from supervisor.slam.Timer import Timer
class SupervisorStateMachine:
......@@ -40,6 +40,8 @@ class SupervisorStateMachine:
self.cfg = control_config
self.timer = Timer()
def update_state(self):
"""
Update the control state
......@@ -57,6 +59,12 @@ class SupervisorStateMachine:
else:
raise Exception("undefined supervisor state or behavior", self.current_state)
# change the goal if robot cannot reach it in time
if self.timer.not_reach_goal_in_time(self.supervisor.goal):
self.transition_to_state_go_to_goal()
print ("GoalNotReachedException: Not reach the goal in time.")
raise GoalNotReachedException() # add a new goal not far from the robot
# === STATE PROCEDURES ===
def execute_state_go_to_goal(self):
if self.condition_at_goal():
......@@ -91,7 +99,9 @@ class SupervisorStateMachine:
self.transition_to_state_at_goal()
elif self.condition_danger():
self.transition_to_state_avoid_obstacles()
elif self.condition_progress_made() and not self.condition_slide_left():
elif self.condition_progress_made() and \
(self.timer.previous_best_distance_not_changed_in_time(self.best_distance_to_goal)
or not self.condition_slide_left()):
self.transition_to_state_go_to_goal()
def execute_state_slide_right(self):
......@@ -99,7 +109,9 @@ class SupervisorStateMachine:
self.transition_to_state_at_goal()
elif self.condition_danger():
self.transition_to_state_avoid_obstacles()
elif self.condition_progress_made() and not self.condition_slide_right():
elif self.condition_progress_made() and \
(self.timer.previous_best_distance_not_changed_in_time(self.best_distance_to_goal)
or not self.condition_slide_right()):
self.transition_to_state_go_to_goal()
# def execute_state_gtg_and_ao( self ):
......@@ -191,4 +203,4 @@ class SupervisorStateMachine:
return linalg.distance(self.supervisor.estimated_pose.vposition(), self.supervisor.goal)
def _update_best_distance_to_goal(self):
self.best_distance_to_goal = min(self.best_distance_to_goal, self._distance_to_goal())
self.best_distance_to_goal = min(self.best_distance_to_goal, self._distance_to_goal())
\ No newline at end of file
import time
T_NOT_REACH_GOAL = 60 # time that robot not reaches a goal in seconds
T_NO_BEST_DIST_CHANGE = 30 # time that robot not updates the best distance to goal
class Timer:
"""
Timer for state transition. Avoid robot only moving around a circle endlessly
"""
def __init__(self):
# Timing how long does the robot not reach the goal?
self.previous_goal = [0, 0]
self.start_time_to_goal = time.time()
# Timing how long does best_distance_to_goal not changed?
self.previous_best_distance = float("inf")
self.start_time_best_distance_to_goal = time.time()
def not_reach_goal_in_time(self, goal):
"""
:param goal: goal position
:return: True or False
"""
flg = False
if goal != self.previous_goal: # update previous goal, if goal is changed
self.previous_goal = goal[:]
self.start_time_to_goal = time.time()
flg = False
elif time.time() - self.start_time_to_goal > T_NOT_REACH_GOAL: # goal dose not change over 30s
self.start_time_to_goal = time.time()
flg = True
return flg
def previous_best_distance_not_changed_in_time(self, best_distance_to_goal):
if best_distance_to_goal != self.previous_best_distance: # update previous distance if it was changed
self.start_time_best_distance_to_goal = time.time()
self.previous_best_distance = best_distance_to_goal
return False
elif time.time() - self.start_time_best_distance_to_goal > T_NO_BEST_DIST_CHANGE: # no changes in time
self.start_time_to_goal = time.time()
print("Best distance dose not changed in time.")
return True
return False
\ No newline at end of file
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