From fd016f05b6ab91e46aa3a1aaecfab86addad816f Mon Sep 17 00:00:00 2001 From: Vincent Winter Date: Thu, 6 Nov 2025 14:36:19 +0100 Subject: [PATCH] fix: Rewrite Lifecycle node --- src/g2_2025_imu_reader_pkg/CMakeLists.txt | 36 +++-- .../{main.cpp => Main.cpp} | 4 +- ...re_interface.cpp => HardwareInterface.cpp} | 137 +++++++++--------- ...re_interface.hpp => HardwareInterface.hpp} | 79 +++++----- ...cycle_manager.cpp => LifecycleManager.cpp} | 54 +++---- ...cycle_manager.hpp => LifecycleManager.hpp} | 30 ++-- .../nodes/MQTTParameters.hpp | 13 ++ .../test/LifecycleManager.test.cpp | 5 +- 8 files changed, 194 insertions(+), 164 deletions(-) rename src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/{main.cpp => Main.cpp} (84%) rename src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/{hardware_interface.cpp => HardwareInterface.cpp} (50%) rename src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/{hardware_interface.hpp => HardwareInterface.hpp} (51%) rename src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/{lifecycle_manager.cpp => LifecycleManager.cpp} (71%) rename src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/{lifecycle_manager.hpp => LifecycleManager.hpp} (70%) create mode 100644 src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/MQTTParameters.hpp diff --git a/src/g2_2025_imu_reader_pkg/CMakeLists.txt b/src/g2_2025_imu_reader_pkg/CMakeLists.txt index b5d9be7..4c2f0f9 100644 --- a/src/g2_2025_imu_reader_pkg/CMakeLists.txt +++ b/src/g2_2025_imu_reader_pkg/CMakeLists.txt @@ -28,8 +28,18 @@ set(PAHO_MQTT_CPP_BUILD_SHARED OFF CACHE BOOL "Build shared library" FORCE) set(PAHO_WITH_SSL OFF CACHE BOOL "Build with SSL support" FORCE) set(PAHO_BUILD_EXAMPLES OFF CACHE BOOL "Build example programs" FORCE) +# Suppress warnings from FetchContent dependencies +set(CMAKE_CXX_FLAGS_BACKUP "${CMAKE_CXX_FLAGS}") +set(CMAKE_C_FLAGS_BACKUP "${CMAKE_C_FLAGS}") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -w") + FetchContent_MakeAvailable(paho_mqtt_cpp) +# Restore original flags +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS_BACKUP}") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS_BACKUP}") + FetchContent_Declare( nlohmann_json GIT_REPOSITORY https://github.com/nlohmann/json.git @@ -61,20 +71,22 @@ ament_target_dependencies(g2_2025_imu_database_writer_node rclcpp sensor_msgs) target_link_libraries(g2_2025_imu_database_writer_node pqxx pq tomlplusplus::tomlplusplus) add_executable(g2_2025_lifecycle_node - src/g2_2025_lifecycle_node/main.cpp - src/g2_2025_lifecycle_node/nodes/hardware_interface.cpp - src/g2_2025_lifecycle_node/nodes/lifecycle_manager.cpp - src/config/serialib.cpp + src/g2_2025_lifecycle_node/Main.cpp + src/g2_2025_lifecycle_node/nodes/HardwareInterface.cpp + src/g2_2025_lifecycle_node/nodes/LifecycleManager.cpp + lib/serialib.cpp ) target_include_directories(g2_2025_lifecycle_node PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src + ${CMAKE_CURRENT_SOURCE_DIR}/lib ${CMAKE_CURRENT_SOURCE_DIR}/src/g2_2025_lifecycle_node ) ament_target_dependencies(g2_2025_lifecycle_node rclcpp rclcpp_lifecycle std_msgs sensor_msgs) target_link_libraries(g2_2025_lifecycle_node - paho-mqttpp3-static + paho-mqttpp3 + paho-mqtt3as nlohmann_json::nlohmann_json ) @@ -85,6 +97,10 @@ install( DESTINATION lib/${PROJECT_NAME} ) +install( + DIRECTORY launch + DESTINATION share/${PROJECT_NAME}/ +) set_target_properties(g2_2025_lifecycle_node PROPERTIES INSTALL_RPATH "/usr/local/lib") if(BUILD_TESTING) @@ -142,13 +158,14 @@ if(BUILD_TESTING) ament_add_gtest(${PROJECT_NAME}_test_lifecycle_manager test/LifecycleManager.test.cpp - src/g2_2025_lifecycle_node/nodes/lifecycle_manager.cpp - src/g2_2025_lifecycle_node/nodes/hardware_interface.cpp - src/config/serialib.cpp + src/g2_2025_lifecycle_node/nodes/LifecycleManager.cpp + src/g2_2025_lifecycle_node/nodes/HardwareInterface.cpp + lib/serialib.cpp ) target_include_directories(${PROJECT_NAME}_test_lifecycle_manager PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/src/g2_2025_lifecycle_node + ${CMAKE_CURRENT_SOURCE_DIR}/lib ) ament_target_dependencies(${PROJECT_NAME}_test_lifecycle_manager rclcpp @@ -157,7 +174,8 @@ if(BUILD_TESTING) sensor_msgs ) target_link_libraries(${PROJECT_NAME}_test_lifecycle_manager - paho-mqttpp3-static + paho-mqttpp3 + paho-mqtt3as nlohmann_json::nlohmann_json ) set_target_properties(${PROJECT_NAME}_test_lifecycle_manager PROPERTIES INSTALL_RPATH "/usr/local/lib") diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/main.cpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/Main.cpp similarity index 84% rename from src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/main.cpp rename to src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/Main.cpp index 145d62b..89180f6 100644 --- a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/main.cpp +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/Main.cpp @@ -7,9 +7,9 @@ */ #include "rclcpp/rclcpp.hpp" -#include "nodes/hardware_interface.hpp" -#include "nodes/lifecycle_manager.hpp" +#include "nodes/HardwareInterface.hpp" +#include "nodes/LifecycleManager.hpp" int main(int argc, char *argv[]) { rclcpp::init(argc, argv); diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.cpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/HardwareInterface.cpp similarity index 50% rename from src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.cpp rename to src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/HardwareInterface.cpp index fbc0fe8..61739ec 100644 --- a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.cpp +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/HardwareInterface.cpp @@ -1,25 +1,24 @@ -#include "hardware_interface.hpp" -#include +#include "HardwareInterface.hpp" namespace assignments::two::g2_2025_lifecycle_node { -HardwareInterface::HardwareInterface() : Node("hardware_interface") { - RCLCPP_INFO(this->get_logger(), "HardwareInterface node has been created"); - imu_publisher = this->create_publisher("imu_data", 10); - RCLCPP_INFO(this->get_logger(), "IMU Publisher has been created"); -} +HardwareInterface::HardwareInterface(MQTTParameters mqtt_config) +: Node("HardwareInterface") +, mqtt_config_(mqtt_config) +{ + RCLCPP_INFO(this->get_logger(), "created HardwareInterface node"); + imu_publisher = this->create_publisher("imu_data", 10); + RCLCPP_INFO(this->get_logger(), "created IMU Publisher on topic 'imu_data'"); +} void HardwareInterface::publish_imu_data(const sensor_msgs::msg::Imu::SharedPtr msg) { imu_publisher->publish(*msg); } -void HardwareInterface::parse_data(const std::string& data) { - // RCLCPP_INFO(this->get_logger(), "Received data: %s", data.c_str()); - +void HardwareInterface::parse_and_publish_imu_data(const std::string& data) { try { auto json_data = nlohmann::json::parse(data); - auto imu_msg = std::make_shared(); imu_msg->header.stamp = this->now(); @@ -39,43 +38,39 @@ void HardwareInterface::parse_data(const std::string& data) { imu_msg->angular_velocity.z = json_data["gyro"].value("z", 0.0); } //print imu data - RCLCPP_INFO(this->get_logger(), "Publishing IMU Data - Accel: [%.3f, %.3f, %.3f], Gyro: [%.3f, %.3f, %.3f]", + RCLCPP_INFO(this->get_logger(), "Publishing IMU Data - Accel: [%.3f, %.3f, %.3f], Gyro: [%.3f, %.3f, %.3f]", imu_msg->linear_acceleration.x, imu_msg->linear_acceleration.y, imu_msg->linear_acceleration.z, imu_msg->angular_velocity.x, imu_msg->angular_velocity.y, imu_msg->angular_velocity.z); - publish_imu_data(imu_msg); + publish_imu_data(imu_msg); } catch (const nlohmann::json::exception& e) { RCLCPP_ERROR(this->get_logger(), "JSON parsing error: %s", e.what()); } } - -void HardwareInterface::start_read() { +void HardwareInterface::uart_start_read() { if (reading_.load()) { return; } reading_.store(true); - read_thread_ = std::thread([this]() { char buffer[116]; RCLCPP_INFO(this->get_logger(), "reader thread started"); while (reading_.load()) { - int ret = serial.readString(buffer, '\n', sizeof(buffer), 1000); + int ret = serial_.readString(buffer, '\n', sizeof(buffer), 1000); if (ret > 0) { RCLCPP_INFO(this->get_logger(), "Received buffer: %s", buffer); - parse_data(buffer); - } else { - // timeout of geen data. + parse_and_publish_imu_data(buffer); } } RCLCPP_INFO(this->get_logger(), "reader thread exiting"); }); } -void HardwareInterface::stop_read() { +void HardwareInterface::uart_stop_read() { if (!reading_.load()) return; reading_.store(false); if (read_thread_.joinable()) { @@ -83,89 +78,98 @@ void HardwareInterface::stop_read() { } } -void HardwareInterface::write(const std::string& data) { - RCLCPP_INFO(this->get_logger(), "Writing to hardware..."); - serial.writeString(data.c_str()); -} - -bool HardwareInterface::open_device(const std::string& device_path, int baud_rate) { - - char errorOpening = serial.openDevice(device_path.c_str(), baud_rate); +bool HardwareInterface::uart_open_device(const std::string& device_path, int baud_rate) { + char errorOpening = serial_.openDevice(device_path.c_str(), baud_rate); if (errorOpening != 1) { - RCLCPP_ERROR(this->get_logger(), "Error opening serial port: %d", errorOpening); + RCLCPP_ERROR(this->get_logger(), "Error opening serial_ port: %d", errorOpening); return false; } - RCLCPP_INFO(this->get_logger(), "Serial port opened successfully."); + RCLCPP_INFO(this->get_logger(), "serial_ port opened successfully."); return true; } -bool HardwareInterface::is_device_open() { - return serial.isDeviceOpen(); +bool HardwareInterface::uart_device_is_open() { + return serial_.isDeviceOpen(); } - -void HardwareInterface::close_device() { - - serial.closeDevice(); - +void HardwareInterface::uart_close_device() { + serial_.closeDevice(); } -class callback : public virtual mqtt::callback { -public: - void message_arrived(mqtt::const_message_ptr msg) override { - RCLCPP_INFO(rclcpp::get_logger("hardware_interface"), - "Message received: %s", msg->get_payload_str().c_str()); - parent_.parse_data(msg->get_payload_str()); - - } -private: -HardwareInterface parent_; -}; - - void HardwareInterface::mqtt_configure() { - if (!mqtt_client) { - mqtt_client = std::make_shared(SERVER_ADDRESS, CLIENT_ID); + try { + if (!mqtt_client) { + RCLCPP_INFO(this->get_logger(), "Creating MQTT client with server: %s, client_id: %s", + mqtt_config_.server_address.c_str(), mqtt_config_.client_id.c_str()); + + // Validate configuration + if (mqtt_config_.server_address.empty()) { + RCLCPP_ERROR(this->get_logger(), "MQTT server address is empty!"); + throw std::runtime_error("MQTT server address is empty"); + } + if (mqtt_config_.client_id.empty()) { + RCLCPP_ERROR(this->get_logger(), "MQTT client_id is empty!"); + throw std::runtime_error("MQTT client_id is empty"); + } + + mqtt_client = std::make_shared( + mqtt_config_.server_address, + mqtt_config_.client_id + ); + RCLCPP_INFO(this->get_logger(), "MQTT client created successfully"); + } + + if (!mqtt_cb) { + RCLCPP_INFO(this->get_logger(), "Creating MQTT callback"); + mqtt_cb = std::make_shared(*this); + RCLCPP_INFO(this->get_logger(), "MQTT callback created successfully"); + } + + // Set callback BEFORE connecting + RCLCPP_INFO(this->get_logger(), "Setting MQTT callback on client"); + mqtt_client->set_callback(*mqtt_cb); + RCLCPP_INFO(this->get_logger(), "MQTT callback set"); + + mqtt_connect(); + } catch (const std::exception& e) { + RCLCPP_ERROR(this->get_logger(), "Exception in mqtt_configure: %s", e.what()); + throw; } - if (!mqtt_cb) { - mqtt_cb = std::make_shared(); - } - mqtt_connect(); } -void HardwareInterface::mqtt_reader() { +void HardwareInterface::mqtt_start_listen() { RCLCPP_INFO(this->get_logger(), "MQTT listener started"); - mqtt_client->set_callback(*mqtt_cb); + // Callback already set in mqtt_configure() } void HardwareInterface::mqtt_connect() { - mqtt::connect_options connOpts; connOpts.set_keep_alive_interval(20); connOpts.set_clean_session(true); try { + RCLCPP_INFO(this->get_logger(), "Connecting to MQTT broker..."); mqtt_client->connect(connOpts)->wait(); RCLCPP_INFO(this->get_logger(), "Connected to broker"); - mqtt_client->subscribe(TOPIC, 1)->wait(); - RCLCPP_INFO(this->get_logger(), "Subscribed to topic: %s", TOPIC.c_str()); - // Note: do not disconnect here; keep client alive until close_mqtt_conn() + RCLCPP_INFO(this->get_logger(), "Subscribing to topic: %s", mqtt_config_.topic.c_str()); + mqtt_client->subscribe(mqtt_config_.topic, 1)->wait(); + RCLCPP_INFO(this->get_logger(), "Subscribed to topic: %s", mqtt_config_.topic.c_str()); } catch (const mqtt::exception& exc) { - RCLCPP_ERROR(this->get_logger(), "Error: %s", exc.what()); + RCLCPP_ERROR(this->get_logger(), "MQTT Error: %s", exc.what()); + throw; } } -void HardwareInterface::close_mqtt_conn() { +void HardwareInterface::mqtt_close_connection() { try { if (mqtt_client) { if (mqtt_client->is_connected()) { mqtt_client->disconnect()->wait(); RCLCPP_INFO(this->get_logger(), "Disconnected MQTT client"); } - // reset to allow destruction mqtt_client.reset(); } if (mqtt_cb) { @@ -176,5 +180,4 @@ void HardwareInterface::close_mqtt_conn() { } } - } // namespace assignments::two::g2_2025_lifecycle_node diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.hpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/HardwareInterface.hpp similarity index 51% rename from src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.hpp rename to src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/HardwareInterface.hpp index 7ff8a39..5969e1b 100644 --- a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.hpp +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/HardwareInterface.hpp @@ -1,4 +1,4 @@ -/* nodes/hardware_interface.hpp +/* nodes/HardwareInterface.hpp * Hardware interface implementation for the IMU reader system. * * Manages the serial communication with the IMU hardware, including initialization, @@ -11,55 +11,47 @@ * [20-10-2025] M.khalaf: Refactored code for readability. * [30-10-2025] M.khalaf: Improved error handling. * [31-10-2025] M.khalaf: Fixed serial read and mqtt configurations. + * [06-11-2025] Wessel T, Vincent W: Clean up code, remove uncessary imports */ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "config/serialib.h" -#include - - +#include #include #include -#include "sensor_msgs/msg/imu.hpp" +#include "serialib.h" + #include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "sensor_msgs/msg/imu.hpp" + +#include "MQTTParameters.hpp" namespace assignments::two::g2_2025_lifecycle_node { class HardwareInterface : public rclcpp::Node { public: - HardwareInterface(); - void start_read(); - void stop_read(); - void write(const std::string& data); - bool open_device(const std::string& device_path, int baud_rate); - bool is_device_open(); - void close_device(); - void mqtt_connect(); - void close_mqtt_conn(); + HardwareInterface(MQTTParameters mqtt_config); + + bool uart_open_device(const std::string& device_path, int baud_rate); + bool uart_device_is_open(); + void uart_close_device(); + + void uart_start_read(); + void uart_stop_read(); + void mqtt_configure(); - void mqtt_reader(); - void parse_data(const std::string& data); + void mqtt_connect(); + + void mqtt_start_listen(); + void mqtt_close_connection(); + void publish_imu_data(const sensor_msgs::msg::Imu::SharedPtr msg); + void parse_and_publish_imu_data(const std::string& data); + private: - serialib serial; - // MQTT defaults. Use static inline so they can be initialized in-class. - static inline const std::string SERVER_ADDRESS = "tcp://localhost:1883"; - static inline const std::string CLIENT_ID = "cpp_mqtt_client"; - static inline const std::string TOPIC = "esp32/imu"; + serialib serial_; + MQTTParameters mqtt_config_ {}; std::shared_ptr mqtt_client; std::shared_ptr mqtt_cb; @@ -68,6 +60,23 @@ private: std::atomic_bool reading_{false}; // Publisher to emit parsed IMU messages (from serial or MQTT) rclcpp::Publisher::SharedPtr imu_publisher; - }; + +class callback : public virtual mqtt::callback { +public: + explicit callback(HardwareInterface& parent) : parent_(parent) {} + + void message_arrived(mqtt::const_message_ptr msg) override { + try { + RCLCPP_INFO(parent_.get_logger(), + "Message received: %s", msg->get_payload_str().c_str()); + parent_.parse_and_publish_imu_data(msg->get_payload_str()); + } catch (const std::exception& e) { + RCLCPP_ERROR(parent_.get_logger(), "Error in message_arrived: %s", e.what()); + } + } +private: + HardwareInterface& parent_; +}; + } // namespace assignments::two::g2_2025_lifecycle_node diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.cpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/LifecycleManager.cpp similarity index 71% rename from src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.cpp rename to src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/LifecycleManager.cpp index 98703c2..df90871 100644 --- a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.cpp +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/LifecycleManager.cpp @@ -1,36 +1,28 @@ -#include "lifecycle_manager.hpp" -#include "hardware_interface.hpp" - +#include "LifecycleManager.hpp" +#include "HardwareInterface.hpp" namespace assignments::two::g2_2025_lifecycle_node { -LifecycleManager::LifecycleManager() : rclcpp_lifecycle::LifecycleNode("lifecycle_manager") { +LifecycleManager::LifecycleManager() : rclcpp_lifecycle::LifecycleNode("LifecycleManager") { + mqtt_config_.server_address = + this->declare_parameter("mqtt_server_address", "tcp://localhost:1883"); + mqtt_config_.client_id = + this->declare_parameter("mqtt_client_id", "cpp_mqtt-client"); + + mqtt_config_.topic = + this->declare_parameter("mqtt_subscribe_topic", "esp32/imu"); RCLCPP_INFO(this->get_logger(), "LifecycleManager node is ready"); device_path_ = this->declare_parameter("device_path", "/dev/ttyUSB0"); baudrate_ = this->declare_parameter("baudrate", 115200); - communication_type_ = this->declare_parameter("comm_t", "serial"); // placeholder default serial or param mqtt - - // hardware interface instance - hw_interface = std::make_shared(); + communication_type_ = this->declare_parameter("comm_t", "serial"); + hw_interface = std::make_shared(mqtt_config_); } -/* - -*in terminal commands to manage lifecycle: -*ros2 lifecycle list /lifecycle_manager ## for status checking -*ros2 lifecycle set /lifecycle_manager configure -*ros2 lifecycle set /lifecycle_manager activate -*ros2 lifecycle set /lifecycle_manager deactivate -*ros2 lifecycle set /lifecycle_manager shutdown -*/ - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleManager::on_configure(const rclcpp_lifecycle::State&) { - - RCLCPP_INFO(this->get_logger(), "configuring lifecycle..."); if (communication_type_ == "mqtt") { @@ -39,33 +31,28 @@ LifecycleManager::on_configure(const rclcpp_lifecycle::State&) { } else { RCLCPP_INFO(this->get_logger(), "Using serial communication."); - if (!hw_interface->open_device(device_path_, baudrate_)) { + if (!hw_interface->uart_open_device(device_path_, baudrate_)) { RCLCPP_ERROR(this->get_logger(), "Failed to open hardware device during configuration."); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; } } - //for testing parsing function. - // std::shared_ptr s; - // s = std::make_shared("{\"accel\":{\"x\": 0.037,\"y\": -1.164,\"z\": 9.775},\"gyro\":{\"x\": -0.024,\"y\": -0.014,\"z\": -0.001},\"Temp\": 41.01}"); - // hw_interface->parse_data(*s); RCLCPP_INFO(this->get_logger(), "Lifecycle configured successfully."); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleManager::on_activate(const rclcpp_lifecycle::State&) { - RCLCPP_INFO(this->get_logger(), "activating lifecycle..."); if (communication_type_ == "mqtt") { RCLCPP_INFO(this->get_logger(), "Reading on MQTT..."); - hw_interface->mqtt_reader(); + hw_interface->mqtt_start_listen(); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } else { RCLCPP_INFO(this->get_logger(), "Reading on Serial..."); - hw_interface->start_read(); + hw_interface->uart_start_read(); } return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; @@ -73,23 +60,22 @@ LifecycleManager::on_activate(const rclcpp_lifecycle::State&) { rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleManager::on_deactivate(const rclcpp_lifecycle::State&) { - RCLCPP_INFO(this->get_logger(), "deactivating lifecycle..."); if (communication_type_ == "mqtt") { - hw_interface->close_mqtt_conn(); + hw_interface->mqtt_close_connection(); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } else { - if (!hw_interface->is_device_open()) { + if (!hw_interface->uart_device_is_open()) { RCLCPP_ERROR(this->get_logger(), "Hardware device is not open during activation."); - hw_interface->close_device(); + hw_interface->uart_close_device(); } RCLCPP_INFO(this->get_logger(), "Hardware device is open,closing device..."); - hw_interface->stop_read(); - hw_interface->close_device(); + hw_interface->uart_stop_read(); + hw_interface->uart_close_device(); RCLCPP_INFO(this->get_logger(), "Lifecycle deactivated successfully."); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.hpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/LifecycleManager.hpp similarity index 70% rename from src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.hpp rename to src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/LifecycleManager.hpp index 72de89f..1fa665a 100644 --- a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.hpp +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/LifecycleManager.hpp @@ -9,39 +9,39 @@ */ #pragma once -#include -#include -#include -#include -#include - #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "std_msgs/msg/string.hpp" -#include "hardware_interface.hpp" +#include "HardwareInterface.hpp" +#include "MQTTParameters.hpp" namespace assignments::two::g2_2025_lifecycle_node { + class LifecycleManager : public rclcpp_lifecycle::LifecycleNode { public: LifecycleManager(); - // Hardware interface to interact with the IMU device. - // Made public for testing purposes + // Hardware interface to interact with the IMU device. Made public for testing purposes std::shared_ptr hw_interface; private: - rclcpp::Publisher::SharedPtr imu_publisher_; - std::string device_path_; int baudrate_; std::string communication_type_; + MQTTParameters mqtt_config_ {}; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(const rclcpp_lifecycle::State&); - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(const rclcpp_lifecycle::State&); - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State&); - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State&); + rclcpp::Publisher::SharedPtr imu_publisher_; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_configure(const rclcpp_lifecycle::State&); + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_activate(const rclcpp_lifecycle::State&); + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_deactivate(const rclcpp_lifecycle::State&); + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_shutdown(const rclcpp_lifecycle::State&); }; + } // namespace assignments::two::g2_2025_lifecycle_node diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/MQTTParameters.hpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/MQTTParameters.hpp new file mode 100644 index 0000000..62f7402 --- /dev/null +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/MQTTParameters.hpp @@ -0,0 +1,13 @@ +#pragma once + +#include + +namespace assignments::two { + +struct MQTTParameters { + std::string server_address; + std::string client_id; + std::string topic; +}; + +} // namespace assignments::two diff --git a/src/g2_2025_imu_reader_pkg/test/LifecycleManager.test.cpp b/src/g2_2025_imu_reader_pkg/test/LifecycleManager.test.cpp index 0867e07..cb1cf84 100644 --- a/src/g2_2025_imu_reader_pkg/test/LifecycleManager.test.cpp +++ b/src/g2_2025_imu_reader_pkg/test/LifecycleManager.test.cpp @@ -2,7 +2,8 @@ #include #include #include -#include "g2_2025_lifecycle_node/nodes/lifecycle_manager.hpp" + +#include "g2_2025_lifecycle_node/nodes/LifecycleManager.hpp" using namespace assignments::two::g2_2025_lifecycle_node; @@ -225,7 +226,7 @@ TEST_F(LifecycleManagerTest, HardwareInterfaceIntegrationTest) { EXPECT_NE(node->hw_interface, nullptr); // Verify we can make method calls on the hardware interface - EXPECT_FALSE(node->hw_interface->is_device_open()); + EXPECT_FALSE(node->hw_interface->uart_device_is_open()); } int main(int argc, char** argv) {