From cb5f2862a4d4705808723d085abaeffa6cd7c76e Mon Sep 17 00:00:00 2001
From: Cynthia Li <cynthia.li@rapyuta-robotics.com>
Date: Thu, 13 Jul 2023 10:39:55 +0900
Subject: [PATCH 01/10] created velocity command check feature

---
 .../abstract_controller_execution.h           | 14 ++++
 .../src/abstract_controller_execution.cpp     | 66 +++++++++++++++++++
 2 files changed, 80 insertions(+)

diff --git a/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h
index ddcdbdb5..40b2ab13 100644
--- a/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h
+++ b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h
@@ -219,6 +219,14 @@ namespace mbf_abstract_nav
      */
     void setVelocityCmd(const geometry_msgs::TwistStamped &vel_cmd_stamped);
 
+    /**
+     * @brief Check if the robot's actual velocity is following the cmd_vel
+     * @param robot_velocity the robot's actual velocity
+     * @param cmd_velocity the commanded velocity to the robot
+     * @return true if there is a mismatched, false otherwise
+    */
+    bool checkVelocityDiff(const geometry_msgs::TwistStamped &robot_velocity, const geometry_msgs::TwistStamped &cmd_velocity);
+
     //! the name of the loaded plugin
     std::string plugin_name_;
 
@@ -234,6 +242,9 @@ namespace mbf_abstract_nav
     //! The time the controller responded with a success output (output < 10).
     ros::Time last_valid_cmd_time_;
 
+    //The first time when the actual robot velocity is abnormal comparing to the velocity command
+    ros::Time first_mismatched_time_;
+
     //! The maximum number of retries
     int max_retries_;
 
@@ -364,6 +375,9 @@ namespace mbf_abstract_nav
 
     //! replaces parameter angle_tolerance_ for the action
     double action_angle_tolerance_;
+
+    int disabled_tolerance_sec_;
+
   };
 
 } /* namespace mbf_abstract_nav */
diff --git a/mbf_abstract_nav/src/abstract_controller_execution.cpp b/mbf_abstract_nav/src/abstract_controller_execution.cpp
index ccde4dfb..95099ca3 100644
--- a/mbf_abstract_nav/src/abstract_controller_execution.cpp
+++ b/mbf_abstract_nav/src/abstract_controller_execution.cpp
@@ -72,6 +72,7 @@ AbstractControllerExecution::AbstractControllerExecution(
   private_nh.param("dist_tolerance", dist_tolerance_, 0.1);
   private_nh.param("angle_tolerance", angle_tolerance_, M_PI / 18.0);
   private_nh.param("tf_timeout", tf_timeout_, 1.0);
+  private_nh.param("disabled_tolerance_sec", disabled_tolerance_sec_, 5);
 
   // dynamically reconfigurable parameters
   reconfigure(config);
@@ -185,6 +186,37 @@ void AbstractControllerExecution::setVelocityCmd(const geometry_msgs::TwistStamp
   // TODO so there should be no loss of information in the feedback stream
 }
 
+bool AbstractControllerExecution::checkVelocityDiff(const geometry_msgs::TwistStamped &robot_velocity,
+                                                        const geometry_msgs::TwistStamped &cmd_velocity)
+{
+  //linear total and angular total
+  double robot_linear_total;
+  double robot_angular_total;
+
+  double cmd_linear_total;
+  double cmd_angular_total;
+
+  //compute linear velocity magnitude
+  robot_linear_total = sqrt(pow(robot_velocity.twist.linear.x,2.0) + pow(robot_velocity.twist.linear.y,2.0) + 
+                            pow(robot_velocity.twist.linear.z,2.0));
+  cmd_linear_total = sqrt(pow(cmd_velocity.twist.linear.x,2.0) + pow(cmd_velocity.twist.linear.y,2.0) + pow(cmd_velocity.twist.linear.z,2.0));
+  
+  //compute angular velocity magnitude
+  robot_angular_total = sqrt(pow(robot_velocity.twist.angular.x,2.0) + 
+                             pow(robot_velocity.twist.angular.y,2.0) + pow(robot_velocity.twist.angular.z,2.0));
+  cmd_angular_total = sqrt(pow(cmd_velocity.twist.angular.x,2.0) + pow(cmd_velocity.twist.angular.y,2.0) + pow(cmd_velocity.twist.angular.z,2.0));
+
+  bool diff = false;
+
+  if (robot_linear_total+robot_angular_total < 1e-06) //if robot is stopped/disabled
+  { 
+    if (cmd_linear_total+cmd_angular_total > 0.01) //but there is non-zero velocity command
+      ROS_INFO("robot velocity mismatched");
+      diff = true; //we have a mismatched
+  }
+  return diff;
+}
+
 geometry_msgs::TwistStamped AbstractControllerExecution::getVelocityCmd() const
 {
   boost::lock_guard<boost::mutex> guard(vel_cmd_mtx_);
@@ -278,6 +310,8 @@ void AbstractControllerExecution::run()
   last_valid_cmd_time_ = ros::Time();
   int retries = 0;
   int seq = 0;
+  first_mismatched_time_ = ros::Time();
+  bool vel_check = false;
 
   try
   {
@@ -301,6 +335,7 @@ void AbstractControllerExecution::run()
         // cannot tell what the problem is, but anyway we command the robot to stop to avoid crashes
         publishZeroVelocity();
         loop_rate_.sleep();
+        vel_check = false;
         continue;
       }
 
@@ -352,6 +387,7 @@ void AbstractControllerExecution::run()
         setState(ARRIVED_GOAL);
         // goal reached, tell it the controller
         moving_ = false;
+        vel_check = false;
         condition_.notify_all();
         // if not, keep moving
       }
@@ -368,6 +404,32 @@ void AbstractControllerExecution::run()
         geometry_msgs::TwistStamped cmd_vel_stamped;
         geometry_msgs::TwistStamped robot_velocity;
         robot_info_.getRobotVelocity(robot_velocity);
+
+        if (vel_check) //if we need to check the velocity
+        {
+          bool mismatched = checkVelocityDiff(robot_velocity, getVelocityCmd()); //check if there is a mismatch
+          if (!mismatched) 
+          //if there is no abnormality, reset the first mismatched time
+          { 
+            ROS_DEBUG("Robot velocity check is ok");
+            first_mismatched_time_ = ros::Time::now();
+          }
+          else //if there is abnormality
+          {
+            if (ros::Time::now()-first_mismatched_time_ > ros::Duration(disabled_tolerance_sec_))
+            //the robot is not behaving as it should for more than 5 seconds
+            {
+              ROS_ERROR("Robot is not moving and it does not follow the velocity command for %d seconds.Please check the state of the robot!",
+              disabled_tolerance_sec_);
+              first_mismatched_time_ = ros::Time::now(); //reset to check again if needed
+            }
+          }
+        }
+        else //if no need to check the robot velocity, reset the first mismatched time
+        {
+          first_mismatched_time_ = ros::Time::now();
+        }
+
         outcome_ = computeVelocityCmd(robot_pose_, robot_velocity, cmd_vel_stamped, message_ = "");
 
         if (outcome_ < 10)
@@ -376,6 +438,7 @@ void AbstractControllerExecution::run()
           vel_pub_.publish(cmd_vel_stamped.twist);
           last_valid_cmd_time_ = ros::Time::now();
           retries = 0;
+          vel_check = true;
         }
         else if (outcome_ == mbf_msgs::ExePathResult::CANCELED)
         {
@@ -402,18 +465,21 @@ void AbstractControllerExecution::run()
             setState(NO_LOCAL_CMD); // useful for server feedback
             // keep trying if we have > 0 or -1 (infinite) retries
             moving_ = max_retries_;
+            vel_check = false;
           }
 
           // could not compute a valid velocity command
           if (!moving_ || force_stop_on_retry_)
           {
             publishZeroVelocity(); // command the robot to stop; we still feedback command calculated by the plugin
+            vel_check = false;
           }
           else
           {
             // we are retrying compute velocity commands; we keep sending the command calculated by the plugin
             // with the expectation that it's a sensible one (e.g. slow down while respecting acceleration limits)
             vel_pub_.publish(cmd_vel_stamped.twist);
+            vel_check = true;
           }
         }
 

