add emergency stop

StoglRobotics-forks · Jan 11, 2024 · ce485b2 · ce485b2
1 parent ff092df
commit ce485b2
Show file tree

Hide file tree

Showing 4 changed files with 33 additions and 0 deletions.
diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp
@@ -132,6 +132,17 @@ class StateInterface : public Handle
 
   StateInterface(StateInterface && other) = default;
 
+  double emergency_stop() const
+  {
+    const double * emergency_stop = std::get_if<double>(&value_);
+    // This means the value does not store the expected datatype. How should we handle this
+    // properly?
+    THROW_ON_NOT_NULLPTR(emergency_stop);
+    return *emergency_stop;
+  }
+
+  void emergency_stop(const double & emergency_stop) { value_ = emergency_stop; }
+
   double warning_code() const
   {
     const double * warning_code = std::get_if<double>(&value_);

diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp
@@ -65,6 +65,8 @@ class LoanedStateInterface
 
   double get_value() const { return state_interface_.get_value(); }
 
+  double has_emergency_stop() const { return state_interface_.emergency_stop(); }
+
   double get_warning_code() const { return state_interface_.warning_code(); }
 
   double get_error_code() const { return state_interface_.error_code(); }

diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp
@@ -200,6 +200,16 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
     return sensor_states_.at(interface_name)->get_value();
   }
 
+  void set_emergency_stop(const std::string & interface_name, const double & emergency_stop)
+  {
+    sensor_states_.at(interface_name)->emergency_stop(emergency_stop);
+  }
+
+  double get_emergency_stop(const std::string & interface_name) const
+  {
+    return sensor_states_.at(interface_name)->emergency_stop();
+  }
+
   void set_warning_code(const std::string & interface_name, const double & warning_code)
   {
     sensor_states_.at(interface_name)->warning_code(warning_code);

diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp
@@ -348,6 +348,16 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
     return system_commands_.at(interface_name)->get_value();
   }
 
+  void set_emergency_stop(const std::string & interface_name, const double & emergency_stop)
+  {
+    system_states_.at(interface_name)->emergency_stop(emergency_stop);
+  }
+
+  double get_emergency_stop(const std::string & interface_name) const
+  {
+    return system_states_.at(interface_name)->emergency_stop();
+  }
+
   void set_warning_code(const std::string & interface_name, const double & warning_code)
   {
     system_states_.at(interface_name)->warning_code(warning_code);