Fixes from ros 2 main

nav2_controller/CMakeLists.txt

@@ -13,6 +13,8 @@ find_package(nav2_msgs REQUIRED)
 find_package(nav_2d_utils REQUIRED)
 find_package(nav_2d_msgs REQUIRED)
 find_package(pluginlib REQUIRED)
+find_package(relox_msgs REQUIRED)
+find_package(diagnostic_updater REQUIRED)
 
 nav2_package()
 
@@ -45,6 +47,8 @@ set(dependencies
   nav2_util
   nav2_core
   pluginlib
+  relox_msgs
+  diagnostic_updater
 )
 
 add_library(simple_progress_checker SHARED plugins/simple_progress_checker.cpp)

nav2_controller/include/nav2_controller/nav2_controller.hpp

@@ -15,6 +15,7 @@
 #ifndef NAV2_CONTROLLER__NAV2_CONTROLLER_HPP_
 #define NAV2_CONTROLLER__NAV2_CONTROLLER_HPP_
 
+#include <diagnostic_updater/diagnostic_updater.hpp>
 #include <memory>
 #include <string>
 #include <thread>
@@ -34,6 +35,7 @@
 #include "nav2_util/robot_utils.hpp"
 #include "pluginlib/class_loader.hpp"
 #include "pluginlib/class_list_macros.hpp"
+#include "relox_msgs/msg/safety_status.hpp"
 
 namespace nav2_controller
 {
@@ -208,6 +210,7 @@ class ControllerServer : public nav2_util::LifecycleNode
   std::unique_ptr<nav_2d_utils::OdomSubscriber> odom_sub_;
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr vel_publisher_;
   rclcpp::Subscription<nav2_msgs::msg::SpeedLimit>::SharedPtr speed_limit_sub_;
+  rclcpp::Subscription<relox_msgs::msg::SafetyStatus>::SharedPtr safety_status_sub_;
 
   // Progress Checker Plugin
   pluginlib::ClassLoader<nav2_core::ProgressChecker> progress_checker_loader_;
@@ -235,7 +238,13 @@ class ControllerServer : public nav2_util::LifecycleNode
   std::vector<std::string> controller_types_;
   std::string controller_ids_concat_, current_controller_;
 
+  void init_diagnostic_updater_();
+  void check_rate(diagnostic_updater::DiagnosticStatusWrapper & status);
+  std::unique_ptr<diagnostic_updater::Updater> diagnostics_updater_;
+
   double controller_frequency_;
+  int controller_frequency_trigger_;
+  int CONTROLLER_FREQUENCY_TRIGGER_THRESHOLD = 20;
   double min_x_velocity_threshold_;
   double min_y_velocity_threshold_;
   double min_theta_velocity_threshold_;
@@ -251,12 +260,16 @@ class ControllerServer : public nav2_util::LifecycleNode
   // Current path container
   nav_msgs::msg::Path current_path_;
 
+  bool is_safety_status_ok_{false};
+
 private:
   /**
     * @brief Callback for speed limiting messages
     * @param msg Shared pointer to nav2_msgs::msg::SpeedLimit
     */
   void speedLimitCallback(const nav2_msgs::msg::SpeedLimit::SharedPtr msg);
+
+  void safetyStatusCallback(const relox_msgs::msg::SafetyStatus::SharedPtr msg);
 };
 
 }  // namespace nav2_controller

nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp

@@ -78,6 +78,9 @@ class SimpleGoalChecker : public nav2_core::GoalChecker
   rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr parameter_event_sub_;
   std::string plugin_name_;
 
