From 62f8d046105589ff320db5fd070e91e8c936a7ed Mon Sep 17 00:00:00 2001
From: Jose Antonio Mendez Ibarra <jamendez@jamendez.lan>
Date: Tue, 25 Nov 2025 17:41:59 +0100
Subject: [PATCH 1/4] Add elevator_action_plugin

---
 robotnik_pad_plugins/CMakeLists.txt           |  4 +-
 .../elevator_action_plugin.h                  | 38 ++++++++
 robotnik_pad_plugins/package.xml              |  2 +
 .../robotnik_pad_pluginlib.xml                |  3 +
 .../src/elevator_action_plugin.cpp            | 92 +++++++++++++++++++
 .../src/generic_pad_plugins.cpp               |  2 +
 6 files changed, 140 insertions(+), 1 deletion(-)
 create mode 100644 robotnik_pad_plugins/include/robotnik_pad_plugins/elevator_action_plugin.h
 create mode 100644 robotnik_pad_plugins/src/elevator_action_plugin.cpp

diff --git a/robotnik_pad_plugins/CMakeLists.txt b/robotnik_pad_plugins/CMakeLists.txt
index 41f61c6..600a217 100644
--- a/robotnik_pad_plugins/CMakeLists.txt
+++ b/robotnik_pad_plugins/CMakeLists.txt
@@ -12,13 +12,14 @@ find_package(catkin REQUIRED COMPONENTS
   pluginlib
   robotnik_pad
   robotnik_pad_msgs
+  actionlib
 )
 
 
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES robotnik_pad_plugins
-  CATKIN_DEPENDS roscpp pluginlib robotnik_pad robotnik_pad_msgs
+  CATKIN_DEPENDS roscpp pluginlib robotnik_pad robotnik_pad_msgs actionlib
 #  DEPENDS system_lib
 )
 
