diff --git a/config/default/generic/RelayBoardV3Node.json b/config/default/generic/RelayBoardV3Node.json index 1a3ddb4..3b6a100 100644 --- 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 index cd602d3..df2ae8e 100644 --- a/generated/include/neo_relayboard_v3/RelayBoardV3AsyncClient.hxx +++ b/generated/include/neo_relayboard_v3/RelayBoardV3AsyncClient.hxx @@ -18,6 +18,7 @@ #include #include #include +#include #include #include #include diff --git a/generated/include/neo_relayboard_v3/RelayBoardV3Base.hxx b/generated/include/neo_relayboard_v3/RelayBoardV3Base.hxx index 469ebf2..043247f 100644 --- a/generated/include/neo_relayboard_v3/RelayBoardV3Base.hxx +++ b/generated/include/neo_relayboard_v3/RelayBoardV3Base.hxx @@ -18,6 +18,7 @@ #include #include #include +#include #include #include #include @@ -74,6 +75,7 @@ public: protected: using Super::handle; + virtual void handle(std::shared_ptr _value) {} virtual void handle(std::shared_ptr _value) {} virtual void handle(std::shared_ptr _value) {} virtual void handle(std::shared_ptr _value) {} diff --git a/generated/include/neo_relayboard_v3/RelayBoardV3Client.hxx b/generated/include/neo_relayboard_v3/RelayBoardV3Client.hxx index aae9ae9..43e2208 100644 --- a/generated/include/neo_relayboard_v3/RelayBoardV3Client.hxx +++ b/generated/include/neo_relayboard_v3/RelayBoardV3Client.hxx @@ -18,6 +18,7 @@ #include #include #include +#include #include #include #include diff --git a/generated/src/RelayBoardV3AsyncClient.cpp b/generated/src/RelayBoardV3AsyncClient.cpp index bea1e9f..cba4046 100644 --- a/generated/src/RelayBoardV3AsyncClient.cpp +++ b/generated/src/RelayBoardV3AsyncClient.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include #include #include diff --git a/generated/src/RelayBoardV3Base.cpp b/generated/src/RelayBoardV3Base.cpp index 2752821..56a1640 100644 --- a/generated/src/RelayBoardV3Base.cpp +++ b/generated/src/RelayBoardV3Base.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include #include @@ -398,6 +399,9 @@ void RelayBoardV3Base::vnx_handle_switch(std::shared_ptr _valu case 0x735822e6960c247ull: handle(std::static_pointer_cast(_value)); return; + case 0x2a13f6d072f9b852ull: + handle(std::static_pointer_cast(_value)); + return; default: _type_code = _type_code->super; } diff --git a/generated/src/RelayBoardV3Client.cpp b/generated/src/RelayBoardV3Client.cpp index 13fd94f..52db0ec 100644 --- a/generated/src/RelayBoardV3Client.cpp +++ b/generated/src/RelayBoardV3Client.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include #include #include diff --git a/include/neo_relayboard_v3/RelayBoardV3.h b/include/neo_relayboard_v3/RelayBoardV3.h index 4382b55..0b98fc3 100644 --- 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 value) override; void handle(std::shared_ptr value) override; void handle(std::shared_ptr value) override; void handle(std::shared_ptr value) override; diff --git a/modules/RelayBoardV3.vni b/modules/RelayBoardV3.vni index 0e09c1d..8dff7df 100644 --- 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 index 31a5100..87f9ab9 100644 --- a/src/RelayBoardV3.cpp +++ b/src/RelayBoardV3.cpp @@ -72,6 +72,28 @@ void RelayBoardV3::main(){ } +void RelayBoardV3::handle(std::shared_ptr 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 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_ptrsuccess = 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_ptrsuccess = 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_ptrstart_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_ptrstop_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_ptrsuccess = 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 index fe3da4f..5caa7f4 100644 --- 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 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(); } {