def signed_distance_to(self, point):
"""
Compute signed distance from this polyline to the given point.
Sign convention is positive to the right and negative to the left, e.g.:
*
|
negative | positive
|
|
*
:param point: query point
:type point: Point
"""
# NOTE: for now, we'll just implement this with a naive linear search.
# In future, if we want to optimize at all we can pass in a guess of
# which index we expect the closest point to be close to.
best_signed_distance = float("inf")
for ii in range(1, len(self.points)):
segment = LineSegment(self.points[ii - 1], self.points[ii])
signed_distance = segment.signed_distance_to(point)
if abs(signed_distance) < abs(best_signed_distance):
best_signed_distance = signed_distance
return best_signed_distance
def draw(self):
if self.is_vertical():
LineSegment(Point(self.point.x, self.rectangle.y_max),
Point(self.point.x,
self.rectangle.y_min)).draw(Color.RED)
else:
LineSegment(Point(self.rectangle.x_max, self.point.y),
Point(self.rectangle.x_min,
self.point.y)).draw(Color.BLUE)
self.point.draw()
def split_intersection_into_segment(self, segment_intersect, parent_segment):
"""
Split a segment in two based on intersection point of a previous segment
:param segment_intersect:
:param parent_segment:
:return:
"""
x_intersection, y_intersection = self.get_intersection_point(segment_intersect,
parent_segment)
segment_up_intersection = LineSegment(segment_intersect.get_identifier() + "1", x_intersection,
y_intersection,
segment_intersect.end_point.get_x(),
segment_intersect.end_point.get_y())
segment_down_intersection = LineSegment(segment_intersect.get_identifier() + "2",
segment_intersect.start_point.get_x(),
segment_intersect.start_point.get_y(), x_intersection,
y_intersection)
return segment_up_intersection, segment_down_intersection
def __init__(self, x_min: float, x_max: float, y_min: float,
y_max: float) -> None:
self.x_min = x_min
self.x_max = x_max
self.y_min = y_min
self.y_max = y_max
self.top_left = Point(x_min, y_max)
self.top_right = Point(x_max, y_max)
self.bottom_left = Point(x_min, y_min)
self.bottom_right = Point(x_max, y_min)
self.line_segments = [
LineSegment(self.top_left, self.top_right),
LineSegment(self.bottom_left, self.bottom_right),
LineSegment(self.top_left, self.bottom_left),
LineSegment(self.top_right, self.bottom_right),
]
def init_interpolator(self):
if self._waypoints is None:
return False
self._markers_msg = MarkerArray()
self._marker_id = 0
self._interp_fcns['pos'] = list()
self._segment_to_wp_map = [0]
if self._waypoints.num_waypoints == 2:
self._interp_fcns['pos'].append(
LineSegment(
self._waypoints.get_waypoint(0).pos,
self._waypoints.get_waypoint(1).pos))
self._segment_to_wp_map.append(1)
elif self._waypoints.num_waypoints > 2:
self._interp_fcns['pos'] = BezierCurve.generate_cubic_curve([
self._waypoints.get_waypoint(i).pos
for i in range(self._waypoints.num_waypoints)
])
else:
return False
# Reparametrizing the curves
lengths = [seg.get_length() for seg in self._interp_fcns['pos']]
lengths = [0] + lengths
self._s = np.cumsum(lengths) / np.sum(lengths)
mean_vel = np.mean([
self._waypoints.get_waypoint(k).max_forward_speed
for k in range(self._waypoints.num_waypoints)
])
if self._duration is None:
self._duration = np.sum(lengths) / mean_vel
if self._start_time is None:
self._start_time = 0.0
if self._waypoints.num_waypoints == 2:
head_offset_line = deepcopy(
self._waypoints.get_waypoint(1).heading_offset)
self._interp_fcns['heading'] = lambda x: head_offset_line
else:
# Set a simple spline to interpolate heading offset, if existent
heading = [
self._waypoints.get_waypoint(i).heading_offset
for i in range(self._waypoints.num_waypoints)
]
self._heading_spline = splrep(self._s, heading, k=3, per=False)
self._interp_fcns['heading'] = lambda x: splev(
x, self._heading_spline)
return True
return True
def get_collinear_line_segments_for_single_point(self, origin):
qualifying_sets = self.get_sets_of_qualifying_points(origin)
line_segments = []
for qualifying_set in qualifying_sets:
qualifying_set.append(origin)
qualifying_set.sort()
min_point = qualifying_set[0]
max_point = qualifying_set[len(qualifying_set) - 1]
line_segments.append(LineSegment(min_point, max_point))
return line_segments
def _eval(self, string):
"""
Try to evaluate given string
(e.g., "``is_on(P(2.0,2.0),P(1.0,1.0),P(3.0,3.0))``").
:param string: The expression to evaluate; the LineSegmentParser \
object unstringifies LineSegment and Point objects in ``string`` \
parameter and tries to call a function of the LineSegment object \
(also given by ``string`` parameter) with unstringified objects as \
arguments.
:type string: ``str``
:raises ValueError: Function provided in ``string`` parameter is not \
a function in the LineSegment class, some argument is not a Point or \
LineSegment after trying to unstringify or the ``string`` parameter \
is improperly formatted.
"""
fn_start, fn_end = string.find("("), string.rfind(")")
fn_name, fn_args = string[:fn_start], string[fn_start + 1: fn_end]
for fn in dir(LineSegment):
if fn_name == fn:
line_segment_function = getattr(LineSegment, fn)
break
else:
raise ValueError("Function not contained in dir of Point")
import re
parsed_args = []
line_segment_pattern = r'(L\(P\(-?\d\.\d+(,-?\d\.\d+)*\),P\(-?\d\.\d+(,-?\d\.\d+)*\)\)|P\(x(,x)*\),P\(x(,x)*\)\))|P\(-?\d\.\d+(,-?\d\.\d+)*\)|P\(x(,x)*\)'
match_obj_iter = re.finditer(line_segment_pattern, fn_args)
for match in match_obj_iter:
if match.group()[0] == 'L':
parsed_args.append(LineSegment.unstringify(match.group()))
else:
parsed_args.append(Point.unstringify(match.group()))
fn_args = fn_args.replace(match.group(), '', 1)
if not all([char == "," for char in fn_args]):
raise ValueError("Only Point arguments acceptable")
try:
return line_segment_function(*parsed_args)
except Exception, e:
raise ValueError("Bad args provided")
def map_file_to_tasks(file):
"""
Read file and map to line segments list
:return: array of line segment
"""
line_segments_list = []
with open(file, 'r') as f:
for line in f:
elements = line.split(",")
if len(elements) == 5:
line_segment = LineSegment(str(elements[0]),
float(elements[1]),
float(elements[2]),
float(elements[3]),
int(elements[4]))
line_segments_list.append(line_segment)
return line_segments_list
def init_interpolator(self):
if self._waypoints is None:
return False
if self._waypoints.num_waypoints < 2:
return False
self._markers_msg = MarkerArray()
self._marker_id = 0
self._interp_fcns['pos'] = list()
# TODO: Segment tracking map
self._segment_to_wp_map = [0]
for i in range(1, self._waypoints.num_waypoints):
self._interp_fcns['pos'].append(
LineSegment(
self._waypoints.get_waypoint(i - 1).pos,
self._waypoints.get_waypoint(i).pos))
# Reparametrizing the curves
lengths = [seg.get_length() for seg in self._interp_fcns['pos']]
lengths = [0] + lengths
self._s = np.cumsum(lengths) / np.sum(lengths)
mean_vel = np.mean([
self._waypoints.get_waypoint(k).max_forward_speed
for k in range(self._waypoints.num_waypoints)
])
if self._duration is None:
self._duration = np.sum(lengths) / mean_vel
if self._start_time is None:
self._start_time = 0.0
# Set a simple spline to interpolate heading offset, if existent
heading = [
self._waypoints.get_waypoint(k).heading_offset
for k in range(self._waypoints.num_waypoints)
]
self._heading_spline = splrep(self._s, heading, k=3, per=False)
self._interp_fcns['heading'] = lambda x: splev(x, self._heading_spline)
return True
def init_interpolator(self):
if self._waypoints is None:
return False
self._markers_msg = MarkerArray()
self._marker_id = 0
self._interp_fcns['pos'] = list()
self._segment_to_wp_map = [0]
if self._waypoints.num_waypoints == 2:
self._interp_fcns['pos'].append(
LineSegment(self._waypoints.get_waypoint(0).pos,
self._waypoints.get_waypoint(1).pos))
self._segment_to_wp_map.append(1)
# Set a simple spline to interpolate heading offset, if existent
heading = [self._waypoints.get_waypoint(k).heading_offset for k in range(self._waypoints.num_waypoints)]
elif self._waypoints.num_waypoints > 2:
q_seg = self._waypoints.get_waypoint(0).pos
q_start_line = q_seg
heading = [self._waypoints.get_waypoint(0).heading_offset]
for i in range(1, self._waypoints.num_waypoints):
first_line = LineSegment(q_start_line, self._waypoints.get_waypoint(i).pos)
radius = min(self._radius, first_line.get_length() / 2)
if i + 1 < self._waypoints.num_waypoints:
second_line = LineSegment(self._waypoints.get_waypoint(i).pos,
self._waypoints.get_waypoint(i + 1).pos)
radius = min(radius, second_line.get_length() / 2)
if i < self._waypoints.num_waypoints - 1:
q_seg = np.vstack(
(q_seg, first_line.interpolate((first_line.get_length() - radius) / first_line.get_length())))
self._interp_fcns['pos'].append(LineSegment(q_start_line, q_seg[-1, :]))
heading.append(self._waypoints.get_waypoint(i).heading_offset)
self._segment_to_wp_map.append(i)
if i == self._waypoints.num_waypoints - 1:
q_seg = np.vstack((q_seg, self._waypoints.get_waypoint(i).pos))
self._interp_fcns['pos'].append(LineSegment(q_seg[-2, :], q_seg[-1, :]))
heading.append(self._waypoints.get_waypoint(i).heading_offset)
self._segment_to_wp_map.append(i)
elif i + 1 < self._waypoints.num_waypoints:
q_seg = np.vstack((q_seg, second_line.interpolate(radius / second_line.get_length())))
self._interp_fcns['pos'].append(
BezierCurve([q_seg[-2, :], self._waypoints.get_waypoint(i).pos, q_seg[-1, :]], 5))
heading.append(self._waypoints.get_waypoint(i).heading_offset)
self._segment_to_wp_map.append(i)
q_start_line = deepcopy(q_seg[-1, :])
else:
return False
# Reparametrizing the curves
lengths = [seg.get_length() for seg in self._interp_fcns['pos']]
lengths = [0] + lengths
self._s = np.cumsum(lengths) / np.sum(lengths)
mean_vel = np.mean(
[self._waypoints.get_waypoint(k).max_forward_speed for k in range(self._waypoints.num_waypoints)])
if self._duration is None:
self._duration = np.sum(lengths) / mean_vel
if self._start_time is None:
self._start_time = 0.0
if self._waypoints.num_waypoints == 2:
head_offset_line = deepcopy(self._waypoints.get_waypoint(1).heading_offset)
self._interp_fcns['heading'] = lambda x: head_offset_line
else:
# Set a simple spline to interpolate heading offset, if existent
self._heading_spline = splrep(self._s, heading, k=3, per=False)
self._interp_fcns['heading'] = lambda x: splev(x, self._heading_spline)
return True
def init_interpolator(self):
if self._waypoints is None:
return False
self._interp_fcns['pos'] = list()
self._segment_to_wp_map = [0]
if self._waypoints.num_waypoints == 2:
self._interp_fcns['pos'].append(
LineSegment(self._waypoints.get_waypoint(0).pos,
self._waypoints.get_waypoint(1).pos))
self._segment_to_wp_map.append(1)
# Set a simple spline to interpolate heading offset, if existent
heading = [self._waypoints.get_waypoint(k).heading_offset for k in range(self._waypoints.num_waypoints)]
elif self._waypoints.num_waypoints > 2:
q_seg = self._waypoints.get_waypoint(0).pos
q_start_line = q_seg
heading = [0]
for i in range(1, self._waypoints.num_waypoints):
first_line = LineSegment(q_start_line, self._waypoints.get_waypoint(i).pos)
radius = min(self._radius, first_line.get_length() / 2)
if i + 1 < self._waypoints.num_waypoints:
second_line = LineSegment(self._waypoints.get_waypoint(i).pos,
self._waypoints.get_waypoint(i + 1).pos)
radius = min(radius, second_line.get_length() / 2)
if i < self._waypoints.num_waypoints - 1:
q_seg = np.vstack(
(q_seg, first_line.interpolate((first_line.get_length() - radius) / first_line.get_length())))
self._interp_fcns['pos'].append(LineSegment(q_start_line, q_seg[-1, :]))
heading.append(0)
self._segment_to_wp_map.append(i)
if i == self._waypoints.num_waypoints - 1:
q_seg = np.vstack((q_seg, self._waypoints.get_waypoint(i).pos))
self._interp_fcns['pos'].append(LineSegment(q_seg[-2, :], q_seg[-1, :]))
heading.append(0)
self._segment_to_wp_map.append(i)
elif i + 1 < self._waypoints.num_waypoints:
q_seg = np.vstack((q_seg, second_line.interpolate(radius / second_line.get_length())))
self._interp_fcns['pos'].append(
BezierCurve([q_seg[-2, :], self._waypoints.get_waypoint(i).pos, q_seg[-1, :]], 5))
heading.append(0)
self._segment_to_wp_map.append(i)
q_start_line = deepcopy(q_seg[-1, :])
else:
return False
# Reparametrizing the curves
lengths = [seg.get_length() for seg in self._interp_fcns['pos']]
lengths = [0] + lengths
self._s = np.cumsum(lengths) / np.sum(lengths)
mean_vel = np.mean(
[self._waypoints.get_waypoint(k).max_forward_speed for k in range(self._waypoints.num_waypoints)])
if self._duration is None:
self._duration = np.sum(lengths) / mean_vel
if self._start_time is None:
self._start_time = 0.0
if self._waypoints.num_waypoints == 2:
head_offset_line = deepcopy(self._waypoints.get_waypoint(1).heading_offset)
self._interp_fcns['heading'] = lambda x: head_offset_line
else:
# Set a simple spline to interpolate heading offset, if existent
self._heading_spline = splrep(self._s, heading, k=3, per=False)
self._interp_fcns['heading'] = lambda x: splev(x, self._heading_spline)
return True
def init_interpolator(self):
if self._waypoints is None:
return False
self._interp_fcns['pos'] = list()
self._segment_to_wp_map = [0]
if self._waypoints.num_waypoints == 2:
self._interp_fcns['pos'].append(
LineSegment(
self._waypoints.get_waypoint(0).pos,
self._waypoints.get_waypoint(1).pos))
self._segment_to_wp_map.append(1)
elif self._waypoints.num_waypoints > 2:
tangents = [
np.zeros(3) for _ in range(self._waypoints.num_waypoints)
]
lengths = [
self._waypoints.get_waypoint(i + 1).dist(
self._waypoints.get_waypoint(i).pos)
for i in range(self._waypoints.num_waypoints - 1)
]
lengths = [0] + lengths
# Initial vector of parametric variables for the curve
u = np.cumsum(lengths) / np.sum(lengths)
delta_u = lambda k: u[k] - u[k - 1]
delta_q = lambda k: self._waypoints.get_waypoint(
k).pos - self._waypoints.get_waypoint(k - 1).pos
lamb_k = lambda k: delta_q(k) / delta_u(k)
alpha_k = lambda k: delta_u(k) / (delta_u(k) + delta_u(k + 1))
for i in range(1, len(u) - 1):
tangents[i] = (
1 - alpha_k(i)) * lamb_k(i) + alpha_k(i) * lamb_k(i + 1)
if i == 1:
tangents[0] = 2 * lamb_k(i) - tangents[1]
tangents[-1] = 2 * lamb_k(len(u) - 1) - tangents[-2]
# Normalize tangent vectors
for i in range(len(tangents)):
tangents[i] = tangents[i] / np.linalg.norm(tangents[i])
# Generate the cubic Bezier curve segments
for i in range(len(tangents) - 1):
self._interp_fcns['pos'].append(
BezierCurve([
self._waypoints.get_waypoint(i).pos,
self._waypoints.get_waypoint(i + 1).pos
], 3, tangents[i:i + 2]))
self._segment_to_wp_map.append(i + 1)
else:
return False
# Reparametrizing the curves
lengths = [seg.get_length() for seg in self._interp_fcns['pos']]
lengths = [0] + lengths
self._s = np.cumsum(lengths) / np.sum(lengths)
mean_vel = np.mean([
self._waypoints.get_waypoint(k).max_forward_speed
for k in range(self._waypoints.num_waypoints)
])
if self._duration is None:
self._duration = np.sum(lengths) / mean_vel
if self._start_time is None:
self._start_time = 0.0
# Set a simple spline to interpolate heading offset, if existent
heading = [
self._waypoints.get_waypoint(k).heading_offset
for k in range(self._waypoints.num_waypoints)
]
self._heading_spline = splrep(self._s, heading, k=3, per=False)
self._interp_fcns['heading'] = lambda x: splev(x, self._heading_spline)
return True
def test_intersects(self):
segment_1 = LineSegment(Point(0, 0), Point(5, 5))
segment_2 = LineSegment(Point(0, 2), Point(5, 2))
segment_3 = LineSegment(Point(1, 0), Point(1, 5))
segment_4 = LineSegment(Point(6, 6), Point(8, 8))
segment_5 = LineSegment(Point(0, 1), Point(5, 6))
segment_6 = LineSegment(Point(0, 2), Point(3, 0))
self.assertTrue(segment_1.intersects(segment_2))
self.assertTrue(segment_1.intersects(segment_3))
self.assertTrue(segment_2.intersects(segment_3))
self.assertFalse(segment_1.intersects(segment_4))
self.assertFalse(segment_1.intersects(segment_5))
self.assertFalse(segment_1.intersects(segment_6))
segment_7 = LineSegment(Point(3, 6), Point(3, 0))
segment_8 = LineSegment(Point(2, 4), Point(6, 4))
self.assertTrue(segment_7.intersects(segment_8))
segment_9 = LineSegment(Point(0, 6), Point(3, 6))
segment_10 = LineSegment(Point(1, 2), Point(1, 1))
self.assertFalse(segment_9.intersects(segment_10))
def test_overlaps(self):
segment_1 = LineSegment(Point(0, 0), Point(5, 5))
segment_2 = LineSegment(Point(0, 2), Point(5, 2))
segment_3 = LineSegment(Point(1, 0), Point(1, 5))
segment_4 = LineSegment(Point(1, 1), Point(2, 2))
segment_5 = LineSegment(Point(-1, -1), Point(-3, -3))
segment_6 = LineSegment(Point(0, 1), Point(5, 6))
self.assertFalse(segment_1.overlaps(segment_2))
self.assertFalse(segment_1.overlaps(segment_3))
self.assertFalse(segment_2.overlaps(segment_3))
self.assertTrue(segment_1.overlaps(segment_4))
self.assertFalse(segment_1.overlaps(segment_5))
self.assertFalse(segment_1.overlaps(segment_6))
def test_contains(self):
segment = LineSegment(Point(0, 0), Point(5, 5))
self.assertTrue(segment.contains(Point(2, 2)))
self.assertTrue(segment.contains(Point(1, 1)))
self.assertFalse(segment.contains(Point(6, 6)))
self.assertFalse(segment.contains(Point(3, 2)))
def test_closest_point_to(self):
segment_1 = LineSegment(Point(0, 0), Point(5, 5))
self.assertEqual(segment_1.closest_point_to(Point(1, 1)), Point(1, 1))
self.assertEqual(segment_1.closest_point_to(Point(0, 5)),
Point(2.5, 2.5))
self.assertEqual(segment_1.closest_point_to(Point(6, 6)), Point(5, 5))
self.assertEqual(segment_1.closest_point_to(Point(-1, -6)),
Point(0, 0))
segment_2 = LineSegment(Point(0, 0), Point(5, 0))
self.assertEqual(segment_2.closest_point_to(Point(-1, 0)), Point(0, 0))
self.assertEqual(segment_2.closest_point_to(Point(-1, 60)),
Point(0, 0))
self.assertEqual(segment_2.closest_point_to(Point(3, 60)), Point(3, 0))
self.assertEqual(segment_2.closest_point_to(Point(33, -60)),
Point(5, 0))
segment_3 = LineSegment(Point(0, 0), Point(0, 5))
self.assertEqual(segment_3.closest_point_to(Point(3, 3)), Point(0, 3))
self.assertEqual(segment_3.closest_point_to(Point(30, 30)),
Point(0, 5))
self.assertEqual(segment_3.closest_point_to(Point(0, 2)), Point(0, 2))
