Merge pull request #15 from neobotix/safety_modes

neobotix · Oct 8, 2024 · 144dc93 · 144dc93
2 parents d20a1e8 + defdaa9
commit 144dc93
Show file tree

Hide file tree

Showing 2 changed files with 41 additions and 0 deletions.
diff --git 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
@@ -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){
@@ -650,6 +653,41 @@ 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 = 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{
+		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();