class DeltaNode(Node):
"""
Node for the graph in the Delta space, which is a 2D space where axis 1 is the first path and
axis 2 is the second path. Units are in meters, representing the distance from the start of the
path.
"""
def __init__(self, x, y):
Node.__init__(self)
self.__pos = Point(x, y)
@property
def x(self):
return self.__pos.x
@property
def y(self):
return self.__pos.y
def position(self):
return self.__pos
def dist(self, arg):
"""
Returns the distance to a certain object.
:param arg: Object to calculate distance. Can be Point, Pose, StablePose or MeshNode.
:return: float, distance to the object.
"""
if isinstance(arg, DeltaNode):
return self.__pos.dist(arg.position())
elif isinstance(arg, Point):
return self.__pos.dist(arg)
else:
raise ValueError(
"DeltaNode can't calculate distances to object of type " +
type(arg).__name__)
def __eq__(self, node):
"""
Checks if two nodes are equal.
:param node:
"""
return self.dist(node) < EPS
def __hash__(self):
"""
Hash function to use in dicts.
:return: HashCode associated to Node.
"""
return self.x.__hash__() ^ self.y.__hash__()
def __repr__(self):
"""
Used for printing node.
:return: String representing MeshNode.
"""
return "[" + str(self.x) + ", " + str(self.y) + "]"
def __repr__(self):
s = "Poses: ["
for i in range(len(self.__poses) - 1):
s += Point(self.__poses[i].position()).__repr__() + " -> "
s += Point(self.__poses[-1].position()).__repr__() + "]"
s += "\nTimes: "
for i in range(len(self.__times) - 1):
s += str(self.__times[i]) + " -> "
s += str(self.__times[-1]) + "]"
return s
def add_pose(self, pose, time=None):
"""
Adds a new point to the path.
:param pose: Pose of the drone.
:param time: Time the drone should reach this pose, in respect to the initial pose in the
path.
"""
self.__poses.append(pose)
if time is not None:
self.__times.append(time)
else:
if len(self.__poses) == 1:
self.__times.append(0)
self.__lengths.append(0)
else:
duration = self.__poses[-1].dist(self.__poses[-2]) / MAX_VEL
self.__times.append(self.__times[-1] + duration)
self.__lengths.append(self.__lengths[-1] +
Point(self.__poses[-2].position()).dist(
Point(self.__poses[-1].position())))
def intersection_2d(self, s):
"""
Finds the intersection with another segment. Only works for 2d segments.
:param s: Segment.
:return: Point or None, representing the intersection.
"""
num = ((s.a.x - self.a.x) * (s.b.y - s.a.y) - (s.a.y - self.a.y) * (s.b.x - s.a.x))
den = ((self.b.x - self.a.x) * (s.b.y - s.a.y) - (self.b.y - self.a.y) * (s.b.x - s.a.x))
if abs(den) > EPS:
i = num / den
pt = Point(self.a.x + (self.b.x - self.a.x) * i, self.a.y + (self.b.y - self.a.y) * i)
if self.contains(pt) and s.contains(pt):
return pt
else:
return None
def add_node(n):
for node in self.__nodes:
# Only short edges that are not on the ground or through obstacles
if n.dist(node) < MESH_EDGE_DIST and (n.z > DRONE_HEIGHT or
node.z > DRONE_HEIGHT):
valid_edge = True
for obs in obstacle_collection.obstacles:
if isinstance(obs, Cylinder):
cylinder_seg = Segment(Point(obs.position),
Point(obs.position))
if obs.axis == 'x':
cylinder_seg.b += Point(1000, 0, 0)
elif obs.axis == 'y':
cylinder_seg.b += Point(0, 1000, 0)
else:
cylinder_seg.b += Point(0, 0, 1000)
edge_seg = Segment(Point(n.position()),
Point(node.position()))
if cylinder_seg.min_distance(
edge_seg) < obs.radius:
valid_edge = False
if valid_edge:
n.add_edge(node)
def convert_to_segments(path):
s = []
for i in range(len(path.poses) - 1):
s.append(Segment(Point(path.poses[i].position()),
Point(path.poses[i + 1].position())))
return s
def __compute_orientations(self, intersections, l1, l2, ids):
"""
Computes orientations (CW or CCW) for every intersection. It does this by running A* on the
2D space of the intersections, in which each intersection generates 4 nodes.
O(i^3), where i is the number of intersections.
TODO: This function only works for monotone paths.
:param intersections: List of Intersection objects.
:param l1: Total length of the first path.
:param l2: Total length of the second path.
"""
# Generating nodes
nodes = [DeltaNode(0, 0), DeltaNode(l1, l2), DeltaNode(l1, 0), DeltaNode(0, l2)]
for i in intersections:
for a in range(2):
for b in range(2):
nodes.append(DeltaNode(i.interval_1[a], i.interval_2[b]))
# For every two nodes, adding edge if it does not pass through any intersection (they can
# have a point in common, though)
for n1 in nodes:
for n2 in nodes:
if n1 != n2 and not n1.has_edge(n2):
intersect = False
for i in intersections:
s = Segment(n1.position(), n2.position())
p1 = Point(i.interval_1[0], i.interval_2[0])
p2 = Point(i.interval_1[0], i.interval_2[1])
p3 = Point(i.interval_1[1], i.interval_2[0])
p4 = Point(i.interval_1[1], i.interval_2[1])
s1 = Segment(p1, p2)
s2 = Segment(p1, p3)
s3 = Segment(p2, p4)
s4 = Segment(p3, p4)
# If edge intersects intersection, but not on extremity
intersect |= s.intersects(s1) and not s.intersects_at_extremity(s1)
intersect |= s.intersects(s2) and not s.intersects_at_extremity(s2)
intersect |= s.intersects(s3) and not s.intersects_at_extremity(s3)
intersect |= s.intersects(s4) and not s.intersects_at_extremity(s4)
# Case where it passes through the diagonal
intersect |= s.contains(p1) and s.contains(p4)
intersect |= s.contains(p2) and s.contains(p3)
if not intersect:
n1.add_edge(n2)
# Changing the distance function. This function only allows monotonous paths, and considers
# only the highest distance one of the two drones. This distance will be proportional to
# time, because both drones are moving at the same time, and the one that has to fly the
# furthest is the one that will take most time.
# TODO: This function only allows monotone paths.
def dist_function(node1, node2):
if node1.x > node2.x + EPS or node1.y > node2.y + EPS:
return float("inf")
return max(abs(node1.x - node2.x), abs(node1.y - node2.y))
self.__delta_path[ids] = AStarPlanner().plan(nodes[0], nodes[1], dist_function)
# Checking sides, for each intersection
for i in intersections:
clock_wise = False
for k in range(len(self.__delta_path[ids]) - 1):
s = Segment(self.__delta_path[ids][k].position(),
self.__delta_path[ids][k + 1].position())
# Using middle of the intersection
p = Point((i.interval_1[0] + i.interval_1[1]) / 2.0,
(i.interval_2[0] + i.interval_2[1]) / 2.0)
# Tracing a ray up. If it intersects the path, then the point is ccw.
# TODO: This only works for monotone paths!!
ray = s.intersection_with_line_2d(p, Point(0, 1))
if ray is not None:
clock_wise = True
break
if clock_wise:
i.orientation = 1
else:
i.orientation = -1
def __init__(self, x, y):
Node.__init__(self)
self.__pos = Point(x, y)
def random_point():
return Point(random.uniform(-1, 1), random.uniform(-1, 1),
random.uniform(-1, 1))
def __event_dt(self, x, x_goal, v):
"""
Computes the time until the next event.
O(n_drones + i), where i is the total number of intersections.
:param x: List of np.array of size n_drones and dtype float. Contains, for each step,
the position for each drone in the coordination space.
:param x_goal: np.array of size n_drones and dtype float. Contains, for each drone, the
final position in the coordination space.
:param v: np.array of size n_drones and dtype float. Contains a maximal velocity for
each drone in the coordination space.
:return: float, Time until the next event.
"""
dt = float("inf")
# Time to reach the goals
for i in range(self.__n_drones):
if v[i] > EPS:
dt = min(dt, (x_goal[i] - x[-1][i]) / v[i])
# For each intersection
for i in range(self.__n_drones):
for j in range(self.__n_drones):
if j <= i:
continue
for intersection in self.__delta[self.__i_to_drone_id[i],
self.__i_to_drone_id[j]]:
# Tracing ray in direction (v[i], v[j])
pt = Point(x[-1][i], x[-1][j])
vec = Point(v[i], v[j])
if intersection.orientation == 1:
seg = Segment(
Point(intersection.interval_1[0], 0),
Point(intersection.interval_1[0],
intersection.interval_2[1]))
else:
seg = Segment(
Point(0, intersection.interval_2[0]),
Point(intersection.interval_1[1],
intersection.interval_2[0]))
# One drone is stopped, the event is at the end of the segment
if seg.contains(pt):
# If it is at the end of the segment, ignore
if not (seg.a == pt or seg.b == pt):
if intersection.orientation == 1:
dt = min(dt,
(intersection.interval_2[1] - pt.y) /
v[j])
else:
dt = min(dt,
(intersection.interval_1[1] - pt.x) /
v[i])
# If it reached the segment
elif seg.intersection_with_line_2d(pt, vec):
if intersection.orientation == 1:
dt = min(dt, (intersection.interval_1[0] - pt.x) /
v[i])
else:
dt = min(dt, (intersection.interval_2[0] - pt.y) /
v[j])
if dt < EPS:
raise ValueError(
"Something went wrong. Next event is current event")
return dt