From 2f615daf3a1587a3eb78e89b1cf41e2195bc4062 Mon Sep 17 00:00:00 2001
From: Luis <luisebt995@gmail.com>
Date: Fri, 8 Jul 2022 11:51:12 +0200
Subject: [PATCH 1/5] Add new axes class.

---
 robotnik_pad/include/robotnik_pad/axes.h      | 84 +++++++++++++++++++
 .../include/robotnik_pad/generic_pad_plugin.h |  7 +-
 .../include/robotnik_pad/robotnik_pad.h       |  3 +-
 robotnik_pad/src/robotnik_pad.cpp             | 11 ++-
 .../ackermann_movement_plugin.h               |  2 +-
 .../robotnik_pad_plugins/elevator_plugin.h    |  7 +-
 .../robotnik_pad_plugins/movement_plugin.h    |  2 +-
 .../src/ackermann_movement_plugin.cpp         |  6 +-
 robotnik_pad_plugins/src/elevator_plugin.cpp  | 82 ++++++++++++------
 robotnik_pad_plugins/src/movement_plugin.cpp  | 22 ++---
 10 files changed, 177 insertions(+), 49 deletions(-)
 create mode 100644 robotnik_pad/include/robotnik_pad/axes.h

diff --git a/robotnik_pad/include/robotnik_pad/axes.h b/robotnik_pad/include/robotnik_pad/axes.h
new file mode 100644
index 0000000..7440dad
--- /dev/null
+++ b/robotnik_pad/include/robotnik_pad/axes.h
@@ -0,0 +1,84 @@
+#ifndef _AXES_
+#define _AXES_
+
+//! Class to save the state of the buttons
+class Axes
+{
+  int is_pressed_pos, is_pressed_neg;
+  bool is_released_pos, is_released_neg;
+	float axis_value;
+
+public:
+  Axes()
+  {
+    is_pressed_pos = 0;
+		is_pressed_neg = 0;
+    is_released_pos = false;
+		is_released_neg = false;
+		axis_value = 0.0;
+  }
+
+  //! Set the button as 'pressed'/'released'
+  void press(float value)
+  {
+    if (is_pressed_pos and !(value > 0))
+    {
+      is_released_pos = true;
+    }
+    else if (is_released_pos and (value > 0))
+			is_released_pos = false;
+
+		if (is_pressed_neg and !(value < 0))
+		{
+			is_released_neg = true;
+		}
+		else if (is_released_neg and (value < 0))
+			is_released_neg = false;
+
+		if (value > 0)
+			is_pressed_pos = true;
+		else
+			is_pressed_pos = false;
+
+		if (value < 0)
+			is_pressed_neg = true;
+		else
+			is_pressed_neg = false;
+
+		axis_value = value;
+  }
+
+  int isPressedPos() const
+  {
+    return is_pressed_pos;
+  }
+	int isPressedNeg() const
+  {
+    return is_pressed_neg;
+  }
+
+  bool isReleasedPos() const
+  {
+    return is_released_pos;
+  }
+
+	bool isReleasedNeg() const
+  {
+    return is_released_neg;
+  }
+
+	void resetReleased()
+  {
+		if (is_released_pos)
+      is_released_pos = false;
+    if (is_released_neg)
+      is_released_neg = false;
+  }
+
+	float getValue() const
+	{
+		return axis_value;
+	}
+};
+
+#endif  // _AXES_
diff --git a/robotnik_pad/include/robotnik_pad/generic_pad_plugin.h b/robotnik_pad/include/robotnik_pad/generic_pad_plugin.h
index 361263b..01baf09 100644
--- a/robotnik_pad/include/robotnik_pad/generic_pad_plugin.h
+++ b/robotnik_pad/include/robotnik_pad/generic_pad_plugin.h
@@ -2,6 +2,7 @@
 #define GENERIC_PAD_PLUGIN_H_
 
 #include <robotnik_pad/button.h>
+#include <robotnik_pad/axes.h>
 #include <ros/ros.h>
 #include <boost/scoped_ptr.hpp>
 #include <vector>
@@ -15,7 +16,7 @@ class GenericPadPlugin
 
 public:
   virtual void initialize(const ros::NodeHandle& nh, const std::string& plugin_ns) = 0;
-  virtual void execute(const std::vector<Button>& buttons, std::vector<float>& axes) = 0;
+  virtual void execute(const std::vector<Button>& buttons, const std::vector<Axes>& axes) = 0;
   virtual ~GenericPadPlugin()
   {
   }
