From c13cd8787261eb645083940ee0aabc831828c068 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Wed, 7 Jun 2023 01:57:55 +0200 Subject: [PATCH] Rename project --- CMakeLists.txt | 10 +- README.md | 13 ++- apps/path-planner-cli/CMakeLists.txt | 2 +- apps/path-planner-cli/path-planner-cli.cpp | 83 +++++++------- apps/selfdriving-simulator-gui/CMakeLists.txt | 2 +- .../selfdriving-simulator-gui.cpp | 104 +++++++++--------- cmake/misc_cmake_functions.cmake | 4 +- cmake/script_version_number.cmake | 10 +- cmake/version.h.in | 2 +- doc/Doxyfile | 4 +- doc/overview.md | 2 +- .../CMakeLists.txt | 4 +- .../include/mpp}/algos/CostEvaluator.h | 6 +- .../include/mpp}/algos/CostEvaluatorCostMap.h | 8 +- .../algos/CostEvaluatorPreferredWaypoint.h | 8 +- .../include/mpp}/algos/NavEngine.h | 43 ++++---- .../include/mpp}/algos/Planner.h | 12 +- .../include/mpp}/algos/TPS_Astar.h | 14 +-- .../include/mpp}/algos/bestTrajectory.h | 8 +- .../mpp}/algos/edge_interpolated_path.h | 6 +- .../include/mpp}/algos/refine_trajectory.h | 8 +- .../include/mpp}/algos/render_tree.h | 10 +- .../include/mpp}/algos/render_vehicle.h | 6 +- .../mpp}/algos/tp_obstacles_single_path.h | 6 +- .../include/mpp}/algos/trajectories.h | 10 +- .../mpp}/algos/transform_pc_square_clipping.h | 4 +- .../include/mpp}/algos/viz.h | 14 +-- .../include/mpp}/algos/within_bbox.h | 4 +- .../include/mpp}/data/EnqueuedMotionCmd.h | 4 +- .../include/mpp}/data/MotionPrimitivesTree.h | 12 +- .../include/mpp}/data/MoveEdgeSE2_TPS.h | 10 +- .../include/mpp}/data/PlannerInput.h | 10 +- .../include/mpp}/data/PlannerOutput.h | 8 +- .../include/mpp}/data/ProgressCallbackData.h | 12 +- .../include/mpp}/data/RenderOptions.h | 4 +- .../include/mpp}/data/SE2_KinState.h | 4 +- .../mpp}/data/TrajectoriesAndRobotShape.h | 6 +- .../mpp}/data/VehicleLocalizationState.h | 4 +- .../include/mpp}/data/VehicleOdometryState.h | 4 +- .../include/mpp}/data/Waypoints.h | 4 +- .../include/mpp}/data/basic_types.h | 4 +- .../include/mpp}/data/const_ref_t.h | 4 +- .../include/mpp}/data/ptg_t.h | 4 +- .../include/mpp}/data/trajectory_t.h | 8 +- .../include/mpp}/interfaces/LidarSource.h | 4 +- .../mpp}/interfaces/MVSIM_VehicleInterface.h | 13 +-- .../include/mpp}/interfaces/ObstacleSource.h | 4 +- .../interfaces/TargetApproachController.h | 18 +-- .../mpp}/interfaces/VehicleMotionInterface.h | 10 +- .../mpp}/ptgs/DiffDriveCollisionGridBased.h | 6 +- .../include/mpp}/ptgs/DiffDrive_C.h | 10 +- .../include/mpp}/ptgs/HolonomicBlend.h | 8 +- .../include/mpp}/ptgs/SpeedTrimmablePTG.h | 4 +- .../src/algos/CostEvaluator.cpp | 6 +- .../src/algos/CostEvaluatorCostMap.cpp | 6 +- .../algos/CostEvaluatorPreferredWaypoint.cpp | 7 +- .../src/algos/NavEngine.cpp | 53 ++++----- .../src/algos/Planner.cpp | 6 +- .../src/algos/TPS_Astar.cpp | 22 ++-- .../src/algos/bestTrajectory.cpp | 8 +- .../src/algos/edge_interpolated_path.cpp | 4 +- .../src/algos/refine_trajectory.cpp | 26 ++--- .../src/algos/render_tree.cpp | 8 +- .../src/algos/render_vehicle.cpp | 6 +- .../src/algos/tp_obstacles_single_path.cpp | 4 +- .../src/algos/trajectories.cpp | 11 +- .../algos/transform_pc_square_clipping.cpp | 4 +- .../src/algos/viz.cpp | 17 ++- .../src/data/MoveEdgeSE2_TPS.cpp | 4 +- .../src/data/PlannerInput.cpp | 4 +- .../src/data/PlannerOutput.cpp | 4 +- .../src/data/SE2_KinState.cpp | 4 +- .../src/data/TrajectoriesAndRobotShape.cpp | 6 +- .../src/data/Waypoints.cpp | 4 +- .../src/interfaces/ObstacleSource.cpp | 4 +- .../interfaces/TargetApproachController.cpp | 6 +- .../src/interfaces/VehicleMotionInterface.cpp | 7 +- .../src/ptgs/DiffDriveCollisionGridBased.cpp | 8 +- .../src/ptgs/DiffDrive_C.cpp | 6 +- .../src/ptgs/HolonomicBlend.cpp | 15 ++- .../src/registerClasses.cpp | 16 +-- scripts/plotPath.m | 2 +- share/ptgs_holonomic_robot.ini | 2 +- wip-experimental/TPS_RRTstar.cpp | 17 ++- wip-experimental/TPS_RRTstar.h | 10 +- 85 files changed, 443 insertions(+), 450 deletions(-) rename {libselfdriving => mrpt_path_planning}/CMakeLists.txt (85%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/algos/CostEvaluator.h (89%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/algos/CostEvaluatorCostMap.h (93%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/algos/CostEvaluatorPreferredWaypoint.h (92%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/algos/NavEngine.h (95%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/algos/Planner.h (87%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/algos/TPS_Astar.h (96%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/algos/bestTrajectory.h (82%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/algos/edge_interpolated_path.h (84%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/algos/refine_trajectory.h (86%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/algos/render_tree.h (76%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/algos/render_vehicle.h (88%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/algos/tp_obstacles_single_path.h (82%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/algos/trajectories.h (76%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/algos/transform_pc_square_clipping.h (93%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/algos/viz.h (73%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/algos/within_bbox.h (94%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/data/EnqueuedMotionCmd.h (96%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/data/MotionPrimitivesTree.h (98%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/data/MoveEdgeSE2_TPS.h (92%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/data/PlannerInput.h (76%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/data/PlannerOutput.h (91%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/data/ProgressCallbackData.h (81%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/data/RenderOptions.h (98%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/data/SE2_KinState.h (98%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/data/TrajectoriesAndRobotShape.h (93%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/data/VehicleLocalizationState.h (97%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/data/VehicleOdometryState.h (96%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/data/Waypoints.h (99%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/data/basic_types.h (95%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/data/const_ref_t.h (91%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/data/ptg_t.h (90%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/data/trajectory_t.h (82%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/interfaces/LidarSource.h (91%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/interfaces/MVSIM_VehicleInterface.h (95%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/interfaces/ObstacleSource.h (98%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/interfaces/TargetApproachController.h (86%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/interfaces/VehicleMotionInterface.h (97%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/ptgs/DiffDriveCollisionGridBased.h (98%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/ptgs/DiffDrive_C.h (92%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/ptgs/HolonomicBlend.h (97%) rename {libselfdriving/include/selfdriving => mrpt_path_planning/include/mpp}/ptgs/SpeedTrimmablePTG.h (91%) rename {libselfdriving => mrpt_path_planning}/src/algos/CostEvaluator.cpp (87%) rename {libselfdriving => mrpt_path_planning}/src/algos/CostEvaluatorCostMap.cpp (97%) rename {libselfdriving => mrpt_path_planning}/src/algos/CostEvaluatorPreferredWaypoint.cpp (95%) rename {libselfdriving => mrpt_path_planning}/src/algos/NavEngine.cpp (98%) rename {libselfdriving => mrpt_path_planning}/src/algos/Planner.cpp (82%) rename {libselfdriving => mrpt_path_planning}/src/algos/TPS_Astar.cpp (98%) rename {libselfdriving => mrpt_path_planning}/src/algos/bestTrajectory.cpp (95%) rename {libselfdriving => mrpt_path_planning}/src/algos/edge_interpolated_path.cpp (95%) rename {libselfdriving => mrpt_path_planning}/src/algos/refine_trajectory.cpp (80%) rename {libselfdriving => mrpt_path_planning}/src/algos/render_tree.cpp (98%) rename {libselfdriving => mrpt_path_planning}/src/algos/render_vehicle.cpp (94%) rename {libselfdriving => mrpt_path_planning}/src/algos/tp_obstacles_single_path.cpp (91%) rename {libselfdriving => mrpt_path_planning}/src/algos/trajectories.cpp (92%) rename {libselfdriving => mrpt_path_planning}/src/algos/transform_pc_square_clipping.cpp (92%) rename {libselfdriving => mrpt_path_planning}/src/algos/viz.cpp (91%) rename {libselfdriving => mrpt_path_planning}/src/data/MoveEdgeSE2_TPS.cpp (95%) rename {libselfdriving => mrpt_path_planning}/src/data/PlannerInput.cpp (81%) rename {libselfdriving => mrpt_path_planning}/src/data/PlannerOutput.cpp (81%) rename {libselfdriving => mrpt_path_planning}/src/data/SE2_KinState.cpp (96%) rename {libselfdriving => mrpt_path_planning}/src/data/TrajectoriesAndRobotShape.cpp (96%) rename {libselfdriving => mrpt_path_planning}/src/data/Waypoints.cpp (99%) rename {libselfdriving => mrpt_path_planning}/src/interfaces/ObstacleSource.cpp (87%) rename {libselfdriving => mrpt_path_planning}/src/interfaces/TargetApproachController.cpp (73%) rename {libselfdriving => mrpt_path_planning}/src/interfaces/VehicleMotionInterface.cpp (79%) rename {libselfdriving => mrpt_path_planning}/src/ptgs/DiffDriveCollisionGridBased.cpp (99%) rename {libselfdriving => mrpt_path_planning}/src/ptgs/DiffDrive_C.cpp (96%) rename {libselfdriving => mrpt_path_planning}/src/ptgs/HolonomicBlend.cpp (99%) rename {libselfdriving => mrpt_path_planning}/src/registerClasses.cpp (70%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 87a02bd..cd96fcb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,7 +8,7 @@ endif() # Set the possible values of build type for cmake-gui set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug" "Release" "MinSizeRel" "RelWithDebInfo" "SanitizeAddress" "SanitizeThread") -project(SelfdrivingProject LANGUAGES CXX) +project(MRPTPathPlanningProject LANGUAGES CXX) # Include std cmake scripts: include(CheckCXXCompilerFlag) # for check_cxx_compiler_flag() @@ -45,16 +45,16 @@ find_package(mrpt-gui REQUIRED) find_package(mrpt-nav REQUIRED) # Targets: -add_subdirectory(libselfdriving) +add_subdirectory(mrpt_path_planning) -option(SELFDRIVING_BUILD_APPS "Build selfdriving cli/gui tools" ON) -if (SELFDRIVING_BUILD_APPS) +option(MRPT_PATH_PLANNING_BUILD_APPS "Build mrpt_path_planning cli/gui tools" ON) +if (MRPT_PATH_PLANNING_BUILD_APPS) add_subdirectory(apps) endif() # Summary: message(STATUS "") -message(STATUS "${PROJECT_NAME} version ${SELFDRIVING_VERSION}") +message(STATUS "${PROJECT_NAME} version ${MRPT_PATH_PLANNING_VERSION}") message(STATUS "----------------------------------------------------------") message(STATUS " CMAKE_BUILD_TYPE : ${CMAKE_BUILD_TYPE}") message(STATUS " MRPT : ${mrpt-nav_VERSION}") diff --git a/README.md b/README.md index 137fd9b..da15a84 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,10 @@ [![CI Linux](https://github.com/jlblancoc/selfdriving/actions/workflows/build-linux.yml/badge.svg)](https://github.com/jlblancoc/selfdriving/actions/workflows/build-linux.yml) [![Documentation Status](https://readthedocs.org/projects/selfdriving/badge/?version=latest)](https://selfdriving.readthedocs.io/en/latest/?badge=latest) -# selfdriving -Self-driving (autonomous navigation) algorithms for robots/vehicles moving on planar environments. This builds upon mrpt-nav and the theory behind PTGs to generate libraries of "motion primitives" for vehicles with arbitrary shape and realistic kinematics and dynamics. +# mrpt_path_planning + +Path planning and navigation algorithms for robots/vehicles moving on planar environments. +This library builds upon mrpt-nav and the theory behind PTGs to generate libraries of "motion primitives" +for vehicles with arbitrary shape and realistic kinematics and dynamics. ## Build requisites @@ -28,8 +31,8 @@ sudo apt install libmrpt-dev From your CMake script: ``` -find_package(selfdriving REQUIRED) -target_link_libraries(YOUR_TARGET selfdriving::selfdriving) +find_package(mrpt_path_planning REQUIRED) +target_link_libraries(YOUR_TARGET mpp::mrpt_path_planning) ``` ## Demo runs @@ -42,7 +45,7 @@ Command-line app to test the A* planner: ``` build-Release/bin/path-planner-cli -g "[4 2.5 45]" -s "[0.5 0 0]" \ - --planner "selfdriving::TPS_Astar" \ + --planner "mpp::TPS_Astar" \ -c share/ptgs_holonomic_robot.ini \ --obstacles share/obstacles_01.txt \ --planner-parameters share/mvsim-demo-astar-planner-params.yaml \ diff --git a/apps/path-planner-cli/CMakeLists.txt b/apps/path-planner-cli/CMakeLists.txt index c8f5eca..e49e609 100644 --- a/apps/path-planner-cli/CMakeLists.txt +++ b/apps/path-planner-cli/CMakeLists.txt @@ -10,6 +10,6 @@ selfdriving_add_executable( mrpt::gui mrpt::nav mrpt::tclap - selfdriving + mrpt_path_planning ) diff --git a/apps/path-planner-cli/path-planner-cli.cpp b/apps/path-planner-cli/path-planner-cli.cpp index a4b3a0e..7d44567 100644 --- a/apps/path-planner-cli/path-planner-cli.cpp +++ b/apps/path-planner-cli/path-planner-cli.cpp @@ -4,6 +4,13 @@ * See LICENSE for license information. * ------------------------------------------------------------------------- */ +#include +#include +#include +#include +#include +#include +#include #include #include #include // exception_to_str() @@ -16,13 +23,6 @@ #include #include // plugins #include -#include -#include -#include -#include -#include -#include -#include #include #include @@ -37,8 +37,8 @@ TCLAP::ValueArg arg_obs_file( true, "", "obs.txt", cmd); TCLAP::ValueArg argPlanner( - "p", "planner", "Planner C++ class name to use", false, - "selfdriving::TPS_Astar", "selfdriving::TPS_Astar", cmd); + "p", "planner", "Planner C++ class name to use", false, "mpp::TPS_Astar", + "mpp::TPS_Astar", cmd); TCLAP::ValueArg argVerbosity( "v", "verbose", "Verbosity level for path planner", false, "INFO", @@ -192,17 +192,16 @@ static void do_plan_path() { // Load obstacles: mrpt::maps::CPointsMap::Ptr obsPts = load_obstacles(); - auto obs = selfdriving::ObstacleSource::FromStaticPointcloud(obsPts); + auto obs = mpp::ObstacleSource::FromStaticPointcloud(obsPts); // Prepare planner input data: - selfdriving::PlannerInput pi; + mpp::PlannerInput pi; pi.stateStart.pose.fromString(arg_start_pose.getValue()); if (arg_start_vel.isSet()) pi.stateStart.vel.fromString(arg_start_vel.getValue()); - pi.stateGoal.state = - selfdriving::PoseOrPoint::FromString(arg_goal_pose.getValue()); + pi.stateGoal.state = mpp::PoseOrPoint::FromString(arg_goal_pose.getValue()); if (arg_goal_vel.isSet()) pi.stateGoal.vel.fromString(arg_goal_vel.getValue()); @@ -235,9 +234,8 @@ static void do_plan_path() << pi.worldBboxMax.asString() << "\n"; // Do the path planning : - selfdriving::Planner::Ptr planner = - std::dynamic_pointer_cast( - mrpt::rtti::classFactory(argPlanner.getValue())); + mpp::Planner::Ptr planner = std::dynamic_pointer_cast( + mrpt::rtti::classFactory(argPlanner.getValue())); if (!planner) { @@ -254,35 +252,32 @@ static void do_plan_path() { // cost map: const auto costMapParams = - selfdriving::CostEvaluatorCostMap::Parameters::FromYAML( + mpp::CostEvaluatorCostMap::Parameters::FromYAML( mrpt::containers::yaml::FromFile(arg_costMap.getValue())); - auto costmap = - selfdriving::CostEvaluatorCostMap::FromStaticPointObstacles( - *obsPts, costMapParams, pi.stateStart.pose); + auto costmap = mpp::CostEvaluatorCostMap::FromStaticPointObstacles( + *obsPts, costMapParams, pi.stateStart.pose); planner->costEvaluators_.push_back(costmap); } // Preferred waypoints: - auto wpParams = selfdriving::CostEvaluatorPreferredWaypoint::Parameters(); + auto wpParams = mpp::CostEvaluatorPreferredWaypoint::Parameters(); if (arg_waypointsParams.isSet()) { - wpParams = - selfdriving::CostEvaluatorPreferredWaypoint::Parameters::FromYAML( - mrpt::containers::yaml::FromFile( - arg_waypointsParams.getValue())); + wpParams = mpp::CostEvaluatorPreferredWaypoint::Parameters::FromYAML( + mrpt::containers::yaml::FromFile(arg_waypointsParams.getValue())); } if (arg_waypoints.isSet()) { - const auto wps = selfdriving::WaypointSequence::FromYAML( + const auto wps = mpp::WaypointSequence::FromYAML( mrpt::containers::yaml::FromFile(arg_waypoints.getValue())); std::vector lstPts; for (const auto& wp : wps.waypoints) lstPts.emplace_back(wp.target); - auto costEval = selfdriving::CostEvaluatorPreferredWaypoint::Create(); + auto costEval = mpp::CostEvaluatorPreferredWaypoint::Create(); costEval->params_ = wpParams; costEval->setPreferredWaypoints(lstPts); planner->costEvaluators_.push_back(costEval); @@ -305,14 +300,12 @@ static void do_plan_path() } // Insert custom progress callback: - planner->progressCallback_ = - [](const selfdriving::ProgressCallbackData& pcd) { - std::cout << "[progressCallback] bestCostFromStart: " - << pcd.bestCostFromStart - << " bestCostToGoal: " << pcd.bestCostToGoal - << " bestPathLength: " << pcd.bestPath.size() - << std::endl; - }; + planner->progressCallback_ = [](const mpp::ProgressCallbackData& pcd) { + std::cout << "[progressCallback] bestCostFromStart: " + << pcd.bestCostFromStart + << " bestCostToGoal: " << pcd.bestCostToGoal + << " bestPathLength: " << pcd.bestPath.size() << std::endl; + }; // PTGs config file: mrpt::config::CConfigFile cfg(arg_ptgs_file.getValue()); @@ -321,7 +314,7 @@ static void do_plan_path() // ================================================== // ACTUAL PATH PLANNING // ================================================== - const selfdriving::PlannerOutput plan = planner->plan(pi); + const mpp::PlannerOutput plan = planner->plan(pi); std::cout << "\nDone.\n"; std::cout << "Success: " << (plan.success ? "YES" : "NO") << "\n"; @@ -342,11 +335,11 @@ static void do_plan_path() if (!arg_noRefine.isSet()) { // refine: - selfdriving::refine_trajectory(plannedPath, pathEdges, pi.ptgs); + mpp::refine_trajectory(plannedPath, pathEdges, pi.ptgs); } // Visualize: - selfdriving::VisualizationOptions vizOpts; + mpp::VisualizationOptions vizOpts; vizOpts.renderOptions.highlight_path_to_node_id = plan.bestNodeId; vizOpts.renderOptions.color_normal_edge = {0xb0b0b0, 0x20}; // RGBA @@ -356,7 +349,7 @@ static void do_plan_path() // Hide regular tree edges and only show best path? if (!arg_showTree.isSet()) vizOpts.renderOptions.width_normal_edge = 0; - std::optional traj; // interpolated path + std::optional traj; // interpolated path if (plan.success) { @@ -372,7 +365,7 @@ static void do_plan_path() { const auto t0 = mrpt::Clock::nowDouble(); - traj = selfdriving::plan_to_trajectory(pathEdges, pi.ptgs); + traj = mpp::plan_to_trajectory(pathEdges, pi.ptgs); const auto dt = mrpt::Clock::nowDouble() - t0; @@ -383,7 +376,7 @@ static void do_plan_path() { std::cout << "Saving path to " << arg_InterpolatePath.getValue() << std::endl; - selfdriving::save_to_txt(*traj, arg_InterpolatePath.getValue()); + mpp::save_to_txt(*traj, arg_InterpolatePath.getValue()); } } } @@ -392,12 +385,12 @@ static void do_plan_path() if (!arg_playAnimation.isSet() || !traj.has_value()) { // regular UI: - selfdriving::viz_nav_plan(plan, vizOpts, planner->costEvaluators_); + mpp::viz_nav_plan(plan, vizOpts, planner->costEvaluators_); } else { // Animation UI: - selfdriving::viz_nav_plan_animation( + mpp::viz_nav_plan_animation( plan, *traj, vizOpts.renderOptions, planner->costEvaluators_); } } @@ -410,8 +403,8 @@ int main(int argc, char** argv) if (argPlanner_yaml_output_file.isSet()) { - selfdriving::TPS_Astar_Parameters defaults; - const auto c = defaults.as_yaml(); + mpp::TPS_Astar_Parameters defaults; + const auto c = defaults.as_yaml(); const auto sFile = argPlanner_yaml_output_file.getValue(); std::ofstream fileYaml(sFile); ASSERT_(fileYaml.is_open()); diff --git a/apps/selfdriving-simulator-gui/CMakeLists.txt b/apps/selfdriving-simulator-gui/CMakeLists.txt index 4fc193f..b932015 100644 --- a/apps/selfdriving-simulator-gui/CMakeLists.txt +++ b/apps/selfdriving-simulator-gui/CMakeLists.txt @@ -10,6 +10,6 @@ selfdriving_add_executable( SOURCES selfdriving-simulator-gui.cpp LINK_LIBRARIES mrpt::tclap - selfdriving + mrpt_path_planning mvsim::mvsim-simulator ) diff --git a/apps/selfdriving-simulator-gui/selfdriving-simulator-gui.cpp b/apps/selfdriving-simulator-gui/selfdriving-simulator-gui.cpp index ce41dc4..0f266f3 100644 --- a/apps/selfdriving-simulator-gui/selfdriving-simulator-gui.cpp +++ b/apps/selfdriving-simulator-gui/selfdriving-simulator-gui.cpp @@ -4,6 +4,14 @@ * See LICENSE for license information. * ------------------------------------------------------------------------- */ +#include +#include +#include +#include +#include +#include +#include +#include #include #include #include @@ -19,14 +27,6 @@ #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include #include @@ -61,9 +61,9 @@ TCLAP::ValueArg argMvsimFile( TCLAP::ValueArg argVehicleInterface( "", "vehicle-interface-class", - "Class name to use (Default: 'selfdriving::MVSIM_VehicleInterface')", false, - "selfdriving::MVSIM_VehicleInterface", - "Class name to use for vehicle interface", cmd); + "Class name to use (Default: 'mpp::MVSIM_VehicleInterface')", false, + "mpp::MVSIM_VehicleInterface", "Class name to use for vehicle interface", + cmd); TCLAP::ValueArg argTargetApproachController( "", "approach-controller-class", @@ -164,12 +164,12 @@ struct SelfDrivingStatus { SelfDrivingStatus() = default; - selfdriving::NavEngine navigator; + mpp::NavEngine navigator; - selfdriving::VisualizationOptions vizOpts; + mpp::VisualizationOptions vizOpts; - selfdriving::WaypointSequence waypts; - selfdriving::WaypointStatusSequence wayptsStatus; + mpp::WaypointSequence waypts; + mpp::WaypointStatusSequence wayptsStatus; SelfDrivingThreadParams sdThreadParams; std::thread selfDrivingThread; @@ -179,8 +179,8 @@ std::shared_ptr sd; static void selfdriving_run_thread(SelfDrivingThreadParams& params); static void on_do_single_path_planning( - mvsim::World& world, const selfdriving::SE2_KinState& stateStart, - const selfdriving::SE2orR2_KinState& stateGoal); + mvsim::World& world, const mpp::SE2_KinState& stateStart, + const mpp::SE2orR2_KinState& stateGoal); // ======= End Self Drive status =================== @@ -251,7 +251,7 @@ void prepare_selfdriving(mvsim::World& world) // Obstacle source: auto obsPts = world_to_static_obstacle_points(world); sd->navigator.config_.globalMapObstacleSource = - selfdriving::ObstacleSource::FromStaticPointcloud(obsPts); + mpp::ObstacleSource::FromStaticPointcloud(obsPts); sd->navigator.logFmt( mrpt::system::LVL_DEBUG, @@ -268,12 +268,12 @@ void prepare_selfdriving(mvsim::World& world) obj, mrpt::format("Unregistered class name '%s'", name.c_str())); sd->navigator.config_.vehicleMotionInterface = - std::dynamic_pointer_cast(obj); + std::dynamic_pointer_cast(obj); ASSERTMSG_( sd->navigator.config_.vehicleMotionInterface, mrpt::format( "Class '%s' seems not to implement the expected interface " - "'selfdriving::VehicleMotionInterface'", + "'mpp::VehicleMotionInterface'", name.c_str())); sd->navigator.config_.vehicleMotionInterface->setMinLoggingLevel( @@ -282,7 +282,7 @@ void prepare_selfdriving(mvsim::World& world) else { // Default: - auto sim = std::make_shared(); + auto sim = std::make_shared(); sd->navigator.config_.vehicleMotionInterface = sim; sd->navigator.config_.vehicleMotionInterface->setMinLoggingLevel( @@ -301,13 +301,12 @@ void prepare_selfdriving(mvsim::World& world) obj, mrpt::format("Unregistered class name '%s'", name.c_str())); sd->navigator.config_.targetApproachController = - std::dynamic_pointer_cast( - obj); + std::dynamic_pointer_cast(obj); ASSERTMSG_( sd->navigator.config_.targetApproachController, mrpt::format( "Class '%s' seems not to implement the expected interface " - "'selfdriving::TargetApproachController'", + "'mpp::TargetApproachController'", name.c_str())); sd->navigator.config_.targetApproachController->setMinLoggingLevel( @@ -317,7 +316,7 @@ void prepare_selfdriving(mvsim::World& world) if (arg_planner_yaml_file.isSet()) { sd->navigator.config_.plannerParams = - selfdriving::TPS_Astar_Parameters::FromYAML( + mpp::TPS_Astar_Parameters::FromYAML( mrpt::containers::yaml::FromFile( arg_planner_yaml_file.getValue())); } @@ -325,7 +324,7 @@ void prepare_selfdriving(mvsim::World& world) if (arg_cost_global_yaml_file.isSet()) { sd->navigator.config_.globalCostParameters = - selfdriving::CostEvaluatorCostMap::Parameters::FromYAML( + mpp::CostEvaluatorCostMap::Parameters::FromYAML( mrpt::containers::yaml::FromFile( arg_cost_global_yaml_file.getValue())); } @@ -333,7 +332,7 @@ void prepare_selfdriving(mvsim::World& world) if (arg_cost_local_yaml_file.isSet()) { sd->navigator.config_.localCostParameters = - selfdriving::CostEvaluatorCostMap::Parameters::FromYAML( + mpp::CostEvaluatorCostMap::Parameters::FromYAML( mrpt::containers::yaml::FromFile( arg_cost_local_yaml_file.getValue())); } @@ -341,7 +340,7 @@ void prepare_selfdriving(mvsim::World& world) if (arg_cost_prefer_waypoints_yaml_file.isSet()) { sd->navigator.config_.preferWaypointsParameters = - selfdriving::CostEvaluatorPreferredWaypoint::Parameters::FromYAML( + mpp::CostEvaluatorPreferredWaypoint::Parameters::FromYAML( mrpt::containers::yaml::FromFile( arg_cost_prefer_waypoints_yaml_file.getValue())); } @@ -362,8 +361,8 @@ void prepare_selfdriving(mvsim::World& world) // -------------------------------------------------------- if (arg_waypoints_yaml_file.isSet()) { - sd->waypts = selfdriving::WaypointSequence::FromYAML( - mrpt::containers::yaml::FromFile( + sd->waypts = + mpp::WaypointSequence::FromYAML(mrpt::containers::yaml::FromFile( arg_waypoints_yaml_file.getValue())); } } @@ -625,7 +624,7 @@ void prepare_selfdriving_window( auto glWaypoints = mrpt::opengl::CSetOfObjects::Create(); glWaypoints->setLocation(0, 0, 0.01); glWaypoints->setName("glWaypoints"); - selfdriving::WaypointsRenderingParams rp; + mpp::WaypointsRenderingParams rp; // rp.xx = x; sd->waypts.getAsOpenglVisualization(*glWaypoints, rp); @@ -644,7 +643,7 @@ void prepare_selfdriving_window( // Update global obstacles, in case the MVSIM world has changed: auto obsPts = world_to_static_obstacle_points(*world); sd->navigator.config_.globalMapObstacleSource = - selfdriving::ObstacleSource::FromStaticPointcloud(obsPts); + mpp::ObstacleSource::FromStaticPointcloud(obsPts); sd->navigator.request_navigation(sd->waypts); }); @@ -663,7 +662,7 @@ void prepare_selfdriving_window( const auto state = sd->navigator.current_status(); lbNavStatus->setCaption(mrpt::format( "Nav status: %s", - mrpt::typemeta::TEnumType::value2name(state) + mrpt::typemeta::TEnumType::value2name(state) .c_str())); }; @@ -671,7 +670,7 @@ void prepare_selfdriving_window( // Single A* planner tab // ------------------------------- { - const selfdriving::SE2_KinState dummyState; + const mpp::SE2_KinState dummyState; auto pnPlanner = wrappers.at(2); pnPlanner->add("Start pose:"); @@ -736,13 +735,13 @@ void prepare_selfdriving_window( btnDoPlan->setCallback([=]() { try { - selfdriving::SE2_KinState stateStart; + mpp::SE2_KinState stateStart; stateStart.pose.fromString(edStateStartPose->value()); stateStart.vel.fromString(edStateStartVel->value()); - selfdriving::SE2orR2_KinState stateGoal; - stateGoal.state = selfdriving::PoseOrPoint::FromString( - edStateGoalPose->value()); + mpp::SE2orR2_KinState stateGoal; + stateGoal.state = + mpp::PoseOrPoint::FromString(edStateGoalPose->value()); stateGoal.vel.fromString(edStateGoalVel->value()); on_do_single_path_planning(*world, stateStart, stateGoal); @@ -811,15 +810,14 @@ void prepare_selfdriving_window( if (!sd->navigator.config_.localSensedObstacleSource) sd->navigator.config_.localSensedObstacleSource = - std::make_shared(); + std::make_shared(); - auto o = - std::dynamic_pointer_cast( - sd->navigator.config_.localSensedObstacleSource); + auto o = std::dynamic_pointer_cast( + sd->navigator.config_.localSensedObstacleSource); if (!o) return; // handle sensor sources: - if (auto d = std::dynamic_pointer_cast( + if (auto d = std::dynamic_pointer_cast( sd->navigator.config_.vehicleMotionInterface); d) { @@ -906,17 +904,17 @@ void selfdriving_run_thread(SelfDrivingThreadParams& params) } void on_do_single_path_planning( - mvsim::World& world, const selfdriving::SE2_KinState& stateStart, - const selfdriving::SE2orR2_KinState& stateGoal) + mvsim::World& world, const mpp::SE2_KinState& stateStart, + const mpp::SE2orR2_KinState& stateGoal) { - selfdriving::TPS_Astar planner; - selfdriving::PlannerInput pi; + mpp::TPS_Astar planner; + mpp::PlannerInput pi; // ############################ // BEGIN: Run path planning // ############################ auto obsPts = world_to_static_obstacle_points(world); - auto obstacles = selfdriving::ObstacleSource::FromStaticPointcloud(obsPts); + auto obstacles = mpp::ObstacleSource::FromStaticPointcloud(obsPts); pi.stateStart = stateStart; pi.stateGoal = stateGoal; @@ -954,7 +952,7 @@ void on_do_single_path_planning( planner.costEvaluators_.clear(); - selfdriving::CostEvaluatorCostMap::Parameters cmP; + mpp::CostEvaluatorCostMap::Parameters cmP; cmP.resolution = 0.05; cmP.preferredClearanceDistance = 1.0; // [m] @@ -962,7 +960,7 @@ void on_do_single_path_planning( if (!obsPts->empty()) { auto staticCostmap = - selfdriving::CostEvaluatorCostMap::FromStaticPointObstacles( + mpp::CostEvaluatorCostMap::FromStaticPointObstacles( *obsPts, cmP, pi.stateStart.pose); planner.costEvaluators_.push_back(staticCostmap); @@ -976,7 +974,7 @@ void on_do_single_path_planning( if (!obs->empty()) { auto lidarCostmap = - selfdriving::CostEvaluatorCostMap::FromStaticPointObstacles( + mpp::CostEvaluatorCostMap::FromStaticPointObstacles( *obs, cmP, pi.stateStart.pose); planner.costEvaluators_.push_back(lidarCostmap); @@ -1000,10 +998,10 @@ void on_do_single_path_planning( mrpt::config::CConfigFile cfg(arg_ptgs_file.getValue()); pi.ptgs.initFromConfigFile(cfg, arg_config_file_section.getValue()); - const selfdriving::PlannerOutput plan = planner.plan(pi); + const mpp::PlannerOutput plan = planner.plan(pi); // Visualize: - selfdriving::NavEngine::PathPlannerOutput ppo; + mpp::NavEngine::PathPlannerOutput ppo; ppo.po = plan; ppo.costEvaluators = planner.costEvaluators_; diff --git a/cmake/misc_cmake_functions.cmake b/cmake/misc_cmake_functions.cmake index 592ead6..6d416fd 100644 --- a/cmake/misc_cmake_functions.cmake +++ b/cmake/misc_cmake_functions.cmake @@ -175,7 +175,7 @@ function(selfdriving_configure_library TARGETNAME) # make project importable from build_dir: export( TARGETS ${TARGETNAME} - NAMESPACE selfdriving:: + NAMESPACE mpp:: # export to ROOT cmake directory (when building MOLA as a superproject) FILE ${CMAKE_BINARY_DIR}/${TARGETNAME}-targets.cmake ) @@ -183,7 +183,7 @@ function(selfdriving_configure_library TARGETNAME) set(ALL_DEPS_LIST ${ARGN}) # used in xxx-config.cmake.in set(SELFDRIVING_MODULE_NAME ${TARGETNAME}) configure_file( - "${SelfdrivingProject_SOURCE_DIR}/cmake/template-config.cmake.in" + "${MRPTPathPlanningProject_SOURCE_DIR}/cmake/template-config.cmake.in" "${CMAKE_BINARY_DIR}/${TARGETNAME}-config.cmake" IMMEDIATE @ONLY ) # Version file: diff --git a/cmake/script_version_number.cmake b/cmake/script_version_number.cmake index 4d7de50..0af47e9 100644 --- a/cmake/script_version_number.cmake +++ b/cmake/script_version_number.cmake @@ -3,16 +3,16 @@ # Example line:" 0.3.2" file(READ package.xml contentPackageXML) string(REGEX MATCH "([0-9\.]*)" _ ${contentPackageXML}) -set(SELFDRIVING_VERSION ${CMAKE_MATCH_1}) -message(STATUS "${PROJECT_NAME} version: ${SELFDRIVING_VERSION} (detected in package.xml)") -string(REGEX MATCH "^([0-9]+)\\.([0-9]+)\\.([0-9]+)" _ ${SELFDRIVING_VERSION}) +set(MRPT_PATH_PLANNING_VERSION ${CMAKE_MATCH_1}) +message(STATUS "${PROJECT_NAME} version: ${MRPT_PATH_PLANNING_VERSION} (detected in package.xml)") +string(REGEX MATCH "^([0-9]+)\\.([0-9]+)\\.([0-9]+)" _ ${MRPT_PATH_PLANNING_VERSION}) set(SELFDRIVING_MAJOR_VERSION ${CMAKE_MATCH_1}) set(SELFDRIVING_MINOR_VERSION ${CMAKE_MATCH_2}) set(SELFDRIVING_PATCH_VERSION ${CMAKE_MATCH_3}) -file(MAKE_DIRECTORY ${SelfdrivingProject_BINARY_DIR}/include/selfdriving/) +file(MAKE_DIRECTORY ${MRPTPathPlanningProject_BINARY_DIR}/include/mpp/) configure_file( cmake/version.h.in - ${SelfdrivingProject_BINARY_DIR}/include/selfdriving/version.h + ${MRPTPathPlanningProject_BINARY_DIR}/include/mpp/version.h @ONLY ) diff --git a/cmake/version.h.in b/cmake/version.h.in index 79b93dd..8b2f6a1 100644 --- a/cmake/version.h.in +++ b/cmake/version.h.in @@ -13,6 +13,6 @@ #define SELFDRIVING_STR_EXP(__A) #__A #define SELFDRIVING_STR(__A) SELFDRIVING_STR_EXP(__A) -#define SELFDRIVING_VERSION SELFDRIVING_STR(SELFDRIVING_MAJOR_VERSION) "." SELFDRIVING_STR(SELFDRIVING_MINOR_VERSION) "." SELFDRIVING_STR(SELFDRIVING_PATCH_VERSION) +#define MRPT_PATH_PLANNING_VERSION SELFDRIVING_STR(SELFDRIVING_MAJOR_VERSION) "." SELFDRIVING_STR(SELFDRIVING_MINOR_VERSION) "." SELFDRIVING_STR(SELFDRIVING_PATCH_VERSION) // clang-format on diff --git a/doc/Doxyfile b/doc/Doxyfile index a8bf255..fb9451c 100644 --- a/doc/Doxyfile +++ b/doc/Doxyfile @@ -161,7 +161,7 @@ STRIP_FROM_PATH = # specify the list of include paths that are normally passed to the compiler # using the -I flag. -STRIP_FROM_INC_PATH = ../libselfdriving/include +STRIP_FROM_INC_PATH = ../mrpt_path_planning/include # If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter (but # less readable) file names. This can be useful is your file systems doesn't @@ -780,7 +780,7 @@ WARN_LOGFILE = # spaces. See also FILE_PATTERNS and EXTENSION_MAPPING # Note: If this tag is empty the current directory is searched. -INPUT = ../libselfdriving/include +INPUT = ../mrpt_path_planning/include INPUT += . # This tag can be used to specify the character encoding of the source files diff --git a/doc/overview.md b/doc/overview.md index c71a3ff..b4957f7 100644 --- a/doc/overview.md +++ b/doc/overview.md @@ -4,4 +4,4 @@ Write me! ![selfdriving-architecture](selfdriving-architecture.png) -selfdriving::TPS_Astar: Implements the A* algorithm itself. +mpp::TPS_Astar: Implements the A* algorithm itself. diff --git a/libselfdriving/CMakeLists.txt b/mrpt_path_planning/CMakeLists.txt similarity index 85% rename from libselfdriving/CMakeLists.txt rename to mrpt_path_planning/CMakeLists.txt index ed15deb..8dd76c8 100644 --- a/libselfdriving/CMakeLists.txt +++ b/mrpt_path_planning/CMakeLists.txt @@ -1,4 +1,4 @@ -project(selfdriving LANGUAGES CXX) +project(mrpt_path_planning LANGUAGES CXX) file(GLOB_RECURSE LIB_SRCS src/*.cpp src/*.h) file(GLOB_RECURSE LIB_PUBLIC_HDRS include/*.h) @@ -6,7 +6,7 @@ file(GLOB_RECURSE LIB_PUBLIC_HDRS include/*.h) # define lib: selfdriving_add_library( TARGET - selfdriving + mrpt_path_planning SOURCES ${LIB_SRCS} ${LIB_PUBLIC_HDRS} PUBLIC_LINK_LIBRARIES diff --git a/libselfdriving/include/selfdriving/algos/CostEvaluator.h b/mrpt_path_planning/include/mpp/algos/CostEvaluator.h similarity index 89% rename from libselfdriving/include/selfdriving/algos/CostEvaluator.h rename to mrpt_path_planning/include/mpp/algos/CostEvaluator.h index 8badc47..54a3ad4 100644 --- a/libselfdriving/include/selfdriving/algos/CostEvaluator.h +++ b/mrpt_path_planning/include/mpp/algos/CostEvaluator.h @@ -6,13 +6,13 @@ #pragma once +#include #include #include -#include #include -namespace selfdriving +namespace mpp { class CostEvaluator : public mrpt::rtti::CObject { @@ -29,4 +29,4 @@ class CostEvaluator : public mrpt::rtti::CObject virtual mrpt::opengl::CSetOfObjects::Ptr get_visualization() const; }; -} // namespace selfdriving +} // namespace mpp diff --git a/libselfdriving/include/selfdriving/algos/CostEvaluatorCostMap.h b/mrpt_path_planning/include/mpp/algos/CostEvaluatorCostMap.h similarity index 93% rename from libselfdriving/include/selfdriving/algos/CostEvaluatorCostMap.h rename to mrpt_path_planning/include/mpp/algos/CostEvaluatorCostMap.h index 32659dc..0c1ed1e 100644 --- a/libselfdriving/include/selfdriving/algos/CostEvaluatorCostMap.h +++ b/mrpt_path_planning/include/mpp/algos/CostEvaluatorCostMap.h @@ -6,18 +6,18 @@ #pragma once +#include #include #include -#include -namespace selfdriving +namespace mpp { /** Defines higher costs to paths that pass closer to obstacles. * */ class CostEvaluatorCostMap : public CostEvaluator { - DEFINE_MRPT_OBJECT(CostEvaluatorCostMap, selfdriving) + DEFINE_MRPT_OBJECT(CostEvaluatorCostMap, mpp) public: CostEvaluatorCostMap() = default; @@ -73,4 +73,4 @@ class CostEvaluatorCostMap : public CostEvaluator double eval_single_pose(const mrpt::math::TPose2D& p) const; }; -} // namespace selfdriving +} // namespace mpp diff --git a/libselfdriving/include/selfdriving/algos/CostEvaluatorPreferredWaypoint.h b/mrpt_path_planning/include/mpp/algos/CostEvaluatorPreferredWaypoint.h similarity index 92% rename from libselfdriving/include/selfdriving/algos/CostEvaluatorPreferredWaypoint.h rename to mrpt_path_planning/include/mpp/algos/CostEvaluatorPreferredWaypoint.h index df98679..74064e7 100644 --- a/libselfdriving/include/selfdriving/algos/CostEvaluatorPreferredWaypoint.h +++ b/mrpt_path_planning/include/mpp/algos/CostEvaluatorPreferredWaypoint.h @@ -6,18 +6,18 @@ #pragma once +#include #include #include -#include -namespace selfdriving +namespace mpp { /** Defines lower (negative) costs to paths that pass closer to one or more * "preferred waypoints" defined by 2D coordinates (x,y) */ class CostEvaluatorPreferredWaypoint : public CostEvaluator { - DEFINE_MRPT_OBJECT(CostEvaluatorPreferredWaypoint, selfdriving) + DEFINE_MRPT_OBJECT(CostEvaluatorPreferredWaypoint, mpp) public: CostEvaluatorPreferredWaypoint() = default; @@ -63,4 +63,4 @@ class CostEvaluatorPreferredWaypoint : public CostEvaluator mrpt::maps::CSimplePointsMap waypoints_; }; -} // namespace selfdriving +} // namespace mpp diff --git a/libselfdriving/include/selfdriving/algos/NavEngine.h b/mrpt_path_planning/include/mpp/algos/NavEngine.h similarity index 95% rename from libselfdriving/include/selfdriving/algos/NavEngine.h rename to mrpt_path_planning/include/mpp/algos/NavEngine.h index a520156..4dd03aa 100644 --- a/libselfdriving/include/selfdriving/algos/NavEngine.h +++ b/mrpt_path_planning/include/mpp/algos/NavEngine.h @@ -6,27 +6,27 @@ #pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include #include #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include #include -namespace selfdriving +namespace mpp { using namespace mrpt; @@ -261,7 +261,7 @@ class NavEngine : public mrpt::system::COutputLogger { PathPlannerOutput() = default; - selfdriving::PlannerOutput po; + mpp::PlannerOutput po; /// A copy of the employed costs. std::vector costEvaluators; @@ -352,7 +352,7 @@ class NavEngine : public mrpt::system::COutputLogger { PathPlannerInput() = default; - selfdriving::PlannerInput pi; + mpp::PlannerInput pi; /** If this is path refining plan request, this is the ID of the node * that acts as starting state, with the ID in the current activePlan. @@ -541,9 +541,8 @@ class NavEngine : public mrpt::system::COutputLogger * in activePlanOutput, activePlanPath, activePlanPathEdges. */ void enqueue_path_planner_towards( - const waypoint_idx_t target, - const selfdriving::SE2_KinState& startingFrom, - const std::optional& startingFromNodeID = std::nullopt); + const waypoint_idx_t target, const mpp::SE2_KinState& startingFrom, + const std::optional& startingFromNodeID = std::nullopt); /** Special behavior: if we are about to reach a WP with a stop condition, * handle it specially if there's an obvious free path towards it. @@ -592,11 +591,11 @@ class NavEngine : public mrpt::system::COutputLogger #endif }; -} // namespace selfdriving +} // namespace mpp -MRPT_ENUM_TYPE_BEGIN(selfdriving::NavStatus) -MRPT_FILL_ENUM_MEMBER(selfdriving, NavStatus::IDLE); -MRPT_FILL_ENUM_MEMBER(selfdriving, NavStatus::NAVIGATING); -MRPT_FILL_ENUM_MEMBER(selfdriving, NavStatus::SUSPENDED); -MRPT_FILL_ENUM_MEMBER(selfdriving, NavStatus::NAV_ERROR); +MRPT_ENUM_TYPE_BEGIN(mpp::NavStatus) +MRPT_FILL_ENUM_MEMBER(mpp, NavStatus::IDLE); +MRPT_FILL_ENUM_MEMBER(mpp, NavStatus::NAVIGATING); +MRPT_FILL_ENUM_MEMBER(mpp, NavStatus::SUSPENDED); +MRPT_FILL_ENUM_MEMBER(mpp, NavStatus::NAV_ERROR); MRPT_ENUM_TYPE_END() diff --git a/libselfdriving/include/selfdriving/algos/Planner.h b/mrpt_path_planning/include/mpp/algos/Planner.h similarity index 87% rename from libselfdriving/include/selfdriving/algos/Planner.h rename to mrpt_path_planning/include/mpp/algos/Planner.h index 4da2731..ee0b755 100644 --- a/libselfdriving/include/selfdriving/algos/Planner.h +++ b/mrpt_path_planning/include/mpp/algos/Planner.h @@ -6,18 +6,18 @@ #pragma once +#include +#include +#include +#include #include #include #include -#include -#include -#include -#include #include #include -namespace selfdriving +namespace mpp { class Planner : public mrpt::rtti::CObject, virtual public mrpt::system::COutputLogger @@ -56,4 +56,4 @@ class Planner : public mrpt::rtti::CObject, mrpt::system::CTimeLogger* customProfiler_ = nullptr; }; -} // namespace selfdriving +} // namespace mpp diff --git a/libselfdriving/include/selfdriving/algos/TPS_Astar.h b/mrpt_path_planning/include/mpp/algos/TPS_Astar.h similarity index 96% rename from libselfdriving/include/selfdriving/algos/TPS_Astar.h rename to mrpt_path_planning/include/mpp/algos/TPS_Astar.h index 2a078ea..f3b64de 100644 --- a/libselfdriving/include/selfdriving/algos/TPS_Astar.h +++ b/mrpt_path_planning/include/mpp/algos/TPS_Astar.h @@ -6,18 +6,18 @@ #pragma once +#include +#include +#include #include // 0.0_deg #include #include #include -#include -#include -#include #include #include -namespace selfdriving +namespace mpp { using astar_heuristic_t = std::function; @@ -64,7 +64,7 @@ struct TPS_Astar_Parameters */ class TPS_Astar : virtual public mrpt::system::COutputLogger, public Planner { - DEFINE_MRPT_OBJECT(TPS_Astar, selfdriving) + DEFINE_MRPT_OBJECT(TPS_Astar, mpp) public: TPS_Astar(); @@ -232,7 +232,7 @@ class TPS_Astar : virtual public mrpt::system::COutputLogger, public Planner /// throws on out of grid limits. /// Returns a ref to the node. Node& getOrCreateNodeByPose( - const selfdriving::SE2_KinState& p, mrpt::graphs::TNodeID& nextFreeId) + const mpp::SE2_KinState& p, mrpt::graphs::TNodeID& nextFreeId) { Node& n = *grid_.getByPos(p.pose.x, p.pose.y, p.pose.phi); if (!n.id.has_value()) @@ -316,4 +316,4 @@ class TPS_Astar : virtual public mrpt::system::COutputLogger, public Planner double MAX_PTG_XY_DIST); }; -} // namespace selfdriving +} // namespace mpp diff --git a/libselfdriving/include/selfdriving/algos/bestTrajectory.h b/mrpt_path_planning/include/mpp/algos/bestTrajectory.h similarity index 82% rename from libselfdriving/include/selfdriving/algos/bestTrajectory.h rename to mrpt_path_planning/include/mpp/algos/bestTrajectory.h index 9d87144..6cf6fb1 100644 --- a/libselfdriving/include/selfdriving/algos/bestTrajectory.h +++ b/mrpt_path_planning/include/mpp/algos/bestTrajectory.h @@ -6,13 +6,13 @@ #pragma once +#include +#include #include -#include -#include #include -namespace selfdriving +namespace mpp { /** Finds the best trajectory between two kinematic states, given the set of * feasible trajectories. @@ -22,4 +22,4 @@ bool bestTrajectory( MoveEdgeSE2_TPS& npa, const TrajectoriesAndRobotShape& trs, std::optional logger = std::nullopt); -} // namespace selfdriving +} // namespace mpp diff --git a/libselfdriving/include/selfdriving/algos/edge_interpolated_path.h b/mrpt_path_planning/include/mpp/algos/edge_interpolated_path.h similarity index 84% rename from libselfdriving/include/selfdriving/algos/edge_interpolated_path.h rename to mrpt_path_planning/include/mpp/algos/edge_interpolated_path.h index d4fe104..8eb9acb 100644 --- a/libselfdriving/include/selfdriving/algos/edge_interpolated_path.h +++ b/mrpt_path_planning/include/mpp/algos/edge_interpolated_path.h @@ -4,12 +4,12 @@ * See LICENSE for license information. * ------------------------------------------------------------------------- */ -#include -#include +#include +#include #include -namespace selfdriving +namespace mpp { void edge_interpolated_path( MoveEdgeSE2_TPS& edge, const TrajectoriesAndRobotShape& trs, diff --git a/libselfdriving/include/selfdriving/algos/refine_trajectory.h b/mrpt_path_planning/include/mpp/algos/refine_trajectory.h similarity index 86% rename from libselfdriving/include/selfdriving/algos/refine_trajectory.h rename to mrpt_path_planning/include/mpp/algos/refine_trajectory.h index 0eaaf4f..0079c6e 100644 --- a/libselfdriving/include/selfdriving/algos/refine_trajectory.h +++ b/mrpt_path_planning/include/mpp/algos/refine_trajectory.h @@ -4,10 +4,10 @@ * See LICENSE for license information. * ------------------------------------------------------------------------- */ -#include -#include +#include +#include -namespace selfdriving +namespace mpp { /** * Takes a sequence of N states (the inPath) and the N-1 edges in between @@ -25,4 +25,4 @@ void refine_trajectory( std::vector& edgesToRefine, const TrajectoriesAndRobotShape& ptgInfo); -} // namespace selfdriving +} // namespace mpp diff --git a/libselfdriving/include/selfdriving/algos/render_tree.h b/mrpt_path_planning/include/mpp/algos/render_tree.h similarity index 76% rename from libselfdriving/include/selfdriving/algos/render_tree.h rename to mrpt_path_planning/include/mpp/algos/render_tree.h index 82b6e0c..e7f1b64 100644 --- a/libselfdriving/include/selfdriving/algos/render_tree.h +++ b/mrpt_path_planning/include/mpp/algos/render_tree.h @@ -6,18 +6,18 @@ #pragma once +#include +#include +#include #include -#include -#include -#include #include -namespace selfdriving +namespace mpp { /** Gets a CSetOfObjects::Ptr visual representation of a motion tree */ auto render_tree( const MotionPrimitivesTreeSE2& tree, const PlannerInput& pi, const RenderOptions& ro) -> std::shared_ptr; -} // namespace selfdriving +} // namespace mpp diff --git a/libselfdriving/include/selfdriving/algos/render_vehicle.h b/mrpt_path_planning/include/mpp/algos/render_vehicle.h similarity index 88% rename from libselfdriving/include/selfdriving/algos/render_vehicle.h rename to mrpt_path_planning/include/mpp/algos/render_vehicle.h index 1ed9cee..ddfe414 100644 --- a/libselfdriving/include/selfdriving/algos/render_vehicle.h +++ b/mrpt_path_planning/include/mpp/algos/render_vehicle.h @@ -6,12 +6,12 @@ #pragma once +#include #include -#include #include -namespace selfdriving +namespace mpp { struct RenderVehicleParams { @@ -32,4 +32,4 @@ auto render_vehicle( const RobotShape& rs, mrpt::opengl::CSetOfLines& outPolygon, const RenderVehicleParams& rvp = {}) -> RenderVehicleExtraResults; -} // namespace selfdriving +} // namespace mpp diff --git a/libselfdriving/include/selfdriving/algos/tp_obstacles_single_path.h b/mrpt_path_planning/include/mpp/algos/tp_obstacles_single_path.h similarity index 82% rename from libselfdriving/include/selfdriving/algos/tp_obstacles_single_path.h rename to mrpt_path_planning/include/mpp/algos/tp_obstacles_single_path.h index aa0cc19..c12c393 100644 --- a/libselfdriving/include/selfdriving/algos/tp_obstacles_single_path.h +++ b/mrpt_path_planning/include/mpp/algos/tp_obstacles_single_path.h @@ -6,10 +6,10 @@ #pragma once +#include // TODO: refactor in smaller headers? #include -#include // TODO: refactor in smaller headers? -namespace selfdriving +namespace mpp { /** Returns TPS-distance (pseudometers, not normalized) to obstacles. * ptg dynamic state must be updated by the caller. @@ -18,4 +18,4 @@ distance_t tp_obstacles_single_path( const trajectory_index_t tp_space_k_direction, const mrpt::maps::CPointsMap& localObstacles, const ptg_t& ptg); -} // namespace selfdriving +} // namespace mpp diff --git a/libselfdriving/include/selfdriving/algos/trajectories.h b/mrpt_path_planning/include/mpp/algos/trajectories.h similarity index 76% rename from libselfdriving/include/selfdriving/algos/trajectories.h rename to mrpt_path_planning/include/mpp/algos/trajectories.h index bc04551..27a40b5 100644 --- a/libselfdriving/include/selfdriving/algos/trajectories.h +++ b/mrpt_path_planning/include/mpp/algos/trajectories.h @@ -6,13 +6,13 @@ #pragma once -#include -#include -#include +#include +#include +#include #include -namespace selfdriving +namespace mpp { trajectory_t plan_to_trajectory( const MotionPrimitivesTreeSE2::edge_sequence_t& planEdges, @@ -21,4 +21,4 @@ trajectory_t plan_to_trajectory( bool save_to_txt(const trajectory_t& traj, const std::string& fileName); -} // namespace selfdriving +} // namespace mpp diff --git a/libselfdriving/include/selfdriving/algos/transform_pc_square_clipping.h b/mrpt_path_planning/include/mpp/algos/transform_pc_square_clipping.h similarity index 93% rename from libselfdriving/include/selfdriving/algos/transform_pc_square_clipping.h rename to mrpt_path_planning/include/mpp/algos/transform_pc_square_clipping.h index 2a81683..df61c7c 100644 --- a/libselfdriving/include/selfdriving/algos/transform_pc_square_clipping.h +++ b/mrpt_path_planning/include/mpp/algos/transform_pc_square_clipping.h @@ -9,7 +9,7 @@ #include #include -namespace selfdriving +namespace mpp { /** Returns local obstacles as seen from a given pose, clipped to a maximum * distance. */ @@ -18,4 +18,4 @@ void transform_pc_square_clipping( const double MAX_DIST_XY, mrpt::maps::CPointsMap& outMap, bool appendToOutMap = true); -} // namespace selfdriving +} // namespace mpp diff --git a/libselfdriving/include/selfdriving/algos/viz.h b/mrpt_path_planning/include/mpp/algos/viz.h similarity index 73% rename from libselfdriving/include/selfdriving/algos/viz.h rename to mrpt_path_planning/include/mpp/algos/viz.h index fc2cfbe..443e6be 100644 --- a/libselfdriving/include/selfdriving/algos/viz.h +++ b/mrpt_path_planning/include/mpp/algos/viz.h @@ -6,12 +6,12 @@ #pragma once -#include -#include -#include -#include +#include +#include +#include +#include -namespace selfdriving +namespace mpp { struct VisualizationOptions { @@ -26,8 +26,8 @@ void viz_nav_plan( const std::vector costEvaluators = {}); void viz_nav_plan_animation( - const PlannerOutput& plan, const selfdriving::trajectory_t& traj, + const PlannerOutput& plan, const mpp::trajectory_t& traj, const RenderOptions& opts = {}, const std::vector costEvaluators = {}); -} // namespace selfdriving +} // namespace mpp diff --git a/libselfdriving/include/selfdriving/algos/within_bbox.h b/mrpt_path_planning/include/mpp/algos/within_bbox.h similarity index 94% rename from libselfdriving/include/selfdriving/algos/within_bbox.h rename to mrpt_path_planning/include/mpp/algos/within_bbox.h index 4621357..569381a 100644 --- a/libselfdriving/include/selfdriving/algos/within_bbox.h +++ b/mrpt_path_planning/include/mpp/algos/within_bbox.h @@ -8,7 +8,7 @@ #include -namespace selfdriving +namespace mpp { static inline bool within_bbox( const mrpt::math::TPose2D& p, const mrpt::math::TPose2D& max, @@ -26,4 +26,4 @@ static inline bool within_bbox( p.x > min.x && p.y > min.y; } -} // namespace selfdriving +} // namespace mpp diff --git a/libselfdriving/include/selfdriving/data/EnqueuedMotionCmd.h b/mrpt_path_planning/include/mpp/data/EnqueuedMotionCmd.h similarity index 96% rename from libselfdriving/include/selfdriving/data/EnqueuedMotionCmd.h rename to mrpt_path_planning/include/mpp/data/EnqueuedMotionCmd.h index e66d33e..d7b5bbe 100644 --- a/libselfdriving/include/selfdriving/data/EnqueuedMotionCmd.h +++ b/mrpt_path_planning/include/mpp/data/EnqueuedMotionCmd.h @@ -9,7 +9,7 @@ #include #include -namespace selfdriving +namespace mpp { /** An odometry position condition used in EnqueuedMotionCmd */ struct EnqueuedCondition @@ -46,4 +46,4 @@ struct EnqueuedMotionCmd EnqueuedCondition nextCondition; }; -} // namespace selfdriving +} // namespace mpp diff --git a/libselfdriving/include/selfdriving/data/MotionPrimitivesTree.h b/mrpt_path_planning/include/mpp/data/MotionPrimitivesTree.h similarity index 98% rename from libselfdriving/include/selfdriving/data/MotionPrimitivesTree.h rename to mrpt_path_planning/include/mpp/data/MotionPrimitivesTree.h index 2cd367a..5720fce 100644 --- a/libselfdriving/include/selfdriving/data/MotionPrimitivesTree.h +++ b/mrpt_path_planning/include/mpp/data/MotionPrimitivesTree.h @@ -6,16 +6,16 @@ #pragma once +#include +#include +#include +#include #include #include #include #include #include #include -#include -#include -#include -#include #include #include @@ -24,7 +24,7 @@ #include #include -namespace selfdriving +namespace mpp { using mrpt::graphs::TNodeID; @@ -416,4 +416,4 @@ using MotionPrimitivesTreeSE2 = MotionPrimitivesTree; /** @} */ -} // namespace selfdriving +} // namespace mpp diff --git a/libselfdriving/include/selfdriving/data/MoveEdgeSE2_TPS.h b/mrpt_path_planning/include/mpp/data/MoveEdgeSE2_TPS.h similarity index 92% rename from libselfdriving/include/selfdriving/data/MoveEdgeSE2_TPS.h rename to mrpt_path_planning/include/mpp/data/MoveEdgeSE2_TPS.h index 6392673..d0dece3 100644 --- a/libselfdriving/include/selfdriving/data/MoveEdgeSE2_TPS.h +++ b/mrpt_path_planning/include/mpp/data/MoveEdgeSE2_TPS.h @@ -6,14 +6,14 @@ #pragma once +#include +#include +#include #include -#include -#include -#include #include -namespace selfdriving +namespace mpp { /** An edge for the move tree used for planning in SE2 and TP-space */ struct MoveEdgeSE2_TPS @@ -62,4 +62,4 @@ struct MoveEdgeSE2_TPS std::string asString() const; }; -} // namespace selfdriving +} // namespace mpp diff --git a/libselfdriving/include/selfdriving/data/PlannerInput.h b/mrpt_path_planning/include/mpp/data/PlannerInput.h similarity index 76% rename from libselfdriving/include/selfdriving/data/PlannerInput.h rename to mrpt_path_planning/include/mpp/data/PlannerInput.h index b233bea..e2fda74 100644 --- a/libselfdriving/include/selfdriving/data/PlannerInput.h +++ b/mrpt_path_planning/include/mpp/data/PlannerInput.h @@ -6,14 +6,14 @@ #pragma once +#include +#include +#include #include -#include -#include -#include #include -namespace selfdriving +namespace mpp { struct PlannerInput { @@ -24,4 +24,4 @@ struct PlannerInput TrajectoriesAndRobotShape ptgs; }; -} // namespace selfdriving +} // namespace mpp diff --git a/libselfdriving/include/selfdriving/data/PlannerOutput.h b/mrpt_path_planning/include/mpp/data/PlannerOutput.h similarity index 91% rename from libselfdriving/include/selfdriving/data/PlannerOutput.h rename to mrpt_path_planning/include/mpp/data/PlannerOutput.h index 3d70d5d..43e1e79 100644 --- a/libselfdriving/include/selfdriving/data/PlannerOutput.h +++ b/mrpt_path_planning/include/mpp/data/PlannerOutput.h @@ -6,14 +6,14 @@ #pragma once +#include +#include #include -#include -#include #include #include -namespace selfdriving +namespace mpp { /** The output of the path planner */ struct PlannerOutput @@ -53,4 +53,4 @@ struct PlannerOutput MotionPrimitivesTreeSE2 motionTree; }; -} // namespace selfdriving +} // namespace mpp diff --git a/libselfdriving/include/selfdriving/data/ProgressCallbackData.h b/mrpt_path_planning/include/mpp/data/ProgressCallbackData.h similarity index 81% rename from libselfdriving/include/selfdriving/data/ProgressCallbackData.h rename to mrpt_path_planning/include/mpp/data/ProgressCallbackData.h index 184c458..c2859b0 100644 --- a/libselfdriving/include/selfdriving/data/ProgressCallbackData.h +++ b/mrpt_path_planning/include/mpp/data/ProgressCallbackData.h @@ -6,15 +6,15 @@ #pragma once -#include -#include -#include -#include +#include +#include +#include +#include #include #include -namespace selfdriving +namespace mpp { struct ProgressCallbackData { @@ -33,4 +33,4 @@ struct ProgressCallbackData using planner_progress_callback_t = std::function; -} // namespace selfdriving +} // namespace mpp diff --git a/libselfdriving/include/selfdriving/data/RenderOptions.h b/mrpt_path_planning/include/mpp/data/RenderOptions.h similarity index 98% rename from libselfdriving/include/selfdriving/data/RenderOptions.h rename to mrpt_path_planning/include/mpp/data/RenderOptions.h index e8c26bd..fcade4b 100644 --- a/libselfdriving/include/selfdriving/data/RenderOptions.h +++ b/mrpt_path_planning/include/mpp/data/RenderOptions.h @@ -15,7 +15,7 @@ #include #include -namespace selfdriving +namespace mpp { /** Options for render_tree() */ struct RenderOptions @@ -85,4 +85,4 @@ struct RenderOptions double log_msg_scale = 0.2; }; -} // namespace selfdriving +} // namespace mpp diff --git a/libselfdriving/include/selfdriving/data/SE2_KinState.h b/mrpt_path_planning/include/mpp/data/SE2_KinState.h similarity index 98% rename from libselfdriving/include/selfdriving/data/SE2_KinState.h rename to mrpt_path_planning/include/mpp/data/SE2_KinState.h index 2cd899f..96644a0 100644 --- a/libselfdriving/include/selfdriving/data/SE2_KinState.h +++ b/mrpt_path_planning/include/mpp/data/SE2_KinState.h @@ -16,7 +16,7 @@ #include // _deg literal #include -namespace selfdriving +namespace mpp { #if MRPT_VERSION >= 0x258 using namespace mrpt::literals; // "_deg" literal @@ -86,4 +86,4 @@ struct SE2orR2_KinState std::string asString() const; }; -} // namespace selfdriving +} // namespace mpp diff --git a/libselfdriving/include/selfdriving/data/TrajectoriesAndRobotShape.h b/mrpt_path_planning/include/mpp/data/TrajectoriesAndRobotShape.h similarity index 93% rename from libselfdriving/include/selfdriving/data/TrajectoriesAndRobotShape.h rename to mrpt_path_planning/include/mpp/data/TrajectoriesAndRobotShape.h index a6b0663..1b2bf1d 100644 --- a/libselfdriving/include/selfdriving/data/TrajectoriesAndRobotShape.h +++ b/mrpt_path_planning/include/mpp/data/TrajectoriesAndRobotShape.h @@ -6,16 +6,16 @@ #pragma once +#include #include #include #include -#include #include #include #include -namespace selfdriving +namespace mpp { using robot_radius_t = double; @@ -46,4 +46,4 @@ bool obstaclePointCollides( const mrpt::math::TPoint2D& obstacleWrtRobot, const TrajectoriesAndRobotShape& trs); -} // namespace selfdriving +} // namespace mpp diff --git a/libselfdriving/include/selfdriving/data/VehicleLocalizationState.h b/mrpt_path_planning/include/mpp/data/VehicleLocalizationState.h similarity index 97% rename from libselfdriving/include/selfdriving/data/VehicleLocalizationState.h rename to mrpt_path_planning/include/mpp/data/VehicleLocalizationState.h index b480239..c217299 100644 --- a/libselfdriving/include/selfdriving/data/VehicleLocalizationState.h +++ b/mrpt_path_planning/include/mpp/data/VehicleLocalizationState.h @@ -14,7 +14,7 @@ #include #include -namespace selfdriving +namespace mpp { /** Data returned by VehicleMotionInterface::get_localization() */ struct VehicleLocalizationState @@ -46,4 +46,4 @@ struct VehicleLocalizationState std::string frame_id = "map"; }; -} // namespace selfdriving +} // namespace mpp diff --git a/libselfdriving/include/selfdriving/data/VehicleOdometryState.h b/mrpt_path_planning/include/mpp/data/VehicleOdometryState.h similarity index 96% rename from libselfdriving/include/selfdriving/data/VehicleOdometryState.h rename to mrpt_path_planning/include/mpp/data/VehicleOdometryState.h index 996e589..d6168bd 100644 --- a/libselfdriving/include/selfdriving/data/VehicleOdometryState.h +++ b/mrpt_path_planning/include/mpp/data/VehicleOdometryState.h @@ -12,7 +12,7 @@ #include -namespace selfdriving +namespace mpp { /** Data returned by VehicleMotionInterface::get_odometry() */ struct VehicleOdometryState @@ -43,4 +43,4 @@ struct VehicleOdometryState bool pendedActionExists = false; }; -} // namespace selfdriving +} // namespace mpp diff --git a/libselfdriving/include/selfdriving/data/Waypoints.h b/mrpt_path_planning/include/mpp/data/Waypoints.h similarity index 99% rename from libselfdriving/include/selfdriving/data/Waypoints.h rename to mrpt_path_planning/include/mpp/data/Waypoints.h index 7b26fe2..99a3fd4 100644 --- a/libselfdriving/include/selfdriving/data/Waypoints.h +++ b/mrpt_path_planning/include/mpp/data/Waypoints.h @@ -20,7 +20,7 @@ #include #include -namespace selfdriving +namespace mpp { using waypoint_idx_t = std::size_t; @@ -236,4 +236,4 @@ struct WaypointStatusSequence mrpt::opengl::CSetOfObjects& obj, const WaypointsRenderingParams& params = {}) const; }; -} // namespace selfdriving +} // namespace mpp diff --git a/libselfdriving/include/selfdriving/data/basic_types.h b/mrpt_path_planning/include/mpp/data/basic_types.h similarity index 95% rename from libselfdriving/include/selfdriving/data/basic_types.h rename to mrpt_path_planning/include/mpp/data/basic_types.h index e014e6b..d8d686e 100644 --- a/libselfdriving/include/selfdriving/data/basic_types.h +++ b/mrpt_path_planning/include/mpp/data/basic_types.h @@ -9,7 +9,7 @@ #include // uint32_t #include // size_t -namespace selfdriving +namespace mpp { /** Distances measured by PoseDistanceMetric, or "pseudodistances" of PTGs, that * is, distances along SE(2), including a weighted distance for rotations */ @@ -35,4 +35,4 @@ using normalized_speed_t = double; /** Time duration in seconds */ using duration_seconds_t = double; -} // namespace selfdriving +} // namespace mpp diff --git a/libselfdriving/include/selfdriving/data/const_ref_t.h b/mrpt_path_planning/include/mpp/data/const_ref_t.h similarity index 91% rename from libselfdriving/include/selfdriving/data/const_ref_t.h rename to mrpt_path_planning/include/mpp/data/const_ref_t.h index 6e5a5c6..54f634d 100644 --- a/libselfdriving/include/selfdriving/data/const_ref_t.h +++ b/mrpt_path_planning/include/mpp/data/const_ref_t.h @@ -9,10 +9,10 @@ #include // reference_wrapper<> #include -namespace selfdriving +namespace mpp { /** Wrapper to a const reference to T */ template using const_ref_t = std::optional>; -} // namespace selfdriving +} // namespace mpp diff --git a/libselfdriving/include/selfdriving/data/ptg_t.h b/mrpt_path_planning/include/mpp/data/ptg_t.h similarity index 90% rename from libselfdriving/include/selfdriving/data/ptg_t.h rename to mrpt_path_planning/include/mpp/data/ptg_t.h index f123551..7feff96 100644 --- a/libselfdriving/include/selfdriving/data/ptg_t.h +++ b/mrpt_path_planning/include/mpp/data/ptg_t.h @@ -8,8 +8,8 @@ #include -namespace selfdriving +namespace mpp { using ptg_t = mrpt::nav::CParameterizedTrajectoryGenerator; -} // namespace selfdriving +} // namespace mpp diff --git a/libselfdriving/include/selfdriving/data/trajectory_t.h b/mrpt_path_planning/include/mpp/data/trajectory_t.h similarity index 82% rename from libselfdriving/include/selfdriving/data/trajectory_t.h rename to mrpt_path_planning/include/mpp/data/trajectory_t.h index 301520e..214bfb5 100644 --- a/libselfdriving/include/selfdriving/data/trajectory_t.h +++ b/mrpt_path_planning/include/mpp/data/trajectory_t.h @@ -6,12 +6,12 @@ #pragma once -#include -#include +#include +#include #include -namespace selfdriving +namespace mpp { struct trajectory_state_t { @@ -25,4 +25,4 @@ struct trajectory_state_t using trajectory_t = std::map; -} // namespace selfdriving +} // namespace mpp diff --git a/libselfdriving/include/selfdriving/interfaces/LidarSource.h b/mrpt_path_planning/include/mpp/interfaces/LidarSource.h similarity index 91% rename from libselfdriving/include/selfdriving/interfaces/LidarSource.h rename to mrpt_path_planning/include/mpp/interfaces/LidarSource.h index 769ed9d..ff151ef 100644 --- a/libselfdriving/include/selfdriving/interfaces/LidarSource.h +++ b/mrpt_path_planning/include/mpp/interfaces/LidarSource.h @@ -8,7 +8,7 @@ #include -namespace selfdriving +namespace mpp { class LidarSource { @@ -16,4 +16,4 @@ class LidarSource /// Returns a copy of the last lidar observation virtual mrpt::obs::CObservation2DRangeScan::Ptr last_lidar_obs() const = 0; }; -} // namespace selfdriving +} // namespace mpp diff --git a/libselfdriving/include/selfdriving/interfaces/MVSIM_VehicleInterface.h b/mrpt_path_planning/include/mpp/interfaces/MVSIM_VehicleInterface.h similarity index 95% rename from libselfdriving/include/selfdriving/interfaces/MVSIM_VehicleInterface.h rename to mrpt_path_planning/include/mpp/interfaces/MVSIM_VehicleInterface.h index 4918f0e..0681914 100644 --- a/libselfdriving/include/selfdriving/interfaces/MVSIM_VehicleInterface.h +++ b/mrpt_path_planning/include/mpp/interfaces/MVSIM_VehicleInterface.h @@ -6,6 +6,8 @@ #pragma once +#include +#include #include #include #include @@ -16,10 +18,8 @@ #include #include #include -#include -#include -namespace selfdriving +namespace mpp { /** Vehicle adaptor class for the MVSIM simulator. * @@ -33,7 +33,7 @@ namespace selfdriving */ class MVSIM_VehicleInterface : public VehicleMotionInterface, public LidarSource { - DEFINE_MRPT_OBJECT(MVSIM_VehicleInterface, selfdriving) + DEFINE_MRPT_OBJECT(MVSIM_VehicleInterface, mpp) public: MVSIM_VehicleInterface() {} @@ -201,7 +201,6 @@ class MVSIM_VehicleInterface : public VehicleMotionInterface, public LidarSource } }; -} // namespace selfdriving +} // namespace mpp -IMPLEMENTS_MRPT_OBJECT( - MVSIM_VehicleInterface, VehicleMotionInterface, selfdriving) +IMPLEMENTS_MRPT_OBJECT(MVSIM_VehicleInterface, VehicleMotionInterface, mpp) diff --git a/libselfdriving/include/selfdriving/interfaces/ObstacleSource.h b/mrpt_path_planning/include/mpp/interfaces/ObstacleSource.h similarity index 98% rename from libselfdriving/include/selfdriving/interfaces/ObstacleSource.h rename to mrpt_path_planning/include/mpp/interfaces/ObstacleSource.h index e9c763e..43f0713 100644 --- a/libselfdriving/include/selfdriving/interfaces/ObstacleSource.h +++ b/mrpt_path_planning/include/mpp/interfaces/ObstacleSource.h @@ -12,7 +12,7 @@ #include #include -namespace selfdriving +namespace mpp { class ObstacleSource { @@ -98,4 +98,4 @@ class ObstacleSourceGenericSensor : public ObstacleSource mrpt::poses::CPose3D robotPoseForObs_; }; -} // namespace selfdriving +} // namespace mpp diff --git a/libselfdriving/include/selfdriving/interfaces/TargetApproachController.h b/mrpt_path_planning/include/mpp/interfaces/TargetApproachController.h similarity index 86% rename from libselfdriving/include/selfdriving/interfaces/TargetApproachController.h rename to mrpt_path_planning/include/mpp/interfaces/TargetApproachController.h index 6e031c1..0a785dc 100644 --- a/libselfdriving/include/selfdriving/interfaces/TargetApproachController.h +++ b/mrpt_path_planning/include/mpp/interfaces/TargetApproachController.h @@ -6,18 +6,18 @@ #pragma once +#include +#include +#include +#include +#include +#include +#include #include #include #include -#include -#include -#include -#include -#include -#include -#include -namespace selfdriving +namespace mpp { using mrpt::kinematics::CVehicleVelCmd; @@ -77,4 +77,4 @@ class TargetApproachController : public mrpt::system::COutputLogger, virtual void reset_state() = 0; }; -} // namespace selfdriving +} // namespace mpp diff --git a/libselfdriving/include/selfdriving/interfaces/VehicleMotionInterface.h b/mrpt_path_planning/include/mpp/interfaces/VehicleMotionInterface.h similarity index 97% rename from libselfdriving/include/selfdriving/interfaces/VehicleMotionInterface.h rename to mrpt_path_planning/include/mpp/interfaces/VehicleMotionInterface.h index fb19672..d879e64 100644 --- a/libselfdriving/include/selfdriving/interfaces/VehicleMotionInterface.h +++ b/mrpt_path_planning/include/mpp/interfaces/VehicleMotionInterface.h @@ -6,14 +6,14 @@ #pragma once +#include +#include +#include #include #include #include -#include -#include -#include -namespace selfdriving +namespace mpp { using mrpt::kinematics::CVehicleVelCmd; @@ -252,4 +252,4 @@ class VehicleMotionInterface : public mrpt::system::COutputLogger, /** @} */ }; -} // namespace selfdriving +} // namespace mpp diff --git a/libselfdriving/include/selfdriving/ptgs/DiffDriveCollisionGridBased.h b/mrpt_path_planning/include/mpp/ptgs/DiffDriveCollisionGridBased.h similarity index 98% rename from libselfdriving/include/selfdriving/ptgs/DiffDriveCollisionGridBased.h rename to mrpt_path_planning/include/mpp/ptgs/DiffDriveCollisionGridBased.h index 925ac17..225954e 100644 --- a/libselfdriving/include/selfdriving/ptgs/DiffDriveCollisionGridBased.h +++ b/mrpt_path_planning/include/mpp/ptgs/DiffDriveCollisionGridBased.h @@ -13,7 +13,7 @@ #include #include -namespace selfdriving::ptg +namespace mpp::ptg { struct TCPoint { @@ -242,10 +242,10 @@ class DiffDriveCollisionGridBased : public mrpt::nav::CPTG_RobotShape_Polygonal m_lambdaFunctionOptimizer; }; -} // namespace selfdriving::ptg +} // namespace mpp::ptg namespace mrpt::typemeta { // Specialization must occur in the same namespace -MRPT_DECLARE_TTYPENAME_NAMESPACE(TCPoint, selfdriving::ptg) +MRPT_DECLARE_TTYPENAME_NAMESPACE(TCPoint, mpp::ptg) } // namespace mrpt::typemeta diff --git a/libselfdriving/include/selfdriving/ptgs/DiffDrive_C.h b/mrpt_path_planning/include/mpp/ptgs/DiffDrive_C.h similarity index 92% rename from libselfdriving/include/selfdriving/ptgs/DiffDrive_C.h rename to mrpt_path_planning/include/mpp/ptgs/DiffDrive_C.h index f38b0a6..6d80dd0 100644 --- a/libselfdriving/include/selfdriving/ptgs/DiffDrive_C.h +++ b/mrpt_path_planning/include/mpp/ptgs/DiffDrive_C.h @@ -8,10 +8,10 @@ +------------------------------------------------------------------------+ */ #pragma once -#include -#include +#include +#include -namespace selfdriving::ptg +namespace mpp::ptg { /** A PTG for circular paths ("C" type PTG in papers). * - **Compatible kinematics**: differential-driven / Ackermann steering @@ -40,7 +40,7 @@ namespace selfdriving::ptg */ class DiffDrive_C : public DiffDriveCollisionGridBased, public SpeedTrimmablePTG { - DEFINE_SERIALIZABLE(DiffDrive_C, selfdriving::ptg) + DEFINE_SERIALIZABLE(DiffDrive_C, mpp::ptg) public: DiffDrive_C() = default; DiffDrive_C( @@ -69,4 +69,4 @@ class DiffDrive_C : public DiffDriveCollisionGridBased, public SpeedTrimmablePTG /** A generation parameter */ double K{0}; }; -} // namespace selfdriving::ptg +} // namespace mpp::ptg diff --git a/libselfdriving/include/selfdriving/ptgs/HolonomicBlend.h b/mrpt_path_planning/include/mpp/ptgs/HolonomicBlend.h similarity index 97% rename from libselfdriving/include/selfdriving/ptgs/HolonomicBlend.h rename to mrpt_path_planning/include/mpp/ptgs/HolonomicBlend.h index 93cf7fc..f801c97 100644 --- a/libselfdriving/include/selfdriving/ptgs/HolonomicBlend.h +++ b/mrpt_path_planning/include/mpp/ptgs/HolonomicBlend.h @@ -14,13 +14,13 @@ +------------------------------------------------------------------------+ */ #pragma once +#include #include #include -#include #include -namespace selfdriving::ptg +namespace mpp::ptg { /** A PTG for circular-shaped robots with holonomic kinematics. * - **Compatible kinematics**: Holonomic robot capable of velocity commands @@ -34,7 +34,7 @@ namespace selfdriving::ptg class HolonomicBlend : public SpeedTrimmablePTG, public mrpt::nav::CPTG_RobotShape_Circular { - DEFINE_SERIALIZABLE(HolonomicBlend, selfdriving::ptg) + DEFINE_SERIALIZABLE(HolonomicBlend, mpp::ptg) public: HolonomicBlend(); HolonomicBlend( @@ -137,4 +137,4 @@ class HolonomicBlend : public SpeedTrimmablePTG, static double calc_trans_distance_t_below_Tramp_abc( double t, double a, double b, double c); }; -} // namespace selfdriving::ptg +} // namespace mpp::ptg diff --git a/libselfdriving/include/selfdriving/ptgs/SpeedTrimmablePTG.h b/mrpt_path_planning/include/mpp/ptgs/SpeedTrimmablePTG.h similarity index 91% rename from libselfdriving/include/selfdriving/ptgs/SpeedTrimmablePTG.h rename to mrpt_path_planning/include/mpp/ptgs/SpeedTrimmablePTG.h index bc485d8..02a51e8 100644 --- a/libselfdriving/include/selfdriving/ptgs/SpeedTrimmablePTG.h +++ b/mrpt_path_planning/include/mpp/ptgs/SpeedTrimmablePTG.h @@ -8,7 +8,7 @@ #include -namespace selfdriving::ptg +namespace mpp::ptg { /** A PTG with a dynamic variable for speed modulation [0,1], usable in * the V and W expressions. @@ -22,4 +22,4 @@ class SpeedTrimmablePTG double trimmableSpeed_ = 1.0; }; -} // namespace selfdriving::ptg +} // namespace mpp::ptg diff --git a/libselfdriving/src/algos/CostEvaluator.cpp b/mrpt_path_planning/src/algos/CostEvaluator.cpp similarity index 87% rename from libselfdriving/src/algos/CostEvaluator.cpp rename to mrpt_path_planning/src/algos/CostEvaluator.cpp index cdc596c..0888a1a 100644 --- a/libselfdriving/src/algos/CostEvaluator.cpp +++ b/mrpt_path_planning/src/algos/CostEvaluator.cpp @@ -4,11 +4,11 @@ * See LICENSE for license information. * ------------------------------------------------------------------------- */ -#include +#include -using namespace selfdriving; +using namespace mpp; -IMPLEMENTS_VIRTUAL_MRPT_OBJECT(CostEvaluator, mrpt::rtti::CObject, selfdriving) +IMPLEMENTS_VIRTUAL_MRPT_OBJECT(CostEvaluator, mrpt::rtti::CObject, mpp) CostEvaluator::~CostEvaluator() = default; diff --git a/libselfdriving/src/algos/CostEvaluatorCostMap.cpp b/mrpt_path_planning/src/algos/CostEvaluatorCostMap.cpp similarity index 97% rename from libselfdriving/src/algos/CostEvaluatorCostMap.cpp rename to mrpt_path_planning/src/algos/CostEvaluatorCostMap.cpp index 41b5c1a..e7d8f9f 100644 --- a/libselfdriving/src/algos/CostEvaluatorCostMap.cpp +++ b/mrpt_path_planning/src/algos/CostEvaluatorCostMap.cpp @@ -4,13 +4,13 @@ * See LICENSE for license information. * ------------------------------------------------------------------------- */ +#include #include #include -#include -using namespace selfdriving; +using namespace mpp; -IMPLEMENTS_MRPT_OBJECT(CostEvaluatorCostMap, CostEvaluator, selfdriving) +IMPLEMENTS_MRPT_OBJECT(CostEvaluatorCostMap, CostEvaluator, mpp) CostEvaluatorCostMap::Parameters::Parameters() = default; diff --git a/libselfdriving/src/algos/CostEvaluatorPreferredWaypoint.cpp b/mrpt_path_planning/src/algos/CostEvaluatorPreferredWaypoint.cpp similarity index 95% rename from libselfdriving/src/algos/CostEvaluatorPreferredWaypoint.cpp rename to mrpt_path_planning/src/algos/CostEvaluatorPreferredWaypoint.cpp index 3052748..070a558 100644 --- a/libselfdriving/src/algos/CostEvaluatorPreferredWaypoint.cpp +++ b/mrpt_path_planning/src/algos/CostEvaluatorPreferredWaypoint.cpp @@ -4,13 +4,12 @@ * See LICENSE for license information. * ------------------------------------------------------------------------- */ +#include #include -#include -using namespace selfdriving; +using namespace mpp; -IMPLEMENTS_MRPT_OBJECT( - CostEvaluatorPreferredWaypoint, CostEvaluator, selfdriving) +IMPLEMENTS_MRPT_OBJECT(CostEvaluatorPreferredWaypoint, CostEvaluator, mpp) CostEvaluatorPreferredWaypoint::Parameters::Parameters() = default; diff --git a/libselfdriving/src/algos/NavEngine.cpp b/mrpt_path_planning/src/algos/NavEngine.cpp similarity index 98% rename from libselfdriving/src/algos/NavEngine.cpp rename to mrpt_path_planning/src/algos/NavEngine.cpp index 25e05b9..b49605a 100644 --- a/libselfdriving/src/algos/NavEngine.cpp +++ b/mrpt_path_planning/src/algos/NavEngine.cpp @@ -4,6 +4,15 @@ * See LICENSE for license information. * ------------------------------------------------------------------------- */ +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include #include @@ -12,17 +21,8 @@ #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace selfdriving; + +using namespace mpp; constexpr double MIN_TIME_BETWEEN_POSE_UPDATES = 20e-3; // [s] constexpr double PREVIOUS_POSES_MAX_AGE = 20; // [s] @@ -394,7 +394,9 @@ void NavEngine::update_robot_kinematic_state() innerState_.latestPoses.begin()->first, innerState_.latestPoses.rbegin()->first) > PREVIOUS_POSES_MAX_AGE) - { innerState_.latestPoses.erase(innerState_.latestPoses.begin()); } + { + innerState_.latestPoses.erase(innerState_.latestPoses.begin()); + } while (innerState_.latestOdomPoses.size() > 1 && mrpt::system::timeDifference( innerState_.latestOdomPoses.begin()->first, @@ -560,7 +562,7 @@ void NavEngine::check_have_to_replan() // Start from the current pose, plus the motion delta if we are already // moving (ideally we should be still while planning...): - selfdriving::SE2_KinState startingFrom; + mpp::SE2_KinState startingFrom; const double deltaTime = std::min(1.0, config_.plannerParams.maximumComputationTime); @@ -596,7 +598,7 @@ void NavEngine::check_have_to_replan() // Start from the current pose, plus the motion delta if we are already // moving (ideally we should be still while planning...): - selfdriving::SE2_KinState startingFrom; + mpp::SE2_KinState startingFrom; // Take the next node after the current under-execution motion edge: const auto& nextNode = @@ -695,7 +697,7 @@ NavEngine::PathPlannerOutput NavEngine::path_planner_function( << config_.vehicleMotionInterface->GetRuntimeClass()->className); // Do the path planning : - selfdriving::TPS_Astar planner; + mpp::TPS_Astar planner; // time profiler: planner.attachExternalProfiler_(navProfiler_); @@ -718,7 +720,7 @@ NavEngine::PathPlannerOutput NavEngine::path_planner_function( if (!lstPts.empty()) { - auto cmWps = selfdriving::CostEvaluatorPreferredWaypoint::Create(); + auto cmWps = mpp::CostEvaluatorPreferredWaypoint::Create(); cmWps->params_ = config_.preferWaypointsParameters; cmWps->setPreferredWaypoints(lstPts); @@ -735,7 +737,7 @@ NavEngine::PathPlannerOutput NavEngine::path_planner_function( obs && !obs->empty()) { planner.costEvaluators_.push_back( - selfdriving::CostEvaluatorCostMap::FromStaticPointObstacles( + mpp::CostEvaluatorCostMap::FromStaticPointObstacles( *obs, config_.globalCostParameters, ppi.pi.stateStart.pose)); } @@ -747,7 +749,7 @@ NavEngine::PathPlannerOutput NavEngine::path_planner_function( obs && !obs->empty()) { planner.costEvaluators_.push_back( - selfdriving::CostEvaluatorCostMap::FromStaticPointObstacles( + mpp::CostEvaluatorCostMap::FromStaticPointObstacles( *obs, config_.localCostParameters, ppi.pi.stateStart.pose)); } } @@ -817,9 +819,8 @@ NavEngine::PathPlannerOutput NavEngine::path_planner_function( } void NavEngine::enqueue_path_planner_towards( - const waypoint_idx_t targetWpIdx, - const selfdriving::SE2_KinState& startingFrom, - const std::optional& startingFromNodeID) + const waypoint_idx_t targetWpIdx, const mpp::SE2_KinState& startingFrom, + const std::optional& startingFromNodeID) { auto& _ = innerState_; @@ -928,7 +929,9 @@ void NavEngine::check_new_planner_output() // Merge or overwrite current plan: if (result.startingFromCurrentPlanNode.has_value()) - { merge_new_plan_if_better(result); } + { + merge_new_plan_if_better(result); + } else { MRPT_LOG_INFO_STREAM("Taking new path planning result."); @@ -953,9 +956,9 @@ void NavEngine::check_new_planner_output() for (const auto& edge : edges) _.activePlanPathEdges.push_back(*edge); #if 0 - const auto traj = selfdriving::plan_to_trajectory( + const auto traj = mpp::plan_to_trajectory( _.activePlanPathEdges, config_.ptgs); - selfdriving::save_to_txt(traj, "traj.txt"); + mpp::save_to_txt(traj, "traj.txt"); #endif } @@ -1230,7 +1233,7 @@ void NavEngine::send_next_motion_cmd_or_nop() << "\n nCurr.pose: " << nCurr.pose // ); - selfdriving::EnqueuedMotionCmd enqMotion; + mpp::EnqueuedMotionCmd enqMotion; enqMotion.nextCmd = generatedMotionCmdAfter; enqMotion.nextCondition.position = condPose; diff --git a/libselfdriving/src/algos/Planner.cpp b/mrpt_path_planning/src/algos/Planner.cpp similarity index 82% rename from libselfdriving/src/algos/Planner.cpp rename to mrpt_path_planning/src/algos/Planner.cpp index 441f163..6de0442 100644 --- a/libselfdriving/src/algos/Planner.cpp +++ b/mrpt_path_planning/src/algos/Planner.cpp @@ -4,11 +4,11 @@ * See LICENSE for license information. * ------------------------------------------------------------------------- */ -#include +#include -using namespace selfdriving; +using namespace mpp; -IMPLEMENTS_VIRTUAL_MRPT_OBJECT(Planner, mrpt::rtti::CObject, selfdriving) +IMPLEMENTS_VIRTUAL_MRPT_OBJECT(Planner, mrpt::rtti::CObject, mpp) Planner::~Planner() = default; diff --git a/libselfdriving/src/algos/TPS_Astar.cpp b/mrpt_path_planning/src/algos/TPS_Astar.cpp similarity index 98% rename from libselfdriving/src/algos/TPS_Astar.cpp rename to mrpt_path_planning/src/algos/TPS_Astar.cpp index 29e4692..d6d9686 100644 --- a/libselfdriving/src/algos/TPS_Astar.cpp +++ b/mrpt_path_planning/src/algos/TPS_Astar.cpp @@ -4,23 +4,23 @@ * See LICENSE for license information. * ------------------------------------------------------------------------- */ +#include +#include +#include +#include +#include +#include +#include +#include #include #include -#include -#include -#include -#include -#include -#include -#include -#include #include #include -IMPLEMENTS_MRPT_OBJECT(TPS_Astar, Planner, selfdriving) +IMPLEMENTS_MRPT_OBJECT(TPS_Astar, Planner, mpp) -using namespace selfdriving; +using namespace mpp; mrpt::containers::yaml TPS_Astar_Parameters::as_yaml() { @@ -466,7 +466,7 @@ PlannerOutput TPS_Astar::plan(const PlannerInput& in) cost_t TPS_Astar::default_heuristic_SE2( const SE2_KinState& from, const mrpt::math::TPose2D& goal) const { - selfdriving::PoseDistanceMetric_Lie metric( + mpp::PoseDistanceMetric_Lie metric( params_.SE2_metricAngleWeight); // Distance in SE(2): diff --git a/libselfdriving/src/algos/bestTrajectory.cpp b/mrpt_path_planning/src/algos/bestTrajectory.cpp similarity index 95% rename from libselfdriving/src/algos/bestTrajectory.cpp rename to mrpt_path_planning/src/algos/bestTrajectory.cpp index b0e0ddd..59d83b0 100644 --- a/libselfdriving/src/algos/bestTrajectory.cpp +++ b/mrpt_path_planning/src/algos/bestTrajectory.cpp @@ -4,13 +4,13 @@ * See LICENSE for license information. * ------------------------------------------------------------------------- */ +#include +#include #include -#include -#include -using namespace selfdriving; +using namespace mpp; -bool selfdriving::bestTrajectory( +bool mpp::bestTrajectory( MoveEdgeSE2_TPS& npa, const TrajectoriesAndRobotShape& trs, std::optional logger) { diff --git a/libselfdriving/src/algos/edge_interpolated_path.cpp b/mrpt_path_planning/src/algos/edge_interpolated_path.cpp similarity index 95% rename from libselfdriving/src/algos/edge_interpolated_path.cpp rename to mrpt_path_planning/src/algos/edge_interpolated_path.cpp index 1d67be1..f9db513 100644 --- a/libselfdriving/src/algos/edge_interpolated_path.cpp +++ b/mrpt_path_planning/src/algos/edge_interpolated_path.cpp @@ -4,9 +4,9 @@ * See LICENSE for license information. * ------------------------------------------------------------------------- */ -#include +#include -void selfdriving::edge_interpolated_path( +void mpp::edge_interpolated_path( MoveEdgeSE2_TPS& edge, const TrajectoriesAndRobotShape& trs, const std::optional& reconstrRelPoseOpt, const std::optional& ptg_stepOpt, diff --git a/libselfdriving/src/algos/refine_trajectory.cpp b/mrpt_path_planning/src/algos/refine_trajectory.cpp similarity index 80% rename from libselfdriving/src/algos/refine_trajectory.cpp rename to mrpt_path_planning/src/algos/refine_trajectory.cpp index d850ef4..5e5258c 100644 --- a/libselfdriving/src/algos/refine_trajectory.cpp +++ b/mrpt_path_planning/src/algos/refine_trajectory.cpp @@ -4,17 +4,17 @@ * See LICENSE for license information. * ------------------------------------------------------------------------- */ -#include -#include -#include +#include +#include +#include #include // see docs in .h -void selfdriving::refine_trajectory( - const selfdriving::MotionPrimitivesTreeSE2::path_t& inPath, - MotionPrimitivesTreeSE2::edge_sequence_t& edgesToRefine, - const TrajectoriesAndRobotShape& ptgInfo) +void mpp::refine_trajectory( + const mpp::MotionPrimitivesTreeSE2::path_t& inPath, + MotionPrimitivesTreeSE2::edge_sequence_t& edgesToRefine, + const TrajectoriesAndRobotShape& ptgInfo) { const size_t nEdges = edgesToRefine.size(); ASSERT_EQUAL_(inPath.size(), nEdges + 1); @@ -89,13 +89,13 @@ void selfdriving::refine_trajectory( } } -void selfdriving::refine_trajectory( - const std::vector& inPath, - std::vector& edgesToRefine, - const TrajectoriesAndRobotShape& ptgInfo) +void mpp::refine_trajectory( + const std::vector& inPath, + std::vector& edgesToRefine, + const TrajectoriesAndRobotShape& ptgInfo) { - selfdriving::MotionPrimitivesTreeSE2::path_t newPath; - MotionPrimitivesTreeSE2::edge_sequence_t newEdges; + mpp::MotionPrimitivesTreeSE2::path_t newPath; + MotionPrimitivesTreeSE2::edge_sequence_t newEdges; for (const auto& p : inPath) newPath.push_back(p); for (auto& e : edgesToRefine) newEdges.push_back(&e); diff --git a/libselfdriving/src/algos/render_tree.cpp b/mrpt_path_planning/src/algos/render_tree.cpp similarity index 98% rename from libselfdriving/src/algos/render_tree.cpp rename to mrpt_path_planning/src/algos/render_tree.cpp index 91b45ed..38e85b4 100644 --- a/libselfdriving/src/algos/render_tree.cpp +++ b/mrpt_path_planning/src/algos/render_tree.cpp @@ -4,6 +4,8 @@ * See LICENSE for license information. * ------------------------------------------------------------------------- */ +#include +#include #include #include #include @@ -13,12 +15,10 @@ #include #include #include -#include -#include -using namespace selfdriving; +using namespace mpp; -auto selfdriving::render_tree( +auto mpp::render_tree( const MotionPrimitivesTreeSE2& tree, const PlannerInput& pi, const RenderOptions& ro) -> std::shared_ptr { diff --git a/libselfdriving/src/algos/render_vehicle.cpp b/mrpt_path_planning/src/algos/render_vehicle.cpp similarity index 94% rename from libselfdriving/src/algos/render_vehicle.cpp rename to mrpt_path_planning/src/algos/render_vehicle.cpp index 1d50b05..b332c5d 100644 --- a/libselfdriving/src/algos/render_vehicle.cpp +++ b/mrpt_path_planning/src/algos/render_vehicle.cpp @@ -4,13 +4,13 @@ * See LICENSE for license information. * ------------------------------------------------------------------------- */ +#include #include -#include -using namespace selfdriving; +using namespace mpp; /** Generates a polygon for the vehicle shape */ -auto selfdriving::render_vehicle( +auto mpp::render_vehicle( const RobotShape& rs, mrpt::opengl::CSetOfLines& outPolygon, const RenderVehicleParams& rvp) -> RenderVehicleExtraResults { diff --git a/libselfdriving/src/algos/tp_obstacles_single_path.cpp b/mrpt_path_planning/src/algos/tp_obstacles_single_path.cpp similarity index 91% rename from libselfdriving/src/algos/tp_obstacles_single_path.cpp rename to mrpt_path_planning/src/algos/tp_obstacles_single_path.cpp index 4a7ffb5..83146d8 100644 --- a/libselfdriving/src/algos/tp_obstacles_single_path.cpp +++ b/mrpt_path_planning/src/algos/tp_obstacles_single_path.cpp @@ -4,9 +4,9 @@ * See LICENSE for license information. * ------------------------------------------------------------------------- */ -#include +#include -selfdriving::distance_t selfdriving::tp_obstacles_single_path( +mpp::distance_t mpp::tp_obstacles_single_path( const trajectory_index_t tp_space_k_direction, const mrpt::maps::CPointsMap& localObstacles, const ptg_t& ptg) { diff --git a/libselfdriving/src/algos/trajectories.cpp b/mrpt_path_planning/src/algos/trajectories.cpp similarity index 92% rename from libselfdriving/src/algos/trajectories.cpp rename to mrpt_path_planning/src/algos/trajectories.cpp index 899aec8..78adeef 100644 --- a/libselfdriving/src/algos/trajectories.cpp +++ b/mrpt_path_planning/src/algos/trajectories.cpp @@ -4,15 +4,15 @@ * See LICENSE for license information. * ------------------------------------------------------------------------- */ -#include -#include +#include +#include #include #include -using namespace selfdriving; +using namespace mpp; -trajectory_t selfdriving::plan_to_trajectory( +trajectory_t mpp::plan_to_trajectory( const MotionPrimitivesTreeSE2::edge_sequence_t& planEdges, const TrajectoriesAndRobotShape& ptgInfo, const duration_seconds_t samplePeriod) @@ -67,8 +67,7 @@ trajectory_t selfdriving::plan_to_trajectory( return out; } -bool selfdriving::save_to_txt( - const trajectory_t& traj, const std::string& fileName) +bool mpp::save_to_txt(const trajectory_t& traj, const std::string& fileName) { std::ofstream f; f.open(fileName); diff --git a/libselfdriving/src/algos/transform_pc_square_clipping.cpp b/mrpt_path_planning/src/algos/transform_pc_square_clipping.cpp similarity index 92% rename from libselfdriving/src/algos/transform_pc_square_clipping.cpp rename to mrpt_path_planning/src/algos/transform_pc_square_clipping.cpp index a1b67b9..564b683 100644 --- a/libselfdriving/src/algos/transform_pc_square_clipping.cpp +++ b/mrpt_path_planning/src/algos/transform_pc_square_clipping.cpp @@ -4,9 +4,9 @@ * See LICENSE for license information. * ------------------------------------------------------------------------- */ -#include +#include -void selfdriving::transform_pc_square_clipping( +void mpp::transform_pc_square_clipping( const mrpt::maps::CPointsMap& inMap, const mrpt::poses::CPose2D& asSeenFrom, const double MAX_DIST_XY, mrpt::maps::CPointsMap& outMap, bool appendToOutMap) diff --git a/libselfdriving/src/algos/viz.cpp b/mrpt_path_planning/src/algos/viz.cpp similarity index 91% rename from libselfdriving/src/algos/viz.cpp rename to mrpt_path_planning/src/algos/viz.cpp index 8b28553..25c4af2 100644 --- a/libselfdriving/src/algos/viz.cpp +++ b/mrpt_path_planning/src/algos/viz.cpp @@ -4,6 +4,8 @@ * See LICENSE for license information. * ------------------------------------------------------------------------- */ +#include +#include #include #include #include @@ -13,19 +15,16 @@ #include #include #include -#include -#include #include -using namespace selfdriving; +using namespace mpp; static std::vector nonmodal_wins; -void selfdriving::viz_nav_plan( - const selfdriving::PlannerOutput& plan, - const selfdriving::VisualizationOptions& opts, - const std::vector costEvaluators) +void mpp::viz_nav_plan( + const mpp::PlannerOutput& plan, const mpp::VisualizationOptions& opts, + const std::vector costEvaluators) { MRPT_START auto win = mrpt::gui::CDisplayWindow3D::Create("Path plan viz", 800, 600); @@ -68,8 +67,8 @@ void selfdriving::viz_nav_plan( MRPT_END } -void selfdriving::viz_nav_plan_animation( - const PlannerOutput& plan, const selfdriving::trajectory_t& traj, +void mpp::viz_nav_plan_animation( + const PlannerOutput& plan, const mpp::trajectory_t& traj, const RenderOptions& opts, const std::vector costEvaluators) { diff --git a/libselfdriving/src/data/MoveEdgeSE2_TPS.cpp b/mrpt_path_planning/src/data/MoveEdgeSE2_TPS.cpp similarity index 95% rename from libselfdriving/src/data/MoveEdgeSE2_TPS.cpp rename to mrpt_path_planning/src/data/MoveEdgeSE2_TPS.cpp index 9c7a2f9..32cc7ad 100644 --- a/libselfdriving/src/data/MoveEdgeSE2_TPS.cpp +++ b/mrpt_path_planning/src/data/MoveEdgeSE2_TPS.cpp @@ -4,11 +4,11 @@ * See LICENSE for license information. * ------------------------------------------------------------------------- */ -#include +#include #include -using namespace selfdriving; +using namespace mpp; ptg_t::TNavDynamicState MoveEdgeSE2_TPS::getPTGDynState() const { diff --git a/libselfdriving/src/data/PlannerInput.cpp b/mrpt_path_planning/src/data/PlannerInput.cpp similarity index 81% rename from libselfdriving/src/data/PlannerInput.cpp rename to mrpt_path_planning/src/data/PlannerInput.cpp index a9bec0c..43aa9e7 100644 --- a/libselfdriving/src/data/PlannerInput.cpp +++ b/mrpt_path_planning/src/data/PlannerInput.cpp @@ -4,6 +4,6 @@ * See LICENSE for license information. * ------------------------------------------------------------------------- */ -#include +#include -using namespace selfdriving; +using namespace mpp; diff --git a/libselfdriving/src/data/PlannerOutput.cpp b/mrpt_path_planning/src/data/PlannerOutput.cpp similarity index 81% rename from libselfdriving/src/data/PlannerOutput.cpp rename to mrpt_path_planning/src/data/PlannerOutput.cpp index d7750d1..111e362 100644 --- a/libselfdriving/src/data/PlannerOutput.cpp +++ b/mrpt_path_planning/src/data/PlannerOutput.cpp @@ -4,6 +4,6 @@ * See LICENSE for license information. * ------------------------------------------------------------------------- */ -#include +#include -using namespace selfdriving; +using namespace mpp; diff --git a/libselfdriving/src/data/SE2_KinState.cpp b/mrpt_path_planning/src/data/SE2_KinState.cpp similarity index 96% rename from libselfdriving/src/data/SE2_KinState.cpp rename to mrpt_path_planning/src/data/SE2_KinState.cpp index e782321..a351573 100644 --- a/libselfdriving/src/data/SE2_KinState.cpp +++ b/mrpt_path_planning/src/data/SE2_KinState.cpp @@ -4,10 +4,10 @@ * See LICENSE for license information. * ------------------------------------------------------------------------- */ +#include #include -#include -using namespace selfdriving; +using namespace mpp; std::string SE2_KinState::asString() const { diff --git a/libselfdriving/src/data/TrajectoriesAndRobotShape.cpp b/mrpt_path_planning/src/data/TrajectoriesAndRobotShape.cpp similarity index 96% rename from libselfdriving/src/data/TrajectoriesAndRobotShape.cpp rename to mrpt_path_planning/src/data/TrajectoriesAndRobotShape.cpp index 25de974..ec9d0e4 100644 --- a/libselfdriving/src/data/TrajectoriesAndRobotShape.cpp +++ b/mrpt_path_planning/src/data/TrajectoriesAndRobotShape.cpp @@ -4,9 +4,9 @@ * See LICENSE for license information. * ------------------------------------------------------------------------- */ -#include +#include -using namespace selfdriving; +using namespace mpp; void TrajectoriesAndRobotShape::clear() { *this = TrajectoriesAndRobotShape(); } @@ -108,7 +108,7 @@ void TrajectoriesAndRobotShape::initFromYAML(const mrpt::containers::yaml& node) } #endif -bool selfdriving::obstaclePointCollides( +bool mpp::obstaclePointCollides( const mrpt::math::TPoint2D& obstacleWrtRobot, const TrajectoriesAndRobotShape& trs) { diff --git a/libselfdriving/src/data/Waypoints.cpp b/mrpt_path_planning/src/data/Waypoints.cpp similarity index 99% rename from libselfdriving/src/data/Waypoints.cpp rename to mrpt_path_planning/src/data/Waypoints.cpp index fbcafd7..613401d 100644 --- a/libselfdriving/src/data/Waypoints.cpp +++ b/mrpt_path_planning/src/data/Waypoints.cpp @@ -7,12 +7,12 @@ | Released under BSD License. See: https://www.mrpt.org/License | +------------------------------------------------------------------------+ */ +#include #include #include #include -#include -using namespace selfdriving; +using namespace mpp; // Waypoint ========== Waypoint::Waypoint( diff --git a/libselfdriving/src/interfaces/ObstacleSource.cpp b/mrpt_path_planning/src/interfaces/ObstacleSource.cpp similarity index 87% rename from libselfdriving/src/interfaces/ObstacleSource.cpp rename to mrpt_path_planning/src/interfaces/ObstacleSource.cpp index ea1bb47..7d40979 100644 --- a/libselfdriving/src/interfaces/ObstacleSource.cpp +++ b/mrpt_path_planning/src/interfaces/ObstacleSource.cpp @@ -4,9 +4,9 @@ * See LICENSE for license information. * ------------------------------------------------------------------------- */ -#include +#include -using namespace selfdriving; +using namespace mpp; ObstacleSource::~ObstacleSource() = default; diff --git a/libselfdriving/src/interfaces/TargetApproachController.cpp b/mrpt_path_planning/src/interfaces/TargetApproachController.cpp similarity index 73% rename from libselfdriving/src/interfaces/TargetApproachController.cpp rename to mrpt_path_planning/src/interfaces/TargetApproachController.cpp index f9d7619..fcedadc 100644 --- a/libselfdriving/src/interfaces/TargetApproachController.cpp +++ b/mrpt_path_planning/src/interfaces/TargetApproachController.cpp @@ -4,11 +4,11 @@ * See LICENSE for license information. * ------------------------------------------------------------------------- */ -#include +#include -using namespace selfdriving; +using namespace mpp; IMPLEMENTS_VIRTUAL_MRPT_OBJECT( - TargetApproachController, mrpt::rtti::CObject, selfdriving) + TargetApproachController, mrpt::rtti::CObject, mpp) TargetApproachController::~TargetApproachController() = default; diff --git a/libselfdriving/src/interfaces/VehicleMotionInterface.cpp b/mrpt_path_planning/src/interfaces/VehicleMotionInterface.cpp similarity index 79% rename from libselfdriving/src/interfaces/VehicleMotionInterface.cpp rename to mrpt_path_planning/src/interfaces/VehicleMotionInterface.cpp index cf9543e..9af4fa8 100644 --- a/libselfdriving/src/interfaces/VehicleMotionInterface.cpp +++ b/mrpt_path_planning/src/interfaces/VehicleMotionInterface.cpp @@ -4,11 +4,10 @@ * See LICENSE for license information. * ------------------------------------------------------------------------- */ -#include +#include -using namespace selfdriving; +using namespace mpp; -IMPLEMENTS_VIRTUAL_MRPT_OBJECT(VehicleMotionInterface, mrpt::rtti::CObject, selfdriving) +IMPLEMENTS_VIRTUAL_MRPT_OBJECT(VehicleMotionInterface, mrpt::rtti::CObject, mpp) VehicleMotionInterface::~VehicleMotionInterface() = default; - diff --git a/libselfdriving/src/ptgs/DiffDriveCollisionGridBased.cpp b/mrpt_path_planning/src/ptgs/DiffDriveCollisionGridBased.cpp similarity index 99% rename from libselfdriving/src/ptgs/DiffDriveCollisionGridBased.cpp rename to mrpt_path_planning/src/ptgs/DiffDriveCollisionGridBased.cpp index 6fe0b6d..464aa0f 100644 --- a/libselfdriving/src/ptgs/DiffDriveCollisionGridBased.cpp +++ b/mrpt_path_planning/src/ptgs/DiffDriveCollisionGridBased.cpp @@ -7,6 +7,7 @@ | Released under BSD License. See: https://www.mrpt.org/License | +------------------------------------------------------------------------+ */ +#include #include #include #include @@ -14,11 +15,10 @@ #include #include #include -#include #include -using namespace selfdriving::ptg; +using namespace mpp::ptg; using mrpt::d2f; using mrpt::format; @@ -83,13 +83,13 @@ void DiffDriveCollisionGridBased::saveToConfigFile( MRPT_END } -mrpt::serialization::CArchive& selfdriving::ptg::operator<<( +mrpt::serialization::CArchive& mpp::ptg::operator<<( mrpt::serialization::CArchive& o, const TCPoint& p) { o << p.x << p.y << p.phi << p.t << p.dist << p.v << p.w; return o; } -mrpt::serialization::CArchive& selfdriving::ptg::operator>>( +mrpt::serialization::CArchive& mpp::ptg::operator>>( mrpt::serialization::CArchive& i, TCPoint& p) { i >> p.x >> p.y >> p.phi >> p.t >> p.dist >> p.v >> p.w; diff --git a/libselfdriving/src/ptgs/DiffDrive_C.cpp b/mrpt_path_planning/src/ptgs/DiffDrive_C.cpp similarity index 96% rename from libselfdriving/src/ptgs/DiffDrive_C.cpp rename to mrpt_path_planning/src/ptgs/DiffDrive_C.cpp index 2d819c8..c1d9a32 100644 --- a/libselfdriving/src/ptgs/DiffDrive_C.cpp +++ b/mrpt_path_planning/src/ptgs/DiffDrive_C.cpp @@ -7,17 +7,17 @@ | Released under BSD License. See: https://www.mrpt.org/License | +------------------------------------------------------------------------+ */ +#include #include #include -#include using namespace mrpt; using namespace mrpt::nav; using namespace mrpt::system; -using namespace selfdriving::ptg; +using namespace mpp::ptg; IMPLEMENTS_SERIALIZABLE( - DiffDrive_C, mrpt::nav::CParameterizedTrajectoryGenerator, selfdriving::ptg) + DiffDrive_C, mrpt::nav::CParameterizedTrajectoryGenerator, mpp::ptg) void DiffDrive_C::loadFromConfigFile( const mrpt::config::CConfigFileBase& cfg, const std::string& sSection) diff --git a/libselfdriving/src/ptgs/HolonomicBlend.cpp b/mrpt_path_planning/src/ptgs/HolonomicBlend.cpp similarity index 99% rename from libselfdriving/src/ptgs/HolonomicBlend.cpp rename to mrpt_path_planning/src/ptgs/HolonomicBlend.cpp index 67a8ed3..c6c4c5b 100644 --- a/libselfdriving/src/ptgs/HolonomicBlend.cpp +++ b/mrpt_path_planning/src/ptgs/HolonomicBlend.cpp @@ -13,23 +13,22 @@ | Released under BSD License. See: https://www.mrpt.org/License | +------------------------------------------------------------------------+ */ +#include #include #include #include #include #include #include -#include #include // debug only, remove! using namespace mrpt::nav; -using namespace selfdriving::ptg; +using namespace mpp::ptg; using namespace mrpt::system; IMPLEMENTS_SERIALIZABLE( - HolonomicBlend, mrpt::nav::CParameterizedTrajectoryGenerator, - selfdriving::ptg) + HolonomicBlend, mrpt::nav::CParameterizedTrajectoryGenerator, mpp::ptg) /* Closed-form PTG. Parameters: @@ -526,7 +525,9 @@ size_t HolonomicBlend::getPathStepCount(uint16_t k) const } ASSERT_(step > 0); if (m_pathStepCountCache.size() != m_alphaValuesCount) - { m_pathStepCountCache.assign(m_alphaValuesCount, -1); } + { + m_pathStepCountCache.assign(m_alphaValuesCount, -1); + } m_pathStepCountCache[k] = step; return step; } @@ -609,7 +610,9 @@ double HolonomicBlend::internal_getPathDist( const double k4 = (vyf - vyi) * TR2_; if (t < T_ramp) - { return calc_trans_distance_t_below_Tramp(k2, k4, vxi, vyi, t); } + { + return calc_trans_distance_t_below_Tramp(k2, k4, vxi, vyi, t); + } else { const double dist_trans = diff --git a/libselfdriving/src/registerClasses.cpp b/mrpt_path_planning/src/registerClasses.cpp similarity index 70% rename from libselfdriving/src/registerClasses.cpp rename to mrpt_path_planning/src/registerClasses.cpp index 1d00c0d..865378e 100644 --- a/libselfdriving/src/registerClasses.cpp +++ b/mrpt_path_planning/src/registerClasses.cpp @@ -4,18 +4,18 @@ * See LICENSE for license information. * ------------------------------------------------------------------------- */ +#include +#include +#include +#include +#include +#include +#include #include -#include -#include -#include -#include -#include -#include -#include MRPT_INITIALIZER(selfdriving_register) { - using namespace selfdriving; + using namespace mpp; using mrpt::rtti::registerClass; // Costs: diff --git a/scripts/plotPath.m b/scripts/plotPath.m index c715239..feb5651 100644 --- a/scripts/plotPath.m +++ b/scripts/plotPath.m @@ -1,5 +1,5 @@ function [] = plotPath(fileName) -% PLOTPATH Plots a trajectory file from selfdriving::save_to_txt() +% PLOTPATH Plots a trajectory file from mpp::save_to_txt() % % Time [s] x_global [m] y_global [m] phi [rad] vx_local [m] diff --git a/share/ptgs_holonomic_robot.ini b/share/ptgs_holonomic_robot.ini index 589143b..4ce6c03 100644 --- a/share/ptgs_holonomic_robot.ini +++ b/share/ptgs_holonomic_robot.ini @@ -19,7 +19,7 @@ max_obstacles_height = 2.0 // Maximum `z` coordinate of obstacles to #------------------------------------------------------------------------------ PTG_COUNT = 1 -PTG0_Type = selfdriving::ptg::HolonomicBlend +PTG0_Type = mpp::ptg::HolonomicBlend PTG0_refDistance = ${NAV_MAX_REF_DIST} # Maximum distance to build PTGs (in meters), i.e. the visibility "range" of tentative paths PTG0_num_paths = 191 PTG0_T_ramp_max = 1.0 // Max duration of the velocity interpolation since a vel_cmd is issued [s]. diff --git a/wip-experimental/TPS_RRTstar.cpp b/wip-experimental/TPS_RRTstar.cpp index 1047857..cd0bbea 100644 --- a/wip-experimental/TPS_RRTstar.cpp +++ b/wip-experimental/TPS_RRTstar.cpp @@ -4,21 +4,21 @@ * See LICENSE for license information. * ------------------------------------------------------------------------- */ +#include +#include +#include +#include +#include #include #include #include #include -#include -#include -#include -#include -#include #include -IMPLEMENTS_MRPT_OBJECT(TPS_RRTstar, Planner, selfdriving) +IMPLEMENTS_MRPT_OBJECT(TPS_RRTstar, Planner, mpp) -using namespace selfdriving; +using namespace mpp; mrpt::containers::yaml TPS_RRTstar_Parameters::as_yaml() { @@ -587,8 +587,7 @@ TPS_RRTstar::draw_pose_return_t TPS_RRTstar::draw_random_euclidean( const auto closestObsWrtRobot = q.inverseComposePoint(closestObs); - if (selfdriving::obstaclePointCollides( - closestObsWrtRobot, p.pi_.ptgs)) + if (mpp::obstaclePointCollides(closestObsWrtRobot, p.pi_.ptgs)) { isCollision = true; break; diff --git a/wip-experimental/TPS_RRTstar.h b/wip-experimental/TPS_RRTstar.h index 7aaee60..83924c9 100644 --- a/wip-experimental/TPS_RRTstar.h +++ b/wip-experimental/TPS_RRTstar.h @@ -6,11 +6,11 @@ #pragma once +#include +#include #include -#include -#include -namespace selfdriving +namespace mpp { struct TPS_RRTstar_Parameters { @@ -44,7 +44,7 @@ struct TPS_RRTstar_Parameters class TPS_RRTstar : virtual public mrpt::system::COutputLogger, public Planner { - DEFINE_MRPT_OBJECT(TPS_RRTstar, selfdriving) + DEFINE_MRPT_OBJECT(TPS_RRTstar, mpp) public: TPS_RRTstar(); @@ -159,4 +159,4 @@ class TPS_RRTstar : virtual public mrpt::system::COutputLogger, public Planner std::map local_obstacles_cache_; }; -} // namespace selfdriving +} // namespace mpp