diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md index adcd7f2f18..a59fce1736 100644 --- a/doc/parameters/param_list.md +++ b/doc/parameters/param_list.md @@ -166,7 +166,7 @@ When `plugins` parameter is not overridden, the following default plugins are lo | ``.marking | true | Whether source should mark in costmap | | ``.clearing | false | Whether source should raytrace clear in costmap | | ``.obstacle_range | 2.5 | Maximum range to mark obstacles in costmap | -| ``.raytrace_range | 3.0 | Maximum range to raytrace clear obstacles from costmap | +| ``.raytrace_range | 3.0 | Maximum range to raytrace clear obstacles from costmap | # controller_server @@ -276,7 +276,7 @@ When `controller_plugins`\`progress_checker_plugin`\`goal_checker_plugin` parame | ``.``.x_only_threshold | 0.05 | Threshold to check in the X velocity direction | | ``.``.scale | 1.0 | Weighed scale for critic | -## kinematic_parameters +## kinematic_parameters | Parameter | Default | Description | | ----------| --------| ------------| @@ -295,7 +295,7 @@ When `controller_plugins`\`progress_checker_plugin`\`goal_checker_plugin` parame | ``.decel_lim_y | 0.0 | Maximum deceleration Y (m/s^2) | | ``.decel_lim_theta | 0.0 | Maximum deceleration rotation (rad/s^2) | -## xy_theta_iterator +## xy_theta_iterator | Parameter | Default | Description | | ----------| --------| ------------| @@ -473,6 +473,46 @@ When `planner_plugins` parameter is not overridden, the following default plugin | ``.use_astar | false | Whether to use A*, if false, uses Dijstra's expansion | | ``.allow_unknown | true | Whether to allow planning in unknown space | +# smac_planner + +* ``: Corresponding planner plugin ID for this type + +| Parameter | Default | Description | +| ----------| --------| ------------| +| ``.tolerance | 0.5 | Tolerance in meters between requested goal pose and end of path | +| ``.downsample_costmap | false | Whether to downsample costmap | +| ``.downsampling_factor | 1 | Factor to downsample costmap by | +| ``.allow_unknown | true | whether to allow traversing in unknown space | +| ``.max_iterations | -1 | Number of iterations before failing, disabled by -1 | +| ``.max_on_approach_iterations | 1000 | Iterations after within threshold before returning approximate path with best heuristic | +| ``.max_planning_time_ms | 5000 | Maximum planning time in ms | +| ``.smooth_path | false | Whether to smooth path with CG smoother | +| ``.motion_model_for_search | DUBIN | Motion model to search with. Options for SE2: DUBIN, REEDS_SHEPP. 2D: MOORE, VON_NEUMANN | +| ``.angle_quantization_bins | 1 | Number of angle quantization bins for SE2 node | +| ``.minimum_turning_radius | 0.20 | Minimum turning radius in m of vehicle or desired path | +| ``.reverse_penalty | 2.0 | Penalty to apply to SE2 node if in reverse direction | +| ``.change_penalty | 0.5 | Penalty to apply to SE2 node if changing direction | +| ``.non_straight_penalty | 1.1 | Penalty to apply to SE2 node if non-straight direction | +| ``.cost_penalty | 1.2 | Penalty to apply to SE2 node for cost at pose | +| ``.analytic_expansion_ratio | 2.0 | For SE2 nodes the planner will attempt an analytic path expansion every N iterations, where N = closest_distance_to_goal / analytic_expansion_ratio. Higher ratios result in more frequent expansions | +| ``.smoother.smoother.w_curve | 1.5 | Smoother weight on curvature of path | +| ``.smoother.smoother.w_dist | 0.0 | Smoother weight on distance from original path | +| ``.smoother.smoother.w_smooth | 15000 | Smoother weight on distance between nodes | +| ``.smoother.smoother.w_cost | 1.5 | Smoother weight on costmap cost | +| ``.smoother.smoother.cost_scaling_factor | 10.0 | Inflation layer's scale factor | +| ``.smoother.optimizer.max_time | 0.10 | Maximum time to spend smoothing, in seconds | +| ``.smoother.optimizer.max_iterations | 500 | Maximum number of iterations to spend smoothing | +| ``.smoother.optimizer.debug_optimizer | false | Whether to print debug info from Ceres | +| ``.smoother.optimizer.gradient_tol | 1e-10 | Minimum change in gradient to terminate smoothing | +| ``.smoother.optimizer.fn_tol | 1e-7 | Minimum change in function to terminate smoothing | +| ``.smoother.optimizer.param_tol | 1e-15 | Minimum change in parameter block to terminate smoothing | + +| ``.smoother.optimizer.advanced.min_line_search_step_size | 1e-20 | Terminate smoothing iteration if change in parameter block less than this | +| ``.smoother.optimizer.advanced.max_num_line_search_step_size_iterations | 50 | Maximum iterations for line search | +| ``.smoother.optimizer.advanced.line_search_sufficient_function_decrease | 1e-20 | Function decrease amount to terminate current line search iteration | +| ``.smoother.optimizer.advanced.max_num_line_search_direction_restarts | 10 | Maximum umber of restarts of line search when over-estimating | +| ``.smoother.optimizer.advanced.max_line_search_step_expansion | 50 | Step size multiplier at each iteration of line search | + # waypoint_follower | Parameter | Default | Description | diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml index 53ab496beb..26b8a54274 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml @@ -159,6 +159,7 @@ local_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 + inflation_radius: 0.55 obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml index a4a62d6b1a..b5d0700074 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml @@ -159,6 +159,7 @@ local_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 + inflation_radius: 0.55 obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index eb290d50d3..d0e1ad4c34 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -168,6 +168,7 @@ local_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 + inflation_radius: 0.55 voxel_layer: plugin: "nav2_costmap_2d::VoxelLayer" enabled: True @@ -204,6 +205,7 @@ global_costmap: use_sim_time: True robot_radius: 0.22 resolution: 0.05 + track_unknown_space: true plugins: ["static_layer", "obstacle_layer", "inflation_layer"] obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" @@ -220,6 +222,8 @@ global_costmap: map_subscribe_transient_local: True inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 always_send_full_costmap: True global_costmap_client: ros__parameters: diff --git a/nav2_bringup/bringup/rviz/nav2_default_view.rviz b/nav2_bringup/bringup/rviz/nav2_default_view.rviz index 04e989eb17..c07e3e0cba 100644 --- a/nav2_bringup/bringup/rviz/nav2_default_view.rviz +++ b/nav2_bringup/bringup/rviz/nav2_default_view.rviz @@ -249,6 +249,20 @@ Visualization Manager: Value: /global_costmap/costmap Use Timestamp: false Value: true + - Alpha: 0.3 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Downsampled Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /downsampled_costmap + Use Timestamp: false + Value: true - Alpha: 1 Buffer Length: 1 Class: rviz_default_plugins/Path diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp index 8f9fd43867..d74895b40a 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp @@ -125,6 +125,14 @@ class Costmap2D */ unsigned char getCost(unsigned int mx, unsigned int my) const; + /** + * @brief Get the cost of a cell in the costmap + * @param mx The x coordinate of the cell + * @param my The y coordinate of the cell + * @return The cost of the cell + */ + unsigned char getCost(unsigned int index) const; + /** * @brief Set the cost of a cell in the costmap * @param mx The x coordinate of the cell diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp index 541a246a51..124d1666a5 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp @@ -248,6 +248,14 @@ class Costmap2DROS : public nav2_util::LifecycleNode std::shared_ptr getTfBuffer() {return tf_buffer_;} + /** + * @brief Get the costmap's use_radius_ parameter, corresponding to + * whether the footprint for the robot is a circle with radius robot_radius_ + * or an arbitrarily defined footprint in footprint_. + * @return use_radius_ + */ + bool getUseRadius() {return use_radius_;} + protected: rclcpp::Node::SharedPtr client_node_; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp index 636a907f07..9f54728e63 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp @@ -68,7 +68,7 @@ class CostmapTopicCollisionChecker CostmapSubscriber & costmap_sub_; FootprintSubscriber & footprint_sub_; double transform_tolerance_; - FootprintCollisionChecker collision_checker_; + FootprintCollisionChecker> collision_checker_; }; } // namespace nav2_costmap_2d 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 24f35aa135..39c51328b8 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 @@ -32,20 +32,21 @@ namespace nav2_costmap_2d { typedef std::vector Footprint; +template class FootprintCollisionChecker { public: FootprintCollisionChecker(); - explicit FootprintCollisionChecker(std::shared_ptr costmap); + explicit FootprintCollisionChecker(CostmapT costmap); double footprintCost(const Footprint footprint); double footprintCostAtPose(double x, double y, double theta, const Footprint footprint); double lineCost(int x0, int x1, int y0, int y1) const; bool worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my); double pointCost(int x, int y) const; - void setCostmap(std::shared_ptr costmap); + void setCostmap(CostmapT costmap); -private: - std::shared_ptr costmap_; +protected: + CostmapT costmap_; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp index e5fc947292..976d09ce26 100644 --- a/nav2_costmap_2d/src/costmap_2d.cpp +++ b/nav2_costmap_2d/src/costmap_2d.cpp @@ -212,6 +212,11 @@ unsigned char Costmap2D::getCost(unsigned int mx, unsigned int my) const return costmap_[getIndex(mx, my)]; } +unsigned char Costmap2D::getCost(unsigned int undex) const +{ + return costmap_[undex]; +} + void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost) { costmap_[getIndex(mx, my)] = cost; diff --git a/nav2_costmap_2d/src/footprint_collision_checker.cpp b/nav2_costmap_2d/src/footprint_collision_checker.cpp index 15adcd8665..20d60b3442 100644 --- a/nav2_costmap_2d/src/footprint_collision_checker.cpp +++ b/nav2_costmap_2d/src/footprint_collision_checker.cpp @@ -31,18 +31,21 @@ using namespace std::chrono_literals; namespace nav2_costmap_2d { -FootprintCollisionChecker::FootprintCollisionChecker() +template +FootprintCollisionChecker::FootprintCollisionChecker() : costmap_(nullptr) { } -FootprintCollisionChecker::FootprintCollisionChecker( - std::shared_ptr costmap) +template +FootprintCollisionChecker::FootprintCollisionChecker( + CostmapT costmap) : costmap_(costmap) { } -double FootprintCollisionChecker::footprintCost(const Footprint footprint) +template +double FootprintCollisionChecker::footprintCost(const Footprint footprint) { // now we really have to lay down the footprint in the costmap_ grid unsigned int x0, x1, y0, y1; @@ -80,7 +83,8 @@ double FootprintCollisionChecker::footprintCost(const Footprint footprint) return footprint_cost; } -double FootprintCollisionChecker::lineCost(int x0, int x1, int y0, int y1) const +template +double FootprintCollisionChecker::lineCost(int x0, int x1, int y0, int y1) const { double line_cost = 0.0; double point_cost = -1.0; @@ -96,23 +100,27 @@ double FootprintCollisionChecker::lineCost(int x0, int x1, int y0, int y1) const return line_cost; } -bool FootprintCollisionChecker::worldToMap( +template +bool FootprintCollisionChecker::worldToMap( double wx, double wy, unsigned int & mx, unsigned int & my) { return costmap_->worldToMap(wx, wy, mx, my); } -double FootprintCollisionChecker::pointCost(int x, int y) const +template +double FootprintCollisionChecker::pointCost(int x, int y) const { return costmap_->getCost(x, y); } -void FootprintCollisionChecker::setCostmap(std::shared_ptr costmap) +template +void FootprintCollisionChecker::setCostmap(CostmapT costmap) { costmap_ = costmap; } -double FootprintCollisionChecker::footprintCostAtPose( +template +double FootprintCollisionChecker::footprintCostAtPose( double x, double y, double theta, const Footprint footprint) { double cos_th = cos(theta); @@ -128,4 +136,8 @@ double FootprintCollisionChecker::footprintCostAtPose( return footprintCost(oriented_footprint); } +// declare our valid template parameters +template class FootprintCollisionChecker>; +template class FootprintCollisionChecker; + } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp b/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp index f0e9c30623..1be0d1282d 100644 --- a/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp +++ b/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp @@ -39,7 +39,8 @@ TEST(collision_footprint, test_basic) nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; - nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_); + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap_); auto value = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint); @@ -51,7 +52,8 @@ TEST(collision_footprint, test_point_cost) std::shared_ptr costmap_ = std::make_shared(100, 100, 0.1, 0, 0, 0); - nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_); + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap_); auto value = collision_checker.pointCost(50, 50); @@ -63,7 +65,8 @@ TEST(collision_footprint, test_world_to_map) std::shared_ptr costmap_ = std::make_shared(100, 100, 0.1, 0, 0, 0); - nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_); + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap_); unsigned int x, y; @@ -105,7 +108,8 @@ TEST(collision_footprint, test_footprint_at_pose_with_movement) nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; - nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_); + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap_); auto value = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint); EXPECT_NEAR(value, 0.0, 0.001); @@ -140,7 +144,8 @@ TEST(collision_footprint, test_point_and_line_cost) nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; - nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_); + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap_); auto value = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint); EXPECT_NEAR(value, 0.0, 0.001); diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index a873a3e5d1..0393fbdaaf 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -19,6 +19,8 @@ // the Global Dynamic Window Approach. IEEE. // https://cs.stanford.edu/group/manips/publications/pdfs/Brock_1999_ICRA.pdf +// #define BENCHMARK_TESTING + #include "nav2_navfn_planner/navfn_planner.hpp" #include @@ -114,6 +116,10 @@ nav_msgs::msg::Path NavfnPlanner::createPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) { +#ifdef BENCHMARK_TESTING + steady_clock::time_point a = steady_clock::now(); +#endif + // Update planner based on the new costmap size if (isPlannerOutOfDate()) { planner_->setNavArr( @@ -128,6 +134,14 @@ nav_msgs::msg::Path NavfnPlanner::createPlan( node_->get_logger(), "%s: failed to create plan with " "tolerance %.2f.", name_.c_str(), tolerance_); } + +#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 path; } diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index ee3a07e473..b6b4615bb0 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -46,7 +46,7 @@ PlannerServer::PlannerServer() // Declare this node's parameters declare_parameter("planner_plugins", default_ids_); - declare_parameter("expected_planner_frequency", 20.0); + declare_parameter("expected_planner_frequency", 1.0); // Setup the global costmap costmap_ros_ = std::make_shared( diff --git a/navigation2/package.xml b/navigation2/package.xml index bbf19b3395..bd72a39fc5 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -30,6 +30,7 @@ nav2_voxel_grid nav2_controller nav2_waypoint_follower + smac_planner ament_cmake diff --git a/smac_planner/CMakeLists.txt b/smac_planner/CMakeLists.txt new file mode 100644 index 0000000000..d161ba516e --- /dev/null +++ b/smac_planner/CMakeLists.txt @@ -0,0 +1,123 @@ +cmake_minimum_required(VERSION 3.5) +project(smac_planner) + +set(CMAKE_BUILD_TYPE Release) #Debug, Release + +find_package(ament_cmake REQUIRED) +find_package(nav2_common REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(std_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(nav2_util REQUIRED) +find_package(nav2_core REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +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) +find_package(OpenMP REQUIRED) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") + +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}") + +set(library_name smac_planner) + +set(dependencies + rclcpp + rclcpp_action + rclcpp_lifecycle + std_msgs + visualization_msgs + nav2_util + nav2_msgs + nav_msgs + geometry_msgs + builtin_interfaces + tf2_ros + nav2_costmap_2d + nav2_core + pluginlib + eigen3_cmake_module +) + +# SE2 plugin +add_library(${library_name} SHARED + src/smac_planner.cpp + src/a_star.cpp + src/node_se2.cpp + src/costmap_downsampler.cpp + src/node_2d.cpp +) + +target_link_libraries(${library_name} ${CERES_LIBRARIES} ${OMPL_LIBRARIES} ${OpenMP_LIBRARIES} OpenMP::OpenMP_CXX) +target_include_directories(${library_name} PUBLIC ${Eigen3_INCLUDE_DIRS}) + +ament_target_dependencies(${library_name} + ${dependencies} +) + +# 2D plugin +add_library(${library_name}_2d SHARED + src/smac_planner_2d.cpp + src/a_star.cpp + src/node_se2.cpp + src/costmap_downsampler.cpp + src/node_2d.cpp +) + +target_link_libraries(${library_name}_2d ${CERES_LIBRARIES} ${OMPL_LIBRARIES}) +target_include_directories(${library_name}_2d PUBLIC ${Eigen3_INCLUDE_DIRS}) + +ament_target_dependencies(${library_name}_2d + ${dependencies} +) + +target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +target_compile_definitions(${library_name}_2d PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +pluginlib_export_plugin_description_file(nav2_core smac_plugin.xml) +pluginlib_export_plugin_description_file(nav2_core smac_plugin_2d.xml) + +install(TARGETS ${library_name} ${library_name}_2d + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include/ +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(test) +endif() + +ament_export_include_directories(include) +ament_export_libraries(${library_name} ${library_name}_2d) +ament_export_dependencies(${dependencies}) +ament_package() diff --git a/smac_planner/README.md b/smac_planner/README.md new file mode 100644 index 0000000000..5bf0cd4217 --- /dev/null +++ b/smac_planner/README.md @@ -0,0 +1,173 @@ +# 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. +- `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. +- `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. + +We have users reporting using this on: +- Delivery robots +- Industrial robots + +## Introduction + +The `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 `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. + +In summary... + +The `SmacPlanner` 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 + +The `SmacPlanner2D` is designed to work with: +- Circular, differential or omnidirectional robots +- Relatively small robots with respect to environment size (e.g. RC car in a hallway or large robot in a convention center) that can be approximated by circular footprint. + +## Features + +We further improve on the Hybrid-A\* work 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. +- Faster planning than original paper by highly optimizing the template A\* algorithm. +- 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. +- Templated Nodes and A\* implementation to support additional robot extensions. + +All of these features (multi-resolution, models, smoother, etc) are also available in the 2D `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 Navigation2 - will not have any issue with grid-based plans. + +## 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: + +- **2-20ms** for planning across 147,456 (1.4x larger) cell maps with 72 angular bins. +- **30-120ms** for planning across 344,128 (3.3x larger) cell map with 72 angular bins. + +For example, the following path (roughly 85 meters) path took 33ms to compute. + +![alt text](test/path.png) + +## Design + +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. + +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. + +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. + +``` +planner_server: + ros__parameters: + planner_plugins: ["GridBased"] + use_sim_time: True + + GridBased: + plugin: "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_ms: 2000.0 # max time in ms 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 + + 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 +``` + +## Topics + +| Topic | Type | +|-----------------|-------------------| +| unsmoothed_path | nav_msgs/Path | + + +## Install + +``` +sudo apt-get install ros--smac-planner +``` + +## Etc (Important Side Notes) + +### Potential Fields + +Many users and default navigation configuration files I find are really missing the point of the inflation layer. While it's true that you can simply inflate a small radius around the walls, the _true_ value of the inflation layer is creating a consistent potential field around the entire map. + +Some of the most popular tuning guides for Navigation / Navigation2 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. + +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 + +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. + +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. + +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`. + +### 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. + +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. diff --git a/smac_planner/include/smac_planner/a_star.hpp b/smac_planner/include/smac_planner/a_star.hpp new file mode 100644 index 0000000000..a53b872a91 --- /dev/null +++ b/smac_planner/include/smac_planner/a_star.hpp @@ -0,0 +1,321 @@ +// 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. + +#ifndef SMAC_PLANNER__A_STAR_HPP_ +#define SMAC_PLANNER__A_STAR_HPP_ + +#include +#include +#include +#include +#include +#include +#include "Eigen/Core" + +#include "nav2_costmap_2d/costmap_2d.hpp" + +#include "smac_planner/node_2d.hpp" +#include "smac_planner/node_se2.hpp" +#include "smac_planner/node_basic.hpp" +#include "smac_planner/types.hpp" +#include "smac_planner/constants.hpp" + +namespace 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 smac_planner::AStarAlgorithm + * @brief An A* implementation for planning in a costmap. Templated based on the Node type. + */ +template +class AStarAlgorithm +{ +public: + typedef NodeT * NodePtr; + typedef std::unordered_map Graph; + typedef std::vector NodeVector; + typedef std::pair> NodeElement; + typedef typename NodeT::Coordinates Coordinates; + typedef typename NodeT::CoordinateVector CoordinateVector; + typedef typename NodeVector::iterator NeighborIterator; + typedef std::function NodeGetter; + + /** + * @struct smac_planner::NodeComparator + * @brief Node comparison for priority queue sorting + */ + struct NodeComparator + { + bool operator()(const NodeElement & a, const NodeElement & b) const + { + return a.first > b.first; + } + }; + + typedef std::priority_queue, NodeComparator> NodeQueue; + + /** + * @brief A constructor for smac_planner::PlannerServer + * @param neighborhood The type of neighborhood to use for search (4 or 8 connected) + */ + explicit AStarAlgorithm(const MotionModel & motion_model, const SearchInfo & search_info); + + /** + * @brief A destructor for smac_planner::AStarAlgorithm + */ + ~AStarAlgorithm(); + + /** + * @brief Initialization of the planner with defaults + * @param allow_unknown Allow search in unknown space, good for navigation while mapping + * @param max_iterations Maximum number of iterations to use while expanding search + * @param max_on_approach_iterations Maximum number of iterations before returning a valid + * path once within thresholds to refine path + * comes at more compute time but smoother paths. + */ + void initialize( + const bool & allow_unknown, + int & max_iterations, + const int & max_on_approach_iterations); + + /** + * @brief Creating path from given costmap, start, and goal + * @param path Reference to a vector of indicies of generated path + * @param num_iterations Reference to number of iterations to create plan + * @param tolerance Reference to tolerance in costmap nodes + * @return if plan was successful + */ + 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 + */ + void createGraph( + const unsigned int & x, + const unsigned int & y, + const unsigned int & dim_3, + nav2_costmap_2d::Costmap2D * & costmap); + + /** + * @brief Set the goal for planning, as a node index + * @param mx The node X index of the goal + * @param my The node Y index of the goal + * @param dim_3 The node dim_3 index of the goal + */ + void setGoal( + const unsigned int & mx, + const unsigned int & my, + const unsigned int & dim_3); + + /** + * @brief Set the starting pose for planning, as a node index + * @param mx The node X index of the goal + * @param my The node Y index of the goal + * @param dim_3 The node dim_3 index of the goal + */ + void setStart( + const unsigned int & mx, + 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 + */ + int & getMaxIterations(); + + /** + * @brief Get pointer reference to starting node + * @return Node pointer reference to starting node + */ + NodePtr & getStart(); + + /** + * @brief Get pointer reference to goal node + * @return Node pointer reference to goal node + */ + NodePtr & getGoal(); + + /** + * @brief Get maximum number of on-approach iterations after within threshold + * @return Reference to Maximum on-appraoch iterations parameter + */ + int & getOnApproachMaxIterations(); + + /** + * @brief Get tolerance, in node nodes + * @return Reference to tolerance parameter + */ + float & getToleranceHeuristic(); + + /** + * @brief Get size of graph in X + * @return Size in X + */ + unsigned int & getSizeX(); + + /** + * @brief Get size of graph in Y + * @return Size in Y + */ + unsigned int & getSizeY(); + + /** + * @brief Get number of angle quantization bins (SE2) or Z coordinate (XYZ) + * @return Number of angle bins / Z dimension + */ + unsigned int & getSizeDim3(); + +protected: + /** + * @brief Get pointer to next goal in open set + * @return Node pointer reference to next heuristically scored node + */ + inline NodePtr getNextNode(); + + /** + * @brief Get pointer to next goal in open set + * @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); + + /** + * @brief Adds node to graph + * @param cost The cost to sort into the open set of the node + * @param node Node pointer reference to add to open set + */ + inline NodePtr addToGraph(const unsigned int & index); + + /** + * @brief Check if this node is the goal node + * @param node Node pointer to check if its the goal node + * @return if node is goal + */ + 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 + */ + inline float & getAccumulatedCost(NodePtr & node); + + /** + * @brief Get cost of heuristic of node + * @param node Node index current + * @param node Node index of new + * @return Heuristic cost between the nodes + */ + inline float getHeuristicCost(const NodePtr & node); + + /** + * @brief Check if inputs to planner are valid + * @return Are valid + */ + inline bool areInputsValid(); + + /** + * @brief Clear hueristic queue of nodes to search + */ + inline void clearQueue(); + + /** + * @brief Clear graph of nodes searched + */ + inline void clearGraph(); + + /** + * @brief Attempt an analytic path completion + * @return Node pointer reference to goal node if successful, else + * return nullptr + */ + inline NodePtr tryAnalyticExpansion( + const NodePtr & current_node, + const NodeGetter & getter, int & iterations, int & best_cost); + + bool _traverse_unknown; + int _max_iterations; + int _max_on_approach_iterations; + float _tolerance; + unsigned int _x_size; + unsigned int _y_size; + unsigned int _dim3_size; + SearchInfo _search_info; + + Coordinates _goal_coordinates; + NodePtr _start; + NodePtr _goal; + + Graph _graph; + NodeQueue _queue; + + MotionModel _motion_model; + NodeHeuristicPair _best_heuristic_node; + + GridCollisionChecker _collision_checker; + nav2_costmap_2d::Footprint _footprint; + bool _is_radius_footprint; + nav2_costmap_2d::Costmap2D * _costmap; +}; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__A_STAR_HPP_ diff --git a/smac_planner/include/smac_planner/collision_checker.hpp b/smac_planner/include/smac_planner/collision_checker.hpp new file mode 100644 index 0000000000..588514901f --- /dev/null +++ b/smac_planner/include/smac_planner/collision_checker.hpp @@ -0,0 +1,113 @@ +// 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 "nav2_costmap_2d/footprint_collision_checker.hpp" +#include "smac_planner/constants.hpp" + +#ifndef SMAC_PLANNER__COLLISION_CHECKER_HPP_ +#define SMAC_PLANNER__COLLISION_CHECKER_HPP_ + +namespace smac_planner +{ + +/** + * @class smac_planner::GridCollisionChecker + * @brief A costmap grid collision checker + */ +class GridCollisionChecker + : public nav2_costmap_2d::FootprintCollisionChecker +{ +public: + /** + * @brief A constructor for smac_planner::GridCollisionChecker + * @param costmap The costmap to collision check against + */ + GridCollisionChecker( + nav2_costmap_2d::Costmap2D * costmap) + : FootprintCollisionChecker(costmap) + { + } + + /** + * @brief Set the footprint to use with collision checker + * @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) + { + unoriented_footprint_ = footprint; + footprint_is_radius_ = radius; + } + + /** + * @brief Check if in collision with costmap and footprint at pose + * @param x X coordinate of pose to check against + * @param y Y coordinate of pose to check against + * @param theta Angle of pose to check against + * @param traverse_unknown Whether or not to traverse in unknown space + * @return boolean if in collision or not. + */ + bool inCollision( + const float & x, + const float & y, + const float & theta, + const bool & traverse_unknown) + { + // Assumes setFootprint already set + double wx, wy; + costmap_->mapToWorld(static_cast(x), static_cast(y), wx, wy); + + if (!footprint_is_radius_) { + // if footprint, then we check for the footprint's points + footprint_cost_ = footprintCostAtPose( + wx, wy, static_cast(theta), unoriented_footprint_); + if (footprint_cost_ == UNKNOWN && traverse_unknown) { + return false; + } + + // if occupied or unknown and not to traverse unknown space + return footprint_cost_ >= OCCUPIED; + } else { + // if radius, then we can check the center of the cost assuming inflation is used + footprint_cost_ = costmap_->getCost( + static_cast(x), static_cast(y)); + + 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 + */ + float getCost() + { + // Assumes inCollision called prior + return static_cast(footprint_cost_); + } + +protected: + nav2_costmap_2d::Footprint unoriented_footprint_; + double footprint_cost_; + bool footprint_is_radius_; +}; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__COLLISION_CHECKER_HPP_ diff --git a/smac_planner/include/smac_planner/constants.hpp b/smac_planner/include/smac_planner/constants.hpp new file mode 100644 index 0000000000..bc2ccfd8c6 --- /dev/null +++ b/smac_planner/include/smac_planner/constants.hpp @@ -0,0 +1,70 @@ +// 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 SMAC_PLANNER__CONSTANTS_HPP_ +#define SMAC_PLANNER__CONSTANTS_HPP_ + +#include + +namespace smac_planner +{ +enum class MotionModel +{ + UNKNOWN = 0, + VON_NEUMANN = 1, + MOORE = 2, + DUBIN = 3, + REEDS_SHEPP = 4, +}; + +inline std::string toString(const MotionModel & n) +{ + switch (n) { + case MotionModel::VON_NEUMANN: + return "Von Neumann"; + case MotionModel::MOORE: + return "Moore"; + case MotionModel::DUBIN: + return "Dubin"; + case MotionModel::REEDS_SHEPP: + return "Reeds-Shepp"; + default: + return "Unknown"; + } +} + +inline MotionModel fromString(const std::string & n) +{ + if (n == "VON_NEUMANN") { + return MotionModel::VON_NEUMANN; + } else if (n == "MOORE") { + return MotionModel::MOORE; + } else if (n == "DUBIN") { + return MotionModel::DUBIN; + } else if (n == "REEDS_SHEPP") { + return MotionModel::REEDS_SHEPP; + } else { + return MotionModel::UNKNOWN; + } +} + +const float UNKNOWN = 255; +const float OCCUPIED = 254; +const float INSCRIBED = 253; +const float MAX_NON_OBSTACLE = 252; +const float FREE = 0; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__CONSTANTS_HPP_ diff --git a/smac_planner/include/smac_planner/costmap_downsampler.hpp b/smac_planner/include/smac_planner/costmap_downsampler.hpp new file mode 100644 index 0000000000..6f23c696e2 --- /dev/null +++ b/smac_planner/include/smac_planner/costmap_downsampler.hpp @@ -0,0 +1,117 @@ +// Copyright (c) 2020, Carlos Luis +// +// 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 SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_ +#define SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_ + +#include +#include +#include + +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "smac_planner/constants.hpp" + +namespace smac_planner +{ + +/** + * @class smac_planner::CostmapDownsampler + * @brief A costmap downsampler for more efficient path planning + */ +class CostmapDownsampler +{ +public: + /** + * @brief A constructor for CostmapDownsampler + * @param node Lifecycle node pointer + */ + explicit CostmapDownsampler(const nav2_util::LifecycleNode::SharedPtr & node); + + /** + * @brief A destructor for CostmapDownsampler + */ + ~CostmapDownsampler(); + + /** + * @brief Initialize the downsampled costmap object and the ROS publisher + * @param global_frame The ID of the global frame used by the costmap + * @param topic_name The name of the topic to publish the downsampled costmap + * @param costmap The costmap we want to downsample + * @param downsampling_factor Multiplier for the costmap resolution + */ + void initialize( + const std::string & global_frame, + const std::string & topic_name, + nav2_costmap_2d::Costmap2D * const costmap, + const unsigned int & downsampling_factor); + + /** + * @brief Activate the publisher of the downsampled costmap + */ + void activatePublisher() + { + _downsampled_costmap_pub->on_activate(); + } + + /** + * @brief Deactivate the publisher of the downsampled costmap + */ + void deactivatePublisher() + { + _downsampled_costmap_pub->on_deactivate(); + } + + /** + * @brief Downsample the given costmap by the downsampling factor, and publish the downsampled costmap + * @param downsampling_factor Multiplier for the costmap resolution + * @return A ptr to the downsampled costmap + */ + nav2_costmap_2d::Costmap2D * downsample(const unsigned int & downsampling_factor); + + /** + * @brief Resize the downsampled costmap. Used in case the costmap changes and we need to update the downsampled version + */ + void resizeCostmap(); + +protected: + /** + * @brief Update the sizes X-Y of the costmap and its downsampled version + */ + void updateCostmapSize(); + + /** + * @brief Explore all subcells of the original costmap and assign the max cost to the new (downsampled) cell + * @param new_mx The X-coordinate of the cell in the new costmap + * @param new_my The Y-coordinate of the cell in the new costmap + */ + void setCostOfCell( + const unsigned int & new_mx, + const unsigned int & new_my); + + unsigned int _size_x; + unsigned int _size_y; + unsigned int _downsampled_size_x; + unsigned int _downsampled_size_y; + unsigned int _downsampling_factor; + float _downsampled_resolution; + std::string _topic_name; + nav2_util::LifecycleNode::SharedPtr _node; + nav2_costmap_2d::Costmap2D * _costmap; + std::unique_ptr _downsampled_costmap; + std::unique_ptr _downsampled_costmap_pub; +}; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_ diff --git a/smac_planner/include/smac_planner/node_2d.hpp b/smac_planner/include/smac_planner/node_2d.hpp new file mode 100644 index 0000000000..b80a316cbb --- /dev/null +++ b/smac_planner/include/smac_planner/node_2d.hpp @@ -0,0 +1,248 @@ +// 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 SMAC_PLANNER__NODE_2D_HPP_ +#define SMAC_PLANNER__NODE_2D_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "smac_planner/constants.hpp" +#include "smac_planner/collision_checker.hpp" + +namespace smac_planner +{ + +/** + * @class smac_planner::Node2D + * @brief Node2D implementation for graph + */ +class Node2D +{ +public: + typedef Node2D * NodePtr; + typedef std::unique_ptr> Graph; + typedef std::vector NodeVector; + + /** + * @class smac_planner::Node2D::Coordinates + * @brief Node2D implementation of coordinate structure + */ + struct Coordinates + { + Coordinates() {} + Coordinates(const float & x_in, const float & y_in) + : x(x_in), y(y_in) + {} + + float x, y; + }; + typedef std::vector CoordinateVector; + + /** + * @brief A constructor for 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); + + /** + * @brief A destructor for smac_planner::Node2D + */ + ~Node2D(); + + /** + * @brief operator== for comparisons + * @param Node2D right hand side node reference + * @return If cell indicies are equal + */ + bool operator==(const Node2D & rhs) + { + return this->_index == rhs._index; + } + + /** + * @brief Reset method for new search + * @param cost_in The costmap cost at this node + */ + void reset(const unsigned char & cost); + /** + * @brief Gets the accumulated cost at this node + * @return accumulated cost + */ + inline float & getAccumulatedCost() + { + return _accumulated_cost; + } + + /** + * @brief Sets the accumulated cost at this node + * @param reference to accumulated cost + */ + inline void setAccumulatedCost(const float cost_in) + { + _accumulated_cost = cost_in; + } + + /** + * @brief Gets the costmap cost at this node + * @return costmap cost + */ + inline float & getCost() + { + return _cell_cost; + } + + /** + * @brief Gets if cell has been visited in search + * @param If cell was visited + */ + inline bool & wasVisited() + { + return _was_visited; + } + + /** + * @brief Sets if cell has been visited in search + */ + 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; + } + + /** + * @brief Gets cell index + * @return Reference to cell index + */ + inline unsigned int & getIndex() + { + return _index; + } + + /** + * @brief Check if this node is valid + * @param traverse_unknown If we can explore unknown nodes on the graph + * @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); + + /** + * @brief get traversal cost from this node to child node + * @param child Node pointer to this node's child + * @return traversal cost + */ + float getTraversalCost(const NodePtr & child); + + /** + * @brief Get index + * @param x x coordinate of point to get index of + * @param y y coordinate of point to get index of + * @param width width of costmap + * @return index + */ + static inline unsigned int getIndex( + const unsigned int & x, const unsigned int & y, const unsigned int & width) + { + return x + y * width; + } + + /** + * @brief Get index + * @param Index Index of point + * @param width width of costmap + * @param angles angle bins to use (must be 1 or throws exception) + * @return coordinates of point + */ + static inline Coordinates getCoords( + const unsigned int & index, const unsigned int & width, const unsigned int & angles) + { + if (angles != 1) { + throw std::runtime_error("Node type Node2D does not have a valid angle quantization."); + } + + return Coordinates(index % width, index / width); + } + + /** + * @brief Get cost of heuristic of node + * @param node Node index current + * @param node Node index of new + * @return Heuristic cost between the nodes + */ + static float getHeuristicCost( + const Coordinates & node_coords, + const Coordinates & goal_coordinates); + + /** + * @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 + */ + static void initNeighborhood( + const unsigned int & x_size_uint, + const MotionModel & neighborhood); + /** + * @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 neighbors Vector of neighbors to be filled + */ + static void getNeighbors( + NodePtr & node, + std::function & validity_checker, + GridCollisionChecker collision_checker, + const bool & traverse_unknown, + NodeVector & neighbors); + + Node2D * parent; + static double neutral_cost; + static std::vector _neighbors_grid_offsets; + +private: + float _cell_cost; + float _accumulated_cost; + unsigned int _index; + bool _was_visited; + bool _is_queued; +}; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__NODE_2D_HPP_ diff --git a/smac_planner/include/smac_planner/node_basic.hpp b/smac_planner/include/smac_planner/node_basic.hpp new file mode 100644 index 0000000000..3f3941717d --- /dev/null +++ b/smac_planner/include/smac_planner/node_basic.hpp @@ -0,0 +1,68 @@ +// 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 SMAC_PLANNER__NODE_BASIC_HPP_ +#define SMAC_PLANNER__NODE_BASIC_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ompl/base/StateSpace.h" + +#include "smac_planner/constants.hpp" +#include "smac_planner/node_se2.hpp" +#include "smac_planner/node_2d.hpp" +#include "smac_planner/types.hpp" +#include "smac_planner/collision_checker.hpp" + +namespace smac_planner +{ + +/** + * @class smac_planner::NodeBasic + * @brief NodeBasic implementation for priority queue insertion + */ +template +class NodeBasic +{ +public: + /** + * @brief A constructor for smac_planner::NodeBasic + * @param cost_in The costmap cost at this node + * @param index The index of this node for self-reference + */ + explicit NodeBasic(const unsigned int index) + : index(index), + graph_node_ptr(nullptr) + { + } + + typename NodeT::Coordinates pose; // Used by NodeSE2 + NodeT * graph_node_ptr; + unsigned int index; +}; + +template class NodeBasic; +template class NodeBasic; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__NODE_BASIC_HPP_ diff --git a/smac_planner/include/smac_planner/node_se2.hpp b/smac_planner/include/smac_planner/node_se2.hpp new file mode 100644 index 0000000000..4fb1ae579d --- /dev/null +++ b/smac_planner/include/smac_planner/node_se2.hpp @@ -0,0 +1,423 @@ +// 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 SMAC_PLANNER__NODE_SE2_HPP_ +#define SMAC_PLANNER__NODE_SE2_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ompl/base/StateSpace.h" + +#include "smac_planner/constants.hpp" +#include "smac_planner/types.hpp" +#include "smac_planner/collision_checker.hpp" + +namespace smac_planner +{ + +// Need seperate pose struct for motion table operations + +/** + * @struct smac_planner::MotionPose + * @brief A struct for poses in motion primitives + */ +struct MotionPose +{ + /** + * @brief A constructor for smac_planner::MotionPose + */ + MotionPose() {} + + /** + * @brief A constructor for smac_planner::MotionPose + * @param x X pose + * @param y Y pose + * @param theta Angle of pose + */ + MotionPose(const float & x, const float & y, const float & theta) + : _x(x), _y(y), _theta(theta) + {} + + float _x; + float _y; + float _theta; +}; + +typedef std::vector MotionPoses; + +// Must forward declare +class NodeSE2; + +/** + * @struct smac_planner::MotionTable + * @brief A table of motion primitives and related functions + */ +struct MotionTable +{ + /** + * @brief A constructor for smac_planner::MotionTable + */ + MotionTable() {} + + /** + * @brief Initializing using Dubin model + * @param size_x_in Size of costmap in X + * @param size_y_in Size of costmap in Y + * @param angle_quantization_in Size of costmap in bin sizes + * @param search_info Parameters for searching + */ + void initDubin( + unsigned int & size_x_in, + unsigned int & size_y_in, + unsigned int & angle_quantization_in, + SearchInfo & search_info); + + /** + * @brief Initializing using Reeds-Shepp model + * @param size_x_in Size of costmap in X + * @param size_y_in Size of costmap in Y + * @param angle_quantization_in Size of costmap in bin sizes + * @param search_info Parameters for searching + */ + void initReedsShepp( + unsigned int & size_x_in, + unsigned int & size_y_in, + unsigned int & angle_quantization_in, + SearchInfo & search_info); + + /** + * @brief Get projections of motion models + * @param node Ptr to SE2 node + * @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 projections; + unsigned int size_x; + unsigned int num_angle_quantization; + float num_angle_quantization_float; + float bin_size; + float change_penalty; + float non_straight_penalty; + float cost_penalty; + float reverse_penalty; + ompl::base::StateSpacePtr state_space; +}; + +/** + * @class smac_planner::NodeSE2 + * @brief NodeSE2 implementation for graph + */ +class NodeSE2 +{ +public: + typedef NodeSE2 * NodePtr; + typedef std::unique_ptr> Graph; + typedef std::vector NodeVector; + /** + * @class smac_planner::NodeSE2::Coordinates + * @brief NodeSE2 implementation of coordinate structure + */ + struct Coordinates + { + /** + * @brief A constructor for smac_planner::NodeSE2::Coordinates + */ + Coordinates() {} + + /** + * @brief A constructor for smac_planner::NodeSE2::Coordinates + * @param x_in X coordinate + * @param y_in Y coordinate + * @param theta_in Theta coordinate + */ + Coordinates(const float & x_in, const float & y_in, const float & theta_in) + : x(x_in), y(y_in), theta(theta_in) + {} + + float x, y, theta; + }; + + typedef std::vector CoordinateVector; + + /** + * @brief A constructor for smac_planner::NodeSE2 + * @param index The index of this node for self-reference + */ + explicit NodeSE2(const unsigned int index); + + /** + * @brief A destructor for smac_planner::NodeSE2 + */ + ~NodeSE2(); + + /** + * @brief operator== for comparisons + * @param NodeSE2 right hand side node reference + * @return If cell indicies are equal + */ + bool operator==(const NodeSE2 & rhs) + { + return this->_index == rhs._index; + } + + /** + * @brief setting continuous coordinate search poses (in partial-cells) + * @param Pose pose + */ + inline void setPose(const Coordinates & pose_in) + { + pose = pose_in; + } + + /** + * @brief Reset method for new search + */ + void reset(); + + /** + * @brief Gets the accumulated cost at this node + * @return accumulated cost + */ + inline float & getAccumulatedCost() + { + return _accumulated_cost; + } + + /** + * @brief Sets the accumulated cost at this node + * @param reference to accumulated cost + */ + inline void setAccumulatedCost(const float cost_in) + { + _accumulated_cost = cost_in; + } + + /** + * @brief Sets the motion primitive index used to achieve node in search + * @param reference to motion primitive idx + */ + inline void setMotionPrimitiveIndex(const unsigned int & idx) + { + _motion_primitive_index = idx; + } + + /** + * @brief Gets the motion primitive index used to achieve node in search + * @return reference to motion primitive idx + */ + inline unsigned int & getMotionPrimitiveIndex() + { + return _motion_primitive_index; + } + + /** + * @brief Gets the costmap cost at this node + * @return costmap cost + */ + inline float & getCost() + { + return _cell_cost; + } + + /** + * @brief Gets if cell has been visited in search + * @param If cell was visited + */ + inline bool & wasVisited() + { + return _was_visited; + } + + /** + * @brief Sets if cell has been visited in search + */ + 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; + } + + /** + * @brief Gets cell index + * @return Reference to cell index + */ + inline unsigned int & getIndex() + { + return _index; + } + + /** + * @brief Check if this node is valid + * @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); + + /** + * @brief Get traversal cost of parent node to child node + * @param child Node pointer to child + * @return traversal cost + */ + float getTraversalCost(const NodePtr & child); + + /** + * @brief Get index at coordinates + * @param x X coordinate of point + * @param y Y coordinate of point + * @param angle Theta coordinate of point + * @param width Width of costmap + * @param angle_quantization Number of theta bins + * @return Index + */ + 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) + { + return angle + x * angle_quantization + y * width * angle_quantization; + } + + /** + * @brief Get index at coordinates + * @param x X coordinate of point + * @param y Y coordinate of point + * @param angle Theta coordinate of point + * @return Index + */ + static inline unsigned int getIndex( + const unsigned int & x, const unsigned int & y, const unsigned int & angle) + { + return getIndex( + x, y, angle, motion_table.size_x, + motion_table.num_angle_quantization); + } + + /** + * @brief Get coordinates at index + * @param index Index of point + * @param width Width of costmap + * @param angle_quantization Theta size of costmap + * @return Coordinates + */ + static inline Coordinates getCoords( + const unsigned int & index, + const unsigned int & width, const unsigned int angle_quantization) + { + return Coordinates( + (index / angle_quantization) % width, // x + index / (angle_quantization * width), // y + index % angle_quantization); // theta + } + + /** + * @brief Get cost of heuristic of node + * @param node Node index current + * @param node Node index of new + * @return Heuristic cost between the nodes + */ + static float getHeuristicCost( + const Coordinates & node_coords, + const Coordinates & goal_coordinates); + + /** + * @brief Initialize motion models + * @param motion_model Motion model enum to use + * @param size_x Size of X of graph + * @param size_y Size of y of graph + * @param angle_quantization Size of theta bins of graph + * @param search_info Search info to use + */ + static void initMotionModel( + const MotionModel & motion_model, + unsigned int & size_x, + unsigned int & size_y, + unsigned int & angle_quantization, + 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 + */ + static void 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); + + /** + * @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 neighbors Vector of neighbors to be filled + */ + static void getNeighbors( + const NodePtr & node, + std::function & validity_checker, + GridCollisionChecker collision_checker, + const bool & traverse_unknown, + NodeVector & neighbors); + + NodeSE2 * parent; + Coordinates pose; + static double neutral_cost; + static MotionTable motion_table; + +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 smac_planner + +#endif // SMAC_PLANNER__NODE_SE2_HPP_ diff --git a/smac_planner/include/smac_planner/options.hpp b/smac_planner/include/smac_planner/options.hpp new file mode 100644 index 0000000000..01951d8950 --- /dev/null +++ b/smac_planner/include/smac_planner/options.hpp @@ -0,0 +1,207 @@ +// 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 SMAC_PLANNER__OPTIONS_HPP_ +#define SMAC_PLANNER__OPTIONS_HPP_ + +#include +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_util/node_utils.hpp" + +namespace smac_planner +{ + +/** + * @struct smac_planner::SmootherParams + * @brief Parameters for the smoother cost function + */ +struct SmootherParams +{ + /** + * @brief A constructor for smac_planner::SmootherParams + */ + SmootherParams() + { + } + + /** + * @brief Get params from ROS parameter + * @param node_ Ptr to node + * @param name Name of plugin + */ + void get(rclcpp_lifecycle::LifecycleNode * node, const std::string & name) + { + std::string local_name = name + std::string(".smoother.smoother."); + + // Smoother params + nav2_util::declare_parameter_if_not_declared( + node, local_name + "w_curve", rclcpp::ParameterValue(1.5)); + node->get_parameter(local_name + "w_curve", curvature_weight); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "w_cost", rclcpp::ParameterValue(0.0)); + node->get_parameter(local_name + "w_cost", costmap_weight); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "w_dist", rclcpp::ParameterValue(0.0)); + node->get_parameter(local_name + "w_dist", distance_weight); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "w_smooth", rclcpp::ParameterValue(15000.0)); + node->get_parameter(local_name + "w_smooth", smooth_weight); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "cost_scaling_factor", rclcpp::ParameterValue(10.0)); + node->get_parameter(local_name + "cost_scaling_factor", costmap_factor); + } + + double smooth_weight{0.0}; + double costmap_weight{0.0}; + double distance_weight{0.0}; + double curvature_weight{0.0}; + double max_curvature{0.0}; + double costmap_factor{0.0}; + double max_time; +}; + +/** + * @struct smac_planner::OptimizerParams + * @brief Parameters for the ceres optimizer + */ +struct OptimizerParams +{ + OptimizerParams() + : debug(false), + max_iterations(50), + max_time(1e4), + param_tol(1e-8), + fn_tol(1e-6), + gradient_tol(1e-10) + { + } + + /** + * @struct AdvancedParams + * @brief Advanced parameters for the ceres optimizer + */ + struct AdvancedParams + { + AdvancedParams() + : min_line_search_step_size(1e-9), + max_num_line_search_step_size_iterations(20), + line_search_sufficient_function_decrease(1e-4), + max_num_line_search_direction_restarts(20), + max_line_search_step_contraction(1e-3), + min_line_search_step_contraction(0.6), + line_search_sufficient_curvature_decrease(0.9), + max_line_search_step_expansion(10) + { + } + + /** + * @brief Get advanced params from ROS parameter + * @param node_ Ptr to node + * @param name Name of plugin + */ + void get(rclcpp_lifecycle::LifecycleNode * node, const std::string & name) + { + std::string local_name = name + std::string(".smoother.optimizer.advanced."); + + // Optimizer advanced params + nav2_util::declare_parameter_if_not_declared( + node, local_name + "min_line_search_step_size", + rclcpp::ParameterValue(1e-20)); + node->get_parameter( + local_name + "min_line_search_step_size", + min_line_search_step_size); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "max_num_line_search_step_size_iterations", + rclcpp::ParameterValue(50)); + node->get_parameter( + local_name + "max_num_line_search_step_size_iterations", + max_num_line_search_step_size_iterations); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "line_search_sufficient_function_decrease", + rclcpp::ParameterValue(1e-20)); + node->get_parameter( + local_name + "line_search_sufficient_function_decrease", + line_search_sufficient_function_decrease); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "max_num_line_search_direction_restarts", + rclcpp::ParameterValue(10)); + node->get_parameter( + local_name + "max_num_line_search_direction_restarts", + max_num_line_search_direction_restarts); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "max_line_search_step_expansion", + rclcpp::ParameterValue(50)); + node->get_parameter( + local_name + "max_line_search_step_expansion", + max_line_search_step_expansion); + } + + + double min_line_search_step_size; // Ceres default: 1e-9 + int max_num_line_search_step_size_iterations; // Ceres default: 20 + double line_search_sufficient_function_decrease; // Ceres default: 1e-4 + int max_num_line_search_direction_restarts; // Ceres default: 5 + + double max_line_search_step_contraction; // Ceres default: 1e-3 + double min_line_search_step_contraction; // Ceres default: 0.6 + double line_search_sufficient_curvature_decrease; // Ceres default: 0.9 + int max_line_search_step_expansion; // Ceres default: 10 + }; + + /** + * @brief Get params from ROS parameter + * @param node_ Ptr to node + * @param name Name of plugin + */ + void get(rclcpp_lifecycle::LifecycleNode * node, const std::string & name) + { + std::string local_name = name + std::string(".smoother.optimizer."); + + // Optimizer params + nav2_util::declare_parameter_if_not_declared( + node, local_name + "param_tol", rclcpp::ParameterValue(1e-15)); + node->get_parameter(local_name + "param_tol", param_tol); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "fn_tol", rclcpp::ParameterValue(1e-7)); + node->get_parameter(local_name + "fn_tol", fn_tol); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "gradient_tol", rclcpp::ParameterValue(1e-10)); + node->get_parameter(local_name + "gradient_tol", gradient_tol); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "max_iterations", rclcpp::ParameterValue(500)); + node->get_parameter(local_name + "max_iterations", max_iterations); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "max_time", rclcpp::ParameterValue(0.100)); + node->get_parameter(local_name + "max_time", max_time); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "debug_optimizer", rclcpp::ParameterValue(false)); + node->get_parameter(local_name + "debug_optimizer", debug); + + advanced.get(node, name); + } + + bool debug; + int max_iterations; // Ceres default: 50 + double max_time; // Ceres default: 1e4 + + double param_tol; // Ceres default: 1e-8 + double fn_tol; // Ceres default: 1e-6 + double gradient_tol; // Ceres default: 1e-10 + + AdvancedParams advanced; +}; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__OPTIONS_HPP_ diff --git a/smac_planner/include/smac_planner/smac_planner.hpp b/smac_planner/include/smac_planner/smac_planner.hpp new file mode 100644 index 0000000000..379f0c92a5 --- /dev/null +++ b/smac_planner/include/smac_planner/smac_planner.hpp @@ -0,0 +1,131 @@ +// 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 SMAC_PLANNER__SMAC_PLANNER_HPP_ +#define SMAC_PLANNER__SMAC_PLANNER_HPP_ + +#include +#include +#include + +#include "smac_planner/a_star.hpp" +#include "smac_planner/smoother.hpp" +#include "smac_planner/costmap_downsampler.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav2_core/global_planner.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/node_utils.hpp" +#include "tf2/utils.h" + +namespace smac_planner +{ + +class SmacPlanner : public nav2_core::GlobalPlanner +{ +public: + /** + * @brief constructor + */ + SmacPlanner(); + + /** + * @brief destructor + */ + ~SmacPlanner(); + + /** + * @brief Configuring plugin + * @param parent Lifecycle node pointer + * @param name Name of plugin map + * @param tf Shared ptr of TF2 buffer + * @param costmap_ros Costmap2DROS object + */ + void configure( + rclcpp_lifecycle::LifecycleNode::SharedPtr parent, + std::string name, std::shared_ptr tf, + std::shared_ptr costmap_ros) override; + + /** + * @brief Cleanup lifecycle node + */ + void cleanup() override; + + /** + * @brief Activate lifecycle node + */ + void activate() override; + + /** + * @brief Deactivate lifecycle node + */ + void deactivate() override; + + /** + * @brief Creating a plan from start and goal poses + * @param start Start pose + * @param goal Goal pose + * @return nav2_msgs::Path of the generated path + */ + nav_msgs::msg::Path createPlan( + 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 _smoother; + nav2_util::LifecycleNode::SharedPtr _node; + nav2_costmap_2d::Costmap2D * _costmap; + std::unique_ptr _costmap_downsampler; + std::string _global_frame, _name; + float _tolerance; + int _downsampling_factor; + unsigned int _angle_quantizations; + 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 smac_planner + +#endif // SMAC_PLANNER__SMAC_PLANNER_HPP_ diff --git a/smac_planner/include/smac_planner/smac_planner_2d.hpp b/smac_planner/include/smac_planner/smac_planner_2d.hpp new file mode 100644 index 0000000000..6d89887c3d --- /dev/null +++ b/smac_planner/include/smac_planner/smac_planner_2d.hpp @@ -0,0 +1,122 @@ +// 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 SMAC_PLANNER__SMAC_PLANNER_2D_HPP_ +#define SMAC_PLANNER__SMAC_PLANNER_2D_HPP_ + +#include +#include +#include + +#include "smac_planner/a_star.hpp" +#include "smac_planner/smoother.hpp" +#include "smac_planner/costmap_downsampler.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav2_core/global_planner.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/node_utils.hpp" +#include "tf2/utils.h" + +namespace smac_planner +{ + +class SmacPlanner2D : public nav2_core::GlobalPlanner +{ +public: + /** + * @brief constructor + */ + SmacPlanner2D(); + + /** + * @brief destructor + */ + ~SmacPlanner2D(); + + /** + * @brief Configuring plugin + * @param parent Lifecycle node pointer + * @param name Name of plugin map + * @param tf Shared ptr of TF2 buffer + * @param costmap_ros Costmap2DROS object + */ + void configure( + rclcpp_lifecycle::LifecycleNode::SharedPtr parent, + std::string name, std::shared_ptr tf, + std::shared_ptr costmap_ros) override; + + /** + * @brief Cleanup lifecycle node + */ + void cleanup() override; + + /** + * @brief Activate lifecycle node + */ + void activate() override; + + /** + * @brief Deactivate lifecycle node + */ + void deactivate() override; + + /** + * @brief Creating a plan from start and goal poses + * @param start Start pose + * @param goal Goal pose + * @return nav2_msgs::Path of the generated path + */ + nav_msgs::msg::Path createPlan( + 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; + std::unique_ptr _smoother; + nav2_costmap_2d::Costmap2D * _costmap; + std::unique_ptr _costmap_downsampler; + nav2_util::LifecycleNode::SharedPtr _node; + std::string _global_frame, _name; + float _tolerance; + int _downsampling_factor; + bool _downsample_costmap; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; + SmootherParams _smoother_params; + OptimizerParams _optimizer_params; + double _max_planning_time; +}; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__SMAC_PLANNER_2D_HPP_ diff --git a/smac_planner/include/smac_planner/smoother.hpp b/smac_planner/include/smac_planner/smoother.hpp new file mode 100644 index 0000000000..756e6b5793 --- /dev/null +++ b/smac_planner/include/smac_planner/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 SMAC_PLANNER__SMOOTHER_HPP_ +#define SMAC_PLANNER__SMOOTHER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "smac_planner/types.hpp" +#include "smac_planner/smoother_cost_function.hpp" + +#include "ceres/ceres.h" +#include "Eigen/Core" + +namespace smac_planner +{ + +/** + * @class smac_planner::Smoother + * @brief A Conjugate Gradient 2D path smoother implementation + */ +class Smoother +{ +public: + /** + * @brief A constructor for smac_planner::Smoother + */ + Smoother() {} + + /** + * @brief A destructor for 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 smac_planner + +#endif // SMAC_PLANNER__SMOOTHER_HPP_ diff --git a/smac_planner/include/smac_planner/smoother_cost_function.hpp b/smac_planner/include/smac_planner/smoother_cost_function.hpp new file mode 100644 index 0000000000..82d1c88a6d --- /dev/null +++ b/smac_planner/include/smac_planner/smoother_cost_function.hpp @@ -0,0 +1,515 @@ +// 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 SMAC_PLANNER__SMOOTHER_COST_FUNCTION_HPP_ +#define SMAC_PLANNER__SMOOTHER_COST_FUNCTION_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "ceres/ceres.h" +#include "Eigen/Core" +#include "smac_planner/types.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "smac_planner/options.hpp" + +#define EPSILON 0.0001 + +namespace smac_planner +{ + +/** + * @struct smac_planner::UnconstrainedSmootherCostFunction + * @brief Cost function for path smoothing with multiple terms + * including curvature, smoothness, collision, and avoid obstacles. + */ +class UnconstrainedSmootherCostFunction : public ceres::FirstOrderFunction +{ +public: + /** + * @brief A constructor for smac_planner::UnconstrainedSmootherCostFunction + * @param original_path Original unsmoothed path to smooth + * @param costmap A costmap to get values for collision and obstacle avoidance + */ + UnconstrainedSmootherCostFunction( + std::vector * original_path, + nav2_costmap_2d::Costmap2D * costmap, + const SmootherParams & params) + : _original_path(original_path), + _num_params(2 * original_path->size()), + _costmap(costmap), + _params(params) + { + } + + /** + * @struct CurvatureComputations + * @brief Cache common computations between the curvature terms to minimize recomputations + */ + struct CurvatureComputations + { + /** + * @brief A constructor for smac_planner::CurvatureComputations + */ + CurvatureComputations() + { + valid = true; + } + + bool valid; + /** + * @brief Check if result is valid for penalty + * @return is valid (non-nan, non-inf, and turning angle > max) + */ + bool isValid() + { + return valid; + } + + Eigen::Vector2d delta_xi{0.0, 0.0}; + Eigen::Vector2d delta_xi_p{0.0, 0.0}; + double delta_xi_norm{0}; + double delta_xi_p_norm{0}; + double delta_phi_i{0}; + double turning_rad{0}; + double ki_minus_kmax{0}; + }; + + /** + * @brief Smoother cost function evaluation + * @param parameters X,Y pairs of points + * @param cost total cost of path + * @param gradient of path at each X,Y pair from cost function derived analytically + * @return if successful in computing values + */ + virtual bool Evaluate( + const double * parameters, + double * cost, + double * gradient) const + { + Eigen::Vector2d xi; + Eigen::Vector2d xi_p1; + Eigen::Vector2d xi_m1; + uint x_index, y_index; + cost[0] = 0.0; + double cost_raw = 0.0; + double grad_x_raw = 0.0; + double grad_y_raw = 0.0; + unsigned int mx, my; + bool valid_coords = true; + double costmap_cost = 0.0; + + // cache some computations between the residual and jacobian + CurvatureComputations curvature_params; + + for (int i = 0; i != NumParameters() / 2; i++) { + x_index = 2 * i; + y_index = 2 * i + 1; + gradient[x_index] = 0.0; + gradient[y_index] = 0.0; + if (i < 1 || i >= NumParameters() / 2 - 1) { + continue; + } + + xi = Eigen::Vector2d(parameters[x_index], parameters[y_index]); + xi_p1 = Eigen::Vector2d(parameters[x_index + 2], parameters[y_index + 2]); + xi_m1 = Eigen::Vector2d(parameters[x_index - 2], parameters[y_index - 2]); + + // compute cost + addSmoothingResidual(_params.smooth_weight, xi, xi_p1, xi_m1, cost_raw); + addCurvatureResidual(_params.curvature_weight, xi, xi_p1, xi_m1, curvature_params, cost_raw); + addDistanceResidual(_params.distance_weight, xi, _original_path->at(i), cost_raw); + + 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); + } + + if (gradient != NULL) { + // compute gradient + gradient[x_index] = 0.0; + gradient[y_index] = 0.0; + addSmoothingJacobian(_params.smooth_weight, xi, xi_p1, xi_m1, grad_x_raw, grad_y_raw); + addCurvatureJacobian( + _params.curvature_weight, xi, xi_p1, xi_m1, curvature_params, + grad_x_raw, grad_y_raw); + addDistanceJacobian( + _params.distance_weight, xi, _original_path->at( + i), grad_x_raw, grad_y_raw); + + if (valid_coords) { + addCostJacobian(_params.costmap_weight, mx, my, costmap_cost, grad_x_raw, grad_y_raw); + } + + gradient[x_index] = grad_x_raw; + gradient[y_index] = grad_y_raw; + } + } + + cost[0] = cost_raw; + + return true; + } + + /** + * @brief Get number of parameter blocks + * @return Number of parameters in cost function + */ + virtual int NumParameters() const {return _num_params;} + +protected: + /** + * @brief Cost function term for smooth paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param r Residual (cost) of term + */ + inline void addSmoothingResidual( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + double & r) const + { + r += weight * ( + pt_p.dot(pt_p) - + 4 * pt_p.dot(pt) + + 2 * pt_p.dot(pt_m) + + 4 * pt.dot(pt) - + 4 * pt.dot(pt_m) + + pt_m.dot(pt_m)); // objective function value + } + + /** + * @brief Cost function derivative term for smooth paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param j0 Gradient of X term + * @param j1 Gradient of Y term + */ + inline void addSmoothingJacobian( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + double & j0, + double & j1) const + { + j0 += weight * + (-4 * pt_m[0] + 8 * pt[0] - 4 * pt_p[0]); // xi x component of partial-derivative + j1 += weight * + (-4 * pt_m[1] + 8 * pt[1] - 4 * pt_p[1]); // xi y component of partial-derivative + } + + /** + * @brief Cost function term for maximum curved paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param curvature_params A struct to cache computations for the jacobian to use + * @param r Residual (cost) of term + */ + inline void addCurvatureResidual( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + CurvatureComputations & curvature_params, + double & r) const + { + curvature_params.valid = true; + curvature_params.delta_xi = Eigen::Vector2d(pt[0] - pt_m[0], pt[1] - pt_m[1]); + curvature_params.delta_xi_p = Eigen::Vector2d(pt_p[0] - pt[0], pt_p[1] - pt[1]); + curvature_params.delta_xi_norm = curvature_params.delta_xi.norm(); + curvature_params.delta_xi_p_norm = curvature_params.delta_xi_p.norm(); + + if (curvature_params.delta_xi_norm < EPSILON || curvature_params.delta_xi_p_norm < EPSILON || + std::isnan(curvature_params.delta_xi_p_norm) || std::isnan(curvature_params.delta_xi_norm) || + std::isinf(curvature_params.delta_xi_p_norm) || std::isinf(curvature_params.delta_xi_norm)) + { + // ensure we have non-nan values returned + curvature_params.valid = false; + return; + } + + const double & delta_xi_by_xi_p = + curvature_params.delta_xi_norm * curvature_params.delta_xi_p_norm; + double projection = + curvature_params.delta_xi.dot(curvature_params.delta_xi_p) / delta_xi_by_xi_p; + if (fabs(1 - projection) < EPSILON || fabs(projection + 1) < EPSILON) { + projection = 1.0; + } + + curvature_params.delta_phi_i = std::acos(projection); + curvature_params.turning_rad = curvature_params.delta_phi_i / curvature_params.delta_xi_norm; + + curvature_params.ki_minus_kmax = curvature_params.turning_rad - _params.max_curvature; + + if (curvature_params.ki_minus_kmax <= EPSILON) { + // Quadratic penalty need not apply + curvature_params.valid = false; + return; + } + + r += weight * + curvature_params.ki_minus_kmax * curvature_params.ki_minus_kmax; // objective function value + } + + /** + * @brief Cost function derivative term for maximum curvature paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param curvature_params A struct with cached values to speed up Jacobian computation + * @param j0 Gradient of X term + * @param j1 Gradient of Y term + */ + inline void addCurvatureJacobian( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & /*pt_m*/, + CurvatureComputations & curvature_params, + double & j0, + double & j1) const + { + if (!curvature_params.isValid()) { + return; + } + + const double & partial_delta_phi_i_wrt_cost_delta_phi_i = + -1 / std::sqrt(1 - std::pow(std::cos(curvature_params.delta_phi_i), 2)); + // const Eigen::Vector2d ones = Eigen::Vector2d(1.0, 1.0); + auto neg_pt_plus = -1 * pt_p; + Eigen::Vector2d p1 = normalizedOrthogonalComplement( + pt, neg_pt_plus, curvature_params.delta_xi_norm, curvature_params.delta_xi_p_norm); + Eigen::Vector2d p2 = normalizedOrthogonalComplement( + neg_pt_plus, pt, curvature_params.delta_xi_p_norm, curvature_params.delta_xi_norm); + + const double & u = 2 * curvature_params.ki_minus_kmax; + const double & common_prefix = + (1 / curvature_params.delta_xi_norm) * partial_delta_phi_i_wrt_cost_delta_phi_i; + const double & common_suffix = curvature_params.delta_phi_i / + (curvature_params.delta_xi_norm * curvature_params.delta_xi_norm); + + const Eigen::Vector2d & d_delta_xi_d_xi = curvature_params.delta_xi / + curvature_params.delta_xi_norm; + + const Eigen::Vector2d jacobian = u * + (common_prefix * (-p1 - p2) - (common_suffix * d_delta_xi_d_xi)); + const Eigen::Vector2d jacobian_im1 = u * + (common_prefix * p2 + (common_suffix * d_delta_xi_d_xi)); + const Eigen::Vector2d jacobian_ip1 = u * (common_prefix * p1); + + // Old formulation we may require again. + // j0 += weight * + // (jacobian_im1[0] + 2 * jacobian[0] + jacobian_ip1[0]); + // j1 += weight * + // (jacobian_im1[1] + 2 * jacobian[1] + jacobian_ip1[1]); + + j0 += weight * jacobian[0]; // xi x component of partial-derivative + j1 += weight * jacobian[1]; // xi x component of partial-derivative + } + + /** + * @brief Cost function derivative term for steering away changes in pose + * @param weight Weight to apply to function + * @param xi Point Xi for evaluation + * @param xi_original original point Xi for evaluation + * @param r Residual (cost) of term + */ + inline void addDistanceResidual( + const double & weight, + const Eigen::Vector2d & xi, + const Eigen::Vector2d & xi_original, + double & r) const + { + r += weight * (xi - xi_original).dot(xi - xi_original); // objective function value + } + + /** + * @brief Cost function derivative term for steering away changes in pose + * @param weight Weight to apply to function + * @param xi Point Xi for evaluation + * @param xi_original original point Xi for evaluation + * @param j0 Gradient of X term + * @param j1 Gradient of Y term + */ + inline void addDistanceJacobian( + const double & weight, + const Eigen::Vector2d & xi, + const Eigen::Vector2d & xi_original, + double & j0, + double & j1) const + { + j0 += weight * 2 * (xi[0] - xi_original[0]); // xi y component of partial-derivative + j1 += weight * 2 * (xi[1] - xi_original[1]); // xi y component of partial-derivative + } + + + /** + * @brief Cost function term for steering away from costs + * @param weight Weight to apply to function + * @param value Point Xi's cost' + * @param params computed values to reduce overhead + * @param r Residual (cost) of term + */ + inline void addCostResidual( + const double & weight, + const double & value, + double & r) const + { + if (value == FREE) { + return; + } + + r += weight * value * value; // objective function value + } + + /** + * @brief Cost function derivative term for steering away from costs + * @param weight Weight to apply to function + * @param mx Point Xi's x coordinate in map frame + * @param mx Point Xi's y coordinate in map frame + * @param value Point Xi's cost' + * @param params computed values to reduce overhead + * @param j0 Gradient of X term + * @param j1 Gradient of Y term + */ + inline void addCostJacobian( + const double & weight, + const unsigned int & mx, + const unsigned int & my, + const double & value, + double & j0, + double & j1) const + { + if (value == FREE) { + return; + } + + const Eigen::Vector2d grad = getCostmapGradient(mx, my); + const double common_prefix = -2.0 * _params.costmap_factor * weight * value * value; + + j0 += common_prefix * grad[0]; // xi x component of partial-derivative + j1 += common_prefix * grad[1]; // xi y component of partial-derivative + } + + /** + * @brief Computing the gradient of the costmap using + * the 2 point numerical differentiation method + * @param mx Point Xi's x coordinate in map frame + * @param mx Point Xi's y coordinate in map frame + * @param params Params reference to store gradients + */ + inline Eigen::Vector2d getCostmapGradient( + const unsigned int mx, + const unsigned int my) const + { + // find unit vector that describes that direction + // via 7 point taylor series approximation for gradient at Xi + Eigen::Vector2d gradient; + + double l_1 = 0.0; + double l_2 = 0.0; + double l_3 = 0.0; + double r_1 = 0.0; + double r_2 = 0.0; + double r_3 = 0.0; + + if (mx < _costmap->getSizeInCellsX()) { + r_1 = static_cast(_costmap->getCost(mx + 1, my)); + } + if (mx + 1 < _costmap->getSizeInCellsX()) { + r_2 = static_cast(_costmap->getCost(mx + 2, my)); + } + if (mx + 2 < _costmap->getSizeInCellsX()) { + r_3 = static_cast(_costmap->getCost(mx + 3, my)); + } + + if (mx > 0) { + l_1 = static_cast(_costmap->getCost(mx - 1, my)); + } + if (mx - 1 > 0) { + l_2 = static_cast(_costmap->getCost(mx - 2, my)); + } + if (mx - 2 > 0) { + l_3 = static_cast(_costmap->getCost(mx - 3, my)); + } + + gradient[1] = (45 * r_1 - 9 * r_2 + r_3 - 45 * l_1 + 9 * l_2 - l_3) / 60; + + if (my < _costmap->getSizeInCellsY()) { + r_1 = static_cast(_costmap->getCost(mx, my + 1)); + } + if (my + 1 < _costmap->getSizeInCellsY()) { + r_2 = static_cast(_costmap->getCost(mx, my + 2)); + } + if (my + 2 < _costmap->getSizeInCellsY()) { + r_3 = static_cast(_costmap->getCost(mx, my + 3)); + } + + if (my > 0) { + l_1 = static_cast(_costmap->getCost(mx, my - 1)); + } + if (my - 1 > 0) { + l_2 = static_cast(_costmap->getCost(mx, my - 2)); + } + if (my - 2 > 0) { + l_3 = static_cast(_costmap->getCost(mx, my - 3)); + } + + gradient[0] = (45 * r_1 - 9 * r_2 + r_3 - 45 * l_1 + 9 * l_2 - l_3) / 60; + + gradient.normalize(); + return gradient; + } + + /** + * @brief Computing the normalized orthogonal component of 2 vectors + * @param a Vector + * @param b Vector + * @param norm a Vector's norm + * @param norm b Vector's norm + * @return Normalized vector of orthogonal components + */ + inline Eigen::Vector2d normalizedOrthogonalComplement( + const Eigen::Vector2d & a, + const Eigen::Vector2d & b, + const double & a_norm, + const double & b_norm) const + { + return (a - (a.dot(b) * b / b.squaredNorm())) / (a_norm * b_norm); + } + + std::vector * _original_path{nullptr}; + int _num_params; + nav2_costmap_2d::Costmap2D * _costmap{nullptr}; + SmootherParams _params; +}; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__SMOOTHER_COST_FUNCTION_HPP_ diff --git a/smac_planner/include/smac_planner/types.hpp b/smac_planner/include/smac_planner/types.hpp new file mode 100644 index 0000000000..e39564d333 --- /dev/null +++ b/smac_planner/include/smac_planner/types.hpp @@ -0,0 +1,42 @@ +// 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 SMAC_PLANNER__TYPES_HPP_ +#define SMAC_PLANNER__TYPES_HPP_ + +#include +#include + +namespace smac_planner +{ + +typedef std::pair NodeHeuristicPair; + +/** + * @struct smac_planner::SearchInfo + * @brief Search properties and penalties + */ +struct SearchInfo +{ + float minimum_turning_radius; + float non_straight_penalty; + float change_penalty; + float reverse_penalty; + float cost_penalty; + float analytic_expansion_ratio; +}; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__TYPES_HPP_ diff --git a/smac_planner/package.xml b/smac_planner/package.xml new file mode 100644 index 0000000000..8de1690786 --- /dev/null +++ b/smac_planner/package.xml @@ -0,0 +1,38 @@ + + + + smac_planner + 0.3.0 + Smac global planning plugin + Steve Macenski + Apache-2.0 + + ament_cmake + + rclcpp + rclcpp_action + rclcpp_lifecycle + visualization_msgs + nav2_util + nav2_msgs + nav_msgs + geometry_msgs + builtin_interfaces + nav2_common + tf2_ros + nav2_costmap_2d + nav2_core + pluginlib + libceres-dev + eigen3_cmake_module + eigen + ompl + + ament_lint_common + ament_lint_auto + + + ament_cmake + + + diff --git a/smac_planner/smac_plugin.xml b/smac_planner/smac_plugin.xml new file mode 100644 index 0000000000..09f17b666f --- /dev/null +++ b/smac_planner/smac_plugin.xml @@ -0,0 +1,5 @@ + + + SE2 version of the SMAC planner + + diff --git a/smac_planner/smac_plugin_2d.xml b/smac_planner/smac_plugin_2d.xml new file mode 100644 index 0000000000..3845a0ffe2 --- /dev/null +++ b/smac_planner/smac_plugin_2d.xml @@ -0,0 +1,5 @@ + + + 2D version of the SMAC Planner + + diff --git a/smac_planner/src/a_star.cpp b/smac_planner/src/a_star.cpp new file mode 100644 index 0000000000..3464442b70 --- /dev/null +++ b/smac_planner/src/a_star.cpp @@ -0,0 +1,641 @@ +// 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 +#include +#include +#include +#include +#include + +#include "smac_planner/a_star.hpp" +using namespace std::chrono; // NOLINT + +namespace smac_planner +{ + +template +AStarAlgorithm::AStarAlgorithm( + const MotionModel & motion_model, + const SearchInfo & search_info) +: _traverse_unknown(true), + _max_iterations(0), + _x_size(0), + _y_size(0), + _search_info(search_info), + _goal_coordinates(Coordinates()), + _start(nullptr), + _goal(nullptr), + _motion_model(motion_model), + _collision_checker(nullptr) +{ + _graph.reserve(100000); +} + +template +AStarAlgorithm::~AStarAlgorithm() +{ +} + +template +void AStarAlgorithm::initialize( + const bool & allow_unknown, + int & max_iterations, + const int & max_on_approach_iterations) +{ + _traverse_unknown = allow_unknown; + _max_iterations = max_iterations; + _max_on_approach_iterations = max_on_approach_iterations; +} + +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) +{ + 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); + } +} + +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) +{ + _costmap = costmap; + _collision_checker = GridCollisionChecker(costmap); + _collision_checker.setFootprint(_footprint, _is_radius_footprint); + + _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); + } +} + +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( + 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); +} + +template<> +void AStarAlgorithm::setStart( + const unsigned int & mx, + const unsigned int & my, + const unsigned int & dim_3) +{ + if (dim_3 != 0) { + throw std::runtime_error("Node type Node2D cannot be given non-zero starting dim 3."); + } + _start = addToGraph(Node2D::getIndex(mx, my, getSizeX())); +} + +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->setPose( + Coordinates( + static_cast(mx), + static_cast(my), + static_cast(dim_3))); +} + +template<> +void AStarAlgorithm::setGoal( + const unsigned int & mx, + const unsigned int & my, + const unsigned int & dim_3) +{ + if (dim_3 != 0) { + throw std::runtime_error("Node type Node2D cannot be given non-zero goal dim 3."); + } + + _goal = addToGraph(Node2D::getIndex(mx, my, getSizeX())); + _goal_coordinates = Node2D::Coordinates(mx, my); +} + +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( + 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); +} + +template +bool AStarAlgorithm::areInputsValid() +{ + // Check if graph was filled in + if (_graph.empty()) { + throw std::runtime_error("Failed to compute path, no costmap given."); + } + + // Check if points were filled in + if (!_start || !_goal) { + throw std::runtime_error("Failed to compute path, no valid start or goal given."); + } + + // Check if ending point is valid + if (getToleranceHeuristic() < 0.001 && + !_goal->isNodeValid(_traverse_unknown, _collision_checker)) + { + throw std::runtime_error("Failed to compute path, goal is occupied with no tolerance."); + } + + // Check if starting point is valid + if (!_start->isNodeValid(_traverse_unknown, _collision_checker)) { + throw std::runtime_error("Starting point in lethal space! Cannot create feasible plan."); + } + + return true; +} + +template +bool AStarAlgorithm::createPath( + CoordinateVector & path, int & iterations, + const float & tolerance) +{ + _tolerance = tolerance * NodeT::neutral_cost; + _best_heuristic_node = {std::numeric_limits::max(), 0}; + clearQueue(); + + if (!areInputsValid()) { + return false; + } + + // 0) Add starting point to the open set + addNode(0.0, getStart()); + getStart()->setAccumulatedCost(0.0); + + // Optimization: preallocate all variables + NodePtr current_node = nullptr; + NodePtr neighbor = nullptr; + float g_cost = 0.0; + NodeVector neighbors; + int approach_iterations = 0; + NeighborIterator neighbor_iterator; + int analytic_iterations = 0; + int closest_distance = std::numeric_limits::max(); + + // Given an index, return a node ptr reference if its collision-free and valid + const unsigned int max_index = getSizeX() * getSizeY() * getSizeDim3(); + NodeGetter neighborGetter = + [&, this](const unsigned int & index, NodePtr & neighbor_rtn) -> bool + { + if (index < 0 || index >= max_index) { + return false; + } + + neighbor_rtn = addToGraph(index); + return true; + }; + + while (iterations < getMaxIterations() && !_queue.empty()) { + // 1) Pick Nbest from O s.t. min(f(Nbest)), remove from queue + current_node = getNextNode(); + + // We allow for nodes to be queued multiple times in case + // shorter paths result in it, but we can visit only once + if (current_node->wasVisited()) { + continue; + } + + iterations++; + + // 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; + } + + // 3) Check if we're at the goal, backtrace if required + if (isGoal(current_node)) { + return backtracePath(current_node, path); + } 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); + } + } + + // 4) Expand neighbors of Nbest not visited + neighbors.clear(); + NodeT::getNeighbors( + current_node, neighborGetter, _collision_checker, _traverse_unknown, neighbors); + + for (neighbor_iterator = neighbors.begin(); + neighbor_iterator != neighbors.end(); ++neighbor_iterator) + { + neighbor = *neighbor_iterator; + + // 4.1) Compute the cost to go to this node + g_cost = getAccumulatedCost(current_node) + getTraversalCost(current_node, 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)) { + neighbor->setAccumulatedCost(g_cost); + neighbor->parent = current_node; + + // 4.3) If not in queue or visited, add it, `getNeighbors()` handles + neighbor->queued(); + addNode(g_cost + getHeuristicCost(neighbor), neighbor); + } + } + } + + return false; +} + +template +bool AStarAlgorithm::isGoal(NodePtr & node) +{ + return node == getGoal(); +} + +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 + constexpr 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; + n->parent = prev; + prev = n; + } + _goal->parent = prev; + 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) +{ + if (!node->parent) { + return false; + } + + NodePtr current_node = node; + + while (current_node->parent) { + path.push_back( + Node2D::getCoords( + current_node->getIndex(), getSizeX(), getSizeDim3())); + current_node = current_node->parent; + } + + return path.size() > 1; +} + +template<> +bool AStarAlgorithm::backtracePath(NodePtr & node, CoordinateVector & path) +{ + if (!node->parent) { + return false; + } + + NodePtr current_node = node; + + while (current_node->parent) { + path.push_back(current_node->pose); + current_node = current_node->parent; + } + + return path.size() > 1; +} + +template +typename AStarAlgorithm::NodePtr & AStarAlgorithm::getStart() +{ + return _start; +} + +template +typename AStarAlgorithm::NodePtr & AStarAlgorithm::getGoal() +{ + return _goal; +} + +template +typename AStarAlgorithm::NodePtr AStarAlgorithm::getNextNode() +{ + NodeBasic node = _queue.top().second; + _queue.pop(); + return node.graph_node_ptr; +} + +template<> +typename AStarAlgorithm::NodePtr AStarAlgorithm::getNextNode() +{ + NodeBasic node = _queue.top().second; + _queue.pop(); + + if (!node.graph_node_ptr->wasVisited()) { + node.graph_node_ptr->pose = node.pose; + } + + return node.graph_node_ptr; +} + +template +void AStarAlgorithm::addNode(const float cost, NodePtr & node) +{ + 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) +{ + 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); + + if (heuristic < _best_heuristic_node.first) { + _best_heuristic_node = {heuristic, node->getIndex()}; + } + + return heuristic; +} + +template +void AStarAlgorithm::clearQueue() +{ + NodeQueue q; + std::swap(_queue, q); +} + +template +void AStarAlgorithm::clearGraph() +{ + Graph g; + g.reserve(100000); + std::swap(_graph, g); +} + +template +int & AStarAlgorithm::getMaxIterations() +{ + return _max_iterations; +} + +template +int & AStarAlgorithm::getOnApproachMaxIterations() +{ + return _max_on_approach_iterations; +} + +template +float & AStarAlgorithm::getToleranceHeuristic() +{ + return _tolerance; +} + +template +unsigned int & AStarAlgorithm::getSizeX() +{ + return _x_size; +} + +template +unsigned int & AStarAlgorithm::getSizeY() +{ + return _y_size; +} + +template +unsigned int & AStarAlgorithm::getSizeDim3() +{ + return _dim3_size; +} + +template +typename AStarAlgorithm::NodePtr AStarAlgorithm::tryAnalyticExpansion( + 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 + + // 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, + static_cast(NodeT::getHeuristicCost( + node_coords, + _goal_coordinates) / NodeT::neutral_cost) + ); + // 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)) + ); + // If we are closer now, we should update the target number of iterations to go + analytic_iterations = + std::min(analytic_iterations, desired_iterations); + + // 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 + analytic_iterations = desired_iterations; + return getAnalyticPath(current_node, getter); + } + analytic_iterations--; + } + // No valid motion model - return nullptr + return NodePtr(nullptr); +} + +// Instantiate algorithm for the supported template types +template class AStarAlgorithm; +template class AStarAlgorithm; + +} // namespace smac_planner diff --git a/smac_planner/src/costmap_downsampler.cpp b/smac_planner/src/costmap_downsampler.cpp new file mode 100644 index 0000000000..6955469831 --- /dev/null +++ b/smac_planner/src/costmap_downsampler.cpp @@ -0,0 +1,129 @@ +// Copyright (c) 2020, Carlos Luis +// 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 "smac_planner/costmap_downsampler.hpp" + +#include +#include +#include + +namespace smac_planner +{ + +CostmapDownsampler::CostmapDownsampler(const nav2_util::LifecycleNode::SharedPtr & node) +: _node(node), + _costmap(nullptr), + _downsampled_costmap(nullptr), + _downsampled_costmap_pub(nullptr) +{ +} + +CostmapDownsampler::~CostmapDownsampler() +{ +} + +void CostmapDownsampler::initialize( + const std::string & global_frame, + const std::string & topic_name, + nav2_costmap_2d::Costmap2D * const costmap, + const unsigned int & downsampling_factor) +{ + _topic_name = topic_name; + _costmap = costmap; + _downsampling_factor = downsampling_factor; + updateCostmapSize(); + + _downsampled_costmap = std::make_unique( + _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); +} + +nav2_costmap_2d::Costmap2D * CostmapDownsampler::downsample( + const unsigned int & downsampling_factor) +{ + _downsampling_factor = downsampling_factor; + updateCostmapSize(); + + // Adjust costmap size if needed + if (_downsampled_costmap->getSizeInCellsX() != _downsampled_size_x || + _downsampled_costmap->getSizeInCellsY() != _downsampled_size_y || + _downsampled_costmap->getResolution() != _downsampled_resolution) + { + resizeCostmap(); + } + + // Assign costs + for (uint i = 0; i < _downsampled_size_x; ++i) { + for (uint j = 0; j < _downsampled_size_y; ++j) { + setCostOfCell(i, j); + } + } + + if (_node->count_subscribers(_topic_name) > 0) { + _downsampled_costmap_pub->publishCostmap(); + } + + return _downsampled_costmap.get(); +} + +void CostmapDownsampler::updateCostmapSize() +{ + _size_x = _costmap->getSizeInCellsX(); + _size_y = _costmap->getSizeInCellsY(); + _downsampled_size_x = ceil(static_cast(_size_x) / _downsampling_factor); + _downsampled_size_y = ceil(static_cast(_size_y) / _downsampling_factor); + _downsampled_resolution = _downsampling_factor * _costmap->getResolution(); +} + +void CostmapDownsampler::resizeCostmap() +{ + _downsampled_costmap->resizeMap( + _downsampled_size_x, + _downsampled_size_y, + _downsampled_resolution, + _costmap->getOriginX(), + _costmap->getOriginY()); +} + +void CostmapDownsampler::setCostOfCell( + const unsigned int & new_mx, + const unsigned int & new_my) +{ + unsigned int mx, my; + unsigned char cost = 0; + unsigned int x_offset = new_mx * _downsampling_factor; + unsigned int y_offset = new_my * _downsampling_factor; + + for (uint i = 0; i < _downsampling_factor; ++i) { + mx = x_offset + i; + if (mx >= _size_x) { + continue; + } + for (uint j = 0; j < _downsampling_factor; ++j) { + my = y_offset + j; + if (my >= _size_y) { + continue; + } + cost = std::max(cost, _costmap->getCost(mx, my)); + } + } + + _downsampled_costmap->setCost(new_mx, new_my, cost); +} + +} // namespace smac_planner diff --git a/smac_planner/src/node_2d.cpp b/smac_planner/src/node_2d.cpp new file mode 100644 index 0000000000..b6afd5b944 --- /dev/null +++ b/smac_planner/src/node_2d.cpp @@ -0,0 +1,151 @@ +// 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 "smac_planner/node_2d.hpp" + +#include +#include + +namespace smac_planner +{ + +// defining static member for all instance to share +std::vector Node2D::_neighbors_grid_offsets; +double Node2D::neutral_cost = 50.0; + +Node2D::Node2D(unsigned char & cost_in, const unsigned int index) +: parent(nullptr), + _cell_cost(static_cast(cost_in)), + _accumulated_cost(std::numeric_limits::max()), + _index(index), + _was_visited(false), + _is_queued(false) +{ +} + +Node2D::~Node2D() +{ + parent = nullptr; +} + +void Node2D::reset(const unsigned char & cost) +{ + parent = nullptr; + _cell_cost = static_cast(cost); + _accumulated_cost = std::numeric_limits::max(); + _was_visited = false; + _is_queued = false; +} + +bool Node2D::isNodeValid( + const bool & traverse_unknown, + 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) { + return false; + } + + return true; +} + +float Node2D::getTraversalCost(const NodePtr & child) +{ + // cost to travel will be the cost of the cell's code + + // 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(); +} + +float Node2D::getHeuristicCost( + const Coordinates & node_coords, + const Coordinates & goal_coordinates) +{ + return hypotf( + goal_coordinates.x - node_coords.x, + goal_coordinates.y - node_coords.y) * Node2D::neutral_cost; +} + +void Node2D::initNeighborhood( + const unsigned int & x_size_uint, + const MotionModel & neighborhood) +{ + int x_size = static_cast(x_size_uint); + switch (neighborhood) { + case MotionModel::UNKNOWN: + throw std::runtime_error("Unknown neighborhood type selected."); + case MotionModel::VON_NEUMANN: + _neighbors_grid_offsets = {-1, +1, -x_size, +x_size}; + break; + case MotionModel::MOORE: + _neighbors_grid_offsets = {-1, +1, -x_size, +x_size, -x_size - 1, + -x_size + 1, +x_size - 1, +x_size + 1}; + break; + default: + throw std::runtime_error( + "Invalid neighborhood type selected. " + "Von-Neumann and Moore are valid for Node2D."); + } +} + +void Node2D::getNeighbors( + NodePtr & node, + std::function & NeighborGetter, + 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 + // 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. + // 100 50 0 + // 100 50 50 + // 100 100 100 where lower-middle '100' is visited with same cost by both bottom '50' nodes + // Therefore, it is valuable to have some low-potential across the entire map + // rather than a small inflation around the obstacles + int index; + NodePtr neighbor; + int node_i = node->getIndex(); + + for (unsigned int i = 0; i != _neighbors_grid_offsets.size(); ++i) { + index = node_i + _neighbors_grid_offsets[i]; + if (NeighborGetter(index, neighbor)) { + if (neighbor->isNodeValid(traverse_unknown, collision_checker) && !neighbor->wasVisited()) { + neighbors.push_back(neighbor); + } + } + } +} + +} // namespace smac_planner diff --git a/smac_planner/src/node_se2.cpp b/smac_planner/src/node_se2.cpp new file mode 100644 index 0000000000..9764df0dbd --- /dev/null +++ b/smac_planner/src/node_se2.cpp @@ -0,0 +1,432 @@ +// 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 "smac_planner/node_se2.hpp" + +using namespace std::chrono; // NOLINT + +namespace 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); +} + +// 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); +} + +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]; + + // transform delta X, Y, and Theta into local coordinates + const float & node_heading = node->pose.theta; + const float cos_theta = cos(node_heading * bin_size); // needs actual angle [0, 2PI] + const float sin_theta = sin(node_heading * bin_size); + const float delta_x = motion_model._x * cos_theta - motion_model._y * sin_theta; + const float delta_y = motion_model._x * sin_theta + motion_model._y * cos_theta; + float new_heading = node_heading + motion_model._theta; + + // normalize 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_x + node->pose.x, delta_y + 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 smac_planner diff --git a/smac_planner/src/smac_planner.cpp b/smac_planner/src/smac_planner.cpp new file mode 100644 index 0000000000..f3db10830a --- /dev/null +++ b/smac_planner/src/smac_planner.cpp @@ -0,0 +1,385 @@ +// 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 + +#include "Eigen/Core" +#include "smac_planner/smac_planner.hpp" + +#define BENCHMARK_TESTING + +namespace smac_planner +{ + +using namespace std::chrono; // NOLINT + +SmacPlanner::SmacPlanner() +: _a_star(nullptr), + _smoother(nullptr), + _node(nullptr), + _costmap(nullptr), + _costmap_downsampler(nullptr) +{ +} + +SmacPlanner::~SmacPlanner() +{ + RCLCPP_INFO( + _node->get_logger(), "Destroying plugin %s of type SmacPlanner", + _name.c_str()); +} + +void SmacPlanner::configure( + rclcpp_lifecycle::LifecycleNode::SharedPtr parent, + std::string name, std::shared_ptr/*tf*/, + std::shared_ptr costmap_ros) +{ + _node = parent; + _costmap = costmap_ros->getCostmap(); + _name = name; + _global_frame = costmap_ros->getGlobalFrameID(); + + bool allow_unknown; + int max_iterations; + int max_on_approach_iterations = std::numeric_limits::max(); + int angle_quantizations; + 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(true)); + _node->get_parameter(name + ".downsample_costmap", _downsample_costmap); + 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 + ".angle_quantization_bins", rclcpp::ParameterValue(1)); + _node->get_parameter(name + ".angle_quantization_bins", angle_quantizations); + _angle_bin_size = 2.0 * M_PI / angle_quantizations; + _angle_quantizations = static_cast(angle_quantizations); + + 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->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->get_parameter(name + ".minimum_turning_radius", search_info.minimum_turning_radius); + + 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->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->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->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->get_parameter(name + ".analytic_expansion_ratio", search_info.analytic_expansion_ratio); + + nav2_util::declare_parameter_if_not_declared( + _node, name + ".max_planning_time_ms", rclcpp::ParameterValue(5000.0)); + _node->get_parameter(name + ".max_planning_time_ms", _max_planning_time); + + nav2_util::declare_parameter_if_not_declared( + _node, name + ".motion_model_for_search", rclcpp::ParameterValue(std::string("DUBIN"))); + _node->get_parameter(name + ".motion_model_for_search", motion_model_for_search); + MotionModel motion_model = fromString(motion_model_for_search); + if (motion_model == MotionModel::UNKNOWN) { + RCLCPP_WARN( + _node->get_logger(), + "Unable to get MotionModel search type. Given '%s', " + "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.", + motion_model_for_search.c_str()); + } + + if (max_on_approach_iterations <= 0) { + RCLCPP_INFO( + _node->get_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( + _node->get_logger(), "maximum iteration selected as <= 0, " + "disabling maximum iterations."); + max_iterations = std::numeric_limits::max(); + } + + // convert to grid coordinates + 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); + + _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); + } + + if (_downsample_costmap && _downsampling_factor > 1) { + std::string topic_name = "downsampled_costmap"; + _costmap_downsampler = std::make_unique(_node); + _costmap_downsampler->initialize(_global_frame, topic_name, _costmap, _downsampling_factor); + } + + _raw_plan_publisher = _node->create_publisher("unsmoothed_plan", 1); + + RCLCPP_INFO( + _node->get_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, + allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal", + toString(motion_model).c_str()); +} + +void SmacPlanner::activate() +{ + RCLCPP_INFO( + _node->get_logger(), "Activating plugin %s of type SmacPlanner", + _name.c_str()); + _raw_plan_publisher->on_activate(); + if (_costmap_downsampler) { + _costmap_downsampler->activatePublisher(); + } +} + +void SmacPlanner::deactivate() +{ + RCLCPP_INFO( + _node->get_logger(), "Deactivating plugin %s of type SmacPlanner", + _name.c_str()); + _raw_plan_publisher->on_deactivate(); + if (_costmap_downsampler) { + _costmap_downsampler->deactivatePublisher(); + } +} + +void SmacPlanner::cleanup() +{ + RCLCPP_INFO( + _node->get_logger(), "Cleaning up plugin %s of type SmacPlanner", + _name.c_str()); + _a_star.reset(); + _smoother.reset(); + _costmap_downsampler.reset(); + _raw_plan_publisher.reset(); +} + +nav_msgs::msg::Path SmacPlanner::createPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) +{ + steady_clock::time_point a = steady_clock::now(); + + std::unique_lock lock(*(_costmap->getMutex())); + + // Downsample costmap, if required + nav2_costmap_2d::Costmap2D * costmap = _costmap; + if (_costmap_downsampler) { + costmap = _costmap_downsampler->downsample(_downsampling_factor); + } + + // Set Costmap + _a_star->createGraph( + costmap->getSizeInCellsX(), + costmap->getSizeInCellsY(), + _angle_quantizations, + costmap); + + // Set starting point, in A* bin search coordinates + unsigned int mx, my; + costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my); + double orientation_bin = tf2::getYaw(start.pose.orientation) / _angle_bin_size; + while (orientation_bin < 0.0) { + orientation_bin += static_cast(_angle_quantizations); + } + unsigned int orientation_bin_id = static_cast(floor(orientation_bin)); + _a_star->setStart(mx, my, orientation_bin_id); + + // Set goal point, in A* bin search coordinates + costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my); + orientation_bin = tf2::getYaw(goal.pose.orientation) / _angle_bin_size; + while (orientation_bin < 0.0) { + orientation_bin += static_cast(_angle_quantizations); + } + orientation_bin_id = static_cast(floor(orientation_bin)); + _a_star->setGoal(mx, my, orientation_bin_id); + + // Setup message + nav_msgs::msg::Path plan; + plan.header.stamp = _node->now(); + plan.header.frame_id = _global_frame; + geometry_msgs::msg::PoseStamped pose; + pose.header = plan.header; + pose.pose.position.z = 0.0; + pose.pose.orientation.x = 0.0; + pose.pose.orientation.y = 0.0; + pose.pose.orientation.z = 0.0; + pose.pose.orientation.w = 1.0; + + // Compute plan + NodeSE2::CoordinateVector path; + int num_iterations = 0; + std::string error; + try { + if (!_a_star->createPath( + path, num_iterations, _tolerance / static_cast(costmap->getResolution()))) + { + if (num_iterations < _a_star->getMaxIterations()) { + error = std::string("no valid path found"); + } else { + error = std::string("exceeded maximum iterations"); + } + } + } catch (const std::runtime_error & e) { + error = "invalid use: "; + error += e.what(); + } + + if (!error.empty()) { + RCLCPP_WARN( + _node->get_logger(), + "%s: failed to create plan, %s.", + _name.c_str(), error.c_str()); + 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()); + 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); + plan.poses.push_back(pose); + } + + // Publish raw path for debug + if (_node->count_subscribers(_raw_plan_publisher->get_topic_name()) > 0) { + _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( + _node->get_logger(), + "%s: failed to smooth plan, Ceres could not find a usable solution to optimize.", + _name.c_str()); + return plan; + } + + 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; + } + + 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 smac_planner + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(smac_planner::SmacPlanner, nav2_core::GlobalPlanner) diff --git a/smac_planner/src/smac_planner_2d.cpp b/smac_planner/src/smac_planner_2d.cpp new file mode 100644 index 0000000000..3c4b8ca83a --- /dev/null +++ b/smac_planner/src/smac_planner_2d.cpp @@ -0,0 +1,339 @@ +// 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 + +#include "smac_planner/smac_planner_2d.hpp" + +// #define BENCHMARK_TESTING + +namespace smac_planner +{ +using namespace std::chrono; // NOLINT + +SmacPlanner2D::SmacPlanner2D() +: _a_star(nullptr), + _smoother(nullptr), + _costmap(nullptr), + _costmap_downsampler(nullptr), + _node(nullptr) +{ +} + +SmacPlanner2D::~SmacPlanner2D() +{ + RCLCPP_INFO( + _node->get_logger(), "Destroying plugin %s of type SmacPlanner2D", + _name.c_str()); +} + +void SmacPlanner2D::configure( + rclcpp_lifecycle::LifecycleNode::SharedPtr parent, + std::string name, std::shared_ptr/*tf*/, + std::shared_ptr costmap_ros) +{ + _node = parent; + _costmap = costmap_ros->getCostmap(); + _name = name; + _global_frame = costmap_ros->getGlobalFrameID(); + + bool allow_unknown; + int max_iterations; + int max_on_approach_iterations; + bool smooth_path; + double minimum_turning_radius; + 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(true)); + _node->get_parameter(name + ".downsample_costmap", _downsample_costmap); + 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 + ".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->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_ms", rclcpp::ParameterValue(1000.0)); + _node->get_parameter(name + ".max_planning_time_ms", _max_planning_time); + + nav2_util::declare_parameter_if_not_declared( + _node, name + ".motion_model_for_search", rclcpp::ParameterValue(std::string("MOORE"))); + _node->get_parameter(name + ".motion_model_for_search", motion_model_for_search); + MotionModel motion_model = fromString(motion_model_for_search); + if (motion_model == MotionModel::UNKNOWN) { + RCLCPP_WARN( + _node->get_logger(), + "Unable to get MotionModel search type. Given '%s', " + "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.", + motion_model_for_search.c_str()); + } + + if (max_on_approach_iterations <= 0) { + RCLCPP_INFO( + _node->get_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( + _node->get_logger(), "maximum iteration selected as <= 0, " + "disabling maximum iterations."); + max_iterations = std::numeric_limits::max(); + } + + _a_star = std::make_unique>(motion_model, SearchInfo()); + _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); + } + + if (_downsample_costmap && _downsampling_factor > 1) { + std::string topic_name = "downsampled_costmap"; + _costmap_downsampler = std::make_unique(_node); + _costmap_downsampler->initialize(_global_frame, topic_name, _costmap, _downsampling_factor); + } + + _raw_plan_publisher = _node->create_publisher("unsmoothed_plan", 1); + + RCLCPP_INFO( + _node->get_logger(), "Configured plugin %s of type SmacPlanner2D 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, + allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal", + toString(motion_model).c_str()); +} + +void SmacPlanner2D::activate() +{ + RCLCPP_INFO( + _node->get_logger(), "Activating plugin %s of type SmacPlanner2D", + _name.c_str()); + _raw_plan_publisher->on_activate(); + if (_costmap_downsampler) { + _costmap_downsampler->activatePublisher(); + } +} + +void SmacPlanner2D::deactivate() +{ + RCLCPP_INFO( + _node->get_logger(), "Deactivating plugin %s of type SmacPlanner2D", + _name.c_str()); + _raw_plan_publisher->on_deactivate(); + if (_costmap_downsampler) { + _costmap_downsampler->deactivatePublisher(); + } +} + +void SmacPlanner2D::cleanup() +{ + RCLCPP_INFO( + _node->get_logger(), "Cleaning up plugin %s of type SmacPlanner2D", + _name.c_str()); + _a_star.reset(); + _smoother.reset(); + _costmap_downsampler.reset(); + _raw_plan_publisher.reset(); +} + +nav_msgs::msg::Path SmacPlanner2D::createPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) +{ + steady_clock::time_point a = steady_clock::now(); + + std::unique_lock lock(*(_costmap->getMutex())); + + // Downsample costmap, if required + nav2_costmap_2d::Costmap2D * costmap = _costmap; + if (_costmap_downsampler) { + costmap = _costmap_downsampler->downsample(_downsampling_factor); + } + + // Set Costmap + _a_star->createGraph( + costmap->getSizeInCellsX(), + costmap->getSizeInCellsY(), + 1, + costmap); + + // Set starting point + unsigned int mx, my; + costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my); + _a_star->setStart(mx, my, 0); + + // Set goal point + costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my); + _a_star->setGoal(mx, my, 0); + + // Setup message + nav_msgs::msg::Path plan; + plan.header.stamp = _node->now(); + plan.header.frame_id = _global_frame; + geometry_msgs::msg::PoseStamped pose; + pose.header = plan.header; + pose.pose.position.z = 0.0; + pose.pose.orientation.x = 0.0; + pose.pose.orientation.y = 0.0; + pose.pose.orientation.z = 0.0; + pose.pose.orientation.w = 1.0; + + // Compute plan + Node2D::CoordinateVector path; + int num_iterations = 0; + std::string error; + try { + if (!_a_star->createPath( + path, num_iterations, _tolerance / static_cast(costmap->getResolution()))) + { + if (num_iterations < _a_star->getMaxIterations()) { + error = std::string("no valid path found"); + } else { + error = std::string("exceeded maximum iterations"); + } + } + } catch (const std::runtime_error & e) { + error = "invalid use: "; + error += e.what(); + } + + if (!error.empty()) { + RCLCPP_WARN( + _node->get_logger(), + "%s: failed to create plan, %s.", + _name.c_str(), error.c_str()); + 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()); + + 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(); + plan.poses.push_back(pose); + } + + // Publish raw path for debug + if (_node->count_subscribers(_raw_plan_publisher->get_topic_name()) > 0) { + _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( + _node->get_logger(), + "%s: failed to smooth plan, Ceres could not find a usable solution to optimize.", + _name.c_str()); + return plan; + } + + removeHook(path_world); + + // 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; + } + + 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 smac_planner + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(smac_planner::SmacPlanner2D, nav2_core::GlobalPlanner) diff --git a/smac_planner/test/CMakeLists.txt b/smac_planner/test/CMakeLists.txt new file mode 100644 index 0000000000..c5ff757d1c --- /dev/null +++ b/smac_planner/test/CMakeLists.txt @@ -0,0 +1,98 @@ +# Test costmap downsampler +ament_add_gtest(test_costmap_downsampler + test_costmap_downsampler.cpp +) +ament_target_dependencies(test_costmap_downsampler + ${dependencies} +) +target_link_libraries(test_costmap_downsampler + ${library_name} +) + +# Test Node2D +ament_add_gtest(test_node2d + test_node2d.cpp +) +ament_target_dependencies(test_node2d + ${dependencies} +) +target_link_libraries(test_node2d + ${library_name} +) + +# Test NodeSE2 +ament_add_gtest(test_nodese2 + test_nodese2.cpp +) +ament_target_dependencies(test_nodese2 + ${dependencies} +) +target_link_libraries(test_nodese2 + ${library_name} +) + +# Test NodeBasic +ament_add_gtest(test_nodebasic + test_nodebasic.cpp +) +ament_target_dependencies(test_nodebasic + ${dependencies} +) +target_link_libraries(test_nodebasic + ${library_name} +) + +# Test collision checker +ament_add_gtest(test_collision_checker + test_collision_checker.cpp +) +ament_target_dependencies(test_collision_checker + ${dependencies} +) +target_link_libraries(test_collision_checker + ${library_name} +) + +# Test A* +ament_add_gtest(test_a_star + test_a_star.cpp +) +ament_target_dependencies(test_a_star + ${dependencies} +) +target_link_libraries(test_a_star + ${library_name} +) + +# Test SMAC SE2 +ament_add_gtest(test_smac_se2 + test_smac_se2.cpp +) +ament_target_dependencies(test_smac_se2 + ${dependencies} +) +target_link_libraries(test_smac_se2 + ${library_name} +) + +# Test SMAC 2D +ament_add_gtest(test_smac_2d + test_smac_2d.cpp +) +ament_target_dependencies(test_smac_2d + ${dependencies} +) +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/smac_planner/test/deprecated_upsampler/upsampler.hpp b/smac_planner/test/deprecated_upsampler/upsampler.hpp new file mode 100644 index 0000000000..8bfc36c00e --- /dev/null +++ b/smac_planner/test/deprecated_upsampler/upsampler.hpp @@ -0,0 +1,213 @@ +// 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_UPSAMPLER__UPSAMPLER_HPP_ +#define DEPRECATED_UPSAMPLER__UPSAMPLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "smac_planner/types.hpp" +#include "smac_planner/upsampler_cost_function.hpp" +#include "smac_planner/upsampler_cost_function_nlls.hpp" + +#include "ceres/ceres.h" +#include "Eigen/Core" + +namespace smac_planner +{ + +/** + * @class smac_planner::Upsampler + * @brief A Conjugate Gradient 2D path upsampler implementation + */ +class Upsampler +{ +public: + /** + * @brief A constructor for smac_planner::Upsampler + */ + Upsampler() {} + + /** + * @brief A destructor for smac_planner::Upsampler + */ + ~Upsampler() {} + + /** + * @brief Initialization of the Upsampler + */ + 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; // 5000 + _options.max_solver_time_in_seconds = params.max_time; // 5.0; // TODO + + _options.function_tolerance = params.fn_tol; + _options.gradient_tolerance = params.gradient_tol; + _options.parameter_tolerance = params.param_tol; // 1e-20; + + _options.min_line_search_step_size = params.advanced.min_line_search_step_size; // 1e-30; + _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; // 1e-30; + _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 Upsampling method + * @param path Reference to path + * @param upsample parameters weights + * @param upsample_ratio upsample ratio + * @return If Upsampler was successful + */ + bool upsample( + std::vector & path, + const SmootherParams & params, + const int & upsample_ratio) + { + _options.max_solver_time_in_seconds = params.max_time; + + if (upsample_ratio != 2 && upsample_ratio != 4) { + // invalid inputs + return false; + } + + const int param_ratio = upsample_ratio * 2.0; + const int total_size = 2 * (path.size() * upsample_ratio - upsample_ratio + 1); + double parameters[total_size]; // NOLINT + + // 20-4hz regularly, but dosnt work in faster cases + // Linearly distribute initial poses for optimization + // TODO(stevemacenski) generalize for 2x and 4x + unsigned int next_pt; + Eigen::Vector2d interpolated; + std::vector temp_path; + for (unsigned int pt = 0; pt != path.size() - 1; pt++) { + next_pt = pt + 1; + interpolated = (path[next_pt] + path[pt]) / 2.0; + + parameters[param_ratio * pt] = path[pt][0]; + parameters[param_ratio * pt + 1] = path[pt][1]; + temp_path.push_back(path[pt]); + + parameters[param_ratio * pt + 2] = interpolated[0]; + parameters[param_ratio * pt + 3] = interpolated[1]; + temp_path.push_back(interpolated); + } + + parameters[total_size - 2] = path.back()[0]; + parameters[total_size - 1] = path.back()[1]; + temp_path.push_back(path.back()); + + // Solve the upsampling problem + ceres::GradientProblemSolver::Summary summary; + ceres::GradientProblem problem(new UpsamplerCostFunction(temp_path, params, upsample_ratio)); + ceres::Solve(_options, problem, parameters, &summary); + + + path.resize(total_size / 2); + for (int i = 0; i != total_size / 2; i++) { + path[i][0] = parameters[2 * i]; + path[i][1] = parameters[2 * i + 1]; + } + + // 10-15 hz, regularly + // std::vector path_double_sampled; + // for (int i = 0; i != path.size() - 1; i++) { // last term should not be upsampled + // path_double_sampled.push_back(path[i]); + // path_double_sampled.push_back((path[i+1] + path[i]) / 2); + // } + + // std::unique_ptr problem = std::make_unique(); + // for (uint i = 1; i != path_double_sampled.size() - 1; i++) { + // ceres::CostFunction * cost_fn = + // new UpsamplerConstrainedCostFunction(path_double_sampled, params, 2, i); + // problem->AddResidualBlock( + // cost_fn, nullptr, &path_double_sampled[i][0], &path_double_sampled[i][1]); + // // locking initial coordinates unnecessary since there's no update between terms in NLLS + // } + + // ceres::Solver::Summary summary; + // _options.minimizer_type = ceres::LINE_SEARCH; + // ceres::Solve(_options, problem.get(), &summary); + + // if (upsample_ratio == 4) { + // std::vector path_quad_sampled; + // for (int i = 0; i != path_double_sampled.size() - 1; i++) { + // path_quad_sampled.push_back(path_double_sampled[i]); + // path_quad_sampled.push_back((path_double_sampled[i+1] + path_double_sampled[i]) / 2.0); + // } + + // std::unique_ptr problem2 = std::make_unique(); + // for (uint i = 1; i != path_quad_sampled.size() - 1; i++) { + // ceres::CostFunction * cost_fn = + // new UpsamplerConstrainedCostFunction(path_quad_sampled, params, 4, i); + // problem2->AddResidualBlock( + // cost_fn, nullptr, &path_quad_sampled[i][0], &path_quad_sampled[i][1]); + // } + + // ceres::Solve(_options, problem2.get(), &summary); + + // path = path_quad_sampled; + // } else { + // path = path_double_sampled; + // } + + if (_debug) { + std::cout << summary.FullReport() << '\n'; + } + + if (!summary.IsSolutionUsable() || summary.initial_cost - summary.final_cost <= 0.0) { + return false; + } + + return true; + } + +private: + bool _debug; + ceres::GradientProblemSolver::Options _options; +}; + +} // namespace smac_planner + +#endif // DEPRECATED_UPSAMPLER__UPSAMPLER_HPP_ diff --git a/smac_planner/test/deprecated_upsampler/upsampler_cost_function.hpp b/smac_planner/test/deprecated_upsampler/upsampler_cost_function.hpp new file mode 100644 index 0000000000..cf84acf623 --- /dev/null +++ b/smac_planner/test/deprecated_upsampler/upsampler_cost_function.hpp @@ -0,0 +1,366 @@ +// 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_UPSAMPLER__UPSAMPLER_COST_FUNCTION_HPP_ +#define DEPRECATED_UPSAMPLER__UPSAMPLER_COST_FUNCTION_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "ceres/ceres.h" +#include "Eigen/Core" +#include "smac_planner/types.hpp" +#include "smac_planner/options.hpp" + +#define EPSILON 0.0001 + +namespace smac_planner +{ +/** + * @struct smac_planner::UpsamplerCostFunction + * @brief Cost function for path upsampling with multiple terms using unconstrained + * optimization including curvature, smoothness, collision, and avoid obstacles. + */ +class UpsamplerCostFunction : public ceres::FirstOrderFunction +{ +public: + /** + * @brief A constructor for smac_planner::UpsamplerCostFunction + * @param num_points Number of path points to consider + */ + UpsamplerCostFunction( + const std::vector & path, + const SmootherParams & params, + const int & upsample_ratio) + : _num_params(2 * path.size()), + _params(params), + _upsample_ratio(upsample_ratio), + _path(path) + { + } + // TODO(stevemacenski) removed upsample_ratio because temp upsampling on path size + + /** + * @struct CurvatureComputations + * @brief Cache common computations between the curvature terms to minimize recomputations + */ + struct CurvatureComputations + { + /** + * @brief A constructor for smac_planner::CurvatureComputations + */ + CurvatureComputations() + { + valid = false; + } + + bool valid; + /** + * @brief Check if result is valid for penalty + * @return is valid (non-nan, non-inf, and turning angle > max) + */ + bool isValid() + { + return valid; + } + + Eigen::Vector2d delta_xi{0, 0}; + Eigen::Vector2d delta_xi_p{0, 0}; + double delta_xi_norm{0}; + double delta_xi_p_norm{0}; + double delta_phi_i{0}; + double turning_rad{0}; + double ki_minus_kmax{0}; + }; + + /** + * @brief Smoother cost function evaluation + * @param parameters X,Y pairs of points + * @param cost total cost of path + * @param gradient of path at each X,Y pair from cost function derived analytically + * @return if successful in computing values + */ + virtual bool Evaluate( + const double * parameters, + double * cost, + double * gradient) const + { + Eigen::Vector2d xi; + Eigen::Vector2d xi_p1; + Eigen::Vector2d xi_m1; + uint x_index, y_index; + cost[0] = 0.0; + double cost_raw = 0.0; + double grad_x_raw = 0.0; + double grad_y_raw = 0.0; + + // cache some computations between the residual and jacobian + CurvatureComputations curvature_params; + + for (int i = 0; i != NumParameters() / 2; i++) { + x_index = 2 * i; + y_index = 2 * i + 1; + gradient[x_index] = 0.0; + gradient[y_index] = 0.0; + if (i < 1 || i >= NumParameters() / 2 - 1) { + continue; + } + + // if original point's neighbors TODO + if (i % _upsample_ratio == 1) { + continue; + } + + xi = Eigen::Vector2d(parameters[x_index], parameters[y_index]); + + // TODO(stevemacenski): from deep copy to make sure no feedback _path + xi_p1 = _path.at(i + 1); + xi_m1 = _path.at(i - 1); + // xi_p1 = Eigen::Vector2d(parameters[x_index + 2], parameters[y_index + 2]); + // xi_m1 = Eigen::Vector2d(parameters[x_index - 2], parameters[y_index - 2]); + + // compute cost + addSmoothingResidual(15000, xi, xi_p1, xi_m1, cost_raw); + addCurvatureResidual(60.0, xi, xi_p1, xi_m1, curvature_params, cost_raw); + + if (gradient != NULL) { + // compute gradient + addSmoothingJacobian(15000, xi, xi_p1, xi_m1, grad_x_raw, grad_y_raw); + addCurvatureJacobian(60.0, xi, xi_p1, xi_m1, curvature_params, grad_x_raw, grad_y_raw); + + gradient[x_index] = grad_x_raw; + gradient[y_index] = grad_y_raw; + } + } + + cost[0] = cost_raw; + return true; + } + + /** + * @brief Get number of parameter blocks + * @return Number of parameters in cost function + */ + virtual int NumParameters() const {return _num_params;} + +protected: + /** + * @brief Cost function term for smooth paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param r Residual (cost) of term + */ + inline void addSmoothingResidual( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + double & r) const + { + r += weight * ( + pt_p.dot(pt_p) - + 4 * pt_p.dot(pt) + + 2 * pt_p.dot(pt_m) + + 4 * pt.dot(pt) - + 4 * pt.dot(pt_m) + + pt_m.dot(pt_m)); // objective function value + } + + /** + * @brief Cost function derivative term for smooth paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param j0 Gradient of X term + * @param j1 Gradient of Y term + */ + inline void addSmoothingJacobian( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + double & j0, + double & j1) const + { + j0 += weight * + (-4 * pt_m[0] + 8 * pt[0] - 4 * pt_p[0]); // xi x component of partial-derivative + j1 += weight * + (-4 * pt_m[1] + 8 * pt[1] - 4 * pt_p[1]); // xi y component of partial-derivative + } + + /** + * @brief Get path curvature information + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param curvature_params A struct to cache computations for the jacobian to use + */ + inline void getCurvatureParams( + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + CurvatureComputations & curvature_params) const + { + curvature_params.valid = true; + curvature_params.delta_xi = Eigen::Vector2d(pt[0] - pt_m[0], pt[1] - pt_m[1]); + curvature_params.delta_xi_p = Eigen::Vector2d(pt_p[0] - pt[0], pt_p[1] - pt[1]); + curvature_params.delta_xi_norm = curvature_params.delta_xi.norm(); + curvature_params.delta_xi_p_norm = curvature_params.delta_xi_p.norm(); + + if (curvature_params.delta_xi_norm < EPSILON || curvature_params.delta_xi_p_norm < EPSILON || + std::isnan(curvature_params.delta_xi_p_norm) || std::isnan(curvature_params.delta_xi_norm) || + std::isinf(curvature_params.delta_xi_p_norm) || std::isinf(curvature_params.delta_xi_norm)) + { + // ensure we have non-nan values returned + curvature_params.valid = false; + return; + } + + const double & delta_xi_by_xi_p = + curvature_params.delta_xi_norm * curvature_params.delta_xi_p_norm; + double projection = + curvature_params.delta_xi.dot(curvature_params.delta_xi_p) / delta_xi_by_xi_p; + if (fabs(1 - projection) < EPSILON || fabs(projection + 1) < EPSILON) { + projection = 1.0; + } + + curvature_params.delta_phi_i = std::acos(projection); + curvature_params.turning_rad = curvature_params.delta_phi_i / curvature_params.delta_xi_norm; + + curvature_params.ki_minus_kmax = curvature_params.turning_rad - _upsample_ratio * + _params.max_curvature; + // TODO(stevemacenski) is use of upsample_ratio correct here? small number? + // TODO(stevemacenski) can remove the subtraction with a + // lower weight value, does have direction issue, maybe just tuning? + + if (curvature_params.ki_minus_kmax <= EPSILON) { + // Quadratic penalty need not apply + curvature_params.valid = false; + } + } + + /** + * @brief Cost function term for maximum curved paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param curvature_params A struct to cache computations for the jacobian to use + * @param r Residual (cost) of term + */ + inline void addCurvatureResidual( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + CurvatureComputations & curvature_params, + double & r) const + { + getCurvatureParams(pt, pt_p, pt_m, curvature_params); + + if (!curvature_params.isValid()) { + return; + } + + r += weight * + curvature_params.ki_minus_kmax * curvature_params.ki_minus_kmax; // objective function value + } + + /** + * @brief Cost function derivative term for maximum curvature paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param curvature_params A struct with cached values to speed up Jacobian computation + * @param j0 Gradient of X term + * @param j1 Gradient of Y term + */ + inline void addCurvatureJacobian( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & /*pt_m*/, + CurvatureComputations & curvature_params, + double & j0, + double & j1) const + { + if (!curvature_params.isValid()) { + return; + } + + const double & partial_delta_phi_i_wrt_cost_delta_phi_i = + -1 / std::sqrt(1 - std::pow(std::cos(curvature_params.delta_phi_i), 2)); + // const Eigen::Vector2d ones = Eigen::Vector2d(1.0, 1.0); + auto neg_pt_plus = -1 * pt_p; + Eigen::Vector2d p1 = normalizedOrthogonalComplement( + pt, neg_pt_plus, curvature_params.delta_xi_norm, curvature_params.delta_xi_p_norm); + Eigen::Vector2d p2 = normalizedOrthogonalComplement( + neg_pt_plus, pt, curvature_params.delta_xi_p_norm, curvature_params.delta_xi_norm); + + const double & u = 2 * curvature_params.ki_minus_kmax; + const double & common_prefix = + (1 / curvature_params.delta_xi_norm) * partial_delta_phi_i_wrt_cost_delta_phi_i; + const double & common_suffix = curvature_params.delta_phi_i / + (curvature_params.delta_xi_norm * curvature_params.delta_xi_norm); + const Eigen::Vector2d & d_delta_xi_d_xi = curvature_params.delta_xi / + curvature_params.delta_xi_norm; + + const Eigen::Vector2d jacobian = u * + (common_prefix * (-p1 - p2) - (common_suffix * d_delta_xi_d_xi)); + const Eigen::Vector2d jacobian_im1 = u * + (common_prefix * p2 + (common_suffix * d_delta_xi_d_xi)); + const Eigen::Vector2d jacobian_ip1 = u * (common_prefix * p1); + j0 += weight * jacobian[0]; // xi y component of partial-derivative + j1 += weight * jacobian[1]; // xi x component of partial-derivative + // j0 += weight * + // (jacobian_im1[0] + 2 * jacobian[0] + jacobian_ip1[0]); + // j1 += weight * + // (jacobian_im1[1] + 2 * jacobian[1] + jacobian_ip1[1]); + } + + /** + * @brief Computing the normalized orthogonal component of 2 vectors + * @param a Vector + * @param b Vector + * @param norm a Vector's norm + * @param norm b Vector's norm + * @return Normalized vector of orthogonal components + */ + inline Eigen::Vector2d normalizedOrthogonalComplement( + const Eigen::Vector2d & a, + const Eigen::Vector2d & b, + const double & a_norm, + const double & b_norm) const + { + return (a - (a.dot(b) * b / b.squaredNorm())) / (a_norm * b_norm); + } + + int _num_params; + SmootherParams _params; + int _upsample_ratio; + std::vector _path; +}; + +} // namespace smac_planner + +#endif // DEPRECATED_UPSAMPLER__UPSAMPLER_COST_FUNCTION_HPP_ diff --git a/smac_planner/test/deprecated_upsampler/upsampler_cost_function_nlls.hpp b/smac_planner/test/deprecated_upsampler/upsampler_cost_function_nlls.hpp new file mode 100644 index 0000000000..a6fd595f96 --- /dev/null +++ b/smac_planner/test/deprecated_upsampler/upsampler_cost_function_nlls.hpp @@ -0,0 +1,334 @@ +// 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_UPSAMPLER__UPSAMPLER_COST_FUNCTION_NLLS_HPP_ +#define DEPRECATED_UPSAMPLER__UPSAMPLER_COST_FUNCTION_NLLS_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "ceres/ceres.h" +#include "Eigen/Core" +#include "smac_planner/types.hpp" +#include "smac_planner/options.hpp" + +#define EPSILON 0.0001 + +namespace smac_planner +{ +/** + * @struct smac_planner::UpsamplerConstrainedCostFunction + * @brief Cost function for path upsampling with multiple terms using NLLS + * including curvature, smoothness, collision, and avoid obstacles. + */ +class UpsamplerConstrainedCostFunction : public ceres::SizedCostFunction<1, 1, 1> +{ +public: + /** + * @brief A constructor for smac_planner::UpsamplerConstrainedCostFunction + * @param num_points Number of path points to consider + */ + UpsamplerConstrainedCostFunction( + const std::vector & path, + const SmootherParams & params, + const int & upsample_ratio, + const int & i) + : _path(path), + _params(params), + _upsample_ratio(upsample_ratio), + index(i) + { + } + + /** + * @struct CurvatureComputations + * @brief Cache common computations between the curvature terms to minimize recomputations + */ + struct CurvatureComputations + { + /** + * @brief A constructor for smac_planner::CurvatureComputations + */ + CurvatureComputations() + { + valid = true; + } + + bool valid; + /** + * @brief Check if result is valid for penalty + * @return is valid (non-nan, non-inf, and turning angle > max) + */ + bool isValid() + { + return valid; + } + + Eigen::Vector2d delta_xi{0, 0}; + Eigen::Vector2d delta_xi_p{0, 0}; + double delta_xi_norm{0}; + double delta_xi_p_norm{0}; + double delta_phi_i{0}; + double turning_rad{0}; + double ki_minus_kmax{0}; + }; + + /** + * @brief Smoother cost function evaluation + * @param parameters X,Y pairs of points + * @param cost total cost of path + * @param gradient of path at each X,Y pair from cost function derived analytically + * @return if successful in computing values + */ + + bool Evaluate( + double const * const * parameters, + double * residuals, + double ** jacobians) const override + { + Eigen::Vector2d xi = Eigen::Vector2d(parameters[0][0], parameters[1][0]); + Eigen::Vector2d xi_p1 = _path.at(index + 1); + Eigen::Vector2d xi_m1 = _path.at(index - 1); + CurvatureComputations curvature_params; + double grad_x_raw = 0, grad_y_raw = 0, cost_raw = 0; + + // compute cost + addSmoothingResidual(15000, xi, xi_p1, xi_m1, cost_raw); + addCurvatureResidual(60.0, xi, xi_p1, xi_m1, curvature_params, cost_raw); + + residuals[0] = 0; + residuals[0] = cost_raw; // objective function value x + + if (jacobians != NULL && jacobians[0] != NULL) { + addSmoothingJacobian(15000, xi, xi_p1, xi_m1, grad_x_raw, grad_y_raw); + addCurvatureJacobian(60.0, xi, xi_p1, xi_m1, curvature_params, grad_x_raw, grad_y_raw); + + jacobians[0][0] = 0; + jacobians[1][0] = 0; + jacobians[0][0] = grad_x_raw; // x derivative + jacobians[1][0] = grad_y_raw; // y derivative + jacobians[0][1] = 0.0; + jacobians[1][1] = 0.0; + } + + return true; + } + +protected: + /** + * @brief Cost function term for smooth paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param r Residual (cost) of term + */ + inline void addSmoothingResidual( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + double & r) const + { + r += weight * ( + pt_p.dot(pt_p) - + 4 * pt_p.dot(pt) + + 2 * pt_p.dot(pt_m) + + 4 * pt.dot(pt) - + 4 * pt.dot(pt_m) + + pt_m.dot(pt_m)); // objective function value + } + + /** + * @brief Cost function derivative term for smooth paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param j0 Gradient of X term + * @param j1 Gradient of Y term + */ + inline void addSmoothingJacobian( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + double & j0, + double & j1) const + { + j0 += weight * + (-4 * pt_m[0] + 8 * pt[0] - 4 * pt_p[0]); // xi x component of partial-derivative + j1 += weight * + (-4 * pt_m[1] + 8 * pt[1] - 4 * pt_p[1]); // xi y component of partial-derivative + } + + /** + * @brief Get path curvature information + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param curvature_params A struct to cache computations for the jacobian to use + */ + inline void getCurvatureParams( + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + CurvatureComputations & curvature_params) const + { + curvature_params.valid = true; + curvature_params.delta_xi = Eigen::Vector2d(pt[0] - pt_m[0], pt[1] - pt_m[1]); + curvature_params.delta_xi_p = Eigen::Vector2d(pt_p[0] - pt[0], pt_p[1] - pt[1]); + curvature_params.delta_xi_norm = curvature_params.delta_xi.norm(); + curvature_params.delta_xi_p_norm = curvature_params.delta_xi_p.norm(); + + if (curvature_params.delta_xi_norm < EPSILON || curvature_params.delta_xi_p_norm < EPSILON || + std::isnan(curvature_params.delta_xi_p_norm) || std::isnan(curvature_params.delta_xi_norm) || + std::isinf(curvature_params.delta_xi_p_norm) || std::isinf(curvature_params.delta_xi_norm)) + { + // ensure we have non-nan values returned + curvature_params.valid = false; + return; + } + + const double & delta_xi_by_xi_p = + curvature_params.delta_xi_norm * curvature_params.delta_xi_p_norm; + double projection = + curvature_params.delta_xi.dot(curvature_params.delta_xi_p) / delta_xi_by_xi_p; + if (fabs(1 - projection) < EPSILON || fabs(projection + 1) < EPSILON) { + projection = 1.0; + } + + curvature_params.delta_phi_i = std::acos(projection); + curvature_params.turning_rad = curvature_params.delta_phi_i / curvature_params.delta_xi_norm; + + curvature_params.ki_minus_kmax = curvature_params.turning_rad - _upsample_ratio * + _params.max_curvature; + + if (curvature_params.ki_minus_kmax <= EPSILON) { + // Quadratic penalty need not apply + curvature_params.valid = false; + } + } + + /** + * @brief Cost function term for maximum curved paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param curvature_params A struct to cache computations for the jacobian to use + * @param r Residual (cost) of term + */ + inline void addCurvatureResidual( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + CurvatureComputations & curvature_params, + double & r) const + { + getCurvatureParams(pt, pt_p, pt_m, curvature_params); + + if (!curvature_params.isValid()) { + return; + } + + // objective function value + r += weight * + curvature_params.ki_minus_kmax * curvature_params.ki_minus_kmax; + } + + /** + * @brief Cost function derivative term for maximum curvature paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param curvature_params A struct with cached values to speed up Jacobian computation + * @param j0 Gradient of X term + * @param j1 Gradient of Y term + */ + inline void addCurvatureJacobian( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & /*pt_m*/, + CurvatureComputations & curvature_params, + double & j0, + double & j1) const + { + if (!curvature_params.isValid()) { + return; + } + + const double & partial_delta_phi_i_wrt_cost_delta_phi_i = + -1 / std::sqrt(1 - std::pow(std::cos(curvature_params.delta_phi_i), 2)); + // const Eigen::Vector2d ones = Eigen::Vector2d(1.0, 1.0); + auto neg_pt_plus = -1 * pt_p; + Eigen::Vector2d p1 = normalizedOrthogonalComplement( + pt, neg_pt_plus, curvature_params.delta_xi_norm, curvature_params.delta_xi_p_norm); + Eigen::Vector2d p2 = normalizedOrthogonalComplement( + neg_pt_plus, pt, curvature_params.delta_xi_p_norm, curvature_params.delta_xi_norm); + + const double & u = 2 * curvature_params.ki_minus_kmax; + const double & common_prefix = + (1 / curvature_params.delta_xi_norm) * partial_delta_phi_i_wrt_cost_delta_phi_i; + const double & common_suffix = curvature_params.delta_phi_i / + (curvature_params.delta_xi_norm * curvature_params.delta_xi_norm); + const Eigen::Vector2d & d_delta_xi_d_xi = curvature_params.delta_xi / + curvature_params.delta_xi_norm; + + const Eigen::Vector2d jacobian = u * + (common_prefix * (-p1 - p2) - (common_suffix * d_delta_xi_d_xi)); + const Eigen::Vector2d jacobian_im1 = u * + (common_prefix * p2 + (common_suffix * d_delta_xi_d_xi)); + const Eigen::Vector2d jacobian_ip1 = u * (common_prefix * p1); + j0 += weight * jacobian[0]; // xi x component of partial-derivative + j1 += weight * jacobian[1]; // xi y component of partial-derivative + // j0 += weight * + // (jacobian_im1[0] + 2 * jacobian[0] + jacobian_ip1[0]); + // j1 += weight * + // (jacobian_im1[1] + 2 * jacobian[1] + jacobian_ip1[1]); + } + /** + * @brief Computing the normalized orthogonal component of 2 vectors + * @param a Vector + * @param b Vector + * @param norm a Vector's norm + * @param norm b Vector's norm + * @return Normalized vector of orthogonal components + */ + inline Eigen::Vector2d normalizedOrthogonalComplement( + const Eigen::Vector2d & a, + const Eigen::Vector2d & b, + const double & a_norm, + const double & b_norm) const + { + return (a - (a.dot(b) * b / b.squaredNorm())) / (a_norm * b_norm); + } + + std::vector _path; + SmootherParams _params; + int _upsample_ratio; + int index; +}; + +} // namespace smac_planner + +#endif // DEPRECATED_UPSAMPLER__UPSAMPLER_COST_FUNCTION_NLLS_HPP_ diff --git a/smac_planner/test/path.png b/smac_planner/test/path.png new file mode 100644 index 0000000000..fc98e7709b Binary files /dev/null and b/smac_planner/test/path.png differ diff --git a/smac_planner/test/test_a_star.cpp b/smac_planner/test/test_a_star.cpp new file mode 100644 index 0000000000..a1e5dcf208 --- /dev/null +++ b/smac_planner/test/test_a_star.cpp @@ -0,0 +1,183 @@ +// 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 "smac_planner/node_se2.hpp" +#include "smac_planner/a_star.hpp" +#include "smac_planner/collision_checker.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(AStarTest, test_a_star_2d) +{ + smac_planner::SearchInfo info; + smac_planner::AStarAlgorithm a_star(smac_planner::MotionModel::MOORE, info); + int max_iterations = 10000; + float tolerance = 0.0; + float some_tolerance = 20.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); + + nav2_costmap_2d::Costmap2D * costmapA = + new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); + // island in the middle of lethal cost to cross + for (unsigned int i = 40; i <= 60; ++i) { + for (unsigned int j = 40; j <= 60; ++j) { + costmapA->setCost(i, j, 254); + } + } + + // functional case testing + a_star.createGraph(costmapA->getSizeInCellsX(), costmapA->getSizeInCellsY(), 1, costmapA); + a_star.setStart(20u, 20u, 0); + a_star.setGoal(80u, 80u, 0); + smac_planner::Node2D::CoordinateVector path; + EXPECT_TRUE(a_star.createPath(path, num_it, tolerance)); + EXPECT_EQ(num_it, 556); + + // check path is the right size and collision free + EXPECT_EQ(path.size(), 81u); + for (unsigned int i = 0; i != path.size(); i++) { + EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); + } + + // 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); + + path.clear(); + // failure cases with invalid inputs + smac_planner::AStarAlgorithm a_star_2( + 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); + 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); + num_it = 0; + EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error); + a_star_2.setStart(50, 50, 0); // invalid + a_star_2.setGoal(0, 0, 0); // valid + num_it = 0; + EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error); + a_star_2.setStart(0, 0, 0); // valid + a_star_2.setGoal(50, 50, 0); // invalid + num_it = 0; + EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error); + num_it = 0; + // invalid goal but liberal tolerance + 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); + for (unsigned int i = 0; i != path.size(); i++) { + EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); + } + + EXPECT_TRUE(a_star_2.getStart() != nullptr); + EXPECT_TRUE(a_star_2.getGoal() != nullptr); + 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.getOnApproachMaxIterations(), 10); + + delete costmapA; +} + +TEST(AStarTest, test_a_star_se2) +{ + 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 + unsigned int size_theta = 72; + smac_planner::AStarAlgorithm a_star( + 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); + + nav2_costmap_2d::Costmap2D * costmapA = + new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); + // island in the middle of lethal cost to cross + for (unsigned int i = 40; i <= 60; ++i) { + for (unsigned int j = 40; j <= 60; ++j) { + costmapA->setCost(i, j, 254); + } + } + + // functional case testing + a_star.createGraph( + costmapA->getSizeInCellsX(), costmapA->getSizeInCellsY(), size_theta, costmapA); + a_star.setStart(10u, 10u, 0u); + a_star.setGoal(80u, 80u, 40u); + smac_planner::NodeSE2::CoordinateVector path; + EXPECT_TRUE(a_star.createPath(path, num_it, tolerance)); + + // check path is the right size and collision free + EXPECT_EQ(num_it, 61); + EXPECT_EQ(path.size(), 75u); + for (unsigned int i = 0; i != path.size(); i++) { + EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); + } + + delete costmapA; +} + +TEST(AStarTest, test_constants) +{ + smac_planner::MotionModel mm = smac_planner::MotionModel::UNKNOWN; // unknown + EXPECT_EQ(smac_planner::toString(mm), std::string("Unknown")); + mm = smac_planner::MotionModel::VON_NEUMANN; // vonneumann + EXPECT_EQ(smac_planner::toString(mm), std::string("Von Neumann")); + mm = smac_planner::MotionModel::MOORE; // moore + EXPECT_EQ(smac_planner::toString(mm), std::string("Moore")); + mm = smac_planner::MotionModel::DUBIN; // dubin + EXPECT_EQ(smac_planner::toString(mm), std::string("Dubin")); + mm = smac_planner::MotionModel::REEDS_SHEPP; // reeds-shepp + EXPECT_EQ(smac_planner::toString(mm), std::string("Reeds-Shepp")); + + EXPECT_EQ(smac_planner::fromString("VON_NEUMANN"), smac_planner::MotionModel::VON_NEUMANN); + EXPECT_EQ(smac_planner::fromString("MOORE"), smac_planner::MotionModel::MOORE); + EXPECT_EQ(smac_planner::fromString("DUBIN"), smac_planner::MotionModel::DUBIN); + EXPECT_EQ(smac_planner::fromString("REEDS_SHEPP"), smac_planner::MotionModel::REEDS_SHEPP); + EXPECT_EQ(smac_planner::fromString("NONE"), smac_planner::MotionModel::UNKNOWN); +} diff --git a/smac_planner/test/test_collision_checker.cpp b/smac_planner/test/test_collision_checker.cpp new file mode 100644 index 0000000000..e6590d610d --- /dev/null +++ b/smac_planner/test/test_collision_checker.cpp @@ -0,0 +1,172 @@ +// Copyright (c) 2020 Shivang Patel +// Copyright (c) 2020 Samsung Research +// +// 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 "gtest/gtest.h" +#include "smac_planner/collision_checker.hpp" + +using namespace nav2_costmap_2d; // NOLINT + +TEST(collision_footprint, test_basic) +{ + nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0); + + geometry_msgs::msg::Point p1; + p1.x = -0.5; + p1.y = 0.0; + geometry_msgs::msg::Point p2; + p2.x = 0.0; + p2.y = 0.5; + geometry_msgs::msg::Point p3; + p3.x = 0.5; + p3.y = 0.0; + geometry_msgs::msg::Point p4; + p4.x = 0.0; + p4.y = -0.5; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; + + smac_planner::GridCollisionChecker collision_checker(costmap_); + collision_checker.setFootprint(footprint, false /*use footprint*/); + collision_checker.inCollision(5.0, 5.0, 0.0, false); + float cost = collision_checker.getCost(); + EXPECT_NEAR(cost, 0.0, 0.001); + delete costmap_; +} + +TEST(collision_footprint, test_point_cost) +{ + nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0); + + smac_planner::GridCollisionChecker collision_checker(costmap_); + nav2_costmap_2d::Footprint footprint; + collision_checker.setFootprint(footprint, true /*radius / pointcose*/); + + collision_checker.inCollision(5.0, 5.0, 0.0, false); + float cost = collision_checker.getCost(); + EXPECT_NEAR(cost, 0.0, 0.001); + delete costmap_; +} + +TEST(collision_footprint, test_world_to_map) +{ + nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0); + + smac_planner::GridCollisionChecker collision_checker(costmap_); + nav2_costmap_2d::Footprint footprint; + collision_checker.setFootprint(footprint, true /*radius / point cost*/); + + unsigned int x, y; + + collision_checker.worldToMap(1.0, 1.0, x, y); + + collision_checker.inCollision(x, y, 0.0, false); + float cost = collision_checker.getCost(); + + EXPECT_NEAR(cost, 0.0, 0.001); + + costmap_->setCost(50, 50, 200); + collision_checker.worldToMap(5.0, 5.0, x, y); + + collision_checker.inCollision(x, y, 0.0, false); + EXPECT_NEAR(collision_checker.getCost(), 200.0, 0.001); + delete costmap_; +} + +TEST(collision_footprint, test_footprint_at_pose_with_movement) +{ + nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 254); + + for (unsigned int i = 40; i <= 60; ++i) { + for (unsigned int j = 40; j <= 60; ++j) { + costmap_->setCost(i, j, 0); + } + } + + geometry_msgs::msg::Point p1; + p1.x = -1.0; + p1.y = 1.0; + geometry_msgs::msg::Point p2; + p2.x = 1.0; + p2.y = 1.0; + geometry_msgs::msg::Point p3; + p3.x = 1.0; + p3.y = -1.0; + geometry_msgs::msg::Point p4; + p4.x = -1.0; + p4.y = -1.0; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; + + smac_planner::GridCollisionChecker collision_checker(costmap_); + collision_checker.setFootprint(footprint, false /*use footprint*/); + + collision_checker.inCollision(50, 50, 0.0, false); + float cost = collision_checker.getCost(); + EXPECT_NEAR(cost, 0.0, 0.001); + + collision_checker.inCollision(50, 49, 0.0, false); + float up_value = collision_checker.getCost(); + EXPECT_NEAR(up_value, 254.0, 0.001); + + collision_checker.inCollision(50, 52, 0.0, false); + float down_value = collision_checker.getCost(); + EXPECT_NEAR(down_value, 254.0, 0.001); + delete costmap_; +} + +TEST(collision_footprint, test_point_and_line_cost) +{ + nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D( + 100, 100, 0.10000, 0, 0.0, + 0.0); + + costmap_->setCost(62, 50, 254); + costmap_->setCost(39, 60, 254); + + geometry_msgs::msg::Point p1; + p1.x = -1.0; + p1.y = 1.0; + geometry_msgs::msg::Point p2; + p2.x = 1.0; + p2.y = 1.0; + geometry_msgs::msg::Point p3; + p3.x = 1.0; + p3.y = -1.0; + geometry_msgs::msg::Point p4; + p4.x = -1.0; + p4.y = -1.0; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; + + smac_planner::GridCollisionChecker collision_checker(costmap_); + collision_checker.setFootprint(footprint, false /*use footprint*/); + + collision_checker.inCollision(50, 50, 0.0, false); + float value = collision_checker.getCost(); + EXPECT_NEAR(value, 0.0, 0.001); + + collision_checker.inCollision(49, 50, 0.0, false); + float left_value = collision_checker.getCost(); + EXPECT_NEAR(left_value, 254.0, 0.001); + + collision_checker.inCollision(52, 50, 0.0, false); + float right_value = collision_checker.getCost(); + EXPECT_NEAR(right_value, 254.0, 0.001); + delete costmap_; +} diff --git a/smac_planner/test/test_costmap_downsampler.cpp b/smac_planner/test/test_costmap_downsampler.cpp new file mode 100644 index 0000000000..040a2eb760 --- /dev/null +++ b/smac_planner/test/test_costmap_downsampler.cpp @@ -0,0 +1,70 @@ +// 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 "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 "smac_planner/costmap_downsampler.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(CostmapDownsampler, costmap_downsample_test) +{ + nav2_util::LifecycleNode::SharedPtr node = std::make_shared( + "CostmapDownsamplerTest"); + smac_planner::CostmapDownsampler downsampler(node); + auto map_sub = nav2_costmap_2d::CostmapSubscriber(node, "unused_topic"); + + // create basic costmap + nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0); + costmapA.setCost(0, 0, 100); + costmapA.setCost(5, 5, 50); + + // downsample it + downsampler.initialize("map", "unused_topic", &costmapA, 2); + nav2_costmap_2d::Costmap2D * downsampledCostmapA = downsampler.downsample(2); + + // validate it + EXPECT_EQ(downsampledCostmapA->getCost(0, 0), 100); + EXPECT_EQ(downsampledCostmapA->getCost(2, 2), 50); + EXPECT_EQ(downsampledCostmapA->getSizeInCellsX(), 5u); + EXPECT_EQ(downsampledCostmapA->getSizeInCellsY(), 5u); + + // give it another costmap of another size + nav2_costmap_2d::Costmap2D costmapB(4, 4, 0.10, 0.0, 0.0, 0); + + // downsample it + downsampler.initialize("map", "unused_topic", &costmapB, 4); + downsampler.activatePublisher(); + nav2_costmap_2d::Costmap2D * downsampledCostmapB = downsampler.downsample(4); + downsampler.deactivatePublisher(); + + // validate size + EXPECT_EQ(downsampledCostmapB->getSizeInCellsX(), 1u); + EXPECT_EQ(downsampledCostmapB->getSizeInCellsY(), 1u); + + downsampler.resizeCostmap(); +} diff --git a/smac_planner/test/test_node2d.cpp b/smac_planner/test/test_node2d.cpp new file mode 100644 index 0000000000..bc42988aca --- /dev/null +++ b/smac_planner/test/test_node2d.cpp @@ -0,0 +1,135 @@ +// 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 "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 "smac_planner/node_2d.hpp" +#include "smac_planner/collision_checker.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(Node2DTest, test_node_2d) +{ + nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0); + smac_planner::GridCollisionChecker checker(&costmapA); + + // test construction + unsigned char cost = static_cast(1); + smac_planner::Node2D testA(cost, 1); + smac_planner::Node2D testB(cost, 1); + EXPECT_EQ(testA.getCost(), 1.0f); + + // test reset + testA.reset(10); + EXPECT_EQ(testA.getCost(), 10.0f); + + // Check constants + EXPECT_EQ(testA.neutral_cost, 50.0f); + + // 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); + + // check traversal cost computation + EXPECT_EQ(testB.getTraversalCost(&testA), 58.0f); + + // check heuristic cost computation + smac_planner::Node2D::Coordinates A(0.0, 0.0); + smac_planner::Node2D::Coordinates B(10.0, 5.0); + EXPECT_NEAR(testB.getHeuristicCost(A, B), 559.016, 0.01); + + // check operator== works on index + unsigned char costC = '2'; + smac_planner::Node2D testC(costC, 1); + 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(), 1u); + + // check static index functions + EXPECT_EQ(smac_planner::Node2D::getIndex(1u, 1u, 10u), 11u); + EXPECT_EQ(smac_planner::Node2D::getIndex(6u, 43u, 10u), 436u); + EXPECT_EQ(smac_planner::Node2D::getCoords(436u, 10u, 1u).x, 6u); + EXPECT_EQ(smac_planner::Node2D::getCoords(436u, 10u, 1u).y, 43u); + EXPECT_THROW(smac_planner::Node2D::getCoords(436u, 10u, 10u), std::runtime_error); +} + +TEST(Node2DTest, test_node_2d_neighbors) +{ + // test neighborhood computation + smac_planner::Node2D::initNeighborhood(10u, smac_planner::MotionModel::VON_NEUMANN); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets.size(), 4u); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[0], -1); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[1], 1); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[2], -10); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[3], 10); + + smac_planner::Node2D::initNeighborhood(100u, smac_planner::MotionModel::MOORE); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets.size(), 8u); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[0], -1); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[1], 1); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[2], -100); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[3], 100); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[4], -101); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[5], -99); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[6], 99); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[7], 101); + + nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0); + smac_planner::GridCollisionChecker checker(&costmapA); + unsigned char cost = static_cast(1); + smac_planner::Node2D * node = new smac_planner::Node2D(cost, 1); + std::function neighborGetter = + [&, this](const unsigned int & index, smac_planner::Node2D * & neighbor_rtn) -> bool + { + return true; + }; + + smac_planner::Node2D::NodeVector neighbors; + smac_planner::Node2D::getNeighbors(node, neighborGetter, checker, false, neighbors); + delete node; + + // should be empty since totally invalid + EXPECT_EQ(neighbors.size(), 0u); +} diff --git a/smac_planner/test/test_nodebasic.cpp b/smac_planner/test/test_nodebasic.cpp new file mode 100644 index 0000000000..0ec90a112f --- /dev/null +++ b/smac_planner/test/test_nodebasic.cpp @@ -0,0 +1,49 @@ +// 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 "smac_planner/node_basic.hpp" +#include "smac_planner/node_2d.hpp" +#include "smac_planner/node_se2.hpp" +#include "smac_planner/collision_checker.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(NodeBasicTest, test_node_basic) +{ + smac_planner::NodeBasic node(50); + + EXPECT_EQ(node.index, 50u); + EXPECT_EQ(node.graph_node_ptr, nullptr); + + smac_planner::NodeBasic node2(100); + + EXPECT_EQ(node2.index, 100u); + EXPECT_EQ(node2.graph_node_ptr, nullptr); +} diff --git a/smac_planner/test/test_nodese2.cpp b/smac_planner/test/test_nodese2.cpp new file mode 100644 index 0000000000..2f23c12aaf --- /dev/null +++ b/smac_planner/test/test_nodese2.cpp @@ -0,0 +1,214 @@ +// 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 "smac_planner/node_se2.hpp" +#include "smac_planner/collision_checker.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(NodeSE2Test, test_node_se2) +{ + 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; + + smac_planner::NodeSE2::initMotionModel( + 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); + smac_planner::GridCollisionChecker checker(costmapA); + checker.setFootprint(nav2_costmap_2d::Footprint(), true); + + // test construction + smac_planner::NodeSE2 testA(49); + 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 + smac_planner::NodeSE2::computeWavefrontHeuristic( + costmapA, + static_cast(10.0), + static_cast(5.0), + 0.0, 0.0); + smac_planner::NodeSE2::Coordinates A(0.0, 0.0, 4.2); + 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 + 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(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(smac_planner::NodeSE2::getIndex(1u, 1u, 4u, 10u, 72u), 796u); + EXPECT_EQ(smac_planner::NodeSE2::getCoords(796u, 10u, 72u).x, 1u); + EXPECT_EQ(smac_planner::NodeSE2::getCoords(796u, 10u, 72u).y, 1u); + EXPECT_EQ(smac_planner::NodeSE2::getCoords(796u, 10u, 72u).theta, 4u); + + delete costmapA; +} + +TEST(NodeSE2Test, test_node_2d_neighbors) +{ + 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; + smac_planner::NodeSE2::initMotionModel( + smac_planner::MotionModel::DUBIN, size_x, size_y, size_theta, info); + + + // test neighborhood computation + EXPECT_EQ(smac_planner::NodeSE2::motion_table.projections.size(), 3u); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[0]._x, 1.731517, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[0]._y, 0, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[0]._theta, 0, 0.01); + + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[1]._x, 1.69047, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[1]._y, 0.3747, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[1]._theta, 5, 0.01); + + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[2]._x, 1.69047, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[2]._y, -0.3747, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[2]._theta, -5, 0.01); + + smac_planner::NodeSE2::initMotionModel( + smac_planner::MotionModel::REEDS_SHEPP, size_x, size_y, size_theta, info); + + EXPECT_EQ(smac_planner::NodeSE2::motion_table.projections.size(), 6u); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[0]._x, 1.731517, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[0]._y, 0, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[0]._theta, 0, 0.01); + + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[1]._x, 1.69047, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[1]._y, 0.3747, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[1]._theta, 5, 0.01); + + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[2]._x, 1.69047, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[2]._y, -0.3747, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[2]._theta, -5, 0.01); + + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[3]._x, -1.731517, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[3]._y, 0, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[3]._theta, 0, 0.01); + + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[4]._x, -1.69047, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[4]._y, 0.3747, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[4]._theta, -5, 0.01); + + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[5]._x, -1.69047, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[5]._y, -0.3747, 0.01); + EXPECT_NEAR(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); + smac_planner::GridCollisionChecker checker(&costmapA); + smac_planner::NodeSE2 * node = new smac_planner::NodeSE2(49); + std::function neighborGetter = + [&, this](const unsigned int & index, smac_planner::NodeSE2 * & neighbor_rtn) -> bool + { + // because we don't return a real object + return false; + }; + + smac_planner::NodeSE2::NodeVector neighbors; + 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/smac_planner/test/test_smac_2d.cpp b/smac_planner/test/test_smac_2d.cpp new file mode 100644 index 0000000000..63b4ba9987 --- /dev/null +++ b/smac_planner/test/test_smac_2d.cpp @@ -0,0 +1,80 @@ +// 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 "geometry_msgs/msg/pose_stamped.hpp" +#include "smac_planner/node_se2.hpp" +#include "smac_planner/a_star.hpp" +#include "smac_planner/collision_checker.hpp" +#include "smac_planner/smac_planner.hpp" +#include "smac_planner/smac_planner_2d.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +// SMAC smoke tests for plugin-level issues rather than algorithms +// (covered by more extensively testing in other files) +// System tests in nav2_system_tests will actually plan with this work + +TEST(SmacTest, test_smac_2d) +{ + rclcpp_lifecycle::LifecycleNode::SharedPtr node2D = + std::make_shared("Smac2DTest"); + + std::shared_ptr costmap_ros = + std::make_shared("global_costmap"); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + + node2D->declare_parameter("test.smooth_path", true); + node2D->set_parameter(rclcpp::Parameter("test.smooth_path", true)); + node2D->declare_parameter("test.downsample_costmap", true); + node2D->set_parameter(rclcpp::Parameter("test.downsample_costmap", true)); + node2D->declare_parameter("test.downsampling_factor", 2); + node2D->set_parameter(rclcpp::Parameter("test.downsampling_factor", 2)); + + 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(node2D, "test", nullptr, costmap_ros); + planner_2d->activate(); + try { + planner_2d->createPlan(start, goal); + } catch (...) { + } + + planner_2d->deactivate(); + planner_2d->cleanup(); + + planner_2d.reset(); + costmap_ros->on_cleanup(rclcpp_lifecycle::State()); + node2D.reset(); + costmap_ros.reset(); +} diff --git a/smac_planner/test/test_smac_se2.cpp b/smac_planner/test/test_smac_se2.cpp new file mode 100644 index 0000000000..caf7d3381e --- /dev/null +++ b/smac_planner/test/test_smac_se2.cpp @@ -0,0 +1,92 @@ +// 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 "geometry_msgs/msg/pose_stamped.hpp" +#include "smac_planner/node_se2.hpp" +#include "smac_planner/a_star.hpp" +#include "smac_planner/collision_checker.hpp" +#include "smac_planner/smac_planner.hpp" +#include "smac_planner/smac_planner_2d.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +// SMAC smoke tests for plugin-level issues rather than algorithms +// (covered by more extensively testing in other files) +// System tests in nav2_system_tests will actually plan with this work + +TEST(SmacTest, test_smac_se2) +{ + rclcpp_lifecycle::LifecycleNode::SharedPtr nodeSE2 = + std::make_shared("SmacSE2Test"); + + std::shared_ptr costmap_ros = + 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); + nodeSE2->set_parameter(rclcpp::Parameter("test.downsampling_factor", 2)); + + 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 = std::make_unique(); + planner->configure(nodeSE2, "test", nullptr, costmap_ros); + planner->activate(); + + try { + planner->createPlan(start, goal); + } catch (...) { + } + + planner->deactivate(); + planner->cleanup(); + + planner.reset(); + costmap_ros->on_cleanup(rclcpp_lifecycle::State()); + 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(smac_planner::squaredDistance(p1, p2), 1.0, 0.001); +} diff --git a/smac_planner/test/test_smoother.cpp b/smac_planner/test/test_smoother.cpp new file mode 100644 index 0000000000..ab05a93959 --- /dev/null +++ b/smac_planner/test/test_smoother.cpp @@ -0,0 +1,104 @@ +// 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 "smac_planner/a_star.hpp" +#include "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 + 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; + smac_planner::AStarAlgorithm a_star( + 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); + 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)); + } + + smac_planner::OptimizerParams params; + params.debug = true; + params.get(node2D.get(), "test"); + + 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; + + 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/tools/ros2_dependencies.repos b/tools/ros2_dependencies.repos index b6645197b0..73d6b1f1e2 100644 --- a/tools/ros2_dependencies.repos +++ b/tools/ros2_dependencies.repos @@ -19,3 +19,11 @@ repositories: type: git url: https://github.com/ros-perception/vision_opencv.git version: ros2 + ros/bond_core: + type: git + url: https://github.com/ros/bond_core.git + version: ros2 + ompl/ompl: + type: git + url: https://github.com/ompl/ompl.git + version: 1.5.0