[All 2D planners] Fix last pose orientation, fix small path and add ignore_goal_orientation parameter

nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp

@@ -30,6 +30,7 @@
 #include "nav2_util/robot_utils.hpp"
 #include "nav2_util/lifecycle_node.hpp"
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "nav2_util/geometry_utils.hpp"
 
 namespace nav2_navfn_planner
 {
@@ -205,7 +206,7 @@ class NavfnPlanner : public nav2_core::GlobalPlanner
   std::string global_frame_, name_;
 
   // Whether or not the planner should be allowed to plan through unknown space
-  bool allow_unknown_;
+  bool allow_unknown_, use_final_approach_orientation_;
 
   // If the goal is obstructed, the tolerance specifies how many meters the planner
   // can relax the constraint in x and y before failing

nav2_navfn_planner/src/navfn_planner.cpp

@@ -85,6 +85,9 @@ NavfnPlanner::configure(
   node->get_parameter(name + ".use_astar", use_astar_);
   declare_parameter_if_not_declared(node, name + ".allow_unknown", rclcpp::ParameterValue(true));
   node->get_parameter(name + ".allow_unknown", allow_unknown_);
+  declare_parameter_if_not_declared(
+    node, name + ".use_final_approach_orientation", rclcpp::ParameterValue(false));
+  node->get_parameter(name + ".use_final_approach_orientation", use_final_approach_orientation_);
 
   // Create a planner based on the new costmap size
   planner_ = std::make_unique<NavFn>(
@@ -144,12 +147,40 @@ nav_msgs::msg::Path NavfnPlanner::createPlan(
 
   nav_msgs::msg::Path path;
 
+  // Corner case of the start(x,y) = goal(x,y)
+  if (start.pose.position.x == goal.pose.position.x &&
+    start.pose.position.y == goal.pose.position.y)
+  {
+    unsigned int mx, my;
+    costmap_->worldToMap(start.pose.position.x, start.pose.position.y, mx, my);
+    if (costmap_->getCost(mx, my) == nav2_costmap_2d::LETHAL_OBSTACLE) {
+      RCLCPP_WARN(logger_, "Failed to create a unique pose path because of obstacles");
+      return path;
+    }
+    path.header.stamp = clock_->now();
+    path.header.frame_id = global_frame_;
+    geometry_msgs::msg::PoseStamped pose;
+    pose.header = path.header;
+    pose.pose.position.z = 0.0;
+
+    pose.pose = start.pose;
+    // if we have a different start and goal orientation, set the unique path pose to the goal
+    // orientation, unless use_final_approach_orientation=true where we need it to be the start
+    // orientation to avoid movement from the local planner
+    if (start.pose.orientation != goal.pose.orientation && !use_final_approach_orientation_) {
+      pose.pose.orientation = goal.pose.orientation;
+    }
+    path.poses.push_back(pose);
+    return path;
+  }
+
   if (!makePlan(start.pose, goal.pose, tolerance_, path)) {
     RCLCPP_WARN(
       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);
@@ -284,6 +315,32 @@ NavfnPlanner::makePlan(
     // extract the plan
     if (getPlanFromPotential(best_pose, plan)) {
       smoothApproachToGoal(best_pose, plan);
+
+      // If use_final_approach_orientation=true, interpolate the last pose orientation from the
+      // previous pose to set the orientation to the 'final approach' orientation of the robot so
+      // it does not rotate.
+      // And deal with corner case of plan of length 1
+      if (use_final_approach_orientation_) {
+        size_t plan_size = plan.poses.size();
+        if (plan_size == 1) {
+          plan.poses.back().pose.orientation = start.orientation;
+        } else if (plan_size > 1) {
+          double dx, dy, theta;
+          auto last_pose = plan.poses.back().pose.position;
+          auto approach_pose = plan.poses[plan_size - 2].pose.position;
+          // Deal with the case of NavFn producing a path with two equal last poses
+          if (std::abs(last_pose.x - approach_pose.x) < 0.0001 &&
+            std::abs(last_pose.y - approach_pose.y) < 0.0001 && plan_size > 2)
+          {
+            approach_pose = plan.poses[plan_size - 3].pose.position;
+          }
+          dx = last_pose.x - approach_pose.x;
+          dy = last_pose.y - approach_pose.y;
+          theta = atan2(dy, dx);
+          plan.poses.back().pose.orientation =
+            nav2_util::geometry_utils::orientationAroundZAxis(theta);
+        }
+      }
     } else {
       RCLCPP_ERROR(
         logger_,
@@ -489,6 +546,8 @@ NavfnPlanner::on_parameter_event_callback(
         use_astar_ = value.bool_value;
       } else if (name == name_ + ".allow_unknown") {
         allow_unknown_ = value.bool_value;
+      } else if (name == name_ + ".use_final_approach_orientation") {
+        use_final_approach_orientation_ = value.bool_value;
       }
     }
   }

nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp

@@ -110,6 +110,7 @@ class SmacPlanner2D : public nav2_core::GlobalPlanner
   bool _allow_unknown;
   int _max_iterations;
   int _max_on_approach_iterations;
+  bool _use_final_approach_orientation;
   SearchInfo _search_info;
   std::string _motion_model_for_search;
   MotionModel _motion_model;

nav2_smac_planner/src/smac_planner_2d.cpp

@@ -19,6 +19,7 @@
 #include <algorithm>
 
 #include "nav2_smac_planner/smac_planner_2d.hpp"
+#include "nav2_util/geometry_utils.hpp"
 
 // #define BENCHMARK_TESTING
 
@@ -80,6 +81,9 @@ void SmacPlanner2D::configure(
   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 + ".use_final_approach_orientation", rclcpp::ParameterValue(false));
+  node->get_parameter(name + ".use_final_approach_orientation", _use_final_approach_orientation);
 
   nav2_util::declare_parameter_if_not_declared(
     node, name + ".max_planning_time", rclcpp::ParameterValue(1.0));
@@ -218,13 +222,13 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan(
   _a_star->setCollisionChecker(&_collision_checker);
 
   // 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);
+  unsigned int mx_start, my_start, mx_goal, my_goal;
+  costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start);
+  _a_star->setStart(mx_start, my_start, 0);
 
   // Set goal point
-  costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my);
-  _a_star->setGoal(mx, my, 0);
+  costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal);
+  _a_star->setGoal(mx_goal, my_goal, 0);
 
   // Setup message
   nav_msgs::msg::Path plan;
@@ -238,6 +242,23 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan(
   pose.pose.orientation.z = 0.0;
   pose.pose.orientation.w = 1.0;
 
+  // Corner case of start and goal beeing on the same cell
+  if (mx_start == mx_goal && my_start == my_goal) {
+    if (costmap->getCost(mx_start, my_start) == nav2_costmap_2d::LETHAL_OBSTACLE) {
+      RCLCPP_WARN(_logger, "Failed to create a unique pose path because of obstacles");
+      return plan;
+    }
+    pose.pose = start.pose;
+    // if we have a different start and goal orientation, set the unique path pose to the goal
+    // orientation, unless use_final_approach_orientation=true where we need it to be the start
+    // orientation to avoid movement from the local planner
+    if (start.pose.orientation != goal.pose.orientation && !_use_final_approach_orientation) {
+      pose.pose.orientation = goal.pose.orientation;
+    }
+    plan.poses.push_back(pose);
+    return plan;
+  }
+
   // Compute plan
   Node2D::CoordinateVector path;
   int num_iterations = 0;
@@ -292,6 +313,30 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan(
     _smoother->smooth(plan, costmap, time_remaining);
   }
 
+
+  // If use_final_approach_orientation=true, interpolate the last pose orientation from the
+  // previous pose to set the orientation to the 'final approach' orientation of the robot so
+  // it does not rotate.
+  // And deal with corner case of plan of length 1
+  // If use_final_approach_orientation=false (default), override last pose orientation to match goal
+  size_t plan_size = plan.poses.size();
+  if (_use_final_approach_orientation) {
+    if (plan_size == 1) {
+      plan.poses.back().pose.orientation = start.pose.orientation;
+    } else if (plan_size > 1) {
+      double dx, dy, theta;
+      auto last_pose = plan.poses.back().pose.position;
+      auto approach_pose = plan.poses[plan_size - 2].pose.position;
+      dx = last_pose.x - approach_pose.x;
+      dy = last_pose.y - approach_pose.y;
+      theta = atan2(dy, dx);
+      plan.poses.back().pose.orientation =
+        nav2_util::geometry_utils::orientationAroundZAxis(theta);
+    }
+  } else if (plan_size > 0) {
+    plan.poses.back().pose.orientation = goal.pose.orientation;
+  }
+
   return plan;
 }
 
@@ -324,6 +369,8 @@ void SmacPlanner2D::on_parameter_event_callback(
       } else if (name == _name + ".allow_unknown") {
         reinit_a_star = true;
         _allow_unknown = value.bool_value;
+      } else if (name == _name + ".use_final_approach_orientation") {
+        _use_final_approach_orientation = value.bool_value;
       }
     } else if (type == ParameterType::PARAMETER_INTEGER) {
       if (name == _name + ".downsampling_factor") {

nav2_system_tests/src/planning/CMakeLists.txt

@@ -46,11 +46,11 @@ ament_add_test(test_planner_random
     TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map.pgm
 )
 
-ament_add_gtest(test_planner_plugin_failures
+ament_add_gtest(test_planner_plugins
   test_planner_plugins.cpp
 )
 
-ament_target_dependencies(test_planner_plugin_failures rclcpp geometry_msgs nav2_msgs ${dependencies})
-target_link_libraries(test_planner_plugin_failures
+ament_target_dependencies(test_planner_plugins rclcpp geometry_msgs nav2_msgs ${dependencies})
+target_link_libraries(test_planner_plugins
   stdc++fs
 )

nav2_system_tests/src/planning/test_planner_plugins.cpp

@@ -20,6 +20,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "planner_tester.hpp"
 #include "nav2_util/lifecycle_utils.hpp"
+#include "nav2_util/geometry_utils.hpp"
 
 using namespace std::chrono_literals;
 
@@ -33,6 +34,60 @@ void callback(const nav_msgs::msg::Path::ConstSharedPtr /*grid*/)
 {
 }
 
+void testSmallPathValidityAndOrientation(std::string plugin, double length)
+{
+  auto obj = std::make_shared<nav2_system_tests::NavFnPlannerTester>();
+  rclcpp_lifecycle::State state;
+  obj->set_parameter(rclcpp::Parameter("GridBased.plugin", plugin));
+  obj->declare_parameter(
+    "GridBased.use_final_approach_orientation", rclcpp::ParameterValue(false));
+  obj->onConfigure(state);
+
+  geometry_msgs::msg::PoseStamped start;
+  geometry_msgs::msg::PoseStamped goal;
+
+  start.pose.position.x = 0.5;
+  start.pose.position.y = 0.5;
+  start.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(M_PI_2);
+  start.header.frame_id = "map";
+
+  goal.pose.position.x = 0.5;
+  goal.pose.position.y = start.pose.position.y + length;
+  goal.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(-M_PI);
+  goal.header.frame_id = "map";
+
+  // Test without use_final_approach_orientation
+  // expecting end path pose orientation to be equal to goal orientation
+  auto path = obj->getPlan(start, goal, "GridBased");
+  EXPECT_GT((int)path.poses.size(), 0);
+  EXPECT_NEAR(tf2::getYaw(path.poses.back().pose.orientation), -M_PI, 0.01);
+  obj->onCleanup(state);
+
+  // Test WITH use_final_approach_orientation
+  // expecting end path pose orientation to be equal to approach orientation
+  // which in the one pose corner case should be the start pose orientation
+  obj->set_parameter(rclcpp::Parameter("GridBased.use_final_approach_orientation", true));
+  obj->onConfigure(state);
+  path = obj->getPlan(start, goal, "GridBased");
+  EXPECT_GT((int)path.poses.size(), 0);
+
+  int path_size = path.poses.size();
+  if (path_size == 1) {
+    EXPECT_NEAR(
+      tf2::getYaw(path.poses.back().pose.orientation),
+      tf2::getYaw(start.pose.orientation),
+      0.01);
+  } else {
+    double dx = path.poses.back().pose.position.x - path.poses.front().pose.position.x;
+    double dy = path.poses.back().pose.position.y - path.poses.front().pose.position.y;
+    EXPECT_NEAR(
+      tf2::getYaw(path.poses.back().pose.orientation),
+      atan2(dy, dx),
+      0.01);
+  }
+  obj->onCleanup(state);
+}
+
 TEST(testPluginMap, Failures)
 {
   auto obj = std::make_shared<nav2_system_tests::NavFnPlannerTester>();
@@ -55,6 +110,81 @@ TEST(testPluginMap, Failures)
   obj->onCleanup(state);
 }
 
+TEST(testPluginMap, Smac2dEqualStartGoal)
+{
+  testSmallPathValidityAndOrientation("nav2_smac_planner/SmacPlanner2D", 0.0);
+}
+
+TEST(testPluginMap, Smac2dVerySmallPath)
+{
+  testSmallPathValidityAndOrientation("nav2_smac_planner/SmacPlanner2D", 0.00001);
+}
+
+TEST(testPluginMap, Smac2dBelowCostmapResolution)
+{
+  testSmallPathValidityAndOrientation("nav2_smac_planner/SmacPlanner2D", 0.09);
+}
+
+TEST(testPluginMap, Smac2dJustAboveCostmapResolution)
+{
+  testSmallPathValidityAndOrientation("nav2_smac_planner/SmacPlanner2D", 0.102);
+}
+
+TEST(testPluginMap, Smac2dAboveCostmapResolution)
+{
+  testSmallPathValidityAndOrientation("nav2_smac_planner/SmacPlanner2D", 1.5);
+}
+
+TEST(testPluginMap, NavFnEqualStartGoal)
+{
+  testSmallPathValidityAndOrientation("nav2_navfn_planner/NavfnPlanner", 0.0);
+}
+
+TEST(testPluginMap, NavFnVerySmallPath)
+{
+  testSmallPathValidityAndOrientation("nav2_navfn_planner/NavfnPlanner", 0.00001);
+}
+
+TEST(testPluginMap, NavFnBelowCostmapResolution)
+{
+  testSmallPathValidityAndOrientation("nav2_navfn_planner/NavfnPlanner", 0.09);
+}
+
+TEST(testPluginMap, NavFnJustAboveCostmapResolution)
+{
+  testSmallPathValidityAndOrientation("nav2_navfn_planner/NavfnPlanner", 0.102);
+}
+
+TEST(testPluginMap, NavFnAboveCostmapResolution)
+{
+  testSmallPathValidityAndOrientation("nav2_navfn_planner/NavfnPlanner", 1.5);
+}
+
+TEST(testPluginMap, ThetaStarEqualStartGoal)
+{
+  testSmallPathValidityAndOrientation("nav2_theta_star_planner/ThetaStarPlanner", 0.0);
+}
+
+TEST(testPluginMap, ThetaStarVerySmallPath)
+{
+  testSmallPathValidityAndOrientation("nav2_theta_star_planner/ThetaStarPlanner", 0.00001);
+}
+
+TEST(testPluginMap, ThetaStarBelowCostmapResolution)
+{
+  testSmallPathValidityAndOrientation("nav2_theta_star_planner/ThetaStarPlanner", 0.09);
+}
+
+TEST(testPluginMap, ThetaStarJustAboveCostmapResolution)
+{
+  testSmallPathValidityAndOrientation("nav2_theta_star_planner/ThetaStarPlanner", 0.102);
+}
+
+TEST(testPluginMap, ThetaStarAboveCostmapResolution)
+{
+  testSmallPathValidityAndOrientation("nav2_theta_star_planner/ThetaStarPlanner", 1.5);
+}
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);