Otis rbrobout

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

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

robotnik_pad_plugins/include/robotnik_pad_plugins/set_output_plugin.h

@@ -0,0 +1,56 @@
+#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
+{
+
+struct io {
+  int number;
+  bool value;
+};
+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);
+  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_;
+  double axis_set_output_;
+  bool output_sent_;
+  bool activate_, deactivate_;
+  bool use_plc_;
+  bool has_feedback_;
+  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 feedback_input_topic_name_;
+  std::string condition_input_topic_name_;
+  ros::ServiceClient set_output_client_;
+  ros::Subscriber io_sub_, feedback_io_sub_, condition_io_sub_;
+  robotnik_msgs::inputs_outputs io_, condition_io_;
+  ros::Time init_timeout_;
+};
+}  // namespace pad_plugins
+#endif  // PAD_PLUGIN_SET_OUTPUT_H_

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>

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;
+      }
+    }
   }
 }
 

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