RobotnikAutomation · Claudia-Gonz-Mor · Jul 8, 2022 · Feb 6, 2023 · Feb 7, 2023 · Feb 7, 2023
diff --git 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
@@ -0,0 +1,65 @@
+#ifndef _AXES_
+#define _AXES_
+
+
+#include <iostream>
+//! Class to save the state of the buttons
+class Axes
+{
+  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(){}
+
+  void press(float value)
+  {
+    bool isValuePositive = (value > 0);
+    bool isValueNegative = (value < 0);
+
+    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
+  {
+    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
@@ -2,9 +2,18 @@
 #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>
+#include <unordered_map>
+
+struct pad_element
+{
+  bool isAButton;
+  int index;
+  bool negativeAxes{false};
+};
 
 namespace pad_plugins
 {
@@ -15,7 +24,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()
   {
   }
@@ -46,16 +55,86 @@ 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()
   {
   }
 
 protected:
   std::vector<Button> buttons_;
-  std::vector<float> axes_;
+  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_
+#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
@@ -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/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>
diff --git 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/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/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/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
@@ -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_
+#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
@@ -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/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
@@ -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_
+#endif  // PAD_PLUGIN_PTZ_H_
diff --git 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>

diff --git 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_);
   }