add humble-navigation2

navigation2/nav2_smac_planner/lattice_primitives/README.md

@@ -0,0 +1,168 @@
+# Lattice Primitive Generator
+## Contents
+
+- **[About](#about)**
+- **[Setup](#setup)**
+- **[Usage](#usage)**
+- **[Parameters](#parameters)**
+- **[Output file structure](#output-file-structure)**
+- **[How it Works](#how-it-works)**
+</br>
+
+## About
+The scripts in this folder are used to generate the minimum control set for the state lattice planner. This work is based on [Generating Near Minimal Control Sets for Constrained Motion Planning in Discrete State Spaces](https://www.ri.cmu.edu/pub_files/pub4/pivtoraiko_mihail_2005_1/pivtoraiko_mihail_2005_1.pdf). An example of the trajectories for a grid resolution of 5cm and turning radius of 0.5m is shown below.
+
+![ ](docs/all_trajectories.png)
+
+## Setup
+To install the required python packages run the following command
+
+```
+pip install -r requirements.txt
+```
+
+## Usage
+Run the primitive generator by using the following command
+```
+python3 generate_motion_primitives.py [--config] [--output] [--visualizations]
+```
+
+To adjust the settings to fit your particular needs you can edit the parameters in the [config.json](config.json) file. Alternatively, you can create your own file and pass it in using the --config flag.
+
+The output file can be specified by passing in a path with the --output flag. The default is set to save in a file called output.json in the same directory as this README.
+
+The directory to save the visualizations can be specified by passing in a path with the --visualizations flag.
+
+## Parameters ##
+Note: None of these parameters have defaults. They all must be specified through the [config.json](config.json) file.
+
+**motion_model** (string)
+    
+The type of motion model used. Accepted values:
+- `ackermann`: Only forward and reversing trajectories
+- `diff`: Forward moving trajectories + rotation in place by a single angular bin
+- `omni`: Forward moving trajectories + rotation in place by a single angular bin + sideways sliding motions
+</br>
+</br>
+
+**turning_radius** (float)
+
+The minimum turning radius of the robot (in meters). Typical values for a service robot range from 0.4 to 1.0m.
+</br>
+</br>
+
+**grid_resolution** (float)
+
+The resolution of the grid (in meters) used to create the lattice. If the grid resolution is too large proportionally to the turning radius then the generator will not return a good result. This should be the same as your costmap resolution.
+</br>
+</br>
+
+**stopping_threshold** (float)
+
+Number of consecutive iterations without a valid trajectory before stopping the search. A value too low may mean that you stop searching too early. A value too high will cause the search to take longer. We found that stopping after 5 iterations produced the best results.
+</br>
+</br>
+
+**num_of_headings** (float)
+
+Number of discrete angular headings used. Due to the way discretization is done this number should be restricted to multiples of 8. Angles are not generated uniformly but instead generated in a way to facilitate straight lines. See [angle discretization](#angle-discretization) for more details. We believe 16 headings is a good number for most use cases.
+</br>
+</br>
+
+## Output file structure
+The output file is a JSON file and contains the following fields:
+
+**version**
+
+The version number of the lattice generator that created the output file
+
+**date_generated**
+
+The date the output file was generated. Format: YYYY-MM-DD
+
+**lattice_metadata**
+
+A dictionary that contains information about the generated lattice. Most of this data comes from the config file used when generating the primitives. More information on each field is given in the [Parameters](#parameters) section. Includes the following fields:
+- **motion_model**
+- **turning_radius** (meters)
+- **grid_resolution** (meters)
+- **stopping_threshold**
+- **num_of_headings**
+- **heading_angles**
+    - A list of the heading angles (in radians) that are used in the primitives
+- **number_of_trajectories**
+    - The total number of trajectories contained in the output file
+
+**primitives**
+
+A list of dictionaries where each dictionary represents an individual motion primitive. Each motion primitive contains the following fields:
+- **trajectory_id**
+    - The id associated with the primitive
+- **start_angle_index**
+    - The start angle of the primitive represented as an index for the heading_angle list given in the lattice_metadata
+- **end_angle_index**
+    - The end angle of the primitive represented as an index for the heading_angle list given in the lattice_metadata
+- **left_turn**
+    - A boolean value that is true if the path curves to the left. Straight paths default to true.
+- **trajectory_radius** (meters)
+    - The radius of the circle that was used to create the arc portion of a primitive. trajectory_radius is 0 if the primitive is purely a straight line
+- **trajectory_length** (meters)
+    - The length of the primitive
+- **arc_length** (meters)
+    - The length of the arc portion of a primitive. arc_length is 0 if the primitive is a straight line
+- **straight_length** (meters)
+    - The length of the straight line portion of a primitive. straight_length is 0 if the primitive is a pure arc.
+- **poses**
+    - A list where each entry is a list containing three values: x, y, and yaw (radians)
+
+## How it works
+This section describes how the various portions of the generation algorithm works.
+
+### Angle Discretization
+Dividing a full turn into uniform angular headings presents several problems. The biggest problem is that it will create angles for which a straight trajectory does not land nicely on an endpoint that aligns with the grid. Instead we discretize the angles in a way that ensures straight paths will land on endpoints aligned with the grid. 
+
+![ ](docs/angle_discretization.png)
+
+The image shows how the angular headings are generated. The same idea can be extended to a higher number of headings. As a result, the number of headings parameter is restricted to multiples of 8.  
+
+### Trajectory Generator
+1. Create two lines. Line 1 passes through start point with angle of start angle, and line 2 passes through the end point with angle of end angle
+
+2. Find the intersection point I of lines 1 and 2
+
+3. Calculate the distance beween I and the origin (let this be d1). Also calculate the distance between I and the end point (let this be d2)
+
+4. If d1 and d2 are equal then proceed to step 5. Otherwise, create intermediate points for each line that are min(d1, d2) distance away along the lines from I. So there should be an intermediate point on line 1 and an intermediate point on line 2. One of these intermediate points should align with either the origin or the end point by nature of how the distance was calculated.
+
+5. Create perpindicular lines for line 1 and line 2 that pass through the respective intermediate points. The intersection of these perpindicular lines is the centre of the circle whose arc represents the curved portion of the trajectory.
+
+6. Finally, if needed append straight segments to the path to ensure we start at the origin and end at the end point.
+
+
+There are several checks we need to make to ensure a valid trajectory is generated:
+- If the start and end angles are parallel then the lines must overlap
+- The intersection point must occur before the end point on line 2 and after the origin on line 1
+- The radius of the generated trajectory must be less than the user supplied minimum turning radius
+
+### Lattice Generator
+The lattice generator is generally based on the generation of the control set as described in [Generating Near Minimal Control Sets for Constrained Motion Planning in Discrete State Spaces](https://www.ri.cmu.edu/pub_files/pub4/pivtoraiko_mihail_2005_1/pivtoraiko_mihail_2005_1.pdf). However, some changes were made to the above method. A brief outline of the implemented method is given below:
+
+1. Create a wavefront that begins a minimum trajectory length away from the origin.
+
+    - The minimum trajectory length is defined as the length a trajectory needs to move from one discrete heading to the next. (Since the headings are not separated equally we use the smallest heading change)
+
+2. Generate paths to all points on this wavefront for all possible end heading angles.
+
+3. When a path is generated it is checked to ensure it does not pass "close" to another path. If it does it is removed, otherwise it remains in the set
+
+    - "Close" is defined to be within half the grid resolution for length and half the average angular bin size for angular rotation
+
+4. Steps 2-3 are repeated for the next wavefront which is a grid resolution step further away from the origin.
+
+5. Steps 1-4 are repeated untill all trajectories are being removed. The generator will continue for a few more wavefront steps until N wavefronts have been searched with no new trajectories. At this point the generator terminates and returns the computed minimal set.
+
+    - The number N is the stopping_threshold parameter
+
+6. Steps 1-5 are repeated for all possible start angles between 0 and 90.
+
+7. The resulting control set will only contain trajectories in quadrant 1. To get the final control set we exploit symmetry across the axess and flip the trajectories in different ways.

navigation2/nav2_smac_planner/lattice_primitives/config.json

@@ -0,0 +1,7 @@
+{
+    "motion_model": "ackermann",
+    "turning_radius": 0.5,
+    "grid_resolution": 0.05,
+    "stopping_threshold": 5,
+    "num_of_headings": 16
+}

navigation2/nav2_smac_planner/lattice_primitives/constants.py

@@ -0,0 +1,15 @@
+# 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.
+
+VERSION = 1.0

navigation2/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py

@@ -0,0 +1,260 @@
+# 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 argparse
+from datetime import datetime
+import json
+import logging
+from pathlib import Path
+import time
+
+import constants
+from lattice_generator import LatticeGenerator
+
+import matplotlib.pyplot as plt
+import numpy as np
+
+
+logging.basicConfig(level=logging.INFO)
+logger = logging.getLogger(__name__)
+
+
+def handle_arg_parsing():
+    """
+    Handle the parsing of arguments.
+
+    Returns
+    -------
+    argparse.Namespace
+        An object containing all parsed arguments
+
+    """
+    parser = argparse.ArgumentParser(description='Generate motionprimitives '
+                                                 "for Nav2's State "
+                                                 'Lattice Planner')
+    parser.add_argument('--config',
+                        type=Path,
+                        default='./config.json',
+                        help='The config file containing the '
+                        'parameters to be used')
+    parser.add_argument('--output',
+                        type=Path,
+                        default='./output.json',
+                        help='The output file containing the '
+                        'trajectory data')
+    parser.add_argument('--visualizations',
+                        type=Path,
+                        default='./visualizations',
+                        help='The output folder where the '
+                        'visualizations of the trajectories will be saved')
+
+    return parser.parse_args()
+
+
+def create_heading_angle_list(minimal_set_trajectories: dict) -> list:
+    """
+    Create a sorted list of heading angles from the minimal trajectory set.
+
+    Args:
+    ----
+    minimal_set_trajectories: dict
+        The minimal spanning set
+
+    Returns
+    -------
+    list
+        A sorted list of heading angles
+
+    """
+    heading_angles = set(minimal_set_trajectories.keys())
+    return sorted(heading_angles, key=lambda x: (x < 0, x))
+
+
+def read_config(config_path) -> dict:
+    """
+    Read in the user defined parameters via JSON.
+
+    Args:
+    ----
+    config_path: Path
+        Path to the config file
+
+    Returns
+    -------
+    dict
+        Dictionary containing the user defined parameters
+
+    """
+    with open(config_path) as config_file:
+        config = json.load(config_file)
+
+    return config
+
+
+def create_header(config: dict, minimal_set_trajectories: dict) -> dict:
+    """
+    Create a dict containing all the fields to populate the header with.
+
+    Args:
+    ----
+    config: dict
+        The dict containing user specified parameters
+    minimal_set_trajectories: dict
+        The minimal spanning set
+
+    Returns
+    -------
+    dict
+        A dictionary containing the fields to populate the header with
+
+    """
+    header_dict = {
+        'version': constants.VERSION,
+        'date_generated': datetime.today().strftime('%Y-%m-%d'),
+        'lattice_metadata': {},
+        'primitives': [],
+    }
+
+    for key, value in config.items():
+        header_dict['lattice_metadata'][key] = value
+
+    heading_angles = create_heading_angle_list(minimal_set_trajectories)
+    adjusted_heading_angles = [angle + 2*np.pi if angle < 0 else angle for angle in heading_angles]
+
+    header_dict['lattice_metadata']['heading_angles'] = adjusted_heading_angles
+
+    return header_dict
+
+
+def write_to_json(output_path: Path, minimal_set_trajectories: dict, config: dict) -> None:
+    """
+    Write the minimal spanning set to an output file.
+
+    Args:
+    ----
+    output_path: Path
+        The output file for the json data
+    minimal_set_trajectories: dict
+        The minimal spanning set
+    config: dict
+        The dict containing user specified parameters
+
+    """
+    output_dict = create_header(config, minimal_set_trajectories)
+
+    trajectory_start_angles = list(minimal_set_trajectories.keys())
+
+    heading_angle_list = create_heading_angle_list(minimal_set_trajectories)
+    heading_lookup = {angle: idx for idx, angle in
+                      enumerate(heading_angle_list)}
+
+    idx = 0
+    for start_angle in sorted(trajectory_start_angles,
+                              key=lambda x: (x < 0, x)):
+
+        for trajectory in sorted(
+            minimal_set_trajectories[start_angle],
+            key=lambda x: x.parameters.end_angle
+        ):
+
+            traj_info = {}
+            traj_info['trajectory_id'] = idx
+            traj_info['start_angle_index'] = heading_lookup[trajectory.parameters.start_angle]
+            traj_info['end_angle_index'] = heading_lookup[trajectory.parameters.end_angle]
+            traj_info['left_turn'] = bool(trajectory.parameters.left_turn)
+            traj_info['trajectory_radius'] = \
+                trajectory.parameters.turning_radius
+            traj_info['trajectory_length'] = round(
+                trajectory.parameters.total_length, 5
+            )
+            traj_info['arc_length'] = round(
+                trajectory.parameters.arc_length,
+                5
+            )
+            traj_info['straight_length'] = round(
+                trajectory.parameters.start_straight_length
+                + trajectory.parameters.end_straight_length,
+                5,
+            )
+            traj_info['poses'] = trajectory.path.to_output_format()
+
+            output_dict['primitives'].append(traj_info)
+            idx += 1
+
+    output_dict['lattice_metadata']['number_of_trajectories'] = idx
+
+    with open(output_path, 'w') as output_file:
+        json.dump(output_dict, output_file, indent='\t')
+
+
+def save_visualizations(visualizations_folder: Path, minimal_set_trajectories: dict) -> None:
+    """
+    Draw the visualizations for every trajectory and save it as an image.
+
+    Args:
+    ----
+    visualizations_folder: Path
+        The path to the folder for where to save the images
+    minimal_set_trajectories: dict
+        The minimal spanning set
+
+    """
+    # Create the directory if it doesnt exist
+    visualizations_folder.mkdir(exist_ok=True)
+
+    for start_angle in minimal_set_trajectories.keys():
+
+        for trajectory in minimal_set_trajectories[start_angle]:
+            plt.plot(trajectory.path.xs, trajectory.path.ys, 'b')
+
+    plt.grid(True)
+    plt.axis('square')
+    left_x, right_x = plt.xlim()
+    left_y, right_y = plt.ylim()
+
+    output_path = visualizations_folder / 'all_trajectories.png'
+    plt.savefig(output_path)
+    plt.clf()
+
+    for start_angle in minimal_set_trajectories.keys():
+
+        angle_in_deg = np.rad2deg(start_angle)
+
+        if start_angle < 0 or start_angle > np.pi / 2:
+            continue
+
+        for trajectory in minimal_set_trajectories[start_angle]:
+            plt.plot(trajectory.path.xs, trajectory.path.ys, 'b')
+            plt.xlim(left_x, right_x)
+            plt.ylim(left_y, right_y)
+
+        plt.grid(True)
+
+        output_path = visualizations_folder / f'{angle_in_deg}.png'
+        plt.savefig(output_path)
+        plt.clf()
+
+
+if __name__ == '__main__':
+
+    args = handle_arg_parsing()
+    config = read_config(args.config)
+
+    start = time.time()
+    lattice_gen = LatticeGenerator(config)
+    minimal_set_trajectories = lattice_gen.run()
+    print(f'Finished Generating. Took {time.time() - start} seconds')
+
+    write_to_json(args.output, minimal_set_trajectories, config)
+    save_visualizations(args.visualizations, minimal_set_trajectories)

navigation2/nav2_smac_planner/lattice_primitives/helper.py

@@ -0,0 +1,127 @@
+# 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 numpy as np
+
+
+def normalize_angle(angle):
+    """
+    Normalize the angle to between [0, 2pi).
+
+    Args:
+    angle: float
+        The angle to normalize in radians
+
+    Returns
+    -------
+    The normalized angle in the range [0,2pi)
+
+    """
+    while angle >= 2*np.pi:
+        angle -= 2*np.pi
+
+    while angle < 0:
+        angle += 2*np.pi
+
+    return angle
+
+
+def angle_difference(angle_1, angle_2, left_turn=None):
+    """
+    Calculate the difference between two angles based on a given direction.
+
+    Args:
+    angle_1: float
+        The starting angle in radians
+    angle_2: float
+        The ending angle in radians
+    left_turn: bool
+        The direction of turn. True if left, false if right
+        and None if smallest angular difference should be
+        returned
+
+    Returns
+    -------
+    The angular difference between the two angles according to
+    the specified turn direction
+
+    """
+    if left_turn is None:
+        dif = abs(angle_1 - angle_2)
+
+        return dif if dif <= np.pi else 2 * np.pi - dif
+
+    elif left_turn:
+
+        if angle_2 >= angle_1:
+            return abs(angle_1 - angle_2)
+        else:
+            return 2 * np.pi - abs(angle_1 - angle_2)
+
+    else:
+        if angle_1 >= angle_2:
+            return abs(angle_1 - angle_2)
+        else:
+            return 2 * np.pi - abs(angle_1 - angle_2)
+
+
+def interpolate_yaws(start_angle, end_angle, left_turn, steps):
+    """
+    Create equally spaced yaws between two angles.
+
+    Args:
+    start_angle: float
+        The starting angle
+    end_angle: float
+        The ending angle
+    left_turn: bool
+        The direction of turn. True if left, False otherwise
+    steps: int
+        The number of yaws to generate between start and end
+        angle
+
+    Returns
+    -------
+    An array of yaws starting at start angle and ending at end
+    angle with steps number of angles between them
+
+    """
+    if left_turn:
+        if start_angle > end_angle:
+            end_angle += 2 * np.pi
+    else:
+        if end_angle > start_angle:
+            end_angle -= 2 * np.pi
+
+    yaws = np.linspace(start_angle, end_angle, steps)
+    yaws = np.vectorize(normalize_angle)(yaws)
+
+    return yaws
+
+
+def get_rotation_matrix(angle):
+    """
+    Return a rotation matrix that is equivalent to a 2D rotation of angle.
+
+    Args:
+    angle: float
+        The angle to create a rotation matrix for
+
+    Returns
+    -------
+    A 2x2 matrix representing a 2D rotation by angle
+
+    """
+    return np.array([[np.cos(angle), -np.sin(angle)],
+                    [np.sin(angle), np.cos(angle)]])

navigation2/nav2_smac_planner/lattice_primitives/lattice_generator.py

@@ -0,0 +1,745 @@
+# 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.
+
+from collections import defaultdict
+from enum import Enum
+
+from helper import angle_difference, interpolate_yaws
+
+import numpy as np
+
+from rtree import index
+
+from trajectory import Path, Trajectory, TrajectoryParameters
+
+from trajectory_generator import TrajectoryGenerator
+
+
+class LatticeGenerator:
+    """
+    Handles all the logic for computing the minimal control set.
+
+    Computes the minimal control set for a vehicle given its parameters.
+    Includes handling the propogating and searching along wavefronts as
+    well as determining if a trajectory is part of the minimal set based
+    on previously added trajectories.
+    """
+
+    class MotionModel(Enum):
+        """An Enum used for determining the motion model to use."""
+
+        ACKERMANN = 1
+        DIFF = 2
+        OMNI = 3
+
+    class Flip(Enum):
+        """An Enum used for determining how a trajectory should be flipped."""
+
+        X = 1
+        Y = 2
+        BOTH = 3
+
+    def __init__(self, config: dict):
+        """Init the lattice generator from the user supplied config."""
+        self.trajectory_generator = TrajectoryGenerator(config)
+        self.grid_resolution = config['grid_resolution']
+        self.turning_radius = config['turning_radius']
+        self.stopping_threshold = config['stopping_threshold']
+        self.num_of_headings = config['num_of_headings']
+        self.headings = \
+            self._get_heading_discretization(config['num_of_headings'])
+
+        self.motion_model = self.MotionModel[config['motion_model'].upper()]
+
+        self.DISTANCE_THRESHOLD = 0.5 * self.grid_resolution
+        self.ROTATION_THRESHOLD = 0.5 * (2 * np.pi / self.num_of_headings)
+
+    def _get_wave_front_points(self, pos: int) -> np.array:
+        """
+        Calculate the end points that lie on the wave front.
+
+        Uses the user supplied grid resolution to calculate the
+        valid end points that lie on a wave front at a discrete
+        interval away from the origin.
+
+        Args:
+        pos: int
+            The number of discrete intervals of grid resolution
+            away from the origin to generate the wave points at
+
+        Returns
+        -------
+        np.array
+            An array of coordinates
+
+        """
+        positions = []
+
+        max_point_coord = self.grid_resolution * pos
+
+        for i in range(pos):
+            varying_point_coord = self.grid_resolution * i
+
+            # Change the y and keep x at max
+            positions.append((max_point_coord, varying_point_coord))
+
+            # Change the x and keep y at max
+            positions.append((varying_point_coord, max_point_coord))
+
+        # Append the corner
+        positions.append((max_point_coord, max_point_coord))
+
+        return np.array(positions)
+
+    def _get_heading_discretization(self, number_of_headings: int) -> list:
+        """
+        Calculate the heading discretization based on the number of headings.
+
+        Does not uniformly generate headings but instead generates a set of
+        discrete headings that is better suited for straight line trajectories.
+
+        Args:
+        number_of_headings: int
+            The number of headings to discretize a 360 degree turn into
+
+        Returns
+        -------
+        list
+            A list of headings in radians
+
+        """
+        max_val = int(number_of_headings / 8)
+
+        outer_edge_x = []
+        outer_edge_y = []
+
+        # Generate points that lie on the perimeter of the surface
+        # of a square with sides of length max_val
+        for i in range(-max_val, max_val + 1):
+            outer_edge_x.extend([i, i])
+            outer_edge_y.extend([-max_val, max_val])
+
+            if i != max_val and i != -max_val:
+                outer_edge_x.extend([-max_val, max_val])
+                outer_edge_y.extend([i, i])
+
+        return sorted([np.arctan2(j, i) for i, j in zip(outer_edge_x, outer_edge_y)])
+
+    def _point_to_line_distance(self, p1: np.array, p2: np.array, q: np.array) -> float:
+        """
+        Return the shortest distance from a point to a line segment.
+
+        Args:
+        p1: np.array(2,)
+            Start point of line segment
+        p2: np.array(2,)
+            End point of line segment
+        q: np.array(2,)
+            Point to get distance away from line of
+
+        Returns
+        -------
+        float
+            The shortest distance between q and line segment p1p2
+
+        """
+        # Get back the l2-norm without the square root
+        l2 = np.inner(p1 - p2, p1 - p2)
+
+        if l2 == 0:
+            return np.linalg.norm(p1 - q)
+
+        # Ensure t lies in [0, 1]
+        t = max(0, min(1, np.dot(q - p1, p2 - p1) / l2))
+        projected_point = p1 + t * (p2 - p1)
+
+        return np.linalg.norm(q - projected_point)
+
+    def _is_minimal_trajectory(
+        self, trajectory: Trajectory, prior_end_poses: index.Rtree
+    ) -> bool:
+        """
+        Determine wheter a trajectory is a minimal trajectory.
+
+        Uses an RTree for speedup.
+
+        Args:
+        trajectory: Trajectory
+            The trajectory to check
+        prior_end_poses: RTree
+            An RTree holding the current minimal set of trajectories
+
+        Returns
+        -------
+        bool
+            True if the trajectory is a minimal trajectory otherwise false
+
+        """
+        # Iterate over line segments in the trajectory
+        for x1, y1, x2, y2, yaw in zip(
+            trajectory.path.xs[:-1],
+            trajectory.path.ys[:-1],
+            trajectory.path.xs[1:],
+            trajectory.path.ys[1:],
+            trajectory.path.yaws[:-1],
+        ):
+
+            p1 = np.array([x1, y1])
+            p2 = np.array([x2, y2])
+
+            # Create a bounding box search region
+            # around the line segment
+            left_bb = min(x1, x2) - self.DISTANCE_THRESHOLD
+            right_bb = max(x1, x2) + self.DISTANCE_THRESHOLD
+            top_bb = max(y1, y2) + self.DISTANCE_THRESHOLD
+            bottom_bb = min(y1, y2) - self.DISTANCE_THRESHOLD
+
+            # For any previous end points in the search region we
+            # check the distance to that point and the angle
+            # difference. If they are within threshold then this
+            # trajectory can be composed from a previous trajectory
+            for prior_end_pose in prior_end_poses.intersection(
+                (left_bb, bottom_bb, right_bb, top_bb), objects='raw'
+            ):
+                if (
+                    self._point_to_line_distance(p1, p2, prior_end_pose[:-1])
+                    < self.DISTANCE_THRESHOLD
+                    and angle_difference(yaw, prior_end_pose[-1])
+                    < self.ROTATION_THRESHOLD
+                ):
+                    return False
+
+        return True
+
+    def _compute_min_trajectory_length(self) -> float:
+        """
+        Compute the minimum trajectory length for the given parameters.
+
+        The minimum trajectory length is defined as the length needed
+        for the sharpest possible turn to move from 0 degrees to the next
+        discrete heading. Since the distance between headings is not uniform
+        we take the smallest possible difference.
+
+        Returns
+        -------
+        float
+            The minimal length of a trajectory
+
+        """
+        # Compute arc length for a turn that moves from 0 degrees to
+        # the minimum heading difference
+        heading_diff = [
+            abs(self.headings[i + 1] - self.headings[i])
+            for i in range(len(self.headings) - 1)
+        ]
+
+        return self.turning_radius * min(heading_diff)
+
+    def _generate_minimal_spanning_set(self) -> dict:
+        """
+        Generate the minimal spanning set.
+
+        Iteratves over all possible trajectories and keeps only those that
+        are part of the minimal set.
+
+        Returns
+        -------
+        dict
+            A dictionary where the key is the start_angle and the value is
+            a list of trajectories that begin at that angle
+
+        """
+        quadrant1_end_poses = defaultdict(list)
+
+        # Since we only compute for quadrant 1 we only need headings between
+        # 0 and 90 degrees
+        initial_headings = sorted(
+            filter(lambda x: 0 <= x and x <= np.pi / 2, self.headings)
+        )
+
+        # Use the minimum trajectory length to find the starting wave front
+        min_trajectory_length = self._compute_min_trajectory_length()
+        wave_front_start_pos = int(
+            np.round(min_trajectory_length / self.grid_resolution)
+        )
+
+        for start_heading in initial_headings:
+            iterations_without_trajectory = 0
+
+            prior_end_poses = index.Index()
+
+            wave_front_cur_pos = wave_front_start_pos
+
+            # To get target headings: sort headings radially and remove those
+            # that are more than 90 degrees away
+            target_headings = sorted(
+                self.headings, key=lambda x: (abs(x - start_heading), -x)
+            )
+            target_headings = list(
+                filter(lambda x: abs(start_heading - x) <= np.pi / 2, target_headings)
+            )
+
+            while iterations_without_trajectory < self.stopping_threshold:
+                iterations_without_trajectory += 1
+
+                # Generate x,y coordinates for current wave front
+                positions = self._get_wave_front_points(wave_front_cur_pos)
+
+                for target_point in positions:
+                    for target_heading in target_headings:
+                        # Use 10% of grid separation for finer granularity
+                        # when checking if trajectory overlaps another already
+                        # seen trajectory
+                        trajectory = self.trajectory_generator.generate_trajectory(
+                            target_point,
+                            start_heading,
+                            target_heading,
+                            0.1 * self.grid_resolution,
+                        )
+
+                        if trajectory is not None:
+                            # Check if path overlaps something in minimal
+                            # spanning set
+                            if self._is_minimal_trajectory(trajectory, prior_end_poses):
+
+                                # Add end pose to minimal set
+                                new_end_pose = np.array(
+                                    [target_point[0], target_point[1], target_heading]
+                                )
+
+                                quadrant1_end_poses[start_heading].append(
+                                    (target_point, target_heading)
+                                )
+
+                                # Create a new bounding box in the RTree
+                                # for this trajectory
+                                left_bb = target_point[0] - self.DISTANCE_THRESHOLD
+                                right_bb = target_point[0] + self.DISTANCE_THRESHOLD
+                                bottom_bb = target_point[1] - self.DISTANCE_THRESHOLD
+                                top_bb = target_point[1] + self.DISTANCE_THRESHOLD
+
+                                prior_end_poses.insert(
+                                    0,
+                                    (left_bb, bottom_bb, right_bb, top_bb),
+                                    new_end_pose,
+                                )
+
+                                iterations_without_trajectory = 0
+
+                wave_front_cur_pos += 1
+
+        # Once we have found the minimal trajectory set for quadrant 1
+        # we can leverage symmetry to create the complete minimal set
+        return self._create_complete_minimal_spanning_set(quadrant1_end_poses)
+
+    def _flip_angle(self, angle: float, flip_type: Flip) -> float:
+        """
+        Return the the appropriate flip of the angle in self.headings.
+
+        Args:
+        angle: float
+            The angle to flip
+        flip_type: Flip
+            Whether to flip acrpss X axis, Y axis, or both
+
+        Returns
+        -------
+        float
+            The angle in self.heading that is the appropriate flip
+
+        """
+        angle_idx = self.headings.index(angle)
+
+        if flip_type == self.Flip.X:
+            heading_idx = (self.num_of_headings / 2 - 1) - angle_idx - 1
+        elif flip_type == self.Flip.Y:
+            heading_idx = self.num_of_headings - angle_idx - 2
+        elif flip_type == self.Flip.BOTH:
+            heading_idx = (
+                angle_idx - (self.num_of_headings / 2)
+            ) % self.num_of_headings
+        else:
+            raise Exception(f'Unsupported flip type: {flip_type}')
+
+        return self.headings[int(heading_idx)]
+
+    def _create_complete_minimal_spanning_set(
+        self, single_quadrant_minimal_set: dict
+    ) -> dict:
+        """
+        Create the full minimal spanning set from a single quadrant set.
+
+        Exploits the symmetry between the quadrants to create the full set.
+        This is done by flipping every trajectory in the first quadrant across
+        either the X-axis, Y-axis, or both axes.
+
+        Args:
+        single_quadrant_minimal_set: dict
+            The minimal set for quadrant 1 (positive x and positive y)
+
+        Returns
+        -------
+        dict
+            The complete minimal spanning set containing the trajectories
+            in all quadrants
+
+        """
+        all_trajectories = defaultdict(list)
+
+        for start_angle in single_quadrant_minimal_set.keys():
+
+            for end_point, end_angle in single_quadrant_minimal_set[start_angle]:
+
+                x, y = end_point
+
+                # Prevent double adding trajectories that lie on axes
+                # (i.e. start and end angle are either both 0 or both pi/2)
+                if start_angle == 0 and end_angle == 0:
+                    unflipped_start_angle = 0.0
+                    flipped_x_start_angle = np.pi
+
+                    unflipped_end_angle = 0.0
+                    flipped_x_end_angle = np.pi
+
+                    # Generate trajectories from calculated parameters
+                    unflipped_trajectory = (
+                        self.trajectory_generator.generate_trajectory(
+                            np.array([x, y]),
+                            unflipped_start_angle,
+                            unflipped_end_angle,
+                            self.grid_resolution,
+                        )
+                    )
+                    flipped_x_trajectory = (
+                        self.trajectory_generator.generate_trajectory(
+                            np.array([-x, -y]),
+                            flipped_x_start_angle,
+                            flipped_x_end_angle,
+                            self.grid_resolution,
+                        )
+                    )
+
+                    all_trajectories[
+                        unflipped_trajectory.parameters.start_angle
+                    ].append(unflipped_trajectory)
+
+                    all_trajectories[
+                        flipped_x_trajectory.parameters.start_angle
+                    ].append(flipped_x_trajectory)
+
+                elif abs(start_angle) == np.pi / 2 and abs(end_angle) == np.pi / 2:
+                    unflipped_start_angle = np.pi / 2
+                    flipped_y_start_angle = -np.pi / 2
+
+                    unflipped_end_angle = np.pi / 2
+                    flipped_y_end_angle = -np.pi / 2
+
+                    # Generate trajectories from calculated parameters
+                    unflipped_trajectory = (
+                        self.trajectory_generator.generate_trajectory(
+                            np.array([-x, y]),
+                            unflipped_start_angle,
+                            unflipped_end_angle,
+                            self.grid_resolution,
+                        )
+                    )
+
+                    flipped_y_trajectory = (
+                        self.trajectory_generator.generate_trajectory(
+                            np.array([x, -y]),
+                            flipped_y_start_angle,
+                            flipped_y_end_angle,
+                            self.grid_resolution,
+                        )
+                    )
+
+                    all_trajectories[
+                        unflipped_trajectory.parameters.start_angle
+                    ].append(unflipped_trajectory)
+                    all_trajectories[
+                        flipped_y_trajectory.parameters.start_angle
+                    ].append(flipped_y_trajectory)
+
+                else:
+                    unflipped_start_angle = start_angle
+                    flipped_x_start_angle = self._flip_angle(start_angle, self.Flip.X)
+                    flipped_y_start_angle = self._flip_angle(start_angle, self.Flip.Y)
+                    flipped_xy_start_angle = self._flip_angle(
+                        start_angle, self.Flip.BOTH
+                    )
+
+                    unflipped_end_angle = end_angle
+                    flipped_x_end_angle = self._flip_angle(end_angle, self.Flip.X)
+                    flipped_y_end_angle = self._flip_angle(end_angle, self.Flip.Y)
+                    flipped_xy_end_angle = self._flip_angle(end_angle, self.Flip.BOTH)
+
+                    # Generate trajectories from calculated parameters
+                    unflipped_trajectory = (
+                        self.trajectory_generator.generate_trajectory(
+                            np.array([x, y]),
+                            unflipped_start_angle,
+                            unflipped_end_angle,
+                            self.grid_resolution,
+                        )
+                    )
+                    flipped_x_trajectory = (
+                        self.trajectory_generator.generate_trajectory(
+                            np.array([-x, y]),
+                            flipped_x_start_angle,
+                            flipped_x_end_angle,
+                            self.grid_resolution,
+                        )
+                    )
+                    flipped_y_trajectory = (
+                        self.trajectory_generator.generate_trajectory(
+                            np.array([x, -y]),
+                            flipped_y_start_angle,
+                            flipped_y_end_angle,
+                            self.grid_resolution,
+                        )
+                    )
+                    flipped_xy_trajectory = (
+                        self.trajectory_generator.generate_trajectory(
+                            np.array([-x, -y]),
+                            flipped_xy_start_angle,
+                            flipped_xy_end_angle,
+                            self.grid_resolution,
+                        )
+                    )
+
+                    all_trajectories[
+                        unflipped_trajectory.parameters.start_angle
+                    ].append(unflipped_trajectory)
+                    all_trajectories[
+                        flipped_x_trajectory.parameters.start_angle
+                    ].append(flipped_x_trajectory)
+                    all_trajectories[
+                        flipped_y_trajectory.parameters.start_angle
+                    ].append(flipped_y_trajectory)
+                    all_trajectories[
+                        flipped_xy_trajectory.parameters.start_angle
+                    ].append(flipped_xy_trajectory)
+
+        return all_trajectories
+
+    def _handle_motion_model(self, spanning_set: dict) -> dict:
+        """
+        Add the appropriate motions for the user supplied motion model.
+
+        Ackerman: No additional trajectories
+
+        Diff: In place turns to the right and left
+
+        Omni: Diff + Sliding movements to right and left
+
+        Args:
+        spanning set: dict
+            The minimal spanning set
+
+        Returns
+        -------
+        dict
+            The minimal spanning set with additional trajectories based
+            on the motion model
+
+        """
+        if self.motion_model == self.MotionModel.ACKERMANN:
+            return spanning_set
+
+        elif self.motion_model == self.MotionModel.DIFF:
+            diff_spanning_set = self._add_in_place_turns(spanning_set)
+            return diff_spanning_set
+
+        elif self.motion_model == self.MotionModel.OMNI:
+            omni_spanning_set = self._add_in_place_turns(spanning_set)
+            omni_spanning_set = self._add_horizontal_motions(omni_spanning_set)
+            return omni_spanning_set
+
+        else:
+            print('No handling implemented for Motion Model: ' +
+                  f'{self.motion_model}')
+            raise NotImplementedError
+
+    def _add_in_place_turns(self, spanning_set: dict) -> dict:
+        """
+        Add in place turns to the spanning set.
+
+        In place turns are trajectories with only a rotational component and
+        only shift a single angular heading step
+
+        Args:
+        spanning_set: dict
+            The minimal spanning set
+
+        Returns
+        -------
+        dict
+            The minimal spanning set containing additional in place turns
+            for each start angle
+
+        """
+        all_angles = sorted(spanning_set.keys())
+
+        for idx, start_angle in enumerate(all_angles):
+            prev_angle_idx = idx - 1 if idx - 1 >= 0 else len(all_angles) - 1
+            next_angle_idx = idx + 1 if idx + 1 < len(all_angles) else 0
+
+            prev_angle = all_angles[prev_angle_idx]
+            next_angle = all_angles[next_angle_idx]
+
+            left_turn_params = TrajectoryParameters.no_arc(
+                end_point=np.array([0, 0]),
+                start_angle=start_angle,
+                end_angle=next_angle,
+            )
+            right_turn_params = TrajectoryParameters.no_arc(
+                end_point=np.array([0, 0]),
+                start_angle=start_angle,
+                end_angle=prev_angle,
+            )
+
+            # Calculate number of steps needed to rotate by roughly 10 degrees
+            # for each pose
+            angle_dif = angle_difference(start_angle, next_angle)
+            steps = int(round(angle_dif / np.deg2rad(10))) + 1
+
+            position = np.full(steps, 0)
+            left_yaws = interpolate_yaws(start_angle, next_angle, True, steps)
+            right_yaws = interpolate_yaws(start_angle, prev_angle, False, steps)
+
+            left_turn_path = Path(xs=position, ys=position, yaws=left_yaws)
+            right_turn_path = Path(xs=position, ys=position, yaws=right_yaws)
+
+            left_turn = Trajectory(parameters=left_turn_params, path=left_turn_path)
+            right_turn = Trajectory(parameters=right_turn_params, path=right_turn_path)
+
+            spanning_set[start_angle].append(left_turn)
+            spanning_set[start_angle].append(right_turn)
+
+        return spanning_set
+
+    def _add_horizontal_motions(self, spanning_set: dict) -> dict:
+        """
+        Add horizontal sliding motions to the spanning set.
+
+        The horizontal sliding motions are simply straight line trajectories
+        at 90 degrees to every start angle in the spanning set. The yaw of these
+        trajectories is the same as the start angle for which it is generated.
+
+        Args:
+        spanning_set: dict
+            The minimal spanning set
+
+        Returns
+        -------
+        dict
+            The minimal spanning set containing additional sliding motions
+            for each start angle
+
+        """
+        # Calculate the offset in the headings list that represents an
+        # angle change of 90 degrees
+        idx_offset = int(self.num_of_headings / 4)
+
+        for idx, angle in enumerate(self.headings):
+
+            # Copy the straight line trajectory for the start angle that
+            # is 90 degrees to the left
+            left_angle_idx = int((idx + idx_offset) % self.num_of_headings)
+            left_angle = self.headings[left_angle_idx]
+            left_trajectories = spanning_set[left_angle]
+            left_straight_trajectory = next(
+                t for t in left_trajectories if t.parameters.end_angle == left_angle
+            )
+
+            # Copy the straight line trajectory for the start angle that
+            # is 90 degrees to the right
+            right_angle_idx = int((idx - idx_offset) % self.num_of_headings)
+            right_angle = self.headings[right_angle_idx]
+            right_trajectories = spanning_set[right_angle]
+            right_straight_trajectory = next(
+                t for t in right_trajectories if t.parameters.end_angle == right_angle
+            )
+
+            yaws = np.full(
+                len(left_straight_trajectory.path.xs), angle, dtype=np.float64
+            )
+
+            # Create a new set of parameters that represents
+            # the left sliding motion
+            parmas_l = left_straight_trajectory.parameters
+            left_motion_parameters = TrajectoryParameters(
+                parmas_l.turning_radius,
+                parmas_l.x_offset,
+                parmas_l.y_offset,
+                parmas_l.end_point,
+                angle,
+                angle,
+                parmas_l.left_turn,
+                parmas_l.arc_start_point,
+                parmas_l.arc_end_point,
+            )
+
+            # Create a new set of parameters that represents
+            # the right sliding motion
+            params_r = right_straight_trajectory.parameters
+            right_motion_parameters = TrajectoryParameters(
+                params_r.turning_radius,
+                params_r.x_offset,
+                params_r.y_offset,
+                params_r.end_point,
+                angle,
+                angle,
+                params_r.left_turn,
+                parmas_l.arc_start_point,
+                parmas_l.arc_end_point,
+            )
+
+            left_motion = Trajectory(
+                parameters=left_motion_parameters,
+                path=Path(
+                    xs=left_straight_trajectory.path.xs,
+                    ys=left_straight_trajectory.path.ys,
+                    yaws=yaws,
+                ),
+            )
+
+            right_motion = Trajectory(
+                parameters=right_motion_parameters,
+                path=Path(
+                    xs=right_straight_trajectory.path.xs,
+                    ys=right_straight_trajectory.path.ys,
+                    yaws=yaws,
+                ),
+            )
+
+            spanning_set[angle].append(left_motion)
+            spanning_set[angle].append(right_motion)
+
+        return spanning_set
+
+    def run(self):
+        """
+        Run the lattice generator.
+
+        Returns
+        -------
+        dict
+            The minimal spanning set including additional motions for the
+            specified motion model
+
+        """
+        complete_spanning_set = self._generate_minimal_spanning_set()
+
+        return self._handle_motion_model(complete_spanning_set)

navigation2/nav2_smac_planner/lattice_primitives/requirements.txt

@@ -0,0 +1,3 @@
+numpy>=1.17.4
+matplotlib>=3.1.2
+Rtree>=0.9.7

navigation2/nav2_smac_planner/lattice_primitives/tests/test_lattice_generator.py

@@ -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()

navigation2/nav2_smac_planner/lattice_primitives/tests/test_trajectory_generator.py

@@ -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()

navigation2/nav2_smac_planner/lattice_primitives/tests/trajectory_visualizer.py

@@ -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()

navigation2/nav2_smac_planner/lattice_primitives/trajectory.py

@@ -0,0 +1,148 @@
+# 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.
+
+from dataclasses import dataclass
+
+from helper import angle_difference, normalize_angle
+
+import numpy as np
+
+
+@dataclass(frozen=True)
+class TrajectoryParameters:
+    """
+    A dataclass that holds the data needed to create the path for a trajectory.
+
+    turning_radius: The radius of the circle used to generate
+        the arc of the path
+    x_offset: The x coordinate of the circle used to generate
+        the arc of the path
+    y_offset: They y coordinate of the circle used to generate
+        the arc of the path
+    end_point: The end coordinate of the path
+    start_angle: The starting angle of the path
+        - given in radians from -pi to pi where 0 radians is along
+            the positive x axis
+    end_angle: The end angle of the path
+        - given in radians from -pi to pi where 0 radians is along
+            the positive x axis
+    left_turn: Whether the arc in the path turns to the left
+    arc_start_point: Coordinates of the starting position of the arc
+    arc_end_point: Coordinates of the ending position of the arc
+    """
+
+    turning_radius: float
+    x_offset: float
+    y_offset: float
+    end_point: np.array
+    start_angle: float
+    end_angle: float
+    left_turn: bool
+
+    arc_start_point: float
+    arc_end_point: float
+
+    @property
+    def arc_length(self):
+        """Arc length of the trajectory."""
+        return self.turning_radius * angle_difference(
+            self.start_angle, self.end_angle, self.left_turn
+        )
+
+    @property
+    def start_straight_length(self):
+        """Length of the straight line from start to arc."""
+        return np.linalg.norm(self.arc_start_point)
+
+    @property
+    def end_straight_length(self):
+        """Length of the straight line from arc to end."""
+        return np.linalg.norm(self.end_point - self.arc_end_point)
+
+    @property
+    def total_length(self):
+        """Total length of trajectory."""
+        return self.arc_length + self.start_straight_length + \
+            self.end_straight_length
+
+    @staticmethod
+    def no_arc(end_point, start_angle, end_angle):
+        """Create the parameters for a trajectory with no arc."""
+        return TrajectoryParameters(
+            turning_radius=0.0,
+            x_offset=0.0,
+            y_offset=0.0,
+            end_point=end_point,
+            start_angle=start_angle,
+            end_angle=end_angle,
+            left_turn=True,
+            arc_start_point=end_point,
+            arc_end_point=end_point,
+        )
+
+
+@dataclass(frozen=True)
+class Path:
+    """
+    A dataclass that holds the generated poses for a given trajectory.
+
+    xs: X coordinates of poses along trajectory
+    ys: Y coordinates of poses along trajectory
+    yaws: Yaws of poses along trajectory
+    """
+
+    xs: np.array
+    ys: np.array
+    yaws: np.array
+
+    def __add__(self, rhs):
+        """Add two paths together by concatenating them."""
+        if self.xs is None:
+            return rhs
+
+        xs = np.concatenate((self.xs, rhs.xs))
+        ys = np.concatenate((self.ys, rhs.ys))
+        yaws = np.concatenate((self.yaws, rhs.yaws))
+
+        return Path(xs, ys, yaws)
+
+    def to_output_format(self):
+        """Return the path data in a format suitable for outputting."""
+        output_xs = self.xs.round(5)
+        output_ys = self.ys.round(5)
+
+        # A bit of a hack but it removes any -0.0
+        output_xs = output_xs + 0.0
+        output_ys = output_ys + 0.0
+        output_yaws = self.yaws + 0.0
+
+        vectorized_normalize_angle = np.vectorize(normalize_angle)
+        output_yaws = vectorized_normalize_angle(output_yaws)
+
+        stacked = np.vstack([output_xs, output_ys, output_yaws]).transpose()
+
+        return stacked.tolist()
+
+
+@dataclass(frozen=True)
+class Trajectory:
+    """
+    A dataclass that holds the path and parameters for a trajectory.
+
+    path: The Path that represents the trajectory
+    parameters: The TrajectoryParameters that represent the trajectory
+    """
+
+    path: Path
+    parameters: TrajectoryParameters

navigation2/nav2_smac_planner/lattice_primitives/trajectory_generator.py

@@ -0,0 +1,568 @@
+# 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 logging
+from typing import Tuple, Union
+
+import numpy as np
+
+from trajectory import Path, Trajectory, TrajectoryParameters
+
+logger = logging.getLogger(__name__)
+
+
+class TrajectoryGenerator:
+    """Handles all the logic for generating trajectories."""
+
+    def __init__(self, config: dict):
+        """Init TrajectoryGenerator using the user supplied config."""
+        self.turning_radius = config['turning_radius']
+
+    def _get_arc_point(
+        self, trajectory_params: TrajectoryParameters, t: float
+    ) -> Tuple[float, float, float]:
+        """
+        Get point on the arc trajectory using the following parameterization.
+
+            r(t) = <R * cos(t - pi/2) + a, R * sin(t - pi/2) + b>
+
+            R = radius
+            a = x offset
+            b = y offset
+
+        Args
+        ----
+        trajectory_params: TrajectoryParameters
+            The parameters that describe the arc to create
+        t: float
+            A value between 0 - 1 that denotes where along the arc
+            to calculate the point
+
+        Returns
+        -------
+        x: float
+            x coordinate of generated point
+        y: float
+            y coordinate of generated point
+        yaw: float
+            angle of tangent line to arc at point (x,y)
+
+        """
+        start_angle = trajectory_params.start_angle
+
+        arc_dist = t * trajectory_params.arc_length
+        angle_step = arc_dist / trajectory_params.turning_radius
+
+        if trajectory_params.left_turn:
+            # Calculate points using
+            # r(t) = <R * cos(t - pi/2) + a, R * sin(t - pi/2) + b>
+            t = start_angle + angle_step
+            x = (
+                trajectory_params.turning_radius * np.cos(t - np.pi / 2)
+                + trajectory_params.x_offset
+            )
+            y = (
+                trajectory_params.turning_radius * np.sin(t - np.pi / 2)
+                + trajectory_params.y_offset
+            )
+
+            yaw = t
+
+        else:
+            # Right turns go the opposite way across the arc, so we
+            # need to invert the angles and adjust the parametrization
+            start_angle = -start_angle
+
+            # Calculate points using
+            # r(t) = <R * -cos(t + pi/2) + a, R * sin(t + pi/2) + b>
+            t = start_angle + angle_step
+            x = (
+                trajectory_params.turning_radius * -np.cos(t + np.pi / 2)
+                + trajectory_params.x_offset
+            )
+            y = (
+                trajectory_params.turning_radius * np.sin(t + np.pi / 2)
+                + trajectory_params.y_offset
+            )
+
+            yaw = -t
+
+        return x, y, yaw
+
+    def _get_line_point(
+        self, start_point: np.array, end_point: np.array, t: float
+    ) -> Tuple[float, float]:
+        """
+        Get point on a line segment using the following parameterization.
+
+            r(t) = p + t * (q - p)
+
+            p = start point
+            q = end point
+
+        Args
+        ----
+        start_point: np.array(2,)
+            Starting point of the line segment
+        end_point: np.array(2,)
+            End point of the line segment
+        t: float
+            A value between 0 - 1 that denotes where along the segment
+            to calculate the point
+
+        Returns
+        -------
+        x: float
+            x coordinate of generated point
+        y: float
+            y coordinate of generated point
+
+        """
+        return start_point + t * (end_point - start_point)
+
+    def _create_path(
+        self, trajectory_params: TrajectoryParameters, primitive_resolution: float
+    ) -> Path:
+        """
+        Create the full trajectory path from the given trajectory parameters.
+
+        Args
+        ----
+        trajectory_params: TrajectoryParameters
+            The parameters that describe the trajectory to create
+        primitive_resolution: float
+            The desired distance between sampled points along the line.
+            This value is not strictly adhered to as the path may not
+            be neatly divisible, however the spacing will be as close
+            as possible.
+
+        Returns
+        -------
+        TrajectoryPath
+            The trajectory path described by the trajectory parameters
+
+        """
+        number_of_steps = np.round(
+            trajectory_params.total_length / primitive_resolution
+        ).astype(int)
+        t_step = 1 / number_of_steps
+
+        start_to_arc_dist = np.linalg.norm(trajectory_params.arc_start_point)
+
+        transition_points = [
+            start_to_arc_dist / trajectory_params.total_length,
+            (start_to_arc_dist + trajectory_params.arc_length)
+            / trajectory_params.total_length,
+        ]
+
+        cur_t = t_step
+
+        xs = []
+        ys = []
+        yaws = []
+
+        for i in range(1, number_of_steps + 1):
+
+            # This prevents cur_t going over 1 due to rounding issues in t_step
+            cur_t = min(cur_t, 1)
+
+            # Handle the initial straight line segment
+            if cur_t <= transition_points[0]:
+                line_t = cur_t / transition_points[0]
+                x, y = self._get_line_point(
+                    np.array([0, 0]), trajectory_params.arc_start_point, line_t
+                )
+                yaw = trajectory_params.start_angle
+
+            # Handle the arc
+            elif cur_t <= transition_points[1]:
+                arc_t = (cur_t - transition_points[0]) / (
+                    transition_points[1] - transition_points[0]
+                )
+                x, y, yaw = self._get_arc_point(trajectory_params, arc_t)
+
+            # Handle the end straight line segment
+            else:
+                line_t = (cur_t - transition_points[1]) / (1 - transition_points[1])
+                x, y = self._get_line_point(
+                    trajectory_params.arc_end_point, trajectory_params.end_point, line_t
+                )
+                yaw = trajectory_params.end_angle
+
+            xs.append(x)
+            ys.append(y)
+            yaws.append(yaw)
+
+            cur_t += t_step
+
+        # Convert to numpy arrays
+        xs = np.array(xs)
+        ys = np.array(ys)
+        yaws = np.array(yaws)
+
+        # The last point may be slightly off due to rounding issues
+        # so we correct the last point to be exactly the end point
+        xs[-1], ys[-1] = trajectory_params.end_point
+        yaws[-1] = trajectory_params.end_angle
+
+        return Path(xs, ys, yaws)
+
+    def _get_intersection_point(
+        self, m1: float, c1: float, m2: float, c2: float
+    ) -> np.array:
+        """
+        Get the intersection point of two lines.
+
+        The two lines are described by m1 * x + c1 and m2 * x + c2.
+
+        Args
+        ----
+        m1: float
+            Gradient of line 1
+        c1: float
+            y-intercept of line 1
+        m2: float
+            Gradient of line 2
+        c2: float
+            y-intercept of line2
+
+        Returns
+        -------
+        np.array (2,)
+            The intersection point of line 1 and 2
+
+        """
+        def line1(x):
+            return m1 * x + c1
+
+        x_point = (c2 - c1) / (m1 - m2)
+
+        return np.array([x_point, line1(x_point)])
+
+    def _is_left_turn(self, intersection_point: np.array, end_point: np.array) -> bool:
+        """
+        Determine if a trajectory will be a left turn.
+
+        Uses the determinant to determine whether the arc formed by the
+        intersection and end point turns left or right.
+
+        Args
+        ----
+        intersection_point: np.array(2,)
+            The intersection point of the lines formed from the start
+            and end angles
+        end_point: np.array(2,)
+            The chosen end point of the trajectory
+
+        Returns
+        -------
+        bool
+            True if curve turns left, false otherwise
+
+        """
+        matrix = np.vstack([intersection_point, end_point])
+        det = np.linalg.det(matrix)
+
+        return det >= 0
+
+    def _is_dir_vec_correct(
+        self, point1: np.array, point2: np.array, line_angle: float
+    ) -> bool:
+        """
+        Check that the direction vector agrees with the line angle.
+
+        The direction vector is defined as the vector from point 1 to
+        point 2.
+
+        Args
+        ----
+        point1: np.array(2,)
+            The start point of the vector
+        point2: np.array(2,)
+            The end point of the vector
+        line_angle: float
+            The angle of a line to compare against the vector
+
+        Returns
+        -------
+        bool
+            True if both line and vector point in same direction
+
+        """
+        # Need to round to prevent very small values for 0
+        m = abs(np.tan(line_angle).round(5))
+
+        if line_angle < 0:
+            m *= -1
+
+        direction_vec_from_points = point2 - point1
+
+        direction_vec_from_gradient = np.array([1, m])
+
+        # Handle when line angle is in quadrant 2 or 3 and when angle is 90
+        if abs(line_angle) > np.pi / 2:
+            direction_vec_from_gradient = np.array([-1, m])
+        elif abs(line_angle) == np.pi / 2:
+            direction_vec_from_gradient = np.array([0, m])
+
+        direction_vec_from_gradient = direction_vec_from_gradient.round(5)
+        direction_vec_from_points = direction_vec_from_points.round(5)
+
+        if np.all(
+            np.sign(direction_vec_from_points) == np.sign(direction_vec_from_gradient)
+        ):
+            return True
+        else:
+            return False
+
+    def _calculate_trajectory_params(
+        self, end_point: np.array, start_angle: float, end_angle: float
+    ) -> Union[TrajectoryParameters, None]:
+        """
+        Calculate the parameters for a trajectory with the desired constraints.
+
+        The trajectory may consist of an arc and at most two line segments.
+        A straight trajectory will consist of a single line segment. Similarly,
+        a purely curving trajectory will only consist of an arc.
+
+        Idea:
+            1. Extend a line from (0,0) with angle of start_angle
+            2. Extend a line from end_point with angle of end_angle
+            3. Compute their intersection point, I
+            4. Check that the intersection point leads to a
+            valid trajectory
+                - If I is too close to (0,0) or the end point then
+                no arc greater than the turning radius will reach
+                from (0,0) to end point
+
+        If two segments from the same exterior point are tangent to
+        a circle then they are congruent
+
+        Args
+        ----
+        end_point: np.array(2,)
+            The desired end point of the trajectory
+        start_angle: float
+            The start angle of the trajectory in radians
+        end_angle: float
+            The end angle of the trajectory in radians
+
+        Returns
+        -------
+        TrajectoryParameters or None
+            If a valid trajectory exists then the Trajectory parameters
+            are returned, otherwise None
+
+        """
+        x2, y2 = end_point
+        arc_start_point = np.array([0, 0])
+        arc_end_point = end_point
+
+        # Find gradient of line 1 passing through (0,0) that makes an angle
+        # of start_angle with x_axis
+        m1 = np.tan(start_angle).round(5)
+
+        # Find gradient of line 2 passing through end point that makes an angle
+        # of end_angle with x-axis
+        m2 = np.tan(end_angle).round(5)
+
+        # Deal with lines that are parallel
+        if m1 == m2:
+            # If they are coincident (i.e. y-intercept is same) then simply
+            # return a circle with infinite radius
+            if round(-m2 * x2 + y2, 5) == 0:
+                return TrajectoryParameters.no_arc(
+                    end_point=end_point, start_angle=start_angle, end_angle=end_angle
+                )
+
+            # Deal with edge case of 90
+            elif (
+                abs(start_angle) == np.pi / 2 and arc_end_point[0] == arc_start_point[0]
+            ):
+                return TrajectoryParameters.no_arc(
+                    end_point=end_point,
+                    start_angle=start_angle,
+                    end_angle=end_angle,
+                )
+
+            else:
+                logger.debug(
+                    'No trajectory possible for equivalent start and '
+                    + f'end angles that also passes through p = {x2, y2}'
+                )
+                return None
+
+        # Find intersection point of lines 1 and 2
+        intersection_point = self._get_intersection_point(m1, 0, m2, -m2 * x2 + y2)
+
+        # Check that the vector from (0,0) to intersection point agrees
+        # with the angle of line 1
+        if not self._is_dir_vec_correct(
+            arc_start_point, intersection_point, start_angle
+        ):
+            logger.debug(
+                'No trajectory possible since intersection point occurs '
+                + 'before start point on line 1'
+            )
+            return None
+
+        # Check that the vector from intersection point to arc start point agrees with
+        # the angle of line 2
+        if not self._is_dir_vec_correct(intersection_point, arc_end_point, end_angle):
+            logger.debug(
+                'No trajectory possible since intersection point occurs '
+                + 'after end point on line 2'
+            )
+            return None
+
+        # Calculate distance between arc start point and intersection point
+        dist_a = round(np.linalg.norm(arc_start_point - intersection_point), 5)
+
+        # Calculate distance between arc end point and intersection point
+        dist_b = round(np.linalg.norm(arc_end_point - intersection_point), 5)
+
+        # Calculate the angle between start angle and end angle lines
+        angle_between_lines = np.pi - abs(end_angle - start_angle)
+
+        # The closer the arc start and end points are to the intersection point the
+        # smaller the turning radius will be. However, we have a constraint on how
+        # small this turning radius can get. To calculate the minimum allowed
+        # distance we draw a right triangle with height equal to the constrained
+        # radius and angle opposite the height leg as half the angle between the
+        # start and end angle lines. The minimum valid distance is then the
+        # base of this triangle.
+        min_valid_distance = round(
+            self.turning_radius / np.tan(angle_between_lines / 2), 5
+        )
+
+        # Both the distance of p along line 2 and intersection point along
+        # line 1 must be greater than the minimum valid distance
+        if dist_a < min_valid_distance or dist_b < min_valid_distance:
+            logger.debug(
+                'No trajectory possible where radius is larger than '
+                + 'minimum turning radius'
+            )
+            return None
+
+        if dist_a < dist_b:
+            # Find new point on line 2 that is equidistant away from
+            # intersection point as arc start point is on line 1
+            vec_line2 = arc_end_point - intersection_point
+            vec_line2 /= np.linalg.norm(vec_line2)
+            arc_end_point = intersection_point + dist_a * vec_line2
+
+        elif dist_a > dist_b:
+            # Find new point on line 1 that is equidistant away from
+            # intersection point as arc end point is on line 2
+            vec_line1 = arc_start_point - intersection_point
+            vec_line1 /= np.linalg.norm(vec_line1)
+
+            arc_start_point = intersection_point + dist_b * vec_line1
+
+        x1, y1 = arc_start_point
+        x2, y2 = arc_end_point
+
+        # Find intersection point of the perpindicular lines of line 1 and 2
+        # that pass through arc start and arc end point respectively
+        if m1 == 0:
+            # If line 1 has gradient 0 then it is the x-axis.
+
+            def perp_line2(x):
+                return -1 / m2 * (x - x2) + y2
+
+            circle_center = np.array([x1, perp_line2(x1)])
+        elif m2 == 0:
+
+            def perp_line1(x):
+                return -1 / m1 * (x - x1) + y1
+
+            circle_center = np.array([x2, perp_line1(x2)])
+        else:
+            perp_m1 = -1 / m1 if m1 != 0 else 0
+            perp_m2 = -1 / m2 if m2 != 0 else 0
+
+            circle_center = self._get_intersection_point(
+                perp_m1, -perp_m1 * x1 + y1, perp_m2, -perp_m2 * x2 + y2
+            )
+
+        # The circles radius is the length from the center to arc start/end point
+        # (both distances are the same)
+        radius = np.linalg.norm(circle_center - arc_end_point).round(5)
+        x_offset = circle_center[0].round(5)
+        y_offset = circle_center[1].round(5)
+
+        if radius < self.turning_radius:
+            logger.debug(
+                'Calculated circle radius is smaller than allowed turning '
+                + f'radius: r = {radius}, min_radius = {self.turning_radius}'
+            )
+            return None
+
+        left_turn = self._is_left_turn(intersection_point, end_point)
+
+        return TrajectoryParameters(
+            radius,
+            x_offset,
+            y_offset,
+            end_point,
+            start_angle,
+            end_angle,
+            left_turn,
+            arc_start_point,
+            arc_end_point,
+        )
+
+    def generate_trajectory(
+        self,
+        end_point: np.array,
+        start_angle: float,
+        end_angle: float,
+        primitive_resolution: float,
+    ) -> Union[Trajectory, None]:
+        """
+        Create a trajectory from (0,0, start_angle) to (end_point, end_angle).
+
+        The trajectory will consist of a path that contains discrete points
+        that are spaced primitive_resolution apart.
+
+        Args
+        ----
+        end_point: np.array(2,)
+            The desired end point of the trajectory
+        start_angle: float
+            The start angle of the trajectory in radians
+        end_angle: float
+            The end angle of the trajectory in radians
+        primitive_resolution: float
+            The spacing between points along the trajectory
+
+        Returns
+        -------
+        Trajectory or None
+            If a valid trajectory exists then the Trajectory is returned,
+            otherwise None
+
+        """
+        trajectory_params = self._calculate_trajectory_params(
+            end_point, start_angle, end_angle
+        )
+
+        if trajectory_params is None:
+            return None
+
+        logger.debug('Trajectory found')
+
+        trajectory_path = self._create_path(trajectory_params, primitive_resolution)
+
+        return Trajectory(trajectory_path, trajectory_params)