From e27bf112030b21f158ebee495bdf44c54b403dbb Mon Sep 17 00:00:00 2001
From: Pradheep <padhupradheep@gmail.com>
Date: Fri, 20 Sep 2024 10:02:47 +0200
Subject: [PATCH 1/2] set safety modes

---
 include/neo_relayboard_v3/RelayBoardV3.h |  3 +++
 src/RelayBoardV3.cpp                     | 33 ++++++++++++++++++++++++
 2 files changed, 36 insertions(+)

diff --git a/include/neo_relayboard_v3/RelayBoardV3.h b/include/neo_relayboard_v3/RelayBoardV3.h
index 9f352d2..67b1906 100644
--- a/include/neo_relayboard_v3/RelayBoardV3.h
+++ b/include/neo_relayboard_v3/RelayBoardV3.h
@@ -19,6 +19,7 @@
 #include <neo_srvs2/srv/relay_board_set_relay.hpp>
 #include <neo_srvs2/srv/io_board_set_dig_out.hpp>
 #include <neo_srvs2/srv/relay_board_set_lcd_msg.hpp>
+#include <neo_srvs2/srv/relay_board_set_safety_mode.hpp>
 #include <neo_srvs2/srv/set_safety_field.hpp>
 #include <neo_srvs2/srv/relay_board_set_led.hpp>
 #include <neo_msgs2/msg/relay_board_v3.hpp>
