Velocity Command Ignored Checking

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
     };
 
     /**
@@ -219,6 +220,13 @@ namespace mbf_abstract_nav
      */
     void setVelocityCmd(const geometry_msgs::TwistStamped &vel_cmd_stamped);
 
+    /**
+     * @brief Check if the robot is ignoring the cmd_vel longer than threshold time
+     * @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 checkCmdVelIgnored(const geometry_msgs::Twist& cmd_vel);
+
     //! 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 robot is ignoring velocity command
+    ros::Time first_ignored_time_;
+
     //! The maximum number of retries
     int max_retries_;
 
@@ -364,6 +375,10 @@ namespace mbf_abstract_nav
 
     //! replaces parameter angle_tolerance_ for the action
     double action_angle_tolerance_;
+
+    //! time tolerance for checking if the robot is ignoring cmd_vel
+    double cmd_vel_ignored_tolerance_;
+
   };
 
 } /* namespace mbf_abstract_nav */

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("cmd_vel_ignored_tolerance", cmd_vel_ignored_tolerance_, 5.0);
 
   // dynamically reconfigurable parameters
   reconfigure(config);
@@ -185,6 +186,47 @@ void AbstractControllerExecution::setVelocityCmd(const geometry_msgs::TwistStamp
   // TODO so there should be no loss of information in the feedback stream
 }
 
+bool AbstractControllerExecution::checkCmdVelIgnored(const geometry_msgs::Twist& cmd_vel)
+{
+  // check if the velocity ignore check is enabled or not
+  if (cmd_vel_ignored_tolerance_ <= 0.0)
+  {
+    return false;
+  }
+
+  const bool robot_stopped = robot_info_.isRobotStopped(1e-3, 1e-3);
+
+  // compute linear and angular velocity magnitude
+  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);
+
+  // no need to check
+  if (!robot_stopped || cmd_is_zero)
+  {
+    first_ignored_time_ = ros::Time::now();
+    return false;
+  }
+
+  // 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, "
+                    "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_)
+  {
+    // 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",
+              cmd_vel_ignored_tolerance_);
+    return true;  // return true
+  }
+
+  return false;
+}
+
 geometry_msgs::TwistStamped AbstractControllerExecution::getVelocityCmd() const
 {
   boost::lock_guard<boost::mutex> guard(vel_cmd_mtx_);
@@ -278,7 +320,9 @@ void AbstractControllerExecution::run()
   last_valid_cmd_time_ = ros::Time();
   int retries = 0;
   int seq = 0;
+  first_ignored_time_ = ros::Time::now();
 
+
   try
   {
     while (moving_ && ros::ok())
@@ -376,6 +420,12 @@ void AbstractControllerExecution::run()
           vel_pub_.publish(cmd_vel_stamped.twist);
           last_valid_cmd_time_ = ros::Time::now();
           retries = 0;
+          // check if robot is ignoring velocity command
+          if (checkCmdVelIgnored(cmd_vel_stamped.twist))
+          {
+            setState(ROBOT_DISABLED);
+            moving_ = false;
+          }
         }
         else if (outcome_ == mbf_msgs::ExePathResult::CANCELED)
         {

mbf_abstract_nav/src/controller_action.cpp

@@ -310,6 +310,14 @@ void ControllerAction::runImpl(GoalHandle &goal_handle, AbstractControllerExecut
         goal_handle.setAborted(result, result.message);
         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);
+        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: "