From 5676bc2425e51839485b6b815ccbbb583319889e Mon Sep 17 00:00:00 2001
From: Dawid Kmak <73443304+KmakD@users.noreply.github.com>
Date: Thu, 25 Jul 2024 16:01:48 +0200
Subject: [PATCH] fix e-stop handling (#21)

---
 .../include/joy2twist/joy2twist_node.hpp      |  1 +
 joy2twist/src/joy2twist_node.cpp              | 34 +++++++++++++------
 2 files changed, 24 insertions(+), 11 deletions(-)

diff --git a/joy2twist/include/joy2twist/joy2twist_node.hpp b/joy2twist/include/joy2twist/joy2twist_node.hpp
index a9d7552..c3ef846 100644
--- a/joy2twist/include/joy2twist/joy2twist_node.hpp
+++ b/joy2twist/include/joy2twist/joy2twist_node.hpp
@@ -51,6 +51,7 @@ class Joy2TwistNode : public rclcpp::Node
   void trigger_service_cb(
     const rclcpp::Client<SrvTrigger>::SharedFuture & future,
     const std::string & service_name) const;
+  void handle_e_stop(const std::shared_ptr<MsgJoy> joy_msg);
 
   std::map<std::string, float> linear_velocity_factors_;
   std::map<std::string, float> angular_velocity_factors_;
diff --git a/joy2twist/src/joy2twist_node.cpp b/joy2twist/src/joy2twist_node.cpp
index 35592e3..256a743 100644
--- a/joy2twist/src/joy2twist_node.cpp
+++ b/joy2twist/src/joy2twist_node.cpp
@@ -84,17 +84,7 @@ void Joy2TwistNode::joy_cb(const MsgJoy::SharedPtr joy_msg)
 {
   MsgTwist twist_msg;
 
-  if (e_stop_present_) {
-    if (joy_msg->buttons.at(button_index_.e_stop_trigger) && !e_stop_state_) {
-      // Stop the robot before trying to call the e-stop trigger service
-      twist_pub_->publish(twist_msg);
-      call_trigger_service(e_stop_trigger_client_);
-    } else if (
-      joy_msg->buttons.at(button_index_.enable_e_stop_reset) &&
-      joy_msg->buttons.at(button_index_.e_stop_reset) && e_stop_state_) {
-      call_trigger_service(e_stop_reset_client_);
-    }
-  }
+  handle_e_stop(joy_msg);
 
   if (joy_msg->buttons.at(button_index_.dead_man_switch)) {
     driving_mode_ = true;
@@ -159,4 +149,26 @@ void Joy2TwistNode::trigger_service_cb(
   RCLCPP_INFO(this->get_logger(), "Successfully called %s service", service_name.c_str());
 }
 
+void Joy2TwistNode::handle_e_stop(const std::shared_ptr<MsgJoy> joy_msg)
+{
+  if (!e_stop_present_) {
+    return;
+  }
+
+  if (joy_msg->buttons.at(button_index_.e_stop_trigger)) {
+    if (!e_stop_state_) {
+      // Stop the robot before trying to call the e-stop trigger service
+      twist_pub_->publish(MsgTwist());
+      call_trigger_service(e_stop_trigger_client_);
+    }
+    return;
+  }
+
+  if (
+    joy_msg->buttons.at(button_index_.enable_e_stop_reset) &&
+    joy_msg->buttons.at(button_index_.e_stop_reset) && e_stop_state_) {
+    call_trigger_service(e_stop_reset_client_);
+  }
+}
+
 }  // namespace joy2twist