From ea09e6fb02e179312de8b8c63a80cc9e69739a58 Mon Sep 17 00:00:00 2001
From: Jose Antonio Mendez <jamendez@robotnik.es>
Date: Mon, 4 Nov 2024 13:12:33 +0100
Subject: [PATCH 1/4] robotnik_pad_plugins: Add condition to stop elevator
 movement if deadman is released

---
 .../robotnik_pad_plugins/elevator_plugin.h    |  1 +
 robotnik_pad_plugins/src/elevator_plugin.cpp  | 61 ++++++++++++++-----
 2 files changed, 46 insertions(+), 16 deletions(-)

diff --git a/robotnik_pad_plugins/include/robotnik_pad_plugins/elevator_plugin.h b/robotnik_pad_plugins/include/robotnik_pad_plugins/elevator_plugin.h
index 9e78437..6042dad 100644
--- a/robotnik_pad_plugins/include/robotnik_pad_plugins/elevator_plugin.h
+++ b/robotnik_pad_plugins/include/robotnik_pad_plugins/elevator_plugin.h
@@ -20,6 +20,7 @@ class PadPluginElevator : public GenericPadPlugin
   double axis_elevator_;
   bool elevator_is_running_;
   bool stop_elevator_;
+  bool stop_elevator_dead_man_;
 
   std::string elevator_service_name_;
   ros::ServiceClient set_elevator_client_;
