Foxy sync smac planner

doc/parameters/param_list.md

@@ -166,7 +166,7 @@ When `plugins` parameter is not overridden, the following default plugins are lo
 | `<data source>`.marking | true | Whether source should mark in costmap |
 | `<data source>`.clearing | false | Whether source should raytrace clear in costmap |
 | `<data source>`.obstacle_range | 2.5 | Maximum range to mark obstacles in costmap |
-| `<data source>`.raytrace_range | 3.0 | Maximum range to raytrace clear obstacles from costmap | 
+| `<data source>`.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
 | `<dwb plugin>`.`<name>`.x_only_threshold | 0.05 | Threshold to check in the X velocity direction |
 | `<dwb plugin>`.`<name>`.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
 | `<dwb plugin>`.decel_lim_y | 0.0 | Maximum deceleration Y (m/s^2) |
 | `<dwb plugin>`.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
 | `<name>`.use_astar | false | Whether to use A*, if false, uses Dijstra's expansion |
 | `<name>`.allow_unknown | true | Whether to allow planning in unknown space |
 
+# smac_planner
+
+* `<name>`: Corresponding planner plugin ID for this type
+
+| Parameter | Default | Description |
+| ----------| --------| ------------|
+| `<name>`.tolerance  | 0.5 | Tolerance in meters between requested goal pose and end of path |
+| `<name>`.downsample_costmap | false | Whether to downsample costmap |
+| `<name>`.downsampling_factor | 1 | Factor to downsample costmap by |
+| `<name>`.allow_unknown | true | whether to allow traversing in unknown space |
+| `<name>`.max_iterations | -1 | Number of iterations before failing, disabled by -1 |
+| `<name>`.max_on_approach_iterations | 1000 | Iterations after within threshold before returning approximate path with best heuristic |
+| `<name>`.max_planning_time_ms | 5000 | Maximum planning time in ms |
+| `<name>`.smooth_path | false | Whether to smooth path with CG smoother |
+| `<name>`.motion_model_for_search | DUBIN | Motion model to search with. Options for SE2: DUBIN, REEDS_SHEPP. 2D: MOORE, VON_NEUMANN |
+| `<name>`.angle_quantization_bins | 1 | Number of angle quantization bins for SE2 node |
+| `<name>`.minimum_turning_radius | 0.20 | Minimum turning radius in m of vehicle or desired path |
+| `<name>`.reverse_penalty | 2.0 | Penalty to apply to SE2 node if in reverse direction |
+| `<name>`.change_penalty | 0.5 | Penalty to apply to SE2 node if changing direction |
+| `<name>`.non_straight_penalty | 1.1 | Penalty to apply to SE2 node if non-straight direction |
+| `<name>`.cost_penalty | 1.2 | Penalty to apply to SE2 node for cost at pose |
+| `<name>`.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 |
+| `<name>`.smoother.smoother.w_curve | 1.5 | Smoother weight on curvature of path |
+| `<name>`.smoother.smoother.w_dist | 0.0 | Smoother weight on distance from original path |
+| `<name>`.smoother.smoother.w_smooth | 15000 | Smoother weight on distance between nodes |
+| `<name>`.smoother.smoother.w_cost | 1.5 | Smoother weight on costmap cost |
+| `<name>`.smoother.smoother.cost_scaling_factor | 10.0 | Inflation layer's scale factor |
+| `<name>`.smoother.optimizer.max_time | 0.10 | Maximum time to spend smoothing, in seconds |
+| `<name>`.smoother.optimizer.max_iterations | 500 | Maximum number of iterations to spend smoothing |
+| `<name>`.smoother.optimizer.debug_optimizer | false | Whether to print debug info from Ceres |
+| `<name>`.smoother.optimizer.gradient_tol | 1e-10 | Minimum change in gradient to terminate smoothing |
+| `<name>`.smoother.optimizer.fn_tol | 1e-7 | Minimum change in function to terminate smoothing |
+| `<name>`.smoother.optimizer.param_tol | 1e-15 | Minimum change in parameter block to terminate smoothing |
+
+| `<name>`.smoother.optimizer.advanced.min_line_search_step_size | 1e-20 | Terminate smoothing iteration if change in parameter block less than this |
+| `<name>`.smoother.optimizer.advanced.max_num_line_search_step_size_iterations | 50 | Maximum iterations for line search |
+| `<name>`.smoother.optimizer.advanced.line_search_sufficient_function_decrease | 1e-20 | Function decrease amount to terminate current line search iteration |
+| `<name>`.smoother.optimizer.advanced.max_num_line_search_direction_restarts | 10 | Maximum umber of restarts of line search when over-estimating |
+| `<name>`.smoother.optimizer.advanced.max_line_search_step_expansion | 50 | Step size multiplier at each iteration of line search |
+
 # waypoint_follower
 
 | Parameter | Default | Description |

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

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

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:

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

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

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp

