Add elevator_action_plugin

.gitignore

@@ -1 +1,2 @@
-.*vscode
+.*vscode
+_codeql_detected_source_root

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})

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_

robotnik_pad_plugins/package.xml

@@ -18,12 +18,15 @@
   <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 -->

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>

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);
+  // Actionlib action 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_)
+      {
+        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

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);