def init_from_waypoint_file(self, filename):
wp_set = WaypointSet()
success = wp_set.read_from_file(filename)
if success:
self._wp_interp.init_waypoints(wp_set)
self._wp_interp_on = True
return success
def test_add_repeated_waypoint(self):
wp = Waypoint(x=1, y=2, z=3, max_forward_speed=1)
wp_set = WaypointSet()
self.assertTrue(wp_set.add_waypoint(wp),
'Error occured while adding waypoint to empty set')
self.assertFalse(wp_set.add_waypoint(wp),
'Repeated waypoint wrongfully added')
def add_waypoint(self, waypoint, add_to_beginning=False):
"""Add waypoint to the existing waypoint set. If no waypoint set has
been initialized, create new waypoint set structure and add the given
waypoint."""
if self._waypoints is None:
self._waypoints = WaypointSet()
self._waypoints.add_waypoint(waypoint, add_to_beginning)
return self.init_interpolator()
def test_invalid_params_circle(self):
wp_set = WaypointSet()
self.assertFalse(
wp_set.generate_circle(radius=-1,
center=None,
num_points=-1,
max_forward_speed=0,
theta_offset=0.0,
heading_offset=0.0),
'Invalid parameters have been wrongly instantiated')
def test_init(self):
wp_set = WaypointSet()
self.assertEqual(wp_set.num_waypoints, 0, 'Waypoint list is not empty')