Smoother error codes (ros-navigation#3296)

* minimum error code set

* test for invalid smoother

* undo

* added rest of error tests

* Solve bug when CostmapInfoServer is reactivated (ros-navigation#3292)

* Solve bug when CostmapInfoServer is reactivated

* Smoothness metrics update (ros-navigation#3294)

* Update metrics for path smoothness

* Support Savitzky-Golay smoother

* preempt/cancel test for time behavior, spin pluguin (ros-navigation#3301)

* include preempt/cancel test for time behavior, spin pluguin

* linting

* fix bug in code

* removed changes to simple_smoother

* reverted simple_smoother

* revert

* revert

* updated constrained smoother

* revert

* added smoother error for invalid path

* linting

* invalid path test

* added error codes

* Timeout exception thrown by smoothers

* code review

Co-authored-by: Joshua Wallace <josho.wallace.com>
Co-authored-by: MartiBolet <43337758+MartiBolet@users.noreply.github.com>
Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com>
Co-authored-by: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com>

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp

@@ -30,6 +30,10 @@ namespace nav2_behavior_tree
  */
 class SmoothPathAction : public nav2_behavior_tree::BtActionNode<nav2_msgs::action::SmoothPath>
 {
+  using Action = nav2_msgs::action::SmoothPath;
+  using ActionResult = Action::Result;
+  using ActionGoal = Action::Goal;
+
 public:
   /**
    * @brief A constructor for nav2_behavior_tree::SmoothPathAction
@@ -52,6 +56,16 @@ class SmoothPathAction : public nav2_behavior_tree::BtActionNode<nav2_msgs::acti
    */
   BT::NodeStatus on_success() override;
 
+  /**
+   * @brief Function to perform some user-defined operation upon abortion of the action
+   */
+  BT::NodeStatus on_aborted() override;
+
+  /**
+   * @brief Function to perform some user-defined operation upon cancellation of the action
+   */
+  BT::NodeStatus on_cancelled() override;
+
   /**
    * @brief Creates list of BT ports
    * @return BT::PortsList Containing basic ports along with node-specific ports
@@ -60,18 +74,20 @@ class SmoothPathAction : public nav2_behavior_tree::BtActionNode<nav2_msgs::acti
   {
     return providedBasicPorts(
       {
-        BT::OutputPort<nav_msgs::msg::Path>(
-          "smoothed_path",
-          "Path smoothed by SmootherServer node"),
-        BT::OutputPort<double>("smoothing_duration", "Time taken to smooth path"),
-        BT::OutputPort<bool>(
-          "was_completed", "True if smoothing was not interrupted by time limit"),
         BT::InputPort<nav_msgs::msg::Path>("unsmoothed_path", "Path to be smoothed"),
         BT::InputPort<double>("max_smoothing_duration", 3.0, "Maximum smoothing duration"),
         BT::InputPort<bool>(
           "check_for_collisions", false,
           "If true collision check will be performed after smoothing"),
         BT::InputPort<std::string>("smoother_id", ""),
+        BT::OutputPort<nav_msgs::msg::Path>(
+          "smoothed_path",
+          "Path smoothed by SmootherServer node"),
+        BT::OutputPort<double>("smoothing_duration", "Time taken to smooth path"),
+        BT::OutputPort<bool>(
+          "was_completed", "True if smoothing was not interrupted by time limit"),
+        BT::OutputPort<ActionResult::_error_code_type>(
+          "error_code_id", "The smooth path error code"),
       });
   }
 };

nav2_behavior_tree/plugins/action/smooth_path_action.cpp

@@ -44,6 +44,21 @@ BT::NodeStatus SmoothPathAction::on_success()
   setOutput("smoothed_path", result_.result->path);
   setOutput("smoothing_duration", rclcpp::Duration(result_.result->smoothing_duration).seconds());
   setOutput("was_completed", result_.result->was_completed);
+  // Set empty error code, action was successful
+  setOutput("error_code_id", ActionGoal::NONE);
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus SmoothPathAction::on_aborted()
+{
+  setOutput("error_code_id", result_.result->error_code);
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus SmoothPathAction::on_cancelled()
+{
+  // Set empty error code, action was cancelled
+  setOutput("error_code_id", ActionGoal::NONE);
   return BT::NodeStatus::SUCCESS;
 }
 

nav2_constrained_smoother/include/nav2_constrained_smoother/smoother.hpp

@@ -28,6 +28,7 @@
 
 #include "nav2_constrained_smoother/smoother_cost_function.hpp"
 #include "nav2_constrained_smoother/utils.hpp"
+#include "nav2_core/smoother_exceptions.hpp"
 
 #include "ceres/ceres.h"
 #include "Eigen/Core"
@@ -94,7 +95,7 @@ class Smoother
   {
     // Path has always at least 2 points
     if (path.size() < 2) {
-      throw std::runtime_error("Constrained smoother: Path must have at least 2 points");
+      throw nav2_core::InvalidPath("Constrained smoother: Path must have at least 2 points");
     }
 
     options_.max_solver_time_in_seconds = params.max_time;
@@ -110,7 +111,7 @@ class Smoother
         RCLCPP_INFO(rclcpp::get_logger("smoother_server"), "%s", summary.FullReport().c_str());
       }
       if (!summary.IsSolutionUsable() || summary.initial_cost - summary.final_cost < 0.0) {
-        return false;
+        throw nav2_core::FailedToSmoothPath("Solution is not usable");
       }
     } else {
       RCLCPP_INFO(rclcpp::get_logger("smoother_server"), "Path too short to optimize");

nav2_constrained_smoother/src/constrained_smoother.cpp

@@ -23,7 +23,7 @@
 #include "nav2_constrained_smoother/constrained_smoother.hpp"
 #include "nav2_util/node_utils.hpp"
 #include "nav2_util/geometry_utils.hpp"
-#include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
+#include "nav2_core/smoother_exceptions.hpp"
 
 #include "pluginlib/class_loader.hpp"
 #include "pluginlib/class_list_macros.hpp"
@@ -137,8 +137,8 @@ bool ConstrainedSmoother::smooth(nav_msgs::msg::Path & path, const rclcpp::Durat
       logger_,
       "%s: failed to smooth plan, Ceres could not find a usable solution to optimize.",
       plugin_name_.c_str());
-    throw std::runtime_error(
-            "Failed to smooth plan, Ceres could not find a usable solution.");
+    throw nav2_core::FailedToSmoothPath(
+            "Failed to smooth plan, Ceres could not find a usable solution");
   }
 
   // populate final path

nav2_core/include/nav2_core/smoother_exceptions.hpp

@@ -0,0 +1,67 @@
+// Copyright (c) 2022 Joshua Wallace
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_CORE__SMOOTHER_EXCEPTIONS_HPP_
+#define NAV2_CORE__SMOOTHER_EXCEPTIONS_HPP_
+
+#include <stdexcept>
+#include <string>
+
+namespace nav2_core
+{
+
+class SmootherException : public std::runtime_error
+{
+public:
+  explicit SmootherException(const std::string & description)
+  : std::runtime_error(description) {}
+};
+
+class InvalidSmoother : public SmootherException
+{
+public:
+  explicit InvalidSmoother(const std::string & description)
+  : SmootherException(description) {}
+};
+
+class InvalidPath : public SmootherException
+{
+public:
+  explicit InvalidPath(const std::string & description)
+  : SmootherException(description) {}
+};
+
+class SmootherTimedOut : public SmootherException
+{
+public:
+  explicit SmootherTimedOut(const std::string & description)
+  : SmootherException(description) {}
+};
+
+class SmoothedPathInCollision : public SmootherException
+{
+public:
+  explicit SmoothedPathInCollision(const std::string & description)
+  : SmootherException(description) {}
+};
+
+class FailedToSmoothPath : public SmootherException
+{
+public:
+  explicit FailedToSmoothPath(const std::string & description)
+  : SmootherException(description) {}
+};
+
+}  // namespace nav2_core
+#endif  // NAV2_CORE__SMOOTHER_EXCEPTIONS_HPP_

nav2_msgs/action/SmoothPath.action

@@ -1,3 +1,13 @@
+# Error codes
+# Note: The expected priority order of the errors should match the message order
+uint16 NONE=0
+uint16 UNKNOWN=500
+uint16 INVALID_SMOOTHER=501
+uint16 TIMEOUT=502
+uint16 SMOOTHED_PATH_IN_COLLISION=503
+uint16 FAILED_TO_SMOOTH_PATH=504
+uint16 INVALID_PATH=505
+
 #goal definition
 nav_msgs/Path path
 string smoother_id
@@ -8,5 +18,6 @@ bool check_for_collisions
 nav_msgs/Path path
 builtin_interfaces/Duration smoothing_duration
 bool was_completed
+uint16 error_code
 ---
 #feedback definition

nav2_simple_commander/nav2_simple_commander/robot_navigator.py

@@ -433,16 +433,18 @@ def _smoothPathImpl(self, path, smoother_id='', max_duration=2.0, check_for_coll
         self.result_future = self.goal_handle.get_result_async()
         rclpy.spin_until_future_complete(self, self.result_future)
         self.status = self.result_future.result().status
-        if self.status != GoalStatus.STATUS_SUCCEEDED:
-            self.warn(f'Getting path failed with status code: {self.status}')
-            return None
 
         return self.result_future.result().result
 
     def smoothPath(self, path, smoother_id='', max_duration=2.0, check_for_collision=False):
         """Send a `SmoothPath` action request."""
         rtn = self._smoothPathImpl(
-            self, path, smoother_id='', max_duration=2.0, check_for_collision=False)
+            path, smoother_id='', max_duration=2.0, check_for_collision=False)
+
+        if self.status != GoalStatus.STATUS_SUCCEEDED:
+            self.warn(f'Getting path failed with status code: {self.status}')
+            return None
+
         if not rtn:
             return None
         else:

nav2_smoother/include/nav2_smoother/nav2_smoother.hpp

@@ -114,6 +114,7 @@ class SmootherServer : public nav2_util::LifecycleNode
   nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
 
   using Action = nav2_msgs::action::SmoothPath;
+  using ActionGoal = Action::Goal;
   using ActionServer = nav2_util::SimpleActionServer<Action>;
 
   /**
@@ -135,6 +136,13 @@ class SmootherServer : public nav2_util::LifecycleNode
    */
   bool findSmootherId(const std::string & c_name, std::string & name);
 
+  /**
+   * @brief Validate that the path contains a meaningful path for smoothing
+   * @param path current path
+   * return bool if the path is valid
+   */
+  bool validate(const nav_msgs::msg::Path & path);
+
   // Our action server implements the SmoothPath action
   std::unique_ptr<ActionServer> action_server_;
 

nav2_smoother/src/nav2_smoother.cpp

@@ -20,7 +20,7 @@
 #include <utility>
 #include <vector>
 
-#include "nav2_core/planner_exceptions.hpp"
+#include "nav2_core/smoother_exceptions.hpp"
 #include "nav2_smoother/nav2_smoother.hpp"
 #include "nav2_util/node_utils.hpp"
 #include "nav_2d_utils/conversions.hpp"
@@ -263,13 +263,17 @@ void SmootherServer::smoothPlan()
     if (findSmootherId(c_name, current_smoother)) {
       current_smoother_ = current_smoother;
     } else {
-      action_server_->terminate_current();
-      return;
+      throw nav2_core::InvalidSmoother("Invalid Smoother: " + c_name);
     }
 
     // Perform smoothing
     auto goal = action_server_->get_current_goal();
     result->path = goal->path;
+
+    if (!validate(result->path)) {
+      throw nav2_core::InvalidPath("Requested path to smooth is invalid");
+    }
+
     result->was_completed = smoothers_[current_smoother_]->smooth(
       result->path, goal->max_smoothing_duration);
     result->smoothing_duration = steady_clock_.now() - start_time;
@@ -283,6 +287,7 @@ void SmootherServer::smoothPlan()
         rclcpp::Duration(goal->max_smoothing_duration).seconds(),
         rclcpp::Duration(result->smoothing_duration).seconds());
     }
+
     plan_publisher_->publish(result->path);
 
     // Check for collisions
@@ -299,8 +304,11 @@ void SmootherServer::smoothPlan()
             get_logger(),
             "Smoothed path leads to a collision at x: %lf, y: %lf, theta: %lf",
             pose2d.x, pose2d.y, pose2d.theta);
-          action_server_->terminate_current(result);
-          return;
+          throw nav2_core::SmoothedPathInCollision(
+                  "Smoothed Path collided at"
+                  "X: " + std::to_string(pose2d.x) +
+                  "Y: " + std::to_string(pose2d.y) +
+                  "Theta: " + std::to_string(pose2d.theta));
         }
         fetch_data = false;
       }
