adding support for in-place rotations in state lattice (#2747)

SteveMacenski · web-flow · commit b598a3854e3c · 2022-01-21T15:17:27.000-08:00
diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md
@@ -121,6 +121,7 @@ planner_server:
       change_penalty: 0.0                 # For Hybrid nodes: penalty to apply if motion is changing directions, must be >= 0
       non_straight_penalty: 1.20          # For Hybrid nodes: penalty to apply if motion is non-straight, must be => 1
       cost_penalty: 2.0                   # For Hybrid nodes: penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable.
+      rotation_penalty: 5.0               # For Lattice node: Penalty to apply only to pure rotate in place commands when using minimum control sets containing rotate in place primitives. This should always be set sufficiently high to weight against this action unless strictly necessary for obstacle avoidance or there may be frequent discontinuities in the plan where it requests the robot to rotate in place to short-cut an otherwise smooth path for marginal path distance savings.
       lookup_table_size: 20               # For Hybrid nodes: Size of the dubin/reeds-sheep distance window to cache, in meters.
       cache_obstacle_heuristic: True      # For Hybrid nodes: Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static.  
       allow_reverse_expansion: False      # For Lattice nodes: Whether to expand state lattice graph in forward primitives or reverse as well, will double the branching factor at each step.   
diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp
@@ -102,6 +102,7 @@ struct LatticeMotionTable
   float non_straight_penalty;
   float cost_penalty;
   float reverse_penalty;
+  float rotation_penalty;
   bool allow_reverse_expansion;
   std::vector<std::vector<MotionPrimitive>> motion_primitives;
   ompl::base::StateSpacePtr state_space;
diff --git a/nav2_smac_planner/include/nav2_smac_planner/types.hpp b/nav2_smac_planner/include/nav2_smac_planner/types.hpp
@@ -39,6 +39,7 @@ struct SearchInfo
   float change_penalty;
   float reverse_penalty;
   float cost_penalty;
+  float rotation_penalty;
   float analytic_expansion_ratio;
   float analytic_expansion_max_length;
   std::string lattice_filepath;
diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp
@@ -314,8 +314,7 @@ float NodeHybrid::getTraversalCost(const NodePtr & child)
 
   float travel_cost = 0.0;
   float travel_cost_raw =
-    NodeHybrid::travel_distance_cost +
-    (NodeHybrid::travel_distance_cost * motion_table.cost_penalty * normalized_cost);
+    NodeHybrid::travel_distance_cost * (1.0 + motion_table.cost_penalty * normalized_cost);
 
   if (child->getMotionPrimitiveIndex() == 0 || child->getMotionPrimitiveIndex() == 3) {
     // New motion is a straight motion, no additional costs to be applied
diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp
@@ -62,6 +62,7 @@ void LatticeMotionTable::initMotionModel(
   reverse_penalty = search_info.reverse_penalty;
   current_lattice_filepath = search_info.lattice_filepath;
   allow_reverse_expansion = search_info.allow_reverse_expansion;
+  rotation_penalty = search_info.rotation_penalty;
 
   // Get the metadata about this minimum control set
   lattice_metadata = getLatticeMetadata(current_lattice_filepath);
@@ -283,8 +284,13 @@ float NodeLattice::getTraversalCost(const NodePtr & child)
     return prim_length;
   }
 
+  // Pure rotation in place 1 angular bin in either direction
+  if (transition_prim->trajectory_length < 1e-4) {
+    return motion_table.rotation_penalty * (1.0 + motion_table.cost_penalty * normalized_cost);
+  }
+
   float travel_cost = 0.0;
-  float travel_cost_raw = prim_length + (prim_length * motion_table.cost_penalty * normalized_cost);
+  float travel_cost_raw = prim_length * (1.0 + motion_table.cost_penalty * normalized_cost);
 
   if (transition_prim->arc_length < 0.001) {
     // New motion is a straight motion, no additional costs to be applied
diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp
@@ -95,6 +95,9 @@ void SmacPlannerLattice::configure(
   nav2_util::declare_parameter_if_not_declared(
     node, name + ".cost_penalty", rclcpp::ParameterValue(2.0));
   node->get_parameter(name + ".cost_penalty", _search_info.cost_penalty);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".rotation_penalty", rclcpp::ParameterValue(5.0));
+  node->get_parameter(name + ".rotation_penalty", _search_info.rotation_penalty);
   nav2_util::declare_parameter_if_not_declared(
     node, name + ".analytic_expansion_ratio", rclcpp::ParameterValue(3.5));
   node->get_parameter(name + ".analytic_expansion_ratio", _search_info.analytic_expansion_ratio);
@@ -357,6 +360,9 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector<rclcpp::Parameter> par
       } else if (name == _name + ".cost_penalty") {
         reinit_a_star = true;
         _search_info.cost_penalty = static_cast<float>(parameter.as_double());
+      } else if (name == _name + ".rotation_penalty") {
+        reinit_a_star = true;
+        _search_info.rotation_penalty = static_cast<float>(parameter.as_double());
       } else if (name == _name + ".analytic_expansion_ratio") {
         reinit_a_star = true;
         _search_info.analytic_expansion_ratio = static_cast<float>(parameter.as_double());
diff --git a/nav2_smac_planner/src/smoother.cpp b/nav2_smac_planner/src/smoother.cpp
@@ -241,6 +241,16 @@ std::vector<PathSegment> Smoother::findDirectionalPathSegments(const nav_msgs::m
       segments.push_back(curr_segment);
       curr_segment.start = idx;
     }
+
+    // Checking for the existance of a differential rotation in place.
+    double cur_theta = tf2::getYaw(path.poses[idx].pose.orientation);
+    double next_theta = tf2::getYaw(path.poses[idx + 1].pose.orientation);
+    double dtheta = angles::shortest_angular_distance(cur_theta, next_theta);
+    if (fabs(ab_x) < 1e-4 && fabs(ab_y) < 1e-4 && fabs(dtheta) > 1e-4) {
+      curr_segment.end = idx;
+      segments.push_back(curr_segment);
+      curr_segment.start = idx;
+    }
   }
 
   curr_segment.end = path.poses.size() - 1;
diff --git a/nav2_smac_planner/test/test_smac_lattice.cpp b/nav2_smac_planner/test/test_smac_lattice.cpp
@@ -123,6 +123,7 @@ TEST(SmacTest, test_smac_lattice_reconfigure)
       rclcpp::Parameter("test.lookup_table_size", 30.0),
       rclcpp::Parameter("test.smooth_path", false),
       rclcpp::Parameter("test.analytic_expansion_max_length", 42.0),
+      rclcpp::Parameter("test.rotation_penalty", 42.0),
       rclcpp::Parameter("test.allow_reverse_expansion", true)});
 
   try {
