robotnik_pad_plugins/Movement: Allow plugin to check several axis fro…

robotnik_pad_plugins/include/robotnik_pad_plugins/movement_plugin.h

@@ -63,13 +63,13 @@ class PadPluginMovement : public GenericPadPlugin
   //! flag to use accelerometer as watchdog
   bool use_accel_watchdog_;
   //! accelerometer axis used for watchdog
-  int axis_accel_watchdog_;
+  std::vector<int> axis_accel_watchdog_;
   //! time to activate the watchdog
   double watchdog_duration_;
   //! last acceleration value
-  double last_accel_value_;
+  std::vector<double> last_accel_value_;
   //! last time new accelerometer value read
-  ros::Time last_accel_time_;
+  std::vector<ros::Time> last_accel_time_;
   //! flag to monitor if the robot watchdog is activated
   bool watchdog_activated_;
   //! Service to set the current velocity level
@@ -79,6 +79,8 @@ class PadPluginMovement : public GenericPadPlugin
 protected:
   std::string kinematicModeToStr(int kinematic_mode);
 
+  std::vector<int> getIntParamAsVector(ros::NodeHandle& nh, const std::string& param_name, const int& default_value);
+
 };
 }  // namespace pad_plugins
 #endif  // PAD_PLUGIN_ELEVATOR_H_

robotnik_pad_plugins/src/movement_plugin.cpp

@@ -40,8 +40,9 @@ void PadPluginMovement::initialize(const ros::NodeHandle& nh, const std::string&
   //watchdog params
   use_accel_watchdog_ = false;
   readParam(pnh_, "config/use_accel_watchdog", use_accel_watchdog_, use_accel_watchdog_, not_required);
-  axis_accel_watchdog_ = 8;
-  readParam(pnh_, "config/axis_watchdog", axis_accel_watchdog_, axis_accel_watchdog_, not_required);
+
+  axis_accel_watchdog_ = getIntParamAsVector(pnh_, "config/axis_watchdog", 8);
+
   watchdog_duration_ = 0.5;
   readParam(pnh_, "config/watchdog_duration", watchdog_duration_, watchdog_duration_, not_required);
 
@@ -71,15 +72,15 @@ void PadPluginMovement::initialize(const ros::NodeHandle& nh, const std::string&
   movement_status_msg_ = robotnik_pad_msgs::MovementStatus();
   current_velocity_level_msg_ = std_msgs::Float64();
   kinematic_mode_ = KinematicModes::Differential;
-
-  last_accel_time_ = ros::Time::now();
-  last_accel_value_ = 0.0;
+  for (int i = 0; i < axis_accel_watchdog_.size(); i++)
+  {
+    last_accel_value_.push_back(0.0);
+    last_accel_time_.push_back(ros::Time::now());
+  }
+
   watchdog_activated_ = false;
-
 }
 
-
-
 bool PadPluginMovement::setVelocityLevel(robotnik_msgs::SetFloat64::Request &req, robotnik_msgs::SetFloat64::Response &res)
 {
   req.data = round(req.data * 10) / 10;
@@ -110,6 +111,56 @@ bool PadPluginMovement::setVelocityLevel(robotnik_msgs::SetFloat64::Request &req
 }
 
 
+
+std::vector<int> PadPluginMovement::getIntParamAsVector(ros::NodeHandle& nh, const std::string& param_name, const int& default_value)
+{
+  std::vector<int> result;
+
+  if (nh.hasParam(param_name))
+  {
+    XmlRpc::XmlRpcValue param_value;
+    nh.getParam(param_name, param_value);
+
+    if (param_value.getType() == XmlRpc::XmlRpcValue::TypeInt)
+    {
+      // If the parameter is an integer, create a vector with that single integer
+      result.push_back(static_cast<int>(param_value));
+    }
+    else if (param_value.getType() == XmlRpc::XmlRpcValue::TypeArray)
+    {
+      // If the parameter is an array, convert it to a std::vector
+      for (int i = 0; i < param_value.size(); ++i)
+      {
+        if (param_value[i].getType() == XmlRpc::XmlRpcValue::TypeInt)
+        {
+          result.push_back(static_cast<int>(param_value[i]));
+        }
+        else
+        {
+          ROS_WARN("Parameter '%s' contains non-integer values. Skipping.", param_name.c_str());
+        }
+      }
+      if (result.size() == 0)
+      {
+        ROS_ERROR("Parameter '%s' does not contain any integer value. Using default value %d", param_name.c_str(), default_value);
+        result.push_back(static_cast<int>(default_value));
+      }
+    }
+    else
+    {
+      ROS_ERROR("Parameter '%s' is neither an integer nor an array. Using default value %d", param_name.c_str(), default_value);
+      result.push_back(static_cast<int>(default_value));
+    }
+  }
+  else
+  {
+    ROS_ERROR("Parameter '%s' not found on the parameter server. Using default value %d", param_name.c_str(), default_value);
+    result.push_back(static_cast<int>(default_value)); 
+  }
+
+  return result;
+}
+
 void PadPluginMovement::execute(const std::vector<Button>& buttons, std::vector<float>& axes)
 {
   if (buttons[button_dead_man_].isPressed())
@@ -118,21 +169,38 @@ void PadPluginMovement::execute(const std::vector<Button>& buttons, std::vector<
     if (use_accel_watchdog_)
     {
       // If new accelerameter value is different to last, we update watchdog values
-      if (last_accel_value_ != axes[axis_accel_watchdog_])
+      bool check_watchdog = true;
+      int i;
+      for (i = 0; i < axis_accel_watchdog_.size(); i++)
       {
-        last_accel_value_ = axes[axis_accel_watchdog_];
-        last_accel_time_ = ros::Time::now();
-        watchdog_activated_ = false;
+        if (last_accel_value_[i] != axes[axis_accel_watchdog_[i]])
+        {
+          last_accel_value_[i] = axes[axis_accel_watchdog_[i]];
+          last_accel_time_[i] = ros::Time::now();
+          watchdog_activated_ = false;
+          check_watchdog = false;
+        }
       }
-      else
+
+      if (check_watchdog)
       {
         if (watchdog_activated_)
         {
           ROS_WARN_THROTTLE(5, "PadPluginMovement::execute: Command discarded by accelerometer watchdog!");
           return;
         }
         // If watchdog is expired, we stop the robot
-        if (ros::Time::now() - last_accel_time_ > ros::Duration(watchdog_duration_))
+        bool timedout = true;
+        for (i = 0; i < last_accel_time_.size(); i++)
+        {
+          if (ros::Time::now() - last_accel_time_[i] < ros::Duration(watchdog_duration_))
+          {
+            timedout = false;
+            break;
+          }
+        }
+
+        if (timedout)
         {
           ROS_WARN("PadPluginMovement::execute: Accelerometer watchdog timedout!");
           cmd_twist_.linear.x = 0.0;