@@ -248,6 +248,14 @@ class Costmap2DROS : public nav2_util::LifecycleNode
 
   std::shared_ptr<tf2_ros::Buffer> 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_;
 

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<std::shared_ptr<Costmap2D>> collision_checker_;
 };
 
 }  // namespace nav2_costmap_2d

nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp

@@ -32,20 +32,21 @@ namespace nav2_costmap_2d
 {
 typedef std::vector<geometry_msgs::msg::Point> Footprint;
 
+template<typename CostmapT>
 class FootprintCollisionChecker
 {
 public:
   FootprintCollisionChecker();
-  explicit FootprintCollisionChecker(std::shared_ptr<nav2_costmap_2d::Costmap2D> 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<nav2_costmap_2d::Costmap2D> costmap);
+  void setCostmap(CostmapT costmap);
 
-private:
-  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_;
+protected:
+  CostmapT costmap_;
 };
 
 }  // namespace nav2_costmap_2d

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;

nav2_costmap_2d/src/footprint_collision_checker.cpp

@@ -31,18 +31,21 @@ using namespace std::chrono_literals;
 namespace nav2_costmap_2d
 {
 
-FootprintCollisionChecker::FootprintCollisionChecker()
+template<typename CostmapT>
+FootprintCollisionChecker<CostmapT>::FootprintCollisionChecker()
 : costmap_(nullptr)
 {
 }
 
-FootprintCollisionChecker::FootprintCollisionChecker(
-  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap)
+template<typename CostmapT>
+FootprintCollisionChecker<CostmapT>::FootprintCollisionChecker(
+  CostmapT costmap)
 : costmap_(costmap)
 {
 }
 
-double FootprintCollisionChecker::footprintCost(const Footprint footprint)
+template<typename CostmapT>
+double FootprintCollisionChecker<CostmapT>::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<typename CostmapT>
+double FootprintCollisionChecker<CostmapT>::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<typename CostmapT>
+bool FootprintCollisionChecker<CostmapT>::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<typename CostmapT>
+double FootprintCollisionChecker<CostmapT>::pointCost(int x, int y) const
 {
   return costmap_->getCost(x, y);
 }
 
-void FootprintCollisionChecker::setCostmap(std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap)
+template<typename CostmapT>
+void FootprintCollisionChecker<CostmapT>::setCostmap(CostmapT costmap)
 {
   costmap_ = costmap;
 }
 
-double FootprintCollisionChecker::footprintCostAtPose(
+template<typename CostmapT>
+double FootprintCollisionChecker<CostmapT>::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<std::shared_ptr<nav2_costmap_2d::Costmap2D>>;
+template class FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>;
+
 }  // namespace nav2_costmap_2d

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<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
+  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<nav2_costmap_2d::Costmap2D> costmap_ =
     std::make_shared<nav2_costmap_2d::Costmap2D>(100, 100, 0.1, 0, 0, 0);
 
-  nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_);
+  nav2_costmap_2d::FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
+  collision_checker(costmap_);
 
   auto value = collision_checker.pointCost(50, 50);
 
@@ -63,7 +65,8 @@ TEST(collision_footprint, test_world_to_map)
   std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ =
     std::make_shared<nav2_costmap_2d::Costmap2D>(100, 100, 0.1, 0, 0, 0);
 
-  nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_);
+  nav2_costmap_2d::FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
+  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<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
+  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<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
+  collision_checker(costmap_);
 
   auto value = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint);
   EXPECT_NEAR(value, 0.0, 0.001);

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 <chrono>
@@ -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<double> time_span = duration_cast<duration<double>>(b - a);
+  std::cout << "It took " << time_span.count() * 1000 <<
+    " milliseconds with " << num_iterations << " iterations." << std::endl;
+#endif
+
   return path;
 }
 

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<nav2_costmap_2d::Costmap2DROS>(