Showing preview only (285K chars total). Download the full file or copy to clipboard to get everything.
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 "glo
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
SYMBOL INDEX (95 symbols across 30 files)
FILE: control/include/control/Models.hpp
type control (line 11) | namespace control {
type models (line 12) | namespace models {
class DiffDrive (line 22) | class DiffDrive {
method DiffDrive (line 38) | DiffDrive(const double& wheel_radius, const double& wheel_base,
method num_variables (line 56) | int num_variables(const int& mpcWindow) {
method num_constraints (line 65) | int num_constraints(const int& mpcWindow) {
method VectorXd (line 81) | VectorXd& gradient() { return m_gradient; }
method MatrixXd (line 88) | MatrixXd& linear_constraint_matrix() { return m_linear_constraint_...
method VectorXd (line 95) | VectorXd& lower_bound() { return m_lower_bound; }
method VectorXd (line 102) | VectorXd& upper_bound() { return m_upper_bound; }
method MatrixXd (line 109) | MatrixXd a() { return m_a; }
method MatrixXd (line 117) | MatrixXd b(const MatrixXd& state) {
FILE: control/include/control/TrajMPC.hpp
type control (line 13) | namespace control {
class TrajMPC (line 49) | class TrajMPC {
method TrajMPC (line 57) | TrajMPC(const Model& model, const int& mpcWindow,
method setup (line 72) | size_t setup(const Problem& problem) {
method solve (line 134) | size_t solve(const Problem& problem, Action* action) {
FILE: control/include/control/rk4.hpp
type control (line 10) | namespace control
class RK4 (line 19) | class RK4
FILE: control/include/control/utilities.hpp
type control (line 11) | namespace control {
type utilities (line 12) | namespace utilities {
function castMPCToQPHessian (line 22) | void castMPCToQPHessian(const DiagonalMatrix<double, Q_rows> &Q,
function castMPCToQPGradient (line 47) | void castMPCToQPGradient(
function castMPCToQPConstraintMatrix (line 67) | void castMPCToQPConstraintMatrix(
function castMPCToQPConstraintVectors (line 117) | void castMPCToQPConstraintVectors(const Eigen::Matrix<double, x_rows...
function updateConstraintVectors (line 166) | void updateConstraintVectors(const Eigen::Matrix<double, rows, 1> &x0,
function getErrorNorm (line 176) | double getErrorNorm(const Eigen::Matrix<double, x_rows, 1> &x,
FILE: control/src/control/Models.cpp
type control (line 5) | namespace control {}
FILE: control/src/control/TrajMPC.cpp
type control (line 5) | namespace control {}
FILE: control/src/control/rk4.cpp
type control (line 7) | namespace control
function MatrixXd (line 27) | MatrixXd RK4::solve(const Ref<VectorXd> x0, const double & horizon)
function MatrixXd (line 56) | MatrixXd RK4::solve(const Ref<VectorXd> x0, const Ref<MatrixXd> u, con...
FILE: control/src/control/utilities.cpp
type control (line 5) | namespace control {
type utilities (line 6) | namespace utilities {}
FILE: control/src/osqp_eigen_example.cpp
function setDynamicsMatrices (line 10) | void setDynamicsMatrices(Eigen::Matrix<double, 12, 12> &a,
function setInequalityConstraints (line 29) | void setInequalityConstraints(Eigen::Matrix<double, 12, 1> &xMax,
function setWeightMatrices (line 51) | void setWeightMatrices(Eigen::DiagonalMatrix<double, 12> &Q,
function main (line 57) | int main(int argc, char **argv) {
FILE: control/src/osqp_example.cpp
function main (line 10) | int main(int argc, char **argv) {
FILE: global_planner/include/global_planner/global_planner.hpp
type global (line 13) | namespace global
class GlobalPlanner (line 23) | class GlobalPlanner
FILE: global_planner/include/global_planner/heuristic.hpp
type global (line 10) | namespace global
type Node (line 20) | struct Node
class HeapComparator (line 56) | class HeapComparator
class Astar (line 72) | class Astar : public GlobalPlanner
class Thetastar (line 130) | class Thetastar : public Astar
FILE: global_planner/include/global_planner/incremental.hpp
type global (line 8) | namespace global
class KeyComparator (line 18) | class KeyComparator
class CostComparator (line 35) | class CostComparator
class LPAstar (line 45) | class LPAstar : public Astar
class DSL (line 125) | class DSL: public LPAstar
FILE: global_planner/include/global_planner/potential_field.hpp
type global (line 7) | namespace global
class PotentialField (line 17) | class PotentialField : public GlobalPlanner
FILE: global_planner/src/astar.cpp
function main (line 32) | int main(int argc, char** argv)
FILE: global_planner/src/dsl.cpp
function main (line 32) | int main(int argc, char** argv)
FILE: global_planner/src/global_planner/global_planner.cpp
type global (line 3) | namespace global
FILE: global_planner/src/global_planner/heuristic.cpp
type global (line 3) | namespace global
function Vertex (line 626) | Vertex find_nearest_node(const Vector2D & position, const std::vector<...
function Cell (line 645) | Cell find_nearest_node(const Vector2D & position, const Grid & grid, c...
FILE: global_planner/src/global_planner/incremental.cpp
type global (line 3) | namespace global
FILE: global_planner/src/global_planner/potential_field.cpp
type global (line 3) | namespace global
function Vector2D (line 19) | Vector2D PotentialField::AttractiveGradient(const Vector2D & cur_pos, ...
function Vector2D (line 40) | Vector2D PotentialField::RepulsiveGradient(const Vector2D & cur_pos, c...
function Vector2D (line 64) | Vector2D PotentialField::FindClosestPoint(const Vector2D & cur_pos, co...
function Vector2D (line 144) | Vector2D PotentialField::OneStepGD(const Vector2D & cur_pos, const Vec...
FILE: global_planner/src/lpastar.cpp
function main (line 32) | int main(int argc, char** argv)
FILE: global_planner/src/pf.cpp
function main (line 33) | int main(int argc, char** argv)
FILE: map/include/map/grid.hpp
type map (line 8) | namespace map
type Index (line 13) | struct Index{
type CellType (line 20) | enum CellType {Occupied, Inflation, Free}
type Cell (line 24) | struct Cell{
class Grid (line 46) | class Grid : public Map
function arange (line 120) | std::vector<T> arange(const T & start, const T & stop, const T & step ...
FILE: map/include/map/map.hpp
type map (line 10) | namespace map
type Obstacle (line 15) | struct Obstacle
class Map (line 28) | class Map
FILE: map/include/map/prm.hpp
type map (line 9) | namespace map
type Edge (line 13) | struct Edge
type Vertex (line 22) | struct Vertex
type ShortestDistance (line 48) | struct ShortestDistance
class PRM (line 57) | class PRM : public Map
FILE: map/src/map/grid.cpp
type map (line 3) | namespace map
function Index (line 71) | Index Grid::world2grid(const Cell & cell) const
function Vector2D (line 106) | Vector2D Grid::grid2world(const int & i, const int & j, const double &...
function Index (line 241) | Index rowmajor2grid(const int & rmj, const int & numcol)
function grid2rowmajor (line 251) | int grid2rowmajor(const int & x, const int & y, const int & numcol)
FILE: map/src/map/map.cpp
type map (line 3) | namespace map
function euclidean_distance (line 89) | double euclidean_distance(const double & x_rel, const double & y_rel)
FILE: map/src/map/prm.cpp
type map (line 4) | namespace map
function no_intersect (line 181) | bool no_intersect(const Vertex & q, const Vertex & q_prime, const std:...
function not_inside (line 267) | bool not_inside(const Vertex & q, const std::vector<Obstacle> & obstac...
function too_close (line 396) | bool too_close(const Vertex & E1, const Vertex & E2, const Vertex & P0...
function ShortestDistance (line 422) | ShortestDistance lineToPoint(const Vertex & E1, const Vertex & E2, con...
FILE: map/src/viz_grid.cpp
function main (line 20) | int main(int argc, char** argv)
FILE: map/src/viz_map.cpp
function main (line 29) | int main(int argc, char** argv)
Condensed preview — 53 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (294K chars).
[
{
"path": "README.md",
"chars": 2726,
"preview": "## Motion Planning Library with ROS\n\nSelf-directed independent study.\n\n### Installation Guide:\n- Make a ROS Workspace: `"
},
{
"path": "control/CMakeLists.txt",
"chars": 8622,
"preview": "cmake_minimum_required(VERSION 3.0.2)\nproject(control)\n\n## Compile as C++11, supported in ROS Kinetic and newer\nadd_comp"
},
{
"path": "control/cmake/FindOsqpEigen.cmake",
"chars": 1096,
"preview": "#.rst:\n# FindOsqpEigen\n# -----------\n#\n# Try to find the OsqpEigen library.\n# Once done this will define the following v"
},
{
"path": "control/config/parallel.yaml",
"chars": 13,
"preview": "waypoints: []"
},
{
"path": "control/config/waypoints.yaml",
"chars": 51,
"preview": "waypoints: [[1, 0], [2, 1], [1, 2], [0, 2], [0, 0]]"
},
{
"path": "control/include/control/Models.hpp",
"chars": 3922,
"preview": "#pragma once\n/// \\file\n/// \\brief Dynamic Models: Differential Drive.\n\n#include <eigen3/Eigen/Dense>\n#include <functiona"
},
{
"path": "control/include/control/TrajMPC.hpp",
"chars": 7576,
"preview": "#pragma once\n/// \\file\n/// \\brief Model Predictive Control for Trajectory Following\n\n#include <OsqpEigen/OsqpEigen.h>\n\n#"
},
{
"path": "control/include/control/rk4.hpp",
"chars": 2406,
"preview": "#ifndef RK4_HPP\n#define RK4_HPP\n/// \\file\n/// \\brief 4th Order Runge Kutta method for Integration\n\n#include <iostream>\n#"
},
{
"path": "control/include/control/utilities.hpp",
"chars": 7014,
"preview": "#pragma once\n/// \\file\n/// \\brief Utilities Library for Quadratic Programming\n\n#include <eigen3/Eigen/Dense>\n#include <f"
},
{
"path": "control/launch/mppi_pentagon.launch",
"chars": 2254,
"preview": "<launch>\n\n\t\t<arg name=\"robot\" default=\"-1\" doc=\"Whether launchfile is run on turtlebot(1-5) directly (0), in simulation "
},
{
"path": "control/launch/osqp_example.launch",
"chars": 417,
"preview": "<launch>\n\n\t\t<arg name=\"eigen\" default=\"False\" doc=\"True: Call Eigen Wrapped OSQP Example\"/>\n\t<group if=\"$(arg eigen)\">\n\t"
},
{
"path": "control/msg/Waypoint.msg",
"chars": 65,
"preview": "# Current Pose\nfloat32[] current\n# Desired Pose\nfloat32[] desired"
},
{
"path": "control/package.xml",
"chars": 3965,
"preview": "<?xml version=\"1.0\"?>\n<package format=\"2\">\n <name>control</name>\n <version>0.0.1</version>\n <description>Control the "
},
{
"path": "control/src/control/Models.cpp",
"chars": 94,
"preview": "#include \"control/Models.hpp\"\n\n#include <iostream>\n\nnamespace control {} // namespace control"
},
{
"path": "control/src/control/TrajMPC.cpp",
"chars": 95,
"preview": "#include \"control/TrajMPC.hpp\"\n\n#include <iostream>\n\nnamespace control {} // namespace control"
},
{
"path": "control/src/control/rk4.cpp",
"chars": 3093,
"preview": "/// \\file\n/// \\brief 4th Order Runge Kutta method for Integration\n\n#include <iostream>\n#include \"control/rk4.hpp\"\n\nnames"
},
{
"path": "control/src/control/utilities.cpp",
"chars": 121,
"preview": "#include \"control/utilities.hpp\"\n\n#include <iostream>\n\nnamespace control {\nnamespace utilities {}\n} // namespace contro"
},
{
"path": "control/src/mppi",
"chars": 15348,
"preview": "#!/usr/bin/env python\n\nimport numpy as np\nimport matplotlib.pyplot as plt\nfrom scipy.signal import savgol_filter\nfrom ma"
},
{
"path": "control/src/osqp_eigen_example.cpp",
"chars": 6365,
"preview": "/// \\file\n/// \\brief Basic OSQP example\n\n#include <geometry_msgs/Point.h>\n#include <ros/console.h>\n#include <ros/ros.h>\n"
},
{
"path": "control/src/osqp_example.cpp",
"chars": 2383,
"preview": "/// \\file\n/// \\brief Basic OSQP example\n\n#include <geometry_msgs/Point.h>\n#include <ros/console.h>\n#include <ros/ros.h>\n"
},
{
"path": "global_planner/CMakeLists.txt",
"chars": 9355,
"preview": "cmake_minimum_required(VERSION 2.8.3)\nproject(global_planner)\n\n## Compile as C++11, supported in ROS Kinetic and newer\na"
},
{
"path": "global_planner/config/path.yaml",
"chars": 142,
"preview": "# NOTE: coordinates in cell #s, choose to convert to m/cm/mm later\n# MUST BE DOUBLES\n# Each iten is x,y\nstart: [5.0, 7."
},
{
"path": "global_planner/include/global_planner/global_planner.hpp",
"chars": 1270,
"preview": "#ifndef GLOBAL_PLANNER_INCLUDE_GUARD_HPP\n#define GLOBAL_PLANNER_INCLUDE_GUARD_HPP\n/// \\file\n/// \\brief global_planners l"
},
{
"path": "global_planner/include/global_planner/heuristic.hpp",
"chars": 7515,
"preview": "#ifndef HEURISTIC_INCLUDE_GUARD_HPP\n#define HEURISTIC_INCLUDE_GUARD_HPP\n/// \\file\n/// \\brief Heuristic search library to"
},
{
"path": "global_planner/include/global_planner/incremental.hpp",
"chars": 5545,
"preview": "#ifndef INCREMENTAL_INCLUDE_GUARD_HPP\n#define INCREMENTAL_INCLUDE_GUARD_HPP\n/// \\file\n/// \\brief Incremental search libr"
},
{
"path": "global_planner/include/global_planner/potential_field.hpp",
"chars": 3705,
"preview": "#ifndef POTENTIAL_FIELD_INCLUDE_GUARD_HPP\n#define POTENTIAL_FIELD_INCLUDE_GUARD_HPP\n/// \\file\n/// \\brief Potential Field"
},
{
"path": "global_planner/launch/astar.launch",
"chars": 2430,
"preview": "<launch>\n <!-- This launchfile loads a differential drive robot into RViz, whose parameters are set\n and can be m"
},
{
"path": "global_planner/launch/incremental.launch",
"chars": 2921,
"preview": "<launch>\n <!-- This launchfile loads a differential drive robot into RViz, whose parameters are set\n and can be m"
},
{
"path": "global_planner/launch/potential_field.launch",
"chars": 2453,
"preview": "<launch>\n <!-- This launchfile loads a differential drive robot into RViz, whose parameters are set\n and can be m"
},
{
"path": "global_planner/package.xml",
"chars": 3539,
"preview": "<?xml version=\"1.0\"?>\n<package format=\"2\">\n <name>global_planner</name>\n <version>0.0.1</version>\n <description>Heuri"
},
{
"path": "global_planner/rviz/astar.rviz",
"chars": 7682,
"preview": "Panels:\n - Class: rviz/Displays\n Help Height: 77\n Name: Displays\n Property Tree Widget:\n Expanded:\n "
},
{
"path": "global_planner/src/astar.cpp",
"chars": 14256,
"preview": "/// \\file\n/// \\brief Draws Each Obstacle in RViz using MarkerArrays\n///\n/// PARAMETERS:\n/// PUBLISHES:\n/// SUBSCRIBES:\n/"
},
{
"path": "global_planner/src/dsl.cpp",
"chars": 12297,
"preview": "/// \\file\n/// \\brief Draws Each Obstacle in RViz using MarkerArrays\n///\n/// PARAMETERS:\n/// PUBLISHES:\n/// SUBSCRIBES:\n/"
},
{
"path": "global_planner/src/global_planner/global_planner.cpp",
"chars": 151,
"preview": "#include \"global_planner/global_planner.hpp\"\n\nnamespace global\n{\n\tusing rigid2d::Vector2D;\n\n\t// Global Planner\n\tGlobalPl"
},
{
"path": "global_planner/src/global_planner/heuristic.cpp",
"chars": 20695,
"preview": "#include \"global_planner/heuristic.hpp\"\n\nnamespace global\n{\n\tNode::Node()\n\t{\n\t\tvertex = Vertex(Vector2D());\n\t\tcell = Cel"
},
{
"path": "global_planner/src/global_planner/incremental.cpp",
"chars": 16317,
"preview": "#include \"global_planner/incremental.hpp\"\n\nnamespace global\n{\n\tvoid LPAstar::ComputeShortestPath()\n\t{\n\t\tNode min;\n\t\tint "
},
{
"path": "global_planner/src/global_planner/potential_field.cpp",
"chars": 4985,
"preview": "#include \"global_planner/potential_field.hpp\"\n\nnamespace global\n{\n\tusing rigid2d::Vector2D;\n\n\t// Global Planner\n\tPotenti"
},
{
"path": "global_planner/src/lpastar.cpp",
"chars": 11284,
"preview": "/// \\file\n/// \\brief Draws Each Obstacle in RViz using MarkerArrays\n///\n/// PARAMETERS:\n/// PUBLISHES:\n/// SUBSCRIBES:\n/"
},
{
"path": "global_planner/src/pf.cpp",
"chars": 6743,
"preview": "/// \\file\n/// \\brief Draws Each Obstacle in RViz using MarkerArrays\n///\n/// PARAMETERS:\n/// PUBLISHES:\n/// SUBSCRIBES:\n/"
},
{
"path": "map/CMakeLists.txt",
"chars": 8456,
"preview": "cmake_minimum_required(VERSION 3.0)\nproject(map)\n\n## Compile as C++11, supported in ROS Kinetic and newer\nadd_compile_op"
},
{
"path": "map/config/map.yaml",
"chars": 1350,
"preview": "# NOTE: coordinates in cell #s, choose to convert to m/cm/mm later\n# Each item (11 total) in list is an obstacle. Each o"
},
{
"path": "map/include/map/grid.hpp",
"chars": 5751,
"preview": "#ifndef GRID_INCLUDE_GUARD_HPP\n#define GRID_INCLUDE_GUARD_HPP\n/// \\file\n/// \\brief GRID Library to build a Probabilistic"
},
{
"path": "map/include/map/map.hpp",
"chars": 2349,
"preview": "#ifndef MAP_INCLUDE_GUARD_HPP\n#define MAP_INCLUDE_GUARD_HPP\n/// \\file\n/// \\brief Map Library to store and display polygo"
},
{
"path": "map/include/map/prm.hpp",
"chars": 7508,
"preview": "#ifndef PRM_INCLUDE_GUARD_HPP\n#define PRM_INCLUDE_GUARD_HPP\n/// \\file\n/// \\brief PRM Library to build a Probabilistic Ro"
},
{
"path": "map/launch/viz_map.launch",
"chars": 2485,
"preview": "<launch>\n <!-- This launchfile loads a differential drive robot into RViz, whose parameters are set\n and can be m"
},
{
"path": "map/package.xml",
"chars": 3364,
"preview": "<?xml version=\"1.0\"?>\n<package format=\"2\">\n <name>map</name>\n <version>0.0.1</version>\n <description>PRM and Grid Map"
},
{
"path": "map/rviz/basic_map.rviz",
"chars": 6683,
"preview": "Panels:\n - Class: rviz/Displays\n Help Height: 77\n Name: Displays\n Property Tree Widget:\n Expanded:\n "
},
{
"path": "map/src/map/grid.cpp",
"chars": 7418,
"preview": "#include \"map/grid.hpp\"\n\nnamespace map\n{\n\tusing rigid2d::Vector2D;\n\n\tCell::Cell(const Vector2D & coords_, const double &"
},
{
"path": "map/src/map/map.cpp",
"chars": 1603,
"preview": "#include \"map/map.hpp\"\n\nnamespace map\n{\n\tusing rigid2d::Vector2D;\n\n\t// Obstacle\n\tObstacle::Obstacle()\n\t{\n\t}\n\n\tObstacle::"
},
{
"path": "map/src/map/prm.cpp",
"chars": 15090,
"preview": "#include \"map/prm.hpp\"\n#include \"nuslam/ekf.hpp\" // for random number engine\n\nnamespace map\n{\n\tusing rigid2d::Vector2D;"
},
{
"path": "map/src/viz_grid.cpp",
"chars": 4165,
"preview": "/// \\file\n/// \\brief Draws Each Obstacle in RViz using MarkerArrays\n///\n/// PARAMETERS:\n/// PUBLISHES:\n/// SUBSCRIBES:\n/"
},
{
"path": "map/src/viz_map.cpp",
"chars": 7968,
"preview": "/// \\file\n/// \\brief Draws Each Obstacle in RViz using MarkerArrays\n///\n/// PARAMETERS:\n/// PUBLISHES:\n/// SUBSCRIBES:\n/"
},
{
"path": "nuturtle.rosinstall",
"chars": 178,
"preview": "- git:\n local-name: tb3\n uri: git@github.com:moribots/turtlebot3_from_scratch.git\n- git:\n local-name: world\n "
}
]
About this extraction
This page contains the full source code of the moribots/motion_planning GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 53 files (268.8 KB), approximately 76.4k tokens, and a symbol index with 95 extracted functions, classes, methods, constants, and types. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.