def move_right(self, e) -> None:
self.reset()
if self._services.algorithm.map.size.n_dim == 3:
p = Point(e.position.x + self.speed, e.position.y, e.position.z)
else:
p = Point(e.position.x + self.speed, e.position.y)
self.move_valid_entity(e, p)
def cast_lines_on_screen(lines, width, height, distance_to_screen,
clipping_distance):
# clipping plane
plane_point1 = Point(20, 2, clipping_distance)
plane_point2 = Point(1, 1, clipping_distance)
plane_point3 = Point(2, 30, clipping_distance)
plane_points = [plane_point1, plane_point2, plane_point3]
screen_lines = []
for line in lines:
line_to_draw = line
# skip lines that are behind the clipping plane
if (line.start.z <= clipping_distance
and line.end.z <= clipping_distance):
continue
# clip lines that go through the clipping plane if needed
line_to_draw = clip_line(line, plane_points)
# cast
points = []
for point in line_to_draw.get_points():
s_point = cast_point_on_screen(point, width, height,
distance_to_screen)
points.append(s_point)
screen_lines.append(Line(points[0], points[1], line.color))
return screen_lines
def init_direction_vectors(self):
"""
This function generates the neighbouring coordinates using the cartesian product.
We must remove the first coordinate as this represents all zeros.
Example Input: n dimensions = 2:
Output: [(1, 0), (-1, 0), (1, 1), ...]
"""
self.ALL_POINTS_MOVE_VECTOR: List[Point] = \
list(map(lambda x: Point(*x),
product(*[(0, -1, 1) for i in range(self.size.n_dim)])
))[1:]
"""
This does the same as the above without generating diagonal coordinates.
First we generate the positive direct neighbouring coordinates.
We concatenate this to the negative coordinates and convert to a list of Points.
"""
POSITIVE_DIRECT_POINTS_DIMENSIONS: List[List[int]] = [[
1 if i == pos else 0 for i in range(self.size.n_dim)
] for pos in range(self.size.n_dim)]
ALL_DIRECT_POINTS_DIMENSIONS: List[List[int]] = POSITIVE_DIRECT_POINTS_DIMENSIONS + \
list(map(lambda x: [-i for i in x], POSITIVE_DIRECT_POINTS_DIMENSIONS))
self.DIRECT_POINTS_MOVE_VECTOR: List[Point] = \
list(map(lambda x: Point(*x), ALL_DIRECT_POINTS_DIMENSIONS))
def script_classic_trajectory(var=10):
"""
Script utilisant le filtre de Kalman avec des mesures simulées à partir d'une trajectoire inventée !
:return:
"""
print("script_classic_trajectory")
l_points = [[-1000., 200.], [-1000., 800.], [-400., 1200.], [500., 500.], [1100., 180.]]
dt = 0.025
real_path = generateur_chemin.generate_path(l_points=l_points, velocity_translation=25,
velocity_rotation=0.7, dt=dt)
measures_pos = np.array(real_path)
real_path_point = []
for couple in real_path:
x, y = couple
pos = Point(x, y)
real_path_point.append(pos)
l_pos_filtre = [real_path_point[0]]
l_pos_measured = [real_path_point[0]]
vite = get_velocity(measures_pos, dt)
measures = np.concatenate((measures_pos, vite), axis=1)
measures = np.asmatrix(measures)
filtering = FiltrageKalman(measures[0, :].T, dt=dt)
for i in range(1, measures.shape[0]):
x = measures[i, 0]
y = measures[i, 1]
# print "x et y", x, y, "i" ,i
x_bruite, y_bruite = x + np.random.randn()*var, y + np.random.randn()*var
filtering.update(x_bruite, y_bruite)
pos = filtering.get_current_position()
l_pos_filtre.append(pos)
l_pos_measured.append(Point(x_bruite, y_bruite))
print(get_mer(real_path_point, l_pos_filtre))
print(get_mer(real_path_point, l_pos_measured))
return real_path_point, l_pos_filtre, l_pos_measured
def test_ne_instance(self) -> None:
map1: SparseMap = SparseMap(Size(200, 200),
Agent(Point(20, 20), 10),
[Obstacle(Point(40, 40), 10), Obstacle(Point(100, 100), 40)],
Goal(Point(180, 160), 10))
map2: int = 2
self.assertNotEqual(map1, map2)
def __get_rand_position(self,
dimensions: Size,
start: Optional[Point] = None) -> Point:
if start is None:
start = Point(*([0] * dimensions.n_dim))
return Point(*np.random.randint([*start], [*dimensions]))
def generateRandomLineSegments(n, min_x, max_x, min_y, max_y):
segments = []
s = set()
while n > 0:
x1 = randint(min_x, max_x)
x2 = randint(min_x, max_x)
y1 = randint(min_y, max_y)
y2 = randint(min_y, max_y)
if x1 == x2:
continue
if x1 in s or x2 in s:
continue
if x1 > x2:
segments.append(Segment(Point(x2, y2), Point(x1, y1)))
else:
segments.append(Segment(Point(x1, y1), Point(x2, y2)))
s.add(x1)
s.add(x2)
n -= 1
return segments
def __subdivide(top_left_corner, dim):
if dim.width < 2 * min_room_size.width - 1 and dim.height < 2 * min_room_size.height - 1:
__place_room(grid, dim, top_left_corner)
return
elif dim.width < 2 * min_room_size.width - 1:
is_vertical_split = False
elif dim.height < 2 * min_room_size.height - 1:
is_vertical_split = True
else:
is_vertical_split = int(torch.randint(0, 2, (1,)).item()) == 0
if dim.width <= max_room_size.width and dim.height <= max_room_size.height:
if int(torch.randint(0, 2, (1,)).item()) == 0:
__place_room(grid, dim, top_left_corner)
return
# split
new_top_left_corner1 = top_left_corner
if is_vertical_split:
new_dim1 = Size(__get_subdivision_number(dim, True), dim.height)
new_top_left_corner2 = Point(top_left_corner.x + new_dim1.width - 1, top_left_corner.y)
new_dim2 = Size(dim.width - new_dim1.width + 1, dim.height)
else:
new_dim1 = Size(dim.width, __get_subdivision_number(dim, False))
new_top_left_corner2 = Point(top_left_corner.x, top_left_corner.y + new_dim1.height - 1)
new_dim2 = Size(dim.width, dim.height - new_dim1.height + 1)
__subdivide(new_top_left_corner1, new_dim1)
__subdivide(new_top_left_corner2, new_dim2)
def _find_path_internal(self) -> None:
"""
Read super description
The internal implementation of :ref:`find_path`
"""
#._get_grid() is in Algorithm class and gets the map
grid: Map = self._get_grid()
trace = main(grid)
if trace == None or trace == 0:
print('No path found')
pass
else:
# for point in trace:
# point.numpy()
print("final path --------", trace)
for point in trace[1:-1]:
self.waypoint.add(Point(point[0] + 0.25, point[1] - 0.25))
self.waypoint.add(Point(point[0] + 0.25, point[1] + 0.25))
self.waypoint.add(Point(point[0] - 0.25, point[1] + 0.25))
self.waypoint.add(Point(point[0] - 0.25, point[1] - 0.25))
print('waypoint =====', self.waypoint)
if type(trace) != bool:
print(
'______________________________________________________________'
)
print('trace ======', trace)
for point in trace:
self.move_agent(Point(int(point[0]), int(point[1])))
self.key_frame(ignore_key_frame_skip=True)
def test_is_valid_position_invalid(self) -> None:
map1: SparseMap = DenseMap([
[DenseMap.EXTENDED_WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.WALL_ID],
[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.GOAL_ID],
]).convert_to_sparse_map()
self.assertFalse(map1.is_agent_valid_pos(Point(1, 0)))
self.assertFalse(map1.is_agent_valid_pos(Point(-1, -1)))
def counterclockwise(self, p1, p2, p3):
"""
Check whether a turn from (p1, p2) to (p1, p3) is counter clockwise.
If it is so, the return value is > 0, if the turn is clockwise, the
return value is < 0. If vectors are collinear, the return value is 0.
"""
return self.cross(Point(p2.x - p1.x, p2.y - p1.y), Point(p3.x - p1.x, p3.y - p1.y))
def cast_point_on_screen(point, width, height, distance_to_screen):
try:
x = point.x * distance_to_screen / point.z + width / 2
y = point.y * distance_to_screen / point.z + height / 2
return Point(x, y, 1)
except Exception as e:
print(e, flush=True)
return Point(width / 2, height / 2, 1)
def _get_new_vertex(self, q_near: Vertex, q_sample: Point, max_dist) -> Vertex:
dir = q_sample.to_tensor() - q_near.position.to_tensor()
if torch.norm(dir) <= max_dist:
return Vertex(q_sample)
dir_normalized = dir / torch.norm(dir)
q_new = Point.from_tensor(q_near.position.to_tensor() + max_dist * dir_normalized)
return Vertex(q_new)
def test_is_valid_position_normal(self) -> None:
map1: DenseMap = DenseMap([
[DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID],
[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.GOAL_ID],
])
self.assertTrue(map1.is_agent_valid_pos(Point(1, 1)))
self.assertTrue(map1.is_agent_valid_pos(Point(0, 1)))
self.assertTrue(map1.is_agent_valid_pos(Point(3, 2)))
def test_move_agent_out_of_bounds(self) -> None:
map1: DenseMap = DenseMap([
[DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID],
[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.GOAL_ID],
])
map1.move_agent(Point(-1, 0))
self.assertEqual(DenseMap.AGENT_ID, map1.at(Point(0, 1)))
self.assertEqual([Trace(Point(0, 1))], map1.trace)
def update(self, x, y):
if self.acceleration_filtering(Point(x, y)):
self.last_point = Point(x, y)
self.kalman_filter.predict()
self.kalman_filter.measure(np.array([x, y])[:, np.newaxis])
self.history.append(self.get_last_state())
else:
self.last_point = None
self.kalman_filter.predict()
def __init__(self, glyph):
points = [Point(line.x, line.y) for line in glyph.lines]
if glyph.orientation == HORIZONTAL:
points.extend([Point(line.x + line.l - 1, line.y)
for line in glyph.lines])
else:
points.extend([Point(line.x, line.y + line.l - 1)
for line in glyph.lines])
self.points = ConvexHull(points).points
def test_move_agent_out_of_bounds(self) -> None:
map1: SparseMap = DenseMap([
[DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID],
[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.GOAL_ID],
]).convert_to_sparse_map()
map1.move_agent(Point(-1, 0))
self.assertEqual(Point(0, 1), map1.agent.position)
self.assertEqual([Trace(Point(0, 1))], map1.trace)
def test_move_agent_no_trace(self) -> None:
map1: SparseMap = DenseMap([
[DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID],
[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.GOAL_ID],
]).convert_to_sparse_map()
map1.move_agent(Point(1, 1), True)
self.assertEqual(Point(1, 1), map1.agent.position)
self.assertEqual([], map1.trace)
def test_convert_to_dense_map(self) -> None:
map1: SparseMap = SparseMap(
Size(4, 3),
Agent(Point(0, 1)),
[Obstacle(Point(0, 0)), Obstacle(Point(1, 0)), Obstacle(Point(2, 0)), Obstacle(Point(3, 0))],
Goal(Point(3, 2))
)
map2: DenseMap = map1.convert_to_dense_map()
self.assertEqual(map1, map2)
def test_move_agent_normal(self) -> None:
map1: DenseMap = DenseMap(
[[[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID]],
[[DenseMap.CLEAR_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID,
DenseMap.GOAL_ID]]]).convert_to_sparse_map()
map1.move_agent(Point(0, 0, 0))
self.assertEqual(Point(0, 0, 0), map1.agent.position)
self.assertTrue([Trace(Point(0, 0, 0))], map1.trace)
def test_is_valid_position_normal(self) -> None:
map1: SparseMap = DenseMap([
[DenseMap.EXTENDED_WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID],
[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.GOAL_ID],
]).convert_to_sparse_map()
self.assertTrue(map1.is_agent_valid_pos(Point(1, 1)))
self.assertTrue(map1.is_agent_valid_pos(Point(0, 1)))
self.assertTrue(map1.is_agent_valid_pos(Point(3, 2)))
self.assertTrue(map1.is_agent_valid_pos(Point(0, 0)))
def test_move_agent_normal(self) -> None:
map1: DenseMap = DenseMap([
[DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID],
[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.GOAL_ID],
])
map1.move_agent(Point(1, 1))
self.assertEqual(Point(1, 1), map1.agent.position)
self.assertEqual(DenseMap.AGENT_ID, map1.at(Point(1, 1)))
self.assertTrue([Trace(Point(1, 1))], map1.trace)
def test_eq_sparse_map(self) -> None:
map1: DenseMap = DenseMap(
[[[DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.CLEAR_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID]],
[[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID],
[DenseMap.GOAL_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID]]])
map2: SparseMap = SparseMap(Size(3, 2, 2), Agent(Point(
0, 1, 0)), [Obstacle(Point(0, 0, 0)),
Obstacle(Point(1, 0, 0))], Goal(Point(0, 1, 1)))
self.assertEqual(map1, map2)
def __render_arc(self, p1: Point, p2: Point, colour: Colour):
center = p1
radius = 1
dir = p2.to_tensor() - p1.to_tensor()
angle = torch.atan2(dir[1], dir[0])
angle %= 2 * np.pi
self.get_renderer_view().draw_arc(center,
angle,
radius=radius,
colour=colour)
def get_measures_from_state(self, x, y):
"""
(x,y) sont les coordonnées du robot
"""
point = Point(x, y)
d1 = point.distance(self.beacon1)
d2 = point.distance(self.beacon2)
d3 = point.distance(self.beacon3)
# d1 = sqrt((L/2.-x)**2+(l/2.-y)**2)
# d2 = sqrt((-L/2.-x)**2+(l-y)**2)
# d3 = sqrt((-x-L/2.)**2+y**2)
return d1-d2, d1-d3, d2-d3
def test_move_agent_no_trace(self) -> None:
map1: DenseMap = DenseMap([
[[DenseMap.CLEAR_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID,
DenseMap.EXTENDED_WALL_ID]],
[[DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.GOAL_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID]]
])
map1.move_agent(Point(0, 1, 1), True)
self.assertEqual(Point(0, 1, 1), map1.agent.position)
self.assertEqual(DenseMap.AGENT_ID, map1.grid[0, 1, 1])
self.assertEqual([], map1.trace)
def get_measures_from_state(self, x, y):
"""
(x,y) sont les coordonnées du robot
"""
point = Point(x, y)
d1 = point.distance(self.beacon1)
d2 = point.distance(self.beacon2)
d3 = point.distance(self.beacon3)
# d1 = sqrt((L/2.-x)**2+(l/2.-y)**2)
# d2 = sqrt((-L/2.-x)**2+(l-y)**2)
# d3 = sqrt((-x-L/2.)**2+y**2)
return d1 - d2, d1 - d3, d2 - d3
def test_move_agent_out_of_bounds(self) -> None:
map1: DenseMap = DenseMap([
[[DenseMap.CLEAR_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID,
DenseMap.EXTENDED_WALL_ID]],
[[DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.GOAL_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID]]
])
map1.move_agent(Point(-1, 0, 0))
self.assertEqual(Point(0, 1, 0), map1.agent.position)
self.assertEqual(DenseMap.AGENT_ID, map1.grid[0, 1, 0])
self.assertEqual([Trace(Point(0, 1, 0))], map1.trace)
def _setup_sim(self,
config: Optional[Configuration] = None,
goal: Optional[Point] = None) -> Simulator:
"""
Sets up the simulator (e.g. algorithm and map configuration).
"""
while self.grid is None or self.agent is None:
rospy.loginfo("Waiting for grid and agent to initialise...")
rospy.sleep(0.5)
if config is None:
config = Configuration()
# general
config.simulator_graphics = True
config.simulator_key_frame_speed = 0.16
config.simulator_key_frame_skip = 20
config.get_agent_position = lambda: self._world_to_grid(
Point(self.agent.pose.position.x, self.agent.pose.position.y))
config.visualiser_simulator_config = False # hide the simulator config window
# algorithm
if config.algorithm_name is None:
config.algorithm_name = "WPN-view"
config.simulator_algorithm_type, config.simulator_testing_type, config.simulator_algorithm_parameters = config.algorithms[
config.algorithm_name]
# map
goal = Goal(Point(0, 0) if goal is None else goal)
agent = Agent(self._world_to_grid(
Point(self.agent.pose.position.x, self.agent.pose.position.y)),
radius=self.INFLATE)
mp = RosMap(agent,
goal,
lambda: self.grid,
traversable_threshold=self.TRAVERSABLE_THRESHOLD,
unmapped_value=-1,
wp_publish=self._send_way_point,
update_requested=self._map_update_requested,
name="ROS Map")
config.maps = {mp.name: mp}
config.simulator_initial_map = list(config.maps.values())[0]
config.map_name = list(config.maps.keys())[0]
# create the simulator
s = Services(config)
s.algorithm.map.request_update()
sim = Simulator(s)
return sim
def __get_nearby_rooms_edges(room):
room_top_left_point, room_size = room
edges = []
full_edge = []
q = 0
for x in range(room_top_left_point.x, room_top_left_point.x + room_size.width):
y = room_top_left_point.y
# up
if in_bounds(x, y):
full_edge.append(Point(x, y))
n_x, n_y = x, y - 1
if in_bounds(n_x, n_y) and grid[n_y][n_x] == DenseMap.WALL_ID:
edges.append(q)
q += 1
for y in range(room_top_left_point.y, room_top_left_point.y + room_size.height):
x = room_top_left_point.x
# right
x = room_top_left_point.x + room_size.width - 1
if in_bounds(x, y):
full_edge.append(Point(x, y))
n_x, n_y = x + 1, y
if in_bounds(n_x, n_y) and grid[n_y][n_x] == DenseMap.WALL_ID:
edges.append(q)
q += 1
for x in range(room_top_left_point.x, room_top_left_point.x + room_size.width):
y = room_top_left_point.y
# bottom
y = room_top_left_point.y + room_size.height - 1
if in_bounds(x, y):
full_edge.append(Point(x, y))
n_x, n_y = x, y + 1
if in_bounds(n_x, n_y) and grid[n_y][n_x] == DenseMap.WALL_ID:
edges.append(q)
q += 1
for y in range(room_top_left_point.y, room_top_left_point.y + room_size.height):
x = room_top_left_point.x
# left
if in_bounds(x, y):
full_edge.append(Point(x, y))
n_x, n_y = x - 1, y
if in_bounds(n_x, n_y) and grid[n_y][n_x] == DenseMap.WALL_ID:
edges.append(q)
q += 1
return edges, full_edge
"""
Addition used to move move point in direction of the vector
"""
from structures import Vector, Point
if __name__ == '__main__':
point = Point(1,0)
vector = Vector(2,3)
point2 = point.add_vector(vector)
print(point2.x, point2.y)
"""
Substraction is used to figure out vector from one point to another
V = P(destination) - P(current)
"""
from structures import Vector, Point
if __name__ == '__main__':
destination = Point(0, -1)
current_position = Point(1, 1)
v = destination.subtract(current_position)
print(v.x, v.y)
"""
Normalized vector is vector of length 1. Used where one direction is matter.
E.g: direction of the view, direction of movement, etc.
"""
from structures import Vector, Point
if __name__ == '__main__':
p= Point(3,4)
i = Point(1,2)
pi = p.subtract(i)
normalized_vector = pi.normalized()
print("view vector: ", normalized_vector.x, normalized_vector.y)
print("view vector length: ", normalized_vector.length())
