diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 8a0faaa93b..63be703442 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -85,7 +85,7 @@ bool BtActionServer::on_configure() // use suffix '_rclcpp_node' to keep parameter file consistency #1773 auto options = rclcpp::NodeOptions().arguments( {"--ros-args", - "-r", std::string("__node:=") + node->get_name() + "_rclcpp_node", + "-r", std::string("__node:=") + std::string(node->get_name()) + action_name_ + "_rclcpp_node", "--"}); // Support for handling the topic-based goal pose from rviz client_node_ = std::make_shared("_", options); diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 086e09189b..b940c7c364 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -61,7 +61,7 @@ BtNavigator::BtNavigator() "nav2_is_battery_low_condition_bt_node", "nav2_navigate_through_poses_action_bt_node", "nav2_navigate_to_pose_action_bt_node", - "nav2_remove_passed_goals_action_bt_node" + "nav2_remove_passed_goals_action_bt_node", "nav2_planner_selector_bt_node", "nav2_controller_selector_bt_node", "nav2_goal_checker_selector_bt_node" diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp index 662dbbdd70..c90bad5ddd 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp @@ -72,6 +72,13 @@ class FootprintCollisionChecker * @brief Set the current costmap object to use for collision detection */ void setCostmap(CostmapT costmap); + /** + * @brief Get the current costmap object + */ + CostmapT getCostmap() + { + return costmap_; + } protected: CostmapT costmap_; diff --git a/nav2_costmap_2d/src/footprint_collision_checker.cpp b/nav2_costmap_2d/src/footprint_collision_checker.cpp index 20d60b3442..09876fb7c7 100644 --- a/nav2_costmap_2d/src/footprint_collision_checker.cpp +++ b/nav2_costmap_2d/src/footprint_collision_checker.cpp @@ -51,12 +51,17 @@ double FootprintCollisionChecker::footprintCost(const Footprint footpr unsigned int x0, x1, y0, y1; double footprint_cost = 0.0; + // get the cell coord of the first point + if (!worldToMap(footprint[0].x, footprint[0].y, x0, y0)) { + return static_cast(LETHAL_OBSTACLE); + } + + // cache the start to eliminate a worldToMap call + unsigned int xstart = x0; + unsigned int ystart = y0; + // we need to rasterize each line in the footprint for (unsigned int i = 0; i < footprint.size() - 1; ++i) { - // get the cell coord of the first point - if (!worldToMap(footprint[i].x, footprint[i].y, x0, y0)) { - return static_cast(LETHAL_OBSTACLE); - } // get the cell coord of the second point if (!worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1)) { @@ -64,23 +69,22 @@ double FootprintCollisionChecker::footprintCost(const Footprint footpr } footprint_cost = std::max(lineCost(x0, x1, y0, y1), footprint_cost); - } - // we also need to connect the first point in the footprint to the last point - // get the cell coord of the last point - if (!worldToMap(footprint.back().x, footprint.back().y, x0, y0)) { - return static_cast(LETHAL_OBSTACLE); - } + // the second point is next iteration's first point + x0 = x1; + y0 = y1; - // get the cell coord of the first point - if (!worldToMap(footprint.front().x, footprint.front().y, x1, y1)) { - return static_cast(LETHAL_OBSTACLE); + // if in collision, no need to continue + if (footprint_cost == static_cast(INSCRIBED_INFLATED_OBSTACLE) || + footprint_cost == static_cast(LETHAL_OBSTACLE)) + { + return footprint_cost; + } } - footprint_cost = std::max(lineCost(x0, x1, y0, y1), footprint_cost); - - // if all line costs are legal... then we can return that the footprint is legal - return footprint_cost; + // we also need to connect the first point in the footprint to the last point + // the last iteration's x1, y1 are the last footprint point's coordinates + return std::max(lineCost(xstart, x1, ystart, y1), footprint_cost); } template @@ -95,6 +99,13 @@ double FootprintCollisionChecker::lineCost(int x0, int x1, int y0, int if (line_cost < point_cost) { line_cost = point_cost; } + + // if in collision, no need to continue + if (line_cost == static_cast(INSCRIBED_INFLATED_OBSTACLE) || + line_cost == static_cast(LETHAL_OBSTACLE)) + { + return line_cost; + } } return line_cost; diff --git a/nav2_smac_planner/CMakeLists.txt b/nav2_smac_planner/CMakeLists.txt index ecc52347f2..05b84e4f8b 100644 --- a/nav2_smac_planner/CMakeLists.txt +++ b/nav2_smac_planner/CMakeLists.txt @@ -19,7 +19,6 @@ find_package(builtin_interfaces REQUIRED) find_package(tf2_ros REQUIRED) find_package(nav2_costmap_2d REQUIRED) find_package(pluginlib REQUIRED) -find_package(Ceres REQUIRED COMPONENTS SuiteSparse) find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) find_package(ompl REQUIRED) @@ -35,12 +34,16 @@ add_compile_options(-O3 -Wextra -Wdeprecated -fPIC) include_directories( include - ${CERES_INCLUDES} ${OMPL_INCLUDE_DIRS} ${OpenMP_INCLUDE_DIRS} ) -set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") -set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") + +find_package(OpenMP) +if(OPENMP_FOUND) + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") + set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") +endif() set(library_name nav2_smac_planner) @@ -62,16 +65,16 @@ set(dependencies eigen3_cmake_module ) -# SE2 plugin +# Hybrid plugin add_library(${library_name} SHARED - src/smac_planner.cpp + src/smac_planner_hybrid.cpp src/a_star.cpp - src/node_se2.cpp + src/node_hybrid.cpp src/costmap_downsampler.cpp src/node_2d.cpp ) -target_link_libraries(${library_name} ${CERES_LIBRARIES} ${OMPL_LIBRARIES} ${OpenMP_LIBRARIES} OpenMP::OpenMP_CXX) +target_link_libraries(${library_name} ${OMPL_LIBRARIES} ${OpenMP_LIBRARIES} OpenMP::OpenMP_CXX) target_include_directories(${library_name} PUBLIC ${Eigen3_INCLUDE_DIRS}) ament_target_dependencies(${library_name} @@ -82,12 +85,12 @@ ament_target_dependencies(${library_name} add_library(${library_name}_2d SHARED src/smac_planner_2d.cpp src/a_star.cpp - src/node_se2.cpp + src/node_hybrid.cpp src/costmap_downsampler.cpp src/node_2d.cpp ) -target_link_libraries(${library_name}_2d ${CERES_LIBRARIES} ${OMPL_LIBRARIES}) +target_link_libraries(${library_name}_2d ${OMPL_LIBRARIES}) target_include_directories(${library_name}_2d PUBLIC ${Eigen3_INCLUDE_DIRS}) ament_target_dependencies(${library_name}_2d diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index 0bd57feb4d..5d36fb47e4 100644 --- a/nav2_smac_planner/README.md +++ b/nav2_smac_planner/README.md @@ -1,14 +1,14 @@ # Smac Planner The SmacPlanner is a plugin for the Nav2 Planner server. It includes currently 2 distinct plugins: -- `SmacPlanner`: a highly optimized fully reconfigurable Hybrid-A* implementation supporting Dubin and Reeds-Shepp models. +- `SmacPlannerHybrid`: a highly optimized fully reconfigurable Hybrid-A* implementation supporting Dubin and Reeds-Shepp models (ackermann and car models). - `SmacPlanner2D`: a highly optimized fully reconfigurable grid-based A* implementation supporting Moore and Von Neumann models. It also introduces the following basic building blocks: - `CostmapDownsampler`: A library to take in a costmap object and downsample it to another resolution. -- `AStar`: A generic and highly optimized A* template library used by the planning plugins to search. Template implementations are provided for grid-A* and SE2 Hybrid-A* planning. Additional template for 3D planning also could be made available. +- `AStar`: A generic and highly optimized A* template library used by the planning plugins to search. Template implementations are provided for grid-A*, SE2 Hybrid-A* planning. Additional template for 3D planning also could be made available. - `CollisionChecker`: Collision check based on a robot's radius or footprint. -- `Smoother`: A Conjugate-gradient (CG) smoother with several optional cost function implementations for use. This is a cost-aware smoother unlike b-splines or bezier curves. +- `Smoother`: A path smoother to smooth out 2D, Hybrid-A\* paths. We have users reporting using this on: - Delivery robots @@ -16,16 +16,16 @@ We have users reporting using this on: ## Introduction -The `nav2_smac_planner` package contains an optimized templated A* search algorithm used to create multiple A\*-based planners for multiple types of robot platforms. It was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/). We support differential-drive and omni-directional drive robots using the `SmacPlanner2D` planner which implements a cost-aware A\* planner. We support cars, car-like, and ackermann vehicles using the `SmacPlanner` plugin which implements a Hybrid-A\* planner. This plugin is also useful for curvature constrained planning, like when planning robot at high speeds to make sure they don't flip over or otherwise skid out of control. It is also applicable to non-round robots (such as large rectangular or arbitrary shaped robots of differential/omnidirectional drivetrains) that need pose-based collision checking. +The `nav2_smac_planner` package contains an optimized templated A* search algorithm used to create multiple A\*-based planners for multiple types of robot platforms. It was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/). We support **circular** differential-drive and omni-directional drive robots using the `SmacPlanner2D` planner which implements a cost-aware A\* planner. We support **cars, car-like, and ackermann vehicles** using the `SmacPlannerHybrid` plugin which implements a Hybrid-A\* planner. These plugins are also useful for curvature constrained planning, like when planning robot at high speeds to make sure they don't flip over or otherwise skid out of control. It is also applicable to non-round robots (such as large rectangular or arbitrary shaped robots of differential/omnidirectional drivetrains) that need pose-based collision checking. -The `SmacPlanner` fully-implements the Hybrid-A* planner as proposed in [Practical Search Techniques in Path Planning for Autonomous Driving](https://ai.stanford.edu/~ddolgov/papers/dolgov_gpp_stair08.pdf), including hybrid searching, CG smoothing, analytic expansions and hueristic functions. +The `SmacPlannerHybrid` implements the Hybrid-A* planner as proposed in [Practical Search Techniques in Path Planning for Autonomous Driving](https://ai.stanford.edu/~ddolgov/papers/dolgov_gpp_stair08.pdf), including hybrid searching, gradient descent smoothing, analytic expansions and hueristic functions. In summary... -The `SmacPlanner` is designed to work with: +The `SmacPlannerHybrid` is designed to work with: - Ackermann, car, and car-like robots - High speed or curvature constrained robots (as to not flip over, skid, or dump load at high speeds) -- Arbitrary shaped, non-circular differential or omnidirectional robots requring SE2 collision checking +- Arbitrary shaped, non-circular differential or omnidirectional robots requiring SE2 collision checking with constrained curvatures The `SmacPlanner2D` is designed to work with: - Circular, differential or omnidirectional robots @@ -33,27 +33,28 @@ The `SmacPlanner2D` is designed to work with: ## Features -We further improve on the Hybrid-A\* work in the following ways: +We further improve in the following ways: - Remove need for upsampling by searching with 10x smaller motion primitives (same as their upsampling ratio). - Multi-resolution search allowing planning to occur at a coarser resolution for wider spaces (O(N^2) faster). - Cost-aware penalty functions in search resulting in far smoother plans (further reducing requirement to smooth). -- Additional cost functions in the CG smoother including path deflection. -- Approximated smoother Voronoi field using costmap inflation function. -- Fixed 3 mathematical issues in the original paper resulting in higher quality smoothing. +- Gradient descent smoother - Faster planning than original paper by highly optimizing the template A\* algorithm. +- Faster planning via precomputed heuristic, motion primitive, and other values. - Automatically adjusted search motion model sizes by motion model, costmap resolution, and bin sizing. - Closest path on approach within tolerance if exact path cannot be found or in invalid space. - Multi-model hybrid searching including Dubin and Reeds-Shepp models. More models may be trivially added. -- Time monitoring of planning to dynamically scale the maximum CG smoothing time based on remaining planning duration requested. - High unit and integration test coverage, doxygen documentation. - Uses modern C++14 language features and individual components are easily reusable. -- Speed optimizations: Fast approximation of shortest paths with wavefront hueristic, no data structure graph lookups in main loop, near-zero copy main loop, Voronoi field approximation, et al. +- Speed optimizations: no data structure graph lookups in main loop, near-zero copy main loop, dynamically generated graph and dynamic programming-based obstacle heuristic, optional recomputation of heuristics for subsiquent planning requests of the same goal, etc. - Templated Nodes and A\* implementation to support additional robot extensions. +- Selective re-evaluation of the obstacle heuristic per goal/map or each iteration, which can speed up subsiquent replanning 20x or more. -All of these features (multi-resolution, models, smoother, etc) are also available in the 2D `SmacPlanner2D` plugin. +All of these features (multi-resolution, models, smoother, etc) are also available in the `SmacPlanner2D` plugin. The 2D A\* implementation also does not have any of the weird artifacts introduced by the gradient wavefront-based 2D A\* implementation in the NavFn Planner. While this 2D A\* planner is slightly slower, I believe it's well worth the increased quality in paths. Though the `SmacPlanner2D` is grid-based, any reasonable local trajectory planner - including those supported by Nav2 - will not have any issue with grid-based plans. +Note: In prior releases, a CG smoother largely implementing the original Hybrid-A\* paper's. However, this smoother failed to consistently provide useful results, took too much compute time, and was deprecated. While smoothing a path 95% of the time seems like a "good" solution, we need something far more reliable for practical use. Since we are working with mobile robots and not autonomous cars at 60 mph, we can take some different liberties in smoothing knowing that our local trajectory planners are pretty smart. If you are looking for it, it now lives in the `test/` directory in a depreciated folder. This smoother has been replaced by a B-Spline inspired solution which is faster, far more consistent, and simpler to understand. While this smoother is **not** cost-aware, we have added cost-aware penalty functions in the planners themselves to push the plans away from high-cost spaces and we do check for validity of smoothed sections to ensure feasibility. + ## Metrics The original Hybrid-A\* implementation boasted planning times of 50-300ms for planning across 102,400 cell maps with 72 angular bins. We see much faster results in our evaluations: @@ -69,17 +70,15 @@ For example, the following path (roughly 85 meters) path took 33ms to compute. The basic design centralizes a templated A\* implementation that handles the search of a graph of nodes. The implementation is templated by the nodes, `NodeT`, which contain the methods needed to compute the hueristics, travel costs, and search neighborhoods. The outcome of this design is then a standard A\* implementation that can be used to traverse any type of graph as long as a node template can be created for it. -We provide by default a 2D node template (`Node2D`) which does 2D grid-search with either 4 or 8-connected neighborhoods, but the smoother can be used to smooth it out. We also provide a SE2 node template (`NodeSE2`) which does SE2 (X, Y, theta) search and collision checking on Dubin or Reeds-Shepp motion models. Additional templates could be easily made and included for 3D grid search and non-grid base searching like routing. - -Additional node templates could be added into the future to better support other types of robot path planning, such as including a state lattice motion primitive node and 3D path planning. Pull requests or discussions aroudn this point is encouraged. +We provide by default a 2D node template (`Node2D`) which does 2D grid-search with either 4 or 8-connected neighborhoods, but the smoother smooths it out on turns. We also provide a Hybrid A\* node template (`NodeHybrid`) which does SE2 (X, Y, theta) search and collision checking on Dubin or Reeds-Shepp motion models. Additional templates could be easily made and included for 3D grid search and non-grid base searching like routing. -In the ROS2 facing plugin, we take in the global goals and pre-process the data to feed into the templated A\* used. This includes processing any requests to downsample the costmap to another resolution to speed up search and smoothing the resulting A\* path. For the SE2 and `SmacPlanner` plugins, the path is promised to be kinematically feasible due to the kinematically valid models used in branching search. The 2D A\* is also promised to be feasible for differential and omni-directional robots. +In the ROS2 facing plugin, we take in the global goals and pre-process the data to feed into the templated A\* used. This includes processing any requests to downsample the costmap to another resolution to speed up search and smoothing the resulting A\* path. For the `SmacPlannerHybrid` plugin, the path is promised to be kinematically feasible due to the kinematically valid models used in branching search. The 2D A\* is also promised to be feasible for differential and omni-directional robots. We isolated the A\*, costmap downsampler, smoother, and Node template objects from ROS2 to allow them to be easily testable independently of ROS or the planner. The only place ROS is used is in the planner plugins themselves. ## Parameters -See inline description of parameters in the `SmacPlanner`. This includes comments as specific parameters apply to `SmacPlanner2D` and `SmacPlanner` in SE2 place. +See inline description of parameters in the `SmacPlanner`. This includes comments as specific parameters apply to `SmacPlanner2D` and `SmacPlanner` in place. ``` planner_server: @@ -89,44 +88,31 @@ planner_server: GridBased: plugin: "nav2_smac_planner/SmacPlanner" - tolerance: 0.5 # tolerance for planning if unable to reach exact pose, in meters, for 2D node - downsample_costmap: false # whether or not to downsample the map - downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) - allow_unknown: false # allow traveling in unknown space - max_iterations: -1 # maximum total iterations to search for before failing - max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance, 2D only - max_planning_time: 2.0 # max time in s for planner to plan, smooth, and upsample. Will scale maximum smoothing and upsampling times based on remaining time after planning. - smooth_path: false # Whether to smooth searched path - motion_model_for_search: "DUBIN" # 2D Moore, Von Neumann; SE2 Dubin, Redds-Shepp - angle_quantization_bins: 72 # For SE2 node: Number of angle bins for search, must be 1 for 2D node (no angle search) - minimum_turning_radius: 0.20 # For SE2 node & smoother: minimum turning radius in m of path / vehicle - reverse_penalty: 2.1 # For Reeds-Shepp model: penalty to apply if motion is reversing, must be => 1 - change_penalty: 0.20 # For SE2 node: penalty to apply if motion is changing directions, must be >= 0 - non_straight_penalty: 1.05 # For SE2 node: penalty to apply if motion is non-straight, must be => 1 - cost_penalty: 1.3 # For SE2 node: penalty to apply to higher cost zones - + tolerance: 0.5 # tolerance for planning if unable to reach exact pose, in meters, for 2D node + downsample_costmap: false # whether or not to downsample the map + downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) + allow_unknown: false # allow traveling in unknown space + max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable + max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance, 2D only + max_planning_time: 3.5 # max time in s for planner to plan, smooth, and upsample. Will scale maximum smoothing and upsampling times based on remaining time after planning. + motion_model_for_search: "DUBIN" # 2D Moore, Von Neumann; Hybrid Dubin, Redds-Shepp; State Lattice set internally + cost_travel_multiplier: 2.0 # For 2D: Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. + angle_quantization_bins: 64 # For Hybrid nodes: Number of angle bins for search, must be 1 for 2D node (no angle search) + minimum_turning_radius: 0.40 # For Hybrid nodes: minimum turning radius in m of path / vehicle + angle_quantization_bins: 64 # For Hybrid/Lattice nodes: Number of angle bins for search, must be 1 for 2D node (no angle search) + analytic_expansion_ratio: 3.5 # For Hybrid/Lattice nodes: The ratio to attempt analytic expansions during search for final approach. + minimum_turning_radius: 0.40 # For Hybrid/Lattice nodes: minimum turning radius in m of path / vehicle + reverse_penalty: 2.1 # For Reeds-Shepp model: penalty to apply if motion is reversing, must be => 1 + change_penalty: 0.15 # For Hybrid nodes: penalty to apply if motion is changing directions, must be >= 0 + non_straight_penalty: 1.50 # For Hybrid nodes: penalty to apply if motion is non-straight, must be => 1 + cost_penalty: 1.7 # For Hybrid nodes: penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. + lookup_table_size: 20 # For Hybrid nodes: Size of the dubin/reeds-sheep distance window to cache, in meters. + cache_obstacle_heuristic: True # For Hybrid nodes: Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. smoother: - smoother: - w_curve: 30.0 # weight to minimize curvature of path - w_dist: 0.0 # weight to bind path to original as optional replacement for cost weight - w_smooth: 30000.0 # weight to maximize smoothness of path - w_cost: 0.025 # weight to steer robot away from collision and cost - cost_scaling_factor: 10.0 # this should match the inflation layer's parameter - - # I do not recommend users mess with this unless they're doing production tuning - optimizer: - max_time: 0.10 # maximum compute time for smoother - max_iterations: 500 # max iterations of smoother - debug_optimizer: false # print debug info - gradient_tol: 1.0e-10 - fn_tol: 1.0e-20 - param_tol: 1.0e-15 - advanced: - min_line_search_step_size: 1.0e-20 - max_num_line_search_step_size_iterations: 50 - line_search_sufficient_function_decrease: 1.0e-20 - max_num_line_search_direction_restarts: 10 - max_line_search_step_expansion: 50 + max_iterations: 1000 + w_smooth: 0.3 + w_data: 0.2 + tolerance: 1e-10 ``` ## Topics @@ -150,24 +136,63 @@ Many users and default navigation configuration files I find are really missing Some of the most popular tuning guides for Navigation / Nav2 even [call this out specifically](https://arxiv.org/pdf/1706.09068.pdf) that there's substantial benefit to creating a gentle potential field across the width of the map - after inscribed costs are applied - yet very few users do this. -This habit actually results in paths produced by NavFn, Global Planner, and now SmacPlanner to be very suboptimal. They really want to look for a smooth potential field rather than wide open 0-cost spaces in order to stay in the middle of spaces and deal with close-by moving obstacles better. +This habit actually results in paths produced by NavFn, Global Planner, and now SmacPlanner to be somewhat suboptimal. They really want to look for a smooth potential field rather than wide open 0-cost spaces in order to stay in the middle of spaces and deal with close-by moving obstacles better. So it is my recommendation in using this package, as well as all other cost-aware search planners available in ROS, to increase your inflation layer cost scale in order to adequately produce a smooth potential across the entire map. For very large open spaces, its fine to have 0-cost areas in the middle, but for halls, aisles, and similar; **please create a smooth potential to provide the best performance**. -### 2D Search and Smoothing +### Hybrid-A* Turning Radius' -While the 2D planner has the smoother available (albeit, default parameters are tuned for the Hybrid-A\* planner, so you may need to play with that), my recommendation is not to use it. +A very reasonable and logical assumption would be to set the `minimum_turning_radius` to the kinematic limits of your vehicle. For an ackermann car, that's a physical quantity; while for differential or omni robots, its a bit of a dance around what kind of turns you'd like your robot to be able to make. Obviously setting this to something insanely small (like 20 cm) means you have alot of options, but also probably means the raw output plans won't be very straight and smooth when you have 2+ meter wide aisles to work in. -The 2D planner provides a 4-connected or 8-connected neighborhood path. This path may have little zig-zags in order to get at another non-90 or non-45 degree heading. That is totally fine. Your local trajectory planner such as DWB and TEB take these points into account to follow, but you won't see any zig-zag behaviors of your final robot motion after given to a trajectory planner. +I assert that you should also consider the environment you operate within when setting this. While you should **absolutely not** set this to be any smaller than the actual limits of your vehicle, there are some useful side effects of increasing this value in practical use. If you work in an area wider than the turning circle of your robot, you have some options that will ultimately improve the performance of the planner (in terms of CPU and compute time) as well as generate paths that are more "smooth" directly out of the planner -- not requiring any explicit path smoothing. -The smoothing is more "pleasing" to human eyes, but you don't want to be owning additional compute when it doesn't largely impact the output. However, if you have a more sensitive local trajectory planner like a carrot follower (e.g. pure pursuit), then you will want to smooth out the paths in order to have something more easily followable. - -Take this advise into account. Some good numbers to potentially start with would be `cost_scaling_factor: 10.0` and `inflation_radius: 5.5`. +By default, `0.4m` is the setting which I think is "reasonable" for the smaller scale industrial grade robots (think Simbe, the small Fetch, or Locus robots) resulting in faster plans and less "wobbly" motions that do not require post-smoothing -- further improving CPU performance. I selected `0.4m` as a trade off between practical robots mentioned above and hobbyist users with a tiny-little-turtlebot-3 which might still need to navigate around some smaller cavities. ### Costmap Resolutions -We provide for both the Hybrid-A\* and 2D A\* implementations a costmap downsampler option. This can be **incredible** beneficial when planning very long paths in larger spaces. The motion models for SE2 planning and neighborhood search in 2D planning is proportional to the costmap resolution. By downsampling it, you can N^2 reduce the number of expansions required to achieve a particular goal. However, the lower the resolution, the larger small obstacles appear and you won't be able to get super close to obstacles. This is a trade-off to make and test. Some numbers I've seen are 2-4x drops in planning CPU time for a 2-3x downsample rate. For 60m paths in an office space, I was able to get it << 100ms at a 2-3x downsample rate. +We provide for the Hybrid-A\* and 2D A\* implementations a costmap downsampler option. This can be **incredible** beneficial when planning very long paths in larger spaces. The motion models for SE2 planning and neighborhood search in 2D planning is proportional to the costmap resolution. By downsampling it, you can N^2 reduce the number of expansions required to achieve a particular goal. However, the lower the resolution, the larger small obstacles appear and you won't be able to get super close to obstacles. This is a trade-off to make and test. Some numbers I've seen are 2-4x drops in planning CPU time for a 2-3x downsample rate. For long and complex paths, I was able to get it << 100ms at only a 2x downsample rate from a plan that otherwise took upward of 400ms. I recommend users using a 5cm resolution costmap and playing with the different values of downsampling rate until they achieve what they think is optimal performance (lowest number of expansions vs. necessity to achieve fine goal poses). Then, I would recommend to change the global costmap resolution to this new value. That way you don't own the compute of downsampling and maintaining a higher-resolution costmap that isn't used. Remember, the global costmap is **only** there to provide an environment for the planner to work in. It is not there for human-viewing even if a more fine resolution costmap is more human "pleasing". If you use multiple planners in the planner server, then you will want to use the highest resolution for the most needed planner and then use the downsampler to downsample to the Hybrid-A* resolution. + +### No path found for clearly valid goals or long compute times + +Before addressing the section below, make sure you have an appropriately set max iterations parameter. If you have a 1 km2 sized warehouse, clearly 5000 expansions will be insufficient. Try increasing this value if you're unable to achieve goals or disable it with the `-1` value to see if you are now able to plan within a reasonable time period. If you still have issues, there is a secondary effect which could be happening that is good to be aware of. + +In maps with small gaps or holes, you may see an issue planning to certain regions. If there are gaps small enough to be untraversible yet large enough that inflation doesn't close it up with inflated costs, then it is recommended to lightly touch up the map or increase your inflation to remove those spaces from non-lethal space. + +Seeing the figures below, you'll see an attempt to plan into a "U" shaped region across the map. The first figure shows the small gap in the map (from an imperfect SLAM session) which is nearly traversible, but not quite. From the starting location, that gap yeilds the shortest path to the goal, so the heuristics will attempt to route the paths in that direction. However, it is not possible to actually pass with a kinematically valid path with the footprint set. As a result, the planner expands all of its maximum 1,000,000 iterations attempting to fit through it (visualized in red). If an infinite number of iterations were allowed, eventually a valid path would be found, but might take significant time. + +By simply increasing the footprint (a bit hackier, the best solution is to edit the map to make this area impassible), then that gap is now properly blocked as un-navigable. In the second figure, you can see that the heuristics influence the expansion down a navigable route and is able to find a path in less than 10,000 iterations (or about 110ms). It is easy now! + +As such, it is recommended if you have sparse SLAM maps, gaps or holes in your map, that you lightly post-process them to fill those gaps or increasing your footprint's padding or radius to make these areas invalid. Without it, it might waste expansions on this small corridor that: A) you dont want your robot actually using B) probably isnt actually valid and a SLAM artifact and C) if there's a more open space, you'd rather it use that. + +![](media/A.png) +![](media/B.png) + +One interesting thing to note from the second figure is that you see a number of expansions in open space. This is due to travel / heuristic values being so similar, tuning values of the penalty weights can have a decent impact there. The defaults are set as a good middle ground between large open spaces and confined aisles (environment specific tuning could be done to reduce the number of expansions for a specific map, speeding up the planner). The planner actually runs substantially faster the more confined the areas of search / environments are -- but still plenty fast for even wide open areas! + +Sometimes visualizing the expansions is very useful to debug potential concerns (why does this goal take longer to compute, why can't I find a path, etc), should you on rare occasion run into an issue. The following snippet is what I used to visualize the expansion in the images above which may help you in future endevours. + +``` cpp +// In createPath() +static auto node = std::make_shared("test"); +static auto pub = node->create_publisher("expansions", 1); +geometry_msgs::msg::PoseArray msg; +geometry_msgs::msg::Pose msg_pose; +msg.header.stamp = node->now(); +msg.header.frame_id = "map"; + +... + +// Each time we expand a new node +msg_pose.position.x = _costmap->getOriginX() + (current_node->pose.x * _costmap->getResolution()); +msg_pose.position.y = _costmap->getOriginY() + (current_node->pose.y * _costmap->getResolution()); +msg.poses.push_back(msg_pose); + +... + +// On backtrace or failure +pub->publish(msg); +``` diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index 6158422a7d..39d5788b97 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -27,7 +27,7 @@ #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_smac_planner/node_2d.hpp" -#include "nav2_smac_planner/node_se2.hpp" +#include "nav2_smac_planner/node_hybrid.hpp" #include "nav2_smac_planner/node_basic.hpp" #include "nav2_smac_planner/types.hpp" #include "nav2_smac_planner/constants.hpp" @@ -35,15 +35,6 @@ namespace nav2_smac_planner { -inline double squaredDistance( - const Eigen::Vector2d & p1, - const Eigen::Vector2d & p2) -{ - const double & dx = p1[0] - p2[0]; - const double & dy = p1[1] - p2[1]; - return hypot(dx, dy); -} - /** * @class nav2_smac_planner::AStarAlgorithm * @brief An A* implementation for planning in a costmap. Templated based on the Node type. @@ -61,6 +52,27 @@ class AStarAlgorithm typedef typename NodeVector::iterator NeighborIterator; typedef std::function NodeGetter; + /** + * @struct nav2_smac_planner::AnalyticExpansionNodes + * @brief Analytic expansion nodes and associated metadata + */ + + struct AnalyticExpansionNode + { + AnalyticExpansionNode( + NodePtr & node_in, + Coordinates & initial_coords_in, + Coordinates & proposed_coords_in) + : node(node_in), initial_coords(initial_coords_in), proposed_coords(proposed_coords_in) + {} + + NodePtr node; + Coordinates initial_coords; + Coordinates proposed_coords; + }; + + typedef std::vector AnalyticExpansionNodes; + /** * @struct nav2_smac_planner::NodeComparator * @brief Node comparison for priority queue sorting @@ -97,7 +109,9 @@ class AStarAlgorithm void initialize( const bool & allow_unknown, int & max_iterations, - const int & max_on_approach_iterations); + const int & max_on_approach_iterations, + const float & lookup_table_size, + const unsigned int & dim_3_size); /** * @brief Creating path from given costmap, start, and goal @@ -109,18 +123,10 @@ class AStarAlgorithm bool createPath(CoordinateVector & path, int & num_iterations, const float & tolerance); /** - * @brief Create the graph based on the node type. For 2D nodes, a cost grid. - * For 3D nodes, a SE2 grid without cost info as needs collision detector for footprint. - * @param x The total number of nodes in the X direction - * @param y The total number of nodes in the X direction - * @param dim_3 The total number of nodes in the theta or Z direction - * @param costmap Costmap to convert into the graph + * @brief Sets the collision checker to use + * @param collision_checker Collision checker to use for checking state validity */ - void createGraph( - const unsigned int & x, - const unsigned int & y, - const unsigned int & dim_3, - nav2_costmap_2d::Costmap2D * & costmap); + void setCollisionChecker(GridCollisionChecker * collision_checker); /** * @brief Set the goal for planning, as a node index @@ -144,29 +150,6 @@ class AStarAlgorithm const unsigned int & my, const unsigned int & dim_3); - /** - * @brief Set the footprint - * @param footprint footprint of robot - * @param use_radius Whether this footprint is a circle with radius - */ - void setFootprint(nav2_costmap_2d::Footprint footprint, bool use_radius); - - /** - * @brief Perform an analytic path expansion to the goal - * @param node The node to start the analytic path from - * @param getter The function object that gets valid nodes from the graph - * @return Node pointer to goal node if successful, else return nullptr - */ - NodePtr getAnalyticPath(const NodePtr & node, const NodeGetter & getter); - - /** - * @brief Set the starting pose for planning, as a node index - * @param node Node pointer to the goal node to backtrace - * @param path Reference to a vector of indicies of generated path - * @return whether the path was able to be backtraced - */ - bool backtracePath(NodePtr & node, CoordinateVector & path); - /** * @brief Get maximum number of iterations to plan * @return Reference to Maximum iterations parameter @@ -227,7 +210,7 @@ class AStarAlgorithm * @param cost The cost to sort into the open set of the node * @param node Node pointer reference to add to open set */ - inline void addNode(const float cost, NodePtr & node); + inline void addNode(const float & cost, NodePtr & node); /** * @brief Adds node to graph @@ -244,19 +227,12 @@ class AStarAlgorithm inline bool isGoal(NodePtr & node); /** - * @brief Get cost of traversal between nodes - * @param current_node Pointer to current node - * @param new_node Pointer to new node - * @return Reference traversal cost between the nodes - */ - inline float getTraversalCost(NodePtr & current_node, NodePtr & new_node); - - /** - * @brief Get total cost of traversal for a node - * @param node Pointer to current node - * @return Reference accumulated cost between the nodes + * @brief Set the starting pose for planning, as a node index + * @param node Node pointer to the goal node to backtrace + * @param path Reference to a vector of indicies of generated path + * @return whether the path was able to be backtraced */ - inline float & getAccumulatedCost(NodePtr & node); + bool backtracePath(NodePtr node, CoordinateVector & path); /** * @brief Get cost of heuristic of node @@ -287,10 +263,26 @@ class AStarAlgorithm * @return Node pointer reference to goal node if successful, else * return nullptr */ - inline NodePtr tryAnalyticExpansion( + NodePtr tryAnalyticExpansion( const NodePtr & current_node, const NodeGetter & getter, int & iterations, int & best_cost); + /** + * @brief Perform an analytic path expansion to the goal + * @param node The node to start the analytic path from + * @param getter The function object that gets valid nodes from the graph + * @return A set of analytically expanded nodes to the goal from current node, if possible + */ + AnalyticExpansionNodes getAnalyticPath(const NodePtr & node, const NodeGetter & getter); + + /** + * @brief Takes final analytic expansion and appends to current expanded node + * @param node The node to start the analytic path from + * @param expanded_nodes Expanded nodes to append to end of current search path + * @return Node pointer to goal node if successful, else return nullptr + */ + NodePtr setAnalyticPath(const NodePtr & node, const AnalyticExpansionNodes & expanded_nodes); + bool _traverse_unknown; int _max_iterations; int _max_on_approach_iterations; @@ -310,9 +302,7 @@ class AStarAlgorithm MotionModel _motion_model; NodeHeuristicPair _best_heuristic_node; - GridCollisionChecker _collision_checker; - nav2_costmap_2d::Footprint _footprint; - bool _is_radius_footprint; + GridCollisionChecker * _collision_checker; nav2_costmap_2d::Costmap2D * _costmap; }; diff --git a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp index 81fbbf887f..78f6c2e4ad 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp @@ -11,7 +11,7 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. Reserved. - +#include #include "nav2_costmap_2d/footprint_collision_checker.hpp" #include "nav2_smac_planner/constants.hpp" @@ -32,10 +32,14 @@ class GridCollisionChecker /** * @brief A constructor for nav2_smac_planner::GridCollisionChecker * @param costmap The costmap to collision check against + * @param num_quantizations The number of quantizations to precompute footprint + * orientations for to speed up collision checking */ GridCollisionChecker( - nav2_costmap_2d::Costmap2D * costmap) - : FootprintCollisionChecker(costmap) + nav2_costmap_2d::Costmap2D * costmap, + unsigned int num_quantizations) + : FootprintCollisionChecker(costmap), + num_quantizations_(num_quantizations) { } @@ -44,10 +48,47 @@ class GridCollisionChecker * @param footprint The footprint to collision check against * @param radius Whether or not the footprint is a circle and use radius collision checking */ - void setFootprint(const nav2_costmap_2d::Footprint & footprint, const bool & radius) + void setFootprint( + const nav2_costmap_2d::Footprint & footprint, + const bool & radius, + const double & possible_inscribed_cost) { - unoriented_footprint_ = footprint; + possible_inscribed_cost_ = possible_inscribed_cost; footprint_is_radius_ = radius; + + // Use radius, no caching required + if (radius) { + return; + } + + // No change, no updates required + if (footprint == unoriented_footprint_) { + return; + } + + bin_size_ = 2.0 * M_PI / static_cast(num_quantizations_); + oriented_footprints_.reserve(num_quantizations_); + double sin_th, cos_th; + geometry_msgs::msg::Point new_pt; + const unsigned int footprint_size = footprint.size(); + + // Precompute the orientation bins for checking to use + for (unsigned int i = 0; i != num_quantizations_; i++) { + sin_th = sin(i * bin_size_); + cos_th = cos(i * bin_size_); + nav2_costmap_2d::Footprint oriented_footprint; + oriented_footprint.reserve(footprint_size); + + for (unsigned int j = 0; j < footprint_size; j++) { + new_pt.x = footprint[j].x * cos_th - footprint[j].y * sin_th; + new_pt.y = footprint[j].x * sin_th + footprint[j].y * cos_th; + oriented_footprint.push_back(new_pt); + } + + oriented_footprints_.push_back(oriented_footprint); + } + + unoriented_footprint_ = footprint; } /** @@ -74,7 +115,7 @@ class GridCollisionChecker footprint_cost_ = costmap_->getCost( static_cast(x), static_cast(y)); - if (footprint_cost_ < POSSIBLY_INSCRIBED) { + if (footprint_cost_ < possible_inscribed_cost_) { return false; } @@ -88,9 +129,22 @@ class GridCollisionChecker return true; } - // if possible inscribed, need to check actual footprint pose - footprint_cost_ = footprintCostAtPose( - wx, wy, static_cast(theta), unoriented_footprint_); + // if possible inscribed, need to check actual footprint pose. + // Use precomputed oriented footprints are done on initialization, + // offset by translation value to collision check + int angle_bin = theta / bin_size_; + geometry_msgs::msg::Point new_pt; + const nav2_costmap_2d::Footprint & oriented_footprint = oriented_footprints_[angle_bin]; + nav2_costmap_2d::Footprint current_footprint; + current_footprint.reserve(oriented_footprint.size()); + for (unsigned int i = 0; i < oriented_footprint.size(); ++i) { + new_pt.x = wx + oriented_footprint[i].x; + new_pt.y = wy + oriented_footprint[i].y; + current_footprint.push_back(new_pt); + } + + footprint_cost_ = footprintCost(current_footprint); + if (footprint_cost_ == UNKNOWN && traverse_unknown) { return false; } @@ -111,6 +165,25 @@ class GridCollisionChecker } } + /** + * @brief Check if in collision with costmap and footprint at pose + * @param i Index to search collision status of + * @param traverse_unknown Whether or not to traverse in unknown space + * @return boolean if in collision or not. + */ + bool inCollision( + const unsigned int & i, + const bool & traverse_unknown) + { + footprint_cost_ = costmap_->getCost(i); + if (footprint_cost_ == UNKNOWN && traverse_unknown) { + return false; + } + + // if occupied or unknown and not to traverse unknown space + return footprint_cost_ >= INSCRIBED; + } + /** * @brief Get cost at footprint pose in costmap * @return the cost at the pose in costmap @@ -122,9 +195,13 @@ class GridCollisionChecker } protected: + std::vector oriented_footprints_; nav2_costmap_2d::Footprint unoriented_footprint_; double footprint_cost_; bool footprint_is_radius_; + unsigned int num_quantizations_; + double bin_size_; + double possible_inscribed_cost_{-1}; }; } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/include/nav2_smac_planner/constants.hpp b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp index 39e7bbd1f8..8bd0db5575 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/constants.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp @@ -26,6 +26,7 @@ enum class MotionModel MOORE = 2, DUBIN = 3, REEDS_SHEPP = 4, + STATE_LATTICE = 5, }; inline std::string toString(const MotionModel & n) @@ -39,6 +40,8 @@ inline std::string toString(const MotionModel & n) return "Dubin"; case MotionModel::REEDS_SHEPP: return "Reeds-Shepp"; + case MotionModel::STATE_LATTICE: + return "State Lattice"; default: return "Unknown"; } @@ -54,16 +57,17 @@ inline MotionModel fromString(const std::string & n) return MotionModel::DUBIN; } else if (n == "REEDS_SHEPP") { return MotionModel::REEDS_SHEPP; + } else if (n == "STATE_LATTICE") { + return MotionModel::STATE_LATTICE; } else { return MotionModel::UNKNOWN; } } -const float UNKNOWN = 255; -const float OCCUPIED = 254; -const float INSCRIBED = 253; -const float MAX_NON_OBSTACLE = 252; -const float POSSIBLY_INSCRIBED = 128; +const float UNKNOWN = 255.0; +const float OCCUPIED = 254.0; +const float INSCRIBED = 253.0; +const float MAX_NON_OBSTACLE = 252.0; const float FREE = 0; } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp index 688e6ea5b9..f5a43d0fef 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp @@ -24,8 +24,10 @@ #include #include +#include "nav2_smac_planner/types.hpp" #include "nav2_smac_planner/constants.hpp" #include "nav2_smac_planner/collision_checker.hpp" +#include "nav2_smac_planner/node_hybrid.hpp" namespace nav2_smac_planner { @@ -58,10 +60,9 @@ class Node2D /** * @brief A constructor for nav2_smac_planner::Node2D - * @param cost_in The costmap cost at this node * @param index The index of this node for self-reference */ - explicit Node2D(unsigned char & cost_in, const unsigned int index); + explicit Node2D(const unsigned int index); /** * @brief A destructor for nav2_smac_planner::Node2D @@ -80,9 +81,8 @@ class Node2D /** * @brief Reset method for new search - * @param cost_in The costmap cost at this node */ - void reset(const unsigned char & cost); + void reset(); /** * @brief Gets the accumulated cost at this node * @return accumulated cost @@ -96,7 +96,7 @@ class Node2D * @brief Sets the accumulated cost at this node * @param reference to accumulated cost */ - inline void setAccumulatedCost(const float cost_in) + inline void setAccumulatedCost(const float & cost_in) { _accumulated_cost = cost_in; } @@ -110,6 +110,15 @@ class Node2D return _cell_cost; } + /** + * @brief Gets the costmap cost at this node + * @return costmap cost + */ + inline void setCost(const float & cost) + { + _cell_cost = cost; + } + /** * @brief Gets if cell has been visited in search * @param If cell was visited @@ -160,7 +169,7 @@ class Node2D * @param collision_checker Pointer to collision checker object * @return whether this node is valid and collision free */ - bool isNodeValid(const bool & traverse_unknown, GridCollisionChecker collision_checker); + bool isNodeValid(const bool & traverse_unknown, GridCollisionChecker * collision_checker); /** * @brief get traversal cost from this node to child node @@ -199,40 +208,60 @@ class Node2D return Coordinates(index % width, index / width); } + /** + * @brief Get index + * @param Index Index of point + * @return coordinates of point + */ + static inline Coordinates getCoords(const unsigned int & index) + { + const unsigned int & size_x = _neighbors_grid_offsets[3]; + return Coordinates(index % size_x, index / size_x); + } + /** * @brief Get cost of heuristic of node * @param node Node index current * @param node Node index of new + * @param costmap Costmap ptr to use * @return Heuristic cost between the nodes */ static float getHeuristicCost( const Coordinates & node_coords, - const Coordinates & goal_coordinates); + const Coordinates & goal_coordinates, + const nav2_costmap_2d::Costmap2D * costmap); /** * @brief Initialize the neighborhood to be used in A* * We support 4-connect (VON_NEUMANN) and 8-connect (MOORE) - * @param x_size_uint The total x size to find neighbors * @param neighborhood The desired neighborhood type + * @param x_size_uint The total x size to find neighbors + * @param y_size The total y size to find neighbors + * @param num_angle_quantization Number of quantizations, must be 0 + * @param search_info Search parameters, unused by 2D node */ - static void initNeighborhood( - const unsigned int & x_size_uint, - const MotionModel & neighborhood); + static void initMotionModel( + const MotionModel & motion_model, + unsigned int & size_x, + unsigned int & size_y, + unsigned int & num_angle_quantization, + SearchInfo & search_info); + /** * @brief Retrieve all valid neighbors of a node. - * @param node Pointer to the node we are currently exploring in A* - * @param graph Reference to graph to discover new nodes + * @param validity_checker Functor for state validity checking + * @param collision_checker Collision checker to use + * @param traverse_unknown If unknown costs are valid to traverse * @param neighbors Vector of neighbors to be filled */ - static void getNeighbors( - NodePtr & node, + void getNeighbors( std::function & validity_checker, - GridCollisionChecker collision_checker, + GridCollisionChecker * collision_checker, const bool & traverse_unknown, NodeVector & neighbors); Node2D * parent; - static double neutral_cost; + static float cost_travel_multiplier; static std::vector _neighbors_grid_offsets; private: diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp index 290ca13162..0b69771ef4 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp @@ -28,7 +28,7 @@ #include "ompl/base/StateSpace.h" #include "nav2_smac_planner/constants.hpp" -#include "nav2_smac_planner/node_se2.hpp" +#include "nav2_smac_planner/node_hybrid.hpp" #include "nav2_smac_planner/node_2d.hpp" #include "nav2_smac_planner/types.hpp" #include "nav2_smac_planner/collision_checker.hpp" @@ -55,13 +55,13 @@ class NodeBasic { } - typename NodeT::Coordinates pose; // Used by NodeSE2 + typename NodeT::Coordinates pose; // Used by NodeHybrid NodeT * graph_node_ptr; unsigned int index; }; template class NodeBasic; -template class NodeBasic; +template class NodeBasic; } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_se2.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp similarity index 66% rename from nav2_smac_planner/include/nav2_smac_planner/node_se2.hpp rename to nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp index 6a10f75086..e2b85f18a2 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_se2.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#ifndef NAV2_SMAC_PLANNER__NODE_SE2_HPP_ -#define NAV2_SMAC_PLANNER__NODE_SE2_HPP_ +#ifndef NAV2_SMAC_PLANNER__NODE_HYBRID_HPP_ +#define NAV2_SMAC_PLANNER__NODE_HYBRID_HPP_ #include #include @@ -30,10 +30,14 @@ #include "nav2_smac_planner/constants.hpp" #include "nav2_smac_planner/types.hpp" #include "nav2_smac_planner/collision_checker.hpp" +#include "nav2_smac_planner/costmap_downsampler.hpp" namespace nav2_smac_planner { +typedef std::vector LookupTable; +typedef std::pair TrigValues; + // Need seperate pose struct for motion table operations /** @@ -65,18 +69,18 @@ struct MotionPose typedef std::vector MotionPoses; // Must forward declare -class NodeSE2; +class NodeHybrid; /** - * @struct nav2_smac_planner::MotionTable + * @struct nav2_smac_planner::HybridMotionTable * @brief A table of motion primitives and related functions */ -struct MotionTable +struct HybridMotionTable { /** - * @brief A constructor for nav2_smac_planner::MotionTable + * @brief A constructor for nav2_smac_planner::HybridMotionTable */ - MotionTable() {} + HybridMotionTable() {} /** * @brief Initializing using Dubin model @@ -106,22 +110,16 @@ struct MotionTable /** * @brief Get projections of motion models - * @param node Ptr to SE2 node + * @param node Ptr to NodeHybrid * @return A set of motion poses */ - MotionPoses getProjections(const NodeSE2 * node); - - /** - * @brief Get a projection of motion model - * @param node Ptr to SE2 node - * @return A motion pose - */ - MotionPose getProjection(const NodeSE2 * node, const unsigned int & motion_index); + MotionPoses getProjections(const NodeHybrid * node); MotionPoses projections; unsigned int size_x; unsigned int num_angle_quantization; float num_angle_quantization_float; + float min_turning_radius; float bin_size; float change_penalty; float non_straight_penalty; @@ -130,31 +128,33 @@ struct MotionTable ompl::base::StateSpacePtr state_space; std::vector> delta_xs; std::vector> delta_ys; + std::vector trig_values; }; /** - * @class nav2_smac_planner::NodeSE2 - * @brief NodeSE2 implementation for graph + * @class nav2_smac_planner::NodeHybrid + * @brief NodeHybrid implementation for graph, Hybrid-A* */ -class NodeSE2 +class NodeHybrid { public: - typedef NodeSE2 * NodePtr; - typedef std::unique_ptr> Graph; + typedef NodeHybrid * NodePtr; + typedef std::unique_ptr> Graph; typedef std::vector NodeVector; + /** - * @class nav2_smac_planner::NodeSE2::Coordinates - * @brief NodeSE2 implementation of coordinate structure + * @class nav2_smac_planner::NodeHybrid::Coordinates + * @brief NodeHybrid implementation of coordinate structure */ struct Coordinates { /** - * @brief A constructor for nav2_smac_planner::NodeSE2::Coordinates + * @brief A constructor for nav2_smac_planner::NodeHybrid::Coordinates */ Coordinates() {} /** - * @brief A constructor for nav2_smac_planner::NodeSE2::Coordinates + * @brief A constructor for nav2_smac_planner::NodeHybrid::Coordinates * @param x_in X coordinate * @param y_in Y coordinate * @param theta_in Theta coordinate @@ -163,28 +163,38 @@ class NodeSE2 : x(x_in), y(y_in), theta(theta_in) {} + inline bool operator==(const Coordinates & rhs) + { + return this->x == rhs.x && this->y == rhs.y && this->theta == rhs.theta; + } + + inline bool operator!=(const Coordinates & rhs) + { + return !(*this == rhs); + } + float x, y, theta; }; typedef std::vector CoordinateVector; /** - * @brief A constructor for nav2_smac_planner::NodeSE2 + * @brief A constructor for nav2_smac_planner::NodeHybrid * @param index The index of this node for self-reference */ - explicit NodeSE2(const unsigned int index); + explicit NodeHybrid(const unsigned int index); /** - * @brief A destructor for nav2_smac_planner::NodeSE2 + * @brief A destructor for nav2_smac_planner::NodeHybrid */ - ~NodeSE2(); + ~NodeHybrid(); /** * @brief operator== for comparisons - * @param NodeSE2 right hand side node reference + * @param NodeHybrid right hand side node reference * @return If cell indicies are equal */ - bool operator==(const NodeSE2 & rhs) + bool operator==(const NodeHybrid & rhs) { return this->_index == rhs._index; } @@ -216,7 +226,7 @@ class NodeSE2 * @brief Sets the accumulated cost at this node * @param reference to accumulated cost */ - inline void setAccumulatedCost(const float cost_in) + inline void setAccumulatedCost(const float & cost_in) { _accumulated_cost = cost_in; } @@ -263,24 +273,6 @@ class NodeSE2 inline void visited() { _was_visited = true; - _is_queued = false; - } - - /** - * @brief Gets if cell is currently queued in search - * @param If cell was queued - */ - inline bool & isQueued() - { - return _is_queued; - } - - /** - * @brief Sets if cell is currently queued in search - */ - inline void queued() - { - _is_queued = true; } /** @@ -297,7 +289,7 @@ class NodeSE2 * @param traverse_unknown If we can explore unknown nodes on the graph * @return whether this node is valid and collision free */ - bool isNodeValid(const bool & traverse_unknown, GridCollisionChecker collision_checker); + bool isNodeValid(const bool & traverse_unknown, GridCollisionChecker * collision_checker); /** * @brief Get traversal cost of parent node to child node @@ -317,7 +309,7 @@ class NodeSE2 */ static inline unsigned int getIndex( const unsigned int & x, const unsigned int & y, const unsigned int & angle, - const unsigned int & width, const unsigned int angle_quantization) + const unsigned int & width, const unsigned int & angle_quantization) { return angle + x * angle_quantization + y * width * angle_quantization; } @@ -346,7 +338,7 @@ class NodeSE2 */ static inline Coordinates getCoords( const unsigned int & index, - const unsigned int & width, const unsigned int angle_quantization) + const unsigned int & width, const unsigned int & angle_quantization) { return Coordinates( (index / angle_quantization) % width, // x @@ -358,11 +350,13 @@ class NodeSE2 * @brief Get cost of heuristic of node * @param node Node index current * @param node Node index of new + * @param costmap Costmap ptr to use * @return Heuristic cost between the nodes */ static float getHeuristicCost( const Coordinates & node_coords, - const Coordinates & goal_coordinates); + const Coordinates & goal_coordinates, + const nav2_costmap_2d::Costmap2D * costmap); /** * @brief Initialize motion models @@ -380,46 +374,87 @@ class NodeSE2 SearchInfo & search_info); /** - * @brief Compute the wavefront heuristic - * @param costmap Costmap to use to compute heuristic - * @param start_x Coordinate of Start X - * @param start_y Coordinate of Start Y - * @param goal_x Coordinate of Goal X - * @param goal_y Coordinate of Goal Y + * @brief Compute the SE2 distance heuristic + * @param lookup_table_dim Size, in costmap pixels, of the + * each lookup table dimension to populate + * @param motion_model Motion model to use for state space + * @param dim_3_size Number of quantization bins for caching + * @param search_info Info containing minimum radius to use + */ + static void precomputeDistanceHeuristic( + const float & lookup_table_dim, + const MotionModel & motion_model, + const unsigned int & dim_3_size, + const SearchInfo & search_info); + + /** + * @brief Compute the Obstacle heuristic + * @param node_coords Coordinates to get heuristic at + * @param goal_coords Coordinates to compute heuristic to + * @return heuristic Heuristic value */ - static void computeWavefrontHeuristic( - nav2_costmap_2d::Costmap2D * & costmap, - const unsigned int & start_x, const unsigned int & start_y, + static float getObstacleHeuristic( + const Coordinates & node_coords, + const Coordinates & goal_coords); + + /** + * @brief Compute the Distance heuristic + * @param node_coords Coordinates to get heuristic at + * @param goal_coords Coordinates to compute heuristic to + * @param obstacle_heuristic Value of the obstacle heuristic to compute + * additional motion heuristics if required + * @return heuristic Heuristic value + */ + static float getDistanceHeuristic( + const Coordinates & node_coords, + const Coordinates & goal_coords, + const float & obstacle_heuristic); + + /** + * @brief reset the obstacle heuristic state + * @param costmap Costmap to use + * @param goal_coords Coordinates to start heuristic expansion at + */ + static void resetObstacleHeuristic( + nav2_costmap_2d::Costmap2D * costmap, const unsigned int & goal_x, const unsigned int & goal_y); /** * @brief Retrieve all valid neighbors of a node. - * @param node Pointer to the node we are currently exploring in A* * @param validity_checker Functor for state validity checking + * @param collision_checker Collision checker to use + * @param traverse_unknown If unknown costs are valid to traverse * @param neighbors Vector of neighbors to be filled */ - static void getNeighbors( - const NodePtr & node, - std::function & validity_checker, - GridCollisionChecker collision_checker, + void getNeighbors( + std::function & validity_checker, + GridCollisionChecker * collision_checker, const bool & traverse_unknown, NodeVector & neighbors); - NodeSE2 * parent; + NodeHybrid * parent; Coordinates pose; - static double neutral_cost; - static MotionTable motion_table; + + // Constants required across all nodes but don't want to allocate more than once + static double travel_distance_cost; + static HybridMotionTable motion_table; + // Wavefront lookup and queue for continuing to expand as needed + static LookupTable obstacle_heuristic_lookup_table; + static std::queue obstacle_heuristic_queue; + static nav2_costmap_2d::Costmap2D * sampled_costmap; + static CostmapDownsampler downsampler; + // Dubin / Reeds-Shepp lookup and size for dereferencing + static LookupTable dist_heuristic_lookup_table; + static float size_lookup; private: float _cell_cost; float _accumulated_cost; unsigned int _index; bool _was_visited; - bool _is_queued; unsigned int _motion_primitive_index; - static std::vector _wavefront_heuristic; }; } // namespace nav2_smac_planner -#endif // NAV2_SMAC_PLANNER__NODE_SE2_HPP_ +#endif // NAV2_SMAC_PLANNER__NODE_HYBRID_HPP_ diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp index 31f1049609..ff235b02cf 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp @@ -21,6 +21,7 @@ #include "nav2_smac_planner/a_star.hpp" #include "nav2_smac_planner/smoother.hpp" +#include "nav2_smac_planner/utils.hpp" #include "nav2_smac_planner/costmap_downsampler.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "nav2_core/global_planner.hpp" @@ -85,24 +86,9 @@ class SmacPlanner2D : public nav2_core::GlobalPlanner const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) override; - /** - * @brief Create an Eigen Vector2D of world poses from continuous map coords - * @param mx float of map X coordinate - * @param my float of map Y coordinate - * @param costmap Costmap pointer - * @return Eigen::Vector2d eigen vector of the generated path - */ - Eigen::Vector2d getWorldCoords( - const float & mx, const float & my, const nav2_costmap_2d::Costmap2D * costmap); - - /** - * @brief Remove hooking at end of paths - * @param path Path to remove hooking from - */ - void removeHook(std::vector & path); - protected: std::unique_ptr> _a_star; + GridCollisionChecker _collision_checker; std::unique_ptr _smoother; nav2_costmap_2d::Costmap2D * _costmap; std::unique_ptr _costmap_downsampler; @@ -113,8 +99,6 @@ class SmacPlanner2D : public nav2_core::GlobalPlanner int _downsampling_factor; bool _downsample_costmap; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; - SmootherParams _smoother_params; - OptimizerParams _optimizer_params; double _max_planning_time; }; diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp similarity index 69% rename from nav2_smac_planner/include/nav2_smac_planner/smac_planner.hpp rename to nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp index 0f49dc7a4c..109913b7fd 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#ifndef NAV2_SMAC_PLANNER__SMAC_PLANNER_HPP_ -#define NAV2_SMAC_PLANNER__SMAC_PLANNER_HPP_ +#ifndef NAV2_SMAC_PLANNER__SMAC_PLANNER_HYBRID_HPP_ +#define NAV2_SMAC_PLANNER__SMAC_PLANNER_HYBRID_HPP_ #include #include @@ -21,6 +21,7 @@ #include "nav2_smac_planner/a_star.hpp" #include "nav2_smac_planner/smoother.hpp" +#include "nav2_smac_planner/utils.hpp" #include "nav2_smac_planner/costmap_downsampler.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "nav2_core/global_planner.hpp" @@ -35,18 +36,18 @@ namespace nav2_smac_planner { -class SmacPlanner : public nav2_core::GlobalPlanner +class SmacPlannerHybrid : public nav2_core::GlobalPlanner { public: /** * @brief constructor */ - SmacPlanner(); + SmacPlannerHybrid(); /** * @brief destructor */ - ~SmacPlanner(); + ~SmacPlannerHybrid(); /** * @brief Configuring plugin @@ -85,34 +86,12 @@ class SmacPlanner : public nav2_core::GlobalPlanner const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) override; - /** - * @brief Create an Eigen Vector2D of world poses from continuous map coords - * @param mx float of map X coordinate - * @param my float of map Y coordinate - * @param costmap Costmap pointer - * @return Eigen::Vector2d eigen vector of the generated path - */ - Eigen::Vector2d getWorldCoords( - const float & mx, const float & my, const nav2_costmap_2d::Costmap2D * costmap); - - /** - * @brief Create quaternion from A* coord bins - * @param theta continuous bin coordinates angle - * @return quaternion orientation in map frame - */ - geometry_msgs::msg::Quaternion getWorldOrientation(const float & theta); - - /** - * @brief Remove hooking at end of paths - * @param path Path to remove hooking from - */ - void removeHook(std::vector & path); - protected: - std::unique_ptr> _a_star; + std::unique_ptr> _a_star; + GridCollisionChecker _collision_checker; std::unique_ptr _smoother; rclcpp::Clock::SharedPtr _clock; - rclcpp::Logger _logger{rclcpp::get_logger("SmacPlanner")}; + rclcpp::Logger _logger{rclcpp::get_logger("SmacPlannerHybrid")}; nav2_costmap_2d::Costmap2D * _costmap; std::unique_ptr _costmap_downsampler; std::string _global_frame, _name; @@ -122,11 +101,9 @@ class SmacPlanner : public nav2_core::GlobalPlanner double _angle_bin_size; bool _downsample_costmap; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; - SmootherParams _smoother_params; - OptimizerParams _optimizer_params; double _max_planning_time; }; } // namespace nav2_smac_planner -#endif // NAV2_SMAC_PLANNER__SMAC_PLANNER_HPP_ +#endif // NAV2_SMAC_PLANNER__SMAC_PLANNER_HYBRID_HPP_ diff --git a/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp index b3b0fe8237..a9c45d4cda 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2020, Samsung Research America +// Copyright (c) 2021, Samsung Research America // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -23,17 +23,15 @@ #include #include "nav2_smac_planner/types.hpp" -#include "nav2_smac_planner/smoother_cost_function.hpp" - -#include "ceres/ceres.h" -#include "Eigen/Core" +#include "nav2_util/geometry_utils.hpp" +#include "nav_msgs/msg/path.hpp" namespace nav2_smac_planner { /** * @class nav2_smac_planner::Smoother - * @brief A Conjugate Gradient 2D path smoother implementation + * @brief A path smoother implementation */ class Smoother { @@ -41,7 +39,13 @@ class Smoother /** * @brief A constructor for nav2_smac_planner::Smoother */ - Smoother() {} + explicit Smoother(const SmootherParams & params) + { + tolerance_ = params.tolerance_; + max_its_ = params.max_its_; + data_w_ = params.w_data_; + smooth_w_ = params.w_smooth_; + } /** * @brief A destructor for nav2_smac_planner::Smoother @@ -50,90 +54,199 @@ class Smoother /** * @brief Initialization of the smoother - * @param params OptimizerParam struct + * @param min_turning_radius Minimum turning radius (m) */ - void initialize(const OptimizerParams params) + void initialize(const double & min_turning_radius) { - _debug = params.debug; - - // General Params - - // 2 most valid options: STEEPEST_DESCENT, NONLINEAR_CONJUGATE_GRADIENT - _options.line_search_direction_type = ceres::NONLINEAR_CONJUGATE_GRADIENT; - _options.line_search_type = ceres::WOLFE; - _options.nonlinear_conjugate_gradient_type = ceres::POLAK_RIBIERE; - _options.line_search_interpolation_type = ceres::CUBIC; - - _options.max_num_iterations = params.max_iterations; - _options.max_solver_time_in_seconds = params.max_time; - - _options.function_tolerance = params.fn_tol; - _options.gradient_tolerance = params.gradient_tol; - _options.parameter_tolerance = params.param_tol; - - _options.min_line_search_step_size = params.advanced.min_line_search_step_size; - _options.max_num_line_search_step_size_iterations = - params.advanced.max_num_line_search_step_size_iterations; - _options.line_search_sufficient_function_decrease = - params.advanced.line_search_sufficient_function_decrease; - _options.max_line_search_step_contraction = params.advanced.max_line_search_step_contraction; - _options.min_line_search_step_contraction = params.advanced.min_line_search_step_contraction; - _options.max_num_line_search_direction_restarts = - params.advanced.max_num_line_search_direction_restarts; - _options.line_search_sufficient_curvature_decrease = - params.advanced.line_search_sufficient_curvature_decrease; - _options.max_line_search_step_expansion = params.advanced.max_line_search_step_expansion; - - if (_debug) { - _options.minimizer_progress_to_stdout = true; - } else { - _options.logging_type = ceres::SILENT; - } + min_turning_rad_ = min_turning_radius; } /** * @brief Smoother method * @param path Reference to path * @param costmap Pointer to minimal costmap - * @param smoother parameters weights + * @param max_time Maximum time to compute, stop early if over limit * @return If smoothing was successful */ bool smooth( - std::vector & path, - nav2_costmap_2d::Costmap2D * costmap, - const SmootherParams & params) + nav_msgs::msg::Path & path, + const nav2_costmap_2d::Costmap2D * costmap, + const double & max_time, + const bool do_refinement = true) { - _options.max_solver_time_in_seconds = params.max_time; + using namespace std::chrono; // NOLINT + steady_clock::time_point a = steady_clock::now(); + rclcpp::Duration max_dur = rclcpp::Duration::from_seconds(max_time); + + int its = 0; + double change = tolerance_; + const unsigned int & path_size = path.poses.size(); + double x_i, y_i, y_m1, y_ip1, y_im2, y_ip2, y_i_org, curvature; + unsigned int mx, my; + + // Adding 5% margin due to floating point error + const double max_curvature = (1.0 / min_turning_rad_) * 1.05; + nav_msgs::msg::Path new_path = path; + nav_msgs::msg::Path last_path = path; + + while (change >= tolerance_) { + its += 1; + change = 0.0; + + // Make sure the smoothing function will converge + if (its >= max_its_) { + RCLCPP_DEBUG( + rclcpp::get_logger("SmacPlannerSmoother"), + "Number of iterations has exceeded limit of %i.", max_its_); + path = last_path; + updateApproximatePathOrientations(path); + return false; + } + + // Make sure still have time left to process + steady_clock::time_point b = steady_clock::now(); + rclcpp::Duration timespan(duration_cast>(b - a)); + if (timespan > max_dur) { + RCLCPP_DEBUG( + rclcpp::get_logger("SmacPlannerSmoother"), + "Smoothing time exceeded allowed duration of %0.2f.", max_time); + path = last_path; + updateApproximatePathOrientations(path); + return false; + } + + for (unsigned int i = 2; i != path_size - 2; i++) { + for (unsigned int j = 0; j != 2; j++) { + x_i = getFieldByDim(path.poses[i], j); + y_i = getFieldByDim(new_path.poses[i], j); + y_m1 = getFieldByDim(new_path.poses[i - 1], j); + y_ip1 = getFieldByDim(new_path.poses[i + 1], j); + y_i_org = y_i; + + if (i > 2 && i < path_size - 2) { + // Smooth based on local 5 point neighborhood and original data locations + y_im2 = getFieldByDim(new_path.poses[i - 2], j); + y_ip2 = getFieldByDim(new_path.poses[i + 2], j); + y_i += data_w_ * (x_i - y_i) + smooth_w_ * (y_im2 + y_ip2 + y_ip1 + y_m1 - (4.0 * y_i)); + } else { + // Smooth based on local 3 point neighborhood and original data locations + // At boundary conditions, need to use a more local neighborhood because the first + // and last 2 points cannot move to ensure the boundry conditions are upheld + y_i += data_w_ * (x_i - y_i) + smooth_w_ * (y_ip1 + y_m1 - (2.0 * y_i)); + } + + setFieldByDim(new_path.poses[i], j, y_i); + change += abs(y_i - y_i_org); + } + + // validate update is admissible, only checks cost if a valid costmap pointer is provided + float cost = 0.0; + if (costmap) { + costmap->worldToMap( + getFieldByDim(new_path.poses[i], 0), + getFieldByDim(new_path.poses[i], 1), + mx, my); + cost = static_cast(costmap->getCost(mx, my)); + } + if (cost > MAX_NON_OBSTACLE) { + RCLCPP_DEBUG( + rclcpp::get_logger("SmacPlannerSmoother"), + "Smoothing process resulted in an infeasible collision. " + "Returning the last path before the infeasibility was introduced."); + path = last_path; + updateApproximatePathOrientations(path); + return false; + } + } + + last_path = new_path; + } - double parameters[path.size() * 2]; // NOLINT - for (uint i = 0; i != path.size(); i++) { - parameters[2 * i] = path[i][0]; - parameters[2 * i + 1] = path[i][1]; + // Lets do additional refinement, it shouldn't take more than a couple milliseconds + // but really puts the path quality over the top. + if (do_refinement) { + smooth(new_path, costmap, max_time, false); } - ceres::GradientProblemSolver::Summary summary; - ceres::GradientProblem problem(new UnconstrainedSmootherCostFunction(&path, costmap, params)); - ceres::Solve(_options, problem, parameters, &summary); + for (unsigned int i = 3; i != path_size - 3; i++) { + if (getCurvature(new_path, i) > max_curvature) { + RCLCPP_DEBUG( + rclcpp::get_logger("SmacPlannerSmoother"), + "Smoothing process resulted in an infeasible curvature somewhere on the path. " + "This is most likely at the end point boundary conditions which will be further " + "refined as a perfect curve as you approach the goal. If this becomes a practical " + "issue for you, please file a ticket mentioning this message."); + updateApproximatePathOrientations(new_path); + path = new_path; + return false; + } + } - if (_debug) { - std::cout << summary.FullReport() << '\n'; + updateApproximatePathOrientations(new_path); + path = new_path; + return true; + } + +protected: + inline double getFieldByDim(const geometry_msgs::msg::PoseStamped & msg, const unsigned int & dim) + { + if (dim == 0) { + return msg.pose.position.x; + } else if (dim == 1) { + return msg.pose.position.y; + } else { + return msg.pose.position.z; } + } - if (!summary.IsSolutionUsable() || summary.initial_cost - summary.final_cost <= 0.0) { - return false; + inline void setFieldByDim( + geometry_msgs::msg::PoseStamped & msg, const unsigned int dim, + const double & value) + { + if (dim == 0) { + msg.pose.position.x = value; + } else if (dim == 1) { + msg.pose.position.y = value; + } else { + msg.pose.position.z = value; } + } - for (uint i = 0; i != path.size(); i++) { - path[i][0] = parameters[2 * i]; - path[i][1] = parameters[2 * i + 1]; + inline double getCurvature(const nav_msgs::msg::Path & path, const unsigned int i) + { + // k = 1 / r = acos(delta_phi) / |xi|, where delta_phi = (xi dot xi+1) / (|xi| * |xi+1|) + const double dxi_x = getFieldByDim(path.poses[i], 0) - getFieldByDim(path.poses[i - 1], 0); + const double dxi_y = getFieldByDim(path.poses[i], 1) - getFieldByDim(path.poses[i - 1], 1); + const double dxip1_x = getFieldByDim(path.poses[i + 1], 0) - getFieldByDim(path.poses[i], 0); + const double dxip1_y = getFieldByDim(path.poses[i + 1], 1) - getFieldByDim(path.poses[i], 1); + const double norm_dx_i = hypot(dxi_x, dxi_y); + const double norm_dx_ip1 = hypot(dxip1_x, dxip1_y); + double arg = (dxi_x * dxip1_x + dxi_y * dxip1_y) / (norm_dx_i * norm_dx_ip1); + + // In case of small out of bounds issues from floating point error + if (arg > 1.0) { + arg = 1.0; + } else if (arg < -1.0) { + arg = -1.0; } - return true; + return acos(arg) / norm_dx_i; + } + + inline void updateApproximatePathOrientations(nav_msgs::msg::Path & path) + { + using namespace nav2_util::geometry_utils; // NOLINT + double dx, dy, theta; + for (unsigned int i = 0; i != path.poses.size() - 1; i++) { + dx = path.poses[i + 1].pose.position.x - path.poses[i].pose.position.x; + dy = path.poses[i + 1].pose.position.y - path.poses[i].pose.position.y; + theta = atan2(dy, dx); + path.poses[i].pose.orientation = orientationAroundZAxis(theta); + } } -private: - bool _debug; - ceres::GradientProblemSolver::Options _options; + double min_turning_rad_, tolerance_, data_w_, smooth_w_; + int max_its_; }; } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/include/nav2_smac_planner/types.hpp b/nav2_smac_planner/include/nav2_smac_planner/types.hpp index eeeb921873..dffc88062b 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/types.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/types.hpp @@ -17,6 +17,11 @@ #include #include +#include +#include + +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_util/node_utils.hpp" namespace nav2_smac_planner { @@ -35,6 +40,51 @@ struct SearchInfo float reverse_penalty; float cost_penalty; float analytic_expansion_ratio; + std::string lattice_filepath; + bool cache_obstacle_heuristic; +}; + +/** + * @struct nav2_smac_planner::SmootherParams + * @brief Parameters for the smoother + */ +struct SmootherParams +{ + /** + * @brief A constructor for nav2_smac_planner::SmootherParams + */ + SmootherParams() + { + } + + /** + * @brief Get params from ROS parameter + * @param node Ptr to node + * @param name Name of plugin + */ + void get(std::shared_ptr node, const std::string & name) + { + std::string local_name = name + std::string(".smoother."); + + // Smoother params + nav2_util::declare_parameter_if_not_declared( + node, local_name + "tolerance", rclcpp::ParameterValue(1e-10)); + node->get_parameter(local_name + "tolerance", tolerance_); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "max_iterations", rclcpp::ParameterValue(1000)); + node->get_parameter(local_name + "max_iterations", max_its_); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "w_data", rclcpp::ParameterValue(0.2)); + node->get_parameter(local_name + "w_data", w_data_); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "w_smooth", rclcpp::ParameterValue(0.3)); + node->get_parameter(local_name + "w_smooth", w_smooth_); + } + + double tolerance_; + int max_its_; + double w_data_; + double w_smooth_; }; } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/include/nav2_smac_planner/utils.hpp b/nav2_smac_planner/include/nav2_smac_planner/utils.hpp new file mode 100644 index 0000000000..0c58fbfc01 --- /dev/null +++ b/nav2_smac_planner/include/nav2_smac_planner/utils.hpp @@ -0,0 +1,109 @@ +// Copyright (c) 2021, Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#ifndef NAV2_SMAC_PLANNER__UTILS_HPP_ +#define NAV2_SMAC_PLANNER__UTILS_HPP_ + +#include +#include + +#include "Eigen/Core" +#include "geometry_msgs/msg/quaternion.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "tf2/utils.h" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/inflation_layer.hpp" + +namespace nav2_smac_planner +{ + +/** +* @brief Create an Eigen Vector2D of world poses from continuous map coords +* @param mx float of map X coordinate +* @param my float of map Y coordinate +* @param costmap Costmap pointer +* @return Eigen::Vector2d eigen vector of the generated path +*/ +inline geometry_msgs::msg::Pose getWorldCoords( + const float & mx, const float & my, const nav2_costmap_2d::Costmap2D * costmap) +{ + geometry_msgs::msg::Pose msg; + msg.position.x = + static_cast(costmap->getOriginX()) + (mx + 0.5) * costmap->getResolution(); + msg.position.y = + static_cast(costmap->getOriginY()) + (my + 0.5) * costmap->getResolution(); + return msg; +} + +/** +* @brief Create quaternion from A* coord bins +* @param theta continuous bin coordinates angle +* @return quaternion orientation in map frame +*/ +inline geometry_msgs::msg::Quaternion getWorldOrientation( + const float & theta, + const float & bin_size) +{ + // theta is in continuous bin coordinates, must convert to world orientation + tf2::Quaternion q; + q.setEuler(0.0, 0.0, theta * static_cast(bin_size)); + return tf2::toMsg(q); +} + +/** +* @brief Find the min cost of the inflation decay function for which the robot MAY be +* in collision in any orientation +* @param costmap Costmap2DROS to get minimum inscribed cost (e.g. 128 in inflation layer documentation) +* @return double circumscribed cost, any higher than this and need to do full footprint collision checking +* since some element of the robot could be in collision +*/ +inline double findCircumscribedCost(std::shared_ptr costmap) +{ + double result = -1.0; + bool inflation_layer_found = false; + std::vector>::iterator layer; + + // check if the costmap has an inflation layer + for (layer = costmap->getLayeredCostmap()->getPlugins()->begin(); + layer != costmap->getLayeredCostmap()->getPlugins()->end(); + ++layer) + { + std::shared_ptr inflation_layer = + std::dynamic_pointer_cast(*layer); + if (!inflation_layer) { + continue; + } + + inflation_layer_found = true; + double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius(); + double resolution = costmap->getCostmap()->getResolution(); + result = static_cast(inflation_layer->computeCost(circum_radius / resolution)); + } + + if (!inflation_layer_found) { + RCLCPP_WARN( + rclcpp::get_logger("computeCircumscribedCost"), + "No inflation layer found in costmap configuration. " + "If this is an SE2-collision checking plugin, it cannot use costmap potential " + "field to speed up collision checking by only checking the full footprint " + "when robot is within possibly-inscribed radius of an obstacle. This may " + "significantly slow down planning times!"); + } + + return result; +} + +} // namespace nav2_smac_planner + +#endif // NAV2_SMAC_PLANNER__UTILS_HPP_ diff --git a/nav2_smac_planner/media/A.png b/nav2_smac_planner/media/A.png new file mode 100644 index 0000000000..8e04208118 Binary files /dev/null and b/nav2_smac_planner/media/A.png differ diff --git a/nav2_smac_planner/media/B.png b/nav2_smac_planner/media/B.png new file mode 100644 index 0000000000..e9098ed5bf Binary files /dev/null and b/nav2_smac_planner/media/B.png differ diff --git a/nav2_smac_planner/package.xml b/nav2_smac_planner/package.xml index 1c9516f16f..af2c8c6952 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -3,7 +3,7 @@ nav2_smac_planner 1.0.6 - Smac global planning plugin + Smac global planning plugin: A*, Hybrid-A*, State Lattice Steve Macenski Apache-2.0 @@ -23,7 +23,6 @@ nav2_costmap_2d nav2_core pluginlib - libceres-dev eigen3_cmake_module eigen ompl diff --git a/nav2_smac_planner/smac_plugin.xml b/nav2_smac_planner/smac_plugin.xml index c28cb32c7f..7b52bb24d7 100644 --- a/nav2_smac_planner/smac_plugin.xml +++ b/nav2_smac_planner/smac_plugin.xml @@ -1,5 +1,5 @@ - - SE2 version of the SMAC planner + + Hybrid-A* SMAC planner diff --git a/nav2_smac_planner/smac_plugin_2d.xml b/nav2_smac_planner/smac_plugin_2d.xml index aab22602e5..5bae687472 100644 --- a/nav2_smac_planner/smac_plugin_2d.xml +++ b/nav2_smac_planner/smac_plugin_2d.xml @@ -1,5 +1,5 @@ - 2D version of the SMAC Planner + 2D A* SMAC Planner diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 4f2f3c35ed..cd09cc7943 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -48,8 +48,7 @@ AStarAlgorithm::AStarAlgorithm( _goal_coordinates(Coordinates()), _start(nullptr), _goal(nullptr), - _motion_model(motion_model), - _collision_checker(nullptr) + _motion_model(motion_model) { _graph.reserve(100000); } @@ -63,75 +62,59 @@ template void AStarAlgorithm::initialize( const bool & allow_unknown, int & max_iterations, - const int & max_on_approach_iterations) + const int & max_on_approach_iterations, + const float & lookup_table_size, + const unsigned int & dim_3_size) { _traverse_unknown = allow_unknown; _max_iterations = max_iterations; _max_on_approach_iterations = max_on_approach_iterations; + NodeT::precomputeDistanceHeuristic(lookup_table_size, _motion_model, dim_3_size, _search_info); + _dim3_size = dim_3_size; } template<> -void AStarAlgorithm::createGraph( - const unsigned int & x_size, - const unsigned int & y_size, - const unsigned int & dim_3_size, - nav2_costmap_2d::Costmap2D * & costmap) +void AStarAlgorithm::initialize( + const bool & allow_unknown, + int & max_iterations, + const int & max_on_approach_iterations, + const float & /*lookup_table_size*/, + const unsigned int & dim_3_size) { + _traverse_unknown = allow_unknown; + _max_iterations = max_iterations; + _max_on_approach_iterations = max_on_approach_iterations; + if (dim_3_size != 1) { throw std::runtime_error("Node type Node2D cannot be given non-1 dim 3 quantization."); } - _costmap = costmap; - _dim3_size = dim_3_size; // 2D search MUST be 2D, not 3D or SE2. - clearGraph(); - - if (getSizeX() != x_size || getSizeY() != y_size) { - _x_size = x_size; - _y_size = y_size; - Node2D::initNeighborhood(_x_size, _motion_model); - } + _dim3_size = dim_3_size; } -template<> -void AStarAlgorithm::createGraph( - const unsigned int & x_size, - const unsigned int & y_size, - const unsigned int & dim_3_size, - nav2_costmap_2d::Costmap2D * & costmap) +template +void AStarAlgorithm::setCollisionChecker(GridCollisionChecker * collision_checker) { - _costmap = costmap; - _collision_checker = GridCollisionChecker(costmap); - _collision_checker.setFootprint(_footprint, _is_radius_footprint); + _collision_checker = collision_checker; + _costmap = collision_checker->getCostmap(); + unsigned int x_size = _costmap->getSizeInCellsX(); + unsigned int y_size = _costmap->getSizeInCellsY(); - _dim3_size = dim_3_size; - unsigned int index; clearGraph(); if (getSizeX() != x_size || getSizeY() != y_size) { _x_size = x_size; _y_size = y_size; - NodeSE2::initMotionModel(_motion_model, _x_size, _y_size, _dim3_size, _search_info); + NodeT::initMotionModel(_motion_model, _x_size, _y_size, _dim3_size, _search_info); } } template -void AStarAlgorithm::setFootprint(nav2_costmap_2d::Footprint footprint, bool use_radius) -{ - _footprint = footprint; - _is_radius_footprint = use_radius; -} - -template<> -typename AStarAlgorithm::NodePtr AStarAlgorithm::addToGraph( +typename AStarAlgorithm::NodePtr AStarAlgorithm::addToGraph( const unsigned int & index) { - return &(_graph.emplace(index, Node2D(_costmap->getCharMap()[index], index)).first->second); -} - -template<> -typename AStarAlgorithm::NodePtr AStarAlgorithm::addToGraph( - const unsigned int & index) -{ - return &(_graph.emplace(index, NodeSE2(index)).first->second); + // Emplace will only create a new object if it doesn't already exist. + // If an element exists, it will return the existing object, not create a new one. + return &(_graph.emplace(index, NodeT(index)).first->second); } template<> @@ -146,13 +129,13 @@ void AStarAlgorithm::setStart( _start = addToGraph(Node2D::getIndex(mx, my, getSizeX())); } -template<> -void AStarAlgorithm::setStart( +template +void AStarAlgorithm::setStart( const unsigned int & mx, const unsigned int & my, const unsigned int & dim_3) { - _start = addToGraph(NodeSE2::getIndex(mx, my, dim_3, getSizeX(), getSizeDim3())); + _start = addToGraph(NodeT::getIndex(mx, my, dim_3)); _start->setPose( Coordinates( static_cast(mx), @@ -174,24 +157,25 @@ void AStarAlgorithm::setGoal( _goal_coordinates = Node2D::Coordinates(mx, my); } -template<> -void AStarAlgorithm::setGoal( +template +void AStarAlgorithm::setGoal( const unsigned int & mx, const unsigned int & my, const unsigned int & dim_3) { - _goal = addToGraph(NodeSE2::getIndex(mx, my, dim_3, getSizeX(), getSizeDim3())); - _goal_coordinates = NodeSE2::Coordinates( + _goal = addToGraph(NodeT::getIndex(mx, my, dim_3)); + + typename NodeT::Coordinates goal_coords( static_cast(mx), static_cast(my), static_cast(dim_3)); - _goal->setPose(_goal_coordinates); - NodeSE2::computeWavefrontHeuristic( - _costmap, - static_cast(getStart()->pose.x), - static_cast(getStart()->pose.y), - mx, my); + if (!_search_info.cache_obstacle_heuristic || goal_coords != _goal_coordinates) { + NodeT::resetObstacleHeuristic(_costmap, mx, my); + } + + _goal_coordinates = goal_coords; + _goal->setPose(_goal_coordinates); } template @@ -227,7 +211,7 @@ bool AStarAlgorithm::createPath( CoordinateVector & path, int & iterations, const float & tolerance) { - _tolerance = tolerance * NodeT::neutral_cost; + _tolerance = tolerance; _best_heuristic_node = {std::numeric_limits::max(), 0}; clearQueue(); @@ -242,6 +226,7 @@ bool AStarAlgorithm::createPath( // Optimization: preallocate all variables NodePtr current_node = nullptr; NodePtr neighbor = nullptr; + NodePtr expansion_result = nullptr; float g_cost = 0.0; NodeVector neighbors; int approach_iterations = 0; @@ -277,13 +262,12 @@ bool AStarAlgorithm::createPath( // 2) Mark Nbest as visited current_node->visited(); - // 2.a) Use an analytic expansion (if available) to generate a path - // to the goal. - NodePtr result = tryAnalyticExpansion( - current_node, neighborGetter, analytic_iterations, - closest_distance); - if (result != nullptr) { - current_node = result; + // 2.1) Use an analytic expansion (if available) to generate a path + expansion_result = nullptr; + expansion_result = tryAnalyticExpansion( + current_node, neighborGetter, analytic_iterations, closest_distance); + if (expansion_result != nullptr) { + current_node = expansion_result; } // 3) Check if we're at the goal, backtrace if required @@ -292,18 +276,14 @@ bool AStarAlgorithm::createPath( } else if (_best_heuristic_node.first < getToleranceHeuristic()) { // Optimization: Let us find when in tolerance and refine within reason approach_iterations++; - if (approach_iterations > getOnApproachMaxIterations() || - iterations + 1 == getMaxIterations()) - { - NodePtr node = &_graph.at(_best_heuristic_node.second); - return backtracePath(node, path); + if (approach_iterations >= getOnApproachMaxIterations()) { + return backtracePath(&_graph.at(_best_heuristic_node.second), path); } } // 4) Expand neighbors of Nbest not visited neighbors.clear(); - NodeT::getNeighbors( - current_node, neighborGetter, _collision_checker, _traverse_unknown, neighbors); + current_node->getNeighbors(neighborGetter, _collision_checker, _traverse_unknown, neighbors); for (neighbor_iterator = neighbors.begin(); neighbor_iterator != neighbors.end(); ++neighbor_iterator) @@ -311,15 +291,14 @@ bool AStarAlgorithm::createPath( neighbor = *neighbor_iterator; // 4.1) Compute the cost to go to this node - g_cost = getAccumulatedCost(current_node) + getTraversalCost(current_node, neighbor); + g_cost = current_node->getAccumulatedCost() + current_node->getTraversalCost(neighbor); // 4.2) If this is a lower cost than prior, we set this as the new cost and new approach - if (g_cost < getAccumulatedCost(neighbor)) { + if (g_cost < neighbor->getAccumulatedCost()) { neighbor->setAccumulatedCost(g_cost); neighbor->parent = current_node; - // 4.3) If not in queue or visited, add it, `getNeighbors()` handles - neighbor->queued(); + // 4.3) Add to queue with heuristic cost addNode(g_cost + getHeuristicCost(neighbor), neighbor); } } @@ -335,109 +314,7 @@ bool AStarAlgorithm::isGoal(NodePtr & node) } template<> -AStarAlgorithm::NodePtr AStarAlgorithm::getAnalyticPath( - const NodePtr & node, - const NodeGetter & node_getter) -{ - ompl::base::ScopedState<> from(node->motion_table.state_space), to( - node->motion_table.state_space), s(node->motion_table.state_space); - const NodeSE2::Coordinates & node_coords = node->pose; - from[0] = node_coords.x; - from[1] = node_coords.y; - from[2] = node_coords.theta * node->motion_table.bin_size; - to[0] = _goal_coordinates.x; - to[1] = _goal_coordinates.y; - to[2] = _goal_coordinates.theta * node->motion_table.bin_size; - - float d = node->motion_table.state_space->distance(from(), to()); - NodePtr prev(node); - // A move of sqrt(2) is guaranteed to be in a new cell - static const float sqrt_2 = std::sqrt(2.); - unsigned int num_intervals = std::floor(d / sqrt_2); - - using PossibleNode = std::pair; - std::vector possible_nodes; - possible_nodes.reserve(num_intervals - 1); // We won't store this node or the goal - std::vector reals; - // Pre-allocate - unsigned int index = 0; - NodePtr next(nullptr); - float angle = 0.0; - Coordinates proposed_coordinates; - // Don't generate the first point because we are already there! - // And the last point is the goal, so ignore it too! - for (float i = 1; i < num_intervals; i++) { - node->motion_table.state_space->interpolate(from(), to(), i / num_intervals, s()); - reals = s.reals(); - angle = reals[2] / node->motion_table.bin_size; - while (angle >= node->motion_table.num_angle_quantization_float) { - angle -= node->motion_table.num_angle_quantization_float; - } - while (angle < 0.0) { - angle += node->motion_table.num_angle_quantization_float; - } - // Turn the pose into a node, and check if it is valid - index = NodeSE2::getIndex( - static_cast(reals[0]), - static_cast(reals[1]), - static_cast(angle)); - // Get the node from the graph - if (node_getter(index, next)) { - Coordinates initial_node_coords = next->pose; - proposed_coordinates = {static_cast(reals[0]), static_cast(reals[1]), angle}; - next->setPose(proposed_coordinates); - if (next->isNodeValid(_traverse_unknown, _collision_checker) && next != prev) { - // Save the node, and its previous coordinates in case we need to abort - possible_nodes.emplace_back(next, initial_node_coords); - prev = next; - } else { - next->setPose(initial_node_coords); - for (const auto & node_pose : possible_nodes) { - const auto & n = node_pose.first; - n->setPose(node_pose.second); - } - return NodePtr(nullptr); - } - } else { - // Abort - for (const auto & node_pose : possible_nodes) { - const auto & n = node_pose.first; - n->setPose(node_pose.second); - } - return NodePtr(nullptr); - } - } - // Legitimate path - set the parent relationships - poses already set - prev = node; - for (const auto & node_pose : possible_nodes) { - const auto & n = node_pose.first; - if (!n->wasVisited() && n->getIndex() != _goal->getIndex()) { - // Make sure this node has not been visited by the regular algorithm. - // If it has been, there is the (slight) chance that it is in the path we are expanding - // from, so we should skip it. - // Skipping to the next node will still create a kinematically feasible path. - n->parent = prev; - n->visited(); - prev = n; - } - } - if (_goal != prev) { - _goal->parent = prev; - _goal->visited(); - } - return _goal; -} - -template -typename AStarAlgorithm::NodePtr AStarAlgorithm::getAnalyticPath( - const NodePtr & node, - const NodeGetter & node_getter) -{ - return NodePtr(nullptr); -} - -template<> -bool AStarAlgorithm::backtracePath(NodePtr & node, CoordinateVector & path) +bool AStarAlgorithm::backtracePath(NodePtr node, CoordinateVector & path) { if (!node->parent) { return false; @@ -455,8 +332,8 @@ bool AStarAlgorithm::backtracePath(NodePtr & node, CoordinateVector & pa return path.size() > 1; } -template<> -bool AStarAlgorithm::backtracePath(NodePtr & node, CoordinateVector & path) +template +bool AStarAlgorithm::backtracePath(NodePtr node, CoordinateVector & path) { if (!node->parent) { return false; @@ -484,20 +361,23 @@ typename AStarAlgorithm::NodePtr & AStarAlgorithm::getGoal() return _goal; } -template -typename AStarAlgorithm::NodePtr AStarAlgorithm::getNextNode() +template<> +typename AStarAlgorithm::NodePtr AStarAlgorithm::getNextNode() { - NodeBasic node = _queue.top().second; + NodeBasic node = _queue.top().second; _queue.pop(); return node.graph_node_ptr; } -template<> -typename AStarAlgorithm::NodePtr AStarAlgorithm::getNextNode() +template +typename AStarAlgorithm::NodePtr AStarAlgorithm::getNextNode() { - NodeBasic node = _queue.top().second; + NodeBasic node = _queue.top().second; _queue.pop(); + // We only want to override the node's pose if it has not yet been visited + // to prevent the case that a node has been queued multiple times and + // a new branch is overriding one of lower cost already visited. if (!node.graph_node_ptr->wasVisited()) { node.graph_node_ptr->pose = node.pose; } @@ -505,44 +385,30 @@ typename AStarAlgorithm::NodePtr AStarAlgorithm::getNextNode() return node.graph_node_ptr; } -template -void AStarAlgorithm::addNode(const float cost, NodePtr & node) +template<> +void AStarAlgorithm::addNode(const float & cost, NodePtr & node) { - NodeBasic queued_node(node->getIndex()); + NodeBasic queued_node(node->getIndex()); queued_node.graph_node_ptr = node; _queue.emplace(cost, queued_node); } -template<> -void AStarAlgorithm::addNode(const float cost, NodePtr & node) +template +void AStarAlgorithm::addNode(const float & cost, NodePtr & node) { - NodeBasic queued_node(node->getIndex()); + NodeBasic queued_node(node->getIndex()); queued_node.pose = node->pose; queued_node.graph_node_ptr = node; _queue.emplace(cost, queued_node); } -template -float AStarAlgorithm::getTraversalCost( - NodePtr & current_node, - NodePtr & new_node) -{ - return current_node->getTraversalCost(new_node); -} - -template -float & AStarAlgorithm::getAccumulatedCost(NodePtr & node) -{ - return node->getAccumulatedCost(); -} - template float AStarAlgorithm::getHeuristicCost(const NodePtr & node) { const Coordinates node_coords = NodeT::getCoords(node->getIndex(), getSizeX(), getSizeDim3()); float heuristic = NodeT::getHeuristicCost( - node_coords, _goal_coordinates); + node_coords, _goal_coordinates, _costmap); if (heuristic < _best_heuristic_node.first) { _best_heuristic_node = {heuristic, node->getIndex()}; @@ -562,8 +428,8 @@ template void AStarAlgorithm::clearGraph() { Graph g; - g.reserve(100000); std::swap(_graph, g); + _graph.reserve(100000); } template @@ -607,26 +473,24 @@ typename AStarAlgorithm::NodePtr AStarAlgorithm::tryAnalyticExpans const NodePtr & current_node, const NodeGetter & getter, int & analytic_iterations, int & closest_distance) { - if (_motion_model == MotionModel::DUBIN || _motion_model == MotionModel::REEDS_SHEPP) { - // This must be a NodeSE2 node if we are using these motion models - + // This must be a NodeHybrid or NodeLattice if we are using these motion models + if (_motion_model == MotionModel::DUBIN || _motion_model == MotionModel::REEDS_SHEPP || + _motion_model == MotionModel::STATE_LATTICE) + { // See if we are closer and should be expanding more often const Coordinates node_coords = NodeT::getCoords(current_node->getIndex(), getSizeX(), getSizeDim3()); - closest_distance = - std::min( + closest_distance = std::min( closest_distance, - static_cast(NodeT::getHeuristicCost( - node_coords, - _goal_coordinates) / NodeT::neutral_cost) - ); + static_cast(NodeT::getHeuristicCost(node_coords, _goal_coordinates, _costmap))); + // We want to expand at a rate of d/expansion_ratio, // but check to see if we are so close that we would be expanding every iteration // If so, limit it to the expansion ratio (rounded up) int desired_iterations = std::max( static_cast(closest_distance / _search_info.analytic_expansion_ratio), - static_cast(std::ceil(_search_info.analytic_expansion_ratio)) - ); + static_cast(std::ceil(_search_info.analytic_expansion_ratio))); + // If we are closer now, we should update the target number of iterations to go analytic_iterations = std::min(analytic_iterations, desired_iterations); @@ -634,18 +498,172 @@ typename AStarAlgorithm::NodePtr AStarAlgorithm::tryAnalyticExpans // Always run the expansion on the first run in case there is a // trivial path to be found if (analytic_iterations <= 0) { - // Reset the counter, and try the analytic path expansion + // Reset the counter and try the analytic path expansion analytic_iterations = desired_iterations; - return getAnalyticPath(current_node, getter); + AnalyticExpansionNodes analytic_nodes = getAnalyticPath(current_node, getter); + if (!analytic_nodes.empty()) { + // If we have a valid path, attempt to refine it + NodePtr node = current_node; + NodePtr test_node = current_node; + AnalyticExpansionNodes refined_analytic_nodes; + for (int i = 0; i < 8; i++) { + // Attempt to create better paths in 5 node increments, need to make sure + // they exist for each in order to do so (maximum of 40 points back). + if (test_node->parent && test_node->parent->parent && test_node->parent->parent->parent && + test_node->parent->parent->parent->parent && + test_node->parent->parent->parent->parent->parent) + { + test_node = test_node->parent->parent->parent->parent->parent; + refined_analytic_nodes = getAnalyticPath(test_node, getter); + if (refined_analytic_nodes.empty()) { + break; + } + analytic_nodes = refined_analytic_nodes; + node = test_node; + } else { + break; + } + } + + return setAnalyticPath(node, analytic_nodes); + } } + analytic_iterations--; } + // No valid motion model - return nullptr return NodePtr(nullptr); } +template +typename AStarAlgorithm::AnalyticExpansionNodes AStarAlgorithm::getAnalyticPath( + const NodePtr & node, + const NodeGetter & node_getter) +{ + static ompl::base::ScopedState<> from(node->motion_table.state_space), to( + node->motion_table.state_space), s(node->motion_table.state_space); + from[0] = node->pose.x; + from[1] = node->pose.y; + from[2] = node->pose.theta * node->motion_table.bin_size; + to[0] = _goal_coordinates.x; + to[1] = _goal_coordinates.y; + to[2] = _goal_coordinates.theta * node->motion_table.bin_size; + + float d = node->motion_table.state_space->distance(from(), to()); + + // A move of sqrt(2) is guaranteed to be in a new cell + static const float sqrt_2 = std::sqrt(2.); + unsigned int num_intervals = std::floor(d / sqrt_2); + + AnalyticExpansionNodes possible_nodes; + possible_nodes.reserve(num_intervals - 1); // We won't store this node or the goal + std::vector reals; + + // Pre-allocate + NodePtr prev(node); + unsigned int index = 0; + NodePtr next(nullptr); + float angle = 0.0; + Coordinates proposed_coordinates; + bool failure = false; + + // Check intermediary poses (non-goal, non-start) + for (float i = 1; i < num_intervals; i++) { + node->motion_table.state_space->interpolate(from(), to(), i / num_intervals, s()); + reals = s.reals(); + angle = reals[2] / node->motion_table.bin_size; + while (angle >= node->motion_table.num_angle_quantization_float) { + angle -= node->motion_table.num_angle_quantization_float; + } + while (angle < 0.0) { + angle += node->motion_table.num_angle_quantization_float; + } + // Turn the pose into a node, and check if it is valid + index = NodeT::getIndex( + static_cast(reals[0]), + static_cast(reals[1]), + static_cast(angle)); + // Get the node from the graph + if (node_getter(index, next)) { + Coordinates initial_node_coords = next->pose; + proposed_coordinates = {static_cast(reals[0]), static_cast(reals[1]), angle}; + next->setPose(proposed_coordinates); + if (next->isNodeValid(_traverse_unknown, _collision_checker) && next != prev) { + // Save the node, and its previous coordinates in case we need to abort + possible_nodes.emplace_back(next, initial_node_coords, proposed_coordinates); + prev = next; + } else { + // Abort + next->setPose(initial_node_coords); + failure = true; + break; + } + } else { + // Abort + failure = true; + break; + } + } + + // Reset to initial poses to not impact future searches + for (const auto & node_pose : possible_nodes) { + const auto & n = node_pose.node; + n->setPose(node_pose.initial_coords); + } + + if (failure) { + return AnalyticExpansionNodes(); + } + + return possible_nodes; +} + +template +typename AStarAlgorithm::NodePtr AStarAlgorithm::setAnalyticPath( + const NodePtr & node, + const AnalyticExpansionNodes & expanded_nodes) +{ + // Legitimate final path - set the parent relationships & poses + NodePtr prev = node; + for (const auto & node_pose : expanded_nodes) { + const auto & n = node_pose.node; + if (!n->wasVisited() && n->getIndex() != _goal->getIndex()) { + // Make sure this node has not been visited by the regular algorithm. + // If it has been, there is the (slight) chance that it is in the path we are expanding + // from, so we should skip it. + // Skipping to the next node will still create a kinematically feasible path. + n->parent = prev; + n->pose = node_pose.proposed_coords; + n->visited(); + prev = n; + } + } + if (_goal != prev) { + _goal->parent = prev; + _goal->visited(); + } + return _goal; +} + +template<> +typename AStarAlgorithm::AnalyticExpansionNodes AStarAlgorithm::getAnalyticPath( + const NodePtr & node, + const NodeGetter & node_getter) +{ + return AnalyticExpansionNodes(); +} + +template<> +typename AStarAlgorithm::NodePtr AStarAlgorithm::setAnalyticPath( + const NodePtr & node, + const AnalyticExpansionNodes & expanded_nodes) +{ + return NodePtr(nullptr); +} + // Instantiate algorithm for the supported template types template class AStarAlgorithm; -template class AStarAlgorithm; +template class AStarAlgorithm; } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/src/costmap_downsampler.cpp b/nav2_smac_planner/src/costmap_downsampler.cpp index c722523423..32bc05b36a 100644 --- a/nav2_smac_planner/src/costmap_downsampler.cpp +++ b/nav2_smac_planner/src/costmap_downsampler.cpp @@ -48,18 +48,24 @@ void CostmapDownsampler::on_configure( _downsampled_size_x, _downsampled_size_y, _downsampled_resolution, _costmap->getOriginX(), _costmap->getOriginY(), UNKNOWN); - _downsampled_costmap_pub = std::make_unique( - node, _downsampled_costmap.get(), global_frame, topic_name, false); + if (!node.expired()) { + _downsampled_costmap_pub = std::make_unique( + node, _downsampled_costmap.get(), global_frame, topic_name, false); + } } void CostmapDownsampler::on_activate() { - _downsampled_costmap_pub->on_activate(); + if (_downsampled_costmap_pub) { + _downsampled_costmap_pub->on_activate(); + } } void CostmapDownsampler::on_deactivate() { - _downsampled_costmap_pub->on_deactivate(); + if (_downsampled_costmap_pub) { + _downsampled_costmap_pub->on_deactivate(); + } } void CostmapDownsampler::on_cleanup() @@ -90,7 +96,9 @@ nav2_costmap_2d::Costmap2D * CostmapDownsampler::downsample( } } - _downsampled_costmap_pub->publishCostmap(); + if (_downsampled_costmap_pub) { + _downsampled_costmap_pub->publishCostmap(); + } return _downsampled_costmap.get(); } diff --git a/nav2_smac_planner/src/node_2d.cpp b/nav2_smac_planner/src/node_2d.cpp index cd8a223879..a9ed0abf58 100644 --- a/nav2_smac_planner/src/node_2d.cpp +++ b/nav2_smac_planner/src/node_2d.cpp @@ -23,11 +23,11 @@ namespace nav2_smac_planner // defining static member for all instance to share std::vector Node2D::_neighbors_grid_offsets; -double Node2D::neutral_cost = 50.0; +float Node2D::cost_travel_multiplier = 2.0; -Node2D::Node2D(unsigned char & cost_in, const unsigned int index) +Node2D::Node2D(const unsigned int index) : parent(nullptr), - _cell_cost(static_cast(cost_in)), + _cell_cost(std::numeric_limits::quiet_NaN()), _accumulated_cost(std::numeric_limits::max()), _index(index), _was_visited(false), @@ -40,10 +40,10 @@ Node2D::~Node2D() parent = nullptr; } -void Node2D::reset(const unsigned char & cost) +void Node2D::reset() { parent = nullptr; - _cell_cost = static_cast(cost); + _cell_cost = std::numeric_limits::quiet_NaN(); _accumulated_cost = std::numeric_limits::max(); _was_visited = false; _is_queued = false; @@ -51,54 +51,53 @@ void Node2D::reset(const unsigned char & cost) bool Node2D::isNodeValid( const bool & traverse_unknown, - GridCollisionChecker /*collision_checker*/) + GridCollisionChecker * collision_checker) { - // NOTE(stevemacenski): Right now, we do not check if the node has wrapped around - // the regular grid (e.g. your node is on the edge of the costmap and i+1 - // goes to the other side). This check would add compute time and my assertion is - // that if you do wrap around, the heuristic will be so high it'll be added far - // in the queue that it will never be called if a valid path exists. - // This is intentionally un-included to increase speed, but be aware. If this causes - // trouble, please file a ticket and we can address it then. - - auto & cost = this->getCost(); - - // occupied node - if (cost == OCCUPIED || cost == INSCRIBED) { - return false; - } - - // unknown node - if (cost == UNKNOWN && !traverse_unknown) { + if (collision_checker->inCollision(this->getIndex(), traverse_unknown)) { return false; } + _cell_cost = collision_checker->getCost(); return true; } float Node2D::getTraversalCost(const NodePtr & child) { - // cost to travel will be the cost of the cell's code + float normalized_cost = child->getCost() / 252.0; + const Coordinates A = getCoords(child->getIndex()); + const Coordinates B = getCoords(this->getIndex()); + const float & dx = A.x - B.x; + const float & dy = A.y - B.y; + static float sqrt_2 = sqrt(2); + + // If a diagonal move, travel cost is sqrt(2) not 1.0. + if ((dx * dx + dy * dy) > 1.05) { + return sqrt_2 + cost_travel_multiplier * normalized_cost; + } - // neutral_cost is neutral cost for cost just to travel anywhere (50) - // 0.8 is a scale factor to remap costs [0, 252] evenly from [50, 252] - return Node2D::neutral_cost + 0.8 * child->getCost(); + return 1.0 + cost_travel_multiplier * normalized_cost; } float Node2D::getHeuristicCost( const Coordinates & node_coords, - const Coordinates & goal_coordinates) + const Coordinates & goal_coordinates, + const nav2_costmap_2d::Costmap2D * /*costmap*/) { - return hypotf( - goal_coordinates.x - node_coords.x, - goal_coordinates.y - node_coords.y) * Node2D::neutral_cost; + // Using Moore distance as it more accurately represents the distances + // even a Van Neumann neighborhood robot can navigate. + return fabs(goal_coordinates.x - node_coords.x) + + fabs(goal_coordinates.y - node_coords.y); } -void Node2D::initNeighborhood( - const unsigned int & x_size_uint, - const MotionModel & neighborhood) +void Node2D::initMotionModel( + const MotionModel & neighborhood, + unsigned int & x_size_uint, + unsigned int & /*size_y*/, + unsigned int & /*num_angle_quantization*/, + SearchInfo & search_info) { int x_size = static_cast(x_size_uint); + cost_travel_multiplier = search_info.cost_penalty; switch (neighborhood) { case MotionModel::UNKNOWN: throw std::runtime_error("Unknown neighborhood type selected."); @@ -117,15 +116,14 @@ void Node2D::initNeighborhood( } void Node2D::getNeighbors( - NodePtr & node, std::function & NeighborGetter, - GridCollisionChecker collision_checker, + GridCollisionChecker * collision_checker, const bool & traverse_unknown, NodeVector & neighbors) { // NOTE(stevemacenski): Irritatingly, the order here matters. If you start in free // space and then expand 8-connected, the first set of neighbors will be all cost - // _neutral_cost. Then its expansion will all be 2 * _neutral_cost but now multiple + // 1.0. Then its expansion will all be 2 * 1.0 but now multiple // nodes are touching that node so the last cell to update the back pointer wins. // Thusly, the ordering ends with the cardinal directions for both sets such that // behavior is consistent in large free spaces between them. @@ -136,7 +134,7 @@ void Node2D::getNeighbors( // rather than a small inflation around the obstacles int index; NodePtr neighbor; - int node_i = node->getIndex(); + int node_i = this->getIndex(); for (unsigned int i = 0; i != _neighbors_grid_offsets.size(); ++i) { index = node_i + _neighbors_grid_offsets[i]; diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp new file mode 100644 index 0000000000..059f9f5445 --- /dev/null +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -0,0 +1,648 @@ +// Copyright (c) 2020, Samsung Research America +// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ompl/base/ScopedState.h" +#include "ompl/base/spaces/DubinsStateSpace.h" +#include "ompl/base/spaces/ReedsSheppStateSpace.h" + +#include "nav2_smac_planner/node_hybrid.hpp" + +using namespace std::chrono; // NOLINT + +namespace nav2_smac_planner +{ + +// defining static member for all instance to share +LookupTable NodeHybrid::obstacle_heuristic_lookup_table; +std::queue NodeHybrid::obstacle_heuristic_queue; +double NodeHybrid::travel_distance_cost = sqrt(2); +HybridMotionTable NodeHybrid::motion_table; +float NodeHybrid::size_lookup = 25; +LookupTable NodeHybrid::dist_heuristic_lookup_table; +nav2_costmap_2d::Costmap2D * NodeHybrid::sampled_costmap = nullptr; +CostmapDownsampler NodeHybrid::downsampler; + +// Each of these tables are the projected motion models through +// time and space applied to the search on the current node in +// continuous map-coordinates (e.g. not meters but partial map cells) +// Currently, these are set to project *at minimum* into a neighboring +// cell. Though this could be later modified to project a certain +// amount of time or particular distance forward. + +// http://planning.cs.uiuc.edu/node821.html +// Model for ackermann style vehicle with minimum radius restriction +void HybridMotionTable::initDubin( + unsigned int & size_x_in, + unsigned int & /*size_y_in*/, + unsigned int & num_angle_quantization_in, + SearchInfo & search_info) +{ + size_x = size_x_in; + change_penalty = search_info.change_penalty; + non_straight_penalty = search_info.non_straight_penalty; + cost_penalty = search_info.cost_penalty; + reverse_penalty = search_info.reverse_penalty; + + // if nothing changed, no need to re-compute primitives + if (num_angle_quantization_in == num_angle_quantization && + min_turning_radius == search_info.minimum_turning_radius) + { + return; + } + + num_angle_quantization = num_angle_quantization_in; + num_angle_quantization_float = static_cast(num_angle_quantization); + min_turning_radius = search_info.minimum_turning_radius; + + // angle must meet 3 requirements: + // 1) be increment of quantized bin size + // 2) chord length must be greater than sqrt(2) to leave current cell + // 3) maximum curvature must be respected, represented by minimum turning angle + // Thusly: + // On circle of radius minimum turning angle, we need select motion primatives + // with chord length > sqrt(2) and be an increment of our bin size + // + // chord >= sqrt(2) >= 2 * R * sin (angle / 2); where angle / N = quantized bin size + // Thusly: angle <= 2.0 * asin(sqrt(2) / (2 * R)) + float angle = 2.0 * asin(sqrt(2.0) / (2 * min_turning_radius)); + // Now make sure angle is an increment of the quantized bin size + // And since its based on the minimum chord, we need to make sure its always larger + bin_size = + 2.0f * static_cast(M_PI) / static_cast(num_angle_quantization); + float increments; + if (angle < bin_size) { + increments = 1.0f; + } else { + // Search dimensions are clean multiples of quantization - this prevents + // paths with loops in them + increments = ceil(angle / bin_size); + } + angle = increments * bin_size; + + // find deflections + // If we make a right triangle out of the chord in circle of radius + // min turning angle, we can see that delta X = R * sin (angle) + float delta_x = min_turning_radius * sin(angle); + // Using that same right triangle, we can see that the complement + // to delta Y is R * cos (angle). If we subtract R, we get the actual value + float delta_y = min_turning_radius - (min_turning_radius * cos(angle)); + + projections.clear(); + projections.reserve(3); + projections.emplace_back(hypotf(delta_x, delta_y), 0.0, 0.0); // Forward + projections.emplace_back(delta_x, delta_y, increments); // Left + projections.emplace_back(delta_x, -delta_y, -increments); // Right + + // Create the correct OMPL state space + if (!state_space) { + state_space = std::make_unique(min_turning_radius); + } + + // Precompute projection deltas + delta_xs.resize(projections.size()); + delta_ys.resize(projections.size()); + trig_values.resize(num_angle_quantization); + + for (unsigned int i = 0; i != projections.size(); i++) { + delta_xs[i].resize(num_angle_quantization); + delta_ys[i].resize(num_angle_quantization); + + for (unsigned int j = 0; j != num_angle_quantization; j++) { + double cos_theta = cos(bin_size * j); + double sin_theta = sin(bin_size * j); + if (i == 0) { + // if first iteration, cache the trig values for later + trig_values[j] = {cos_theta, sin_theta}; + } + delta_xs[i][j] = projections[i]._x * cos_theta - projections[i]._y * sin_theta; + delta_ys[i][j] = projections[i]._x * sin_theta + projections[i]._y * cos_theta; + } + } +} + +// http://planning.cs.uiuc.edu/node822.html +// Same as Dubin model but now reverse is valid +// See notes in Dubin for explanation +void HybridMotionTable::initReedsShepp( + unsigned int & size_x_in, + unsigned int & /*size_y_in*/, + unsigned int & num_angle_quantization_in, + SearchInfo & search_info) +{ + size_x = size_x_in; + change_penalty = search_info.change_penalty; + non_straight_penalty = search_info.non_straight_penalty; + cost_penalty = search_info.cost_penalty; + reverse_penalty = search_info.reverse_penalty; + + // if nothing changed, no need to re-compute primitives + if (num_angle_quantization_in == num_angle_quantization && + min_turning_radius == search_info.minimum_turning_radius) + { + return; + } + + num_angle_quantization = num_angle_quantization_in; + num_angle_quantization_float = static_cast(num_angle_quantization); + min_turning_radius = search_info.minimum_turning_radius; + + float angle = 2.0 * asin(sqrt(2.0) / (2 * min_turning_radius)); + bin_size = + 2.0f * static_cast(M_PI) / static_cast(num_angle_quantization); + float increments; + if (angle < bin_size) { + increments = 1.0f; + } else { + increments = ceil(angle / bin_size); + } + angle = increments * bin_size; + + float delta_x = min_turning_radius * sin(angle); + float delta_y = min_turning_radius - (min_turning_radius * cos(angle)); + + projections.clear(); + projections.reserve(6); + projections.emplace_back(hypotf(delta_x, delta_y), 0.0, 0.0); // Forward + projections.emplace_back(delta_x, delta_y, increments); // Forward + Left + projections.emplace_back(delta_x, -delta_y, -increments); // Forward + Right + projections.emplace_back(-hypotf(delta_x, delta_y), 0.0, 0.0); // Backward + projections.emplace_back(-delta_x, delta_y, -increments); // Backward + Left + projections.emplace_back(-delta_x, -delta_y, increments); // Backward + Right + + // Create the correct OMPL state space + if (!state_space) { + state_space = std::make_unique( + min_turning_radius); + } + + // Precompute projection deltas + delta_xs.resize(projections.size()); + delta_ys.resize(projections.size()); + trig_values.resize(num_angle_quantization); + + for (unsigned int i = 0; i != projections.size(); i++) { + delta_xs[i].resize(num_angle_quantization); + delta_ys[i].resize(num_angle_quantization); + + for (unsigned int j = 0; j != num_angle_quantization; j++) { + double cos_theta = cos(bin_size * j); + double sin_theta = sin(bin_size * j); + if (i == 0) { + // if first iteration, cache the trig values for later + trig_values[j] = {cos_theta, sin_theta}; + } + delta_xs[i][j] = projections[i]._x * cos_theta - projections[i]._y * sin_theta; + delta_ys[i][j] = projections[i]._x * sin_theta + projections[i]._y * cos_theta; + } + } +} + +MotionPoses HybridMotionTable::getProjections(const NodeHybrid * node) +{ + MotionPoses projection_list; + projection_list.reserve(projections.size()); + + for (unsigned int i = 0; i != projections.size(); i++) { + const MotionPose & motion_model = projections[i]; + + // normalize theta, I know its overkill, but I've been burned before... + const float & node_heading = node->pose.theta; + float new_heading = node_heading + motion_model._theta; + + if (new_heading >= num_angle_quantization_float) { + new_heading -= num_angle_quantization_float; + } + + if (new_heading < 0.0) { + new_heading += num_angle_quantization_float; + } + + projection_list.emplace_back( + delta_xs[i][node_heading] + node->pose.x, + delta_ys[i][node_heading] + node->pose.y, + new_heading); + } + + return projection_list; +} + +NodeHybrid::NodeHybrid(const unsigned int index) +: parent(nullptr), + pose(0.0f, 0.0f, 0.0f), + _cell_cost(std::numeric_limits::quiet_NaN()), + _accumulated_cost(std::numeric_limits::max()), + _index(index), + _was_visited(false), + _motion_primitive_index(std::numeric_limits::max()) +{ +} + +NodeHybrid::~NodeHybrid() +{ + parent = nullptr; +} + +void NodeHybrid::reset() +{ + parent = nullptr; + _cell_cost = std::numeric_limits::quiet_NaN(); + _accumulated_cost = std::numeric_limits::max(); + _was_visited = false; + _motion_primitive_index = std::numeric_limits::max(); + pose.x = 0.0f; + pose.y = 0.0f; + pose.theta = 0.0f; +} + +bool NodeHybrid::isNodeValid( + const bool & traverse_unknown, + GridCollisionChecker * collision_checker) +{ + if (collision_checker->inCollision( + this->pose.x, this->pose.y, this->pose.theta * motion_table.bin_size, traverse_unknown)) + { + return false; + } + + _cell_cost = collision_checker->getCost(); + return true; +} + +float NodeHybrid::getTraversalCost(const NodePtr & child) +{ + const float normalized_cost = child->getCost() / 252.0; + if (std::isnan(normalized_cost)) { + throw std::runtime_error( + "Node attempted to get traversal " + "cost without a known SE2 collision cost!"); + } + + // this is the first node + if (getMotionPrimitiveIndex() == std::numeric_limits::max()) { + return NodeHybrid::travel_distance_cost; + } + + // Note(stevemacenski): `travel_cost_raw` at one point contained a term: + // `+ motion_table.cost_penalty * normalized_cost;` + // It has been removed, but we may want to readdress this point and determine + // the technically and theoretically correctness of that choice. I feel technically speaking + // that term has merit, but it doesn't seem to impact performance or path quality. + // W/o it lowers the travel cost, which would drive the heuristics up proportionally where I + // would expect it to plan much faster in all cases, but I only see it in some cases. Since + // this term would weight against moving to high cost zones, I would expect to see more smooth + // central motion, but I only see it in some cases, possibly because the obstacle heuristic is + // already driving the robot away from high cost areas; implicitly handling this. However, + // then I would expect that not adding it here would make it unbalanced enough that path quality + // would suffer, which I did not see in my limited experimentation, possibly due to the smoother. + float travel_cost = 0.0; + float travel_cost_raw = NodeHybrid::travel_distance_cost; + + if (child->getMotionPrimitiveIndex() == 0 || child->getMotionPrimitiveIndex() == 3) { + // New motion is a straight motion, no additional costs to be applied + travel_cost = travel_cost_raw; + } else { + if (getMotionPrimitiveIndex() == child->getMotionPrimitiveIndex()) { + // Turning motion but keeps in same direction: encourages to commit to turning if starting it + travel_cost = travel_cost_raw * motion_table.non_straight_penalty; + } else { + // Turning motion and changing direction: penalizes wiggling + travel_cost = travel_cost_raw * + (motion_table.non_straight_penalty + motion_table.change_penalty); + } + } + + if (getMotionPrimitiveIndex() > 2) { + // reverse direction + travel_cost *= motion_table.reverse_penalty; + } + + return travel_cost; +} + +float NodeHybrid::getHeuristicCost( + const Coordinates & node_coords, + const Coordinates & goal_coords, + const nav2_costmap_2d::Costmap2D * /*costmap*/) +{ + const float obstacle_heuristic = getObstacleHeuristic(node_coords, goal_coords); + const float dist_heuristic = getDistanceHeuristic(node_coords, goal_coords, obstacle_heuristic); + return std::max(obstacle_heuristic, dist_heuristic); +} + +void NodeHybrid::initMotionModel( + const MotionModel & motion_model, + unsigned int & size_x, + unsigned int & size_y, + unsigned int & num_angle_quantization, + SearchInfo & search_info) +{ + // find the motion model selected + switch (motion_model) { + case MotionModel::DUBIN: + motion_table.initDubin(size_x, size_y, num_angle_quantization, search_info); + break; + case MotionModel::REEDS_SHEPP: + motion_table.initReedsShepp(size_x, size_y, num_angle_quantization, search_info); + break; + default: + throw std::runtime_error( + "Invalid motion model for Hybrid A*. Please select between" + " Dubin (Ackermann forward only)," + " Reeds-Shepp (Ackermann forward and back)."); + } + + travel_distance_cost = motion_table.projections[0]._x; +} + +void NodeHybrid::resetObstacleHeuristic( + nav2_costmap_2d::Costmap2D * costmap, + const unsigned int & goal_x, const unsigned int & goal_y) +{ + // Downsample costmap 2x to compute a sparse obstacle heuristic. This speeds up + // the planner considerably to search through 75% less cells with no detectable + // erosion of path quality after even modest smoothing. The error would be no more + // than 0.05 * normalized cost. Since this is just a search prior, there's no loss in generality + std::weak_ptr ptr; + downsampler.on_configure(ptr, "fake_frame", "fake_topic", costmap, 2.0); + downsampler.on_activate(); + sampled_costmap = downsampler.downsample(2.0); + + // Clear lookup table + unsigned int size = sampled_costmap->getSizeInCellsX() * sampled_costmap->getSizeInCellsY(); + if (obstacle_heuristic_lookup_table.size() == size) { + // must reset all values + std::fill( + obstacle_heuristic_lookup_table.begin(), + obstacle_heuristic_lookup_table.end(), 0.0); + } else { + unsigned int obstacle_size = obstacle_heuristic_lookup_table.size(); + obstacle_heuristic_lookup_table.resize(size, 0.0); + // must reset values for non-constructed indices + std::fill_n( + obstacle_heuristic_lookup_table.begin(), obstacle_size, 0.0); + } + + // Set initial goal point to queue from. Divided by 2 due to downsampled costmap. + std::queue q; + std::swap(obstacle_heuristic_queue, q); + obstacle_heuristic_queue.emplace( + ceil(goal_y / 2.0) * sampled_costmap->getSizeInCellsX() + ceil(goal_x / 2.0)); +} + +float NodeHybrid::getObstacleHeuristic( + const Coordinates & node_coords, + const Coordinates & goal_coords) +{ + // If already expanded, return the cost + unsigned int size_x = sampled_costmap->getSizeInCellsX(); + // Divided by 2 due to downsampled costmap. + const unsigned int start_index = ceil(node_coords.y / 2.0) * size_x + ceil(node_coords.x / 2.0); + const float & starting_cost = obstacle_heuristic_lookup_table[start_index]; + if (starting_cost > 0.0) { + // costs are doubled due to downsampling + return 2.0 * starting_cost; + } + + // If not, expand until it is included. This dynamic programming ensures that + // we only expand the MINIMUM spanning set of the costmap per planning request. + // Rather than naively expanding the entire (potentially massive) map for a limited + // path, we only expand to the extent required for the furthest expansion in the + // search-planning request that dynamically updates during search as needed. + const int size_x_int = static_cast(size_x); + const unsigned int size_y = sampled_costmap->getSizeInCellsY(); + const float sqrt_2 = sqrt(2); + unsigned int mx, my, mx_idx, my_idx; + unsigned int idx = 0, new_idx = 0; + float last_accumulated_cost = 0.0, travel_cost = 0.0; + float heuristic_cost = 0.0, current_accumulated_cost = 0.0; + float cost = 0.0, existing_cost = 0.0; + + const std::vector neighborhood = {1, -1, // left right + size_x_int, -size_x_int, // up down + size_x_int + 1, size_x_int - 1, // upper diagonals + -size_x_int + 1, -size_x_int - 1}; // lower diagonals + + while (!obstacle_heuristic_queue.empty()) { + // get next one + idx = obstacle_heuristic_queue.front(); + obstacle_heuristic_queue.pop(); + last_accumulated_cost = obstacle_heuristic_lookup_table[idx]; + + + if (idx == start_index) { + // costs are doubled due to downsampling + return 2.0 * last_accumulated_cost; + } + + my_idx = idx / size_x; + mx_idx = idx - (my_idx * size_x); + + // find neighbors + for (unsigned int i = 0; i != neighborhood.size(); i++) { + new_idx = static_cast(static_cast(idx) + neighborhood[i]); + cost = static_cast(sampled_costmap->getCost(idx)); + travel_cost = + ((i <= 3) ? 1.0 : sqrt_2) + (motion_table.cost_penalty * cost / 252.0); + current_accumulated_cost = last_accumulated_cost + travel_cost; + + // if neighbor path is better and non-lethal, set new cost and add to queue + if (new_idx > 0 && new_idx < size_x * size_y && cost < INSCRIBED) { + my = new_idx / size_x; + mx = new_idx - (my * size_x); + + if (mx == 0 && mx_idx >= size_x - 1 || mx >= size_x - 1 && mx_idx == 0) { + continue; + } + if (my == 0 && my_idx >= size_y - 1 || my >= size_y - 1 && my_idx == 0) { + continue; + } + + existing_cost = obstacle_heuristic_lookup_table[new_idx]; + if (existing_cost == 0.0 || existing_cost > current_accumulated_cost) { + obstacle_heuristic_lookup_table[new_idx] = current_accumulated_cost; + obstacle_heuristic_queue.emplace(new_idx); + } + } + } + } + + // costs are doubled due to downsampling + return 2.0 * obstacle_heuristic_lookup_table[start_index]; +} + +float NodeHybrid::getDistanceHeuristic( + const Coordinates & node_coords, + const Coordinates & goal_coords, + const float & obstacle_heuristic) +{ + // rotate and translate node_coords such that goal_coords relative is (0,0,0) + // Due to the rounding involved in exact cell increments for caching, + // this is not an exact replica of a live heuristic, but has bounded error. + // (Usually less than 1 cell) + + // This angle is negative since we are de-rotating the current node + // by the goal angle; cos(-th) = cos(th) & sin(-th) = -sin(th) + const TrigValues & trig_vals = motion_table.trig_values[goal_coords.theta]; + const float cos_th = trig_vals.first; + const float sin_th = -trig_vals.second; + const float dx = node_coords.x - goal_coords.x; + const float dy = node_coords.y - goal_coords.y; + + double dtheta_bin = node_coords.theta - goal_coords.theta; + if (dtheta_bin > motion_table.num_angle_quantization) { + dtheta_bin -= motion_table.num_angle_quantization; + } else if (dtheta_bin < 0) { + dtheta_bin += motion_table.num_angle_quantization; + } + + Coordinates node_coords_relative( + round(dx * cos_th - dy * sin_th), + round(dx * sin_th + dy * cos_th), + round(dtheta_bin)); + + // Check if the relative node coordinate is within the localized window around the goal + // to apply the distance heuristic. Since the lookup table is contains only the positive + // X axis, we mirror the Y and theta values across the X axis to find the heuristic values. + float motion_heuristic = 0.0; + const int floored_size = floor(size_lookup / 2.0); + const int ceiling_size = ceil(size_lookup / 2.0); + const float mirrored_relative_y = abs(node_coords_relative.y); + if (abs(node_coords_relative.x) < floored_size && mirrored_relative_y < floored_size) { + // Need to mirror angle if Y coordinate was mirrored + int theta_pos; + if (node_coords_relative.y < 0.0) { + theta_pos = motion_table.num_angle_quantization - node_coords_relative.theta; + } else { + theta_pos = node_coords_relative.theta; + } + const int x_pos = node_coords_relative.x + floored_size; + const int y_pos = static_cast(mirrored_relative_y); + const int index = + x_pos * ceiling_size * motion_table.num_angle_quantization + + y_pos * motion_table.num_angle_quantization + + theta_pos; + motion_heuristic = dist_heuristic_lookup_table[index]; + } else if (obstacle_heuristic == 0.0) { + // If no obstacle heuristic value, must have some H to use + // In nominal situations, this should never be called. + static ompl::base::ScopedState<> from(motion_table.state_space), to(motion_table.state_space); + to[0] = goal_coords.x; + to[1] = goal_coords.y; + to[2] = goal_coords.theta * motion_table.num_angle_quantization; + from[0] = node_coords.x; + from[1] = node_coords.y; + from[2] = node_coords.theta * motion_table.num_angle_quantization; + motion_heuristic = motion_table.state_space->distance(from(), to()); + } + + return motion_heuristic; +} + +void NodeHybrid::precomputeDistanceHeuristic( + const float & lookup_table_dim, + const MotionModel & motion_model, + const unsigned int & dim_3_size, + const SearchInfo & search_info) +{ + // Dubin or Reeds-Shepp shortest distances + if (motion_model == MotionModel::DUBIN) { + motion_table.state_space = std::make_unique( + search_info.minimum_turning_radius); + } else if (motion_model == MotionModel::REEDS_SHEPP) { + motion_table.state_space = std::make_unique( + search_info.minimum_turning_radius); + } else { + throw std::runtime_error( + "Node attempted to precompute distance heuristics " + "with invalid motion model!"); + } + + ompl::base::ScopedState<> from(motion_table.state_space), to(motion_table.state_space); + to[0] = 0.0; + to[1] = 0.0; + to[2] = 0.0; + size_lookup = lookup_table_dim; + float motion_heuristic = 0.0; + unsigned int index = 0; + int dim_3_size_int = static_cast(dim_3_size); + float angular_bin_size = 2 * M_PI / static_cast(dim_3_size); + + // Create a lookup table of Dubin/Reeds-Shepp distances in a window around the goal + // to help drive the search towards admissible approaches. Deu to symmetries in the + // Heuristic space, we need to only store 2 of the 4 quadrants and simply mirror + // around the X axis any relative node lookup. This reduces memory overhead and increases + // the size of a window a platform can store in memory. + dist_heuristic_lookup_table.resize(size_lookup * ceil(size_lookup / 2.0) * dim_3_size_int); + for (float x = ceil(-size_lookup / 2.0); x <= floor(size_lookup / 2.0); x += 1.0) { + for (float y = 0.0; y <= floor(size_lookup / 2.0); y += 1.0) { + for (int heading = 0; heading != dim_3_size_int; heading++) { + from[0] = x; + from[1] = y; + from[2] = heading * angular_bin_size; + motion_heuristic = motion_table.state_space->distance(from(), to()); + dist_heuristic_lookup_table[index] = motion_heuristic; + index++; + } + } + } +} + +void NodeHybrid::getNeighbors( + std::function & NeighborGetter, + GridCollisionChecker * collision_checker, + const bool & traverse_unknown, + NodeVector & neighbors) +{ + unsigned int index = 0; + NodePtr neighbor = nullptr; + Coordinates initial_node_coords; + const MotionPoses motion_projections = motion_table.getProjections(this); + + for (unsigned int i = 0; i != motion_projections.size(); i++) { + index = NodeHybrid::getIndex( + static_cast(motion_projections[i]._x), + static_cast(motion_projections[i]._y), + static_cast(motion_projections[i]._theta), + motion_table.size_x, motion_table.num_angle_quantization); + + if (NeighborGetter(index, neighbor) && !neighbor->wasVisited()) { + // Cache the initial pose in case it was visited but valid + // don't want to disrupt continuous coordinate expansion + initial_node_coords = neighbor->pose; + neighbor->setPose( + Coordinates( + motion_projections[i]._x, + motion_projections[i]._y, + motion_projections[i]._theta)); + if (neighbor->isNodeValid(traverse_unknown, collision_checker)) { + neighbor->setMotionPrimitiveIndex(i); + neighbors.push_back(neighbor); + } else { + neighbor->setPose(initial_node_coords); + } + } + } +} + +} // namespace nav2_smac_planner diff --git a/nav2_smac_planner/src/node_se2.cpp b/nav2_smac_planner/src/node_se2.cpp deleted file mode 100644 index 96ab084b8a..0000000000 --- a/nav2_smac_planner/src/node_se2.cpp +++ /dev/null @@ -1,462 +0,0 @@ -// Copyright (c) 2020, Samsung Research America -// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. Reserved. - -#include -#include -#include -#include -#include -#include -#include - -#include "ompl/base/ScopedState.h" -#include "ompl/base/spaces/DubinsStateSpace.h" -#include "ompl/base/spaces/ReedsSheppStateSpace.h" - -#include "nav2_smac_planner/node_se2.hpp" - -using namespace std::chrono; // NOLINT - -namespace nav2_smac_planner -{ - -// defining static member for all instance to share -std::vector NodeSE2::_wavefront_heuristic; -double NodeSE2::neutral_cost = sqrt(2); -MotionTable NodeSE2::motion_table; - -// Each of these tables are the projected motion models through -// time and space applied to the search on the current node in -// continuous map-coordinates (e.g. not meters but partial map cells) -// Currently, these are set to project *at minimum* into a neighboring -// cell. Though this could be later modified to project a certain -// amount of time or particular distance forward. - -// http://planning.cs.uiuc.edu/node821.html -// Model for ackermann style vehicle with minimum radius restriction -void MotionTable::initDubin( - unsigned int & size_x_in, - unsigned int & /*size_y_in*/, - unsigned int & num_angle_quantization_in, - SearchInfo & search_info) -{ - size_x = size_x_in; - num_angle_quantization = num_angle_quantization_in; - num_angle_quantization_float = static_cast(num_angle_quantization); - change_penalty = search_info.change_penalty; - non_straight_penalty = search_info.non_straight_penalty; - cost_penalty = search_info.cost_penalty; - reverse_penalty = search_info.reverse_penalty; - - // angle must meet 3 requirements: - // 1) be increment of quantized bin size - // 2) chord length must be greater than sqrt(2) to leave current cell - // 3) maximum curvature must be respected, represented by minimum turning angle - // Thusly: - // On circle of radius minimum turning angle, we need select motion primatives - // with chord length > sqrt(2) and be an increment of our bin size - // - // chord >= sqrt(2) >= 2 * R * sin (angle / 2); where angle / N = quantized bin size - // Thusly: angle <= 2.0 * asin(sqrt(2) / (2 * R)) - float angle = 2.0 * asin(sqrt(2.0) / (2 * search_info.minimum_turning_radius)); - // Now make sure angle is an increment of the quantized bin size - // And since its based on the minimum chord, we need to make sure its always larger - bin_size = - 2.0f * static_cast(M_PI) / static_cast(num_angle_quantization); - float increments; - if (angle < bin_size) { - increments = 1.0f; - } else { - // Search dimensions are clean multiples of quantization - this prevents - // paths with loops in them - increments = ceil(angle / bin_size); - } - angle = increments * bin_size; - - // find deflections - // If we make a right triangle out of the chord in circle of radius - // min turning angle, we can see that delta X = R * sin (angle) - float delta_x = search_info.minimum_turning_radius * sin(angle); - // Using that same right triangle, we can see that the complement - // to delta Y is R * cos (angle). If we subtract R, we get the actual value - float delta_y = search_info.minimum_turning_radius - - (search_info.minimum_turning_radius * cos(angle)); - - projections.clear(); - projections.reserve(3); - projections.emplace_back(hypotf(delta_x, delta_y), 0.0, 0.0); // Forward - projections.emplace_back(delta_x, delta_y, increments); // Left - projections.emplace_back(delta_x, -delta_y, -increments); // Right - - // Create the correct OMPL state space - state_space = std::make_unique(search_info.minimum_turning_radius); - - // Precompute projection deltas - delta_xs.resize(projections.size()); - delta_ys.resize(projections.size()); - - for (unsigned int i = 0; i != projections.size(); i++) { - delta_xs[i].resize(num_angle_quantization); - delta_ys[i].resize(num_angle_quantization); - - for (unsigned int j = 0; j != num_angle_quantization; j++) { - double cos_theta = cos(bin_size * j); - double sin_theta = sin(bin_size * j); - delta_xs[i][j] = projections[i]._x * cos_theta - projections[i]._y * sin_theta; - delta_ys[i][j] = projections[i]._x * sin_theta + projections[i]._y * cos_theta; - } - } -} - -// http://planning.cs.uiuc.edu/node822.html -// Same as Dubin model but now reverse is valid -// See notes in Dubin for explanation -void MotionTable::initReedsShepp( - unsigned int & size_x_in, - unsigned int & /*size_y_in*/, - unsigned int & num_angle_quantization_in, - SearchInfo & search_info) -{ - size_x = size_x_in; - num_angle_quantization = num_angle_quantization_in; - num_angle_quantization_float = static_cast(num_angle_quantization); - change_penalty = search_info.change_penalty; - non_straight_penalty = search_info.non_straight_penalty; - cost_penalty = search_info.cost_penalty; - reverse_penalty = search_info.reverse_penalty; - - float angle = 2.0 * asin(sqrt(2.0) / (2 * search_info.minimum_turning_radius)); - bin_size = - 2.0f * static_cast(M_PI) / static_cast(num_angle_quantization); - float increments; - if (angle < bin_size) { - increments = 1.0f; - } else { - increments = ceil(angle / bin_size); - } - angle = increments * bin_size; - - float delta_x = search_info.minimum_turning_radius * sin(angle); - float delta_y = search_info.minimum_turning_radius - - (search_info.minimum_turning_radius * cos(angle)); - - projections.clear(); - projections.reserve(6); - projections.emplace_back(hypotf(delta_x, delta_y), 0.0, 0.0); // Forward - projections.emplace_back(delta_x, delta_y, increments); // Forward + Left - projections.emplace_back(delta_x, -delta_y, -increments); // Forward + Right - projections.emplace_back(-hypotf(delta_x, delta_y), 0.0, 0.0); // Backward - projections.emplace_back(-delta_x, delta_y, -increments); // Backward + Left - projections.emplace_back(-delta_x, -delta_y, increments); // Backward + Right - - // Create the correct OMPL state space - state_space = std::make_unique( - search_info.minimum_turning_radius); - - // Precompute projection deltas - delta_xs.resize(projections.size()); - delta_ys.resize(projections.size()); - - for (unsigned int i = 0; i != projections.size(); i++) { - delta_xs[i].resize(num_angle_quantization); - delta_ys[i].resize(num_angle_quantization); - - for (unsigned int j = 0; j != num_angle_quantization; j++) { - double cos_theta = cos(bin_size * j); - double sin_theta = sin(bin_size * j); - delta_xs[i][j] = projections[i]._x * cos_theta - projections[i]._y * sin_theta; - delta_ys[i][j] = projections[i]._x * sin_theta + projections[i]._y * cos_theta; - } - } -} - -MotionPoses MotionTable::getProjections(const NodeSE2 * node) -{ - MotionPoses projection_list; - for (unsigned int i = 0; i != projections.size(); i++) { - projection_list.push_back(getProjection(node, i)); - } - - return projection_list; -} - -MotionPose MotionTable::getProjection(const NodeSE2 * node, const unsigned int & motion_index) -{ - const MotionPose & motion_model = projections[motion_index]; - - // normalize theta, I know its overkill, but I've been burned before... - const float & node_heading = node->pose.theta; - float new_heading = node_heading + motion_model._theta; - - while (new_heading >= num_angle_quantization_float) { - new_heading -= num_angle_quantization_float; - } - while (new_heading < 0.0) { - new_heading += num_angle_quantization_float; - } - - return MotionPose( - delta_xs[motion_index][node_heading] + node->pose.x, - delta_ys[motion_index][node_heading] + node->pose.y, - new_heading); -} - -NodeSE2::NodeSE2(const unsigned int index) -: parent(nullptr), - pose(0.0f, 0.0f, 0.0f), - _cell_cost(std::numeric_limits::quiet_NaN()), - _accumulated_cost(std::numeric_limits::max()), - _index(index), - _was_visited(false), - _is_queued(false), - _motion_primitive_index(std::numeric_limits::max()) -{ -} - -NodeSE2::~NodeSE2() -{ - parent = nullptr; -} - -void NodeSE2::reset() -{ - parent = nullptr; - _cell_cost = std::numeric_limits::quiet_NaN(); - _accumulated_cost = std::numeric_limits::max(); - _was_visited = false; - _is_queued = false; - _motion_primitive_index = std::numeric_limits::max(); - pose.x = 0.0f; - pose.y = 0.0f; - pose.theta = 0.0f; -} - -bool NodeSE2::isNodeValid(const bool & traverse_unknown, GridCollisionChecker collision_checker) -{ - if (collision_checker.inCollision( - this->pose.x, this->pose.y, this->pose.theta * motion_table.bin_size, traverse_unknown)) - { - return false; - } - - _cell_cost = collision_checker.getCost(); - return true; -} - -float NodeSE2::getTraversalCost(const NodePtr & child) -{ - const float normalized_cost = child->getCost() / 252.0; - if (std::isnan(normalized_cost)) { - throw std::runtime_error( - "Node attempted to get traversal " - "cost without a known SE2 collision cost!"); - } - - // this is the first node - if (getMotionPrimitiveIndex() == std::numeric_limits::max()) { - return NodeSE2::neutral_cost; - } - - float travel_cost = 0.0; - float travel_cost_raw = NodeSE2::neutral_cost + motion_table.cost_penalty * normalized_cost; - - if (child->getMotionPrimitiveIndex() == 0 || child->getMotionPrimitiveIndex() == 3) { - // straight motion, no additional costs to be applied - travel_cost = travel_cost_raw; - } else { - if (getMotionPrimitiveIndex() == child->getMotionPrimitiveIndex()) { - // Turning motion but keeps in same direction: encourages to commit to turning if starting it - travel_cost = travel_cost_raw * motion_table.non_straight_penalty; - } else { - // Turning motion and changing direction: penalizes wiggling - travel_cost = travel_cost_raw * motion_table.change_penalty; - travel_cost += travel_cost_raw * motion_table.non_straight_penalty; - } - } - - if (getMotionPrimitiveIndex() > 2) { - // reverse direction - travel_cost *= motion_table.reverse_penalty; - } - - return travel_cost; -} - -float NodeSE2::getHeuristicCost( - const Coordinates & node_coords, - const Coordinates & goal_coords) -{ - // Dubin or Reeds-Shepp shortest distances - // Create OMPL states for checking - ompl::base::ScopedState<> from(motion_table.state_space), to(motion_table.state_space); - from[0] = node_coords.x; - from[1] = node_coords.y; - from[2] = node_coords.theta * motion_table.bin_size; - to[0] = goal_coords.x; - to[1] = goal_coords.y; - to[2] = goal_coords.theta * motion_table.bin_size; - - const float motion_heuristic = motion_table.state_space->distance(from(), to()); - - const unsigned int & wavefront_idx = static_cast(node_coords.y) * - motion_table.size_x + static_cast(node_coords.x); - const unsigned int & wavefront_value = _wavefront_heuristic[wavefront_idx]; - - // if lethal or didn't visit, use the motion heuristic instead. - if (wavefront_value == 0) { - return NodeSE2::neutral_cost * motion_heuristic; - } - - // -2 because wavefront starts at 2 - const float wavefront_heuristic = static_cast(wavefront_value - 2); - - return NodeSE2::neutral_cost * std::max(wavefront_heuristic, motion_heuristic); -} - -void NodeSE2::initMotionModel( - const MotionModel & motion_model, - unsigned int & size_x, - unsigned int & size_y, - unsigned int & num_angle_quantization, - SearchInfo & search_info) -{ - // find the motion model selected - switch (motion_model) { - case MotionModel::DUBIN: - motion_table.initDubin(size_x, size_y, num_angle_quantization, search_info); - break; - case MotionModel::REEDS_SHEPP: - motion_table.initReedsShepp(size_x, size_y, num_angle_quantization, search_info); - break; - default: - throw std::runtime_error( - "Invalid motion model for SE2 node. Please select between" - " Dubin (Ackermann forward only)," - " Reeds-Shepp (Ackermann forward and back)."); - } -} - -void NodeSE2::computeWavefrontHeuristic( - nav2_costmap_2d::Costmap2D * & costmap, - const unsigned int & start_x, const unsigned int & start_y, - const unsigned int & goal_x, const unsigned int & goal_y) -{ - unsigned int size = costmap->getSizeInCellsX() * costmap->getSizeInCellsY(); - if (_wavefront_heuristic.size() == size) { - // must reset all values - for (unsigned int i = 0; i != _wavefront_heuristic.size(); i++) { - _wavefront_heuristic[i] = 0; - } - } else { - unsigned int wavefront_size = _wavefront_heuristic.size(); - _wavefront_heuristic.resize(size, 0); - // must reset values for non-constructed indices - for (unsigned int i = 0; i != wavefront_size; i++) { - _wavefront_heuristic[i] = 0; - } - } - - const unsigned int & size_x = motion_table.size_x; - const int size_x_int = static_cast(size_x); - const unsigned int size_y = costmap->getSizeInCellsY(); - const unsigned int goal_index = goal_y * size_x + goal_x; - const unsigned int start_index = start_y * size_x + start_x; - unsigned int mx, my, mx_idx, my_idx; - - std::queue q; - q.emplace(goal_index); - - unsigned int idx = goal_index; - _wavefront_heuristic[idx] = 2; - - static const std::vector neighborhood = {1, -1, // left right - size_x_int, -size_x_int, // up down - size_x_int + 1, size_x_int - 1, // upper diagonals - -size_x_int + 1, -size_x_int - 1}; // lower diagonals - - while (!q.empty() || idx == start_index) { - // get next one - idx = q.front(); - q.pop(); - - my_idx = idx / size_x; - mx_idx = idx - (my_idx * size_x); - - // find neighbors - for (unsigned int i = 0; i != neighborhood.size(); i++) { - unsigned int new_idx = static_cast(static_cast(idx) + neighborhood[i]); - unsigned int last_wave_cost = _wavefront_heuristic[idx]; - - // if neighbor is unvisited and non-lethal, set N and add to queue - if (new_idx > 0 && new_idx < size_x * size_y && - _wavefront_heuristic[new_idx] == 0 && - static_cast(costmap->getCost(idx)) < INSCRIBED) - { - my = new_idx / size_x; - mx = new_idx - (my * size_x); - - if (mx == 0 && mx_idx >= size_x - 1 || mx >= size_x - 1 && mx_idx == 0) { - continue; - } - if (my == 0 && my_idx >= size_y - 1 || my >= size_y - 1 && my_idx == 0) { - continue; - } - - _wavefront_heuristic[new_idx] = last_wave_cost + 1; - q.emplace(idx + neighborhood[i]); - } - } - } -} - -void NodeSE2::getNeighbors( - const NodePtr & node, - std::function & NeighborGetter, - GridCollisionChecker collision_checker, - const bool & traverse_unknown, - NodeVector & neighbors) -{ - unsigned int index = 0; - NodePtr neighbor = nullptr; - Coordinates initial_node_coords; - const MotionPoses motion_projections = motion_table.getProjections(node); - - for (unsigned int i = 0; i != motion_projections.size(); i++) { - index = NodeSE2::getIndex( - static_cast(motion_projections[i]._x), - static_cast(motion_projections[i]._y), - static_cast(motion_projections[i]._theta), - motion_table.size_x, motion_table.num_angle_quantization); - - if (NeighborGetter(index, neighbor) && !neighbor->wasVisited()) { - // Cache the initial pose in case it was visited but valid - // don't want to disrupt continuous coordinate expansion - initial_node_coords = neighbor->pose; - neighbor->setPose( - Coordinates( - motion_projections[i]._x, - motion_projections[i]._y, - motion_projections[i]._theta)); - if (neighbor->isNodeValid(traverse_unknown, collision_checker)) { - neighbor->setMotionPrimitiveIndex(i); - neighbors.push_back(neighbor); - } else { - neighbor->setPose(initial_node_coords); - } - } - } -} - -} // namespace nav2_smac_planner diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 16da85f15e..dfd295bcdf 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -28,6 +28,7 @@ using namespace std::chrono; // NOLINT SmacPlanner2D::SmacPlanner2D() : _a_star(nullptr), + _collision_checker(nullptr, 1), _smoother(nullptr), _costmap(nullptr), _costmap_downsampler(nullptr) @@ -56,8 +57,7 @@ void SmacPlanner2D::configure( bool allow_unknown; int max_iterations; int max_on_approach_iterations; - bool smooth_path; - double minimum_turning_radius; + SearchInfo search_info; std::string motion_model_for_search; // General planner params @@ -70,22 +70,19 @@ void SmacPlanner2D::configure( nav2_util::declare_parameter_if_not_declared( node, name + ".downsampling_factor", rclcpp::ParameterValue(1)); node->get_parameter(name + ".downsampling_factor", _downsampling_factor); + nav2_util::declare_parameter_if_not_declared( + node, name + ".cost_travel_multiplier", rclcpp::ParameterValue(2.0)); + node->get_parameter(name + ".cost_travel_multiplier", search_info.cost_penalty); nav2_util::declare_parameter_if_not_declared( node, name + ".allow_unknown", rclcpp::ParameterValue(true)); node->get_parameter(name + ".allow_unknown", allow_unknown); nav2_util::declare_parameter_if_not_declared( - node, name + ".max_iterations", rclcpp::ParameterValue(-1)); + node, name + ".max_iterations", rclcpp::ParameterValue(1000000)); node->get_parameter(name + ".max_iterations", max_iterations); nav2_util::declare_parameter_if_not_declared( node, name + ".max_on_approach_iterations", rclcpp::ParameterValue(1000)); node->get_parameter(name + ".max_on_approach_iterations", max_on_approach_iterations); - nav2_util::declare_parameter_if_not_declared( - node, name + ".smooth_path", rclcpp::ParameterValue(false)); - node->get_parameter(name + ".smooth_path", smooth_path); - nav2_util::declare_parameter_if_not_declared( - node, name + ".minimum_turning_radius", rclcpp::ParameterValue(0.2)); - node->get_parameter(name + ".minimum_turning_radius", minimum_turning_radius); nav2_util::declare_parameter_if_not_declared( node, name + ".max_planning_time", rclcpp::ParameterValue(1.0)); @@ -117,20 +114,29 @@ void SmacPlanner2D::configure( max_iterations = std::numeric_limits::max(); } - _a_star = std::make_unique>(motion_model, SearchInfo()); + // Initialize collision checker + _collision_checker = GridCollisionChecker(_costmap, 1 /*for 2D, most be 1*/); + _collision_checker.setFootprint( + costmap_ros->getRobotFootprint(), + true /*for 2D, most use radius*/, + 0.0 /*for 2D cost at inscribed isn't relevent*/); + + // Initialize A* template + _a_star = std::make_unique>(motion_model, search_info); _a_star->initialize( allow_unknown, max_iterations, - max_on_approach_iterations); - - if (smooth_path) { - _smoother = std::make_unique(); - _optimizer_params.get(node.get(), name); - _smoother_params.get(node.get(), name); - _smoother_params.max_curvature = 1.0f / minimum_turning_radius; - _smoother->initialize(_optimizer_params); - } + max_on_approach_iterations, + 0.0 /*unused for 2D*/, + 1.0 /*unused for 2D*/); + + // Initialize path smoother + SmootherParams params; + params.get(node, name); + _smoother = std::make_unique(params); + _smoother->initialize(1e-50 /*No valid minimum turning radius for 2D*/); + // Initialize costmap downsampler if (_downsample_costmap && _downsampling_factor > 1) { std::string topic_name = "downsampled_costmap"; _costmap_downsampler = std::make_unique(); @@ -195,14 +201,11 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( nav2_costmap_2d::Costmap2D * costmap = _costmap; if (_costmap_downsampler) { costmap = _costmap_downsampler->downsample(_downsampling_factor); + _collision_checker.setCostmap(costmap); } - // Set Costmap - _a_star->createGraph( - costmap->getSizeInCellsX(), - costmap->getSizeInCellsY(), - 1, - costmap); + // Set collision checker and costmap information + _a_star->setCollisionChecker(&_collision_checker); // Set starting point unsigned int mx, my; @@ -252,21 +255,10 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( return plan; } - // Convert to world coordinates and downsample path for smoothing if necesssary - // We're going to downsample by 4x to give terms room to move. - const int downsample_ratio = 4; - std::vector path_world; - path_world.reserve(_smoother ? path.size() / downsample_ratio : path.size()); - plan.poses.reserve(_smoother ? path.size() / downsample_ratio : path.size()); - + // Convert to world coordinates + plan.poses.reserve(path.size()); for (int i = path.size() - 1; i >= 0; --i) { - if (_smoother && i % downsample_ratio != 0) { - continue; - } - - path_world.push_back(getWorldCoords(path[i].x, path[i].y, costmap)); - pose.pose.position.x = path_world.back().x(); - pose.pose.position.y = path_world.back().y(); + pose.pose = getWorldCoords(path[i].x, path[i].y, costmap); plan.poses.push_back(pose); } @@ -275,67 +267,24 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( _raw_plan_publisher->publish(plan); } - // If not smoothing or too short to smooth, return path - if (!_smoother || path_world.size() < 4) { -#ifdef BENCHMARK_TESTING - steady_clock::time_point b = steady_clock::now(); - duration time_span = duration_cast>(b - a); - std::cout << "It took " << time_span.count() * 1000 << - " milliseconds with " << num_iterations << " iterations." << std::endl; -#endif - return plan; - } - // Find how much time we have left to do smoothing steady_clock::time_point b = steady_clock::now(); duration time_span = duration_cast>(b - a); double time_remaining = _max_planning_time - static_cast(time_span.count()); - _smoother_params.max_time = std::min(time_remaining, _optimizer_params.max_time); - - // Smooth plan - if (!_smoother->smooth(path_world, costmap, _smoother_params)) { - RCLCPP_WARN( - _logger, - "%s: failed to smooth plan, Ceres could not find a usable solution to optimize.", - _name.c_str()); - return plan; - } - removeHook(path_world); +#ifdef BENCHMARK_TESTING + std::cout << "It took " << time_span.count() * 1000 << + " milliseconds with " << num_iterations << " iterations." << std::endl; +#endif - // populate final path - for (uint i = 0; i != path_world.size(); i++) { - pose.pose.position.x = path_world[i][0]; - pose.pose.position.y = path_world[i][1]; - plan.poses[i] = pose; + // Smooth plan + if (plan.poses.size() > 6) { + _smoother->smooth(plan, costmap, time_remaining); } return plan; } -void SmacPlanner2D::removeHook(std::vector & path) -{ - // Removes the end "hooking" since goal is locked in place - Eigen::Vector2d interpolated_second_to_last_point; - interpolated_second_to_last_point = (path.end()[-3] + path.end()[-1]) / 2.0; - if ( - squaredDistance(path.end()[-2], path.end()[-1]) > - squaredDistance(interpolated_second_to_last_point, path.end()[-1])) - { - path.end()[-2] = interpolated_second_to_last_point; - } -} - -Eigen::Vector2d SmacPlanner2D::getWorldCoords( - const float & mx, const float & my, const nav2_costmap_2d::Costmap2D * costmap) -{ - float world_x = - static_cast(costmap->getOriginX()) + (mx + 0.5) * costmap->getResolution(); - float world_y = - static_cast(costmap->getOriginY()) + (my + 0.5) * costmap->getResolution(); - return Eigen::Vector2d(world_x, world_y); -} - } // namespace nav2_smac_planner #include "pluginlib/class_list_macros.hpp" diff --git a/nav2_smac_planner/src/smac_planner.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp similarity index 62% rename from nav2_smac_planner/src/smac_planner.cpp rename to nav2_smac_planner/src/smac_planner_hybrid.cpp index 55b4dc15cb..1581bfbedc 100644 --- a/nav2_smac_planner/src/smac_planner.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -19,7 +19,7 @@ #include #include "Eigen/Core" -#include "nav2_smac_planner/smac_planner.hpp" +#include "nav2_smac_planner/smac_planner_hybrid.hpp" // #define BENCHMARK_TESTING @@ -28,22 +28,23 @@ namespace nav2_smac_planner using namespace std::chrono; // NOLINT -SmacPlanner::SmacPlanner() +SmacPlannerHybrid::SmacPlannerHybrid() : _a_star(nullptr), + _collision_checker(nullptr, 1), _smoother(nullptr), _costmap(nullptr), _costmap_downsampler(nullptr) { } -SmacPlanner::~SmacPlanner() +SmacPlannerHybrid::~SmacPlannerHybrid() { RCLCPP_INFO( - _logger, "Destroying plugin %s of type SmacPlanner", + _logger, "Destroying plugin %s of type SmacPlannerHybrid", _name.c_str()); } -void SmacPlanner::configure( +void SmacPlannerHybrid::configure( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, std::string name, std::shared_ptr/*tf*/, std::shared_ptr costmap_ros) @@ -57,16 +58,12 @@ void SmacPlanner::configure( bool allow_unknown; int max_iterations; - int max_on_approach_iterations = std::numeric_limits::max(); int angle_quantizations; + double lookup_table_size; SearchInfo search_info; - bool smooth_path; std::string motion_model_for_search; // General planner params - nav2_util::declare_parameter_if_not_declared( - node, name + ".tolerance", rclcpp::ParameterValue(0.125)); - _tolerance = static_cast(node->get_parameter(name + ".tolerance").as_double()); nav2_util::declare_parameter_if_not_declared( node, name + ".downsample_costmap", rclcpp::ParameterValue(false)); node->get_parameter(name + ".downsample_costmap", _downsample_costmap); @@ -84,35 +81,37 @@ void SmacPlanner::configure( node, name + ".allow_unknown", rclcpp::ParameterValue(true)); node->get_parameter(name + ".allow_unknown", allow_unknown); nav2_util::declare_parameter_if_not_declared( - node, name + ".max_iterations", rclcpp::ParameterValue(-1)); + node, name + ".max_iterations", rclcpp::ParameterValue(1000000)); node->get_parameter(name + ".max_iterations", max_iterations); - nav2_util::declare_parameter_if_not_declared( - node, name + ".smooth_path", rclcpp::ParameterValue(false)); - node->get_parameter(name + ".smooth_path", smooth_path); nav2_util::declare_parameter_if_not_declared( - node, name + ".minimum_turning_radius", rclcpp::ParameterValue(0.2)); + node, name + ".minimum_turning_radius", rclcpp::ParameterValue(0.4)); node->get_parameter(name + ".minimum_turning_radius", search_info.minimum_turning_radius); - + nav2_util::declare_parameter_if_not_declared( + node, name + ".cache_obstacle_heuristic", rclcpp::ParameterValue(false)); + node->get_parameter(name + ".cache_obstacle_heuristic", search_info.cache_obstacle_heuristic); nav2_util::declare_parameter_if_not_declared( node, name + ".reverse_penalty", rclcpp::ParameterValue(2.0)); node->get_parameter(name + ".reverse_penalty", search_info.reverse_penalty); nav2_util::declare_parameter_if_not_declared( - node, name + ".change_penalty", rclcpp::ParameterValue(0.5)); + node, name + ".change_penalty", rclcpp::ParameterValue(0.15)); node->get_parameter(name + ".change_penalty", search_info.change_penalty); nav2_util::declare_parameter_if_not_declared( - node, name + ".non_straight_penalty", rclcpp::ParameterValue(1.05)); + node, name + ".non_straight_penalty", rclcpp::ParameterValue(1.50)); node->get_parameter(name + ".non_straight_penalty", search_info.non_straight_penalty); nav2_util::declare_parameter_if_not_declared( - node, name + ".cost_penalty", rclcpp::ParameterValue(1.2)); + node, name + ".cost_penalty", rclcpp::ParameterValue(1.7)); node->get_parameter(name + ".cost_penalty", search_info.cost_penalty); nav2_util::declare_parameter_if_not_declared( - node, name + ".analytic_expansion_ratio", rclcpp::ParameterValue(2.0)); + node, name + ".analytic_expansion_ratio", rclcpp::ParameterValue(3.5)); node->get_parameter(name + ".analytic_expansion_ratio", search_info.analytic_expansion_ratio); nav2_util::declare_parameter_if_not_declared( node, name + ".max_planning_time", rclcpp::ParameterValue(5.0)); node->get_parameter(name + ".max_planning_time", _max_planning_time); + nav2_util::declare_parameter_if_not_declared( + node, name + ".lookup_table_size", rclcpp::ParameterValue(20.0)); + node->get_parameter(name + ".lookup_table_size", lookup_table_size); nav2_util::declare_parameter_if_not_declared( node, name + ".motion_model_for_search", rclcpp::ParameterValue(std::string("DUBIN"))); @@ -122,17 +121,10 @@ void SmacPlanner::configure( RCLCPP_WARN( _logger, "Unable to get MotionModel search type. Given '%s', " - "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.", + "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP, STATE_LATTICE.", motion_model_for_search.c_str()); } - if (max_on_approach_iterations <= 0) { - RCLCPP_INFO( - _logger, "On approach iteration selected as <= 0, " - "disabling tolerance and on approach iterations."); - max_on_approach_iterations = std::numeric_limits::max(); - } - if (max_iterations <= 0) { RCLCPP_INFO( _logger, "maximum iteration selected as <= 0, " @@ -144,22 +136,45 @@ void SmacPlanner::configure( const double minimum_turning_radius_global_coords = search_info.minimum_turning_radius; search_info.minimum_turning_radius = search_info.minimum_turning_radius / (_costmap->getResolution() * _downsampling_factor); + float lookup_table_dim = + static_cast(lookup_table_size) / + static_cast(_costmap->getResolution() * _downsampling_factor); + + // Make sure its a whole number + lookup_table_dim = static_cast(static_cast(lookup_table_dim)); - _a_star = std::make_unique>(motion_model, search_info); + // Make sure its an odd number + if (static_cast(lookup_table_dim) % 2 == 0) { + RCLCPP_INFO( + _logger, + "Even sized heuristic lookup table size set %f, increasing size by 1 to make odd", + lookup_table_dim); + lookup_table_dim += 1.0; + } + + // Initialize collision checker + _collision_checker = GridCollisionChecker(_costmap, _angle_quantizations); + _collision_checker.setFootprint( + costmap_ros->getRobotFootprint(), + costmap_ros->getUseRadius(), + findCircumscribedCost(costmap_ros)); + + // Initialize A* template + _a_star = std::make_unique>(motion_model, search_info); _a_star->initialize( allow_unknown, max_iterations, - max_on_approach_iterations); - _a_star->setFootprint(costmap_ros->getRobotFootprint(), costmap_ros->getUseRadius()); - - if (smooth_path) { - _smoother = std::make_unique(); - _optimizer_params.get(node.get(), name); - _smoother_params.get(node.get(), name); - _smoother_params.max_curvature = 1.0f / minimum_turning_radius_global_coords; - _smoother->initialize(_optimizer_params); - } + std::numeric_limits::max(), + lookup_table_dim, + _angle_quantizations); + + // Initialize path smoother + SmootherParams params; + params.get(node, name); + _smoother = std::make_unique(params); + _smoother->initialize(minimum_turning_radius_global_coords); + // Initialize costmap downsampler if (_downsample_costmap && _downsampling_factor > 1) { std::string topic_name = "downsampled_costmap"; _costmap_downsampler = std::make_unique(); @@ -170,18 +185,17 @@ void SmacPlanner::configure( _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); RCLCPP_INFO( - _logger, "Configured plugin %s of type SmacPlanner with " - "tolerance %.2f, maximum iterations %i, " - "max on approach iterations %i, and %s. Using motion model: %s.", - _name.c_str(), _tolerance, max_iterations, max_on_approach_iterations, + _logger, "Configured plugin %s of type SmacPlannerHybrid with " + "maximum iterations %i, and %s. Using motion model: %s.", + _name.c_str(), max_iterations, allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal", toString(motion_model).c_str()); } -void SmacPlanner::activate() +void SmacPlannerHybrid::activate() { RCLCPP_INFO( - _logger, "Activating plugin %s of type SmacPlanner", + _logger, "Activating plugin %s of type SmacPlannerHybrid", _name.c_str()); _raw_plan_publisher->on_activate(); if (_costmap_downsampler) { @@ -189,10 +203,10 @@ void SmacPlanner::activate() } } -void SmacPlanner::deactivate() +void SmacPlannerHybrid::deactivate() { RCLCPP_INFO( - _logger, "Deactivating plugin %s of type SmacPlanner", + _logger, "Deactivating plugin %s of type SmacPlannerHybrid", _name.c_str()); _raw_plan_publisher->on_deactivate(); if (_costmap_downsampler) { @@ -200,10 +214,10 @@ void SmacPlanner::deactivate() } } -void SmacPlanner::cleanup() +void SmacPlannerHybrid::cleanup() { RCLCPP_INFO( - _logger, "Cleaning up plugin %s of type SmacPlanner", + _logger, "Cleaning up plugin %s of type SmacPlannerHybrid", _name.c_str()); _a_star.reset(); _smoother.reset(); @@ -212,7 +226,7 @@ void SmacPlanner::cleanup() _raw_plan_publisher.reset(); } -nav_msgs::msg::Path SmacPlanner::createPlan( +nav_msgs::msg::Path SmacPlannerHybrid::createPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) { @@ -224,14 +238,11 @@ nav_msgs::msg::Path SmacPlanner::createPlan( nav2_costmap_2d::Costmap2D * costmap = _costmap; if (_costmap_downsampler) { costmap = _costmap_downsampler->downsample(_downsampling_factor); + _collision_checker.setCostmap(costmap); } - // Set Costmap - _a_star->createGraph( - costmap->getSizeInCellsX(), - costmap->getSizeInCellsY(), - _angle_quantizations, - costmap); + // Set collision checker and costmap information + _a_star->setCollisionChecker(&_collision_checker); // Set starting point, in A* bin search coordinates unsigned int mx, my; @@ -265,13 +276,11 @@ nav_msgs::msg::Path SmacPlanner::createPlan( pose.pose.orientation.w = 1.0; // Compute plan - NodeSE2::CoordinateVector path; + NodeHybrid::CoordinateVector path; int num_iterations = 0; std::string error; try { - if (!_a_star->createPath( - path, num_iterations, _tolerance / static_cast(costmap->getResolution()))) - { + if (!_a_star->createPath(path, num_iterations, 0.0)) { if (num_iterations < _a_star->getMaxIterations()) { error = std::string("no valid path found"); } else { @@ -291,18 +300,11 @@ nav_msgs::msg::Path SmacPlanner::createPlan( return plan; } - // Convert to world coordinates and downsample path for smoothing if necesssary - // We're going to downsample by 4x to give terms room to move. - const int downsample_ratio = 4; - std::vector path_world; - path_world.reserve(path.size()); + // Convert to world coordinates plan.poses.reserve(path.size()); - for (int i = path.size() - 1; i >= 0; --i) { - path_world.push_back(getWorldCoords(path[i].x, path[i].y, costmap)); - pose.pose.position.x = path_world.back().x(); - pose.pose.position.y = path_world.back().y(); - pose.pose.orientation = getWorldOrientation(path[i].theta); + pose.pose = getWorldCoords(path[i].x, path[i].y, costmap); + pose.pose.orientation = getWorldOrientation(path[i].theta, _angle_bin_size); plan.poses.push_back(pose); } @@ -311,78 +313,32 @@ nav_msgs::msg::Path SmacPlanner::createPlan( _raw_plan_publisher->publish(plan); } - // If not smoothing or too short to smooth, return path - if (!_smoother || path_world.size() < 4) { -#ifdef BENCHMARK_TESTING - steady_clock::time_point b = steady_clock::now(); - duration time_span = duration_cast>(b - a); - std::cout << "It took " << time_span.count() * 1000 << - " milliseconds with " << num_iterations << " iterations." << std::endl; -#endif - return plan; - } - // Find how much time we have left to do smoothing steady_clock::time_point b = steady_clock::now(); duration time_span = duration_cast>(b - a); double time_remaining = _max_planning_time - static_cast(time_span.count()); - _smoother_params.max_time = std::min(time_remaining, _optimizer_params.max_time); + +#ifdef BENCHMARK_TESTING + std::cout << "It took " << time_span.count() * 1000 << + " milliseconds with " << num_iterations << " iterations." << std::endl; +#endif // Smooth plan - if (!_smoother->smooth(path_world, costmap, _smoother_params)) { - RCLCPP_WARN( - _logger, - "%s: failed to smooth plan, Ceres could not find a usable solution to optimize.", - _name.c_str()); - return plan; + if (num_iterations > 1 && plan.poses.size() > 6) { + _smoother->smooth(plan, costmap, time_remaining); } - removeHook(path_world); - - // populate final path - // TODO(stevemacenski): set orientation to tangent of path - for (uint i = 0; i != path_world.size(); i++) { - pose.pose.position.x = path_world[i][0]; - pose.pose.position.y = path_world[i][1]; - plan.poses[i] = pose; - } +#ifdef BENCHMARK_TESTING + steady_clock::time_point c = steady_clock::now(); + duration time_span2 = duration_cast>(c - b); + std::cout << "It took " << time_span2.count() * 1000 << + " milliseconds to smooth path." << std::endl; +#endif return plan; } -void SmacPlanner::removeHook(std::vector & path) -{ - // Removes the end "hooking" since goal is locked in place - Eigen::Vector2d interpolated_second_to_last_point; - interpolated_second_to_last_point = (path.end()[-3] + path.end()[-1]) / 2.0; - if ( - squaredDistance(path.end()[-2], path.end()[-1]) > - squaredDistance(interpolated_second_to_last_point, path.end()[-1])) - { - path.end()[-2] = interpolated_second_to_last_point; - } -} - -Eigen::Vector2d SmacPlanner::getWorldCoords( - const float & mx, const float & my, const nav2_costmap_2d::Costmap2D * costmap) -{ - // mx, my are in continuous grid coordinates, must convert to world coordinates - double world_x = - static_cast(costmap->getOriginX()) + (mx + 0.5) * costmap->getResolution(); - double world_y = - static_cast(costmap->getOriginY()) + (my + 0.5) * costmap->getResolution(); - return Eigen::Vector2d(world_x, world_y); -} - -geometry_msgs::msg::Quaternion SmacPlanner::getWorldOrientation(const float & theta) -{ - // theta is in continuous bin coordinates, must convert to world orientation - tf2::Quaternion q; - q.setEuler(0.0, 0.0, theta * static_cast(_angle_bin_size)); - return tf2::toMsg(q); -} - } // namespace nav2_smac_planner #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(nav2_smac_planner::SmacPlanner, nav2_core::GlobalPlanner) +PLUGINLIB_EXPORT_CLASS(nav2_smac_planner::SmacPlannerHybrid, nav2_core::GlobalPlanner) diff --git a/nav2_smac_planner/test/CMakeLists.txt b/nav2_smac_planner/test/CMakeLists.txt index c5ff757d1c..1409c86deb 100644 --- a/nav2_smac_planner/test/CMakeLists.txt +++ b/nav2_smac_planner/test/CMakeLists.txt @@ -20,14 +20,14 @@ target_link_libraries(test_node2d ${library_name} ) -# Test NodeSE2 -ament_add_gtest(test_nodese2 - test_nodese2.cpp +# Test NodeHybrid +ament_add_gtest(test_nodehybrid + test_nodehybrid.cpp ) -ament_target_dependencies(test_nodese2 +ament_target_dependencies(test_nodehybrid ${dependencies} ) -target_link_libraries(test_nodese2 +target_link_libraries(test_nodehybrid ${library_name} ) @@ -64,14 +64,14 @@ target_link_libraries(test_a_star ${library_name} ) -# Test SMAC SE2 -ament_add_gtest(test_smac_se2 - test_smac_se2.cpp +# Test SMAC Hybrid +ament_add_gtest(test_smac_hybrid + test_smac_hybrid.cpp ) -ament_target_dependencies(test_smac_se2 +ament_target_dependencies(test_smac_hybrid ${dependencies} ) -target_link_libraries(test_smac_se2 +target_link_libraries(test_smac_hybrid ${library_name} ) @@ -86,13 +86,3 @@ target_link_libraries(test_smac_2d ${library_name}_2d ) -# Test smoother -ament_add_gtest(test_smoother - test_smoother.cpp -) -ament_target_dependencies(test_smoother - ${dependencies} -) -target_link_libraries(test_smoother - ${library_name}_2d -) diff --git a/nav2_smac_planner/include/nav2_smac_planner/options.hpp b/nav2_smac_planner/test/deprecated/options.hpp similarity index 98% rename from nav2_smac_planner/include/nav2_smac_planner/options.hpp rename to nav2_smac_planner/test/deprecated/options.hpp index 159ab8b53a..5d33223dc4 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/options.hpp +++ b/nav2_smac_planner/test/deprecated/options.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#ifndef NAV2_SMAC_PLANNER__OPTIONS_HPP_ -#define NAV2_SMAC_PLANNER__OPTIONS_HPP_ +#ifndef DEPRECATED__OPTIONS_HPP_ +#define DEPRECATED__OPTIONS_HPP_ #include #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -204,4 +204,4 @@ struct OptimizerParams } // namespace nav2_smac_planner -#endif // NAV2_SMAC_PLANNER__OPTIONS_HPP_ +#endif // DEPRECATED__OPTIONS_HPP_ diff --git a/nav2_smac_planner/test/deprecated/smoother.hpp b/nav2_smac_planner/test/deprecated/smoother.hpp new file mode 100644 index 0000000000..e523eaea1b --- /dev/null +++ b/nav2_smac_planner/test/deprecated/smoother.hpp @@ -0,0 +1,141 @@ +// Copyright (c) 2020, Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#ifndef DEPRECATED__SMOOTHER_HPP_ +#define DEPRECATED__SMOOTHER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "nav2_smac_planner/types.hpp" +#include "nav2_smac_planner/smoother_cost_function.hpp" + +#include "ceres/ceres.h" +#include "Eigen/Core" + +namespace nav2_smac_planner +{ + +/** + * @class nav2_smac_planner::Smoother + * @brief A Conjugate Gradient 2D path smoother implementation + */ +class Smoother +{ +public: + /** + * @brief A constructor for nav2_smac_planner::Smoother + */ + Smoother() {} + + /** + * @brief A destructor for nav2_smac_planner::Smoother + */ + ~Smoother() {} + + /** + * @brief Initialization of the smoother + * @param params OptimizerParam struct + */ + void initialize(const OptimizerParams params) + { + _debug = params.debug; + + // General Params + + // 2 most valid options: STEEPEST_DESCENT, NONLINEAR_CONJUGATE_GRADIENT + _options.line_search_direction_type = ceres::NONLINEAR_CONJUGATE_GRADIENT; + _options.line_search_type = ceres::WOLFE; + _options.nonlinear_conjugate_gradient_type = ceres::POLAK_RIBIERE; + _options.line_search_interpolation_type = ceres::CUBIC; + + _options.max_num_iterations = params.max_iterations; + _options.max_solver_time_in_seconds = params.max_time; + + _options.function_tolerance = params.fn_tol; + _options.gradient_tolerance = params.gradient_tol; + _options.parameter_tolerance = params.param_tol; + + _options.min_line_search_step_size = params.advanced.min_line_search_step_size; + _options.max_num_line_search_step_size_iterations = + params.advanced.max_num_line_search_step_size_iterations; + _options.line_search_sufficient_function_decrease = + params.advanced.line_search_sufficient_function_decrease; + _options.max_line_search_step_contraction = params.advanced.max_line_search_step_contraction; + _options.min_line_search_step_contraction = params.advanced.min_line_search_step_contraction; + _options.max_num_line_search_direction_restarts = + params.advanced.max_num_line_search_direction_restarts; + _options.line_search_sufficient_curvature_decrease = + params.advanced.line_search_sufficient_curvature_decrease; + _options.max_line_search_step_expansion = params.advanced.max_line_search_step_expansion; + + if (_debug) { + _options.minimizer_progress_to_stdout = true; + } else { + _options.logging_type = ceres::SILENT; + } + } + + /** + * @brief Smoother method + * @param path Reference to path + * @param costmap Pointer to minimal costmap + * @param smoother parameters weights + * @return If smoothing was successful + */ + bool smooth( + std::vector & path, + nav2_costmap_2d::Costmap2D * costmap, + const SmootherParams & params) + { + _options.max_solver_time_in_seconds = params.max_time; + + double parameters[path.size() * 2]; // NOLINT + for (uint i = 0; i != path.size(); i++) { + parameters[2 * i] = path[i][0]; + parameters[2 * i + 1] = path[i][1]; + } + + ceres::GradientProblemSolver::Summary summary; + ceres::GradientProblem problem(new UnconstrainedSmootherCostFunction(&path, costmap, params)); + ceres::Solve(_options, problem, parameters, &summary); + + if (_debug) { + std::cout << summary.FullReport() << '\n'; + } + + if (!summary.IsSolutionUsable() || summary.initial_cost - summary.final_cost <= 0.0) { + return false; + } + + for (uint i = 0; i != path.size(); i++) { + path[i][0] = parameters[2 * i]; + path[i][1] = parameters[2 * i + 1]; + } + + return true; + } + +private: + bool _debug; + ceres::GradientProblemSolver::Options _options; +}; + +} // namespace nav2_smac_planner + +#endif // DEPRECATED__SMOOTHER_HPP_ diff --git a/nav2_smac_planner/include/nav2_smac_planner/smoother_cost_function.hpp b/nav2_smac_planner/test/deprecated/smoother_cost_function.hpp similarity index 94% rename from nav2_smac_planner/include/nav2_smac_planner/smoother_cost_function.hpp rename to nav2_smac_planner/test/deprecated/smoother_cost_function.hpp index eec8c61b4e..7e5349c894 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smoother_cost_function.hpp +++ b/nav2_smac_planner/test/deprecated/smoother_cost_function.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#ifndef NAV2_SMAC_PLANNER__SMOOTHER_COST_FUNCTION_HPP_ -#define NAV2_SMAC_PLANNER__SMOOTHER_COST_FUNCTION_HPP_ +#ifndef DEPRECATED__SMOOTHER_COST_FUNCTION_HPP_ +#define DEPRECATED__SMOOTHER_COST_FUNCTION_HPP_ #include #include @@ -56,6 +56,22 @@ class UnconstrainedSmootherCostFunction : public ceres::FirstOrderFunction _costmap(costmap), _params(params) { + // int height = costmap->getSizeInCellsX(); + // int width = costmap->getSizeInCellsY(); + // bool** binMap; + // binMap = new bool*[width]; + + // for (int x = 0; x < width; x++) { binMap[x] = new bool[height]; } + + // for (int x = 0; x < width; ++x) { + // for (int y = 0; y < height; ++y) { + // binMap[x][y] = costmap->getCost(x,y) >= 253 ? true : false; + // } + // } + + // voronoiDiagram.initializeMap(width, height, binMap); + // voronoiDiagram.update(); + // voronoiDiagram.visualize(); } /** @@ -138,7 +154,7 @@ class UnconstrainedSmootherCostFunction : public ceres::FirstOrderFunction if (valid_coords = _costmap->worldToMap(xi[0], xi[1], mx, my)) { costmap_cost = _costmap->getCost(mx, my); - addCostResidual(_params.costmap_weight, costmap_cost, cost_raw); + addCostResidual(_params.costmap_weight, costmap_cost, cost_raw, xi); } if (gradient != NULL) { @@ -379,13 +395,23 @@ class UnconstrainedSmootherCostFunction : public ceres::FirstOrderFunction inline void addCostResidual( const double & weight, const double & value, - double & r) const + double & r, + Eigen::Vector2d & xi) const { if (value == FREE) { return; } r += weight * value * value; // objective function value + + + // float obsDst = voronoiDiagram.getDistance((int)xi[0], (int)xi[1]); + + // if (abs(obsDst) > 0.3) { + // return; + // } + + // r += weight * (abs(obsDst) - 0.3) * (abs(obsDst) - 0.3); } /** @@ -508,8 +534,9 @@ class UnconstrainedSmootherCostFunction : public ceres::FirstOrderFunction int _num_params; nav2_costmap_2d::Costmap2D * _costmap{nullptr}; SmootherParams _params; + // DynamicVoronoi voronoiDiagram; }; } // namespace nav2_smac_planner -#endif // NAV2_SMAC_PLANNER__SMOOTHER_COST_FUNCTION_HPP_ +#endif // DEPRECATED__SMOOTHER_COST_FUNCTION_HPP_ diff --git a/nav2_smac_planner/test/deprecated_upsampler/upsampler.hpp b/nav2_smac_planner/test/deprecated/upsampler.hpp similarity index 98% rename from nav2_smac_planner/test/deprecated_upsampler/upsampler.hpp rename to nav2_smac_planner/test/deprecated/upsampler.hpp index 84ba625fa1..90e0f8b464 100644 --- a/nav2_smac_planner/test/deprecated_upsampler/upsampler.hpp +++ b/nav2_smac_planner/test/deprecated/upsampler.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#ifndef DEPRECATED_UPSAMPLER__UPSAMPLER_HPP_ -#define DEPRECATED_UPSAMPLER__UPSAMPLER_HPP_ +#ifndef DEPRECATED__UPSAMPLER_HPP_ +#define DEPRECATED__UPSAMPLER_HPP_ #include #include @@ -210,4 +210,4 @@ class Upsampler } // namespace nav2_smac_planner -#endif // DEPRECATED_UPSAMPLER__UPSAMPLER_HPP_ +#endif // DEPRECATED__UPSAMPLER_HPP_ diff --git a/nav2_smac_planner/test/deprecated_upsampler/upsampler_cost_function.hpp b/nav2_smac_planner/test/deprecated/upsampler_cost_function.hpp similarity index 98% rename from nav2_smac_planner/test/deprecated_upsampler/upsampler_cost_function.hpp rename to nav2_smac_planner/test/deprecated/upsampler_cost_function.hpp index 44df1c3f22..4a89800b5b 100644 --- a/nav2_smac_planner/test/deprecated_upsampler/upsampler_cost_function.hpp +++ b/nav2_smac_planner/test/deprecated/upsampler_cost_function.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#ifndef DEPRECATED_UPSAMPLER__UPSAMPLER_COST_FUNCTION_HPP_ -#define DEPRECATED_UPSAMPLER__UPSAMPLER_COST_FUNCTION_HPP_ +#ifndef DEPRECATED__UPSAMPLER_COST_FUNCTION_HPP_ +#define DEPRECATED__UPSAMPLER_COST_FUNCTION_HPP_ #include #include @@ -363,4 +363,4 @@ class UpsamplerCostFunction : public ceres::FirstOrderFunction } // namespace nav2_smac_planner -#endif // DEPRECATED_UPSAMPLER__UPSAMPLER_COST_FUNCTION_HPP_ +#endif // DEPRECATED__UPSAMPLER_COST_FUNCTION_HPP_ diff --git a/nav2_smac_planner/test/deprecated_upsampler/upsampler_cost_function_nlls.hpp b/nav2_smac_planner/test/deprecated/upsampler_cost_function_nlls.hpp similarity index 98% rename from nav2_smac_planner/test/deprecated_upsampler/upsampler_cost_function_nlls.hpp rename to nav2_smac_planner/test/deprecated/upsampler_cost_function_nlls.hpp index 17204694b1..f1599bcd78 100644 --- a/nav2_smac_planner/test/deprecated_upsampler/upsampler_cost_function_nlls.hpp +++ b/nav2_smac_planner/test/deprecated/upsampler_cost_function_nlls.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#ifndef DEPRECATED_UPSAMPLER__UPSAMPLER_COST_FUNCTION_NLLS_HPP_ -#define DEPRECATED_UPSAMPLER__UPSAMPLER_COST_FUNCTION_NLLS_HPP_ +#ifndef DEPRECATED__UPSAMPLER_COST_FUNCTION_NLLS_HPP_ +#define DEPRECATED__UPSAMPLER_COST_FUNCTION_NLLS_HPP_ #include #include @@ -331,4 +331,4 @@ class UpsamplerConstrainedCostFunction : public ceres::SizedCostFunction<1, 1, 1 } // namespace nav2_smac_planner -#endif // DEPRECATED_UPSAMPLER__UPSAMPLER_COST_FUNCTION_NLLS_HPP_ +#endif // DEPRECATED__UPSAMPLER_COST_FUNCTION_NLLS_HPP_ diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index 721eee4710..17e10708f2 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -22,7 +22,7 @@ #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/costmap_subscriber.hpp" #include "nav2_util/lifecycle_node.hpp" -#include "nav2_smac_planner/node_se2.hpp" +#include "nav2_smac_planner/node_hybrid.hpp" #include "nav2_smac_planner/a_star.hpp" #include "nav2_smac_planner/collision_checker.hpp" @@ -45,8 +45,7 @@ TEST(AStarTest, test_a_star_2d) int it_on_approach = 10; int num_it = 0; - a_star.initialize(false, max_iterations, it_on_approach); - a_star.setFootprint(nav2_costmap_2d::Footprint(), true); + a_star.initialize(false, max_iterations, it_on_approach, 0.0, 1); nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); @@ -58,12 +57,15 @@ TEST(AStarTest, test_a_star_2d) } // functional case testing - a_star.createGraph(costmapA->getSizeInCellsX(), costmapA->getSizeInCellsY(), 1, costmapA); + std::unique_ptr checker = + std::make_unique(costmapA, 1); + checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); + a_star.setCollisionChecker(checker.get()); a_star.setStart(20u, 20u, 0); a_star.setGoal(80u, 80u, 0); nav2_smac_planner::Node2D::CoordinateVector path; EXPECT_TRUE(a_star.createPath(path, num_it, tolerance)); - EXPECT_EQ(num_it, 556); + EXPECT_EQ(num_it, 102); // check path is the right size and collision free EXPECT_EQ(path.size(), 81u); @@ -72,9 +74,6 @@ TEST(AStarTest, test_a_star_2d) } // setting non-zero dim 3 for 2D search - EXPECT_THROW( - a_star.createGraph( - costmapA->getSizeInCellsX(), costmapA->getSizeInCellsY(), 10, costmapA), std::runtime_error); EXPECT_THROW(a_star.setGoal(0, 0, 10), std::runtime_error); EXPECT_THROW(a_star.setStart(0, 0, 10), std::runtime_error); @@ -82,11 +81,10 @@ TEST(AStarTest, test_a_star_2d) // failure cases with invalid inputs nav2_smac_planner::AStarAlgorithm a_star_2( nav2_smac_planner::MotionModel::VON_NEUMANN, info); - a_star_2.initialize(false, max_iterations, it_on_approach); - a_star_2.setFootprint(nav2_costmap_2d::Footprint(), true); + a_star_2.initialize(false, max_iterations, it_on_approach, 0, 1); num_it = 0; EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error); - a_star_2.createGraph(costmapA->getSizeInCellsX(), costmapA->getSizeInCellsY(), 1, costmapA); + a_star_2.setCollisionChecker(checker.get()); num_it = 0; EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error); a_star_2.setStart(50, 50, 0); // invalid @@ -102,7 +100,7 @@ TEST(AStarTest, test_a_star_2d) a_star_2.setStart(20, 20, 0); // valid a_star_2.setGoal(50, 50, 0); // invalid EXPECT_TRUE(a_star_2.createPath(path, num_it, some_tolerance)); - EXPECT_EQ(path.size(), 32u); + EXPECT_EQ(path.size(), 42u); for (unsigned int i = 0; i != path.size(); i++) { EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); } @@ -112,7 +110,7 @@ TEST(AStarTest, test_a_star_2d) EXPECT_EQ(a_star_2.getSizeX(), 100u); EXPECT_EQ(a_star_2.getSizeY(), 100u); EXPECT_EQ(a_star_2.getSizeDim3(), 1u); - EXPECT_EQ(a_star_2.getToleranceHeuristic(), 1000.0); + EXPECT_EQ(a_star_2.getToleranceHeuristic(), 20.0); EXPECT_EQ(a_star_2.getOnApproachMaxIterations(), 10); delete costmapA; @@ -121,20 +119,20 @@ TEST(AStarTest, test_a_star_2d) TEST(AStarTest, test_a_star_se2) { nav2_smac_planner::SearchInfo info; - info.change_penalty = 1.2; - info.non_straight_penalty = 1.4; - info.reverse_penalty = 2.1; - info.minimum_turning_radius = 2.0; // in grid coordinates + info.change_penalty = 0.1; + info.non_straight_penalty = 1.1; + info.reverse_penalty = 2.0; + info.minimum_turning_radius = 8; // in grid coordinates unsigned int size_theta = 72; - nav2_smac_planner::AStarAlgorithm a_star( + info.cost_penalty = 1.7; + nav2_smac_planner::AStarAlgorithm a_star( nav2_smac_planner::MotionModel::DUBIN, info); int max_iterations = 10000; float tolerance = 10.0; int it_on_approach = 10; int num_it = 0; - a_star.initialize(false, max_iterations, it_on_approach); - a_star.setFootprint(nav2_costmap_2d::Footprint(), true); + a_star.initialize(false, max_iterations, it_on_approach, 401, size_theta); nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); @@ -145,17 +143,20 @@ TEST(AStarTest, test_a_star_se2) } } + std::unique_ptr checker = + std::make_unique(costmapA, size_theta); + checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); + // functional case testing - a_star.createGraph( - costmapA->getSizeInCellsX(), costmapA->getSizeInCellsY(), size_theta, costmapA); + a_star.setCollisionChecker(checker.get()); a_star.setStart(10u, 10u, 0u); a_star.setGoal(80u, 80u, 40u); - nav2_smac_planner::NodeSE2::CoordinateVector path; + nav2_smac_planner::NodeHybrid::CoordinateVector path; EXPECT_TRUE(a_star.createPath(path, num_it, tolerance)); // check path is the right size and collision free - EXPECT_EQ(num_it, 44); - EXPECT_EQ(path.size(), 75u); + EXPECT_EQ(num_it, 351); + EXPECT_EQ(path.size(), 73u); for (unsigned int i = 0; i != path.size(); i++) { EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); } diff --git a/nav2_smac_planner/test/test_collision_checker.cpp b/nav2_smac_planner/test/test_collision_checker.cpp index 68084993b2..40e73c82ad 100644 --- a/nav2_smac_planner/test/test_collision_checker.cpp +++ b/nav2_smac_planner/test/test_collision_checker.cpp @@ -41,8 +41,8 @@ TEST(collision_footprint, test_basic) nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; - nav2_smac_planner::GridCollisionChecker collision_checker(costmap_); - collision_checker.setFootprint(footprint, false /*use footprint*/); + nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72); + collision_checker.setFootprint(footprint, false /*use footprint*/, 0.0); collision_checker.inCollision(5.0, 5.0, 0.0, false); float cost = collision_checker.getCost(); EXPECT_NEAR(cost, 0.0, 0.001); @@ -53,9 +53,9 @@ TEST(collision_footprint, test_point_cost) { nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0); - nav2_smac_planner::GridCollisionChecker collision_checker(costmap_); + nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72); nav2_costmap_2d::Footprint footprint; - collision_checker.setFootprint(footprint, true /*radius / pointcose*/); + collision_checker.setFootprint(footprint, true /*radius / pointcose*/, 0.0); collision_checker.inCollision(5.0, 5.0, 0.0, false); float cost = collision_checker.getCost(); @@ -67,9 +67,9 @@ TEST(collision_footprint, test_world_to_map) { nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0); - nav2_smac_planner::GridCollisionChecker collision_checker(costmap_); + nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72); nav2_costmap_2d::Footprint footprint; - collision_checker.setFootprint(footprint, true /*radius / point cost*/); + collision_checker.setFootprint(footprint, true /*radius / point cost*/, 0.0); unsigned int x, y; @@ -113,8 +113,8 @@ TEST(collision_footprint, test_footprint_at_pose_with_movement) nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; - nav2_smac_planner::GridCollisionChecker collision_checker(costmap_); - collision_checker.setFootprint(footprint, false /*use footprint*/); + nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72); + collision_checker.setFootprint(footprint, false /*use footprint*/, 0.0); collision_checker.inCollision(50, 50, 0.0, false); float cost = collision_checker.getCost(); @@ -153,8 +153,8 @@ TEST(collision_footprint, test_point_and_line_cost) nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; - nav2_smac_planner::GridCollisionChecker collision_checker(costmap_); - collision_checker.setFootprint(footprint, false /*use footprint*/); + nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72); + collision_checker.setFootprint(footprint, false /*use footprint*/, 0.0); collision_checker.inCollision(50, 50, 0.0, false); float value = collision_checker.getCost(); diff --git a/nav2_smac_planner/test/test_node2d.cpp b/nav2_smac_planner/test/test_node2d.cpp index 33377254a7..f4520bd496 100644 --- a/nav2_smac_planner/test/test_node2d.cpp +++ b/nav2_smac_planner/test/test_node2d.cpp @@ -35,41 +35,45 @@ RclCppFixture g_rclcppfixture; TEST(Node2DTest, test_node_2d) { nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0); - nav2_smac_planner::GridCollisionChecker checker(&costmapA); + std::unique_ptr checker = + std::make_unique(&costmapA, 72); + checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); // test construction unsigned char cost = static_cast(1); - nav2_smac_planner::Node2D testA(cost, 1); - nav2_smac_planner::Node2D testB(cost, 1); + nav2_smac_planner::Node2D testA(1); + testA.setCost(cost); + nav2_smac_planner::Node2D testB(1); + testB.setCost(cost); EXPECT_EQ(testA.getCost(), 1.0f); + nav2_smac_planner::SearchInfo info; + info.cost_penalty = 1.0; + unsigned int size = 10; + nav2_smac_planner::Node2D::initMotionModel( + nav2_smac_planner::MotionModel::MOORE, size, size, size, info); // test reset - testA.reset(10); - EXPECT_EQ(testA.getCost(), 10.0f); - - // Check constants - EXPECT_EQ(testA.neutral_cost, 50.0f); + testA.reset(); + EXPECT_TRUE(std::isnan(testA.getCost())); // check collision checking - EXPECT_EQ(testA.isNodeValid(false, checker), true); - testA.reset(254); - EXPECT_EQ(testA.isNodeValid(false, checker), false); - testA.reset(255); - EXPECT_EQ(testA.isNodeValid(true, checker), true); - EXPECT_EQ(testA.isNodeValid(false, checker), false); - testA.reset(10); + EXPECT_EQ(testA.isNodeValid(false, checker.get()), true); + testA.setCost(255); + EXPECT_EQ(testA.isNodeValid(true, checker.get()), true); + testA.setCost(10); // check traversal cost computation - EXPECT_EQ(testB.getTraversalCost(&testA), 58.0f); + EXPECT_NEAR(testB.getTraversalCost(&testA), 1.03f, 0.1f); // check heuristic cost computation nav2_smac_planner::Node2D::Coordinates A(0.0, 0.0); nav2_smac_planner::Node2D::Coordinates B(10.0, 5.0); - EXPECT_NEAR(testB.getHeuristicCost(A, B), 559.016, 0.01); + EXPECT_NEAR(testB.getHeuristicCost(A, B, nullptr), 15., 0.01); // check operator== works on index unsigned char costC = '2'; - nav2_smac_planner::Node2D testC(costC, 1); + nav2_smac_planner::Node2D testC(1); + testC.setCost(costC); EXPECT_TRUE(testA == testC); // check accumulated costs are set @@ -97,15 +101,24 @@ TEST(Node2DTest, test_node_2d) TEST(Node2DTest, test_node_2d_neighbors) { + nav2_smac_planner::SearchInfo info; + unsigned int size_x = 10u; + unsigned int size_y = 10u; + unsigned int quant = 0u; // test neighborhood computation - nav2_smac_planner::Node2D::initNeighborhood(10u, nav2_smac_planner::MotionModel::VON_NEUMANN); + nav2_smac_planner::Node2D::initMotionModel( + nav2_smac_planner::MotionModel::VON_NEUMANN, size_x, + size_y, quant, info); EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets.size(), 4u); EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[0], -1); EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[1], 1); EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[2], -10); EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[3], 10); - nav2_smac_planner::Node2D::initNeighborhood(100u, nav2_smac_planner::MotionModel::MOORE); + size_x = 100u; + nav2_smac_planner::Node2D::initMotionModel( + nav2_smac_planner::MotionModel::MOORE, size_x, size_y, + quant, info); EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets.size(), 8u); EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[0], -1); EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[1], 1); @@ -117,17 +130,19 @@ TEST(Node2DTest, test_node_2d_neighbors) EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[7], 101); nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0); - nav2_smac_planner::GridCollisionChecker checker(&costmapA); + std::unique_ptr checker = + std::make_unique(&costmapA, 72); unsigned char cost = static_cast(1); - nav2_smac_planner::Node2D * node = new nav2_smac_planner::Node2D(cost, 1); + nav2_smac_planner::Node2D * node = new nav2_smac_planner::Node2D(1); + node->setCost(cost); std::function neighborGetter = [&, this](const unsigned int & index, nav2_smac_planner::Node2D * & neighbor_rtn) -> bool { - return true; + return false; }; nav2_smac_planner::Node2D::NodeVector neighbors; - nav2_smac_planner::Node2D::getNeighbors(node, neighborGetter, checker, false, neighbors); + node->getNeighbors(neighborGetter, checker.get(), false, neighbors); delete node; // should be empty since totally invalid diff --git a/nav2_smac_planner/test/test_nodebasic.cpp b/nav2_smac_planner/test/test_nodebasic.cpp index 23d445ce54..965246d67c 100644 --- a/nav2_smac_planner/test/test_nodebasic.cpp +++ b/nav2_smac_planner/test/test_nodebasic.cpp @@ -24,7 +24,7 @@ #include "nav2_util/lifecycle_node.hpp" #include "nav2_smac_planner/node_basic.hpp" #include "nav2_smac_planner/node_2d.hpp" -#include "nav2_smac_planner/node_se2.hpp" +#include "nav2_smac_planner/node_hybrid.hpp" #include "nav2_smac_planner/collision_checker.hpp" class RclCppFixture @@ -37,7 +37,7 @@ RclCppFixture g_rclcppfixture; TEST(NodeBasicTest, test_node_basic) { - nav2_smac_planner::NodeBasic node(50); + nav2_smac_planner::NodeBasic node(50); EXPECT_EQ(node.index, 50u); EXPECT_EQ(node.graph_node_ptr, nullptr); diff --git a/nav2_smac_planner/test/test_nodehybrid.cpp b/nav2_smac_planner/test/test_nodehybrid.cpp new file mode 100644 index 0000000000..9954a011ef --- /dev/null +++ b/nav2_smac_planner/test/test_nodehybrid.cpp @@ -0,0 +1,219 @@ +// Copyright (c) 2020, Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_smac_planner/node_hybrid.hpp" +#include "nav2_smac_planner/collision_checker.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(NodeHybridTest, test_node_hybrid) +{ + nav2_smac_planner::SearchInfo info; + info.change_penalty = 0.1; + info.non_straight_penalty = 1.1; + info.reverse_penalty = 2.0; + info.minimum_turning_radius = 8; // 0.4m/5cm resolution costmap + info.cost_penalty = 1.7; + unsigned int size_x = 10; + unsigned int size_y = 10; + unsigned int size_theta = 72; + + // Check defaulted constants + nav2_smac_planner::NodeHybrid testA(49); + EXPECT_EQ(testA.travel_distance_cost, sqrt(2)); + + nav2_smac_planner::NodeHybrid::initMotionModel( + nav2_smac_planner::MotionModel::DUBIN, size_x, size_y, size_theta, info); + + nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D( + 10, 10, 0.05, 0.0, 0.0, 0); + std::unique_ptr checker = + std::make_unique(costmapA, 72); + checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); + + // test construction + nav2_smac_planner::NodeHybrid testB(49); + EXPECT_TRUE(std::isnan(testA.getCost())); + + // test node valid and cost + testA.pose.x = 5; + testA.pose.y = 5; + testA.pose.theta = 0; + EXPECT_EQ(testA.isNodeValid(true, checker.get()), true); + EXPECT_EQ(testA.isNodeValid(false, checker.get()), true); + EXPECT_EQ(testA.getCost(), 0.0f); + + // test reset + testA.reset(); + EXPECT_TRUE(std::isnan(testA.getCost())); + + // Check motion-specific constants + EXPECT_NEAR(testA.travel_distance_cost, 2.08842, 0.1); + + // check collision checking + EXPECT_EQ(testA.isNodeValid(false, checker.get()), true); + + // check traversal cost computation + // simulated first node, should return neutral cost + EXPECT_NEAR(testB.getTraversalCost(&testA), 2.088, 0.1); + // now with straight motion, cost is 0, so will be sqrt(2) as well + testB.setMotionPrimitiveIndex(1); + testA.setMotionPrimitiveIndex(0); + EXPECT_NEAR(testB.getTraversalCost(&testA), 2.088, 0.1); + // same direction as parent, testB + testA.setMotionPrimitiveIndex(1); + EXPECT_NEAR(testB.getTraversalCost(&testA), 2.297f, 0.01); + // opposite direction as parent, testB + testA.setMotionPrimitiveIndex(2); + EXPECT_NEAR(testB.getTraversalCost(&testA), 2.506f, 0.01); + + // will throw because never collision checked testB + EXPECT_THROW(testA.getTraversalCost(&testB), std::runtime_error); + + // check motion primitives + EXPECT_EQ(testA.getMotionPrimitiveIndex(), 2u); + + // check operator== works on index + nav2_smac_planner::NodeHybrid testC(49); + EXPECT_TRUE(testA == testC); + + // check accumulated costs are set + testC.setAccumulatedCost(100); + EXPECT_EQ(testC.getAccumulatedCost(), 100.0f); + + // check visiting state + EXPECT_EQ(testC.wasVisited(), false); + testC.visited(); + EXPECT_EQ(testC.wasVisited(), true); + + // check index + EXPECT_EQ(testC.getIndex(), 49u); + + // check set pose and pose + testC.setPose(nav2_smac_planner::NodeHybrid::Coordinates(10.0, 5.0, 4)); + EXPECT_EQ(testC.pose.x, 10.0); + EXPECT_EQ(testC.pose.y, 5.0); + EXPECT_EQ(testC.pose.theta, 4); + + // check static index functions + EXPECT_EQ(nav2_smac_planner::NodeHybrid::getIndex(1u, 1u, 4u, 10u, 72u), 796u); + EXPECT_EQ(nav2_smac_planner::NodeHybrid::getCoords(796u, 10u, 72u).x, 1u); + EXPECT_EQ(nav2_smac_planner::NodeHybrid::getCoords(796u, 10u, 72u).y, 1u); + EXPECT_EQ(nav2_smac_planner::NodeHybrid::getCoords(796u, 10u, 72u).theta, 4u); + + delete costmapA; +} + +TEST(NodeHybridTest, test_node_debin_neighbors) +{ + nav2_smac_planner::SearchInfo info; + info.change_penalty = 1.2; + info.non_straight_penalty = 1.4; + info.reverse_penalty = 2.1; + info.minimum_turning_radius = 4; // 0.2 in grid coordinates + unsigned int size_x = 100; + unsigned int size_y = 100; + unsigned int size_theta = 72; + nav2_smac_planner::NodeHybrid::initMotionModel( + nav2_smac_planner::MotionModel::DUBIN, size_x, size_y, size_theta, info); + + + // test neighborhood computation + EXPECT_EQ(nav2_smac_planner::NodeHybrid::motion_table.projections.size(), 3u); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[0]._x, 1.731517, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[0]._y, 0, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[0]._theta, 0, 0.01); + + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[1]._x, 1.69047, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[1]._y, 0.3747, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[1]._theta, 5, 0.01); + + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[2]._x, 1.69047, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[2]._y, -0.3747, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[2]._theta, -5, 0.01); +} + +TEST(NodeHybridTest, test_node_reeds_neighbors) +{ + nav2_smac_planner::SearchInfo info; + info.change_penalty = 1.2; + info.non_straight_penalty = 1.4; + info.reverse_penalty = 2.1; + info.minimum_turning_radius = 8; // 0.4 in grid coordinates + unsigned int size_x = 100; + unsigned int size_y = 100; + unsigned int size_theta = 72; + nav2_smac_planner::NodeHybrid::initMotionModel( + nav2_smac_planner::MotionModel::REEDS_SHEPP, size_x, size_y, size_theta, info); + + EXPECT_EQ(nav2_smac_planner::NodeHybrid::motion_table.projections.size(), 6u); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[0]._x, 2.088, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[0]._y, 0, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[0]._theta, 0, 0.01); + + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[1]._x, 2.070, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[1]._y, 0.272, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[1]._theta, 3, 0.01); + + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[2]._x, 2.070, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[2]._y, -0.272, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[2]._theta, -3, 0.01); + + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[3]._x, -2.088, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[3]._y, 0, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[3]._theta, 0, 0.01); + + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[4]._x, -2.07, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[4]._y, 0.272, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[4]._theta, -3, 0.01); + + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[5]._x, -2.07, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[5]._y, -0.272, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[5]._theta, 3, 0.01); + + nav2_costmap_2d::Costmap2D costmapA(100, 100, 0.05, 0.0, 0.0, 0); + std::unique_ptr checker = + std::make_unique(&costmapA, 72); + checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); + nav2_smac_planner::NodeHybrid * node = new nav2_smac_planner::NodeHybrid(49); + std::function neighborGetter = + [&, this](const unsigned int & index, nav2_smac_planner::NodeHybrid * & neighbor_rtn) -> bool + { + // because we don't return a real object + return false; + }; + + nav2_smac_planner::NodeHybrid::NodeVector neighbors; + node->getNeighbors(neighborGetter, checker.get(), false, neighbors); + delete node; + + // should be empty since totally invalid + EXPECT_EQ(neighbors.size(), 0u); +} diff --git a/nav2_smac_planner/test/test_nodese2.cpp b/nav2_smac_planner/test/test_nodese2.cpp deleted file mode 100644 index 39b9aa2dfa..0000000000 --- a/nav2_smac_planner/test/test_nodese2.cpp +++ /dev/null @@ -1,214 +0,0 @@ -// Copyright (c) 2020, Samsung Research America -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. Reserved. - -#include -#include -#include -#include - -#include "gtest/gtest.h" -#include "rclcpp/rclcpp.hpp" -#include "nav2_costmap_2d/costmap_2d.hpp" -#include "nav2_costmap_2d/costmap_subscriber.hpp" -#include "nav2_util/lifecycle_node.hpp" -#include "nav2_smac_planner/node_se2.hpp" -#include "nav2_smac_planner/collision_checker.hpp" - -class RclCppFixture -{ -public: - RclCppFixture() {rclcpp::init(0, nullptr);} - ~RclCppFixture() {rclcpp::shutdown();} -}; -RclCppFixture g_rclcppfixture; - -TEST(NodeSE2Test, test_node_se2) -{ - nav2_smac_planner::SearchInfo info; - info.change_penalty = 1.2; - info.non_straight_penalty = 1.4; - info.reverse_penalty = 2.1; - info.minimum_turning_radius = 0.20; - unsigned int size_x = 10; - unsigned int size_y = 10; - unsigned int size_theta = 72; - - nav2_smac_planner::NodeSE2::initMotionModel( - nav2_smac_planner::MotionModel::DUBIN, size_x, size_y, size_theta, info); - - nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D( - 10, 10, 0.05, 0.0, 0.0, 0); - nav2_smac_planner::GridCollisionChecker checker(costmapA); - checker.setFootprint(nav2_costmap_2d::Footprint(), true); - - // test construction - nav2_smac_planner::NodeSE2 testA(49); - nav2_smac_planner::NodeSE2 testB(49); - EXPECT_TRUE(std::isnan(testA.getCost())); - - // test node valid and cost - testA.pose.x = 5; - testA.pose.y = 5; - testA.pose.theta = 0; - EXPECT_EQ(testA.isNodeValid(true, checker), true); - EXPECT_EQ(testA.isNodeValid(false, checker), true); - EXPECT_EQ(testA.getCost(), 0.0f); - - // test reset - testA.reset(); - EXPECT_TRUE(std::isnan(testA.getCost())); - - // Check constants - EXPECT_EQ(testA.neutral_cost, sqrt(2)); - - // check collision checking - EXPECT_EQ(testA.isNodeValid(false, checker), true); - - // check traversal cost computation - // simulated first node, should return neutral cost - EXPECT_NEAR(testB.getTraversalCost(&testA), sqrt(2), 0.01); - // now with straight motion, cost is 0, so will be sqrt(2) as well - testB.setMotionPrimitiveIndex(1); - testA.setMotionPrimitiveIndex(0); - EXPECT_NEAR(testB.getTraversalCost(&testA), sqrt(2), 0.01); - // same direction as parent, testB - testA.setMotionPrimitiveIndex(1); - EXPECT_NEAR(testB.getTraversalCost(&testA), 1.9799f, 0.01); - // opposite direction as parent, testB - testA.setMotionPrimitiveIndex(2); - EXPECT_NEAR(testB.getTraversalCost(&testA), 3.67696f, 0.01); - - // will throw because never collision checked testB - EXPECT_THROW(testA.getTraversalCost(&testB), std::runtime_error); - - // check motion primitives - EXPECT_EQ(testA.getMotionPrimitiveIndex(), 2u); - - // check heuristic cost computation - nav2_smac_planner::NodeSE2::computeWavefrontHeuristic( - costmapA, - static_cast(10.0), - static_cast(5.0), - 0.0, 0.0); - nav2_smac_planner::NodeSE2::Coordinates A(0.0, 0.0, 4.2); - nav2_smac_planner::NodeSE2::Coordinates B(10.0, 5.0, 54.1); - EXPECT_NEAR(testB.getHeuristicCost(B, A), 16.723, 0.01); - - // check operator== works on index - nav2_smac_planner::NodeSE2 testC(49); - EXPECT_TRUE(testA == testC); - - // check accumulated costs are set - testC.setAccumulatedCost(100); - EXPECT_EQ(testC.getAccumulatedCost(), 100.0f); - - // check visiting state - EXPECT_EQ(testC.wasVisited(), false); - testC.queued(); - EXPECT_EQ(testC.isQueued(), true); - testC.visited(); - EXPECT_EQ(testC.wasVisited(), true); - EXPECT_EQ(testC.isQueued(), false); - - // check index - EXPECT_EQ(testC.getIndex(), 49u); - - // check set pose and pose - testC.setPose(nav2_smac_planner::NodeSE2::Coordinates(10.0, 5.0, 4)); - EXPECT_EQ(testC.pose.x, 10.0); - EXPECT_EQ(testC.pose.y, 5.0); - EXPECT_EQ(testC.pose.theta, 4); - - // check static index functions - EXPECT_EQ(nav2_smac_planner::NodeSE2::getIndex(1u, 1u, 4u, 10u, 72u), 796u); - EXPECT_EQ(nav2_smac_planner::NodeSE2::getCoords(796u, 10u, 72u).x, 1u); - EXPECT_EQ(nav2_smac_planner::NodeSE2::getCoords(796u, 10u, 72u).y, 1u); - EXPECT_EQ(nav2_smac_planner::NodeSE2::getCoords(796u, 10u, 72u).theta, 4u); - - delete costmapA; -} - -TEST(NodeSE2Test, test_node_2d_neighbors) -{ - nav2_smac_planner::SearchInfo info; - info.change_penalty = 1.2; - info.non_straight_penalty = 1.4; - info.reverse_penalty = 2.1; - info.minimum_turning_radius = 4; // 0.2 in grid coordinates - unsigned int size_x = 100; - unsigned int size_y = 100; - unsigned int size_theta = 72; - nav2_smac_planner::NodeSE2::initMotionModel( - nav2_smac_planner::MotionModel::DUBIN, size_x, size_y, size_theta, info); - - - // test neighborhood computation - EXPECT_EQ(nav2_smac_planner::NodeSE2::motion_table.projections.size(), 3u); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[0]._x, 1.731517, 0.01); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[0]._y, 0, 0.01); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[0]._theta, 0, 0.01); - - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[1]._x, 1.69047, 0.01); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[1]._y, 0.3747, 0.01); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[1]._theta, 5, 0.01); - - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[2]._x, 1.69047, 0.01); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[2]._y, -0.3747, 0.01); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[2]._theta, -5, 0.01); - - nav2_smac_planner::NodeSE2::initMotionModel( - nav2_smac_planner::MotionModel::REEDS_SHEPP, size_x, size_y, size_theta, info); - - EXPECT_EQ(nav2_smac_planner::NodeSE2::motion_table.projections.size(), 6u); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[0]._x, 1.731517, 0.01); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[0]._y, 0, 0.01); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[0]._theta, 0, 0.01); - - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[1]._x, 1.69047, 0.01); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[1]._y, 0.3747, 0.01); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[1]._theta, 5, 0.01); - - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[2]._x, 1.69047, 0.01); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[2]._y, -0.3747, 0.01); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[2]._theta, -5, 0.01); - - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[3]._x, -1.731517, 0.01); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[3]._y, 0, 0.01); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[3]._theta, 0, 0.01); - - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[4]._x, -1.69047, 0.01); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[4]._y, 0.3747, 0.01); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[4]._theta, -5, 0.01); - - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[5]._x, -1.69047, 0.01); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[5]._y, -0.3747, 0.01); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[5]._theta, 5, 0.01); - - nav2_costmap_2d::Costmap2D costmapA(100, 100, 0.05, 0.0, 0.0, 0); - nav2_smac_planner::GridCollisionChecker checker(&costmapA); - nav2_smac_planner::NodeSE2 * node = new nav2_smac_planner::NodeSE2(49); - std::function neighborGetter = - [&, this](const unsigned int & index, nav2_smac_planner::NodeSE2 * & neighbor_rtn) -> bool - { - // because we don't return a real object - return false; - }; - - nav2_smac_planner::NodeSE2::NodeVector neighbors; - nav2_smac_planner::NodeSE2::getNeighbors(node, neighborGetter, checker, false, neighbors); - delete node; - - // should be empty since totally invalid - EXPECT_EQ(neighbors.size(), 0u); -} diff --git a/nav2_smac_planner/test/test_smac_2d.cpp b/nav2_smac_planner/test/test_smac_2d.cpp index 59a31eb17b..f3391d4240 100644 --- a/nav2_smac_planner/test/test_smac_2d.cpp +++ b/nav2_smac_planner/test/test_smac_2d.cpp @@ -23,10 +23,10 @@ #include "nav2_costmap_2d/costmap_subscriber.hpp" #include "nav2_util/lifecycle_node.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "nav2_smac_planner/node_se2.hpp" +#include "nav2_smac_planner/node_hybrid.hpp" #include "nav2_smac_planner/a_star.hpp" #include "nav2_smac_planner/collision_checker.hpp" -#include "nav2_smac_planner/smac_planner.hpp" +#include "nav2_smac_planner/smac_planner_hybrid.hpp" #include "nav2_smac_planner/smac_planner_2d.hpp" class RclCppFixture diff --git a/nav2_smac_planner/test/test_smac_se2.cpp b/nav2_smac_planner/test/test_smac_hybrid.cpp similarity index 85% rename from nav2_smac_planner/test/test_smac_se2.cpp rename to nav2_smac_planner/test/test_smac_hybrid.cpp index c0a4cf8e8e..9c6d50a0b5 100644 --- a/nav2_smac_planner/test/test_smac_se2.cpp +++ b/nav2_smac_planner/test/test_smac_hybrid.cpp @@ -23,10 +23,10 @@ #include "nav2_costmap_2d/costmap_subscriber.hpp" #include "nav2_util/lifecycle_node.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "nav2_smac_planner/node_se2.hpp" +#include "nav2_smac_planner/node_hybrid.hpp" #include "nav2_smac_planner/a_star.hpp" #include "nav2_smac_planner/collision_checker.hpp" -#include "nav2_smac_planner/smac_planner.hpp" +#include "nav2_smac_planner/smac_planner_hybrid.hpp" #include "nav2_smac_planner/smac_planner_2d.hpp" class RclCppFixture @@ -50,8 +50,6 @@ TEST(SmacTest, test_smac_se2) std::make_shared("global_costmap"); costmap_ros->on_configure(rclcpp_lifecycle::State()); - nodeSE2->declare_parameter("test.smooth_path", true); - nodeSE2->set_parameter(rclcpp::Parameter("test.smooth_path", true)); nodeSE2->declare_parameter("test.downsample_costmap", true); nodeSE2->set_parameter(rclcpp::Parameter("test.downsample_costmap", true)); nodeSE2->declare_parameter("test.downsampling_factor", 2); @@ -61,8 +59,10 @@ TEST(SmacTest, test_smac_se2) start.pose.position.x = 0.0; start.pose.position.y = 0.0; start.pose.orientation.w = 1.0; - goal = start; - auto planner = std::make_unique(); + goal.pose.position.x = 1.0; + goal.pose.position.y = 1.0; + goal.pose.orientation.w = 1.0; + auto planner = std::make_unique(); planner->configure(nodeSE2, "test", nullptr, costmap_ros); planner->activate(); @@ -79,14 +79,3 @@ TEST(SmacTest, test_smac_se2) costmap_ros.reset(); nodeSE2.reset(); } - -TEST(SmacTestSE2, test_dist) -{ - Eigen::Vector2d p1; - p1[0] = 0.0; - p1[1] = 0.0; - Eigen::Vector2d p2; - p2[0] = 0.0; - p2[1] = 1.0; - EXPECT_NEAR(nav2_smac_planner::squaredDistance(p1, p2), 1.0, 0.001); -} diff --git a/nav2_smac_planner/test/test_smoother.cpp b/nav2_smac_planner/test/test_smoother.cpp deleted file mode 100644 index 41c8280950..0000000000 --- a/nav2_smac_planner/test/test_smoother.cpp +++ /dev/null @@ -1,104 +0,0 @@ -// Copyright (c) 2020, Samsung Research America -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. Reserved. - -#include -#include -#include -#include - -#include "gtest/gtest.h" -#include "rclcpp/rclcpp.hpp" -#include "nav2_costmap_2d/costmap_2d.hpp" -#include "nav2_util/lifecycle_node.hpp" -#include "nav2_smac_planner/a_star.hpp" -#include "nav2_smac_planner/smoother.hpp" - -class RclCppFixture -{ -public: - RclCppFixture() {rclcpp::init(0, nullptr);} - ~RclCppFixture() {rclcpp::shutdown();} -}; -RclCppFixture g_rclcppfixture; - -TEST(SmootherTest, test_smoother) -{ - rclcpp_lifecycle::LifecycleNode::SharedPtr node2D = - std::make_shared("SmootherTest"); - - // create and populate costmap - nav2_costmap_2d::Costmap2D * costmap = new nav2_costmap_2d::Costmap2D(100, 100, 0.05, 0, 0, 0); - - for (unsigned int i = 40; i <= 60; ++i) { - for (unsigned int j = 40; j <= 60; ++j) { - costmap->setCost(i, j, 254); - } - } - - // compute path to use - nav2_smac_planner::SearchInfo info; - info.change_penalty = 1.2; - info.non_straight_penalty = 1.4; - info.reverse_penalty = 2.1; - info.minimum_turning_radius = 4.0; // in grid coordinates - unsigned int size_theta = 72; - nav2_smac_planner::AStarAlgorithm a_star( - nav2_smac_planner::MotionModel::DUBIN, info); - int max_iterations = 1000000; - float tolerance = 0.0; - int it_on_approach = 1000000000; - int num_it = 0; - a_star.initialize(false, max_iterations, it_on_approach); - a_star.setFootprint(nav2_costmap_2d::Footprint(), true); - a_star.createGraph(costmap->getSizeInCellsX(), costmap->getSizeInCellsY(), size_theta, costmap); - a_star.setStart(10u, 10u, 0u); - a_star.setGoal(80u, 80u, 40u); - nav2_smac_planner::NodeSE2::CoordinateVector plan; - EXPECT_TRUE(a_star.createPath(plan, num_it, tolerance)); - - // populate our smoothing paths - std::vector path; - std::vector initial_path; - for (unsigned int i = 0; i != plan.size(); i++) { - path.push_back(Eigen::Vector2d(plan[i].x, plan[i].y)); - initial_path.push_back(Eigen::Vector2d(plan[i].x, plan[i].y)); - } - - nav2_smac_planner::OptimizerParams params; - params.debug = true; - params.get(node2D.get(), "test"); - - nav2_smac_planner::SmootherParams smoother_params; - smoother_params.get(node2D.get(), "test"); - smoother_params.max_curvature = 5.0; - smoother_params.curvature_weight = 30.0; - smoother_params.distance_weight = 0.0; - smoother_params.smooth_weight = 00.0; - smoother_params.costmap_weight = 0.025; - - nav2_smac_planner::Smoother smoother; - smoother.initialize(params); - smoother.smooth(path, costmap, smoother_params); - - // kept at the right size - EXPECT_EQ(path.size(), 73u); - - for (unsigned int i = 1; i != path.size() - 1; i++) { - // check distance between points is in a good range - EXPECT_NEAR( - hypot(path[i][0] - path[i + 1][0], path[i][1] - path[i + 1][1]), 1.407170, 0.5); - } - - delete costmap; -} diff --git a/nav2_theta_star_planner/CMakeLists.txt b/nav2_theta_star_planner/CMakeLists.txt new file mode 100644 index 0000000000..8cf8a28c35 --- /dev/null +++ b/nav2_theta_star_planner/CMakeLists.txt @@ -0,0 +1,81 @@ +cmake_minimum_required(VERSION 3.5) +project(nav2_theta_star_planner) + +find_package(ament_cmake REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(nav2_common REQUIRED) +find_package(nav2_core REQUIRED) +find_package(nav2_costmap_2d REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(nav2_util REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(tf2_ros REQUIRED) + +nav2_package() #Calls the nav2_package.cmake file +add_compile_options(-O3) + +include_directories( + include +) + +set(library_name ${PROJECT_NAME}) + +set(dependencies ament_cmake + builtin_interfaces + nav2_common + nav2_core + nav2_costmap_2d + nav2_msgs + nav2_util + pluginlib + rclcpp + rclcpp_action + rclcpp_lifecycle + tf2_ros +) + + +add_library(${library_name} SHARED + src/theta_star.cpp + src/theta_star_planner.cpp +) + +ament_target_dependencies(${library_name} ${dependencies}) + +target_compile_definitions(${library_name} PUBLIC "PLUGINLIB_DISABLE_BOOST_FUNCTIONS") + +pluginlib_export_plugin_description_file(nav2_core theta_star_planner.xml) + +install(TARGETS ${library_name} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include/ +) + +install(FILES theta_star_planner.xml + DESTINATION share/${PROJECT_NAME} +) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(gtest_disable_pthreads OFF) + ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(test_theta_star test/test_theta_star.cpp) + ament_target_dependencies(test_theta_star ${dependencies}) + target_link_libraries(test_theta_star ${library_name}) +endif() + + +ament_export_include_directories(include) +ament_export_libraries(${library_name}) +ament_export_dependencies(${dependencies}) +ament_package() diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md new file mode 100644 index 0000000000..ed3f810f09 --- /dev/null +++ b/nav2_theta_star_planner/README.md @@ -0,0 +1,95 @@ +# Nav2 Theta Star Planner +The Theta Star Planner is a global planning plugin meant to be used with the Nav2 Planner Server. The `nav2_theta_star_planner` implements a highly optimized version of the Theta\* Planner (specifically the [Lazy Theta\* P variant](http://idm-lab.org/bib/abstracts/papers/aaai10b.pdf)) meant to plan any-angle paths using A\*. The planner supports differential-drive and omni-directional robots. + +## Features +- The planner uses A\* search along with line of sight (LOS) checks to form any-angle paths thus avoiding zig-zag paths that may be present in the usual implementation of A\* +- As it also considers the costmap traversal cost during execution it tends to smoothen the paths automatically, thus mitigating the need to smoothen the path (The presence of sharp turns depends on the resolution of the map, and it decreases as the map resolution increases) +- Uses the costs from the costmap to penalise high cost regions +- Allows to control the path behavior to either be any angle directed or to be in the middle of the spaces +- Is well suited for smaller robots of the omni-directional and differential drive kind +- The algorithmic part of the planner has been segregated from the plugin part to allow for reusability + +## Metrics +For the below example the planner took ~46ms (averaged value) to compute the path of 87.5m - +![example.png](img/00-37.png) + +The parameters were set to - `w_euc_cost: 1.0`, `w_traversal_cost: 5.0`, `w_heuristic_cost: 1.0` and the `global_costmap`'s `inflation_layer` parameters are set as - `cost_scaling_factor:5.0`, `inflation_radius: 5.5` + +## Cost Function Details +### Symbols and their meanings +**g(a)** - cost function cost for the node 'a' + +**h(a)** - heuristic function cost for the node 'a' + +**f(a)** - total cost (g(a) + h(a)) for the node 'a' + +**LETHAL_COST** - a value of the costmap traversal cost that inscribes an obstacle with +respect to a function, value = 253 + +**curr** - represents the node whose neighbours are being added to the list + +**neigh** - one of the neighboring nodes of curr + +**par** - parent node of curr + +**euc_cost(a,b)** - euclidean distance between the node type 'a' and type 'b' + +**costmap_cost(a,b)** - the costmap traversal cost (ranges from 0 - 252, 254 = unknown value) between the node type 'a' and type 'b' + +### Cost function +``` +g(neigh) = g(curr) + w_euc_cost*euc_cost(curr, neigh) + w_traversal_cost*(costmap_cost(curr,neigh)/LETHAL_COST)^2 +h(neigh) = w_heuristic_cost * euc_cost(neigh, goal) +f(neigh) = g(neigh) + h(neigh) +``` +Because of how the program works when the 'neigh' init_rclcpp is to be expanded, depending +on the result of the LOS check, (if the LOS check returns true) the value of g(neigh) might change to `g(par) + +w1*euc_cost(par, neigh) + w2*(costmap(par,neigh)/LETHAL_COST)^2` + +## Parameters +The parameters of the planner are : +- ` .how_many_corners ` : to choose between 4-connected and 8-connected graph expansions, the accepted values are 4 and 8 +- ` .w_euc_cost ` : weight applied on the length of the path. +- ` .w_traversal_cost ` : it tunes how harshly the nodes of high cost are penalised. From the above g(neigh) equation you can see that the cost-aware component of the cost function forms a parabolic curve, thus this parameter would, on increasing its value, make that curve steeper allowing for a greater differentiation (as the delta of costs would increase, when the graph becomes steep) among the nodes of different costs. +- ` .w_heuristic_cost ` : it has been provided to have an admissible heuristic + +Below are the default values of the parameters : +``` +planner_server: + ros__parameters: + planner_plugin_types: ["nav2_theta_star_planner/ThetaStarPlanner"] + use_sim_time: True + planner_plugin_ids: ["GridBased"] + GridBased: + how_many_corners: 8 + w_euc_cost: 1.0 + w_traversal_cost: 2.0 + w_heuristic_cost: 1.0 +``` + +## Usage Notes + +### Tuning the Parameters +Before starting off, do note that the costmap_cost(curr,neigh) component after being operated (ie before being multiplied to its parameter and being substituted in g(init_rclcpp)) varies from 0 to 1. + +This planner uses the costs associated with each cell from the `global_costmap` as a measure of the point's proximity to the obstacles. Providing a gentle potential field that covers the entirety of the region (thus leading to only small pocket like regions of cost = 0) is recommended in order to achieve paths that pass through the middle of the spaces. A good starting point could be to set the `inflation_layer`'s parameters as - `cost_scaling_factor: 10.0`, `inflation_radius: 5.5` and then decrease the value of `cost_scaling_factor` to achieve the said potential field. + +Providing a gentle potential field over the region allows the planner to exchange the increase in path lengths for a decrease in the accumulated traversal cost, thus leading to an increase in the distance from the obstacles. This around a corner allows for naturally smoothing the turns and removes the requirement for an external path smoother. + +To begin with, you can set the parameters to its default values and then try increasing the value of `w_traversal_cost` to achieve those middling paths, but this would adversly make the paths less taut. So it is recommend ed that you simultaneously tune the value of `w_euc_cost`. Increasing `w_euc_cost` increases the tautness of the path, which leads to more straight line like paths (any-angled paths). Do note that the query time from the planner would increase for higher values of `w_traversal_cost` as more nodes would have to be expanded to lower the cost of the path, to counteract this you may also try setting `w_euc_cost` to a higher value and check if it reduces the query time. + +Usually set `w_heuristic_cost` at the same value as `w_euc_cost` or 1.0 (whichever is lower). Though as a last resort you may increase the value of `w_heuristic_cost` to speed up the planning process, this is not recommended as it might not always lead to shorter query times, but would definitely give you sub optimal paths + +Also note that changes to `w_traversal_cost` might cause slow downs, in case the number of node expanisions increase thus tuning it along with `w_euc_cost` is the way to go to. + +While tuning the planner's parameters you can also change the `inflation_layer`'s parameters (of the global costmap) to tune the behavior of the paths. + +### Path Smoothing +Because of how the cost function works, the output path has a natural tendency to form smooth curves around corners, though the smoothness of the path depends on how wide the turn is, and the number of cells in that turn. + +This planner is recommended to be used with local planners like DWB or TEB (or other any local planner / controllers that form a local trajectory to be traversed) as these take into account the abrupt turns which might arise due to the planner not being able to find a smoother turns owing to the aforementioned reasons. + +While smoother paths can be achieved by increasing the costmap resolution (ie using a costmap of 1cm resolution rather than a 5cm one) it is not recommended to do so because it might increase the query times from the planner. Test the planners performance on the higher cm/px costmaps before making a switch to finer costmaps. + +### Possible Applications +This planner could be used in scenarios where planning speed matters over an extremely smooth path, which could anyways be handled by using a local planner/controller as mentioned above. Because of the any-angled nature of paths you can traverse environments diagonally (wherever it is allowed, eg: in a wide open region). diff --git a/nav2_theta_star_planner/img/00-37.png b/nav2_theta_star_planner/img/00-37.png new file mode 100644 index 0000000000..fa4115bfb9 Binary files /dev/null and b/nav2_theta_star_planner/img/00-37.png differ diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp new file mode 100644 index 0000000000..dacd8a57bc --- /dev/null +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp @@ -0,0 +1,306 @@ +// Copyright 2020 Anshumaan Singh +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_THETA_STAR_PLANNER__THETA_STAR_HPP_ +#define NAV2_THETA_STAR_PLANNER__THETA_STAR_HPP_ + +#include +#include +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" + +const double INF_COST = DBL_MAX; +const int LETHAL_COST = 252; + +struct coordsM +{ + int x, y; +}; + +struct coordsW +{ + double x, y; +}; + +struct tree_node +{ + int x, y; + double g = INF_COST; + double h = INF_COST; + const tree_node * parent_id = nullptr; + bool is_in_queue = false; + double f = INF_COST; +}; + +struct comp +{ + bool operator()(const tree_node * p1, const tree_node * p2) + { + return (p1->f) > (p2->f); + } +}; + +namespace theta_star +{ +class ThetaStar +{ +public: + coordsM src_{}, dst_{}; + nav2_costmap_2d::Costmap2D * costmap_{}; + + /// weight on the costmap traversal cost + double w_traversal_cost_; + /// weight on the euclidean distance cost (used for calculations of g_cost) + double w_euc_cost_; + /// weight on the heuristic cost (used for h_cost calculations) + double w_heuristic_cost_; + /// parameter to set the number of adjacent nodes to be searched on + int how_many_corners_; + /// the x-directional and y-directional lengths of the map respectively + int size_x_, size_y_; + + ThetaStar(); + + ~ThetaStar() = default; + + /** + * @brief it iteratively searches upon the nodes in the queue (open list) until the + * current node is the goal pose or the size of queue becomes 0 + * @param raw_path is used to return the path obtained by executing the algorithm + * @return true if a path is found, false if no path is found in between the start and goal pose + */ + bool generatePath(std::vector & raw_path); + + /** + * @brief this function checks whether the cost of a point(cx, cy) on the costmap is less than the LETHAL_COST + * @return the result of the check + */ + inline bool isSafe(const int & cx, const int & cy) const + { + return costmap_->getCost(cx, cy) < LETHAL_COST; + } + + /** + * @brief initialises the values of the start and goal points + */ + void setStartAndGoal( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal); + + /** + * @brief checks whether the start and goal points have costmap costs greater than LETHAL_COST + * @return true if the cost of any one of the points is greater than LETHAL_COST + */ + bool isUnsafeToPlan() const + { + return !(isSafe(src_.x, src_.y)) || !(isSafe(dst_.x, dst_.y)); + } + + int nodes_opened = 0; + +protected: + /// for the coordinates (x,y), it stores at node_position_[size_x_ * y + x], + /// the pointer to the location at which the data of the node is present in nodes_data_ + /// it is initialised with size_x_ * size_y_ elements + /// and its number of elements increases to account for a change in map size + std::vector node_position_; + + /// the vector nodes_data_ stores the coordinates, costs and index of the parent node, + /// and whether or not the node is present in queue_, for all the nodes searched + /// it is initialised with no elements + /// and its size increases depending on the number of nodes searched + std::vector nodes_data_; + + /// this is the priority queue (open_list) to select the next node to be expanded + std::priority_queue, comp> queue_; + + /// it is a counter like variable used to generate consecutive indices + /// such that the data for all the nodes (in open and closed lists) could be stored + /// consecutively in nodes_data_ + int index_generated_; + + const coordsM moves[8] = {{0, 1}, + {0, -1}, + {1, 0}, + {-1, 0}, + {1, -1}, + {-1, 1}, + {1, 1}, + {-1, -1}}; + + tree_node * exp_node; + + + /** @brief it performs a line of sight (los) check between the current node and the parent node of its parent node; + * if an los is found and the new costs calculated are lesser, then the cost and parent node + * of the current node is updated + * @param data of the current node + */ + void resetParent(tree_node * curr_data); + + /** + * @brief this function expands the current node + * @param curr_data used to send the data of the current node + * @param curr_id used to send the index of the current node as stored in nodes_position_ + */ + void setNeighbors(const tree_node * curr_data); + + /** + * @brief performs the line of sight check using Bresenham's Algorithm, + * and has been modified to calculate the traversal cost incurred in a straight line path between + * the two points whose coordinates are (x0, y0) and (x1, y1) + * @param sl_cost is used to return the cost thus incurred + * @return true if a line of sight exists between the points + */ + bool losCheck( + const int & x0, const int & y0, const int & x1, const int & y1, + double & sl_cost) const; + + /** + * @brief it returns the path by backtracking from the goal to the start, by using their parent nodes + * @param raw_points used to return the path thus found + * @param curr_id sends in the index of the goal coordinate, as stored in nodes_position + */ + void backtrace(std::vector & raw_points, const tree_node * curr_n) const; + + /** + * @brief it is an overloaded function to ease the cost calculations while performing the LOS check + * @param cost denotes the total straight line traversal cost; it adds the traversal cost for the node (cx, cy) at every instance; it is also being returned + * @return false if the traversal cost is greater than / equal to the LETHAL_COST and true otherwise + */ + bool isSafe(const int & cx, const int & cy, double & cost) const + { + double curr_cost = getCost(cx, cy); + if (curr_cost < LETHAL_COST) { + cost += w_traversal_cost_ * curr_cost * curr_cost / LETHAL_COST / LETHAL_COST; + return true; + } else { + return false; + } + } + + /* + * @brief this function scales the costmap cost by shifting the origin to 25 and then multiply + * the actual costmap cost by 0.9 to keep the output in the range of [25, 255) + */ + inline double getCost(const int & cx, const int & cy) const + { + return 26 + 0.9 * costmap_->getCost(cx, cy); + } + + /** + * @brief for the point(cx, cy), its traversal cost is calculated by + * *()^2/()^2 + * @return the traversal cost thus calculated + */ + inline double getTraversalCost(const int & cx, const int & cy) + { + double curr_cost = getCost(cx, cy); + return w_traversal_cost_ * curr_cost * curr_cost / LETHAL_COST / LETHAL_COST; + } + + /** + * @brief calculates the piecewise straight line euclidean distances by + * * + * @return the distance thus calculated + */ + inline double getEuclideanCost(const int & ax, const int & ay, const int & bx, const int & by) + { + return w_euc_cost_ * std::hypot(ax - bx, ay - by); + } + + /** + * @brief for the point(cx, cy), its heuristic cost is calculated by + * * + * @return the heuristic cost + */ + inline double getHCost(const int & cx, const int & cy) + { + return w_heuristic_cost_ * std::hypot(cx - dst_.x, cy - dst_.y); + } + + /** + * @brief checks if the given coordinates(cx, cy) lies within the map + * @return the result of the check + */ + inline bool withinLimits(const int & cx, const int & cy) const + { + return cx >= 0 && cx < size_x_ && cy >= 0 && cy < size_y_; + } + + /** + * @brief checks if the coordinates of a node is the goal or not + * @return the result of the check + */ + inline bool isGoal(const tree_node & this_node) const + { + return this_node.x == dst_.x && this_node.y == dst_.y; + } + + /** + * @brief initialises the node_position_ vector by storing -1 as index for all points(x, y) within the limits of the map + * @param size_inc is used to increase the number of elements in node_position_ in case the size of the map increases + */ + void initializePosn(int size_inc = 0); + + /** + * @brief it stores id_this in node_position_ at the index [ size_x_*cy + cx ] + * @param id_this a pointer to the location at which the data of the point(cx, cy) is stored in nodes_data_ + */ + inline void addIndex(const int & cx, const int & cy, tree_node * node_this) + { + node_position_[size_x_ * cy + cx] = node_this; + } + + /** + * @brief retrieves the pointer of the location at which the data of the point(cx, cy) is stored in nodes_data + * @return id_this is the pointer to that location + */ + inline tree_node * getIndex(const int & cx, const int & cy) + { + return node_position_[size_x_ * cy + cx]; + } + + /** + * @brief this function depending on the size of the nodes_data_ vector allots space to store the data for a node(x, y) + * @param id_this is the index at which the data is stored/has to be stored for that node + */ + void addToNodesData(const int & id_this) + { + if (static_cast(nodes_data_.size()) <= id_this) { + nodes_data_.push_back({}); + } else { + nodes_data_[id_this] = {}; + } + } + + /** + * @brief initialises the values of global variables at beginning of the execution of the generatePath function + */ + void resetContainers(); + + /** + * @brief clears the priority queue after each execution of the generatePath function + */ + void clearQueue() + { + queue_ = std::priority_queue, comp>(); + } +}; +} // namespace theta_star + +#endif // NAV2_THETA_STAR_PLANNER__THETA_STAR_HPP_ diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp new file mode 100644 index 0000000000..c90b809291 --- /dev/null +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp @@ -0,0 +1,83 @@ +// Copyright 2020 Anshumaan Singh +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_THETA_STAR_PLANNER__THETA_STAR_PLANNER_HPP_ +#define NAV2_THETA_STAR_PLANNER__THETA_STAR_PLANNER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "nav2_core/global_planner.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_theta_star_planner/theta_star.hpp" + +namespace nav2_theta_star_planner +{ + +class ThetaStarPlanner : public nav2_core::GlobalPlanner +{ +public: + void configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + std::string name, std::shared_ptr tf, + std::shared_ptr costmap_ros) override; + + void cleanup() override; + + void activate() override; + + void deactivate() override; + + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) override; + +protected: + std::shared_ptr tf_; + rclcpp::Clock::SharedPtr clock_; + rclcpp::Logger logger_{rclcpp::get_logger("ThetaStarPlanner")}; + std::string global_frame_, name_; + + std::unique_ptr planner_; + + /** + * @brief the function responsible for calling the algorithm and retrieving a path from it + * @return global_path is the planned path to be taken + */ + void getPlan(nav_msgs::msg::Path & global_path); + + /** + * @brief interpolates points between the consecutive waypoints of the path + * @param raw_path is used to send in the path received from the planner + * @param dist_bw_points is used to send in the interpolation_resolution (which has been set as the costmap resolution) + * @return the final path with waypoints at a distance of the value of interpolation_resolution of each other + */ + static nav_msgs::msg::Path linearInterpolation( + const std::vector & raw_path, + const double & dist_bw_points); +}; +} // namespace nav2_theta_star_planner + +#endif // NAV2_THETA_STAR_PLANNER__THETA_STAR_PLANNER_HPP_ diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml new file mode 100644 index 0000000000..67a4839abf --- /dev/null +++ b/nav2_theta_star_planner/package.xml @@ -0,0 +1,34 @@ + + + + nav2_theta_star_planner + 0.4.0 + Theta* Global Planning Plugin + Steve Macenski + Anshumaan Singh + Apache-2.0 + + ament_cmake + + builtin_interfaces + nav2_common + nav2_core + nav2_costmap_2d + nav2_msgs + nav2_util + pluginlib + rclcpp + rclcpp_action + rclcpp_lifecycle + tf2_ros + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + + ament_cmake + + + + diff --git a/nav2_theta_star_planner/src/theta_star.cpp b/nav2_theta_star_planner/src/theta_star.cpp new file mode 100644 index 0000000000..e0267b34c0 --- /dev/null +++ b/nav2_theta_star_planner/src/theta_star.cpp @@ -0,0 +1,262 @@ +// Copyright 2020 Anshumaan Singh +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include "nav2_theta_star_planner/theta_star.hpp" + +namespace theta_star +{ + +ThetaStar::ThetaStar() +: w_traversal_cost_(1.0), + w_euc_cost_(2.0), + w_heuristic_cost_(1.0), + how_many_corners_(8), + size_x_(0), + size_y_(0), + index_generated_(0) +{ + exp_node = new tree_node; +} + +void ThetaStar::setStartAndGoal( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) +{ + unsigned int s[2], d[2]; + costmap_->worldToMap(start.pose.position.x, start.pose.position.y, s[0], s[1]); + costmap_->worldToMap(goal.pose.position.x, goal.pose.position.y, d[0], d[1]); + + src_ = {static_cast(s[0]), static_cast(s[1])}; + dst_ = {static_cast(d[0]), static_cast(d[1])}; +} + +bool ThetaStar::generatePath(std::vector & raw_path) +{ + resetContainers(); + addToNodesData(index_generated_); + double src_g_cost = getTraversalCost(src_.x, src_.y), src_h_cost = getHCost(src_.x, src_.y); + nodes_data_[index_generated_] = + {src_.x, src_.y, src_g_cost, src_h_cost, &nodes_data_[index_generated_], true, + src_g_cost + src_h_cost}; + queue_.push({&nodes_data_[index_generated_]}); + addIndex(src_.x, src_.y, &nodes_data_[index_generated_]); + tree_node * curr_data = &nodes_data_[index_generated_]; + index_generated_++; + nodes_opened = 0; + + while (!queue_.empty()) { + nodes_opened++; + + if (isGoal(*curr_data)) { + break; + } + + resetParent(curr_data); + setNeighbors(curr_data); + + curr_data = queue_.top(); + queue_.pop(); + } + + if (queue_.empty()) { + raw_path.clear(); + return false; + } + + backtrace(raw_path, curr_data); + clearQueue(); + + return true; +} + +void ThetaStar::resetParent(tree_node * curr_data) +{ + double g_cost, los_cost = 0; + curr_data->is_in_queue = false; + const tree_node * curr_par = curr_data->parent_id; + const tree_node * maybe_par = curr_par->parent_id; + + if (losCheck(curr_data->x, curr_data->y, maybe_par->x, maybe_par->y, los_cost)) { + g_cost = maybe_par->g + + getEuclideanCost(curr_data->x, curr_data->y, maybe_par->x, maybe_par->y) + los_cost; + + if (g_cost < curr_data->g) { + curr_data->parent_id = maybe_par; + curr_data->g = g_cost; + curr_data->f = g_cost + curr_data->h; + } + } +} + +void ThetaStar::setNeighbors(const tree_node * curr_data) +{ + int mx, my; + tree_node * m_id = nullptr; + double g_cost, h_cost, cal_cost; + + for (int i = 0; i < how_many_corners_; i++) { + mx = curr_data->x + moves[i].x; + my = curr_data->y + moves[i].y; + + if (withinLimits(mx, my)) { + if (!isSafe(mx, my)) { + continue; + } + } else { + continue; + } + + g_cost = curr_data->g + getEuclideanCost(curr_data->x, curr_data->y, mx, my) + + getTraversalCost(mx, my); + + m_id = getIndex(mx, my); + + if (m_id == nullptr) { + addToNodesData(index_generated_); + m_id = &nodes_data_[index_generated_]; + addIndex(mx, my, m_id); + index_generated_++; + } + + exp_node = m_id; + + h_cost = getHCost(mx, my); + cal_cost = g_cost + h_cost; + if (exp_node->f > cal_cost) { + exp_node->g = g_cost; + exp_node->h = h_cost; + exp_node->f = cal_cost; + exp_node->parent_id = curr_data; + if (!exp_node->is_in_queue) { + exp_node->x = mx; + exp_node->y = my; + exp_node->is_in_queue = true; + queue_.push({m_id}); + } + } + } +} + +void ThetaStar::backtrace(std::vector & raw_points, const tree_node * curr_n) const +{ + std::vector path_rev; + coordsW world{}; + do { + costmap_->mapToWorld(curr_n->x, curr_n->y, world.x, world.y); + path_rev.push_back(world); + if (path_rev.size() > 1) { + curr_n = curr_n->parent_id; + } + } while (curr_n->parent_id != curr_n); + costmap_->mapToWorld(curr_n->x, curr_n->y, world.x, world.y); + path_rev.push_back(world); + + raw_points.reserve(path_rev.size()); + for (int i = static_cast(path_rev.size()) - 1; i >= 0; i--) { + raw_points.push_back(path_rev[i]); + } +} + +bool ThetaStar::losCheck( + const int & x0, const int & y0, const int & x1, const int & y1, + double & sl_cost) const +{ + sl_cost = 0; + + int cx, cy; + int dy = abs(y1 - y0), dx = abs(x1 - x0), f = 0; + int sx, sy; + sx = x1 > x0 ? 1 : -1; + sy = y1 > y0 ? 1 : -1; + + int u_x = (sx - 1) / 2; + int u_y = (sy - 1) / 2; + cx = x0; + cy = y0; + + if (dx >= dy) { + while (cx != x1) { + f += dy; + if (f >= dx) { + if (!isSafe(cx + u_x, cy + u_y, sl_cost)) { + return false; + } + cy += sy; + f -= dx; + } + if (f != 0 && !isSafe(cx + u_x, cy + u_y, sl_cost)) { + return false; + } + if (dy == 0 && !isSafe(cx + u_x, cy, sl_cost) && !isSafe(cx + u_x, cy - 1, sl_cost)) { + return false; + } + cx += sx; + } + } else { + while (cy != y1) { + f = f + dx; + if (f >= dy) { + if (!isSafe(cx + u_x, cy + u_y, sl_cost)) { + return false; + } + cx += sx; + f -= dy; + } + if (f != 0 && !isSafe(cx + u_x, cy + u_y, sl_cost)) { + return false; + } + if (dx == 0 && !isSafe(cx, cy + u_y, sl_cost) && !isSafe(cx - 1, cy + u_y, sl_cost)) { + return false; + } + cy += sy; + } + } + return true; +} + +void ThetaStar::resetContainers() +{ + index_generated_ = 0; + int last_size_x = size_x_; + int last_size_y = size_y_; + int curr_size_x = static_cast(costmap_->getSizeInCellsX()); + int curr_size_y = static_cast(costmap_->getSizeInCellsY()); + if (((last_size_x != curr_size_x) || (last_size_y != curr_size_y)) && + static_cast(node_position_.size()) < (curr_size_x * curr_size_y)) + { + initializePosn(curr_size_y * curr_size_x - last_size_y * last_size_x); + nodes_data_.reserve(curr_size_x * curr_size_y); + } else { + initializePosn(); + } + size_x_ = curr_size_x; + size_y_ = curr_size_y; +} + +void ThetaStar::initializePosn(int size_inc) +{ + int i = 0; + + if (!node_position_.empty()) { + for (; i < size_x_ * size_y_; i++) { + node_position_[i] = nullptr; + } + } + + for (; i < size_inc; i++) { + node_position_.push_back(nullptr); + } +} +} // namespace theta_star diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp new file mode 100644 index 0000000000..437cbd4471 --- /dev/null +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -0,0 +1,142 @@ +// Copyright 2020 Anshumaan Singh +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include "nav2_theta_star_planner/theta_star_planner.hpp" +#include "nav2_theta_star_planner/theta_star.hpp" + +namespace nav2_theta_star_planner +{ +void ThetaStarPlanner::configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + std::string name, std::shared_ptr tf, + std::shared_ptr costmap_ros) +{ + planner_ = std::make_unique(); + auto node = parent.lock(); + logger_ = node->get_logger(); + clock_ = node->get_clock(); + name_ = name; + tf_ = tf; + planner_->costmap_ = costmap_ros->getCostmap(); + global_frame_ = costmap_ros->getGlobalFrameID(); + + nav2_util::declare_parameter_if_not_declared( + node, name_ + ".how_many_corners", rclcpp::ParameterValue(8)); + + node->get_parameter(name_ + ".how_many_corners", planner_->how_many_corners_); + + if (planner_->how_many_corners_ != 8 && planner_->how_many_corners_ != 4) { + planner_->how_many_corners_ = 8; + RCLCPP_WARN(logger_, "Your value for - .how_many_corners was overridden, and is now set to 8"); + } + + nav2_util::declare_parameter_if_not_declared( + node, name_ + ".w_euc_cost", rclcpp::ParameterValue(1.0)); + node->get_parameter(name_ + ".w_euc_cost", planner_->w_euc_cost_); + + nav2_util::declare_parameter_if_not_declared( + node, name_ + ".w_traversal_cost", rclcpp::ParameterValue(2.0)); + node->get_parameter(name_ + ".w_traversal_cost", planner_->w_traversal_cost_); + + nav2_util::declare_parameter_if_not_declared( + node, name_ + ".w_heuristic_cost", rclcpp::ParameterValue(1.0)); + node->get_parameter(name_ + ".w_heuristic_cost", planner_->w_heuristic_cost_); +} + +void ThetaStarPlanner::cleanup() +{ + RCLCPP_INFO(logger_, "CleaningUp plugin %s of type nav2_theta_star_planner", name_.c_str()); + planner_.reset(); +} + +void ThetaStarPlanner::activate() +{ + RCLCPP_INFO(logger_, "Activating plugin %s of type nav2_theta_star_planner", name_.c_str()); +} + +void ThetaStarPlanner::deactivate() +{ + RCLCPP_INFO(logger_, "Deactivating plugin %s of type nav2_theta_star_planner", name_.c_str()); +} + +nav_msgs::msg::Path ThetaStarPlanner::createPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) +{ + nav_msgs::msg::Path global_path; + auto start_time = std::chrono::steady_clock::now(); + planner_->setStartAndGoal(start, goal); + RCLCPP_DEBUG( + logger_, "Got the src and dst... (%i, %i) && (%i, %i)", + planner_->src_.x, planner_->src_.y, planner_->dst_.x, planner_->dst_.y); + getPlan(global_path); + auto stop_time = std::chrono::steady_clock::now(); + auto dur = std::chrono::duration_cast(stop_time - start_time); + RCLCPP_DEBUG(logger_, "the time taken for pointy is : %i", static_cast(dur.count())); + RCLCPP_DEBUG(logger_, "the nodes_opened are: %i", planner_->nodes_opened); + return global_path; +} + +void ThetaStarPlanner::getPlan(nav_msgs::msg::Path & global_path) +{ + std::vector path; + if (planner_->isUnsafeToPlan()) { + RCLCPP_ERROR(logger_, "Either of the start or goal pose are an obstacle! "); + global_path.poses.clear(); + } else if (planner_->generatePath(path)) { + global_path = linearInterpolation(path, planner_->costmap_->getResolution()); + } else { + RCLCPP_ERROR(logger_, "Could not generate path between the given poses"); + global_path.poses.clear(); + } + global_path.header.stamp = clock_->now(); + global_path.header.frame_id = global_frame_; +} + +nav_msgs::msg::Path ThetaStarPlanner::linearInterpolation( + const std::vector & raw_path, + const double & dist_bw_points) +{ + nav_msgs::msg::Path pa; + + for (unsigned int j = 0; j < raw_path.size() - 1; j++) { + geometry_msgs::msg::PoseStamped p; + coordsW pt1 = raw_path[j]; + p.pose.position.x = pt1.x; + p.pose.position.y = pt1.y; + pa.poses.push_back(p); + + coordsW pt2 = raw_path[j + 1]; + geometry_msgs::msg::PoseStamped p1; + double distance = std::hypot(pt2.x - pt1.x, pt2.y - pt1.y); + int loops = static_cast(distance / dist_bw_points); + double sin_alpha = (pt2.y - pt1.y) / distance; + double cos_alpha = (pt2.x - pt1.x) / distance; + for (int k = 1; k < loops; k++) { + p1.pose.position.x = pt1.x + k * dist_bw_points * cos_alpha; + p1.pose.position.y = pt1.y + k * dist_bw_points * sin_alpha; + pa.poses.push_back(p1); + } + } + return pa; +} + + +} // namespace nav2_theta_star_planner + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_theta_star_planner::ThetaStarPlanner, nav2_core::GlobalPlanner) diff --git a/nav2_theta_star_planner/test/test_theta_star.cpp b/nav2_theta_star_planner/test/test_theta_star.cpp new file mode 100644 index 0000000000..3deb02a4cc --- /dev/null +++ b/nav2_theta_star_planner/test/test_theta_star.cpp @@ -0,0 +1,176 @@ +// Copyright 2020 Anshumaan Singh +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "nav2_theta_star_planner/theta_star.hpp" +#include "nav2_theta_star_planner/theta_star_planner.hpp" + +class init_rclcpp +{ +public: + init_rclcpp() {rclcpp::init(0, nullptr);} + ~init_rclcpp() {rclcpp::shutdown();} +}; + +/// class created to access the protected members of the ThetaStar class +/// u is used as shorthand for use +class test_theta_star : public theta_star::ThetaStar +{ +public: + int getSizeOfNodePosition() + { + return static_cast(node_position_.size()); + } + + bool ulosCheck(const int & x0, const int & y0, const int & x1, const int & y1, double & sl_cost) + { + return losCheck(x0, y0, x1, y1, sl_cost); + } + + bool uwithinLimits(const int & cx, const int & cy) {return withinLimits(cx, cy);} + + bool uisGoal(const tree_node & this_node) {return isGoal(this_node);} + + void uinitializePosn(int size_inc = 0) + { + node_position_.reserve(size_x_ * size_y_); initializePosn(size_inc); + } + + void uaddIndex(const int & cx, const int & cy) {addIndex(cx, cy, &nodes_data_[0]);} + + tree_node * ugetIndex(const int & cx, const int & cy) {return getIndex(cx, cy);} + + tree_node * test_getIndex() {return &nodes_data_[0];} + + void uaddToNodesData(const int & id) {addToNodesData(id);} + + void uresetContainers() {nodes_data_.clear(); resetContainers();} + + bool runAlgo(std::vector & path) + { + if (!isUnsafeToPlan()) { + return generatePath(path); + } + return false; + } +}; + +init_rclcpp node; + +// Tests meant to test the algorithm itself and its helper functions +TEST(ThetaStarTest, test_theta_star) { + auto planner_ = std::make_unique(); + planner_->costmap_ = new nav2_costmap_2d::Costmap2D(50, 50, 1.0, 0.0, 0.0, 0); + for (int i = 7; i <= 14; i++) { + for (int j = 7; j <= 14; j++) { + planner_->costmap_->setCost(i, j, 253); + } + } + coordsM s = {5, 5}, g = {18, 18}; + int size_x = 20, size_y = 20; + planner_->size_x_ = size_x; + planner_->size_y_ = size_y; + geometry_msgs::msg::PoseStamped start, goal; + start.pose.position.x = s.x; + start.pose.position.y = s.y; + start.pose.orientation.w = 1.0; + goal.pose.position.x = g.x; + goal.pose.position.y = g.y; + goal.pose.orientation.w = 1.0; + /// Check if the setStartAndGoal function works properly + planner_->setStartAndGoal(start, goal); + EXPECT_TRUE(planner_->src_.x == s.x && planner_->src_.y == s.y); + EXPECT_TRUE(planner_->dst_.x == g.x && planner_->dst_.y == g.y); + + /// Check if the initializePosn function works properly + planner_->uinitializePosn(size_x * size_y); + EXPECT_EQ(planner_->getSizeOfNodePosition(), (size_x * size_y)); + + /// Check if the withinLimits function works properly + EXPECT_TRUE(planner_->uwithinLimits(18, 18)); + EXPECT_FALSE(planner_->uwithinLimits(120, 140)); + + tree_node n = {g.x, g.y, 120, 0, NULL, false, 20}; + n.parent_id = &n; + /// Check if the isGoal function works properly + EXPECT_TRUE(planner_->uisGoal(n)); // both (x,y) are the goal coordinates + n.x = 25; + EXPECT_FALSE(planner_->uisGoal(n)); // only y coordinate matches with that of goal + n.x = g.x; + n.y = 20; + EXPECT_FALSE(planner_->uisGoal(n)); // only x coordinate matches with that of goal + n.x = 30; + EXPECT_FALSE(planner_->uisGoal(n)); // both (x, y) are different from the goal coordinate + + /// Check if the isSafe functions work properly + EXPECT_TRUE(planner_->isSafe(5, 5)); // cost at this point is 0 + EXPECT_FALSE(planner_->isSafe(10, 10)); // cost at this point is 253 (>LETHAL_COST) + + /// Check if the functions addIndex & getIndex work properly + coordsM c = {20, 30}; + planner_->uaddToNodesData(0); + planner_->uaddIndex(c.x, c.y); + tree_node * c_node = planner_->ugetIndex(c.x, c.y); + EXPECT_EQ(c_node, planner_->test_getIndex()); + + double sl_cost = 0.0; + /// Checking for the case where the losCheck should return the value as true + EXPECT_TRUE(planner_->ulosCheck(2, 2, 7, 20, sl_cost)); + /// and as false + EXPECT_FALSE(planner_->ulosCheck(2, 2, 18, 18, sl_cost)); + + planner_->uresetContainers(); + std::vector path; + /// Check if the planner returns a path for the case where a path exists + EXPECT_TRUE(planner_->runAlgo(path)); + EXPECT_GT(static_cast(path.size()), 0); + /// and where it doesn't exist + path.clear(); + planner_->src_ = {10, 10}; + EXPECT_FALSE(planner_->runAlgo(path)); + EXPECT_EQ(static_cast(path.size()), 0); +} + +// Smoke tests meant to detect issues arising from the plugin part rather than the algorithm +TEST(ThetaStarPlanner, test_theta_star_planner) { + rclcpp_lifecycle::LifecycleNode::SharedPtr life_node = + std::make_shared("ThetaStarPlannerTest"); + + std::shared_ptr costmap_ros = + std::make_shared("global_costmap"); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + + geometry_msgs::msg::PoseStamped start, goal; + start.pose.position.x = 0.0; + start.pose.position.y = 0.0; + start.pose.orientation.w = 1.0; + goal = start; + auto planner_2d = std::make_unique(); + planner_2d->configure(life_node, "test", nullptr, costmap_ros); + planner_2d->activate(); + + nav_msgs::msg::Path path = planner_2d->createPlan(start, goal); + EXPECT_GT(static_cast(path.poses.size()), 0); + + planner_2d->deactivate(); + planner_2d->cleanup(); + + planner_2d.reset(); + costmap_ros->on_cleanup(rclcpp_lifecycle::State()); + life_node.reset(); + costmap_ros.reset(); +} diff --git a/nav2_theta_star_planner/theta_star_planner.xml b/nav2_theta_star_planner/theta_star_planner.xml new file mode 100644 index 0000000000..1890ab14c3 --- /dev/null +++ b/nav2_theta_star_planner/theta_star_planner.xml @@ -0,0 +1,5 @@ + + + An implementation of the Theta* Algorithm + + diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index 1abbdba75b..ab1f76baab 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -118,7 +118,8 @@ class WaypointFollower : public nav2_util::LifecycleNode // Our action server std::unique_ptr action_server_; ActionClient::SharedPtr nav_to_pose_client_; - rclcpp::Node::SharedPtr client_node_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor callback_group_executor_; std::shared_future::SharedPtr> future_goal_handle_; bool stop_on_failure_; ActionStatus current_goal_status_; diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 3dbb7d21f1..e95203d628 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -56,16 +56,17 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) loop_rate_ = get_parameter("loop_rate").as_int(); waypoint_task_executor_id_ = get_parameter("waypoint_task_executor_plugin").as_string(); - std::vector new_args = rclcpp::NodeOptions().arguments(); - new_args.push_back("--ros-args"); - new_args.push_back("-r"); - new_args.push_back(std::string("__node:=") + this->get_name() + "_rclcpp_node"); - new_args.push_back("--"); - client_node_ = std::make_shared( - "_", "", rclcpp::NodeOptions().arguments(new_args)); + callback_group_ = create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + callback_group_executor_.add_callback_group(callback_group_, get_node_base_interface()); nav_to_pose_client_ = rclcpp_action::create_client( - client_node_, "navigate_to_pose"); + get_node_base_interface(), + get_node_graph_interface(), + get_node_logging_interface(), + get_node_waitables_interface(), + "navigate_to_pose", callback_group_); action_server_ = std::make_unique( get_node_base_interface(), @@ -167,9 +168,9 @@ WaypointFollower::followWaypoints() // Check if asked to stop processing action if (action_server_->is_cancel_requested()) { auto cancel_future = nav_to_pose_client_->async_cancel_all_goals(); - rclcpp::spin_until_future_complete(client_node_, cancel_future); + callback_group_executor_.spin_until_future_complete(cancel_future); // for result callback processing - spin_some(client_node_); + callback_group_executor_.spin_some(); action_server_->terminate_all(); return; } @@ -267,7 +268,7 @@ WaypointFollower::followWaypoints() "Processing waypoint %i...", goal_index); } - rclcpp::spin_some(client_node_); + callback_group_executor_.spin_some(); r.sleep(); } }