[ { "path": "README.md", "content": "## Motion Planning Library with ROS\n\nSelf-directed independent study.\n\n### Installation Guide:\n- Make a ROS Workspace: `mkdir -p ws/src`\n- Go to `src` and clone my repo: `cd ws/src/` `git clone git@github.com:moribots/motion_planning.git`\n- Go back to the workspace root: `cd ..`\n- Initialize `nuturtle.rosinstall` to get my rigid2d library and other utilities: `wstool init src src/motion_planning/nuturtle.rosinstall`\n- Build the workspace: `catkin_make` and source: `source devel/setup.bash`\n\n### OSQP and OSQP-Eigen installation\n[OSQP](https://osqp.org/docs/get_started/sources.html)\n[OSQP-Eigen (C++ wrapper)](https://github.com/robotology/osqp-eigen) - Do the manual install.\n\nNotes for OSQP-Eigen:\n- Choose `/usr/local` in `cmake -DCMAKE_INSTALL_PREFIX:PATH= ../`\n- 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.\n\n### In this Repo:\n* The `map` package: `roslaunch map viz_map.launch`\n\t- Probabilistic Roadmap:\n\n\t\"PRM\"\n\n\t- Tunable-resolution Grid Map\n\n\t\"GRID\"\n\n* The `global_planner` package:\n\t- A* (green) on PRM:\n\n\t\"ASTAR\"\n\n\t- Theta* (green) on PRM (A* in red for comparison): `roslaunch global_planner astar.launch`\n\n\t\"ASTAR\"\n\n\t- A* (green) on Grid\n\n\t\"ASTARG\"\n\n\t- LPA* with Simulated Grid Updates [re-evaluated cells in orange]: `roslaunch global_planner incremental.launch lpa:=True`\n\t\"LPASTAR\"\n\n\t- D* Lite on Grid [re-evaluated cells in orange]: `roslaunch global_planner incremental.launch`\n\t\"DSL\"\n\n\t- Naive Potential Field (Local Minimum Escape TBD): `roslaunch global_planner potential_field.launch`\n\t\"PFN\"\n\n\n* Trajectory Optimization:\n\t- Model Predictive Path Integral Control on a parallel parking task: `roslaunch control mppi_pentagon.launch parallel:=True`\n\t\"PFN\"\n\n\t- The associated states and controls for the above demo:\n\t\"PFN\"\n\n\tNOTE: To launch waypoint following method, simply don't include the `parallel` argument.\n\n\tIn a separate terminal, do:\n\n\t`rosservice call /set_pose \"x: 0.0\n\ty: 0.0\n\ttheta: 0.0\"`\n\n\tTo start the node.\n\n" }, { "path": "control/CMakeLists.txt", "content": "cmake_minimum_required(VERSION 3.0.2)\nproject(control)\n\n## Compile as C++11, supported in ROS Kinetic and newer\nadd_compile_options(-Wall -Wextra -pipe -Wno-psabi)\n\nset(CMAKE_CXX_STANDARD 17)\nset(CMAKE_CXX_STANDARD_REQUIRED ON)\nset(CMAKE_CXX_EXTENSIONS OFF)\n\n## Find catkin macros and libraries\n## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)\n## is used, also find other catkin packages\nfind_package(catkin REQUIRED COMPONENTS\n global_planner\n map\t\n message_generation\n message_runtime\n rigid2d\n roscpp\n rospy\n rostest\n nuslam\n visualization_msgs\n std_msgs\n)\n\n## System dependencies are found with CMake's conventions\n# list(APPEND CMAKE_MODULE_PATH \"${CMAKE_CURRENT_LIST_DIR}/cmake\")\nfind_package(Eigen3 3.3 REQUIRED NO_MODULE)\n\nfind_package(osqp REQUIRED)\n\nfind_package(OsqpEigen REQUIRED)\n\nset(osqp_INCLUDE_DIR /usr/local/include/osqp)\nset(OsqpEigen_INCLUDE_DIR /usr/local/include/OsqpEigen)\n\nmessage(STATUS \"OsqpEigen_INCLUDE_DIR : \" ${OsqpEigen_INCLUDE_DIR})\nmessage(STATUS \"osqp_INCLUDE_DIR : \" ${osqp_INCLUDE_DIR})\n\n# message(STATUS \"QP : \" ${OsqpEigen_FOUND})\n# message(STATUS \"QP lib: \" ${OsqpEigen_LIBRARIES})\n# message(STATUS \"QP include: \" ${OsqpEigen_INCLUDE_DIRS})\n\n# if (OsqpEigenFOUND)\n# message(STATUS \"Found OsqpEigen\")\n# else ()\n# message(ERROR \" OsqpEigen not found\")\n# endif ()\n\n\n## Uncomment this if the package has a setup.py. This macro ensures\n## modules and global scripts declared therein get installed\n## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html\n# catkin_python_setup()\n\n################################################\n## Declare ROS messages, services and actions ##\n################################################\n\n## To declare and build messages, services or actions from within this\n## package, follow these steps:\n## * Let MSG_DEP_SET be the set of packages whose message types you use in\n## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).\n## * In the file package.xml:\n## * add a build_depend tag for \"message_generation\"\n## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET\n## * If MSG_DEP_SET isn't empty the following dependency has been pulled in\n## but can be declared for certainty nonetheless:\n## * add a exec_depend tag for \"message_runtime\"\n## * In this file (CMakeLists.txt):\n## * add \"message_generation\" and every package in MSG_DEP_SET to\n## find_package(catkin REQUIRED COMPONENTS ...)\n## * add \"message_runtime\" and every package in MSG_DEP_SET to\n## catkin_package(CATKIN_DEPENDS ...)\n## * uncomment the add_*_files sections below as needed\n## and list every .msg/.srv/.action file to be processed\n## * uncomment the generate_messages entry below\n## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)\n\n## Generate messages in the 'msg' folder\nadd_message_files(\n FILES\n Waypoint.msg\n)\n\n## Generate services in the 'srv' folder\n# add_service_files(\n# FILES\n# Service1.srv\n# Service2.srv\n# )\n\n## Generate actions in the 'action' folder\n# add_action_files(\n# FILES\n# Action1.action\n# Action2.action\n# )\n\n## Generate added messages and services with any dependencies listed here\ngenerate_messages(\n DEPENDENCIES\n std_msgs # Or other packages containing msgs\n)\n\n################################################\n## Declare ROS dynamic reconfigure parameters ##\n################################################\n\n## To declare and build dynamic reconfigure parameters within this\n## package, follow these steps:\n## * In the file package.xml:\n## * add a build_depend and a exec_depend tag for \"dynamic_reconfigure\"\n## * In this file (CMakeLists.txt):\n## * add \"dynamic_reconfigure\" to\n## find_package(catkin REQUIRED COMPONENTS ...)\n## * uncomment the \"generate_dynamic_reconfigure_options\" section below\n## and list every .cfg file to be processed\n\n## Generate dynamic reconfigure parameters in the 'cfg' folder\n# generate_dynamic_reconfigure_options(\n# cfg/DynReconf1.cfg\n# cfg/DynReconf2.cfg\n# )\n\n###################################\n## catkin specific configuration ##\n###################################\n## The catkin_package macro generates cmake config files for your package\n## Declare things to be passed to dependent projects\n## INCLUDE_DIRS: uncomment this if your package contains header files\n## LIBRARIES: libraries you create in this project that dependent projects also need\n## CATKIN_DEPENDS: catkin_packages dependent projects also need\n## DEPENDS: system dependencies of this project that dependent projects also need\ncatkin_package(\nINCLUDE_DIRS include\nLIBRARIES ${PROJECT_NAME} global_planner map rigid2d nuslam\nCATKIN_DEPENDS global_planner map message_generation message_runtime rigid2d roscpp nuslam visualization_msgs std_msgs\n# DEPENDS system_lib OsqpEigen\n)\n\n###########\n## Build ##\n###########\n\n## Specify additional locations of header files\n## Your package locations should be listed before other locations\ninclude_directories(\n include\n ${catkin_INCLUDE_DIRS}\n ${Eigen3_INCLUDE_DIRS}\n ${OsqpEigen_INCLUDE_DIR}\n ${osqp_INCLUDE_DIR}\n)\n\n## Declare a C++ library\nadd_library(${PROJECT_NAME}\n src/${PROJECT_NAME}/rk4.cpp\n src/${PROJECT_NAME}/TrajMPC.cpp\n src/${PROJECT_NAME}/Models.cpp\n src/${PROJECT_NAME}/utilities.cpp\n)\n\n## Add cmake target dependencies of the library\n## as an example, code may need to be generated before libraries\n## either from message generation or dynamic reconfigure\nadd_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})\n\n## Declare a C++ executable\n## With catkin_make all packages are built within a single CMake context\n## The recommended prefix ensures that target names across packages don't collide\nadd_executable(osqp_example src/osqp_example.cpp)\nadd_executable(osqp_eigen_example src/osqp_eigen_example.cpp)\n\n## Rename C++ executable without prefix\n## The above recommended prefix causes long target names, the following renames the\n## target back to the shorter version for ease of user use\n## e.g. \"rosrun someones_pkg node\" instead of \"rosrun someones_pkg someones_pkg_node\"\nset_target_properties(osqp_example PROPERTIES OUTPUT_NAME osqp_example PREFIX \"\")\nset_target_properties(osqp_eigen_example PROPERTIES OUTPUT_NAME osqp_eigen_example PREFIX \"\")\n\n## Add cmake target dependencies of the executable\n## same as for the library above\nadd_dependencies(osqp_example ${${PROJECT_NAME}_EXPORTED_TARGETS} ${Eigen3_EXPORTED_TARGETS} ${rigid2d_EXPORTED_TARGETS} ${nuslam_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})\nadd_dependencies(osqp_eigen_example ${${PROJECT_NAME}_EXPORTED_TARGETS} ${Eigen3_EXPORTED_TARGETS} ${rigid2d_EXPORTED_TARGETS} ${nuslam_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})\n\n## Specify libraries to link a library or executable target against\ntarget_link_libraries(\nosqp_example # this is my node that will use below libraries\n${rigid2d_LIBRARIES}\n${nuslam_LIBRARIES}\nEigen3::Eigen\nosqp::osqp\n${PROJECT_NAME}\n${catkin_LIBRARIES}\n)\n\ntarget_link_libraries(\nosqp_eigen_example # this is my node that will use below libraries\n${rigid2d_LIBRARIES}\n${nuslam_LIBRARIES}\nEigen3::Eigen\nosqp::osqp\nOsqpEigen::OsqpEigen\n${PROJECT_NAME}\n${catkin_LIBRARIES}\n)\n\n#############\n## Install ##\n#############\n\n# all install targets should use catkin DESTINATION variables\n# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html\n\n## Mark executable scripts (Python etc.) for installation\n## in contrast to setup.py, you can choose the destination\ninstall(PROGRAMS\n src/mppi\n DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}\n)\n\n## Mark executables for installation\n## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html\ninstall(TARGETS osqp_example osqp_eigen_example\n RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}\n)\n\n## Mark libraries for installation\ninstall(TARGETS ${PROJECT_NAME}\n ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}\n LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}\n RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}\n)\n\n## Mark cpp header files for installation\ninstall(DIRECTORY include/${PROJECT_NAME}/\n DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}\n FILES_MATCHING PATTERN \"*.h\"\n PATTERN \".svn\" EXCLUDE\n)\n\n#############\n## Testing ##\n#############\n\n## Add gtest based cpp test target and link libraries\n# catkin_add_gtest(${PROJECT_NAME}-test test/test_control.cpp)\n# if(TARGET ${PROJECT_NAME}-test)\n# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})\n# endif()\n\n## Add folders to be run by python nosetests\n# catkin_add_nosetests(test)\n" }, { "path": "control/cmake/FindOsqpEigen.cmake", "content": "#.rst:\n# FindOsqpEigen\n# -----------\n#\n# Try to find the OsqpEigen library.\n# Once done this will define the following variables::\n#\n# OsqpEigen_FOUND - System has OsqpEigen\n# OsqpEigen_INCLUDE_DIRS - OsqpEigen include directory\n# OsqpEigen_LIBRARIES - OsqpEigen libraries\n\n\ninclude(FindPackageHandleStandardArgs)\n\nfind_path(OsqpEigen_INCLUDEDIR\n NAMES OsqpEigen.hpp\n HINTS \"${OsqpEigen_SOURCE_DIR}\"\n ENV OsqpEigen_SOURCE_DIR\n PATH_SUFFIXES include)\n \nfind_library(OsqpEigen_LIB\n NAMES OsqpEigen\n HINTS \"${OsqpEigen_BINARY_DIR}\"\n ENV OsqpEigen_BINARY_DIR\n PATH_SUFFIXES lib\n libs)\n\nset(OsqpEigen_INCLUDE_DIRS /home/mori/Desktop/Projects/solvers/osqp-eigen/build/include/)\nset(OsqpEigen_LIBRARIES /home/mori/Desktop/Projects/solvers/osqp-eigen/build/lib/)\n\nfind_package_handle_standard_args(OsqpEigen DEFAULT_MSG OsqpEigen_LIBRARIES\n OsqpEigen_INCLUDE_DIRS)\nset(OsqpEigen_FOUND ${OsqpEigen_FOUND})" }, { "path": "control/config/parallel.yaml", "content": "waypoints: []" }, { "path": "control/config/waypoints.yaml", "content": "waypoints: [[1, 0], [2, 1], [1, 2], [0, 2], [0, 0]]" }, { "path": "control/include/control/Models.hpp", "content": "#pragma once\n/// \\file\n/// \\brief Dynamic Models: Differential Drive.\n\n#include \n#include \n#include \n\n#include \"control/utilities.hpp\"\n\nnamespace control {\nnamespace models {\nusing Eigen::MatrixXd;\nusing Eigen::Ref;\nusing Eigen::Vector2d;\nusing Eigen::VectorXd;\n\n/**\n * @brief Differential Drive Model.\n *\n */\nclass DiffDrive {\n public:\n /**\n * @brief Construct a new Diff Drive object which creates the following:\n *\n * m_a: A matrix (xdot = Ax + Bu).\n * m_b: B matrix (xdot = Ax + Bu). Depends on heading angle.\n * m_gradient: EOM first derivative (to be set by TrajMPC).\n * m_hessian: EOM second derivative (to be set by TrajMPC).\n * m_linear_constraint_matrix: container for linear constraints in QP problem.\n * m_lower_bound, m_upper_bound: bounds for constraints in QP problem.\n *\n * @param wheel_radius Differential Drive robot's wheel radius in m.\n * @param wheel_base Differential Drive robot's wheelbase in m.\n * @param abs_max_wheel_vel Maximum Wheel Velocity (absolute) in rad/s.\n */\n DiffDrive(const double& wheel_radius, const double& wheel_base,\n const double& abs_max_wheel_vel)\n : m_wheel_radius(wheel_radius),\n m_wheel_base(wheel_base),\n m_abs_max_wheel_vel(abs_max_wheel_vel) {\n // DOI: 10.2478/auseme-2018-0002 Model Predictive Control of a\n // Differential-Drive Mobile Robot Samir BOUZOUALEGH 1 , El-Hadi GUECHI 1\n // and Ridha KELAIAIA 2\n\n // A matrix is identity since no actuation = no movement.\n m_a = MatrixXd::Identity(3, 3);\n }\n\n /**\n * @brief Returns the number of variables in this optimization.\n *\n * @param mpcWindow Timesteps for which we perform MPC.\n */\n int num_variables(const int& mpcWindow) {\n return m_a.rows() * (mpcWindow + 1) + m_b.cols() * mpcWindow;\n }\n\n /**\n * @brief Returns the number of constraints in this optimization.\n *\n * @param mpcWindow Timesteps for which we perform MPC.\n */\n int num_constraints(const int& mpcWindow) {\n return 2 * m_a.rows() * (mpcWindow + 1) + m_b.cols() * mpcWindow;\n }\n\n /**\n * @brief\n *\n * @return Eigen::SparseMatrix&\n */\n Eigen::SparseMatrix& hessian() { return m_hessian; }\n\n /**\n * @brief\n *\n * @return VectorXd&\n */\n VectorXd& gradient() { return m_gradient; }\n\n /**\n * @brief\n *\n * @return MatrixXd&\n */\n MatrixXd& linear_constraint_matrix() { return m_linear_constraint_matrix; }\n\n /**\n * @brief\n *\n * @return VectorXd&\n */\n VectorXd& lower_bound() { return m_lower_bound; }\n\n /**\n * @brief\n *\n * @return VectorXd&\n */\n VectorXd& upper_bound() { return m_upper_bound; }\n\n /**\n * @brief\n *\n * @return MatrixXd&\n */\n MatrixXd a() { return m_a; }\n\n /**\n * @brief\n *\n * @param theta Robot heading in radians.\n * @return MatrixXd\n */\n MatrixXd b(const MatrixXd& state) {\n // B matrix is a function of heading and robot params - see mppi node.\n const double& theta = state(2);\n\n /**\n * r: wheel radius\n * wb: wheel base\n * u0: left wheel speed\n * u1: right wheel speed\n * th: robot heading\n * | 0.5rcos(th), 0.5rcos(th) | | u0 |\n * | 0.5rsin(th), 0.5rsin(th) | X | |\n * | -r/wb, r/wb | | u1 |\n */\n\n m_b << 0.5 * m_wheel_radius * std::cos(theta),\n 0.5 * m_wheel_radius * std::cos(theta),\n 0.5 * m_wheel_radius * std::sin(theta),\n 0.5 * m_wheel_radius * std::sin(theta), -m_wheel_radius / m_wheel_base,\n m_wheel_radius / m_wheel_base;\n\n return m_b;\n }\n\n private:\n double m_wheel_radius;\n double m_wheel_base;\n double m_abs_max_wheel_vel;\n Eigen::Matrix m_a;\n Eigen::Matrix m_b;\n VectorXd m_gradient;\n Eigen::SparseMatrix m_hessian;\n MatrixXd m_linear_constraint_matrix;\n VectorXd m_lower_bound, m_upper_bound;\n};\n} // namespace models\n} // namespace control" }, { "path": "control/include/control/TrajMPC.hpp", "content": "#pragma once\n/// \\file\n/// \\brief Model Predictive Control for Trajectory Following\n\n#include \n\n#include \n#include \n#include \n\n#include \"control/utilities.hpp\"\n\nnamespace control {\nusing Eigen::MatrixXd;\nusing Eigen::Ref;\nusing Eigen::Vector2d;\nusing Eigen::VectorXd;\n\n/**\n * @brief Model Predictive Control for Trajectory Following.\n *\n * Model: Contains the Dynamic Model for the optimization.\n * METHODS:\n * num_variables(): return number of variables in the problem.\n * num_constraints(): return number of constraints in the problem.\n * gradient(): return a reference to the dynamic model's gradient.\n * hessian(): return a reference to the dynamic model's hessian.\n * linear_constraint_matrix(): return the dynamic model's constraint matrix.\n * lower_bound(): return a reference to the lower bound constraint vector.\n * upper_bound(): return a reference to the upper bound constraint vector.\n * a(): return A matrix (xdot = Ax + Bu)\n * b(current state): return B matrix (xdot = Ax + Bu)\n *\n * Action: Container for returning minimizing commands.\n *\n * Problem: Contains the problem formulation for the optimization.\n * METHODS:\n * Q(): return state cost Matrix (LQR formulation).\n * R(): return action cost Matrix (LQR formulation).\n * xMax(): return upper state constraint bound.\n * xMin(): return lower state constraint bound.\n * uMax(): return upper action constraint bound.\n * uMin(): return lower action constraint bound.\n * x0(): return initial state for optimization.\n * xRef(): return reference state for optimization.\n *\n */\ntemplate \nclass TrajMPC {\n public:\n /**\n * @brief Constructor which takes in dynamic model.\n *\n * @param model contains the xdot = Ax + Bu transition, as well as the\n * constraints.\n */\n TrajMPC(const Model& model, const int& mpcWindow,\n const double& solve_threshold = 1.0e-12,\n const int& max_solve_steps = 50)\n : m_model(model),\n m_mpcWindow(mpcWindow),\n m_solve_threshold(solve_threshold),\n m_max_solve_steps(max_solve_steps) {}\n\n /**\n * @brief Set up OSQP Solver with relevant parameters.\n *\n * @param problem Container for problem information: Cost Matrices, State and\n * Control Limits, Reference and Initial Trajectory.\n * @return size_t solver setup result. 0 is success, 1 is failure.\n */\n size_t setup(const Problem& problem) {\n // Grab problem data.\n const auto& Q = problem.Q();\n const auto& R = problem.R();\n const auto& xMax = problem.xMax();\n const auto& xMin = problem.xMin();\n const auto& uMax = problem.uMax();\n const auto& uMin = problem.uMin();\n const auto& x0 = problem.x0();\n const auto& xRef = problem.xRef();\n\n // Grab model data. (xdot = Ax + Bu)\n const auto a = m_model.a();\n const auto b = m_model.b(x0);\n\n // cast the MPC problem as QP problem\n control::utilities::castMPCToQPHessian(\n Q, R, m_mpcWindow, m_model.hessian());\n control::utilities::castMPCToQPGradient(\n Q, xRef, m_mpcWindow, m_model.gradient());\n control::utilities::castMPCToQPConstraintMatrix(\n a, b, m_mpcWindow, m_model.linear_constraint_matrix());\n control::utilities::castMPCToQPConstraintVectors(\n xMax, xMin, uMax, uMin, x0, m_mpcWindow, m_model.lower_bound(),\n m_model.upper_bound());\n\n // instantiate the solver\n OsqpEigen::Solver solver;\n\n // settings\n solver.settings()->setVerbosity(false);\n solver.settings()->setWarmStart(true);\n\n // set the initial data of the QP solver\n solver.data()->setNumberOfVariables(m_model.num_variables(m_mpcWindow));\n solver.data()->setNumberOfConstraints(m_model.num_constraints(m_mpcWindow));\n if (!solver.data()->setHessianMatrix(m_model.hessian())) return 1;\n if (!solver.data()->setGradient(m_model.gradient())) return 1;\n if (!solver.data()->setLinearConstraintsMatrix(\n m_model.linear_constraint_matrix()))\n return 1;\n if (!solver.data()->setLowerBound(m_model.lower_bound())) return 1;\n if (!solver.data()->setUpperBound(m_model.upper_bound())) return 1;\n\n // instantiate the solver\n if (!solver.initSolver()) return 1;\n\n m_solver = solver;\n return 0;\n }\n\n /**\n * @brief Assign the optimal control action around the desired reference and\n * return an exit code for this OSQP solver iteration.\n *\n * @param problem Container for problem information: Cost Matrices, State and\n * Control Limits, Reference and Initial Trajectory.\n * @param action Container for assigning optimal control action.\n * @return OSQP exit code.\n */\n size_t solve(const Problem& problem, Action* action) {\n // Grab updated problem parameters\n const auto& Q = problem.Q();\n auto& x0 = problem.x0();\n const auto& xRef = problem.xRef();\n\n // Grab solver\n auto& solver = m_solver;\n\n // Update Gradient based on new xRef\n control::utilities::castMPCToQPGradient(\n Q, xRef, m_mpcWindow, m_model.gradient());\n if (!solver.updateGradient(m_model.gradient())) return 1;\n // Update Constraint Vector based on new initial state\n control::utilities::updateConstraintVectors(\n x0, m_model.lower_bound(), m_model.upper_bound());\n if (!solver.updateBounds(m_model.lower_bound(), m_model.upper_bound()))\n return 1;\n\n // controller input and QPSolution vector\n Eigen::Vector4d ctr;\n // Assign zero control to action in case of failure.\n action->cmd = ctr;\n Eigen::VectorXd QPSolution;\n\n // Exceeded max steps.\n size_t exit_code = -1;\n\n double obj_val = std::numeric_limits::max();\n\n for (int i = 0; i < m_max_solve_steps; i++) {\n // solve the QP problem\n if (!solver.solve()) return 1;\n\n // get the controller input\n QPSolution = solver.getSolution();\n ctr = QPSolution.block(problem.xMax().rows() * (m_mpcWindow + 1), 0,\n problem.uMax().rows(), 1);\n\n // save data into file\n // auto x0Data = x0.data();\n\n // propagate the model - TODO: RK4\n x0 = m_model.a() * x0 + m_model.b(x0) * ctr;\n\n // update the constraint bound\n control::utilities::updateConstraintVectors(\n x0, m_model.lower_bound(), m_model.upper_bound());\n if (!solver.updateBounds(m_model.lower_bound(), m_model.upper_bound()))\n return 1;\n\n /// Get the optimal objective value, can be used to exit early.\n /// https://osqp.org/docs/interfaces/cc++?highlight=osqpworkspace#_CPPv413OSQPWorkspace\n const auto new_obj_val = solver.workspace()->info->obj_val;\n\n if (std::abs(new_obj_val - obj_val) < m_solve_threshold) {\n obj_val = new_obj_val;\n break;\n }\n obj_val = new_obj_val;\n }\n action->cmd = ctr;\n return 0;\n }\n\n private:\n /// MPC Horizon.\n int m_mpcWindow;\n /// Cost change threshold for stopping optimization.\n double m_solve_threshold;\n /// Maximum number of optimization steps before giving up.\n int m_max_solve_steps;\n /// Dynamic Model used to solve optimization problem.\n Model m_model;\n /// OSQP Solver with Eigen C++ Wrapper.\n OsqpEigen::Solver m_solver;\n};\n} // namespace control" }, { "path": "control/include/control/rk4.hpp", "content": "#ifndef RK4_HPP\n#define RK4_HPP\n/// \\file\n/// \\brief 4th Order Runge Kutta method for Integration\n\n#include \n#include \n#include \n\nnamespace control\n{\n using Eigen::MatrixXd;\n using Eigen::VectorXd;\n using Eigen::Vector2d;\n using Eigen::Ref;\n\n\n /// \\brief Integrator\n class RK4\n {\n public:\n /// \\brief Integrator constructor with fixed step size\n /// \\param dt - time step for integration\n RK4(const double & dt);\n\n /// \\brief Register a generic ODE without a control signal. updates the last input by ref (non-const)\n /// \\param ode - function to integrate\n void registerODE(std::function, Ref)> ode);\n\n /// \\brief Register a generic ODE with a control signal. updates the last input by ref (non-const)\n /// \\param ode - function to integrate\n void registerODE(std::function, const Ref, Ref)> ode);\n\n /// \\brief Solve ODE or system of ODEs without a control signal using RK4\n /// \\param x0 - initial condition(state)\n /// \\param horizon - final time of integration\n MatrixXd solve(const Ref x0, const double & horizon);\n\n /// \\brief Solve ODE or system of ODEs with a control signal using RK4\n /// \\param x0 - initial condition(state)\n /// \\param u - control signal (must be of length equal to number of rk4 iterations => horizon/step)\n /// \\param horizon - final time of integration\n MatrixXd solve(const Ref x0, const Ref u, const double & horizon);\n private:\n /// \\brief Perform one iteration of rk4 without a control signal\n /// x_t[out] - update the current state x_t\n void integrate(Ref x_t);\n\n /// \\brief Perform one iteration of rk4 with a control signal\n /// param u_t - control vector\n /// x_t[out] - update the current state x_t\n void integrate(Ref x_t, const Ref u_t);\n\n // ODE without control args: x(t), xdot[out]. updates the last input by ref (non-const)\n std::function, Ref)> func;\n\n // ODE with control args: x(t), u(t), xdot[out]. updates the last input by ref (non-const)\n std::function, const Ref, Ref)> func_ctrl;\n\n // time step\n double dt;\n };\n}\n#endif" }, { "path": "control/include/control/utilities.hpp", "content": "#pragma once\n/// \\file\n/// \\brief Utilities Library for Quadratic Programming\n\n#include \n#include \n#include \n\n#include \"OsqpEigen/OsqpEigen.h\"\n\nnamespace control {\nnamespace utilities {\nusing Eigen::DiagonalMatrix;\nusing Eigen::Dynamic;\nusing Eigen::MatrixXd;\nusing Eigen::Ref;\nusing Eigen::SparseMatrix;\nusing Eigen::Vector2d;\nusing Eigen::VectorXd;\n\ntemplate \nvoid castMPCToQPHessian(const DiagonalMatrix &Q,\n const DiagonalMatrix &R, int mpcWindow,\n SparseMatrix &hessianMatrix) {\n // Return size of diagonal matrices: size = row * col so we use sqrt.\n const int Q_size = std::sqrt(Q.size());\n const int R_size = std::sqrt(R.size());\n\n hessianMatrix.resize(Q_size * (mpcWindow + 1) + R_size * mpcWindow,\n Q_size * (mpcWindow + 1) + R_size * mpcWindow);\n\n // populate hessian matrix\n for (auto i = 0; i < Q_size * (mpcWindow + 1) + R_size * mpcWindow; i++) {\n if (i < Q_size * (mpcWindow + 1)) {\n int posQ = i % Q_size;\n float value = Q.diagonal()[posQ];\n if (value != 0) hessianMatrix.insert(i, i) = value;\n } else {\n int posR = i % R_size;\n float value = R.diagonal()[posR];\n if (value != 0) hessianMatrix.insert(i, i) = value;\n }\n }\n}\n\ntemplate \nvoid castMPCToQPGradient(\n const Eigen::DiagonalMatrix &Q,\n const Eigen::Matrix &xRef, int mpcWindow,\n Eigen::VectorXd &gradient) {\n // Return size of diagonal matrices: size = row * col so we use sqrt.\n const int Q_size = std::sqrt(Q.size());\n\n Eigen::Matrix Qx_ref;\n Qx_ref = Q * (-xRef);\n\n // populate the gradient vector\n gradient = Eigen::VectorXd::Zero(Q_size * (mpcWindow + 1) + 4 * mpcWindow, 1);\n for (auto i = 0; i < Q_size * (mpcWindow + 1); i++) {\n int posQ = i % Q_size;\n float value = Qx_ref(posQ, 0);\n gradient(i, 0) = value;\n }\n}\n\ntemplate \nvoid castMPCToQPConstraintMatrix(\n const Eigen::Matrix &dynamicMatrix,\n const Eigen::Matrix &controlMatrix,\n int mpcWindow, Eigen::SparseMatrix &constraintMatrix) {\n // Get Rows and Cols from dynamicMatrix and controlMatrix.\n const int dynamicMatrix_rows = dynamicMatrix.rows();\n const int dynamicMatrix_cols = dynamicMatrix.cols();\n const int controlMatrix_rows = controlMatrix.rows();\n const int controlMatrix_cols = controlMatrix.cols();\n\n constraintMatrix.resize(\n dynamicMatrix_rows * (mpcWindow + 1) +\n dynamicMatrix_cols * (mpcWindow + 1) + controlMatrix_cols * mpcWindow,\n controlMatrix_rows * (mpcWindow + 1) + controlMatrix_cols * mpcWindow);\n\n // populate linear constraint matrix\n for (auto i = 0; i < dynamicMatrix_rows * (mpcWindow + 1); i++) {\n constraintMatrix.insert(i, i) = -1;\n }\n\n for (auto i = 0; i < mpcWindow; i++)\n for (int j = 0; j < dynamicMatrix_rows; j++)\n for (int k = 0; k < dynamicMatrix_rows; k++) {\n float value = dynamicMatrix(j, k);\n if (value != 0) {\n constraintMatrix.insert(dynamicMatrix_rows * (i + 1) + j,\n dynamicMatrix_rows * i + k) = value;\n }\n }\n\n for (auto i = 0; i < mpcWindow; i++)\n for (int j = 0; j < dynamicMatrix_rows; j++)\n for (int k = 0; k < controlMatrix_cols; k++) {\n float value = controlMatrix(j, k);\n if (value != 0) {\n constraintMatrix.insert(dynamicMatrix_rows * (i + 1) + j,\n controlMatrix_cols * i + k +\n dynamicMatrix_rows * (mpcWindow + 1)) =\n value;\n }\n }\n\n for (auto i = 0; i < dynamicMatrix_rows * (mpcWindow + 1) +\n controlMatrix_cols * mpcWindow;\n i++) {\n constraintMatrix.insert(i + (mpcWindow + 1) * dynamicMatrix_rows, i) = 1;\n }\n}\n\ntemplate \nvoid castMPCToQPConstraintVectors(const Eigen::Matrix &xMax,\n const Eigen::Matrix &xMin,\n const Eigen::Matrix &uMax,\n const Eigen::Matrix &uMin,\n const Eigen::Matrix &x0,\n int mpcWindow, Eigen::VectorXd &lowerBound,\n Eigen::VectorXd &upperBound) {\n // Get argument vector lenghts\n const int xMax_size = xMax.size();\n const int xMin_size = xMin.size();\n const int uMax_size = uMax.size();\n const int uMin_size = uMin.size();\n const int x0_size = x0.size();\n\n // evaluate the lower and the upper inequality vectors\n Eigen::VectorXd lowerInequality =\n Eigen::MatrixXd::Zero(x0_size * (mpcWindow + 1) + 4 * mpcWindow, 1);\n Eigen::VectorXd upperInequality =\n Eigen::MatrixXd::Zero(x0_size * (mpcWindow + 1) + 4 * mpcWindow, 1);\n for (auto i = 0; i < mpcWindow + 1; i++) {\n lowerInequality.block(xMin_size * i, 0, xMin_size, 1) = xMin;\n upperInequality.block(xMax_size * i, 0, xMax_size, 1) = xMax;\n }\n for (auto i = 0; i < mpcWindow; i++) {\n lowerInequality.block(uMin_size * i + x0_size * (mpcWindow + 1), 0,\n uMin_size, 1) = uMin;\n upperInequality.block(uMax_size * i + x0_size * (mpcWindow + 1), 0,\n uMax_size, 1) = uMax;\n }\n\n // evaluate the lower and the upper equality vectors\n Eigen::VectorXd lowerEquality =\n Eigen::MatrixXd::Zero(x0_size * (mpcWindow + 1), 1);\n Eigen::VectorXd upperEquality;\n lowerEquality.block(0, 0, x0_size, 1) = -x0;\n upperEquality = lowerEquality;\n lowerEquality = lowerEquality;\n\n // merge inequality and equality vectors\n lowerBound = Eigen::MatrixXd::Zero(\n 2 * x0_size * (mpcWindow + 1) + uMax_size * mpcWindow, 1);\n lowerBound << lowerEquality, lowerInequality;\n\n upperBound = Eigen::MatrixXd::Zero(\n 2 * x0_size * (mpcWindow + 1) + uMax_size * mpcWindow, 1);\n upperBound << upperEquality, upperInequality;\n}\n\ntemplate \nvoid updateConstraintVectors(const Eigen::Matrix &x0,\n Eigen::VectorXd &lowerBound,\n Eigen::VectorXd &upperBound) {\n // Get Size\n const int x0_size = x0.size();\n lowerBound.block(0, 0, x0_size, 1) = -x0;\n upperBound.block(0, 0, x0_size, 1) = -x0;\n}\n\ntemplate \ndouble getErrorNorm(const Eigen::Matrix &x,\n const Eigen::Matrix &xRef) {\n // Get Size\n const auto x_size = x.size();\n // evaluate the error\n Eigen::Matrix error = x - xRef;\n\n // return the norm\n return error.norm();\n}\n} // namespace utilities\n} // namespace control" }, { "path": "control/launch/mppi_pentagon.launch", "content": "\n\n\t\t\n\t\n\t\t\n\t\t\n\t\t\t\n\t\t\t\n\t\t\n\n\t\t\n\t\t\n\t\t\t\n\t\t\t\n\t\t\t\n\t\t\t\n\t\t\t\n\t\t\n\n\t\t\n\t\t\n\t\t\t\n\t\t\n\n\t\t\n\t\t\n\t\t\t\n\t\t\t\n\t\t\t \n\t\t\t\n\t\t\t\n\t\t\n\n\t\n\t\n\t\t\n\t\t\n\t\n\n\t\n\t\t\n\t\t\n\t\n\n\n\n\t\n\t\n\t\n\t\n\t\n\t \n\n" }, { "path": "control/launch/osqp_example.launch", "content": "\n\n\t\t\n\t\n\t\t\n\t\t\n\t\n\n\t\n\t\t\n\t\t\n\t\n\n" }, { "path": "control/msg/Waypoint.msg", "content": "# Current Pose\nfloat32[] current\n# Desired Pose\nfloat32[] desired" }, { "path": "control/package.xml", "content": "\n\n control\n 0.0.1\n Control the turtlebot between waypoints\n\n \n \n \n Maurice Rahme\n\n\n \n \n \n TODO\n\n\n \n \n \n \n\n\n \n \n \n \n\n\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n catkin\n global_planner\n map\n message_generation\n message_runtime\n rigid2d\n roscpp\n rospy\n nuslam\n visualization_msgs\n std_msgs\n global_planner\n map\n message_generation\n message_runtime\n rigid2d\n nuslam\n roscpp\n rospy\n visualization_msgs\n std_msgs\n global_planner\n map\n message_generation\n message_runtime\n roscpp\n rospy\n rigid2d\n nuslam\n visualization_msgs\n std_msgs\n\n rostest\n rosunit\n \n \n \n\n \n\n" }, { "path": "control/src/control/Models.cpp", "content": "#include \"control/Models.hpp\"\n\n#include \n\nnamespace control {} // namespace control" }, { "path": "control/src/control/TrajMPC.cpp", "content": "#include \"control/TrajMPC.hpp\"\n\n#include \n\nnamespace control {} // namespace control" }, { "path": "control/src/control/rk4.cpp", "content": "/// \\file\n/// \\brief 4th Order Runge Kutta method for Integration\n\n#include \n#include \"control/rk4.hpp\"\n\nnamespace control\n{\n\nRK4::RK4(const double & dt) : dt(dt)\n{\n}\n\n\nvoid RK4::registerODE(std::function, Ref)> ode)\n{\n func = ode;\n}\n\n\nvoid RK4::registerODE(std::function, const Ref, Ref)> ode)\n{\n func_ctrl = ode;\n}\n\n\nMatrixXd RK4::solve(const Ref x0, const double & horizon)\n{\n // catch undeclared fcn\n if(!func)\n {\n std::cerr << \"No ODE Function!\" << std::endl;\n exit(EXIT_FAILURE);\n }\n\n // Initial State\n VectorXd state = x0;\n // Solver iterations\n const auto num_samples = static_cast(horizon/dt);\n // Solution Trajectory Shape\n MatrixXd trajectory(state.rows(), num_samples);\n\n // Integrate for all samples\n for(int i = 0; i < num_samples; i++)\n {\n // Integrate once (pass state by reference using Eigen::Ref<>)\n integrate(state);\n // Store next state in trajectory vector\n trajectory.col(i) = state;\n }\n\n return trajectory;\n}\n\n\nMatrixXd RK4::solve(const Ref x0, const Ref u, const double & horizon)\n{\n // catch undeclared fcn\n if(!func_ctrl)\n {\n std::cerr << \"No ODE Function!\" << std::endl;\n exit(EXIT_FAILURE);\n }\n\n // Initial State\n VectorXd state = x0;\n // Solver iterations\n const auto num_samples = static_cast(horizon/dt);\n // Solution Trajectory Shape\n MatrixXd trajectory(state.rows(), num_samples);\n\n // Integrate for all samples\n for(int i = 0; i < num_samples; i++)\n {\n // Get Control Vector\n VectorXd u_t = u.col(i);\n // Integrate once (pass state by reference using Eigen::Ref<>)\n integrate(state, u_t);\n // Store next state in trajectory vector\n trajectory.col(i) = state;\n }\n\n return trajectory;\n}\n\n\nvoid RK4::integrate(Ref x_t)\n{\n // Pass input as reference to save memory\n\n // Initialize at zero\n VectorXd k1 = VectorXd::Zero(x_t.size());\n VectorXd k2, k3, k4, temp_pass;\n k2 = k3 = k4 = temp_pass = k1;\n\n // RK4 method without control signal\n\n // get k1 by ref\n func(x_t, k1);\n // get k2 by ref\n temp_pass = x_t + dt*(0.5*k1);\n func(temp_pass, k2);\n // get k3 by ref\n temp_pass = x_t + dt*(0.5*k2);\n func(temp_pass, k3);\n // get k4 by ref\n temp_pass = x_t + dt*k3;\n func(temp_pass, k4);\n\n // update (init + increment)\n x_t += (dt/6.0)*(k1 + 2.0*k2 + 2.0*k3 + k4);\n}\n\n\nvoid RK4::integrate(Ref x_t, const Ref u_t)\n{\n // Pass input as reference to save memory\n\n // Initialize at zero\n VectorXd k1 = VectorXd::Zero(x_t.size());\n VectorXd k2, k3, k4, temp_pass;\n k2 = k3 = k4 = temp_pass = k1;\n\n // RK4 method with control signal\n\n // get k1 by ref\n func_ctrl(x_t, u_t, k1);\n // get k2 by ref\n temp_pass = x_t + dt*(0.5*k1);\n func_ctrl(temp_pass, u_t, k2);\n // get k3 by ref\n temp_pass = x_t + dt*(0.5*k2);\n func_ctrl(temp_pass, u_t, k3);\n // get k4 by ref\n temp_pass = x_t + dt*k3;\n func_ctrl(temp_pass, u_t, k4);\n\n // update (init + increment)\n x_t += (dt/6.0)*(k1 + 2.0*k2 + 2.0*k3 + k4);\n}\n\n\n\n\n}" }, { "path": "control/src/control/utilities.cpp", "content": "#include \"control/utilities.hpp\"\n\n#include \n\nnamespace control {\nnamespace utilities {}\n} // namespace control" }, { "path": "control/src/mppi", "content": "#!/usr/bin/env python\n\nimport numpy as np\nimport matplotlib.pyplot as plt\nfrom scipy.signal import savgol_filter\nfrom matplotlib import animation\nimport copy\nimport time\nimport rospy\nfrom nav_msgs.msg import Odometry\nfrom geometry_msgs.msg import Twist\nimport tf\nfrom geometry_msgs.msg import Twist, Quaternion, Vector3\n\nnp.random.seed(0)\n\n# rad/s\nWHEEL_VEL_MAX = 6.35492\nWHEEL_RADIUS = 0.033\nWHEEL_BASE = 0.16\n\n\ndef dd_dynamics(x, u):\n \"\"\" Diff Drive dd_dynamics\n \"\"\"\n return np.array([\n (WHEEL_RADIUS / 2.0) * np.cos(x[2, :]) * (u[0, :] + u[1, :]),\n (WHEEL_RADIUS / 2.0) * np.sin(x[2, :]) * (u[0, :] + u[1, :]),\n (WHEEL_RADIUS / WHEEL_BASE) * (u[1, :] - u[0, :])\n ])\n\n\ndef unicycle_dynamics(x, u):\n return np.array(\n [np.cos(x[2, :]) * u[0, :],\n np.sin(x[2, :]) * u[0, :], u[1, :]])\n\n\ndef rk4(x0, u, dt):\n \"\"\" Returns position updates for\n given velocity commands after one timestep\n using a Runge-Kutta 4 integrator\n \"\"\"\n # update calculated here\n k1 = dt * dd_dynamics(x0, u)\n k2 = dt * dd_dynamics(x0 + k1 / 2, u)\n k3 = dt * dd_dynamics(x0 + k2 / 2, u)\n k4 = dt * dd_dynamics(x0 + k3, u)\n # initial plus update\n xnew = x0 + (1.0 / 6.0) * (k1 + 2 * k2 + 2 * k3 + k4)\n # Clip Theta\n xnew[2, :] = xnew[2, :] - (np.ceil(\n (xnew[2, :] + np.pi) / (2.0 * np.pi)) - 1.0) * 2.0 * np.pi\n return xnew\n\n\ndef euler(x0, u, dt):\n return x0 + dt * unicycle_dynamics(x0, u)\n\n\nclass MPPI:\n def __init__(self, model=rk4, horizon=100, samples=10, thresh=0.05):\n self.horizon = horizon\n self.samples = samples\n self.uvec_init = np.zeros((2, horizon))\n self.model = model\n self.dt = 1.0 / float(horizon)\n # Cost during trajectory\n self.Q = np.array([[1e3, 0.0, 0.0], [0.0, 1e3, 0.0], [0.0, 0.0, 0.0]])\n # Cost of controls\n self.R = np.array([[1.0, 0.0], [0.0, 1.0]])\n # Termination cost\n self.P1 = np.array([[1e3, 0.0, 0.0], [0.0, 1e3, 0.0], [0.0, 0.0, 1e3]])\n self.thresh = thresh\n self.start = np.array([0.0, 0.0, 0.0])\n self.goal = np.array([0.0, 0.0, 0.0])\n self.initialize()\n\n def initialize(self):\n self.fin_time = [0] # time stamp of the position/control\n self.latest_uvec = copy.deepcopy(self.uvec_init)\n self.uvec = np.array([self.uvec_init[:, 0]])\n self.path = np.array([self.start])\n\n def get_path(self,\n state,\n goal,\n sig=np.array([[.9, 0.0], [0.0, .9]]),\n lam=.001):\n value_fcn, eps = self.get_cost2go(state, self.latest_uvec, goal, lam,\n sig)\n self.latest_uvec = self.update_action(self.latest_uvec, eps, value_fcn,\n sig, lam)\n state = self.perform_action(state, self.latest_uvec)\n self.path = np.concatenate((self.path, np.array([state])))\n self.uvec = np.concatenate(\n (self.uvec, np.array([self.latest_uvec[:, 0]])))\n self.fin_time.append(self.fin_time[-1] + self.dt)\n # Shift controls for Receding Horizon MPPI\n self.latest_uvec[:, :-1] = self.latest_uvec[:, 1:]\n self.latest_uvec[:, -1] = self.uvec_init[:, 0]\n return state\n\n def solve_path(self,\n start,\n goal,\n sig=np.array([[1.0, 0.0], [0.0, 1.0]]),\n lam=.01):\n self.start = start\n self.goal = goal\n state = start\n self.path = np.array([state])\n self.latest_uvec = copy.deepcopy(self.uvec_init)\n print(\"STARTING\")\n i = 0\n tstart = time.time()\n while np.linalg.norm(state[:2] - goal[:2]) > self.thresh:\n i += 1\n state = self.get_path(state, goal, sig, lam)\n if i % 200 == 0:\n print(\"Iteration: {} \\t State: {}\".format(i, state))\n t_elapsed = time.time() - tstart\n print(\n \"Finished after {} iterations. Final State: {}, Time Taken: {}, Time Per Iter: {}\"\n .format(i, state, t_elapsed, t_elapsed / float(i)))\n\n def get_cost2go(self, state, uvec, goal, lam, sig):\n cost2go = []\n # Dim: statedim x N\n # State at current timestep\n states = np.tile(state, (self.samples, 1)).T\n\n eps = []\n # Each timed eps contains one disturbance for each timestep\n # so eps is t x udim x N, where N is num of samples\n for t in range(self.horizon):\n # statedim x N, where N is num of samples\n # Sample N states forward in time for N costs to go\n # eps.append(\n # np.random.multivariate_normal([0.0, 0.0],\n # sig,\n # size=(self.samples)).T)\n eps.append(\n np.random.normal(0,\n sig[0, 0],\n size=(uvec.shape[0], self.samples)))\n u_samp = np.tile(uvec[:, t], (self.samples, 1)).T\n # EXPLORE\n u_samp += eps[-1]\n # CLIP\n u_samp[0, :] = np.clip(u_samp[0, :], -WHEEL_VEL_MAX, WHEEL_VEL_MAX)\n u_samp[1, :] = np.clip(u_samp[1, :], -WHEEL_VEL_MAX, WHEEL_VEL_MAX)\n # Predict Next State\n st = self.model(states, u_samp, self.dt)\n\n # Trajectory Cost\n costs = np.zeros(self.samples)\n for s in range(self.samples):\n states[:, s] = st[:, s]\n costs[s] = self.get_cost(st[:, s], goal, uvec[:, t], lam,\n sig, eps[-1][:, s])\n\n cost2go.append(costs)\n\n costs = np.zeros(self.samples)\n for s in range(self.samples):\n # ADD TERMINAL COST\n mxt = (st[:, s] - goal).T.dot(self.P1).dot(st[:, s] - goal)\n costs[s] = mxt\n\n cost2go[-1] += costs\n # get value function over time as cost2go at each timestep\n # (first reversed and summed)\n # value_fcn = np.cumsum(cost2go[::-1], 0)[::-1, :]\n value_fcn = np.flip(np.cumsum(np.flip(cost2go, 0), axis=0), 0)\n # print(\"VALUE FCN: {}\".format(value_fcn))\n\n return value_fcn, eps\n\n def get_cost(self, state, desired_state, u, lam, sig, eps):\n # return euclid dist\n return 0.5 * (\n (state - desired_state).T.dot(self.Q).dot(state - desired_state) +\n u.T.dot(self.R).dot(u)) + lam * u.dot(sig).dot(eps)\n\n def update_action(self, uvec, eps, value_fcn, sig, lam):\n for t in range(self.horizon):\n # First, subtract the min cost out from each sample\n value_fcn[t] -= np.amin(value_fcn[t])\n\n # Get weight for optimal action update\n # This tells us the usefulness of each eps sample in [t]\n omg = np.exp(-value_fcn[t] / lam) + 1e-8\n # Normalize\n omg /= np.sum(omg)\n uvec[:, t] += np.dot(eps[t], omg)\n # CLIP\n uvec[0, :] = np.clip(uvec[0, :], -WHEEL_VEL_MAX, WHEEL_VEL_MAX)\n uvec[1, :] = np.clip(uvec[1, :], -WHEEL_VEL_MAX, WHEEL_VEL_MAX)\n\n # filter controls\n uvec = savgol_filter(uvec, self.horizon - 1, 3, axis=1)\n\n # CLIP\n uvec[0, :] = np.clip(uvec[0, :], -WHEEL_VEL_MAX, WHEEL_VEL_MAX)\n uvec[1, :] = np.clip(uvec[1, :], -WHEEL_VEL_MAX, WHEEL_VEL_MAX)\n\n return uvec\n\n def perform_action(self, state, uvec):\n state = np.tile(state, (2, 1)).T\n u = np.tile(uvec[:, 0], (2, 1)).T\n return self.model(state, u, self.dt)[:, 0]\n\n def get_animation(self):\n print(\"generating animation...\")\n path = np.array(self.path) # final path of robot\n control = np.array(self.uvec) # controls used to generate the path\n fig, (ax1, ax2, ax3) = plt.subplots(nrows=3, ncols=1, figsize=(10, 10))\n fig.suptitle(\"MPPI\", fontsize=12)\n ax1.set(xlabel=\"X position (m)\", ylabel=\"Y position (m)\")\n ax1.set_xlim(np.amin(path[:, 0]) - 0.1, np.amax(path[:, 0]) + 0.1)\n ax1.set_ylim(np.amin(path[:, 1]) - 0.1, np.amax(path[:, 1]) + 0.1)\n line11, = ax1.plot([], [])\n ax2.set(xlabel=\"Time (s)\", ylabel=\"State (m or rad)\")\n ax2.set_xlim(0, self.fin_time[-1]) # fin_time is my time vector\n ax2.set_ylim(np.amin(path) - 0.1, np.amax(path) + 0.1)\n line21, = ax2.plot([], [])\n line22, = ax2.plot([], [])\n line23, = ax2.plot([], [])\n ax2.legend([\"x\", \"y\", r\"$\\theta$\"])\n ax3.set(xlabel=\"Time (s)\", ylabel=\"Wheel Velocity (rad/s)\")\n ax3.set_xlim(0, self.fin_time[-1])\n ax3.set_ylim(np.amin(control) - 0.1, np.amax(control) + 0.1)\n line31, = ax3.plot([], [])\n line32, = ax3.plot([], [])\n ax3.legend([\"u_l\", \"u_r\"])\n\n def animate(i):\n ax1.clear()\n ax1.set(xlabel=\"X position (m)\", ylabel=\"Y position (m)\")\n ax1.set_xlim(np.amin(path[:, 0]) - 0.1, np.amax(path[:, 0]) + 0.1)\n ax1.set_ylim(np.amin(path[:, 1]) - 0.1, np.amax(path[:, 1]) + 0.1)\n ax1.arrow(x=path[i, 0],\n y=path[i, 1],\n dx=0.025 * np.cos(path[i, 2]),\n dy=0.025 * np.sin(path[i, 2]),\n head_width=0.03,\n length_includes_head=True,\n overhang=0.1,\n facecolor=\"black\",\n zorder=0)\n line11, = ax1.plot([], [])\n line11.set_data(path[0:i, 0], path[0:i, 1])\n line21.set_data(self.fin_time[0:i], path[0:i, 0])\n line22.set_data(self.fin_time[0:i], path[0:i, 1])\n line23.set_data(self.fin_time[0:i], path[0:i, 2])\n line31.set_data(self.fin_time[0:i], control[0:i, 0])\n line32.set_data(self.fin_time[0:i], control[0:i, 1])\n return line11, line21, line22, line23, line31, line32\n\n # may want to change the interval to match your dt, the number is in milliseconds\n anim = animation.FuncAnimation(fig,\n animate,\n frames=len(self.fin_time),\n interval=10,\n repeat=False)\n print(\"saving animation as mp4...this might take a while...\")\n anim.save(filename='sim.mp4', fps=30, dpi=300)\n\n\ndef plot_path(path, start, goal):\n plt.figure()\n plt.plot(path[:, 0], path[:, 1], label=\"Path\")\n plt.plot(start[0], start[1], marker='o', markersize=3, label=\"Start\")\n plt.plot(goal[0], goal[1], marker='o', markersize=3, label=\"Goal\")\n plt.legend()\n plt.show()\n\n plt.figure()\n plt.plot(path[:, 0], label=\"x\")\n plt.plot(path[:, 1], label=\"y\")\n plt.plot(path[:, 2], label=\"theta\")\n plt.legend()\n plt.show()\n\n\ndef plot_controls(uvec):\n plt.figure()\n plt.plot(uvec[:, 0], label=\"u1\")\n plt.plot(uvec[:, 1], label=\"u2\")\n plt.legend()\n plt.show()\n\n\nclass Controller:\n def __init__(self):\n self.mppi = MPPI()\n self.sub_pos = rospy.Subscriber('odom',\n Odometry,\n self.pos_cb,\n queue_size=1)\n self.tw_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)\n self.done = False\n if not rospy.get_param(\"waypoints\"):\n self.parallel_park = True\n else:\n self.parallel_park = False\n self.waypoints = rospy.get_param(\"waypoints\")\n self.idx = 0\n self.init = True\n self.state = self.mppi.start\n tw = Twist()\n tw.linear.x = 0.0\n tw.angular.z = 0.0\n self.tw_pub.publish(tw)\n self.last_time = time.time()\n\n def wheelsToTwist(self, wheel_vels):\n ul = wheel_vels[0]\n ur = wheel_vels[1]\n\n vx = WHEEL_RADIUS * (ul + ur) / 2.0\n wz = WHEEL_RADIUS * (-ul + ur) / WHEEL_BASE\n return vx, wz\n\n def pos_cb(self, odom):\n try:\n # Update Start\n x = odom.pose.pose.position.x\n y = odom.pose.pose.position.y\n orn = odom.pose.pose.orientation\n orn_list = [orn.x, orn.y, orn.z, orn.w]\n theta = tf.transformations.euler_from_quaternion(orn_list)[2]\n self.mppi.start = np.array([x, y, theta])\n if self.parallel_park:\n self.mppi.goal = np.array([0.0, -1.0, 0.0])\n # Now do another round of MPPI\n if np.linalg.norm(self.mppi.start[:2] - self.mppi.goal[:2]\n ) > self.mppi.thresh and not self.init:\n self.state = self.mppi.get_path(self.mppi.start,\n self.mppi.goal)\n self.done = False\n elif self.init:\n self.mppi.initialize()\n if not self.parallel_park:\n # Goal only contains x,y\n goal = self.waypoints[self.idx]\n # Calculate theta and polulate self.goal\n theta = np.arctan2(goal[1] - self.mppi.start[1],\n goal[0] - self.mppi.start[0])\n self.mppi.goal = np.array([goal[0], goal[1], theta])\n self.state = self.mppi.start\n self.init = False\n rospy.loginfo(\"NEXT WAYPOINT: {}\".format(self.mppi.goal[:2]))\n else:\n if not self.parallel_park:\n # Goal Reached, go to next wpt\n if self.idx + 1 >= len(self.waypoints):\n self.idx = 0\n else:\n self.idx += 1\n self.mppi.initialize()\n rospy.loginfo(\n \"WAYPOINT REACHED: {} \\n NEXT WAYPOINT: {}\".format(\n self.mppi.goal[:2], self.waypoints[self.idx]))\n # Goal only contains x,y\n goal = self.waypoints[self.idx]\n # Calculate theta and polulate self.goal\n theta = np.arctan2(goal[1] - self.mppi.start[1],\n goal[0] - self.mppi.start[0])\n self.mppi.goal = np.array([goal[0], goal[1], theta])\n self.state = self.mppi.start\n else:\n self.done = True\n\n # Grab latest controls\n if not self.done:\n u = self.mppi.uvec[-1, :]\n else:\n u = np.array([0.0, 0.0])\n vx, wz = self.wheelsToTwist(u)\n tw = Twist()\n tw.linear.x = vx\n tw.angular.z = wz\n self.tw_pub.publish(tw)\n\n except rospy.ROSInterruptException:\n pass\n\n\ndef main():\n # \"\"\" The main() function. \"\"\"\n rospy.init_node('mppi', anonymous=True)\n controller = Controller()\n rospy.spin()\n # mppi = MPPI()\n # start = np.array([0.0, 0.0, np.pi / 2.0])\n # goal = np.array([1.0, 0.0, np.pi / 2.0])\n # mppi.solve_path(start, goal)\n # plot_path(mppi.path, start, goal)\n # plot_controls(mppi.uvec)\n # mppi.get_animation()\n\n\nif __name__ == '__main__':\n main()\n" }, { "path": "control/src/osqp_eigen_example.cpp", "content": "/// \\file\n/// \\brief Basic OSQP example\n\n#include \n#include \n#include \n\n#include \"control/utilities.hpp\"\n\nvoid setDynamicsMatrices(Eigen::Matrix &a,\n Eigen::Matrix &b) {\n a << 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0.,\n 0., 0.1, 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0.,\n 0.0488, 0., 0., 1., 0., 0., 0.0016, 0., 0., 0.0992, 0., 0., 0., -0.0488,\n 0., 0., 1., 0., 0., -0.0016, 0., 0., 0.0992, 0., 0., 0., 0., 0., 0., 1.,\n 0., 0., 0., 0., 0., 0.0992, 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0.,\n 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 0., 0., 0.,\n 0., 0., 0., 1., 0., 0., 0., 0.9734, 0., 0., 0., 0., 0., 0.0488, 0., 0.,\n 0.9846, 0., 0., 0., -0.9734, 0., 0., 0., 0., 0., -0.0488, 0., 0., 0.9846,\n 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.9846;\n\n b << 0., -0.0726, 0., 0.0726, -0.0726, 0., 0.0726, 0., -0.0152, 0.0152,\n -0.0152, 0.0152, -0., -0.0006, -0., 0.0006, 0.0006, 0., -0.0006, 0.0000,\n 0.0106, 0.0106, 0.0106, 0.0106, 0, -1.4512, 0., 1.4512, -1.4512, 0.,\n 1.4512, 0., -0.3049, 0.3049, -0.3049, 0.3049, -0., -0.0236, 0., 0.0236,\n 0.0236, 0., -0.0236, 0., 0.2107, 0.2107, 0.2107, 0.2107;\n}\n\nvoid setInequalityConstraints(Eigen::Matrix &xMax,\n Eigen::Matrix &xMin,\n Eigen::Matrix &uMax,\n Eigen::Matrix &uMin) {\n double u0 = 10.5916;\n\n // input inequality constraints\n uMin << 9.6 - u0, 9.6 - u0, 9.6 - u0, 9.6 - u0;\n\n uMax << 13 - u0, 13 - u0, 13 - u0, 13 - u0;\n\n // state inequality constraints\n xMin << -M_PI / 6, -M_PI / 6, -OsqpEigen::INFTY, -OsqpEigen::INFTY,\n -OsqpEigen::INFTY, -1., -OsqpEigen::INFTY, -OsqpEigen::INFTY,\n -OsqpEigen::INFTY, -OsqpEigen::INFTY, -OsqpEigen::INFTY,\n -OsqpEigen::INFTY;\n\n xMax << M_PI / 6, M_PI / 6, OsqpEigen::INFTY, OsqpEigen::INFTY,\n OsqpEigen::INFTY, OsqpEigen::INFTY, OsqpEigen::INFTY, OsqpEigen::INFTY,\n OsqpEigen::INFTY, OsqpEigen::INFTY, OsqpEigen::INFTY, OsqpEigen::INFTY;\n}\n\nvoid setWeightMatrices(Eigen::DiagonalMatrix &Q,\n Eigen::DiagonalMatrix &R) {\n Q.diagonal() << 0, 0, 10., 10., 10., 10., 0, 0, 0, 5., 5., 5.;\n R.diagonal() << 0.1, 0.1, 0.1, 0.1;\n}\n\nint main(int argc, char **argv) {\n ROS_INFO(\"STARTING NODE: osqp_example\");\n\n ros::init(argc, argv, \"osqp_example_node\"); // register the node on ROS\n ros::NodeHandle nh; // get a handle to ROS\n ros::NodeHandle nh_(\"~\"); // get a handle to ROS\n\n // OSQP Eigen Example Start:\n // https://robotology.github.io/osqp-eigen/md_pages_mpc.html\n\n // set the preview window\n int mpcWindow = 20;\n\n // allocate the dynamics matrices\n Eigen::Matrix a;\n Eigen::Matrix b;\n\n // allocate the constraints vector\n Eigen::Matrix xMax;\n Eigen::Matrix xMin;\n Eigen::Matrix uMax;\n Eigen::Matrix uMin;\n\n // allocate the weight matrices\n Eigen::DiagonalMatrix Q;\n Eigen::DiagonalMatrix R;\n\n // allocate the initial and the reference state space\n Eigen::Matrix x0;\n Eigen::Matrix xRef;\n\n // allocate QP problem matrices and vectores\n Eigen::SparseMatrix hessian;\n Eigen::VectorXd gradient;\n Eigen::SparseMatrix linearMatrix;\n Eigen::VectorXd lowerBound;\n Eigen::VectorXd upperBound;\n\n // set the initial and the desired states\n x0 << 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;\n xRef << 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0;\n\n // set MPC problem quantities\n setDynamicsMatrices(a, b);\n setInequalityConstraints(xMax, xMin, uMax, uMin);\n setWeightMatrices(Q, R);\n\n // cast the MPC problem as QP problem\n control::utilities::castMPCToQPHessian<12, 4>(Q, R, mpcWindow, hessian);\n control::utilities::castMPCToQPGradient<12, 12, 1>(Q, xRef, mpcWindow,\n gradient);\n control::utilities::castMPCToQPConstraintMatrix<12, 12, 12, 4>(\n a, b, mpcWindow, linearMatrix);\n control::utilities::castMPCToQPConstraintVectors<12, 4, 12>(\n xMax, xMin, uMax, uMin, x0, mpcWindow, lowerBound, upperBound);\n\n // instantiate the solver\n OsqpEigen::Solver solver;\n\n // settings\n // solver.settings()->setVerbosity(false);\n solver.settings()->setWarmStart(true);\n\n // set the initial data of the QP solver\n solver.data()->setNumberOfVariables(12 * (mpcWindow + 1) + 4 * mpcWindow);\n solver.data()->setNumberOfConstraints(2 * 12 * (mpcWindow + 1) +\n 4 * mpcWindow);\n if (!solver.data()->setHessianMatrix(hessian)) return 1;\n if (!solver.data()->setGradient(gradient)) return 1;\n if (!solver.data()->setLinearConstraintsMatrix(linearMatrix)) return 1;\n if (!solver.data()->setLowerBound(lowerBound)) return 1;\n if (!solver.data()->setUpperBound(upperBound)) return 1;\n\n // instantiate the solver\n if (!solver.initSolver()) return 1;\n\n // controller input and QPSolution vector\n Eigen::Vector4d ctr;\n Eigen::VectorXd QPSolution;\n\n // number of iteration steps\n int numberOfSteps = 50;\n\n double obj_val;\n\n for (int i = 0; i < numberOfSteps; i++) {\n ROS_INFO(\"---------------------------------------\\nIteration: %d\", i);\n // solve the QP problem\n if (!solver.solve()) return 1;\n\n // get the controller input\n QPSolution = solver.getSolution();\n ctr = QPSolution.block(12 * (mpcWindow + 1), 0, 4, 1);\n\n // save data into file\n // auto x0Data = x0.data();\n\n // propagate the model\n x0 = a * x0 + b * ctr;\n\n /// Get the optimal objective value, can be used to exit early.\n /// https://osqp.org/docs/interfaces/cc++?highlight=osqpworkspace#_CPPv413OSQPWorkspace\n obj_val = solver.workspace()->info->obj_val;\n\n // update the constraint bound\n control::utilities::updateConstraintVectors<12>(x0, lowerBound, upperBound);\n if (!solver.updateBounds(lowerBound, upperBound)) return 1;\n }\n\n // OSQP Eigen Example End\n\n ROS_INFO(\"Solved with obj val [%f]: Ctrl + C to end.\", obj_val);\n\n while (ros::ok()) {\n ros::spinOnce();\n }\n\n return 0;\n}\n" }, { "path": "control/src/osqp_example.cpp", "content": "/// \\file\n/// \\brief Basic OSQP example\n\n#include \n#include \n#include \n\n#include \"osqp.h\"\n\nint main(int argc, char **argv) {\n ROS_INFO(\"STARTING NODE: osqp_example\");\n\n ros::init(argc, argv, \"osqp_example_node\"); // register the node on ROS\n ros::NodeHandle nh; // get a handle to ROS\n ros::NodeHandle nh_(\"~\"); // get a handle to ROS\n\n ros::Publisher osqp_pub = nh.advertise(\"success\", 1);\n\n geometry_msgs::Point min_point;\n\n // OSQP Example Start: https://osqp.org/docs/examples/setup-and-solve.html\n // Load problem data\n c_float P_x[3] = {\n 4.0,\n 1.0,\n 2.0,\n };\n c_int P_nnz = 3;\n c_int P_i[3] = {\n 0,\n 0,\n 1,\n };\n c_int P_p[3] = {\n 0,\n 1,\n 3,\n };\n c_float q[2] = {\n 1.0,\n 1.0,\n };\n c_float A_x[4] = {\n 1.0,\n 1.0,\n 1.0,\n 1.0,\n };\n c_int A_nnz = 4;\n c_int A_i[4] = {\n 0,\n 1,\n 0,\n 2,\n };\n c_int A_p[3] = {\n 0,\n 2,\n 4,\n };\n c_float l[3] = {\n 1.0,\n 0.0,\n 0.0,\n };\n c_float u[3] = {\n 1.0,\n 0.7,\n 0.7,\n };\n c_int n = 2;\n c_int m = 3;\n\n // Exitflag\n c_int exitflag = 0;\n\n // Workspace structures\n OSQPWorkspace *work;\n OSQPSettings *settings = (OSQPSettings *)c_malloc(sizeof(OSQPSettings));\n OSQPData *data = (OSQPData *)c_malloc(sizeof(OSQPData));\n\n // Populate data\n if (data) {\n data->n = n;\n data->m = m;\n data->P = csc_matrix(data->n, data->n, P_nnz, P_x, P_i, P_p);\n data->q = q;\n data->A = csc_matrix(data->m, data->n, A_nnz, A_x, A_i, A_p);\n data->l = l;\n data->u = u;\n }\n\n // Define solver settings as default\n if (settings) {\n osqp_set_default_settings(settings);\n settings->alpha = 1.0; // Change alpha parameter\n }\n\n // Setup workspace\n exitflag = osqp_setup(&work, data, settings);\n\n // Solve Problem\n osqp_solve(work);\n\n // Cleanup\n if (data) {\n if (data->A) c_free(data->A);\n if (data->P) c_free(data->P);\n c_free(data);\n }\n if (settings) c_free(settings);\n\n // OSQP Example End\n\n min_point.x = work->solution->x[0];\n min_point.y = work->solution->x[1];\n\n ROS_INFO(\"Calculated min: [%f, %f]\", min_point.x, min_point.y);\n\n while (ros::ok()) {\n ros::spinOnce();\n osqp_pub.publish(min_point);\n }\n\n return exitflag;\n}\n" }, { "path": "global_planner/CMakeLists.txt", "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(global_planner)\n\n## Compile as C++11, supported in ROS Kinetic and newer\nadd_compile_options(-Wall -Wextra -pipe -Wno-psabi)\n\nset(CMAKE_CXX_STANDARD 17)\nset(CMAKE_CXX_STANDARD_REQUIRED ON)\nset(CMAKE_CXX_EXTENSIONS OFF)\n\n## Find catkin macros and libraries\n## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)\n## is used, also find other catkin packages\nfind_package(catkin REQUIRED COMPONENTS\n map\t\n message_generation\n message_runtime\n rigid2d\n roscpp\n rostest\n nuslam\n visualization_msgs\n)\n\nfind_package(Eigen3 3.3 REQUIRED NO_MODULE)\n\n## System dependencies are found with CMake's conventions\n# find_package(Boost REQUIRED COMPONENTS system)\n\n\n## Uncomment this if the package has a setup.py. This macro ensures\n## modules and global scripts declared therein get installed\n## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html\n# catkin_python_setup()\n\n################################################\n## Declare ROS messages, services and actions ##\n################################################\n\n## To declare and build messages, services or actions from within this\n## package, follow these steps:\n## * Let MSG_DEP_SET be the set of packages whose message types you use in\n## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).\n## * In the file package.xml:\n## * add a build_depend tag for \"message_generation\"\n## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET\n## * If MSG_DEP_SET isn't empty the following dependency has been pulled in\n## but can be declared for certainty nonetheless:\n## * add a exec_depend tag for \"message_runtime\"\n## * In this file (CMakeLists.txt):\n## * add \"message_generation\" and every package in MSG_DEP_SET to\n## find_package(catkin REQUIRED COMPONENTS ...)\n## * add \"message_runtime\" and every package in MSG_DEP_SET to\n## catkin_package(CATKIN_DEPENDS ...)\n## * uncomment the add_*_files sections below as needed\n## and list every .msg/.srv/.action file to be processed\n## * uncomment the generate_messages entry below\n## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)\n\n## Generate messages in the 'msg' folder\n# add_message_files(\n# FILES\n# Message1.msg\n# Message2.msg\n# )\n\n## Generate services in the 'srv' folder\n# add_service_files(\n# FILES\n# Service1.srv\n# Service2.srv\n# )\n\n## Generate actions in the 'action' folder\n# add_action_files(\n# FILES\n# Action1.action\n# Action2.action\n# )\n\n## Generate added messages and services with any dependencies listed here\n# generate_messages(\n# DEPENDENCIES\n# std_msgs # Or other packages containing msgs\n# )\n\n################################################\n## Declare ROS dynamic reconfigure parameters ##\n################################################\n\n## To declare and build dynamic reconfigure parameters within this\n## package, follow these steps:\n## * In the file package.xml:\n## * add a build_depend and a exec_depend tag for \"dynamic_reconfigure\"\n## * In this file (CMakeLists.txt):\n## * add \"dynamic_reconfigure\" to\n## find_package(catkin REQUIRED COMPONENTS ...)\n## * uncomment the \"generate_dynamic_reconfigure_options\" section below\n## and list every .cfg file to be processed\n\n## Generate dynamic reconfigure parameters in the 'cfg' folder\n# generate_dynamic_reconfigure_options(\n# cfg/DynReconf1.cfg\n# cfg/DynReconf2.cfg\n# )\n\n###################################\n## catkin specific configuration ##\n###################################\n## The catkin_package macro generates cmake config files for your package\n## Declare things to be passed to dependent projects\n## INCLUDE_DIRS: uncomment this if your package contains header files\n## LIBRARIES: libraries you create in this project that dependent projects also need\n## CATKIN_DEPENDS: catkin_packages dependent projects also need\n## DEPENDS: system dependencies of this project that dependent projects also need\ncatkin_package(\nINCLUDE_DIRS include\nLIBRARIES ${PROJECT_NAME} map rigid2d nuslam\nCATKIN_DEPENDS map message_generation message_runtime rigid2d roscpp nuslam visualization_msgs\n# DEPENDS system_lib\n)\n\n###########\n## Build ##\n###########\n\n## Specify additional locations of header files\n## Your package locations should be listed before other locations\ninclude_directories(\n include\n ${catkin_INCLUDE_DIRS}\n ${Eigen3_INCLUDE_DIRS}\n)\n\nadd_library(${PROJECT_NAME}\n src/${PROJECT_NAME}/${PROJECT_NAME}.cpp\n src/${PROJECT_NAME}/heuristic.cpp\n src/${PROJECT_NAME}/incremental.cpp\n src/${PROJECT_NAME}/potential_field.cpp\n)\n\n## Add cmake target dependencies of the library\n## as an example, code may need to be generated before libraries\n## either from message generation or dynamic reconfigure\nadd_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})\n\n## Declare a C++ executable\n## With catkin_make all packages are built within a single CMake context\n## The recommended prefix ensures that target names across packages don't collide\n# add_executable(${PROJECT_NAME}_node src/map_node.cpp)\nadd_executable(astar src/astar.cpp)\nadd_executable(lpastar src/lpastar.cpp)\nadd_executable(dsl src/dsl.cpp)\nadd_executable(pf src/pf.cpp)\n\n## Rename C++ executable without prefix\n## The above recommended prefix causes long target names, the following renames the\n## target back to the shorter version for ease of user use\n## e.g. \"rosrun someones_pkg node\" instead of \"rosrun someones_pkg someones_pkg_node\"\n# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX \"\")\nset_target_properties(astar PROPERTIES OUTPUT_NAME astar PREFIX \"\")\nset_target_properties(lpastar PROPERTIES OUTPUT_NAME lpastar PREFIX \"\")\nset_target_properties(dsl PROPERTIES OUTPUT_NAME dsl PREFIX \"\")\nset_target_properties(pf PROPERTIES OUTPUT_NAME pf PREFIX \"\")\n\n## Add cmake target dependencies of the executable\n## same as for the library above\n# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})\nadd_dependencies(astar ${${PROJECT_NAME}_EXPORTED_TARGETS} ${Eigen3_EXPORTED_TARGETS} ${map_EXPORTED_TARGETS} ${rigid2d_EXPORTED_TARGETS} ${nuslam_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})\nadd_dependencies(lpastar ${${PROJECT_NAME}_EXPORTED_TARGETS} ${Eigen3_EXPORTED_TARGETS} ${map_EXPORTED_TARGETS} ${rigid2d_EXPORTED_TARGETS} ${nuslam_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})\nadd_dependencies(dsl ${${PROJECT_NAME}_EXPORTED_TARGETS} ${Eigen3_EXPORTED_TARGETS} ${map_EXPORTED_TARGETS} ${rigid2d_EXPORTED_TARGETS} ${nuslam_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})\nadd_dependencies(pf ${${PROJECT_NAME}_EXPORTED_TARGETS} ${Eigen3_EXPORTED_TARGETS} ${map_EXPORTED_TARGETS} ${rigid2d_EXPORTED_TARGETS} ${nuslam_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})\n\n## Specify libraries to link a library or executable target against\ntarget_link_libraries(\nastar\n${map_LIBRARIES}\n${rigid2d_LIBRARIES}\n${nuslam_LIBRARIES}\nEigen3::Eigen\n${PROJECT_NAME}\n${catkin_LIBRARIES}\n)\n\ntarget_link_libraries(\nlpastar\n${map_LIBRARIES}\n${rigid2d_LIBRARIES}\n${nuslam_LIBRARIES}\nEigen3::Eigen\n${PROJECT_NAME}\n${catkin_LIBRARIES}\n)\n\ntarget_link_libraries(\ndsl\n${map_LIBRARIES}\n${rigid2d_LIBRARIES}\n${nuslam_LIBRARIES}\nEigen3::Eigen\n${PROJECT_NAME}\n${catkin_LIBRARIES}\n)\n\ntarget_link_libraries(\npf\n${map_LIBRARIES}\n${rigid2d_LIBRARIES}\n${nuslam_LIBRARIES}\nEigen3::Eigen\n${PROJECT_NAME}\n${catkin_LIBRARIES}\n)\n\n#############\n## Install ##\n#############\n\n# all install targets should use catkin DESTINATION variables\n# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html\n\n## Mark executable scripts (Python etc.) for installation\n## in contrast to setup.py, you can choose the destination\n# install(PROGRAMS\n# scripts/my_python_script\n# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}\n# )\n\n## Mark executables for installation\n## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html\ninstall(TARGETS astar lpastar dsl pf\n RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}\n)\n\n## Mark libraries for installation\n## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html\ninstall(TARGETS ${PROJECT_NAME}\n ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}\n LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}\n RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}\n)\n\n## Mark cpp header files for installation\ninstall(DIRECTORY include/${PROJECT_NAME}/\n DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}\n FILES_MATCHING PATTERN \"*.h\"\n PATTERN \".svn\" EXCLUDE\n)\n\n## Mark other files for installation (e.g. launch and bag files, etc.)\n# install(FILES\n# # myfile1\n# # myfile2\n# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}\n# )\n\n#############\n## Testing ##\n#############\n\n## Add gtest based cpp test target and link libraries\n# catkin_add_gtest(${PROJECT_NAME}-test test/${PROJECT_NAME}_test.cpp)\n# if(TARGET ${PROJECT_NAME}-test)\n# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})\n# endif()\n\n## Add folders to be run by python nosetests\n# catkin_add_nosetests(test)\n\n# if (CATKIN_ENABLE_TESTING)\n# catkin_add_gtest(${PROJECT_NAME}_test tests/${PROJECT_NAME}_test.cpp)\n# target_link_libraries(${PROJECT_NAME}_test ${catkin_Libraries} gtest_main ${PROJECT_NAME})\n# endif()\n" }, { "path": "global_planner/config/path.yaml", "content": "# 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.5]\ngoal: [31.5, 36.0]" }, { "path": "global_planner/include/global_planner/global_planner.hpp", "content": "#ifndef GLOBAL_PLANNER_INCLUDE_GUARD_HPP\n#define GLOBAL_PLANNER_INCLUDE_GUARD_HPP\n/// \\file\n/// \\brief global_planners library which contains basic planner structure: nodes, neighbours, paths etc.\n/// NOTE: rigid2d is from turtlebot3_from_scratch (tb3 in nuturtle.rosinstall)\n#include \n#include \n#include \n#include \n#include \n#include \n\nnamespace global\n{\n // Used to store Obstacle vertex coordinates\n using rigid2d::Vector2D;\n using map::Vertex;\n using map::Cell;\n using map::Index;\n using map::Obstacle;\n\n /// \\brief stores Obstacle(s) to construct basic Planner\n class GlobalPlanner\n {\n public:\n /// \\brief the default constructor creates an empty Planner\n GlobalPlanner();\n\n\n // protected instead of private so that child Class can access\n protected:\n std::vector PRM;\n\n std::vector GRID;\n\n // Map obstacles\n std::vector obstacles;\n\n // Map maximum coordinatesin x,y\n Vector2D map_max;\n\n // Map minimum coordinatesin x,y\n Vector2D map_min;\n\n // Robot's inflation radius for collision checking\n double inflate_robot;\n };\n}\n\n#endif\n" }, { "path": "global_planner/include/global_planner/heuristic.hpp", "content": "#ifndef HEURISTIC_INCLUDE_GUARD_HPP\n#define HEURISTIC_INCLUDE_GUARD_HPP\n/// \\file\n/// \\brief Heuristic search library to encompass A*, Theta* planners.\n\n#include \"global_planner/global_planner.hpp\"\n#include \n#include \n\nnamespace global\n{\n // Used to store Obstacle vertex coordinates\n using rigid2d::Vector2D;\n using map::Vertex;\n using map::Cell;\n using map::Index;\n using map::Grid;\n\n // \\brief contains relevant information for heuristic planners\n struct Node\n {\n // Empty constructor\n Node();\n\n // Stores position and other attributes wrt PRM map\n Vertex vertex = Vertex(Vector2D());\n\n // Stores position and other attributes wrt Grid Map\n Cell cell = Cell(Vector2D(), 0.0);\n\n // row-major index for Grid or connected Vertex ID for PRM\n int parent_id = -1;\n\n // node ID for generic find in both PRM and Grid\n int id = -1;\n\n // Keys are for incremental search\n double key1 = 0.0;\n double key2 = 0.0;\n double rhs = 1e12;\n\n // NOTE: gcost will be initialized to 1e12 (inf) for LPA* and D* Lite\n double gcost, hcost, fcost = 0.0;\n\n // Store children for update using incremental gridmap\n std::vector children_ids;\n };\n\n // Bolean operator so that std::find can be used with struct sub-element in closed list\n // inline so as to not confuse linker with multiple definitions\n inline bool operator<(const Node & n, const int & id) { return n.id < id; }\n inline bool operator<(const int & id, const Node & n) { return id < n.id; }\n inline bool operator<(const Node & n1, const Node & n2) { return n1.id < n2.id; }\n\n // \\brief functor (function object) which compares the costs of two Nodes for heap sorting \n class HeapComparator\n { \n public: \n int operator() (const Node& n1, const Node& n2) \n { \n if (rigid2d::almost_equal(n1.fcost, n2.fcost))\n {\n return n1.hcost > n2.hcost;\n } else\n {\n return n1.fcost > n2.fcost;\n }\n } \n };\n\n /// \\brief A* Planner\n class Astar : public GlobalPlanner\n {\n public:\n /// \\brief constructor to initialize the A* Planner\n Astar(const std::vector & obstacles_, const double & inflate_robot_);\n\n // \\brief Plans a path on a PRM.\n // \\param start: the starting coordinates\n // \\param goal: the goal coordinates\n // \\param map: the PRM - may be modified if shorter paths are found\n // \\returns: the path as a vector of Nodes\n virtual std::vector plan(const Vector2D & start, const Vector2D & goal, std::vector & map);\n\n // \\brief potentially modify the g cost and parent of a Node in PRM. virtual so it can be overriden by Thetastar.\n // \\param open_list: list containing nodes to evaluate. Not const because it can be modified\n // \\param neighbour: Node in the open list being potentially modified (also not const)\n virtual void update_vtx(std::priority_queue , HeapComparator > & open_list, Node & neighbour, const Node & current_node);\n\n // \\brief insert a Node into the open list as is defined in the A* method for PRM\n // \\param open_list: list containing nodes to evaluate. Not const because it can be modified\n // \\param neighbour: Node to be added to open list (also not const)\n virtual void create_vtx(std::priority_queue , HeapComparator > & open_list, Node & neighbour, const Node & current_node);\n\n // \\brief returns the path planned on the PRM or GRID\n // \\param closed_list: Nodes to traverse\n std::vector trace_path(const Node & final_node, const std::set> & closed_list);\n\n // \\brief Plans a path on a Grid.\n // \\param start: the starting coordinates\n // \\param goal: the goal coordinates\n // \\param map: the Grid Map\n // \\returns: the path as a vector of Nodes\n std::vector plan(const Vector2D & start, const Vector2D & goal, const map::Grid & grid_, const double & resolution);\n\n // \\brief potentially modify the g cost and parent of a Node in GRID. virtual so it can be overriden by Thetastar.\n // \\param open_list: list containing nodes to evaluate. Not const because it can be modified\n // \\param neighbour: Node in the open list being potentially modified (also not const)\n virtual void update_cell(std::priority_queue , HeapComparator > & open_list, Node & neighbour, const Node & current_node);\n\n // \\brief insert a Node into the open list as is defined in the A* method for GRID\n // \\param open_list: list containing nodes to evaluate. Not const because it can be modified\n // \\param neighbour: Node to be added to open list (also not const)\n virtual void create_cell(std::priority_queue , HeapComparator > & open_list, Node & neighbour, const Node & current_node);\n\n // \\brief Computes the heuristic to the goal using a custom admissible heuristic optimized for 8-point connectivity\n // NOTE: distance between nodes is simply computed using euclidean distance\n // \\param n1: start Node for heuristic\n // \\param n2: end Node for heuristic\n // \\returns the grid distance\n double heuristic(const Node & n1, const Node & n2);\n\n // \\brief retrieves the 8 neighbours of a node in a grid\n // \\param n: Node whose neighbours to retrieve\n // \\returns: vector of Nodes that are n's neighbours\n std::vector get_neighbours(const Node & n, const std::vector & map);\n };\n\n /// \\brief Theta* Planner\n class Thetastar : public Astar\n {\n public:\n\n // Inherit constructor from A*\n using Astar::Astar;\n\n // \\brief insert a Node into the open list as is defined in the A* method for PRM\n // \\param open_list: list containing nodes to evaluate. Not const because it can be modified\n // \\param neighbour: Node to be added to open list (also not const)\n void create_vtx(std::priority_queue , HeapComparator > & open_list, Node & neighbour, const Node & current_node) override;\n\n // \\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\n // \\param open_list: list containing nodes to evaluate. Not const because it can be modified\n // \\param neighbour: Node in the open list being potentially modified (also not const)\n void update_vtx(std::priority_queue , HeapComparator > & open_list, Node & neighbour, const Node & current_node) override;\n };\n\n\n // \\brief returns the vertex that most closely matches the given cartesian coordinates in PRM\n // \\param position: the coordinates\n // \\param map: the map whose elements are being searched for coordinates\n Vertex find_nearest_node(const Vector2D & position, const std::vector & map);\n\n // \\brief returns the vertex that most closely matches the given cartesian coordinates in GRID\n // \\param position: the coordinates\n // \\param map: the map whose elements are being searched for coordinates\n Cell find_nearest_node(const Vector2D & position, const Grid & grid, const double & resolution);\n}\n\n#endif\n" }, { "path": "global_planner/include/global_planner/incremental.hpp", "content": "#ifndef INCREMENTAL_INCLUDE_GUARD_HPP\n#define INCREMENTAL_INCLUDE_GUARD_HPP\n/// \\file\n/// \\brief Incremental search library to encompass LPA* and D* Lite planners. I might add Incr. Phi* in the future.\n\n#include \"global_planner/heuristic.hpp\"\n\nnamespace global\n{\n // Used to store Obstacle vertex coordinates\n using rigid2d::Vector2D;\n using map::Vertex;\n using map::Cell;\n using map::Index;\n using map::Grid;\n\n // \\brief functor (function object) which compares the priorities of two Nodes for heap sorting \n class KeyComparator\n { \n public: \n int operator() (const Node& n1, const Node& n2) \n { \n if (rigid2d::almost_equal(n1.key1, n2.key1))\n {\n return n1.key2 > n2.key2;\n } else\n {\n return n1.key1 > n2.key1;\n }\n } \n }; \n\n\n // \\brief functor (function object) which compares the costs of the predecessor of a Node\n class CostComparator\n { \n public: \n int operator() (const Node& n1, const Node& n2) \n { \n return n1.gcost > n2.gcost;\n } \n }; \n\n /// \\brief LPA* Planner\n class LPAstar : public Astar\n {\n public:\n // Inherit constructor from A*\n using Astar::Astar;\n\n // NOTE: The method names below directly reference the LPA* pseudocode: http://idm-lab.org/bib/abstracts/papers/aaai02b.pdf\n\n // \\brief Plans an incremental path on a Grid.\n // \\returns: the path as a vector of Nodes\n void ComputeShortestPath();\n\n // Update the cost of a cell and remove it from the open list if it's there\n // param n: the node to update\n void UpdateCell(Node & n);\n\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.\n // Also inserts start (locally inconsistent vertex) into the (otherwise empty) priority queue.\n // This guarantees that the first run of ComputeShortestPath performs an exact A* search.\n virtual void Initialize(const Vector2D & start, const Vector2D & goal, const Grid & grid_, const double & resolution);\n\n // Calculates priorities 1 and 2 of a node, used in sorting for LPA*\n // \\param n: the node whose keys we calculate\n void CalculateKeys(Node & n);\n\n // \\brief Termination condition for ComputeShortestPath method.\n bool Continue(const int & iterations);\n\n // \\brief traces the most up-to-date path\n std::vector trace_path(const Node & final_node);\n\n // \\brief retrieves the 8 neighbours of a node in a grid of Nodes\n // \\param n: Node whose neighbours to retrieve\n // \\returns: vector of Nodes that are n's neighbours\n std::vector get_neighbours(const Node & n, const std::vector & map);\n\n // \\brief returns the most current path\n // \\returns: vector of Node\n virtual std::vector return_path();\n\n // \\brief update FakeGrid (internal perception) with updated grid and then\n // use this new information to update each affected Node, and hence the path\n // param updated_grid: the updated grid\n // \\returns: nodes that had to be updated based on new information\n virtual std::vector SimulateUpdate(const std::vector & updated_grid);\n\n // \\brief returns whether the current path is valid\n bool return_valid();\n\n protected:\n std::vector path;\n // Fake grid with limited visibility for simulating increment\n std::vector FakeGrid;\n\n // TODO: DELETE\n // Open list used for finding existing IDs\n // std::set open_list_v;\n // // Open List\n // std::priority_queue , KeyComparator > open_list;\n\n // Open List. Uses ref wrapper for faster execution\n std::vector open_list; \n\n // For easy management/record keeping\n Node start_node;\n Node goal_node;\n\n // For tracing most up-to-date path\n Node top_node;\n\n // Number of visible cells added per increment\n int viz_cells = 1;\n\n double BIG_NUM = std::numeric_limits::infinity();\n\n bool valid_path = true;\n };\n\n\n // \\brief D* Lite Planner\n class DSL: public LPAstar\n {\n using LPAstar::LPAstar;\n\n public:\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.\n // Also inserts start (locally inconsistent vertex) into the (otherwise empty) priority queue.\n // This guarantees that the first run of ComputeShortestPath performs an exact A* search.\n // NOTE: flips start and goal for D*Lite so that everything else stays the same\n void Initialize(const Vector2D & start, const Vector2D & goal, const Grid & grid_, const double & resolution);\n\n // \\brief update FakeGrid (internal perception) with updated grid and then\n // use this new information to update each affected Node, and hence the path\n // param updated_grid: the updated grid\n // \\returns: nodes that had to be updated based on new information\n std::vector SimulateUpdate(const std::vector & updated_grid);\n\n // \\brief returns the most current path. Flips path returned by LPAstar since D*L plans the opposite way\n // \\returns: vector of Node\n std::vector return_path();\n\n };\n}\n\n#endif\n" }, { "path": "global_planner/include/global_planner/potential_field.hpp", "content": "#ifndef POTENTIAL_FIELD_INCLUDE_GUARD_HPP\n#define POTENTIAL_FIELD_INCLUDE_GUARD_HPP\n/// \\file\n/// \\brief Potential Field planning library.\n#include \n\nnamespace global\n{\n // Used to store Obstacle vertex coordinates\n using rigid2d::Vector2D;\n using map::Vertex;\n using map::Cell;\n using map::Index;\n using map::Obstacle;\n\n /// \\brief Potential Field (online-capable) planner\n class PotentialField : public GlobalPlanner\n {\n public:\n /// \\brief constructor to initialize the Potential Field Planner\n // param obstacles_: the obstacle vectors containing bocked vertices in the map\n // param eta_: the termination condition for gradient descent. Also the descent direction\n // param zeta_: the attractive factor multiplier\n // param ada_: the repulsive factor multiplier\n // param d_thresh_: threshold for transition between quadratic and conic potential\n // param Q_thresh_: range of influence of obstacle\n PotentialField(const std::vector & obstacles_, const double & eta_, const double & ada_,\n const double & zeta_, const double & d_thresh_, const double & Q_thresh_);\n\n // \\brief Calculates Attractive Gradient towards goal\n // \\param cur_pos: the robot's current position, used for gradient computation\n // \\param goal: the goal position, used for gradient computation\n // \\returns: Vector2D containing gradient in x and y dimensions\n Vector2D AttractiveGradient(const Vector2D & cur_pos, const Vector2D & goal);\n\n // \\brief Calculates Repulsive Gradient (cummulative) based on all obstacles\n // \\param cur_pos: the robot's current position, used for gradient computation\n // \\param obs: vector of Obstacle used for gradient computation. may be full map or reduced for\n // simulated visibility\n // \\returns: Vector2D containing gradient in x and y dimensions\n Vector2D RepulsiveGradient(const Vector2D & cur_pos, const std::vector & obs);\n\n // \\brief find the closest point between a robot position and an obstacle\n // \\param cur_pos: the robot's current position, used for gradient computation\n // \\param vertices: vertices of an obstacle that we will use to find the closest point\n // \\returns: Vector2D containing closest point's x,y coordinates\n Vector2D FindClosestPoint(const Vector2D & cur_pos, const std::vector & vertices);\n\n // \\brief returns the multi-dimensional norm of our gradient\n // \\param partials: the partial gradients in each dimension\n // \\returns: the norm\n double MultiDimNorm(const std::vector partials);\n\n // \\brief perform Gradient Descent for one step to move closer to the goal\n // \\param cur_pos: the robot's current position, used for gradient computation\n // \\param goal: the goal position, used for gradient computation\n // \\param obs: vector of Obstacle used for gradient computation. may be full map or reduced for\n // simulated visibility\n // \\returns: the robot's new x,y coordinates after Gradient Descent\n Vector2D OneStepGD(const Vector2D & cur_pos, const Vector2D & goal, std::vector & obs);\n\n // \\brief returns whether we have reached the goal\n // \\returns: boolean to indicate termination\n bool return_terminate();\n\n\n // protected instead of private so that child Class can access\n protected:\n double eta;\n double zeta;\n double ada;\n double d_thresh;\n double Q_thresh;\n bool terminate = false;\n };\n}\n\n#endif\n" }, { "path": "global_planner/launch/astar.launch", "content": "\n \n\n \n \n\n\n \n \n\n \n \n \n \n \n \n \n \n\n \n \n\n \n\n \n\n\n \n \n \n \n \n\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n\n" }, { "path": "global_planner/launch/incremental.launch", "content": "\n \n\n \n \n\n\n \n \n\n \n \n \n \n \n \n \n \n\n \n \n\n \n\n \n\n\n \n \n \n \n \n\n\n\n \n \n \n \n \n \n \n \n \n \n \n\n\n \n \n \n \n \n \n \n \n \n \n \n \n\n\n" }, { "path": "global_planner/launch/potential_field.launch", "content": "\n \n\n \n \n\n\n \n \n\n \n \n \n \n \n \n \n \n\n \n \n\n \n\n \n\n\n \n \n \n \n \n\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n\n" }, { "path": "global_planner/package.xml", "content": "\n\n global_planner\n 0.0.1\n Heuristic Search and Potential Field global planners\n\n \n \n \n Maurice Rahme\n\n\n \n \n \n MIT\n\n\n\n \n \n \n \n\n\n \n \n \n \n\n\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n catkin\n map\n message_generation\n message_runtime\n rigid2d\n roscpp\n nuslam\n visualization_msgs\n map\n message_generation\n message_runtime\n rigid2d\n nuslam\n roscpp\n visualization_msgs\n map\n message_runtime\n roscpp\n rigid2d\n nuslam\n visualization_msgs\n\n rostest\n rosunit\n \n \n \n\n \n\n" }, { "path": "global_planner/rviz/astar.rviz", "content": "Panels:\n - Class: rviz/Displays\n Help Height: 77\n Name: Displays\n Property Tree Widget:\n Expanded:\n - /Global Options1\n - /Grid1\n - /TF1/Frames1\n - /TF1/Tree1\n - /Obstacles1\n - /PRM1\n - /PATH1\n - /PATH DEBUG1\n - /Path1\n Splitter Ratio: 0.5\n Tree Height: 714\n - Class: rviz/Selection\n Name: Selection\n - Class: rviz/Tool Properties\n Expanded:\n - /2D Pose Estimate1\n - /2D Nav Goal1\n - /Publish Point1\n Name: Tool Properties\n Splitter Ratio: 0.5886790156364441\n - Class: rviz/Views\n Expanded:\n - /Current View1\n Name: Views\n Splitter Ratio: 0.5\n - Class: rviz/Time\n Experimental: false\n Name: Time\n SyncMode: 0\n SyncSource: \"\"\nPreferences:\n PromptSaveOnExit: true\nToolbars:\n toolButtonStyle: 2\nVisualization Manager:\n Class: \"\"\n Displays:\n - Alpha: 0.5\n Cell Size: 0.10000000149011612\n Class: rviz/Grid\n Color: 85; 87; 83\n Enabled: true\n Line Style:\n Line Width: 0.029999999329447746\n Value: Lines\n Name: Grid\n Normal Cell Count: 0\n Offset:\n X: 0\n Y: 0\n Z: 0\n Plane: XY\n Plane Cell Count: 200\n Reference Frame: \n Value: true\n - Class: rviz/TF\n Enabled: true\n Frame Timeout: 15\n Frames:\n All Enabled: false\n ball_caster:\n Value: false\n base_footprint:\n Value: false\n base_link:\n Value: false\n base_scan:\n Value: true\n rl_wheel:\n Value: false\n rr_wheel:\n Value: false\n Marker Scale: 1\n Name: TF\n Show Arrows: true\n Show Axes: true\n Show Names: true\n Tree:\n base_footprint:\n base_link:\n ball_caster:\n {}\n base_scan:\n {}\n rl_wheel:\n {}\n rr_wheel:\n {}\n Update Interval: 0\n Value: true\n - Alpha: 1\n Class: rviz/RobotModel\n Collision Enabled: false\n Enabled: true\n Links:\n All Links Enabled: true\n Expand Joint Details: false\n Expand Link Details: false\n Expand Tree: false\n Link Tree Style: Links in Alphabetic Order\n ball_caster:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n base_footprint:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n base_link:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n base_scan:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n rl_wheel:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n rr_wheel:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n Name: RobotModel\n Robot Description: robot_description\n TF Prefix: \"\"\n Update Interval: 0\n Value: true\n Visual Enabled: true\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /map\n Name: Obstacles\n Namespaces:\n \"\": true\n Queue Size: 100\n Value: true\n - Class: rviz/MarkerArray\n Enabled: false\n Marker Topic: /prm\n Name: PRM\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /path\n Name: PATH\n Namespaces:\n \"\": true\n Queue Size: 100\n Value: true\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /path_debug\n Name: PATH DEBUG\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n - Alpha: 0.699999988079071\n Class: rviz/Map\n Color Scheme: map\n Draw Behind: false\n Enabled: true\n Name: GridMap\n Topic: /grid_map\n Unreliable: false\n Use Timestamp: false\n Value: true\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 214; 109; 13\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Billboards\n Line Width: 0.029999999329447746\n Name: Path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /pf/robot_path\n Unreliable: false\n Value: true\n Enabled: true\n Global Options:\n Background Color: 48; 48; 48\n Default Light: true\n Fixed Frame: base_footprint\n Frame Rate: 30\n Name: root\n Tools:\n - Class: rviz/Interact\n Hide Inactive Objects: true\n - Class: rviz/MoveCamera\n - Class: rviz/Select\n - Class: rviz/FocusCamera\n - Class: rviz/Measure\n - Class: rviz/SetInitialPose\n Theta std deviation: 0.2617993950843811\n Topic: /initialpose\n X std deviation: 0.5\n Y std deviation: 0.5\n - Class: rviz/SetGoal\n Topic: /move_base_simple/goal\n - Class: rviz/PublishPoint\n Single click: true\n Topic: /clicked_point\n Value: true\n Views:\n Current:\n Class: rviz/Orbit\n Distance: 6.352022171020508\n Enable Stereo Rendering:\n Stereo Eye Separation: 0.05999999865889549\n Stereo Focal Distance: 1\n Swap Stereo Eyes: false\n Value: false\n Focal Point:\n X: 1.7924532890319824\n Y: 2.2708258628845215\n Z: 0.14110127091407776\n Focal Shape Fixed Size: true\n Focal Shape Size: 0.05000000074505806\n Invert Z Axis: false\n Name: Current View\n Near Clip Distance: 0.009999999776482582\n Pitch: 1.5697963237762451\n Target Frame: \n Value: Orbit (rviz)\n Yaw: 4.708594799041748\n Saved: ~\nWindow Geometry:\n Displays:\n collapsed: false\n Height: 1004\n Hide Left Dock: false\n Hide Right Dock: true\n QMainWindow State: 000000ff00000000fd00000004000000000000015600000352fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000352000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000352fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b00000352000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004680000003efc0100000002fb0000000800540069006d00650100000000000004680000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000030c0000035200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000\n Selection:\n collapsed: false\n Time:\n collapsed: false\n Tool Properties:\n collapsed: false\n Views:\n collapsed: true\n Width: 1128\n X: 167\n Y: 0\n" }, { "path": "global_planner/src/astar.cpp", "content": "/// \\file\n/// \\brief Draws Each Obstacle in RViz using MarkerArrays\n///\n/// PARAMETERS:\n/// PUBLISHES:\n/// SUBSCRIBES:\n/// FUNCTIONS:\n\n#include \n\n#include \n#include \n#include \n\n#include \"map/map.hpp\"\n#include \"map/prm.hpp\"\n#include \"map/grid.hpp\"\n#include \n#include \"global_planner/heuristic.hpp\"\n\n#include \"nuslam/TurtleMap.h\"\n\n#include // To use std::bind\n#include \n#include \n#include \n\n// Used to deal with YAML list of lists\n#include // catkin component\n\n\nint main(int argc, char** argv)\n/// The Main Function ///\n{\n ROS_INFO(\"STARTING NODE: astar\");\n\n // Vars\n double frequency = 10.0;\n // Scale by which to transform marker poses (10 cm/cell so we use 10)\n double SCALE = 10.0;\n std::string frame_id = \"base_footprint\";\n std::string planner_type = \"astar\";\n std::string map_type = \"prm\";\n XmlRpc::XmlRpcValue xml_obstacles;\n std::vector start_vec{7.0, 3.0};\n std::vector goal_vec{7.0, 26.0};\n // resolution by which to transform marker poses (10 cm/cell so we use 10)\n double resolution = 0.01;\n\n // PRM Parameters\n int n = 10;\n int k = 5;\n double thresh = 0.01;\n double inflate = 0.1;\n\n // store Obstacle(s) here to create Map\n std::vector obstacles_v;\n\n ros::init(argc, argv, \"astar_node\"); // register the node on ROS\n ros::NodeHandle nh; // get a handle to ROS\n ros::NodeHandle nh_(\"~\"); // get a handle to ROS\n // Parameters\n nh_.getParam(\"frequency\", frequency);\n nh_.getParam(\"obstacles\", xml_obstacles);\n nh_.getParam(\"start\", start_vec);\n nh_.getParam(\"goal\", goal_vec);\n nh_.getParam(\"map_frame_id\", frame_id);\n nh_.getParam(\"n\", n);\n nh_.getParam(\"k\", k);\n nh_.getParam(\"thresh\", thresh);\n nh_.getParam(\"inflate\", inflate);\n nh_.getParam(\"scale\", SCALE);\n nh_.getParam(\"planner\", planner_type);\n nh_.getParam(\"map_type\", map_type);\n nh_.getParam(\"resolution\", resolution);\n\n rigid2d::Vector2D start(start_vec.at(0)/SCALE, start_vec.at(1)/SCALE);\n rigid2d::Vector2D goal(goal_vec.at(0)/SCALE, goal_vec.at(1)/SCALE);\n\n // 'obstacles' is a triple-nested list.\n // 1st level: obstacle (Obstacle), 2nd level: vertices (std::vector), 3rd level: coordinates (Vector2D)\n\n // std::vector\n if(xml_obstacles.getType() != XmlRpc::XmlRpcValue::TypeArray)\n {\n ROS_ERROR(\"There is no list of obstacles\");\n } else\n {\n for(int i = 0; i < xml_obstacles.size(); ++i)\n {\n // Obstacle contains std::vector\n // create Obstacle with empty vertex vector\n map::Obstacle obs;\n if(xml_obstacles[i].getType() != XmlRpc::XmlRpcValue::TypeArray)\n {\n ROS_ERROR(\"obstacles[%d] has no vertices\", i);\n } else {\n for(int j = 0; j < xml_obstacles[i].size(); ++j)\n {\n // Vector2D contains x,y coords\n if(xml_obstacles[i][j].size() != 2)\n {\n ROS_ERROR(\"Vertex[%d] of obstacles[%d] is not a pair of coordinates\", j, i);\n } else if(\n xml_obstacles[i][j][0].getType() != XmlRpc::XmlRpcValue::TypeDouble or\n xml_obstacles[i][j][1].getType() != XmlRpc::XmlRpcValue::TypeDouble)\n {\n ROS_ERROR(\"The coordinates of vertex[%d] of obstacles[%d] are not doubles\", j, i);\n } else {\n // PASSED ALL THE TESTS: push Vector2D to vertices list in Obstacle object\n rigid2d::Vector2D vertex(xml_obstacles[i][j][0], xml_obstacles[i][j][1]);\n // NOTE: SCALE DOWN\n vertex.x /= SCALE;\n vertex.y /= SCALE;\n obs.vertices.push_back(vertex);\n }\n }\n }\n // push Obstacle object to vector of Object(s) in Map\n obstacles_v.push_back(obs);\n }\n }\n\n // Initialize Marker\n // Init Marker Array Publisher\n ros::Publisher map_pub = nh.advertise(\"prm\", 1);\n\n ros::Publisher path_pub = nh.advertise(\"path\", 1);\n\n ros::Publisher debug_pub = nh.advertise(\"path_debug\", 1);\n\n ros::Publisher grid_pub = nh.advertise(\"grid_map\", 1);\n // Initialize grid_map outside\n nav_msgs::OccupancyGrid grid_map;\n // rviz representation of the grid\n std::vector map;\n\n // Init Marker Array\n visualization_msgs::MarkerArray map_arr;\n visualization_msgs::MarkerArray path_arr;\n visualization_msgs::MarkerArray path_debug;\n\n // Init Marker\n visualization_msgs::Marker marker;\n marker.header.frame_id = frame_id;\n marker.header.stamp = ros::Time::now();\n // marker.ns = \"my_namespace\";\n // marker.id = 0;\n marker.type = visualization_msgs::Marker::LINE_STRIP;\n marker.action = visualization_msgs::Marker::ADD;\n marker.pose.position.z = 0;\n marker.pose.orientation.x = 0.0;\n marker.pose.orientation.y = 0.0;\n marker.pose.orientation.z = 0.0;\n marker.pose.orientation.w = 1.0;\n marker.scale.x = 0.02;\n marker.color.r = 0.96f;\n marker.color.g = 0.475f;\n marker.color.b = 0.0f;\n marker.color.a = 1.0;\n marker.lifetime = ros::Duration();\n\n visualization_msgs::Marker sph_mkr;\n sph_mkr = marker;\n sph_mkr.type = visualization_msgs::Marker::SPHERE;\n sph_mkr.scale.x = 0.02;\n sph_mkr.scale.y = 0.02;\n sph_mkr.scale.z = 0.02;\n\n // Color\n sph_mkr.color.r = 0.5f;\n sph_mkr.color.g = 0.0f;\n sph_mkr.color.b = 0.5f;\n\n // LINE_STRIP relative to this pose\n marker.pose.position.x = 0.0;\n marker.pose.position.y = 0.0;\n\n\n // PATH MARKERS\n visualization_msgs::Marker path_marker;\n path_marker = marker;\n // More visible than PRM\n path_marker.scale.x = 0.03;\n path_marker.color.r = 0.2;\n path_marker.color.g = 1.0;\n path_marker.color.b = 0.2;\n\n visualization_msgs::Marker path_sph_mkr;\n path_sph_mkr = sph_mkr;\n // More visible than PRM\n path_sph_mkr.type = visualization_msgs::Marker::CUBE;\n path_sph_mkr.scale.x = 0.02;\n path_sph_mkr.scale.y = 0.02;\n path_sph_mkr.scale.z = 0.02;\n path_sph_mkr.color.r = 0.0f;\n path_sph_mkr.color.g = 0.0f;\n path_sph_mkr.color.b = 0.0f;\n\n\n // A* or Theta* Path\n std::vector path;\n // Debug Path (Usually A* to compare with Theta*)\n std::vector path2;\n\n if (map_type == \"prm\")\n // PRM VERSION\n {\n // Build PRM\n map::PRM prm(obstacles_v, inflate);\n prm.build_map(n, k, thresh);\n // DRAW PRM\n int prm_marker_id = 0;\n auto configurations = prm.return_prm();\n for (auto node_iter = configurations.begin(); node_iter != configurations.end(); node_iter++)\n {\n marker.points.clear();\n marker.id = prm_marker_id;\n prm_marker_id++;\n\n // Check if a node has edges before plotting\n if (node_iter->id_set.size() > 0)\n {\n for (auto id_iter = node_iter->id_set.begin(); id_iter != node_iter->id_set.end(); id_iter++)\n {\n // Add node as first marker vertex\n geometry_msgs::Point first_vertex;\n first_vertex.x = node_iter->coords.x;\n first_vertex.y = node_iter->coords.y;\n first_vertex.z = 0.0;\n marker.points.push_back(first_vertex);\n\n // Find Vertex for each ID\n auto neighbor_iter = configurations.at(*id_iter);\n\n geometry_msgs::Point new_vertex;\n new_vertex.x = neighbor_iter.coords.x;\n new_vertex.y = neighbor_iter.coords.y;\n new_vertex.z = 0.0;\n marker.points.push_back(new_vertex);\n\n // Also push back cylinders\n sph_mkr.pose.position.x = node_iter->coords.x;\n sph_mkr.pose.position.y = node_iter->coords.y;\n sph_mkr.id = prm_marker_id;\n prm_marker_id++;\n map_arr.markers.push_back(sph_mkr);\n }\n // Push to Marker Array\n map_arr.markers.push_back(marker);\n }\n }\n\n ROS_INFO(\"PRM Built!\");\n\n // PLAN on PRM using A* or Theta*\n if (planner_type == \"astar\")\n {\n ROS_INFO(\"Planning using A*!\");\n global::Astar astar(obstacles_v, inflate);\n path = astar.plan(start, goal, configurations);\n } else\n {\n ROS_INFO(\"Planning using Theta*!\");\n global::Thetastar theta_star(obstacles_v, inflate);\n path = theta_star.plan(start, goal, configurations);\n ROS_INFO(\"Planning using A*!\");\n global::Astar astar(obstacles_v, inflate);\n path2 = astar.plan(start, goal, configurations);\n }\n\n // DRAW PATH\n int path_marker_id = 0;\n for (auto path_iter = path.begin(); path_iter != path.end(); path_iter++)\n {\n // Add node as marker vertex\n geometry_msgs::Point vtx;\n vtx.x = path_iter->vertex.coords.x;\n vtx.y = path_iter->vertex.coords.y;\n vtx.z = 0.0;\n path_marker.points.push_back(vtx);\n\n // Also push back cylinders\n path_sph_mkr.pose.position.x = path_iter->vertex.coords.x;\n path_sph_mkr.pose.position.y = path_iter->vertex.coords.y;\n path_sph_mkr.id = path_marker_id;\n path_marker_id++;\n path_arr.markers.push_back(path_sph_mkr);\n\n }\n path_marker.id = path_marker_id;\n path_arr.markers.push_back(path_marker);\n\n // DRAW DEBUG PATH\n path_marker_id = 0;\n path_marker.points.clear();\n path_marker.color.r = 1.0;\n path_marker.color.g = 0.2;\n path_marker.color.b = 0.2;\n for (auto path2_iter = path2.begin(); path2_iter != path2.end(); path2_iter++)\n {\n // Add node as marker vertex\n geometry_msgs::Point vtx;\n vtx.x = path2_iter->vertex.coords.x;\n vtx.y = path2_iter->vertex.coords.y;\n vtx.z = 0.0;\n path_marker.points.push_back(vtx);\n\n // Also push back cylinders\n path_sph_mkr.pose.position.x = path2_iter->vertex.coords.x;\n path_sph_mkr.pose.position.y = path2_iter->vertex.coords.y;\n path_sph_mkr.id = path_marker_id;\n path_marker_id++;\n path_debug.markers.push_back(path_sph_mkr);\n\n }\n path_marker.id = path_marker_id;\n path_debug.markers.push_back(path_marker);\n\n } else\n // GRID Version\n {\n // Initialize Grid\n map::Grid grid(obstacles_v, inflate);\n\n // Build Map\n grid.build_map(resolution);\n\n ROS_INFO(\"Grid Built!\");\n\n // Get map bounds\n auto bounds = grid.return_map_bounds();\n auto gridsize = grid.return_grid_dimensions();\n\n // rviz representation of the grid\n grid.occupancy_grid(map);\n\n // Grid Pose\n geometry_msgs::Pose map_pose;\n map_pose.position.x = bounds.at(0).x;\n map_pose.position.y = bounds.at(0).y;\n map_pose.position.z = 0.0;\n map_pose.orientation.x = 0.0;\n map_pose.orientation.y = 0.0;\n map_pose.orientation.z = 0.0;\n map_pose.orientation.w = 1.0;\n\n // Occupancy grid for visualization\n grid_map.header.frame_id = frame_id;\n grid_map.info.resolution = resolution;\n grid_map.info.width = gridsize.at(0);\n grid_map.info.height = gridsize.at(1);\n\n // std::cout << \"Width: \" << gridsize.at(0) << \"\\t Height: \" << gridsize.at(1) << std::endl;\n\n grid_map.info.origin = map_pose;\n\n ROS_INFO(\"Planning using A*!\");\n global::Astar astar(obstacles_v, inflate);\n path = astar.plan(start, goal, grid, resolution);\n path2 = path;\n\n\n // DRAW PATH\n int path_marker_id = 0;\n for (auto path_iter = path.begin(); path_iter != path.end(); path_iter++)\n {\n // Add node as marker cell\n geometry_msgs::Point vtx;\n vtx.x = path_iter->cell.coords.x;\n vtx.y = path_iter->cell.coords.y;\n vtx.z = 0.0;\n path_marker.points.push_back(vtx);\n\n // Also push back cylinders\n path_sph_mkr.pose.position.x = path_iter->cell.coords.x;\n path_sph_mkr.pose.position.y = path_iter->cell.coords.y;\n path_sph_mkr.id = path_marker_id;\n path_marker_id++;\n path_arr.markers.push_back(path_sph_mkr);\n\n }\n path_marker.id = path_marker_id;\n path_arr.markers.push_back(path_marker);\n\n // DRAW DEBUG PATH\n path_marker_id = 0;\n path_marker.points.clear();\n path_marker.color.r = 1.0;\n path_marker.color.g = 0.2;\n path_marker.color.b = 0.2;\n for (auto path2_iter = path2.begin(); path2_iter != path2.end(); path2_iter++)\n {\n // Add node as marker cell\n geometry_msgs::Point vtx;\n vtx.x = path2_iter->cell.coords.x;\n vtx.y = path2_iter->cell.coords.y;\n vtx.z = 0.0;\n path_marker.points.push_back(vtx);\n\n // Also push back cylinders\n path_sph_mkr.pose.position.x = path2_iter->cell.coords.x;\n path_sph_mkr.pose.position.y = path2_iter->cell.coords.y;\n path_sph_mkr.id = path_marker_id;\n path_marker_id++;\n path_debug.markers.push_back(path_sph_mkr);\n\n }\n path_marker.id = path_marker_id;\n path_debug.markers.push_back(path_marker);\n\n }\n\n ros::Rate rate(frequency);\n\n // Main While\n while (ros::ok())\n {\n ros::spinOnce();\n\n // Publish PRM Map\n map_pub.publish(map_arr);\n\n // Publish GRID Map\n grid_map.header.stamp = ros::Time::now();\n grid_map.info.map_load_time = ros::Time::now();\n grid_map.data = map;\n grid_pub.publish(grid_map);\n\n // Publish Path\n path_pub.publish(path_arr);\n\n // Publish Path DEBUG\n debug_pub.publish(path_debug);\n rate.sleep();\n }\n\n return 0;\n}" }, { "path": "global_planner/src/dsl.cpp", "content": "/// \\file\n/// \\brief Draws Each Obstacle in RViz using MarkerArrays\n///\n/// PARAMETERS:\n/// PUBLISHES:\n/// SUBSCRIBES:\n/// FUNCTIONS:\n\n#include \n\n#include \n#include \n#include \n\n#include \"map/map.hpp\"\n#include \"map/prm.hpp\"\n#include \"map/grid.hpp\"\n#include \n#include \"global_planner/incremental.hpp\"\n\n#include \"nuslam/TurtleMap.h\"\n\n#include // To use std::bind\n#include \n#include \n#include \n\n// Used to deal with YAML list of lists\n#include // catkin component\n\n\nint main(int argc, char** argv)\n/// The Main Function ///\n{\n ROS_INFO(\"STARTING NODE: astar\");\n\n // Vars\n double frequency = 10.0;\n // Scale by which to transform marker poses (10 cm/cell so we use 10)\n double SCALE = 10.0;\n std::string frame_id = \"base_footprint\";\n XmlRpc::XmlRpcValue xml_obstacles;\n std::vector start_vec{7.0, 3.0};\n std::vector goal_vec{7.0, 26.0};\n double resolution = 0.01;\n\n // Map Parameters\n double thresh = 0.01;\n double inflate = 0.1;\n int visibility = 5;\n\n // store Obstacle(s) here to create Map\n std::vector obstacles_v;\n\n ros::init(argc, argv, \"astar_node\"); // register the node on ROS\n ros::NodeHandle nh; // get a handle to ROS\n ros::NodeHandle nh_(\"~\"); // get a handle to ROS\n // Parameters\n nh_.getParam(\"frequency\", frequency);\n nh_.getParam(\"obstacles\", xml_obstacles);\n nh_.getParam(\"start\", start_vec);\n nh_.getParam(\"goal\", goal_vec);\n nh_.getParam(\"map_frame_id\", frame_id);\n nh_.getParam(\"thresh\", thresh);\n nh_.getParam(\"inflate\", inflate);\n nh_.getParam(\"scale\", SCALE);\n nh_.getParam(\"resolution\", resolution);\n nh_.getParam(\"visibility\", visibility);\n\n rigid2d::Vector2D start(start_vec.at(0)/SCALE, start_vec.at(1)/SCALE);\n rigid2d::Vector2D goal(goal_vec.at(0)/SCALE, goal_vec.at(1)/SCALE);\n\n // 'obstacles' is a triple-nested list.\n // 1st level: obstacle (Obstacle), 2nd level: vertices (std::vector), 3rd level: coordinates (Vector2D)\n\n // std::vector\n if(xml_obstacles.getType() != XmlRpc::XmlRpcValue::TypeArray)\n {\n ROS_ERROR(\"There is no list of obstacles\");\n } else\n {\n for(int i = 0; i < xml_obstacles.size(); ++i)\n {\n // Obstacle contains std::vector\n // create Obstacle with empty vertex vector\n map::Obstacle obs;\n if(xml_obstacles[i].getType() != XmlRpc::XmlRpcValue::TypeArray)\n {\n ROS_ERROR(\"obstacles[%d] has no vertices\", i);\n } else {\n for(int j = 0; j < xml_obstacles[i].size(); ++j)\n {\n // Vector2D contains x,y center_coords\n if(xml_obstacles[i][j].size() != 2)\n {\n ROS_ERROR(\"Vertex[%d] of obstacles[%d] is not a pair of coordinates\", j, i);\n } else if(\n xml_obstacles[i][j][0].getType() != XmlRpc::XmlRpcValue::TypeDouble or\n xml_obstacles[i][j][1].getType() != XmlRpc::XmlRpcValue::TypeDouble)\n {\n ROS_ERROR(\"The coordinates of vertex[%d] of obstacles[%d] are not doubles\", j, i);\n } else {\n // PASSED ALL THE TESTS: push Vector2D to vertices list in Obstacle object\n rigid2d::Vector2D vertex(xml_obstacles[i][j][0], xml_obstacles[i][j][1]);\n // NOTE: SCALE DOWN\n vertex.x /= SCALE;\n vertex.y /= SCALE;\n obs.vertices.push_back(vertex);\n }\n }\n }\n // push Obstacle object to vector of Object(s) in Map\n obstacles_v.push_back(obs);\n }\n }\n\n // Initialize Marker\n // Init Marker Array Publisher\n ros::Publisher path_pub = nh.advertise(\"path\", 1);\n\n ros::Publisher grid_pub = nh.advertise(\"grid_map\", 1);\n // Initialize grid_map outside\n nav_msgs::OccupancyGrid grid_map;\n // rviz representation of the grid\n std::vector map;\n\n // Init Marker Array\n visualization_msgs::MarkerArray path_arr;\n\n // Init Marker\n visualization_msgs::Marker marker;\n marker.header.frame_id = frame_id;\n marker.header.stamp = ros::Time::now();\n marker.type = visualization_msgs::Marker::LINE_STRIP;\n marker.action = visualization_msgs::Marker::ADD;\n marker.pose.position.z = 0;\n marker.pose.orientation.x = 0.0;\n marker.pose.orientation.y = 0.0;\n marker.pose.orientation.z = 0.0;\n marker.pose.orientation.w = 1.0;\n marker.scale.x = 0.02;\n marker.color.r = 0.96f;\n marker.color.g = 0.475f;\n marker.color.b = 0.0f;\n marker.color.a = 1.0;\n marker.lifetime = ros::Duration();\n\n // LINE_STRIP relative to this pose\n marker.pose.position.x = 0.0;\n marker.pose.position.y = 0.0;\n\n // PATH MARKERS\n visualization_msgs::Marker path_marker;\n path_marker = marker;\n // More visible than PRM\n path_marker.scale.x = 0.03;\n path_marker.color.r = 0.2;\n path_marker.color.g = 1.0;\n path_marker.color.b = 0.2;\n\n visualization_msgs::Marker path_sph_mkr;\n path_sph_mkr = marker;\n // More visible than PRM\n path_sph_mkr.type = visualization_msgs::Marker::CUBE;\n path_sph_mkr.scale.x = 0.02;\n path_sph_mkr.scale.y = 0.02;\n path_sph_mkr.scale.z = 0.02;\n path_sph_mkr.color.r = 0.0f;\n path_sph_mkr.color.g = 0.0f;\n path_sph_mkr.color.b = 0.0f;\n\n // CURRENT POSITION MARKER\n visualization_msgs::Marker curr_pos_marker;\n curr_pos_marker = path_sph_mkr;\n // More visible than PRM\n curr_pos_marker.scale.x = 0.1;\n curr_pos_marker.scale.y = 0.1;\n curr_pos_marker.color.r = 1.0;\n curr_pos_marker.color.g = 0.2;\n curr_pos_marker.color.b = 0.2;\n\n // UpdateCell() MARKER\n visualization_msgs::Marker update_marker;\n update_marker = path_sph_mkr;\n // More visible than PRM\n update_marker.scale.x = 0.04;\n update_marker.scale.y = 0.04;\n update_marker.scale.z = 0.04;\n update_marker.color.r = 0.96f;\n update_marker.color.g = 0.475f;\n update_marker.color.b = 0.0f;\n update_marker.color.a = 1.0;\n update_marker.lifetime = ros::Duration(1.3 / frequency);\n\n\n // LPA* or D* Lite Path\n std::vector path;\n \n // GRID Version\n // Initialize Grid\n map::Grid grid(obstacles_v, inflate);\n\n // Build Map\n grid.build_map(resolution);\n\n ROS_INFO(\"Grid Built!\");\n\n // Get map bounds\n auto bounds = grid.return_map_bounds();\n auto gridsize = grid.return_grid_dimensions();\n\n // Grid Pose\n geometry_msgs::Pose map_pose;\n map_pose.position.x = bounds.at(0).x;\n map_pose.position.y = bounds.at(0).y;\n map_pose.position.z = 0.0;\n map_pose.orientation.x = 0.0;\n map_pose.orientation.y = 0.0;\n map_pose.orientation.z = 0.0;\n map_pose.orientation.w = 1.0;\n\n // Occupancy grid for visualization\n grid_map.header.frame_id = frame_id;\n grid_map.info.resolution = resolution;\n grid_map.info.width = gridsize.at(0);\n grid_map.info.height = gridsize.at(1);\n\n // std::cout << \"Width: \" << gridsize.at(0) << \"\\t Height: \" << gridsize.at(1) << std::endl;\n\n grid_map.info.origin = map_pose;\n\n ROS_INFO(\"Planning using D* Lite!\");\n global::DSL dsl(obstacles_v, inflate);\n dsl.Initialize(start, goal, grid, resolution);\n dsl.ComputeShortestPath();\n path = dsl.return_path();\n\n ros::Rate rate(frequency);\n\n // 0th cell\n unsigned int path_counter = 0;\n std::vector updated_nodes;\n\n // For displaying final path at the end\n // Push back twice since out visualizer displays from the 1th element\n std::vector total_path;\n total_path.push_back(path.front());\n // total_path.push_back(path.front());\n // Update Current Position Counter\n curr_pos_marker.pose.position.x = path.at(path_counter).cell.center_coords.x;\n curr_pos_marker.pose.position.y = path.at(path_counter).cell.center_coords.y;\n\n // End condition for update\n bool planning = true;\n\n // SLEEP so that I can prepare my screen recorder\n ros::Duration(2.0).sleep();\n\n // For visualization purposes (See below)\n bool firstpass = true;\n\n bool valid_path = dsl.return_valid();\n\n // Main While\n while (ros::ok())\n {\n ros::spinOnce();\n\n // rviz representation of the grid\n grid.fake_occupancy_grid(map);\n // Publish GRID Map\n grid_map.header.stamp = ros::Time::now();\n grid_map.info.map_load_time = ros::Time::now();\n grid_map.data = map;\n grid_pub.publish(grid_map);\n\n\n // DRAW PATH\n path_arr.markers.clear();\n path_marker.points.clear();\n int path_marker_id = 0;\n // +1 to simulate move forward\n for (auto path_iter = path.begin(); path_iter != path.end(); path_iter++)\n {\n // Add node as marker cell\n geometry_msgs::Point vtx;\n vtx.x = path_iter->cell.center_coords.x;\n vtx.y = path_iter->cell.center_coords.y;\n vtx.z = 0.0;\n path_marker.points.push_back(vtx);\n\n // Also push back cubes\n path_sph_mkr.pose.position.x = path_iter->cell.center_coords.x;\n path_sph_mkr.pose.position.y = path_iter->cell.center_coords.y;\n path_sph_mkr.id = path_marker_id;\n path_marker_id++;\n path_arr.markers.push_back(path_sph_mkr);\n }\n path_marker.id = path_marker_id;\n path_arr.markers.push_back(path_marker);\n\n // Push Back Current Position\n path_marker_id++;\n curr_pos_marker.id = path_marker_id;\n path_arr.markers.push_back(curr_pos_marker);\n\n // Push Back Updated Cell Markers\n for (auto node_iter = updated_nodes.begin(); node_iter != updated_nodes.end(); node_iter++)\n {\n path_marker_id++;\n update_marker.pose.position.x = node_iter->cell.center_coords.x;\n update_marker.pose.position.y = node_iter->cell.center_coords.y;\n update_marker.id = path_marker_id;\n path_arr.markers.push_back(update_marker);\n }\n\n // Publish Path\n path_pub.publish(path_arr);\n\n // FAKE Update\n updated_nodes.clear();\n if (path.size() > 1 and planning and valid_path)\n {\n // Update Grid\n grid.update_grid(path.at(path_counter + 1).cell, visibility);\n // D*Lite Update\n updated_nodes = dsl.SimulateUpdate(grid.return_fake_grid());\n // Return Path\n path = dsl.return_path();\n // Check path validity (obstacles etc)\n valid_path = dsl.return_valid();\n // Update total path\n total_path.push_back(path.at(path_counter));\n // Update Current Position Counter\n curr_pos_marker.pose.position.x = path.at(path_counter).cell.center_coords.x;\n curr_pos_marker.pose.position.y = path.at(path_counter).cell.center_coords.y;\n } else\n {\n // FINAL PATH. INFINITE MARKER DURATION\n path_marker.lifetime = ros::Duration();\n path_sph_mkr.lifetime = ros::Duration();\n curr_pos_marker.lifetime = ros::Duration();\n if (!valid_path)\n {\n ROS_WARN(\"INVALID PATH\");\n for (auto path_iter = path.begin(); path_iter != path.end(); path_iter++)\n {\n total_path.push_back(*path_iter);\n }\n // So we don't enter this loop again\n valid_path = true;\n }\n path = total_path;\n planning = false;\n }\n\n // Make the markers persist for the first iteration to show original path\n if (firstpass)\n {\n ros::Duration(1.0).sleep();\n path_marker.lifetime = ros::Duration(1.0 / frequency);\n path_sph_mkr.lifetime = ros::Duration(1.0 / frequency);\n curr_pos_marker.lifetime = ros::Duration(1.0 / frequency);\n }\n\n firstpass = false;\n rate.sleep();\n }\n\n return 0;\n}" }, { "path": "global_planner/src/global_planner/global_planner.cpp", "content": "#include \"global_planner/global_planner.hpp\"\n\nnamespace global\n{\n\tusing rigid2d::Vector2D;\n\n\t// Global Planner\n\tGlobalPlanner::GlobalPlanner()\n\t{\n\t}\n}\n" }, { "path": "global_planner/src/global_planner/heuristic.cpp", "content": "#include \"global_planner/heuristic.hpp\"\n\nnamespace global\n{\n\tNode::Node()\n\t{\n\t\tvertex = Vertex(Vector2D());\n\t\tcell = Cell(Vector2D(), 0.0);\n\t}\n\n\tAstar::Astar(const std::vector & obstacles_, const double & inflate_robot_)\n\t{\n\t\tobstacles = obstacles_;\n\t\tinflate_robot = inflate_robot_;\n\t}\n\n\t// PRM version\n\tstd::vector Astar::plan(const Vector2D & start, const Vector2D & goal, std::vector & map)\n\t{\n\t\tPRM = map;\n\n\t\t/**\n\t\t\tMain Planning Loop\n for A* (naive)\n\n Steps:\n\n 1. Append start node to open list\n 2. LOOP:\n a- set current node to that with lowest f cost\n in open list. If some nodes have the same f cost,\n choose current node using h cost\n\n b- get the 8 neighbours of each node (or less depending\n on proximity to grid limits)\n\n c- for each node:\n i - if it is inside the closed list,\n or if it is an obstacle, ignore it\n ii - if it is on the open list, check\n whether it is a better path than the current node,\n if so, update its g cost and make its parent node the\n current node\n iii - if none of these conditions are met,\n add the node to the open list and set its parent\n to the current node after calculating its g and h cost\n 3. If goal is found, return path, else, keep looping\n until goal is found or open list is 0 (path was never found)\n **/\n\n // Create a Min heap of Nodes \n\t std::priority_queue , HeapComparator > open_list;\n\n\t // Store the goal node\n\t Node goal_node;\n\t // Find PRM vertex whose coordinates most closely match the goal coordinates\n\t goal_node.vertex = find_nearest_node(goal, PRM);\n\t goal_node.id = goal_node.vertex.id;\n\n\t // Add the start node to the queue\n\t Node current_node;\n\t // Find PRM vertex whose coordinates most closely match the start coordinates\n\t current_node.vertex = find_nearest_node(start, PRM);\n\t current_node.id = current_node.vertex.id;\n\n\t open_list.push(current_node);\n\n\t // For re-ordering and finding NOTE: inefficient\n\t std::set open_list_v;\n\t open_list_v.insert(current_node.id);\n\n\t // Store the start node\n\t Node start_node = current_node;\n\n\n\t // Closed List\n\t std::set> closed_list;\n\n\t int iterations = 0;\n\t while (!open_list.empty())\n\t {\n\t \titerations++;\n\n\t \t// Get the minimum node on the open list\n\t \tcurrent_node = open_list.top();\n\t \t// Remove said node from open list\n\t \topen_list.pop();\n\t \topen_list_v.erase(current_node.id);\n\n\t \t// Add current node ID to closed list\n\t \tclosed_list.insert(current_node);\n\n\t \t// END condition\n\t \tif (current_node.vertex.id == goal_node.vertex.id)\n\t \t{\n\t \t\tstd::cout << \"Goal found after \" << iterations << \" Iterations!\" << std::endl;\n\t \t\treturn trace_path(current_node, closed_list);\n\t \t}\n\n\t \t// Loop through each node's neighbors\n\t \tfor (auto edge_iter = current_node.vertex.edges.begin(); edge_iter < current_node.vertex.edges.end(); edge_iter++)\n\t \t{\n\t \t\t// Skip if neighbour in closed list, or is obstacle (GRID) \n\t \t\tbool skip = false;\n\t \t\t// Modify node cost if neighbour but also is in open list\n\t \t\tbool opened = false;\n\n\t \t\t// Find the neighbour node\n\t \t\tNode neighbour;\n\t \t\tneighbour.vertex = PRM.at(edge_iter->next_id);\n\t \t\t// Self-identify\n\t\t\t\tneighbour.id = neighbour.vertex.id;\n\n\t \t\t// First, check if in closed list\n\t \t\tskip = closed_list.find(neighbour.id) != closed_list.end();\n\t \t\tif (skip)\n\t \t\t{\n\t \t\t\tcontinue;\n\t \t\t}\n\n\t \t\t// No obstacle list check since not GRID\n\n\t \t\t// Check if in open list\n\t \t\topened = open_list_v.find(neighbour.id) != open_list_v.end();\n\n\t \t\tif (!opened)\n\t \t\t// Create a new node and push\n\t \t\t{\n\t \t\t\t// hcost is distance from neighbor to goal\n\t \t\t\tneighbour.hcost = map::euclidean_distance(neighbour.vertex.coords.x - goal_node.vertex.coords.x,\n\t \t\t\t\t\t\t\t\t\t\t\t\t\t neighbour.vertex.coords.y - goal_node.vertex.coords.y);\n\t \t\t\t\n\t \t\t\tcreate_vtx(open_list, neighbour, current_node);\n\t \t\t\t// Push to record keeping set\n\t \t\t\topen_list_v.insert(neighbour.id);\n\n\t \t\t} else\n\t \t\t// Potentially modify existing node and resort open list\n\t \t\t{\n\t \t\t\tupdate_vtx(open_list, neighbour, current_node);\n\t \t\t}\n\t \t}\n\t }\n\n\t // If we have reached this point, then there was no valid path\n\t std::cout << \"No valid path! returning most complete path\" << std::endl;\n\t return trace_path(current_node, closed_list);\n\n\t}\n\n\tvoid Astar::create_vtx(std::priority_queue , HeapComparator > & open_list, Node & neighbour, const Node & current_node)\n\t{\n\t\t// gcost is current node's gcost + distance from neighbor to current node\n\t\tneighbour.gcost = current_node.gcost + map::euclidean_distance(neighbour.vertex.coords.x - current_node.vertex.coords.x,\n\t\t\t\t\t\t\t\t\t\t\t\t \t\t\t\t\t neighbour.vertex.coords.y - current_node.vertex.coords.y);\n\t\t// Update f cost\n\t\tneighbour.fcost = neighbour.gcost + neighbour.hcost;\n\n\t\t// Add parent id\n\t\tneighbour.parent_id = current_node.id;\n\n\t\t// Push to open list\n\t\topen_list.push(neighbour);\n\t}\n\n\tvoid Astar::update_vtx(std::priority_queue , HeapComparator > & open_list, Node & neighbour, const Node & current_node)\n\t{\n\t\t// Calculate a new tentative g cost\n\t\tdouble gcost = current_node.gcost + map::euclidean_distance(neighbour.vertex.coords.x - current_node.vertex.coords.x,\n\t\t\t\t\t\t\t\t\t\t\t\t \t\t\t\t\t neighbour.vertex.coords.y - current_node.vertex.coords.y);\n\t\tif (gcost < neighbour.gcost)\n\t\t// Modify Node\n\t\t{\n\t\t\t// Update g cost\n\t\t\tneighbour.gcost = gcost;\n\t\t\t// Update f cost\n\t\t\tneighbour.fcost = neighbour.gcost + neighbour.hcost;\n\t\t\t// Update parent\n\t\t\tneighbour.parent_id = current_node.id;\n\n\t\t\t// If this happens, we need to re-sort the open list\n\t\t\tstd::priority_queue , HeapComparator > temp_open_list;\n\t\t\twhile (!open_list.empty())\n\t\t\t{\n\t\t\t\tNode temp = open_list.top();\n\t\t\t\tif (temp.id == neighbour.id)\n\t\t\t\t{\n\t\t\t\t\ttemp = neighbour;\n\t\t\t\t}\n\t\t\t\ttemp_open_list.push(temp);\n\t\t\t\topen_list.pop();\n\t\t\t}\n\n\t\t\topen_list = temp_open_list;\n\t\t}\n\n\t}\n\n\tstd::vector Astar::trace_path(const Node & final_node, const std::set> & closed_list)\n\t{\n\t\t// First node in the vector is 'final node'\n\t\tstd::vector path{final_node};\n\n\t\tbool done = false;\n\n\t\twhile (!done)\n\t\t{\n\t\t\t// std::cout << \"GOING FROM NODE: \" << path.back().vertex.id << \" TO: \" << path.back().parent_id << std::endl;\n\t\t\tNode next_node = *closed_list.find(path.back().parent_id);\n\t\t\tpath.push_back(next_node);\n\t\t\tif (next_node.parent_id == -1)\n\t\t\t{\n\t\t\t\tdone = true;\n\t\t\t\tbreak;\n\t\t\t}\n\t\t}\n\n\t\tstd::reverse(path.begin(), path.end());\n\n\t\tstd::cout << \"The path contains \" << path.size() << \" Nodes.\" << std::endl;\n\t\treturn path;\n\t}\n\n\n\tstd::vector Astar::plan(const Vector2D & start, const Vector2D & goal, const Grid & grid_, const double & resolution)\n\t{\n\t\tGrid grid = grid_;\n\t\tGRID = grid.return_grid();\n\n\t\t/**\n\t\t\tMain Planning Loop\n for A* (naive)\n\n Steps:\n\n 1. Append start node to open list\n 2. LOOP:\n a- set current node to that with lowest f cost\n in open list. If some nodes have the same f cost,\n choose current node using h cost\n\n b- get the 8 neighbours of each node (or less depending\n on proximity to grid limits)\n\n c- for each node:\n i - if it is inside the closed list,\n or if it is an obstacle, ignore it\n ii - if it is on the open list, check\n whether it is a better path than the current node,\n if so, update its g cost and make its parent node the\n current node\n iii - if none of these conditions are met,\n add the node to the open list and set its parent\n to the current node after calculating its g and h cost\n 3. If goal is found, return path, else, keep looping\n until goal is found or open list is 0 (path was never found)\n **/\n\n // Create a Min heap of Nodes \n\t std::priority_queue , HeapComparator > open_list;\n\n\t // Store the goal node\n\t Node goal_node;\n\t // Find GRID cellwhose coordinates most closely match the goal coordinates\n\t goal_node.cell = find_nearest_node(goal, grid, resolution);\n\t goal_node.id = goal_node.cell.index.row_major;\n\n\t // Add the start node to the queue\n\t Node current_node;\n\t // Find GRID cell whose coordinates most closely match the start coordinates\n\t current_node.cell = find_nearest_node(start, grid, resolution);\n\t current_node.id = current_node.cell.index.row_major;\n\t // std::cout << \"START IDX: [\" << current_node.cell.index.x << \", \" << current_node.cell.index.y << \"]\" << std::endl;\n\n\t // std::cout << \"GOAL IDX: [\" << goal_node.cell.index.x << \", \" << goal_node.cell.index.y << \"]\" << std::endl;\n\n\t open_list.push(current_node);\n\n\t // For re-ordering and finding NOTE: inefficient\n\t std::set open_list_v;\n\t open_list_v.insert(current_node.id);\n\n\t // Store the start node\n\t Node start_node = current_node;\n\n\n\t // Closed List\n\t std::set> closed_list;\n\n\t int iterations = 0;\n\t while (!open_list.empty())\n\t {\n\t \titerations++;\n\n\t \t// Get the minimum node on the open list\n\t \tcurrent_node = open_list.top();\n\t \t// Remove said node from open list\n\t \topen_list.pop();\n\t \topen_list_v.erase(current_node.id);\n\n\t \t// Add current node ID to closed list\n\t \tclosed_list.insert(current_node);\n\n\t \t// END condition\n\t \tif (current_node.cell.index.row_major == goal_node.cell.index.row_major)\n\t \t{\n\t \t\tstd::cout << \"Goal found after \" << iterations << \" Iterations!\" << std::endl;\n\t \t\treturn trace_path(current_node, closed_list);\n\t \t}\n\n\t \t// Find the current node's neighbours\n\t \tstd::vector neighbours = get_neighbours(current_node, GRID);\n\n\t \t// Loop through each node's neighbors\n\t \tfor (auto nbr_iter = neighbours.begin(); nbr_iter < neighbours.end(); nbr_iter++)\n\t \t{\n\t \t\t// Skip if neighbour in closed list, or is obstacle (GRID) \n\t \t\tbool skip = false;\n\t \t\t// Modify node cost if neighbour but also is in open list\n\t \t\tbool opened = false;\n\n\t \t\t// Find the neighbour node\n\t \t\tNode neighbour;\n\t \t\tneighbour.cell = *nbr_iter;\n\t \t\tneighbour.id = neighbour.cell.index.row_major;\n\n\t \t\t// First, check if in closed list\n\t \t\tskip = closed_list.find(neighbour.id) != closed_list.end();\n\t \t\tif (skip)\n\t \t\t{\n\t \t\t\t// std::cout << \"CLOSED Node at [\" << neighbour.cell.index.x << \", \" << neighbour.cell.index.y << \"]\" << \"\\t [\" << neighbour.cell.coords.x << \", \" << neighbour.cell.coords.y << \"]\" << std::endl;\n\t \t\t\tcontinue;\n\t \t\t}\n\n\t \t\t// Now, check if neighbour is an obstacle\n\t \t\tif (neighbour.cell.celltype == map::Occupied or\\\n\t \t\t\tneighbour.cell.celltype == map::Inflation)\n\t \t\t{\t \t\t\t\n\t \t\t\tcontinue;\n\t \t\t}\n\n\t \t\t// Check if in open list\n\t \t\topened = open_list_v.find(neighbour.id) != open_list_v.end();\n\n\t \t\tif (!opened)\n\t \t\t// Create a new node and push\n\t \t\t{\n\t \t\t\tneighbour.hcost = heuristic(neighbour, goal_node);\n\t \t\t\t// std::cout << \"h cost: \" << neighbour.hcost << std::endl;\n\t \t\t\t\n\t \t\t\tcreate_cell(open_list, neighbour, current_node);\n\t \t\t\t// Push to record keeping set\n\t \t\t\topen_list_v.insert(neighbour.id);\n\n\t \t\t} else\n\t \t\t// Potentially modify existing node and resort open list\n\t \t\t{\n\t \t\t\tupdate_cell(open_list, neighbour, current_node);\n\t \t\t}\n\t \t}\n\t }\n\n\t // If we have reached this point, then there was no valid path\n\t std::cout << \"No valid path! returning most complete path\" << std::endl;\n\t return trace_path(current_node, closed_list);\n\n\t}\n\n\tvoid Astar::update_cell(std::priority_queue , HeapComparator > & open_list, Node & neighbour, const Node & current_node)\n\t{\n\t\t// Calculate a new tentative g cost\n\t\tdouble gcost = current_node.gcost + heuristic(neighbour, current_node);\n\t\tif (gcost < neighbour.gcost)\n\t\t// Modify Node\n\t\t{\n\t\t\t// Update g cost\n\t\t\tneighbour.gcost = gcost;\n\t\t\t// Update f cost\n\t\t\tneighbour.fcost = neighbour.gcost + neighbour.hcost;\n\t\t\t// Update parent\n\t\t\tneighbour.parent_id = current_node.id;\n\n\t\t\t// If this happens, we need to re-sort the open list\n\t\t\tstd::priority_queue , HeapComparator > temp_open_list;\n\t\t\twhile (!open_list.empty())\n\t\t\t{\n\t\t\t\tNode temp = open_list.top();\n\t\t\t\tif (temp.id == neighbour.id)\n\t\t\t\t{\n\t\t\t\t\ttemp = neighbour;\n\t\t\t\t}\n\t\t\t\ttemp_open_list.push(temp);\n\t\t\t\topen_list.pop();\n\t\t\t}\n\n\t\t\topen_list = temp_open_list;\n\t\t}\n\n\t}\n\n\tvoid Astar::create_cell(std::priority_queue , HeapComparator > & open_list, Node & neighbour, const Node & current_node)\n\t{\n\t\t// gcost is current node's gcost + distance from neighbor to current node\n\t\tneighbour.gcost = current_node.gcost + heuristic(neighbour, current_node);\n\t\t// std::cout << \"g cost: \" << neighbour.gcost << std::endl;\n\t\t// Update f cost\n\t\tneighbour.fcost = neighbour.gcost + neighbour.hcost;\n\n\t\t// Add parent id\n\t\tneighbour.parent_id = current_node.id;\n\n\t\t// Push to open list\n\t\topen_list.push(neighbour);\n\t}\n\n\tdouble Astar::heuristic(const Node & n1, const Node & n2)\n\t{\n\t\tdouble x_dist = fabs(n1.cell.center_coords.x - n2.cell.center_coords.x);\n\t\tdouble y_dist = fabs(n1.cell.center_coords.y - n2.cell.center_coords.y);\n\t\t// return map::euclidean_distance(x_dist, y_dist);\n\n\t\tdouble D1 = 1.0;\n\t\tdouble D2 = sqrt(2.0);\n\n\t\treturn D1 * (x_dist + y_dist) + (D2 - 2 * D1) * std::min(x_dist, y_dist);\n\t}\n\n\n\tstd::vector Astar::get_neighbours(const Node & n, const std::vector & map)\n\t{\n\t\tint x_max = map.back().index.x;\n\t\tint y_max = map.back().index.y; \n\t\tstd::vector neighbours;\n\t\t// std::cout << \"Node at [\" << n.cell.index.x << \", \" << n.cell.index.y << \"]\" << std::endl;\n\n\t\t// Evaluate about 3x3 block for 8-connectivity\n\t\tfor (int x = -1; x < 2; x++)\n\t\t{\n\t\t\tfor (int y = -1; y < 2; y++)\n\t\t\t{\n\t\t\t\t// Skip x,y = (0,0) since that's the current node\n\t\t\t\tif (x == 0 and y == 0)\n\t\t\t\t{\n\t\t\t\t\tcontinue;\n\t\t\t\t} else\n\t\t\t\t{\n\t\t\t\t\tint check_x = n.cell.index.x + x;\n\t\t\t\t\tint check_y = n.cell.index.y + y;\n\n\t\t\t\t\t// Ensure potential neighbour is within grid bounds\n\t\t\t\t\tif (check_x >= 0 and check_x <= x_max and check_y >= 0 and check_y <= y_max)\n\t\t\t\t\t{\n\t\t\t\t\t\t// Now we need to grab the right cell from the map. To do this: index->RMJ\n\t\t\t\t\t\t// std::cout << \"Neighbour at [\" << check_x << \", \" << check_y << \"]\" << std::endl;\n\t\t\t\t\t\tint rmj = map::grid2rowmajor(check_x, check_y, x_max + 1);\n\t\t\t\t\t\tCell nbr = map.at(rmj);\n\t\t\t\t\t\tneighbours.push_back(nbr);\n\t\t\t\t\t}\n\t\t\t\t}\n\t\t\t}\n\t\t}\n\t\treturn neighbours;\n\t}\n\n\n\tvoid Thetastar::create_vtx(std::priority_queue , HeapComparator > & open_list, Node & neighbour, const Node & current_node)\n\t{\n\t\t// First, do line of sight check between PARENT of current node and neighbour\n\t\tbool clear = true;\n\n\t\tint grandparent_id = current_node.parent_id;\n\t\tif (grandparent_id == -1)\n\t\t{\n\t\t\t// There is no grandparent since this is the start node\n\t\t\tgrandparent_id = current_node.id;\n\t\t\tclear = false;\n\t\t}\n\n\t\tif (clear)\n\t\t{\n\t\t\tfor (auto obs_iter = obstacles.begin(); obs_iter != obstacles.end(); obs_iter++)\n\t\t\t{\n\t\t\t\t// Edge on Edge Check\n\t\t\t\tif (!no_intersect(neighbour.vertex, PRM.at(grandparent_id), obs_iter))\n\t\t\t\t{\n\t\t\t\t\tclear = false;\n\t\t\t\t\tbreak;\n\t\t\t\t}\n\n\t\t\t\t// Edge Near Point Check\n\t\t\t\tfor (auto v_iter = obs_iter->vertices.begin(); v_iter != obs_iter->vertices.end(); v_iter++)\n\t\t\t\t{\n\t\t\t\t\tif (too_close(neighbour.vertex, PRM.at(grandparent_id), Vertex(*v_iter), inflate_robot))\n\t\t\t\t\t{\n\t\t\t\t\t\tclear = false;\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t}\n\t\t\t\t}\n\n\t\t\t\t// No need to loop over other obstacles if not clear\n\t\t\t\tif (!clear)\n\t\t\t\t{\n\t\t\t\t\tbreak;\n\t\t\t\t}\n\t\t\t}\n\n\t\t}\n\n\t\tif (!clear)\n\t\t// Perform vanilla update from A*\n\t\t{\n\t\t\t// Use UNMODIFIED inherited function\n\t\t\tAstar::create_vtx(open_list, neighbour, current_node);\n\n\t\t} else\n\t\t{\n\t\t\t// g cost of grandparent = current_node gcost - dist(grandparent->current)\n\t\t\tdouble grandparent_gcost = current_node.gcost - map::euclidean_distance(current_node.vertex.coords.x - PRM.at(grandparent_id).coords.x,\n\t\t\t\t\t\t\t\t\t\t\t\t\t \t\t\t\t\t current_node.vertex.coords.y - PRM.at(grandparent_id).coords.y);\n\n\t\t\t// g cost is grandparent node g cost + dist(grandparent -> neighbor)\n\t\t\tneighbour.gcost = grandparent_gcost + map::euclidean_distance(neighbour.vertex.coords.x - PRM.at(grandparent_id).coords.x,\n\t\t\t\t\t\t\t\t\t\t\t\t\t \t\t\t\t\tneighbour.vertex.coords.y - PRM.at(grandparent_id).coords.y);\n\t\t\t// Update f cost\n\t\t\tneighbour.fcost = neighbour.gcost + neighbour.hcost;\n\n\t\t\t// Add parent id\n\t\t\tneighbour.parent_id = grandparent_id;\n\n\t\t\t// Push to open list\n\t\t\topen_list.push(neighbour);\n\n\t\t}\n\t}\n\n\n\t// The overridden update_vtx fcn with line of sight check\n\tvoid Thetastar::update_vtx(std::priority_queue , HeapComparator > & open_list, Node & neighbour, const Node & current_node)\n\t{\n\t\t// First, do line of sight check between PARENT of current node and neighbour\n\t\tbool clear = true;\n\n\t\tint grandparent_id = current_node.parent_id;\n\t\tif (grandparent_id == -1)\n\t\t{\n\t\t\t// There is no grandparent since this is the start node\n\t\t\tgrandparent_id = current_node.id;\n\t\t\tclear = false;\n\t\t}\n\n\t\tif (clear)\n\t\t{\n\t\t\tfor (auto obs_iter = obstacles.begin(); obs_iter != obstacles.end(); obs_iter++)\n\t\t\t{\n\t\t\t\t// Edge on Edge Check\n\t\t\t\tif (!no_intersect(neighbour.vertex, PRM.at(grandparent_id), obs_iter))\n\t\t\t\t{\n\t\t\t\t\tclear = false;\n\t\t\t\t\tbreak;\n\t\t\t\t}\n\n\t\t\t\t// Edge Near Point Check\n\t\t\t\tfor (auto v_iter = obs_iter->vertices.begin(); v_iter != obs_iter->vertices.end(); v_iter++)\n\t\t\t\t{\n\t\t\t\t\tif (too_close(neighbour.vertex, PRM.at(grandparent_id), Vertex(*v_iter), inflate_robot))\n\t\t\t\t\t{\n\t\t\t\t\t\tclear = false;\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t}\n\t\t\t\t}\n\n\t\t\t\t// No need to loop over other obstacles if not clear\n\t\t\t\tif (!clear)\n\t\t\t\t{\n\t\t\t\t\tbreak;\n\t\t\t\t}\n\t\t\t}\n\n\t\t}\n\n\t\tif (!clear)\n\t\t// Perform vanilla update from A*\n\t\t{\n\t\t\t// Use UNMODIFIED inherited function\n\t\t\tAstar::update_vtx(open_list, neighbour, current_node);\n\n\t\t} else\n\t\t// Perform cost update with new parent\n\t\t{\n\t\t\t// g cost of grandparent = current_node gcost - dist(grandparent->current)\n\t\t\tdouble grandparent_gcost = current_node.gcost - map::euclidean_distance(current_node.vertex.coords.x - PRM.at(grandparent_id).coords.x,\n\t\t\t\t\t\t\t\t\t\t\t\t\t \t\t\t\t\t current_node.vertex.coords.y - PRM.at(grandparent_id).coords.y);\n\t\t\t// Calculate a new tentative g cost\n\t\t\tdouble gcost = grandparent_gcost + map::euclidean_distance(neighbour.vertex.coords.x - PRM.at(grandparent_id).coords.x,\n\t\t\t\t\t\t\t\t\t\t\t\t\t \t\t\t\t\tneighbour.vertex.coords.y - PRM.at(grandparent_id).coords.y);\n\t\t\tif (gcost < neighbour.gcost)\n\t\t\t// Modify Node\n\t\t\t{\n\t\t\t\t// Update g cost\n\t\t\t\tneighbour.gcost = gcost;\n\t\t\t\t// Update f cost\n\t\t\t\tneighbour.fcost = neighbour.gcost + neighbour.hcost;\n\t\t\t\t// Update parent with PARENT of parent\n\t\t\t\t// std::cout << \"PARENT ID: \" << current_node.vertex.id << std::endl;\n\t\t\t\tneighbour.parent_id = grandparent_id;\n\t\t\t\t// std::cout << \"GRANDPARENT (NEW PARENT) ID: \" << neighbour.parent_id << std::endl;\n\n\t\t\t\t// If this happens, we need to re-sort the open list\n\t\t\t\tstd::priority_queue , HeapComparator > temp_open_list;\n\t\t\t\twhile (!open_list.empty())\n\t\t\t\t{\n\t\t\t\t\tNode temp = open_list.top();\n\t\t\t\t\tif (temp.id == neighbour.id)\n\t\t\t\t\t{\n\t\t\t\t\t\ttemp = neighbour;\n\t\t\t\t\t}\n\t\t\t\t\ttemp_open_list.push(temp);\n\t\t\t\t\topen_list.pop();\n\t\t\t\t}\n\n\t\t\t\topen_list = temp_open_list;\n\t\t\t}\n\t\t}\n\t}\n\n\tVertex find_nearest_node(const Vector2D & position, const std::vector & map)\n\t{\n\t\tdouble min_dist = map::euclidean_distance(position.x - map.at(0).coords.x, position.y - map.at(0).coords.y);\n\t\tint min_idx = 0;\n\n\t\tfor (auto iter = map.begin(); iter < map.end(); iter++)\n\t\t{\n\t\t\tdouble curr_dist = map::euclidean_distance(position.x - iter->coords.x, position.y - iter->coords.y);\n\t\t\tif (curr_dist < min_dist)\n\t\t\t{\n\t\t\t\tmin_idx = static_cast(std::distance(map.begin(), iter));\n\t\t\t\tmin_dist = curr_dist;\n\t\t\t}\n\t\t}\n\n\t\treturn map.at(min_idx);\n\t}\n\n\n\tCell find_nearest_node(const Vector2D & position, const Grid & grid, const double & resolution)\n\t{\n\t\tCell temp_cell(position, resolution);\n\n\t\tIndex idx = grid.world2grid(temp_cell);\n\n\t\treturn (grid.return_grid().at(idx.row_major));\n\t}\n}" }, { "path": "global_planner/src/global_planner/incremental.cpp", "content": "#include \"global_planner/incremental.hpp\"\n\nnamespace global\n{\n\tvoid LPAstar::ComputeShortestPath()\n\t{\n\t\tNode min;\n\t\tint iterations= 0;\n\t\twhile (Continue(iterations))\n\t\t{\n\t\t\titerations++;\n\t\t\t// std::cout << \"Iteration: \" << iterations << std::endl;\n\n\t\t\t// if (iterations > 200)\n\t\t\t// {\n\t\t\t// \tbreak;\n\t\t\t// }\n\n\t\t\t// Get min node and erase from open list\n\t\t\tmin = open_list.at(0);\n\t\t\tstd::pop_heap(open_list.begin(), open_list.end(), KeyComparator());\n\t\t\topen_list.back();\n\t\t\topen_list.pop_back();\n\n\t\t\t// Check if Overconsistent (start always satisfies this)\n\t\t\tif (min.gcost > min.rhs)\n\t\t\t{\n\t\t\t\t// std::cout << \"Locally Overconsistent!\" << std::endl;\n\t\t\t\t// Update True Cost\n\t\t\t\tmin.gcost = min.rhs;\n\t\t\t\t// Update min in FakeGrid\n\t\t\t\tFakeGrid.at(min.id) = min;\n\t\t\t\t// Check Successors\n\t\t\t\tstd::vector successors = get_neighbours(min, FakeGrid);\n\t\t\t\tfor (auto succ = successors.begin(); succ < successors.end(); succ++)\n\t\t\t\t{\n\t\t\t\t\tUpdateCell(*succ);\n\t\t\t\t}\n\n\t\t\t} else\n\t\t\t{\n\t\t\t\t// std::cout << \"NOT Locally Overconsistent!\" << std::endl;\n\t\t\t\tmin.gcost = BIG_NUM;\n\t\t\t\tFakeGrid.at(min.id) = min;\n\t\t\t\t// Check Successors\n\t\t\t\tstd::vector successors = get_neighbours(min, FakeGrid);\n\t\t\t\t// ALSO check min itself\n\t\t\t\tsuccessors.push_back(min);\n\t\t\t\tfor (auto succ = successors.begin(); succ < successors.end(); succ++)\n\t\t\t\t{\n\t\t\t\t\tUpdateCell(*succ);\n\t\t\t\t}\n\t\t\t}\n\t\t}\n\t}\n\n\n\tvoid LPAstar::UpdateCell(Node & n)\n\t{\n \t// Check this node isn't the start node\n\t\tif (n.id != start_node.id)\n\t\t{\n\t\t\tstd::priority_queue , CostComparator > pred_costs;\n\t\t\t// Find the predecessors of node n\n\t\t\tstd::vector predecessors = get_neighbours(n, FakeGrid);\n\n\t\t\tfor (auto pred_iter = predecessors.begin(); pred_iter < predecessors.end(); pred_iter++)\n\t \t{\n\t \t\t// Find the neighbour node\n\t \t\tNode predecessor;\n\t \t\tpredecessor = *pred_iter;\n\t \t\tpredecessor.id = predecessor.cell.index.row_major;\n\n\t \t\t// Skip Occupied or Inflated Cells\n\t \t\tif (predecessor.cell.celltype == map::Occupied or\\\n\t \t\t\tpredecessor.cell.celltype == map::Inflation)\n\t \t\t{\t \t\t\t\n\t \t\t\t// Push to priority queue for auto sort\n\t \t\t\t// This is not actually stored anywhere, just useful in getting min gcost for n\n\t \t\t\tpredecessor.gcost += heuristic(predecessor, n) + BIG_NUM;\n\t \t\t} else\n\t \t\t// Free Cells\n\t \t\t{\n\t \t\t\t// Push to priority queue for auto sort\n\t \t\t\t// This is not actually stored anywhere, just useful in getting min gcost for n\n\t \t\t\tpredecessor.gcost += heuristic(predecessor, n);\n\t \t\t}\n\n\t \t\tpred_costs.push(predecessor);\n\n\t \t}\n\n\t \tNode min_predecessor = pred_costs.top();\n\n\t \t// Update RHS value\n\t \tn.rhs = min_predecessor.gcost;\n\t \t// Update Parent\n\t \tn.parent_id = min_predecessor.id;\n\t }\n \t// If it's on the open list, remove it\n \tauto cell_iter = std::find_if(open_list.begin(), open_list.end(), [&](const Node & node) {return node.id == n.id;});\n \tif (cell_iter != open_list.end())\n \t{\n \t\t// Delete using points\n \t\topen_list.erase(cell_iter);\n \t\t// Re-sort open list\n \t\tstd::push_heap(open_list.begin(), open_list.end(), KeyComparator());\n \t}\n\n \t// If n is locally inconsistent (ie if n.gcost != n.rhs), add it to the open list with updated keys\n \tif (!(rigid2d::almost_equal(n.gcost, n.rhs)))\n \t{\n \t\tn.hcost = heuristic(n, goal_node);\n \t\tCalculateKeys(n);\n \t\topen_list.push_back(n);\n \t\tstd::push_heap(open_list.begin(), open_list.end(), KeyComparator());\n \t}\n\n \t// Update n in FakeGrid\n \tFakeGrid.at(n.id) = n;\n\t}\n\n\n\tvoid LPAstar::Initialize(const Vector2D & start, const Vector2D & goal, const map::Grid & grid_, const double & resolution)\n\t{\n\t\tGRID = grid_.return_fake_grid();\n\t\tFakeGrid.clear();\n\n\t\t// Find Start and Goal Nodes\n\t goal_node.cell = find_nearest_node(goal, grid_, resolution);\n\t goal_node.id = goal_node.cell.index.row_major;\n\t // Make sure the algo thinks it's free to begin with\n\t goal_node.cell.celltype = map::Free;\n\t goal_node.hcost = 0.0;\n\t goal_node.gcost = BIG_NUM;\n\t CalculateKeys(goal_node);\n\n\t // Find GRID cell whose coordinates most closely match the start coordinates\n\t start_node.cell = find_nearest_node(start, grid_, resolution);\n\t start_node.id = start_node.cell.index.row_major;\n\t // Make sure the algo thinks it's free to begin with\n\t start_node.cell.celltype = map::Free;\n\t start_node.hcost = heuristic(start_node, goal_node);\n\t start_node.rhs = 0.0;\n\t start_node.gcost = BIG_NUM;\n\t CalculateKeys(start_node);\n\t // std::cout << \"START, IDX: [\" << start_node.cell.index.x << \", \" << start_node.cell.index.y << \"]\"<< std::endl;\n\t // std::cout << \"GOAL, IDX: [\" << goal_node.cell.index.x << \", \" << goal_node.cell.index.y << \"]\" << \"ID: \" << goal_node.id << std::endl;\n\n\t\t// Populate Fake Grid\n\t\tfor (auto iter = GRID.begin(); iter < GRID.end(); iter++)\n\t\t{\n\t\t\tNode node;\n\t\t\tnode.cell = *iter;\n\t\t\tnode.id = node.cell.index.row_major;\n\t\t\tif (node.id == goal_node.id)\n\t\t\t{\n\t\t\t\tnode = goal_node;\n\t\t\t} else if (node.id == start_node.id)\n\t\t\t{\n\t\t\t\tnode = start_node;\n\t\t\t}\n\t\t\tnode.gcost = BIG_NUM;\n\t\t\t// TODO: When vanilla is done, clear all obstacle/inflated in FakeGrid for simulated increment\n\t\t\tFakeGrid.push_back(node);\n\t\t}\n\n\t\t// Populate Open List\n\t\topen_list.push_back(start_node);\n\t\tstd::make_heap(open_list.begin(), open_list.end(), KeyComparator());\n\n\t\t// Cross-update FakeGrid with start and goal\n\t\tFakeGrid.at(start_node.id) = start_node;\n\t\tFakeGrid.at(goal_node.id) = goal_node;\n\n\t\tstd::cout << \"Initialized!\" << std::endl;\n\t}\n\n\n\tvoid LPAstar::CalculateKeys(Node & n)\n\t{\n\t\tn.hcost = heuristic(n, goal_node);\n\t\tn.key1 = std::min(n.gcost, n.rhs) + n.hcost;\n\t\tn.key2 = std::min(n.gcost, n.rhs);\n\n\t\t// CAP at BIG_NUM\n\t\tif (n.gcost > BIG_NUM)\n\t\t{\n\t\t\tn.gcost = BIG_NUM;\n\t\t}\n\n\t\tif (n.rhs > BIG_NUM)\n\t\t{\n\t\t\tn.rhs = BIG_NUM;\n\t\t}\n\n\t\tif (n.hcost > BIG_NUM)\n\t\t{\n\t\t\tn.hcost = BIG_NUM;\n\t\t}\n\n\t\tif (n.key1 > BIG_NUM)\n\t\t{\n\t\t\tn.key1 = BIG_NUM;\n\t\t}\n\n\t\tif (n.key2 > BIG_NUM)\n\t\t{\n\t\t\tn.key2 = BIG_NUM;\n\t\t}\n\t}\n\n\n\tbool LPAstar::Continue(const int & iterations)\n\t{\n\t\tif(!std::is_heap(open_list.begin(), open_list.end(), KeyComparator()))\n\t\t{\n\t\t\tstd::make_heap(open_list.begin(), open_list.end(), KeyComparator());\n\t\t}\n\n\t\tNode top = open_list.at(0);\n\t\tCalculateKeys(top);\n\t\t// Update Goal Node from Fake Grid\n\t\tgoal_node = FakeGrid.at(goal_node.id);\n\t\tgoal_node.hcost = 0.0;\n\t\tCalculateKeys(goal_node);\n\t\tFakeGrid.at(goal_node.id) = goal_node;\n\n\t\t// Goal is NOT the minimum key\n\t\tbool not_minkey = true;\n\n\t\tif (rigid2d::almost_equal(top.key1, goal_node.key1))\n\t\t{\n\t\t\tif (!rigid2d::almost_equal(top.key2, goal_node.key2))\n\t\t\t{\n\t\t\t\tif (top.key2 > goal_node.key2)\n\t\t\t\t{\n\t\t\t\t\tnot_minkey = false;\n\t\t\t\t}\n\n\t\t\t} else\n\t\t\t{\n\t\t\t\tnot_minkey = false;\n\t\t\t}\n\t\t} else\n\t\t{\n\t\t\tif (top.key1 > goal_node.key1)\n\t\t\t{\n\t\t\t\tnot_minkey = false;\n\t\t\t}\n\t\t}\n\n\t\tbool consistent = false;\n\n\t\tif (rigid2d::almost_equal(goal_node.gcost, goal_node.rhs))\n\t\t{\n\t\t\tconsistent = true;\n\t\t}\n\n\t\t\n\t\t// std::cout << \"GOAL NODE GCOST: \" << goal_node.gcost << \" RHS: \" << goal_node.rhs << \" KEY1: \" << goal_node.key1 << \" KEY2: \" << goal_node.key2 << std::endl; \n\t\t// std::cout << \"MIN, IDX: [\" << top.cell.index.x << \", \" << top.cell.index.y << \"]\"<< \" KEY1: \" << top.key1 << \" KEY2: \" << top.key2 << std::endl;\n\t\t// if (not_minkey)\n\t\t// {\n\t\t// \tstd::cout << \"GOAL NODE NOT MIN KEY\" << std::endl;\n\t\t// }\n\t\t// if (consistent)\n\t\t// {\n\t\t// \tstd::cout << \"CONSISTENT\" << std::endl;\n\t\t// }\n\n\t\t// Conditions for Continuing\n\t\tif (not_minkey or (!consistent))\n\t\t{\n\t\t\treturn true;\n\t\t} else\n\t\t{\n\t\t\tif (goal_node.gcost >= BIG_NUM)\n\t\t\t{\n\t\t\t\tROS_WARN(\"There is no valid path. The goal is in a blocked cell! \\n Returning closest path.\");\n\t\t\t\tvalid_path = false;\n\t\t\t} else\n\t\t\t{\n\t\t\t\tstd::cout << \"Goal found after \" << iterations << \" Iterations!\" << std::endl;\n\t\t\t}\n\t\t\treturn false;\n\t\t}\n\t}\n\t\n\n\tstd::vector LPAstar::trace_path(const Node & final_node)\n\t{\n\t\t// Store old path in case we see an invalid one\n\t\tstd::vector old_path = path;\n\t\t// First node in the vector is 'final node'\n\t\tpath.clear();\n\t\tpath.push_back(final_node);\n\n\t\tNode next_node = final_node;\n\n\t\twhile (next_node.parent_id != -1)\n\t\t{\n\t\t\tint parent_id = next_node.parent_id;\n\t\t\tif (parent_id >= 0)\n\t\t\t{\n\t\t\t\tint grandparent_id = FakeGrid.at(parent_id).parent_id;\n\t\t\t\tif (grandparent_id == next_node.id)\n\t\t\t\t\t// TWO NODES ARE EACH OTHERS PARENTS.\n\t\t\t\t\t// THIS USUALLY MEANS THAT THE GOAL IS INSIDE AN OBSTACLE\n\t\t\t\t{\n\t\t\t\t\tNode n1 = next_node;\n\t\t\t\t\tNode n2 = FakeGrid.at(parent_id);\n\t\t\t\t\tstd::cout << \"Two Nodes are each others' parents!\" << std::endl;\n\t\t\t\t\tstd::cout << \"Node 1 at [\" << n1.cell.index.x << \", \" << n1.cell.index.y << \"]\" << std::endl;\n\t\t\t\t\tstd::cout << \"Node 2 at [\" << n2.cell.index.x << \", \" << n2.cell.index.y << \"]\" << std::endl;\n\n\t\t\t\t\tif (n1.hcost < n2.hcost or rigid2d::almost_equal(n1.hcost, n2.hcost))\n\t\t\t\t\t{\n\t\t\t\t\t\t// The current node is closest to the goal, so return it as the final path\n\t\t\t\t\t\tvalid_path = false;\n\t\t\t\t\t\tpath = old_path;\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t}\n\t\t\t\t}\n\t\t\t}\n\t\t\t// std::cout << \"GOING FROM NODE: \" << path.back().vertex.id << \" TO: \" << path.back().parent_id << std::endl;\n\t\t\tnext_node = FakeGrid.at(next_node.parent_id);\n\t\t\tpath.push_back(next_node);\n\t\t}\n\n\t\tstd::reverse(path.begin(), path.end());\n\n\t\tstd::cout << \"The path contains \" << path.size() << \" Nodes.\" << std::endl;\n\t\treturn path;\n\t}\n\n\n\tstd::vector LPAstar::get_neighbours(const Node & n, const std::vector & map)\n\t{\n\t\tint x_max = map.back().cell.index.x;\n\t\tint y_max = map.back().cell.index.y; \n\t\tstd::vector neighbours;\n\t\t// std::cout << \"Node at [\" << n.cell.index.x << \", \" << n.cell.index.y << \"]\" << std::endl;\n\n\t\t// Evaluate about 3x3 block for 8-connectivity\n\t\tfor (int x = -1; x < 2; x++)\n\t\t{\n\t\t\tfor (int y = -1; y < 2; y++)\n\t\t\t{\n\t\t\t\t// Skip x,y = (0,0) since that's the current node\n\t\t\t\tif (x == 0 and y == 0)\n\t\t\t\t{\n\t\t\t\t\tcontinue;\n\t\t\t\t} else\n\t\t\t\t{\n\t\t\t\t\tint check_x = n.cell.index.x + x;\n\t\t\t\t\tint check_y = n.cell.index.y + y;\n\n\t\t\t\t\t// Ensure potential neighbour is within grid bounds\n\t\t\t\t\tif (check_x >= 0 and check_x <= x_max and check_y >= 0 and check_y <= y_max)\n\t\t\t\t\t{\n\t\t\t\t\t\t// Now we need to grab the right cell from the map. To do this: index->RMJ\n\t\t\t\t\t\t// std::cout << \"Neighbour at [\" << check_x << \", \" << check_y << \"]\" << std::endl;\n\t\t\t\t\t\tint rmj = map::grid2rowmajor(check_x, check_y, x_max + 1);\n\t\t\t\t\t\tNode nbr = map.at(rmj);\n\t\t\t\t\t\t// std::cout << \"Checked Neighbour at [\" << nbr.cell.index.x << \", \" << nbr.cell.index.y << \"]\" << std::endl;\n\t\t\t\t\t\tneighbours.push_back(nbr);\n\t\t\t\t\t}\n\t\t\t\t}\n\t\t\t}\n\t\t}\n\t\treturn neighbours;\n\t}\n\n\n\tstd::vector LPAstar::return_path()\n\t{\n\t\ttrace_path(goal_node);\n\t\treturn path;\n\t}\n\n\n\tstd::vector LPAstar::SimulateUpdate(const std::vector & updated_grid)\n\t{\n\t\tGRID = updated_grid;\n\t\tstd::vector updated_nodes;\n\t\tfor (unsigned int i = 0; i < updated_grid.size(); i++)\n\t\t{\n\t\t\t// For each UPDATED Cell (cell.newView = true;), UpdateCell() on its neighbours\n\t\t\tif (updated_grid.at(i).newView and FakeGrid.at(i).cell.celltype != updated_grid.at(i).celltype)\n\t\t\t{\n\t\t\t\t// First, update FakeGrid\n\t\t\t\tFakeGrid.at(i).cell = updated_grid.at(i);\n\t\t\t\t// Push back culprit\n\t\t\t\tupdated_nodes.push_back(FakeGrid.at(i));\n\n\t\t\t\t// std::cout << \"NEW OCCUPANCY: \" << FakeGrid.at(i).cell.index.row_major << std::endl;\n\n\t\t\t\tstd::vector neighbours = get_neighbours(FakeGrid.at(i), FakeGrid);\n\t\t\t\tfor (auto iter = neighbours.begin(); iter < neighbours.end(); iter++)\n\t\t\t\t{\n\t\t\t\t\tUpdateCell(*iter);\n\t\t\t\t\t// Push back neighbours\n\t\t\t\t\tupdated_nodes.push_back(*iter);\n\t\t\t\t}\n\t\t\t\t// Update Changed Node\n\t\t\t\tUpdateCell(FakeGrid.at(i));\n\t\t\t}\n\t\t}\n\n\t\t// Update Goal Node\n\t\t// UpdateCell(FakeGrid.at(goal_node.id));\n\n\t\t// Re-sort open-list\n\t\t// std::priority_queue , KeyComparator > temp_open_list;\n\t\t// while (!open_list.empty())\n\t\t// {\n\t\t// \tNode temp = open_list.top();\n\t\t// \ttemp.hcost = heuristic(temp, goal_node);\n\t\t// \tCalculateKeys(temp);\n\t\t// \ttemp_open_list.push(temp);\n\t\t// \topen_list.pop();\n\t\t// \tFakeGrid.at(temp.id) = temp;\n\t\t// }\n\n\t\t// open_list = temp_open_list;\n\n\t\t// Compute Shortest Path\n\t\tComputeShortestPath();\n\t\t// Return updated nodes\n\t\treturn updated_nodes;\n\t}\n\n\tbool LPAstar::return_valid()\n\t{\n\t\treturn valid_path;\n\t}\n\n\n\tvoid DSL::Initialize(const Vector2D & start, const Vector2D & goal, const map::Grid & grid_, const double & resolution)\n\t{\n\t\tGRID = grid_.return_fake_grid();\n\t\tFakeGrid.clear();\n\n\t\t// NOTE: START AND GOAL POSITIONS FLIPPED FOR D*LITE\n\t\t// Find Start and Goal Nodes\n\t goal_node.cell = find_nearest_node(start, grid_, resolution);\n\t goal_node.id = goal_node.cell.index.row_major;\n\t // Make sure the algo thinks it's free to begin with\n\t goal_node.cell.celltype = map::Free;\n\t goal_node.hcost = 0.0;\n\t goal_node.gcost = BIG_NUM;\n\t CalculateKeys(goal_node);\n\n\t // Find GRID cell whose coordinates most closely match the start coordinates\n\t start_node.cell = find_nearest_node(goal, grid_, resolution);\n\t // Make sure the algo thinks it's free to begin with\n\t start_node.cell.celltype = map::Free;\n\t start_node.id = start_node.cell.index.row_major;\n\t start_node.hcost = heuristic(start_node, goal_node);\n\t start_node.rhs = 0.0;\n\t start_node.gcost = BIG_NUM;\n\t CalculateKeys(start_node);\n\t // std::cout << \"START, IDX: [\" << start_node.cell.index.x << \", \" << start_node.cell.index.y << \"]\"<< std::endl;\n\t // std::cout << \"GOAL, IDX: [\" << goal_node.cell.index.x << \", \" << goal_node.cell.index.y << \"]\"<< std::endl;\n\n\t\t// Populate Fake Grid\n\t\tfor (auto iter = GRID.begin(); iter < GRID.end(); iter++)\n\t\t{\n\t\t\tNode node;\n\t\t\tnode.cell = *iter;\n\t\t\tnode.id = node.cell.index.row_major;\n\t\t\tif (node.id == goal_node.id)\n\t\t\t{\n\t\t\t\tnode = goal_node;\n\t\t\t} else if (node.id == start_node.id)\n\t\t\t{\n\t\t\t\tnode = start_node;\n\t\t\t}\n\t\t\tnode.gcost = BIG_NUM;\n\t\t\t// TODO: When vanilla is done, clear all obstacle/inflated in FakeGrid for simulated increment\n\t\t\tFakeGrid.push_back(node);\n\t\t}\n\n\t\t// Populate Open List\n\t\topen_list.push_back(start_node);\n\t\tstd::make_heap(open_list.begin(), open_list.end(), KeyComparator());\n\n\t\t// Cross-update FakeGrid with start and goal\n\t\tFakeGrid.at(start_node.id) = start_node;\n\t\tFakeGrid.at(goal_node.id) = goal_node;\n\n\t\tstd::cout << \"Initialized!\" << std::endl;\n\t}\n\n\n\tstd::vector DSL::SimulateUpdate(const std::vector & updated_grid)\n\t{\n\t\t// First, make sure g(goal [start in paper]!= inf, otherwise no path)\n\t\tif (goal_node.gcost >= BIG_NUM)\n\t\t{\n\t\t\tstd::cout << \"There is no valid path.\" << std::endl;\n\t\t\ttrace_path(goal_node);\n\t\t}\n\t\t// Change new goal node [start in D*Lite paper]\n\t\tstd::priority_queue , CostComparator > pred_costs;\n\t\t// Find the predecessors of node n\n\t\tstd::vector predecessors = get_neighbours(goal_node, FakeGrid);\n\n\t\tfor (auto pred_iter = predecessors.begin(); pred_iter < predecessors.end(); pred_iter++)\n \t{\n \t\t// Find the neighbour node\n \t\tNode predecessor;\n \t\tpredecessor = *pred_iter;\n \t\tpredecessor.id = predecessor.cell.index.row_major;\n\n \t\t// Occupied or Inflated Cells\n \t\tif (predecessor.cell.celltype == map::Occupied or\\\n \t\t\tpredecessor.cell.celltype == map::Inflation)\n \t\t{\t \t\t\t\n \t\t\t// Push to priority queue for auto sort\n \t\t\t// This is not actually stored anywhere, just useful in getting min gcost for n\n \t\t\tpredecessor.gcost += heuristic(predecessor, goal_node) + BIG_NUM;\n \t\t} else\n \t\t// Free Cells\n \t\t{\n \t\t\t// Push to priority queue for auto sort\n \t\t\t// This is not actually stored anywhere, just useful in getting min gcost for n\n \t\t\tpredecessor.gcost += heuristic(predecessor, goal_node);\n \t\t}\n\n \t\tpred_costs.push(predecessor);\n \t}\n\n \tNode min_predecessor = pred_costs.top();\n\n \t//Update Goal Node (start in D*L paper)\n \tgoal_node = FakeGrid.at(min_predecessor.id);\n\t\tGRID = updated_grid;\n\t\tstd::vector updated_nodes = LPAstar::SimulateUpdate(updated_grid);\n\n\t\treturn updated_nodes;\n\t}\n\n\tstd::vector DSL::return_path()\n\t{\n\t\ttrace_path(goal_node);\n\t\tstd::vector p = path;\n\t\t// If the path is invalid, we don't reverse it for viz purposes\n\t\tif (valid_path)\n\t\t{\n\t\t\tstd::reverse(p.begin(), p.end());\n\t\t} else\n\t\t{\n\t\t\t// Remove first element of old path since we moved 1 up from here\n\t\t\tpath.erase(path.begin());\n\t\t}\n\t\treturn p;\n\t}\n\n}\t" }, { "path": "global_planner/src/global_planner/potential_field.cpp", "content": "#include \"global_planner/potential_field.hpp\"\n\nnamespace global\n{\n\tusing rigid2d::Vector2D;\n\n\t// Global Planner\n\tPotentialField::PotentialField(const std::vector & obstacles_, const double & eta_, const double & ada_,\n \t\t\t const double & zeta_, const double & d_thresh_, const double & Q_thresh_)\n\t{\n\t\tobstacles = obstacles_;\n\t\teta = eta_;\n\t\tzeta = zeta_;\n\t\tada = ada_;\n\t\td_thresh = d_thresh_;\n\t\tQ_thresh = Q_thresh_;\n\t}\n\n\tVector2D PotentialField::AttractiveGradient(const Vector2D & cur_pos, const Vector2D & goal)\n\t{\n\t\tVector2D dUA;\n\n\t\tdouble d_goal = map::euclidean_distance(cur_pos.x - goal.x, cur_pos.y - goal.y);\n\n\t\tif (d_goal <= d_thresh or rigid2d::almost_equal(d_goal, d_thresh))\n\t\t{\n\t\t\t// Quadratic\n\t\t\tdUA.x = zeta * (cur_pos.x - goal.x);\n\t\t\tdUA.y = zeta * (cur_pos.y - goal.y);\n\t\t} else\n\t\t{\n\t\t\t// Conic\n\t\t\tdUA.x = d_thresh * zeta * (cur_pos.x - goal.x) / d_goal;\n\t\t\tdUA.y = d_thresh * zeta * (cur_pos.y - goal.y) / d_goal;\n\t\t}\n\n\t\treturn dUA;\n\t}\n\n\tVector2D PotentialField::RepulsiveGradient(const Vector2D & cur_pos, const std::vector & obs)\n\t{\n\t\tVector2D dUR(0.0, 0.0);\n\n\t\tfor (auto obs_iter = obs.begin(); obs_iter < obs.end(); obs_iter++)\n\t\t{\n\t\t\tVector2D closest_point = FindClosestPoint(cur_pos, obs_iter->vertices);\n\n\t\t\tdouble Q_obs = map::euclidean_distance(cur_pos.x - closest_point.x, cur_pos.y - closest_point.y);\n\n\t\t\tif (Q_obs <= Q_thresh or rigid2d::almost_equal(Q_obs, Q_thresh))\n\t\t\t// Closest point within range of influence\n\t\t\t{\n\t\t\t\tdouble deltaDx = (cur_pos.x - closest_point.x) / Q_obs;\n\t\t\t\tdouble deltaDy = (cur_pos.y - closest_point.y) / Q_obs;\n\n\t\t\t\tdUR.x += ada * ((1.0 / Q_thresh) - (1.0 / Q_obs)) * (1.0 / (std::pow(Q_obs, 2))) * deltaDx;\n\t\t\t\tdUR.y += ada * ((1.0 / Q_thresh) - (1.0 / Q_obs)) * (1.0 / (std::pow(Q_obs, 2))) * deltaDy;\n\t\t\t}\n\t\t}\n\n\t\treturn dUR;\n\t}\n\n\tVector2D PotentialField::FindClosestPoint(const Vector2D & cur_pos, const std::vector & vertices)\n\t{\n\t\tmap::Vertex P0(cur_pos);\n\n\t\tdouble shortest_dist = 1e12;\n\n\t\tVector2D closest_point = vertices.front();\n\n\t\tfor (auto v_iter = vertices.begin(); v_iter < vertices.end(); v_iter++)\n\t\t{\n\t\t\t// Vector from current and next vertex. so if 4 vertices: 0->1, 1->2, 2->3, 3->0\n\t\t\t// Record current index\n\t\t\tint i = static_cast(std::distance(vertices.begin(), v_iter));\n\t\t\t// If current index is the last one, loop back to zero for vector construction\n\t\t\t// -1 because indeces start at 0\n\t\t\tif (i == static_cast(vertices.size()) - 1)\n\t\t\t{\n\t\t\t\ti = 0;\n\t\t\t} else\n\t\t\t// Otherwise, just use the next index\n\t\t\t{\n\t\t\t\ti += 1;\n\t\t\t}\n\n\t\t\t// Referencing: https://drive.google.com/file/d/1gQuR4J80aXZ9BBL1s3K83TmxQSWD3AWt/view?usp=sharing Slide 28\n\t\t\tauto E1 = map::Vertex(*v_iter);\n\t\t\tauto E2 = map::Vertex(vertices.at(i));\n\n\t\t\tmap::ShortestDistance shrt = map::lineToPoint(E1, E2, P0);\n\n\t\t\tif ((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))\n\t\t\t// cur_pos inside line segment\n\t\t\t{\n\t\t\t\tif (std::abs(shrt.D) < shortest_dist)\n\t\t\t\t{\n\t\t\t\t\tclosest_point = shrt.point;\n\t\t\t\t\tshortest_dist = std::abs(shrt.D);\n\t\t\t\t}\n\t\t\t} else\n\t\t\t// cur_pos outside line segment\n\t\t\t{\n\t\t\t\t// Find distance to both vertices and take minimum for evaluation with whole minimum\n\t\t\t\tdouble dist_E1 = map::euclidean_distance(cur_pos.x - v_iter->x, cur_pos.y - v_iter->y);\n\t\t\t\tdouble dist_E2 = map::euclidean_distance(cur_pos.x - vertices.at(i).x, cur_pos.y - vertices.at(i).y);\n\n\t\t\t\tif (dist_E1 < shortest_dist)\n\t\t\t\t{\n\t\t\t\t\tclosest_point = *v_iter;\n\t\t\t\t\tshortest_dist = dist_E1;\n\t\t\t\t}\n\t\t\t\t// Possible to overwrite E1\n\t\t\t\tif (dist_E2 < shortest_dist)\n\t\t\t\t{\n\t\t\t\t\tclosest_point = vertices.at(i);\n\t\t\t\t\tshortest_dist = dist_E2;\n\t\t\t\t}\n\t\t\t}\n\t\t}\n\n\t\t// std::cout << \"CLOSEST POINT TO: [\" << cur_pos.x << \", \" << cur_pos.y << \"] IS: [\" << closest_point.x << \", \" <<\n\t\t// \t\t\t closest_point.y << \"]\" << std::endl;\n\n\t\treturn closest_point;\n\t}\n\n\tdouble PotentialField::MultiDimNorm(const std::vector partials)\n\t{\n\t\tdouble norm_squared = 0.0;\n\n\t\tfor (auto iter = partials.begin(); iter < partials.end(); iter++)\n\t\t{\n\t\t\tnorm_squared += std::pow(*iter, 2);\n\t\t}\n\n\t\t// Avoid division by zero\n\t\tnorm_squared += 1e-3;\n\n\t\treturn std::sqrt(norm_squared);\n\t}\n\n\tVector2D PotentialField::OneStepGD(const Vector2D & cur_pos, const Vector2D & goal, std::vector & obs)\n\t{\n\t\tVector2D dUA = AttractiveGradient(cur_pos, goal);\n\t\tVector2D dUR = RepulsiveGradient(cur_pos, obs);\n\n\t\tVector2D dU(dUA.x + dUR.x, dUA.y + dUR.y);\n\n\t\tstd::vector partials{dU.x, dU.y};\n\n\t\tdouble norm = MultiDimNorm(partials);\n\n\t\tdouble Descent_x = dU.x / norm;\n\t\tdouble Descent_y = dU.y / norm;\n\n\t\tVector2D new_pos(cur_pos.x - Descent_x * eta, cur_pos.y - Descent_y * eta);\n\n\t\tdouble dist2goal = map::euclidean_distance(new_pos.x - goal.x, new_pos.y - goal.y);\n\n\t\tif (dist2goal <= eta or rigid2d::almost_equal(dist2goal, eta))\n\t\t{\n\t\t\tterminate = true;\n\t\t}\n\n\t\treturn new_pos;\n\t}\n\n\tbool PotentialField::return_terminate()\n\t{\n\t\treturn terminate;\n\t}\n}\n" }, { "path": "global_planner/src/lpastar.cpp", "content": "/// \\file\n/// \\brief Draws Each Obstacle in RViz using MarkerArrays\n///\n/// PARAMETERS:\n/// PUBLISHES:\n/// SUBSCRIBES:\n/// FUNCTIONS:\n\n#include \n\n#include \n#include \n#include \n\n#include \"map/map.hpp\"\n#include \"map/prm.hpp\"\n#include \"map/grid.hpp\"\n#include \n#include \"global_planner/incremental.hpp\"\n\n#include \"nuslam/TurtleMap.h\"\n\n#include // To use std::bind\n#include \n#include \n#include \n\n// Used to deal with YAML list of lists\n#include // catkin component\n\n\nint main(int argc, char** argv)\n/// The Main Function ///\n{\n ROS_INFO(\"STARTING NODE: astar\");\n\n // Vars\n double frequency = 10.0;\n // Scale by which to transform marker poses (10 cm/cell so we use 10)\n double SCALE = 10.0;\n std::string frame_id = \"base_footprint\";\n XmlRpc::XmlRpcValue xml_obstacles;\n std::vector start_vec{7.0, 3.0};\n std::vector goal_vec{7.0, 26.0};\n double resolution = 0.01;\n\n // Map Parameters\n double thresh = 0.01;\n double inflate = 0.1;\n int visibility = 5;\n\n // store Obstacle(s) here to create Map\n std::vector obstacles_v;\n\n ros::init(argc, argv, \"astar_node\"); // register the node on ROS\n ros::NodeHandle nh; // get a handle to ROS\n ros::NodeHandle nh_(\"~\"); // get a handle to ROS\n // Parameters\n nh_.getParam(\"frequency\", frequency);\n nh_.getParam(\"obstacles\", xml_obstacles);\n nh_.getParam(\"start\", start_vec);\n nh_.getParam(\"goal\", goal_vec);\n nh_.getParam(\"map_frame_id\", frame_id);\n nh_.getParam(\"thresh\", thresh);\n nh_.getParam(\"inflate\", inflate);\n nh_.getParam(\"scale\", SCALE);\n nh_.getParam(\"resolution\", resolution);\n nh_.getParam(\"visibility\", visibility);\n\n rigid2d::Vector2D start(start_vec.at(0)/SCALE, start_vec.at(1)/SCALE);\n rigid2d::Vector2D goal(goal_vec.at(0)/SCALE, goal_vec.at(1)/SCALE);\n\n // 'obstacles' is a triple-nested list.\n // 1st level: obstacle (Obstacle), 2nd level: vertices (std::vector), 3rd level: coordinates (Vector2D)\n\n // std::vector\n if(xml_obstacles.getType() != XmlRpc::XmlRpcValue::TypeArray)\n {\n ROS_ERROR(\"There is no list of obstacles\");\n } else\n {\n for(int i = 0; i < xml_obstacles.size(); ++i)\n {\n // Obstacle contains std::vector\n // create Obstacle with empty vertex vector\n map::Obstacle obs;\n if(xml_obstacles[i].getType() != XmlRpc::XmlRpcValue::TypeArray)\n {\n ROS_ERROR(\"obstacles[%d] has no vertices\", i);\n } else {\n for(int j = 0; j < xml_obstacles[i].size(); ++j)\n {\n // Vector2D contains x,y center_coords\n if(xml_obstacles[i][j].size() != 2)\n {\n ROS_ERROR(\"Vertex[%d] of obstacles[%d] is not a pair of coordinates\", j, i);\n } else if(\n xml_obstacles[i][j][0].getType() != XmlRpc::XmlRpcValue::TypeDouble or\n xml_obstacles[i][j][1].getType() != XmlRpc::XmlRpcValue::TypeDouble)\n {\n ROS_ERROR(\"The coordinates of vertex[%d] of obstacles[%d] are not doubles\", j, i);\n } else {\n // PASSED ALL THE TESTS: push Vector2D to vertices list in Obstacle object\n rigid2d::Vector2D vertex(xml_obstacles[i][j][0], xml_obstacles[i][j][1]);\n // NOTE: SCALE DOWN\n vertex.x /= SCALE;\n vertex.y /= SCALE;\n obs.vertices.push_back(vertex);\n }\n }\n }\n // push Obstacle object to vector of Object(s) in Map\n obstacles_v.push_back(obs);\n }\n }\n\n // Initialize Marker\n // Init Marker Array Publisher\n ros::Publisher path_pub = nh.advertise(\"path\", 1);\n\n ros::Publisher grid_pub = nh.advertise(\"grid_map\", 1);\n // Initialize grid_map outside\n nav_msgs::OccupancyGrid grid_map;\n // rviz representation of the grid\n std::vector map;\n\n // Init Marker Array\n visualization_msgs::MarkerArray path_arr;\n\n // Init Marker\n visualization_msgs::Marker marker;\n marker.header.frame_id = frame_id;\n marker.header.stamp = ros::Time::now();\n // marker.ns = \"my_namespace\";\n // marker.id = 0;\n marker.type = visualization_msgs::Marker::LINE_STRIP;\n marker.action = visualization_msgs::Marker::ADD;\n marker.pose.position.z = 0;\n marker.pose.orientation.x = 0.0;\n marker.pose.orientation.y = 0.0;\n marker.pose.orientation.z = 0.0;\n marker.pose.orientation.w = 1.0;\n marker.scale.x = 0.02;\n marker.color.r = 0.96f;\n marker.color.g = 0.475f;\n marker.color.b = 0.0f;\n marker.color.a = 1.0;\n marker.lifetime = ros::Duration();\n\n // LINE_STRIP relative to this pose\n marker.pose.position.x = 0.0;\n marker.pose.position.y = 0.0;\n\n // PATH MARKERS\n visualization_msgs::Marker path_marker;\n path_marker = marker;\n // More visible than PRM\n path_marker.scale.x = 0.03;\n path_marker.color.r = 0.2;\n path_marker.color.g = 1.0;\n path_marker.color.b = 0.2;\n\n visualization_msgs::Marker path_sph_mkr;\n path_sph_mkr = marker;\n // More visible than PRM\n path_sph_mkr.type = visualization_msgs::Marker::CUBE;\n path_sph_mkr.scale.x = 0.02;\n path_sph_mkr.scale.y = 0.02;\n path_sph_mkr.scale.z = 0.02;\n path_sph_mkr.color.r = 0.0f;\n path_sph_mkr.color.g = 0.0f;\n path_sph_mkr.color.b = 0.0f;\n\n // CURRENT POSITION MARKER\n visualization_msgs::Marker curr_pos_marker;\n curr_pos_marker = path_sph_mkr;\n // More visible than PRM\n curr_pos_marker.scale.x = 0.1;\n curr_pos_marker.scale.y = 0.1;\n curr_pos_marker.color.r = 1.0;\n curr_pos_marker.color.g = 0.2;\n curr_pos_marker.color.b = 0.2;\n\n // UpdateCell() MARKER\n visualization_msgs::Marker update_marker;\n update_marker = path_sph_mkr;\n // More visible than PRM\n update_marker.scale.x = 0.04;\n update_marker.scale.y = 0.04;\n update_marker.scale.z = 0.04;\n update_marker.color.r = 0.96f;\n update_marker.color.g = 0.475f;\n update_marker.color.b = 0.0f;\n update_marker.color.a = 1.0;\n update_marker.lifetime = ros::Duration(1.0 / frequency);\n\n\n // LPA* or D* Lite Path\n std::vector path;\n \n // GRID Version\n // Initialize Grid\n map::Grid grid(obstacles_v, inflate);\n\n // Build Map\n grid.build_map(resolution);\n\n ROS_INFO(\"Grid Built!\");\n\n // Get map bounds\n auto bounds = grid.return_map_bounds();\n auto gridsize = grid.return_grid_dimensions();\n\n // Grid Pose\n geometry_msgs::Pose map_pose;\n map_pose.position.x = bounds.at(0).x;\n map_pose.position.y = bounds.at(0).y;\n map_pose.position.z = 0.0;\n map_pose.orientation.x = 0.0;\n map_pose.orientation.y = 0.0;\n map_pose.orientation.z = 0.0;\n map_pose.orientation.w = 1.0;\n\n // Occupancy grid for visualization\n grid_map.header.frame_id = frame_id;\n grid_map.info.resolution = resolution;\n grid_map.info.width = gridsize.at(0);\n grid_map.info.height = gridsize.at(1);\n\n // std::cout << \"Width: \" << gridsize.at(0) << \"\\t Height: \" << gridsize.at(1) << std::endl;\n\n grid_map.info.origin = map_pose;\n\n ROS_INFO(\"Planning using LPA*!\");\n global::LPAstar lpastar(obstacles_v, inflate);\n lpastar.Initialize(start, goal, grid, resolution);\n lpastar.ComputeShortestPath();\n path = lpastar.return_path();\n\n ros::Rate rate(frequency);\n\n unsigned int path_counter = 0;\n std::vector updated_nodes;\n\n // SLEEP so that I can prepare my screen recorder\n ros::Duration(2.0).sleep();\n\n // For visualization purposes (See below)\n bool firstpass = true;\n\n // Tells us if path contains obstacles\n bool valid_path = true;\n\n // Main While\n while (ros::ok())\n {\n ros::spinOnce();\n\n // rviz representation of the grid\n grid.fake_occupancy_grid(map);\n // Publish GRID Map\n grid_map.header.stamp = ros::Time::now();\n grid_map.info.map_load_time = ros::Time::now();\n grid_map.data = map;\n grid_pub.publish(grid_map);\n\n // DRAW PATH\n path_arr.markers.clear();\n path_marker.points.clear();\n int path_marker_id = 0;\n for (auto path_iter = path.begin(); path_iter != path.end(); path_iter++)\n {\n // Add node as marker cell\n geometry_msgs::Point vtx;\n vtx.x = path_iter->cell.center_coords.x;\n vtx.y = path_iter->cell.center_coords.y;\n vtx.z = 0.0;\n path_marker.points.push_back(vtx);\n\n // Also push back cubes\n path_sph_mkr.pose.position.x = path_iter->cell.center_coords.x;\n path_sph_mkr.pose.position.y = path_iter->cell.center_coords.y;\n path_sph_mkr.id = path_marker_id;\n path_marker_id++;\n path_arr.markers.push_back(path_sph_mkr);\n }\n path_marker.id = path_marker_id;\n path_arr.markers.push_back(path_marker);\n curr_pos_marker.pose.position.x = path.at(path_counter).cell.center_coords.x;\n curr_pos_marker.pose.position.y = path.at(path_counter).cell.center_coords.y;\n\n // Push Back Current Position\n path_marker_id++;\n curr_pos_marker.id = path_marker_id;\n path_arr.markers.push_back(curr_pos_marker);\n\n // Push Back Updated Cell Markers\n for (auto node_iter = updated_nodes.begin(); node_iter != updated_nodes.end(); node_iter++)\n {\n path_marker_id++;\n update_marker.pose.position.x = node_iter->cell.center_coords.x;\n update_marker.pose.position.y = node_iter->cell.center_coords.y;\n update_marker.id = path_marker_id;\n path_arr.markers.push_back(update_marker);\n }\n\n // Publish Path\n path_pub.publish(path_arr);\n\n // FAKE Update\n updated_nodes.clear();\n if (path_counter < path.size() - 1 and valid_path)\n {\n grid.update_grid(path.at(path_counter).cell, visibility);\n path_counter++;\n // ROS_INFO(\"UPDATE NUMBER: %d\", path_counter);\n // LPA* Update\n updated_nodes = lpastar.SimulateUpdate(grid.return_fake_grid());\n path = lpastar.return_path();\n // Stop condition in case of obstacles\n valid_path = lpastar.return_valid();\n } else\n {\n // FINAL PATH. INFINITE MARKER DURATION\n path_marker.lifetime = ros::Duration();\n path_sph_mkr.lifetime = ros::Duration();\n curr_pos_marker.lifetime = ros::Duration();\n }\n\n // Make the markers persist for the first iteration to show original path\n if (firstpass)\n {\n ros::Duration(1.0).sleep();\n path_marker.lifetime = ros::Duration(2.0 / frequency);\n path_sph_mkr.lifetime = ros::Duration(2.0 / frequency);\n curr_pos_marker.lifetime = ros::Duration(2.0 / frequency);\n }\n\n firstpass = false;\n\n rate.sleep();\n }\n\n return 0;\n}" }, { "path": "global_planner/src/pf.cpp", "content": "/// \\file\n/// \\brief Draws Each Obstacle in RViz using MarkerArrays\n///\n/// PARAMETERS:\n/// PUBLISHES:\n/// SUBSCRIBES:\n/// FUNCTIONS:\n\n#include \n\n#include \n#include \n#include \n\n#include \"map/map.hpp\"\n#include \"map/prm.hpp\"\n#include \"map/grid.hpp\"\n#include \n#include \"global_planner/potential_field.hpp\"\n\n#include \n#include \n#include \n#include \n#include \n\n#include // To use std::bind\n\n// Used to deal with YAML list of lists\n#include // catkin component\n\n\nint main(int argc, char** argv)\n/// The Main Function ///\n{\n ROS_INFO(\"STARTING NODE: pf\");\n\n // Vars\n double frequency = 10.0;\n // Scale by which to transform marker poses (10 cm/cell so we use 10)\n double SCALE = 10.0;\n std::string frame_id = \"base_footprint\";\n XmlRpc::XmlRpcValue xml_obstacles;\n\n // Map Parameters\n double thresh = 0.01;\n double inflate = 0.1;\n\n std::vector start_vec{7.0, 3.0};\n std::vector goal_vec{7.0, 26.0};\n\n // PF Parameters\n double eta = 1.0;\n double ada = .1;\n double zeta = 10.0;\n double d_thresh = 2.0;\n double Q_thresh = 0.1;\n\n // store Obstacle(s) here to create Map\n std::vector obstacles_v;\n\n ros::init(argc, argv, \"pf_node\"); // register the node on ROS\n ros::NodeHandle nh; // get a handle to ROS\n ros::NodeHandle nh_(\"~\"); // get a handle to ROS\n // Parameters\n nh_.getParam(\"frequency\", frequency);\n nh_.getParam(\"obstacles\", xml_obstacles);\n nh_.getParam(\"map_frame_id\", frame_id);\n nh_.getParam(\"thresh\", thresh);\n nh_.getParam(\"inflate\", inflate);\n nh_.getParam(\"scale\", SCALE);\n nh_.getParam(\"start\", start_vec);\n nh_.getParam(\"goal\", goal_vec);\n // PF Params\n nh_.getParam(\"eta\", eta);\n nh_.getParam(\"ada\", ada);\n nh_.getParam(\"zeta\", zeta);\n nh_.getParam(\"d_thresh\", d_thresh);\n nh_.getParam(\"Q_thresh\", Q_thresh);\n\n // START and GOAL\n rigid2d::Vector2D start(start_vec.at(0)/SCALE, start_vec.at(1)/SCALE);\n rigid2d::Vector2D goal(goal_vec.at(0)/SCALE, goal_vec.at(1)/SCALE);\n\n // Path Publishers\n ros::Publisher path_pub = nh_.advertise(\"robot_path\", 1);\n\n // Using path topic since it's already in rviz\n ros::Publisher sg_pub = nh.advertise(\"path\", 1);\n\n\n // Init Markers\n visualization_msgs::Marker s_marker;\n s_marker.header.frame_id = frame_id;\n s_marker.header.stamp = ros::Time::now();\n s_marker.type = visualization_msgs::Marker::SPHERE;\n s_marker.action = visualization_msgs::Marker::ADD;\n s_marker.pose.position.z = 0;\n s_marker.pose.position.x = start.x;\n s_marker.pose.position.y = start.y;\n s_marker.pose.orientation.x = 0.0;\n s_marker.pose.orientation.y = 0.0;\n s_marker.pose.orientation.z = 0.0;\n s_marker.pose.orientation.w = 1.0;\n s_marker.scale.x = 0.05;\n s_marker.scale.y = 0.05;\n s_marker.scale.z = 0.05;\n s_marker.color.r = 0.5;\n s_marker.color.g = 0.0;\n s_marker.color.b = 0.5;\n s_marker.color.a = 1.0;\n s_marker.id = 0;\n s_marker.lifetime = ros::Duration();\n\n visualization_msgs::Marker g_marker = s_marker;\n g_marker.pose.position.x = goal.x;\n g_marker.pose.position.y = goal.y;\n g_marker.color.r = 0.0;\n g_marker.color.g = 1.0;\n g_marker.color.b = 0.0;\n g_marker.id = 1;\n\n // Init Marker Array\n visualization_msgs::MarkerArray sg_arr;\n\n sg_arr.markers.clear();\n sg_arr.markers.push_back(s_marker);\n sg_arr.markers.push_back(g_marker);\n\n\n\n // 'obstacles' is a triple-nested list.\n // 1st level: obstacle (Obstacle), 2nd level: vertices (std::vector), 3rd level: coordinates (Vector2D)\n // std::vector\n if(xml_obstacles.getType() != XmlRpc::XmlRpcValue::TypeArray)\n {\n ROS_ERROR(\"There is no list of obstacles\");\n } else {\n for(int i = 0; i < xml_obstacles.size(); ++i)\n {\n // Obstacle contains std::vector\n // create Obstacle with empty vertex vector\n map::Obstacle obs;\n if(xml_obstacles[i].getType() != XmlRpc::XmlRpcValue::TypeArray)\n {\n ROS_ERROR(\"obstacles[%d] has no vertices\", i);\n } else {\n for(int j = 0; j < xml_obstacles[i].size(); ++j)\n {\n // Vector2D contains x,y coords\n if(xml_obstacles[i][j].size() != 2)\n {\n ROS_ERROR(\"Vertex[%d] of obstacles[%d] is not a pair of coordinates\", j, i);\n } else if(\n xml_obstacles[i][j][0].getType() != XmlRpc::XmlRpcValue::TypeDouble or\n xml_obstacles[i][j][1].getType() != XmlRpc::XmlRpcValue::TypeDouble)\n {\n ROS_ERROR(\"The coordinates of vertex[%d] of obstacles[%d] are not doubles\", j, i);\n } else {\n // PASSED ALL THE TESTS: push Vector2D to vertices list in Obstacle object\n rigid2d::Vector2D vertex(xml_obstacles[i][j][0], xml_obstacles[i][j][1]);\n // NOTE: SCALE DOWN\n vertex.x /= SCALE;\n vertex.y /= SCALE;\n obs.vertices.push_back(vertex);\n }\n }\n }\n // push Obstacle object to vector of Object(s) in Map\n obstacles_v.push_back(obs);\n }\n }\n geometry_msgs::PoseStamped ps;\n ps.pose.position.x = start.x;\n ps.pose.position.y = start.y;\n std::vector robot_poses{ps};\n nav_msgs::Path path;\n path.header.frame_id = frame_id;\n\n global::PotentialField PF(obstacles_v, eta, ada, zeta, d_thresh, Q_thresh);\n bool terminate = PF.return_terminate();\n\n // SLEEP so that I can prepare my screen recorder\n ros::Duration(2.0).sleep();\n\n ros::Rate rate(frequency);\n\n bool finito = false;\n\n // Main While\n while (ros::ok())\n {\n ros::spinOnce();\n\n path.header.stamp = ros::Time::now();\n\n if (!terminate)\n {\n start = PF.OneStepGD(start, goal, obstacles_v);\n ps.pose.position.x = start.x;\n ps.pose.position.y = start.y;\n robot_poses.push_back(ps);\n terminate = PF.return_terminate();\n ROS_DEBUG(\"NEW POS: [%.2f, %.2f]\", start.x, start.y);\n } else if (!finito)\n {\n ROS_INFO(\"GOAL REACHED.\");\n finito = true;\n }\n\n path.poses = robot_poses;\n path_pub.publish(path);\n\n sg_pub.publish(sg_arr);\n\n \n rate.sleep();\n }\n\n return 0;\n}" }, { "path": "map/CMakeLists.txt", "content": "cmake_minimum_required(VERSION 3.0)\nproject(map)\n\n## Compile as C++11, supported in ROS Kinetic and newer\nadd_compile_options(-Wall -Wextra -pipe -Wno-psabi)\n\nset(CMAKE_CXX_STANDARD 17)\nset(CMAKE_CXX_STANDARD_REQUIRED ON)\nset(CMAKE_CXX_EXTENSIONS OFF)\n\n## Find catkin macros and libraries\n## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)\n## is used, also find other catkin packages\nfind_package(catkin REQUIRED COMPONENTS\n message_generation\n message_runtime\n rigid2d\n roscpp\n rostest\n nuslam\n visualization_msgs\n)\n\nfind_package(Eigen3 3.3 REQUIRED NO_MODULE)\n\n## System dependencies are found with CMake's conventions\n# find_package(Boost REQUIRED COMPONENTS system)\n\n\n## Uncomment this if the package has a setup.py. This macro ensures\n## modules and global scripts declared therein get installed\n## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html\n# catkin_python_setup()\n\n################################################\n## Declare ROS messages, services and actions ##\n################################################\n\n## To declare and build messages, services or actions from within this\n## package, follow these steps:\n## * Let MSG_DEP_SET be the set of packages whose message types you use in\n## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).\n## * In the file package.xml:\n## * add a build_depend tag for \"message_generation\"\n## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET\n## * If MSG_DEP_SET isn't empty the following dependency has been pulled in\n## but can be declared for certainty nonetheless:\n## * add a exec_depend tag for \"message_runtime\"\n## * In this file (CMakeLists.txt):\n## * add \"message_generation\" and every package in MSG_DEP_SET to\n## find_package(catkin REQUIRED COMPONENTS ...)\n## * add \"message_runtime\" and every package in MSG_DEP_SET to\n## catkin_package(CATKIN_DEPENDS ...)\n## * uncomment the add_*_files sections below as needed\n## and list every .msg/.srv/.action file to be processed\n## * uncomment the generate_messages entry below\n## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)\n\n## Generate messages in the 'msg' folder\n# add_message_files(\n# FILES\n# Message1.msg\n# Message2.msg\n# )\n\n## Generate services in the 'srv' folder\n# add_service_files(\n# FILES\n# Service1.srv\n# Service2.srv\n# )\n\n## Generate actions in the 'action' folder\n# add_action_files(\n# FILES\n# Action1.action\n# Action2.action\n# )\n\n## Generate added messages and services with any dependencies listed here\n# generate_messages(\n# DEPENDENCIES\n# std_msgs # Or other packages containing msgs\n# )\n\n################################################\n## Declare ROS dynamic reconfigure parameters ##\n################################################\n\n## To declare and build dynamic reconfigure parameters within this\n## package, follow these steps:\n## * In the file package.xml:\n## * add a build_depend and a exec_depend tag for \"dynamic_reconfigure\"\n## * In this file (CMakeLists.txt):\n## * add \"dynamic_reconfigure\" to\n## find_package(catkin REQUIRED COMPONENTS ...)\n## * uncomment the \"generate_dynamic_reconfigure_options\" section below\n## and list every .cfg file to be processed\n\n## Generate dynamic reconfigure parameters in the 'cfg' folder\n# generate_dynamic_reconfigure_options(\n# cfg/DynReconf1.cfg\n# cfg/DynReconf2.cfg\n# )\n\n###################################\n## catkin specific configuration ##\n###################################\n## The catkin_package macro generates cmake config files for your package\n## Declare things to be passed to dependent projects\n## INCLUDE_DIRS: uncomment this if your package contains header files\n## LIBRARIES: libraries you create in this project that dependent projects also need\n## CATKIN_DEPENDS: catkin_packages dependent projects also need\n## DEPENDS: system dependencies of this project that dependent projects also need\ncatkin_package(\nINCLUDE_DIRS include\nLIBRARIES ${PROJECT_NAME} rigid2d nuslam\nCATKIN_DEPENDS message_generation message_runtime rigid2d roscpp nuslam visualization_msgs\n# DEPENDS system_lib\n)\n\n###########\n## Build ##\n###########\n\n## Specify additional locations of header files\n## Your package locations should be listed before other locations\ninclude_directories(\n include\n ${catkin_INCLUDE_DIRS}\n ${Eigen3_INCLUDE_DIRS}\n)\n\nadd_library(${PROJECT_NAME}\n src/${PROJECT_NAME}/${PROJECT_NAME}.cpp\n src/${PROJECT_NAME}/prm.cpp\n src/${PROJECT_NAME}/grid.cpp\n)\n\n## Add cmake target dependencies of the library\n## as an example, code may need to be generated before libraries\n## either from message generation or dynamic reconfigure\nadd_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})\n\n## Declare a C++ executable\n## With catkin_make all packages are built within a single CMake context\n## The recommended prefix ensures that target names across packages don't collide\n# add_executable(${PROJECT_NAME}_node src/map_node.cpp)\nadd_executable(viz_map src/viz_map.cpp)\nadd_executable(viz_grid src/viz_grid.cpp)\n\n## Rename C++ executable without prefix\n## The above recommended prefix causes long target names, the following renames the\n## target back to the shorter version for ease of user use\n## e.g. \"rosrun someones_pkg node\" instead of \"rosrun someones_pkg someones_pkg_node\"\n# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX \"\")\nset_target_properties(viz_map PROPERTIES OUTPUT_NAME viz_map PREFIX \"\")\nset_target_properties(viz_grid PROPERTIES OUTPUT_NAME viz_grid PREFIX \"\")\n\n## Add cmake target dependencies of the executable\n## same as for the library above\n# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})\nadd_dependencies(viz_map ${${PROJECT_NAME}_EXPORTED_TARGETS} ${Eigen3_EXPORTED_TARGETS} ${rigid2d_EXPORTED_TARGETS} ${nuslam_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})\nadd_dependencies(viz_grid ${${PROJECT_NAME}_EXPORTED_TARGETS} ${Eigen3_EXPORTED_TARGETS} ${rigid2d_EXPORTED_TARGETS} ${nuslam_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})\n\n## Specify libraries to link a library or executable target against\ntarget_link_libraries(\nviz_map # this is my node that will use below libraries\n${rigid2d_LIBRARIES}\n${nuslam_LIBRARIES}\nEigen3::Eigen\n${PROJECT_NAME}\n${catkin_LIBRARIES}\n)\n\ntarget_link_libraries(\nviz_grid # this is my node that will use below libraries\n${rigid2d_LIBRARIES}\n${nuslam_LIBRARIES}\nEigen3::Eigen\n${PROJECT_NAME}\n${catkin_LIBRARIES}\n)\n\n#############\n## Install ##\n#############\n\n# all install targets should use catkin DESTINATION variables\n# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html\n\n## Mark executable scripts (Python etc.) for installation\n## in contrast to setup.py, you can choose the destination\n# install(PROGRAMS\n# scripts/my_python_script\n# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}\n# )\n\n## Mark executables for installation\n## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html\ninstall(TARGETS viz_map viz_grid\n RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}\n)\n\n## Mark libraries for installation\n## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html\ninstall(TARGETS ${PROJECT_NAME}\n ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}\n LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}\n RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}\n)\n\n## Mark cpp header files for installation\ninstall(DIRECTORY include/${PROJECT_NAME}/\n DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}\n FILES_MATCHING PATTERN \"*.h\"\n PATTERN \".svn\" EXCLUDE\n)\n\n## Mark other files for installation (e.g. launch and bag files, etc.)\n# install(FILES\n# # myfile1\n# # myfile2\n# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}\n# )\n\n#############\n## Testing ##\n#############\n\n## Add gtest based cpp test target and link libraries\n# catkin_add_gtest(${PROJECT_NAME}-test test/${PROJECT_NAME}_test.cpp)\n# if(TARGET ${PROJECT_NAME}-test)\n# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})\n# endif()\n\n## Add folders to be run by python nosetests\n# catkin_add_nosetests(test)\n\n# if (CATKIN_ENABLE_TESTING)\n# catkin_add_gtest(${PROJECT_NAME}_test tests/${PROJECT_NAME}_test.cpp)\n# target_link_libraries(${PROJECT_NAME}_test ${catkin_Libraries} gtest_main ${PROJECT_NAME})\n# endif()\n" }, { "path": "map/config/map.yaml", "content": "# NOTE: coordinates in cell #s, choose to convert to m/cm/mm later\n# Each item (11 total) in list is an obstacle. Each obstacle has vertices\n# with cartesian coordinates\nobstacles: [[[12., 6.], [14.5, 3.5], [17., 5.5], [17., 8.5], [14., 8.]], #A\n [[24., 6.], [26., 3.5], [31., 7.5], [24.5, 9.5]], #B\n [[34., 26.], [10., 26.], [10., 12.], [34., 12.]], #C\n [[0., 26.], [0., 6.], [4., 6.], [4., 26.]], #D\n [[4., 32.], [6., 30.], [8., 32.]], #E\n [[17., 32.], [18., 30.], [19., 32.]], #F\n [[0., 36.], [0., 32.], [29., 32.], [29., 36.]], #G\n [[34., 36.], [33., 34.], [34., 32.]], #H\n [[6., 44.], [2., 43.], [2., 39.], [6., 38.], [8., 41.]], #I\n [[11., 48.], [17., 41.], [14., 48.]], #J\n [[30., 48.], [22., 40.], [32., 48.]], #K\n [[0., 0.], [34., 0.]], # OUTER WALL 1\n [[34., 0.], [34., 48.]], # OUTER WALL 2\n [[34., 48.], [0., 48.]], # OUTER WALL 3\n [[0., 48.], [0., 0.]]] # OUTER WALL 4\n\nmap_bounds: [[0., 0.], [34., 0.], [34., 48.], [0., 48.]]" }, { "path": "map/include/map/grid.hpp", "content": "#ifndef GRID_INCLUDE_GUARD_HPP\n#define GRID_INCLUDE_GUARD_HPP\n/// \\file\n/// \\brief GRID Library to build a Probabilistic Roadmap.\n#include \n#include // to use PRM::too_close method\n\nnamespace map\n{\n using rigid2d::Vector2D;\n\n // \\brief stores indeces to acces elements in row-major order or grid-wise\n struct Index{\n int row_major = -1;\n int x = -1;\n int y = -1;\n\n };\n\n enum CellType {Occupied, Inflation, Free};\n\n // \\brief struct to store Cell paramters such as coordinates, center coordinates,\n // whether a cell has been visited, and the cell type (Start, Goal, Obstacle, Standard)\n struct Cell{\n\n // \\brief Cell constructor with input coordinates\n Cell(const Vector2D & coords_, const double & resolution_);\n\n // Corner coordinates\n Vector2D coords;\n // Centre coordinates\n Vector2D center_coords;\n bool visited = false;\n // Default Cell Type\n CellType celltype = Free;\n // Cell value\n double value = 0.0;\n // Cell index in grid (also RMJ)\n Index index;\n double resolution;\n // Used for simulated grid update\n bool newView = false;\n };\n\n /// \\brief stores Obstacle(s) to construct basic Grid. Inherits from Map in map.hpp.\n class Grid : public Map\n {\n // Inherits Constructors\n using Map::Map;\n\n public:\n\n // \\brief Constructs a Grid Map.\n // \\param resolution: determines the grid cell size\n void build_map(const double & resolution);\n\n // \\brief converts world coordinates to grid coordinates and returns result\n // \\param cell: a grid cell\n // \\returns the grid cell's world coordinates\n Index world2grid(const Cell & cell) const;\n\n // \\brief converts grid coordinates to world coordinates and returns result\n // \\param i: x-coordinate of grid cell\n // \\param j: y-coordinate of grid cell\n // \\param resolution: determines the grid cell size\n // \\returns the grid cell's grid coordinates\n Vector2D grid2world(const int & i, const int & j, const double & resolution) const;\n\n // \\brief Return Grid in row-major order\n // \\returns vector containing grid cells as Cell\n std::vector return_grid() const;\n\n // \\brief populates Occupancy Grid with values for visualization\n // \\param map: the Occupancy Grid map to populate\n void occupancy_grid(std::vector & map) const;\n\n // \\brief returns the width and height of the grid in cells\n // \\returns int vector containing width and height respectively.\n std::vector return_grid_dimensions() const;\n\n // \\brief gets the neighbours of a given cell according to a visibility bound\n // \\param current_cell: the cell from which we simulate the update\n // \\param visibility: size of bounding box used for update (can vary each iteration if desired). DEFAULT is 1 for 3x3 eval.\n // \\returns: vector of neighbour Cells\n std::vector get_neighbours(const Cell & cc, const std::vector & map, const int & visibility=1);\n\n // \\brief Increment fake copy of grid for simulated updates\n // \\param current_cell: the cell from which we simulate the update\n // \\param visibility: size of bounding box used for update (can vary each iteration if desired)\n void update_grid(const Cell & cc, const int & visibility);\n\n // \\brief Return FAKE (simulated increment) Grid in row-major order\n // \\returns vector containing grid cells as Cell\n std::vector return_fake_grid() const;\n\n // \\brief populates Occupancy Grid with FAKE values for visualization\n // \\param map: the Occupancy Grid map to populate\n void fake_occupancy_grid(std::vector & map) const;\n\n // \\brief Checks whether a potential Vertex lies on an Obstacle. 'map::PRM::sample_configurations' calls this function.\n // \\param q: the Vertex being examined\n friend bool not_inside(const Vertex & q, const std::vector & obstacles, const double & inflate_robot);\n\n // \\brief Checks if a Vertex is too close to an Edge.\n // \\param E1: the first Vertex forming an edge\n // \\param E2: the second Vertex forming an edge\n // \\param P0: the Vertex whose closeness is being examined.\n // \\param inflate_robot: approximate robot radius used for collision checking.\n friend bool too_close(const Vertex & E1, const Vertex & E2, const Vertex & P0, const double & inflate_robot);\n\n private:\n std::vector cells;\n std::vector fake_grid;\n std::vector xcells;\n std::vector ycells;\n };\n\n // \\brief numpy arange in C++, can take any type T (int,double,etc)\n template\n std::vector arange(const T & start, const T & stop, const T & step = 1)\n {\n std::vector values;\n for (T value = start; value < stop; value += step)\n values.push_back(value);\n return values;\n }\n\n // \\brief convert from row-major-order coordinates to grid coordinates\n // \\param rmj: row-major-order coordinate\n // \\param numrow: number of rows in grid\n // \\returns Index containing grid coordintates\n Index rowmajor2grid(const int & rmj, const int & numrow);\n\n // \\brief convert from grid coordinates to row-major-order coordinates\n // \\param x: x-coordinate of grid cell\n // \\param y: y-coordinate of grid cell\n // \\param numcol: number of columns in grid\n // \\returns row-major-order index\n int grid2rowmajor(const int & x, const int & y, const int & numrow);\n}\n\n#endif\n" }, { "path": "map/include/map/map.hpp", "content": "#ifndef MAP_INCLUDE_GUARD_HPP\n#define MAP_INCLUDE_GUARD_HPP\n/// \\file\n/// \\brief Map Library to store and display polygon-based custom map.\n/// NOTE: rigid2d is from turtlebot3_from_scratch (tb3 in nuturtle.rosinstall)\n#include \n#include \n#include \n\nnamespace map\n{\n // Used to store Obstacle vertex coordinates\n using rigid2d::Vector2D;\n\n struct Obstacle\n /// \\brief stores obstacle vertex coordinates in CCW order\n {\n std::vector vertices;\n\n // \\brief constructor for Obstacle without inputs\n Obstacle();\n\n // \\brief constructor for Obstacle with inputs\n Obstacle(const std::vector & vertices_);\n };\n\n /// \\brief stores Obstacle(s) to construct basic map\n class Map\n {\n public:\n /// \\brief the default constructor creates an empty map\n Map();\n\n /// \\brief this constructor creates a map that holds user-specified obstacles\n /// \\param obstacles_: the list of obstacles in this map\n Map(const std::vector & obstacles_);\n\n /// \\brief this constructor creates a map that holds user-specified obstacles and robot size threshold.\n /// \\param obstacles_: the list of obstacles in this map\n // \\param inflate_robot_: approximate robot radius used for collision checking.\n Map(const std::vector & obstacles_, const double inflate_robot_);\n\n /// \\brief return vector of obstacles\n /// \\returns vector of Obstacle\n std::vector return_obstacles();\n\n /// \\brief assigns the map space to map_extent in absolute(x,y).\n void find_map_extent();\n\n // \\brief return map bounds\n // \\returns std::vector containing minimum and maximum bounds respectively\n std::vector return_map_bounds();\n\n\n // protected instead of private so that child Class can access\n protected:\n // Map obstacles\n std::vector obstacles;\n\n // Map maximum coordinatesin x,y\n Vector2D map_max;\n\n // Map minimum coordinatesin x,y\n Vector2D map_min;\n\n // approximate robot radius used for collision checking.\n double inflate_robot;\n };\n\n double euclidean_distance(const double & x_rel, const double & y_rel);\n}\n\n#endif\n" }, { "path": "map/include/map/prm.hpp", "content": "#ifndef PRM_INCLUDE_GUARD_HPP\n#define PRM_INCLUDE_GUARD_HPP\n/// \\file\n/// \\brief PRM Library to build a Probabilistic Roadmap.\n#include \n#include \n#include \n\nnamespace map\n{\n using rigid2d::Vector2D;\n\n struct Edge\n {\n // ID of next node connected by edge\n // init to -1 for error checking\n int next_id = -1;\n // Euclidean Distance between Nodes (Vertices)\n int distance;\n };\n\n struct Vertex\n {\n // \\brief Constructor with Coordinates\n Vertex(const Vector2D coords_);\n\n // ID of current Node/Vertex\n // init to -1 for error checking\n int id = -1;\n // Cartesian Coordinates of Vertex\n Vector2D coords;\n // Edges connected to this node\n std::vector edges;\n // IDs of Adjacent Nodes\n // O(1) best case or O(n) worst case\n std::unordered_set id_set;\n\n // Helps with search\n bool visited;\n\n /// \\brief Check if edge exists in ID Hash Table\n /// \\param check_id: ID of node to connect to\n /// \\returns True if current Vertex connected to given ID.\n bool edge_exists(const int & check_id) const;\n };\n\n // \\brief struct to store important attributes of the shortest distance from a point to a line segment\n struct ShortestDistance\n {\n Vector2D point; // The point whose shortest distance to a line segment is computed.\n double u; // A point is within a line segment if u is between [0,1]\n double D; // The signed shotrest distance distance from the point to the line segment.\n // if D > 0, the point is on the left-hand side of the line segment from Vertex 1 to Vertex 2 \n };\n\n /// \\brief stores Obstacle(s) to construct basic PRM. Inherits from Map in map.hpp.\n class PRM : public Map\n {\n // Inherits Constructors\n using Map::Map;\n\n public:\n\n // \\brief Collision checking on Concave polygons is very tedious, so we divide them into Convex ones.\n void divide_concave_polygons();\n\n // \\brief Constructs a Roadmap.\n // \\param n: number of nodes to put in the Roadmap.\n // \\param k: number of closest neighbours to examine for each configuration.\n // \\param thresh: Euclidean Distance Threshold for valid Edge.\n void build_map(const int & n, int & k, const double & thresh);\n\n // \\brief Sample free space Q for configurations q. Steps 3-8 of algorithm.\n // \\param n: number of nodes to put in the Roadmap.\n void sample_configurations(const int & n);\n\n // \\brief For a given configuration q, assigns indeces of K Nearest Neighbours to q id_set. Step 10 of algorithm.\n // \\param q: the Vertex being examined. Not const because id_set is modified.\n // \\param k: number of closest neighbours to examine for each configuration.\n // NOTE: Using Brute Force Now, replace with KD-Tree + Rebalance\n std::vector find_knn(Vertex & q, const int & k);\n\n // \\brief Check is the Edge between two nodes is valid (no collision, and above some euclidean distance)\n // \\param q: the main Vertex being examined\n // \\param q_prime: the second Vertex being examined\n // \\param thresh: Euclidean Distance Threshold for valid Edge.\n bool edge_valid(const Vertex & q, const Vertex & q_prime, const double & thresh);\n\n // \\brief Checks whether a potential Edge intersects a Polygon.\n // 'map::PRM::no_collision(const Vertex & q, const Vertex & q_prime, const double & inflate_robot)' calls this function.\n // \\param q: the main Vertex being examined\n // \\param q_prime: the second Vertex being examined\n // \\param obs_iter: iterator for the Obstacle (Polygon) whose vertices we examine.\n bool no_collision(const Vertex & q, const Vertex & q_prime, const std::vector::iterator & obs_iter);\n\n // \\brief Checks whether a potential Edge intersects an Obstacle considering the robot's geometry.\n // \\param q: the main Vertex being examined\n // \\param q_prime: the second Vertex being examined\n // \\param inflate_robot: approximate robot radius used for collision checking.\n bool no_collision(const Vertex & q, const Vertex & q_prime, const double & inflate_robot);\n\n // \\brief computes the shortest distance from a line to a point and returns the result in an informative struct.\n // \\param E1: the first Vertex forming an edge\n // \\param E2: the second Vertex forming an edge\n // \\param P0: the Vertex whose closeness is being examined.\n // \\param inflate_robot: approximate robot radius used for collision checking.\n // \\returns ShortestDistance struct\n friend ShortestDistance lineToPoint(const Vertex & E1, const Vertex & E2, const Vertex & P0, const double & inflate_robot);\n\n // \\brief Checks whether a potential Vertex lies on an Obstacle. 'map::PRM::sample_configurations' calls this function.\n // \\param q: the Vertex being examined\n friend bool not_inside(const Vertex & q, const std::vector & obstacles, const double & inflate_robot);\n\n // \\brief Checks if a Vertex is too close to an Edge.\n // \\param E1: the first Vertex forming an edge\n // \\param E2: the second Vertex forming an edge\n // \\param P0: the Vertex whose closeness is being examined.\n // \\param inflate_robot: approximate robot radius used for collision checking.\n friend bool too_close(const Vertex & E1, const Vertex & E2, const Vertex & P0, const double & inflate_robot);\n\n // \\brief Return Probabilistic Road Map\n // \\returns Probabilistic Road Map \n std::vector return_prm();\n private:\n // Hash Table\n std::vector configurations;\n };\n\n // \\brief Checks whether a potential Edge intersects a Polygon.\n // 'map::PRM::no_collision(const Vertex & q, const Vertex & q_prime, const double & inflate_robot)' calls this function.\n // \\param q: the main Vertex being examined\n // \\param q_prime: the second Vertex being examined\n // \\param obs_iter: iterator for the Obstacle (Polygon) whose vertices we examine.\n bool no_intersect(const Vertex & q, const Vertex & q_prime, const std::vector::iterator & obs_iter);\n\n // \\brief Checks whether a potential Vertex lies on an Obstacle. 'map::PRM::sample_configurations' calls this function.\n // \\param q: the Vertex being examined\n bool not_inside(const Vertex & q, const std::vector & obstacles, const double & inflate_robot);\n\n // \\brief Checks if a Vertex is too close to an Edge.\n // \\param E1: the first Vertex forming an edge\n // \\param E2: the second Vertex forming an edge\n // \\param P0: the Vertex whose closeness is being examined.\n // \\param inflate_robot: approximate robot radius used for collision checking.\n bool too_close(const Vertex & E1, const Vertex & E2, const Vertex & P0, const double & inflate_robot);\n\n // \\brief computes the shortest distance from a line to a point and returns the result in an informative struct.\n // \\param E1: the first Vertex forming an edge\n // \\param E2: the second Vertex forming an edge\n // \\param P0: the Vertex whose closeness is being examined.\n // \\returns ShortestDistance struct\n ShortestDistance lineToPoint(const Vertex & E1, const Vertex & E2, const Vertex & P0);\n}\n\n#endif\n" }, { "path": "map/launch/viz_map.launch", "content": "\n \n\n \n \n\n\n \n \n\n \n \n \n \n \n \n \n \n\n \n \n\n \n\n \n\n\n \n \n \n \n \n\n \n \n \n \n \n \n \n \n \n \n \n\n \n \n \n \n \n \n \n\n" }, { "path": "map/package.xml", "content": "\n\n map\n 0.0.1\n PRM and Grid Maps\n\n \n \n \n Maurice Rahme\n\n\n \n \n \n MIT\n\n\n \n \n \n \n\n\n \n \n \n \n\n\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n catkin\n message_generation\n message_runtime\n rigid2d\n roscpp\n nuslam\n visualization_msgs\n message_generation\n message_runtime\n rigid2d\n nuslam\n roscpp\n visualization_msgs\n message_runtime\n roscpp\n rigid2d\n nuslam\n visualization_msgs\n\n rostest\n rosunit\n \n \n \n\n \n\n" }, { "path": "map/rviz/basic_map.rviz", "content": "Panels:\n - Class: rviz/Displays\n Help Height: 77\n Name: Displays\n Property Tree Widget:\n Expanded:\n - /Global Options1\n - /Grid1\n - /TF1/Frames1\n - /TF1/Tree1\n - /Obstacles1\n - /PRM1\n Splitter Ratio: 0.5\n Tree Height: 714\n - Class: rviz/Selection\n Name: Selection\n - Class: rviz/Tool Properties\n Expanded:\n - /2D Pose Estimate1\n - /2D Nav Goal1\n - /Publish Point1\n Name: Tool Properties\n Splitter Ratio: 0.5886790156364441\n - Class: rviz/Views\n Expanded:\n - /Current View1\n Name: Views\n Splitter Ratio: 0.5\n - Class: rviz/Time\n Experimental: false\n Name: Time\n SyncMode: 0\n SyncSource: \"\"\nPreferences:\n PromptSaveOnExit: true\nToolbars:\n toolButtonStyle: 2\nVisualization Manager:\n Class: \"\"\n Displays:\n - Alpha: 0.5\n Cell Size: 0.10000000149011612\n Class: rviz/Grid\n Color: 85; 87; 83\n Enabled: true\n Line Style:\n Line Width: 0.029999999329447746\n Value: Lines\n Name: Grid\n Normal Cell Count: 0\n Offset:\n X: 0\n Y: 0\n Z: 0\n Plane: XY\n Plane Cell Count: 300\n Reference Frame: \n Value: true\n - Class: rviz/TF\n Enabled: true\n Frame Timeout: 15\n Frames:\n All Enabled: false\n ball_caster:\n Value: false\n base_footprint:\n Value: false\n base_link:\n Value: false\n base_scan:\n Value: true\n rl_wheel:\n Value: false\n rr_wheel:\n Value: false\n Marker Scale: 1\n Name: TF\n Show Arrows: true\n Show Axes: true\n Show Names: true\n Tree:\n base_footprint:\n base_link:\n ball_caster:\n {}\n base_scan:\n {}\n rl_wheel:\n {}\n rr_wheel:\n {}\n Update Interval: 0\n Value: true\n - Alpha: 1\n Class: rviz/RobotModel\n Collision Enabled: false\n Enabled: true\n Links:\n All Links Enabled: true\n Expand Joint Details: false\n Expand Link Details: false\n Expand Tree: false\n Link Tree Style: Links in Alphabetic Order\n ball_caster:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n base_footprint:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n base_link:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n base_scan:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n rl_wheel:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n rr_wheel:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n Name: RobotModel\n Robot Description: robot_description\n TF Prefix: \"\"\n Update Interval: 0\n Value: true\n Visual Enabled: true\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /map\n Name: Obstacles\n Namespaces:\n \"\": true\n Queue Size: 100\n Value: true\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /prm\n Name: PRM\n Namespaces:\n \"\": true\n Queue Size: 100\n Value: true\n - Alpha: 0.699999988079071\n Class: rviz/Map\n Color Scheme: map\n Draw Behind: false\n Enabled: true\n Name: Map\n Topic: /grid_map\n Unreliable: false\n Use Timestamp: false\n Value: true\n Enabled: true\n Global Options:\n Background Color: 48; 48; 48\n Default Light: true\n Fixed Frame: base_footprint\n Frame Rate: 30\n Name: root\n Tools:\n - Class: rviz/Interact\n Hide Inactive Objects: true\n - Class: rviz/MoveCamera\n - Class: rviz/Select\n - Class: rviz/FocusCamera\n - Class: rviz/Measure\n - Class: rviz/SetInitialPose\n Theta std deviation: 0.2617993950843811\n Topic: /initialpose\n X std deviation: 0.5\n Y std deviation: 0.5\n - Class: rviz/SetGoal\n Topic: /move_base_simple/goal\n - Class: rviz/PublishPoint\n Single click: true\n Topic: /clicked_point\n Value: true\n Views:\n Current:\n Class: rviz/Orbit\n Distance: 12.403410911560059\n Enable Stereo Rendering:\n Stereo Eye Separation: 0.05999999865889549\n Stereo Focal Distance: 1\n Swap Stereo Eyes: false\n Value: false\n Focal Point:\n X: 3.8209285736083984\n Y: 4.755727291107178\n Z: 0.14359386265277863\n Focal Shape Fixed Size: true\n Focal Shape Size: 0.05000000074505806\n Invert Z Axis: false\n Name: Current View\n Near Clip Distance: 0.009999999776482582\n Pitch: 1.5697963237762451\n Target Frame: \n Value: Orbit (rviz)\n Yaw: 4.708594799041748\n Saved: ~\nWindow Geometry:\n Displays:\n collapsed: false\n Height: 1004\n Hide Left Dock: false\n Hide Right Dock: true\n QMainWindow State: 000000ff00000000fd00000004000000000000015600000352fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000352000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000352fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b00000352000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004680000003efc0100000002fb0000000800540069006d00650100000000000004680000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000030c0000035200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000\n Selection:\n collapsed: false\n Time:\n collapsed: false\n Tool Properties:\n collapsed: false\n Views:\n collapsed: true\n Width: 1128\n X: 178\n Y: 0\n" }, { "path": "map/src/map/grid.cpp", "content": "#include \"map/grid.hpp\"\n\nnamespace map\n{\n\tusing rigid2d::Vector2D;\n\n\tCell::Cell(const Vector2D & coords_, const double & resolution_)\n\t{\n\t\tcoords = coords_;\n\n\t\tdouble offset = resolution_ / 2.0;\n\n\t\tcenter_coords = Vector2D(coords.x + offset, coords.y + offset);\n\t\tresolution = resolution_;\n\t}\n\n\tvoid Grid::build_map(const double & resolution)\n\t{\n\t\t// Step 1. divide grid into cells based on resolution and store in class members\n\t\txcells = arange(map_min.x, map_max.x, resolution);\n\t\tycells = arange(map_min.y, map_max.y, resolution);\n\n\t\t// Step 2. Populate Cell vector (cells) with world coordinates and labels in row-major-order\n\t\tfor (int i = 0; i < static_cast(ycells.size()); i++)\n\t\t{\n\t\t\tfor (int j = 0; j < static_cast(xcells.size()); j++)\n\t\t\t{\n\t\t\t\t// std::cout << \"[\" << j << \", \" << i << \"]\" << std::endl;\n\t\t\t\t// For each cell to be added, convert the grid index to world coords\n\t\t\t\tCell cell(Vector2D(xcells.at(j), ycells.at(i)), resolution);\n\n\t\t\t\t// Perform inside-obstacle check for labeling Obstacle\n\t\t\t\tif (!not_inside(Vertex(cell.center_coords), obstacles, 0.0))\n\t\t\t\t{\n\t\t\t\t\t// std::cout << \"OCCUPIED: [\" << cell.center_coords.x << \", \" << cell.center_coords.y << \"]\" << std::endl;\n\t\t\t\t\tcell.celltype = Occupied;\n\t\t\t\t} else if (!not_inside(Vertex(cell.center_coords), obstacles, inflate_robot))\n\t\t\t\t// Perform close-to-obstacle check for labeling Inflation\n\t\t\t\t{\n\t\t\t\t\tcell.celltype = Inflation;\n\t\t\t\t\t// std::cout << \"INFLATED: [\" << cell.center_coords.x << \", \" << cell.center_coords.y << \"]\" << std::endl;\n\t\t\t\t}\n\t\t\t\tcells.push_back(cell);\n\t\t\t}\n\t\t}\n\n\t\t// Populate more cell information such as row-major order and grid index\n\t\tfor(unsigned int i = 0; i < cells.size(); i++)\n\t\t{\n\t\t\t// Convert from row-major-order to grid coordinates\n\t\t\tauto index = rowmajor2grid(i, static_cast(xcells.size()));\n\t\t\t// Convert back to rmj\n\t\t\tint rmj = grid2rowmajor(index.x, index.y, static_cast(xcells.size()));\n\t\t\tindex.row_major = rmj;\n\n\t\t\tif (!(rmj == static_cast(i)))\n\t\t\t{\n\t\t\t\tthrow std::runtime_error(\"Row-Major Order Conversion Failed!\\\n\t\t\t\t\t\t\t\t\t\t \\n where(): Grid::build_map(const double & resolution)\");\n\t\t\t}\n\n\t\t\tcells.at(i).index = index;\n\n\t\t\t// Set fake_grid to same cells but all free\n\t\t\tCell fake_cell = cells.at(i);\n\t\t\tfake_cell.celltype = Free;\n\t\t\tfake_grid.push_back(fake_cell);\n\t\t}\n\t}\n\n\tIndex Grid::world2grid(const Cell & cell) const\n\t{\n\t\t// Get x coordinate\n\t\tint x_index = -1;\n\t\tfor (int i = 0; i < static_cast(xcells.size()); i++)\n\t\t{\n\t\t\tif (cell.center_coords.x >= xcells.at(i) and cell.center_coords.x < xcells.at(i) + cell.resolution)\n\t\t\t{\n\t\t\t\tx_index = i;\n\t\t\t}\n\t\t}\n\n\t\t// Get x coordinate\n\t\tint y_index = -1;\n\t\tfor (int j = 0; j < static_cast(ycells.size()); j++)\n\t\t{\n\t\t\tif (cell.center_coords.y >= ycells.at(j) and cell.center_coords.y < ycells.at(j) + cell.resolution)\n\t\t\t{\n\t\t\t\ty_index = j;\n\t\t\t}\n\t\t}\n\n\t\tif (x_index == -1 or y_index == -1)\n\t\t{\n\t\t\tthrow std::runtime_error(\"Could not convert from world to grid coordinates!\");\n\t\t}\n\n\t\tIndex index;\n\t\tindex.x = x_index;\n\t\tindex.y = y_index;\n\t\tindex.row_major = grid2rowmajor(index.x, index.y, static_cast(xcells.size()));\n\n\t\treturn index;\n\t}\n\n\tVector2D Grid::grid2world(const int & i, const int & j, const double & resolution) const\n\t{\n\t\tif (!(i >= 0 and i <= static_cast(xcells.size()) - 1))\n\t\t{\n\t\t\tthrow std::invalid_argument(\"cell's x coordinate out of bounds!\");\n\t\t} else if (!(j >= 0 and j <= static_cast(ycells.size()) - 1))\n\t\t{\n\t\t\tthrow std::invalid_argument(\"cell's y coordinate out of bounds!\");\n\t\t}\n\t\tVector2D coord;\n\t\tcoord.x = i * resolution + map_min.x;\n\t\tcoord.y = j * resolution + map_min.y;\n\t\treturn coord;\n\t}\n\n\tstd::vector Grid::return_grid() const\n\t{\n\t\treturn cells;\n\t}\n\n\tvoid Grid::occupancy_grid(std::vector & map) const\n\t{\n\t\tmap.resize(cells.size());\n\n\t\tfor(unsigned int i = 0; i < cells.size(); i++)\n\t\t{\n\t\t\t// For each cell type, assign a value to map\n\t\t\tif (cells.at(i).celltype == Free)\n\t\t\t{\n\t\t\t\tmap.at(i) = 0;\n\t\t\t} else if (cells.at(i).celltype == Inflation)\n\t\t\t{\n\t\t\t\tmap.at(i) = 50;\n\t\t\t} else if (cells.at(i).celltype == Occupied)\n\t\t\t{\n\t\t\t\tmap.at(i) = 100;\n\t\t\t}\n\t\t}\n\t}\n\n\tstd::vector Grid::return_grid_dimensions() const\n\t{\n\t\tstd::vector v;\n\t\tv.push_back(static_cast(xcells.size()));\n\t\tv.push_back(static_cast(ycells.size()));\n\t\treturn v;\n\t}\n\n\n\tvoid Grid::update_grid(const Cell & cc, const int & visibility)\n\t{\n\n\t\tstd::vector cells_to_update = get_neighbours(cc, cells, visibility);\n\n\t\tfor (auto iter = cells_to_update.begin(); iter < cells_to_update.end(); iter++)\n\t\t{\n\t\t\tif (fake_grid.at(iter->index.row_major).celltype != iter->celltype)\n\t\t\t{\n\t\t\t\t// std::cout << \"NEW OCCUPANCY: \" << iter->index.row_major << std::endl;\n\t\t\t\tfake_grid.at(iter->index.row_major).celltype = iter->celltype;\n\t\t\t\tfake_grid.at(iter->index.row_major).newView = true;\n\t\t\t} else\n\t\t\t{\n\t\t\t\tfake_grid.at(iter->index.row_major).newView = false;\n\t\t\t}\n\t\t}\n\t}\n\n\n\tstd::vector Grid::return_fake_grid() const\n\t{\n\t\treturn fake_grid;\n\t}\n\n\n\tvoid Grid::fake_occupancy_grid(std::vector & map) const\n\t{\n\t\tmap.resize(fake_grid.size());\n\n\t\tfor(unsigned int i = 0; i < fake_grid.size(); i++)\n\t\t{\n\t\t\t// For each cell type, assign a value to map\n\t\t\tif (fake_grid.at(i).celltype == Free)\n\t\t\t{\n\t\t\t\tmap.at(i) = 0;\n\t\t\t} else if (fake_grid.at(i).celltype == Inflation)\n\t\t\t{\n\t\t\t\tmap.at(i) = 50;\n\t\t\t} else if (fake_grid.at(i).celltype == Occupied)\n\t\t\t{\n\t\t\t\tmap.at(i) = 100;\n\t\t\t}\n\t\t}\n\t}\n\n\tstd::vector Grid::get_neighbours(const Cell & cc, const std::vector & map, const int & visibility)\n\t{\n\t\tint lower_bound = - visibility;\n\t\tint upper_bound = - lower_bound;\n\t\tint x_max = map.back().index.x;\n\t\tint y_max = map.back().index.y; \n\t\tstd::vector neighbours;\n\n\t\t// Evaluate about block. Default is 1 -> 3x3 for 8-connectivity\n\t\tfor (int x = lower_bound; x <= upper_bound; x++)\n\t\t{\n\t\t\tfor (int y = lower_bound; y <= upper_bound; y++)\n\t\t\t{\n\t\t\t\t// Skip x,y = (0,0) since that's the current node\n\t\t\t\tif (x == 0 and y == 0)\n\t\t\t\t{\n\t\t\t\t\tcontinue;\n\t\t\t\t} else\n\t\t\t\t{\n\t\t\t\t\tint check_x = cc.index.x + x;\n\t\t\t\t\tint check_y = cc.index.y + y;\n\n\t\t\t\t\t// Ensure potential neighbour is within grid bounds\n\t\t\t\t\tif (check_x >= 0 and check_x <= x_max and check_y >= 0 and check_y <= y_max)\n\t\t\t\t\t{\n\t\t\t\t\t\t// Now we need to grab the right cell from the map. To do this: index->RMJ\n\t\t\t\t\t\t// std::cout << \"Neighbour at [\" << check_x << \", \" << check_y << \"]\" << std::endl;\n\t\t\t\t\t\tint rmj = map::grid2rowmajor(check_x, check_y, x_max + 1);\n\t\t\t\t\t\tCell nbr = map.at(rmj);\n\t\t\t\t\t\tneighbours.push_back(nbr);\n\t\t\t\t\t}\n\t\t\t\t}\n\t\t\t}\n\t\t}\n\n\t\treturn neighbours;\n\t}\n\n\n\n\tIndex rowmajor2grid(const int & rmj, const int & numcol)\n {\n Index index;\n // NOTE: RViz uses x=col, y=row\n index.y = rmj / numcol;\n index.x = rmj - (index.y * numcol);\n\n return index;\n }\n\n int grid2rowmajor(const int & x, const int & y, const int & numcol)\n {\n // NOTE: RViz uses x=col, y=row\n // So numcol is the number of x squares in each y row so it's x.size()\n // index = y * num_row + x\n // EX: to get RMJ index 3 we do: 3 = y * num_col + x\n // => 3 = 1 * 3 + 0\n /**\n Y|(0,3)=9|(1,3)=10|(2,3)=11|\n Y|(0,2)=6|(1,2)= 7|(2,2)= 8|\n Y|(0,1)=3|(1,1)= 4|(2,1)= 5|\n Y|(0,0)=0|(1,0)= 1|(2,0)= 2|\n X X X\n **/\n return x + y * numcol;\n }\n}" }, { "path": "map/src/map/map.cpp", "content": "#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::Obstacle(const std::vector & vertices_)\n\t{\n\t\tvertices = vertices_;\n\t}\n\n\t// Map\n\tMap::Map()\n\t{\n\t\tfind_map_extent();\n\t\tinflate_robot = 0.2;\n\t}\n\n\tMap::Map(const std::vector & obstacles_)\n\t{\n\t\tobstacles = obstacles_;\n\n\t\tfind_map_extent();\n\n\t\tinflate_robot = 0.2;\n\t}\n\n\tMap::Map(const std::vector & obstacles_, const double inflate_robot_)\n\t{\n\t\tobstacles = obstacles_;\n\n\t\tfind_map_extent();\n\n\t\tinflate_robot = inflate_robot_;\n\t}\n\n\tstd::vector Map::return_obstacles()\n\t{\n\t\treturn obstacles;\n\t}\n\n\tvoid Map::find_map_extent()\n\t{\n\t\tdouble x_min, x_max, y_min, y_max = 0;\n\n\t\tfor (auto obs_iter = obstacles.begin(); obs_iter != obstacles.end(); obs_iter++)\n\t {\n\n\t\t\tfor (auto v_iter = obs_iter->vertices.begin(); v_iter != obs_iter->vertices.end(); v_iter++)\n\t\t {\n\t\t \tif (v_iter->x > x_max)\n\t\t \t{\n\t\t \t\tx_max = v_iter->x;\n\t\t \t} else if (v_iter->x < x_min)\n\t\t \t{\n\t\t \t\tx_min = v_iter->x;\n\t\t \t}\n\n\t\t \tif (v_iter->y > y_max)\n\t\t \t{\n\t\t \t\ty_max = v_iter->y;\n\t\t \t} else if (v_iter->y < y_min)\n\t\t \t{\n\t\t \t\ty_min = v_iter->y;\n\t\t \t}\n\n\t\t \tmap_max = Vector2D(x_max, y_max);\n\t\t \tmap_min = Vector2D(x_min, y_min);\n\t\t }\n\t\t}\n\t}\n\n\tstd::vector Map::return_map_bounds()\n\t{\n\t\tstd::vector v;\n\t\tv.push_back(map_min);\n\t\tv.push_back(map_max);\n\n\t\treturn v;\n\t}\n\n\n\t// Helper Functions\n\tdouble euclidean_distance(const double & x_rel, const double & y_rel)\n\t{\n\t\treturn sqrt(pow(x_rel, 2) + pow(y_rel, 2));\n\t}\n\n}" }, { "path": "map/src/map/prm.cpp", "content": "#include \"map/prm.hpp\"\n#include \"nuslam/ekf.hpp\" // for random number engine\n\nnamespace map\n{\n\tusing rigid2d::Vector2D;\n\n\t// Vertex\n\tVertex::Vertex(const Vector2D coords_)\n\t{\n\t\tcoords = coords_;\n\t}\n\n\tbool Vertex::edge_exists(const int & check_id) const\n\t{\n\t\tconst auto search = id_set.find(check_id);\n\t\treturn (search == id_set.end()) ? false : true;\n\t}\n\n\t// PRM\n\tvoid PRM::build_map(const int & n, int & k, const double & thresh)\n\t{\n\t\tif (k > n)\n\t\t{\n\t\t\tstd::cout << \"[WARNING]: k > n. We have set k = n.\" << std::endl;\n\t\t\tk = n;\n\t\t}\n\t\t// Following PRM Algorithm from Lavalle - Planning Algorithms\n\n\t\t// Steps 1-2, empty Vertex and Edge List\n\t\tconfigurations.clear();\n\n\t\t// Steps 3-8: Sample Configurations\n\t\tsample_configurations(n);\n\n\t\t// Step 9: start loop for validity check\n\t\tfor (auto q = configurations.begin(); q != configurations.end(); q++)\n\t {\n\t \t// Step 10: get k nearest neighbours\n\t \tauto knn = find_knn(*q, k);\n\n\t \t// Step 11: start inner loop for validity check\n\t \tfor (auto id_iter = knn.begin(); id_iter != knn.end(); id_iter++)\n\t\t {\n\t\t \t// Search for Vertex of corresponding ID\n\t\t \tauto neighbor_ptr = &configurations.at(id_iter->id);\n\n\t\t \t// Step 12: perform validity check\n\t\t \tif (edge_valid(*q, *neighbor_ptr, thresh))\n\t\t \t{\n\t\t \t\t// Step 13: add neighbor ID to set of connected edges\n\t\t \t\tEdge qn_edge;\n\t\t \t\tqn_edge.next_id = neighbor_ptr->id;\n\t\t \t\tqn_edge.distance = euclidean_distance(q->coords.x - neighbor_ptr->coords.x,\\\n\t\t\t\t \t\t\t\t\t\t\t\t\t \t q->coords.y - neighbor_ptr->coords.y);\n\t\t \t\t// Add to edges and id_set\n\t\t \t\tq->edges.push_back(qn_edge);\n\t\t \t\tq->id_set.insert(qn_edge.next_id);\n\n\t\t \t\t// Also do vice versa: add q to neighbor edges\n\t\t \t\tEdge nq_edge;\n\t\t \t\tnq_edge.next_id = q->id;\n\t\t \t\tnq_edge.distance = qn_edge.distance;\n\t\t \t\t// Add to edges and id_set\n\t\t \t\tneighbor_ptr->edges.push_back(nq_edge);\n\t\t \t\tneighbor_ptr->id_set.insert(nq_edge.next_id);\n\t\t \t}\n\t\t }\n\t }\n\t}\n\n\tvoid PRM::sample_configurations(const int & n)\n\t{\n\t\t// MAP EXTENT\n\t\t// std::cout << \"Map Extent: (\" << map_max.x << \", \" << map_max.y << \")\" << std::endl;\n\t\tint kill_counter = 0;\n\t\twhile (static_cast(configurations.size()) < n or kill_counter > 1000 * n)\n\t\t{\n\t\t\t// Sample random number with assigned limits\n\t\t\tstd::uniform_real_distribution dx(map_min.x, map_max.x);\n\t\t\tstd::uniform_real_distribution dy(map_min.y, map_max.y);\n\t\t\t// Random Number Generator defined in nuslam package\n\t\t\tdouble sample_x = dx(nuslam::get_random());\n\t\t\tdouble sample_y = dy(nuslam::get_random());\n\n\t\t\tVector2D coords(sample_x, sample_y);\n\n\t\t\tVertex q(coords);\n\n\t\t\t// ID = position in vector. eg: (0 elements) -> ID = 0 (first one)\n\t\t\tq.id = static_cast(configurations.size());\n\n\t\t\t// Ensure to free-space collison\n\t\t\tif (not_inside(q, obstacles, inflate_robot))\n\t\t\t{\n\t\t\t\t// KEY, OBJECT\n\t\t\t\tconfigurations.push_back(q);\n\t\t\t} else {\n\t\t\t\t// Increment kill counter if there's a collision\n\t\t\t\tkill_counter++;\n\t\t\t}\n\t\t}\n\t}\n\n\tstd::vector PRM::find_knn(Vertex & q, const int & k)\n\t{\n\t\t// First, order set in ascending distance from q order\n\t\t// copy into vector to use sort fcn\n\t\tstd::vector configs;\n\t\tconfigs.reserve(configurations.size());\n\t\tfor (auto q_iter = configurations.begin(); q_iter != configurations.end(); q_iter++)\n\t {\n\t \tconfigs.push_back(*q_iter);\n\t }\n\t\t// using lambda for sort criterion. put q in [q] so it can be accessed\n\t\tsort(configs.begin(), configs.end(), [q](const Vertex& lhs, const Vertex& rhs)\n\t\t{\n\t return euclidean_distance(q.coords.x - lhs.coords.x, q.coords.y - lhs.coords.y)\\\n\t < euclidean_distance(q.coords.x - rhs.coords.x, q.coords.y - rhs.coords.y)\\\n\t and lhs.id != q.id and rhs.id != q.id;\n\t\t});\n\n\t\t// Now, assign IDs of first k elements\n\t\tstd::vector knn;\n\t\tfor (int i = 0; i < k; i++)\n\t\t{\n\t\t\tknn.push_back(configs.at(i));\n\t\t}\n\n\t\treturn knn;\n\t}\n\n\tbool PRM::edge_valid(const Vertex & q, const Vertex & q_prime, const double & thresh)\n\t{\n\t\t// Check if New Edge\n\t\tif (q.edge_exists(q_prime.id))\n\t\t{\n\t\t\treturn false;\n\t\t} else if (euclidean_distance(q.coords.x - q_prime.coords.x,\\\n\t\t\t\t q.coords.y - q_prime.coords.y) < thresh)\n\t\t// Check Distance Above Useful Threshold\n\t\t{\n\t\t\treturn false;\n\t\t} else if (!no_collision(q, q_prime, inflate_robot))\n\t\t// Check if Inflated Robot Intersects Polygon Edge or Path Edge intersects Polygon\n\t\t{\n\t\t\treturn false;\n\t\t} else \n\t\t{\n\t\t\treturn true;\n\t\t}\n\t}\n\n\tbool PRM::no_collision(const Vertex & q, const Vertex & q_prime, const double & inflate_robot)\n\t{\n\t\tbool free = true;\n\t\t// Loop over all obstacles\n\t\tfor (auto obs_iter = obstacles.begin(); obs_iter != obstacles.end(); obs_iter++)\n\t\t{\n\t\t\t// int i = static_cast(std::distance(obstacles.begin(), obs_iter));\n\t\t\t// std::cout << \"Obstacle #\" << i << std::endl;\n\t\t\tif (!no_intersect(q, q_prime, obs_iter))\n\t\t\t\t// Collision! Not a valid Edge, exit here.\n\t\t\t{\n\t\t\t\treturn false;\n\t\t\t} else {\n\t\t\t\t// Check if inflated robot intersects edge\n\t\t\t\tfor (auto v_iter = obs_iter->vertices.begin(); v_iter != obs_iter->vertices.end(); v_iter++)\n\t\t\t\t{\n\t\t\t\t\t// If one Vertex is to close to an Edge, return false and Exit. Otherwise keep checking.\n\t\t\t\t\tif (too_close(q, q_prime, Vertex(*v_iter), inflate_robot))\n\t\t\t\t\t{\n\t\t\t\t\t\treturn false;\n\t\t\t\t\t}\n\t\t\t\t}\n\t\t\t}\n\t\t}\n\t\treturn free;\n\t}\n\n\tbool no_intersect(const Vertex & q, const Vertex & q_prime, const std::vector::iterator & obs_iter)\n\t{\n\t\t// Initialize tE/tL: the max/min entering/leaving segment parameters.\n\t\t// NOTE: these evolve over time for each obstacle.\n\t\tdouble tE = 0.0;\n\t\tdouble tL = 1.0;\n\t\t// Initialize dS: P0->P1 vector \n\t\tEigen::Vector2d dS(q_prime.coords.x - q.coords.x, q_prime.coords.y - q.coords.y);\n\t\t// Loop over all vertices and treat them as directional Edge vectors.\n\t\tfor (auto v_iter = obs_iter->vertices.begin(); v_iter != obs_iter->vertices.end(); v_iter++)\n\t\t{\n\t\t\t// Vector from current and next vertex. so if 4 vertices: 0->1, 1->2, 2->3, 3->0\n\t\t\t// Record current index\n\t\t\tint i = static_cast(std::distance(obs_iter->vertices.begin(), v_iter));\n\t\t\t// If current index is the last one, loop back to zero for vector construction\n\t\t\t// -1 because indeces start at 0\n\t\t\tif (i == static_cast(obs_iter->vertices.size()) - 1)\n\t\t\t{\n\t\t\t\ti = 0;\n\t\t\t} else\n\t\t\t// Otherwise, just use the next index\n\t\t\t{\n\t\t\t\ti += 1;\n\t\t\t}\n\n\t\t\t// Referencing: https://drive.google.com/file/d/1gQuR4J80aXZ9BBL1s3K83TmxQSWD3AWt/view?usp=sharing Slide 28\n\t\t\tauto Vi = *v_iter;\n\t\t\tauto Vip1 = obs_iter->vertices.at(i);\n\t\t\t// edge i is Vi->Vi+1\n\t\t\tEigen::Vector2d ei(Vip1.x - Vi.x, Vip1.y - Vi.y);\n\n\t\t\t// ni is the outward normal of the edge ei\n\t\t\t// NOTE: if we have {x,y} --> {y, -x} = RHS perp. (outward normal) | {-y, x} = LHS perp. (inward normal)\n\t\t\t// outward normal\n\t\t\tEigen::Vector2d ni(Vip1.y - Vi.y, -(Vip1.x - Vi.x));\n\t\t\tif (!rigid2d::almost_equal(ei.dot(ni), 0.0))\n\t\t\t{\n\t\t\t\tthrow std::runtime_error(\"ni is NOT orthogonal to ei!\\\n\t\t\t\t\t\t\t\t\t\t \\n where(): PRM::no_collision(const Vertex &q, const Vertex & q_prime)\");\n\t\t\t}\n\t\t\tdouble N = - ni.dot(Eigen::Vector2d(q.coords.x - Vi.x, q.coords.y - Vi.y));\n\t\t\tdouble D = ni.dot(dS);\n\n\t\t\tif (rigid2d::almost_equal(D, 0.0))\n\t\t\t{\n\t\t\t\t// Then q->q' is parallel to edge ei. Double condition to catch numerical errors\n\t\t\t\tif (N < 0.0 and !(rigid2d::almost_equal(N, 0.0)))\n\t\t\t\t{\n\t\t\t\t\t// Then q is outside of polygon Obstacle.\n\t\t\t\t\treturn true; // Intersection not possible. Exit here.\n\t\t\t\t} else {\n\t\t\t\t\t// q->q' cannot enter/leave across edge ei. Process next edge\n\t\t\t\t\tcontinue;\n\t\t\t\t}\n\t\t\t}\n\t\t\t// Intersection time\n\t\t\tdouble t = N / D;\n\n\t\t\tif (D < 0.0)\n\t\t\t{\n\t\t\t\t// Then segment q->q' enters across edge ei\n\t\t\t\ttE = std::max(tE, t);\n\t\t\t\tif (tE > tL)\n\t\t\t\t{\n\t\t\t\t\t// Then the segment enters AFTER leaving. Intersection not possible on this edge.\n\t\t\t\t\treturn true;\n\t\t\t\t}\n\n\t\t\t} else {\n\t\t\t\t// D > 0 in this case\n\t\t\t\t// Then segment q->q' leaves across edge ei\n\t\t\t\ttL = std::min(tL, t);\n\t\t\t\tif (tL < tE)\n\t\t\t\t{\n\t\t\t\t\t// Then the segment leaves BEFORE entering. Intersection not possible on this edge.\n\t\t\t\t\treturn true;\n\t\t\t\t}\n\t\t\t}\n\t\t}\n\t\t// tE <= tL if we have not exited at an earlier edge.\n\t\t// There is a valid intersection. Exit here. No need to check additional obstacles.\n\t\t// Entering Point: P(tE) = q + tE * dS\n\t\t// Leaving Point: P(tL) = q + tL * dS\n\t\treturn false;\n\t}\n\n\tbool not_inside(const Vertex & q, const std::vector & obstacles, const double & inflate_robot)\n\t{\n\t\tbool free = true;\n\t\t// Loop over all obstacles\n\t\tfor (auto obs_iter = obstacles.begin(); obs_iter != obstacles.end(); obs_iter++)\n\t\t{\n\t\t\t// Loop over all vertices and treat them as directional vectors.\n\t\t\t// if q is ON ANY edge, then it is on an obstacle\n\t\t\t// If q is on the left side of ALL edges, then it is inside an obstacle\n\t\t\tbool on_all_left = true;\n\n\t\t\tfor (auto v_iter = obs_iter->vertices.begin(); v_iter != obs_iter->vertices.end(); v_iter++)\n\t\t\t{\n\t\t\t\t// Vector from current and next vertex. so if 4 vertices: 0->1, 1->2, 2->3, 3->0\n\t\t\t\t// Record current index\n\t\t\t\tint i = static_cast(std::distance(obs_iter->vertices.begin(), v_iter));\n\t\t\t\t// If current index is the last one, loop back to zero for vector construction\n\t\t\t\t// -1 because indeces start at 0\n\t\t\t\tif (i == static_cast(obs_iter->vertices.size()) - 1)\n\t\t\t\t{\n\t\t\t\t\ti = 0;\n\t\t\t\t} else\n\t\t\t\t// Otherwise, just use the next index\n\t\t\t\t{\n\t\t\t\t\ti += 1;\n\t\t\t\t}\n\n\t\t\t\t// Referencing: https://drive.google.com/file/d/1gQuR4J80aXZ9BBL1s3K83TmxQSWD3AWt/view?usp=sharing Slide 32\n\t\t\t\t// v_iter is A\n\t\t\t\t// obs_iter->vertices.at(i) is B\n\t\t\t\t// DIRECTION is A->B\n\t\t\t\t// q is P\n\t\t\t\tauto A = *v_iter;\n\t\t\t\tauto B = obs_iter->vertices.at(i);\n\t\t\t\tauto P = q;\n\n\t\t\t\t// see lineToPoint: determine whether point is on line segment, find closest point on line,\n\t\t\t\t// and determine signed distance\n\t\t\t\tauto shrt = lineToPoint(Vertex(A), Vertex(B), Vertex(P.coords));\n\n\t\t\t\tif (shrt.D < 0.0)\n\t\t\t\t\t// P is on the right side of at least one edge, not necessarily on obstacle\n\t\t\t\t\t// Only way this can be an obstacle now is if it's ON and edge\n\t\t\t\t{\n\t\t\t\t\t// if P is within the segment window, check whether it is in the inflation bound\n\t\t\t\t\tif ((shrt.u >= 0.0 and shrt.u <= 1.0) or rigid2d::almost_equal(shrt.u, 0.0)\\\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t or rigid2d::almost_equal(shrt.u, 1.0))\n\t\t\t\t\t{\n\t\t\t\t\t\tif (shrt.D < - inflate_robot)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\ton_all_left = false;\n\t\t\t\t\t\t}\n\t\t\t\t\t} else\n\t\t\t\t\t// P is not within the segment window, so check if distance to closest segment vertex is above\n\t\t\t\t\t// inflation bound\n\t\t\t\t\t{\n\t\t\t\t\t\tif (shrt.u > 1.0)\n\t\t\t\t\t\t// CLOSEST TO Vertex 2\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tauto dist = euclidean_distance(B.x - P.coords.x, B.y - P.coords.y);\n\t\t\t\t\t\t\tif (dist > inflate_robot)\n\t\t\t\t\t\t\t{\n\t\t\t\t\t\t\t\ton_all_left = false;\n\t\t\t\t\t\t\t} else {\n\t\t\t\t\t\t\t\treturn false;\n\t\t\t\t\t\t\t}\n\n\t\t\t\t\t\t} else if (shrt.u < 0.0)\n\t\t\t\t\t\t// CLOSEST TO VERTEX 1\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tauto dist = euclidean_distance(A.x - P.coords.x, A.y - P.coords.y);\n\t\t\t\t\t\t\tif (dist > inflate_robot)\n\t\t\t\t\t\t\t{\n\t\t\t\t\t\t\t\ton_all_left = false;\n\t\t\t\t\t\t\t} else {\n\t\t\t\t\t\t\t\treturn false;\n\t\t\t\t\t\t\t}\n\t\t\t\t\t\t}\n\t\t\t\t\t}\n\t\t\t\t} else if (rigid2d::almost_equal(shrt.D, 0.0) and ((shrt.u >= 0.0 and shrt.u <= 1.0)\\\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\tor (rigid2d::almost_equal(shrt.u, 0.0)\\\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\tor rigid2d::almost_equal(shrt.u, 1.0))))\n\t\t\t\t\t// if P is on an edge of an obstacle and within segment bounds, it is disqualified immediately\n\t\t\t\t{\n\t\t\t\t\treturn false;\n\t\t\t\t} else if (rigid2d::almost_equal(shrt.D, 0.0))\n\t\t\t\t// EDGE CASE: zero signed distance without being on line segment\n\t\t\t\t{\n\t\t\t\t\t// Now we check which vertex is closest to our point.\n\t\t\t\t\tif (shrt.u > 1.0)\n\t\t\t\t\t// CLOSEST TO Vertex 2\n\t\t\t\t\t{\n\t\t\t\t\t\tauto dist = euclidean_distance(B.x - P.coords.x, B.y - P.coords.y);\n\t\t\t\t\t\tif (dist > inflate_robot)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\ton_all_left = false;\n\t\t\t\t\t\t} else {\n\t\t\t\t\t\t\treturn false;\n\t\t\t\t\t\t}\n\n\t\t\t\t\t} else if (shrt.u < 0.0)\n\t\t\t\t\t// CLOSEST TO VERTEX 1\n\t\t\t\t\t{\n\t\t\t\t\t\tauto dist = euclidean_distance(A.x - P.coords.x, A.y - P.coords.y);\n\t\t\t\t\t\tif (dist > inflate_robot)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\ton_all_left = false;\n\t\t\t\t\t\t} else {\n\t\t\t\t\t\t\treturn false;\n\t\t\t\t\t\t}\n\t\t\t\t\t}\n\t\t\t\t} else\n\t\t\t\t// this means shrt.D > 0.0\n\t\t\t\t{\n\n\t\t\t\t}\n\t\t\t}\n\n\t\t\t// is q is on the inside of all the edges of an obstacle, it is not free (disqualified)\n\t\t\tif (on_all_left)\n\t\t\t{\n\t\t\t\tfree = false;\n\t\t\t}\n\n\t\t}\n\n\t\treturn free;\n\t}\n\n\tbool too_close(const Vertex & E1, const Vertex & E2, const Vertex & P0, const double & inflate_robot)\n\t{\n\t\t// Referencing: https://docs.google.com/presentation/d/1gQuR4J80aXZ9BBL1s3K83TmxQSWD3AWt/edit#slide=id.g731274b3d5_0_94 Slide 33\n\t\tEigen::Vector2d P1(E1.coords.x, E1.coords.y);\n\t\tEigen::Vector2d P2(E2.coords.x, E2.coords.y);\n\t\tEigen::Vector2d P3(P0.coords.x, P0.coords.y);\n\n\t\tauto shrt = lineToPoint(E1, E2, P0);\n\n\t\tif (shrt.u >= 0.0 and shrt.u <= 1.0)\n\t\t{\n\t\t\t// the Polygon Vertex is somwhere on the segment, so analysis check is valid\n\t\t\t// get distance to closest point\n\t\t\tEigen::Vector2d dist(P3(0) - shrt.point.x, P3(1) - shrt.point.y);\n\n\t\t\tif (dist.norm() < inflate_robot)\n\t\t\t{\n\t\t\t\t// The PRM Edge is too close to the Polygon Vertex\n\t\t\t\t// std::cout << \"TOO CLOSE\" << std::endl;\n\t\t\t\treturn true;\n\t\t\t}\n\t\t}\n\n\t\treturn false;\n\t}\n\n\tShortestDistance lineToPoint(const Vertex & E1, const Vertex & E2, const Vertex & P0)\n\t{\n\t\t// Declare struct for storage\n\t\tShortestDistance shrt;\n\n\t\t// Referencing: https://docs.google.com/presentation/d/1gQuR4J80aXZ9BBL1s3K83TmxQSWD3AWt/edit#slide=id.g731274b3d5_0_94 Slide 33\n\t\tEigen::Vector2d P1(E1.coords.x, E1.coords.y);\n\t\tEigen::Vector2d P2(E2.coords.x, E2.coords.y);\n\t\tEigen::Vector2d P3(P0.coords.x, P0.coords.y);\n\n\t\t// Step 1. determine whether point is on line segment by finding u\n\t\tshrt.u = ((P3(0) - P1(0)) * (P2(0) - P1(0)) + (P3(1) - P1(1)) * (P2(1) - P1(1)))\\\n\t\t\t\t\t/ Eigen::Vector2d(P2(0) - P1(0), P2(1) - P1(1)).squaredNorm();\n\t\t// Step 2. Determine closest point\n\t\tshrt.point = Vector2D(P1(0) + shrt.u * (P2(0) - P1(0)), P1(1) + shrt.u * (P2(1) - P1(1)));\n\n\t\t// Step 3. find the signed distance between P3 and line P1->P2 (more informative than the norm)\n\t\t// u is perpendicular to P1->P2. flip xs and ys and negate one component\n\t\t// NOTE: if we have {x,y} --> {y, -x} = RHS perp. (outward normal) | {-y, x} = LHS perp. (inward normal)\n\t\t// inward normal\n\t\tEigen::Vector2d u(-(P2(1) - P1(1)), P2(0) - P1(0));\n\t\t// dot product with P1->P2 = 0 to ensure orthogonal\n\t\tEigen::Vector2d Segment(P2(0) - P1(0), P2(1) - P1(1));\n\t\tdouble Segu_test = Segment.dot(u);\n\t\tif (!rigid2d::almost_equal(Segu_test, 0.0))\n\t\t{\n\t\t\tthrow std::runtime_error(\"u is NOT orthogonal to P1->P2!\\\n\t\t\t\t\t\t\t\t\t \\n where(): map::lineToPoint(const Vertex & E1, const Vertex & E2, const Vertex & P0, const double & inflate_robot)\");\n\t\t}\n\n\t\t// n is the normal vector of u\n\t\tauto n = u.normalized(); // .normalize() for in place\n\t\t// D is vector P1->P3\n\t\tEigen::Vector2d D(P3(0) - P1(0), P3(1) - P1(1));\n\t\t// Finally, D*n (dot product) gives us the minimum signed distance from P1->P2 to P\n\t\t// if d > 0, P is on the left of P1->P2, if d = 0, P is ON P1->P2, if d < 0, P is on the right of AB\n\t\tshrt.D = D.dot(n);\n\n\t\treturn shrt;\n\t}\n\n\tstd::vector PRM::return_prm()\n\t{\n\t\treturn configurations;\n\t}\n}" }, { "path": "map/src/viz_grid.cpp", "content": "/// \\file\n/// \\brief Draws Each Obstacle in RViz using MarkerArrays\n///\n/// PARAMETERS:\n/// PUBLISHES:\n/// SUBSCRIBES:\n/// FUNCTIONS:\n\n#include \n#include \n#include \n// #include \n#include \n#include \n\n#include \"map/grid.hpp\"\n\nusing rigid2d::Vector2D;\n\nint main(int argc, char** argv)\n{\n ROS_INFO(\"STARTING NODE: grid_map\");\n\n // Vars\n double frequency = 10.0;\n // resolution by which to transform marker poses (10 cm/cell so we use 10)\n double resolution = 0.01;\n std::string frame_id = \"base_footprint\";\n XmlRpc::XmlRpcValue xml_obstacles;\n\n // Grid Parameters\n double inflate = 0.1;\n double SCALE = 10.0;\n std::string map_type = \"grid_map\";\n\n // store Obstacle(s) here to create Map\n std::vector obstacles_v;\n\n ros::init(argc, argv, \"grid_map_node\"); // register the node on ROS\n ros::NodeHandle nh; // get a handle to ROS\n ros::NodeHandle nh_(\"~\"); // get a handle to ROS\n // Parameters\n nh_.getParam(\"frequency\", frequency);\n nh_.getParam(\"obstacles\", xml_obstacles);\n nh_.getParam(\"map_frame_id\", frame_id);\n nh_.getParam(\"inflate\", inflate);\n nh_.getParam(\"resolution\", resolution);\n nh_.getParam(\"scale\", SCALE);\n\n ros::Publisher grid_pub = nh.advertise(\"grid_map\", 1);\n\n // 'obstacles' is a triple-nested list.\n // 1st level: obstacle (Obstacle), 2nd level: vertices (std::vector), 3rd level: coordinates (Vector2D)\n\n // std::vector\n if(xml_obstacles.getType() != XmlRpc::XmlRpcValue::TypeArray)\n {\n ROS_ERROR(\"There is no list of obstacles\");\n } else {\n for(int i = 0; i < xml_obstacles.size(); ++i)\n {\n // Obstacle contains std::vector\n // create Obstacle with empty vertex vector\n map::Obstacle obs;\n if(xml_obstacles[i].getType() != XmlRpc::XmlRpcValue::TypeArray)\n {\n ROS_ERROR(\"obstacles[%d] has no vertices\", i);\n } else {\n for(int j = 0; j < xml_obstacles[i].size(); ++j)\n {\n // Vector2D contains x,y coords\n if(xml_obstacles[i][j].size() != 2)\n {\n ROS_ERROR(\"Vertex[%d] of obstacles[%d] is not a pair of coordinates\", j, i);\n } else if(\n xml_obstacles[i][j][0].getType() != XmlRpc::XmlRpcValue::TypeDouble or\n xml_obstacles[i][j][1].getType() != XmlRpc::XmlRpcValue::TypeDouble)\n {\n ROS_ERROR(\"The coordinates of vertex[%d] of obstacles[%d] are not doubles\", j, i);\n } else {\n // PASSED ALL THE TESTS: push Vector2D to vertices list in Obstacle object\n rigid2d::Vector2D vertex(xml_obstacles[i][j][0], xml_obstacles[i][j][1]);\n // NOTE: SCALE DOWN\n vertex.x /= SCALE;\n vertex.y /= SCALE;\n obs.vertices.push_back(vertex);\n }\n }\n }\n // push Obstacle object to vector of Object(s) in Map\n obstacles_v.push_back(obs);\n }\n }\n\n\n // Initialize Grid\n map::Grid grid(obstacles_v, inflate);\n\n // Build Map\n grid.build_map(resolution);\n\n ROS_INFO(\"Grid Built!\");\n\n // Get map bounds\n auto bounds = grid.return_map_bounds();\n auto gridsize = grid.return_grid_dimensions();\n\n // rviz representation of the grid\n std::vector map;\n grid.occupancy_grid(map);\n\n // Grid Pose\n geometry_msgs::Pose map_pose;\n map_pose.position.x = bounds.at(0).x;\n map_pose.position.y = bounds.at(0).y;\n map_pose.position.z = 0.0;\n map_pose.orientation.x = 0.0;\n map_pose.orientation.y = 0.0;\n map_pose.orientation.z = 0.0;\n map_pose.orientation.w = 1.0;\n\n // Occupancy grid for visualization\n nav_msgs::OccupancyGrid grid_map;\n grid_map.header.frame_id = frame_id;\n grid_map.info.resolution = resolution;\n grid_map.info.width = gridsize.at(0);\n grid_map.info.height = gridsize.at(1);\n\n grid_map.info.origin = map_pose;\n\n while(ros::ok())\n {\n ros::spinOnce();\n grid_map.header.stamp = ros::Time::now();\n grid_map.info.map_load_time = ros::Time::now();\n grid_map.data = map;\n grid_pub.publish(grid_map);\n }\n\n return 0;\n}\n" }, { "path": "map/src/viz_map.cpp", "content": "/// \\file\n/// \\brief Draws Each Obstacle in RViz using MarkerArrays\n///\n/// PARAMETERS:\n/// PUBLISHES:\n/// SUBSCRIBES:\n/// FUNCTIONS:\n\n#include \n\n#include \n#include \n#include \n\n#include \"map/map.hpp\"\n#include \"map/prm.hpp\"\n\n#include \"nuslam/TurtleMap.h\"\n\n#include // To use std::bind\n#include \n#include \n#include \n\n// Used to deal with YAML list of lists\n#include // catkin component\n\n\nint main(int argc, char** argv)\n/// The Main Function ///\n{\n ROS_INFO(\"STARTING NODE: viz_map\");\n\n // Vars\n double frequency = 10.0;\n // Scale by which to transform marker poses (10 cm/cell so we use 10)\n double SCALE = 10.0;\n std::string frame_id = \"base_footprint\";\n XmlRpc::XmlRpcValue xml_obstacles;\n\n // PRM Parameters\n int n = 10;\n int k = 5;\n double thresh = 0.01;\n double inflate = 0.1;\n std::string map_type = \"map\";\n\n // store Obstacle(s) here to create Map\n std::vector obstacles_v;\n\n ros::init(argc, argv, \"viz_map_node\"); // register the node on ROS\n ros::NodeHandle nh; // get a handle to ROS\n ros::NodeHandle nh_(\"~\"); // get a handle to ROS\n // Parameters\n nh_.getParam(\"frequency\", frequency);\n nh_.getParam(\"obstacles\", xml_obstacles);\n nh_.getParam(\"map_frame_id\", frame_id);\n nh_.getParam(\"n\", n);\n nh_.getParam(\"k\", k);\n nh_.getParam(\"thresh\", thresh);\n nh_.getParam(\"inflate\", inflate);\n nh_.getParam(\"map_type\", map_type);\n nh_.getParam(\"scale\", SCALE);\n\n // 'obstacles' is a triple-nested list.\n // 1st level: obstacle (Obstacle), 2nd level: vertices (std::vector), 3rd level: coordinates (Vector2D)\n\n // std::vector\n if(xml_obstacles.getType() != XmlRpc::XmlRpcValue::TypeArray)\n {\n ROS_ERROR(\"There is no list of obstacles\");\n } else {\n for(int i = 0; i < xml_obstacles.size(); ++i)\n {\n // Obstacle contains std::vector\n // create Obstacle with empty vertex vector\n map::Obstacle obs;\n if(xml_obstacles[i].getType() != XmlRpc::XmlRpcValue::TypeArray)\n {\n ROS_ERROR(\"obstacles[%d] has no vertices\", i);\n } else {\n for(int j = 0; j < xml_obstacles[i].size(); ++j)\n {\n // Vector2D contains x,y coords\n if(xml_obstacles[i][j].size() != 2)\n {\n ROS_ERROR(\"Vertex[%d] of obstacles[%d] is not a pair of coordinates\", j, i);\n } else if(\n xml_obstacles[i][j][0].getType() != XmlRpc::XmlRpcValue::TypeDouble or\n xml_obstacles[i][j][1].getType() != XmlRpc::XmlRpcValue::TypeDouble)\n {\n ROS_ERROR(\"The coordinates of vertex[%d] of obstacles[%d] are not doubles\", j, i);\n } else {\n // PASSED ALL THE TESTS: push Vector2D to vertices list in Obstacle object\n rigid2d::Vector2D vertex(xml_obstacles[i][j][0], xml_obstacles[i][j][1]);\n // NOTE: SCALE DOWN\n vertex.x /= SCALE;\n vertex.y /= SCALE;\n obs.vertices.push_back(vertex);\n }\n }\n }\n // push Obstacle object to vector of Object(s) in Map\n obstacles_v.push_back(obs);\n }\n }\n\n // Initialize Marker\n // Init Marker Array Publisher\n ros::Publisher map_pub = nh.advertise(\"map\", 1);\n\n // Init Marker Array\n visualization_msgs::MarkerArray map_arr;\n\n // Init Marker\n visualization_msgs::Marker marker;\n marker.header.frame_id = frame_id;\n marker.header.stamp = ros::Time::now();\n // marker.ns = \"my_namespace\";\n // marker.id = 0;\n marker.type = visualization_msgs::Marker::LINE_STRIP;\n marker.action = visualization_msgs::Marker::ADD;\n marker.pose.position.z = 0;\n marker.pose.orientation.x = 0.0;\n marker.pose.orientation.y = 0.0;\n marker.pose.orientation.z = 0.0;\n marker.pose.orientation.w = 1.0;\n marker.scale.x = 0.02;\n marker.color.r = 0.5f;\n marker.color.g = 0.0f;\n marker.color.b = 0.5f;\n marker.color.a = 1.0;\n marker.lifetime = ros::Duration();\n\n visualization_msgs::Marker sph_mkr;\n sph_mkr = marker;\n sph_mkr.type = visualization_msgs::Marker::SPHERE;\n sph_mkr.scale.x = 0.02;\n sph_mkr.scale.y = 0.02;\n sph_mkr.scale.z = 0.02;\n\n // Color based on map_type\n if (map_type == \"map\")\n {\n marker.color.r = 0.5f;\n marker.color.g = 0.0f;\n marker.color.b = 0.5f;\n sph_mkr.color.r = 0.96f;\n sph_mkr.color.g = 0.475f;\n sph_mkr.color.b = 0.0f;\n } else if (map_type == \"prm\")\n {\n marker.color.r = 0.96f;\n marker.color.g = 0.475f;\n marker.color.b = 0.0f;\n sph_mkr.color.r = 0.5f;\n sph_mkr.color.g = 0.0f;\n sph_mkr.color.b = 0.5f;\n }\n\n // LINE_STRIP relative to this pose\n marker.pose.position.x = 0.0;\n marker.pose.position.y = 0.0;\n\n int marker_id = 0;\n\n if (map_type == \"map\")\n {\n // DRAW MAP\n map::Map map(obstacles_v);\n // Now, loop through obstacles in map to create line strips.\n // Each obstacle = 1 line strip\n int marker_id = 0;\n for (auto obs_iter = obstacles_v.begin(); obs_iter != obstacles_v.end(); obs_iter++)\n {\n marker.points.clear();\n marker.id = marker_id;\n marker_id++;\n ROS_DEBUG(\"obstacle [%d] starts at (%.2f, %.2f)\", marker.id + 1, obs_iter->vertices.at(0).x / SCALE, obs_iter->vertices.at(0).y / SCALE);\n for (auto v_iter = obs_iter->vertices.begin(); v_iter != obs_iter->vertices.end(); v_iter++)\n {\n geometry_msgs::Point new_vertex;\n new_vertex.x = v_iter->x;\n new_vertex.y = v_iter->y;\n new_vertex.z = 0.0;\n marker.points.push_back(new_vertex);\n \n // Also push back cylinders\n sph_mkr.pose.position.x = v_iter->x;\n sph_mkr.pose.position.y = v_iter->y;\n sph_mkr.id = marker_id;\n marker_id++;\n map_arr.markers.push_back(sph_mkr);\n }\n // Before pushing back, connect last vertex to first vertex\n geometry_msgs::Point new_vertex;\n new_vertex.x = obs_iter->vertices.at(0).x;\n new_vertex.y = obs_iter->vertices.at(0).y;\n new_vertex.z = 0.0;\n marker.points.push_back(new_vertex);\n map_arr.markers.push_back(marker);\n }\n\n ROS_INFO(\"Obstacle Map Built!\");\n } else if (map_type == \"prm\")\n {\n // Build PRM\n map::PRM prm(obstacles_v, inflate);\n prm.build_map(n, k, thresh);\n // DRAW PRM\n auto configurations = prm.return_prm();\n for (auto node_iter = configurations.begin(); node_iter != configurations.end(); node_iter++)\n {\n marker.points.clear();\n marker.id = marker_id;\n marker_id++;\n\n // Check if a node has edges before plotting\n if (node_iter->id_set.size() > 0)\n {\n for (auto id_iter = node_iter->id_set.begin(); id_iter != node_iter->id_set.end(); id_iter++)\n {\n // Add node as first marker vertex\n geometry_msgs::Point first_vertex;\n first_vertex.x = node_iter->coords.x;\n first_vertex.y = node_iter->coords.y;\n first_vertex.z = 0.0;\n marker.points.push_back(first_vertex);\n\n // Find Vertex for each ID\n auto neighbor_iter = configurations.at(*id_iter);\n\n geometry_msgs::Point new_vertex;\n new_vertex.x = neighbor_iter.coords.x;\n new_vertex.y = neighbor_iter.coords.y;\n new_vertex.z = 0.0;\n marker.points.push_back(new_vertex);\n\n // Also push back cylinders\n sph_mkr.pose.position.x = node_iter->coords.x;\n sph_mkr.pose.position.y = node_iter->coords.y;\n sph_mkr.id = marker_id;\n marker_id++;\n map_arr.markers.push_back(sph_mkr);\n }\n // Push to Marker Array\n map_arr.markers.push_back(marker);\n }\n }\n ROS_INFO(\"PRM Built!\");\n }\n\n ros::Rate rate(frequency);\n\n // Main While\n while (ros::ok())\n {\n \tros::spinOnce();\n\n // Publish Map\n map_pub.publish(map_arr);\n\n rate.sleep();\n }\n\n return 0;\n}" }, { "path": "nuturtle.rosinstall", "content": "- git:\n local-name: tb3\n uri: git@github.com:moribots/turtlebot3_from_scratch.git\n- git:\n local-name: world\n uri: https://github.com/ME495-Navigation/nuturtlebot.git\n" } ]