@@ -311,13 +319,50 @@ void SmootherServer::smoothPlan()
       rclcpp::Duration(result->smoothing_duration).seconds());
 
     action_server_->succeeded_current(result);
-  } catch (nav2_core::PlannerException & e) {
-    RCLCPP_ERROR(this->get_logger(), e.what());
-    action_server_->terminate_current();
+  } catch (nav2_core::InvalidSmoother & ex) {
+    RCLCPP_ERROR(this->get_logger(), "%s", ex.what());
+    result->error_code = ActionGoal::INVALID_SMOOTHER;
+    action_server_->terminate_current(result);
+    return;
+  } catch (nav2_core::SmootherTimedOut & ex) {
+    RCLCPP_ERROR(this->get_logger(), "%s", ex.what());
+    result->error_code = ActionGoal::TIMEOUT;
+    action_server_->terminate_current(result);
+    return;
+  } catch (nav2_core::SmoothedPathInCollision & ex) {
+    RCLCPP_ERROR(this->get_logger(), "%s", ex.what());
+    result->error_code = ActionGoal::SMOOTHED_PATH_IN_COLLISION;
+    action_server_->terminate_current(result);
+    return;
+  } catch (nav2_core::FailedToSmoothPath & ex) {
+    RCLCPP_ERROR(this->get_logger(), "%s", ex.what());
+    result->error_code = ActionGoal::FAILED_TO_SMOOTH_PATH;
+    action_server_->terminate_current(result);
+    return;
+  } catch (nav2_core::InvalidPath & ex) {
+    RCLCPP_ERROR(this->get_logger(), "%s", ex.what());
+    result->error_code = ActionGoal::INVALID_PATH;
+    action_server_->terminate_current(result);
+    return;
+  } catch (nav2_core::SmootherException & ex) {
+    RCLCPP_ERROR(this->get_logger(), "%s", ex.what());
+    result->error_code = ActionGoal::UNKNOWN;
+    action_server_->terminate_current(result);
     return;
   }
 }
 
+bool SmootherServer::validate(const nav_msgs::msg::Path & path)
+{
+  if (path.poses.empty()) {
+    RCLCPP_WARN(get_logger(), "Requested path to smooth is empty");
+    return false;
+  }
+
+  RCLCPP_DEBUG(get_logger(), "Requested path to smooth is valid");
+  return true;
+}
+
 }  // namespace nav2_smoother
 
 #include "rclcpp_components/register_node_macro.hpp"

nav2_smoother/src/savitzky_golay_smoother.cpp

@@ -15,6 +15,7 @@
 #include <vector>
 #include <memory>
 #include "nav2_smoother/savitzky_golay_smoother.hpp"
+#include "nav2_core/smoother_exceptions.hpp"
 
 namespace nav2_smoother
 {
@@ -71,7 +72,7 @@ bool SavitzkyGolaySmoother::smooth(
         RCLCPP_WARN(
           logger_,
           "Smoothing time exceeded allowed duration of %0.2f.", max_time.seconds());
-        return false;
+        throw nav2_core::SmootherTimedOut("Smoothing time exceed allowed duration");
       }
 
       // Smooth path segment