Merge branch 'roslog' into 'master'

Roslog Closes #3 See merge request ros/neo_relayboard_v3!4
neobotix · Mar 8, 2024 · 323bb3e · 323bb3e
2 parents 5193710 + 63b78fd
commit 323bb3e
Show file tree

Hide file tree

Showing 11 changed files with 43 additions and 12 deletions.
diff --git a/config/default/generic/RelayBoardV3Node.json b/config/default/generic/RelayBoardV3Node.json
@@ -18,6 +18,7 @@
 		}
 	},
 	"topics_from_board": [
+		"vnx.log_out",
 		"platform.system_state",
 		"platform.power_state"
 	],

diff --git a/generated/include/neo_relayboard_v3/RelayBoardV3AsyncClient.hxx b/generated/include/neo_relayboard_v3/RelayBoardV3AsyncClient.hxx
@@ -18,6 +18,7 @@
 #include <pilot/kinematics/differential/DriveState.hxx>
 #include <pilot/kinematics/mecanum/DriveState.hxx>
 #include <pilot/kinematics/omnidrive/DriveState.hxx>
+#include <vnx/LogMsg.hxx>
 #include <vnx/Module.h>
 #include <vnx/Object.hpp>
 #include <vnx/TopicPtr.hpp>

diff --git a/generated/include/neo_relayboard_v3/RelayBoardV3Base.hxx b/generated/include/neo_relayboard_v3/RelayBoardV3Base.hxx
@@ -18,6 +18,7 @@
 #include <pilot/kinematics/differential/DriveState.hxx>
 #include <pilot/kinematics/mecanum/DriveState.hxx>
 #include <pilot/kinematics/omnidrive/DriveState.hxx>
+#include <vnx/LogMsg.hxx>
 #include <vnx/Module.h>
 #include <vnx/Object.hpp>
 #include <vnx/TopicPtr.hpp>
@@ -74,6 +75,7 @@ public:
 protected:
 	using Super::handle;
 
+	virtual void handle(std::shared_ptr<const ::vnx::LogMsg> _value) {}
 	virtual void handle(std::shared_ptr<const ::pilot::SystemState> _value) {}
 	virtual void handle(std::shared_ptr<const ::pilot::SafetyState> _value) {}
 	virtual void handle(std::shared_ptr<const ::pilot::EmergencyState> _value) {}

diff --git a/generated/include/neo_relayboard_v3/RelayBoardV3Client.hxx b/generated/include/neo_relayboard_v3/RelayBoardV3Client.hxx
@@ -18,6 +18,7 @@
 #include <pilot/kinematics/differential/DriveState.hxx>
 #include <pilot/kinematics/mecanum/DriveState.hxx>
 #include <pilot/kinematics/omnidrive/DriveState.hxx>
+#include <vnx/LogMsg.hxx>
 #include <vnx/Module.h>
 #include <vnx/Object.hpp>
 #include <vnx/TopicPtr.hpp>

diff --git a/generated/src/RelayBoardV3AsyncClient.cpp b/generated/src/RelayBoardV3AsyncClient.cpp
@@ -16,6 +16,7 @@
 #include <pilot/kinematics/differential/DriveState.hxx>
 #include <pilot/kinematics/mecanum/DriveState.hxx>
 #include <pilot/kinematics/omnidrive/DriveState.hxx>
+#include <vnx/LogMsg.hxx>
 #include <vnx/Module.h>
 #include <vnx/ModuleInterface_vnx_get_config.hxx>
 #include <vnx/ModuleInterface_vnx_get_config_return.hxx>

diff --git a/generated/src/RelayBoardV3Base.cpp b/generated/src/RelayBoardV3Base.cpp
@@ -17,6 +17,7 @@
 #include <pilot/kinematics/differential/DriveState.hxx>
 #include <pilot/kinematics/mecanum/DriveState.hxx>
 #include <pilot/kinematics/omnidrive/DriveState.hxx>
+#include <vnx/LogMsg.hxx>
 #include <vnx/Module.h>
 #include <vnx/ModuleInterface_vnx_get_config.hxx>
 #include <vnx/ModuleInterface_vnx_get_config_return.hxx>
@@ -398,6 +399,9 @@ void RelayBoardV3Base::vnx_handle_switch(std::shared_ptr<const vnx::Value> _valu
 			case 0x735822e6960c247ull:
 				handle(std::static_pointer_cast<const ::pilot::kinematics::omnidrive::DriveState>(_value));
 				return;
+			case 0x2a13f6d072f9b852ull:
+				handle(std::static_pointer_cast<const ::vnx::LogMsg>(_value));
+				return;
 			default:
 				_type_code = _type_code->super;
 		}

diff --git a/generated/src/RelayBoardV3Client.cpp b/generated/src/RelayBoardV3Client.cpp
@@ -16,6 +16,7 @@
 #include <pilot/kinematics/differential/DriveState.hxx>
 #include <pilot/kinematics/mecanum/DriveState.hxx>
 #include <pilot/kinematics/omnidrive/DriveState.hxx>
