def _get_wheel_speeds_new(v, theta, target_theta, with_reverse,target_distance):
angle = theta - target_theta
angle = MeasurementUtils.normalize_angle(angle)
#angle = abs(angle)
histerezaMin = 10.0
histerezaMax = 30.0
if (target_distance is not None) and (target_distance < 0.3):
Vl = 0
Vr = 0
else:
if angle <= math.radians(-histerezaMax):
Vl = v
Vr = -v
elif (angle > math.radians(-histerezaMax)) and (angle < math.radians(-histerezaMin)):
Vr = v
Vl = MeasurementUtils.normalize_min_max(math.degrees(angle),-histerezaMax,-histerezaMin) * v * (-1);
elif (angle > math.radians(-histerezaMin)) and (angle < math.radians(histerezaMin)):
Vl = v
Vr = v
elif (angle > math.radians(histerezaMin)) and (angle < math.radians(histerezaMax)):
Vr = MeasurementUtils.normalize_min_max(math.degrees(angle),histerezaMin,histerezaMax) * v * (-1);
Vl = v
else:
Vl = -v
Vr = v
return Vl,Vr
def _compute_fear_factors(self):
self._fear_factors = {}
for robot_name, state in self._states.items():
x = state['x']
y = state['y']
theta = state['theta']
fear_factor = state['fear_factor']
for robot_name2, state2 in self._states.items():
if robot_name == robot_name2:
continue
x2 = state2['x']
y2 = state2['y']
theta2 = state2['theta']
dist = MeasurementUtils.distance(x, y, x2, y2)
if dist > FearfulAlgorithm.FF_RADIUS:
continue
angle_diff = abs(MeasurementUtils.normalize_angle(theta - theta2))
if angle_diff > 0.5 * math.pi:
continue
fear_factor += (1 - dist / FearfulAlgorithm.FF_RADIUS) * math.cos(angle_diff) \
* self._states[robot_name]['fear_factor']
self._fear_factors[robot_name] = fear_factor
def _compute_fear_factors(self):
self._fear_factors = {}
for robot_name, state in self._states.items():
x = state['x']
y = state['y']
theta = state['theta']
fear_factor = state['fear_factor']
for robot_name2, state2 in self._states.items():
if robot_name == robot_name2:
continue
x2 = state2['x']
y2 = state2['y']
theta2 = state2['theta']
dist = MeasurementUtils.distance(x, y, x2, y2)
if dist > FearfulAlgorithm.FF_RADIUS:
continue
angle_diff = abs(MeasurementUtils.normalize_angle(theta - theta2))
if angle_diff > 0.5 * math.pi:
continue
fear_factor += (1 - dist / FearfulAlgorithm.FF_RADIUS) * math.cos(angle_diff) \
* self._states[robot_name]['fear_factor']
self._fear_factors[robot_name] = fear_factor
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 _navigate_fun(v, theta, target_theta, with_reverse):
angle = theta - target_theta
angle = MeasurementUtils.normalize_angle(angle)
if with_reverse and abs(angle) > 0.5 * math.pi:
angle = math.copysign(math.pi - abs(angle), angle)
v *= -1.0
p = angle / math.pi
omega = -10.0 * p * 2.0
omega = math.copysign(min(abs(omega), 5.0), omega)
if abs(omega) > 0.5:
v = 0.0
return v, omega
def _navigate_fun(v, theta, target_theta, with_reverse):
angle = theta - target_theta
angle = MeasurementUtils.normalize_angle(angle)
if with_reverse and abs(angle) > 0.5 * math.pi:
angle = math.copysign(math.pi - abs(angle), angle)
v *= -1.0
p = angle / math.pi
omega = -10.0 * p * 2.0
omega = math.copysign(min(abs(omega), 5.0), omega)
if abs(omega) > 0.5:
v = 0.0
return v, omega
def _navigate_fun(v, theta, target_theta, with_reverse):
angle = theta - target_theta
angle = MeasurementUtils.normalize_angle(angle)
if with_reverse and abs(angle) > 0.5 * math.pi:
angle = math.copysign(math.pi - abs(angle), angle)
v *= -1.0
p = angle / math.pi
omega = -10.0 * p * 2.0
omega = math.copysign(min(abs(omega), 5.0), omega)
if abs(omega) > math.pi:
v = 0.0
#if abs(v) > FearfulAlgorithm.CRUISE_SPEED:
# v = math.copysign(FearfulAlgorithm.CRUISE_SPEED,v)
return v, omega
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