Repository: moribots/motion_planning
Branch: master
Commit: 4a479a067ef5
Files: 53
Total size: 268.8 KB

Directory structure:
gitextract_wllwl564/

├── README.md
├── control/
│   ├── CMakeLists.txt
│   ├── cmake/
│   │   └── FindOsqpEigen.cmake
│   ├── config/
│   │   ├── parallel.yaml
│   │   └── waypoints.yaml
│   ├── include/
│   │   └── control/
│   │       ├── Models.hpp
│   │       ├── TrajMPC.hpp
│   │       ├── rk4.hpp
│   │       └── utilities.hpp
│   ├── launch/
│   │   ├── mppi_pentagon.launch
│   │   └── osqp_example.launch
│   ├── msg/
│   │   └── Waypoint.msg
│   ├── package.xml
│   └── src/
│       ├── control/
│       │   ├── Models.cpp
│       │   ├── TrajMPC.cpp
│       │   ├── rk4.cpp
│       │   └── utilities.cpp
│       ├── mppi
│       ├── osqp_eigen_example.cpp
│       └── osqp_example.cpp
├── global_planner/
│   ├── CMakeLists.txt
│   ├── config/
│   │   └── path.yaml
│   ├── include/
│   │   └── global_planner/
│   │       ├── global_planner.hpp
│   │       ├── heuristic.hpp
│   │       ├── incremental.hpp
│   │       └── potential_field.hpp
│   ├── launch/
│   │   ├── astar.launch
│   │   ├── incremental.launch
│   │   └── potential_field.launch
│   ├── package.xml
│   ├── rviz/
│   │   └── astar.rviz
│   └── src/
│       ├── astar.cpp
│       ├── dsl.cpp
│       ├── global_planner/
│       │   ├── global_planner.cpp
│       │   ├── heuristic.cpp
│       │   ├── incremental.cpp
│       │   └── potential_field.cpp
│       ├── lpastar.cpp
│       └── pf.cpp
├── map/
│   ├── CMakeLists.txt
│   ├── config/
│   │   └── map.yaml
│   ├── include/
│   │   └── map/
│   │       ├── grid.hpp
│   │       ├── map.hpp
│   │       └── prm.hpp
│   ├── launch/
│   │   └── viz_map.launch
│   ├── package.xml
│   ├── rviz/
│   │   └── basic_map.rviz
│   └── src/
│       ├── map/
│       │   ├── grid.cpp
│       │   ├── map.cpp
│       │   └── prm.cpp
│       ├── viz_grid.cpp
│       └── viz_map.cpp
└── nuturtle.rosinstall

================================================
FILE CONTENTS
================================================

================================================
FILE: README.md
================================================
## Motion Planning Library with ROS

Self-directed independent study.

### Installation Guide:
- Make a ROS Workspace: `mkdir -p ws/src`
- Go to `src` and clone my repo: `cd ws/src/` `git clone git@github.com:moribots/motion_planning.git`
- Go back to the workspace root: `cd ..`
- Initialize `nuturtle.rosinstall` to get my rigid2d library and other utilities: `wstool init src src/motion_planning/nuturtle.rosinstall`
- Build the workspace: `catkin_make` and source: `source devel/setup.bash`

### OSQP and OSQP-Eigen installation
[OSQP](https://osqp.org/docs/get_started/sources.html)
[OSQP-Eigen (C++ wrapper)](https://github.com/robotology/osqp-eigen) - Do the manual install.

Notes for OSQP-Eigen:
- Choose `/usr/local` in `cmake -DCMAKE_INSTALL_PREFIX:PATH=<custom-folder> ../`
- Do `sudo ln -s /usr/include/eigen3/Eigen /usr/local/include/Eigen` to make a symbolic link for the Eigen header files so that OSQP-Eigen knows where to find them.

### In this Repo:
* The `map` package: `roslaunch map viz_map.launch`
	- Probabilistic Roadmap:

	<img src="map/media/prm.png" alt="PRM" width="300"/>

	- Tunable-resolution Grid Map

	<img src="map/media/grid.png" alt="GRID" width="300"/>

* The `global_planner` package:
	- A* (green) on PRM:

	<img src="global_planner/media/astar.png" alt="ASTAR" width="300"/>

	- Theta* (green) on PRM (A* in red for comparison): `roslaunch global_planner astar.launch`

	<img src="global_planner/media/thetastar.png" alt="ASTAR" width="300"/>

	- A* (green) on Grid

	<img src="global_planner/media/lpastar.png" alt="ASTARG" width="300"/>

	- LPA* with Simulated Grid Updates [re-evaluated cells in orange]: `roslaunch global_planner incremental.launch lpa:=True`
	<img src="global_planner/media/LPAstar.gif" alt="LPASTAR" width="300"/>

	- D* Lite on Grid [re-evaluated cells in orange]: `roslaunch global_planner incremental.launch`
	<img src="global_planner/media/DstarLite.gif" alt="DSL" width="300"/>

	- Naive Potential Field (Local Minimum Escape TBD): `roslaunch global_planner potential_field.launch`
	<img src="global_planner/media/PF_NAIVE.gif" alt="PFN" width="300"/>

<!-- * Local planner:
	- Dynamic Window Approach -->
* Trajectory Optimization:
	- Model Predictive Path Integral Control on a parallel parking task: `roslaunch control mppi_pentagon.launch parallel:=True`
	<img src="control/media/mppi_parallel_park.gif" alt="PFN" width="450"/>

	- The associated states and controls for the above demo:
	<img src="control/media/mppi_plt.gif" alt="PFN" width="500"/>

	NOTE: To launch waypoint following method, simply don't include the `parallel` argument.

	In a separate terminal, do:

	`rosservice call /set_pose "x: 0.0
	y: 0.0
	theta: 0.0"`

	To start the node.



================================================
FILE: control/CMakeLists.txt
================================================
cmake_minimum_required(VERSION 3.0.2)
project(control)

## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-Wall -Wextra -pipe -Wno-psabi)

set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
  global_planner
  map	
  message_generation
  message_runtime
  rigid2d
  roscpp
  rospy
  rostest
  nuslam
  visualization_msgs
  std_msgs
)

## System dependencies are found with CMake's conventions
# list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/cmake")
find_package(Eigen3 3.3 REQUIRED NO_MODULE)

find_package(osqp REQUIRED)

find_package(OsqpEigen REQUIRED)

set(osqp_INCLUDE_DIR /usr/local/include/osqp)
set(OsqpEigen_INCLUDE_DIR /usr/local/include/OsqpEigen)

message(STATUS "OsqpEigen_INCLUDE_DIR : " ${OsqpEigen_INCLUDE_DIR})
message(STATUS "osqp_INCLUDE_DIR : " ${osqp_INCLUDE_DIR})

# message(STATUS "QP : " ${OsqpEigen_FOUND})
# message(STATUS "QP lib: " ${OsqpEigen_LIBRARIES})
# message(STATUS "QP include: " ${OsqpEigen_INCLUDE_DIRS})

# if (OsqpEigenFOUND)
#   message(STATUS "Found OsqpEigen")
# else ()
#   message(ERROR " OsqpEigen not found")
# endif ()


## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()

################################################
## Declare ROS messages, services and actions ##
################################################

## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
##   * add a build_depend tag for "message_generation"
##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
##     but can be declared for certainty nonetheless:
##     * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
##   * add "message_generation" and every package in MSG_DEP_SET to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * add "message_runtime" and every package in MSG_DEP_SET to
##     catkin_package(CATKIN_DEPENDS ...)
##   * uncomment the add_*_files sections below as needed
##     and list every .msg/.srv/.action file to be processed
##   * uncomment the generate_messages entry below
##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)

## Generate messages in the 'msg' folder
add_message_files(
  FILES
  Waypoint.msg
)

## Generate services in the 'srv' folder
# add_service_files(
#   FILES
#   Service1.srv
#   Service2.srv
# )

## Generate actions in the 'action' folder
# add_action_files(
#   FILES
#   Action1.action
#   Action2.action
# )

## Generate added messages and services with any dependencies listed here
generate_messages(
  DEPENDENCIES
  std_msgs  # Or other packages containing msgs
)

################################################
## Declare ROS dynamic reconfigure parameters ##
################################################

## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
##   * add "dynamic_reconfigure" to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * uncomment the "generate_dynamic_reconfigure_options" section below
##     and list every .cfg file to be processed

## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
#   cfg/DynReconf1.cfg
#   cfg/DynReconf2.cfg
# )

###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME} global_planner map rigid2d nuslam
CATKIN_DEPENDS global_planner map message_generation message_runtime rigid2d roscpp nuslam visualization_msgs std_msgs
#  DEPENDS system_lib OsqpEigen
)

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
  include
  ${catkin_INCLUDE_DIRS}
  ${Eigen3_INCLUDE_DIRS}
  ${OsqpEigen_INCLUDE_DIR}
  ${osqp_INCLUDE_DIR}
)

## Declare a C++ library
add_library(${PROJECT_NAME}
  src/${PROJECT_NAME}/rk4.cpp
  src/${PROJECT_NAME}/TrajMPC.cpp
  src/${PROJECT_NAME}/Models.cpp
  src/${PROJECT_NAME}/utilities.cpp
)

## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
add_executable(osqp_example src/osqp_example.cpp)
add_executable(osqp_eigen_example src/osqp_eigen_example.cpp)

## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
set_target_properties(osqp_example PROPERTIES OUTPUT_NAME osqp_example PREFIX "")
set_target_properties(osqp_eigen_example PROPERTIES OUTPUT_NAME osqp_eigen_example PREFIX "")