@@ -36,6 +37,7 @@ add_library(robotnik_pad_plugins
   src/poi_plugin.cpp
   src/ptz_plugin.cpp
   src/blkarc_plugin.cpp
+  src/elevator_action_plugin.cpp
 )
 
 add_dependencies(robotnik_pad_plugins ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
diff --git a/robotnik_pad_plugins/include/robotnik_pad_plugins/elevator_action_plugin.h b/robotnik_pad_plugins/include/robotnik_pad_plugins/elevator_action_plugin.h
new file mode 100644
index 0000000..777cdda
--- /dev/null
+++ b/robotnik_pad_plugins/include/robotnik_pad_plugins/elevator_action_plugin.h
@@ -0,0 +1,38 @@
+#ifndef PAD_PLUGIN_ELEVATOR_ACTION_H_
+#define PAD_PLUGIN_ELEVATOR_ACTION_H_
+
+#include <robotnik_msgs/ElevatorAction.h>
+#include <robotnik_msgs/SetElevatorAction.h>
+#include <robotnik_msgs/SetElevatorActionGoal.h>
+#include <robotnik_msgs/SetElevatorGoal.h>
+#include <robotnik_msgs/SetElevatorResult.h>
+#include <robotnik_pad/generic_pad_plugin.h>
+#include <actionlib/client/simple_action_client.h>
+#include <actionlib/client/simple_client_goal_state.h>
+
+namespace pad_plugins
+{
+class PadPluginElevatorAction : public GenericPadPlugin
+{
+public:
+  PadPluginElevatorAction();
+  ~PadPluginElevatorAction();
+
+  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:
+  int button_dead_man_;
+  double axis_elevator_;
+  bool elevator_is_running_;
+  bool stop_elevator_;
+  bool stop_elevator_dead_man_;
+
+  std::string elevator_action_ns_;
+  std::shared_ptr<actionlib::SimpleActionClient<robotnik_msgs::SetElevatorAction>> elevator_action_client_;
+
+  bool sendGoal(const int action);
+  void actionDoneCb(const actionlib::SimpleClientGoalState& state,  const robotnik_msgs::SetElevatorResultConstPtr& result);
+};
+}; // namespace pad_plugins
+#endif  // PAD_PLUGIN_ELEVATOR_ACTION_H_
\ No newline at end of file
diff --git a/robotnik_pad_plugins/package.xml b/robotnik_pad_plugins/package.xml
index 516875b..ad6a6bb 100644
--- a/robotnik_pad_plugins/package.xml
+++ b/robotnik_pad_plugins/package.xml
@@ -18,6 +18,7 @@
   <build_depend>pluginlib</build_depend>
   <build_depend>robotnik_pad</build_depend>
   <build_depend>robotnik_pad_msgs</build_depend>
+  <build_depend>actionlib</build_depend>
 
   <build_export_depend>roscpp</build_export_depend>
   <exec_depend>roscpp</exec_depend>
@@ -26,6 +27,7 @@
   <exec_depend>robotnik_pad_msgs</exec_depend>
 
 
+
   <!-- The export tag contains other, unspecified, tags -->
   <export>
     <!-- Other tools can request additional information be placed here -->
diff --git a/robotnik_pad_plugins/robotnik_pad_pluginlib.xml b/robotnik_pad_plugins/robotnik_pad_pluginlib.xml
index 558991e..8d41389 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/ElevatorAction" type="pad_plugins::PadPluginElevatorAction" base_class_type="pad_plugins::GenericPadPlugin">
+        <description>This is the Pad elevator action plugin.</description>
+    </class>
 </library>
diff --git a/robotnik_pad_plugins/src/elevator_action_plugin.cpp b/robotnik_pad_plugins/src/elevator_action_plugin.cpp
new file mode 100644
index 0000000..ab43109
--- /dev/null
+++ b/robotnik_pad_plugins/src/elevator_action_plugin.cpp
@@ -0,0 +1,92 @@
+#include <robotnik_pad_plugins/elevator_action_plugin.h>
+
+namespace pad_plugins
+{
+PadPluginElevatorAction::PadPluginElevatorAction()
+{
+}
+
+PadPluginElevatorAction::~PadPluginElevatorAction()
+{
+}
+
+void PadPluginElevatorAction::initialize(const ros::NodeHandle& nh, const std::string& plugin_ns)
+{
+  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_elevator", axis_elevator_, axis_elevator_, required);
+  readParam(pnh_, "elevator_action_ns", elevator_action_ns_, elevator_action_ns_, 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
+  elevator_action_client_ = std::make_shared
+      <actionlib::SimpleActionClient<robotnik_msgs::SetElevatorAction>>(nh_, elevator_action_ns_, true);
+
+  elevator_is_running_ = false;
+}
+
+void PadPluginElevatorAction::execute(const std::vector<Button>& buttons, std::vector<float>& axes)
+{
+  if (buttons[button_dead_man_].isPressed())
+  {
+    if (axes[axis_elevator_] > 0.95)
+    {
+      if (!elevator_is_running_)
+      {
+        elevator_is_running_ = sendGoal(robotnik_msgs::ElevatorAction::RAISE);
+      }
+    }
+    if (axes[axis_elevator_] < -0.95)
+    {
+      if (!elevator_is_running_)
+      {
+        elevator_is_running_ = sendGoal(robotnik_msgs::ElevatorAction::LOWER);
+      }
+    }
+
+    if (stop_elevator_)
+    {
+      if (axes[axis_elevator_] > -0.1 && axes[axis_elevator_] < 0.1 && elevator_is_running_ == true)
+      {
+        elevator_is_running_ = !sendGoal(robotnik_msgs::ElevatorAction::STOP);
+      }
+    }
+
+  }
+  else if (buttons[button_dead_man_].isReleased())
+  {
+    if (stop_elevator_dead_man_ && elevator_is_running_)
+    {
+      elevator_is_running_ = !sendGoal(robotnik_msgs::ElevatorAction::STOP);
+    }
+  }
+}
+
+void PadPluginElevatorAction::actionDoneCb(const actionlib::SimpleClientGoalState& state,  const robotnik_msgs::SetElevatorResultConstPtr& result)
+{
+  elevator_is_running_ = false;
+}
+
+bool PadPluginElevatorAction::sendGoal(const int action)
+{
+  robotnik_msgs::SetElevatorGoal elevator_goal;
+  elevator_goal.action.action = action;
+  ROS_INFO_NAMED("PadPluginElevatorAction", "PadPluginElevatorAction::execute: %d", action);
+  elevator_action_client_->sendGoal(elevator_goal,
+                                  boost::bind(&PadPluginElevatorAction::actionDoneCb, this, _1, _2));
+  auto state = elevator_action_client_->getState();
+  if (!(state == actionlib::SimpleClientGoalState::PENDING ||
+  state == actionlib::SimpleClientGoalState::ACTIVE))
+  {
+    ROS_ERROR_NAMED("PadPluginElevatorAction", "PadPluginElevatorAction::execute: Goal was not accepted. Current state: %s", state.toString().c_str());
+    return false;
+  }
+  return true;
+}
+
+
+}  // namespace pad_plugins
\ No newline at end of file
diff --git a/robotnik_pad_plugins/src/generic_pad_plugins.cpp b/robotnik_pad_plugins/src/generic_pad_plugins.cpp
index a939997..0d6e093 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/elevator_action_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::PadPluginElevatorAction, pad_plugins::GenericPadPlugin);

From 2fc414e9207f00a4ffde0321f254d40890837197 Mon Sep 17 00:00:00 2001
From: Jose Antonio Mendez Ibarra
 <126663352+jamendezib@users.noreply.github.com>
Date: Wed, 17 Dec 2025 13:04:55 +0100
Subject: [PATCH 2/4] Update
 robotnik_pad_plugins/src/elevator_action_plugin.cpp

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
---
 robotnik_pad_plugins/src/elevator_action_plugin.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/robotnik_pad_plugins/src/elevator_action_plugin.cpp b/robotnik_pad_plugins/src/elevator_action_plugin.cpp
index ab43109..adf64a4 100644
--- a/robotnik_pad_plugins/src/elevator_action_plugin.cpp
+++ b/robotnik_pad_plugins/src/elevator_action_plugin.cpp
@@ -50,7 +50,7 @@ void PadPluginElevatorAction::execute(const std::vector<Button>& buttons, std::v
 
     if (stop_elevator_)
     {
-      if (axes[axis_elevator_] > -0.1 && axes[axis_elevator_] < 0.1 && elevator_is_running_ == true)
+      if (axes[axis_elevator_] > -0.1 && axes[axis_elevator_] < 0.1 && elevator_is_running_)
       {
         elevator_is_running_ = !sendGoal(robotnik_msgs::ElevatorAction::STOP);
       }

From 72026bb3bf4a0c643504130a43d27cf4e2749f53 Mon Sep 17 00:00:00 2001
From: Jose Antonio Mendez Ibarra
 <126663352+jamendezib@users.noreply.github.com>
Date: Wed, 17 Dec 2025 13:05:05 +0100
Subject: [PATCH 3/4] Update
 robotnik_pad_plugins/src/elevator_action_plugin.cpp

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
---
 robotnik_pad_plugins/src/elevator_action_plugin.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/robotnik_pad_plugins/src/elevator_action_plugin.cpp b/robotnik_pad_plugins/src/elevator_action_plugin.cpp
index adf64a4..8f6d891 100644
--- a/robotnik_pad_plugins/src/elevator_action_plugin.cpp
+++ b/robotnik_pad_plugins/src/elevator_action_plugin.cpp
@@ -22,7 +22,7 @@ void PadPluginElevatorAction::initialize(const ros::NodeHandle& nh, const std::s
   readParam(pnh_, "elevator_action_ns", elevator_action_ns_, elevator_action_ns_, 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
+  // Actionlib action client
   elevator_action_client_ = std::make_shared
       <actionlib::SimpleActionClient<robotnik_msgs::SetElevatorAction>>(nh_, elevator_action_ns_, true);
 

From 9d465b50eec6da4b0a77bbd0aad3df2838a0953e Mon Sep 17 00:00:00 2001
From: Copilot <198982749+Copilot@users.noreply.github.com>
Date: Wed, 17 Dec 2025 13:10:46 +0100
Subject: [PATCH 4/4] * Add actionlib exec_depend to package.xml

---
 .gitignore                       | 3 ++-
 robotnik_pad_plugins/package.xml | 1 +
 2 files changed, 3 insertions(+), 1 deletion(-)

diff --git a/.gitignore b/.gitignore
index b503cae..4b09c8e 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1 +1,2 @@
-.*vscode
\ No newline at end of file
+.*vscode
+_codeql_detected_source_root
\ No newline at end of file
diff --git a/robotnik_pad_plugins/package.xml b/robotnik_pad_plugins/package.xml
index ad6a6bb..6f36dcb 100644
--- a/robotnik_pad_plugins/package.xml
+++ b/robotnik_pad_plugins/package.xml
@@ -25,6 +25,7 @@
   <exec_depend>pluginlib</exec_depend>
   <exec_depend>robotnik_pad</exec_depend>
   <exec_depend>robotnik_pad_msgs</exec_depend>
+  <exec_depend>actionlib</exec_depend>