def _avoid_close_objects(self, x, y, theta):
distances = self._distances_to_robots()
new_yield_timestamps = {}
self._yield_timestamps = new_yield_timestamps
bearings = set()
for robot_name, distance in distances:
if distance > SimpleAlgorithm.CIRCLES_RADIUS * 1:
break
if self._states[self._robot_name]['fear_factor'] < self._states[
robot_name]['fear_factor']:
state = self._states[robot_name]
rx = state['x']
ry = state['y']
bearing_to_robot = MeasurementUtils.angle(x, y, rx, ry)
#bearings.add(bearing_to_robot)
else:
self._send_stop_command()
return True
for wx, wy in self._maze_util.get_close_walls_points(x, y, 0.25):
bearing_to_wall = MeasurementUtils.angle(x, y, wx, wy)
bearings.add(bearing_to_wall)
if len(bearings) == 0:
return False
else:
bearings = list(bearings)
bearings.sort()
bearings_len = len(bearings)
bearings.append(bearings[0] + 2.0 * math.pi)
max_gap = 0
best_bearing = None
for i in range(bearings_len):
gap = abs(bearings[i + 1] - bearings[i])
if gap > max_gap:
max_gap = gap
best_bearing = MeasurementUtils.normalize_angle(
bearings[i] + 0.5 * (bearings[i + 1] - bearings[i]))
v, omega = SimpleAlgorithm._navigate_fun(
SimpleAlgorithm.CRUISE_SPEED * 0.5, theta, best_bearing, True)
preds = PredictionUtils.predict_positions(x, y, v, best_bearing,
0.0)
self._predictions[self._robot_name] = preds
Vl, Vr = SimpleAlgorithm._get_wheel_speeds(v, omega)
self._send_robot_command(RobotCommand(Vl, Vr, Vl, Vr))
return True
def _avoid_close_objects(self, x, y, theta):
distances = self._distances_to_robots()
new_yield_timestamps = {}
self._yield_timestamps = new_yield_timestamps
bearings = set()
for robot_name, distance in distances:
if distance > SimpleAlgorithm.CIRCLES_RADIUS * 1:
break
if self._states[self._robot_name]['fear_factor'] < self._states[robot_name]['fear_factor']:
state = self._states[robot_name]
rx = state['x']
ry = state['y']
bearing_to_robot = MeasurementUtils.angle(x, y, rx, ry)
#bearings.add(bearing_to_robot)
else:
self._send_stop_command()
return True
for wx, wy in self._maze_util.get_close_walls_points(x, y, 0.25):
bearing_to_wall = MeasurementUtils.angle(x, y, wx, wy)
bearings.add(bearing_to_wall)
if len(bearings) == 0:
return False
else:
bearings = list(bearings)
bearings.sort()
bearings_len = len(bearings)
bearings.append(bearings[0] + 2.0 * math.pi)
max_gap = 0
best_bearing = None
for i in range(bearings_len):
gap = abs(bearings[i+1] - bearings[i])
if gap > max_gap:
max_gap = gap
best_bearing = MeasurementUtils.normalize_angle(bearings[i] + 0.5 * (bearings[i+1] - bearings[i]))
v, omega = SimpleAlgorithm._navigate_fun(SimpleAlgorithm.CRUISE_SPEED * 0.5, theta, best_bearing, True)
preds = PredictionUtils.predict_positions(x, y, v, best_bearing, 0.0)
self._predictions[self._robot_name] = preds
Vl, Vr = SimpleAlgorithm._get_wheel_speeds(v, omega)
self._send_robot_command(
RobotCommand(Vl, Vr, Vl, Vr))
return True
def _target_angle(self):
assert type(self._target) == type([])
assert len(self._target) > 0
(tx, ty) = self._target[0]
mx = self._own_robot['x']
my = self._own_robot['y']
return MeasurementUtils.angle(mx, my, tx, ty)
def _target_angle(self):
assert type(self._target) == type([])
assert len(self._target) > 0
(tx, ty) = self._target[0]
mx = self._own_robot['x']
my = self._own_robot['y']
return MeasurementUtils.angle(mx, my, tx, ty)
def _avoid_close_objects(self, x, y, theta,Otheta):
#self._logger.info("insert sction _avoid_close_objects");
distances = self._distances_to_robots()
new_yield_timestamps = {}
if self._own_robot['v'] <= 0.1:
for robot_name, distance in distances:
if distance > FearfulAlgorithm.CIRCLES_RADIUS * 2.0:
break
if self._fear_factors[self._robot_name] > self._fear_factors[robot_name]:
if robot_name not in self._yield_timestamps:
new_yield_timestamps[robot_name] = self._own_robot['timestamp'] + self.YIELD_DELAY
else:
timestamp = self._yield_timestamps[robot_name]
if self._own_robot['timestamp'] > timestamp:
self._yield_set.add(robot_name)
self._own_fear_factor = self._states[robot_name]['fear_factor'] - 0.1
else:
new_yield_timestamps[robot_name] = timestamp
self._yield_timestamps = new_yield_timestamps
bearings = set()
for robot_name, distance in distances:
if distance > FearfulAlgorithm.CIRCLES_RADIUS * 1:
break
if self._fear_factors[self._robot_name] < self._fear_factors[robot_name]:
state = self._states[robot_name]
rx = state['x']
ry = state['y']
bearing_to_robot = MeasurementUtils.angle(x, y, rx, ry)
#bearings.add(bearing_to_robot)
else:
self._logger.debug("Avoid Close Object ->>>> stop robot")
self._send_stop_command()
return True
for wx, wy in self._maze_util.get_close_walls_points(x, y, 0.25):
bearing_to_wall = MeasurementUtils.angle(x, y, wx, wy)
bearings.add(bearing_to_wall)
if len(bearings) == 0:
return False
else:
bearings = list(bearings)
bearings.sort()
bearings_len = len(bearings)
bearings.append(bearings[0] + 2.0 * math.pi)
max_gap = 0
best_bearing = None
for i in range(bearings_len):
gap = abs(bearings[i+1] - bearings[i])
if gap > max_gap:
max_gap = gap
best_bearing = MeasurementUtils.normalize_angle(bearings[i] + 0.5 * (bearings[i+1] - bearings[i]))
v = FearfulAlgorithm.CRUISE_SPEED
#v, omega = FearfulAlgorithm._navigate_fun(FearfulAlgorithm.CRUISE_SPEED * 0.5, theta, best_bearing, True)
preds = PredictionUtils.predict_positions(x, y, v, best_bearing, 0.0)
self._predictions[self._robot_name] = preds
#Vl, Vr = FearfulAlgorithm._get_wheel_speeds(v, omega)
#Vl, Vr = FearfulAlgorithm._get_wheel_speeds_new2(v,theta,best_bearing,True,None,None,None)
self._logger.debug("Avoid Close Object")
Vl, Vr = FearfulAlgorithm._get_wheel_speeds_new2(self._logger,v,Otheta,best_bearing,True,None)
self._send_robot_command(RobotCommand(Vl, Vr, Vl, Vr))
return True
def _navigate(self, avail_time):
dist = None
#self._logger.info("Nowy kod")
x = self._own_robot['x']
y = self._own_robot['y']
theta = self._own_robot['theta']
Otheta = self._own_robot['org_theta']
Otheta = 1.57 - Otheta
v = FearfulAlgorithm.CRUISE_SPEED
if self._target is None:
tx, ty = self._controller.get_new_target()
self._target = self._maze_util.find_path((x, y), (tx, ty))
if not self._target_reached and self._target_distance() < FearfulAlgorithm.DISTANCE_GOAL:
if len(self._target) > 1:
self._target.pop(0)
else:
new_target = self._controller.get_new_target()
if new_target is None or new_target != self._target[0]:
tx, ty = new_target
self._target = self._maze_util.find_path((x, y), (tx, ty))
else:
self._target_reached = True
self._logger.info("Target reached")
self.send_finish_msg()
space_id = self._maze_util.find_space_by_point((x, y))
if space_id != self._space_id:
if self._space_id is not None:
tx, ty = self._target[-1]
self._target = self._maze_util.find_path((x, y), (tx, ty))
self._space_id = space_id
# ominiecie robota lub robotow
if self._avoid_close_objects(x, y, theta,Otheta):
return
target = self._target[0]
tx, ty = target
bearing = MeasurementUtils.angle(x, y, tx, ty)
bearingNew = math.atan2(ty - y, tx - x) #tylko dla nawigacji
preds = PredictionUtils.predict_positions(x, y, v, bearing, 0.0)
preds = FearfulAlgorithm._cut_predictions(preds, target)
self._predictions[self._robot_name] = preds
target_dist = MeasurementUtils.distance(x, y, tx, ty)
if not self._find_intersections(preds, self.CIRCLES_RADIUS * 1.5, 0):
self._own_fear_factor = self._base_fear_factor
self._logger.info("find intersectio")
Vl, Vr = FearfulAlgorithm._get_wheel_speeds_new2(self._logger,v,Otheta,bearingNew,False,target_dist)
self._send_robot_command(RobotCommand(Vl, Vr, Vl, Vr))
#v, omega = FearfulAlgorithm._navigate_fun(v, theta, bearing, False)
#if target_dist < 0.2:
# v *= target_dist
#if target_dist < 0.05:
# v = 0
##Vl, Vr = FearfulAlgorithm._get_wheel_speeds(v, omega)
#Vl, Vr = FearfulAlgorithm._get_wheel_speeds_new2(v,Otheta,bearing,False,target_dist,tx - x,ty - y)
#self._logger.info("FindIntersction Vl: %f; Vr: %f; v: %f; theta: %f; bearing: %f; target_distance: %f; sub: %f,x: %f, y: %f" % (Vl, Vr, v, Otheta, bearing, target_dist, Otheta - bearing,x,y))
#self._send_robot_command(RobotCommand(Vl, Vr, Vl, Vr))
return
for robot_name in list(self._yield_set):
rx = self._states[robot_name]['x']
ry = self._states[robot_name]['y']
if MeasurementUtils.distance(x, y, rx, ry) > 2.0:
self._yield_set.remove(robot_name)
best_v = None
best_bearing = None
best_midpoint = None
best_rate = None
best_preds = None
self._predictions[self._robot_name] = None
if self._last_params is not None:
best_v, best_bearing, best_midpoint = self._last_params
best_preds = PredictionUtils.predict_positions(x, y, best_v, best_bearing, 0.0)
best_preds = FearfulAlgorithm._cut_predictions(best_preds, best_midpoint)
best_rate = self._rate_predictions(best_preds)
if self._find_intersections(best_preds, self.CIRCLES_RADIUS, 1):
best_v = None
best_bearing = None
best_midpoint = None
best_rate = None
best_preds = None
try_stop = True
variants_stop_time = time.time() + avail_time - 0.001
var_num = 0
while time.time() < variants_stop_time:
var_num += 1
if try_stop:
rand_v = 0.0
rand_bearing = theta
rand_radius = 0.0
try_stop = False
else:
rand_v = random.uniform(0.0, self.CRUISE_SPEED * 2.0)
rand_bearing = random.uniform(-math.pi, math.pi)
rand_radius = random.uniform(0.0, self.CRUISE_SPEED * PredictionUtils.CIRCLES_DELTA * PredictionUtils.CIRCLES_NUM)
mp_x = rand_radius * math.cos(rand_bearing) + x
mp_y = rand_radius * math.sin(rand_bearing) + y
rand_midpoint = mp_x, mp_y
rand_preds = PredictionUtils.predict_positions(x, y, rand_v, rand_bearing, 0.0)
rand_preds = FearfulAlgorithm._cut_predictions(rand_preds, rand_midpoint)
rand_rate = self._rate_predictions(rand_preds)
if self._find_intersections(rand_preds, self.CIRCLES_RADIUS * 1.5, 1):
continue
if best_rate is None or rand_rate < best_rate - 0.5:
best_v, best_bearing, best_preds, best_rate, best_midpoint \
= rand_v, rand_bearing, rand_preds, rand_rate, rand_midpoint
self._last_params = best_v, best_bearing, best_midpoint
if best_preds is None:
self._logger.info("best pred is None -->>>>> Stop command:")
self._send_stop_command()
return
else:
self._predictions[self._robot_name] = best_preds
#best_v, omega = FearfulAlgorithm._navigate_fun(best_v, theta, best_bearing, False)
mp_x, mp_y = best_midpoint
midpoint_dist = MeasurementUtils.distance(x, y, mp_x, mp_y)
#if midpoint_dist < 0.5:
# best_v *= midpoint_dist
#Vl, Vr = FearfulAlgorithm._get_wheel_speeds(best_v, omega)
##Vl, Vr = FearfulAlgorithm._get_wheel_speeds_new2(v,Otheta,best_bearing,False,target_dist, mp_x - x, mp_y - y)
##self._logger.info("Prediction Vl: %f; Vr: %f; v: %f; theta: %f; best_bearing: %f; target_distance: %f; sub: %f,x: %f,y: %f" % (Vl, Vr, v, Otheta, best_bearing, target_dist,Otheta - best_bearing,x,y))
self._logger.info("Prediction:")
Vl, Vr = FearfulAlgorithm._get_wheel_speeds_new2(self._logger,v,Otheta,best_bearing,False,target_dist)
self._send_robot_command(RobotCommand(Vl, Vr, Vl, Vr))
def _navigate(self, avail_time):
dist = None
x = self._own_robot['x']
y = self._own_robot['y']
theta = self._own_robot['theta']
v = SimpleAlgorithm.CRUISE_SPEED
if self._target is None:
tx, ty = self._controller.get_new_target()
self._target = self._maze_util.find_path((x, y), (tx, ty))
if not self._target_reached and self._target_distance() < 0.1:
if len(self._target) > 1:
self._target.pop(0)
else:
new_target = self._controller.get_new_target()
if new_target is None or new_target != self._target[0]:
tx, ty = new_target
self._target = self._maze_util.find_path((x, y), (tx, ty))
else:
self._target_reached = True
self._logger.info("Target reached")
self.send_finish_msg()
space_id = self._maze_util.find_space_by_point((x, y))
if space_id != self._space_id:
if self._space_id is not None:
tx, ty = self._target[-1]
self._target = self._maze_util.find_path((x, y), (tx, ty))
self._space_id = space_id
if self._avoid_close_objects(x, y, theta):
return
target = self._target[0]
tx, ty = target
bearing = MeasurementUtils.angle(x, y, tx, ty)
preds = PredictionUtils.predict_positions(x, y, v, bearing, 0.0)
preds = SimpleAlgorithm._cut_predictions(preds, target)
self._predictions[self._robot_name] = preds
target_dist = MeasurementUtils.distance(x, y, tx, ty)
if not self._find_intersections(preds, self.CIRCLES_RADIUS * 1.5, 0):
self._own_fear_factor = self._base_fear_factor
v, omega = SimpleAlgorithm._navigate_fun(v, theta, bearing, False)
if target_dist < 0.2:
v *= target_dist
if target_dist < 0.05:
v = 0
Vl, Vr = SimpleAlgorithm._get_wheel_speeds(v, omega)
self._send_robot_command(
RobotCommand(Vl, Vr, Vl, Vr))
return
for robot_name in list(self._yield_set):
rx = self._states[robot_name]['x']
ry = self._states[robot_name]['y']
if MeasurementUtils.distance(x, y, rx, ry) > 2.0:
self._yield_set.remove(robot_name)
best_v = None
best_bearing = None
best_midpoint = None
best_rate = None
best_preds = None
self._predictions[self._robot_name] = None
if self._last_params is not None:
best_v, best_bearing, best_midpoint = self._last_params
best_preds = PredictionUtils.predict_positions(x, y, best_v, best_bearing, 0.0)
best_preds = SimpleAlgorithm._cut_predictions(best_preds, best_midpoint)
best_rate = self._rate_predictions(best_preds)
if self._find_intersections(best_preds, self.CIRCLES_RADIUS, 1):
best_v = None
best_bearing = None
best_midpoint = None
best_rate = None
best_preds = None
try_stop = True
variants_stop_time = time.time() + avail_time - 0.001
var_num = 0
while time.time() < variants_stop_time:
var_num += 1
if try_stop:
rand_v = 0.0
rand_bearing = theta
rand_radius = 0.0
try_stop = False
else:
rand_v = random.uniform(0.0, self.CRUISE_SPEED * 2.0)
rand_bearing = random.uniform(-math.pi, math.pi)
rand_radius = random.uniform(0.0, self.CRUISE_SPEED * PredictionUtils.CIRCLES_DELTA * PredictionUtils.CIRCLES_NUM)
mp_x = rand_radius * math.cos(rand_bearing) + x
mp_y = rand_radius * math.sin(rand_bearing) + y
rand_midpoint = mp_x, mp_y
rand_preds = PredictionUtils.predict_positions(x, y, rand_v, rand_bearing, 0.0)
rand_preds = SimpleAlgorithm._cut_predictions(rand_preds, rand_midpoint)
rand_rate = self._rate_predictions(rand_preds)
if self._find_intersections(rand_preds, self.CIRCLES_RADIUS * 1.5, 1):
continue
if best_rate is None or rand_rate < best_rate - 0.5:
best_v, best_bearing, best_preds, best_rate, best_midpoint \
= rand_v, rand_bearing, rand_preds, rand_rate, rand_midpoint
self._last_params = best_v, best_bearing, best_midpoint
if best_preds is None:
self._send_stop_command()
return
else:
self._predictions[self._robot_name] = best_preds
best_v, omega = SimpleAlgorithm._navigate_fun(best_v, theta, best_bearing, False)
mp_x, mp_y = best_midpoint
midpoint_dist = MeasurementUtils.distance(x, y, mp_x, mp_y)
if midpoint_dist < 0.5:
best_v *= midpoint_dist
Vl, Vr = SimpleAlgorithm._get_wheel_speeds(best_v, omega)
self._send_robot_command(
RobotCommand(Vl, Vr, Vl, Vr))
def _navigate(self, avail_time):
dist = None
x = self._own_robot['x']
y = self._own_robot['y']
theta = self._own_robot['theta']
v = SimpleAlgorithm.CRUISE_SPEED
if self._target is None:
tx, ty = self._controller.get_new_target()
self._target = self._maze_util.find_path((x, y), (tx, ty))
if not self._target_reached and self._target_distance() < 0.1:
if len(self._target) > 1:
self._target.pop(0)
else:
new_target = self._controller.get_new_target()
if new_target is None or new_target != self._target[0]:
tx, ty = new_target
self._target = self._maze_util.find_path((x, y), (tx, ty))
else:
self._target_reached = True
self._logger.info("Target reached")
self.send_finish_msg()
space_id = self._maze_util.find_space_by_point((x, y))
if space_id != self._space_id:
if self._space_id is not None:
tx, ty = self._target[-1]
self._target = self._maze_util.find_path((x, y), (tx, ty))
self._space_id = space_id
if self._avoid_close_objects(x, y, theta):
return
target = self._target[0]
tx, ty = target
bearing = MeasurementUtils.angle(x, y, tx, ty)
preds = PredictionUtils.predict_positions(x, y, v, bearing, 0.0)
preds = SimpleAlgorithm._cut_predictions(preds, target)
self._predictions[self._robot_name] = preds
target_dist = MeasurementUtils.distance(x, y, tx, ty)
if not self._find_intersections(preds, self.CIRCLES_RADIUS * 1.5, 0):
self._own_fear_factor = self._base_fear_factor
v, omega = SimpleAlgorithm._navigate_fun(v, theta, bearing, False)
if target_dist < 0.2:
v *= target_dist
if target_dist < 0.05:
v = 0
Vl, Vr = SimpleAlgorithm._get_wheel_speeds(v, omega)
self._send_robot_command(RobotCommand(Vl, Vr, Vl, Vr))
return
for robot_name in list(self._yield_set):
rx = self._states[robot_name]['x']
ry = self._states[robot_name]['y']
if MeasurementUtils.distance(x, y, rx, ry) > 2.0:
self._yield_set.remove(robot_name)
best_v = None
best_bearing = None
best_midpoint = None
best_rate = None
best_preds = None
self._predictions[self._robot_name] = None
if self._last_params is not None:
best_v, best_bearing, best_midpoint = self._last_params
best_preds = PredictionUtils.predict_positions(
x, y, best_v, best_bearing, 0.0)
best_preds = SimpleAlgorithm._cut_predictions(
best_preds, best_midpoint)
best_rate = self._rate_predictions(best_preds)
if self._find_intersections(best_preds, self.CIRCLES_RADIUS, 1):
best_v = None
best_bearing = None
best_midpoint = None
best_rate = None
best_preds = None
try_stop = True
variants_stop_time = time.time() + avail_time - 0.001
var_num = 0
while time.time() < variants_stop_time:
var_num += 1
if try_stop:
rand_v = 0.0
rand_bearing = theta
rand_radius = 0.0
try_stop = False
else:
rand_v = random.uniform(0.0, self.CRUISE_SPEED * 2.0)
rand_bearing = random.uniform(-math.pi, math.pi)
rand_radius = random.uniform(
0.0, self.CRUISE_SPEED * PredictionUtils.CIRCLES_DELTA *
PredictionUtils.CIRCLES_NUM)
mp_x = rand_radius * math.cos(rand_bearing) + x
mp_y = rand_radius * math.sin(rand_bearing) + y
rand_midpoint = mp_x, mp_y
rand_preds = PredictionUtils.predict_positions(
x, y, rand_v, rand_bearing, 0.0)
rand_preds = SimpleAlgorithm._cut_predictions(
rand_preds, rand_midpoint)
rand_rate = self._rate_predictions(rand_preds)
if self._find_intersections(rand_preds, self.CIRCLES_RADIUS * 1.5,
1):
continue
if best_rate is None or rand_rate < best_rate - 0.5:
best_v, best_bearing, best_preds, best_rate, best_midpoint \
= rand_v, rand_bearing, rand_preds, rand_rate, rand_midpoint
self._last_params = best_v, best_bearing, best_midpoint
if best_preds is None:
self._send_stop_command()
return
else:
self._predictions[self._robot_name] = best_preds
best_v, omega = SimpleAlgorithm._navigate_fun(
best_v, theta, best_bearing, False)
mp_x, mp_y = best_midpoint
midpoint_dist = MeasurementUtils.distance(x, y, mp_x, mp_y)
if midpoint_dist < 0.5:
best_v *= midpoint_dist
Vl, Vr = SimpleAlgorithm._get_wheel_speeds(best_v, omega)
self._send_robot_command(RobotCommand(Vl, Vr, Vl, Vr))