@@ -53,9 +54,9 @@ class GenericPadPlugin
 
 protected:
   std::vector<Button> buttons_;
-  std::vector<float> axes_;
+  std::vector<Axes> axes_;
   ros::NodeHandle pnh_;
   ros::NodeHandle nh_;
 };
 }
-#endif  // GENERIC_PAD_PLUGIN_H_
\ No newline at end of file
+#endif  // GENERIC_PAD_PLUGIN_H_
diff --git a/robotnik_pad/include/robotnik_pad/robotnik_pad.h b/robotnik_pad/include/robotnik_pad/robotnik_pad.h
index db9694c..f33c4f2 100644
--- a/robotnik_pad/include/robotnik_pad/robotnik_pad.h
+++ b/robotnik_pad/include/robotnik_pad/robotnik_pad.h
@@ -14,6 +14,7 @@
 
 #include <rcomponent/rcomponent.h>
 #include <robotnik_pad/button.h>
+#include <robotnik_pad/axes.h>
 
 class RobotnikPad : public rcomponent::RComponent
 {
@@ -65,7 +66,7 @@ class RobotnikPad : public rcomponent::RComponent
   double joy_timeout_;
 
   //! Vector to save the axis values
-  std::vector<float> axes_;
+  std::vector<Axes> axes_;
   //! Vector to save and control the axis values
   std::vector<Button> buttons_;
 
diff --git a/robotnik_pad/src/robotnik_pad.cpp b/robotnik_pad/src/robotnik_pad.cpp
index cf9c867..c65d84e 100644
--- a/robotnik_pad/src/robotnik_pad.cpp
+++ b/robotnik_pad/src/robotnik_pad.cpp
@@ -65,7 +65,8 @@ void RobotnikPad::initState()
 
   for (int i = 0; i < num_of_axes_; i++)
   {
-    axes_.push_back(0.0);
+		Axes a;
+    axes_.push_back(a);
   }
 
   pad_plugins_loader_ = new pluginlib::ClassLoader<pad_plugins::GenericPadPlugin>("robotnik_pad",
@@ -122,6 +123,11 @@ void RobotnikPad::readyState()
   for (auto& button : buttons_)
   {
     button.resetReleased();
+  }
+
+	for (auto& axes : axes_)
+  {
+    axes.resetReleased();
   }
 }
 
@@ -142,7 +148,8 @@ void RobotnikPad::joyCb(const sensor_msgs::Joy::ConstPtr& joy)
   // Fill in the axes and buttons arrays
   for (int i = 0; i < joy->axes.size(); i++)
   {
-    axes_[i] = joy->axes[i];
+    //axes_[i] = joy->axes[i];
+		axes_[i].press(joy->axes[i]);
   }
   for (int i = 0; i < joy->buttons.size(); i++)
   {
diff --git a/robotnik_pad_plugins/include/robotnik_pad_plugins/ackermann_movement_plugin.h b/robotnik_pad_plugins/include/robotnik_pad_plugins/ackermann_movement_plugin.h
index 2452a66..98022f4 100644
--- a/robotnik_pad_plugins/include/robotnik_pad_plugins/ackermann_movement_plugin.h
+++ b/robotnik_pad_plugins/include/robotnik_pad_plugins/ackermann_movement_plugin.h
@@ -15,7 +15,7 @@ class PadPluginAckermannMovement : public GenericPadPlugin
   ~PadPluginAckermannMovement();
 
   virtual void initialize(const ros::NodeHandle& nh, const std::string& plugin_ns);
-  virtual void execute(const std::vector<Button>& buttons, std::vector<float>& axes);
+  virtual void execute(const std::vector<Button>& buttons, const std::vector<Axes>& axes);
 
 protected:
   int button_dead_man_, axis_linear_x_, axis_linear_y_, axis_angular_z_;
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 6549a8f..62568d3 100644
--- a/robotnik_pad_plugins/include/robotnik_pad_plugins/elevator_plugin.h
+++ b/robotnik_pad_plugins/include/robotnik_pad_plugins/elevator_plugin.h
@@ -13,14 +13,15 @@ class PadPluginElevator : public GenericPadPlugin
   ~PadPluginElevator();
 
   virtual void initialize(const ros::NodeHandle &nh, const std::string &plugin_ns);
-  virtual void execute(const std::vector<Button> &buttons, std::vector<float> &axes);
+  virtual void execute(const std::vector<Button> &buttons, const std::vector<Axes> &axes);
 
 protected:
   int button_dead_man_;
-  double axis_elevator_;
+  int axis_elevator_up_;
+	int axis_elevator_down_;
 
   std::string elevator_service_name_;
   ros::ServiceClient set_elevator_client_;
 };
 }  // namespace pad_plugins