From 924c598de5b9e1d989c516f6c09dcece6c53fd12 Mon Sep 17 00:00:00 2001
From: Cynthia Li <cynthia.li@rapyuta-robotics.com>
Date: Thu, 13 Jul 2023 14:04:07 +0900
Subject: [PATCH 02/10] fixed formatting

---
 .../abstract_controller_execution.h           |  5 +-
 .../src/abstract_controller_execution.cpp     | 61 ++++++++++---------
 2 files changed, 36 insertions(+), 30 deletions(-)

diff --git a/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h
index 40b2ab13..57cf3c49 100644
--- a/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h
+++ b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h
@@ -224,8 +224,9 @@ namespace mbf_abstract_nav
      * @param robot_velocity the robot's actual velocity
      * @param cmd_velocity the commanded velocity to the robot
      * @return true if there is a mismatched, false otherwise
-    */
-    bool checkVelocityDiff(const geometry_msgs::TwistStamped &robot_velocity, const geometry_msgs::TwistStamped &cmd_velocity);
+     */
+    bool checkVelocityDiff(const geometry_msgs::TwistStamped& robot_velocity,
+                           const geometry_msgs::TwistStamped& cmd_velocity);
 
     //! the name of the loaded plugin
     std::string plugin_name_;
diff --git a/mbf_abstract_nav/src/abstract_controller_execution.cpp b/mbf_abstract_nav/src/abstract_controller_execution.cpp
index 95099ca3..1622c803 100644
--- a/mbf_abstract_nav/src/abstract_controller_execution.cpp
+++ b/mbf_abstract_nav/src/abstract_controller_execution.cpp
@@ -186,33 +186,37 @@ void AbstractControllerExecution::setVelocityCmd(const geometry_msgs::TwistStamp
   // TODO so there should be no loss of information in the feedback stream
 }
 