+#include <vnx/LogMsg.hxx>
 #include <vnx/Module.h>
 #include <vnx/ModuleInterface_vnx_get_config.hxx>
 #include <vnx/ModuleInterface_vnx_get_config_return.hxx>

diff --git a/include/neo_relayboard_v3/RelayBoardV3.h b/include/neo_relayboard_v3/RelayBoardV3.h
@@ -34,6 +34,7 @@ class RelayBoardV3 : public RelayBoardV3Base{
 protected:
 	void main() override;
 
+	void handle(std::shared_ptr<const vnx::LogMsg> value) override;
 	void handle(std::shared_ptr<const pilot::SystemState> value) override;
 	void handle(std::shared_ptr<const pilot::SafetyState> value) override;
 	void handle(std::shared_ptr<const pilot::EmergencyState> value) override;

diff --git a/modules/RelayBoardV3.vni b/modules/RelayBoardV3.vni
@@ -17,6 +17,7 @@ module RelayBoardV3{
 
 	pilot.kinematic_type_e kinematic_type;
 
+	void handle(vnx.LogMsg value);
 	void handle(pilot.SystemState value);
 	void handle(pilot.SafetyState value);
 	void handle(pilot.EmergencyState value);

diff --git a/src/RelayBoardV3.cpp b/src/RelayBoardV3.cpp
@@ -72,6 +72,28 @@ void RelayBoardV3::main(){
 }
 
 
+void RelayBoardV3::handle(std::shared_ptr<const vnx::LogMsg> value){
+	const std::string message = value->get_output();
+	auto logger = nh->get_logger();
+
+	switch(value->level){
+	case ERROR:
+		RCLCPP_ERROR(logger, message);
+		break;
+	case WARN:
+		RCLCPP_WARN(logger, message);
+		break;
+	case DEBUG:
+		RCLCPP_DEBUG(logger, message);
+		break;
+	case INFO:
+	default:
+		RCLCPP_INFO(logger, message);
+		break;
+	}
+}
+
+
 void RelayBoardV3::handle(std::shared_ptr<const pilot::SystemState> value){
 	if (value->is_shutdown && !is_shutdown) {
 		RCLCPP_INFO(nh->get_logger(),"-----------SHUTDOWN Signal from RelayBoardV3----------");
@@ -497,8 +519,7 @@ bool RelayBoardV3::service_set_relay(std::shared_ptr<neo_srvs2::srv::RelayBoardS
 		res->success = true;
 		return true;
 	}catch(const std::exception &err){
-		const std::string error = "Service call failed with: " + std::string(err.what());
-		RCLCPP_ERROR_STREAM(nh->get_logger(), error);
+		log(WARN) << "Service call failed with: " << err.what();
 		res->success = false;
 		return false;
 	}
@@ -511,8 +532,7 @@ bool RelayBoardV3::service_set_digital_output(std::shared_ptr<neo_srvs2::srv::IO
 		res->success = true;
 		return true;
 	}catch(const std::exception &err){
-		const std::string error = "Service call failed with: " + std::string(err.what());
-		RCLCPP_ERROR_STREAM(nh->get_logger(), error);
+		log(WARN) << "Service call failed with: " << err.what();
 		res->success = false;
 		return false;
 	}
@@ -524,8 +544,7 @@ bool RelayBoardV3::service_start_charging(std::shared_ptr<std_srvs::srv::Empty::
 		platform_interface->start_charging();
 		return true;
 	}catch(const std::exception &err){
-		const std::string error = "Service call failed with: " + std::string(err.what());
-		RCLCPP_ERROR_STREAM(nh->get_logger(), error);
+		log(WARN) << "Service call failed with: " << err.what();
 		return false;
 	}
 }
@@ -536,8 +555,7 @@ bool RelayBoardV3::service_stop_charging(std::shared_ptr<std_srvs::srv::Empty::R
 		platform_interface->stop_charging();
 		return true;
 	}catch(const std::exception &err){
-		const std::string error = "Service call failed with: " + std::string(err.what());
-		RCLCPP_ERROR_STREAM(nh->get_logger(), error);
+		log(WARN) << "Service call failed with: " << err.what();
 		return false;
 	}
 }
@@ -549,8 +567,7 @@ bool RelayBoardV3::service_set_safety_field(std::shared_ptr<neo_srvs2::srv::SetS
 		res->success = true;
 		return true;
 	}catch(const std::exception &err){
-		const std::string error = "Service call failed with: " + std::string(err.what());
-		RCLCPP_ERROR_STREAM(nh->get_logger(), error);
+		log(WARN) << "Service call failed with: " << err.what();
 		res->success = false;
 		return false;
 	}

diff --git a/src/relayboardv3_node.cpp b/src/relayboardv3_node.cpp
@@ -29,8 +29,9 @@ int main(int argc, char **argv){
 	vnx::read_config_tree(pilot_config);
 
 	{
-		vnx::Handle<vnx::Terminal> module = new vnx::Terminal("Terminal");
-		module.start_detached();
+		// disable vnx log, since the messages will be printed by ROS
+		vnx::ProcessClient process(process_name);
+		process.pause_log();
 	}
 
 	{