diff --git a/robotnik_pad_plugins/src/elevator_plugin.cpp b/robotnik_pad_plugins/src/elevator_plugin.cpp
index 897678e..e0aedb9 100644
--- a/robotnik_pad_plugins/src/elevator_plugin.cpp
+++ b/robotnik_pad_plugins/src/elevator_plugin.cpp
@@ -21,7 +21,7 @@ void PadPluginElevator::initialize(const ros::NodeHandle& nh, const std::string&
   readParam(pnh_, "config/axis_elevator", axis_elevator_, axis_elevator_, required);
   readParam(pnh_, "elevator_service_name", elevator_service_name_, elevator_service_name_, required);
   readParam(pnh_, "stop_elevator", stop_elevator_, false, not_required);
-
+  readParam(pnh_, "stop_elevator_dead_man", stop_elevator_dead_man_, false, not_required);
   // Service client
   set_elevator_client_ = nh_.serviceClient<robotnik_msgs::SetElevator>(elevator_service_name_);
 
@@ -34,47 +34,76 @@ void PadPluginElevator::execute(const std::vector<Button>& buttons, std::vector<
   {
     if (axes[axis_elevator_] > 0.95)
     {
-      robotnik_msgs::SetElevator elevator_msg_srv;
-      elevator_msg_srv.request.action.action = robotnik_msgs::ElevatorAction::RAISE;
-      ROS_INFO_NAMED("PadPluginElevator", "PadPluginElevator::execute: Raising...");
-      elevator_is_running_ = true;
-      if (set_elevator_client_.call(elevator_msg_srv) != true || elevator_msg_srv.response.ret != true)
+      if (!elevator_is_running_)
       {
-        ROS_ERROR_NAMED("PadPluginElevator", "PadPluginElevator::execute: Error raising");
+        robotnik_msgs::SetElevator elevator_msg_srv;
+        elevator_msg_srv.request.action.action = robotnik_msgs::ElevatorAction::RAISE;
+        ROS_INFO_NAMED("PadPluginElevator", "PadPluginElevator::execute: Raising...");
+        if (set_elevator_client_.call(elevator_msg_srv) != true || elevator_msg_srv.response.ret != true)
+        {
+          ROS_ERROR_NAMED("PadPluginElevator", "PadPluginElevator::execute: Error raising");
+        }
+        else
+        { 
+          elevator_is_running_ = true;
+        }
       }
     }
     if (axes[axis_elevator_] < -0.95)
     {
-      robotnik_msgs::SetElevator elevator_msg_srv;
-      elevator_msg_srv.request.action.action = robotnik_msgs::ElevatorAction::LOWER;
-      elevator_is_running_ = true;
-      ROS_INFO_NAMED("PadPluginElevator", "PadPluginElevator::execute: Lowering...");
-
-      if (set_elevator_client_.call(elevator_msg_srv) != true || elevator_msg_srv.response.ret != true)
+      if (!elevator_is_running_)
       {
-        ROS_ERROR_NAMED("PadPluginElevator", "PadPluginElevator::execute: Error lowering");
+        robotnik_msgs::SetElevator elevator_msg_srv;
+        elevator_msg_srv.request.action.action = robotnik_msgs::ElevatorAction::LOWER;
+        ROS_INFO_NAMED("PadPluginElevator", "PadPluginElevator::execute: Lowering...");
+        if (set_elevator_client_.call(elevator_msg_srv) != true || elevator_msg_srv.response.ret != true)
+        {
+          ROS_ERROR_NAMED("PadPluginElevator", "PadPluginElevator::execute: Error lowering");
+        }
+        else
+        {
+          elevator_is_running_ = true;
+        }
       }
     }
 
-    if (stop_elevator_==true){
+    if (stop_elevator_){
 
       if (axes[axis_elevator_] > -0.1 && axes[axis_elevator_] < 0.1 && elevator_is_running_ == true)
       {
         robotnik_msgs::SetElevator elevator_msg_srv;
         elevator_msg_srv.request.action.action = robotnik_msgs::ElevatorAction::STOP;
-        elevator_is_running_ = false;
         ROS_INFO_NAMED("PadPluginElevator", "PadPluginElevator::execute: Stoping...");
 
         if (set_elevator_client_.call(elevator_msg_srv) != true || elevator_msg_srv.response.ret != true)
         {
           ROS_ERROR_NAMED("PadPluginElevator", "PadPluginElevator::execute: Error stoping");
         }
+        else
+        {
+          elevator_is_running_ = false;
+        }
       }
     }
 
   }
   else if (buttons[button_dead_man_].isReleased())
   {
+    if (stop_elevator_dead_man_ && elevator_is_running_)
+    {
+      robotnik_msgs::SetElevator elevator_msg_srv;
+      elevator_msg_srv.request.action.action = robotnik_msgs::ElevatorAction::STOP;
+      ROS_INFO_NAMED("PadPluginElevator", "PadPluginElevator::execute: Stoping...");
+
+      if (set_elevator_client_.call(elevator_msg_srv) != true || elevator_msg_srv.response.ret != true)
+      {
+        ROS_ERROR_NAMED("PadPluginElevator", "PadPluginElevator::execute: Error stoping");
+      }
+      else
+      {
+        elevator_is_running_ = false;
+      }
+    }
   }
 }
 

From df66bb007759c821074a3771b837b00362bff0c1 Mon Sep 17 00:00:00 2001
From: Jose Antonio Mendez <jamendez@robotnik.es>
Date: Thu, 14 Nov 2024 18:18:11 +0100
Subject: [PATCH 2/4] Add set output plugin

---
 robotnik_pad_plugins/CMakeLists.txt           |   1 +
 .../robotnik_pad_plugins/set_output_plugin.h  |  44 +++++
 .../robotnik_pad_pluginlib.xml                |   3 +
 .../src/set_output_plugin.cpp                 | 173 ++++++++++++++++++
 4 files changed, 221 insertions(+)
 create mode 100644 robotnik_pad_plugins/include/robotnik_pad_plugins/set_output_plugin.h
 create mode 100644 robotnik_pad_plugins/src/set_output_plugin.cpp

diff --git a/robotnik_pad_plugins/CMakeLists.txt b/robotnik_pad_plugins/CMakeLists.txt
index 41f61c6..5c9a15f 100644
--- a/robotnik_pad_plugins/CMakeLists.txt
+++ b/robotnik_pad_plugins/CMakeLists.txt
@@ -36,6 +36,7 @@ add_library(robotnik_pad_plugins
   src/poi_plugin.cpp
   src/ptz_plugin.cpp
   src/blkarc_plugin.cpp
+  src/set_output_plugin.cpp
 )
 
 add_dependencies(robotnik_pad_plugins ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
diff --git a/robotnik_pad_plugins/include/robotnik_pad_plugins/set_output_plugin.h b/robotnik_pad_plugins/include/robotnik_pad_plugins/set_output_plugin.h
new file mode 100644
index 0000000..0bbf384
--- /dev/null
+++ b/robotnik_pad_plugins/include/robotnik_pad_plugins/set_output_plugin.h
@@ -0,0 +1,44 @@
+#ifndef PAD_PLUGIN_SET_OUTPUT_H_
+#define PAD_PLUGIN_SET_OUTPUT_H_
+
+#include <robotnik_msgs/set_digital_output.h>
+#include <robotnik_msgs/inputs_outputs.h>
+#include <robotnik_pad/generic_pad_plugin.h>
+
+namespace pad_plugins
+{
+class PadPluginSetOutput : public GenericPadPlugin
+{
+public:
+  PadPluginSetOutput();
+  ~PadPluginSetOutput();
+
+  virtual void initialize(const ros::NodeHandle &nh, const std::string &plugin_ns);
+  virtual void execute(const std::vector<Button> &buttons, std::vector<float> &axes);
+
+protected:
+  bool checkFeedback(bool invert_values);
+  void ioCb(const robotnik_msgs::inputs_outputs::ConstPtr& msg);
+
+  double timeout_;
+  int button_dead_man_;
+  double axis_set_output_;
+  bool output_sent_;
+  bool activate_, deactivate_;
+  bool use_plc_;
+  bool has_feedback_;
+  std::vector<int> outputs_;
+  std::vector<bool> output_values_;
+  std::vector<int> inputs_;
+  std::vector<bool> input_values_; 
+  std::string base_hw_set_output_name_;
+  std::string modbus_set_output_name_;
+  std::string base_hw_io_topic_name_;
+  std::string modbus_io_topic_name_;
+  ros::ServiceClient set_output_client_;
+  ros::Subscriber io_sub_;
+  robotnik_msgs::inputs_outputs io_;
+  ros::Time init_timeout_;
+};
+}  // namespace pad_plugins
+#endif  // PAD_PLUGIN_SET_OUTPUT_H_
\ No newline at end of file
diff --git a/robotnik_pad_plugins/robotnik_pad_pluginlib.xml b/robotnik_pad_plugins/robotnik_pad_pluginlib.xml
index 558991e..f7efb4d 100644
--- a/robotnik_pad_plugins/robotnik_pad_pluginlib.xml
+++ b/robotnik_pad_plugins/robotnik_pad_pluginlib.xml
@@ -20,4 +20,7 @@
     <class name="robotnik_pad_plugins/BlkArc" type="pad_plugins::PadPluginBlkArc" base_class_type="pad_plugins::GenericPadPlugin">
         <description>This is the BLK ARC plugin.</description>
     </class>
+    <class name="robotnik_pad_plugins/setOutput" type="pad_plugins::PadPluginSetOutput" base_class_type="pad_plugins::GenericPadPlugin">
+        <description>This is the Set Output plugin.</description>
+    </class>
 </library>
diff --git a/robotnik_pad_plugins/src/set_output_plugin.cpp b/robotnik_pad_plugins/src/set_output_plugin.cpp
new file mode 100644
index 0000000..53f3f11
--- /dev/null
+++ b/robotnik_pad_plugins/src/set_output_plugin.cpp
@@ -0,0 +1,173 @@
+#include <robotnik_pad_plugins/set_output_plugin.h>
+
+namespace pad_plugins
+{
+PadPluginSetOutput::PadPluginSetOutput()
+{
+}
+
+PadPluginSetOutput::~PadPluginSetOutput()
+{
+}
+
+void PadPluginSetOutput::initialize(const ros::NodeHandle& nh, const std::string& plugin_ns)
+{
+  activate_ = false;
+  deactivate_ = false;
+  output_sent_ = false;
+  bool required = true;
+  bool not_required = false;
+
+  pnh_ = ros::NodeHandle(nh, plugin_ns);
+
+  readParam(pnh_, "config/deadman", button_dead_man_, button_dead_man_, required);
+  readParam(pnh_, "config/axis_set_output", axis_set_output_, axis_set_output_, required);
+  readParam(pnh_, "base_hw_set_output_name", base_hw_set_output_name_, base_hw_set_output_name_, required);
+  readParam(pnh_, "modbus_set_output_name", modbus_set_output_name_, modbus_set_output_name_, required);
+  readParam(pnh_, "use_plc", use_plc_, false, not_required);
+  readParam(pnh_, "has_feedback", has_feedback_, false, not_required);
+  pnh_.getParam("outputs", outputs_);
+  pnh_.getParam("inputs", inputs_);
+  pnh_.getParam("output_values", output_values_);
+  pnh_.getParam("input_values", input_values_);
+  // readParam(pnh_, "outputs", outputs_, void_int_list, required);
+  // readParam(pnh_, "output_values", output_values_, void_bool_list, not_required);
+  // readParam(pnh_, "inputs", inputs_, void_int_list, not_required);
+  // readParam(pnh_, "input_values", input_values_, void_bool_list, not_required);
+  timeout_ = 1;
+  readParam(pnh_, "timeout", timeout_, timeout_, not_required);
+
+  base_hw_io_topic_name_ = "";
+  readParam(pnh_, "base_hw_io_topic_name", base_hw_io_topic_name_, base_hw_io_topic_name_, not_required);
+  modbus_io_topic_name_ = "";
+  readParam(pnh_, "modbus_io_topic_name", modbus_io_topic_name_, modbus_io_topic_name_, not_required);
+  int n = outputs_.size();
+  int n_val = output_values_.size();
+  if (n != n_val)
+  {
+    output_values_.clear();
+    for (int i = 0; i < n; i++)
+      output_values_[i] = true;
+  }
+  if (has_feedback_)
+  {
+    n = inputs_.size();
+    if (n == 0)
+      has_feedback_ = false;
+    else if (base_hw_io_topic_name_ == "" && !use_plc_)
+      has_feedback_ = false;
+    else if (modbus_io_topic_name_ == "" && use_plc_)
+      has_feedback_ = false;
+    else
+    {
+      n_val = input_values_.size();
+      if (n != n_val)
+      {
+        input_values_.clear();
+        for (int j = 0; j < n; j++)
+          input_values_[j] = true;
+      }
+    }
+  }
+
+  // Service client
+  std::string service_name = use_plc_ ? modbus_set_output_name_ : base_hw_set_output_name_;
+  set_output_client_ = nh_.serviceClient<robotnik_msgs::set_digital_output>(service_name);
+
+  //Subscriber
+  if (has_feedback_)
+  {
+    std::string topic_name = use_plc_ ? modbus_io_topic_name_ : base_hw_io_topic_name_;
+    io_sub_ = nh_.subscribe<robotnik_msgs::inputs_outputs>(topic_name, 1, &PadPluginSetOutput::ioCb, this);
+  }
+}
+
+void PadPluginSetOutput::ioCb(const robotnik_msgs::inputs_outputs::ConstPtr& msg)
+{
+  io_ = *msg;
+}
+
+void PadPluginSetOutput::execute(const std::vector<Button>& buttons, std::vector<float>& axes)
+{
+  if (buttons[button_dead_man_].isPressed())
+  {
+    if (axes[axis_set_output_] > 0.95)
+    {
+      if (!output_sent_)
+      {
+        output_sent_ = true;
+        activate_ = true;
+        init_timeout_ = ros::Time::now();
+      }
+    }
+    if (axes[axis_set_output_] < -0.95)
+    {
+      if (!output_sent_)
+      {
+        output_sent_ = true;
+        deactivate_ = true;
+        init_timeout_ = ros::Time::now();
+      }
+    }
+  }
+  else if (buttons[button_dead_man_].isReleased())
+  {
+  }
+
+  if (output_sent_)
+  {
+    robotnik_msgs::set_digital_output set_output_srv;
+    if (activate_)
+    {
+      ROS_INFO_NAMED("PadPluginSetOutput", "PadPluginSetOutput::execute: Setting outputs...");
+      for (int i = 0; i < outputs_.size(); i++)
+      {
+        set_output_srv.request.output = outputs_[i];
+        set_output_srv.request.value = output_values_[i];
+      if (set_output_client_.call(set_output_srv) != true || set_output_srv.response.ret != true)
+      {
+        ROS_ERROR_NAMED("PadPluginSetOutput", "PadPluginSetOutput::execute: Error setting outputs");
+      }
+      }
+      if (checkFeedback(false) || ros::Time::now() - init_timeout_ > ros::Duration(timeout_))
+      {
+        output_sent_ = false;
+        activate_ = false;
+      }
+    }
+    else if (deactivate_)
+    {
+      ROS_INFO_NAMED("PadPluginSetOutput", "PadPluginSetOutput::execute: Setting outputs...");
+      for (int i = 0; i < outputs_.size(); i++)
+      {
+        set_output_srv.request.output = outputs_[i];
+        set_output_srv.request.value = !output_values_[i];
+        if (set_output_client_.call(set_output_srv) != true || set_output_srv.response.ret != true)
+        {
+          ROS_ERROR_NAMED("PadPluginSetOutput", "PadPluginSetOutput::execute: Error setting outputs");
+        }
+      }
+      if (checkFeedback(true) || ros::Time::now() - init_timeout_ > ros::Duration(timeout_))
+      {
+        output_sent_ = false;
+        deactivate_ = false;
+      }
+    }
+  }
+}
+
+bool PadPluginSetOutput::checkFeedback(bool invert_values)
+{
+  if (!has_feedback_)
+    return true;
+  
+  bool success = true;
+
+  for (int i = 0; i < inputs_.size(); i++)
+  {
+    success &= io_.digital_inputs[inputs_[i] - 1] == (input_values_[i] ^ invert_values);
+  }
+  return success;
+}
+
+}  // namespace pad_plugins

From f05af5e7767c1a00a5c19f9c0e6245dd2e311adb Mon Sep 17 00:00:00 2001
From: Jose Antonio Mendez <jamendez@robotnik.es>
Date: Tue, 3 Dec 2024 16:04:51 +0100
Subject: [PATCH 3/4] Complete set output plugin and add input condition to
 activate output

---
 .../robotnik_pad_plugins/set_output_plugin.h  |  28 ++-
 .../src/generic_pad_plugins.cpp               |   2 +
 .../src/set_output_plugin.cpp                 | 173 ++++++++++++------
 3 files changed, 140 insertions(+), 63 deletions(-)

diff --git a/robotnik_pad_plugins/include/robotnik_pad_plugins/set_output_plugin.h b/robotnik_pad_plugins/include/robotnik_pad_plugins/set_output_plugin.h
index 0bbf384..7ef4261 100644
--- a/robotnik_pad_plugins/include/robotnik_pad_plugins/set_output_plugin.h
+++ b/robotnik_pad_plugins/include/robotnik_pad_plugins/set_output_plugin.h
@@ -7,6 +7,11 @@
 
 namespace pad_plugins
 {
+
+struct io {
+  int number;
+  bool value;
+};
 class PadPluginSetOutput : public GenericPadPlugin
 {
 public:
@@ -18,7 +23,11 @@ class PadPluginSetOutput : public GenericPadPlugin
 
 protected:
   bool checkFeedback(bool invert_values);
+  bool checkCondition();
   void ioCb(const robotnik_msgs::inputs_outputs::ConstPtr& msg);
+  void feedbackIoCb(const robotnik_msgs::inputs_outputs::ConstPtr& msg);
+  void conditionIoCb(const robotnik_msgs::inputs_outputs::ConstPtr& msg);
+  void getIoValues(ros::NodeHandle& nh, const std::string& param_name, std::vector<io>& variable);
 
   double timeout_;
   int button_dead_man_;
@@ -27,17 +36,20 @@ class PadPluginSetOutput : public GenericPadPlugin
   bool activate_, deactivate_;
   bool use_plc_;
   bool has_feedback_;
-  std::vector<int> outputs_;
-  std::vector<bool> output_values_;
-  std::vector<int> inputs_;
-  std::vector<bool> input_values_; 
+  bool has_condition_;
+  bool use_buttons_;
+  int button_set_output_;
+  int button_reset_output_;
+  std::vector<io> outputs_;
+  std::vector<io> inputs_;
+  std::vector<io> condition_inputs_;
   std::string base_hw_set_output_name_;
   std::string modbus_set_output_name_;
-  std::string base_hw_io_topic_name_;
-  std::string modbus_io_topic_name_;
+  std::string feedback_input_topic_name_;
+  std::string condition_input_topic_name_;
   ros::ServiceClient set_output_client_;
-  ros::Subscriber io_sub_;
-  robotnik_msgs::inputs_outputs io_;
+  ros::Subscriber io_sub_, feedback_io_sub_, condition_io_sub_;
+  robotnik_msgs::inputs_outputs io_, condition_io_;
   ros::Time init_timeout_;
 };
 }  // namespace pad_plugins
diff --git a/robotnik_pad_plugins/src/generic_pad_plugins.cpp b/robotnik_pad_plugins/src/generic_pad_plugins.cpp
index a939997..9c2f10c 100644
--- a/robotnik_pad_plugins/src/generic_pad_plugins.cpp
+++ b/robotnik_pad_plugins/src/generic_pad_plugins.cpp
@@ -8,6 +8,7 @@
 #include <robotnik_pad_plugins/poi_plugin.h>
 #include <robotnik_pad_plugins/ptz_plugin.h>
 #include <robotnik_pad_plugins/blkarc_plugin.h>
+#include <robotnik_pad_plugins/set_output_plugin.h>
 
 PLUGINLIB_EXPORT_CLASS(pad_plugins::PadPluginMovement, pad_plugins::GenericPadPlugin);
 PLUGINLIB_EXPORT_CLASS(pad_plugins::PadPluginSafetyMovement, pad_plugins::GenericPadPlugin);
@@ -16,3 +17,4 @@ PLUGINLIB_EXPORT_CLASS(pad_plugins::PadPluginAckermannMovement, pad_plugins::Gen
 PLUGINLIB_EXPORT_CLASS(pad_plugins::PadPluginPoi, pad_plugins::GenericPadPlugin);
 PLUGINLIB_EXPORT_CLASS(pad_plugins::PadPluginPtz, pad_plugins::GenericPadPlugin);
 PLUGINLIB_EXPORT_CLASS(pad_plugins::PadPluginBlkArc, pad_plugins::GenericPadPlugin);
+PLUGINLIB_EXPORT_CLASS(pad_plugins::PadPluginSetOutput, pad_plugins::GenericPadPlugin);
diff --git a/robotnik_pad_plugins/src/set_output_plugin.cpp b/robotnik_pad_plugins/src/set_output_plugin.cpp
index 53f3f11..22563d1 100644
--- a/robotnik_pad_plugins/src/set_output_plugin.cpp
+++ b/robotnik_pad_plugins/src/set_output_plugin.cpp
@@ -22,52 +22,37 @@ void PadPluginSetOutput::initialize(const ros::NodeHandle& nh, const std::string
 
   readParam(pnh_, "config/deadman", button_dead_man_, button_dead_man_, required);
   readParam(pnh_, "config/axis_set_output", axis_set_output_, axis_set_output_, required);
+  readParam(pnh_, "config/button_set_output", button_set_output_, button_set_output_, required);
+  readParam(pnh_, "config/button_reset_output", button_reset_output_, button_reset_output_, required);
+  readParam(pnh_, "use_buttons", use_buttons_, false, not_required);
   readParam(pnh_, "base_hw_set_output_name", base_hw_set_output_name_, base_hw_set_output_name_, required);
   readParam(pnh_, "modbus_set_output_name", modbus_set_output_name_, modbus_set_output_name_, required);
-  readParam(pnh_, "use_plc", use_plc_, false, not_required);
+  readParam(pnh_, "use_plc", use_plc_, false, required);
   readParam(pnh_, "has_feedback", has_feedback_, false, not_required);
-  pnh_.getParam("outputs", outputs_);
-  pnh_.getParam("inputs", inputs_);
-  pnh_.getParam("output_values", output_values_);
-  pnh_.getParam("input_values", input_values_);
-  // readParam(pnh_, "outputs", outputs_, void_int_list, required);
-  // readParam(pnh_, "output_values", output_values_, void_bool_list, not_required);
-  // readParam(pnh_, "inputs", inputs_, void_int_list, not_required);
-  // readParam(pnh_, "input_values", input_values_, void_bool_list, not_required);
+  readParam(pnh_, "has_condition", has_condition_, false, not_required);
   timeout_ = 1;
   readParam(pnh_, "timeout", timeout_, timeout_, not_required);
+  if (use_plc_)
+    feedback_input_topic_name_ = "robotnik_modbus_io/inputs_outputs";
+  else
+    feedback_input_topic_name_ = "robotnik_base_hw/io";
+  condition_input_topic_name_ = feedback_input_topic_name_;
 
-  base_hw_io_topic_name_ = "";
-  readParam(pnh_, "base_hw_io_topic_name", base_hw_io_topic_name_, base_hw_io_topic_name_, not_required);
-  modbus_io_topic_name_ = "";
-  readParam(pnh_, "modbus_io_topic_name", modbus_io_topic_name_, modbus_io_topic_name_, not_required);
-  int n = outputs_.size();
-  int n_val = output_values_.size();
-  if (n != n_val)
-  {
-    output_values_.clear();
-    for (int i = 0; i < n; i++)
-      output_values_[i] = true;
-  }
+  readParam(pnh_, "feedback_input_topic_name", feedback_input_topic_name_, feedback_input_topic_name_, not_required);
+  readParam(pnh_, "condition_input_topic_name", condition_input_topic_name_, condition_input_topic_name_, not_required);
+
+  getIoValues(pnh_, "outputs", outputs_);
   if (has_feedback_)
   {
-    n = inputs_.size();
-    if (n == 0)
-      has_feedback_ = false;
-    else if (base_hw_io_topic_name_ == "" && !use_plc_)
+    getIoValues(pnh_, "feedback_inputs", inputs_);
+    if (inputs_.size() == 0)
       has_feedback_ = false;
-    else if (modbus_io_topic_name_ == "" && use_plc_)
-      has_feedback_ = false;
-    else
-    {
-      n_val = input_values_.size();
-      if (n != n_val)
-      {
-        input_values_.clear();
-        for (int j = 0; j < n; j++)
-          input_values_[j] = true;
-      }
-    }
+  }
+  if (has_condition_)
+  {
+    getIoValues(pnh_, "condition_inputs", condition_inputs_);
+    if (condition_inputs_.size() == 0)
+      has_condition_ = false;
   }
 
   // Service client
@@ -75,35 +60,88 @@ void PadPluginSetOutput::initialize(const ros::NodeHandle& nh, const std::string
   set_output_client_ = nh_.serviceClient<robotnik_msgs::set_digital_output>(service_name);
 
   //Subscriber
-  if (has_feedback_)
+  if (has_feedback_ || has_condition_)
+  {
+    if (feedback_input_topic_name_ == condition_input_topic_name_ && has_feedback_ && has_condition_)
+    {
+      io_sub_ = nh_.subscribe<robotnik_msgs::inputs_outputs>(feedback_input_topic_name_, 1, &PadPluginSetOutput::ioCb, this);
+    }
+    else
+    {
+      if (has_feedback_)
+      {
+        feedback_io_sub_ = nh_.subscribe<robotnik_msgs::inputs_outputs>(feedback_input_topic_name_, 1, &PadPluginSetOutput::feedbackIoCb, this);
+      }
+      if (has_condition_)
+      {
+        condition_io_sub_ = nh_.subscribe<robotnik_msgs::inputs_outputs>(condition_input_topic_name_, 1, &PadPluginSetOutput::conditionIoCb, this);
+      }
+    }
+  }
+}
+
+void PadPluginSetOutput::getIoValues(ros::NodeHandle& nh, const std::string& param_name, std::vector<io>& variable)
+{
+  XmlRpc::XmlRpcValue in_out;
+  nh.getParam(param_name, in_out);
+  int i;
+  io aux_in_out;
+  if (in_out.getType() == XmlRpc::XmlRpcValue::TypeArray)
+  {
+    for (i = 0; i < in_out.size(); i++)
+    {
+      aux_in_out.number = in_out[i]["number"];
+      aux_in_out.value = in_out[i]["value"];
+      variable.push_back(aux_in_out);
+    }
+  }
+  else if (in_out.getType() == XmlRpc::XmlRpcValue::TypeStruct)
   {
-    std::string topic_name = use_plc_ ? modbus_io_topic_name_ : base_hw_io_topic_name_;
-    io_sub_ = nh_.subscribe<robotnik_msgs::inputs_outputs>(topic_name, 1, &PadPluginSetOutput::ioCb, this);
+    aux_in_out.number = in_out["number"];
+    aux_in_out.value = in_out["value"];
+    variable.push_back(aux_in_out);
+  }
+  else
+  {
+    ROS_ERROR("Parameter %s does not contain a list or a struct with int 'number' and bool 'value'.", param_name.c_str());
   }
 }
 
 void PadPluginSetOutput::ioCb(const robotnik_msgs::inputs_outputs::ConstPtr& msg)
 {
   io_ = *msg;
+  condition_io_ = *msg;
+}
+
+void PadPluginSetOutput::feedbackIoCb(const robotnik_msgs::inputs_outputs::ConstPtr& msg)
+{
+  io_ = *msg;
+}
+
+void PadPluginSetOutput::conditionIoCb(const robotnik_msgs::inputs_outputs::ConstPtr& msg)
+{
+  condition_io_ = *msg;
 }
 
 void PadPluginSetOutput::execute(const std::vector<Button>& buttons, std::vector<float>& axes)
 {
   if (buttons[button_dead_man_].isPressed())
   {
-    if (axes[axis_set_output_] > 0.95)
+    if (((axes[axis_set_output_] > 0.95 && !use_buttons_) || (buttons[button_set_output_].isReleased() && use_buttons_)) && checkCondition())
     {
       if (!output_sent_)
       {
+        ROS_INFO_NAMED("PadPluginSetOutput", "PadPluginSetOutput::execute: Setting outputs...");
         output_sent_ = true;
         activate_ = true;
         init_timeout_ = ros::Time::now();
       }
     }
-    if (axes[axis_set_output_] < -0.95)
+    if (((axes[axis_set_output_] < -0.95  && !use_buttons_) || (buttons[button_reset_output_].isReleased() && use_buttons_)) && checkCondition())
     {
       if (!output_sent_)
       {
+        ROS_INFO_NAMED("PadPluginSetOutput", "PadPluginSetOutput::execute: Resetting outputs...");
         output_sent_ = true;
         deactivate_ = true;
         init_timeout_ = ros::Time::now();
@@ -119,38 +157,50 @@ void PadPluginSetOutput::execute(const std::vector<Button>& buttons, std::vector
     robotnik_msgs::set_digital_output set_output_srv;
     if (activate_)
     {
-      ROS_INFO_NAMED("PadPluginSetOutput", "PadPluginSetOutput::execute: Setting outputs...");
       for (int i = 0; i < outputs_.size(); i++)
       {
-        set_output_srv.request.output = outputs_[i];
-        set_output_srv.request.value = output_values_[i];
-      if (set_output_client_.call(set_output_srv) != true || set_output_srv.response.ret != true)
-      {
-        ROS_ERROR_NAMED("PadPluginSetOutput", "PadPluginSetOutput::execute: Error setting outputs");
+        set_output_srv.request.output = outputs_[i].number;
+        set_output_srv.request.value = outputs_[i].value;
+        if (set_output_client_.call(set_output_srv) != true || set_output_srv.response.ret != true)
+        {
+          ROS_ERROR_NAMED("PadPluginSetOutput", "PadPluginSetOutput::execute: Error setting outputs");
+        }
       }
+      if (checkFeedback(false))
+      {
+        output_sent_ = false;
+        activate_ = false;
+        ROS_INFO_NAMED("PadPluginSetOutput", "PadPluginSetOutput::execute: Outputs correctly set");
       }
-      if (checkFeedback(false) || ros::Time::now() - init_timeout_ > ros::Duration(timeout_))
+      else if (ros::Time::now() - init_timeout_ > ros::Duration(timeout_))
       {
         output_sent_ = false;
         activate_ = false;
+        ROS_ERROR_NAMED("PadPluginSetOutput", "PadPluginSetOutput::execute: Error setting outputs after %.2f seconds", timeout_);
       }
     }
     else if (deactivate_)
     {
-      ROS_INFO_NAMED("PadPluginSetOutput", "PadPluginSetOutput::execute: Setting outputs...");
       for (int i = 0; i < outputs_.size(); i++)
       {
-        set_output_srv.request.output = outputs_[i];
-        set_output_srv.request.value = !output_values_[i];
+        set_output_srv.request.output = outputs_[i].number;
+        set_output_srv.request.value = !outputs_[i].value;
         if (set_output_client_.call(set_output_srv) != true || set_output_srv.response.ret != true)
         {
-          ROS_ERROR_NAMED("PadPluginSetOutput", "PadPluginSetOutput::execute: Error setting outputs");
+          ROS_ERROR_NAMED("PadPluginSetOutput", "PadPluginSetOutput::execute: Error resetting outputs");
         }
       }
-      if (checkFeedback(true) || ros::Time::now() - init_timeout_ > ros::Duration(timeout_))
+      if (checkFeedback(true))
+      {
+        output_sent_ = false;
+        deactivate_ = false;
+        ROS_INFO_NAMED("PadPluginSetOutput", "PadPluginSetOutput::execute: Outputs correctly reset");
+      }
+      else if (ros::Time::now() - init_timeout_ > ros::Duration(timeout_))
       {
         output_sent_ = false;
         deactivate_ = false;
+        ROS_ERROR_NAMED("PadPluginSetOutput", "PadPluginSetOutput::execute: Error resetting outputs after %.2f seconds", timeout_);
       }
     }
   }
@@ -165,7 +215,20 @@ bool PadPluginSetOutput::checkFeedback(bool invert_values)
 
   for (int i = 0; i < inputs_.size(); i++)
   {
-    success &= io_.digital_inputs[inputs_[i] - 1] == (input_values_[i] ^ invert_values);
+    success &= io_.digital_inputs[inputs_[i].number - 1] == (inputs_[i].value ^ invert_values);
+  }
+  return success;
+}
+
+bool PadPluginSetOutput::checkCondition()
+{
+  if (!has_condition_)
+    return true;
+
+  bool success = true;
+  for (int i = 0; i < condition_inputs_.size(); i++)
+  {
+    success &= (condition_io_.digital_inputs[condition_inputs_[i].number - 1] == condition_inputs_[i].value);
   }
   return success;
 }

From d9263d27ee1f4c1fd0b16416c9ad94fd35ec8325 Mon Sep 17 00:00:00 2001
From: Jose Antonio Mendez <jamendez@robotnik.es>
Date: Mon, 10 Feb 2025 13:53:48 +0100
Subject: [PATCH 4/4] Debug if outputs are not set because of the unmet
 condition

---
 robotnik_pad_plugins/src/set_output_plugin.cpp | 16 ++++++++++++----
 1 file changed, 12 insertions(+), 4 deletions(-)

diff --git a/robotnik_pad_plugins/src/set_output_plugin.cpp b/robotnik_pad_plugins/src/set_output_plugin.cpp
index 22563d1..3591aa0 100644
--- a/robotnik_pad_plugins/src/set_output_plugin.cpp
+++ b/robotnik_pad_plugins/src/set_output_plugin.cpp
@@ -127,9 +127,13 @@ void PadPluginSetOutput::execute(const std::vector<Button>& buttons, std::vector
 {
   if (buttons[button_dead_man_].isPressed())
   {
-    if (((axes[axis_set_output_] > 0.95 && !use_buttons_) || (buttons[button_set_output_].isReleased() && use_buttons_)) && checkCondition())
+    if ((axes[axis_set_output_] > 0.95 && !use_buttons_) || (buttons[button_set_output_].isReleased() && use_buttons_))
     {
-      if (!output_sent_)
+      if (!checkCondition())
+      {
+        ROS_ERROR_NAMED("PadPluginSetOutput", "PadPluginSetOutput::execute: Conditions not met for output activation");
+      }
+      else if (!output_sent_)
       {
         ROS_INFO_NAMED("PadPluginSetOutput", "PadPluginSetOutput::execute: Setting outputs...");
         output_sent_ = true;
@@ -137,9 +141,13 @@ void PadPluginSetOutput::execute(const std::vector<Button>& buttons, std::vector
         init_timeout_ = ros::Time::now();
       }
     }
-    if (((axes[axis_set_output_] < -0.95  && !use_buttons_) || (buttons[button_reset_output_].isReleased() && use_buttons_)) && checkCondition())
+    if ((axes[axis_set_output_] < -0.95  && !use_buttons_) || (buttons[button_reset_output_].isReleased() && use_buttons_))
     {
-      if (!output_sent_)
+      if (!checkCondition())
+      {
+        ROS_ERROR_NAMED("PadPluginSetOutput", "PadPluginSetOutput::execute: Conditions not met for output deactivation");
+      }
+      else if (!output_sent_)
       {
         ROS_INFO_NAMED("PadPluginSetOutput", "PadPluginSetOutput::execute: Resetting outputs...");
         output_sent_ = true;