@@ -84,6 +85,7 @@ class RelayBoardV3 : public RelayBoardV3Base{
 	rclcpp::Service<std_srvs::srv::Empty>::SharedPtr srv_shutdown_platform;
 	rclcpp::Service<neo_srvs2::srv::SetSafetyField>::SharedPtr srv_set_safety_field;
 	rclcpp::Service<neo_srvs2::srv::RelayBoardSetLED>::SharedPtr srv_set_leds;
+	rclcpp::Service<neo_srvs2::srv::RelayBoardSetSafetyMode>::SharedPtr srv_set_safety_mode;
 
 	bool is_shutdown = false;
 
@@ -93,6 +95,7 @@ class RelayBoardV3 : public RelayBoardV3Base{
 	bool service_stop_charging(std::shared_ptr<std_srvs::srv::Empty::Request> req, std::shared_ptr<std_srvs::srv::Empty::Response> res);
 	bool service_shutdown_platform(std::shared_ptr<std_srvs::srv::Empty::Request> req, std::shared_ptr<std_srvs::srv::Empty::Response> res);
 	bool service_set_safety_field(std::shared_ptr<neo_srvs2::srv::SetSafetyField::Request> req, std::shared_ptr<neo_srvs2::srv::SetSafetyField::Response> res);
+	bool service_set_safety_mode(std::shared_ptr<neo_srvs2::srv::RelayBoardSetSafetyMode::Request> req, std::shared_ptr<neo_srvs2::srv::RelayBoardSetSafetyMode::Response> res);
 	bool service_set_leds(std::shared_ptr<neo_srvs2::srv::RelayBoardSetLED::Request> req, std::shared_ptr<neo_srvs2::srv::RelayBoardSetLED::Response> res);
 };
 
diff --git a/src/RelayBoardV3.cpp b/src/RelayBoardV3.cpp
index 70235c1..f4319f8 100644
--- a/src/RelayBoardV3.cpp
+++ b/src/RelayBoardV3.cpp
@@ -21,6 +21,8 @@
 #include <neo_msgs2/msg/relay_board_v3.hpp>
 #include <neo_msgs2/msg/safety_state.hpp>
 #include <neo_msgs2/msg/kinematics_state.hpp>
+#include <neo_msgs2/msg/safety_mode.hpp>
+
 #include <std_msgs/msg/color_rgba.hpp>
 
 #include <cmath>
@@ -71,6 +73,7 @@ void RelayBoardV3::main(){
 	srv_stop_charging = nh->create_service<std_srvs::srv::Empty>("stop_charging", std::bind(&RelayBoardV3::service_stop_charging, this, std::placeholders::_1, std::placeholders::_2));
 	srv_shutdown_platform = nh->create_service<std_srvs::srv::Empty>("shutdown_platform", std::bind(&RelayBoardV3::service_shutdown_platform, this, std::placeholders::_1, std::placeholders::_2));
 	srv_set_safety_field = nh->create_service<neo_srvs2::srv::SetSafetyField>("set_safety_field", std::bind(&RelayBoardV3::service_set_safety_field, this, std::placeholders::_1, std::placeholders::_2));
+	srv_set_safety_mode  = nh->create_service<neo_srvs2::srv::RelayBoardSetSafetyMode>("set_safety_mode", std::bind(&RelayBoardV3::service_set_safety_mode, this, std::placeholders::_1, std::placeholders::_2));
 	srv_set_leds = nh->create_service<neo_srvs2::srv::RelayBoardSetLED>("set_leds", std::bind(&RelayBoardV3::service_set_leds, this, std::placeholders::_1, std::placeholders::_2));
 
 	if(board_init_interval_ms > 0){
@@ -645,6 +648,36 @@ bool RelayBoardV3::service_set_safety_field(std::shared_ptr<neo_srvs2::srv::SetS
 	}
 }
 
+bool RelayBoardV3::service_set_safety_mode(
+	std::shared_ptr<neo_srvs2::srv::RelayBoardSetSafetyMode::Request> req,
+	std::shared_ptr<neo_srvs2::srv::RelayBoardSetSafetyMode::Response> res)
+{
+	pilot::safety_mode_e mode;
+
+	switch(req->set_safety_mode.mode) {
+		case neo_msgs2::msg::SafetyMode::SM_NONE:
+			mode = pilot::safety_mode_e::NONE;
+		case neo_msgs2::msg::SafetyMode::SM_APPROACHING:
+			mode = pilot::safety_mode_e::APPROACHING;
+		case neo_msgs2::msg::SafetyMode::SM_DEPARTING:
+			mode = pilot::safety_mode_e::DEPARTING;
+		case neo_msgs2::msg::SafetyMode::SM_WORKING:
+			mode = pilot::safety_mode_e::WORKING;
+		case neo_msgs2::msg::SafetyMode::SM_HANDLING:
+			mode = pilot::safety_mode_e::HANDLING;
+	}
+
+	try{
+		safety_interface->set_safety_mode(mode, req->station);
+		res->success = true;
+		return true;
+	}catch(const std::exception &err){
+		log(WARN) << "Service call failed with: " << err.what();
+		res->success = false;
+		return false;
+	}
+}
+
 bool RelayBoardV3::service_shutdown_platform(std::shared_ptr<std_srvs::srv::Empty::Request> req, std::shared_ptr<std_srvs::srv::Empty::Response> res){
 	try{
 		platform_interface->shutdown();

From defdaa9f9572b490504c0bdddd65478c1b55b6b8 Mon Sep 17 00:00:00 2001
From: Pradheep <padhupradheep@gmail.com>
Date: Fri, 20 Sep 2024 10:12:32 +0200
Subject: [PATCH 2/2] adding the missing breaks

---
 src/RelayBoardV3.cpp | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/src/RelayBoardV3.cpp b/src/RelayBoardV3.cpp
index f4319f8..ee895cd 100644
--- a/src/RelayBoardV3.cpp
+++ b/src/RelayBoardV3.cpp
@@ -652,19 +652,24 @@ bool RelayBoardV3::service_set_safety_mode(
 	std::shared_ptr<neo_srvs2::srv::RelayBoardSetSafetyMode::Request> req,
 	std::shared_ptr<neo_srvs2::srv::RelayBoardSetSafetyMode::Response> res)
 {
-	pilot::safety_mode_e mode;
+	pilot::safety_mode_e mode = pilot::safety_mode_e::NONE;
 
 	switch(req->set_safety_mode.mode) {
 		case neo_msgs2::msg::SafetyMode::SM_NONE:
 			mode = pilot::safety_mode_e::NONE;
+			break;
 		case neo_msgs2::msg::SafetyMode::SM_APPROACHING:
 			mode = pilot::safety_mode_e::APPROACHING;
+			break;
 		case neo_msgs2::msg::SafetyMode::SM_DEPARTING:
 			mode = pilot::safety_mode_e::DEPARTING;
+			break;
 		case neo_msgs2::msg::SafetyMode::SM_WORKING:
 			mode = pilot::safety_mode_e::WORKING;
+			break;
 		case neo_msgs2::msg::SafetyMode::SM_HANDLING:
 			mode = pilot::safety_mode_e::HANDLING;
+			break;
 	}
 
 	try{