+  // Make sure we are outside the tolerance of the goal before we enter inside the tolerance
+  bool has_left_goal_;
+
   /**
    * @brief Callback executed when a paramter change is detected
    * @param event ParameterEvent message

nav2_controller/package.xml

@@ -19,6 +19,7 @@
   <depend>nav_2d_msgs</depend>
   <depend>nav2_core</depend>
   <depend>pluginlib</depend>
+  <depend>relox_msgs</depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>

nav2_controller/plugins/simple_goal_checker.cpp

@@ -55,8 +55,9 @@ SimpleGoalChecker::SimpleGoalChecker()
 : xy_goal_tolerance_(0.25),
   yaw_goal_tolerance_(0.25),
   stateful_(true),
-  check_xy_(true),
-  xy_goal_tolerance_sq_(0.0625)
+  check_xy_(true), 
+  xy_goal_tolerance_sq_(0.0625),
+  has_left_goal_{false}
 {
 }
 
@@ -97,28 +98,37 @@ void SimpleGoalChecker::initialize(
 void SimpleGoalChecker::reset()
 {
   check_xy_ = true;
+  has_left_goal_ = false;
 }
 
 bool SimpleGoalChecker::isGoalReached(
   const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
   const geometry_msgs::msg::Twist &)
 {
-  if (check_xy_) {
+  if (has_left_goal_ == false) {
     double dx = query_pose.position.x - goal_pose.position.x,
-      dy = query_pose.position.y - goal_pose.position.y;
+           dy = query_pose.position.y - goal_pose.position.y;
     if (dx * dx + dy * dy > xy_goal_tolerance_sq_) {
-      return false;
+      has_left_goal_ = true;
     }
-    // We are within the window
-    // If we are stateful, change the state.
-    if (stateful_) {
-      check_xy_ = false;
+    return false;
+  } else {
+    if (check_xy_) {
+      double dx = query_pose.position.x - goal_pose.position.x,
+             dy = query_pose.position.y - goal_pose.position.y;
+      if (dx * dx + dy * dy > xy_goal_tolerance_sq_) {
+        return false;
+      }
+      // We are within the window
+      // If we are stateful, change the state.
+      if (stateful_) {
+        check_xy_ = false;
+      }
     }
+    double dyaw = angles::shortest_angular_distance(
+      tf2::getYaw(query_pose.orientation), tf2::getYaw(goal_pose.orientation));
+    return fabs(dyaw) < yaw_goal_tolerance_;
   }
-  double dyaw = angles::shortest_angular_distance(
-    tf2::getYaw(query_pose.orientation),
-    tf2::getYaw(goal_pose.orientation));
-  return fabs(dyaw) < yaw_goal_tolerance_;
 }
 
 bool SimpleGoalChecker::getTolerances(

nav2_controller/src/nav2_controller.cpp

@@ -41,7 +41,9 @@ ControllerServer::ControllerServer()
   default_goal_checker_types_{"nav2_controller::SimpleGoalChecker"},
   lp_loader_("nav2_core", "nav2_core::Controller"),
   default_ids_{"FollowPath"},
-  default_types_{"dwb_core::DWBLocalPlanner"}
+  default_types_{"dwb_core::DWBLocalPlanner"},
+  diagnostics_updater_(new diagnostic_updater::Updater(this, 0.5)),
+  controller_frequency_trigger_(0)
 {
   RCLCPP_INFO(get_logger(), "Creating controller server");
 
@@ -74,11 +76,30 @@ ControllerServer::~ControllerServer()
   costmap_thread_.reset();
 }
 
+void ControllerServer::init_diagnostic_updater_()
+{
+  diagnostics_updater_->setHardwareID("nav2_controller");
+  diagnostics_updater_->add("Nav2 Rate Low", this, &ControllerServer::check_rate);
+  diagnostics_updater_->broadcast(
+    diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data in nav2 monitor yet");
+}
+
+void ControllerServer::check_rate(diagnostic_updater::DiagnosticStatusWrapper & status)
+{
+  if (controller_frequency_trigger_ >= CONTROLLER_FREQUENCY_TRIGGER_THRESHOLD) {
+    status.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Nav2 multiple rate misses");
+  } else {
+    status.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Nav2 rate OK");
+  }
+}
+
 nav2_util::CallbackReturn
 ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
 {
   auto node = shared_from_this();
 
+  init_diagnostic_updater_();
+
   RCLCPP_INFO(get_logger(), "Configuring controller interface");
 
   get_parameter("progress_checker_plugin", progress_checker_id_);
@@ -203,6 +224,10 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
     speed_limit_topic, rclcpp::QoS(10),
     std::bind(&ControllerServer::speedLimitCallback, this, std::placeholders::_1));
 
+  safety_status_sub_ = create_subscription<relox_msgs::msg::SafetyStatus>(
+    "/safety_status", rclcpp::QoS(10),
+    std::bind(&ControllerServer::safetyStatusCallback, this, std::placeholders::_1));
+
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
@@ -266,6 +291,7 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & state)
   odom_sub_.reset();
   vel_publisher_.reset();
   speed_limit_sub_.reset();
+  safety_status_sub_.reset();
   action_server_.reset();
 
   return nav2_util::CallbackReturn::SUCCESS;
@@ -341,6 +367,7 @@ void ControllerServer::computeControl()
       current_controller_ = current_controller;
     } else {
       action_server_->terminate_current();
+      controller_frequency_trigger_ = 0;
       return;
     }
 
@@ -350,6 +377,7 @@ void ControllerServer::computeControl()
       current_goal_checker_ = current_goal_checker;
     } else {
       action_server_->terminate_current();
+      controller_frequency_trigger_ = 0;
       return;
     }
 
@@ -361,13 +389,15 @@ void ControllerServer::computeControl()
     while (rclcpp::ok()) {
       if (action_server_ == nullptr || !action_server_->is_server_active()) {
         RCLCPP_DEBUG(get_logger(), "Action server unavailable or inactive. Stopping.");
+        controller_frequency_trigger_ = 0;
         return;
       }
 
       if (action_server_->is_cancel_requested()) {
         RCLCPP_INFO(get_logger(), "Goal was canceled. Stopping the robot.");
         action_server_->terminate_all();
         publishZeroVelocity();
+        controller_frequency_trigger_ = 0;
         return;
       }
 
@@ -387,10 +417,24 @@ void ControllerServer::computeControl()
       }
 
       if (!loop_rate.sleep()) {
-        RCLCPP_WARN(
-          get_logger(), "Control loop missed its desired rate of %.4fHz",
-          controller_frequency_);
+        controller_frequency_trigger_ += 4;
+        auto & clk = *this->get_clock();
+        if (controller_frequency_trigger_ >= CONTROLLER_FREQUENCY_TRIGGER_THRESHOLD) {
+          RCLCPP_ERROR_THROTTLE(
+            get_logger(), clk, 1000, "Control loop missed its desired rate of %.4fHz",
+            controller_frequency_);
+        } else {
+          RCLCPP_WARN(
+            get_logger(), "Control loop missed its desired rate of %.4fHz", controller_frequency_);
+        }
+      } else {
+        controller_frequency_trigger_ -= 1;
       }
+
+      // Cap the trigger counter so that it doesn't go negative or become to large
+      controller_frequency_trigger_ = std::min(
+        std::max(controller_frequency_trigger_, 0),
+        static_cast<int>(CONTROLLER_FREQUENCY_TRIGGER_THRESHOLD * 1.5));
     }
   } catch (nav2_core::PlannerException & e) {
     RCLCPP_ERROR(this->get_logger(), e.what());
@@ -405,6 +449,7 @@ void ControllerServer::computeControl()
 
   // TODO(orduno) #861 Handle a pending preemption and set controller name
   action_server_->succeeded_current();
+  controller_frequency_trigger_ = 0;
 }
 
 void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path)
@@ -430,46 +475,61 @@ void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path)
 
 void ControllerServer::computeAndPublishVelocity()
 {
+  geometry_msgs::msg::TwistStamped cmd_vel_2d;
   geometry_msgs::msg::PoseStamped pose;
 
   if (!getRobotPose(pose)) {
     throw nav2_core::PlannerException("Failed to obtain robot pose");
   }
 
-  if (!progress_checker_->check(pose)) {
-    throw nav2_core::PlannerException("Failed to make progress");
-  }
-
-  nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist());
+  if (is_safety_status_ok_) {
+    if (!progress_checker_->check(pose)) {
+      throw nav2_core::PlannerException("Failed to make progress");
+    }
 
-  geometry_msgs::msg::TwistStamped cmd_vel_2d;
+    nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist());
 
-  try {
-    cmd_vel_2d =
-      controllers_[current_controller_]->computeVelocityCommands(
-      pose,
-      nav_2d_utils::twist2Dto3D(twist),
-      goal_checkers_[current_goal_checker_].get());
-    last_valid_cmd_time_ = now();
-  } catch (nav2_core::PlannerException & e) {
-    if (failure_tolerance_ > 0 || failure_tolerance_ == -1.0) {
-      RCLCPP_WARN(this->get_logger(), e.what());
-      cmd_vel_2d.twist.angular.x = 0;
-      cmd_vel_2d.twist.angular.y = 0;
-      cmd_vel_2d.twist.angular.z = 0;
-      cmd_vel_2d.twist.linear.x = 0;
-      cmd_vel_2d.twist.linear.y = 0;
-      cmd_vel_2d.twist.linear.z = 0;
-      cmd_vel_2d.header.frame_id = costmap_ros_->getBaseFrameID();
-      cmd_vel_2d.header.stamp = now();
-      if ((now() - last_valid_cmd_time_).seconds() > failure_tolerance_ &&
-        failure_tolerance_ != -1.0)
-      {
-        throw nav2_core::PlannerException("Controller patience exceeded");
+    try {
+      cmd_vel_2d =
+        controllers_[current_controller_]->computeVelocityCommands(
+        pose,
+        nav_2d_utils::twist2Dto3D(twist),
+        goal_checkers_[current_goal_checker_].get());
+      last_valid_cmd_time_ = now();
+    } catch (nav2_core::PlannerException & e) {
+      if (failure_tolerance_ > 0 || failure_tolerance_ == -1.0) {
+        RCLCPP_WARN(this->get_logger(), e.what());
+        cmd_vel_2d.twist.angular.x = 0;
+        cmd_vel_2d.twist.angular.y = 0;
+        cmd_vel_2d.twist.angular.z = 0;
+        cmd_vel_2d.twist.linear.x = 0;
+        cmd_vel_2d.twist.linear.y = 0;
+        cmd_vel_2d.twist.linear.z = 0;
+        cmd_vel_2d.header.frame_id = costmap_ros_->getBaseFrameID();
+        cmd_vel_2d.header.stamp = now();
+        if ((now() - last_valid_cmd_time_).seconds() > failure_tolerance_ &&
+          failure_tolerance_ != -1.0)
+        {
+          throw nav2_core::PlannerException("Controller patience exceeded");
+        }
+      } else {
+        throw nav2_core::PlannerException(e.what());
       }
-    } else {
-      throw nav2_core::PlannerException(e.what());
     }
+  } else {
+    // Pause navigation due to safety status not OK
+    progress_checker_->reset();
+    last_valid_cmd_time_ = now();
+    cmd_vel_2d.twist.angular.x = 0;
+    cmd_vel_2d.twist.angular.y = 0;
+    cmd_vel_2d.twist.angular.z = 0;
+    cmd_vel_2d.twist.linear.x = 0;
+    cmd_vel_2d.twist.linear.y = 0;
+    cmd_vel_2d.twist.linear.z = 0;
+    cmd_vel_2d.header.frame_id = costmap_ros_->getBaseFrameID();
+    cmd_vel_2d.header.stamp = now();
+    // Setting the speed limit to -1 is an ugly hack to reset speed ramp in pure pursuit
+    controllers_[current_controller_]->setSpeedLimit(-1.0, false);
   }
 
   std::shared_ptr<Action::Feedback> feedback = std::make_shared<Action::Feedback>();
@@ -581,4 +641,9 @@ void ControllerServer::speedLimitCallback(const nav2_msgs::msg::SpeedLimit::Shar
   }
 }
 
+void ControllerServer::safetyStatusCallback(const relox_msgs::msg::SafetyStatus::SharedPtr msg)
+{
+  is_safety_status_ok_ = msg->status == relox_msgs::msg::SafetyStatus::OK;
+}
+
 }  // namespace nav2_controller