-bool AbstractControllerExecution::checkVelocityDiff(const geometry_msgs::TwistStamped &robot_velocity,
-                                                        const geometry_msgs::TwistStamped &cmd_velocity)
+bool AbstractControllerExecution::checkVelocityDiff(const geometry_msgs::TwistStamped& robot_velocity,
+                                                    const geometry_msgs::TwistStamped& cmd_velocity)
 {
-  //linear total and angular total
+  // linear total and angular total
   double robot_linear_total;
   double robot_angular_total;
 
   double cmd_linear_total;
   double cmd_angular_total;
 
-  //compute linear velocity magnitude
-  robot_linear_total = sqrt(pow(robot_velocity.twist.linear.x,2.0) + pow(robot_velocity.twist.linear.y,2.0) + 
-                            pow(robot_velocity.twist.linear.z,2.0));
-  cmd_linear_total = sqrt(pow(cmd_velocity.twist.linear.x,2.0) + pow(cmd_velocity.twist.linear.y,2.0) + pow(cmd_velocity.twist.linear.z,2.0));
-  
-  //compute angular velocity magnitude
-  robot_angular_total = sqrt(pow(robot_velocity.twist.angular.x,2.0) + 
-                             pow(robot_velocity.twist.angular.y,2.0) + pow(robot_velocity.twist.angular.z,2.0));
-  cmd_angular_total = sqrt(pow(cmd_velocity.twist.angular.x,2.0) + pow(cmd_velocity.twist.angular.y,2.0) + pow(cmd_velocity.twist.angular.z,2.0));
+  // compute linear velocity magnitude
+  robot_linear_total = sqrt(pow(robot_velocity.twist.linear.x, 2.0) + pow(robot_velocity.twist.linear.y, 2.0) +
+                            pow(robot_velocity.twist.linear.z, 2.0));
+  cmd_linear_total = sqrt(pow(cmd_velocity.twist.linear.x, 2.0) + pow(cmd_velocity.twist.linear.y, 2.0) +
+                          pow(cmd_velocity.twist.linear.z, 2.0));
+
+  // compute angular velocity magnitude
+  robot_angular_total = sqrt(pow(robot_velocity.twist.angular.x, 2.0) + pow(robot_velocity.twist.angular.y, 2.0) +
+                             pow(robot_velocity.twist.angular.z, 2.0));
+  cmd_angular_total = sqrt(pow(cmd_velocity.twist.angular.x, 2.0) + pow(cmd_velocity.twist.angular.y, 2.0) +
+                           pow(cmd_velocity.twist.angular.z, 2.0));
 
   bool diff = false;
 
-  if (robot_linear_total+robot_angular_total < 1e-06) //if robot is stopped/disabled
-  { 
-    if (cmd_linear_total+cmd_angular_total > 0.01) //but there is non-zero velocity command
+  if (robot_linear_total + robot_angular_total < 1e-06)  // if robot is stopped/disabled
+  {
+    if (cmd_linear_total + cmd_angular_total > 0.01)
+    { // but if there is non-zero velocity command
       ROS_INFO("robot velocity mismatched");
-      diff = true; //we have a mismatched
+      diff = true;  // we have a mismatched
+    }
   }
   return diff;
 }
@@ -405,27 +409,28 @@ void AbstractControllerExecution::run()
         geometry_msgs::TwistStamped robot_velocity;
         robot_info_.getRobotVelocity(robot_velocity);
 
-        if (vel_check) //if we need to check the velocity
+        if (vel_check) // if we need to check the velocity
         {
-          bool mismatched = checkVelocityDiff(robot_velocity, getVelocityCmd()); //check if there is a mismatch
-          if (!mismatched) 
-          //if there is no abnormality, reset the first mismatched time
-          { 
+          bool mismatched = checkVelocityDiff(robot_velocity, getVelocityCmd());  // check if there is a mismatch
+          if (!mismatched)
+          // if there is no abnormality, reset the first mismatched time
+          {
             ROS_DEBUG("Robot velocity check is ok");
             first_mismatched_time_ = ros::Time::now();
           }
-          else //if there is abnormality
+          else  // if there is abnormality
           {
-            if (ros::Time::now()-first_mismatched_time_ > ros::Duration(disabled_tolerance_sec_))
-            //the robot is not behaving as it should for more than 5 seconds
+            if (ros::Time::now() - first_mismatched_time_ > ros::Duration(disabled_tolerance_sec_))
+            // the robot is not behaving as it should for more than 5 seconds
             {
-              ROS_ERROR("Robot is not moving and it does not follow the velocity command for %d seconds.Please check the state of the robot!",
-              disabled_tolerance_sec_);
-              first_mismatched_time_ = ros::Time::now(); //reset to check again if needed
+              ROS_ERROR("Robot is not moving and does not follow the velocity command for %d seconds.Please check "
+                        "the state of the robot!",
+                        disabled_tolerance_sec_);
+              first_mismatched_time_ = ros::Time::now();  // reset to check again if needed
             }
           }
         }
-        else //if no need to check the robot velocity, reset the first mismatched time
+        else  // if no need to check the robot velocity, reset the first mismatched time
         {
           first_mismatched_time_ = ros::Time::now();
         }

From 21d738cd5c0dbcd9c7f92da93d34477f6771d85d Mon Sep 17 00:00:00 2001
From: Cynthia Li <cynthia.li@rapyuta-robotics.com>
Date: Fri, 14 Jul 2023 09:54:51 +0900
Subject: [PATCH 03/10] fixed velocity check logic and variable naming, added
 ROBOT_DISABLED controller state

---
 .../abstract_controller_execution.h           |  50 ++++----
 .../src/abstract_controller_execution.cpp     | 108 ++++++++----------
 mbf_abstract_nav/src/controller_action.cpp    |   7 ++
 3 files changed, 81 insertions(+), 84 deletions(-)

diff --git a/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h
index 57cf3c49..5a434f4f 100644
--- a/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h
+++ b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h
@@ -135,20 +135,21 @@ namespace mbf_abstract_nav
      */
     enum ControllerState
     {
-      INITIALIZED,  ///< Controller has been initialized successfully.
-      STARTED,      ///< Controller has been started.
-      PLANNING,     ///< Executing the plugin.
-      NO_PLAN,      ///< The controller has been started without a plan.
-      MAX_RETRIES,  ///< Exceeded the maximum number of retries without a valid command.
-      PAT_EXCEEDED, ///< Exceeded the patience time without a valid command.
-      EMPTY_PLAN,   ///< Received an empty plan.
-      INVALID_PLAN, ///< Received an invalid plan that the controller plugin rejected.
-      NO_LOCAL_CMD, ///< Received no velocity command by the plugin, in the current cycle.
-      GOT_LOCAL_CMD,///< Got a valid velocity command from the plugin.
-      ARRIVED_GOAL, ///< The robot arrived the goal.
-      CANCELED,     ///< The controller has been canceled.
-      STOPPED,      ///< The controller has been stopped!
-      INTERNAL_ERROR///< An internal error occurred.
+      INITIALIZED,     ///< Controller has been initialized successfully.
+      STARTED,         ///< Controller has been started.
+      PLANNING,        ///< Executing the plugin.
+      NO_PLAN,         ///< The controller has been started without a plan.
+      MAX_RETRIES,     ///< Exceeded the maximum number of retries without a valid command.
+      PAT_EXCEEDED,    ///< Exceeded the patience time without a valid command.
+      EMPTY_PLAN,      ///< Received an empty plan.
+      INVALID_PLAN,    ///< Received an invalid plan that the controller plugin rejected.
+      NO_LOCAL_CMD,    ///< Received no velocity command by the plugin, in the current cycle.
+      GOT_LOCAL_CMD,   ///< Got a valid velocity command from the plugin.
+      ARRIVED_GOAL,    ///< The robot arrived the goal.
+      CANCELED,        ///< The controller has been canceled.
+      STOPPED,         ///< The controller has been stopped!
+      INTERNAL_ERROR,  ///< An internal error occurred.
+      ROBOT_DISABLED   ///< The robot is stuck and ignored velocity command
     };
 
     /**
@@ -220,13 +221,12 @@ namespace mbf_abstract_nav
     void setVelocityCmd(const geometry_msgs::TwistStamped &vel_cmd_stamped);
 
     /**
-     * @brief Check if the robot's actual velocity is following the cmd_vel
-     * @param robot_velocity the robot's actual velocity
-     * @param cmd_velocity the commanded velocity to the robot
-     * @return true if there is a mismatched, false otherwise
+     * @brief Check if the robot's actual velocity is ignoring the cmd_vel
+     * @param robot_stopped true if robot is stopped, false otherwise
+     * @param cmd_velocity the commanded velocity being published to the robot
+     * @return true if there the ignoring time exceeded tolerance, false otherwise
      */
-    bool checkVelocityDiff(const geometry_msgs::TwistStamped& robot_velocity,
-                           const geometry_msgs::TwistStamped& cmd_velocity);
+    bool checkVelocityIgnore(const bool& robot_stopped, const geometry_msgs::TwistStamped& cmd_velocity);
 
     //! the name of the loaded plugin
     std::string plugin_name_;
@@ -243,8 +243,8 @@ namespace mbf_abstract_nav
     //! The time the controller responded with a success output (output < 10).
     ros::Time last_valid_cmd_time_;
 
-    //The first time when the actual robot velocity is abnormal comparing to the velocity command
-    ros::Time first_mismatched_time_;
+    //! The first time when the actual robot velocity is abnormal comparing to the velocity command
+    ros::Time first_ignored_time_;
 
     //! The maximum number of retries
     int max_retries_;
@@ -377,7 +377,11 @@ namespace mbf_abstract_nav
     //! replaces parameter angle_tolerance_ for the action
     double action_angle_tolerance_;
 
-    int disabled_tolerance_sec_;
+    //! time duration for checking if the robot is ignoring cmd_vel
+    double robot_ignore_check_tolerance_;
+
+    //! robot ignore velocity feature enable/disable
+    bool robot_ignore_vel_enabled_;
 
   };
 
diff --git a/mbf_abstract_nav/src/abstract_controller_execution.cpp b/mbf_abstract_nav/src/abstract_controller_execution.cpp
index 1622c803..a5593f3b 100644
--- a/mbf_abstract_nav/src/abstract_controller_execution.cpp
+++ b/mbf_abstract_nav/src/abstract_controller_execution.cpp
@@ -72,7 +72,7 @@ AbstractControllerExecution::AbstractControllerExecution(
   private_nh.param("dist_tolerance", dist_tolerance_, 0.1);
   private_nh.param("angle_tolerance", angle_tolerance_, M_PI / 18.0);
   private_nh.param("tf_timeout", tf_timeout_, 1.0);
-  private_nh.param("disabled_tolerance_sec", disabled_tolerance_sec_, 5);
+  private_nh.param("ignore_check_tolerance", robot_ignore_check_tolerance_, 5.0);
 
   // dynamically reconfigurable parameters
   reconfigure(config);
@@ -186,39 +186,49 @@ void AbstractControllerExecution::setVelocityCmd(const geometry_msgs::TwistStamp
   // TODO so there should be no loss of information in the feedback stream
 }
 
-bool AbstractControllerExecution::checkVelocityDiff(const geometry_msgs::TwistStamped& robot_velocity,
-                                                    const geometry_msgs::TwistStamped& cmd_velocity)
+bool AbstractControllerExecution::checkVelocityIgnore(const bool& robot_stopped,
+                                                      const geometry_msgs::TwistStamped& cmd_velocity)
 {
-  // linear total and angular total
-  double robot_linear_total;
-  double robot_angular_total;
+  bool ignored_exceed_tolerance = false;
 
-  double cmd_linear_total;
-  double cmd_angular_total;
-
-  // compute linear velocity magnitude
-  robot_linear_total = sqrt(pow(robot_velocity.twist.linear.x, 2.0) + pow(robot_velocity.twist.linear.y, 2.0) +
-                            pow(robot_velocity.twist.linear.z, 2.0));
-  cmd_linear_total = sqrt(pow(cmd_velocity.twist.linear.x, 2.0) + pow(cmd_velocity.twist.linear.y, 2.0) +
-                          pow(cmd_velocity.twist.linear.z, 2.0));
+  if (robot_stopped)
+  {
+    // linear total and angular
+    double cmd_linear;
+    double cmd_angular;
 
-  // compute angular velocity magnitude
-  robot_angular_total = sqrt(pow(robot_velocity.twist.angular.x, 2.0) + pow(robot_velocity.twist.angular.y, 2.0) +
-                             pow(robot_velocity.twist.angular.z, 2.0));
-  cmd_angular_total = sqrt(pow(cmd_velocity.twist.angular.x, 2.0) + pow(cmd_velocity.twist.angular.y, 2.0) +
-                           pow(cmd_velocity.twist.angular.z, 2.0));
+    // compute linear velocity magnitude
+    cmd_linear = hypot(cmd_velocity.twist.linear.x, cmd_velocity.twist.linear.y);
+    // compute angular velocity magnitude
+    cmd_angular = fabs(cmd_velocity.twist.angular.z);
 
-  bool diff = false;
+    if (cmd_linear > 0.01 || cmd_angular > 0.01)  // if cmd_vel is non-zero
+    {
+      // output robot ignored velocity command warning message
+      double ignored_duration = (ros::Time::now() - first_ignored_time_).toSec();
+      ROS_WARN_THROTTLE(1, "Robot ignores velocity command for %.2f seconds", ignored_duration);
 
-  if (robot_linear_total + robot_angular_total < 1e-06)  // if robot is stopped/disabled
-  {
-    if (cmd_linear_total + cmd_angular_total > 0.01)
-    { // but if there is non-zero velocity command
-      ROS_INFO("robot velocity mismatched");
-      diff = true;  // we have a mismatched
+      if (ignored_duration > robot_ignore_check_tolerance_)
+      {
+        // the robot is ignoring the velocity command exceed the threshold time
+        ROS_ERROR("Robot is disabled and the time it ignored velocity command exceeded the tolerance: %.2f seconds",
+                  robot_ignore_check_tolerance_);
+        ignored_exceed_tolerance = true;
+      }
     }
+    else  // robot stopped but velocity command is also close to zero
+    {
+      ROS_DEBUG("Robot ignore velocity command check is ok");
+      first_ignored_time_ = ros::Time::now();
+    }
+  }
+  else  // robot is moving, not need to check
+  {
+    ROS_DEBUG("Robot ignore velocity command check is ok");
+    first_ignored_time_ = ros::Time::now();
   }
-  return diff;
+
+  return ignored_exceed_tolerance;
 }
 
 geometry_msgs::TwistStamped AbstractControllerExecution::getVelocityCmd() const
@@ -314,9 +324,11 @@ void AbstractControllerExecution::run()
   last_valid_cmd_time_ = ros::Time();
   int retries = 0;
   int seq = 0;
-  first_mismatched_time_ = ros::Time();
-  bool vel_check = false;
+  first_ignored_time_ = ros::Time::now();
 
+  //check if the velocity ignore check is enabled or not
+  robot_ignore_vel_enabled_ = (robot_ignore_check_tolerance_ > 0.0);
+ 
   try
   {
     while (moving_ && ros::ok())
@@ -339,7 +351,6 @@ void AbstractControllerExecution::run()
         // cannot tell what the problem is, but anyway we command the robot to stop to avoid crashes
         publishZeroVelocity();
         loop_rate_.sleep();
-        vel_check = false;
         continue;
       }
 
@@ -391,7 +402,6 @@ void AbstractControllerExecution::run()
         setState(ARRIVED_GOAL);
         // goal reached, tell it the controller
         moving_ = false;
-        vel_check = false;
         condition_.notify_all();
         // if not, keep moving
       }
@@ -409,32 +419,6 @@ void AbstractControllerExecution::run()
         geometry_msgs::TwistStamped robot_velocity;
         robot_info_.getRobotVelocity(robot_velocity);
 
-        if (vel_check) // if we need to check the velocity
-        {
-          bool mismatched = checkVelocityDiff(robot_velocity, getVelocityCmd());  // check if there is a mismatch
-          if (!mismatched)
-          // if there is no abnormality, reset the first mismatched time
-          {
-            ROS_DEBUG("Robot velocity check is ok");
-            first_mismatched_time_ = ros::Time::now();
-          }
-          else  // if there is abnormality
-          {
-            if (ros::Time::now() - first_mismatched_time_ > ros::Duration(disabled_tolerance_sec_))
-            // the robot is not behaving as it should for more than 5 seconds
-            {
-              ROS_ERROR("Robot is not moving and does not follow the velocity command for %d seconds.Please check "
-                        "the state of the robot!",
-                        disabled_tolerance_sec_);
-              first_mismatched_time_ = ros::Time::now();  // reset to check again if needed
-            }
-          }
-        }
-        else  // if no need to check the robot velocity, reset the first mismatched time
-        {
-          first_mismatched_time_ = ros::Time::now();
-        }
-
         outcome_ = computeVelocityCmd(robot_pose_, robot_velocity, cmd_vel_stamped, message_ = "");
 
         if (outcome_ < 10)
@@ -443,7 +427,12 @@ void AbstractControllerExecution::run()
           vel_pub_.publish(cmd_vel_stamped.twist);
           last_valid_cmd_time_ = ros::Time::now();
           retries = 0;
-          vel_check = true;
+          // check if robot ignores velocity command
+          if (robot_ignore_vel_enabled_ && checkVelocityIgnore(robot_info_.isRobotStopped(1e-3, 1e-3), cmd_vel_stamped))
+          {
+            setState(ROBOT_DISABLED);
+            moving_ = false;
+          }
         }
         else if (outcome_ == mbf_msgs::ExePathResult::CANCELED)
         {
@@ -470,21 +459,18 @@ void AbstractControllerExecution::run()
             setState(NO_LOCAL_CMD); // useful for server feedback
             // keep trying if we have > 0 or -1 (infinite) retries
             moving_ = max_retries_;
-            vel_check = false;
           }
 
           // could not compute a valid velocity command
           if (!moving_ || force_stop_on_retry_)
           {
             publishZeroVelocity(); // command the robot to stop; we still feedback command calculated by the plugin
-            vel_check = false;
           }
           else
           {
             // we are retrying compute velocity commands; we keep sending the command calculated by the plugin
             // with the expectation that it's a sensible one (e.g. slow down while respecting acceleration limits)
             vel_pub_.publish(cmd_vel_stamped.twist);
-            vel_check = true;
           }
         }
 
diff --git a/mbf_abstract_nav/src/controller_action.cpp b/mbf_abstract_nav/src/controller_action.cpp
index bd0305aa..0315946a 100644
--- a/mbf_abstract_nav/src/controller_action.cpp
+++ b/mbf_abstract_nav/src/controller_action.cpp
@@ -310,6 +310,13 @@ void ControllerAction::runImpl(GoalHandle &goal_handle, AbstractControllerExecut
         goal_handle.setAborted(result, result.message);
         break;
 
+      case AbstractControllerExecution::ROBOT_DISABLED:
+        ROS_FATAL_STREAM_NAMED(name_, "Robot stopped and ignored velocity commands!");
+        controller_active = false;
+        fillExePathResult(mbf_msgs::ExePathResult::STOPPED,"Robot disabled and ignored velocity commands: ",result);
+        goal_handle.setAborted(result, result.message);
+        break;
+
       default:
         std::stringstream ss;
         ss << "Internal error: Unknown state in a move base flex controller execution with the number: "

From 589e11886306c3dd241707d6f4257586a9b44302 Mon Sep 17 00:00:00 2001
From: Cynthia Li <cynthia.li@rapyuta-robotics.com>
Date: Tue, 18 Jul 2023 13:13:09 +0900
Subject: [PATCH 04/10] Fix: address Tiago's comments

---
 .../abstract_controller_execution.h           | 12 ++--
 .../src/abstract_controller_execution.cpp     | 64 ++++++++-----------
 mbf_abstract_nav/src/controller_action.cpp    |  2 +-
 3 files changed, 33 insertions(+), 45 deletions(-)

diff --git a/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h
index 5a434f4f..8e2ec32c 100644
--- a/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h
+++ b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h
@@ -221,12 +221,11 @@ namespace mbf_abstract_nav
     void setVelocityCmd(const geometry_msgs::TwistStamped &vel_cmd_stamped);
 
     /**
-     * @brief Check if the robot's actual velocity is ignoring the cmd_vel
-     * @param robot_stopped true if robot is stopped, false otherwise
-     * @param cmd_velocity the commanded velocity being published to the robot
-     * @return true if there the ignoring time exceeded tolerance, false otherwise
+     * @brief Check if the robot is ignoring the cmd_vel longer than threshold time
+     * @param cmd_velocity the latest cmd_vel being published to the robot
+     * @return true if cmd_vel is being ignored by the robot longer than tolerance time, false otherwise
      */
-    bool checkVelocityIgnore(const bool& robot_stopped, const geometry_msgs::TwistStamped& cmd_velocity);
+    bool checkVelocityIgnore(const geometry_msgs::Twist& cmd_velocity);
 
     //! the name of the loaded plugin
     std::string plugin_name_;
@@ -380,9 +379,6 @@ namespace mbf_abstract_nav
     //! time duration for checking if the robot is ignoring cmd_vel
     double robot_ignore_check_tolerance_;
 
-    //! robot ignore velocity feature enable/disable
-    bool robot_ignore_vel_enabled_;
-
   };
 
 } /* namespace mbf_abstract_nav */
