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 init_interpolator(self):
if self._waypoints is None:
return False
if self._waypoints.num_waypoints < 2:
print 'At least 2 waypoints are necessary'
return False
self._markers_msg = MarkerArray()
self._marker_id = 0
self._interp_fcns = list()
path = list()
last_heading = self._waypoints.get_waypoint(0).heading_offset
dist = lambda x, y: np.sqrt(np.sum((x - y)**2))
for i in range(1, self._waypoints.num_waypoints):
heading_init = 0.0
heading_final = 0.0
if i - 1 == 0:
heading_init = self._waypoints.get_waypoint(i - 1).heading_offset
else:
if not np.isclose(dist(self._waypoints.get_waypoint(i - 1).pos[0:2], self._waypoints.get_waypoint(i).pos[0:2]), 0):
heading_init = self._waypoints.get_waypoint(i - 1).calculate_heading(self._waypoints.get_waypoint(i))
else:
heading_init = last_heading
if i == self._waypoints.num_waypoints - 1:
if not np.isclose(dist(self._waypoints.get_waypoint(i - 1).pos[0:2], self._waypoints.get_waypoint(i).pos[0:2]), 0):
heading_final = self._waypoints.get_waypoint(i - 1).calculate_heading(self._waypoints.get_waypoint(i))
else:
heading_final = last_heading
else:
if not np.isclose(dist(self._waypoints.get_waypoint(i + 1).pos[0:2], self._waypoints.get_waypoint(i).pos[0:2]), 0):
heading_final = self._waypoints.get_waypoint(i).calculate_heading(self._waypoints.get_waypoint(i + 1))
else:
heading_final = last_heading
last_heading = heading_final
path += self._generate_path(
self._waypoints.get_waypoint(i - 1), heading_init,
self._waypoints.get_waypoint(i), heading_final)
inter_pnts = list()
for i in range(len(path) - 1):
if not np.isclose(np.sqrt(np.sum((path[i + 1] - path[i])**2)), 0):
inter_pnts += [path[i]]
if not np.isclose(np.sqrt(np.sum((path[-1] - path[-2])**2)), 0):
inter_pnts += [path[-1]]
self._interp_fcns = BezierCurve.generate_cubic_curve(inter_pnts)
# Reparametrizing the curves
lengths = [seg.get_length() for seg in self._interp_fcns]
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
return True
def init_interpolator(self):
if self._waypoints is None:
return False
if self._waypoints.num_waypoints < 2:
print 'At least 2 waypoints are necessary'
return False
self._markers_msg = MarkerArray()
self._marker_id = 0
self._interp_fcns = list()
path = list()
last_heading = self._waypoints.get_waypoint(0).heading_offset
dist = lambda x, y: np.sqrt(np.sum((x - y)**2))
for i in range(1, self._waypoints.num_waypoints):
heading_init = 0.0
heading_final = 0.0
if i - 1 == 0:
heading_init = self._waypoints.get_waypoint(i - 1).heading_offset
else:
if not np.isclose(dist(self._waypoints.get_waypoint(i - 1).pos[0:2], self._waypoints.get_waypoint(i).pos[0:2]), 0):
heading_init = self._waypoints.get_waypoint(i - 1).calculate_heading(self._waypoints.get_waypoint(i))
else:
heading_init = last_heading
if i == self._waypoints.num_waypoints - 1:
if not np.isclose(dist(self._waypoints.get_waypoint(i - 1).pos[0:2], self._waypoints.get_waypoint(i).pos[0:2]), 0):
heading_final = self._waypoints.get_waypoint(i - 1).calculate_heading(self._waypoints.get_waypoint(i))
else:
heading_final = last_heading
else:
if not np.isclose(dist(self._waypoints.get_waypoint(i + 1).pos[0:2], self._waypoints.get_waypoint(i).pos[0:2]), 0):
heading_final = self._waypoints.get_waypoint(i).calculate_heading(self._waypoints.get_waypoint(i + 1))
else:
heading_final = last_heading
last_heading = heading_final
path += self._generate_path(
self._waypoints.get_waypoint(i - 1), heading_init,
self._waypoints.get_waypoint(i), heading_final)
inter_pnts = list()
for i in range(len(path) - 1):
if not np.isclose(np.sqrt(np.sum((path[i + 1] - path[i])**2)), 0):
inter_pnts += [path[i]]
if not np.isclose(np.sqrt(np.sum((path[-1] - path[-2])**2)), 0):
inter_pnts += [path[-1]]
self._interp_fcns = BezierCurve.generate_cubic_curve(inter_pnts)
# Reparametrizing the curves
lengths = [seg.get_length() for seg in self._interp_fcns]
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
return True