## Add cmake target dependencies of the executable
## same as for the library above
add_dependencies(osqp_example ${${PROJECT_NAME}_EXPORTED_TARGETS} ${Eigen3_EXPORTED_TARGETS} ${rigid2d_EXPORTED_TARGETS} ${nuslam_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(osqp_eigen_example ${${PROJECT_NAME}_EXPORTED_TARGETS} ${Eigen3_EXPORTED_TARGETS} ${rigid2d_EXPORTED_TARGETS} ${nuslam_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Specify libraries to link a library or executable target against
target_link_libraries(
osqp_example # this is my node that will use below libraries
${rigid2d_LIBRARIES}
${nuslam_LIBRARIES}
Eigen3::Eigen
osqp::osqp
${PROJECT_NAME}
${catkin_LIBRARIES}
)

target_link_libraries(
osqp_eigen_example # this is my node that will use below libraries
${rigid2d_LIBRARIES}
${nuslam_LIBRARIES}
Eigen3::Eigen
osqp::osqp
OsqpEigen::OsqpEigen
${PROJECT_NAME}
${catkin_LIBRARIES}
)

#############
## Install ##
#############

# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html

## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
install(PROGRAMS
  src/mppi
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
install(TARGETS osqp_example osqp_eigen_example
  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

## Mark libraries for installation
install(TARGETS ${PROJECT_NAME}
  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
)

## Mark cpp header files for installation
install(DIRECTORY include/${PROJECT_NAME}/
  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
  FILES_MATCHING PATTERN "*.h"
  PATTERN ".svn" EXCLUDE
)

#############
## Testing ##
#############

## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_control.cpp)
# if(TARGET ${PROJECT_NAME}-test)
#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()

## Add folders to be run by python nosetests
# catkin_add_nosetests(test)


================================================
FILE: control/cmake/FindOsqpEigen.cmake
================================================
#.rst:
# FindOsqpEigen
# -----------
#
# Try to find the OsqpEigen library.
# Once done this will define the following variables::
#
#  OsqpEigen_FOUND         - System has OsqpEigen
#  OsqpEigen_INCLUDE_DIRS  - OsqpEigen include directory
#  OsqpEigen_LIBRARIES     - OsqpEigen libraries


include(FindPackageHandleStandardArgs)

find_path(OsqpEigen_INCLUDEDIR
          NAMES OsqpEigen.hpp
          HINTS "${OsqpEigen_SOURCE_DIR}"
                ENV OsqpEigen_SOURCE_DIR
          PATH_SUFFIXES include)
          
find_library(OsqpEigen_LIB
             NAMES OsqpEigen
             HINTS "${OsqpEigen_BINARY_DIR}"
                   ENV OsqpEigen_BINARY_DIR
             PATH_SUFFIXES lib
                           libs)

set(OsqpEigen_INCLUDE_DIRS /home/mori/Desktop/Projects/solvers/osqp-eigen/build/include/)
set(OsqpEigen_LIBRARIES /home/mori/Desktop/Projects/solvers/osqp-eigen/build/lib/)

find_package_handle_standard_args(OsqpEigen DEFAULT_MSG OsqpEigen_LIBRARIES
                                                      OsqpEigen_INCLUDE_DIRS)
set(OsqpEigen_FOUND ${OsqpEigen_FOUND})

================================================
FILE: control/config/parallel.yaml
================================================
waypoints: []

================================================
FILE: control/config/waypoints.yaml
================================================
waypoints: [[1, 0], [2, 1], [1, 2], [0, 2], [0, 0]]

================================================
FILE: control/include/control/Models.hpp
================================================
#pragma once
/// \file
/// \brief Dynamic Models: Differential Drive.

#include <eigen3/Eigen/Dense>
#include <functional>
#include <iostream>

#include "control/utilities.hpp"

namespace control {
namespace models {
using Eigen::MatrixXd;
using Eigen::Ref;
using Eigen::Vector2d;
using Eigen::VectorXd;

/**
 * @brief Differential Drive Model.
 *
 */
class DiffDrive {
 public:
  /**
   * @brief Construct a new Diff Drive object which creates the following:
   *
   * m_a: A matrix (xdot = Ax + Bu).
   * m_b: B matrix (xdot = Ax + Bu). Depends on heading angle.
   * m_gradient: EOM first derivative (to be set by TrajMPC).
   * m_hessian: EOM second derivative (to be set by TrajMPC).
   * m_linear_constraint_matrix: container for linear constraints in QP problem.
   * m_lower_bound, m_upper_bound: bounds for constraints in QP problem.
   *
   * @param wheel_radius Differential Drive robot's wheel radius in m.
   * @param wheel_base Differential Drive robot's wheelbase in m.
   * @param abs_max_wheel_vel Maximum Wheel Velocity (absolute) in rad/s.
   */
  DiffDrive(const double& wheel_radius, const double& wheel_base,
            const double& abs_max_wheel_vel)
      : m_wheel_radius(wheel_radius),
        m_wheel_base(wheel_base),
        m_abs_max_wheel_vel(abs_max_wheel_vel) {
    // DOI: 10.2478/auseme-2018-0002 Model Predictive Control of a
    // Differential-Drive Mobile Robot Samir BOUZOUALEGH 1 , El-Hadi GUECHI 1
    // and Ridha KELAIAIA 2

    // A matrix is identity since no actuation = no movement.
    m_a = MatrixXd::Identity(3, 3);
  }

  /**
   * @brief Returns the number of variables in this optimization.
   *
   * @param mpcWindow Timesteps for which we perform MPC.
   */
  int num_variables(const int& mpcWindow) {
    return m_a.rows() * (mpcWindow + 1) + m_b.cols() * mpcWindow;
  }

  /**
   * @brief Returns the number of constraints in this optimization.
   *
   * @param mpcWindow Timesteps for which we perform MPC.
   */
  int num_constraints(const int& mpcWindow) {
    return 2 * m_a.rows() * (mpcWindow + 1) + m_b.cols() * mpcWindow;
  }

  /**
   * @brief
   *
   * @return Eigen::SparseMatrix<double>&
   */
  Eigen::SparseMatrix<double>& hessian() { return m_hessian; }

  /**
   * @brief
   *
   * @return VectorXd&
   */
  VectorXd& gradient() { return m_gradient; }

  /**
   * @brief
   *
   * @return MatrixXd&
   */
  MatrixXd& linear_constraint_matrix() { return m_linear_constraint_matrix; }

  /**
   * @brief
   *
   * @return VectorXd&
   */
  VectorXd& lower_bound() { return m_lower_bound; }

  /**
   * @brief
   *
   * @return VectorXd&
   */
  VectorXd& upper_bound() { return m_upper_bound; }

  /**
   * @brief
   *
   * @return MatrixXd&
   */
  MatrixXd a() { return m_a; }

  /**
   * @brief
   *
   * @param theta Robot heading in radians.
   * @return MatrixXd
   */
  MatrixXd b(const MatrixXd& state) {
    // B matrix is a function of heading and robot params - see mppi node.
    const double& theta = state(2);

    /**
     * r: wheel radius
     * wb: wheel base
     * u0: left wheel speed
     * u1: right wheel speed
     * th: robot heading
     * | 0.5rcos(th), 0.5rcos(th) |     | u0 |
     * | 0.5rsin(th), 0.5rsin(th) |  X  |    |
     * | -r/wb, r/wb              |     | u1 |
     */

    m_b << 0.5 * m_wheel_radius * std::cos(theta),
        0.5 * m_wheel_radius * std::cos(theta),
        0.5 * m_wheel_radius * std::sin(theta),
        0.5 * m_wheel_radius * std::sin(theta), -m_wheel_radius / m_wheel_base,
        m_wheel_radius / m_wheel_base;

    return m_b;
  }

 private:
  double m_wheel_radius;
  double m_wheel_base;
  double m_abs_max_wheel_vel;
  Eigen::Matrix<double, 3, 3> m_a;
  Eigen::Matrix<double, 3, 2> m_b;
  VectorXd m_gradient;
  Eigen::SparseMatrix<double> m_hessian;
  MatrixXd m_linear_constraint_matrix;
  VectorXd m_lower_bound, m_upper_bound;
};
}  // namespace models
}  // namespace control

================================================
FILE: control/include/control/TrajMPC.hpp
================================================
#pragma once
/// \file
/// \brief Model Predictive Control for Trajectory Following

#include <OsqpEigen/OsqpEigen.h>

#include <eigen3/Eigen/Dense>
#include <functional>
#include <iostream>

#include "control/utilities.hpp"

namespace control {
using Eigen::MatrixXd;
using Eigen::Ref;
using Eigen::Vector2d;
using Eigen::VectorXd;

/**
 * @brief Model Predictive Control for Trajectory Following.
 *
 * Model: Contains the Dynamic Model for the optimization.
 *  METHODS:
 *    num_variables(): return number of variables in the problem.
 *    num_constraints(): return number of constraints in the problem.
 *    gradient(): return a reference to the dynamic model's gradient.
 *    hessian(): return a reference to the dynamic model's hessian.
 *    linear_constraint_matrix(): return the dynamic model's constraint matrix.
 *    lower_bound(): return a reference to the lower bound constraint vector.
 *    upper_bound(): return a reference to the upper bound constraint vector.
 *    a(): return A matrix (xdot = Ax + Bu)
 *    b(current state): return B matrix (xdot = Ax + Bu)
 *
 * Action: Container for returning minimizing commands.
 *
 * Problem: Contains the problem formulation for the optimization.
 *  METHODS:
 *    Q(): return state cost Matrix (LQR formulation).
 *    R(): return action cost Matrix (LQR formulation).
 *    xMax(): return upper state constraint bound.
 *    xMin(): return lower state constraint bound.
 *    uMax(): return upper action constraint bound.
 *    uMin(): return lower action constraint bound.
 *    x0(): return initial state for optimization.
 *    xRef(): return reference state for optimization.
 *
 */
template <typename Model, typename Problem, typename State, typename Action>
class TrajMPC {
 public:
  /**
   * @brief Constructor which takes in dynamic model.
   *
   * @param model contains the xdot = Ax + Bu transition, as well as the
   * constraints.
   */
  TrajMPC(const Model& model, const int& mpcWindow,
          const double& solve_threshold = 1.0e-12,
          const int& max_solve_steps = 50)
      : m_model(model),
        m_mpcWindow(mpcWindow),
        m_solve_threshold(solve_threshold),
        m_max_solve_steps(max_solve_steps) {}

  /**
   * @brief Set up OSQP Solver with relevant parameters.
   *
   * @param problem Container for problem information: Cost Matrices, State and
   * Control Limits, Reference and Initial Trajectory.
   * @return size_t solver setup result. 0 is success, 1 is failure.
   */
  size_t setup(const Problem& problem) {
    // Grab problem data.
    const auto& Q = problem.Q();
    const auto& R = problem.R();
    const auto& xMax = problem.xMax();
    const auto& xMin = problem.xMin();
    const auto& uMax = problem.uMax();
    const auto& uMin = problem.uMin();
    const auto& x0 = problem.x0();
    const auto& xRef = problem.xRef();

    // Grab model data. (xdot = Ax + Bu)
    const auto a = m_model.a();
    const auto b = m_model.b(x0);

    // cast the MPC problem as QP problem
    control::utilities::castMPCToQPHessian<Q.rows(), R.rows()>(
        Q, R, m_mpcWindow, m_model.hessian());
    control::utilities::castMPCToQPGradient<Q.rows(), xRef.rows(), xRef.cols()>(
        Q, xRef, m_mpcWindow, m_model.gradient());
    control::utilities::castMPCToQPConstraintMatrix<a.rows(), a.cols(),
                                                    b.rows(), b.cols()>(
        a, b, m_mpcWindow, m_model.linear_constraint_matrix());
    control::utilities::castMPCToQPConstraintVectors<xMax.rows(), uMax.rows(),
                                                     x0.rows()>(
        xMax, xMin, uMax, uMin, x0, m_mpcWindow, m_model.lower_bound(),
        m_model.upper_bound());

    // instantiate the solver
    OsqpEigen::Solver solver;

    // settings
    solver.settings()->setVerbosity(false);
    solver.settings()->setWarmStart(true);

    // set the initial data of the QP solver
    solver.data()->setNumberOfVariables(m_model.num_variables(m_mpcWindow));
    solver.data()->setNumberOfConstraints(m_model.num_constraints(m_mpcWindow));
    if (!solver.data()->setHessianMatrix(m_model.hessian())) return 1;
    if (!solver.data()->setGradient(m_model.gradient())) return 1;
    if (!solver.data()->setLinearConstraintsMatrix(
            m_model.linear_constraint_matrix()))
      return 1;
    if (!solver.data()->setLowerBound(m_model.lower_bound())) return 1;
    if (!solver.data()->setUpperBound(m_model.upper_bound())) return 1;

    // instantiate the solver
    if (!solver.initSolver()) return 1;

    m_solver = solver;
    return 0;
  }

  /**
   * @brief Assign the optimal control action around the desired reference and
   * return an exit code for this OSQP solver iteration.
   *
   * @param problem Container for problem information: Cost Matrices, State and
   * Control Limits, Reference and Initial Trajectory.
   * @param action Container for assigning optimal control action.
   * @return OSQP exit code.
   */
  size_t solve(const Problem& problem, Action* action) {
    // Grab updated problem parameters
    const auto& Q = problem.Q();
    auto& x0 = problem.x0();
    const auto& xRef = problem.xRef();

    // Grab solver
    auto& solver = m_solver;

    // Update Gradient based on new xRef
    control::utilities::castMPCToQPGradient<Q.rows(), xRef.rows(), xRef.cols()>(
        Q, xRef, m_mpcWindow, m_model.gradient());
    if (!solver.updateGradient(m_model.gradient())) return 1;
    // Update Constraint Vector based on new initial state
    control::utilities::updateConstraintVectors<x0.rows()>(
        x0, m_model.lower_bound(), m_model.upper_bound());
    if (!solver.updateBounds(m_model.lower_bound(), m_model.upper_bound()))
      return 1;

    // controller input and QPSolution vector
    Eigen::Vector4d ctr;
    // Assign zero control to action in case of failure.
    action->cmd = ctr;
    Eigen::VectorXd QPSolution;

    // Exceeded max steps.
    size_t exit_code = -1;

    double obj_val = std::numeric_limits<double>::max();

    for (int i = 0; i < m_max_solve_steps; i++) {
      // solve the QP problem
      if (!solver.solve()) return 1;

      // get the controller input
      QPSolution = solver.getSolution();
      ctr = QPSolution.block(problem.xMax().rows() * (m_mpcWindow + 1), 0,
                             problem.uMax().rows(), 1);

      // save data into file
      // auto x0Data = x0.data();

      // propagate the model - TODO: RK4
      x0 = m_model.a() * x0 + m_model.b(x0) * ctr;

      // update the constraint bound
      control::utilities::updateConstraintVectors<x0.rows()>(
          x0, m_model.lower_bound(), m_model.upper_bound());
      if (!solver.updateBounds(m_model.lower_bound(), m_model.upper_bound()))
        return 1;

      /// Get the optimal objective value, can be used to exit early.
      /// https://osqp.org/docs/interfaces/cc++?highlight=osqpworkspace#_CPPv413OSQPWorkspace
      const auto new_obj_val = solver.workspace()->info->obj_val;

      if (std::abs(new_obj_val - obj_val) < m_solve_threshold) {
        obj_val = new_obj_val;
        break;
      }
      obj_val = new_obj_val;
    }
    action->cmd = ctr;
    return 0;
  }

 private:
  /// MPC Horizon.
  int m_mpcWindow;
  /// Cost change threshold for stopping optimization.
  double m_solve_threshold;
  /// Maximum number of optimization steps before giving up.
  int m_max_solve_steps;
  /// Dynamic Model used to solve optimization problem.
  Model m_model;
  /// OSQP Solver with Eigen C++ Wrapper.
  OsqpEigen::Solver m_solver;
};
}  // namespace control

================================================
FILE: control/include/control/rk4.hpp
================================================
#ifndef RK4_HPP
#define RK4_HPP
/// \file
/// \brief 4th Order Runge Kutta method for Integration

#include <iostream>
#include <eigen3/Eigen/Dense>
#include <functional>

namespace control
{
  using Eigen::MatrixXd;
  using Eigen::VectorXd;
  using Eigen::Vector2d;
  using Eigen::Ref;


  /// \brief Integrator
  class RK4
  {
    public:
      /// \brief Integrator constructor with fixed step size
      /// \param dt - time step for integration
      RK4(const double & dt);

      /// \brief Register a generic ODE without a control signal. updates the last input by ref (non-const)
      /// \param ode - function to integrate
      void registerODE(std::function<void(const Ref<VectorXd>, Ref<VectorXd>)> ode);

      /// \brief Register a generic ODE with a control signal. updates the last input by ref (non-const)
      /// \param ode - function to integrate
      void registerODE(std::function<void(const Ref<VectorXd>, const Ref<VectorXd>, Ref<VectorXd>)> ode);

      /// \brief Solve ODE or system of ODEs without a control signal using RK4
      /// \param x0 - initial condition(state)
      /// \param horizon - final time of integration
      MatrixXd solve(const Ref<VectorXd> x0, const double & horizon);

      /// \brief Solve ODE or system of ODEs with a control signal using RK4
      /// \param x0 - initial condition(state)
      /// \param u - control signal (must be of length equal to number of rk4 iterations => horizon/step)
      /// \param horizon - final time of integration
      MatrixXd solve(const Ref<VectorXd> x0, const Ref<MatrixXd> u, const double & horizon);
    private:
      /// \brief Perform one iteration of rk4 without a control signal
      /// x_t[out] - update the current state x_t
      void integrate(Ref<VectorXd> x_t);

      /// \brief Perform one iteration of rk4 with a control signal
      /// param u_t - control vector
      /// x_t[out] - update the current state x_t
      void integrate(Ref<VectorXd> x_t, const Ref<VectorXd> u_t);

      // ODE without control args: x(t), xdot[out]. updates the last input by ref (non-const)
      std::function<void(const Ref<VectorXd>, Ref<VectorXd>)> func;

      // ODE with control args: x(t), u(t), xdot[out]. updates the last input by ref (non-const)
      std::function<void(const Ref<VectorXd>, const Ref<VectorXd>, Ref<VectorXd>)> func_ctrl;

      // time step
      double dt;
  };
}
#endif

================================================
FILE: control/include/control/utilities.hpp
================================================
#pragma once
/// \file
/// \brief Utilities Library for Quadratic Programming

#include <eigen3/Eigen/Dense>
#include <functional>
#include <iostream>

#include "OsqpEigen/OsqpEigen.h"

namespace control {
namespace utilities {
using Eigen::DiagonalMatrix;
using Eigen::Dynamic;
using Eigen::MatrixXd;
using Eigen::Ref;
using Eigen::SparseMatrix;
using Eigen::Vector2d;
using Eigen::VectorXd;

template <int Q_rows, int R_rows>
void castMPCToQPHessian(const DiagonalMatrix<double, Q_rows> &Q,
                        const DiagonalMatrix<double, R_rows> &R, int mpcWindow,
                        SparseMatrix<double> &hessianMatrix) {
  // Return size of diagonal matrices: size = row * col so we use sqrt.
  const int Q_size = std::sqrt(Q.size());
  const int R_size = std::sqrt(R.size());

  hessianMatrix.resize(Q_size * (mpcWindow + 1) + R_size * mpcWindow,
                       Q_size * (mpcWindow + 1) + R_size * mpcWindow);

  // populate hessian matrix
  for (auto i = 0; i < Q_size * (mpcWindow + 1) + R_size * mpcWindow; i++) {
    if (i < Q_size * (mpcWindow + 1)) {
      int posQ = i % Q_size;
      float value = Q.diagonal()[posQ];
      if (value != 0) hessianMatrix.insert(i, i) = value;
    } else {
      int posR = i % R_size;
      float value = R.diagonal()[posR];
      if (value != 0) hessianMatrix.insert(i, i) = value;
    }
  }
}

template <int Q_rows, int xRef_rows, int xRef_cols>
void castMPCToQPGradient(
    const Eigen::DiagonalMatrix<double, Q_rows> &Q,
    const Eigen::Matrix<double, xRef_rows, xRef_cols> &xRef, int mpcWindow,
    Eigen::VectorXd &gradient) {
  // Return size of diagonal matrices: size = row * col so we use sqrt.
  const int Q_size = std::sqrt(Q.size());

  Eigen::Matrix<double, xRef_rows, xRef_cols> Qx_ref;
  Qx_ref = Q * (-xRef);

  // populate the gradient vector
  gradient = Eigen::VectorXd::Zero(Q_size * (mpcWindow + 1) + 4 * mpcWindow, 1);
  for (auto i = 0; i < Q_size * (mpcWindow + 1); i++) {
    int posQ = i % Q_size;
    float value = Qx_ref(posQ, 0);
    gradient(i, 0) = value;
  }
}

template <int dyn_rows, int dyn_cols, int ctrl_rows, int ctrl_cols>
void castMPCToQPConstraintMatrix(
    const Eigen::Matrix<double, dyn_rows, dyn_cols> &dynamicMatrix,
    const Eigen::Matrix<double, ctrl_rows, ctrl_cols> &controlMatrix,
    int mpcWindow, Eigen::SparseMatrix<double> &constraintMatrix) {
  // Get Rows and Cols from dynamicMatrix and controlMatrix.
  const int dynamicMatrix_rows = dynamicMatrix.rows();
  const int dynamicMatrix_cols = dynamicMatrix.cols();
  const int controlMatrix_rows = controlMatrix.rows();
  const int controlMatrix_cols = controlMatrix.cols();

  constraintMatrix.resize(
      dynamicMatrix_rows * (mpcWindow + 1) +
          dynamicMatrix_cols * (mpcWindow + 1) + controlMatrix_cols * mpcWindow,
      controlMatrix_rows * (mpcWindow + 1) + controlMatrix_cols * mpcWindow);

  // populate linear constraint matrix
  for (auto i = 0; i < dynamicMatrix_rows * (mpcWindow + 1); i++) {
    constraintMatrix.insert(i, i) = -1;
  }

  for (auto i = 0; i < mpcWindow; i++)
    for (int j = 0; j < dynamicMatrix_rows; j++)
      for (int k = 0; k < dynamicMatrix_rows; k++) {
        float value = dynamicMatrix(j, k);
        if (value != 0) {
          constraintMatrix.insert(dynamicMatrix_rows * (i + 1) + j,
                                  dynamicMatrix_rows * i + k) = value;
        }
      }

  for (auto i = 0; i < mpcWindow; i++)
    for (int j = 0; j < dynamicMatrix_rows; j++)
      for (int k = 0; k < controlMatrix_cols; k++) {
        float value = controlMatrix(j, k);
        if (value != 0) {
          constraintMatrix.insert(dynamicMatrix_rows * (i + 1) + j,
                                  controlMatrix_cols * i + k +
                                      dynamicMatrix_rows * (mpcWindow + 1)) =
              value;
        }
      }

  for (auto i = 0; i < dynamicMatrix_rows * (mpcWindow + 1) +
                           controlMatrix_cols * mpcWindow;
       i++) {
    constraintMatrix.insert(i + (mpcWindow + 1) * dynamicMatrix_rows, i) = 1;
  }
}

template <int x_rows, int u_rows, int x0_rows>
void castMPCToQPConstraintVectors(const Eigen::Matrix<double, x_rows, 1> &xMax,
                                  const Eigen::Matrix<double, x_rows, 1> &xMin,
                                  const Eigen::Matrix<double, u_rows, 1> &uMax,
                                  const Eigen::Matrix<double, u_rows, 1> &uMin,
                                  const Eigen::Matrix<double, x0_rows, 1> &x0,
                                  int mpcWindow, Eigen::VectorXd &lowerBound,
                                  Eigen::VectorXd &upperBound) {
  // Get argument vector lenghts
  const int xMax_size = xMax.size();
  const int xMin_size = xMin.size();
  const int uMax_size = uMax.size();
  const int uMin_size = uMin.size();
  const int x0_size = x0.size();

  // evaluate the lower and the upper inequality vectors
  Eigen::VectorXd lowerInequality =
      Eigen::MatrixXd::Zero(x0_size * (mpcWindow + 1) + 4 * mpcWindow, 1);
  Eigen::VectorXd upperInequality =
      Eigen::MatrixXd::Zero(x0_size * (mpcWindow + 1) + 4 * mpcWindow, 1);
  for (auto i = 0; i < mpcWindow + 1; i++) {
    lowerInequality.block(xMin_size * i, 0, xMin_size, 1) = xMin;
    upperInequality.block(xMax_size * i, 0, xMax_size, 1) = xMax;
  }
  for (auto i = 0; i < mpcWindow; i++) {
    lowerInequality.block(uMin_size * i + x0_size * (mpcWindow + 1), 0,
                          uMin_size, 1) = uMin;
    upperInequality.block(uMax_size * i + x0_size * (mpcWindow + 1), 0,
                          uMax_size, 1) = uMax;
  }

  // evaluate the lower and the upper equality vectors
  Eigen::VectorXd lowerEquality =
      Eigen::MatrixXd::Zero(x0_size * (mpcWindow + 1), 1);
  Eigen::VectorXd upperEquality;
  lowerEquality.block(0, 0, x0_size, 1) = -x0;
  upperEquality = lowerEquality;
  lowerEquality = lowerEquality;

  // merge inequality and equality vectors
  lowerBound = Eigen::MatrixXd::Zero(
      2 * x0_size * (mpcWindow + 1) + uMax_size * mpcWindow, 1);
  lowerBound << lowerEquality, lowerInequality;

  upperBound = Eigen::MatrixXd::Zero(
      2 * x0_size * (mpcWindow + 1) + uMax_size * mpcWindow, 1);
  upperBound << upperEquality, upperInequality;
}

template <int rows>
void updateConstraintVectors(const Eigen::Matrix<double, rows, 1> &x0,
                             Eigen::VectorXd &lowerBound,
                             Eigen::VectorXd &upperBound) {
  // Get Size
  const int x0_size = x0.size();
  lowerBound.block(0, 0, x0_size, 1) = -x0;
  upperBound.block(0, 0, x0_size, 1) = -x0;
}

template <int x_rows, int xRef_rows>
double getErrorNorm(const Eigen::Matrix<double, x_rows, 1> &x,
                    const Eigen::Matrix<double, xRef_rows, 1> &xRef) {
  // Get Size
  const auto x_size = x.size();
  // evaluate the error
  Eigen::Matrix<double, x_size, 1> error = x - xRef;

  // return the norm
  return error.norm();
}
}  // namespace utilities
}  // namespace control

================================================
FILE: control/launch/mppi_pentagon.launch
================================================
<launch>

		<arg name="robot" default="-1" doc="Whether launchfile is run on turtlebot(1-5) directly (0), in simulation (-1) or on remote PC (X: 1-5)"/>
	
		<!-- Fake Encoders Node - USE FOR TESTING WHEN TURTLEBOT UNAVAILABLE -->
		<node name="fake_diff_encoders_node" pkg="rigid2d" type="fake_diff_encoders_node" output="screen">
			<rosparam command="load" file="$(find rigid2d)/config/odometer.yaml"/>
			<remap from="turtle1/cmd_vel" to="cmd_vel" />
		</node>

		<!-- Turtle Interface -->
		<node pkg="nuturtle_robot" type="turtle_interface" name="turtle_interface" output="screen">
			<!-- <rosparam command="load" file="$(find rigid2d)/config/odometer.yaml"/> -->
			<param name="right_wheel_joint" value="right_wheel_axle" />
			<param name="left_wheel_joint" value="left_wheel_axle" />
			<remap from="wheel_cmd" to="/nuturtlebot/WheelCommands"/>
			<remap from="sensor_data" to="/nuturtlebot/SensorData"/>
		</node>

		<!-- MPPI Node -->
		<node pkg="control" type="mppi" name="mppi" output="screen">
			<rosparam command="load" file="$(find nuturtle_description)/config/diff_params.yaml" />
		</node>

		<!-- Odometer Node -->
		<node name="odometer_node" pkg="rigid2d" type="odometer_node" output="screen">
			<!-- <rosparam command="load" file="$(find rigid2d)/config/odometer.yaml"/> -->
			<param name="odom_frame_id" value="odom" />
			<param name="body_frame_id" value="base_footprint" /> 
			<param name="right_wheel_joint" value="left_wheel_axle" />
			<param name="left_wheel_joint" value="left_wheel_axle" />
		</node>

	<arg name="parallel" default="False" doc="True: Parallel Park | False: Follow Waypoints"/>
	<group if="$(arg parallel)">
		<!-- Load Parallel Park -->
		<rosparam command="load" file="$(find control)/config/parallel.yaml" />
	</group>

	<group unless="$(arg parallel)">
		<!-- Load Waypoints -->
		<rosparam command="load" file="$(find control)/config/waypoints.yaml" />
	</group>



	<!-- Include diff drive viewer without joint state publisher -->
	<include file="$(find nuturtle_description)/launch/view_diff_drive.launch">
	<arg name="js_pub" value="False" />
	<arg name="robot" value="$(arg robot)"/>
	<arg name="rviz_view" value="$(find nuturtle_description)/rviz/ddrive_odom.rviz"/>
	</include> 

</launch>

================================================
FILE: control/launch/osqp_example.launch
================================================
<launch>

		<arg name="eigen" default="False" doc="True: Call Eigen Wrapped OSQP Example"/>
	<group if="$(arg eigen)">
		<!-- OSQP Eigen Example Node -->
		<node pkg="control" type="osqp_eigen_example" name="osqp_eigen_example" output="screen"/>
	</group>

	<group unless="$(arg eigen)">
		<!-- OSQP Example Node -->
		<node pkg="control" type="osqp_example" name="osqp_example" output="screen"/>
	</group>

</launch>

================================================
FILE: control/msg/Waypoint.msg
================================================
# Current Pose
float32[] current
# Desired Pose
float32[] desired

================================================
FILE: control/package.xml
================================================
<?xml version="1.0"?>
<package format="2">
  <name>control</name>
  <version>0.0.1</version>
  <description>Control the turtlebot between waypoints</description>

  <!-- One maintainer tag required, multiple allowed, one person per tag -->
  <!-- Example:  -->
  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
  <maintainer email="mrahme97@gmail.com">Maurice Rahme</maintainer>


  <!-- One license tag required, multiple allowed, one license per tag -->
  <!-- Commonly used license strings: -->
  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
  <license>TODO</license>


  <!-- Url tags are optional, but multiple are allowed, one per tag -->
  <!-- Optional attribute type can be: website, bugtracker, or repository -->
  <!-- Example: -->
  <!-- <url type="website">http://wiki.ros.org/control</url> -->


  <!-- Author tags are optional, multiple are allowed, one per tag -->
  <!-- Authors do not have to be maintainers, but could be -->
  <!-- Example: -->
  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->


  <!-- The *depend tags are used to specify dependencies -->
  <!-- Dependencies can be catkin packages or system dependencies -->
  <!-- Examples: -->
  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
  <!--   <depend>roscpp</depend> -->
  <!--   Note that this is equivalent to the following: -->
  <!--   <build_depend>roscpp</build_depend> -->
  <!--   <exec_depend>roscpp</exec_depend> -->
  <!-- Use build_depend for packages you need at compile time: -->
  <!--   <build_depend>message_generation</build_depend> -->
  <!-- Use build_export_depend for packages you need in order to build against this package: -->
  <!--   <build_export_depend>message_generation</build_export_depend> -->
  <!-- Use buildtool_depend for build tool packages: -->
  <!--   <buildtool_depend>catkin</buildtool_depend> -->
  <!-- Use exec_depend for packages you need at runtime: -->
  <!--   <exec_depend>message_runtime</exec_depend> -->
  <!-- Use test_depend for packages you need only for testing: -->
  <!--   <test_depend>gtest</test_depend> -->
  <!-- Use doc_depend for packages you need only for building documentation: -->
  <!--   <doc_depend>doxygen</doc_depend> -->
  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>global_planner</build_depend>
  <build_depend>map</build_depend>
  <build_depend>message_generation</build_depend>
  <build_depend>message_runtime</build_depend>
  <build_depend>rigid2d</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>nuslam</build_depend>
  <build_depend>visualization_msgs</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_export_depend>global_planner</build_export_depend>
  <build_export_depend>map</build_export_depend>
  <build_export_depend>message_generation</build_export_depend>
  <build_export_depend>message_runtime</build_export_depend>
  <build_export_depend>rigid2d</build_export_depend>
  <build_export_depend>nuslam</build_export_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
  <build_export_depend>visualization_msgs</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>
  <exec_depend>global_planner</exec_depend>
  <exec_depend>map</exec_depend>
  <exec_depend>message_generation</exec_depend>
  <exec_depend>message_runtime</exec_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>rigid2d</exec_depend>
  <exec_depend>nuslam</exec_depend>
  <exec_depend>visualization_msgs</exec_depend>
  <exec_depend>std_msgs</exec_depend>

  <test_depend>rostest</test_depend>
  <test_depend>rosunit</test_depend>
  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->

  </export>
</package>


================================================
FILE: control/src/control/Models.cpp
================================================
#include "control/Models.hpp"

#include <iostream>

namespace control {}  // namespace control

================================================
FILE: control/src/control/TrajMPC.cpp
================================================
#include "control/TrajMPC.hpp"

#include <iostream>

namespace control {}  // namespace control

================================================
FILE: control/src/control/rk4.cpp
================================================
/// \file
/// \brief 4th Order Runge Kutta method for Integration

#include <iostream>
#include "control/rk4.hpp"

namespace control
{

RK4::RK4(const double & dt) : dt(dt)
{
}


void RK4::registerODE(std::function<void(const Ref<VectorXd>, Ref<VectorXd>)> ode)
{
  func = ode;
}


void RK4::registerODE(std::function<void(const Ref<VectorXd>, const Ref<VectorXd>, Ref<VectorXd>)> ode)
{
  func_ctrl = ode;
}


MatrixXd RK4::solve(const Ref<VectorXd> x0, const double & horizon)
{
  // catch undeclared fcn
  if(!func)
  {
    std::cerr << "No ODE Function!" << std::endl;
    exit(EXIT_FAILURE);
  }

  // Initial State
  VectorXd state = x0;
  // Solver iterations
  const auto num_samples = static_cast<int>(horizon/dt);
  // Solution Trajectory Shape
  MatrixXd trajectory(state.rows(), num_samples);

  // Integrate for all samples
  for(int i = 0; i < num_samples; i++)
  {
    // Integrate once (pass state by reference using Eigen::Ref<>)
    integrate(state);
    // Store next state in trajectory vector
    trajectory.col(i) = state;
  }

  return trajectory;
}


MatrixXd RK4::solve(const Ref<VectorXd> x0, const Ref<MatrixXd> u, const double & horizon)
{
  // catch undeclared fcn
  if(!func_ctrl)
  {
    std::cerr << "No ODE Function!" << std::endl;
    exit(EXIT_FAILURE);
  }

  // Initial State
  VectorXd state = x0;
  // Solver iterations
  const auto num_samples = static_cast<int>(horizon/dt);
  // Solution Trajectory Shape
  MatrixXd trajectory(state.rows(), num_samples);

  // Integrate for  all samples
  for(int i = 0; i < num_samples; i++)
  {
    // Get Control Vector
    VectorXd u_t = u.col(i);
    // Integrate once (pass state by reference using Eigen::Ref<>)
    integrate(state, u_t);
    // Store next state in trajectory vector
    trajectory.col(i) = state;
  }

  return trajectory;
}


void RK4::integrate(Ref<VectorXd> x_t)
{
  // Pass input as reference to save memory

  // Initialize at zero
  VectorXd k1 = VectorXd::Zero(x_t.size());
  VectorXd k2, k3, k4, temp_pass;
  k2 = k3 = k4 = temp_pass = k1;

  // RK4 method without control signal

  // get k1 by ref
  func(x_t, k1);
  // get k2 by ref
  temp_pass = x_t + dt*(0.5*k1);
  func(temp_pass, k2);
  // get k3 by ref
  temp_pass = x_t + dt*(0.5*k2);
  func(temp_pass, k3);
  // get k4 by ref
  temp_pass = x_t + dt*k3;
  func(temp_pass, k4);

  // update (init + increment)
  x_t += (dt/6.0)*(k1 + 2.0*k2 + 2.0*k3 + k4);
}


void RK4::integrate(Ref<VectorXd> x_t, const Ref<VectorXd> u_t)
{
  // Pass input as reference to save memory

  // Initialize at zero
  VectorXd k1 = VectorXd::Zero(x_t.size());
  VectorXd k2, k3, k4, temp_pass;
  k2 = k3 = k4 = temp_pass = k1;

  // RK4 method with control signal

  // get k1 by ref
  func_ctrl(x_t, u_t, k1);
  // get k2 by ref
  temp_pass = x_t + dt*(0.5*k1);
  func_ctrl(temp_pass, u_t, k2);
  // get k3 by ref
  temp_pass = x_t + dt*(0.5*k2);
  func_ctrl(temp_pass, u_t, k3);
  // get k4 by ref
  temp_pass = x_t + dt*k3;
  func_ctrl(temp_pass, u_t, k4);

  // update (init + increment)
  x_t += (dt/6.0)*(k1 + 2.0*k2 + 2.0*k3 + k4);
}




}

================================================
FILE: control/src/control/utilities.cpp
================================================
#include "control/utilities.hpp"

#include <iostream>

namespace control {
namespace utilities {}
}  // namespace control

================================================
FILE: control/src/mppi
================================================
#!/usr/bin/env python

import numpy as np
import matplotlib.pyplot as plt
from scipy.signal import savgol_filter
from matplotlib import animation
import copy
import time
import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
import tf
from geometry_msgs.msg import Twist, Quaternion, Vector3

np.random.seed(0)

# rad/s
WHEEL_VEL_MAX = 6.35492
WHEEL_RADIUS = 0.033
WHEEL_BASE = 0.16


def dd_dynamics(x, u):
    """ Diff Drive dd_dynamics
    """
    return np.array([
        (WHEEL_RADIUS / 2.0) * np.cos(x[2, :]) * (u[0, :] + u[1, :]),
        (WHEEL_RADIUS / 2.0) * np.sin(x[2, :]) * (u[0, :] + u[1, :]),
        (WHEEL_RADIUS / WHEEL_BASE) * (u[1, :] - u[0, :])
    ])


def unicycle_dynamics(x, u):
    return np.array(
        [np.cos(x[2, :]) * u[0, :],
         np.sin(x[2, :]) * u[0, :], u[1, :]])


def rk4(x0, u, dt):
    """ Returns position updates for
        given velocity commands after one timestep
        using a Runge-Kutta 4 integrator
    """
    # update calculated here
    k1 = dt * dd_dynamics(x0, u)
    k2 = dt * dd_dynamics(x0 + k1 / 2, u)
    k3 = dt * dd_dynamics(x0 + k2 / 2, u)
    k4 = dt * dd_dynamics(x0 + k3, u)
    # initial plus update
    xnew = x0 + (1.0 / 6.0) * (k1 + 2 * k2 + 2 * k3 + k4)
    # Clip Theta
    xnew[2, :] = xnew[2, :] - (np.ceil(
        (xnew[2, :] + np.pi) / (2.0 * np.pi)) - 1.0) * 2.0 * np.pi
    return xnew


def euler(x0, u, dt):
    return x0 + dt * unicycle_dynamics(x0, u)


class MPPI:
    def __init__(self, model=rk4, horizon=100, samples=10, thresh=0.05):
        self.horizon = horizon
        self.samples = samples
        self.uvec_init = np.zeros((2, horizon))
        self.model = model
        self.dt = 1.0 / float(horizon)
        # Cost during trajectory
        self.Q = np.array([[1e3, 0.0, 0.0], [0.0, 1e3, 0.0], [0.0, 0.0, 0.0]])
        # Cost of controls
        self.R = np.array([[1.0, 0.0], [0.0, 1.0]])
        # Termination cost
        self.P1 = np.array([[1e3, 0.0, 0.0], [0.0, 1e3, 0.0], [0.0, 0.0, 1e3]])
        self.thresh = thresh
        self.start = np.array([0.0, 0.0, 0.0])
        self.goal = np.array([0.0, 0.0, 0.0])
        self.initialize()

    def initialize(self):
        self.fin_time = [0]  # time stamp of the position/control
        self.latest_uvec = copy.deepcopy(self.uvec_init)
        self.uvec = np.array([self.uvec_init[:, 0]])
        self.path = np.array([self.start])

    def get_path(self,
                 state,
                 goal,
                 sig=np.array([[.9, 0.0], [0.0, .9]]),
                 lam=.001):
        value_fcn, eps = self.get_cost2go(state, self.latest_uvec, goal, lam,
                                          sig)
        self.latest_uvec = self.update_action(self.latest_uvec, eps, value_fcn,
                                              sig, lam)
        state = self.perform_action(state, self.latest_uvec)
        self.path = np.concatenate((self.path, np.array([state])))
        self.uvec = np.concatenate(
            (self.uvec, np.array([self.latest_uvec[:, 0]])))
        self.fin_time.append(self.fin_time[-1] + self.dt)
        # Shift controls for Receding Horizon MPPI
        self.latest_uvec[:, :-1] = self.latest_uvec[:, 1:]
        self.latest_uvec[:, -1] = self.uvec_init[:, 0]
        return state

    def solve_path(self,
                   start,
                   goal,
                   sig=np.array([[1.0, 0.0], [0.0, 1.0]]),
                   lam=.01):
        self.start = start
        self.goal = goal
        state = start
        self.path = np.array([state])
        self.latest_uvec = copy.deepcopy(self.uvec_init)
        print("STARTING")
        i = 0
        tstart = time.time()
        while np.linalg.norm(state[:2] - goal[:2]) > self.thresh:
            i += 1
            state = self.get_path(state, goal, sig, lam)
            if i % 200 == 0:
                print("Iteration: {} \t State: {}".format(i, state))
        t_elapsed = time.time() - tstart
        print(
            "Finished after {} iterations. Final State: {}, Time Taken: {}, Time Per Iter: {}"
            .format(i, state, t_elapsed, t_elapsed / float(i)))

    def get_cost2go(self, state, uvec, goal, lam, sig):
        cost2go = []
        # Dim: statedim x N
        # State at current timestep
        states = np.tile(state, (self.samples, 1)).T

        eps = []
        # Each timed eps contains one disturbance for each timestep
        # so eps is t x udim x N, where N is num of samples
        for t in range(self.horizon):
            # statedim x N, where N is num of samples
            # Sample N states forward in time for N costs to go
            # eps.append(
            #     np.random.multivariate_normal([0.0, 0.0],
            #                                   sig,
            #                                   size=(self.samples)).T)
            eps.append(
                np.random.normal(0,
                                 sig[0, 0],
                                 size=(uvec.shape[0], self.samples)))
            u_samp = np.tile(uvec[:, t], (self.samples, 1)).T
            # EXPLORE
            u_samp += eps[-1]
            # CLIP
            u_samp[0, :] = np.clip(u_samp[0, :], -WHEEL_VEL_MAX, WHEEL_VEL_MAX)
            u_samp[1, :] = np.clip(u_samp[1, :], -WHEEL_VEL_MAX, WHEEL_VEL_MAX)
            # Predict Next State
            st = self.model(states, u_samp, self.dt)

            # Trajectory Cost
            costs = np.zeros(self.samples)
            for s in range(self.samples):
                states[:, s] = st[:, s]
                costs[s] = self.get_cost(st[:, s], goal, uvec[:, t], lam,
                                         sig, eps[-1][:, s])

            cost2go.append(costs)

        costs = np.zeros(self.samples)
        for s in range(self.samples):
            # ADD TERMINAL COST
            mxt = (st[:, s] - goal).T.dot(self.P1).dot(st[:, s] - goal)
            costs[s] = mxt

        cost2go[-1] += costs
        # get value function over time as cost2go at each timestep
        # (first reversed and summed)
        # value_fcn = np.cumsum(cost2go[::-1], 0)[::-1, :]
        value_fcn = np.flip(np.cumsum(np.flip(cost2go, 0), axis=0), 0)
        # print("VALUE FCN: {}".format(value_fcn))

        return value_fcn, eps

    def get_cost(self, state, desired_state, u, lam, sig, eps):
        # return euclid dist
        return 0.5 * (
            (state - desired_state).T.dot(self.Q).dot(state - desired_state) +
            u.T.dot(self.R).dot(u)) + lam * u.dot(sig).dot(eps)

    def update_action(self, uvec, eps, value_fcn, sig, lam):
        for t in range(self.horizon):
            # First, subtract the min cost out from each sample
            value_fcn[t] -= np.amin(value_fcn[t])

            # Get weight for optimal action update
            # This tells us the usefulness of each eps sample in [t]
            omg = np.exp(-value_fcn[t] / lam) + 1e-8
            # Normalize
            omg /= np.sum(omg)
            uvec[:, t] += np.dot(eps[t], omg)
        # CLIP
        uvec[0, :] = np.clip(uvec[0, :], -WHEEL_VEL_MAX, WHEEL_VEL_MAX)
        uvec[1, :] = np.clip(uvec[1, :], -WHEEL_VEL_MAX, WHEEL_VEL_MAX)

        # filter controls
        uvec = savgol_filter(uvec, self.horizon - 1, 3, axis=1)

        # CLIP
        uvec[0, :] = np.clip(uvec[0, :], -WHEEL_VEL_MAX, WHEEL_VEL_MAX)
        uvec[1, :] = np.clip(uvec[1, :], -WHEEL_VEL_MAX, WHEEL_VEL_MAX)

        return uvec

    def perform_action(self, state, uvec):
        state = np.tile(state, (2, 1)).T
        u = np.tile(uvec[:, 0], (2, 1)).T
        return self.model(state, u, self.dt)[:, 0]

    def get_animation(self):
        print("generating animation...")
        path = np.array(self.path)  # final path of robot
        control = np.array(self.uvec)  # controls used to generate the path
        fig, (ax1, ax2, ax3) = plt.subplots(nrows=3, ncols=1, figsize=(10, 10))
        fig.suptitle("MPPI", fontsize=12)
        ax1.set(xlabel="X position (m)", ylabel="Y position (m)")
        ax1.set_xlim(np.amin(path[:, 0]) - 0.1, np.amax(path[:, 0]) + 0.1)
        ax1.set_ylim(np.amin(path[:, 1]) - 0.1, np.amax(path[:, 1]) + 0.1)
        line11, = ax1.plot([], [])
        ax2.set(xlabel="Time (s)", ylabel="State (m or rad)")
        ax2.set_xlim(0, self.fin_time[-1])  # fin_time is my time vector
        ax2.set_ylim(np.amin(path) - 0.1, np.amax(path) + 0.1)
        line21, = ax2.plot([], [])
        line22, = ax2.plot([], [])
        line23, = ax2.plot([], [])
        ax2.legend(["x", "y", r"$\theta$"])
        ax3.set(xlabel="Time (s)", ylabel="Wheel Velocity (rad/s)")
        ax3.set_xlim(0, self.fin_time[-1])
        ax3.set_ylim(np.amin(control) - 0.1, np.amax(control) + 0.1)
        line31, = ax3.plot([], [])
        line32, = ax3.plot([], [])
        ax3.legend(["u_l", "u_r"])

        def animate(i):
            ax1.clear()
            ax1.set(xlabel="X position (m)", ylabel="Y position (m)")
            ax1.set_xlim(np.amin(path[:, 0]) - 0.1, np.amax(path[:, 0]) + 0.1)
            ax1.set_ylim(np.amin(path[:, 1]) - 0.1, np.amax(path[:, 1]) + 0.1)
            ax1.arrow(x=path[i, 0],
                      y=path[i, 1],
                      dx=0.025 * np.cos(path[i, 2]),
                      dy=0.025 * np.sin(path[i, 2]),
                      head_width=0.03,
                      length_includes_head=True,
                      overhang=0.1,
                      facecolor="black",
                      zorder=0)
            line11, = ax1.plot([], [])
            line11.set_data(path[0:i, 0], path[0:i, 1])
            line21.set_data(self.fin_time[0:i], path[0:i, 0])
            line22.set_data(self.fin_time[0:i], path[0:i, 1])
            line23.set_data(self.fin_time[0:i], path[0:i, 2])
            line31.set_data(self.fin_time[0:i], control[0:i, 0])
            line32.set_data(self.fin_time[0:i], control[0:i, 1])
            return line11, line21, line22, line23, line31, line32

        # may want to change the interval to match your dt, the number is in milliseconds
        anim = animation.FuncAnimation(fig,
                                       animate,
                                       frames=len(self.fin_time),
                                       interval=10,
                                       repeat=False)
        print("saving animation as mp4...this might take a while...")
        anim.save(filename='sim.mp4', fps=30, dpi=300)


def plot_path(path, start, goal):
    plt.figure()
    plt.plot(path[:, 0], path[:, 1], label="Path")
    plt.plot(start[0], start[1], marker='o', markersize=3, label="Start")
    plt.plot(goal[0], goal[1], marker='o', markersize=3, label="Goal")
    plt.legend()
    plt.show()

    plt.figure()
    plt.plot(path[:, 0], label="x")
    plt.plot(path[:, 1], label="y")
    plt.plot(path[:, 2], label="theta")
    plt.legend()
    plt.show()


def plot_controls(uvec):
    plt.figure()
    plt.plot(uvec[:, 0], label="u1")
    plt.plot(uvec[:, 1], label="u2")
    plt.legend()
    plt.show()


class Controller:
    def __init__(self):
        self.mppi = MPPI()
        self.sub_pos = rospy.Subscriber('odom',
                                        Odometry,
                                        self.pos_cb,
                                        queue_size=1)
        self.tw_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
        self.done = False
        if not rospy.get_param("waypoints"):
            self.parallel_park = True
        else:
            self.parallel_park = False
            self.waypoints = rospy.get_param("waypoints")
        self.idx = 0
        self.init = True
        self.state = self.mppi.start
        tw = Twist()
        tw.linear.x = 0.0
        tw.angular.z = 0.0
        self.tw_pub.publish(tw)
        self.last_time = time.time()

    def wheelsToTwist(self, wheel_vels):
        ul = wheel_vels[0]
        ur = wheel_vels[1]

        vx = WHEEL_RADIUS * (ul + ur) / 2.0
        wz = WHEEL_RADIUS * (-ul + ur) / WHEEL_BASE
        return vx, wz

    def pos_cb(self, odom):
        try:
            # Update Start
            x = odom.pose.pose.position.x
            y = odom.pose.pose.position.y
            orn = odom.pose.pose.orientation
            orn_list = [orn.x, orn.y, orn.z, orn.w]
            theta = tf.transformations.euler_from_quaternion(orn_list)[2]
            self.mppi.start = np.array([x, y, theta])
            if self.parallel_park:
                self.mppi.goal = np.array([0.0, -1.0, 0.0])
            # Now do another round of MPPI
            if np.linalg.norm(self.mppi.start[:2] - self.mppi.goal[:2]
                              ) > self.mppi.thresh and not self.init:
                self.state = self.mppi.get_path(self.mppi.start,
                                                self.mppi.goal)
                self.done = False
            elif self.init:
                self.mppi.initialize()
                if not self.parallel_park:
                    # Goal only contains x,y
                    goal = self.waypoints[self.idx]
                    # Calculate theta and polulate self.goal
                    theta = np.arctan2(goal[1] - self.mppi.start[1],
                                       goal[0] - self.mppi.start[0])
                    self.mppi.goal = np.array([goal[0], goal[1], theta])
                    self.state = self.mppi.start
                self.init = False
                rospy.loginfo("NEXT WAYPOINT: {}".format(self.mppi.goal[:2]))
            else:
                if not self.parallel_park:
                    # Goal Reached, go to next wpt
                    if self.idx + 1 >= len(self.waypoints):
                        self.idx = 0
                    else:
                        self.idx += 1
                    self.mppi.initialize()
                    rospy.loginfo(
                        "WAYPOINT REACHED: {} \n NEXT WAYPOINT: {}".format(
                            self.mppi.goal[:2], self.waypoints[self.idx]))
                    # Goal only contains x,y
                    goal = self.waypoints[self.idx]
                    # Calculate theta and polulate self.goal
                    theta = np.arctan2(goal[1] - self.mppi.start[1],
                                       goal[0] - self.mppi.start[0])
                    self.mppi.goal = np.array([goal[0], goal[1], theta])
                    self.state = self.mppi.start
                else:
                    self.done = True

            # Grab latest controls
            if not self.done:
                u = self.mppi.uvec[-1, :]
            else:
                u = np.array([0.0, 0.0])
            vx, wz = self.wheelsToTwist(u)
            tw = Twist()
            tw.linear.x = vx
            tw.angular.z = wz
            self.tw_pub.publish(tw)

        except rospy.ROSInterruptException:
            pass


def main():
    # """ The main() function. """
    rospy.init_node('mppi', anonymous=True)
    controller = Controller()
    rospy.spin()
    # mppi = MPPI()
    # start = np.array([0.0, 0.0, np.pi / 2.0])
    # goal = np.array([1.0, 0.0, np.pi / 2.0])
    # mppi.solve_path(start, goal)
    # plot_path(mppi.path, start, goal)
    # plot_controls(mppi.uvec)
    # mppi.get_animation()


if __name__ == '__main__':
    main()


================================================
FILE: control/src/osqp_eigen_example.cpp
================================================
/// \file
/// \brief Basic OSQP example

#include <geometry_msgs/Point.h>
#include <ros/console.h>
#include <ros/ros.h>

#include "control/utilities.hpp"

void setDynamicsMatrices(Eigen::Matrix<double, 12, 12> &a,
                         Eigen::Matrix<double, 12, 4> &b) {
  a << 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0.,
      0., 0.1, 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0.,
      0.0488, 0., 0., 1., 0., 0., 0.0016, 0., 0., 0.0992, 0., 0., 0., -0.0488,
      0., 0., 1., 0., 0., -0.0016, 0., 0., 0.0992, 0., 0., 0., 0., 0., 0., 1.,
      0., 0., 0., 0., 0., 0.0992, 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0.,
      0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 0., 0., 0.,
      0., 0., 0., 1., 0., 0., 0., 0.9734, 0., 0., 0., 0., 0., 0.0488, 0., 0.,
      0.9846, 0., 0., 0., -0.9734, 0., 0., 0., 0., 0., -0.0488, 0., 0., 0.9846,
      0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.9846;

  b << 0., -0.0726, 0., 0.0726, -0.0726, 0., 0.0726, 0., -0.0152, 0.0152,
      -0.0152, 0.0152, -0., -0.0006, -0., 0.0006, 0.0006, 0., -0.0006, 0.0000,
      0.0106, 0.0106, 0.0106, 0.0106, 0, -1.4512, 0., 1.4512, -1.4512, 0.,
      1.4512, 0., -0.3049, 0.3049, -0.3049, 0.3049, -0., -0.0236, 0., 0.0236,
      0.0236, 0., -0.0236, 0., 0.2107, 0.2107, 0.2107, 0.2107;
}

void setInequalityConstraints(Eigen::Matrix<double, 12, 1> &xMax,
                              Eigen::Matrix<double, 12, 1> &xMin,
                              Eigen::Matrix<double, 4, 1> &uMax,
                              Eigen::Matrix<double, 4, 1> &uMin) {
  double u0 = 10.5916;

  // input inequality constraints
  uMin << 9.6 - u0, 9.6 - u0, 9.6 - u0, 9.6 - u0;

  uMax << 13 - u0, 13 - u0, 13 - u0, 13 - u0;

  // state inequality constraints
  xMin << -M_PI / 6, -M_PI / 6, -OsqpEigen::INFTY, -OsqpEigen::INFTY,
      -OsqpEigen::INFTY, -1., -OsqpEigen::INFTY, -OsqpEigen::INFTY,
      -OsqpEigen::INFTY, -OsqpEigen::INFTY, -OsqpEigen::INFTY,
      -OsqpEigen::INFTY;

  xMax << M_PI / 6, M_PI / 6, OsqpEigen::INFTY, OsqpEigen::INFTY,
      OsqpEigen::INFTY, OsqpEigen::INFTY, OsqpEigen::INFTY, OsqpEigen::INFTY,
      OsqpEigen::INFTY, OsqpEigen::INFTY, OsqpEigen::INFTY, OsqpEigen::INFTY;
}

void setWeightMatrices(Eigen::DiagonalMatrix<double, 12> &Q,
                       Eigen::DiagonalMatrix<double, 4> &R) {
  Q.diagonal() << 0, 0, 10., 10., 10., 10., 0, 0, 0, 5., 5., 5.;
  R.diagonal() << 0.1, 0.1, 0.1, 0.1;
}

int main(int argc, char **argv) {
  ROS_INFO("STARTING NODE: osqp_example");

  ros::init(argc, argv, "osqp_example_node");  // register the node on ROS
  ros::NodeHandle nh;                          // get a handle to ROS
  ros::NodeHandle nh_("~");                    // get a handle to ROS

  // OSQP Eigen Example Start:
  // https://robotology.github.io/osqp-eigen/md_pages_mpc.html

  // set the preview window
  int mpcWindow = 20;

  // allocate the dynamics matrices
  Eigen::Matrix<double, 12, 12> a;
  Eigen::Matrix<double, 12, 4> b;

  // allocate the constraints vector
  Eigen::Matrix<double, 12, 1> xMax;
  Eigen::Matrix<double, 12, 1> xMin;
  Eigen::Matrix<double, 4, 1> uMax;
  Eigen::Matrix<double, 4, 1> uMin;

  // allocate the weight matrices
  Eigen::DiagonalMatrix<double, 12> Q;
  Eigen::DiagonalMatrix<double, 4> R;

  // allocate the initial and the reference state space
  Eigen::Matrix<double, 12, 1> x0;
  Eigen::Matrix<double, 12, 1> xRef;

  // allocate QP problem matrices and vectores
  Eigen::SparseMatrix<double> hessian;
  Eigen::VectorXd gradient;
  Eigen::SparseMatrix<double> linearMatrix;
  Eigen::VectorXd lowerBound;
  Eigen::VectorXd upperBound;

  // set the initial and the desired states
  x0 << 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
  xRef << 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0;

  // set MPC problem quantities
  setDynamicsMatrices(a, b);
  setInequalityConstraints(xMax, xMin, uMax, uMin);
  setWeightMatrices(Q, R);

  // cast the MPC problem as QP problem
  control::utilities::castMPCToQPHessian<12, 4>(Q, R, mpcWindow, hessian);
  control::utilities::castMPCToQPGradient<12, 12, 1>(Q, xRef, mpcWindow,
                                                     gradient);
  control::utilities::castMPCToQPConstraintMatrix<12, 12, 12, 4>(
      a, b, mpcWindow, linearMatrix);
  control::utilities::castMPCToQPConstraintVectors<12, 4, 12>(
      xMax, xMin, uMax, uMin, x0, mpcWindow, lowerBound, upperBound);

  // instantiate the solver
  OsqpEigen::Solver solver;

  // settings
  // solver.settings()->setVerbosity(false);
  solver.settings()->setWarmStart(true);

  // set the initial data of the QP solver
  solver.data()->setNumberOfVariables(12 * (mpcWindow + 1) + 4 * mpcWindow);
  solver.data()->setNumberOfConstraints(2 * 12 * (mpcWindow + 1) +
                                        4 * mpcWindow);
  if (!solver.data()->setHessianMatrix(hessian)) return 1;
  if (!solver.data()->setGradient(gradient)) return 1;
  if (!solver.data()->setLinearConstraintsMatrix(linearMatrix)) return 1;
  if (!solver.data()->setLowerBound(lowerBound)) return 1;
  if (!solver.data()->setUpperBound(upperBound)) return 1;

  // instantiate the solver
  if (!solver.initSolver()) return 1;

  // controller input and QPSolution vector
  Eigen::Vector4d ctr;
  Eigen::VectorXd QPSolution;

  // number of iteration steps
  int numberOfSteps = 50;

  double obj_val;

  for (int i = 0; i < numberOfSteps; i++) {
    ROS_INFO("---------------------------------------\nIteration: %d", i);
    // solve the QP problem
    if (!solver.solve()) return 1;

    // get the controller input
    QPSolution = solver.getSolution();
    ctr = QPSolution.block(12 * (mpcWindow + 1), 0, 4, 1);

    // save data into file
    // auto x0Data = x0.data();

    // propagate the model
    x0 = a * x0 + b * ctr;

    /// Get the optimal objective value, can be used to exit early.
    /// https://osqp.org/docs/interfaces/cc++?highlight=osqpworkspace#_CPPv413OSQPWorkspace
    obj_val = solver.workspace()->info->obj_val;

    // update the constraint bound
    control::utilities::updateConstraintVectors<12>(x0, lowerBound, upperBound);
    if (!solver.updateBounds(lowerBound, upperBound)) return 1;
  }

  // OSQP Eigen Example End

  ROS_INFO("Solved with obj val [%f]: Ctrl + C to end.", obj_val);

  while (ros::ok()) {
    ros::spinOnce();
  }

  return 0;
}


================================================
FILE: control/src/osqp_example.cpp
================================================
/// \file
/// \brief Basic OSQP example

#include <geometry_msgs/Point.h>
#include <ros/console.h>
#include <ros/ros.h>

#include "osqp.h"

int main(int argc, char **argv) {
  ROS_INFO("STARTING NODE: osqp_example");

  ros::init(argc, argv, "osqp_example_node");  // register the node on ROS
  ros::NodeHandle nh;                          // get a handle to ROS
  ros::NodeHandle nh_("~");                    // get a handle to ROS

  ros::Publisher osqp_pub = nh.advertise<geometry_msgs::Point>("success", 1);

  geometry_msgs::Point min_point;

  // OSQP Example Start: https://osqp.org/docs/examples/setup-and-solve.html
  // Load problem data
  c_float P_x[3] = {
      4.0,
      1.0,
      2.0,
  };
  c_int P_nnz = 3;
  c_int P_i[3] = {
      0,
      0,
      1,
  };
  c_int P_p[3] = {
      0,
      1,
      3,
  };
  c_float q[2] = {
      1.0,
      1.0,
  };
  c_float A_x[4] = {
      1.0,
      1.0,
      1.0,
      1.0,
  };
  c_int A_nnz = 4;
  c_int A_i[4] = {
      0,
      1,
      0,
      2,
  };
  c_int A_p[3] = {
      0,
      2,
      4,
  };
  c_float l[3] = {
      1.0,
      0.0,
      0.0,
  };
  c_float u[3] = {
      1.0,
      0.7,
      0.7,
  };
  c_int n = 2;
  c_int m = 3;

  // Exitflag
  c_int exitflag = 0;

  // Workspace structures
  OSQPWorkspace *work;
  OSQPSettings *settings = (OSQPSettings *)c_malloc(sizeof(OSQPSettings));
  OSQPData *data = (OSQPData *)c_malloc(sizeof(OSQPData));

  // Populate data
  if (data) {
    data->n = n;
    data->m = m;
    data->P = csc_matrix(data->n, data->n, P_nnz, P_x, P_i, P_p);
    data->q = q;
    data->A = csc_matrix(data->m, data->n, A_nnz, A_x, A_i, A_p);
    data->l = l;
    data->u = u;
  }

  // Define solver settings as default
  if (settings) {
    osqp_set_default_settings(settings);
    settings->alpha = 1.0;  // Change alpha parameter
  }

  // Setup workspace
  exitflag = osqp_setup(&work, data, settings);

  // Solve Problem
  osqp_solve(work);

  // Cleanup
  if (data) {
    if (data->A) c_free(data->A);
    if (data->P) c_free(data->P);
    c_free(data);
  }
  if (settings) c_free(settings);

  // OSQP Example End

  min_point.x = work->solution->x[0];
  min_point.y = work->solution->x[1];

  ROS_INFO("Calculated min: [%f, %f]", min_point.x, min_point.y);

  while (ros::ok()) {
    ros::spinOnce();
    osqp_pub.publish(min_point);
  }

  return exitflag;
}


================================================
FILE: global_planner/CMakeLists.txt
================================================
cmake_minimum_required(VERSION 2.8.3)
project(global_planner)

## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-Wall -Wextra -pipe -Wno-psabi)

set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
  map	
  message_generation
  message_runtime
  rigid2d
  roscpp
  rostest
  nuslam
  visualization_msgs
)

find_package(Eigen3 3.3 REQUIRED NO_MODULE)

## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)


## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()

################################################
## Declare ROS messages, services and actions ##
################################################

## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
##   * add a build_depend tag for "message_generation"
##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
##     but can be declared for certainty nonetheless:
##     * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
##   * add "message_generation" and every package in MSG_DEP_SET to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * add "message_runtime" and every package in MSG_DEP_SET to
##     catkin_package(CATKIN_DEPENDS ...)
##   * uncomment the add_*_files sections below as needed
##     and list every .msg/.srv/.action file to be processed
##   * uncomment the generate_messages entry below
##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)

## Generate messages in the 'msg' folder
# add_message_files(
#   FILES
#   Message1.msg
#   Message2.msg
# )

## Generate services in the 'srv' folder
# add_service_files(
#   FILES
#   Service1.srv
#   Service2.srv
# )

## Generate actions in the 'action' folder
# add_action_files(
#   FILES
#   Action1.action
#   Action2.action
# )

## Generate added messages and services with any dependencies listed here
# generate_messages(
#   DEPENDENCIES
#   std_msgs  # Or other packages containing msgs
# )

################################################
## Declare ROS dynamic reconfigure parameters ##
################################################

## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
##   * add "dynamic_reconfigure" to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * uncomment the "generate_dynamic_reconfigure_options" section below
##     and list every .cfg file to be processed

## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
#   cfg/DynReconf1.cfg
#   cfg/DynReconf2.cfg
# )

###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME} map rigid2d nuslam
CATKIN_DEPENDS map message_generation message_runtime rigid2d roscpp nuslam visualization_msgs
#  DEPENDS system_lib
)

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
  include
  ${catkin_INCLUDE_DIRS}
  ${Eigen3_INCLUDE_DIRS}
)

add_library(${PROJECT_NAME}
  src/${PROJECT_NAME}/${PROJECT_NAME}.cpp
  src/${PROJECT_NAME}/heuristic.cpp
  src/${PROJECT_NAME}/incremental.cpp
  src/${PROJECT_NAME}/potential_field.cpp
)

## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/map_node.cpp)
add_executable(astar src/astar.cpp)
add_executable(lpastar src/lpastar.cpp)
add_executable(dsl src/dsl.cpp)
add_executable(pf src/pf.cpp)

## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
set_target_properties(astar PROPERTIES OUTPUT_NAME astar PREFIX "")
set_target_properties(lpastar PROPERTIES OUTPUT_NAME lpastar PREFIX "")
set_target_properties(dsl PROPERTIES OUTPUT_NAME dsl PREFIX "")
set_target_properties(pf PROPERTIES OUTPUT_NAME pf PREFIX "")

## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(astar ${${PROJECT_NAME}_EXPORTED_TARGETS} ${Eigen3_EXPORTED_TARGETS} ${map_EXPORTED_TARGETS} ${rigid2d_EXPORTED_TARGETS} ${nuslam_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(lpastar ${${PROJECT_NAME}_EXPORTED_TARGETS} ${Eigen3_EXPORTED_TARGETS} ${map_EXPORTED_TARGETS} ${rigid2d_EXPORTED_TARGETS} ${nuslam_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(dsl ${${PROJECT_NAME}_EXPORTED_TARGETS} ${Eigen3_EXPORTED_TARGETS} ${map_EXPORTED_TARGETS} ${rigid2d_EXPORTED_TARGETS} ${nuslam_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(pf ${${PROJECT_NAME}_EXPORTED_TARGETS} ${Eigen3_EXPORTED_TARGETS} ${map_EXPORTED_TARGETS} ${rigid2d_EXPORTED_TARGETS} ${nuslam_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Specify libraries to link a library or executable target against
target_link_libraries(
astar
${map_LIBRARIES}
${rigid2d_LIBRARIES}
${nuslam_LIBRARIES}
Eigen3::Eigen
${PROJECT_NAME}
${catkin_LIBRARIES}
)

target_link_libraries(
lpastar
${map_LIBRARIES}
${rigid2d_LIBRARIES}
${nuslam_LIBRARIES}
Eigen3::Eigen
${PROJECT_NAME}
${catkin_LIBRARIES}
)

target_link_libraries(
dsl
${map_LIBRARIES}
${rigid2d_LIBRARIES}
${nuslam_LIBRARIES}
Eigen3::Eigen
${PROJECT_NAME}
${catkin_LIBRARIES}
)

target_link_libraries(
pf
${map_LIBRARIES}
${rigid2d_LIBRARIES}
${nuslam_LIBRARIES}
Eigen3::Eigen
${PROJECT_NAME}
${catkin_LIBRARIES}
)

#############
## Install ##
#############

# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html

## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
#   scripts/my_python_script
#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
install(TARGETS astar lpastar dsl pf
  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
install(TARGETS ${PROJECT_NAME}
  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
)

## Mark cpp header files for installation
install(DIRECTORY include/${PROJECT_NAME}/
  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
  FILES_MATCHING PATTERN "*.h"
  PATTERN ".svn" EXCLUDE
)

## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
#   # myfile1
#   # myfile2
#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )

#############
## Testing ##
#############

## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/${PROJECT_NAME}_test.cpp)
# if(TARGET ${PROJECT_NAME}-test)
#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()

## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

# if (CATKIN_ENABLE_TESTING)
#     catkin_add_gtest(${PROJECT_NAME}_test tests/${PROJECT_NAME}_test.cpp)
#     target_link_libraries(${PROJECT_NAME}_test ${catkin_Libraries} gtest_main ${PROJECT_NAME})
# endif()


================================================
FILE: global_planner/config/path.yaml
================================================
# NOTE: coordinates in cell #s, choose to convert to m/cm/mm later
# MUST BE DOUBLES
# Each iten is x,y
start: [5.0,  7.5]
goal:  [31.5, 36.0]

================================================
FILE: global_planner/include/global_planner/global_planner.hpp
================================================
#ifndef GLOBAL_PLANNER_INCLUDE_GUARD_HPP
#define GLOBAL_PLANNER_INCLUDE_GUARD_HPP
/// \file
/// \brief global_planners library which contains basic planner structure: nodes, neighbours, paths etc.
/// NOTE: rigid2d is from turtlebot3_from_scratch (tb3 in nuturtle.rosinstall)
#include <rigid2d/rigid2d.hpp>
#include <map/grid.hpp>
#include <map/prm.hpp>
#include <vector>
#include <eigen3/Eigen/Dense>
#include <ros/ros.h>

namespace global
{
    // Used to store Obstacle vertex coordinates
    using rigid2d::Vector2D;
    using map::Vertex;
    using map::Cell;
    using map::Index;
    using map::Obstacle;

    /// \brief stores Obstacle(s) to construct basic Planner
    class GlobalPlanner
    {
    public:
        /// \brief the default constructor creates an empty Planner
        GlobalPlanner();


    // protected instead of private so that child Class can access
    protected:
        std::vector<Vertex> PRM;

        std::vector<Cell> GRID;

         // Map obstacles
        std::vector<Obstacle> obstacles;

        // Map maximum coordinatesin x,y
        Vector2D map_max;

        // Map minimum coordinatesin x,y
        Vector2D map_min;

        // Robot's inflation radius for collision checking
        double inflate_robot;
    };
}

#endif


================================================
FILE: global_planner/include/global_planner/heuristic.hpp
================================================
#ifndef HEURISTIC_INCLUDE_GUARD_HPP
#define HEURISTIC_INCLUDE_GUARD_HPP
/// \file
/// \brief Heuristic search library to encompass A*, Theta* planners.

#include "global_planner/global_planner.hpp"
#include <queue>
#include <set>

namespace global
{
    // Used to store Obstacle vertex coordinates
    using rigid2d::Vector2D;
    using map::Vertex;
    using map::Cell;
    using map::Index;
    using map::Grid;

    // \brief contains relevant information for heuristic planners
    struct Node
    {
        // Empty constructor
        Node();

        // Stores position and other attributes  wrt PRM map
        Vertex vertex = Vertex(Vector2D());

        // Stores position and other attributes wrt Grid Map
        Cell cell = Cell(Vector2D(), 0.0);

        // row-major index for Grid or connected Vertex ID for PRM
        int parent_id = -1;

        // node ID for generic find in both PRM and Grid
        int id = -1;

        // Keys are for incremental search
        double key1 = 0.0;
        double key2 = 0.0;
        double rhs = 1e12;

        // NOTE: gcost will be initialized to 1e12 (inf) for LPA* and D* Lite
        double gcost, hcost, fcost = 0.0;

        // Store children for update using incremental gridmap
        std::vector<int> children_ids;
    };

    // Bolean operator so that std::find can be used with struct sub-element in closed list
    // inline so as to not confuse linker with multiple definitions
    inline bool operator<(const Node & n, const int & id) { return n.id < id; }
    inline bool operator<(const int & id, const Node & n) { return id < n.id; }
    inline bool operator<(const Node & n1, const Node & n2) { return n1.id < n2.id; }

    // \brief functor (function object) which compares the costs of two Nodes for heap sorting 
    class HeapComparator
    { 
    public: 
        int operator() (const Node& n1, const Node& n2) 
        { 
            if (rigid2d::almost_equal(n1.fcost, n2.fcost))
            {
                return n1.hcost > n2.hcost;
            } else
            {
                return n1.fcost > n2.fcost;
            }
        } 
    };

    /// \brief A* Planner
    class Astar : public GlobalPlanner
    {
    public:
        /// \brief constructor to initialize the A* Planner
        Astar(const std::vector<Obstacle> & obstacles_, const double & inflate_robot_);

        // \brief Plans a path on a PRM.
        // \param start: the starting coordinates
        // \param goal: the goal coordinates
        // \param map: the PRM - may be modified if shorter paths are found
        // \returns: the path as a vector of Nodes
        virtual std::vector<Node> plan(const Vector2D & start, const Vector2D & goal, std::vector<Vertex> & map);

        // \brief potentially modify the g cost and parent of a Node in PRM. virtual so it can be overriden by Thetastar.
        // \param open_list: list containing nodes to evaluate. Not const because it can be modified
        // \param neighbour: Node in the open list being potentially modified (also not const)
        virtual void update_vtx(std::priority_queue <Node, std::vector<Node>, HeapComparator > & open_list, Node & neighbour, const Node & current_node);

        // \brief insert a Node into the open list as is defined in the A* method for PRM
        // \param open_list: list containing nodes to evaluate. Not const because it can be modified
        // \param neighbour: Node to be added to open list (also not const)
        virtual void create_vtx(std::priority_queue <Node, std::vector<Node>, HeapComparator > & open_list, Node & neighbour, const Node & current_node);

        // \brief returns the path planned on the PRM or GRID
        // \param closed_list: Nodes to traverse
        std::vector<Node> trace_path(const Node & final_node, const std::set<Node, std::less<>> & closed_list);

        // \brief Plans a path on a Grid.
        // \param start: the starting coordinates
        // \param goal: the goal coordinates
        // \param map: the Grid Map
        // \returns: the path as a vector of Nodes
        std::vector<Node> plan(const Vector2D & start, const Vector2D & goal, const map::Grid & grid_, const double & resolution);

        // \brief potentially modify the g cost and parent of a Node in GRID. virtual so it can be overriden by Thetastar.
        // \param open_list: list containing nodes to evaluate. Not const because it can be modified
        // \param neighbour: Node in the open list being potentially modified (also not const)
        virtual void update_cell(std::priority_queue <Node, std::vector<Node>, HeapComparator > & open_list, Node & neighbour, const Node & current_node);

        // \brief insert a Node into the open list as is defined in the A* method for GRID
        // \param open_list: list containing nodes to evaluate. Not const because it can be modified
        // \param neighbour: Node to be added to open list (also not const)
        virtual void create_cell(std::priority_queue <Node, std::vector<Node>, HeapComparator > & open_list, Node & neighbour, const Node & current_node);

        // \brief Computes the heuristic to the goal using a custom admissible heuristic optimized for 8-point connectivity
        // NOTE: distance between nodes is simply computed using euclidean distance
        // \param n1: start Node for heuristic
        // \param n2: end Node for heuristic
        // \returns the grid distance
        double heuristic(const Node & n1, const Node & n2);

        // \brief retrieves the 8 neighbours of a node in a grid
        // \param n: Node whose neighbours to retrieve
        // \returns: vector of Nodes that are n's neighbours
        std::vector<Cell> get_neighbours(const Node & n, const std::vector<Cell> & map);
    };

    /// \brief Theta* Planner
    class Thetastar : public Astar
    {
    public:

        // Inherit constructor from A*
        using Astar::Astar;

        // \brief insert a Node into the open list as is defined in the A* method for PRM
        // \param open_list: list containing nodes to evaluate. Not const because it can be modified
        // \param neighbour: Node to be added to open list (also not const)
        void create_vtx(std::priority_queue <Node, std::vector<Node>, HeapComparator > & open_list, Node & neighbour, const Node & current_node) override;

        // \brief Overriden: potentially modify the g cost and parent of a Node in PRM. May get parent of parent as parent depending on line of sight
        // \param open_list: list containing nodes to evaluate. Not const because it can be modified
        // \param neighbour: Node in the open list being potentially modified (also not const)
        void update_vtx(std::priority_queue <Node, std::vector<Node>, HeapComparator > & open_list, Node & neighbour, const Node & current_node) override;
    };


    // \brief returns the vertex that most closely matches the given cartesian coordinates in PRM
    // \param position: the coordinates
    // \param map: the map whose elements are being searched for coordinates
    Vertex find_nearest_node(const Vector2D & position, const std::vector<Vertex> & map);

    // \brief returns the vertex that most closely matches the given cartesian coordinates in GRID
    // \param position: the coordinates
    // \param map: the map whose elements are being searched for coordinates
    Cell find_nearest_node(const Vector2D & position, const Grid & grid, const double & resolution);
}

#endif


================================================
FILE: global_planner/include/global_planner/incremental.hpp
================================================
#ifndef INCREMENTAL_INCLUDE_GUARD_HPP
#define INCREMENTAL_INCLUDE_GUARD_HPP
/// \file
/// \brief Incremental search library to encompass LPA* and D* Lite planners. I might add Incr. Phi* in the future.

#include "global_planner/heuristic.hpp"

namespace global
{
    // Used to store Obstacle vertex coordinates
    using rigid2d::Vector2D;
    using map::Vertex;
    using map::Cell;
    using map::Index;
    using map::Grid;

    // \brief functor (function object) which compares the priorities of two Nodes for heap sorting 
    class KeyComparator
    { 
    public: 
        int operator() (const Node& n1, const Node& n2) 
        { 
            if (rigid2d::almost_equal(n1.key1, n2.key1))
            {
                return n1.key2 > n2.key2;
            } else
            {
                return n1.key1 > n2.key1;
            }
        } 
    }; 


    // \brief functor (function object) which compares the costs of the predecessor of a Node
    class CostComparator
    { 
    public: 
        int operator() (const Node& n1, const Node& n2) 
        { 
            return n1.gcost > n2.gcost;
        } 
    }; 

    /// \brief LPA* Planner
    class LPAstar : public Astar
    {
    public:
        // Inherit constructor from A*
        using Astar::Astar;

        // NOTE: The method names below directly reference the LPA* pseudocode: http://idm-lab.org/bib/abstracts/papers/aaai02b.pdf

        // \brief Plans an incremental path on a Grid.
        // \returns: the path as a vector of Nodes
        void ComputeShortestPath();

        // Update the cost of a cell and remove it from the open list if it's there
        // param n: the node to update
        void UpdateCell(Node & n);

        // \brief sets the g-values of all cells (start and goal for efficiency) to infinity and sets their rhs values according to eqn 1.
        // Also inserts start (locally inconsistent vertex) into the (otherwise empty) priority queue.
        // This guarantees that the first run of ComputeShortestPath performs an exact A* search.
        virtual void Initialize(const Vector2D & start, const Vector2D & goal, const Grid & grid_, const double & resolution);

        // Calculates priorities 1 and 2 of a node, used in sorting for LPA*
        // \param n: the node whose keys we calculate
        void CalculateKeys(Node & n);

        // \brief Termination condition for ComputeShortestPath method.
        bool Continue(const int & iterations);

        // \brief traces the most up-to-date path
        std::vector<Node> trace_path(const Node & final_node);

        // \brief retrieves the 8 neighbours of a node in a grid of Nodes
        // \param n: Node whose neighbours to retrieve
        // \returns: vector of Nodes that are n's neighbours
        std::vector<Node> get_neighbours(const Node & n, const std::vector<Node> & map);

        // \brief returns the most current path
        // \returns: vector of Node
        virtual std::vector<Node> return_path();

        // \brief update FakeGrid (internal perception) with updated grid and then
        // use this new information to update each affected Node, and hence the path
        // param updated_grid: the updated grid
        // \returns: nodes that had to be updated based on new information
        virtual std::vector<Node> SimulateUpdate(const std::vector<Cell> & updated_grid);

        // \brief returns whether the current path is valid
        bool return_valid();

    protected:
        std::vector<Node> path;
        // Fake grid with limited visibility for simulating increment
        std::vector<Node> FakeGrid;

        // TODO: DELETE
        // Open list used for finding existing IDs
        // std::set<int> open_list_v;
        // // Open List
        // std::priority_queue <Node, std::vector<Node>, KeyComparator > open_list;

        // Open List. Uses ref wrapper for faster execution
        std::vector<Node> open_list;   

        // For easy management/record keeping
        Node start_node;
        Node goal_node;

        // For tracing most up-to-date path
        Node top_node;

        // Number of visible cells added per increment
        int viz_cells = 1;

        double BIG_NUM = std::numeric_limits<double>::infinity();

        bool valid_path = true;
    };


    // \brief D* Lite Planner
    class DSL: public LPAstar
    {
        using LPAstar::LPAstar;

    public:
        // \brief sets the g-values of all cells (start and goal for efficiency) to infinity and sets their rhs values according to eqn 1.
        // Also inserts start (locally inconsistent vertex) into the (otherwise empty) priority queue.
        // This guarantees that the first run of ComputeShortestPath performs an exact A* search.
        // NOTE: flips start and goal for D*Lite so that everything else stays the same
        void Initialize(const Vector2D & start, const Vector2D & goal, const Grid & grid_, const double & resolution);

        // \brief update FakeGrid (internal perception) with updated grid and then
        // use this new information to update each affected Node, and hence the path
        // param updated_grid: the updated grid
        // \returns: nodes that had to be updated based on new information
        std::vector<Node> SimulateUpdate(const std::vector<Cell> & updated_grid);

        // \brief returns the most current path. Flips path returned by LPAstar since D*L plans the opposite way
        // \returns: vector of Node
        std::vector<Node> return_path();

    };
}

#endif


================================================
FILE: global_planner/include/global_planner/potential_field.hpp
================================================
#ifndef POTENTIAL_FIELD_INCLUDE_GUARD_HPP
#define POTENTIAL_FIELD_INCLUDE_GUARD_HPP
/// \file
/// \brief Potential Field planning library.
#include <global_planner/global_planner.hpp>

namespace global
{
    // Used to store Obstacle vertex coordinates
    using rigid2d::Vector2D;
    using map::Vertex;
    using map::Cell;
    using map::Index;
    using map::Obstacle;

    /// \brief Potential Field (online-capable) planner
    class PotentialField : public GlobalPlanner
    {
    public:
        /// \brief constructor to initialize the Potential Field Planner
        // param obstacles_: the obstacle vectors containing bocked vertices in the map
        // param eta_: the termination condition for gradient descent. Also the descent direction
        // param zeta_: the attractive factor multiplier
        // param ada_: the repulsive factor multiplier
        // param d_thresh_: threshold for transition between quadratic and conic potential
        // param Q_thresh_: range of influence of obstacle
        PotentialField(const std::vector<Obstacle> & obstacles_, const double & eta_, const double & ada_,
                       const double & zeta_, const double & d_thresh_, const double & Q_thresh_);

        // \brief Calculates Attractive Gradient towards goal
        // \param cur_pos: the robot's current position, used for gradient computation
        // \param goal: the goal position, used for gradient computation
        // \returns: Vector2D containing gradient in x and y dimensions
        Vector2D AttractiveGradient(const Vector2D & cur_pos, const Vector2D & goal);

        // \brief Calculates Repulsive Gradient (cummulative) based on all obstacles
        // \param cur_pos: the robot's current position, used for gradient computation
        // \param obs: vector of Obstacle used for gradient computation. may be full map or reduced for
        // simulated visibility
        // \returns: Vector2D containing gradient in x and y dimensions
        Vector2D RepulsiveGradient(const Vector2D & cur_pos, const std::vector<Obstacle> & obs);

        // \brief find the closest point between a robot position and an obstacle
        // \param cur_pos: the robot's current position, used for gradient computation
        // \param vertices: vertices of an obstacle that we will use to find the closest point
        // \returns: Vector2D containing closest point's x,y coordinates
        Vector2D FindClosestPoint(const Vector2D & cur_pos, const std::vector<Vector2D> & vertices);

        // \brief returns the multi-dimensional norm of our gradient
        // \param partials: the partial gradients in each dimension
        // \returns: the norm
        double MultiDimNorm(const std::vector<double> partials);

        // \brief perform Gradient Descent for one step to move closer to the goal
        // \param cur_pos: the robot's current position, used for gradient computation
        // \param goal: the goal position, used for gradient computation
        // \param obs: vector of Obstacle used for gradient computation. may be full map or reduced for
        // simulated visibility
        // \returns: the robot's new x,y coordinates after Gradient Descent
        Vector2D OneStepGD(const Vector2D & cur_pos, const Vector2D & goal, std::vector<Obstacle> & obs);

        // \brief returns whether we have reached the goal
        // \returns: boolean to indicate termination
        bool return_terminate();


    // protected instead of private so that child Class can access
    protected:
        double eta;
        double zeta;
        double ada;
        double d_thresh;
        double Q_thresh;
        bool terminate = false;
    };
}

#endif


================================================
FILE: global_planner/launch/astar.launch
================================================
<launch>
  <!-- This launchfile loads a differential drive robot into RViz, whose parameters are set
       and can be modified in diff_params.yaml

       It also loads the custom map -->

  <!-- load the urdf into the parameter server from the xacro file-->
  <param name="robot_description" command="xacro '$(find nuturtle_gazebo)/urdf/diff_drive.gazebo.xacro'" />


  <!-- The robot_state_publisher reads the urdf from /robot_description parameter
       and listens to joint information on the /joint_states topic -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>

  <!-- The joint state publisher will be launched with a gui, read the urdf from /robot_description
       and publish the joint values on /joint_states. Optional launch using use_jsp_gui:=1 --> 
  <arg name="js_pub" default="True" doc="Launch the joint_state_publisher"/>
  <group if="$(arg js_pub)">
    <arg name="use_jsp_gui" default="False" doc="Launch the joint_state_publisher GUI to publish joint angles manually"/>
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
      <param name="use_gui" value="$(eval use_jsp_gui)" />
    </node>
  </group>

  <!-- load diff_drive_bot parameters to parameter server -->
  <rosparam command="load" file="$(find nuturtle_description)/config/diff_params.yaml" />

  <arg name="rviz_view" default="$(find global_planner)/rviz/astar.rviz"/>

  <node name="rviz" pkg="rviz" type="rviz" required="True" args="-d $(arg rviz_view)"/>


  <!-- Map Visualizer Node -->
  <node name="viz_map" pkg="map" type="viz_map" output="screen">
    <rosparam command="load" file="$(find map)/config/map.yaml"/>
    <param name="scale" value="10.0"/>
  </node>

  <!-- PRM with A*/Thetastar Node -->
  <node name="astar" pkg="global_planner" type="astar" output="screen">
    <rosparam command="load" file="$(find map)/config/map.yaml"/>
    <rosparam command="load" file="$(find global_planner)/config/path.yaml"/>
    <param name="n" value="300"/>
    <param name="k" value="10"/>
    <param name="thresh" value="0.1"/>
    <param name="inflate" value="0.1"/>
    <param name="planner" value="thetastar"/>
    <remap from="line_map" to="line_prm"/>
    <remap from="sph_map" to="sph_prm"/>
    <param name="scale" value="10.0"/>
    <param name="resolution" value="0.03"/>
    <param name="map_type" value="grid"/>
  </node>

</launch>

================================================
FILE: global_planner/launch/incremental.launch
================================================
<launch>
  <!-- This launchfile loads a differential drive robot into RViz, whose parameters are set
       and can be modified in diff_params.yaml

       It also loads the custom map -->

  <!-- load the urdf into the parameter server from the xacro file-->
  <param name="robot_description" command="xacro '$(find nuturtle_gazebo)/urdf/diff_drive.gazebo.xacro'" />


  <!-- The robot_state_publisher reads the urdf from /robot_description parameter
       and listens to joint information on the /joint_states topic -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>

  <!-- The joint state publisher will be launched with a gui, read the urdf from /robot_description
       and publish the joint values on /joint_states. Optional launch using use_jsp_gui:=1 --> 
  <arg name="js_pub" default="True" doc="Launch the joint_state_publisher"/>
  <group if="$(arg js_pub)">
    <arg name="use_jsp_gui" default="False" doc="Launch the joint_state_publisher GUI to publish joint angles manually"/>
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
      <param name="use_gui" value="$(eval use_jsp_gui)" />
    </node>
  </group>

  <!-- load diff_drive_bot parameters to parameter server -->
  <rosparam command="load" file="$(find nuturtle_description)/config/diff_params.yaml" />

  <arg name="rviz_view" default="$(find global_planner)/rviz/astar.rviz"/>

  <node name="rviz" pkg="rviz" type="rviz" required="True" args="-d $(arg rviz_view)"/>


  <!-- Map Visualizer Node -->
  <node name="viz_map" pkg="map" type="viz_map" output="screen">
    <rosparam command="load" file="$(find map)/config/map.yaml"/>
    <param name="scale" value="10.0"/>
  </node>

<arg name="lpa" default="False" doc="Plan with LPA*(True) or D*Lite(False)"/>
<group if="$(arg lpa)">
  <!-- Grid with LPA* Node -->
  <node name="lpastar" pkg="global_planner" type="lpastar" output="screen">
    <rosparam command="load" file="$(find map)/config/map.yaml"/>
    <rosparam command="load" file="$(find global_planner)/config/path.yaml"/>
    <param name="thresh" value="0.1"/>
    <param name="inflate" value="0.1"/>
    <param name="scale" value="10.0"/>
    <param name="resolution" value="0.1"/>
    <param name="frequency" value="6.0"/>
    <param name="visibility" value="5"/>
  </node>
</group>

  <group unless="$(arg lpa)">
   <!-- Grid with D*Lite Node -->
  <node name="dsl" pkg="global_planner" type="dsl" output="screen">
    <rosparam command="load" file="$(find map)/config/map.yaml"/>
    <rosparam command="load" file="$(find global_planner)/config/path.yaml"/>
    <param name="thresh" value="0.1"/>
    <param name="inflate" value="0.1"/>
    <param name="scale" value="10.0"/>
    <param name="resolution" value="0.1"/>
    <param name="frequency" value="6.0"/>
    <param name="visibility" value="5"/>
  </node>
</group>

</launch>

================================================
FILE: global_planner/launch/potential_field.launch
================================================
<launch>
  <!-- This launchfile loads a differential drive robot into RViz, whose parameters are set
       and can be modified in diff_params.yaml

       It also loads the custom map -->

  <!-- load the urdf into the parameter server from the xacro file-->
  <param name="robot_description" command="xacro '$(find nuturtle_gazebo)/urdf/diff_drive.gazebo.xacro'" />


  <!-- The robot_state_publisher reads the urdf from /robot_description parameter
       and listens to joint information on the /joint_states topic -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>

  <!-- The joint state publisher will be launched with a gui, read the urdf from /robot_description
       and publish the joint values on /joint_states. Optional launch using use_jsp_gui:=1 --> 
  <arg name="js_pub" default="True" doc="Launch the joint_state_publisher"/>
  <group if="$(arg js_pub)">
    <arg name="use_jsp_gui" default="False" doc="Launch the joint_state_publisher GUI to publish joint angles manually"/>
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
      <param name="use_gui" value="$(eval use_jsp_gui)" />
    </node>
  </group>

  <!-- load diff_drive_bot parameters to parameter server -->
  <rosparam command="load" file="$(find nuturtle_description)/config/diff_params.yaml" />

  <arg name="rviz_view" default="$(find global_planner)/rviz/astar.rviz"/>

  <node name="rviz" pkg="rviz" type="rviz" required="True" args="-d $(arg rviz_view)"/>


  <!-- Map Visualizer Node -->
  <node name="viz_map" pkg="map" type="viz_map" output="screen">
    <rosparam command="load" file="$(find map)/config/map.yaml"/>
    <param name="scale" value="10.0"/>
  </node>

  <!-- Potential Field Node -->
  <node name="pf" pkg="global_planner" type="pf" output="screen">
    <rosparam command="load" file="$(find map)/config/map.yaml"/>
    <rosparam command="load" file="$(find global_planner)/config/path.yaml"/>
    <param name="thresh" value="0.1"/>
    <param name="inflate" value="0.1"/>
    <param name="scale" value="10.0"/>
    <param name="resolution" value="0.1"/>
    <param name="frequency" value="100.0"/>
    <param name="visibility" value="5"/>
    <param name="eta" value=".01"/>
    <param name="ada" value=".1"/>
    <param name="zeta" value=".9"/>
    <param name="d_thresh" value="5.0"/>
    <param name="Q_thresh" value="0.15"/>
  </node>

</launch>

================================================
FILE: global_planner/package.xml
================================================
<?xml version="1.0"?>
<package format="2">
  <name>global_planner</name>
  <version>0.0.1</version>
  <description>Heuristic Search and Potential Field global planners</description>

  <!-- One maintainer tag required, multiple allowed, one person per tag -->
  <!-- Example:  -->
  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
  <maintainer email="mrahme97@gmail.com">Maurice Rahme</maintainer>


  <!-- One license tag required, multiple allowed, one license per tag -->
  <!-- Commonly used license strings: -->
  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
  <license>MIT</license>



  <!-- Url tags are optional, but multiple are allowed, one per tag -->
  <!-- Optional attribute type can be: website, bugtracker, or repository -->
  <!-- Example: -->
  <!-- <url type="website">http://wiki.ros.org/global_planner</url> -->


  <!-- Author tags are optional, multiple are allowed, one per tag -->
  <!-- Authors do not have to be maintainers, but could be -->
  <!-- Example: -->
  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->


  <!-- The *depend tags are used to specify dependencies -->
  <!-- Dependencies can be catkin packages or system dependencies -->
  <!-- Examples: -->
  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
  <!--   <depend>roscpp</depend> -->
  <!--   Note that this is equivalent to the following: -->
  <!--   <build_depend>roscpp</build_depend> -->
  <!--   <exec_depend>roscpp</exec_depend> -->
  <!-- Use build_depend for packages you need at compile time: -->
  <!--   <build_depend>message_generation</build_depend> -->
  <!-- Use build_export_depend for packages you need in order to build against this package: -->
  <!--   <build_export_depend>message_generation</build_export_depend> -->
  <!-- Use buildtool_depend for build tool packages: -->
  <!--   <buildtool_depend>catkin</buildtool_depend> -->
  <!-- Use exec_depend for packages you need at runtime: -->
  <!--   <exec_depend>message_runtime</exec_depend> -->
  <!-- Use test_depend for packages you need only for testing: -->
  <!--   <test_depend>gtest</test_depend> -->
  <!-- Use doc_depend for packages you need only for building documentation: -->
  <!--   <doc_depend>doxygen</doc_depend> -->
  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>map</build_depend>
  <build_depend>message_generation</build_depend>
  <build_depend>message_runtime</build_depend>
  <build_depend>rigid2d</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>nuslam</build_depend>
  <build_depend>visualization_msgs</build_depend>
  <build_export_depend>map</build_export_depend>
  <build_export_depend>message_generation</build_export_depend>
  <build_export_depend>message_runtime</build_export_depend>
  <build_export_depend>rigid2d</build_export_depend>
  <build_export_depend>nuslam</build_export_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>visualization_msgs</build_export_depend>
  <exec_depend>map</exec_depend>
  <exec_depend>message_runtime</exec_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>rigid2d</exec_depend>
  <exec_depend>nuslam</exec_depend>
  <exec_depend>visualization_msgs</exec_depend>

  <test_depend>rostest</test_depend>
  <test_depend>rosunit</test_depend>
  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->

  </export>
</package>


================================================
FILE: global_planner/rviz/astar.rviz
================================================
Panels:
  - Class: rviz/Displays
    Help Height: 77
    Name: Displays
    Property Tree Widget:
      Expanded:
        - /Global Options1
        - /Grid1
        - /TF1/Frames1
        - /TF1/Tree1
        - /Obstacles1
        - /PRM1
        - /PATH1
        - /PATH DEBUG1
        - /Path1
      Splitter Ratio: 0.5
    Tree Height: 714
  - Class: rviz/Selection
    Name: Selection
  - Class: rviz/Tool Properties
    Expanded:
      - /2D Pose Estimate1
      - /2D Nav Goal1
      - /Publish Point1
    Name: Tool Properties
    Splitter Ratio: 0.5886790156364441
  - Class: rviz/Views
    Expanded:
      - /Current View1
    Name: Views
    Splitter Ratio: 0.5
  - Class: rviz/Time
    Experimental: false
    Name: Time
    SyncMode: 0
    SyncSource: ""
Preferences:
  PromptSaveOnExit: true
Toolbars:
  toolButtonStyle: 2
Visualization Manager:
  Class: ""
  Displays:
    - Alpha: 0.5
      Cell Size: 0.10000000149011612
      Class: rviz/Grid
      Color: 85; 87; 83
      Enabled: true
      Line Style:
        Line Width: 0.029999999329447746
        Value: Lines
      Name: Grid
      Normal Cell Count: 0
      Offset:
        X: 0
        Y: 0
        Z: 0
      Plane: XY
      Plane Cell Count: 200
      Reference Frame: <Fixed Frame>
      Value: true
    - Class: rviz/TF
      Enabled: true
      Frame Timeout: 15
      Frames:
        All Enabled: false
        ball_caster:
          Value: false
        base_footprint:
          Value: false
        base_link:
          Value: false
        base_scan:
          Value: true
        rl_wheel:
          Value: false
        rr_wheel:
          Value: false
      Marker Scale: 1
      Name: TF
      Show Arrows: true
      Show Axes: true
      Show Names: true
      Tree:
        base_footprint:
          base_link:
            ball_caster:
              {}
            base_scan:
              {}
            rl_wheel:
              {}
            rr_wheel:
              {}
      Update Interval: 0
      Value: true
    - Alpha: 1
      Class: rviz/RobotModel
      Collision Enabled: false
      Enabled: true
      Links:
        All Links Enabled: true
        Expand Joint Details: false
        Expand Link Details: false
        Expand Tree: false
        Link Tree Style: Links in Alphabetic Order
        ball_caster:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        base_footprint:
          Alpha: 1
          Show Axes: false
          Show Trail: false
        base_link:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        base_scan:
          Alpha: 1
          Show Axes: false
          Show Trail: false
        rl_wheel:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        rr_wheel:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
      Name: RobotModel
      Robot Description: robot_description
      TF Prefix: ""
      Update Interval: 0
      Value: true
      Visual Enabled: true
    - Class: rviz/MarkerArray
      Enabled: true
      Marker Topic: /map
      Name: Obstacles
      Namespaces:
        "": true
      Queue Size: 100
      Value: true
    - Class: rviz/MarkerArray
      Enabled: false
      Marker Topic: /prm
      Name: PRM
      Namespaces:
        {}
      Queue Size: 100
      Value: false
    - Class: rviz/MarkerArray
      Enabled: true
      Marker Topic: /path
      Name: PATH
      Namespaces:
        "": true
      Queue Size: 100
      Value: true
    - Class: rviz/MarkerArray
      Enabled: true
      Marker Topic: /path_debug
      Name: PATH DEBUG
      Namespaces:
        {}
      Queue Size: 100
      Value: true
    - Alpha: 0.699999988079071
      Class: rviz/Map
      Color Scheme: map
      Draw Behind: false
      Enabled: true
      Name: GridMap
      Topic: /grid_map
      Unreliable: false
      Use Timestamp: false
      Value: true
    - Alpha: 1
      Buffer Length: 1
      Class: rviz/Path
      Color: 214; 109; 13
      Enabled: true
      Head Diameter: 0.30000001192092896
      Head Length: 0.20000000298023224
      Length: 0.30000001192092896
      Line Style: Billboards
      Line Width: 0.029999999329447746
      Name: Path
      Offset:
        X: 0
        Y: 0
        Z: 0
      Pose Color: 255; 85; 255
      Pose Style: None
      Radius: 0.029999999329447746
      Shaft Diameter: 0.10000000149011612
      Shaft Length: 0.10000000149011612
      Topic: /pf/robot_path
      Unreliable: false
      Value: true
  Enabled: true
  Global Options:
    Background Color: 48; 48; 48
    Default Light: true
    Fixed Frame: base_footprint
    Frame Rate: 30
  Name: root
  Tools:
    - Class: rviz/Interact
      Hide Inactive Objects: true
    - Class: rviz/MoveCamera
    - Class: rviz/Select
    - Class: rviz/FocusCamera
    - Class: rviz/Measure
    - Class: rviz/SetInitialPose
      Theta std deviation: 0.2617993950843811
      Topic: /initialpose
      X std deviation: 0.5
      Y std deviation: 0.5
    - Class: rviz/SetGoal
      Topic: /move_base_simple/goal
    - Class: rviz/PublishPoint
      Single click: true
      Topic: /clicked_point
  Value: true
  Views:
    Current:
      Class: rviz/Orbit
      Distance: 6.352022171020508
      Enable Stereo Rendering:
        Stereo Eye Separation: 0.05999999865889549
        Stereo Focal Distance: 1
        Swap Stereo Eyes: false
        Value: false
      Focal Point:
        X: 1.7924532890319824
        Y: 2.2708258628845215
        Z: 0.14110127091407776
      Focal Shape Fixed Size: true
      Focal Shape Size: 0.05000000074505806
      Invert Z Axis: false
      Name: Current View
      Near Clip Distance: 0.009999999776482582
      Pitch: 1.5697963237762451
      Target Frame: <Fixed Frame>
      Value: Orbit (rviz)
      Yaw: 4.708594799041748
    Saved: ~
Window Geometry:
  Displays:
    collapsed: false
  Height: 1004
  Hide Left Dock: false
  Hide Right Dock: true
  QMainWindow State: 000000ff00000000fd00000004000000000000015600000352fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000352000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000352fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b00000352000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004680000003efc0100000002fb0000000800540069006d00650100000000000004680000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000030c0000035200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
  Selection:
    collapsed: false
  Time:
    collapsed: false
  Tool Properties:
    collapsed: false
  Views:
    collapsed: true
  Width: 1128
  X: 167
  Y: 0


================================================
FILE: global_planner/src/astar.cpp
================================================
/// \file
/// \brief Draws Each Obstacle in RViz using MarkerArrays
///
/// PARAMETERS:
/// PUBLISHES:
/// SUBSCRIBES:
/// FUNCTIONS:

#include <ros/ros.h>

#include <math.h>
#include <string>
#include <vector>

#include "map/map.hpp"
#include "map/prm.hpp"
#include "map/grid.hpp"
#include <nav_msgs/OccupancyGrid.h>
#include "global_planner/heuristic.hpp"

#include "nuslam/TurtleMap.h"

#include <functional>  // To use std::bind
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <geometry_msgs/Point.h>

// Used to deal with YAML list of lists
#include <xmlrpcpp/XmlRpcValue.h> // catkin component


int main(int argc, char** argv)
/// The Main Function ///
{
    ROS_INFO("STARTING NODE: astar");

    // Vars
    double frequency = 10.0;
    // Scale by which to transform marker poses (10 cm/cell so we use 10)
    double SCALE = 10.0;
    std::string frame_id = "base_footprint";
    std::string planner_type = "astar";
    std::string map_type = "prm";
    XmlRpc::XmlRpcValue xml_obstacles;
    std::vector<double> start_vec{7.0, 3.0};
    std::vector<double> goal_vec{7.0, 26.0};
    // resolution by which to transform marker poses (10 cm/cell so we use 10)
    double resolution = 0.01;

    // PRM Parameters
    int n = 10;
    int k = 5;
    double thresh = 0.01;
    double inflate = 0.1;

    // store Obstacle(s) here to create Map
    std::vector<map::Obstacle> obstacles_v;

    ros::init(argc, argv, "astar_node"); // register the node on ROS
    ros::NodeHandle nh; // get a handle to ROS
    ros::NodeHandle nh_("~"); // get a handle to ROS
    // Parameters
    nh_.getParam("frequency", frequency);
    nh_.getParam("obstacles", xml_obstacles);
    nh_.getParam("start", start_vec);
    nh_.getParam("goal", goal_vec);
    nh_.getParam("map_frame_id", frame_id);
    nh_.getParam("n", n);
    nh_.getParam("k", k);
    nh_.getParam("thresh", thresh);
    nh_.getParam("inflate", inflate);
    nh_.getParam("scale", SCALE);
    nh_.getParam("planner", planner_type);
    nh_.getParam("map_type", map_type);
    nh_.getParam("resolution", resolution);

    rigid2d::Vector2D start(start_vec.at(0)/SCALE, start_vec.at(1)/SCALE);
    rigid2d::Vector2D goal(goal_vec.at(0)/SCALE, goal_vec.at(1)/SCALE);

    // 'obstacles' is a triple-nested list.
    // 1st level: obstacle (Obstacle), 2nd level: vertices (std::vector), 3rd level: coordinates (Vector2D)

    // std::vector<Obstacle>
    if(xml_obstacles.getType() != XmlRpc::XmlRpcValue::TypeArray)
    {
    ROS_ERROR("There is no list of obstacles");
    } else
    {
        for(int i = 0; i < xml_obstacles.size(); ++i)
        {
          // Obstacle contains std::vector<Vector2D>
          // create Obstacle with empty vertex vector
          map::Obstacle obs;
          if(xml_obstacles[i].getType() != XmlRpc::XmlRpcValue::TypeArray)
          {
              ROS_ERROR("obstacles[%d] has no vertices", i);
          } else {
              for(int j = 0; j < xml_obstacles[i].size(); ++j)
              {
                // Vector2D contains x,y coords
                if(xml_obstacles[i][j].size() != 2)
                {
                   ROS_ERROR("Vertex[%d] of obstacles[%d] is not a pair of coordinates", j, i);
                } else if(
                  xml_obstacles[i][j][0].getType() != XmlRpc::XmlRpcValue::TypeDouble or
                  xml_obstacles[i][j][1].getType() != XmlRpc::XmlRpcValue::TypeDouble)
                {
                  ROS_ERROR("The coordinates of vertex[%d] of obstacles[%d] are not doubles", j, i);
                } else {
                  // PASSED ALL THE TESTS: push Vector2D to vertices list in Obstacle object
                  rigid2d::Vector2D vertex(xml_obstacles[i][j][0], xml_obstacles[i][j][1]);
                  // NOTE: SCALE DOWN
                  vertex.x /= SCALE;
                  vertex.y /= SCALE;
                  obs.vertices.push_back(vertex);
                }
              }
          }
          // push Obstacle object to vector of Object(s) in Map
          obstacles_v.push_back(obs);
        }
    }

    // Initialize Marker
    // Init Marker Array Publisher
    ros::Publisher map_pub = nh.advertise<visualization_msgs::MarkerArray>("prm", 1);

    ros::Publisher path_pub = nh.advertise<visualization_msgs::MarkerArray>("path", 1);

    ros::Publisher debug_pub = nh.advertise<visualization_msgs::MarkerArray>("path_debug", 1);

    ros::Publisher grid_pub = nh.advertise<nav_msgs::OccupancyGrid>("grid_map", 1);
    // Initialize grid_map outside
    nav_msgs::OccupancyGrid grid_map;
    // rviz representation of the grid
    std::vector<int8_t> map;

    // Init Marker Array
    visualization_msgs::MarkerArray map_arr;
    visualization_msgs::MarkerArray path_arr;
    visualization_msgs::MarkerArray path_debug;

    // Init Marker
    visualization_msgs::Marker marker;
    marker.header.frame_id = frame_id;
    marker.header.stamp = ros::Time::now();
    // marker.ns = "my_namespace";
    // marker.id = 0;
    marker.type = visualization_msgs::Marker::LINE_STRIP;
    marker.action = visualization_msgs::Marker::ADD;
    marker.pose.position.z = 0;
    marker.pose.orientation.x = 0.0;
    marker.pose.orientation.y = 0.0;
    marker.pose.orientation.z = 0.0;
    marker.pose.orientation.w = 1.0;
    marker.scale.x = 0.02;
    marker.color.r = 0.96f;
    marker.color.g = 0.475f;
    marker.color.b = 0.0f;
    marker.color.a = 1.0;
    marker.lifetime = ros::Duration();

    visualization_msgs::Marker sph_mkr;
    sph_mkr = marker;
    sph_mkr.type = visualization_msgs::Marker::SPHERE;
    sph_mkr.scale.x = 0.02;
    sph_mkr.scale.y = 0.02;
    sph_mkr.scale.z = 0.02;

    // Color
    sph_mkr.color.r = 0.5f;
    sph_mkr.color.g = 0.0f;
    sph_mkr.color.b = 0.5f;

    // LINE_STRIP relative to this pose
    marker.pose.position.x = 0.0;
    marker.pose.position.y = 0.0;


    // PATH MARKERS
    visualization_msgs::Marker path_marker;
    path_marker = marker;
    // More visible than PRM
    path_marker.scale.x = 0.03;
    path_marker.color.r = 0.2;
    path_marker.color.g = 1.0;
    path_marker.color.b = 0.2;

    visualization_msgs::Marker path_sph_mkr;
    path_sph_mkr = sph_mkr;
    // More visible than PRM
    path_sph_mkr.type = visualization_msgs::Marker::CUBE;
    path_sph_mkr.scale.x = 0.02;
    path_sph_mkr.scale.y = 0.02;
    path_sph_mkr.scale.z = 0.02;
    path_sph_mkr.color.r = 0.0f;
    path_sph_mkr.color.g = 0.0f;
    path_sph_mkr.color.b = 0.0f;


    // A* or Theta* Path
    std::vector<global::Node> path;
    // Debug Path (Usually A* to compare with Theta*)
    std::vector<global::Node> path2;

    if (map_type == "prm")
      // PRM VERSION
    {
      // Build PRM
      map::PRM prm(obstacles_v, inflate);
      prm.build_map(n, k, thresh);
      // DRAW PRM
      int prm_marker_id = 0;
      auto configurations = prm.return_prm();
      for (auto node_iter = configurations.begin(); node_iter != configurations.end(); node_iter++)
      {
          marker.points.clear();
          marker.id = prm_marker_id;
          prm_marker_id++;

          // Check if a node has edges before plotting
          if (node_iter->id_set.size() > 0)
          {
              for (auto id_iter = node_iter->id_set.begin(); id_iter != node_iter->id_set.end(); id_iter++)
                  {
                    // Add node as first marker vertex
                    geometry_msgs::Point first_vertex;
                    first_vertex.x = node_iter->coords.x;
                    first_vertex.y = node_iter->coords.y;
                    first_vertex.z = 0.0;
                    marker.points.push_back(first_vertex);

                    // Find Vertex for each ID
                    auto neighbor_iter = configurations.at(*id_iter);

                    geometry_msgs::Point new_vertex;
                    new_vertex.x = neighbor_iter.coords.x;
                    new_vertex.y = neighbor_iter.coords.y;
                    new_vertex.z = 0.0;
                    marker.points.push_back(new_vertex);

                    // Also push back cylinders
                    sph_mkr.pose.position.x = node_iter->coords.x;
                    sph_mkr.pose.position.y = node_iter->coords.y;
                    sph_mkr.id = prm_marker_id;
                    prm_marker_id++;
                    map_arr.markers.push_back(sph_mkr);
                  }
              // Push to Marker Array
              map_arr.markers.push_back(marker);
          }
      }

      ROS_INFO("PRM Built!");

      // PLAN on PRM using A* or Theta*
      if (planner_type == "astar")
      {
        ROS_INFO("Planning using A*!");
        global::Astar astar(obstacles_v, inflate);
        path = astar.plan(start, goal, configurations);
      } else
      {
        ROS_INFO("Planning using Theta*!");
        global::Thetastar theta_star(obstacles_v, inflate);
        path = theta_star.plan(start, goal, configurations);
        ROS_INFO("Planning using A*!");
        global::Astar astar(obstacles_v, inflate);
        path2 = astar.plan(start, goal, configurations);
      }

      // DRAW PATH
      int path_marker_id = 0;
      for (auto path_iter = path.begin(); path_iter != path.end(); path_iter++)
      {
          // Add node as marker vertex
          geometry_msgs::Point vtx;
          vtx.x = path_iter->vertex.coords.x;
          vtx.y = path_iter->vertex.coords.y;
          vtx.z = 0.0;
          path_marker.points.push_back(vtx);

          // Also push back cylinders
          path_sph_mkr.pose.position.x = path_iter->vertex.coords.x;
          path_sph_mkr.pose.position.y = path_iter->vertex.coords.y;
          path_sph_mkr.id = path_marker_id;
          path_marker_id++;
          path_arr.markers.push_back(path_sph_mkr);

      }
      path_marker.id = path_marker_id;
      path_arr.markers.push_back(path_marker);

      // DRAW DEBUG PATH
      path_marker_id = 0;
      path_marker.points.clear();
      path_marker.color.r = 1.0;
      path_marker.color.g = 0.2;
      path_marker.color.b = 0.2;
      for (auto path2_iter = path2.begin(); path2_iter != path2.end(); path2_iter++)
      {
          // Add node as marker vertex
          geometry_msgs::Point vtx;
          vtx.x = path2_iter->vertex.coords.x;
          vtx.y = path2_iter->vertex.coords.y;
          vtx.z = 0.0;
          path_marker.points.push_back(vtx);

          // Also push back cylinders
          path_sph_mkr.pose.position.x = path2_iter->vertex.coords.x;
          path_sph_mkr.pose.position.y = path2_iter->vertex.coords.y;
          path_sph_mkr.id = path_marker_id;
          path_marker_id++;
          path_debug.markers.push_back(path_sph_mkr);

      }
      path_marker.id = path_marker_id;
      path_debug.markers.push_back(path_marker);

    } else
    // GRID Version
    {
      // Initialize Grid
      map::Grid grid(obstacles_v, inflate);

      // Build Map
      grid.build_map(resolution);

      ROS_INFO("Grid Built!");

      // Get map bounds
      auto bounds = grid.return_map_bounds();
      auto gridsize = grid.return_grid_dimensions();

      // rviz representation of the grid
      grid.occupancy_grid(map);

      // Grid Pose
      geometry_msgs::Pose map_pose;
      map_pose.position.x = bounds.at(0).x;
      map_pose.position.y = bounds.at(0).y;
      map_pose.position.z = 0.0;
      map_pose.orientation.x = 0.0;
      map_pose.orientation.y = 0.0;
      map_pose.orientation.z = 0.0;
      map_pose.orientation.w = 1.0;

      // Occupancy grid for visualization
      grid_map.header.frame_id = frame_id;
      grid_map.info.resolution = resolution;
      grid_map.info.width = gridsize.at(0);
      grid_map.info.height = gridsize.at(1);

      // std::cout << "Width: " << gridsize.at(0) << "\t Height: " << gridsize.at(1) << std::endl;

      grid_map.info.origin = map_pose;

      ROS_INFO("Planning using A*!");
      global::Astar astar(obstacles_v, inflate);
      path = astar.plan(start, goal, grid, resolution);
      path2 = path;


      // DRAW PATH
      int path_marker_id = 0;
      for (auto path_iter = path.begin(); path_iter != path.end(); path_iter++)
      {
          // Add node as marker cell
          geometry_msgs::Point vtx;
          vtx.x = path_iter->cell.coords.x;
          vtx.y = path_iter->cell.coords.y;
          vtx.z = 0.0;
          path_marker.points.push_back(vtx);

          // Also push back cylinders
          path_sph_mkr.pose.position.x = path_iter->cell.coords.x;
          path_sph_mkr.pose.position.y = path_iter->cell.coords.y;
          path_sph_mkr.id = path_marker_id;
          path_marker_id++;
          path_arr.markers.push_back(path_sph_mkr);

      }
      path_marker.id = path_marker_id;
      path_arr.markers.push_back(path_marker);

      // DRAW DEBUG PATH
      path_marker_id = 0;
      path_marker.points.clear();
      path_marker.color.r = 1.0;
      path_marker.color.g = 0.2;
      path_marker.color.b = 0.2;
      for (auto path2_iter = path2.begin(); path2_iter != path2.end(); path2_iter++)
      {
          // Add node as marker cell
          geometry_msgs::Point vtx;
          vtx.x = path2_iter->cell.coords.x;
          vtx.y = path2_iter->cell.coords.y;
          vtx.z = 0.0;
          path_marker.points.push_back(vtx);

          // Also push back cylinders
          path_sph_mkr.pose.position.x = path2_iter->cell.coords.x;
          path_sph_mkr.pose.position.y = path2_iter->cell.coords.y;
          path_sph_mkr.id = path_marker_id;
          path_marker_id++;
          path_debug.markers.push_back(path_sph_mkr);

      }
      path_marker.id = path_marker_id;
      path_debug.markers.push_back(path_marker);

    }

    ros::Rate rate(frequency);

    // Main While
    while (ros::ok())
    {
        ros::spinOnce();

        // Publish PRM Map
        map_pub.publish(map_arr);

        // Publish GRID Map
        grid_map.header.stamp = ros::Time::now();
        grid_map.info.map_load_time = ros::Time::now();
        grid_map.data = map;
        grid_pub.publish(grid_map);

        // Publish Path
        path_pub.publish(path_arr);

        // Publish Path DEBUG
        debug_pub.publish(path_debug);
        rate.sleep();
    }

    return 0;
}

================================================
FILE: global_planner/src/dsl.cpp
================================================
/// \file
/// \brief Draws Each Obstacle in RViz using MarkerArrays
///
/// PARAMETERS:
/// PUBLISHES:
/// SUBSCRIBES:
/// FUNCTIONS:

#include <ros/ros.h>

#include <math.h>
#include <string>
#include <vector>

#include "map/map.hpp"
#include "map/prm.hpp"
#include "map/grid.hpp"
#include <nav_msgs/OccupancyGrid.h>
#include "global_planner/incremental.hpp"

#include "nuslam/TurtleMap.h"

#include <functional>  // To use std::bind
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <geometry_msgs/Point.h>

// Used to deal with YAML list of lists
#include <xmlrpcpp/XmlRpcValue.h> // catkin component


int main(int argc, char** argv)
/// The Main Function ///
{
    ROS_INFO("STARTING NODE: astar");

    // Vars
    double frequency = 10.0;
    // Scale by which to transform marker poses (10 cm/cell so we use 10)
    double SCALE = 10.0;
    std::string frame_id = "base_footprint";
    XmlRpc::XmlRpcValue xml_obstacles;
    std::vector<double> start_vec{7.0, 3.0};
    std::vector<double> goal_vec{7.0, 26.0};
    double resolution = 0.01;

    // Map Parameters
    double thresh = 0.01;
    double inflate = 0.1;
    int visibility = 5;

    // store Obstacle(s) here to create Map
    std::vector<map::Obstacle> obstacles_v;

    ros::init(argc, argv, "astar_node"); // register the node on ROS
    ros::NodeHandle nh; // get a handle to ROS
    ros::NodeHandle nh_("~"); // get a handle to ROS
    // Parameters
    nh_.getParam("frequency", frequency);
    nh_.getParam("obstacles", xml_obstacles);
    nh_.getParam("start", start_vec);
    nh_.getParam("goal", goal_vec);
    nh_.getParam("map_frame_id", frame_id);
    nh_.getParam("thresh", thresh);
    nh_.getParam("inflate", inflate);
    nh_.getParam("scale", SCALE);
    nh_.getParam("resolution", resolution);
    nh_.getParam("visibility", visibility);

    rigid2d::Vector2D start(start_vec.at(0)/SCALE, start_vec.at(1)/SCALE);
    rigid2d::Vector2D goal(goal_vec.at(0)/SCALE, goal_vec.at(1)/SCALE);

    // 'obstacles' is a triple-nested list.
    // 1st level: obstacle (Obstacle), 2nd level: vertices (std::vector), 3rd level: coordinates (Vector2D)

    // std::vector<Obstacle>
    if(xml_obstacles.getType() != XmlRpc::XmlRpcValue::TypeArray)
    {
    ROS_ERROR("There is no list of obstacles");
    } else
    {
        for(int i = 0; i < xml_obstacles.size(); ++i)
        {
          // Obstacle contains std::vector<Vector2D>
          // create Obstacle with empty vertex vector
          map::Obstacle obs;
          if(xml_obstacles[i].getType() != XmlRpc::XmlRpcValue::TypeArray)
          {
              ROS_ERROR("obstacles[%d] has no vertices", i);
          } else {
              for(int j = 0; j < xml_obstacles[i].size(); ++j)
              {
                // Vector2D contains x,y center_coords
                if(xml_obstacles[i][j].size() != 2)
                {
                   ROS_ERROR("Vertex[%d] of obstacles[%d] is not a pair of coordinates", j, i);
                } else if(
                  xml_obstacles[i][j][0].getType() != XmlRpc::XmlRpcValue::TypeDouble or
                  xml_obstacles[i][j][1].getType() != XmlRpc::XmlRpcValue::TypeDouble)
                {
                  ROS_ERROR("The coordinates of vertex[%d] of obstacles[%d] are not doubles", j, i);
                } else {
                  // PASSED ALL THE TESTS: push Vector2D to vertices list in Obstacle object
                  rigid2d::Vector2D vertex(xml_obstacles[i][j][0], xml_obstacles[i][j][1]);
                  // NOTE: SCALE DOWN
                  vertex.x /= SCALE;
                  vertex.y /= SCALE;
                  obs.vertices.push_back(vertex);
                }
              }
          }
          // push Obstacle object to vector of Object(s) in Map
          obstacles_v.push_back(obs);
        }
    }

    // Initialize Marker
    // Init Marker Array Publisher
    ros::Publisher path_pub = nh.advertise<visualization_msgs::MarkerArray>("path", 1);

    ros::Publisher grid_pub = nh.advertise<nav_msgs::OccupancyGrid>("grid_map", 1);
    // Initialize grid_map outside
    nav_msgs::OccupancyGrid grid_map;
    // rviz representation of the grid
    std::vector<int8_t> map;

    // Init Marker Array
    visualization_msgs::MarkerArray path_arr;

    // Init Marker
    visualization_msgs::Marker marker;
    marker.header.frame_id = frame_id;
    marker.header.stamp = ros::Time::now();
    marker.type = visualization_msgs::Marker::LINE_STRIP;
    marker.action = visualization_msgs::Marker::ADD;
    marker.pose.position.z = 0;
    marker.pose.orientation.x = 0.0;
    marker.pose.orientation.y = 0.0;
    marker.pose.orientation.z = 0.0;
    marker.pose.orientation.w = 1.0;
    marker.scale.x = 0.02;
    marker.color.r = 0.96f;
    marker.color.g = 0.475f;
    marker.color.b = 0.0f;
    marker.color.a = 1.0;
    marker.lifetime = ros::Duration();

    // LINE_STRIP relative to this pose
    marker.pose.position.x = 0.0;
    marker.pose.position.y = 0.0;

    // PATH MARKERS
    visualization_msgs::Marker path_marker;
    path_marker = marker;
    // More visible than PRM
    path_marker.scale.x = 0.03;
    path_marker.color.r = 0.2;
    path_marker.color.g = 1.0;
    path_marker.color.b = 0.2;

    visualization_msgs::Marker path_sph_mkr;
    path_sph_mkr = marker;
    // More visible than PRM
    path_sph_mkr.type = visualization_msgs::Marker::CUBE;
    path_sph_mkr.scale.x = 0.02;
    path_sph_mkr.scale.y = 0.02;
    path_sph_mkr.scale.z = 0.02;
    path_sph_mkr.color.r = 0.0f;
    path_sph_mkr.color.g = 0.0f;
    path_sph_mkr.color.b = 0.0f;

    // CURRENT POSITION MARKER
    visualization_msgs::Marker curr_pos_marker;
    curr_pos_marker = path_sph_mkr;
    // More visible than PRM
    curr_pos_marker.scale.x = 0.1;
    curr_pos_marker.scale.y = 0.1;
    curr_pos_marker.color.r = 1.0;
    curr_pos_marker.color.g = 0.2;
    curr_pos_marker.color.b = 0.2;

    // UpdateCell() MARKER
    visualization_msgs::Marker update_marker;
    update_marker = path_sph_mkr;
    // More visible than PRM
    update_marker.scale.x = 0.04;
    update_marker.scale.y = 0.04;
    update_marker.scale.z = 0.04;
    update_marker.color.r = 0.96f;
    update_marker.color.g = 0.475f;
    update_marker.color.b = 0.0f;
    update_marker.color.a = 1.0;
    update_marker.lifetime = ros::Duration(1.3 / frequency);


    // LPA* or D* Lite Path
    std::vector<global::Node> path;
    
    // GRID Version
    // Initialize Grid
    map::Grid grid(obstacles_v, inflate);

    // Build Map
    grid.build_map(resolution);

    ROS_INFO("Grid Built!");

    // Get map bounds
    auto bounds = grid.return_map_bounds();
    auto gridsize = grid.return_grid_dimensions();

    // Grid Pose
    geometry_msgs::Pose map_pose;
    map_pose.position.x = bounds.at(0).x;
    map_pose.position.y = bounds.at(0).y;
    map_pose.position.z = 0.0;
    map_pose.orientation.x = 0.0;
    map_pose.orientation.y = 0.0;
    map_pose.orientation.z = 0.0;
    map_pose.orientation.w = 1.0;

    // Occupancy grid for visualization
    grid_map.header.frame_id = frame_id;
    grid_map.info.resolution = resolution;
    grid_map.info.width = gridsize.at(0);
    grid_map.info.height = gridsize.at(1);

    // std::cout << "Width: " << gridsize.at(0) << "\t Height: " << gridsize.at(1) << std::endl;

    grid_map.info.origin = map_pose;

    ROS_INFO("Planning using D* Lite!");
    global::DSL dsl(obstacles_v, inflate);
    dsl.Initialize(start, goal, grid, resolution);
    dsl.ComputeShortestPath();
    path = dsl.return_path();

    ros::Rate rate(frequency);

    // 0th cell
    unsigned int path_counter = 0;
    std::vector<global::Node> updated_nodes;

    // For displaying final path at the end
    // Push back twice since out visualizer displays from the 1th element
    std::vector<global::Node> total_path;
    total_path.push_back(path.front());
    // total_path.push_back(path.front());
    // Update Current Position Counter
    curr_pos_marker.pose.position.x = path.at(path_counter).cell.center_coords.x;
    curr_pos_marker.pose.position.y = path.at(path_counter).cell.center_coords.y;

    // End condition for update
    bool planning = true;

    // SLEEP so that I can prepare my screen recorder
    ros::Duration(2.0).sleep();

    // For visualization purposes (See below)
    bool firstpass = true;

    bool valid_path = dsl.return_valid();

    // Main While
    while (ros::ok())
    {
        ros::spinOnce();

        // rviz representation of the grid
        grid.fake_occupancy_grid(map);
        // Publish GRID Map
        grid_map.header.stamp = ros::Time::now();
        grid_map.info.map_load_time = ros::Time::now();
        grid_map.data = map;
        grid_pub.publish(grid_map);


        // DRAW PATH
        path_arr.markers.clear();
        path_marker.points.clear();
        int path_marker_id = 0;
        // +1 to simulate move forward
        for (auto path_iter = path.begin(); path_iter != path.end(); path_iter++)
        {
            // Add node as marker cell
            geometry_msgs::Point vtx;
            vtx.x = path_iter->cell.center_coords.x;
            vtx.y = path_iter->cell.center_coords.y;
            vtx.z = 0.0;
            path_marker.points.push_back(vtx);

            // Also push back cubes
            path_sph_mkr.pose.position.x = path_iter->cell.center_coords.x;
            path_sph_mkr.pose.position.y = path_iter->cell.center_coords.y;
            path_sph_mkr.id = path_marker_id;
            path_marker_id++;
            path_arr.markers.push_back(path_sph_mkr);
        }
        path_marker.id = path_marker_id;
        path_arr.markers.push_back(path_marker);

        // Push Back Current Position
        path_marker_id++;
        curr_pos_marker.id = path_marker_id;
        path_arr.markers.push_back(curr_pos_marker);

        // Push Back Updated Cell Markers
        for (auto node_iter = updated_nodes.begin(); node_iter != updated_nodes.end(); node_iter++)
        {
          path_marker_id++;
          update_marker.pose.position.x = node_iter->cell.center_coords.x;
          update_marker.pose.position.y = node_iter->cell.center_coords.y;
          update_marker.id = path_marker_id;
          path_arr.markers.push_back(update_marker);
        }

        // Publish Path
        path_pub.publish(path_arr);

        // FAKE Update
        updated_nodes.clear();
        if (path.size() > 1 and planning and valid_path)
        {
            // Update Grid
            grid.update_grid(path.at(path_counter + 1).cell, visibility);
            // D*Lite Update
            updated_nodes = dsl.SimulateUpdate(grid.return_fake_grid());
            // Return Path
            path = dsl.return_path();
            // Check path validity (obstacles etc)
            valid_path = dsl.return_valid();
            // Update total path
            total_path.push_back(path.at(path_counter));
            // Update Current Position Counter
            curr_pos_marker.pose.position.x = path.at(path_counter).cell.center_coords.x;
            curr_pos_marker.pose.position.y = path.at(path_counter).cell.center_coords.y;
        } else
        {
            // FINAL PATH. INFINITE MARKER DURATION
            path_marker.lifetime = ros::Duration();
            path_sph_mkr.lifetime = ros::Duration();
            curr_pos_marker.lifetime = ros::Duration();
            if (!valid_path)
            {
                ROS_WARN("INVALID PATH");
                for (auto path_iter = path.begin(); path_iter != path.end(); path_iter++)
                {
                    total_path.push_back(*path_iter);
                }
                // So we don't enter this loop again
                valid_path = true;
            }
            path = total_path;
            planning = false;
        }

        // Make the markers persist for the first iteration to show original path
        if (firstpass)
        {
            ros::Duration(1.0).sleep();
            path_marker.lifetime = ros::Duration(1.0 / frequency);
            path_sph_mkr.lifetime = ros::Duration(1.0 / frequency);
            curr_pos_marker.lifetime = ros::Duration(1.0 / frequency);
        }

        firstpass = false;
        rate.sleep();
    }

    return 0;
}

================================================
FILE: global_planner/src/global_planner/global_planner.cpp
================================================
#include "global_planner/global_planner.hpp"

namespace global
{
	using rigid2d::Vector2D;

	// Global Planner
	GlobalPlanner::GlobalPlanner()
	{
	}
}


================================================
FILE: global_planner/src/global_planner/heuristic.cpp
================================================
#include "global_planner/heuristic.hpp"

namespace global
{
	Node::Node()
	{
		vertex = Vertex(Vector2D());
		cell = Cell(Vector2D(), 0.0);
	}

	Astar::Astar(const std::vector<Obstacle> & obstacles_, const double & inflate_robot_)
	{
		obstacles = obstacles_;
		inflate_robot = inflate_robot_;
	}

	// PRM version
	std::vector<Node> Astar::plan(const Vector2D & start, const Vector2D & goal, std::vector<Vertex> & map)
	{
		PRM = map;

		/**
			Main Planning Loop
            for A* (naive)

            Steps:

            1. Append start node to open list
            2. LOOP:
                a- set current node to that with lowest f cost
                in open list. If some nodes have the same f cost,
                choose current node using h cost

                b- get the 8 neighbours of each node (or less depending
                on proximity to grid limits)

                c- for each node:
                    i - if it is inside the closed list,
                    or if it is an obstacle, ignore it
                    ii - if it is on the open list, check
                    whether it is a better path than the current node,
                    if so, update its g cost and make its parent node the
                    current node
                    iii - if none of these conditions are met,
                    add the node to the open list and set its parent
                    to the current node after calculating its g and h cost
            3. If goal is found, return path, else, keep looping
            until goal is found or open list is 0 (path was never found)
        **/

        // Create a Min heap of Nodes 
	    std::priority_queue <Node, std::vector<Node>, HeapComparator > open_list;

	    // Store the goal node
	    Node goal_node;
	    // Find PRM vertex whose coordinates most closely match the goal coordinates
	    goal_node.vertex = find_nearest_node(goal, PRM);
	    goal_node.id = goal_node.vertex.id;

	    // Add the start node to the queue
	    Node current_node;
	    // Find PRM vertex whose coordinates most closely match the start coordinates
	    current_node.vertex = find_nearest_node(start, PRM);
	    current_node.id = current_node.vertex.id;

	    open_list.push(current_node);

	    // For re-ordering and finding NOTE: inefficient
	    std::set<int> open_list_v;
	    open_list_v.insert(current_node.id);

	    // Store the start node
	    Node start_node = current_node;


	    // Closed List
	    std::set<Node, std::less<>> closed_list;

	    int iterations = 0;
	    while (!open_list.empty())
	    {
	    	iterations++;

	    	// Get the minimum node on the open list
	    	current_node = open_list.top();
	    	// Remove said node from open list
	    	open_list.pop();
	    	open_list_v.erase(current_node.id);

	    	// Add current node ID to closed list
	    	closed_list.insert(current_node);

	    	// END condition
	    	if (current_node.vertex.id == goal_node.vertex.id)
	    	{
	    		std::cout << "Goal found after " << iterations << " Iterations!" << std::endl;
	    		return trace_path(current_node, closed_list);
	    	}

	    	// Loop through each node's neighbors
	    	for (auto edge_iter = current_node.vertex.edges.begin(); edge_iter < current_node.vertex.edges.end(); edge_iter++)
	    	{
	    		// Skip if neighbour in closed list, or is obstacle (GRID) 
	    		bool skip = false;
	    		// Modify node cost if neighbour but also is in open list
	    		bool opened = false;

	    		// Find the neighbour node
	    		Node neighbour;
	    		neighbour.vertex = PRM.at(edge_iter->next_id);
	    		// Self-identify
				neighbour.id = neighbour.vertex.id;

	    		// First, check if in closed list
	    		skip = closed_list.find(neighbour.id) != closed_list.end();
	    		if (skip)
	    		{
	    			continue;
	    		}

	    		// No obstacle list check since not GRID

	    		// Check if in open list
	    		opened = open_list_v.find(neighbour.id) != open_list_v.end();

	    		if (!opened)
	    		// Create a new node and push
	    		{
	    			// hcost is distance from neighbor to goal
	    			neighbour.hcost = map::euclidean_distance(neighbour.vertex.coords.x - goal_node.vertex.coords.x,
	    													  neighbour.vertex.coords.y - goal_node.vertex.coords.y);
	    			
	    			create_vtx(open_list, neighbour, current_node);
	    			// Push to record keeping set
	    			open_list_v.insert(neighbour.id);

	    		} else
	    		// Potentially modify existing node and resort open list
	    		{
	    			update_vtx(open_list, neighbour, current_node);
	    		}
	    	}
	    }

	    // If we have reached this point, then there was no valid path
	    std::cout << "No valid path! returning most complete path" << std::endl;
	    return trace_path(current_node, closed_list);

	}

	void Astar::create_vtx(std::priority_queue <Node, std::vector<Node>, HeapComparator > & open_list, Node & neighbour, const Node & current_node)
	{
		// gcost is current node's gcost + distance from neighbor to current node
		neighbour.gcost = current_node.gcost + map::euclidean_distance(neighbour.vertex.coords.x - current_node.vertex.coords.x,
												  					   neighbour.vertex.coords.y - current_node.vertex.coords.y);
		// Update f cost
		neighbour.fcost = neighbour.gcost + neighbour.hcost;

		// Add parent id
		neighbour.parent_id = current_node.id;

		// Push to open list
		open_list.push(neighbour);
	}

	void Astar::update_vtx(std::priority_queue <Node, std::vector<Node>, HeapComparator > & open_list, Node & neighbour, const Node & current_node)
	{
		// Calculate a new tentative g cost
		double gcost = current_node.gcost + map::euclidean_distance(neighbour.vertex.coords.x - current_node.vertex.coords.x,
												  					   neighbour.vertex.coords.y - current_node.vertex.coords.y);
		if (gcost < neighbour.gcost)
		// Modify Node
		{
			// Update g cost
			neighbour.gcost = gcost;
			// Update f cost
			neighbour.fcost = neighbour.gcost + neighbour.hcost;
			// Update parent
			neighbour.parent_id = current_node.id;

			// If this happens, we need to re-sort the open list
			std::priority_queue <Node, std::vector<Node>, HeapComparator > temp_open_list;
			while (!open_list.empty())
			{
				Node temp = open_list.top();
				if (temp.id == neighbour.id)
				{
					temp = neighbour;
				}
				temp_open_list.push(temp);
				open_list.pop();
			}

			open_list = temp_open_list;
		}

	}

	std::vector<Node> Astar::trace_path(const Node & final_node, const std::set<Node, std::less<>> & closed_list)
	{
		// First node in the vector is 'final node'
		std::vector<Node> path{final_node};

		bool done = false;

		while (!done)
		{
			// std::cout << "GOING FROM NODE: " << path.back().vertex.id << " TO: " << path.back().parent_id << std::endl;
			Node next_node = *closed_list.find(path.back().parent_id);
			path.push_back(next_node);
			if (next_node.parent_id == -1)
			{
				done = true;
				break;
			}
		}

		std::reverse(path.begin(), path.end());

		std::cout << "The path contains " << path.size() << " Nodes." << std::endl;
		return path;
	}


	std::vector<Node> Astar::plan(const Vector2D & start, const Vector2D & goal, const Grid & grid_, const double & resolution)
	{
		Grid grid = grid_;
		GRID = grid.return_grid();

		/**
			Main Planning Loop
            for A* (naive)

            Steps:

            1. Append start node to open list
            2. LOOP:
                a- set current node to that with lowest f cost
                in open list. If some nodes have the same f cost,
                choose current node using h cost

                b- get the 8 neighbours of each node (or less depending
                on proximity to grid limits)

                c- for each node:
                    i - if it is inside the closed list,
                    or if it is an obstacle, ignore it
                    ii - if it is on the open list, check
                    whether it is a better path than the current node,
                    if so, update its g cost and make its parent node the
                    current node
                    iii - if none of these conditions are met,
                    add the node to the open list and set its parent
                    to the current node after calculating its g and h cost
            3. If goal is found, return path, else, keep looping
            until goal is found or open list is 0 (path was never found)
        **/

        // Create a Min heap of Nodes 
	    std::priority_queue <Node, std::vector<Node>, HeapComparator > open_list;

	    // Store the goal node
	    Node goal_node;
	    // Find GRID cellwhose coordinates most closely match the goal coordinates
	    goal_node.cell = find_nearest_node(goal, grid, resolution);
	    goal_node.id = goal_node.cell.index.row_major;

	    // Add the start node to the queue
	    Node current_node;
	    // Find GRID cell whose coordinates most closely match the start coordinates
	    current_node.cell = find_nearest_node(start, grid, resolution);
	    current_node.id = current_node.cell.index.row_major;
	    // std::cout << "START IDX: [" << current_node.cell.index.x << ", " << current_node.cell.index.y << "]" << std::endl;

	    // std::cout << "GOAL IDX: [" << goal_node.cell.index.x << ", " << goal_node.cell.index.y << "]" << std::endl;

	    open_list.push(current_node);

	    // For re-ordering and finding NOTE: inefficient
	    std::set<int> open_list_v;
	    open_list_v.insert(current_node.id);

	    // Store the start node
	    Node start_node = current_node;


	    // Closed List
	    std::set<Node, std::less<>> closed_list;

	    int iterations = 0;
	    while (!open_list.empty())
	    {
	    	iterations++;

	    	// Get the minimum node on the open list
	    	current_node = open_list.top();
	    	// Remove said node from open list
	    	open_list.pop();
	    	open_list_v.erase(current_node.id);

	    	// Add current node ID to closed list
	    	closed_list.insert(current_node);

	    	// END condition
	    	if (current_node.cell.index.row_major == goal_node.cell.index.row_major)
	    	{
	    		std::cout << "Goal found after " << iterations << " Iterations!" << std::endl;
	    		return trace_path(current_node, closed_list);
	    	}

	    	// Find the current node's neighbours
	    	std::vector<Cell> neighbours = get_neighbours(current_node, GRID);

	    	// Loop through each node's neighbors
	    	for (auto nbr_iter = neighbours.begin(); nbr_iter < neighbours.end(); nbr_iter++)
	    	{
	    		// Skip if neighbour in closed list, or is obstacle (GRID) 
	    		bool skip = false;
	    		// Modify node cost if neighbour but also is in open list
	    		bool opened = false;

	    		// Find the neighbour node
	    		Node neighbour;
	    		neighbour.cell = *nbr_iter;
	    		neighbour.id = neighbour.cell.index.row_major;

	    		// First, check if in closed list
	    		skip = closed_list.find(neighbour.id) != closed_list.end();
	    		if (skip)
	    		{
	    			// std::cout << "CLOSED Node at [" << neighbour.cell.index.x << ", " << neighbour.cell.index.y << "]" << "\t [" << neighbour.cell.coords.x << ", " << neighbour.cell.coords.y << "]" << std::endl;
	    			continue;
	    		}

	    		// Now, check if neighbour is an obstacle
	    		if (neighbour.cell.celltype == map::Occupied or\
	    			neighbour.cell.celltype == map::Inflation)
	    		{	    			
	    			continue;
	    		}

	    		// Check if in open list
	    		opened = open_list_v.find(neighbour.id) != open_list_v.end();

	    		if (!opened)
	    		// Create a new node and push
	    		{
	    			neighbour.hcost = heuristic(neighbour, goal_node);
	    			// std::cout << "h cost: " << neighbour.hcost << std::endl;
	    			
	    			create_cell(open_list, neighbour, current_node);
	    			// Push to record keeping set
	    			open_list_v.insert(neighbour.id);

	    		} else
	    		// Potentially modify existing node and resort open list
	    		{
	    			update_cell(open_list, neighbour, current_node);
	    		}
	    	}
	    }

	    // If we have reached this point, then there was no valid path
	    std::cout << "No valid path! returning most complete path" << std::endl;
	    return trace_path(current_node, closed_list);

	}

	void Astar::update_cell(std::priority_queue <Node, std::vector<Node>, HeapComparator > & open_list, Node & neighbour, const Node & current_node)
	{
		// Calculate a new tentative g cost
		double gcost = current_node.gcost + heuristic(neighbour, current_node);
		if (gcost < neighbour.gcost)
		// Modify Node
		{
			// Update g cost
			neighbour.gcost = gcost;
			// Update f cost
			neighbour.fcost = neighbour.gcost + neighbour.hcost;
			// Update parent
			neighbour.parent_id = current_node.id;

			// If this happens, we need to re-sort the open list
			std::priority_queue <Node, std::vector<Node>, HeapComparator > temp_open_list;
			while (!open_list.empty())
			{
				Node temp = open_list.top();
				if (temp.id == neighbour.id)
				{
					temp = neighbour;
				}
				temp_open_list.push(temp);
				open_list.pop();
			}

			open_list = temp_open_list;
		}

	}

	void Astar::create_cell(std::priority_queue <Node, std::vector<Node>, HeapComparator > & open_list, Node & neighbour, const Node & current_node)
	{
		// gcost is current node's gcost + distance from neighbor to current node
		neighbour.gcost = current_node.gcost + heuristic(neighbour, current_node);
		// std::cout << "g cost: " << neighbour.gcost << std::endl;
		// Update f cost
		neighbour.fcost = neighbour.gcost + neighbour.hcost;

		// Add parent id
		neighbour.parent_id = current_node.id;

		// Push to open list
		open_list.push(neighbour);
	}

	double Astar::heuristic(const Node & n1, const Node & n2)
	{
		double x_dist = fabs(n1.cell.center_coords.x - n2.cell.center_coords.x);
		double y_dist = fabs(n1.cell.center_coords.y - n2.cell.center_coords.y);
		// return map::euclidean_distance(x_dist, y_dist);

		double D1 = 1.0;
		double D2 = sqrt(2.0);

		return D1 * (x_dist + y_dist) + (D2 - 2 * D1) * std::min(x_dist, y_dist);
	}


	std::vector<Cell> Astar::get_neighbours(const Node & n, const std::vector<Cell> & map)
	{
		int x_max = map.back().index.x;
		int y_max = map.back().index.y; 
		std::vector<Cell> neighbours;
		// std::cout << "Node at [" << n.cell.index.x << ", " << n.cell.index.y << "]" << std::endl;

		// Evaluate about 3x3 block for 8-connectivity
		for (int x = -1; x < 2; x++)
		{
			for (int y = -1; y < 2; y++)
			{
				// Skip x,y = (0,0) since that's the current node
				if (x == 0 and y == 0)
				{
					continue;
				} else
				{
					int check_x = n.cell.index.x + x;
					int check_y = n.cell.index.y + y;

					// Ensure potential neighbour is within grid bounds
					if (check_x >= 0 and check_x <= x_max and check_y >= 0 and check_y <= y_max)
					{
						// Now we need to grab the right cell from the map. To do this: index->RMJ
						// std::cout << "Neighbour at [" << check_x << ", " << check_y << "]" << std::endl;
						int rmj = map::grid2rowmajor(check_x, check_y, x_max + 1);
						Cell nbr = map.at(rmj);
						neighbours.push_back(nbr);
					}
				}
			}
		}
		return neighbours;
	}


	void Thetastar::create_vtx(std::priority_queue <Node, std::vector<Node>, HeapComparator > & open_list, Node & neighbour, const Node & current_node)
	{
		// First, do line of sight check between PARENT of current node and neighbour
		bool clear = true;

		int grandparent_id = current_node.parent_id;
		if (grandparent_id == -1)
		{
			// There is no grandparent since this is the start node
			grandparent_id = current_node.id;
			clear = false;
		}

		if (clear)
		{
			for (auto obs_iter = obstacles.begin(); obs_iter != obstacles.end(); obs_iter++)
			{
				// Edge on Edge Check
				if (!no_intersect(neighbour.vertex, PRM.at(grandparent_id), obs_iter))
				{
					clear = false;
					break;
				}

				// Edge Near Point Check
				for (auto v_iter = obs_iter->vertices.begin(); v_iter != obs_iter->vertices.end(); v_iter++)
				{
					if (too_close(neighbour.vertex, PRM.at(grandparent_id), Vertex(*v_iter), inflate_robot))
					{
						clear = false;
						break;
					}
				}

				// No need to loop over other obstacles if not clear
				if (!clear)
				{
					break;
				}
			}

		}

		if (!clear)
		// Perform vanilla update from A*
		{
			// Use UNMODIFIED inherited function
			Astar::create_vtx(open_list, neighbour, current_node);

		} else
		{
			// g cost of grandparent = current_node gcost - dist(grandparent->current)
			double grandparent_gcost = current_node.gcost - map::euclidean_distance(current_node.vertex.coords.x - PRM.at(grandparent_id).coords.x,
													  					            current_node.vertex.coords.y - PRM.at(grandparent_id).coords.y);

			// g cost is grandparent node g cost + dist(grandparent -> neighbor)
			neighbour.gcost = grandparent_gcost + map::euclidean_distance(neighbour.vertex.coords.x - PRM.at(grandparent_id).coords.x,
													  					neighbour.vertex.coords.y - PRM.at(grandparent_id).coords.y);
			// Update f cost
			neighbour.fcost = neighbour.gcost + neighbour.hcost;

			// Add parent id
			neighbour.parent_id = grandparent_id;

			// Push to open list
			open_list.push(neighbour);

		}
	}


	// The overridden update_vtx fcn with line of sight check
	void Thetastar::update_vtx(std::priority_queue <Node, std::vector<Node>, HeapComparator > & open_list, Node & neighbour, const Node & current_node)
	{
		// First, do line of sight check between PARENT of current node and neighbour
		bool clear = true;

		int grandparent_id = current_node.parent_id;
		if (grandparent_id == -1)
		{
			// There is no grandparent since this is the start node
			grandparent_id = current_node.id;
			clear = false;
		}

		if (clear)
		{
			for (auto obs_iter = obstacles.begin(); obs_iter != obstacles.end(); obs_iter++)
			{
				// Edge on Edge Check
				if (!no_intersect(neighbour.vertex, PRM.at(grandparent_id), obs_iter))
				{
					clear = false;
					break;
				}

				// Edge Near Point Check
				for (auto v_iter = obs_iter->vertices.begin(); v_iter != obs_iter->vertices.end(); v_iter++)
				{
					if (too_close(neighbour.vertex, PRM.at(grandparent_id), Vertex(*v_iter), inflate_robot))
					{
						clear = false;
						break;
					}
				}

				// No need to loop over other obstacles if not clear
				if (!clear)
				{
					break;
				}
			}

		}

		if (!clear)
		// Perform vanilla update from A*
		{
			// Use UNMODIFIED inherited function
			Astar::update_vtx(open_list, neighbour, current_node);

		} else
		// Perform cost update with new parent
		{
			// g cost of grandparent = current_node gcost - dist(grandparent->current)
			double grandparent_gcost = current_node.gcost - map::euclidean_distance(current_node.vertex.coords.x - PRM.at(grandparent_id).coords.x,
													  					            current_node.vertex.coords.y - PRM.at(grandparent_id).coords.y);
			// Calculate a new tentative g cost
			double gcost = grandparent_gcost + map::euclidean_distance(neighbour.vertex.coords.x - PRM.at(grandparent_id).coords.x,
													  					neighbour.vertex.coords.y - PRM.at(grandparent_id).coords.y);
			if (gcost < neighbour.gcost)
			// Modify Node
			{
				// Update g cost
				neighbour.gcost = gcost;
				// Update f cost
				neighbour.fcost = neighbour.gcost + neighbour.hcost;
				// Update parent with PARENT of parent
				// std::cout << "PARENT ID: " << current_node.vertex.id << std::endl;
				neighbour.parent_id = grandparent_id;
				// std::cout << "GRANDPARENT (NEW PARENT) ID: " << neighbour.parent_id << std::endl;

				// If this happens, we need to re-sort the open list
				std::priority_queue <Node, std::vector<Node>, HeapComparator > temp_open_list;
				while (!open_list.empty())
				{
					Node temp = open_list.top();
					if (temp.id == neighbour.id)
					{
						temp = neighbour;
					}
					temp_open_list.push(temp);
					open_list.pop();
				}

				open_list = temp_open_list;
			}
		}
	}

	Vertex find_nearest_node(const Vector2D & position, const std::vector<Vertex> & map)
	{
		double min_dist = map::euclidean_distance(position.x - map.at(0).coords.x, position.y - map.at(0).coords.y);
		int min_idx = 0;

		for (auto iter = map.begin(); iter < map.end(); iter++)
		{
			double curr_dist = map::euclidean_distance(position.x - iter->coords.x, position.y - iter->coords.y);
			if (curr_dist < min_dist)
			{
				min_idx = static_cast<int>(std::distance(map.begin(), iter));
				min_dist = curr_dist;
			}
		}

		return map.at(min_idx);
	}


	Cell find_nearest_node(const Vector2D & position, const Grid & grid, const double & resolution)
	{
		Cell temp_cell(position, resolution);

		Index idx = grid.world2grid(temp_cell);

		return (grid.return_grid().at(idx.row_major));
	}
}

================================================
FILE: global_planner/src/global_planner/incremental.cpp
================================================
#include "global_planner/incremental.hpp"

namespace global
{
	void LPAstar::ComputeShortestPath()
	{
		Node min;
		int iterations= 0;
		while (Continue(iterations))
		{
			iterations++;
			// std::cout << "Iteration: " << iterations << std::endl;

			// if (iterations > 200)
			// {
			// 	break;
			// }

			// Get min node and erase from open list
			min = open_list.at(0);
			std::pop_heap(open_list.begin(), open_list.end(), KeyComparator());
			open_list.back();
			open_list.pop_back();

			// Check if Overconsistent (start always satisfies this)
			if (min.gcost > min.rhs)
			{
				// std::cout << "Locally Overconsistent!" << std::endl;
				// Update True Cost
				min.gcost = min.rhs;
				// Update min in FakeGrid
				FakeGrid.at(min.id) = min;
				// Check Successors
				std::vector<Node> successors = get_neighbours(min, FakeGrid);
				for (auto succ = successors.begin(); succ < successors.end(); succ++)
				{
					UpdateCell(*succ);
				}

			} else
			{
				// std::cout << "NOT Locally Overconsistent!" << std::endl;
				min.gcost = BIG_NUM;
				FakeGrid.at(min.id) = min;
				// Check Successors
				std::vector<Node> successors = get_neighbours(min, FakeGrid);
				// ALSO check min itself
				successors.push_back(min);
				for (auto succ = successors.begin(); succ < successors.end(); succ++)
				{
					UpdateCell(*succ);
				}
			}
		}
	}


	void LPAstar::UpdateCell(Node & n)
	{
    	// Check this node isn't the start node
		if (n.id != start_node.id)
		{
			std::priority_queue <Node, std::vector<Node>, CostComparator > pred_costs;
			// Find the predecessors of node n
			std::vector<Node> predecessors = get_neighbours(n, FakeGrid);

			for (auto pred_iter = predecessors.begin(); pred_iter < predecessors.end(); pred_iter++)
	    	{
	    		// Find the neighbour node
	    		Node predecessor;
	    		predecessor = *pred_iter;
	    		predecessor.id = predecessor.cell.index.row_major;

	    		// Skip Occupied or Inflated Cells
	    		if (predecessor.cell.celltype == map::Occupied or\
	    			predecessor.cell.celltype == map::Inflation)
	    		{	    			
	    			// Push to priority queue for auto sort
	    			// This is not actually stored anywhere, just useful in getting min gcost for n
	    			predecessor.gcost += heuristic(predecessor, n) + BIG_NUM;
	    		} else
	    		// Free Cells
	    		{
	    			// Push to priority queue for auto sort
	    			// This is not actually stored anywhere, just useful in getting min gcost for n
	    			predecessor.gcost += heuristic(predecessor, n);
	    		}

	    		pred_costs.push(predecessor);

	    	}

	    	Node min_predecessor = pred_costs.top();

	    	// Update RHS value
	    	n.rhs = min_predecessor.gcost;
	    	// Update Parent
	    	n.parent_id = min_predecessor.id;
	    }
    	// If it's on the open list, remove it
    	auto cell_iter = std::find_if(open_list.begin(), open_list.end(), [&](const Node & node) {return node.id == n.id;});
    	if (cell_iter != open_list.end())
    	{
    		// Delete using points
    		open_list.erase(cell_iter);
    		// Re-sort open list
    		std::push_heap(open_list.begin(), open_list.end(), KeyComparator());
    	}

    	// If n is locally inconsistent (ie if n.gcost != n.rhs), add it to the open list with updated keys
    	if (!(rigid2d::almost_equal(n.gcost, n.rhs)))
    	{
    		n.hcost = heuristic(n, goal_node);
    		CalculateKeys(n);
    		open_list.push_back(n);
    		std::push_heap(open_list.begin(), open_list.end(), KeyComparator());
    	}

    	// Update n in FakeGrid
    	FakeGrid.at(n.id) = n;
	}


	void LPAstar::Initialize(const Vector2D & start, const Vector2D & goal, const map::Grid & grid_, const double & resolution)
	{
		GRID = grid_.return_fake_grid();
		FakeGrid.clear();

		// Find Start and Goal Nodes
	    goal_node.cell = find_nearest_node(goal, grid_, resolution);
	    goal_node.id = goal_node.cell.index.row_major;
	    // Make sure the algo thinks it's free to begin with
	    goal_node.cell.celltype = map::Free;
	    goal_node.hcost = 0.0;
	    goal_node.gcost = BIG_NUM;
	    CalculateKeys(goal_node);

	    // Find GRID cell whose coordinates most closely match the start coordinates
	    start_node.cell = find_nearest_node(start, grid_, resolution);
	    start_node.id = start_node.cell.index.row_major;
	    // Make sure the algo thinks it's free to begin with
	    start_node.cell.celltype = map::Free;
	    start_node.hcost = heuristic(start_node, goal_node);
	    start_node.rhs = 0.0;
	    start_node.gcost = BIG_NUM;
	    CalculateKeys(start_node);
	    // std::cout << "START, IDX: [" << start_node.cell.index.x << ", " << start_node.cell.index.y << "]"<< std::endl;
	    // std::cout << "GOAL, IDX: [" << goal_node.cell.index.x << ", " << goal_node.cell.index.y << "]" << "ID: " << goal_node.id << std::endl;

		// Populate Fake Grid
		for (auto iter = GRID.begin(); iter < GRID.end(); iter++)
		{
			Node node;
			node.cell = *iter;
			node.id = node.cell.index.row_major;
			if (node.id == goal_node.id)
			{
				node = goal_node;
			} else if (node.id == start_node.id)
			{
				node = start_node;
			}
			node.gcost = BIG_NUM;
			// TODO: When vanilla is done, clear all obstacle/inflated in FakeGrid for simulated increment
			FakeGrid.push_back(node);
		}

		// Populate Open List
		open_list.push_back(start_node);
		std::make_heap(open_list.begin(), open_list.end(), KeyComparator());

		// Cross-update FakeGrid with start and goal
		FakeGrid.at(start_node.id) = start_node;
		FakeGrid.at(goal_node.id) = goal_node;

		std::cout << "Initialized!" << std::endl;
	}


	void LPAstar::CalculateKeys(Node & n)
	{
		n.hcost = heuristic(n, goal_node);
		n.key1 = std::min(n.gcost, n.rhs) + n.hcost;
		n.key2 = std::min(n.gcost, n.rhs);

		// CAP at BIG_NUM
		if (n.gcost > BIG_NUM)
		{
			n.gcost = BIG_NUM;
		}

		if (n.rhs > BIG_NUM)
		{
			n.rhs = BIG_NUM;
		}

		if (n.hcost > BIG_NUM)
		{
			n.hcost = BIG_NUM;
		}

		if (n.key1 > BIG_NUM)
		{
			n.key1 = BIG_NUM;
		}

		if (n.key2 > BIG_NUM)
		{
			n.key2 = BIG_NUM;
		}
	}


	bool LPAstar::Continue(const int & iterations)
	{
		if(!std::is_heap(open_list.begin(), open_list.end(), KeyComparator()))
		{
			std::make_heap(open_list.begin(), open_list.end(), KeyComparator());
		}

		Node top = open_list.at(0);
		CalculateKeys(top);
		// Update Goal Node from Fake Grid
		goal_node = FakeGrid.at(goal_node.id);
		goal_node.hcost = 0.0;
		CalculateKeys(goal_node);
		FakeGrid.at(goal_node.id) = goal_node;

		// Goal is NOT the minimum key
		bool not_minkey = true;

		if (rigid2d::almost_equal(top.key1, goal_node.key1))
		{
			if (!rigid2d::almost_equal(top.key2, goal_node.key2))
			{
				if (top.key2 > goal_node.key2)
				{
					not_minkey = false;
				}

			} else
			{
				not_minkey = false;
			}
		} else
		{
			if (top.key1 > goal_node.key1)
			{
				not_minkey = false;
			}
		}

		bool consistent = false;

		if (rigid2d::almost_equal(goal_node.gcost, goal_node.rhs))
		{
			consistent = true;
		}

		
		// std::cout << "GOAL NODE GCOST: " << goal_node.gcost << " RHS: " << goal_node.rhs << " KEY1: " << goal_node.key1 << "  KEY2: " << goal_node.key2 << std::endl; 
		// std::cout << "MIN, IDX: [" << top.cell.index.x << ", " << top.cell.index.y << "]"<< " KEY1: " << top.key1 << "  KEY2: " << top.key2 << std::endl;
		// if (not_minkey)
		// {
		// 	std::cout << "GOAL NODE NOT MIN KEY" << std::endl;
		// }
		// if (consistent)
		// {
		// 	std::cout << "CONSISTENT" << std::endl;
		// }

		// Conditions for Continuing
		if (not_minkey or (!consistent))
		{
			return true;
		} else
		{
			if (goal_node.gcost >= BIG_NUM)
			{
				ROS_WARN("There is no valid path. The goal is in a blocked cell! \n Returning closest path.");
				valid_path = false;
			} else
			{
				std::cout << "Goal found after " << iterations << " Iterations!" << std::endl;
			}
			return false;
		}
	}
	

	std::vector<Node> LPAstar::trace_path(const Node & final_node)
	{
		// Store old path in case we see an invalid one
		std::vector<Node> old_path = path;
		// First node in the vector is 'final node'
		path.clear();
		path.push_back(final_node);

		Node next_node = final_node;

		while (next_node.parent_id != -1)
		{
			int parent_id = next_node.parent_id;
			if (parent_id >= 0)
			{
				int grandparent_id = FakeGrid.at(parent_id).parent_id;
				if (grandparent_id == next_node.id)
					// TWO NODES ARE EACH OTHERS PARENTS.
					// THIS USUALLY MEANS THAT THE GOAL IS INSIDE AN OBSTACLE
				{
					Node n1 = next_node;
					Node n2 = FakeGrid.at(parent_id);
					std::cout << "Two Nodes are each others' parents!" << std::endl;
					std::cout << "Node 1 at [" << n1.cell.index.x << ", " << n1.cell.index.y << "]" << std::endl;
					std::cout << "Node 2 at [" << n2.cell.index.x << ", " << n2.cell.index.y << "]" << std::endl;

					if (n1.hcost < n2.hcost or rigid2d::almost_equal(n1.hcost, n2.hcost))
					{
						// The current node is closest to the goal, so return it as the final path
						valid_path = false;
						path = old_path;
						break;
					}
				}
			}
			// std::cout << "GOING FROM NODE: " << path.back().vertex.id << " TO: " << path.back().parent_id << std::endl;
			next_node = FakeGrid.at(next_node.parent_id);
			path.push_back(next_node);
		}

		std::reverse(path.begin(), path.end());

		std::cout << "The path contains " << path.size() << " Nodes." << std::endl;
		return path;
	}


	std::vector<Node> LPAstar::get_neighbours(const Node & n, const std::vector<Node> & map)
	{
		int x_max = map.back().cell.index.x;
		int y_max = map.back().cell.index.y; 
		std::vector<Node> neighbours;
		// std::cout << "Node at [" << n.cell.index.x << ", " << n.cell.index.y << "]" << std::endl;

		// Evaluate about 3x3 block for 8-connectivity
		for (int x = -1; x < 2; x++)
		{
			for (int y = -1; y < 2; y++)
			{
				// Skip x,y = (0,0) since that's the current node
				if (x == 0 and y == 0)
				{
					continue;
				} else
				{
					int check_x = n.cell.index.x + x;
					int check_y = n.cell.index.y + y;

					// Ensure potential neighbour is within grid bounds
					if (check_x >= 0 and check_x <= x_max and check_y >= 0 and check_y <= y_max)
					{
						// Now we need to grab the right cell from the map. To do this: index->RMJ
						// std::cout << "Neighbour at [" << check_x << ", " << check_y << "]" << std::endl;
						int rmj = map::grid2rowmajor(check_x, check_y, x_max + 1);
						Node nbr = map.at(rmj);
						// std::cout << "Checked Neighbour at [" << nbr.cell.index.x << ", " << nbr.cell.index.y << "]" << std::endl;
						neighbours.push_back(nbr);
					}
				}
			}
		}
		return neighbours;
	}


	std::vector<Node> LPAstar::return_path()
	{
		trace_path(goal_node);
		return path;
	}


	std::vector<Node> LPAstar::SimulateUpdate(const std::vector<Cell> & updated_grid)
	{
		GRID = updated_grid;
		std::vector<Node> updated_nodes;
		for (unsigned int i = 0; i < updated_grid.size(); i++)
		{
			// For each UPDATED Cell (cell.newView = true;), UpdateCell() on its neighbours
			if (updated_grid.at(i).newView and FakeGrid.at(i).cell.celltype != updated_grid.at(i).celltype)
			{
				// First, update FakeGrid
				FakeGrid.at(i).cell = updated_grid.at(i);
				// Push back culprit
				updated_nodes.push_back(FakeGrid.at(i));

				// std::cout << "NEW OCCUPANCY: " << FakeGrid.at(i).cell.index.row_major << std::endl;

				std::vector<Node> neighbours = get_neighbours(FakeGrid.at(i), FakeGrid);
				for (auto iter = neighbours.begin(); iter < neighbours.end(); iter++)
				{
					UpdateCell(*iter);
					// Push back neighbours
					updated_nodes.push_back(*iter);
				}
				// Update Changed Node
				UpdateCell(FakeGrid.at(i));
			}
		}

		// Update Goal Node
		// UpdateCell(FakeGrid.at(goal_node.id));

		// Re-sort open-list
		// std::priority_queue <Node, std::vector<Node>, KeyComparator > temp_open_list;
		// while (!open_list.empty())
		// {
		// 	Node temp = open_list.top();
		// 	temp.hcost = heuristic(temp, goal_node);
		// 	CalculateKeys(temp);
		// 	temp_open_list.push(temp);
		// 	open_list.pop();
		// 	FakeGrid.at(temp.id) = temp;
		// }

		// open_list = temp_open_list;

		// Compute Shortest Path
		ComputeShortestPath();
		// Return updated nodes
		return updated_nodes;
	}

	bool LPAstar::return_valid()
	{
		return valid_path;
	}


	void DSL::Initialize(const Vector2D & start, const Vector2D & goal, const map::Grid & grid_, const double & resolution)
	{
		GRID = grid_.return_fake_grid();
		FakeGrid.clear();

		// NOTE: START AND GOAL POSITIONS FLIPPED FOR D*LITE
		// Find Start and Goal Nodes
	    goal_node.cell = find_nearest_node(start, grid_, resolution);
	    goal_node.id = goal_node.cell.index.row_major;
	    // Make sure the algo thinks it's free to begin with
	    goal_node.cell.celltype = map::Free;
	    goal_node.hcost = 0.0;
	    goal_node.gcost = BIG_NUM;
	    CalculateKeys(goal_node);

	    // Find GRID cell whose coordinates most closely match the start coordinates
	    start_node.cell = find_nearest_node(goal, grid_, resolution);
	    // Make sure the algo thinks it's free to begin with
	    start_node.cell.celltype = map::Free;
	    start_node.id = start_node.cell.index.row_major;
	    start_node.hcost = heuristic(start_node, goal_node);
	    start_node.rhs = 0.0;
	    start_node.gcost = BIG_NUM;
	    CalculateKeys(start_node);
	    // std::cout << "START, IDX: [" << start_node.cell.index.x << ", " << start_node.cell.index.y << "]"<< std::endl;
	    // std::cout << "GOAL, IDX: [" << goal_node.cell.index.x << ", " << goal_node.cell.index.y << "]"<< std::endl;

		// Populate Fake Grid
		for (auto iter = GRID.begin(); iter < GRID.end(); iter++)
		{
			Node node;
			node.cell = *iter;
			node.id = node.cell.index.row_major;
			if (node.id == goal_node.id)
			{
				node = goal_node;
			} else if (node.id == start_node.id)
			{
				node = start_node;
			}
			node.gcost = BIG_NUM;
			// TODO: When vanilla is done, clear all obstacle/inflated in FakeGrid for simulated increment
			FakeGrid.push_back(node);
		}

		// Populate Open List
		open_list.push_back(start_node);
		std::make_heap(open_list.begin(), open_list.end(), KeyComparator());

		// Cross-update FakeGrid with start and goal
		FakeGrid.at(start_node.id) = start_node;
		FakeGrid.at(goal_node.id) = goal_node;

		std::cout << "Initialized!" << std::endl;
	}


	std::vector<Node> DSL::SimulateUpdate(const std::vector<Cell> & updated_grid)
	{
		// First, make sure g(goal [start in paper]!= inf, otherwise no path)
		if (goal_node.gcost >= BIG_NUM)
		{
			std::cout << "There is no valid path." << std::endl;
			trace_path(goal_node);
		}
		// Change new goal node [start in D*Lite paper]
		std::priority_queue <Node, std::vector<Node>, CostComparator > pred_costs;
		// Find the predecessors of node n
		std::vector<Node> predecessors = get_neighbours(goal_node, FakeGrid);

		for (auto pred_iter = predecessors.begin(); pred_iter < predecessors.end(); pred_iter++)
    	{
    		// Find the neighbour node
    		Node predecessor;
    		predecessor = *pred_iter;
    		predecessor.id = predecessor.cell.index.row_major;

    		// Occupied or Inflated Cells
    		if (predecessor.cell.celltype == map::Occupied or\
    			predecessor.cell.celltype == map::Inflation)
    		{	    			
    			// Push to priority queue for auto sort
    			// This is not actually stored anywhere, just useful in getting min gcost for n
    			predecessor.gcost += heuristic(predecessor, goal_node) + BIG_NUM;
    		} else
    		// Free Cells
    		{
    			// Push to priority queue for auto sort
    			// This is not actually stored anywhere, just useful in getting min gcost for n
    			predecessor.gcost += heuristic(predecessor, goal_node);
    		}

    		pred_costs.push(predecessor);
    	}

    	Node min_predecessor = pred_costs.top();

    	//Update Goal Node (start in D*L paper)
    	goal_node = FakeGrid.at(min_predecessor.id);
		GRID = updated_grid;
		std::vector<Node> updated_nodes = LPAstar::SimulateUpdate(updated_grid);

		return updated_nodes;
	}

	std::vector<Node> DSL::return_path()
	{
		trace_path(goal_node);
		std::vector<Node> p = path;
		// If the path is invalid, we don't reverse it for viz purposes
		if (valid_path)
		{
			std::reverse(p.begin(), p.end());
		} else
		{
			// Remove first element of old path since we moved 1 up from here
			path.erase(path.begin());
		}
		return p;
	}

}	

================================================
FILE: global_planner/src/global_planner/potential_field.cpp
================================================
#include "global_planner/potential_field.hpp"

namespace global
{
	using rigid2d::Vector2D;

	// Global Planner
	PotentialField::PotentialField(const std::vector<Obstacle> & obstacles_, const double & eta_, const double & ada_,
                       			   const double & zeta_, const double & d_thresh_, const double & Q_thresh_)
	{
		obstacles = obstacles_;
		eta = eta_;
		zeta = zeta_;
		ada = ada_;
		d_thresh = d_thresh_;
		Q_thresh = Q_thresh_;
	}

	Vector2D PotentialField::AttractiveGradient(const Vector2D & cur_pos, const Vector2D & goal)
	{
		Vector2D dUA;

		double d_goal = map::euclidean_distance(cur_pos.x - goal.x, cur_pos.y - goal.y);

		if (d_goal <= d_thresh or rigid2d::almost_equal(d_goal, d_thresh))
		{
			// Quadratic
			dUA.x = zeta * (cur_pos.x - goal.x);
			dUA.y = zeta * (cur_pos.y - goal.y);
		} else
		{
			// Conic
			dUA.x = d_thresh * zeta * (cur_pos.x - goal.x) / d_goal;
			dUA.y = d_thresh * zeta * (cur_pos.y - goal.y) / d_goal;
		}

		return dUA;
	}

	Vector2D PotentialField::RepulsiveGradient(const Vector2D & cur_pos, const std::vector<Obstacle> & obs)
	{
		Vector2D dUR(0.0, 0.0);

		for (auto obs_iter = obs.begin(); obs_iter < obs.end(); obs_iter++)
		{
			Vector2D closest_point = FindClosestPoint(cur_pos, obs_iter->vertices);

			double Q_obs = map::euclidean_distance(cur_pos.x - closest_point.x, cur_pos.y - closest_point.y);

			if (Q_obs <= Q_thresh or rigid2d::almost_equal(Q_obs, Q_thresh))
			// Closest point within range of influence
			{
				double deltaDx = (cur_pos.x - closest_point.x) / Q_obs;
				double deltaDy = (cur_pos.y - closest_point.y) / Q_obs;

				dUR.x += ada * ((1.0 / Q_thresh) - (1.0 / Q_obs)) * (1.0 / (std::pow(Q_obs, 2))) * deltaDx;
				dUR.y += ada * ((1.0 / Q_thresh) - (1.0 / Q_obs)) * (1.0 / (std::pow(Q_obs, 2))) * deltaDy;
			}
		}

		return dUR;
	}

	Vector2D PotentialField::FindClosestPoint(const Vector2D & cur_pos, const std::vector<Vector2D> & vertices)
	{
		map::Vertex P0(cur_pos);

		double shortest_dist = 1e12;

		Vector2D closest_point = vertices.front();

		for (auto v_iter = vertices.begin(); v_iter < vertices.end(); v_iter++)
		{
			// Vector from current and next vertex. so if 4 vertices: 0->1, 1->2, 2->3, 3->0
			// Record current index
			int i = static_cast<int>(std::distance(vertices.begin(), v_iter));
			// If current index is the last one, loop back to zero for vector construction
			// -1 because indeces start at 0
			if (i == static_cast<int>(vertices.size()) - 1)
			{
				i = 0;
			} else
			// Otherwise, just use the next index
			{
				i += 1;
			}

			// Referencing: https://drive.google.com/file/d/1gQuR4J80aXZ9BBL1s3K83TmxQSWD3AWt/view?usp=sharing Slide 28
			auto E1 = map::Vertex(*v_iter);
			auto E2 = map::Vertex(vertices.at(i));

			map::ShortestDistance shrt = map::lineToPoint(E1, E2, P0);

			if ((shrt.u >= 0.0 and shrt.u <= 1.0) or rigid2d::almost_equal(0.0, shrt.u) or rigid2d::almost_equal(1.0, shrt.u))
			// cur_pos inside line segment
			{
				if (std::abs(shrt.D) < shortest_dist)
				{
					closest_point = shrt.point;
					shortest_dist = std::abs(shrt.D);
				}
			} else
			// cur_pos outside line segment
			{
				// Find distance to both vertices and take minimum for evaluation with whole minimum
				double dist_E1 = map::euclidean_distance(cur_pos.x - v_iter->x, cur_pos.y - v_iter->y);
				double dist_E2 = map::euclidean_distance(cur_pos.x - vertices.at(i).x, cur_pos.y - vertices.at(i).y);

				if (dist_E1 < shortest_dist)
				{
					closest_point = *v_iter;
					shortest_dist = dist_E1;
				}
				// Possible to overwrite E1
				if (dist_E2 < shortest_dist)
				{
					closest_point = vertices.at(i);
					shortest_dist = dist_E2;
				}
			}
		}

		// std::cout << "CLOSEST POINT TO: [" << cur_pos.x << ", " << cur_pos.y << "] IS:  [" << closest_point.x << ", " <<
		// 			 closest_point.y << "]" << std::endl;

		return closest_point;
	}

	double PotentialField::MultiDimNorm(const std::vector<double> partials)
	{
		double norm_squared = 0.0;

		for (auto iter = partials.begin(); iter < partials.end(); iter++)
		{
			norm_squared += std::pow(*iter, 2);
		}

		// Avoid division by zero
		norm_squared += 1e-3;

		return std::sqrt(norm_squared);
	}

	Vector2D PotentialField::OneStepGD(const Vector2D & cur_pos, const Vector2D & goal, std::vector<Obstacle> & obs)
	{
		Vector2D dUA = AttractiveGradient(cur_pos, goal);
		Vector2D dUR = RepulsiveGradient(cur_pos, obs);

		Vector2D dU(dUA.x + dUR.x, dUA.y + dUR.y);

		std::vector<double> partials{dU.x, dU.y};

		double norm = MultiDimNorm(partials);

		double Descent_x = dU.x / norm;
		double Descent_y = dU.y / norm;

		Vector2D new_pos(cur_pos.x - Descent_x * eta, cur_pos.y - Descent_y * eta);

		double dist2goal = map::euclidean_distance(new_pos.x - goal.x, new_pos.y - goal.y);

		if (dist2goal <= eta or rigid2d::almost_equal(dist2goal, eta))
		{
			terminate = true;
		}

		return new_pos;
	}

	bool PotentialField::return_terminate()
	{
		return terminate;
	}
}


================================================
FILE: global_planner/src/lpastar.cpp
================================================
/// \file
/// \brief Draws Each Obstacle in RViz using MarkerArrays
///
/// PARAMETERS:
/// PUBLISHES:
/// SUBSCRIBES:
/// FUNCTIONS:

#include <ros/ros.h>

#include <math.h>
#include <string>
#include <vector>

#include "map/map.hpp"
#include "map/prm.hpp"
#include "map/grid.hpp"
#include <nav_msgs/OccupancyGrid.h>
#include "global_planner/incremental.hpp"

#include "nuslam/TurtleMap.h"

#include <functional>  // To use std::bind
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <geometry_msgs/Point.h>

// Used to deal with YAML list of lists
#include <xmlrpcpp/XmlRpcValue.h> // catkin component


int main(int argc, char** argv)
/// The Main Function ///
{
    ROS_INFO("STARTING NODE: astar");

    // Vars
    double frequency = 10.0;
    // Scale by which to transform marker poses (10 cm/cell so we use 10)
    double SCALE = 10.0;
    std::string frame_id = "base_footprint";
    XmlRpc::XmlRpcValue xml_obstacles;
    std::vector<double> start_vec{7.0, 3.0};
    std::vector<double> goal_vec{7.0, 26.0};
    double resolution = 0.01;

    // Map Parameters
    double thresh = 0.01;
    double inflate = 0.1;
    int visibility = 5;

    // store Obstacle(s) here to create Map
    std::vector<map::Obstacle> obstacles_v;

    ros::init(argc, argv, "astar_node"); // register the node on ROS
    ros::NodeHandle nh; // get a handle to ROS
    ros::NodeHandle nh_("~"); // get a handle to ROS
    // Parameters
    nh_.getParam("frequency", frequency);
    nh_.getParam("obstacles", xml_obstacles);
    nh_.getParam("start", start_vec);
    nh_.getParam("goal", goal_vec);
    nh_.getParam("map_frame_id", frame_id);
    nh_.getParam("thresh", thresh);
    nh_.getParam("inflate", inflate);
    nh_.getParam("scale", SCALE);
    nh_.getParam("resolution", resolution);
    nh_.getParam("visibility", visibility);

    rigid2d::Vector2D start(start_vec.at(0)/SCALE, start_vec.at(1)/SCALE);
    rigid2d::Vector2D goal(goal_vec.at(0)/SCALE, goal_vec.at(1)/SCALE);

    // 'obstacles' is a triple-nested list.
    // 1st level: obstacle (Obstacle), 2nd level: vertices (std::vector), 3rd level: coordinates (Vector2D)

    // std::vector<Obstacle>
    if(xml_obstacles.getType() != XmlRpc::XmlRpcValue::TypeArray)
    {
    ROS_ERROR("There is no list of obstacles");
    } else
    {
        for(int i = 0; i < xml_obstacles.size(); ++i)
        {
          // Obstacle contains std::vector<Vector2D>
          // create Obstacle with empty vertex vector
          map::Obstacle obs;
          if(xml_obstacles[i].getType() != XmlRpc::XmlRpcValue::TypeArray)
          {
              ROS_ERROR("obstacles[%d] has no vertices", i);
          } else {
              for(int j = 0; j < xml_obstacles[i].size(); ++j)
              {
                // Vector2D contains x,y center_coords
                if(xml_obstacles[i][j].size() != 2)
                {
                   ROS_ERROR("Vertex[%d] of obstacles[%d] is not a pair of coordinates", j, i);
                } else if(
                  xml_obstacles[i][j][0].getType() != XmlRpc::XmlRpcValue::TypeDouble or
                  xml_obstacles[i][j][1].getType() != XmlRpc::XmlRpcValue::TypeDouble)
                {
                  ROS_ERROR("The coordinates of vertex[%d] of obstacles[%d] are not doubles", j, i);
                } else {
                  // PASSED ALL THE TESTS: push Vector2D to vertices list in Obstacle object
                  rigid2d::Vector2D vertex(xml_obstacles[i][j][0], xml_obstacles[i][j][1]);
                  // NOTE: SCALE DOWN
                  vertex.x /= SCALE;
                  vertex.y /= SCALE;
                  obs.vertices.push_back(vertex);
                }
              }
          }
          // push Obstacle object to vector of Object(s) in Map
          obstacles_v.push_back(obs);
        }
    }

    // Initialize Marker
    // Init Marker Array Publisher
    ros::Publisher path_pub = nh.advertise<visualization_msgs::MarkerArray>("path", 1);

    ros::Publisher grid_pub = nh.advertise<nav_msgs::OccupancyGrid>("grid_map", 1);
    // Initialize grid_map outside
    nav_msgs::OccupancyGrid grid_map;
    // rviz representation of the grid
    std::vector<int8_t> map;

    // Init Marker Array
    visualization_msgs::MarkerArray path_arr;

    // Init Marker
    visualization_msgs::Marker marker;
    marker.header.frame_id = frame_id;
    marker.header.stamp = ros::Time::now();
    // marker.ns = "my_namespace";
    // marker.id = 0;
    marker.type = visualization_msgs::Marker::LINE_STRIP;
    marker.action = visualization_msgs::Marker::ADD;
    marker.pose.position.z = 0;
    marker.pose.orientation.x = 0.0;
    marker.pose.orientation.y = 0.0;
    marker.pose.orientation.z = 0.0;
    marker.pose.orientation.w = 1.0;
    marker.scale.x = 0.02;
    marker.color.r = 0.96f;
    marker.color.g = 0.475f;
    marker.color.b = 0.0f;
    marker.color.a = 1.0;
    marker.lifetime = ros::Duration();

    // LINE_STRIP relative to this pose
    marker.pose.position.x = 0.0;
    marker.pose.position.y = 0.0;

    // PATH MARKERS
    visualization_msgs::Marker path_marker;
    path_marker = marker;
    // More visible than PRM
    path_marker.scale.x = 0.03;
    path_marker.color.r = 0.2;
    path_marker.color.g = 1.0;
    path_marker.color.b = 0.2;

    visualization_msgs::Marker path_sph_mkr;
    path_sph_mkr = marker;
    // More visible than PRM
    path_sph_mkr.type = visualization_msgs::Marker::CUBE;
    path_sph_mkr.scale.x = 0.02;
    path_sph_mkr.scale.y = 0.02;
    path_sph_mkr.scale.z = 0.02;
    path_sph_mkr.color.r = 0.0f;
    path_sph_mkr.color.g = 0.0f;
    path_sph_mkr.color.b = 0.0f;

    // CURRENT POSITION MARKER
    visualization_msgs::Marker curr_pos_marker;
    curr_pos_marker = path_sph_mkr;
    // More visible than PRM
    curr_pos_marker.scale.x = 0.1;
    curr_pos_marker.scale.y = 0.1;
    curr_pos_marker.color.r = 1.0;
    curr_pos_marker.color.g = 0.2;
    curr_pos_marker.color.b = 0.2;

    // UpdateCell() MARKER
    visualization_msgs::Marker update_marker;
    update_marker = path_sph_mkr;
    // More visible than PRM
    update_marker.scale.x = 0.04;
    update_marker.scale.y = 0.04;
    update_marker.scale.z = 0.04;
    update_marker.color.r = 0.96f;
    update_marker.color.g = 0.475f;
    update_marker.color.b = 0.0f;
    update_marker.color.a = 1.0;
    update_marker.lifetime = ros::Duration(1.0 / frequency);


    // LPA* or D* Lite Path
    std::vector<global::Node> path;
    
    // GRID Version
    // Initialize Grid
    map::Grid grid(obstacles_v, inflate);

    // Build Map
    grid.build_map(resolution);

    ROS_INFO("Grid Built!");

    // Get map bounds
    auto bounds = grid.return_map_bounds();
    auto gridsize = grid.return_grid_dimensions();

    // Grid Pose
    geometry_msgs::Pose map_pose;
    map_pose.position.x = bounds.at(0).x;
    map_pose.position.y = bounds.at(0).y;
    map_pose.position.z = 0.0;
    map_pose.orientation.x = 0.0;
    map_pose.orientation.y = 0.0;
    map_pose.orientation.z = 0.0;
    map_pose.orientation.w = 1.0;

    // Occupancy grid for visualization
    grid_map.header.frame_id = frame_id;
    grid_map.info.resolution = resolution;
    grid_map.info.width = gridsize.at(0);
    grid_map.info.height = gridsize.at(1);

    // std::cout << "Width: " << gridsize.at(0) << "\t Height: " << gridsize.at(1) << std::endl;

    grid_map.info.origin = map_pose;

    ROS_INFO("Planning using LPA*!");
    global::LPAstar lpastar(obstacles_v, inflate);
    lpastar.Initialize(start, goal, grid, resolution);
    lpastar.ComputeShortestPath();
    path = lpastar.return_path();

    ros::Rate rate(frequency);

    unsigned int path_counter = 0;
    std::vector<global::Node> updated_nodes;

    // SLEEP so that I can prepare my screen recorder
    ros::Duration(2.0).sleep();

    // For visualization purposes (See below)
    bool firstpass = true;

    // Tells us if path contains obstacles
    bool valid_path = true;

    // Main While
    while (ros::ok())
    {
        ros::spinOnce();

        // rviz representation of the grid
        grid.fake_occupancy_grid(map);
        // Publish GRID Map
        grid_map.header.stamp = ros::Time::now();
        grid_map.info.map_load_time = ros::Time::now();
        grid_map.data = map;
        grid_pub.publish(grid_map);

        // DRAW PATH
        path_arr.markers.clear();
        path_marker.points.clear();
        int path_marker_id = 0;
        for (auto path_iter = path.begin(); path_iter != path.end(); path_iter++)
        {
            // Add node as marker cell
            geometry_msgs::Point vtx;
            vtx.x = path_iter->cell.center_coords.x;
            vtx.y = path_iter->cell.center_coords.y;
            vtx.z = 0.0;
            path_marker.points.push_back(vtx);

            // Also push back cubes
            path_sph_mkr.pose.position.x = path_iter->cell.center_coords.x;
            path_sph_mkr.pose.position.y = path_iter->cell.center_coords.y;
            path_sph_mkr.id = path_marker_id;
            path_marker_id++;
            path_arr.markers.push_back(path_sph_mkr);
        }
        path_marker.id = path_marker_id;
        path_arr.markers.push_back(path_marker);
        curr_pos_marker.pose.position.x = path.at(path_counter).cell.center_coords.x;
        curr_pos_marker.pose.position.y = path.at(path_counter).cell.center_coords.y;

        // Push Back Current Position
        path_marker_id++;
        curr_pos_marker.id = path_marker_id;
        path_arr.markers.push_back(curr_pos_marker);

        // Push Back Updated Cell Markers
        for (auto node_iter = updated_nodes.begin(); node_iter != updated_nodes.end(); node_iter++)
        {
          path_marker_id++;
          update_marker.pose.position.x = node_iter->cell.center_coords.x;
          update_marker.pose.position.y = node_iter->cell.center_coords.y;
          update_marker.id = path_marker_id;
          path_arr.markers.push_back(update_marker);
        }

        // Publish Path
        path_pub.publish(path_arr);

        // FAKE Update
        updated_nodes.clear();
        if (path_counter < path.size() - 1 and valid_path)
        {
          grid.update_grid(path.at(path_counter).cell, visibility);
          path_counter++;
          // ROS_INFO("UPDATE NUMBER: %d", path_counter);
          // LPA* Update
          updated_nodes = lpastar.SimulateUpdate(grid.return_fake_grid());
          path = lpastar.return_path();
          // Stop condition in case of obstacles
          valid_path = lpastar.return_valid();
        } else
        {
          // FINAL PATH. INFINITE MARKER DURATION
          path_marker.lifetime = ros::Duration();
          path_sph_mkr.lifetime = ros::Duration();
          curr_pos_marker.lifetime = ros::Duration();
        }

        // Make the markers persist for the first iteration to show original path
        if (firstpass)
        {
            ros::Duration(1.0).sleep();
            path_marker.lifetime = ros::Duration(2.0 / frequency);
            path_sph_mkr.lifetime = ros::Duration(2.0 / frequency);
            curr_pos_marker.lifetime = ros::Duration(2.0 / frequency);
        }

        firstpass = false;

        rate.sleep();
    }

    return 0;
}

================================================
FILE: global_planner/src/pf.cpp
================================================
/// \file
/// \brief Draws Each Obstacle in RViz using MarkerArrays
///
/// PARAMETERS:
/// PUBLISHES:
/// SUBSCRIBES:
/// FUNCTIONS:

#include <ros/ros.h>

#include <math.h>
#include <string>
#include <vector>

#include "map/map.hpp"
#include "map/prm.hpp"
#include "map/grid.hpp"
#include <nav_msgs/OccupancyGrid.h>
#include "global_planner/potential_field.hpp"

#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

#include <functional>  // To use std::bind

// Used to deal with YAML list of lists
#include <xmlrpcpp/XmlRpcValue.h> // catkin component


int main(int argc, char** argv)
/// The Main Function ///
{
    ROS_INFO("STARTING NODE: pf");

    // Vars
    double frequency = 10.0;
    // Scale by which to transform marker poses (10 cm/cell so we use 10)
    double SCALE = 10.0;
    std::string frame_id = "base_footprint";
    XmlRpc::XmlRpcValue xml_obstacles;

    // Map Parameters
    double thresh = 0.01;
    double inflate = 0.1;

    std::vector<double> start_vec{7.0, 3.0};
    std::vector<double> goal_vec{7.0, 26.0};

    // PF Parameters
    double eta = 1.0;
    double ada = .1;
    double zeta = 10.0;
    double d_thresh = 2.0;
    double Q_thresh = 0.1;

    // store Obstacle(s) here to create Map
    std::vector<map::Obstacle> obstacles_v;

    ros::init(argc, argv, "pf_node"); // register the node on ROS
    ros::NodeHandle nh; // get a handle to ROS
    ros::NodeHandle nh_("~"); // get a handle to ROS
    // Parameters
    nh_.getParam("frequency", frequency);
    nh_.getParam("obstacles", xml_obstacles);
    nh_.getParam("map_frame_id", frame_id);
    nh_.getParam("thresh", thresh);
    nh_.getParam("inflate", inflate);
    nh_.getParam("scale", SCALE);
    nh_.getParam("start", start_vec);
    nh_.getParam("goal", goal_vec);
    // PF Params
    nh_.getParam("eta", eta);
    nh_.getParam("ada", ada);
    nh_.getParam("zeta", zeta);
    nh_.getParam("d_thresh", d_thresh);
    nh_.getParam("Q_thresh", Q_thresh);

    // START and GOAL
    rigid2d::Vector2D start(start_vec.at(0)/SCALE, start_vec.at(1)/SCALE);
    rigid2d::Vector2D goal(goal_vec.at(0)/SCALE, goal_vec.at(1)/SCALE);

    // Path Publishers
    ros::Publisher path_pub = nh_.advertise<nav_msgs::Path>("robot_path", 1);

    // Using path topic since it's already in rviz
    ros::Publisher sg_pub = nh.advertise<visualization_msgs::MarkerArray>("path", 1);


    // Init Markers
    visualization_msgs::Marker s_marker;
    s_marker.header.frame_id = frame_id;
    s_marker.header.stamp = ros::Time::now();
    s_marker.type = visualization_msgs::Marker::SPHERE;
    s_marker.action = visualization_msgs::Marker::ADD;
    s_marker.pose.position.z = 0;
    s_marker.pose.position.x = start.x;
    s_marker.pose.position.y = start.y;
    s_marker.pose.orientation.x = 0.0;
    s_marker.pose.orientation.y = 0.0;
    s_marker.pose.orientation.z = 0.0;
    s_marker.pose.orientation.w = 1.0;
    s_marker.scale.x = 0.05;
    s_marker.scale.y = 0.05;
    s_marker.scale.z = 0.05;
    s_marker.color.r = 0.5;
    s_marker.color.g = 0.0;
    s_marker.color.b = 0.5;
    s_marker.color.a = 1.0;
    s_marker.id = 0;
    s_marker.lifetime = ros::Duration();

    visualization_msgs::Marker g_marker = s_marker;
    g_marker.pose.position.x = goal.x;
    g_marker.pose.position.y = goal.y;
    g_marker.color.r = 0.0;
    g_marker.color.g = 1.0;
    g_marker.color.b = 0.0;
    g_marker.id = 1;

    // Init Marker Array
    visualization_msgs::MarkerArray sg_arr;

    sg_arr.markers.clear();
    sg_arr.markers.push_back(s_marker);
    sg_arr.markers.push_back(g_marker);



    // 'obstacles' is a triple-nested list.
    // 1st level: obstacle (Obstacle), 2nd level: vertices (std::vector), 3rd level: coordinates (Vector2D)
    // std::vector<Obstacle>
    if(xml_obstacles.getType() != XmlRpc::XmlRpcValue::TypeArray)
    {
    ROS_ERROR("There is no list of obstacles");
    } else {
        for(int i = 0; i < xml_obstacles.size(); ++i)
        {
          // Obstacle contains std::vector<Vector2D>
          // create Obstacle with empty vertex vector
          map::Obstacle obs;
          if(xml_obstacles[i].getType() != XmlRpc::XmlRpcValue::TypeArray)
          {
              ROS_ERROR("obstacles[%d] has no vertices", i);
          } else {
              for(int j = 0; j < xml_obstacles[i].size(); ++j)
              {
                // Vector2D contains x,y coords
                if(xml_obstacles[i][j].size() != 2)
                {
                   ROS_ERROR("Vertex[%d] of obstacles[%d] is not a pair of coordinates", j, i);
                } else if(
                  xml_obstacles[i][j][0].getType() != XmlRpc::XmlRpcValue::TypeDouble or
                  xml_obstacles[i][j][1].getType() != XmlRpc::XmlRpcValue::TypeDouble)
                {
                  ROS_ERROR("The coordinates of vertex[%d] of obstacles[%d] are not doubles", j, i);
                } else {
                  // PASSED ALL THE TESTS: push Vector2D to vertices list in Obstacle object
                  rigid2d::Vector2D vertex(xml_obstacles[i][j][0], xml_obstacles[i][j][1]);
                  // NOTE: SCALE DOWN
                  vertex.x /= SCALE;
                  vertex.y /= SCALE;
                  obs.vertices.push_back(vertex);
                }
              }
          }
          // push Obstacle object to vector of Object(s) in Map
          obstacles_v.push_back(obs);
        }
    }
    geometry_msgs::PoseStamped ps;
    ps.pose.position.x = start.x;
    ps.pose.position.y = start.y;
    std::vector<geometry_msgs::PoseStamped> robot_poses{ps};
    nav_msgs::Path path;
    path.header.frame_id = frame_id;

    global::PotentialField PF(obstacles_v, eta, ada, zeta, d_thresh, Q_thresh);
    bool terminate = PF.return_terminate();

    // SLEEP so that I can prepare my screen recorder
    ros::Duration(2.0).sleep();

    ros::Rate rate(frequency);

    bool finito = false;

    // Main While
    while (ros::ok())
    {
        ros::spinOnce();

        path.header.stamp = ros::Time::now();

        if (!terminate)
        {
          start = PF.OneStepGD(start, goal, obstacles_v);
          ps.pose.position.x = start.x;
          ps.pose.position.y = start.y;
          robot_poses.push_back(ps);
          terminate = PF.return_terminate();
          ROS_DEBUG("NEW POS: [%.2f, %.2f]", start.x, start.y);
        } else if (!finito)
        {
          ROS_INFO("GOAL REACHED.");
          finito = true;
        }

        path.poses = robot_poses;
        path_pub.publish(path);

        sg_pub.publish(sg_arr);

        
        rate.sleep();
    }

    return 0;
}

================================================
FILE: map/CMakeLists.txt
================================================
cmake_minimum_required(VERSION 3.0)
project(map)

## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-Wall -Wextra -pipe -Wno-psabi)

set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
  message_generation
  message_runtime
  rigid2d
  roscpp
  rostest
  nuslam
  visualization_msgs
)

find_package(Eigen3 3.3 REQUIRED NO_MODULE)

## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)


## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()

################################################
## Declare ROS messages, services and actions ##
################################################

## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
##   * add a build_depend tag for "message_generation"
##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
##     but can be declared for certainty nonetheless:
##     * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
##   * add "message_generation" and every package in MSG_DEP_SET to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * add "message_runtime" and every package in MSG_DEP_SET to
##     catkin_package(CATKIN_DEPENDS ...)
##   * uncomment the add_*_files sections below as needed
##     and list every .msg/.srv/.action file to be processed
##   * uncomment the generate_messages entry below
##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)

## Generate messages in the 'msg' folder
# add_message_files(
#   FILES
#   Message1.msg
#   Message2.msg
# )

## Generate services in the 'srv' folder
# add_service_files(
#   FILES
#   Service1.srv
#   Service2.srv
# )

## Generate actions in the 'action' folder
# add_action_files(
#   FILES
#   Action1.action
#   Action2.action
# )

## Generate added messages and services with any dependencies listed here
# generate_messages(
#   DEPENDENCIES
#   std_msgs  # Or other packages containing msgs
# )

################################################
## Declare ROS dynamic reconfigure parameters ##
################################################

## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
##   * add "dynamic_reconfigure" to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * uncomment the "generate_dynamic_reconfigure_options" section below
##     and list every .cfg file to be processed

## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
#   cfg/DynReconf1.cfg
#   cfg/DynReconf2.cfg
# )

###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME} rigid2d nuslam
CATKIN_DEPENDS message_generation message_runtime rigid2d roscpp nuslam visualization_msgs
#  DEPENDS system_lib
)

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
  include
  ${catkin_INCLUDE_DIRS}
  ${Eigen3_INCLUDE_DIRS}
)

add_library(${PROJECT_NAME}
  src/${PROJECT_NAME}/${PROJECT_NAME}.cpp
  src/${PROJECT_NAME}/prm.cpp
  src/${PROJECT_NAME}/grid.cpp
)

## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/map_node.cpp)
add_executable(viz_map src/viz_map.cpp)
add_executable(viz_grid src/viz_grid.cpp)

## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
set_target_properties(viz_map PROPERTIES OUTPUT_NAME viz_map PREFIX "")
set_target_properties(viz_grid PROPERTIES OUTPUT_NAME viz_grid PREFIX "")

## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(viz_map ${${PROJECT_NAME}_EXPORTED_TARGETS} ${Eigen3_EXPORTED_TARGETS} ${rigid2d_EXPORTED_TARGETS} ${nuslam_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(viz_grid ${${PROJECT_NAME}_EXPORTED_TARGETS} ${Eigen3_EXPORTED_TARGETS} ${rigid2d_EXPORTED_TARGETS} ${nuslam_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Specify libraries to link a library or executable target against
target_link_libraries(
viz_map # this is my node that will use below libraries
${rigid2d_LIBRARIES}
${nuslam_LIBRARIES}
Eigen3::Eigen
${PROJECT_NAME}
${catkin_LIBRARIES}
)

target_link_libraries(
viz_grid # this is my node that will use below libraries
${rigid2d_LIBRARIES}
${nuslam_LIBRARIES}
Eigen3::Eigen
${PROJECT_NAME}
${catkin_LIBRARIES}
)

#############
## Install ##
#############

# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html

## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
#   scripts/my_python_script
#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
install(TARGETS viz_map viz_grid
  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
install(TARGETS ${PROJECT_NAME}
  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
)

## Mark cpp header files for installation
install(DIRECTORY include/${PROJECT_NAME}/
  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
  FILES_MATCHING PATTERN "*.h"
  PATTERN ".svn" EXCLUDE
)

## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
#   # myfile1
#   # myfile2
#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )

#############
## Testing ##
#############

## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/${PROJECT_NAME}_test.cpp)
# if(TARGET ${PROJECT_NAME}-test)
#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()

## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

# if (CATKIN_ENABLE_TESTING)
#     catkin_add_gtest(${PROJECT_NAME}_test tests/${PROJECT_NAME}_test.cpp)
#     target_link_libraries(${PROJECT_NAME}_test ${catkin_Libraries} gtest_main ${PROJECT_NAME})
# endif()


================================================
FILE: map/config/map.yaml
================================================
# NOTE: coordinates in cell #s, choose to convert to m/cm/mm later
# Each item (11 total) in list is an obstacle. Each obstacle has vertices
# with cartesian coordinates
obstacles: [[[12.,  6.], [14.5,   3.5], [17., 5.5], [17.,  8.5],  [14., 8.]], #A
            [[24.,  6.], [26.,    3.5], [31., 7.5], [24.5, 9.5]],             #B
            [[34., 26.], [10.,    26.], [10., 12.], [34.,  12.]],             #C
            [[0.,  26.], [0.,      6.], [4.,   6.], [4.,   26.]],             #D
            [[4.,  32.], [6.,     30.], [8.,  32.]],                          #E
            [[17., 32.], [18.,    30.], [19., 32.]],                          #F
            [[0.,  36.], [0.,     32.], [29., 32.], [29.,  36.]],             #G
            [[34., 36.], [33.,    34.], [34., 32.]],                          #H
            [[6.,  44.], [2.,     43.], [2.,  39.], [6.,   38.],  [8., 41.]], #I
            [[11., 48.], [17.,    41.], [14., 48.]],                          #J
            [[30., 48.], [22.,    40.], [32., 48.]],                          #K
            [[0.,   0.], [34.,     0.]], # OUTER WALL 1
            [[34.,  0.], [34.,    48.]], # OUTER WALL 2
            [[34., 48.], [0.,     48.]], # OUTER WALL 3
            [[0.,  48.], [0.,      0.]]] # OUTER WALL 4

map_bounds: [[0.,   0.], [34.,     0.], [34., 48.], [0.,   48.]]

================================================
FILE: map/include/map/grid.hpp
================================================
#ifndef GRID_INCLUDE_GUARD_HPP
#define GRID_INCLUDE_GUARD_HPP
/// \file
/// \brief GRID Library to build a Probabilistic Roadmap.
#include <map/map.hpp>
#include <map/prm.hpp> // to use PRM::too_close method

namespace map
{
    using rigid2d::Vector2D;

    // \brief stores indeces to acces elements in row-major order or grid-wise
    struct Index{
        int row_major = -1;
        int x = -1;
        int y = -1;

    };

    enum CellType {Occupied, Inflation, Free};

    // \brief struct to store Cell paramters such as coordinates, center coordinates,
    // whether a cell has been visited, and the cell type (Start, Goal, Obstacle, Standard)
    struct Cell{

        // \brief Cell constructor with input coordinates
        Cell(const Vector2D & coords_, const double & resolution_);

        // Corner coordinates
        Vector2D coords;
        // Centre coordinates
        Vector2D center_coords;
        bool visited = false;
        // Default Cell Type
        CellType celltype = Free;
        // Cell value
        double value = 0.0;
        // Cell index in grid (also RMJ)
        Index index;
        double resolution;
        // Used for simulated grid update
        bool newView = false;
    };

    /// \brief stores Obstacle(s) to construct basic Grid. Inherits from Map in map.hpp.
    class Grid : public Map
    {
        // Inherits Constructors
        using Map::Map;

    public:

        // \brief Constructs a Grid Map.
        // \param resolution: determines the grid cell size
        void build_map(const double & resolution);

        // \brief converts world coordinates to grid coordinates and returns result
        // \param cell: a grid cell
        // \returns the grid cell's world coordinates
        Index world2grid(const Cell & cell) const;

        // \brief converts grid coordinates to world coordinates and returns result
        // \param i: x-coordinate of grid cell
        // \param j: y-coordinate of grid cell
        // \param resolution: determines the grid cell size
        // \returns the grid cell's grid coordinates
        Vector2D grid2world(const int & i, const int & j, const double & resolution) const;

        // \brief Return Grid in row-major order
        // \returns vector containing grid cells as Cell
        std::vector<Cell> return_grid() const;

        // \brief populates Occupancy Grid with values for visualization
        // \param map: the Occupancy Grid map to populate
        void occupancy_grid(std::vector<int8_t> & map) const;

        // \brief returns the width and height of the grid in cells
        // \returns int vector containing width and height respectively.
        std::vector<int> return_grid_dimensions() const;

        // \brief gets the neighbours of a given cell according to a visibility bound
        // \param current_cell: the cell from which we simulate the update
        // \param visibility: size of bounding box used for update (can vary each iteration if desired). DEFAULT is 1 for 3x3 eval.
        // \returns: vector of neighbour Cells
        std::vector<Cell> get_neighbours(const Cell & cc, const std::vector<Cell> & map, const int & visibility=1);

        // \brief Increment fake copy of grid for simulated updates
        // \param current_cell: the cell from which we simulate the update
        // \param visibility: size of bounding box used for update (can vary each iteration if desired)
        void update_grid(const Cell & cc, const int & visibility);

        // \brief Return FAKE (simulated increment) Grid in row-major order
        // \returns vector containing grid cells as Cell
        std::vector<Cell> return_fake_grid() const;

        // \brief populates Occupancy Grid with FAKE values for visualization
        // \param map: the Occupancy Grid map to populate
        void fake_occupancy_grid(std::vector<int8_t> & map) const;

        // \brief Checks whether a potential Vertex lies on an Obstacle. 'map::PRM::sample_configurations' calls this function.
        // \param q: the Vertex being examined
        friend bool not_inside(const Vertex & q, const std::vector<Obstacle> & obstacles, const double & inflate_robot);

        // \brief Checks if a Vertex is too close to an Edge.
        // \param E1: the first Vertex forming an edge
        // \param E2: the second Vertex forming an edge
        // \param P0: the Vertex whose closeness is being examined.
        // \param inflate_robot: approximate robot radius used for collision checking.
        friend bool too_close(const Vertex & E1, const Vertex & E2, const Vertex & P0, const double & inflate_robot);

    private:
        std::vector<Cell> cells;
        std::vector<Cell> fake_grid;
        std::vector<double> xcells;
        std::vector<double> ycells;
    };

    // \brief numpy arange in C++, can take any type T (int,double,etc)
    template<typename T>
    std::vector<T> arange(const T & start, const T & stop, const T & step = 1)
    {
        std::vector<T> values;
        for (T value = start; value < stop; value += step)
            values.push_back(value);
        return values;
    }

    // \brief convert from row-major-order coordinates to grid coordinates
    // \param rmj: row-major-order coordinate
    // \param numrow: number of rows in grid
    // \returns Index containing grid coordintates
    Index rowmajor2grid(const int & rmj, const int & numrow);

    // \brief convert from grid coordinates to row-major-order coordinates
    // \param x: x-coordinate of grid cell
    // \param y: y-coordinate of grid cell
    // \param numcol: number of columns in grid
    // \returns row-major-order index
    int grid2rowmajor(const int & x, const int & y, const int & numrow);
}

#endif


================================================
FILE: map/include/map/map.hpp
================================================
#ifndef MAP_INCLUDE_GUARD_HPP
#define MAP_INCLUDE_GUARD_HPP
/// \file
/// \brief Map Library to store and display polygon-based custom map.
/// NOTE: rigid2d is from turtlebot3_from_scratch (tb3 in nuturtle.rosinstall)
#include <rigid2d/rigid2d.hpp>
#include <vector>
#include <eigen3/Eigen/Dense>

namespace map
{
    // Used to store Obstacle vertex coordinates
    using rigid2d::Vector2D;

    struct Obstacle
    /// \brief stores obstacle vertex coordinates in CCW order
    {
        std::vector<Vector2D> vertices;

        // \brief constructor for Obstacle without inputs
        Obstacle();

        // \brief constructor for Obstacle with inputs
        Obstacle(const std::vector<Vector2D> & vertices_);
    };

    /// \brief stores Obstacle(s) to construct basic map
    class Map
    {
    public:
        /// \brief the default constructor creates an empty map
        Map();

        /// \brief this constructor creates a map that holds user-specified obstacles
        /// \param obstacles_: the list of obstacles in this map
        Map(const std::vector<Obstacle> & obstacles_);

        /// \brief this constructor creates a map that holds user-specified obstacles and robot size threshold.
        /// \param obstacles_: the list of obstacles in this map
        // \param inflate_robot_: approximate robot radius used for collision checking.
        Map(const std::vector<Obstacle> & obstacles_, const double inflate_robot_);

        /// \brief return vector of obstacles
        /// \returns vector of Obstacle
        std::vector<Obstacle> return_obstacles();

        /// \brief assigns the map space to map_extent in absolute(x,y).
        void find_map_extent();

        // \brief return map bounds
        // \returns std::vector<Vector2D> containing minimum and maximum bounds respectively
        std::vector<Vector2D> return_map_bounds();


    // protected instead of private so that child Class can access
    protected:
        // Map obstacles
        std::vector<Obstacle> obstacles;

        // Map maximum coordinatesin x,y
        Vector2D map_max;

        // Map minimum coordinatesin x,y
        Vector2D map_min;

        // approximate robot radius used for collision checking.
        double inflate_robot;
    };

    double euclidean_distance(const double & x_rel, const double & y_rel);
}

#endif


================================================
FILE: map/include/map/prm.hpp
================================================
#ifndef PRM_INCLUDE_GUARD_HPP
#define PRM_INCLUDE_GUARD_HPP
/// \file
/// \brief PRM Library to build a Probabilistic Roadmap.
#include <map/map.hpp>
#include <unordered_set>
#include <unordered_map>

namespace map
{
    using rigid2d::Vector2D;

    struct Edge
    {
        // ID of next node connected by edge
        // init to -1 for error checking
        int next_id = -1;
        // Euclidean Distance between Nodes (Vertices)
        int distance;
    };

    struct Vertex
    {
        // \brief Constructor with Coordinates
        Vertex(const Vector2D coords_);

        // ID of current Node/Vertex
        // init to -1 for error checking
        int id = -1;
        // Cartesian Coordinates of Vertex
        Vector2D coords;
        // Edges connected to this node
        std::vector<Edge> edges;
        // IDs of Adjacent Nodes
        // O(1) best case or O(n) worst case
        std::unordered_set<int> id_set;

        // Helps with search
        bool visited;

        /// \brief Check if edge exists in ID Hash Table
        /// \param check_id: ID of node to connect to
        /// \returns True if current Vertex connected to given ID.
        bool edge_exists(const int & check_id) const;
    };

    // \brief struct to store important attributes of the shortest distance from a point to a line segment
    struct ShortestDistance
    {
        Vector2D point; // The point whose shortest distance to a line segment is computed.
        double u; // A point is within a line segment if u is between [0,1]
        double D; // The signed shotrest distance distance from the point to the line segment.
        // if D > 0, the point is on the left-hand side of the line segment from Vertex 1 to Vertex 2 
    };

    /// \brief stores Obstacle(s) to construct basic PRM. Inherits from Map in map.hpp.
    class PRM : public Map
    {
        // Inherits Constructors
        using Map::Map;

    public:

        // \brief Collision checking on Concave polygons is very tedious, so we divide them into Convex ones.
        void divide_concave_polygons();

        // \brief Constructs a Roadmap.
        // \param n: number of nodes to put in the Roadmap.
        // \param k: number of closest neighbours to examine for each configuration.
        // \param thresh: Euclidean Distance Threshold for valid Edge.
        void build_map(const int & n, int & k, const double & thresh);

        // \brief Sample free space Q for configurations q. Steps 3-8 of algorithm.
        // \param n: number of nodes to put in the Roadmap.
        void sample_configurations(const int & n);

        // \brief For a given configuration q, assigns indeces of K Nearest Neighbours to q id_set. Step 10 of algorithm.
        // \param q: the Vertex being examined. Not const because id_set is modified.
        // \param k: number of closest neighbours to examine for each configuration.
        // NOTE: Using Brute Force Now, replace with KD-Tree + Rebalance
        std::vector<Vertex> find_knn(Vertex & q, const int & k);

        // \brief Check is the Edge between two nodes is valid (no collision, and above some euclidean distance)
        // \param q: the main Vertex being examined
        // \param q_prime: the second Vertex being examined
        // \param thresh: Euclidean Distance Threshold for valid Edge.
        bool edge_valid(const Vertex & q, const Vertex & q_prime, const double & thresh);

        // \brief Checks whether a potential Edge intersects a Polygon.
        // 'map::PRM::no_collision(const Vertex & q, const Vertex & q_prime, const double & inflate_robot)' calls this function.
        // \param q: the main Vertex being examined
        // \param q_prime: the second Vertex being examined
        // \param obs_iter: iterator for the Obstacle (Polygon) whose vertices we examine.
        bool no_collision(const Vertex & q, const Vertex & q_prime, const std::vector<Obstacle>::iterator & obs_iter);

        // \brief Checks whether a potential Edge intersects an Obstacle considering the robot's geometry.
        // \param q: the main Vertex being examined
        // \param q_prime: the second Vertex being examined
        // \param inflate_robot: approximate robot radius used for collision checking.
        bool no_collision(const Vertex & q, const Vertex & q_prime, const double & inflate_robot);

        // \brief computes the shortest distance from a line to a point and returns the result in an informative struct.
        // \param E1: the first Vertex forming an edge
        // \param E2: the second Vertex forming an edge
        // \param P0: the Vertex whose closeness is being examined.
        // \param inflate_robot: approximate robot radius used for collision checking.
        // \returns ShortestDistance struct
        friend ShortestDistance lineToPoint(const Vertex & E1, const Vertex & E2, const Vertex & P0, const double & inflate_robot);

        // \brief Checks whether a potential Vertex lies on an Obstacle. 'map::PRM::sample_configurations' calls this function.
        // \param q: the Vertex being examined
        friend bool not_inside(const Vertex & q, const std::vector<Obstacle> & obstacles, const double & inflate_robot);

        // \brief Checks if a Vertex is too close to an Edge.
        // \param E1: the first Vertex forming an edge
        // \param E2: the second Vertex forming an edge
        // \param P0: the Vertex whose closeness is being examined.
        // \param inflate_robot: approximate robot radius used for collision checking.
        friend bool too_close(const Vertex & E1, const Vertex & E2, const Vertex & P0, const double & inflate_robot);

        // \brief Return Probabilistic Road Map
        // \returns Probabilistic Road Map 
        std::vector<Vertex> return_prm();
    private:
        // Hash Table
        std::vector<Vertex> configurations;
    };

    // \brief Checks whether a potential Edge intersects a Polygon.
    // 'map::PRM::no_collision(const Vertex & q, const Vertex & q_prime, const double & inflate_robot)' calls this function.
    // \param q: the main Vertex being examined
    // \param q_prime: the second Vertex being examined
    // \param obs_iter: iterator for the Obstacle (Polygon) whose vertices we examine.
    bool no_intersect(const Vertex & q, const Vertex & q_prime, const std::vector<Obstacle>::iterator & obs_iter);

    // \brief Checks whether a potential Vertex lies on an Obstacle. 'map::PRM::sample_configurations' calls this function.
    // \param q: the Vertex being examined
    bool not_inside(const Vertex & q, const std::vector<Obstacle> & obstacles, const double & inflate_robot);

    // \brief Checks if a Vertex is too close to an Edge.
    // \param E1: the first Vertex forming an edge
    // \param E2: the second Vertex forming an edge
    // \param P0: the Vertex whose closeness is being examined.
    // \param inflate_robot: approximate robot radius used for collision checking.
    bool too_close(const Vertex & E1, const Vertex & E2, const Vertex & P0, const double & inflate_robot);

    // \brief computes the shortest distance from a line to a point and returns the result in an informative struct.
    // \param E1: the first Vertex forming an edge
    // \param E2: the second Vertex forming an edge
    // \param P0: the Vertex whose closeness is being examined.
    // \returns ShortestDistance struct
    ShortestDistance lineToPoint(const Vertex & E1, const Vertex & E2, const Vertex & P0);
}

#endif


================================================
FILE: map/launch/viz_map.launch
================================================
<launch>
  <!-- This launchfile loads a differential drive robot into RViz, whose parameters are set
       and can be modified in diff_params.yaml

       It also loads the custom map -->

  <!-- load the urdf into the parameter server from the xacro file-->
  <param name="robot_description" command="xacro '$(find nuturtle_gazebo)/urdf/diff_drive.gazebo.xacro'" />


  <!-- The robot_state_publisher reads the urdf from /robot_description parameter
       and listens to joint information on the /joint_states topic -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>

  <!-- The joint state publisher will be launched with a gui, read the urdf from /robot_description
       and publish the joint values on /joint_states. Optional launch using use_jsp_gui:=1 --> 
  <arg name="js_pub" default="True" doc="Launch the joint_state_publisher"/>
  <group if="$(arg js_pub)">
    <arg name="use_jsp_gui" default="False" doc="Launch the joint_state_publisher GUI to publish joint angles manually"/>
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
      <param name="use_gui" value="$(eval use_jsp_gui)" />
    </node>
  </group>

  <!-- load diff_drive_bot parameters to parameter server -->
  <rosparam command="load" file="$(find nuturtle_description)/config/diff_params.yaml" />

  <arg name="rviz_view" default="$(find map)/rviz/basic_map.rviz"/>

  <node name="rviz" pkg="rviz" type="rviz" required="True" args="-d $(arg rviz_view)"/>


  <!-- Map Visualizer Node -->
  <node name="viz_map" pkg="map" type="viz_map" output="screen">
    <rosparam command="load" file="$(find map)/config/map.yaml"/>
    <param name="scale" value="5.0"/>
  </node>

  <!-- PRM Visualizer Node -->
  <node name="viz_prm" pkg="map" type="viz_map" output="screen">
    <rosparam command="load" file="$(find map)/config/map.yaml"/>
    <param name="map_type" value="prm"/>
    <param name="n" value="300"/>
    <param name="k" value="10"/>
    <param name="thresh" value="0.1"/>
    <param name="inflate" value="0.1"/>
    <remap from="map" to="prm"/>
    <param name="scale" value="5.0"/>
  </node>

  <!-- Grid Visualizer Node -->
  <node name="viz_grid" pkg="map" type="viz_grid" output="screen">
    <rosparam command="load" file="$(find map)/config/map.yaml"/>
    <param name="resolution" value="0.06"/>
    <param name="inflate" value="0.1"/>
    <param name="scale" value="5.0"/>
  </node>

</launch>

================================================
FILE: map/package.xml
================================================
<?xml version="1.0"?>
<package format="2">
  <name>map</name>
  <version>0.0.1</version>
  <description>PRM and Grid Maps</description>

  <!-- One maintainer tag required, multiple allowed, one person per tag -->
  <!-- Example:  -->
  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
  <maintainer email="mrahme97@gmail.com">Maurice Rahme</maintainer>


  <!-- One license tag required, multiple allowed, one license per tag -->
  <!-- Commonly used license strings: -->
  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
  <license>MIT</license>


  <!-- Url tags are optional, but multiple are allowed, one per tag -->
  <!-- Optional attribute type can be: website, bugtracker, or repository -->
  <!-- Example: -->
  <!-- <url type="website">http://wiki.ros.org/map</url> -->


  <!-- Author tags are optional, multiple are allowed, one per tag -->
  <!-- Authors do not have to be maintainers, but could be -->
  <!-- Example: -->
  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->


  <!-- The *depend tags are used to specify dependencies -->
  <!-- Dependencies can be catkin packages or system dependencies -->
  <!-- Examples: -->
  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
  <!--   <depend>roscpp</depend> -->
  <!--   Note that this is equivalent to the following: -->
  <!--   <build_depend>roscpp</build_depend> -->
  <!--   <exec_depend>roscpp</exec_depend> -->
  <!-- Use build_depend for packages you need at compile time: -->
  <!--   <build_depend>message_generation</build_depend> -->
  <!-- Use build_export_depend for packages you need in order to build against this package: -->
  <!--   <build_export_depend>message_generation</build_export_depend> -->
  <!-- Use buildtool_depend for build tool packages: -->
  <!--   <buildtool_depend>catkin</buildtool_depend> -->
  <!-- Use exec_depend for packages you need at runtime: -->
  <!--   <exec_depend>message_runtime</exec_depend> -->
  <!-- Use test_depend for packages you need only for testing: -->
  <!--   <test_depend>gtest</test_depend> -->
  <!-- Use doc_depend for packages you need only for building documentation: -->
  <!--   <doc_depend>doxygen</doc_depend> -->
  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>message_generation</build_depend>
  <build_depend>message_runtime</build_depend>
  <build_depend>rigid2d</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>nuslam</build_depend>
  <build_depend>visualization_msgs</build_depend>
  <build_export_depend>message_generation</build_export_depend>
  <build_export_depend>message_runtime</build_export_depend>
  <build_export_depend>rigid2d</build_export_depend>
  <build_export_depend>nuslam</build_export_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>visualization_msgs</build_export_depend>
  <exec_depend>message_runtime</exec_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>rigid2d</exec_depend>
  <exec_depend>nuslam</exec_depend>
  <exec_depend>visualization_msgs</exec_depend>

  <test_depend>rostest</test_depend>
  <test_depend>rosunit</test_depend>
  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->

  </export>
</package>


================================================
FILE: map/rviz/basic_map.rviz
================================================
Panels:
  - Class: rviz/Displays
    Help Height: 77
    Name: Displays
    Property Tree Widget:
      Expanded:
        - /Global Options1
        - /Grid1
        - /TF1/Frames1
        - /TF1/Tree1
        - /Obstacles1
        - /PRM1
      Splitter Ratio: 0.5
    Tree Height: 714
  - Class: rviz/Selection
    Name: Selection
  - Class: rviz/Tool Properties
    Expanded:
      - /2D Pose Estimate1
      - /2D Nav Goal1
      - /Publish Point1
    Name: Tool Properties
    Splitter Ratio: 0.5886790156364441
  - Class: rviz/Views
    Expanded:
      - /Current View1
    Name: Views
    Splitter Ratio: 0.5
  - Class: rviz/Time
    Experimental: false
    Name: Time
    SyncMode: 0
    SyncSource: ""
Preferences:
  PromptSaveOnExit: true
Toolbars:
  toolButtonStyle: 2
Visualization Manager:
  Class: ""
  Displays:
    - Alpha: 0.5
      Cell Size: 0.10000000149011612
      Class: rviz/Grid
      Color: 85; 87; 83
      Enabled: true
      Line Style:
        Line Width: 0.029999999329447746
        Value: Lines
      Name: Grid
      Normal Cell Count: 0
      Offset:
        X: 0
        Y: 0
        Z: 0
      Plane: XY
      Plane Cell Count: 300
      Reference Frame: <Fixed Frame>
      Value: true
    - Class: rviz/TF
      Enabled: true
      Frame Timeout: 15
      Frames:
        All Enabled: false
        ball_caster:
          Value: false
        base_footprint:
          Value: false
        base_link:
          Value: false
        base_scan:
          Value: true
        rl_wheel:
          Value: false
        rr_wheel:
          Value: false
      Marker Scale: 1
      Name: TF
      Show Arrows: true
      Show Axes: true
      Show Names: true
      Tree:
        base_footprint:
          base_link:
            ball_caster:
              {}
            base_scan:
              {}
            rl_wheel:
              {}
            rr_wheel:
              {}
      Update Interval: 0
      Value: true
    - Alpha: 1
      Class: rviz/RobotModel
      Collision Enabled: false
      Enabled: true
      Links:
        All Links Enabled: true
        Expand Joint Details: false
        Expand Link Details: false
        Expand Tree: false
        Link Tree Style: Links in Alphabetic Order
        ball_caster:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        base_footprint:
          Alpha: 1
          Show Axes: false
          Show Trail: false
        base_link:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        base_scan:
          Alpha: 1
          Show Axes: false
          Show Trail: false
        rl_wheel:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        rr_wheel:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
      Name: RobotModel
      Robot Description: robot_description
      TF Prefix: ""
      Update Interval: 0
      Value: true
      Visual Enabled: true
    - Class: rviz/MarkerArray
      Enabled: true
      Marker Topic: /map
      Name: Obstacles
      Namespaces:
        "": true
      Queue Size: 100
      Value: true
    - Class: rviz/MarkerArray
      Enabled: true
      Marker Topic: /prm
      Name: PRM
      Namespaces:
        "": true
      Queue Size: 100
      Value: true
    - Alpha: 0.699999988079071
      Class: rviz/Map
      Color Scheme: map
      Draw Behind: false
      Enabled: true
      Name: Map
      Topic: /grid_map
      Unreliable: false
      Use Timestamp: false
      Value: true
  Enabled: true
  Global Options:
    Background Color: 48; 48; 48
    Default Light: true
    Fixed Frame: base_footprint
    Frame Rate: 30
  Name: root
  Tools:
    - Class: rviz/Interact
      Hide Inactive Objects: true
    - Class: rviz/MoveCamera
    - Class: rviz/Select
    - Class: rviz/FocusCamera
    - Class: rviz/Measure
    - Class: rviz/SetInitialPose
      Theta std deviation: 0.2617993950843811
      Topic: /initialpose
      X std deviation: 0.5
      Y std deviation: 0.5
    - Class: rviz/SetGoal
      Topic: /move_base_simple/goal
    - Class: rviz/PublishPoint
      Single click: true
      Topic: /clicked_point
  Value: true
  Views:
    Current:
      Class: rviz/Orbit
      Distance: 12.403410911560059
      Enable Stereo Rendering:
        Stereo Eye Separation: 0.05999999865889549
        Stereo Focal Distance: 1
        Swap Stereo Eyes: false
        Value: false
      Focal Point:
        X: 3.8209285736083984
        Y: 4.755727291107178
        Z: 0.14359386265277863
      Focal Shape Fixed Size: true
      Focal Shape Size: 0.05000000074505806
      Invert Z Axis: false
      Name: Current View
      Near Clip Distance: 0.009999999776482582
      Pitch: 1.5697963237762451
      Target Frame: <Fixed Frame>
      Value: Orbit (rviz)
      Yaw: 4.708594799041748
    Saved: ~
Window Geometry:
  Displays:
    collapsed: false
  Height: 1004
  Hide Left Dock: false
  Hide Right Dock: true
  QMainWindow State: 000000ff00000000fd00000004000000000000015600000352fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000352000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000352fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b00000352000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004680000003efc0100000002fb0000000800540069006d00650100000000000004680000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000030c0000035200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
  Selection:
    collapsed: false
  Time:
    collapsed: false
  Tool Properties:
    collapsed: false
  Views:
    collapsed: true
  Width: 1128
  X: 178
  Y: 0


================================================
FILE: map/src/map/grid.cpp
================================================
#include "map/grid.hpp"

namespace map
{
	using rigid2d::Vector2D;

	Cell::Cell(const Vector2D & coords_, const double & resolution_)
	{
		coords = coords_;

		double offset = resolution_ / 2.0;

		center_coords = Vector2D(coords.x + offset, coords.y + offset);
		resolution = resolution_;
	}

	void Grid::build_map(const double & resolution)
	{
		// Step 1. divide grid into cells based on resolution and store in class members
		xcells = arange<double>(map_min.x, map_max.x, resolution);
		ycells = arange<double>(map_min.y, map_max.y, resolution);

		// Step 2. Populate Cell vector (cells) with world coordinates and labels in row-major-order
		for (int i = 0; i < static_cast<int>(ycells.size()); i++)
		{
			for (int j = 0; j < static_cast<int>(xcells.size()); j++)
			{
				// std::cout << "[" << j << ", " << i << "]" << std::endl;
				// For each cell to be added, convert the grid index to world coords
				Cell cell(Vector2D(xcells.at(j), ycells.at(i)), resolution);

				// Perform inside-obstacle check for labeling Obstacle
				if (!not_inside(Vertex(cell.center_coords), obstacles, 0.0))
				{
					// std::cout << "OCCUPIED: [" << cell.center_coords.x << ", " << cell.center_coords.y << "]" << std::endl;
					cell.celltype = Occupied;
				} else if (!not_inside(Vertex(cell.center_coords), obstacles, inflate_robot))
				// Perform close-to-obstacle check for labeling Inflation
				{
					cell.celltype = Inflation;
					// std::cout << "INFLATED: [" << cell.center_coords.x << ", " << cell.center_coords.y << "]" << std::endl;
				}
				cells.push_back(cell);
			}
		}

		// Populate more cell information such as row-major order and grid index
		for(unsigned int i = 0; i < cells.size(); i++)
		{
			// Convert from row-major-order to grid coordinates
			auto index = rowmajor2grid(i, static_cast<int>(xcells.size()));
			// Convert back to rmj
			int rmj = grid2rowmajor(index.x, index.y, static_cast<int>(xcells.size()));
			index.row_major = rmj;

			if (!(rmj == static_cast<int>(i)))
			{
				throw std::runtime_error("Row-Major Order Conversion Failed!\
										 \n  where(): Grid::build_map(const double & resolution)");
			}

			cells.at(i).index = index;

			// Set fake_grid to same cells but all free
			Cell fake_cell = cells.at(i);
			fake_cell.celltype = Free;
			fake_grid.push_back(fake_cell);
		}
	}

	Index Grid::world2grid(const Cell & cell) const
	{
		// Get x coordinate
		int x_index = -1;
		for (int i = 0; i < static_cast<int>(xcells.size()); i++)
		{
			if (cell.center_coords.x >= xcells.at(i) and cell.center_coords.x < xcells.at(i) + cell.resolution)
			{
				x_index = i;
			}
		}

		// Get x coordinate
		int y_index = -1;
		for (int j = 0; j < static_cast<int>(ycells.size()); j++)
		{
			if (cell.center_coords.y >= ycells.at(j) and cell.center_coords.y < ycells.at(j) + cell.resolution)
			{
				y_index = j;
			}
		}

		if (x_index == -1 or y_index == -1)
		{
			throw std::runtime_error("Could not convert from world to grid coordinates!");
		}

		Index index;
		index.x = x_index;
		index.y = y_index;
		index.row_major = grid2rowmajor(index.x, index.y, static_cast<int>(xcells.size()));

		return index;
	}

	Vector2D Grid::grid2world(const int & i, const int & j, const double & resolution) const
	{
		if (!(i >= 0 and i <= static_cast<int>(xcells.size()) - 1))
		{
			throw std::invalid_argument("cell's x coordinate out of bounds!");
		} else if (!(j >= 0 and j <= static_cast<int>(ycells.size()) - 1))
		{
			throw std::invalid_argument("cell's y coordinate out of bounds!");
		}
		Vector2D coord;
		coord.x = i * resolution + map_min.x;
		coord.y = j * resolution + map_min.y;
		return coord;
	}

	std::vector<Cell> Grid::return_grid() const
	{
		return cells;
	}

	void Grid::occupancy_grid(std::vector<int8_t> & map) const
	{
		map.resize(cells.size());

		for(unsigned int i = 0; i < cells.size(); i++)
		{
			// For each cell type, assign a value to map
			if (cells.at(i).celltype == Free)
			{
				map.at(i) = 0;
			} else if (cells.at(i).celltype == Inflation)
			{
				map.at(i) = 50;
			} else if (cells.at(i).celltype == Occupied)
			{
				map.at(i) = 100;
			}
		}
	}

	std::vector<int> Grid::return_grid_dimensions() const
	{
		std::vector<int> v;
		v.push_back(static_cast<int>(xcells.size()));
		v.push_back(static_cast<int>(ycells.size()));
		return v;
	}


	void Grid::update_grid(const Cell & cc, const int & visibility)
	{

		std::vector<Cell> cells_to_update = get_neighbours(cc, cells, visibility);

		for (auto iter = cells_to_update.begin(); iter < cells_to_update.end(); iter++)
		{
			if (fake_grid.at(iter->index.row_major).celltype != iter->celltype)
			{
				// std::cout << "NEW OCCUPANCY: " << iter->index.row_major << std::endl;
				fake_grid.at(iter->index.row_major).celltype = iter->celltype;
				fake_grid.at(iter->index.row_major).newView = true;
			} else
			{
				fake_grid.at(iter->index.row_major).newView = false;
			}
		}
	}


	std::vector<Cell> Grid::return_fake_grid() const
	{
		return fake_grid;
	}


	void Grid::fake_occupancy_grid(std::vector<int8_t> & map) const
	{
		map.resize(fake_grid.size());

		for(unsigned int i = 0; i < fake_grid.size(); i++)
		{
			// For each cell type, assign a value to map
			if (fake_grid.at(i).celltype == Free)
			{
				map.at(i) = 0;
			} else if (fake_grid.at(i).celltype == Inflation)
			{
				map.at(i) = 50;
			} else if (fake_grid.at(i).celltype == Occupied)
			{
				map.at(i) = 100;
			}
		}
	}

	std::vector<Cell> Grid::get_neighbours(const Cell & cc, const std::vector<Cell> & map, const int & visibility)
	{
		int lower_bound = - visibility;
		int upper_bound = - lower_bound;
		int x_max = map.back().index.x;
		int y_max = map.back().index.y; 
		std::vector<Cell> neighbours;

		// Evaluate about block. Default is 1 -> 3x3 for 8-connectivity
		for (int x = lower_bound; x <= upper_bound; x++)
		{
			for (int y = lower_bound; y <= upper_bound; y++)
			{
				// Skip x,y = (0,0) since that's the current node
				if (x == 0 and y == 0)
				{
					continue;
				} else
				{
					int check_x = cc.index.x + x;
					int check_y = cc.index.y + y;

					// Ensure potential neighbour is within grid bounds
					if (check_x >= 0 and check_x <= x_max and check_y >= 0 and check_y <= y_max)
					{
						// Now we need to grab the right cell from the map. To do this: index->RMJ
						// std::cout << "Neighbour at [" << check_x << ", " << check_y << "]" << std::endl;
						int rmj = map::grid2rowmajor(check_x, check_y, x_max + 1);
						Cell nbr = map.at(rmj);
						neighbours.push_back(nbr);
					}
				}
			}
		}

		return neighbours;
	}



	Index rowmajor2grid(const int & rmj, const int & numcol)
    {
        Index index;
        // NOTE: RViz uses x=col, y=row
        index.y = rmj / numcol;
        index.x = rmj - (index.y * numcol);

        return index;
    }

    int grid2rowmajor(const int & x, const int & y, const int & numcol)
    {
        // NOTE: RViz uses x=col, y=row
        // So numcol is the number of x squares in each y row so it's x.size()
        // index = y * num_row + x
        // EX: to get RMJ index 3 we do: 3 = y * num_col + x
        //                            => 3 = 1 * 3 + 0
        /**
        Y|(0,3)=9|(1,3)=10|(2,3)=11|
        Y|(0,2)=6|(1,2)= 7|(2,2)= 8|
        Y|(0,1)=3|(1,1)= 4|(2,1)= 5|
        Y|(0,0)=0|(1,0)= 1|(2,0)= 2|
             X      X       X
        **/
        return x + y * numcol;
    }
}

================================================
FILE: map/src/map/map.cpp
================================================
#include "map/map.hpp"

namespace map
{
	using rigid2d::Vector2D;

	// Obstacle
	Obstacle::Obstacle()
	{
	}

	Obstacle::Obstacle(const std::vector<Vector2D> & vertices_)
	{
		vertices = vertices_;
	}

	// Map
	Map::Map()
	{
		find_map_extent();
		inflate_robot = 0.2;
	}

	Map::Map(const std::vector<Obstacle> & obstacles_)
	{
		obstacles = obstacles_;

		find_map_extent();

		inflate_robot = 0.2;
	}

	Map::Map(const std::vector<Obstacle> & obstacles_, const double inflate_robot_)
	{
		obstacles = obstacles_;

		find_map_extent();

		inflate_robot = inflate_robot_;
	}

	std::vector<Obstacle> Map::return_obstacles()
	{
		return obstacles;
	}

	void Map::find_map_extent()
	{
		double x_min, x_max, y_min, y_max = 0;

		for (auto obs_iter = obstacles.begin(); obs_iter != obstacles.end(); obs_iter++)
	    {

			for (auto v_iter = obs_iter->vertices.begin(); v_iter != obs_iter->vertices.end(); v_iter++)
		    {
		    	if (v_iter->x > x_max)
		    	{
		    		x_max = v_iter->x;
		    	} else if (v_iter->x < x_min)
		    	{
		    		x_min = v_iter->x;
		    	}

		    	if (v_iter->y > y_max)
		    	{
		    		y_max = v_iter->y;
		    	} else if (v_iter->y < y_min)
		    	{
		    		y_min = v_iter->y;
		    	}

		    	map_max = Vector2D(x_max, y_max);
		    	map_min = Vector2D(x_min, y_min);
		    }
		}
	}

	std::vector<Vector2D> Map::return_map_bounds()
	{
		std::vector<Vector2D> v;
		v.push_back(map_min);
		v.push_back(map_max);

		return v;
	}


	// Helper Functions
	double euclidean_distance(const double & x_rel, const double & y_rel)
	{
		return sqrt(pow(x_rel, 2) + pow(y_rel, 2));
	}

}

================================================
FILE: map/src/map/prm.cpp
================================================
#include "map/prm.hpp"
#include "nuslam/ekf.hpp"  // for random number engine

namespace map
{
	using rigid2d::Vector2D;

	// Vertex
	Vertex::Vertex(const Vector2D coords_)
	{
		coords = coords_;
	}

	bool Vertex::edge_exists(const int & check_id) const
	{
		const auto search = id_set.find(check_id);
		return (search == id_set.end()) ? false : true;
	}

	// PRM
	void PRM::build_map(const int & n, int & k, const double & thresh)
	{
		if (k > n)
		{
			std::cout << "[WARNING]: k > n. We have set k = n." << std::endl;
			k = n;
		}
		// Following PRM Algorithm from Lavalle - Planning Algorithms

		// Steps 1-2, empty Vertex and Edge List
		configurations.clear();

		// Steps 3-8: Sample Configurations
		sample_configurations(n);

		// Step 9: start loop for validity check
		for (auto q = configurations.begin(); q != configurations.end(); q++)
	    {
	    	// Step 10: get k nearest neighbours
	    	auto knn = find_knn(*q, k);

	    	// Step 11: start inner loop for validity check
	    	for (auto id_iter = knn.begin(); id_iter != knn.end(); id_iter++)
		    {
		    	// Search for Vertex of corresponding ID
		    	auto neighbor_ptr = &configurations.at(id_iter->id);

		    	// Step 12: perform validity check
		    	if (edge_valid(*q, *neighbor_ptr, thresh))
		    	{
		    		// Step 13: add neighbor ID to set of connected edges
		    		Edge qn_edge;
		    		qn_edge.next_id = neighbor_ptr->id;
		    		qn_edge.distance = euclidean_distance(q->coords.x - neighbor_ptr->coords.x,\
				   									  	  q->coords.y - neighbor_ptr->coords.y);
		    		// Add to edges and id_set
		    		q->edges.push_back(qn_edge);
		    		q->id_set.insert(qn_edge.next_id);

		    		// Also do vice versa: add q to neighbor edges
		    		Edge nq_edge;
		    		nq_edge.next_id = q->id;
		    		nq_edge.distance = qn_edge.distance;
		    		// Add to edges and id_set
		    		neighbor_ptr->edges.push_back(nq_edge);
		    		neighbor_ptr->id_set.insert(nq_edge.next_id);
		    	}
		    }
	    }
	}

	void PRM::sample_configurations(const int & n)
	{
		// MAP EXTENT
		// std::cout << "Map Extent: (" << map_max.x << ", " << map_max.y << ")" << std::endl;
		int kill_counter = 0;
		while (static_cast<int>(configurations.size()) < n or kill_counter > 1000 * n)
		{
			// Sample random number with assigned limits
			std::uniform_real_distribution<double> dx(map_min.x, map_max.x);
			std::uniform_real_distribution<double> dy(map_min.y, map_max.y);
			// Random Number Generator defined in nuslam package
			double sample_x = dx(nuslam::get_random());
			double sample_y = dy(nuslam::get_random());

			Vector2D coords(sample_x, sample_y);

			Vertex q(coords);

			// ID = position in vector. eg: (0 elements) -> ID = 0 (first one)
			q.id = static_cast<int>(configurations.size());

			// Ensure to free-space collison
			if (not_inside(q, obstacles, inflate_robot))
			{
				// KEY, OBJECT
				configurations.push_back(q);
			} else {
				// Increment kill counter if there's a collision
				kill_counter++;
			}
		}
	}

	std::vector<Vertex> PRM::find_knn(Vertex & q, const int & k)
	{
		// First, order set in ascending distance from q order
		// copy into vector to use sort fcn
		std::vector<Vertex> configs;
		configs.reserve(configurations.size());
		for (auto q_iter = configurations.begin(); q_iter != configurations.end(); q_iter++)
	    {
	    	configs.push_back(*q_iter);
	    }
		// using lambda for sort criterion. put q in [q] so it can be accessed
		sort(configs.begin(), configs.end(), [q](const Vertex& lhs, const Vertex& rhs)
		{
	      return euclidean_distance(q.coords.x - lhs.coords.x, q.coords.y - lhs.coords.y)\
	      < euclidean_distance(q.coords.x - rhs.coords.x, q.coords.y - rhs.coords.y)\
	      and lhs.id != q.id and rhs.id != q.id;
		});

		// Now, assign IDs of first k elements
		std::vector<Vertex> knn;
		for (int i = 0; i < k; i++)
		{
			knn.push_back(configs.at(i));
		}

		return knn;
	}

	bool PRM::edge_valid(const Vertex & q, const Vertex & q_prime, const double & thresh)
	{
		// Check if New Edge
		if (q.edge_exists(q_prime.id))
		{
			return false;
		} else if (euclidean_distance(q.coords.x - q_prime.coords.x,\
				   q.coords.y - q_prime.coords.y) < thresh)
		// Check Distance Above Useful Threshold
		{
			return false;
		} else if (!no_collision(q, q_prime, inflate_robot))
		// Check if Inflated Robot Intersects Polygon Edge or Path Edge intersects Polygon
		{
			return false;
		} else 
		{
			return true;
		}
	}

	bool PRM::no_collision(const Vertex & q, const Vertex & q_prime, const double & inflate_robot)
	{
		bool free = true;
		// Loop over all obstacles
		for (auto obs_iter = obstacles.begin(); obs_iter != obstacles.end(); obs_iter++)
		{
			// int i = static_cast<int>(std::distance(obstacles.begin(), obs_iter));
			// std::cout << "Obstacle #" << i << std::endl;
			if (!no_intersect(q, q_prime, obs_iter))
				// Collision! Not a valid Edge, exit here.
			{
				return false;
			} else {
				// Check if inflated robot intersects edge
				for (auto v_iter = obs_iter->vertices.begin(); v_iter != obs_iter->vertices.end(); v_iter++)
				{
					// If one Vertex is to close to an Edge, return false and Exit. Otherwise keep checking.
					if (too_close(q, q_prime, Vertex(*v_iter), inflate_robot))
					{
						return false;
					}
				}
			}
		}
		return free;
	}

	bool no_intersect(const Vertex & q, const Vertex & q_prime, const std::vector<Obstacle>::iterator & obs_iter)
	{
		// Initialize tE/tL: the max/min entering/leaving segment parameters.
		// NOTE: these evolve over time for each obstacle.
		double tE = 0.0;
		double tL = 1.0;
		// Initialize dS: P0->P1 vector 
		Eigen::Vector2d dS(q_prime.coords.x - q.coords.x, q_prime.coords.y - q.coords.y);
		// Loop over all vertices and treat them as directional Edge vectors.
		for (auto v_iter = obs_iter->vertices.begin(); v_iter != obs_iter->vertices.end(); v_iter++)
		{
			// Vector from current and next vertex. so if 4 vertices: 0->1, 1->2, 2->3, 3->0
			// Record current index
			int i = static_cast<int>(std::distance(obs_iter->vertices.begin(), v_iter));
			// If current index is the last one, loop back to zero for vector construction
			// -1 because indeces start at 0
			if (i == static_cast<int>(obs_iter->vertices.size()) - 1)
			{
				i = 0;
			} else
			// Otherwise, just use the next index
			{
				i += 1;
			}

			// Referencing: https://drive.google.com/file/d/1gQuR4J80aXZ9BBL1s3K83TmxQSWD3AWt/view?usp=sharing Slide 28
			auto Vi = *v_iter;
			auto Vip1 = obs_iter->vertices.at(i);
			// edge i is Vi->Vi+1
			Eigen::Vector2d ei(Vip1.x - Vi.x, Vip1.y - Vi.y);

			// ni is the outward normal of the edge ei
			// NOTE: if we have {x,y} --> {y, -x} = RHS perp. (outward normal) | {-y, x} = LHS perp. (inward normal)
			// outward normal
			Eigen::Vector2d ni(Vip1.y - Vi.y, -(Vip1.x - Vi.x));
			if (!rigid2d::almost_equal(ei.dot(ni), 0.0))
			{
				throw std::runtime_error("ni is NOT orthogonal to ei!\
										 \n  where(): PRM::no_collision(const Vertex &q, const Vertex & q_prime)");
			}
			double N = - ni.dot(Eigen::Vector2d(q.coords.x - Vi.x, q.coords.y - Vi.y));
			double D = ni.dot(dS);

			if (rigid2d::almost_equal(D, 0.0))
			{
				// Then q->q' is parallel to edge ei. Double condition to catch numerical errors
				if (N < 0.0 and !(rigid2d::almost_equal(N, 0.0)))
				{
					// Then q is outside of polygon Obstacle.
					return true; // Intersection not possible. Exit here.
				} else {
					// q->q' cannot enter/leave across edge ei. Process next edge
					continue;
				}
			}
			// Intersection time
			double t = N / D;

			if (D < 0.0)
			{
				// Then segment q->q' enters across edge ei
				tE = std::max(tE, t);
				if (tE > tL)
				{
					// Then the segment enters AFTER leaving. Intersection not possible on this edge.
					return true;
				}

			} else {
				// D > 0 in this case
				// Then segment q->q' leaves across edge ei
				tL = std::min(tL, t);
				if (tL < tE)
				{
					// Then the segment leaves BEFORE entering. Intersection not possible on this edge.
					return true;
				}
			}
		}
		// tE <= tL if we have not exited at an earlier edge.
		// There is a valid intersection. Exit here. No need to check additional obstacles.
		// Entering Point: P(tE) = q + tE * dS
		// Leaving Point: P(tL) =  q + tL * dS
		return false;
	}

	bool not_inside(const Vertex & q, const std::vector<Obstacle> & obstacles, const double & inflate_robot)
	{
		bool free = true;
		// Loop over all obstacles
		for (auto obs_iter = obstacles.begin(); obs_iter != obstacles.end(); obs_iter++)
		{
			// Loop over all vertices and treat them as directional vectors.
			// if q is ON ANY edge, then it is on an obstacle
			// If q is on the left side of ALL edges, then it is inside an obstacle
			bool on_all_left = true;

			for (auto v_iter = obs_iter->vertices.begin(); v_iter != obs_iter->vertices.end(); v_iter++)
			{
				// Vector from current and next vertex. so if 4 vertices: 0->1, 1->2, 2->3, 3->0
				// Record current index
				int i = static_cast<int>(std::distance(obs_iter->vertices.begin(), v_iter));
				// If current index is the last one, loop back to zero for vector construction
				// -1 because indeces start at 0
				if (i == static_cast<int>(obs_iter->vertices.size()) - 1)
				{
					i = 0;
				} else
				// Otherwise, just use the next index
				{
					i += 1;
				}

				// Referencing: https://drive.google.com/file/d/1gQuR4J80aXZ9BBL1s3K83TmxQSWD3AWt/view?usp=sharing Slide 32
				// v_iter is A
				// obs_iter->vertices.at(i) is B
				// DIRECTION is A->B
				// q is P
				auto A = *v_iter;
				auto B = obs_iter->vertices.at(i);
				auto P = q;

				// see lineToPoint: determine whether point is on line segment, find closest point on line,
				// and determine signed distance
				auto shrt = lineToPoint(Vertex(A), Vertex(B), Vertex(P.coords));

				if (shrt.D < 0.0)
					// P is on the right side of at least one edge, not necessarily on obstacle
					// Only way this can be an obstacle now is if it's ON and edge
				{
					// if P is within the segment window, check whether it is in the inflation bound
					if ((shrt.u >= 0.0 and shrt.u <= 1.0) or rigid2d::almost_equal(shrt.u, 0.0)\
														  or rigid2d::almost_equal(shrt.u, 1.0))
					{
						if (shrt.D < - inflate_robot)
						{
							on_all_left = false;
						}
					} else
					// P is not within the segment window, so check if distance to closest segment vertex is above
					// inflation bound
					{
						if (shrt.u > 1.0)
						// CLOSEST TO Vertex 2
						{
							auto dist = euclidean_distance(B.x - P.coords.x, B.y - P.coords.y);
							if (dist > inflate_robot)
							{
								on_all_left = false;
							} else {
								return false;
							}

						} else if (shrt.u < 0.0)
						// CLOSEST TO VERTEX 1
						{
							auto dist = euclidean_distance(A.x - P.coords.x, A.y - P.coords.y);
							if (dist > inflate_robot)
							{
								on_all_left = false;
							} else {
								return false;
							}
						}
					}
				} else if (rigid2d::almost_equal(shrt.D, 0.0) and ((shrt.u >= 0.0 and shrt.u <= 1.0)\
																	or (rigid2d::almost_equal(shrt.u, 0.0)\
																		or rigid2d::almost_equal(shrt.u, 1.0))))
					// if P is on an edge of an obstacle and within segment bounds, it is disqualified immediately
				{
					return false;
				} else if (rigid2d::almost_equal(shrt.D, 0.0))
				// EDGE CASE: zero signed distance without being on line segment
				{
					// Now we check which vertex is closest to our point.
					if (shrt.u > 1.0)
					// CLOSEST TO Vertex 2
					{
						auto dist = euclidean_distance(B.x - P.coords.x, B.y - P.coords.y);
						if (dist > inflate_robot)
						{
							on_all_left = false;
						} else {
							return false;
						}

					} else if (shrt.u < 0.0)
					// CLOSEST TO VERTEX 1
					{
						auto dist = euclidean_distance(A.x - P.coords.x, A.y - P.coords.y);
						if (dist > inflate_robot)
						{
							on_all_left = false;
						} else {
							return false;
						}
					}
				} else
				// this means shrt.D > 0.0
				{

				}
			}

			// is q is on the inside of all the edges of an obstacle, it is not free (disqualified)
			if (on_all_left)
			{
				free = false;
			}

		}

		return free;
	}

	bool too_close(const Vertex & E1, const Vertex & E2, const Vertex & P0, const double & inflate_robot)
	{
		// Referencing: https://docs.google.com/presentation/d/1gQuR4J80aXZ9BBL1s3K83TmxQSWD3AWt/edit#slide=id.g731274b3d5_0_94 Slide 33
		Eigen::Vector2d P1(E1.coords.x, E1.coords.y);
		Eigen::Vector2d P2(E2.coords.x, E2.coords.y);
		Eigen::Vector2d P3(P0.coords.x, P0.coords.y);

		auto shrt = lineToPoint(E1, E2, P0);

		if (shrt.u >= 0.0 and shrt.u <= 1.0)
		{
			// the Polygon Vertex is somwhere on the segment, so analysis check is valid
			// get distance to closest point
			Eigen::Vector2d dist(P3(0) - shrt.point.x, P3(1) - shrt.point.y);

			if (dist.norm() < inflate_robot)
			{
				// The PRM Edge is too close to the Polygon Vertex
				// std::cout << "TOO CLOSE" << std::endl;
				return true;
			}
		}

		return false;
	}

	ShortestDistance lineToPoint(const Vertex & E1, const Vertex & E2, const Vertex & P0)
	{
		// Declare struct for storage
		ShortestDistance shrt;

		// Referencing: https://docs.google.com/presentation/d/1gQuR4J80aXZ9BBL1s3K83TmxQSWD3AWt/edit#slide=id.g731274b3d5_0_94 Slide 33
		Eigen::Vector2d P1(E1.coords.x, E1.coords.y);
		Eigen::Vector2d P2(E2.coords.x, E2.coords.y);
		Eigen::Vector2d P3(P0.coords.x, P0.coords.y);

		// Step 1. determine whether point is on line segment by finding u
		shrt.u = ((P3(0) - P1(0)) * (P2(0) - P1(0)) + (P3(1) - P1(1)) * (P2(1) - P1(1)))\
					/ Eigen::Vector2d(P2(0) - P1(0), P2(1) - P1(1)).squaredNorm();
		// Step 2. Determine closest point
		shrt.point = Vector2D(P1(0) + shrt.u * (P2(0) - P1(0)), P1(1) + shrt.u * (P2(1) - P1(1)));

		// Step 3. find the signed distance between P3 and line P1->P2 (more informative than the norm)
		// u is perpendicular to P1->P2. flip xs and ys and negate one component
		// NOTE: if we have {x,y} --> {y, -x} = RHS perp. (outward normal) | {-y, x} = LHS perp. (inward normal)
		// inward normal
		Eigen::Vector2d u(-(P2(1) - P1(1)), P2(0) - P1(0));
		// dot product with P1->P2 = 0 to ensure orthogonal
		Eigen::Vector2d Segment(P2(0) - P1(0), P2(1) - P1(1));
		double Segu_test = Segment.dot(u);
		if (!rigid2d::almost_equal(Segu_test, 0.0))
		{
			throw std::runtime_error("u is NOT orthogonal to P1->P2!\
									 \n  where(): map::lineToPoint(const Vertex & E1, const Vertex & E2, const Vertex & P0, const double & inflate_robot)");
		}

		// n is the normal vector of u
		auto n = u.normalized(); // .normalize() for in place
		// D is vector P1->P3
		Eigen::Vector2d D(P3(0) - P1(0), P3(1) - P1(1));
		// Finally, D*n (dot product) gives us the minimum signed distance from P1->P2 to P
		// if d > 0, P is on the left of P1->P2, if d = 0, P is ON P1->P2, if d < 0, P is on the right of AB
		shrt.D = D.dot(n);

		return shrt;
	}

	std::vector<Vertex> PRM::return_prm()
	{
		return configurations;
	}
}

================================================
FILE: map/src/viz_grid.cpp
================================================
/// \file
/// \brief Draws Each Obstacle in RViz using MarkerArrays
///
/// PARAMETERS:
/// PUBLISHES:
/// SUBSCRIBES:
/// FUNCTIONS:

#include <ros/ros.h>
#include <ros/console.h>
#include <nav_msgs/OccupancyGrid.h>
// #include <nav_msgs/GridCells.h>
#include <geometry_msgs/Pose.h>
#include <xmlrpcpp/XmlRpcValue.h>

#include "map/grid.hpp"

using rigid2d::Vector2D;

int main(int argc, char** argv)
{
  ROS_INFO("STARTING NODE: grid_map");

  // Vars
  double frequency = 10.0;
  // resolution by which to transform marker poses (10 cm/cell so we use 10)
  double resolution = 0.01;
  std::string frame_id = "base_footprint";
  XmlRpc::XmlRpcValue xml_obstacles;

  // Grid Parameters
  double inflate = 0.1;
  double SCALE = 10.0;
  std::string map_type = "grid_map";

  // store Obstacle(s) here to create Map
  std::vector<map::Obstacle> obstacles_v;

  ros::init(argc, argv, "grid_map_node"); // register the node on ROS
  ros::NodeHandle nh; // get a handle to ROS
  ros::NodeHandle nh_("~"); // get a handle to ROS
  // Parameters
  nh_.getParam("frequency", frequency);
  nh_.getParam("obstacles", xml_obstacles);
  nh_.getParam("map_frame_id", frame_id);
  nh_.getParam("inflate", inflate);
  nh_.getParam("resolution", resolution);
  nh_.getParam("scale", SCALE);

  ros::Publisher grid_pub = nh.advertise<nav_msgs::OccupancyGrid>("grid_map", 1);

  // 'obstacles' is a triple-nested list.
  // 1st level: obstacle (Obstacle), 2nd level: vertices (std::vector), 3rd level: coordinates (Vector2D)

  // std::vector<Obstacle>
  if(xml_obstacles.getType() != XmlRpc::XmlRpcValue::TypeArray)
  {
    ROS_ERROR("There is no list of obstacles");
  } else {
    for(int i = 0; i < xml_obstacles.size(); ++i)
    {
      // Obstacle contains std::vector<Vector2D>
      // create Obstacle with empty vertex vector
      map::Obstacle obs;
      if(xml_obstacles[i].getType() != XmlRpc::XmlRpcValue::TypeArray)
      {
          ROS_ERROR("obstacles[%d] has no vertices", i);
      } else {
          for(int j = 0; j < xml_obstacles[i].size(); ++j)
          {
            // Vector2D contains x,y coords
            if(xml_obstacles[i][j].size() != 2)
            {
               ROS_ERROR("Vertex[%d] of obstacles[%d] is not a pair of coordinates", j, i);
            } else if(
              xml_obstacles[i][j][0].getType() != XmlRpc::XmlRpcValue::TypeDouble or
              xml_obstacles[i][j][1].getType() != XmlRpc::XmlRpcValue::TypeDouble)
            {
              ROS_ERROR("The coordinates of vertex[%d] of obstacles[%d] are not doubles", j, i);
            } else {
              // PASSED ALL THE TESTS: push Vector2D to vertices list in Obstacle object
              rigid2d::Vector2D vertex(xml_obstacles[i][j][0], xml_obstacles[i][j][1]);
              // NOTE: SCALE DOWN
              vertex.x /= SCALE;
              vertex.y /= SCALE;
              obs.vertices.push_back(vertex);
            }
          }
      }
      // push Obstacle object to vector of Object(s) in Map
      obstacles_v.push_back(obs);
    }
  }


  // Initialize Grid
  map::Grid grid(obstacles_v, inflate);

  // Build Map
  grid.build_map(resolution);

  ROS_INFO("Grid Built!");

  // Get map bounds
  auto bounds = grid.return_map_bounds();
  auto gridsize = grid.return_grid_dimensions();

  // rviz representation of the grid
  std::vector<int8_t> map;
  grid.occupancy_grid(map);

  // Grid Pose
  geometry_msgs::Pose map_pose;
  map_pose.position.x = bounds.at(0).x;
  map_pose.position.y = bounds.at(0).y;
  map_pose.position.z = 0.0;
  map_pose.orientation.x = 0.0;
  map_pose.orientation.y = 0.0;
  map_pose.orientation.z = 0.0;
  map_pose.orientation.w = 1.0;

  // Occupancy grid for visualization
  nav_msgs::OccupancyGrid grid_map;
  grid_map.header.frame_id = frame_id;
  grid_map.info.resolution = resolution;
  grid_map.info.width = gridsize.at(0);
  grid_map.info.height = gridsize.at(1);

  grid_map.info.origin = map_pose;

  while(ros::ok())
  {
    ros::spinOnce();
    grid_map.header.stamp = ros::Time::now();
    grid_map.info.map_load_time = ros::Time::now();
    grid_map.data = map;
    grid_pub.publish(grid_map);
  }

  return 0;
}


================================================
FILE: map/src/viz_map.cpp
================================================
/// \file
/// \brief Draws Each Obstacle in RViz using MarkerArrays
///
/// PARAMETERS:
/// PUBLISHES:
/// SUBSCRIBES:
/// FUNCTIONS:

#include <ros/ros.h>

#include <math.h>
#include <string>
#include <vector>

#include "map/map.hpp"
#include "map/prm.hpp"

#include "nuslam/TurtleMap.h"

#include <functional>  // To use std::bind
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <geometry_msgs/Point.h>

// Used to deal with YAML list of lists
#include <xmlrpcpp/XmlRpcValue.h> // catkin component


int main(int argc, char** argv)
/// The Main Function ///
{
  ROS_INFO("STARTING NODE: viz_map");

  // Vars
  double frequency = 10.0;
  // Scale by which to transform marker poses (10 cm/cell so we use 10)
  double SCALE = 10.0;
  std::string frame_id = "base_footprint";
  XmlRpc::XmlRpcValue xml_obstacles;

  // PRM Parameters
  int n = 10;
  int k = 5;
  double thresh = 0.01;
  double inflate = 0.1;
  std::string map_type = "map";

  // store Obstacle(s) here to create Map
  std::vector<map::Obstacle> obstacles_v;

  ros::init(argc, argv, "viz_map_node"); // register the node on ROS
  ros::NodeHandle nh; // get a handle to ROS
  ros::NodeHandle nh_("~"); // get a handle to ROS
  // Parameters
  nh_.getParam("frequency", frequency);
  nh_.getParam("obstacles", xml_obstacles);
  nh_.getParam("map_frame_id", frame_id);
  nh_.getParam("n", n);
  nh_.getParam("k", k);
  nh_.getParam("thresh", thresh);
  nh_.getParam("inflate", inflate);
  nh_.getParam("map_type", map_type);
  nh_.getParam("scale", SCALE);

  // 'obstacles' is a triple-nested list.
  // 1st level: obstacle (Obstacle), 2nd level: vertices (std::vector), 3rd level: coordinates (Vector2D)

  // std::vector<Obstacle>
  if(xml_obstacles.getType() != XmlRpc::XmlRpcValue::TypeArray)
  {
    ROS_ERROR("There is no list of obstacles");
  } else {
    for(int i = 0; i < xml_obstacles.size(); ++i)
    {
      // Obstacle contains std::vector<Vector2D>
      // create Obstacle with empty vertex vector
      map::Obstacle obs;
      if(xml_obstacles[i].getType() != XmlRpc::XmlRpcValue::TypeArray)
      {
          ROS_ERROR("obstacles[%d] has no vertices", i);
      } else {
          for(int j = 0; j < xml_obstacles[i].size(); ++j)
          {
            // Vector2D contains x,y coords
            if(xml_obstacles[i][j].size() != 2)
            {
               ROS_ERROR("Vertex[%d] of obstacles[%d] is not a pair of coordinates", j, i);
            } else if(
              xml_obstacles[i][j][0].getType() != XmlRpc::XmlRpcValue::TypeDouble or
              xml_obstacles[i][j][1].getType() != XmlRpc::XmlRpcValue::TypeDouble)
            {
              ROS_ERROR("The coordinates of vertex[%d] of obstacles[%d] are not doubles", j, i);
            } else {
              // PASSED ALL THE TESTS: push Vector2D to vertices list in Obstacle object
              rigid2d::Vector2D vertex(xml_obstacles[i][j][0], xml_obstacles[i][j][1]);
              // NOTE: SCALE DOWN
              vertex.x /= SCALE;
              vertex.y /= SCALE;
              obs.vertices.push_back(vertex);
            }
          }
      }
      // push Obstacle object to vector of Object(s) in Map
      obstacles_v.push_back(obs);
    }
  }

  // Initialize Marker
  // Init Marker Array Publisher
  ros::Publisher map_pub = nh.advertise<visualization_msgs::MarkerArray>("map", 1);

  // Init Marker Array
  visualization_msgs::MarkerArray map_arr;

  // Init Marker
  visualization_msgs::Marker marker;
  marker.header.frame_id = frame_id;
  marker.header.stamp = ros::Time::now();
  // marker.ns = "my_namespace";
  // marker.id = 0;
  marker.type = visualization_msgs::Marker::LINE_STRIP;
  marker.action = visualization_msgs::Marker::ADD;
  marker.pose.position.z = 0;
  marker.pose.orientation.x = 0.0;
  marker.pose.orientation.y = 0.0;
  marker.pose.orientation.z = 0.0;
  marker.pose.orientation.w = 1.0;
  marker.scale.x = 0.02;
  marker.color.r = 0.5f;
  marker.color.g = 0.0f;
  marker.color.b = 0.5f;
  marker.color.a = 1.0;
  marker.lifetime = ros::Duration();

  visualization_msgs::Marker sph_mkr;
  sph_mkr = marker;
  sph_mkr.type = visualization_msgs::Marker::SPHERE;
  sph_mkr.scale.x = 0.02;
  sph_mkr.scale.y = 0.02;
  sph_mkr.scale.z = 0.02;

  // Color based on map_type
  if (map_type == "map")
  {
    marker.color.r = 0.5f;
    marker.color.g = 0.0f;
    marker.color.b = 0.5f;
    sph_mkr.color.r = 0.96f;
    sph_mkr.color.g = 0.475f;
    sph_mkr.color.b = 0.0f;
  } else if (map_type == "prm")
  {
    marker.color.r = 0.96f;
    marker.color.g = 0.475f;
    marker.color.b = 0.0f;
    sph_mkr.color.r = 0.5f;
    sph_mkr.color.g = 0.0f;
    sph_mkr.color.b = 0.5f;
  }

  // LINE_STRIP relative to this pose
  marker.pose.position.x = 0.0;
  marker.pose.position.y = 0.0;

  int marker_id = 0;

  if (map_type == "map")
  {
    // DRAW MAP
    map::Map map(obstacles_v);
    // Now, loop through obstacles in map to create line strips.
    // Each obstacle = 1 line strip
    int marker_id = 0;
    for (auto obs_iter = obstacles_v.begin(); obs_iter != obstacles_v.end(); obs_iter++)
    {
      marker.points.clear();
      marker.id = marker_id;
      marker_id++;
      ROS_DEBUG("obstacle [%d] starts at (%.2f, %.2f)", marker.id + 1, obs_iter->vertices.at(0).x / SCALE, obs_iter->vertices.at(0).y / SCALE);
      for (auto v_iter = obs_iter->vertices.begin(); v_iter != obs_iter->vertices.end(); v_iter++)
      {
        geometry_msgs::Point new_vertex;
        new_vertex.x = v_iter->x;
        new_vertex.y = v_iter->y;
        new_vertex.z = 0.0;
        marker.points.push_back(new_vertex);
        
        // Also push back cylinders
        sph_mkr.pose.position.x = v_iter->x;
        sph_mkr.pose.position.y = v_iter->y;
        sph_mkr.id = marker_id;
        marker_id++;
        map_arr.markers.push_back(sph_mkr);
      }
      // Before pushing back, connect last vertex to first vertex
      geometry_msgs::Point new_vertex;
      new_vertex.x = obs_iter->vertices.at(0).x;
      new_vertex.y = obs_iter->vertices.at(0).y;
      new_vertex.z = 0.0;
      marker.points.push_back(new_vertex);
      map_arr.markers.push_back(marker);
    }

    ROS_INFO("Obstacle Map Built!");
  } else if (map_type == "prm")
  {
    // Build PRM
    map::PRM prm(obstacles_v, inflate);
    prm.build_map(n, k, thresh);
    // DRAW PRM
    auto configurations = prm.return_prm();
    for (auto node_iter = configurations.begin(); node_iter != configurations.end(); node_iter++)
    {
      marker.points.clear();
      marker.id = marker_id;
      marker_id++;

      // Check if a node has edges before plotting
      if (node_iter->id_set.size() > 0)
      {
        for (auto id_iter = node_iter->id_set.begin(); id_iter != node_iter->id_set.end(); id_iter++)
        {
          // Add node as first marker vertex
          geometry_msgs::Point first_vertex;
          first_vertex.x = node_iter->coords.x;
          first_vertex.y = node_iter->coords.y;
          first_vertex.z = 0.0;
          marker.points.push_back(first_vertex);

          // Find Vertex for each ID
          auto neighbor_iter = configurations.at(*id_iter);

          geometry_msgs::Point new_vertex;
          new_vertex.x = neighbor_iter.coords.x;
          new_vertex.y = neighbor_iter.coords.y;
          new_vertex.z = 0.0;
          marker.points.push_back(new_vertex);

          // Also push back cylinders
          sph_mkr.pose.position.x = node_iter->coords.x;
          sph_mkr.pose.position.y = node_iter->coords.y;
          sph_mkr.id = marker_id;
          marker_id++;
          map_arr.markers.push_back(sph_mkr);
        }
        // Push to Marker Array
        map_arr.markers.push_back(marker);
      }
    }
    ROS_INFO("PRM Built!");
  }

  ros::Rate rate(frequency);

  // Main While
  while (ros::ok())
  {
  	ros::spinOnce();

    // Publish Map
    map_pub.publish(map_arr);

    rate.sleep();
  }

  return 0;
}

================================================
FILE: nuturtle.rosinstall
================================================
- git:
    local-name: tb3
    uri: git@github.com:moribots/turtlebot3_from_scratch.git
- git:
    local-name: world
    uri: https://github.com/ME495-Navigation/nuturtlebot.git