Initial Commit

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

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

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/

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})

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

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`.

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>

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 */

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 */

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 */

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>

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;
+  }
+}

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);
+  }
+}

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;
+  }
+};

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>

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>

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>

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>

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>

docker/scripts/1_gazebo.bash

@@ -0,0 +1,3 @@
+#!/bin/bash
+
+roslaunch husky_gazebo husky_playpen.launch

docker/scripts/2_rviz.bash

@@ -0,0 +1,3 @@
+#!/bin/bash
+
+roslaunch husky_viz view_robot.launch

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

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

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

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