-#endif  // PAD_PLUGIN_ELEVATOR_H_
\ No newline at end of file
+#endif  // PAD_PLUGIN_ELEVATOR_H_
diff --git a/robotnik_pad_plugins/include/robotnik_pad_plugins/movement_plugin.h b/robotnik_pad_plugins/include/robotnik_pad_plugins/movement_plugin.h
index d47d5de..fd623f6 100644
--- a/robotnik_pad_plugins/include/robotnik_pad_plugins/movement_plugin.h
+++ b/robotnik_pad_plugins/include/robotnik_pad_plugins/movement_plugin.h
@@ -28,7 +28,7 @@ class PadPluginMovement : public GenericPadPlugin
   ~PadPluginMovement();
 
   virtual void initialize(const ros::NodeHandle& nh, const std::string& plugin_ns);
-  virtual void execute(const std::vector<Button>& buttons, std::vector<float>& axes);
+  virtual void execute(const std::vector<Button>& buttons, const std::vector<Axes>& axes);
 
 protected:
   int button_dead_man_, axis_linear_x_, axis_linear_y_, axis_angular_z_, button_kinematic_mode_;
diff --git a/robotnik_pad_plugins/src/ackermann_movement_plugin.cpp b/robotnik_pad_plugins/src/ackermann_movement_plugin.cpp
index e60e8ce..90e2160 100644
--- a/robotnik_pad_plugins/src/ackermann_movement_plugin.cpp
+++ b/robotnik_pad_plugins/src/ackermann_movement_plugin.cpp
@@ -47,7 +47,7 @@ void PadPluginAckermannMovement::initialize(const ros::NodeHandle& nh, const std
   movement_status_msg_ = robotnik_pad_msgs::MovementStatus();
 }
 
-void PadPluginAckermannMovement::execute(const std::vector<Button>& buttons, std::vector<float>& axes)
+void PadPluginAckermannMovement::execute(const std::vector<Button>& buttons, const std::vector<Axes>& axes)
 {
   if (buttons[button_dead_man_].isPressed())
   {
@@ -62,8 +62,8 @@ void PadPluginAckermannMovement::execute(const std::vector<Button>& buttons, std
       ROS_INFO("PadPluginAckermannMovement::execute: speed up -> velocity level = %.1f%%", current_velocity_level_ * 100.0);
     }
 
-    cmd_ackermann_.speed = current_velocity_level_ * max_speed_ * axes[axis_linear_x_];
-    cmd_ackermann_.steering_angle = max_steering_angle_ * axes[axis_angular_z_];
+    cmd_ackermann_.speed = current_velocity_level_ * max_speed_ * axes[axis_linear_x_].getValue();
+    cmd_ackermann_.steering_angle = max_steering_angle_ * axes[axis_angular_z_].getValue();
 
     ackermann_pub_.publish(cmd_ackermann_);
   }
diff --git a/robotnik_pad_plugins/src/elevator_plugin.cpp b/robotnik_pad_plugins/src/elevator_plugin.cpp
index 13ab675..f8523a2 100644
--- a/robotnik_pad_plugins/src/elevator_plugin.cpp
+++ b/robotnik_pad_plugins/src/elevator_plugin.cpp
@@ -16,39 +16,73 @@ void PadPluginElevator::initialize(const ros::NodeHandle& nh, const std::string&
   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_, "config/axis_elevator_up", axis_elevator_up_, axis_elevator_up_, required);
+	readParam(pnh_, "config/axis_elevator_down", axis_elevator_down_, axis_elevator_down_, required);
   readParam(pnh_, "elevator_service_name", elevator_service_name_, elevator_service_name_, required);
 
   // Service client
   set_elevator_client_ = nh_.serviceClient<robotnik_msgs::SetElevator>(elevator_service_name_);
 }
 
