def measure(self, angle):
measurement = self.world.get_distance_to_wall(angle)
if measurement > self.max_distance:
measurement = self.max_distance - self.safety_distance
otype = ObservationType.FREE
else:
otype = ObservationType.OBSTACLE
polar = geometry.Polar(angle, measurement)
data = SensorMeasurement(polar, otype)
return data
def scan(self):
logging.info("Scanning started")
prev_measurement = 10
start_angle = int(-self.view_angle / 2)
for angle in range(start_angle, start_angle + self.view_angle + 1,
self.precision):
new_measurement = prev_measurement + random.random() * 2 - 1
polar = geometry.Polar(angle, new_measurement)
data = SensorMeasurement(polar, ObservationType.OBSTACLE)
self.data_queue.put(data)
prev_measurement = new_measurement
time.sleep(0.5)
self.data_queue.put(None)
logging.info("Scanning finished")
def loop():
self.socket.send(f"SCAN {self.precision} {num_steps} {increasing}")
while (data := self.socket.receive()) != "END":
angle, measurement, *rest = data.split(" ")
angle = float(angle)
if not increasing:
angle = -angle
measurement = float(measurement)
if len(rest) > 0 and rest[0] == "FREE":
otype = ObservationType.FREE
measurement -= self.safety_distance
else:
otype = ObservationType.OBSTACLE
polar_angle = self.orientation.in_degrees() + angle
polar = geometry.Polar(polar_angle, measurement)
data = SensorMeasurement(polar, otype)
self.data_queue.put(data)
def find_path(self, graph: sgraph.Graph, goal: geometry.Point):
"""
Returns a node that is less than self.tollerance away from the
goal. Use Node.parent recursively to get the whole path.
"""
min_step_size = self.min_step_size
while not self.shutdown_flag.is_set():
r = random.random()
if r < self.tilt_towards_goal:
approx_target = goal
# Randomly change the target
d = abs(random.gauss(0, self.tollerance))
a = random.randint(0, 359)
p = geometry.Polar(a, d)
target = approx_target.plus_polar(p)
else:
# Select random point
target = self.observed_world.get_random_point()
# Find node that is closest to the target
parent = min(graph, key=get_fun_distance_to(target))
angle = parent.location.angle_to(target).in_degrees()
distance = parent.location.distance_to(target)
if distance < min_step_size:
# Do not plan too small steps
min_step_size *= 0.99
if min_step_size < self.min_step_size / 4:
logging.warning(f"min_step_size reduced to a quarter "
f"({min_step_size:.2f})")
continue
step = min(self.max_step_size, distance)
polar = geometry.Polar(angle, step)
candidate = parent.location.plus_polar(polar)
if not self.observed_world.point_in_bounds(candidate):
continue
free_radius = int(self.robot_size / 2)
if not self.observed_world.is_surrrounding_free(
candidate, free_radius, threshold=1):
# Candidate is not free
continue
if not self.observed_world.is_path_free(
parent.location, candidate, radius=free_radius,
threshold=1.0):
# Path to the candidate is not free
continue
new_node = sgraph.Node(candidate, parent)
graph.add_node(new_node)
min_step_size = self.min_step_size
if candidate.distance_to(goal) < self.tollerance:
return new_node
if len(graph) > 200:
# Give up trying to find path to the goal
logging.warning(f"Couldn't find a path to node {goal}")
return None
return None
def measure(self, angle):
measurement = self.world.get_distance_to_wall(angle)
polar = geometry.Polar(angle, measurement)
data = SensorMeasurement(polar, ObservationType.OBSTACLE)
return data
def test_helper(angle, radius, expected_x, expected_y):
polar = geometry.Polar(angle, radius)
result = cart.plus_polar(polar)
self.assertAlmostEqual(result.x, expected_x)
self.assertAlmostEqual(result.y, expected_y)
def test_change_negative_invalid(self):
p = geometry.Polar(45, 5.)
with self.assertRaises(ValueError):
p.change(-90, -9.)
def test_helper(angle, radius, expected_x, expected_y):
polar = geometry.Polar(angle, radius)
cart = polar.to_cartesian()
self.assertAlmostEqual(cart.x, expected_x)
self.assertAlmostEqual(cart.y, expected_y)
def test_change_negative(self):
p = geometry.Polar(45, 5.)
p.change(-90, -2.)
self.assertAlmostEqual(p.angle.in_degrees(), -45)
self.assertAlmostEqual(p.radius, 3.)
def test_change(self):
p = geometry.Polar(45, 5.)
p.change(20, 1)
self.assertAlmostEqual(p.angle.in_degrees(), 65)
self.assertAlmostEqual(p.radius, 6.)
def test_str(self):
p = geometry.Polar(45, 5.)
self.assertEqual(str(p), "<45.00°, 5.00>")
def test_creation_invalid(self):
with self.assertRaises(ValueError):
geometry.Polar(45, -5.)
def test_creation(self):
p = geometry.Polar(45, 5.)
self.assertAlmostEqual(p.angle.in_degrees(), 45)
self.assertAlmostEqual(p.radius, 5.)