add humble-navigation2
This commit is contained in:
X-lanni
@@ -0,0 +1,90 @@
|
||||
# Copyright (c) 2021, Matthew Booker
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License. Reserved.
|
||||
|
||||
import unittest
|
||||
|
||||
from lattice_generator import LatticeGenerator
|
||||
import numpy as np
|
||||
|
||||
MOTION_MODEL = 'ackermann'
|
||||
TURNING_RADIUS = 0.5
|
||||
GRID_RESOLUTION = 0.05
|
||||
STOPPING_THRESHOLD = 5
|
||||
NUM_OF_HEADINGS = 16
|
||||
|
||||
|
||||
class TestLatticeGenerator(unittest.TestCase):
|
||||
"""Contains the unit tests for the TrajectoryGenerator."""
|
||||
|
||||
def setUp(self) -> None:
|
||||
config = {'motion_model': MOTION_MODEL,
|
||||
'turning_radius': TURNING_RADIUS,
|
||||
'grid_resolution': GRID_RESOLUTION,
|
||||
'stopping_threshold': STOPPING_THRESHOLD,
|
||||
'num_of_headings': NUM_OF_HEADINGS}
|
||||
|
||||
lattice_gen = LatticeGenerator(config)
|
||||
|
||||
self.minimal_set = lattice_gen.run()
|
||||
|
||||
def test_minimal_set_lengths_are_positive(self):
|
||||
# Test that lengths are all positive
|
||||
|
||||
for start_angle in self.minimal_set.keys():
|
||||
for trajectory in self.minimal_set[start_angle]:
|
||||
|
||||
self.assertGreaterEqual(trajectory.parameters.arc_length, 0)
|
||||
self.assertGreaterEqual(trajectory.parameters.start_straight_length, 0)
|
||||
self.assertGreaterEqual(trajectory.parameters.end_straight_length, 0)
|
||||
self.assertGreaterEqual(trajectory.parameters.total_length, 0)
|
||||
|
||||
def test_minimal_set_end_points_lie_on_grid(self):
|
||||
# Test that end points lie on the grid resolution
|
||||
|
||||
for start_angle in self.minimal_set.keys():
|
||||
for trajectory in self.minimal_set[start_angle]:
|
||||
|
||||
end_point_x = trajectory.path.xs[-1]
|
||||
end_point_y = trajectory.path.ys[-1]
|
||||
|
||||
div_x = end_point_x / GRID_RESOLUTION
|
||||
div_y = end_point_y / GRID_RESOLUTION
|
||||
|
||||
self.assertAlmostEqual(div_x, np.round(div_x), delta=0.00001)
|
||||
self.assertAlmostEqual(div_y, np.round(div_y), delta=0.00001)
|
||||
|
||||
def test_minimal_set_end_angle_is_correct(self):
|
||||
# Test that end angle agrees with the end angle parameter
|
||||
|
||||
for start_angle in self.minimal_set.keys():
|
||||
for trajectory in self.minimal_set[start_angle]:
|
||||
|
||||
end_point_angle = trajectory.path.yaws[-1]
|
||||
|
||||
self.assertEqual(end_point_angle, trajectory.parameters.end_angle)
|
||||
|
||||
def test_output_angles_in_correct_range(self):
|
||||
# Test that the outputted angles always lie within 0 to 2*pi
|
||||
|
||||
for start_angle in self.minimal_set.keys():
|
||||
for trajectory in self.minimal_set[start_angle]:
|
||||
output = trajectory.path.to_output_format()
|
||||
|
||||
for x, y, angle in output:
|
||||
self.assertGreaterEqual(angle, 0)
|
||||
self.assertLessEqual(angle, 2*np.pi)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
@@ -0,0 +1,226 @@
|
||||
# Copyright (c) 2021, Matthew Booker
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License. Reserved.
|
||||
|
||||
import unittest
|
||||
|
||||
import numpy as np
|
||||
from trajectory_generator import TrajectoryGenerator
|
||||
|
||||
TURNING_RADIUS = 1
|
||||
STEP_DISTANCE = 0.1
|
||||
|
||||
|
||||
class TestTrajectoryGenerator(unittest.TestCase):
|
||||
"""Contains the unit tests for the TrajectoryGenerator."""
|
||||
|
||||
def setUp(self) -> None:
|
||||
config = {'turning_radius': TURNING_RADIUS}
|
||||
self.trajectory_generator = TrajectoryGenerator(config)
|
||||
|
||||
def test_generate_trajectory_only_arc(self):
|
||||
# Quadrant 1
|
||||
end_point = np.array([1, 1])
|
||||
trajectory = self.trajectory_generator.generate_trajectory(
|
||||
end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE)
|
||||
|
||||
self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
|
||||
self.assertGreater(len(trajectory.path.xs), 0)
|
||||
|
||||
# Quadrant 2
|
||||
end_point = np.array([-1, 1])
|
||||
trajectory = self.trajectory_generator.generate_trajectory(
|
||||
end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE)
|
||||
|
||||
self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
|
||||
self.assertGreater(len(trajectory.path.xs), 0)
|
||||
|
||||
# Quadrant 3
|
||||
end_point = np.array([-1, -1])
|
||||
trajectory = self.trajectory_generator.generate_trajectory(
|
||||
end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE)
|
||||
|
||||
self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
|
||||
self.assertGreater(len(trajectory.path.xs), 0)
|
||||
|
||||
# Quadrant 4
|
||||
end_point = np.array([1, -1])
|
||||
trajectory = self.trajectory_generator.generate_trajectory(
|
||||
end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE)
|
||||
|
||||
self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
|
||||
self.assertGreater(len(trajectory.path.xs), 0)
|
||||
|
||||
def test_generate_trajectory_only_line(self):
|
||||
# Quadrant 1
|
||||
end_point = np.array([1, 1])
|
||||
trajectory = self.trajectory_generator.generate_trajectory(
|
||||
end_point, np.deg2rad(45), np.deg2rad(45), STEP_DISTANCE)
|
||||
|
||||
self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
|
||||
self.assertGreater(len(trajectory.path.xs), 0)
|
||||
|
||||
# Quadrant 2
|
||||
end_point = np.array([-1, 1])
|
||||
trajectory = self.trajectory_generator.generate_trajectory(
|
||||
end_point, np.deg2rad(135), np.deg2rad(135), STEP_DISTANCE)
|
||||
|
||||
self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
|
||||
self.assertGreater(len(trajectory.path.xs), 0)
|
||||
|
||||
# Quadrant 3
|
||||
end_point = np.array([-1, -1])
|
||||
trajectory = self.trajectory_generator.generate_trajectory(
|
||||
end_point, -np.deg2rad(135), -np.deg2rad(135), STEP_DISTANCE)
|
||||
|
||||
self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
|
||||
self.assertGreater(len(trajectory.path.xs), 0)
|
||||
|
||||
# Quadrant 4
|
||||
end_point = np.array([1, -1])
|
||||
trajectory = self.trajectory_generator.generate_trajectory(
|
||||
end_point, -np.deg2rad(45), -np.deg2rad(45), STEP_DISTANCE)
|
||||
|
||||
self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
|
||||
self.assertGreater(len(trajectory.path.xs), 0)
|
||||
|
||||
def test_generate_trajectory_line_to_arc(self):
|
||||
# Quadrant 1
|
||||
end_point = np.array([2, 1])
|
||||
trajectory = self.trajectory_generator.generate_trajectory(
|
||||
end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE)
|
||||
|
||||
self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
|
||||
self.assertGreater(len(trajectory.path.xs), 0)
|
||||
|
||||
# Quadrant 2
|
||||
end_point = np.array([-2, 1])
|
||||
trajectory = self.trajectory_generator.generate_trajectory(
|
||||
end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE)
|
||||
|
||||
self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
|
||||
self.assertGreater(len(trajectory.path.xs), 0)
|
||||
|
||||
# Quadrant 3
|
||||
end_point = np.array([-2, -1])
|
||||
trajectory = self.trajectory_generator.generate_trajectory(
|
||||
end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE)
|
||||
|
||||
self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
|
||||
self.assertGreater(len(trajectory.path.xs), 0)
|
||||
|
||||
# Quadrant 1
|
||||
end_point = np.array([2, -1])
|
||||
trajectory = self.trajectory_generator.generate_trajectory(
|
||||
end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE)
|
||||
|
||||
self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
|
||||
self.assertGreater(len(trajectory.path.xs), 0)
|
||||
|
||||
def test_generate_trajectory_line_to_end(self):
|
||||
# Quadrant 1
|
||||
end_point = np.array([1, 2])
|
||||
trajectory = self.trajectory_generator.generate_trajectory(
|
||||
end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE)
|
||||
|
||||
self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
|
||||
self.assertGreater(len(trajectory.path.xs), 0)
|
||||
|
||||
# Quadrant 2
|
||||
end_point = np.array([-1, 2])
|
||||
trajectory = self.trajectory_generator.generate_trajectory(
|
||||
end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE)
|
||||
|
||||
self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
|
||||
self.assertGreater(len(trajectory.path.xs), 0)
|
||||
|
||||
# Quadrant 3
|
||||
end_point = np.array([-1, -2])
|
||||
trajectory = self.trajectory_generator.generate_trajectory(
|
||||
end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE)
|
||||
|
||||
self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
|
||||
self.assertGreater(len(trajectory.path.xs), 0)
|
||||
|
||||
# Quadrant 4
|
||||
end_point = np.array([1, -2])
|
||||
trajectory = self.trajectory_generator.generate_trajectory(
|
||||
end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE)
|
||||
|
||||
self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
|
||||
self.assertGreater(len(trajectory.path.xs), 0)
|
||||
|
||||
def test_generate_trajectory_radius_too_small(self):
|
||||
# Quadrant 1
|
||||
end_point = np.array([.9, .9])
|
||||
trajectory = self.trajectory_generator.generate_trajectory(
|
||||
end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE)
|
||||
|
||||
self.assertEqual(trajectory, None)
|
||||
|
||||
# Quadrant 2
|
||||
end_point = np.array([-.9, -.9])
|
||||
trajectory = self.trajectory_generator.generate_trajectory(
|
||||
end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE)
|
||||
|
||||
self.assertEqual(trajectory, None)
|
||||
|
||||
# Quadrant 3
|
||||
end_point = np.array([-.9, -.9])
|
||||
trajectory = self.trajectory_generator.generate_trajectory(
|
||||
end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE)
|
||||
|
||||
self.assertEqual(trajectory, None)
|
||||
|
||||
# Quadrant 4
|
||||
end_point = np.array([.9, -.9])
|
||||
trajectory = self.trajectory_generator.generate_trajectory(
|
||||
end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE)
|
||||
|
||||
self.assertEqual(trajectory, None)
|
||||
|
||||
def test_generate_trajectory_parallel_lines_coincident(self):
|
||||
# Quadrant 1
|
||||
end_point = np.array([5, 0])
|
||||
trajectory = self.trajectory_generator.generate_trajectory(
|
||||
end_point, np.deg2rad(0), np.deg2rad(0), STEP_DISTANCE)
|
||||
|
||||
self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
|
||||
self.assertGreater(len(trajectory.path.xs), 0)
|
||||
|
||||
# Quadrant 2
|
||||
end_point = np.array([-5, 0])
|
||||
trajectory = self.trajectory_generator.generate_trajectory(
|
||||
end_point, -np.deg2rad(180), -np.deg2rad(180), STEP_DISTANCE)
|
||||
|
||||
self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
|
||||
self.assertGreater(len(trajectory.path.xs), 0)
|
||||
|
||||
def test_generate_trajectory_parallel_lines_not_coincident(self):
|
||||
# Quadrant 1
|
||||
end_point = np.array([0, 3])
|
||||
trajectory = self.trajectory_generator.generate_trajectory(
|
||||
end_point, np.deg2rad(0), np.deg2rad(0), STEP_DISTANCE)
|
||||
|
||||
self.assertEqual(trajectory, None)
|
||||
|
||||
# Quadrant 2
|
||||
end_point = np.array([0, 3])
|
||||
trajectory = self.trajectory_generator.generate_trajectory(
|
||||
end_point, -np.deg2rad(180), -np.deg2rad(180), STEP_DISTANCE)
|
||||
|
||||
self.assertEqual(trajectory, None)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
@@ -0,0 +1,98 @@
|
||||
# Copyright (c) 2021, Matthew Booker
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License. Reserved.
|
||||
|
||||
"""
|
||||
This script is used visualize each trajectory individually.
|
||||
|
||||
This helps to better understand how a single trajectory looks and
|
||||
to ensure that the x, y, and yaw values are correct. This is mainly
|
||||
used for debugging when making changes to parts of the code.
|
||||
However, if you would like to see how each trajectory in your
|
||||
ouput file looks then you can run this script.
|
||||
"""
|
||||
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def plot_arrow(x, y, yaw, length=1.0, fc='r', ec='k'):
|
||||
"""Plot arrow."""
|
||||
plt.arrow(x, y, length * np.cos(yaw), length *
|
||||
np.sin(yaw), width=0.05*length, length_includes_head=True)
|
||||
plt.plot(x, y)
|
||||
plt.plot(0, 0)
|
||||
|
||||
|
||||
def read_trajectories_data(file_path):
|
||||
|
||||
with open(file_path) as data_file:
|
||||
trajectory_data = json.load(data_file)
|
||||
|
||||
return trajectory_data
|
||||
|
||||
|
||||
cur_file_path = Path(__file__)
|
||||
trajectory_file_path = cur_file_path.parent.parent / 'output.json'
|
||||
|
||||
|
||||
trajectory_data = read_trajectories_data(trajectory_file_path)
|
||||
min_x = min([min([pose[0] for pose in primitive['poses']])
|
||||
for primitive in trajectory_data['primitives']])
|
||||
max_x = max([max([pose[0] for pose in primitive['poses']])
|
||||
for primitive in trajectory_data['primitives']])
|
||||
|
||||
min_y = min([min([pose[1] for pose in primitive['poses']])
|
||||
for primitive in trajectory_data['primitives']])
|
||||
max_y = max([max([pose[1] for pose in primitive['poses']])
|
||||
for primitive in trajectory_data['primitives']])
|
||||
|
||||
heading_angles = trajectory_data['lattice_metadata']['heading_angles']
|
||||
|
||||
for primitive in trajectory_data['primitives']:
|
||||
arrow_length = (primitive['arc_length'] +
|
||||
primitive['straight_length']) / len(primitive['poses'])
|
||||
|
||||
if arrow_length == 0:
|
||||
arrow_length = max_x / len(primitive['poses'])
|
||||
|
||||
xs = np.array([pose[0] for pose in primitive['poses']])
|
||||
ys = np.array([pose[1] for pose in primitive['poses']])
|
||||
|
||||
lengths = np.sqrt((xs[1:] - xs[:-1])**2 + (ys[1:] - ys[:-1])**2)
|
||||
print('Distances between points: ', lengths)
|
||||
|
||||
for x, y, yaw in primitive['poses']:
|
||||
plot_arrow(x, y, yaw, length=arrow_length)
|
||||
|
||||
plt.scatter(xs, ys)
|
||||
plt.grid(True)
|
||||
plt.axis('square')
|
||||
|
||||
left_x, right_x = plt.xlim()
|
||||
left_y, right_y = plt.ylim()
|
||||
plt.xlim(1.2*min_x, 1.2*max_x)
|
||||
plt.ylim(1.2*min_y, 1.2*max_y)
|
||||
|
||||
start_angle = np.rad2deg(heading_angles[primitive['start_angle_index']])
|
||||
end_angle = np.rad2deg(heading_angles[primitive['end_angle_index']])
|
||||
|
||||
plt.title(f"Trajectory ID: {primitive['trajectory_id']}")
|
||||
plt.figtext(
|
||||
0.7, 0.9, f'Start: {start_angle}\nEnd: {end_angle}')
|
||||
|
||||
plt.show()
|
||||
Reference in New Issue
Block a user