-void PadPluginElevator::execute(const std::vector<Button>& buttons, std::vector<float>& axes)
+void PadPluginElevator::execute(const std::vector<Button>& buttons, const std::vector<Axes>& axes)
 {
   if (buttons[button_dead_man_].isPressed())
   {
-    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...");
-      if (set_elevator_client_.call(elevator_msg_srv) != true || elevator_msg_srv.response.ret != true)
-      {
-        ROS_ERROR_NAMED("PadPluginElevator", "PadPluginElevator::execute: Error raising");
-      }
-    }
-    if (axes[axis_elevator_] < -0.95)
-    {
-      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");
-      }
-    }
+		if (axis_elevator_up_ > 0)
+		{
+    	if (axes[axis_elevator_up_].isReleasedPos())
+    	{
+      	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
+		{
+			if (axes[abs(axis_elevator_up_)].isReleasedNeg())
+    	{
+      	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");
+      	}
+    	}
+		}
+		if (axis_elevator_down_ > 0)
+		{
+    	if (axes[axis_elevator_down_].isReleasedPos())
+    	{
+      	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
+		{
+			if (axes[abs(axis_elevator_down_)].isReleasedNeg())
+    	{
+      	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 if (buttons[button_dead_man_].isReleased())
   {
diff --git a/robotnik_pad_plugins/src/movement_plugin.cpp b/robotnik_pad_plugins/src/movement_plugin.cpp
index ee5d5e4..62c6867 100644
--- a/robotnik_pad_plugins/src/movement_plugin.cpp
+++ b/robotnik_pad_plugins/src/movement_plugin.cpp
@@ -44,7 +44,7 @@ void PadPluginMovement::initialize(const ros::NodeHandle& nh, const std::string&
   readParam(pnh_, "config/axis_watchdog", axis_accel_watchdog_, axis_accel_watchdog_, not_required);
   watchdog_duration_ = 0.5;
   readParam(pnh_, "config/watchdog_duration", watchdog_duration_, watchdog_duration_, not_required);
-  
+
   // if not set, then ackermann mode cannot be used
   wheel_base_ = 0;
   readParam(pnh_, "wheel_base", wheel_base_, wheel_base_);
@@ -67,7 +67,7 @@ void PadPluginMovement::initialize(const ros::NodeHandle& nh, const std::string&
   watchdog_activated_ = false;
 }
 
-void PadPluginMovement::execute(const std::vector<Button>& buttons, std::vector<float>& axes)
+void PadPluginMovement::execute(const std::vector<Button>& buttons, const std::vector<Axes>& axes)
 {
   if (buttons[button_dead_man_].isPressed())
   {
@@ -75,9 +75,9 @@ 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_])
+      if (last_accel_value_ != axes[axis_accel_watchdog_].getValue())
       {
-        last_accel_value_ = axes[axis_accel_watchdog_];
+        last_accel_value_ = axes[axis_accel_watchdog_].getValue();
         last_accel_time_ = ros::Time::now();
         watchdog_activated_ = false;
       }
@@ -100,7 +100,7 @@ void PadPluginMovement::execute(const std::vector<Button>& buttons, std::vector<
         }
       }
     }
-    
+
     if (buttons[button_speed_down_].isReleased())
     {
       current_velocity_level_ = std::max(min_velocity_level_, current_velocity_level_ - velocity_level_step_);
@@ -141,19 +141,19 @@ void PadPluginMovement::execute(const std::vector<Button>& buttons, std::vector<
 
     if (kinematic_mode_ == KinematicModes::Ackermann)
     {
-      cmd_twist_.linear.x = current_velocity_level_ * max_linear_speed_ * axes[axis_linear_x_] * std::cos(axes[axis_angular_z_] * (M_PI / 2.0));
-      cmd_twist_.angular.z = current_velocity_level_ * max_linear_speed_ * axes[axis_linear_x_] *
-                             std::sin(axes[axis_angular_z_] * (M_PI / 2.0)) / wheel_base_;
+      cmd_twist_.linear.x = current_velocity_level_ * max_linear_speed_ * axes[axis_linear_x_].getValue() * std::cos(axes[axis_angular_z_].getValue() * (M_PI / 2.0));
+      cmd_twist_.angular.z = current_velocity_level_ * max_linear_speed_ * axes[axis_linear_x_].getValue() *
+                             std::sin(axes[axis_angular_z_].getValue() * (M_PI / 2.0)) / wheel_base_;
     }
     else
     {
-      cmd_twist_.linear.x = current_velocity_level_ * max_linear_speed_ * axes[axis_linear_x_];
-      cmd_twist_.angular.z = current_velocity_level_ * max_angular_speed_ * axes[axis_angular_z_];
+      cmd_twist_.linear.x = current_velocity_level_ * max_linear_speed_ * axes[axis_linear_x_].getValue();
+      cmd_twist_.angular.z = current_velocity_level_ * max_angular_speed_ * axes[axis_angular_z_].getValue();
     }
 
     if (kinematic_mode_ == KinematicModes::Omnidirectional)
     {
-      cmd_twist_.linear.y = current_velocity_level_ * max_linear_speed_ * axes[axis_linear_y_];
+      cmd_twist_.linear.y = current_velocity_level_ * max_linear_speed_ * axes[axis_linear_y_].getValue();
     }
     else
     {

From a1941276378980df1b5e91a08e2b919e6bb5febc Mon Sep 17 00:00:00 2001
From: Claudia <cgonzalez@robotnik.es>
Date: Mon, 6 Feb 2023 13:49:10 +0100
Subject: [PATCH 2/5] wip

---
 .../include/robotnik_pad_plugins/poi_plugin.h        |  2 +-
 .../include/robotnik_pad_plugins/ptz_plugin.h        |  6 +++---
 robotnik_pad_plugins/src/movement_plugin.cpp         |  1 +
 robotnik_pad_plugins/src/poi_plugin.cpp              |  2 +-
 robotnik_pad_plugins/src/ptz_plugin.cpp              | 12 ++++++------
 5 files changed, 12 insertions(+), 11 deletions(-)

diff --git a/robotnik_pad_plugins/include/robotnik_pad_plugins/poi_plugin.h b/robotnik_pad_plugins/include/robotnik_pad_plugins/poi_plugin.h
index 1320bb1..438fb7e 100644
--- a/robotnik_pad_plugins/include/robotnik_pad_plugins/poi_plugin.h
+++ b/robotnik_pad_plugins/include/robotnik_pad_plugins/poi_plugin.h
@@ -15,7 +15,7 @@ class PadPluginPoi : public GenericPadPlugin
   ~PadPluginPoi();
 
   virtual void initialize(const ros::NodeHandle& nh, const std::string& plugin_ns);
-  virtual void execute(const std::vector<Button>& buttons, std::vector<float>& axes);
+  virtual void execute(const std::vector<Button>& buttons, const std::vector<Axes>& axes);
 
   int button_dead_man_;
   int save_poi_l3_;
diff --git a/robotnik_pad_plugins/include/robotnik_pad_plugins/ptz_plugin.h b/robotnik_pad_plugins/include/robotnik_pad_plugins/ptz_plugin.h
index f62bf27..ae42622 100644
--- a/robotnik_pad_plugins/include/robotnik_pad_plugins/ptz_plugin.h
+++ b/robotnik_pad_plugins/include/robotnik_pad_plugins/ptz_plugin.h
@@ -24,9 +24,9 @@ class PadPluginPtz : public GenericPadPlugin
   ~PadPluginPtz();
 
   virtual void initialize(const ros::NodeHandle &nh, const std::string &plugin_ns);
-  virtual void execute(const std::vector<Button> &buttons, std::vector<float> &axes);
+  virtual void execute(const std::vector<Button> &buttons, const std::vector<Axes> &axes);
 
-  void updateArrows(std::vector<float> &axes);
+  void updateArrows(const std::vector<Axes> &axes);
   void resetReleasedArrows();
   robotnik_msgs::ptz positionControl(const std::vector<Button> &buttons);
   robotnik_msgs::ptz speedControl(const std::vector<Button> &buttons);
@@ -79,4 +79,4 @@ class PadPluginPtz : public GenericPadPlugin
 
 };
 }  // namespace pad_plugins
-#endif  // PAD_PLUGIN_PTZ_H_
\ No newline at end of file
+#endif  // PAD_PLUGIN_PTZ_H_
diff --git a/robotnik_pad_plugins/src/movement_plugin.cpp b/robotnik_pad_plugins/src/movement_plugin.cpp
index 62c6867..1de34d0 100644
--- a/robotnik_pad_plugins/src/movement_plugin.cpp
+++ b/robotnik_pad_plugins/src/movement_plugin.cpp
@@ -71,6 +71,7 @@ void PadPluginMovement::execute(const std::vector<Button>& buttons, const std::v
 {
   if (buttons[button_dead_man_].isPressed())
   {
+    // ROS_WARN("DEAD MAN BUTTON PRESSED");
     // We monitor watchdog
     if (use_accel_watchdog_)
     {
diff --git a/robotnik_pad_plugins/src/poi_plugin.cpp b/robotnik_pad_plugins/src/poi_plugin.cpp
index fa41c1c..445de4d 100644
--- a/robotnik_pad_plugins/src/poi_plugin.cpp
+++ b/robotnik_pad_plugins/src/poi_plugin.cpp
@@ -33,7 +33,7 @@ namespace pad_plugins{
     }
 
 
-    void PadPluginPoi::execute(const std::vector<Button>& buttons, std::vector<float>& axes)
+    void PadPluginPoi::execute(const std::vector<Button>& buttons, const std::vector<Axes>& axes)
     {
 
         if (buttons[button_dead_man_].isPressed())
diff --git a/robotnik_pad_plugins/src/ptz_plugin.cpp b/robotnik_pad_plugins/src/ptz_plugin.cpp
index 26aca84..de21bc8 100644
--- a/robotnik_pad_plugins/src/ptz_plugin.cpp
+++ b/robotnik_pad_plugins/src/ptz_plugin.cpp
@@ -75,7 +75,7 @@ namespace pad_plugins
 
     }
 
-    void PadPluginPtz::updateArrows(std::vector<float> &axes){
+    void PadPluginPtz::updateArrows(const std::vector<Axes> &axes){
 
         /*
         * Arrow buttons are considered axes by the joy node. 
@@ -83,12 +83,12 @@ namespace pad_plugins
         * the methods of the Button class
         */
 
-        if (axes[button_vertical_arrow_] > 0){
+        if (axes[button_vertical_arrow_].getValue() > 0){
             up_arrow.press(1);
             down_arrow.press(0);
 
         }
-        else if (axes[button_vertical_arrow_] < 0){
+        else if (axes[button_vertical_arrow_].getValue() < 0){
             up_arrow.press(0);
             down_arrow.press(1);
 
@@ -99,11 +99,11 @@ namespace pad_plugins
         }
 
 
-        if (axes[button_horizontal_arrow_] > 0){
+        if (axes[button_horizontal_arrow_].getValue() > 0){
             right_arrow.press(1);
             left_arrow.press(0);
         }
-        else if (axes[button_horizontal_arrow_] < 0){
+        else if (axes[button_horizontal_arrow_].getValue() < 0){
             right_arrow.press(0);
             left_arrow.press(1);
         }
@@ -344,7 +344,7 @@ namespace pad_plugins
     }
 
 
-    void PadPluginPtz::execute(const std::vector<Button> &buttons, std::vector<float> &axes){
+    void PadPluginPtz::execute(const std::vector<Button> &buttons, const std::vector<Axes> &axes){
 
         // Revisar zoom en posicion y velocidad
         // Revisar timeout en zoom

From ab525a907ca02e10f30c86c605628db6236d819b Mon Sep 17 00:00:00 2001
From: Claudia <cgonzalez@robotnik.es>
Date: Tue, 7 Feb 2023 13:10:13 +0100
Subject: [PATCH 3/5] modified axes class so it is a bit cleaner

---
 robotnik_pad/CMakeLists.txt              |  3 +-
 robotnik_pad/include/robotnik_pad/axes.h | 55 ++++++++----------------
 robotnik_pad/package.xml                 |  2 +
 robotnik_pad_plugins/CMakeLists.txt      |  3 +-
 robotnik_pad_plugins/package.xml         |  3 +-
 5 files changed, 26 insertions(+), 40 deletions(-)

diff --git a/robotnik_pad/CMakeLists.txt b/robotnik_pad/CMakeLists.txt
index 454c7ed..0174ef1 100644
--- a/robotnik_pad/CMakeLists.txt
+++ b/robotnik_pad/CMakeLists.txt
@@ -15,12 +15,13 @@ find_package(catkin REQUIRED COMPONENTS
   geometry_msgs
   ackermann_msgs
   pluginlib
+  actionlib
 )
 
 catkin_package(
   INCLUDE_DIRS include
 #  LIBRARIES robotnik_pad
-  CATKIN_DEPENDS roscpp rcomponent robotnik_msgs sensor_msgs geometry_msgs ackermann_msgs pluginlib
+  CATKIN_DEPENDS roscpp rcomponent robotnik_msgs sensor_msgs geometry_msgs ackermann_msgs pluginlib actionlib
 #  DEPENDS system_lib
 )
 
diff --git a/robotnik_pad/include/robotnik_pad/axes.h b/robotnik_pad/include/robotnik_pad/axes.h
index 7440dad..2c12d5c 100644
--- a/robotnik_pad/include/robotnik_pad/axes.h
+++ b/robotnik_pad/include/robotnik_pad/axes.h
@@ -1,51 +1,32 @@
 #ifndef _AXES_
 #define _AXES_
 
+
+#include <iostream>
 //! Class to save the state of the buttons
 class Axes
 {
-  int is_pressed_pos, is_pressed_neg;
-  bool is_released_pos, is_released_neg;
-	float axis_value;
+  int is_pressed_pos{0};
+  int is_pressed_neg{0};
+  bool is_released_pos{false};
+  bool is_released_neg{false};
+	float axis_value{0.0};
 
 public:
-  Axes()
-  {
-    is_pressed_pos = 0;
-		is_pressed_neg = 0;
-    is_released_pos = false;
-		is_released_neg = false;
-		axis_value = 0.0;
-  }
+  Axes(){}
 
-  //! Set the button as 'pressed'/'released'
   void press(float value)
   {
-    if (is_pressed_pos and !(value > 0))
-    {
-      is_released_pos = true;
-    }
-    else if (is_released_pos and (value > 0))
-			is_released_pos = false;
-
-		if (is_pressed_neg and !(value < 0))
-		{
-			is_released_neg = true;
-		}
-		else if (is_released_neg and (value < 0))
-			is_released_neg = false;
-
-		if (value > 0)
-			is_pressed_pos = true;
-		else
-			is_pressed_pos = false;
-
-		if (value < 0)
-			is_pressed_neg = true;
-		else
-			is_pressed_neg = false;
+    bool isValuePositive = (value > 0);
+    bool isValueNegative = (value < 0);
 
-		axis_value = value;
+    is_released_pos = (is_pressed_pos && !isValuePositive);
+    is_released_neg = (is_pressed_neg && !isValueNegative);
+    
+		is_pressed_pos = isValuePositive;
+		is_pressed_neg = isValueNegative;
+		
+    axis_value = value;
   }
 
   int isPressedPos() const
@@ -81,4 +62,4 @@ class Axes
 	}
 };
 
-#endif  // _AXES_
+#endif  // _AXES_
\ No newline at end of file
diff --git a/robotnik_pad/package.xml b/robotnik_pad/package.xml
index af4b9a6..47e56ed 100644
--- a/robotnik_pad/package.xml
+++ b/robotnik_pad/package.xml
@@ -25,6 +25,7 @@
   <build_depend>geometry_msgs</build_depend>
   <build_depend>ackermann_msgs</build_depend>
   <build_depend>pluginlib</build_depend>
+  <build_depend>actionlib</build_depend>
 
   <build_export_depend>roscpp</build_export_depend>
 
@@ -37,6 +38,7 @@
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>ackermann_msgs</exec_depend>
   <exec_depend>pluginlib</exec_depend>
+  <exec_depend>actionlib</exec_depend>
   <exec_depend>joy</exec_depend>
 
 
diff --git a/robotnik_pad_plugins/CMakeLists.txt b/robotnik_pad_plugins/CMakeLists.txt
index 3d7870d..52fd037 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
 )
 
diff --git a/robotnik_pad_plugins/package.xml b/robotnik_pad_plugins/package.xml
index 79dadff..0aa4583 100644
--- a/robotnik_pad_plugins/package.xml
+++ b/robotnik_pad_plugins/package.xml
@@ -18,13 +18,14 @@
   <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>
   <exec_depend>pluginlib</exec_depend>
   <exec_depend>robotnik_pad</exec_depend>
   <exec_depend>robotnik_pad_msgs</exec_depend>
-
+  <exec_depend>actionlib</exec_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>

From 18d73d95cc6474433748d3014d8db94074d0155e Mon Sep 17 00:00:00 2001
From: Claudia <cgonzalez@robotnik.es>
Date: Tue, 7 Feb 2023 15:23:11 +0100
Subject: [PATCH 4/5] moved functions to use axes as buttons to robotnik pad

---
 .../include/robotnik_pad/generic_pad_plugin.h | 78 +++++++++++++++++++
 1 file changed, 78 insertions(+)

diff --git a/robotnik_pad/include/robotnik_pad/generic_pad_plugin.h b/robotnik_pad/include/robotnik_pad/generic_pad_plugin.h
index 01baf09..e0bd5a9 100644
--- a/robotnik_pad/include/robotnik_pad/generic_pad_plugin.h
+++ b/robotnik_pad/include/robotnik_pad/generic_pad_plugin.h
@@ -6,6 +6,14 @@
 #include <ros/ros.h>
 #include <boost/scoped_ptr.hpp>
 #include <vector>
+#include <unordered_map>
+
+struct pad_element
+{
+  bool isAButton;
+  int index;
+  bool negativeAxes{false};
+};
 
 namespace pad_plugins
 {
@@ -47,6 +55,74 @@ class GenericPadPlugin
     return true;
   }
 
+void stringParser()
+{
+  bool required = true;
+  for(auto &it : button_axes_params_to_load_)
+  {
+    readParam(pnh_, it.first, it.second, it.second, required);
+    auto found_plus = it.second.find("+");
+    auto found_minus = it.second.find("-");
+    pad_element element_to_fill;
+    element_to_fill.isAButton = (found_plus==std::string::npos) && (found_minus==std::string::npos);
+    
+    if(!it.second.empty())
+    {
+      element_to_fill.index = std::stoi(it.second);
+    }
+    if(element_to_fill.index < 0 && !element_to_fill.isAButton)
+    {
+      element_to_fill.index = abs(element_to_fill.index);
+      element_to_fill.negativeAxes = true;
+    }    
+    pad_elements_vector_.push_back(element_to_fill);
+  }
+  
+}
+
+void addNewParamToRead(std::string name)
+{
+  button_axes_params_to_load_.insert({name, ""});
+}
+
+float checkIfPadElementIsPressed(int index_to_check, const std::vector<Button> &buttons, const std::vector<Axes> &axes)
+{
+  float return_value{false};
+  if(index_to_check > pad_elements_vector_.size())
+  {
+    ROS_ERROR("Index cannot be found %d", index_to_check);
+    return -1;
+  }
+  auto element = pad_elements_vector_[index_to_check];
+  if(element.isAButton)
+  {
+     return_value = buttons[element.index].isPressed();
+  }else
+  {
+    return_value = element.negativeAxes ? axes[pad_elements_vector_[index_to_check].index].isPressedNeg() : axes[pad_elements_vector_[index_to_check].index].isPressedPos();
+  }
+  return return_value;
+}
+
+float checkIfPadElementIsReleased(int index_to_check, const std::vector<Button> &buttons, const std::vector<Axes> &axes)
+{
+  float return_value;
+  if(index_to_check > pad_elements_vector_.size())
+  {
+    ROS_ERROR("Index cannot be found %d", index_to_check);
+    return -1;
+  }
+  auto element = pad_elements_vector_[index_to_check];
+  if(element.isAButton)
+  {
+     return_value = buttons[element.index].isReleased();
+  }else
+  {
+    return_value = element.negativeAxes ? axes[pad_elements_vector_[index_to_check].index].isReleasedNeg() : axes[pad_elements_vector_[index_to_check].index].isReleasedPos();
+  }
+  return return_value;
+}
+
 protected:
   GenericPadPlugin()
   {
@@ -57,6 +133,8 @@ class GenericPadPlugin
   std::vector<Axes> axes_;
   ros::NodeHandle pnh_;
   ros::NodeHandle nh_;
+  std::unordered_map<std::string, std::string> button_axes_params_to_load_;
+  std::vector<pad_element> pad_elements_vector_;
 };
 }
 #endif  // GENERIC_PAD_PLUGIN_H_

From 1f8c319f5f52b4a8fd885d707087128300685603 Mon Sep 17 00:00:00 2001
From: Claudia <cgonzalez@robotnik.es>
Date: Fri, 3 Mar 2023 09:00:18 +0100
Subject: [PATCH 5/5] missing tag in launch fle

---
 robotnik_pad/launch/robotnik_pad.launch | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/robotnik_pad/launch/robotnik_pad.launch b/robotnik_pad/launch/robotnik_pad.launch
index dad7367..7d0415b 100644
--- a/robotnik_pad/launch/robotnik_pad.launch
+++ b/robotnik_pad/launch/robotnik_pad.launch
@@ -24,4 +24,4 @@
         <param name="desired_freq" value="100.0" />
     </node>
 
-</launch>
+</launch>
\ No newline at end of file