commit 3d59c49b49fc571fff3c9ce0d6ecc392463b5670
Author: Sravan Balaji <sbalaji@mitre.org>
Date:   Thu Jan 26 09:11:58 2023 -0500

    Initial Commit
    
    - Add "version 1" of containerized Fast-RRT* ROS package

diff --git a/LICENSE b/LICENSE
new file mode 100644
index 0000000..0b79e8d
--- /dev/null
+++ b/LICENSE
@@ -0,0 +1,8 @@
+Copyright (c) 2022 The MITRE Corporation
+
+Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
\ No newline at end of file
diff --git a/README.md b/README.md
new file mode 100644
index 0000000..96057de
--- /dev/null
+++ b/README.md
@@ -0,0 +1,139 @@
+# Fast-RRT* Global Path Planner
+
+ROS Global Path Planner Plugin implementation of the [Fast-RRT* algorithm](https://doi.org/10.1016/j.eswa.2021.115457) for [move_base](http://wiki.ros.org/move_base).
+
+- [Fast-RRT\* Global Path Planner](#fast-rrt-global-path-planner)
+  - [Pseudocode](#pseudocode)
+  - [ROS](#ros)
+    - [Publisher List](#publisher-list)
+    - [Parameter List](#parameter-list)
+      - [Global Planner](#global-planner)
+      - [Global Costmap](#global-costmap)
+  - [Quick Start Guide](#quick-start-guide)
+    - [Important Locations Inside Container](#important-locations-inside-container)
+    - [Running Simulation](#running-simulation)
+      - [Terminal 1: Gazebo](#terminal-1-gazebo)
+      - [Terminal 2: RViz](#terminal-2-rviz)
+        - [Suggested RViz Settings](#suggested-rviz-settings)
+      - [Terminal 3: Navigation](#terminal-3-navigation)
+      - [Recording Activity](#recording-activity)
+  - [License](#license)
+
+## Pseudocode
+
+<img src="./docker/fast_rrt_ros/docs/fast_rrt.PNG" width="500">
+
+Fast-RRT* is more optimal than its predecessors RRT and RRT* through its novel method of creating an additional node when planning around obstacles. 
+
+The node placement draws upon the observation that paths that traverse closer to obstacles in the environment tend to be shorter, or more optimal, overall. 
+
+Check out the [documentation file](./docker/fast_rrt_ros/docs/Fast_RRTdocumentation.md) for an in depth explanation of how the Fast-RRT* functions work.
+
+## ROS
+
+The Fast-RRT* code is a plugin to the `global_planner` in the [move_base](http://wiki.ros.org/move_base) navigation stack.
+
+<img src="./docker/fast_rrt_ros/docs/move_base.png" width="700">
+
+This has been tested on the following ROS 1 distributions:
+
+- [ROS Noetic Ninjemys (Ubuntu 20.04)](http://wiki.ros.org/noetic)
+   - [Husky Noetic Documentation](https://www.clearpathrobotics.com/assets/guides/noetic/husky/)
+
+### Publisher List
+
+`~<name>/plan` (nav_msgs/Path): Last plan computed, published every time the planner computes a new path
+
+### Parameter List
+
+#### Global Planner 
+
+- `int max_num_iter_`: Maximum number of iterations to search for path
+- `int min_num_iter_`: Minimum number of iterations to search for path
+- `world_coords world_near_radius_`: Search radius (in world coordinates)
+- `world_coords world_dist_dichotomy_`:  Dichotomy distance (in world coordinates), the minimum allowable distance between nodes created in CreateNode
+- `world_coords world_goal_accessible_dist_`: Threshold for considering goal position to be accessible from current position
+- `world_coords world_expand_dist_`: Maximum distance to expand tree when adding new nodes
+- `double goal_frequency_`: Frequency of random node being set to goal node
+
+#### Global Costmap
+
+- `map_cost lethal_cost_`: Cost at which to consider a cell inaccessible
+
+## Quick Start Guide
+
+We have containerized our planner to make testing quick and easy. Due to the use of GUI applications, there may be some additional setup required beyond just [installing Docker](https://docs.docker.com/get-docker/).
+
+- Linux
+   - Allow X forwarding with `xhost +`
+- Windows 10/11
+   - We suggest using [Windows Subsystem for Linux](https://learn.microsoft.com/en-us/windows/wsl/install) and [WSLg](https://github.com/microsoft/wslg) for GUI support
+- MacOS
+  - Untested
+
+Once you have setup Docker and a way of displaying GUI applications running inside a Docker container, **check the sections below**.
+
+Next, build the docker image and run the container with the included bash script: [build_and_run_docker.bash](./build_and_run_docker.bash).
+
+Assuming nothing went wrong you should now be inside the husky image container.
+
+### Important Locations Inside Container
+
+There are two important locations inside the container - `/opt/ros/$ROS_DISTRO` where all the ros debian packages such as the husky stack are installed and `/root/catkin_ws` which contains any source packages.
+
+`/root/catkin_ws` contains a `scripts` and a `src` folder. The former contains scripts for running Gazebo, Rviz, and the planner in various modes. The latter contains the source code for the Fast-RRT* planner which was copied into the image from [docker/fast_rrt_ros](./docker/fast_rrt_ros/).
+
+Other than the planner source code, you can also modify the husky description and navigation launch files found in `/opt/ros/$ROS_DISTRO/share/husky_description` and `/opt/ros/$ROS_DISTRO/share/husky_navigation` respectively. Modified versions are included in the [docker](./docker/) folder and are copied into the image when the [Dockerfile](./docker/Dockerfile) is built by [build_and_run_docker.bash](./build_and_run_docker.bash).
+
+The [Fast-RRT* planner configuration file](./docker/fast_rrt_ros/config/planner.yaml) is copied into the appropriate location in the image as specified in the [Dockerfile](./docker/Dockerfile). This has parameters that control the behavior of the planner and should be adjusted before running the docker container.
+
+The [husky description](./docker/husky_description/urdf/husky.urdf.xacro) was modified from the default to enable the LiDAR sensor.
+
+The [husky navigation launch files](./docker/husky_navigation/) were modified to use our Fast-RRT* global planner instead of the default and to allow for debugging with valgrind if desired.
+
+### Running Simulation
+
+To actually run the sim, you will need at least three terminal sessions. feel free to use `tmux` or `screen` if you're comfortable with those utilities. Otherwise you can connect to the container using additional terminals and running `docker exec -it <container_id> bash`. Use `docker ps` to see running containers and their IDs.
+
+**NOTE:** The following scripts should be run from the `/root/catkin_ws/` directory inside the docker container, which you should be in by default (i.e., run `./scripts/<script_name>` and not `cd scripts && ./<script_name>`).
+
+#### Terminal 1: Gazebo
+
+In the first bash session, execute the first script: `./scripts/1_gazebo.bash`
+
+#### Terminal 2: RViz
+
+In the second bash session, execute the second script: `./scripts/2_rviz.bash`
+
+##### Suggested RViz Settings
+
+- Disable **Odometry**
+- Enable **Navigation**
+- Disable **Local Costmap**
+- Expand **Global Plan** by clicking on the arrow to the left
+   - After running the navigation node (see terminal 3), adjust **Topic**:
+      - Fast-RRT*: `/move_base/DWAPlannerROS/global_plan`
+      - Default: `/move_base/NavfnROS/plan`
+
+#### Terminal 3: Navigation
+
+In the third bash session, execute the third script corresponding to the option you want:
+
+- Fast-RRT*
+   - Release: `./scripts/3_fast_rrt_star.bash`
+   - Debug: `./scripts/3_fast_rrt_star_debug.bash`
+   - Debug with Valgrind: `./scripts/3_fast_rrt_star_debug_valgrind.bash`
+- `move_base` Default
+   - `./scripts/3_default.bash`
+
+#### Recording Activity
+
+A gazebo instance with a husky in it and an rviz instance should open. You should also check that the third terminal has not crashed before proceeding.
+
+If you wish to record your current activity you can use the command `rosbag record --topics <topic1> <topic2> <topic3> .....` to do so. You can also make a standardized bash script to make this easier.
+
+Once you've confirmed everything is working and started your bag, select the 2d nav goal in rviz and place a goal down where you'd like the robot to move. If your planner is working correctly you'll see a green line from the robot to the goal that doesn't intersect obstacles. If you don't see that happen then it is time to debug.
+
+## License
+
+This work is published under the MIT License. Please see [LICENSE](./LICENSE) for details.
diff --git a/build_and_run_docker.bash b/build_and_run_docker.bash
new file mode 100755
index 0000000..62e0b5b
--- /dev/null
+++ b/build_and_run_docker.bash
@@ -0,0 +1,32 @@
+#!/bin/bash
+
+# Linux X server setup
+XAUTH=/tmp/docker.xauth
+if [ ! -f $XAUTH ]
+then
+    xauth_list=$(xauth nlist :0 | sed -e 's/^..../ffff/')
+    if [ ! -z "$xauth_list" ]
+    then
+        echo $xauth_list | xauth -f $XAUTH nmerge -
+    else
+        touch $XAUTH
+    fi
+    chmod a+r $XAUTH
+fi
+
+# Build husky_image from Dockerfile in this directory
+docker build -t husky_image ./docker/
+
+# Run husky_image with specified environment variables, volume mounting, and host networking
+docker run \
+    -it \
+    --env="DISPLAY=$DISPLAY" \
+    --env="LIBGL_ALWAYS_INDIRECT=$LIBGL_ALWAYS_INDIRECT" \
+    --env="QT_X11_NO_MITSHM=1" \
+    --env="XAUTHORITY=$XAUTH" \
+    --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \
+    --volume="$XAUTH:$XAUTH" \
+    --privileged \
+    --network=host \
+    husky_image \
+    bash
\ No newline at end of file
diff --git a/docker/CMakeLists.txt b/docker/CMakeLists.txt
new file mode 100644
index 0000000..5bd85e7
--- /dev/null
+++ b/docker/CMakeLists.txt
@@ -0,0 +1,69 @@
+# toplevel CMakeLists.txt for a catkin workspace
+# catkin/cmake/toplevel.cmake
+
+cmake_minimum_required(VERSION 3.0.2)
+
+project(Project)
+
+set(CATKIN_TOPLEVEL TRUE)
+
+# search for catkin within the workspace
+set(_cmd "catkin_find_pkg" "catkin" "${CMAKE_SOURCE_DIR}")
+execute_process(COMMAND ${_cmd}
+  RESULT_VARIABLE _res
+  OUTPUT_VARIABLE _out
+  ERROR_VARIABLE _err
+  OUTPUT_STRIP_TRAILING_WHITESPACE
+  ERROR_STRIP_TRAILING_WHITESPACE
+)
+if(NOT _res EQUAL 0 AND NOT _res EQUAL 2)
+  # searching fot catkin resulted in an error
+  string(REPLACE ";" " " _cmd_str "${_cmd}")
+  message(FATAL_ERROR "Search for 'catkin' in workspace failed (${_cmd_str}): ${_err}")
+endif()
+
+# include catkin from workspace or via find_package()
+if(_res EQUAL 0)
+  set(catkin_EXTRAS_DIR "${CMAKE_SOURCE_DIR}/${_out}/cmake")
+  # include all.cmake without add_subdirectory to let it operate in same scope
+  include(${catkin_EXTRAS_DIR}/all.cmake NO_POLICY_SCOPE)
+  add_subdirectory("${_out}")
+
+else()
+  # use either CMAKE_PREFIX_PATH explicitly passed to CMake as a command line argument
+  # or CMAKE_PREFIX_PATH from the environment
+  if(NOT DEFINED CMAKE_PREFIX_PATH)
+    if(NOT "$ENV{CMAKE_PREFIX_PATH}" STREQUAL "")
+      if(NOT WIN32)
+        string(REPLACE ":" ";" CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH})
+      else()
+        set(CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH})
+      endif()
+    endif()
+  endif()
+
+  # list of catkin workspaces
+  set(catkin_search_path "")
+  foreach(path ${CMAKE_PREFIX_PATH})
+    if(EXISTS "${path}/.catkin")
+      list(FIND catkin_search_path ${path} _index)
+      if(_index EQUAL -1)
+        list(APPEND catkin_search_path ${path})
+      endif()
+    endif()
+  endforeach()
+
+  # search for catkin in all workspaces
+  set(CATKIN_TOPLEVEL_FIND_PACKAGE TRUE)
+  find_package(catkin QUIET
+    NO_POLICY_SCOPE
+    PATHS ${catkin_search_path}
+    NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH)
+  unset(CATKIN_TOPLEVEL_FIND_PACKAGE)
+
+  if(NOT catkin_FOUND)
+    message(FATAL_ERROR "find_package(catkin) failed. catkin was neither found in the workspace nor in the CMAKE_PREFIX_PATH. One reason may be that no ROS setup.sh was sourced before.")
+  endif()
+endif()
+
+catkin_workspace()
\ No newline at end of file
diff --git a/docker/Dockerfile b/docker/Dockerfile
new file mode 100644
index 0000000..b1d1f54
--- /dev/null
+++ b/docker/Dockerfile
@@ -0,0 +1,45 @@
+ARG ROS_DISTRO=noetic
+FROM ros:$ROS_DISTRO-ros-base
+ARG DEBIAN_FRONTEND=noninteractive
+
+# Install ros desktop, husky dependencies, fast_rrt_ros dependencies, and valgrind for debugging
+RUN apt-get update && \
+    apt-get upgrade -y && \
+    apt-get install -y \
+    ros-$ROS_DISTRO-desktop-full \
+    ros-$ROS_DISTRO-husky-desktop \
+    ros-$ROS_DISTRO-husky-simulator \
+    ros-$ROS_DISTRO-husky-navigation \
+    ros-$ROS_DISTRO-costmap-2d \
+    ros-$ROS_DISTRO-nav-core \
+    ros-$ROS_DISTRO-base-local-planner \
+    valgrind
+
+# Copy launch files and husky description to appropriate locations
+ARG HUSKY_NAV_LAUNCH=husky_navigation/launch
+ARG HUSKY_DES_URDF=husky_description/urdf
+COPY ./$HUSKY_NAV_LAUNCH/move_base_fast_rrt_star.launch /opt/ros/$ROS_DISTRO/share/$HUSKY_NAV_LAUNCH/move_base_fast_rrt_star.launch
+COPY ./$HUSKY_NAV_LAUNCH/move_base_fast_rrt_star_debug.launch /opt/ros/$ROS_DISTRO/share/$HUSKY_NAV_LAUNCH/move_base_fast_rrt_star_debug.launch
+COPY ./$HUSKY_NAV_LAUNCH/move_base_fast_rrt_star_mapless_demo.launch /opt/ros/$ROS_DISTRO/share/$HUSKY_NAV_LAUNCH/move_base_fast_rrt_star_mapless_demo.launch
+COPY ./$HUSKY_NAV_LAUNCH/move_base_fast_rrt_star_mapless_demo_debug.launch /opt/ros/$ROS_DISTRO/share/$HUSKY_NAV_LAUNCH/move_base_fast_rrt_star_mapless_demo_debug.launch
+COPY ./$HUSKY_DES_URDF/husky.urdf.xacro /opt/ros/$ROS_DISTRO/share/$HUSKY_DES_URDF/husky.urdf.xacro
+
+# Declare some environment variables
+ENV ROS_WORKSPACE=/root/catkin_ws
+ENV ROS_PACKAGE_PATH=/root/catkin_ws/src:/opt/ros/$ROS_DISTRO/share
+
+# Add setup.bash sourcing to bashrc
+RUN echo "source /opt/ros/$ROS_DISTRO/setup.bash" >> /etc/bash.bashrc
+
+# Place Fast-RRT* in catkin workspace
+COPY ./fast_rrt_ros/ /root/catkin_ws/src/fast_rrt_ros
+COPY ./CMakeLists.txt /root/catkin_ws/src/CMakeLists.txt
+
+# Copy scripts to image
+COPY ./scripts/ /root/catkin_ws/scripts/
+
+# Copy planner configuration
+COPY ./fast_rrt_ros/config/planner.yaml /opt/ros/$ROS_DISTRO/share/husky_navigation/config/planner.yaml
+
+# Set working directory
+WORKDIR /root/catkin_ws/
\ No newline at end of file
diff --git a/docker/fast_rrt_ros/CMakeLists.txt b/docker/fast_rrt_ros/CMakeLists.txt
new file mode 100644
index 0000000..45c2b06
--- /dev/null
+++ b/docker/fast_rrt_ros/CMakeLists.txt
@@ -0,0 +1,24 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(fast_rrt_ros)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+  costmap_2d
+  nav_core
+  base_local_planner
+)
+
+catkin_package(
+  INCLUDE_DIRS include
+  CATKIN_DEPENDS roscpp rospy std_msgs costmap_2d nav_core base_local_planner
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_library(fast_rrt_star_planner_lib src/fast_rrt_star_planner.cpp src/fast_rrt_star_node.cpp src/fast_rrt_star_helper.cpp)
+target_link_libraries(fast_rrt_star_planner_lib ${catkin_LIBRARIES})
diff --git a/docker/fast_rrt_ros/config/planner.yaml b/docker/fast_rrt_ros/config/planner.yaml
new file mode 100644
index 0000000..6f430ec
--- /dev/null
+++ b/docker/fast_rrt_ros/config/planner.yaml
@@ -0,0 +1,103 @@
+controller_frequency: 5.0
+recovery_behavior_enabled: true
+
+NavfnROS:
+  allow_unknown: true # Specifies whether or not to allow navfn to create plans that traverse unknown space.
+  default_tolerance: 0.1 # A tolerance on the goal point for the planner.
+
+TrajectoryPlannerROS:
+  # Robot Configuration Parameters
+  acc_lim_x: 2.5
+  acc_lim_theta: 3.2
+
+  max_vel_x: 1.0
+  min_vel_x: 0.0
+
+  max_vel_theta: 1.0
+  min_vel_theta: -1.0
+  min_in_place_vel_theta: 0.2
+
+  holonomic_robot: false
+  escape_vel: -0.1
+
+  # Goal Tolerance Parameters
+  yaw_goal_tolerance: 0.1
+  xy_goal_tolerance: 0.2
+  latch_xy_goal_tolerance: false
+
+  # Forward Simulation Parameters
+  sim_time: 2.0
+  sim_granularity: 0.02
+  angular_sim_granularity: 0.02
+  vx_samples: 6
+  vtheta_samples: 20
+  controller_frequency: 20.0
+
+  # Trajectory scoring parameters
+  meter_scoring: true # Whether the gdist_scale and pdist_scale parameters should assume that goal_distance and path_distance are expressed in units of meters or cells. Cells are assumed by default (false).
+  occdist_scale: 0.1 #The weighting for how much the controller should attempt to avoid obstacles. default 0.01
+  pdist_scale: 0.75 #     The weighting for how much the controller should stay close to the path it was given . default 0.6
+  gdist_scale: 1.0 #     The weighting for how much the controller should attempt to reach its local goal, also controls speed  default 0.8
+
+  heading_lookahead: 0.325 #How far to look ahead in meters when scoring different in-place-rotation trajectories
+  heading_scoring: false #Whether to score based on the robot's heading to the path or its distance from the path. default false
+  heading_scoring_timestep: 0.8 #How far to look ahead in time in seconds along the simulated trajectory when using heading scoring (double, default: 0.8)
+  dwa: true #Whether to use the Dynamic Window Approach (DWA)_ or whether to use Trajectory Rollout
+  simple_attractor: false
+  publish_cost_grid_pc: true
+
+  # Oscillation Prevention Parameters
+  oscillation_reset_dist: 0.25 #How far the robot must travel in meters before oscillation flags are reset (double, default: 0.05)
+  escape_reset_dist: 0.1
+  escape_reset_theta: 0.1
+
+DWAPlannerROS:
+  # Robot configuration parameters
+  acc_lim_x: 2.5
+  acc_lim_y: 0
+  acc_lim_th: 3.2
+
+  max_vel_x: 0.5
+  min_vel_x: 0.0
+  max_vel_y: 0
+  min_vel_y: 0
+
+  max_trans_vel: 0.5
+  min_trans_vel: 0.1
+  max_rot_vel: 1.0
+  min_rot_vel: 0.2
+
+  # Goal Tolerance Parameters
+  yaw_goal_tolerance: 0.1
+  xy_goal_tolerance: 0.2
+  latch_xy_goal_tolerance: false
+
+  # # Forward Simulation Parameters
+  # sim_time: 2.0
+  # sim_granularity: 0.02
+  # vx_samples: 6
+  # vy_samples: 0
+  # vtheta_samples: 20
+  # penalize_negative_x: true
+
+  # # Trajectory scoring parameters
+  # path_distance_bias: 32.0 # The weighting for how much the controller should stay close to the path it was given
+  # goal_distance_bias: 24.0 # The weighting for how much the controller should attempt to reach its local goal, also controls speed
+  # occdist_scale: 0.01 # The weighting for how much the controller should attempt to avoid obstacles
+  # forward_point_distance: 0.325 # The distance from the center point of the robot to place an additional scoring point, in meters
+  # stop_time_buffer: 0.2  # The amount of time that the robot must stThe absolute value of the veolicty at which to start scaling the robot's footprint, in m/sop before a collision in order for a trajectory to be considered valid in seconds
+  # scaling_speed: 0.25 # The absolute value of the veolicty at which to start scaling the robot's footprint, in m/s
+  # max_scaling_factor: 0.2 # The maximum factor to scale the robot's footprint by
+
+  # # Oscillation Prevention Parameters
+  # oscillation_reset_dist: 0.25 #How far the robot must travel in meters before oscillation flags are reset (double, default: 0.05)
+
+Fast_RRTStarPlanner:
+  max_num_iter: 10000 # Integer: Maximum number of iterations
+  min_num_iter: 200 # Integer: Minimum number of iterations
+  world_near_radius: 2.0 # Double (meters): Search radius (in world coordinates)
+  world_dist_dichotomy: 0.05 # Double (meters): Dichotomy distance (in world coordinates), the minimum allowable distance between nodes created in CreateNode
+  world_expand_dist: 5.0 # Double (meters): Maximum distance to expand tree when adding new nodes
+  world_goal_accessible_dist: 0.25 # Double (meters): Threshold for considering goal position to be accessible from current position
+  goal_frequency: 0.2 # Double (meters): Frequency of random node being set to goal node
+  lethal_cost: 128 # Integer (between 0 and 255 inclusive): Cost at which to consider a cell inaccessible (greater than or equal to this value), FREE_SPACE = 0, INSCRIBED_INFLATED_OBSTACLE = 253, LETHAL_OBSTACLE = 254, NO_INFORMATION = 255
diff --git a/docker/fast_rrt_ros/docs/Capture1.PNG b/docker/fast_rrt_ros/docs/Capture1.PNG
new file mode 100644
index 0000000..e3bdbfe
Binary files /dev/null and b/docker/fast_rrt_ros/docs/Capture1.PNG differ
diff --git a/docker/fast_rrt_ros/docs/Capture2.PNG b/docker/fast_rrt_ros/docs/Capture2.PNG
new file mode 100644
index 0000000..74da595
Binary files /dev/null and b/docker/fast_rrt_ros/docs/Capture2.PNG differ
diff --git a/docker/fast_rrt_ros/docs/Capture3.PNG b/docker/fast_rrt_ros/docs/Capture3.PNG
new file mode 100644
index 0000000..feb895d
Binary files /dev/null and b/docker/fast_rrt_ros/docs/Capture3.PNG differ
diff --git a/docker/fast_rrt_ros/docs/Capture4.PNG b/docker/fast_rrt_ros/docs/Capture4.PNG
new file mode 100644
index 0000000..d2f2d4e
Binary files /dev/null and b/docker/fast_rrt_ros/docs/Capture4.PNG differ
diff --git a/docker/fast_rrt_ros/docs/Capture5.PNG b/docker/fast_rrt_ros/docs/Capture5.PNG
new file mode 100644
index 0000000..fc869e5
Binary files /dev/null and b/docker/fast_rrt_ros/docs/Capture5.PNG differ
diff --git a/docker/fast_rrt_ros/docs/Capture6.PNG b/docker/fast_rrt_ros/docs/Capture6.PNG
new file mode 100644
index 0000000..bd3e16b
Binary files /dev/null and b/docker/fast_rrt_ros/docs/Capture6.PNG differ
diff --git a/docker/fast_rrt_ros/docs/Capture7.PNG b/docker/fast_rrt_ros/docs/Capture7.PNG
new file mode 100644
index 0000000..39fbc99
Binary files /dev/null and b/docker/fast_rrt_ros/docs/Capture7.PNG differ
diff --git a/docker/fast_rrt_ros/docs/Fast_RRTdocumentation.md b/docker/fast_rrt_ros/docs/Fast_RRTdocumentation.md
new file mode 100644
index 0000000..43c721d
--- /dev/null
+++ b/docker/fast_rrt_ros/docs/Fast_RRTdocumentation.md
@@ -0,0 +1,97 @@
+# Fast-RRT* Documentation
+
+- [Fast-RRT* Documentation](#Fast-RRT-documentation)
+  - [`FindReachest()`](#findreachest)
+    - [Psuedo Code](#psuedo-code)
+    - [Written Explanation](#written-explanation)
+    - [Visual](#visual)
+  - [`CreateNode()`](#createnode)
+    - [Psuedo Code](#psuedo-code-1)
+    - [Written Explanation](#written-explanation-1)
+    - [Visual](#visual-1)
+  - [`Rewire()`](#rewire)
+    - [Written Explanation](#written-explanation-2)
+    - [Visual](#visual-2)
+  - [`SampleFree()`](#samplefree)
+  - [`Nearest()`](#nearest)
+  - [`Near()`](#near)
+
+## `FindReachest()`
+
+### Psuedo Code
+
+<img src="findReachest.PNG" width="300">
+
+### Written Explanation
+
+Find the furthest possible node of `xNearest`'s parents from `xRand` that does not have an obstacle when a straight line is drawn between them.
+
+The selected node `xReachest` must be chosen from `xNearest`'s ancestors. The starting node may not be selected as `xReachest`.
+
+### Visual
+
+<img src="findReachestSketch.PNG" width="500">
+
+## `CreateNode()`
+
+### Psuedo Code
+
+<img src="createNode.PNG" width="300" >
+
+### Written Explanation
+
+Creates a node (`xCreate`) based on `xRand`, `xReachest`, and parent (`xReachest`) locations such that the path is drawn more near to the obstacle in few steps. Remember that these two nodes are guaranteed to have an obstacle between them.
+
+The node is created between `xRand` and `xReachest` where the path between `xRand` and `xCreate` becomes free of all obstacles.
+
+### Visual
+
+<figure>
+  <img src="Capture1.PNG" width="380" >
+  <img src="Capture2.PNG" width="380" >
+  <figcaption> 1) xAllow and xForbid are set </figcaption>
+</figure>
+
+<figure>
+  <img src="Capture3.PNG" width="380" >
+  <figcaption> 2) xMid is established between xAllow and xForbid when the path is collision free to xRand</figcaption>
+</figure>
+
+<figure>
+  <img src="Capture4.PNG" width="380" >
+  <img src="Capture5.PNG" width="380" >
+  <figcaption> 3) xMid is the new xAllow and xRand is the new xForbid</figcaption>
+</figure>
+
+<figure>
+  <img src="Capture6.PNG" width="380" >
+  <figcaption> 4) a new xMid is established between xAllow and xForbid </figcaption>
+</figure>
+
+<figure>
+  <img src="Capture7.PNG" width="380" >
+  <figcaption> 4) a new xMid is established between xAllow and xForbid when collision free to parent(xReachest) </figcaption>
+</figure>
+
+## `Rewire()`
+
+### Written Explanation
+
+Changes connection relationships of tree vertices.
+For any point in `xNear`, if replacing its parent node to be `xRand` reduces the cost of the path from `xStart` to `xNear`, then replace it.
+
+### Visual
+
+<img src="rewireSketch.PNG" width="150" >
+
+## `SampleFree()`
+
+Returns a sample `xRand` that is selected randomly from xFree.
+
+## `Nearest()`
+
+Nearest vector to `xRand` in terms of Euclidean distance.
+
+## `Near()`
+
+Return the set of vectors contained in specific radius around `xRand`.
diff --git a/docker/fast_rrt_ros/docs/createNode.PNG b/docker/fast_rrt_ros/docs/createNode.PNG
new file mode 100644
index 0000000..18ee434
Binary files /dev/null and b/docker/fast_rrt_ros/docs/createNode.PNG differ
diff --git a/docker/fast_rrt_ros/docs/fast_rrt.PNG b/docker/fast_rrt_ros/docs/fast_rrt.PNG
new file mode 100644
index 0000000..9a83bfa
Binary files /dev/null and b/docker/fast_rrt_ros/docs/fast_rrt.PNG differ
diff --git a/docker/fast_rrt_ros/docs/findReachest.PNG b/docker/fast_rrt_ros/docs/findReachest.PNG
new file mode 100644
index 0000000..327bce8
Binary files /dev/null and b/docker/fast_rrt_ros/docs/findReachest.PNG differ
diff --git a/docker/fast_rrt_ros/docs/findReachestSketch.PNG b/docker/fast_rrt_ros/docs/findReachestSketch.PNG
new file mode 100644
index 0000000..d3c470b
Binary files /dev/null and b/docker/fast_rrt_ros/docs/findReachestSketch.PNG differ
diff --git a/docker/fast_rrt_ros/docs/move_base.png b/docker/fast_rrt_ros/docs/move_base.png
new file mode 100644
index 0000000..ad3139a
Binary files /dev/null and b/docker/fast_rrt_ros/docs/move_base.png differ
diff --git a/docker/fast_rrt_ros/docs/rewireSketch.PNG b/docker/fast_rrt_ros/docs/rewireSketch.PNG
new file mode 100644
index 0000000..bcf1ae5
Binary files /dev/null and b/docker/fast_rrt_ros/docs/rewireSketch.PNG differ
diff --git a/docker/fast_rrt_ros/fast_rrt_star_planner_plugin.xml b/docker/fast_rrt_ros/fast_rrt_star_planner_plugin.xml
new file mode 100644
index 0000000..3f29e99
--- /dev/null
+++ b/docker/fast_rrt_ros/fast_rrt_star_planner_plugin.xml
@@ -0,0 +1,5 @@
+<library path="lib/libfast_rrt_star_planner_lib">
+	<class name="fast_rrt_star_planner/Fast_RRTStarPlanner" type="fast_rrt_star_planner::Fast_RRTStarPlanner" base_class_type="nav_core::BaseGlobalPlanner">
+		<description>This is a global planner plugin that uses the Fast-RRT* algorithm.</description>
+	</class>
+</library>
diff --git a/docker/fast_rrt_ros/include/fast_rrt_star_helper.h b/docker/fast_rrt_ros/include/fast_rrt_star_helper.h
new file mode 100644
index 0000000..bd72ab5
--- /dev/null
+++ b/docker/fast_rrt_ros/include/fast_rrt_star_helper.h
@@ -0,0 +1,130 @@
+#ifndef FAST_RRT_STAR_HELPER_H
+#define FAST_RRT_STAR_HELPER_H
+
+#include "fast_rrt_star_node.h"
+
+#include <costmap_2d/costmap_2d.h>
+#include <costmap_2d/costmap_2d_ros.h>
+#include <base_local_planner/footprint_helper.h>
+#include <vector>
+
+/**
+ * Create type aliases for map coordinates and cost
+ */
+using map_coords = unsigned int;
+using map_cost = unsigned char;
+
+namespace fast_rrt_star_planner
+{
+  /**
+   * @brief Class with helper functions for Fast_RRTStarPlanner and Fast_RRTStarNode classes
+   */
+  class Fast_RRTStarHelper
+  {
+  public:
+    /**
+     * @brief Find euclidean distance between two world coordinate points 
+     * 
+     * @param x0 First position x axis coordinate
+     * @param y0 First position y axis coordinate
+     * @param x1 Second position x axis coordinate
+     * @param y1 Second position y axis coordinate
+     * @return world_coords Euclidean distance between (x0,y0) and (x1,y1)
+     */
+    static world_coords distance(world_coords x0, world_coords y0, world_coords x1, world_coords y1);
+
+    /**
+     * @brief Find euclidean distance between two nodes
+     *
+     * @param node0 First node
+     * @param node1 Second node
+     * @return world_coords Euclidean distance between node0 and node1
+     */
+    static world_coords distance(Fast_RRTStarNode *node0, Fast_RRTStarNode *node1);
+
+    /**
+     * @brief Find angle of line from node0 to node1
+     *        relative to 0 radians being to the right
+     *        (i.e., just like a unit circle in XY plane)
+     *
+     * @param node0 First node
+     * @param node1 Second node
+     * @return double Angle of line from node0 to node1 in radians
+     */
+    static double angle(Fast_RRTStarNode *node0, Fast_RRTStarNode *node1);
+
+    /**
+     * @brief Checks if line between two world coordinates is collision free 
+     *
+     * @param costmap Map of environment
+     * @param lethal_cost Cost map value that corresponds to a region that is inaccessible
+     * @param wx0 X component of first point in world coordinates
+     * @param wy0 Y component of first point in world coordinates
+     * @param wx1 X component of second point in world coordinates
+     * @param wy1 Y coponent of second point in world coordinates
+     * @return true If line from (wx0,wy0) to (wx1,wy1) doesn't have collisions
+     * @return false If line from (wx0,wy0) to (wx1,wy1) does have collisions
+     */
+    static bool isLineCollisionFree(costmap_2d::Costmap2D *costmap, map_cost lethal_cost, world_coords wx0, world_coords wy0, world_coords wx1, world_coords wy1);
+
+    /**
+     * @brief Checks if map coordinate point is collision free
+     * 
+     * @param costmap Map of environment
+     * @param lethal_cost Cost map value that corresponds to a region that is inaccessible
+     * @param mx X component of point in map coordinates
+     * @param my Y component of point in map coordinates
+     * @return true If point has not collisions
+     * @return false If point does have collisions
+     */
+    static bool isPointCollisionFree(costmap_2d::Costmap2D *costmap, map_cost lethal_cost, map_coords mx, map_coords my);
+
+    /**
+     * @brief Discretizes continuous line into grid cells and returns a vector of base_local_planner::Position2DInt.
+     * 
+     * @param costmap Map of environment
+     * @param start_node Starting node
+     * @param end_node Ending node
+     * @return std::vector<base_local_planner::Position2DInt> Vector of costmap cells that ray traced line passes through
+     */
+    static std::vector<base_local_planner::Position2DInt> Bresenham(costmap_2d::Costmap2D *costmap, Fast_RRTStarNode *start_node, Fast_RRTStarNode *end_node);
+    
+    /**
+     * @brief Discretizes continuous line into grid cells and returns a vector of base_local_planner::Position2DInt.
+     * 
+     * @param costmap Map of environment
+     * @param wx0 X component of first point in world coordinates
+     * @param wy0 Y component of first point in world coordinates
+     * @param wx1 X component of second point in world coordinates
+     * @param wy1 Y component of second point in world coordinates
+     * @return std::vector<base_local_planner::Position2DInt> Vector of costmap cells that ray traced line passes through
+     */
+    static std::vector<base_local_planner::Position2DInt> Bresenham(costmap_2d::Costmap2D *costmap, world_coords wx0, world_coords wy0, world_coords wx1, world_coords wy1);
+    
+    /**
+     * @brief Discretizes continuous line into grid cells and returns a vector of base_local_planner::Position2DInt.
+     * 
+     * @param x0 X component of first point in map coordinates
+     * @param y0 Y component of first point in map coordinates
+     * @param x1 X component of second point in map coordinates
+     * @param y1 Y component of second point in map coordinates
+     * @return std::vector<base_local_planner::Position2DInt> Vector of costmap cells that ray traced line passes through
+     */
+    static std::vector<base_local_planner::Position2DInt> Bresenham(map_coords x0, map_coords y0, map_coords x1, map_coords y1);
+
+    /**
+     * @brief Checks if path between two nodes is collision free when accounting for robot footprint and orientation 
+     *
+     * @param costmap Map of environment
+     * @param costmap_ros ROS version of map of environment
+     * @param lethal_cost Cost map value that corresponds to a region that is inaccessible
+     * @param node0 First node
+     * @param node1 Second node
+     * @return true If path has no collisions with footprint of robot
+     * @return false If path does have collisions with footprint of robot
+     */
+    static bool isFootprintPathCollisionFree(costmap_2d::Costmap2D *costmap, costmap_2d::Costmap2DROS *costmap_ros, map_cost lethal_cost, Fast_RRTStarNode *node0, Fast_RRTStarNode *node1);
+  };
+}
+
+#endif /* FAST_RRT_STAR_HELPER_H */
\ No newline at end of file
diff --git a/docker/fast_rrt_ros/include/fast_rrt_star_node.h b/docker/fast_rrt_ros/include/fast_rrt_star_node.h
new file mode 100644
index 0000000..8dcbe61
--- /dev/null
+++ b/docker/fast_rrt_ros/include/fast_rrt_star_node.h
@@ -0,0 +1,97 @@
+#ifndef FAST_RRT_STAR_NODE_H
+#define FAST_RRT_STAR_NODE_H
+
+/**
+ * Create type alias for world coordinates
+ */
+using world_coords = double;
+
+namespace fast_rrt_star_planner
+{
+  /**
+   * @brief Fast_RRTStarNode class used to track nodes of path planning tree.
+   */
+  class Fast_RRTStarNode
+  {
+  public:
+    /**
+     * @brief Public Variables
+     */
+    world_coords x;         ///< x coordinate of node
+    world_coords y;         ///< y coordinate of node
+    world_coords path_cost; ///< Cumulative cost from node_start to current node
+    Fast_RRTStarNode *parent;   ///< Pointer to parent of current node
+
+    /**
+     * @brief Construct a new Fast_RRTStarNode object with default values
+     */
+    Fast_RRTStarNode();
+
+    /**
+     * @brief Construct a new Fast_RRTStarNode object with given position
+     * 
+     * @param x_in X component of node in world coordinates
+     * @param y_in Y component of node in world coordinates
+     */
+    Fast_RRTStarNode(world_coords x_in, world_coords y_in);
+
+    /**
+     * @brief Construct a new Fast_RRTStarNode object as a deep copy of pointer to input node
+     * 
+     * @param node Pointer to input node to deep copy
+     */
+    Fast_RRTStarNode(Fast_RRTStarNode *node);
+
+    /**
+     * @brief Construct a new Fast_RRTStarNode object as a deep copy of constant pointer to constant input node
+     * 
+     * @param node Constant pointer to constant input node to deep copy
+     */
+    Fast_RRTStarNode(const Fast_RRTStarNode *const node);
+
+    /** 
+     * @brief "Connects" node to node_parent by updating parent pointer and path_cost
+     *
+     * @param node_parent Desired node being assigned as parent
+     */
+    void updateParentAndCost(Fast_RRTStarNode *node_parent);
+
+    /** 
+     * @brief "Connects" this node to node_parent by updating parent pointer and path_cost
+     *        without computing distance between node and node_parent internally,
+     *        just uses the provided cost (reason: saves re-computation)
+     *  
+     * @param node_parent Desired node being assigned as parent
+     * @param path_cost Path cost (i.e. distance) between current node and node_parent
+     */
+    void updateParentAndCost(Fast_RRTStarNode *node_parent, world_coords path_cost);
+
+    /** 
+     * @brief Compute cost of connecting this node to node_connect (hypothetical parent) 
+     *
+     * @param node_connect Node chosen to calculate cost to
+     * @return world_coords Cost of path between the two nodes
+     */
+    world_coords getConnectionCost(Fast_RRTStarNode *node_connect);
+
+    /**
+     * @brief Overloaded equality operator to compare two Fast_RRTStarNodes
+     * 
+     * @param node Node to compare to current node for equality
+     * @return true If properties of current and input node are the same
+     * @return false If properties of current and input node are not the same
+     */
+    bool operator==(const Fast_RRTStarNode node);
+
+    /**
+     * @brief Overloaded inequality operator to compare two Fast_RRTStarNodes
+     * 
+     * @param node Node to compare to current node for inequality
+     * @return true If properties of current and input node are not the same
+     * @return false If properties of current and input node are the same
+     */
+    bool operator!=(const Fast_RRTStarNode node);
+  };
+}
+
+#endif /* FAST_RRT_STAR_NODE_H */
\ No newline at end of file
diff --git a/docker/fast_rrt_ros/include/fast_rrt_star_planner.h b/docker/fast_rrt_ros/include/fast_rrt_star_planner.h
new file mode 100644
index 0000000..b0dd050
--- /dev/null
+++ b/docker/fast_rrt_ros/include/fast_rrt_star_planner.h
@@ -0,0 +1,206 @@
+/**
+ * @brief This implementation largely follows what is described in ROS documentation:
+ *        http://wiki.ros.org/navigation/Tutorials/Writing%20A%20Global%20Path%20Planner%20As%20Plugin%20in%20ROS
+ */
+#ifndef FAST_RRT_STAR_PLANNER_H
+#define FAST_RRT_STAR_PLANNER_H
+
+#include "fast_rrt_star_helper.h"
+
+#include <costmap_2d/costmap_2d_ros.h>
+#include <costmap_2d/costmap_2d.h>
+#include <nav_core/base_global_planner.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <nav_msgs/Path.h>
+#include <angles/angles.h>
+#include <tf/tf.h>
+#include <base_local_planner/world_model.h>
+#include <base_local_planner/costmap_model.h>
+
+using std::string;
+using pose_value_type = double;
+
+namespace fast_rrt_star_planner
+{
+  /**
+   * Main class for Fast-RRT* algorithm that extends move_base's BaseGlobalPlanner.
+   * Takes a starting location, goal location, and map of environment as input
+   * and generates a set of poses for local planner to follow to get from
+   * start location to goal location.
+   *        
+   * NOTE on units and variable names:
+   * - map or m: Map coordinates (i.e., cell index)
+   * - world or w: World coordinates (i.e., meters)
+   */
+  class Fast_RRTStarPlanner : public nav_core::BaseGlobalPlanner
+  {
+  private:
+    /**
+     * @brief Private Variables
+     */
+    bool initialized_;                                ///< Flag indicating if planner is initialized
+    bool path_found_;                                 ///< Flag indicating if planner found a path
+    world_coords best_path_cost_;                     ///< Track the shortest path distance found so far
+    Fast_RRTStarNode *node_best_path_end_;                ///< Pointer to last node in best path
+    base_local_planner::CostmapModel *costmap_model_; ///< Used by planner to check for collisions
+    costmap_2d::Costmap2DROS *costmap_ros_;           ///< ROS 2D Costmap
+    costmap_2d::Costmap2D *costmap_;                  ///< 2D Costmap
+    std::vector<Fast_RRTStarNode *> node_vec_;            ///< Vector of planning nodes pointers
+    world_coords resolution_;                         ///< Costmap resolution (size of grid cell in world coordinates)
+    world_coords origin_x_;                           ///< x coordinate of costmap origin
+    world_coords origin_y_;                           ///< y coordinate of costmap origin
+    world_coords size_x_;                             ///< Size of costmap's x axis in world coordinates
+    world_coords size_y_;                             ///< Size of costmap's y axis in world coordinates
+
+    /**
+     * @brief Tunable Parameters
+     */
+    int max_num_iter_;                        ///< Maximum number of iterations
+    int min_num_iter_;                        ///< Minimum number of iterations
+    world_coords world_near_radius_;          ///< Search radius (in world coordinates)
+    world_coords world_dist_dichotomy_;       ///< Dichotomy distance (in world coordinates), the minimum
+                                              ///< allowable distance between nodes created in CreateNode
+    world_coords world_goal_accessible_dist_; ///< Threshold for considering goal position to be
+                                              ///< accessible from current position
+    world_coords world_expand_dist_;          ///< Maximum distance to expand tree when adding new nodes
+    double goal_frequency_;                   ///< Frequency of random node being set to goal node
+    map_cost lethal_cost_;                    ///< Cost at which to consider a cell inaccessible
+
+    /**
+     * @brief Randomly samples free space in costmap to generate a new node
+     * 
+     * @param node_goal Fixed goal node
+     * @return Fast_RRTStarNode* Pointer to randomly sampled node
+     */
+    Fast_RRTStarNode *sampleFree(const Fast_RRTStarNode *const node_goal);
+
+    /**
+     * @brief Find the index of node in existing tree that is closest to node_rand
+     * 
+     * @param node_rand Randomly sampled node
+     * @return size_t Index of node in existing tree that is nearest to node_rand 
+     */
+    size_t nearest(Fast_RRTStarNode *node_rand);
+
+    /**
+     * @brief Find all nodes (as indices in near_vec_) in existing tree
+     *        that are within world_near_radius_ of random node
+     * 
+     * @param node_rand Randomly sampled node
+     * @return std::vector<size_t> Vector of node indices that are within world_near_radius_ of node_rand
+     */
+    std::vector<size_t> near(Fast_RRTStarNode *node_rand);
+
+    /**
+     * @brief Find furthest possible away node from node_rand that is collision free.
+     *        Only search through ancestors of node_nearest.
+     * 
+     * @param node_nearest Nearest node in distance to randomly sampled node node_rand
+     * @param node_rand Randomly sampled node
+     * @return Fast_RRTStarNode* Pointer to node_reachest
+     */
+    Fast_RRTStarNode *findReachest(Fast_RRTStarNode *node_nearest, Fast_RRTStarNode *node_rand);
+
+    /**
+     * @brief Create a new node to connect node_rand to node_nearest using
+     *        world_dist_dichotomy_, node_reachest, node_rand, and costmap_.
+     * 
+     * @param node_reachest Eldest ancestor node of node_rand that is collision free to node_rand
+     * @param node_rand Randomly sampled node
+     * @return Fast_RRTStarNode* Pointer to node_create
+     */
+    Fast_RRTStarNode *createNode(Fast_RRTStarNode *node_reachest, Fast_RRTStarNode *node_rand);
+
+    /**
+     * @brief Check if latest node added to node_vec_ is within threshold
+     *        distance of goal position
+     * 
+     * @param node_goal Fixed goal node
+     * @return true if tree is close enough to node_goal
+     * @return false if tree is not close enough to node_goal
+     */
+    bool initialPathFound(const Fast_RRTStarNode *const node_goal);
+
+    /**
+     * @brief Changes the connection relationships of tree vertices.
+     *        If replacing the parent node of node_near by node_rand reduces path from start to node_near, then do so.
+     *        Returns the re-ordered node vector
+     * 
+     * @param node_rand Randomly sampled node
+     * @param near_nodes_list List of nodes within specified distance limit of node_rand
+     */
+    void rewire(Fast_RRTStarNode *node_rand, const std::vector<size_t> &near_nodes_list);
+
+    /**
+     * @brief Construct the vector of stamped poses from linked list of
+     *        Fast-RRT* node pointers
+     * 
+     * @param start Time stamped pose of start position
+     * @param goal Time stamped pose of goal position
+     * @param plan List of stamped poses, starting with start node and ending with goal
+     * @param node_goal Fixed goal node
+     */
+    void constructPlan(const geometry_msgs::PoseStamped &start,
+                       const geometry_msgs::PoseStamped &goal,
+                       std::vector<geometry_msgs::PoseStamped> &plan,
+                       const Fast_RRTStarNode *const node_goal);
+
+    /**
+     * @brief Update node_rand so tree extends from node_nearest in direction of original
+     *        node_rand until it reaches an occupied or unknown cell
+     * 
+     * @param node_nearest Node nearest to sampled node
+     * @param node_rand Randomly sampled node
+     * @param node_goal Fixed goal node
+     */
+    void steer(Fast_RRTStarNode *node_nearest, Fast_RRTStarNode *node_rand, const Fast_RRTStarNode *const node_goal);
+
+  public:
+    /**
+     * @brief Public Variables
+     */
+    ros::Publisher plan_pub_;
+    std::string frame_id_;
+
+    /**
+     * @brief Construct a new Fast_RRTStarPlanner object.
+     *        Initializes the planner attributes with the default values.
+     */
+    Fast_RRTStarPlanner();
+
+    /**
+     * @brief Construct a new Fast_RRTStarPlanner object.
+     *        Used to initialize the costmap, that is the map that will be used for planning (costmap_ros),
+     *        and the name of the planner (name).
+     * 
+     * @param name Name of the planner
+     * @param costmap_ros ROS version of costmap
+     */
+    Fast_RRTStarPlanner(std::string name, costmap_2d::Costmap2DROS *costmap_ros);
+
+    /**
+	   * @brief Overridden class from interface nav_core::BaseGlobalPlanner
+     *        An initialization function for the BaseGlobalPlanner, which initializes the costmap, 
+	   *        that is the map that will be used for planning (costmap_ros), and the name of the planner (name).
+     * 
+     * @param name Name of planner
+     * @param costmap_ros ROS version of costmap
+     */
+    void initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros);
+	
+    /**
+     * @brief Overridden class from interface nav_core::BaseGlobalPlanner
+     * 
+	   * @param start Pose of starting position
+	   * @param goal Pose of end position
+	   * @param plan Vector of timestamped poses
+     * @return true if planner could find a path from start to goal
+     * @return false if planner could not find a path from start to goal
+     */
+    bool makePlan(const geometry_msgs::PoseStamped &start,
+                  const geometry_msgs::PoseStamped &goal,
+                  std::vector<geometry_msgs::PoseStamped> &plan);
+  };
+};
+
+#endif /* FAST_RRT_STAR_PLANNER_H */
\ No newline at end of file
diff --git a/docker/fast_rrt_ros/package.xml b/docker/fast_rrt_ros/package.xml
new file mode 100644
index 0000000..d8c1b3c
--- /dev/null
+++ b/docker/fast_rrt_ros/package.xml
@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<package format="2">
+	<name>fast_rrt_ros</name>
+	<version>1.0.0</version>
+	<description>The fast_rrt_ros package</description>
+
+	<maintainer email="bcanaday@mitre.org">Bradley Canaday</maintainer>
+	<license>MIT</license>   
+
+	<buildtool_depend>catkin</buildtool_depend>
+	<build_depend>roscpp</build_depend>
+	<build_depend>rospy</build_depend>
+	<build_depend>std_msgs</build_depend>
+	<build_depend>costmap_2d</build_depend>
+	<build_depend>nav_core</build_depend>
+	<build_depend>base_local_planner</build_depend>
+	<build_export_depend>roscpp</build_export_depend>
+	<build_export_depend>rospy</build_export_depend>
+	<build_export_depend>std_msgs</build_export_depend>
+	<exec_depend>roscpp</exec_depend>
+	<exec_depend>rospy</exec_depend>
+	<exec_depend>std_msgs</exec_depend>
+	<exec_depend>costmap_2d</exec_depend>
+	<exec_depend>nav_core</exec_depend>
+	<exec_depend>base_local_planner</exec_depend>
+
+	<export>  
+		<nav_core plugin="${prefix}/fast_rrt_star_planner_plugin.xml" />
+	</export>
+</package>
\ No newline at end of file
diff --git a/docker/fast_rrt_ros/src/fast_rrt_star_helper.cpp b/docker/fast_rrt_ros/src/fast_rrt_star_helper.cpp
new file mode 100644
index 0000000..eef4b65
--- /dev/null
+++ b/docker/fast_rrt_ros/src/fast_rrt_star_helper.cpp
@@ -0,0 +1,220 @@
+#include "fast_rrt_star_helper.h"
+
+#include <ros/ros.h>
+#include <cmath>
+
+using namespace std;
+
+namespace fast_rrt_star_planner
+{
+  world_coords Fast_RRTStarHelper::distance(world_coords x0, world_coords y0, world_coords x1, world_coords y1)
+  {
+    // World (x,y) coordinate difference between candidate and random nodes
+    
+    /**
+     * World (x,y) coordinate difference between candidate and random nodes
+     */
+    world_coords diff_x = x0 - x1;
+    world_coords diff_y = y0 - y1;
+
+    /**
+     * Compute Euclidean distance between points
+     */
+    world_coords dist = sqrt(pow(diff_x, 2) + pow(diff_y, 2));
+
+    return dist;
+  }
+
+  world_coords Fast_RRTStarHelper::distance(Fast_RRTStarNode *node0, Fast_RRTStarNode *node1)
+  {
+    return distance(node0->x, node0->y, node1->x, node1->y);
+  }
+
+  double Fast_RRTStarHelper::angle(Fast_RRTStarNode *node0, Fast_RRTStarNode *node1)
+  {
+    world_coords wdx = node1->x - node0->x;
+    world_coords wdy = node1->y - node0->y;
+
+    double angle = atan2(wdy, wdx);
+
+    return angle;
+  }
+
+  bool Fast_RRTStarHelper::isLineCollisionFree(costmap_2d::Costmap2D *costmap, map_cost lethal_cost, world_coords wx0, world_coords wy0, world_coords wx1, world_coords wy1)
+  {
+    /**
+     * Bresenham Ray-Tracing
+     */
+    vector<base_local_planner::Position2DInt> ray_traced_line = Bresenham(costmap, wx0, wy0, wx1, wy1);
+
+    for (size_t i = 0; i < ray_traced_line.size(); ++i)
+    {
+      /**
+       * Get cost
+       */
+      map_coords mx = ray_traced_line[i].x;
+      map_coords my = ray_traced_line[i].y;
+
+      if (isPointCollisionFree(costmap, lethal_cost, mx, my) == false)
+      {
+        return false;
+      }
+    }
+
+    /**
+     * If no points had collisions, then the whole
+     * line has no collisions
+     */
+    return true;
+  }
+
+  bool Fast_RRTStarHelper::isPointCollisionFree(costmap_2d::Costmap2D *costmap, map_cost lethal_cost, map_coords mx, map_coords my)
+  {
+    map_cost cell_cost = costmap->getCost(mx, my);
+
+    if (cell_cost >= lethal_cost)
+    {
+      return false;
+    }
+
+    return true;
+  }
+
+  vector<base_local_planner::Position2DInt> Fast_RRTStarHelper::Bresenham(costmap_2d::Costmap2D *costmap, Fast_RRTStarNode *start_node, Fast_RRTStarNode *end_node)
+  {
+    if (start_node == nullptr)
+    {
+      ROS_ERROR("NULLPTR: start_node");
+    }
+
+    if (end_node == nullptr)
+    {
+      ROS_ERROR("NULLPTR: end_node");
+    }
+
+    world_coords start_wx = start_node->x;
+    world_coords start_wy = start_node->y;
+    world_coords end_wx = end_node->x;
+    world_coords end_wy = end_node->y;
+
+    return Bresenham(costmap, start_wx, start_wy, end_wx, end_wy);
+  }
+
+  vector<base_local_planner::Position2DInt> Fast_RRTStarHelper::Bresenham(costmap_2d::Costmap2D *costmap, world_coords wx0, world_coords wy0, world_coords wx1, world_coords wy1)
+  {
+    map_coords mx0, mx1, my0, my1;
+
+    costmap->worldToMap(wx0, wy0, mx0, my0);
+    costmap->worldToMap(wx1, wy1, mx1, my1);
+
+    return Bresenham(mx0, my0, mx1, my1);
+  }
+
+  vector<base_local_planner::Position2DInt> Fast_RRTStarHelper::Bresenham(map_coords x0, map_coords y0, map_coords x1, map_coords y1)
+  {
+    vector<base_local_planner::Position2DInt> ray_traced_line;
+
+    base_local_planner::FootprintHelper fh;
+    fh.getLineCells(x0, x1, y0, y1, ray_traced_line);
+
+    return ray_traced_line;
+  }
+
+  bool Fast_RRTStarHelper::isFootprintPathCollisionFree(costmap_2d::Costmap2D *costmap, costmap_2d::Costmap2DROS *costmap_ros, map_cost lethal_cost, Fast_RRTStarNode *node0, Fast_RRTStarNode *node1)
+  {
+    /**
+     * Bresenham Ray-Tracing
+     */
+    vector<base_local_planner::Position2DInt> ray_traced_line = Bresenham(costmap, node0, node1);
+
+    for (size_t i = 0; i < ray_traced_line.size(); ++i)
+    {
+      /**
+       * Check center point of robot
+       */
+      map_coords mx = ray_traced_line[i].x;
+      map_coords my = ray_traced_line[i].y;
+      world_coords wx, wy;
+      costmap->mapToWorld(mx, my, wx, wy);
+
+      if (isPointCollisionFree(costmap, lethal_cost, mx, my) == false)
+      {
+        return false;
+      }
+
+      /**
+       * Get footprint and angle
+       */
+      vector<geometry_msgs::Point> base_footprint = costmap_ros->getRobotFootprint();
+      double theta = angle(node0, node1);
+
+      /**
+       * Need to transform footprint points to a theoretical
+       * to check for collisions
+       *
+       * Construct rotation matrix for yaw angle theta
+       * R_z(theta) = [ cos(theta), -sin(theta), 0;
+       *                sin(theta), cos(theta),  0;
+       *                0,          0,           1 ];
+       */
+      vector<double> R_z_theta = {cos(theta), -sin(theta), 0, sin(theta), cos(theta), 0, 0, 0};
+
+      /**
+       * Construct translation matrix
+       * t(tx, ty, tz) = [ tx;
+       *                   ty;
+       *                   tz;
+       *                   1; ]
+       */
+      vector<double> t = {wx, wy, 0};
+
+      /**
+       * Transform base_footprint points with transformation matrix
+       * and populate oriented_footprint vector
+       */
+      vector<geometry_msgs::Point> oriented_footprint;
+      for (size_t i = 0; i < base_footprint.size(); ++i)
+      {
+        geometry_msgs::Point old_pt = base_footprint[i];
+        geometry_msgs::Point new_pt;
+
+        /**
+         * Do matrix multiplication manually
+         */
+        new_pt.x = old_pt.x * R_z_theta[0] + old_pt.y * R_z_theta[1] + old_pt.z * R_z_theta[2] + t[0];
+        new_pt.y = old_pt.x * R_z_theta[3] + old_pt.y * R_z_theta[4] + old_pt.z * R_z_theta[5] + t[1];
+        new_pt.z = old_pt.x * R_z_theta[6] + old_pt.y * R_z_theta[7] + old_pt.z * R_z_theta[8] + t[2];
+
+        oriented_footprint.push_back(new_pt);
+      }
+
+      /**
+       * Check lines connecting footprint vertices for collisions
+       */
+      for (size_t i = 0; i < oriented_footprint.size(); ++i)
+      {
+        world_coords wx0 = oriented_footprint[i].x;
+        world_coords wy0 = oriented_footprint[i].y;
+        world_coords wx1, wy1;
+
+        if (i + 1 < oriented_footprint.size())
+        {
+          wx1 = oriented_footprint[i + 1].x;
+          wy1 = oriented_footprint[i + 1].y;
+        }
+        else
+        {
+          wx1 = oriented_footprint[0].x;
+          wy1 = oriented_footprint[0].y;
+        }
+
+        if (isLineCollisionFree(costmap, lethal_cost, wx0, wy0, wx1, wy1) == false)
+        {
+          return false;
+        }
+      }
+    }
+
+    return true;
+  }
+}
\ No newline at end of file
diff --git a/docker/fast_rrt_ros/src/fast_rrt_star_node.cpp b/docker/fast_rrt_ros/src/fast_rrt_star_node.cpp
new file mode 100644
index 0000000..3c646ce
--- /dev/null
+++ b/docker/fast_rrt_ros/src/fast_rrt_star_node.cpp
@@ -0,0 +1,55 @@
+#include "fast_rrt_star_node.h"
+#include "fast_rrt_star_helper.h"
+
+using namespace std;
+
+namespace fast_rrt_star_planner
+{
+  Fast_RRTStarNode::Fast_RRTStarNode()
+    : x(0.0), y(0.0), parent(nullptr), path_cost(0.0) {}
+
+  Fast_RRTStarNode::Fast_RRTStarNode(world_coords x_in, world_coords y_in)
+    : x(x_in), y(y_in), parent(nullptr), path_cost(0.0) {}
+
+  Fast_RRTStarNode::Fast_RRTStarNode(Fast_RRTStarNode *node)
+    : x(node->x), y(node->y), parent(node->parent), path_cost(node->path_cost) {}
+
+  Fast_RRTStarNode::Fast_RRTStarNode(const Fast_RRTStarNode *const node)
+    : x(node->x), y(node->y), parent(node->parent), path_cost(node->path_cost) {}
+
+  void Fast_RRTStarNode::updateParentAndCost(Fast_RRTStarNode *node_parent)
+  {
+    this->parent = node_parent;
+    this->path_cost = node_parent->path_cost + this->getConnectionCost(node_parent);
+
+    return;
+  }
+
+  void Fast_RRTStarNode::updateParentAndCost(Fast_RRTStarNode *node_parent, world_coords path_cost)
+  {
+    this->parent = node_parent;
+    this->path_cost = path_cost;
+
+    return;
+  }
+
+  world_coords Fast_RRTStarNode::getConnectionCost(Fast_RRTStarNode *node_connect)
+  {
+    return Fast_RRTStarHelper::distance(node_connect, this);
+  }
+
+  bool Fast_RRTStarNode::operator==(const Fast_RRTStarNode node)
+  {
+    if (this->x == node.x && this->y == node.y)
+    {
+      return true;
+    }
+
+    return false;
+  }
+
+  bool Fast_RRTStarNode::operator!=(const Fast_RRTStarNode node)
+  {
+    return !(*this == node);
+  }
+}
\ No newline at end of file
diff --git a/docker/fast_rrt_ros/src/fast_rrt_star_planner.cpp b/docker/fast_rrt_ros/src/fast_rrt_star_planner.cpp
new file mode 100644
index 0000000..0eb4696
--- /dev/null
+++ b/docker/fast_rrt_ros/src/fast_rrt_star_planner.cpp
@@ -0,0 +1,693 @@
+#include "fast_rrt_star_planner.h"
+
+#include <pluginlib/class_list_macros.h>
+#include <stdlib.h>
+#include <limits>
+#include <random>
+
+PLUGINLIB_EXPORT_CLASS(fast_rrt_star_planner::Fast_RRTStarPlanner, nav_core::BaseGlobalPlanner)
+
+using namespace std;
+
+namespace fast_rrt_star_planner
+{
+  Fast_RRTStarPlanner::Fast_RRTStarPlanner()
+    : costmap_ros_(nullptr), initialized_(false) {}
+
+  Fast_RRTStarPlanner::Fast_RRTStarPlanner(std::string name, costmap_2d::Costmap2DROS *costmap_ros)
+    : costmap_ros_(nullptr), initialized_(false)
+  {
+    initialize(name, costmap_ros);
+  }
+
+  void Fast_RRTStarPlanner::initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros)
+  {
+    if (initialized_ == false)
+    {
+      costmap_ros_ = costmap_ros;			        ///< initialize the costmap_ros_ attribute to the parameter
+      costmap_ = costmap_ros_->getCostmap();  ///< get the costmap_ from costmap_ros_
+      costmap_model_ = new base_local_planner::CostmapModel(*costmap_);
+      resolution_ = costmap_->getResolution();
+      origin_x_ = costmap_->getOriginX();
+      origin_y_ = costmap_->getOriginY();
+      size_x_ = costmap_->getSizeInMetersX();
+      size_y_ = costmap_->getSizeInMetersY();
+
+      int lethal_cost_int;
+
+      /**
+       * Initialize planner parameters from parameter server
+       * Reference: http://wiki.ros.org/roscpp/Overview/Parameter%20Server
+       */
+      ros::NodeHandle nh("~/" + name);
+      nh.param("max_num_iter", max_num_iter_, 500);
+      nh.param("min_num_iter", min_num_iter_, 40);
+      nh.param("world_near_radius", world_near_radius_, 2.0);
+      nh.param("world_dist_dichotomy", world_dist_dichotomy_, 0.1);
+      nh.param("world_expand_dist", world_expand_dist_, 2.5);
+      nh.param("world_goal_accessible_dist", world_goal_accessible_dist_, 0.5);
+      nh.param("goal_frequency", goal_frequency_, 0.2);
+      nh.param("lethal_cost", lethal_cost_int, 253);
+
+      lethal_cost_ = static_cast<map_cost>(lethal_cost_int);
+
+      frame_id_ = costmap_ros->getGlobalFrameID();
+      plan_pub_ = nh.advertise<nav_msgs::Path>("plan", 1);
+
+      initialized_ = true;
+    }
+    else
+    {
+      ROS_WARN("This planner has already been initialized... doing nothing");
+    }
+  }
+
+  bool Fast_RRTStarPlanner::makePlan(const geometry_msgs::PoseStamped &start,
+                                 const geometry_msgs::PoseStamped &goal,
+                                 std::vector<geometry_msgs::PoseStamped> &plan)
+  {
+    if (initialized_ == false)
+    {
+      ROS_ERROR("The planner has not been initialized, please call initialize() to use the planner");
+      return false;
+    }
+
+    /**
+     * Get updated costmap
+     */
+    plan.clear();
+    costmap_ = costmap_ros_->getCostmap();
+
+    if (start.header.frame_id != frame_id_)
+    {
+      ROS_ERROR("This planner as configured will only accept start poses in the %s frame, but a start was sent in the %s frame.",
+                frame_id_.c_str(), start.header.frame_id.c_str());
+    }
+
+    if (goal.header.frame_id != frame_id_)
+    {
+      ROS_ERROR("This planner as configured will only accept goal poses in the %s frame, but a goal was sent in the %s frame.",
+                frame_id_.c_str(), goal.header.frame_id.c_str());
+    }
+
+    ROS_INFO("Got a start: %.2f, %.2f, and a goal: %.2f, %.2f", start.pose.position.x, start.pose.position.y, goal.pose.position.x, goal.pose.position.y);
+
+    /**
+     * Reset global plan variables
+     * 
+     */
+    path_found_ = false;
+    best_path_cost_ = numeric_limits<double>::max();
+    node_best_path_end_ = nullptr;
+
+    /**
+     * Create nodes for start and goal poses
+     * These are constant pointers to a const, so neither the pointer nor the values it points to can change
+     */
+    const Fast_RRTStarNode *const node_start = new Fast_RRTStarNode(start.pose.position.x, start.pose.position.y);
+    const Fast_RRTStarNode *const node_goal = new Fast_RRTStarNode(goal.pose.position.x, goal.pose.position.y);
+
+    /**
+     * Initialize node vector with start node
+     */
+    Fast_RRTStarNode *node_start_copy = new Fast_RRTStarNode(node_start);
+    node_vec_.push_back(node_start_copy);
+
+    /**
+     * For loop over maximum number of iterations
+     */
+    for (int i = 0; i < max_num_iter_; ++i)
+    {
+      /**
+       * Find a random unoccupied node
+       */
+      Fast_RRTStarNode *node_rand = sampleFree(node_goal); ///< node's parent is not defined yet
+
+      /**
+       * nearest() to find index of node in existing tree
+       *       that is closest to random position from
+       *       SampleFree()
+       */
+      size_t node_nearest_ind = nearest(node_rand);
+      Fast_RRTStarNode *node_nearest = node_vec_[node_nearest_ind];
+
+      /**
+       * Expand tree from node_nearest in direction of node_rand
+       * until distance limit is reached
+       */
+      steer(node_nearest, node_rand, node_goal);
+
+      /**
+       * Check if straight line path has collision with obstacles
+       */
+      if (Fast_RRTStarHelper::isFootprintPathCollisionFree(costmap_, costmap_ros_, lethal_cost_, node_nearest, node_rand) == true)
+      {
+        /**
+         * near() to find all nodes in existing tree
+         *       that are within R_near of random node
+         */
+        vector<size_t> near_nodes_list = near(node_rand); ///< Used by rewire()
+
+        /**
+         * findReachest() to find reachest node based
+         *       on nearest node and random node and map
+         */
+        Fast_RRTStarNode *node_reachest = findReachest(node_nearest, node_rand);
+
+        /**
+         * createNode() to create new node to connect
+         *       random node to nearest node using D_dichotomy
+         *       and reachest node and random node and map
+         */
+        Fast_RRTStarNode *node_create = createNode(node_reachest, node_rand);
+
+        /**
+         * Check if node was able to be created
+         */
+        if (node_create != nullptr)
+        {
+          /**
+           * If node was created, add created node and random node to tree
+           * ! NOTE: Make sure node_rand is appended AFTER node_create
+           *         in node_vec_ so it is the last element
+           */
+          if (Fast_RRTStarHelper::isFootprintPathCollisionFree(costmap_, costmap_ros_, lethal_cost_, node_create, node_reachest->parent) == false)
+          {
+            ROS_WARN("Missed collision check b/w node_create and node_reachest->parent");
+          }
+
+          node_create->updateParentAndCost(node_reachest->parent);
+
+          if (Fast_RRTStarHelper::isFootprintPathCollisionFree(costmap_, costmap_ros_, lethal_cost_, node_rand, node_create) == false)
+          {
+            ROS_WARN("Missed collision check b/w node_rand and node_create");
+          }
+
+          node_rand->updateParentAndCost(node_create);
+
+          node_vec_.push_back(node_create);
+          node_vec_.push_back(node_rand);
+        }
+        else
+        {
+          /**
+           * If node was not created, add only random node to tree
+           */
+          if (Fast_RRTStarHelper::isFootprintPathCollisionFree(costmap_, costmap_ros_, lethal_cost_, node_rand, node_reachest) == false)
+          {
+            ROS_WARN("Missed collision check b/w node_rand and node_reachest");
+          }
+
+          node_rand->updateParentAndCost(node_reachest);
+
+          node_vec_.push_back(node_rand);
+        }
+
+        /**
+         * Check if initial path was found
+         * Compute distance between goal location and node_vec_.back() location
+         */
+        if (initialPathFound(node_goal) == true)
+        {
+          path_found_ = true;
+          double current_path_cost = node_vec_.back()->path_cost;
+
+          if (current_path_cost < best_path_cost_)
+          {
+            best_path_cost_ = current_path_cost;
+            node_best_path_end_ = node_vec_.back();
+          }
+
+          if (i > min_num_iter_)
+          {
+            /**
+             * Exit for loop early if a valid plan was found
+             * after the minimum number of iterations
+             */
+            break;
+          }
+        }
+
+        /**
+         * rewire() to improve path cost
+         */
+        rewire(node_rand, near_nodes_list);
+      }
+      else
+      {
+        delete node_rand;
+      }
+    }
+
+    if (path_found_ == true)
+    {
+      ROS_INFO("PATH FOUND");
+      constructPlan(start, goal, plan, node_goal);
+    }
+    else
+    {
+      ROS_INFO("NO PATH FOUND");
+    }
+
+    /**
+     * Cleanup pointers when done
+     */
+    delete node_start;
+    delete node_goal;
+    for (size_t i = 0; i < node_vec_.size(); ++i)
+    {
+      delete node_vec_[i];
+    }
+    node_vec_.clear();
+
+    return path_found_;
+  }
+
+  Fast_RRTStarNode *Fast_RRTStarPlanner::sampleFree(const Fast_RRTStarNode *const node_goal)
+  {
+    /**
+     * Random sampling setup
+     */
+    std::random_device rd;  ///< Will be used to obtain a seed for the random number engine
+    std::mt19937 gen(rd()); ///< Standard mersenne_twister_engine seeded with rd()
+
+    /**
+     * Sample between 0 and 1
+     */
+    uniform_real_distribution<double> rand_is_goal_dist(0, 1);
+    double rand_is_goal_val = rand_is_goal_dist(gen);
+
+    /**
+     * If sample is between 0 and goal_frequency, set node_rand to node_goal
+     */
+    if (rand_is_goal_val < goal_frequency_)
+    {
+      Fast_RRTStarNode *node_rand = new Fast_RRTStarNode(node_goal);
+
+      return node_rand;
+    }
+
+    /**
+     * Otherwise do stuff below
+     */
+    Fast_RRTStarNode *node_rand = new Fast_RRTStarNode();
+
+    /**
+     * Get lower and upper bounds of costmap in world coordinates
+     */
+    world_coords rand_wx_lb = origin_x_;
+    world_coords rand_wx_ub = origin_x_ + size_x_;
+    world_coords rand_wy_lb = origin_y_;
+    world_coords rand_wy_ub = origin_y_ + size_y_;
+
+    /**
+     * Create uniform distributions between lower and upper bound for x and y
+     */
+    uniform_real_distribution<world_coords> rand_wx_dist(rand_wx_lb, rand_wx_ub);
+    uniform_real_distribution<world_coords> rand_wy_dist(rand_wy_lb, rand_wy_ub);
+
+    /**
+     * Sample from uniform distributions with random engine
+     */
+    world_coords rand_wx = rand_wx_dist(gen);
+    world_coords rand_wy = rand_wy_dist(gen);
+
+    /**
+     * Update node_rand with randomly generated position
+     */
+    node_rand->x = rand_wx;
+    node_rand->y = rand_wy;
+
+    return node_rand;
+  }
+
+  size_t Fast_RRTStarPlanner::nearest(Fast_RRTStarNode *node_rand)
+  {
+    /**
+     * Set initial index and world coordinate distance of nearest node
+     */
+    size_t nearest_node_ind = 0;
+    world_coords nearest_node_cost = std::numeric_limits<world_coords>::max();
+
+    /**
+     * Search through node_vec_ to find closest node (based on connection cost)
+     */
+    for (size_t i = 0; i < node_vec_.size(); ++i)
+    {
+      world_coords candidate_cost = node_rand->getConnectionCost(node_vec_[i]);
+
+      /**
+       * Update nearest node index and nearest node distance
+       * if candidate node is closer than previous closest node
+       */
+      if (candidate_cost < nearest_node_cost)
+      {
+        nearest_node_cost = candidate_cost;
+        nearest_node_ind = i;
+      }
+    }
+
+    return nearest_node_ind;
+  }
+
+  vector<size_t> Fast_RRTStarPlanner::near(Fast_RRTStarNode *node_rand)
+  {
+    /**
+     * Create vector to hold indexes of near nodes
+     */
+    vector<size_t> near_node_idxs;
+
+    /**
+     * Loop through node_vec_
+     */
+    for (size_t i = 0; i < node_vec_.size(); ++i)
+    {
+      /**
+       * Check if distance between node_rand and node in node_vec_
+       * is less than the near radius
+       */
+      if (Fast_RRTStarHelper::distance(node_rand, node_vec_[i]) < world_near_radius_)
+      {
+        /**
+         * Add near node index to near_node_idxs vector
+         */
+        near_node_idxs.push_back(i);
+      }
+    }
+
+    return near_node_idxs;
+  }
+
+  Fast_RRTStarNode *Fast_RRTStarPlanner::findReachest(Fast_RRTStarNode *node_nearest, Fast_RRTStarNode *node_rand)
+  {
+    /**
+     * Set new node to node_nearest
+     */
+    Fast_RRTStarNode *node_reachest = new Fast_RRTStarNode(node_nearest);
+
+    /**
+     * While not back at start node, search through node_nearest parents
+     * to find the furthest collision free node from node_rand
+     * grab start node, first pointer in node_vec
+     */
+    Fast_RRTStarNode *node_start = node_vec_.front();
+    while (*node_start != *node_reachest)
+    {
+      if (Fast_RRTStarHelper::isFootprintPathCollisionFree(costmap_, costmap_ros_, lethal_cost_, node_rand, node_reachest->parent) == true)
+      {
+        /**
+         * Create temporary pointer to parent of node_reachest
+         */
+        Fast_RRTStarNode *node_reachest_parent = node_reachest->parent;
+        
+        /**
+         * Delete old node_reachest
+         */
+        delete node_reachest;
+        
+        /**
+         * Create new node_reachest as copy of old node_reachest parent
+         */
+        node_reachest = new Fast_RRTStarNode(node_reachest_parent);
+      }
+      else
+      {
+        break;
+      }
+    }
+
+    return node_reachest;
+  }
+
+  Fast_RRTStarNode *Fast_RRTStarPlanner::createNode(Fast_RRTStarNode *node_reachest, Fast_RRTStarNode *node_rand)
+  {
+    Fast_RRTStarNode *node_allow = new Fast_RRTStarNode(node_reachest);
+    Fast_RRTStarNode *node_start = node_vec_.front();
+    Fast_RRTStarNode *node_mid = new Fast_RRTStarNode();
+    Fast_RRTStarNode *node_forbid = nullptr;
+
+    if (*node_start != *node_reachest)
+    {
+      node_forbid = new Fast_RRTStarNode(node_reachest->parent);
+
+      /**
+       * while allow/forbid nodes distance > dichotomy distance
+       */
+      while (Fast_RRTStarHelper::distance(node_allow, node_forbid) > world_dist_dichotomy_)
+      {
+        node_mid->x = (node_allow->x + node_forbid->x) / 2.0;
+        node_mid->y = (node_allow->y + node_forbid->y) / 2.0;
+
+        if (Fast_RRTStarHelper::isFootprintPathCollisionFree(costmap_, costmap_ros_, lethal_cost_, node_rand, node_mid) == true)
+        {
+          node_allow->x = node_mid->x;
+          node_allow->y = node_mid->y;
+        }
+        else
+        {
+          node_forbid->x = node_mid->x;
+          node_forbid->y = node_mid->y;
+        }
+      }
+
+      node_forbid->x = node_rand->x;
+      node_forbid->y = node_rand->y;
+
+      while (Fast_RRTStarHelper::distance(node_allow, node_forbid) > world_dist_dichotomy_)
+      {
+        node_mid->x = (node_allow->x + node_forbid->x) / 2.0;
+        node_mid->y = (node_allow->y + node_forbid->y) / 2.0;
+
+        if (Fast_RRTStarHelper::isFootprintPathCollisionFree(costmap_, costmap_ros_, lethal_cost_, node_mid, node_reachest->parent) == true)
+        {
+          node_allow->x = node_mid->x;
+          node_allow->y = node_mid->y;
+        }
+        else
+        {
+          node_forbid->x = node_mid->x;
+          node_forbid->y = node_mid->y;
+        }
+      }
+    }
+
+    Fast_RRTStarNode *node_create = new Fast_RRTStarNode();
+    if (*node_allow != *node_reachest)
+    {
+      node_create->x = node_allow->x;
+      node_create->y = node_allow->y;
+    }
+    else
+    {
+      node_create = nullptr;
+    }
+
+    delete node_allow;
+    delete node_mid;
+    delete node_forbid;
+
+    return node_create;
+  }
+
+  void Fast_RRTStarPlanner::rewire(Fast_RRTStarNode *node_rand, const vector<size_t> &near_nodes_list)
+  {
+    /**
+     * for each node in near
+     */
+    for (size_t i = 0; i < near_nodes_list.size(); ++i)
+    {
+      size_t node_near_index_i = near_nodes_list[i];
+      Fast_RRTStarNode *node_near_i = node_vec_[node_near_index_i];
+
+      world_coords rand_rewire_cost = node_rand->path_cost + node_near_i->getConnectionCost(node_rand);
+      if (node_near_i->path_cost > rand_rewire_cost)
+      {
+        if (Fast_RRTStarHelper::isFootprintPathCollisionFree(costmap_, costmap_ros_, lethal_cost_, node_near_i, node_rand) == true)
+        {
+          node_near_i->updateParentAndCost(node_rand, rand_rewire_cost);
+        }
+      }
+    }
+  }
+
+  bool Fast_RRTStarPlanner::initialPathFound(const Fast_RRTStarNode *const node_goal)
+  {
+    Fast_RRTStarNode *node_goal_copy = new Fast_RRTStarNode(node_goal);
+    Fast_RRTStarNode *node_last = node_vec_.back();
+    world_coords dist_to_goal = Fast_RRTStarHelper::distance(node_last, node_goal_copy);
+
+    bool goal_dist_threshold_met = dist_to_goal < world_goal_accessible_dist_;
+    bool last_to_goal_collision_free = Fast_RRTStarHelper::isFootprintPathCollisionFree(costmap_, costmap_ros_, lethal_cost_, node_last, node_goal_copy);
+    bool path_collision_free = true;
+
+    Fast_RRTStarNode *node_n = node_last;
+
+    /**
+     * Verify that path is still collision free even if map has changed
+     */
+    while (node_n->parent != nullptr)
+    {
+      if (!Fast_RRTStarHelper::isFootprintPathCollisionFree(costmap_, costmap_ros_, lethal_cost_, node_n, node_n->parent) == true)
+      {
+        path_collision_free = false;
+        break;
+      }
+
+      node_n = node_n->parent;
+    }
+
+    delete node_goal_copy;
+
+    return goal_dist_threshold_met && last_to_goal_collision_free && path_collision_free;
+  }
+
+  void Fast_RRTStarPlanner::constructPlan(const geometry_msgs::PoseStamped &start,
+                                      const geometry_msgs::PoseStamped &goal,
+                                      vector<geometry_msgs::PoseStamped> &plan,
+                                      const Fast_RRTStarNode *const node_goal)
+  {
+    /**
+     * Empty plan vector
+     */
+    plan.clear();
+    ros::Time plan_time = ros::Time::now();
+
+    /**
+     * Create a node for the goal position
+     */
+    Fast_RRTStarNode *node_goal_copy = new Fast_RRTStarNode(node_goal);
+
+    if (Fast_RRTStarHelper::isFootprintPathCollisionFree(costmap_, costmap_ros_, lethal_cost_, node_goal_copy, node_best_path_end_) == false)
+    {
+      ROS_WARN("Missed collision check b/w node_goal_copy and node_best_path_end_");
+    }
+
+    node_goal_copy->updateParentAndCost(node_best_path_end_);
+    node_vec_.push_back(node_goal_copy);
+
+    /**
+     * Initialize node_n to goal node
+     */
+    Fast_RRTStarNode *node_n = node_vec_.back();
+    bool isGoalNode = true;
+    pose_value_type zVal = 0.0; ///< Z-axis is ignored
+
+    /**
+     * Go through parents of node until hitting start_node
+     */
+    while (node_n->parent != nullptr)
+    {
+      /**
+       * Initialize new stamped pose
+       */
+      geometry_msgs::PoseStamped parent_pose_n;
+
+      /**
+       * Plan headers
+       */
+      parent_pose_n.header.stamp = plan_time;
+      parent_pose_n.header.frame_id = frame_id_;
+
+      /**
+       * Set position
+       */
+      parent_pose_n.pose.position.x = node_n->parent->x;
+      parent_pose_n.pose.position.y = node_n->parent->y;
+      parent_pose_n.pose.position.z = zVal;
+
+      /**
+       * Set orientation
+       * ! NOTE: Assuming FLU (x: Front, y: Left, z: Up) coordinate frame
+       *         This means 0 yaw points front
+       *         Positive yaw is CCW and negative yaw is CW.
+       */
+      world_coords start_x = node_n->parent->x;
+      world_coords start_y = node_n->parent->y;
+      world_coords end_x = node_n->x;
+      world_coords end_y = node_n->y;
+      world_coords dx = end_x - start_x;
+      world_coords dy = end_y - start_y;
+
+      pose_value_type yaw_in_rad = -atan2(dx, dy);
+      tf::Quaternion parent_pose_n_quat = tf::createQuaternionFromYaw(yaw_in_rad);
+
+      parent_pose_n.pose.orientation.x = parent_pose_n_quat.x();
+      parent_pose_n.pose.orientation.y = parent_pose_n_quat.y();
+      parent_pose_n.pose.orientation.z = parent_pose_n_quat.z();
+      parent_pose_n.pose.orientation.w = parent_pose_n_quat.w();
+
+      /**
+       * Goal node re-uses orientation of parent, but updates position
+       * NOTE: Make sure to push back goal pose first so reverse
+       *       ordering of poses is correct
+       */
+      if (isGoalNode == true)
+      {
+        geometry_msgs::PoseStamped pose_n = parent_pose_n;
+
+        /**
+         * Set position
+         */
+        pose_n.pose.position.x = node_n->x;
+        pose_n.pose.position.y = node_n->y;
+
+        /**
+         * Set orientation
+         */
+        pose_n.pose.orientation = goal.pose.orientation;
+
+        /**
+         * Add pose of goal node to plan
+         */
+        plan.push_back(pose_n);
+
+        isGoalNode = false;
+      }
+
+      /**
+       * Add pose of parent node to plan
+       */
+      plan.push_back(parent_pose_n);
+
+      /**
+       * Update node_n to its parent
+       */
+      node_n = node_n->parent;
+    }
+
+    /**
+     * Reverse order of plan so pose associated with start_node is first
+     * and pose associated with goal_node is last
+     */
+    reverse(plan.begin(), plan.end());
+
+    return;
+  }
+
+  void Fast_RRTStarPlanner::steer(Fast_RRTStarNode *node_nearest, Fast_RRTStarNode *node_rand, const Fast_RRTStarNode *const node_goal)
+  {
+    /**
+     * Calculate dist of vector from node_nearest to node_rand
+     */
+    world_coords dist = Fast_RRTStarHelper::distance(node_nearest, node_rand);
+
+    /**
+     * If node_rand is within expand dist, keep node_rand as is
+     */
+    if (dist < world_expand_dist_)
+    {
+      node_rand->updateParentAndCost(node_nearest);
+      return;
+    }
+
+    double angle = Fast_RRTStarHelper::angle(node_nearest, node_rand);
+
+    /**
+     * Re-calculate x/y components of vector based on new distance
+     */
+    node_rand->x = node_nearest->x + world_expand_dist_ * cos(angle);
+    node_rand->y = node_nearest->y + world_expand_dist_ * sin(angle);
+
+    return;
+  }
+};
diff --git a/docker/husky_description/urdf/husky.urdf.xacro b/docker/husky_description/urdf/husky.urdf.xacro
new file mode 100644
index 0000000..f11b36e
--- /dev/null
+++ b/docker/husky_description/urdf/husky.urdf.xacro
@@ -0,0 +1,381 @@
+<?xml version="1.0"?>
+<!--
+Software License Agreement (BSD)
+
+\file      husky.urdf.xacro
+\authors   Paul Bovbel <pbovbel@clearpathrobotics.com>, Devon Ash <dash@clearpathrobotics.com>
+\copyright Copyright (c) 2015, Clearpath Robotics, Inc., All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification, are permitted provided that
+the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice, this list of conditions and the
+   following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
+   following disclaimer in the documentation and/or other materials provided with the distribution.
+ * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote
+   products derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR-
+RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN-
+DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
+OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+-->
+<robot name="husky" xmlns:xacro="http://ros.org/wiki/xacro">
+  <!-- IMU Link -->
+  <xacro:arg name="imu_xyz"     default="$(optenv HUSKY_IMU_XYZ 0.19 0 0.149)"/>
+  <xacro:arg name="imu_rpy"     default="$(optenv HUSKY_IMU_RPY 0 -1.5708 3.1416)"/>
+  <xacro:arg name="imu_parent"  default="$(optenv HUSKY_IMU_PARENT base_link)"/>
+
+  <!-- LMS1XX Laser Primary and Secondary -->
+  <xacro:arg name="laser_enabled"           default="$(optenv HUSKY_LMS1XX_ENABLED 1)" />
+  <xacro:arg name="laser_topic"             default="$(optenv HUSKY_LMS1XX_TOPIC front/scan)"/>
+  <xacro:arg name="laser_tower"             default="$(optenv HUSKY_LMS1XX_TOWER 1)"/>
+  <xacro:arg name="laser_prefix"            default="$(optenv HUSKY_LMS1XX_PREFIX front)"/>
+  <xacro:arg name="laser_parent"            default="$(optenv HUSKY_LMS1XX_PARENT top_plate_link)"/>
+  <xacro:arg name="laser_xyz"               default="$(optenv HUSKY_LMS1XX_XYZ 0.2206 0.0 0.00635)" />
+  <xacro:arg name="laser_rpy"               default="$(optenv HUSKY_LMS1XX_RPY 0.0 0.0 0.0)" />
+
+  <xacro:arg name="laser_secondary_enabled" default="$(optenv HUSKY_LMS1XX_SECONDARY_ENABLED 0)" />
+  <xacro:arg name="laser_secondary_topic"   default="$(optenv HUSKY_LMS1XX_SECONDARY_TOPIC rear/scan)"/>
+  <xacro:arg name="laser_secondary_tower"   default="$(optenv HUSKY_LMS1XX_SECONDARY_TOWER 1)"/>
+  <xacro:arg name="laser_secondary_prefix"  default="$(optenv HUSKY_LMS1XX_SECONDARY_PREFIX rear)"/>
+  <xacro:arg name="laser_secondary_parent"  default="$(optenv HUSKY_LMS1XX_SECONDARY_PARENT top_plate_link)"/>
+  <xacro:arg name="laser_secondary_xyz"     default="$(optenv HUSKY_LMS1XX_SECONDARY_XYZ -0.2206 0.0 0.00635)" />
+  <xacro:arg name="laser_secondary_rpy"     default="$(optenv HUSKY_LMS1XX_SECONDARY_RPY 0.0 0.0 3.14159)" />
+
+  <!-- UST10 Laser Primary  and Secondary -->
+  <xacro:arg name="laser_ust10_front_enabled" default="$(optenv HUSKY_UST10_ENABLED 0)" />
+  <xacro:arg name="laser_ust10_front_topic"   default="$(optenv HUSKY_UST10_TOPIC front/scan)" />
+  <xacro:arg name="laser_ust10_front_prefix"  default="$(optenv HUSKY_UST10_PREFIX front)" />
+  <xacro:arg name="laser_ust10_front_parent"  default="$(optenv HUSKY_UST10_PARENT top_plate_link)" />
+  <xacro:arg name="laser_ust10_front_xyz"     default="$(optenv HUSKY_UST10_XYZ 0.2206 0.0 0.00635)" />
+  <xacro:arg name="laser_ust10_front_rpy"     default="$(optenv HUSKY_UST10_RPY 0 0 0)" />
+
+  <xacro:arg name="laser_ust10_rear_enabled"  default="$(optenv HUSKY_UST10_SECONDARY_ENABLED 0)" />
+  <xacro:arg name="laser_ust10_rear_topic"    default="$(optenv HUSKY_UST10_SECONDARY_TOPIC rear/scan)" />
+  <xacro:arg name="laser_ust10_rear_prefix"   default="$(optenv HUSKY_UST10_SECONDARY_PREFIX rear)" />
+  <xacro:arg name="laser_ust10_rear_parent"   default="$(optenv HUSKY_UST10_SECONDARY_PARENT top_plate_link)" />
+  <xacro:arg name="laser_ust10_rear_xyz"      default="$(optenv HUSKY_UST10_SECONDARY_XYZ -0.2206 0.0 0.00635)" />
+  <xacro:arg name="laser_ust10_rear_rpy"      default="$(optenv HUSKY_UST10_SECONDARY_RPY 0 0 3.14159)" />
+
+  <!-- Velodyne LiDAR Primary and Secondary -->
+  <xacro:arg name="laser_3d_enabled"              default="$(optenv HUSKY_LASER_3D_ENABLED 0)" />
+  <xacro:arg name="laser_3d_topic"                default="$(optenv HUSKY_LASER_3D_TOPIC points)"/>
+  <xacro:arg name="laser_3d_tower"                default="$(optenv HUSKY_LASER_3D_TOWER 1)"/>
+  <xacro:arg name="laser_3d_prefix"               default="$(optenv HUSKY_LASER_3D_PREFIX )"/>
+  <xacro:arg name="laser_3d_parent"               default="$(optenv HUSKY_LASER_3D_PARENT top_plate_link)"/>
+  <xacro:arg name="laser_3d_xyz"                  default="$(optenv HUSKY_LASER_3D_XYZ 0 0 0)" />
+  <xacro:arg name="laser_3d_rpy"                  default="$(optenv HUSKY_LASER_3D_RPY 0 0 0)" />
+
+  <xacro:arg name="laser_3d_secondary_enabled"    default="$(optenv HUSKY_LASER_3D_SECONDARY_ENABLED 0)" />
+  <xacro:arg name="laser_3d_secondary_topic"      default="$(optenv HUSKY_LASER_3D_SECONDARY_TOPIC secondary_points)"/>
+  <xacro:arg name="laser_3d_secondary_tower"      default="$(optenv HUSKY_LASER_3D_SECONDARY_TOWER 1)"/>
+  <xacro:arg name="laser_3d_secondary_prefix"     default="$(optenv HUSKY_LASER_3D_SECONDARY_PREFIX secondary_)"/>
+  <xacro:arg name="laser_3d_secondary_parent"     default="$(optenv HUSKY_LASER_3D_SECONDARY_PARENT top_plate_link)"/>
+  <xacro:arg name="laser_3d_secondary_xyz"        default="$(optenv HUSKY_LASER_3D_SECONDARY_XYZ 0 0 0)" />
+  <xacro:arg name="laser_3d_secondary_rpy"        default="$(optenv HUSKY_LASER_3D_SECONDARY_RPY 0 0 -3.14159)" />
+
+  <!-- Realsense Camera Primary and Secondary -->
+  <xacro:arg name="realsense_enabled"             default="$(optenv HUSKY_REALSENSE_ENABLED 0)" />
+  <xacro:arg name="realsense_topic"               default="$(optenv HUSKY_REALSENSE_TOPIC realsense)" />
+  <xacro:arg name="realsense_prefix"              default="$(optenv HUSKY_REALSENSE_PREFIX camera)" />
+  <xacro:arg name="realsense_parent"              default="$(optenv HUSKY_REALSENSE_PARENT top_plate_link)" />
+  <xacro:arg name="realsense_xyz"                 default="$(optenv HUSKY_REALSENSE_XYZ 0 0 0)" />
+  <xacro:arg name="realsense_rpy"                 default="$(optenv HUSKY_REALSENSE_RPY 0 0 0)" />
+
+  <xacro:arg name="realsense_secondary_enabled"   default="$(optenv HUSKY_REALSENSE_SECONDARY_ENABLED 0)" />
+  <xacro:arg name="realsense_secondary_topic"     default="$(optenv HUSKY_REALSENSE_SECONDARY_TOPIC realsense_secondary)" />
+  <xacro:arg name="realsense_secondary_prefix"    default="$(optenv HUSKY_REALSENSE_SECONDARY_PREFIX camera_secondary)" />
+  <xacro:arg name="realsense_secondary_parent"    default="$(optenv HUSKY_REALSENSE_SECONDARY_PARENT top_plate_link)" />
+  <xacro:arg name="realsense_secondary_xyz"       default="$(optenv HUSKY_REALSENSE_SECONDARY_XYZ 0 0 0)" />
+  <xacro:arg name="realsense_secondary_rpy"       default="$(optenv HUSKY_REALSENSE_SECONDARY_RPY 0 0 0)" />
+
+  <!-- BlackflyS Camera Primary and Secondary -->
+  <xacro:arg name="blackfly_enabled"                  default="$(optenv HUSKY_BLACKFLY 0)"/>
+  <xacro:arg name="blackfly_mount_enabled"            default="$(optenv HUSKY_BLACKFLY_MOUNT_ENABLED 1)"/>
+  <xacro:arg name="blackfly_mount_angle"              default="$(optenv HUSKY_BLACKFLY_MOUNT_ANGLE 0)"/>
+  <xacro:arg name="blackfly_prefix"                   default="$(optenv HUSKY_BLACKFLY_PREFIX blackfly)"/>
+  <xacro:arg name="blackfly_parent"                   default="$(optenv HUSKY_BLACKFLY_PARENT top_plate_link)"/>
+  <xacro:arg name="blackfly_xyz"                      default="$(optenv HUSKY_BLACKFLY_XYZ 0 0 0)"/>
+  <xacro:arg name="blackfly_rpy"                      default="$(optenv HUSKY_BLACKFLY_RPY 0 0 0)"/>
+
+  <xacro:arg name="blackfly_secondary_enabled"        default="$(optenv HUSKY_BLACKFLY_SECONDARY 0)"/>
+  <xacro:arg name="blackfly_secondary_mount_enabled"  default="$(optenv HUSKY_BLACKFLY_SECONDARY_MOUNT_ENABLED 1)"/>
+  <xacro:arg name="blackfly_secondary_mount_angle"    default="$(optenv HUSKY_BLACKFLY_SECONDARY_MOUNT_ANGLE 0)"/>
+  <xacro:arg name="blackfly_secondary_prefix"         default="$(optenv HUSKY_BLACKFLY_SECONDARY_PREFIX blackfly_secondary)"/>
+  <xacro:arg name="blackfly_secondary_parent"         default="$(optenv HUSKY_BLACKFLY_SECONDARY_PARENT top_plate_link)"/>
+  <xacro:arg name="blackfly_secondary_xyz"            default="$(optenv HUSKY_BLACKFLY_SECONDARY_XYZ 0 0 0)"/>
+  <xacro:arg name="blackfly_secondary_rpy"            default="$(optenv HUSKY_BLACKFLY_SECONDARY_RPY 0 0 0)"/>
+
+  <!-- Bumper Extension -->
+  <xacro:property name="husky_front_bumper_extend"  value="$(optenv HUSKY_FRONT_BUMPER_EXTEND 0)" />
+  <xacro:property name="husky_rear_bumper_extend"   value="$(optenv HUSKY_REAR_BUMPER_EXTEND 0)" />
+
+  <!-- Height of the sensor arch in mm.  Must be either 510 or 300 -->
+  <xacro:arg name="sensor_arch"         default="$(optenv HUSKY_SENSOR_ARCH 0)" />
+  <xacro:arg name="sensor_arch_height"  default="$(optenv HUSKY_SENSOR_ARCH_HEIGHT 510)" />
+  <xacro:arg name="sensor_arch_xyz"     default="$(optenv HUSKY_SENSOR_ARCH_OFFSET 0 0 0)"/>
+  <xacro:arg name="sensor_arch_rpy"     default="$(optenv HUSKY_SENSOR_ARCH_RPY 0 0 0)"/>
+
+  <!-- Extras -->
+  <xacro:arg name="robot_namespace" default="$(optenv ROBOT_NAMESPACE /)" />
+  <xacro:arg name="urdf_extras"     default="$(optenv HUSKY_URDF_EXTRAS empty.urdf)" />
+  <xacro:arg name="cpr_urdf_extras" default="$(optenv CPR_URDF_EXTRAS empty.urdf)" />
+
+  <!-- Included URDF/XACRO Files -->
+  <xacro:include filename="$(find husky_description)/urdf/accessories/hokuyo_ust10.urdf.xacro" />
+  <xacro:include filename="$(find husky_description)/urdf/accessories/intel_realsense.urdf.xacro"/>
+  <xacro:include filename="$(find husky_description)/urdf/accessories/flir_blackfly_mount.urdf.xacro"/>
+  <xacro:include filename="$(find husky_description)/urdf/accessories/sensor_arch.urdf.xacro"/>
+  <xacro:include filename="$(find husky_description)/urdf/accessories/sick_lms1xx_mount.urdf.xacro"/>
+  <xacro:include filename="$(find husky_description)/urdf/accessories/vlp16_mount.urdf.xacro"/>
+  <xacro:include filename="$(find husky_description)/urdf/decorations.urdf.xacro" />
+  <xacro:include filename="$(find husky_description)/urdf/wheel.urdf.xacro" />
+
+  <xacro:property name="M_PI" value="3.14159"/>
+
+  <!-- Base Size -->
+  <xacro:property name="base_x_size" value="0.98740000" />
+  <xacro:property name="base_y_size" value="0.57090000" />
+  <xacro:property name="base_z_size" value="0.24750000" />
+
+  <!-- Wheel Mounting Positions -->
+  <xacro:property name="wheelbase" value="0.5120" />
+  <xacro:property name="track" value="0.5708" />
+  <xacro:property name="wheel_vertical_offset" value="0.03282" />
+
+  <!-- Wheel Properties -->
+  <xacro:property name="wheel_length" value="0.1143" />
+  <xacro:property name="wheel_radius" value="0.1651" />
+
+  <!-- Base link is the center of the robot's bottom plate -->
+  <link name="base_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://husky_description/meshes/base_link.dae" />
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="${( husky_front_bumper_extend - husky_rear_bumper_extend ) / 2.0} 0 ${base_z_size/4}" rpy="0 0 0" />
+      <geometry>
+        <box size="${ base_x_size + husky_front_bumper_extend + husky_rear_bumper_extend } ${base_y_size} ${base_z_size/2}"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="0 0 ${base_z_size*3/4-0.01}" rpy="0 0 0" />
+      <geometry>
+        <box size="${base_x_size*4/5} ${base_y_size} ${base_z_size/2-0.02}"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- Base footprint is on the ground under the robot -->
+  <link name="base_footprint"/>
+
+  <joint name="base_footprint_joint" type="fixed">
+    <origin xyz="0 0 ${wheel_vertical_offset - wheel_radius}" rpy="0 0 0" />
+    <parent link="base_link" />
+    <child link="base_footprint" />
+  </joint>
+
+  <!-- Inertial link stores the robot's inertial information -->
+  <link name="inertial_link">
+    <inertial>
+      <mass value="46.034" />
+      <origin xyz="-0.00065 -0.085 0.062" />
+      <inertia ixx="0.6022" ixy="-0.02364" ixz="-0.1197" iyy="1.7386" iyz="-0.001544" izz="2.0296" />
+    </inertial>
+  </link>
+
+  <joint name="inertial_joint" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0" />
+    <parent link="base_link" />
+    <child link="inertial_link" />
+  </joint>
+
+  <!-- Husky wheel macros -->
+  <xacro:husky_wheel wheel_prefix="front_left">
+    <origin xyz="${wheelbase/2} ${track/2} ${wheel_vertical_offset}" rpy="0 0 0" />
+  </xacro:husky_wheel>
+  <xacro:husky_wheel wheel_prefix="front_right">
+    <origin xyz="${wheelbase/2} ${-track/2} ${wheel_vertical_offset}" rpy="0 0 0" />
+  </xacro:husky_wheel>
+  <xacro:husky_wheel wheel_prefix="rear_left">
+    <origin xyz="${-wheelbase/2} ${track/2} ${wheel_vertical_offset}" rpy="0 0 0" />
+  </xacro:husky_wheel>
+  <xacro:husky_wheel wheel_prefix="rear_right">
+    <origin xyz="${-wheelbase/2} ${-track/2} ${wheel_vertical_offset}" rpy="0 0 0" />
+  </xacro:husky_wheel>
+
+  <!-- Husky Decorations -->
+  <xacro:husky_decorate />
+
+  <!--
+    Add the main sensor arch if the user has specifically enabled it, or if a sensor
+    requires it for mounting
+  -->
+  <xacro:if value="$(arg sensor_arch)">
+    <xacro:sensor_arch prefix="" parent="top_plate_link" size="$(arg sensor_arch_height)">
+      <origin xyz="$(arg sensor_arch_xyz)" rpy="$(arg sensor_arch_rpy)"/>
+    </xacro:sensor_arch>
+  </xacro:if>
+
+  <!--
+      IMU Link: Standard location to add an IMU (i.e. UM7 or Microstrain)
+  -->
+  <link name="imu_link"/>
+  <joint name="imu_joint" type="fixed">
+    <origin xyz="$(arg imu_xyz)" rpy="$(arg imu_rpy)" />
+    <parent link="$(arg imu_parent)" />
+    <child link="imu_link" />
+  </joint>
+  <gazebo reference="imu_link">
+  </gazebo>
+
+  <!--
+    SICK LMS1XX Priamry and Secondary Laser Scans
+  -->
+  <xacro:if value="$(arg laser_enabled)">
+    <xacro:sick_lms1xx_mount prefix="$(arg laser_prefix)" parent="$(arg laser_parent)" topic="$(arg laser_topic)" robot_namespace="$(arg robot_namespace)" tower="$(arg laser_tower)">
+      <origin xyz="$(arg laser_xyz)" rpy="$(arg laser_rpy)"/>
+    </xacro:sick_lms1xx_mount>
+  </xacro:if>
+  <xacro:if value="$(arg laser_secondary_enabled)">
+    <xacro:sick_lms1xx_mount prefix="$(arg laser_secondary_prefix)" parent="$(arg laser_secondary_parent)" topic="$(arg laser_secondary_topic)" robot_namespace="$(arg robot_namespace)" tower="$(arg laser_secondary_tower)">
+      <origin xyz="$(arg laser_secondary_xyz)" rpy="$(arg laser_secondary_rpy)"/>
+    </xacro:sick_lms1xx_mount>
+  </xacro:if>
+
+  <!--
+    Hokuyo UST10 Primary and Secondary Laser Scans
+  -->
+  <xacro:if value="$(arg laser_ust10_front_enabled)">
+    <xacro:hokuyo_ust10_mount topic="$(arg laser_ust10_front_topic)" prefix="$(arg laser_ust10_front_prefix)" parent_link="$(arg laser_ust10_front_parent)">
+        <origin xyz="$(arg laser_ust10_front_xyz)" rpy="$(arg laser_ust10_front_rpy)" />
+      </xacro:hokuyo_ust10_mount>
+  </xacro:if>
+
+  <xacro:if value="$(arg laser_ust10_rear_enabled)">
+    <xacro:hokuyo_ust10_mount topic="$(arg laser_ust10_rear_topic)" prefix="$(arg laser_ust10_rear_prefix)" parent_link="$(arg laser_ust10_rear_parent)">
+        <origin xyz="$(arg laser_ust10_rear_xyz)" rpy="$(arg laser_ust10_rear_rpy)" />
+      </xacro:hokuyo_ust10_mount>
+  </xacro:if>
+
+  <!-- Intel Realsense Primary and Secondary -->
+  <xacro:if value="$(arg realsense_enabled)">
+    <link name="$(arg realsense_prefix)_realsense_mountpoint"/>
+    <joint name="$(arg realsense_prefix)_realsense_mountpoint_joint" type="fixed">
+      <origin xyz="$(arg realsense_xyz)" rpy="$(arg realsense_rpy)" />
+      <parent link="$(arg realsense_parent)"/>
+      <child link="$(arg realsense_prefix)_realsense_mountpoint" />
+    </joint>
+    <xacro:intel_realsense_mount prefix="$(arg realsense_prefix)" topic="$(arg realsense_topic)" parent_link="$(arg realsense_prefix)_realsense_mountpoint"/>
+  </xacro:if>
+
+  <xacro:if value="$(arg realsense_secondary_enabled)">
+    <link name="$(arg realsense_secondary_prefix)_realsense_mountpoint"/>
+    <joint name="$(arg realsense_secondary_prefix)_realsense_mountpoint_joint" type="fixed">
+      <origin xyz="$(arg realsense_secondary_xyz)" rpy="$(arg realsense_secondary_rpy)" />
+      <parent link="$(arg realsense_secondary_parent)"/>
+      <child link="$(arg realsense_secondary_prefix)_realsense_mountpoint" />
+    </joint>
+    <xacro:intel_realsense_mount prefix="$(arg realsense_secondary_prefix)" topic="$(arg realsense_secondary_topic)" parent_link="$(arg realsense_secondary_prefix)_realsense_mountpoint"/>
+  </xacro:if>
+
+  <!-- BlackflyS Camera Primary and Secondary -->
+  <xacro:if value="$(arg blackfly_enabled)">
+    <xacro:flir_blackfly_mount prefix="$(arg blackfly_prefix)"
+                                parent="$(arg blackfly_parent)"
+                                mount_enabled="$(arg blackfly_mount_enabled)"
+                                mount_angle="$(arg blackfly_mount_angle)">
+      <origin xyz="$(arg blackfly_xyz)" rpy="$(arg blackfly_rpy)"/>
+    </xacro:flir_blackfly_mount>
+  </xacro:if>
+
+  <xacro:if value="$(arg blackfly_secondary_enabled)">
+    <xacro:flir_blackfly_mount prefix="$(arg blackfly_secondary_prefix)"
+                                parent="$(arg blackfly_secondary_parent)"
+                                mount_enabled="$(arg blackfly_secondary_mount_enabled)"
+                                mount_angle="$(arg blackfly_secondary_mount_angle)">
+      <origin xyz="$(arg blackfly_secondary_xyz)" rpy="$(arg blackfly_secondary_rpy)"/>
+    </xacro:flir_blackfly_mount>
+  </xacro:if>
+
+  <!--
+    Velodyne 3D LiDAR Primary and Secondary
+  -->
+  <xacro:if value="$(arg laser_3d_enabled)">
+    <xacro:if value="$(arg laser_3d_tower)">
+      <xacro:vlp16_mount prefix="$(arg laser_3d_prefix)" parent_link="$(arg laser_3d_parent)" topic="$(arg laser_3d_topic)">
+        <origin xyz="$(arg laser_3d_xyz)" rpy="$(arg laser_3d_rpy)" />
+      </xacro:vlp16_mount>
+    </xacro:if>
+    <xacro:unless value="$(arg laser_3d_tower)">
+      <xacro:VLP-16 parent="$(arg laser_3d_parent)" topic="$(arg laser_3d_topic)" name="$(arg laser_3d_prefix)velodyne">
+        <origin xyz="$(arg laser_3d_xyz)" rpy="$(arg laser_3d_rpy)" />
+      </xacro:VLP-16>
+    </xacro:unless>
+  </xacro:if>
+
+  <xacro:if value="$(arg laser_3d_secondary_enabled)">
+    <xacro:if value="$(arg laser_3d_secondary_tower)">
+      <xacro:vlp16_mount prefix="$(arg laser_3d_secondary_prefix)" parent_link="$(arg laser_3d_secondary_parent)" topic="$(arg laser_3d_secondary_topic)">
+        <origin xyz="$(arg laser_3d_secondary_xyz)" rpy="$(arg laser_3d_secondary_rpy)" />
+      </xacro:vlp16_mount>
+    </xacro:if>
+    <xacro:unless value="$(arg laser_3d_secondary_tower)">
+      <xacro:VLP-16 parent="$(arg laser_3d_secondary_parent)" topic="$(arg laser_3d_secondary_topic)" name="$(arg laser_3d_secondary_prefix)velodyne">
+        <origin xyz="$(arg laser_3d_secondary_xyz)" rpy="$(arg laser_3d_secondary_rpy)" />
+      </xacro:VLP-16>
+    </xacro:unless>
+  </xacro:if>
+
+  <gazebo>
+    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
+      <robotNamespace>$(arg robot_namespace)</robotNamespace>
+      <legacyModeNS>true</legacyModeNS>
+    </plugin>
+  </gazebo>
+
+  <gazebo>
+    <plugin name="imu_controller" filename="libhector_gazebo_ros_imu.so">
+      <robotNamespace>$(arg robot_namespace)</robotNamespace>
+      <updateRate>50.0</updateRate>
+      <bodyName>base_link</bodyName>
+      <topicName>imu/data</topicName>
+      <accelDrift>0.005 0.005 0.005</accelDrift>
+      <accelGaussianNoise>0.005 0.005 0.005</accelGaussianNoise>
+      <rateDrift>0.005 0.005 0.005 </rateDrift>
+      <rateGaussianNoise>0.005 0.005 0.005 </rateGaussianNoise>
+      <headingDrift>0.005</headingDrift>
+      <headingGaussianNoise>0.005</headingGaussianNoise>
+    </plugin>
+  </gazebo>
+
+  <gazebo>
+    <plugin name="gps_controller" filename="libhector_gazebo_ros_gps.so">
+      <robotNamespace>$(arg robot_namespace)</robotNamespace>
+      <updateRate>40</updateRate>
+      <bodyName>base_link</bodyName>
+      <frameId>base_link</frameId>
+      <topicName>navsat/fix</topicName>
+      <velocityTopicName>navsat/vel</velocityTopicName>
+      <referenceLatitude>$(optenv GAZEBO_WORLD_LAT 49.9)</referenceLatitude>
+      <referenceLongitude>$(optenv GAZEBO_WORLD_LON 8.9)</referenceLongitude>
+      <referenceHeading>90</referenceHeading>
+      <referenceAltitude>0</referenceAltitude>
+      <drift>0.0001 0.0001 0.0001</drift>
+    </plugin>
+  </gazebo>
+
+  <!-- Optional custom includes. -->
+  <xacro:include filename="$(arg urdf_extras)" />
+
+  <!-- Optional for Clearpath internal softwares -->
+  <xacro:include filename="$(arg cpr_urdf_extras)" />
+
+</robot>
diff --git a/docker/husky_navigation/launch/move_base_fast_rrt_star.launch b/docker/husky_navigation/launch/move_base_fast_rrt_star.launch
new file mode 100644
index 0000000..c539638
--- /dev/null
+++ b/docker/husky_navigation/launch/move_base_fast_rrt_star.launch
@@ -0,0 +1,58 @@
+<?xml version="1.0"?>
+<!--
+Software License Agreement (BSD)
+
+\file      description.launch
+\authors   Paul Bovbel <pbovbel@clearpathrobotics.com>, Prasenjit Mukherjee <pmukherj@clearpathrobotics.com>
+\copyright Copyright (c) 2015, Clearpath Robotics, Inc., All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification, are permitted provided that
+the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice, this list of conditions and the
+   following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the 
+   following disclaimer in the documentation and/or other materials provided with the distribution.
+ * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote
+   products derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR-
+RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN-
+DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
+OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE
+)ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+-->
+<launch>
+
+  <arg name="no_static_map" default="false"/>
+
+  <arg name="base_global_planner" default="fast_rrt_star_planner/Fast_RRTStarPlanner"/>
+  <arg name="base_local_planner" default="dwa_local_planner/DWAPlannerROS"/>
+  <!-- <arg name="base_local_planner" default="base_local_planner/TrajectoryPlannerROS"/> -->
+
+  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+
+    <param name="base_global_planner" value="$(arg base_global_planner)"/>
+    <param name="base_local_planner" value="$(arg base_local_planner)"/>  
+    <rosparam file="$(find husky_navigation)/config/planner.yaml" command="load"/>
+
+    <!-- observation sources located in costmap_common.yaml -->
+    <rosparam file="$(find husky_navigation)/config/costmap_common.yaml" command="load" ns="global_costmap" />
+    <rosparam file="$(find husky_navigation)/config/costmap_common.yaml" command="load" ns="local_costmap" />
+
+    <!-- local costmap, needs size -->
+    <rosparam file="$(find husky_navigation)/config/costmap_local.yaml" command="load" ns="local_costmap" />
+    <param name="local_costmap/width" value="10.0"/>
+    <param name="local_costmap/height" value="10.0"/>
+
+    <!-- static global costmap, static map provides size -->
+    <rosparam file="$(find husky_navigation)/config/costmap_global_static.yaml" command="load" ns="global_costmap" unless="$(arg no_static_map)"/>
+    
+    <!-- global costmap with laser, for odom_navigation_demo -->
+    <rosparam file="$(find husky_navigation)/config/costmap_global_laser.yaml" command="load" ns="global_costmap" if="$(arg no_static_map)"/>
+    <param name="global_costmap/width" value="100.0" if="$(arg no_static_map)"/>
+    <param name="global_costmap/height" value="100.0" if="$(arg no_static_map)"/>
+  </node>
+
+</launch>
diff --git a/docker/husky_navigation/launch/move_base_fast_rrt_star_debug.launch b/docker/husky_navigation/launch/move_base_fast_rrt_star_debug.launch
new file mode 100644
index 0000000..d3c5de6
--- /dev/null
+++ b/docker/husky_navigation/launch/move_base_fast_rrt_star_debug.launch
@@ -0,0 +1,58 @@
+<?xml version="1.0"?>
+<!--
+Software License Agreement (BSD)
+
+\file      description.launch
+\authors   Paul Bovbel <pbovbel@clearpathrobotics.com>, Prasenjit Mukherjee <pmukherj@clearpathrobotics.com>
+\copyright Copyright (c) 2015, Clearpath Robotics, Inc., All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification, are permitted provided that
+the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice, this list of conditions and the
+   following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the 
+   following disclaimer in the documentation and/or other materials provided with the distribution.
+ * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote
+   products derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR-
+RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN-
+DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
+OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE
+)ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+-->
+<launch>
+
+  <arg name="no_static_map" default="false"/>
+
+  <arg name="base_global_planner" default="fast_rrt_star_planner/Fast_RRTStarPlanner"/>
+  <arg name="base_local_planner" default="dwa_local_planner/DWAPlannerROS"/>
+  <!-- <arg name="base_local_planner" default="base_local_planner/TrajectoryPlannerROS"/> -->
+
+  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" launch-prefix="valgrind --leak-check=full --show-leak-kinds=all">
+
+    <param name="base_global_planner" value="$(arg base_global_planner)"/>
+    <param name="base_local_planner" value="$(arg base_local_planner)"/>  
+    <rosparam file="$(find husky_navigation)/config/planner.yaml" command="load"/>
+
+    <!-- observation sources located in costmap_common.yaml -->
+    <rosparam file="$(find husky_navigation)/config/costmap_common.yaml" command="load" ns="global_costmap" />
+    <rosparam file="$(find husky_navigation)/config/costmap_common.yaml" command="load" ns="local_costmap" />
+
+    <!-- local costmap, needs size -->
+    <rosparam file="$(find husky_navigation)/config/costmap_local.yaml" command="load" ns="local_costmap" />
+    <param name="local_costmap/width" value="10.0"/>
+    <param name="local_costmap/height" value="10.0"/>
+
+    <!-- static global costmap, static map provides size -->
+    <rosparam file="$(find husky_navigation)/config/costmap_global_static.yaml" command="load" ns="global_costmap" unless="$(arg no_static_map)"/>
+    
+    <!-- global costmap with laser, for odom_navigation_demo -->
+    <rosparam file="$(find husky_navigation)/config/costmap_global_laser.yaml" command="load" ns="global_costmap" if="$(arg no_static_map)"/>
+    <param name="global_costmap/width" value="100.0" if="$(arg no_static_map)"/>
+    <param name="global_costmap/height" value="100.0" if="$(arg no_static_map)"/>
+  </node>
+
+</launch>
diff --git a/docker/husky_navigation/launch/move_base_fast_rrt_star_mapless_demo.launch b/docker/husky_navigation/launch/move_base_fast_rrt_star_mapless_demo.launch
new file mode 100644
index 0000000..8e7ea7e
--- /dev/null
+++ b/docker/husky_navigation/launch/move_base_fast_rrt_star_mapless_demo.launch
@@ -0,0 +1,33 @@
+<?xml version="1.0"?>
+<!--
+Software License Agreement (BSD)
+
+\file      description.launch
+\authors   Paul Bovbel <pbovbel@clearpathrobotics.com>, Prasenjit Mukherjee <pmukherj@clearpathrobotics.com>
+\copyright Copyright (c) 2015, Clearpath Robotics, Inc., All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification, are permitted provided that
+the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice, this list of conditions and the
+   following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the 
+   following disclaimer in the documentation and/or other materials provided with the distribution.
+ * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote
+   products derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR-
+RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN-
+DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
+OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+-->
+<launch>
+  
+  <!--- Run Move Base -->  
+  <include file="$(find husky_navigation)/launch/move_base_fast_rrt_star.launch">
+    <arg name="no_static_map" value="true"/>
+  </include>
+  
+</launch>
diff --git a/docker/husky_navigation/launch/move_base_fast_rrt_star_mapless_demo_debug.launch b/docker/husky_navigation/launch/move_base_fast_rrt_star_mapless_demo_debug.launch
new file mode 100644
index 0000000..1281ca5
--- /dev/null
+++ b/docker/husky_navigation/launch/move_base_fast_rrt_star_mapless_demo_debug.launch
@@ -0,0 +1,33 @@
+<?xml version="1.0"?>
+<!--
+Software License Agreement (BSD)
+
+\file      description.launch
+\authors   Paul Bovbel <pbovbel@clearpathrobotics.com>, Prasenjit Mukherjee <pmukherj@clearpathrobotics.com>
+\copyright Copyright (c) 2015, Clearpath Robotics, Inc., All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification, are permitted provided that
+the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice, this list of conditions and the
+   following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the 
+   following disclaimer in the documentation and/or other materials provided with the distribution.
+ * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote
+   products derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR-
+RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN-
+DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
+OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+-->
+<launch>
+  
+  <!--- Run Move Base -->  
+  <include file="$(find husky_navigation)/launch/move_base_fast_rrt_star_debug.launch">
+    <arg name="no_static_map" value="true"/>
+  </include>
+  
+</launch>
diff --git a/docker/scripts/1_gazebo.bash b/docker/scripts/1_gazebo.bash
new file mode 100755
index 0000000..6b0ac9d
--- /dev/null
+++ b/docker/scripts/1_gazebo.bash
@@ -0,0 +1,3 @@
+#!/bin/bash
+
+roslaunch husky_gazebo husky_playpen.launch
\ No newline at end of file
diff --git a/docker/scripts/2_rviz.bash b/docker/scripts/2_rviz.bash
new file mode 100755
index 0000000..bddddd1
--- /dev/null
+++ b/docker/scripts/2_rviz.bash
@@ -0,0 +1,3 @@
+#!/bin/bash
+
+roslaunch husky_viz view_robot.launch
\ No newline at end of file
diff --git a/docker/scripts/3_default.bash b/docker/scripts/3_default.bash
new file mode 100755
index 0000000..fe7d562
--- /dev/null
+++ b/docker/scripts/3_default.bash
@@ -0,0 +1,7 @@
+#!/bin/bash
+
+source /opt/ros/$ROS_DISTRO/setup.bash && \
+rm -rf build/ devel/ && \
+catkin_make -DCMAKE_BUILD_TYPE=Release && \
+source devel/setup.bash && \
+roslaunch husky_navigation move_base_mapless_demo.launch
diff --git a/docker/scripts/3_fast_rrt_star.bash b/docker/scripts/3_fast_rrt_star.bash
new file mode 100755
index 0000000..b959def
--- /dev/null
+++ b/docker/scripts/3_fast_rrt_star.bash
@@ -0,0 +1,7 @@
+#!/bin/bash
+
+source /opt/ros/$ROS_DISTRO/setup.bash && \
+rm -rf build/ devel/ && \
+catkin_make -DCMAKE_BUILD_TYPE=Release && \
+source devel/setup.bash && \
+roslaunch husky_navigation move_base_fast_rrt_star_mapless_demo.launch
diff --git a/docker/scripts/3_fast_rrt_star_debug.bash b/docker/scripts/3_fast_rrt_star_debug.bash
new file mode 100755
index 0000000..f8b0f14
--- /dev/null
+++ b/docker/scripts/3_fast_rrt_star_debug.bash
@@ -0,0 +1,7 @@
+#!/bin/bash
+
+source /opt/ros/$ROS_DISTRO/setup.bash && \
+rm -rf build/ devel/ && \
+catkin_make -DCMAKE_BUILD_TYPE=RelWithDebInfo && \
+source devel/setup.bash && \
+roslaunch husky_navigation move_base_fast_rrt_star_mapless_demo.launch
diff --git a/docker/scripts/3_fast_rrt_star_debug_valgrind.bash b/docker/scripts/3_fast_rrt_star_debug_valgrind.bash
new file mode 100755
index 0000000..96b930d
--- /dev/null
+++ b/docker/scripts/3_fast_rrt_star_debug_valgrind.bash
@@ -0,0 +1,7 @@
+#!/bin/bash
+
+source /opt/ros/$ROS_DISTRO/setup.bash && \
+rm -rf build/ devel/ && \
+catkin_make -DCMAKE_BUILD_TYPE=RelWithDebInfo && \
+source devel/setup.bash && \
+roslaunch husky_navigation move_base_fast_rrt_star_mapless_demo_debug.launch