diff --git a/mbf_abstract_nav/src/abstract_controller_execution.cpp b/mbf_abstract_nav/src/abstract_controller_execution.cpp
index a5593f3b..b4bca08d 100644
--- a/mbf_abstract_nav/src/abstract_controller_execution.cpp
+++ b/mbf_abstract_nav/src/abstract_controller_execution.cpp
@@ -72,7 +72,7 @@ AbstractControllerExecution::AbstractControllerExecution(
   private_nh.param("dist_tolerance", dist_tolerance_, 0.1);
   private_nh.param("angle_tolerance", angle_tolerance_, M_PI / 18.0);
   private_nh.param("tf_timeout", tf_timeout_, 1.0);
-  private_nh.param("ignore_check_tolerance", robot_ignore_check_tolerance_, 5.0);
+  private_nh.param("robot_ignore_check_tolerance", robot_ignore_check_tolerance_, 5.0);
 
   // dynamically reconfigurable parameters
   reconfigure(config);
@@ -186,46 +186,38 @@ void AbstractControllerExecution::setVelocityCmd(const geometry_msgs::TwistStamp
   // TODO so there should be no loss of information in the feedback stream
 }
 
-bool AbstractControllerExecution::checkVelocityIgnore(const bool& robot_stopped,
-                                                      const geometry_msgs::TwistStamped& cmd_velocity)
+bool AbstractControllerExecution::checkVelocityIgnore(const geometry_msgs::Twist& cmd_velocity)
 {
   bool ignored_exceed_tolerance = false;
 
-  if (robot_stopped)
-  {
-    // linear total and angular
-    double cmd_linear;
-    double cmd_angular;
+  bool robot_stopped = robot_info_.isRobotStopped(1e-3, 1e-3);
 
-    // compute linear velocity magnitude
-    cmd_linear = hypot(cmd_velocity.twist.linear.x, cmd_velocity.twist.linear.y);
-    // compute angular velocity magnitude
-    cmd_angular = fabs(cmd_velocity.twist.angular.z);
+  // compute linear and angular velocity magnitude
+  const double cmd_linear = std::hypot(cmd_velocity.linear.x, cmd_velocity.linear.y);
+  const double cmd_angular = std::abs(cmd_velocity.angular.z);
 
-    if (cmd_linear > 0.01 || cmd_angular > 0.01)  // if cmd_vel is non-zero
-    {
-      // output robot ignored velocity command warning message
-      double ignored_duration = (ros::Time::now() - first_ignored_time_).toSec();
-      ROS_WARN_THROTTLE(1, "Robot ignores velocity command for %.2f seconds", ignored_duration);
+  bool cmd_is_not_zero = (cmd_linear > 0.01 || cmd_angular > 0.01);
 
-      if (ignored_duration > robot_ignore_check_tolerance_)
-      {
-        // the robot is ignoring the velocity command exceed the threshold time
-        ROS_ERROR("Robot is disabled and the time it ignored velocity command exceeded the tolerance: %.2f seconds",
-                  robot_ignore_check_tolerance_);
-        ignored_exceed_tolerance = true;
-      }
-    }
-    else  // robot stopped but velocity command is also close to zero
-    {
-      ROS_DEBUG("Robot ignore velocity command check is ok");
-      first_ignored_time_ = ros::Time::now();
-    }
-  }
-  else  // robot is moving, not need to check
+  // no need to check
+  if (!robot_stopped || !cmd_is_not_zero)
   {
-    ROS_DEBUG("Robot ignore velocity command check is ok");
     first_ignored_time_ = ros::Time::now();
+    return false;
+  }
+
+  // check if robot ignores the cmd_vel
+  double ignored_duration = (ros::Time::now() - first_ignored_time_).toSec();
+  ROS_WARN_THROTTLE(1,
+                    "Robot ignores velocity command for %.2f seconds.\n The cmd_vel being ignored is: (x=%.2f, "
+                    "y=%.2f, w=%.2f)",
+                    ignored_duration, cmd_velocity.linear.x, cmd_velocity.linear.y, cmd_velocity.angular.z);
+
+  if (ignored_duration > robot_ignore_check_tolerance_)
+  {
+    // the robot is ignoring the velocity command more the threshold time
+    ROS_ERROR("Robot is disabled and the time it ignored velocity command exceeded the tolerance time: %.2f seconds",
+              robot_ignore_check_tolerance_);
+    ignored_exceed_tolerance = true;  // return true
   }
 
   return ignored_exceed_tolerance;
@@ -326,8 +318,8 @@ void AbstractControllerExecution::run()
   int seq = 0;
   first_ignored_time_ = ros::Time::now();
 
-  //check if the velocity ignore check is enabled or not
-  robot_ignore_vel_enabled_ = (robot_ignore_check_tolerance_ > 0.0);
+  // check if the velocity ignore check is enabled or not
+  bool robot_ignore_vel_enabled = (robot_ignore_check_tolerance_ > 0.0);
  
   try
   {
@@ -428,7 +420,7 @@ void AbstractControllerExecution::run()
           last_valid_cmd_time_ = ros::Time::now();
           retries = 0;
           // check if robot ignores velocity command
-          if (robot_ignore_vel_enabled_ && checkVelocityIgnore(robot_info_.isRobotStopped(1e-3, 1e-3), cmd_vel_stamped))
+          if (robot_ignore_vel_enabled && checkVelocityIgnore(cmd_vel_stamped.twist))
           {
             setState(ROBOT_DISABLED);
             moving_ = false;
diff --git a/mbf_abstract_nav/src/controller_action.cpp b/mbf_abstract_nav/src/controller_action.cpp
index 0315946a..389c5ad5 100644
--- a/mbf_abstract_nav/src/controller_action.cpp
+++ b/mbf_abstract_nav/src/controller_action.cpp
@@ -313,7 +313,7 @@ void ControllerAction::runImpl(GoalHandle &goal_handle, AbstractControllerExecut
       case AbstractControllerExecution::ROBOT_DISABLED:
         ROS_FATAL_STREAM_NAMED(name_, "Robot stopped and ignored velocity commands!");
         controller_active = false;
-        fillExePathResult(mbf_msgs::ExePathResult::STOPPED,"Robot disabled and ignored velocity commands: ",result);
+        fillExePathResult(mbf_msgs::ExePathResult::ROBOT_STUCK,"Robot disabled and ignored velocity commands: ",result);
         goal_handle.setAborted(result, result.message);
         break;
 

From bb6eb9a56e8ba68bf7756663f74afef45add86c4 Mon Sep 17 00:00:00 2001
From: Cynthia Li <cynthia.li@rapyuta-robotics.com>
Date: Wed, 19 Jul 2023 11:50:34 +0900
Subject: [PATCH 05/10] fixed phrasing and logic

---
 .../abstract_controller_execution.h           |  8 ++--
 .../src/abstract_controller_execution.cpp     | 42 ++++++++++---------
 mbf_abstract_nav/src/controller_action.cpp    |  5 ++-
 3 files changed, 29 insertions(+), 26 deletions(-)

diff --git a/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h
index 8e2ec32c..80876b52 100644
--- a/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h
+++ b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h
@@ -222,10 +222,10 @@ namespace mbf_abstract_nav
 
     /**
      * @brief Check if the robot is ignoring the cmd_vel longer than threshold time
-     * @param cmd_velocity the latest cmd_vel being published to the robot
+     * @param cmd_vel the latest cmd_vel being published by the controller
      * @return true if cmd_vel is being ignored by the robot longer than tolerance time, false otherwise
      */
-    bool checkVelocityIgnore(const geometry_msgs::Twist& cmd_velocity);
+    bool checkCmdVelIgnored(const geometry_msgs::Twist& cmd_vel);
 
     //! the name of the loaded plugin
     std::string plugin_name_;
@@ -376,8 +376,8 @@ namespace mbf_abstract_nav
     //! replaces parameter angle_tolerance_ for the action
     double action_angle_tolerance_;
 
-    //! time duration for checking if the robot is ignoring cmd_vel
-    double robot_ignore_check_tolerance_;
+    //! time tolerance for checking if the robot is ignoring cmd_vel
+    double cmd_vel_ignored_tolerance_;
 
   };
 
diff --git a/mbf_abstract_nav/src/abstract_controller_execution.cpp b/mbf_abstract_nav/src/abstract_controller_execution.cpp
index b4bca08d..c5a415ba 100644
--- a/mbf_abstract_nav/src/abstract_controller_execution.cpp
+++ b/mbf_abstract_nav/src/abstract_controller_execution.cpp
@@ -72,7 +72,7 @@ AbstractControllerExecution::AbstractControllerExecution(
   private_nh.param("dist_tolerance", dist_tolerance_, 0.1);
   private_nh.param("angle_tolerance", angle_tolerance_, M_PI / 18.0);
   private_nh.param("tf_timeout", tf_timeout_, 1.0);
-  private_nh.param("robot_ignore_check_tolerance", robot_ignore_check_tolerance_, 5.0);
+  private_nh.param("cmd_vel_ignored_tolerance", cmd_vel_ignored_tolerance_, 5.0);
 
   // dynamically reconfigurable parameters
   reconfigure(config);
@@ -186,41 +186,45 @@ void AbstractControllerExecution::setVelocityCmd(const geometry_msgs::TwistStamp
   // TODO so there should be no loss of information in the feedback stream
 }
 
-bool AbstractControllerExecution::checkVelocityIgnore(const geometry_msgs::Twist& cmd_velocity)
+bool AbstractControllerExecution::checkCmdVelIgnored(const geometry_msgs::Twist& cmd_vel)
 {
-  bool ignored_exceed_tolerance = false;
+  // check if the velocity ignore check is enabled or not
+  if (cmd_vel_ignored_tolerance_ <= 0.0)
+  {
+    return false;
+  }
 
-  bool robot_stopped = robot_info_.isRobotStopped(1e-3, 1e-3);
+  const bool robot_stopped = robot_info_.isRobotStopped(1e-3, 1e-3);
 
   // compute linear and angular velocity magnitude
-  const double cmd_linear = std::hypot(cmd_velocity.linear.x, cmd_velocity.linear.y);
-  const double cmd_angular = std::abs(cmd_velocity.angular.z);
+  const double cmd_linear = std::hypot(cmd_vel.linear.x, cmd_vel.linear.y);
+  const double cmd_angular = std::abs(cmd_vel.angular.z);
 
-  bool cmd_is_not_zero = (cmd_linear > 0.01 || cmd_angular > 0.01);
+  const bool cmd_is_zero = (cmd_linear < 0.01 && cmd_angular < 0.01);
 
   // no need to check
-  if (!robot_stopped || !cmd_is_not_zero)
+  if (!robot_stopped || cmd_is_zero)
   {
     first_ignored_time_ = ros::Time::now();
     return false;
   }
 
   // check if robot ignores the cmd_vel
-  double ignored_duration = (ros::Time::now() - first_ignored_time_).toSec();
+  const double ignored_duration = (ros::Time::now() - first_ignored_time_).toSec();
   ROS_WARN_THROTTLE(1,
-                    "Robot ignores velocity command for %.2f seconds.\n The cmd_vel being ignored is: (x=%.2f, "
+                    "Robot is ignoring velocity command for %.2f seconds. (Commanded velocity: x=%.2f, "
                     "y=%.2f, w=%.2f)",
-                    ignored_duration, cmd_velocity.linear.x, cmd_velocity.linear.y, cmd_velocity.angular.z);
+                    ignored_duration, cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z);
 
-  if (ignored_duration > robot_ignore_check_tolerance_)
+  if (ignored_duration > cmd_vel_ignored_tolerance_)
   {
     // the robot is ignoring the velocity command more the threshold time
-    ROS_ERROR("Robot is disabled and the time it ignored velocity command exceeded the tolerance time: %.2f seconds",
-              robot_ignore_check_tolerance_);
-    ignored_exceed_tolerance = true;  // return true
+    ROS_ERROR("Robot is ignoring velocity command for more than the tolerance time: %.2f seconds",
+              cmd_vel_ignored_tolerance_);
+    return true;  // return true
   }
 
-  return ignored_exceed_tolerance;
+  return false;
 }
 
 geometry_msgs::TwistStamped AbstractControllerExecution::getVelocityCmd() const
@@ -318,8 +322,6 @@ void AbstractControllerExecution::run()
   int seq = 0;
   first_ignored_time_ = ros::Time::now();
 
-  // check if the velocity ignore check is enabled or not
-  bool robot_ignore_vel_enabled = (robot_ignore_check_tolerance_ > 0.0);
  
   try
   {
@@ -419,8 +421,8 @@ void AbstractControllerExecution::run()
           vel_pub_.publish(cmd_vel_stamped.twist);
           last_valid_cmd_time_ = ros::Time::now();
           retries = 0;
-          // check if robot ignores velocity command
-          if (robot_ignore_vel_enabled && checkVelocityIgnore(cmd_vel_stamped.twist))
+          // check if robot is ignoring velocity command
+          if (checkCmdVelIgnored(cmd_vel_stamped.twist))
           {
             setState(ROBOT_DISABLED);
             moving_ = false;
diff --git a/mbf_abstract_nav/src/controller_action.cpp b/mbf_abstract_nav/src/controller_action.cpp
index 389c5ad5..b5a7491e 100644
--- a/mbf_abstract_nav/src/controller_action.cpp
+++ b/mbf_abstract_nav/src/controller_action.cpp
@@ -311,9 +311,10 @@ void ControllerAction::runImpl(GoalHandle &goal_handle, AbstractControllerExecut
         break;
 
       case AbstractControllerExecution::ROBOT_DISABLED:
-        ROS_FATAL_STREAM_NAMED(name_, "Robot stopped and ignored velocity commands!");
+        ROS_FATAL_STREAM_NAMED(name_, "Robot ignored velocity commands for more than tolerance time");
         controller_active = false;
-        fillExePathResult(mbf_msgs::ExePathResult::ROBOT_STUCK,"Robot disabled and ignored velocity commands: ",result);
+        fillExePathResult(mbf_msgs::ExePathResult::ROBOT_STUCK,
+                          "Robot ignored velocity commands for more than tolerance time!", result);
         goal_handle.setAborted(result, result.message);
         break;
 

From 87f7c05e1a6e5a08197ed1ad603fcc3258cfc570 Mon Sep 17 00:00:00 2001
From: Cynthia Li <cynthia.li@rapyuta-robotics.com>
Date: Thu, 20 Jul 2023 09:27:30 +0900
Subject: [PATCH 06/10] edited commenting

---
 .../include/mbf_abstract_nav/abstract_controller_execution.h    | 2 +-
 mbf_abstract_nav/src/abstract_controller_execution.cpp          | 1 -
 2 files changed, 1 insertion(+), 2 deletions(-)

diff --git a/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h
index 80876b52..90dfb3b6 100644
--- a/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h
+++ b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h
@@ -242,7 +242,7 @@ namespace mbf_abstract_nav
     //! The time the controller responded with a success output (output < 10).
     ros::Time last_valid_cmd_time_;
 
-    //! The first time when the actual robot velocity is abnormal comparing to the velocity command
+    //! The first time when the robot is ignoring velocity command
     ros::Time first_ignored_time_;
 
     //! The maximum number of retries
diff --git a/mbf_abstract_nav/src/abstract_controller_execution.cpp b/mbf_abstract_nav/src/abstract_controller_execution.cpp
index c5a415ba..0eb4a4a4 100644
--- a/mbf_abstract_nav/src/abstract_controller_execution.cpp
+++ b/mbf_abstract_nav/src/abstract_controller_execution.cpp
@@ -412,7 +412,6 @@ void AbstractControllerExecution::run()
         geometry_msgs::TwistStamped cmd_vel_stamped;
         geometry_msgs::TwistStamped robot_velocity;
         robot_info_.getRobotVelocity(robot_velocity);
-
         outcome_ = computeVelocityCmd(robot_pose_, robot_velocity, cmd_vel_stamped, message_ = "");
 
         if (outcome_ < 10)

From 5517f16eb5270d6e1e32628b4c1f607d279309df Mon Sep 17 00:00:00 2001
From: Cynthia Li <cynthia.li@rapyuta-robotics.com>
Date: Mon, 24 Jul 2023 09:57:03 +0900
Subject: [PATCH 07/10] addressed Jorge's comments

---
 .../src/abstract_controller_execution.cpp     | 31 ++++++++++++++-----
 1 file changed, 23 insertions(+), 8 deletions(-)

diff --git a/mbf_abstract_nav/src/abstract_controller_execution.cpp b/mbf_abstract_nav/src/abstract_controller_execution.cpp
index 0eb4a4a4..d300a3ca 100644
--- a/mbf_abstract_nav/src/abstract_controller_execution.cpp
+++ b/mbf_abstract_nav/src/abstract_controller_execution.cpp
@@ -190,7 +190,12 @@ bool AbstractControllerExecution::checkCmdVelIgnored(const geometry_msgs::Twist&
 {
   // check if the velocity ignore check is enabled or not
   if (cmd_vel_ignored_tolerance_ <= 0.0)
-  {
+  { 
+    // set to zero if not already zero
+    if (!first_ignored_time_.is_zero())
+    {
+      first_ignored_time_ = ros::Time();
+    }
     return false;
   }
 
@@ -200,15 +205,26 @@ bool AbstractControllerExecution::checkCmdVelIgnored(const geometry_msgs::Twist&
   const double cmd_linear = std::hypot(cmd_vel.linear.x, cmd_vel.linear.y);
   const double cmd_angular = std::abs(cmd_vel.angular.z);
 
-  const bool cmd_is_zero = (cmd_linear < 0.01 && cmd_angular < 0.01);
+  const bool cmd_is_zero = cmd_linear < 1e-3 && cmd_angular < 1e-3;
 
-  // no need to check
+  // velocity is not being ignored
   if (!robot_stopped || cmd_is_zero)
   {
-    first_ignored_time_ = ros::Time::now();
+    // set to zero if not already zero
+    if (!first_ignored_time_.is_zero())
+    {
+      first_ignored_time_ = ros::Time();
+    }
     return false;
   }
 
+  // check if first_ignored_time_ is zero or not
+  if (first_ignored_time_.is_zero())
+  {
+    // set first_ignored_time_ to now if it was zero
+    first_ignored_time_ = ros::Time::now();
+  }
+
   // check if robot ignores the cmd_vel
   const double ignored_duration = (ros::Time::now() - first_ignored_time_).toSec();
   ROS_WARN_THROTTLE(1,
@@ -219,9 +235,9 @@ bool AbstractControllerExecution::checkCmdVelIgnored(const geometry_msgs::Twist&
   if (ignored_duration > cmd_vel_ignored_tolerance_)
   {
     // the robot is ignoring the velocity command more the threshold time
-    ROS_ERROR("Robot is ignoring velocity command for more than the tolerance time: %.2f seconds",
+    ROS_ERROR("Robot is ignoring velocity command for more than %.2f seconds. Tolerance exceeded!",
               cmd_vel_ignored_tolerance_);
-    return true;  // return true
+    return true;
   }
 
   return false;
@@ -320,9 +336,8 @@ void AbstractControllerExecution::run()
   last_valid_cmd_time_ = ros::Time();
   int retries = 0;
   int seq = 0;
-  first_ignored_time_ = ros::Time::now();
+  first_ignored_time_ = ros::Time();
 
- 
   try
   {
     while (moving_ && ros::ok())

From caed98a199932d3de506d31e6007cfa9128c3ba5 Mon Sep 17 00:00:00 2001
From: Cynthia Li <cynthia.li@rapyuta-robotics.com>
Date: Tue, 25 Jul 2023 10:13:38 +0900
Subject: [PATCH 08/10] addressed additional comments from Jorge

---
 mbf_abstract_nav/src/abstract_controller_execution.cpp | 10 +---------
 mbf_abstract_nav/src/controller_action.cpp             |  1 -
 mbf_msgs/action/ExePath.action                         |  2 +-
 3 files changed, 2 insertions(+), 11 deletions(-)

diff --git a/mbf_abstract_nav/src/abstract_controller_execution.cpp b/mbf_abstract_nav/src/abstract_controller_execution.cpp
index d300a3ca..1d92b20b 100644
--- a/mbf_abstract_nav/src/abstract_controller_execution.cpp
+++ b/mbf_abstract_nav/src/abstract_controller_execution.cpp
@@ -188,14 +188,9 @@ void AbstractControllerExecution::setVelocityCmd(const geometry_msgs::TwistStamp
 
 bool AbstractControllerExecution::checkCmdVelIgnored(const geometry_msgs::Twist& cmd_vel)
 {
-  // check if the velocity ignore check is enabled or not
+  // check if the velocity ignored check is enabled or not
   if (cmd_vel_ignored_tolerance_ <= 0.0)
   { 
-    // set to zero if not already zero
-    if (!first_ignored_time_.is_zero())
-    {
-      first_ignored_time_ = ros::Time();
-    }
     return false;
   }
 
@@ -210,7 +205,6 @@ bool AbstractControllerExecution::checkCmdVelIgnored(const geometry_msgs::Twist&
   // velocity is not being ignored
   if (!robot_stopped || cmd_is_zero)
   {
-    // set to zero if not already zero
     if (!first_ignored_time_.is_zero())
     {
       first_ignored_time_ = ros::Time();
@@ -218,14 +212,12 @@ bool AbstractControllerExecution::checkCmdVelIgnored(const geometry_msgs::Twist&
     return false;
   }
 
-  // check if first_ignored_time_ is zero or not
   if (first_ignored_time_.is_zero())
   {
     // set first_ignored_time_ to now if it was zero
     first_ignored_time_ = ros::Time::now();
   }
 
-  // check if robot ignores the cmd_vel
   const double ignored_duration = (ros::Time::now() - first_ignored_time_).toSec();
   ROS_WARN_THROTTLE(1,
                     "Robot is ignoring velocity command for %.2f seconds. (Commanded velocity: x=%.2f, "
diff --git a/mbf_abstract_nav/src/controller_action.cpp b/mbf_abstract_nav/src/controller_action.cpp
index b5a7491e..3c602b0c 100644
--- a/mbf_abstract_nav/src/controller_action.cpp
+++ b/mbf_abstract_nav/src/controller_action.cpp
@@ -311,7 +311,6 @@ void ControllerAction::runImpl(GoalHandle &goal_handle, AbstractControllerExecut
         break;
 
       case AbstractControllerExecution::ROBOT_DISABLED:
-        ROS_FATAL_STREAM_NAMED(name_, "Robot ignored velocity commands for more than tolerance time");
         controller_active = false;
         fillExePathResult(mbf_msgs::ExePathResult::ROBOT_STUCK,
                           "Robot ignored velocity commands for more than tolerance time!", result);
diff --git a/mbf_msgs/action/ExePath.action b/mbf_msgs/action/ExePath.action
index 8e372168..6d5f718e 100644
--- a/mbf_msgs/action/ExePath.action
+++ b/mbf_msgs/action/ExePath.action
@@ -25,7 +25,7 @@ uint8 NO_VALID_CMD      = 102
 uint8 PAT_EXCEEDED      = 103
 uint8 COLLISION         = 104
 uint8 OSCILLATION       = 105
-uint8 ROBOT_STUCK       = 106
+uint8 ROBOT_STUCK       = 106  # The robot is not obeying the commanded velocity
 uint8 MISSED_GOAL       = 107  # The robot has overshot the goal and cannot reach it anymore
 uint8 MISSED_PATH       = 108  # The robot has diverged from the path and cannot rejoin it
 uint8 BLOCKED_GOAL      = 109  # There's an obstacle at the goal

From faa86548d91743611580232b8e121e09d0a5ebae Mon Sep 17 00:00:00 2001
From: Cynthia Li <cynthia.li@rapyuta-robotics.com>
Date: Wed, 26 Jul 2023 11:31:09 +0900
Subject: [PATCH 09/10] addressed Jorge's extra suggestions

---
 .../mbf_abstract_nav/abstract_controller_execution.h  |  2 +-
 .../src/abstract_controller_execution.cpp             | 11 ++++++-----
 2 files changed, 7 insertions(+), 6 deletions(-)

diff --git a/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h
index 90dfb3b6..d6bd4308 100644
--- a/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h
+++ b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h
@@ -242,7 +242,7 @@ namespace mbf_abstract_nav
     //! The time the controller responded with a success output (output < 10).
     ros::Time last_valid_cmd_time_;
 
-    //! The first time when the robot is ignoring velocity command
+    //! The time when the robot started ignoring velocity commands
     ros::Time first_ignored_time_;
 
     //! The maximum number of retries
diff --git a/mbf_abstract_nav/src/abstract_controller_execution.cpp b/mbf_abstract_nav/src/abstract_controller_execution.cpp
index 1d92b20b..cb70e512 100644
--- a/mbf_abstract_nav/src/abstract_controller_execution.cpp
+++ b/mbf_abstract_nav/src/abstract_controller_execution.cpp
@@ -202,9 +202,9 @@ bool AbstractControllerExecution::checkCmdVelIgnored(const geometry_msgs::Twist&
 
   const bool cmd_is_zero = cmd_linear < 1e-3 && cmd_angular < 1e-3;
 
-  // velocity is not being ignored
   if (!robot_stopped || cmd_is_zero)
   {
+    // velocity is not being ignored
     if (!first_ignored_time_.is_zero())
     {
       first_ignored_time_ = ros::Time();
@@ -219,10 +219,6 @@ bool AbstractControllerExecution::checkCmdVelIgnored(const geometry_msgs::Twist&
   }
 
   const double ignored_duration = (ros::Time::now() - first_ignored_time_).toSec();
-  ROS_WARN_THROTTLE(1,
-                    "Robot is ignoring velocity command for %.2f seconds. (Commanded velocity: x=%.2f, "
-                    "y=%.2f, w=%.2f)",
-                    ignored_duration, cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z);
 
   if (ignored_duration > cmd_vel_ignored_tolerance_)
   {
@@ -232,6 +228,11 @@ bool AbstractControllerExecution::checkCmdVelIgnored(const geometry_msgs::Twist&
     return true;
   }
 
+  ROS_WARN_THROTTLE(1,
+                    "Robot is ignoring velocity command for %.2f seconds. (Commanded velocity: x=%.2f, "
+                    "y=%.2f, w=%.2f)",
+                    ignored_duration, cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z);
+
   return false;
 }
 

From e5660209b129c84ec765f092a1f1b9dcae57003a Mon Sep 17 00:00:00 2001
From: Cynthia Li <cynthia.li@rapyuta-robotics.com>
Date: Thu, 27 Jul 2023 18:07:54 +0900
Subject: [PATCH 10/10] addressed Tiago's suggestions

---
 mbf_abstract_nav/src/abstract_controller_execution.cpp | 6 +-----
 1 file changed, 1 insertion(+), 5 deletions(-)

diff --git a/mbf_abstract_nav/src/abstract_controller_execution.cpp b/mbf_abstract_nav/src/abstract_controller_execution.cpp
index cb70e512..267b91d7 100644
--- a/mbf_abstract_nav/src/abstract_controller_execution.cpp
+++ b/mbf_abstract_nav/src/abstract_controller_execution.cpp
@@ -205,10 +205,7 @@ bool AbstractControllerExecution::checkCmdVelIgnored(const geometry_msgs::Twist&
   if (!robot_stopped || cmd_is_zero)
   {
     // velocity is not being ignored
-    if (!first_ignored_time_.is_zero())
-    {
-      first_ignored_time_ = ros::Time();
-    }
+    first_ignored_time_ = ros::Time();
     return false;
   }
 
@@ -222,7 +219,6 @@ bool AbstractControllerExecution::checkCmdVelIgnored(const geometry_msgs::Twist&
 
   if (ignored_duration > cmd_vel_ignored_tolerance_)
   {
-    // the robot is ignoring the velocity command more the threshold time
     ROS_ERROR("Robot is ignoring velocity command for more than %.2f seconds. Tolerance exceeded!",
               cmd_vel_ignored_tolerance_);
     return true;