diff --git a/doc/installation/installation.md b/doc/installation/installation.md index 7d63cf8..dd128dc 100644 --- a/doc/installation/installation.md +++ b/doc/installation/installation.md @@ -41,3 +41,54 @@ You can configure specific database settings in the `docker-compose.yaml` in the ros2 launch g2_2025_grade_calculator_pkg grade_calculator.launch.xml ``` To change parameters when using the launch file it will need to be edited in the `src/g2_2025_grade_calculator_pkg/launch` folder. All parameters are already added to this document and thus only the values will need to be changed + + + +### installation and setup for mqtt + +```bash +sudo apt install mosquitto + +sudo apt-get install libpaho-mqtt-dev + + +git clone https://github.com/eclipse/paho.mqtt.cpp +cd paho.mqtt.cpp +git co v1.5.4 +git submodule init +git submodule update + +cmake -Bbuild -H. -DPAHO_WITH_MQTT_C=ON -DPAHO_BUILD_EXAMPLES=ON +sudo cmake --build build/ --target install +``` + +## for launching lifecycle mqtt node + +first: +```bash +ros2 run g2_2025_imu_reader_pkg g2_2025_lifecycle_node --ros-args -p comm_t:='mqtt' +``` + +in other terminal: + +```bash +mosquitto -p 1884 +``` + +and in other terminal to inialize the subsecriber: +```bash + +ros2 lifecycle set /lifecycle_manager configure +ros2 lifecycle set /lifecycle_manager activate + +``` + +an finally publish a mesg to the sub in other terminal: + +```bash +mosquitto_pub -h localhost -p 1884 -t "esp32/imu" -m "nirvana" +``` +close conn via: +```bash +ros2 lifecycle set /lifecycle_manager deactivate +``` diff --git a/src/g2_2025_imu_reader_pkg/CMakeLists.txt b/src/g2_2025_imu_reader_pkg/CMakeLists.txt index 8771f5e..3b66cb9 100644 --- a/src/g2_2025_imu_reader_pkg/CMakeLists.txt +++ b/src/g2_2025_imu_reader_pkg/CMakeLists.txt @@ -53,6 +53,13 @@ target_include_directories(g2_2025_lifecycle_node PRIVATE ) ament_target_dependencies(g2_2025_lifecycle_node rclcpp rclcpp_lifecycle std_msgs sensor_msgs) +target_link_libraries(g2_2025_lifecycle_node + paho-mqttpp3 + paho-mqtt3a +) + + + install( TARGETS imu_database_writer @@ -60,6 +67,8 @@ install( DESTINATION lib/${PROJECT_NAME} ) +set_target_properties(g2_2025_lifecycle_node PROPERTIES INSTALL_RPATH "/usr/local/lib") + if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) 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/hardware_interface.cpp index 3893759..8929a79 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/hardware_interface.cpp @@ -45,4 +45,61 @@ void HardwareInterface::close_device() { } -} // namespace assignments::two::g2_2025_lifecycle_node +class callback : public virtual mqtt::callback { +public: + void message_arrived(mqtt::const_message_ptr msg) override { + // Use a named logger since this class isn't a rclcpp::Node + RCLCPP_INFO(rclcpp::get_logger("hardware_interface"), + "Message received: %s", msg->get_payload_str().c_str()); + } +}; + + +void HardwareInterface::mqtt_listener() { + // Create persistent client and callback so they outlive this function. + if (!mqtt_client) { + mqtt_client = std::make_shared(SERVER_ADDRESS, CLIENT_ID); + } + if (!mqtt_cb) { + mqtt_cb = std::make_shared(); + } + + // attach callback (expects a reference) + mqtt_client->set_callback(*mqtt_cb); + + mqtt::connect_options connOpts; + connOpts.set_keep_alive_interval(20); + connOpts.set_clean_session(true); + + try { + 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() + } catch (const mqtt::exception& exc) { + RCLCPP_ERROR(this->get_logger(), "Error: %s", exc.what()); + } +} + +void HardwareInterface::close_mqtt_conn() { + 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) { + mqtt_cb.reset(); + } + } catch (const mqtt::exception& exc) { + RCLCPP_ERROR(this->get_logger(), "Error while disconnecting MQTT: %s", exc.what()); + } +} + + +} // 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/hardware_interface.hpp index 4dd702d..7db910f 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/hardware_interface.hpp @@ -19,6 +19,11 @@ #include #include #include "config/serialib.h" +#include + + +#include +#include #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -30,12 +35,22 @@ class HardwareInterface : public rclcpp::Node { public: HardwareInterface(); void read(); - void write(const std::string & data); - bool open_device(const std::string & device_path, int baud_rate); + 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_listener(); + void close_mqtt_conn(); private: serialib serial; + // MQTT defaults. Use static inline so they can be initialized in-class. + static inline const std::string SERVER_ADDRESS = "tcp://localhost:1884"; + static inline const std::string CLIENT_ID = "cpp_mqtt_client"; + static inline const std::string TOPIC = "esp32/imu"; + + std::shared_ptr mqtt_client; + std::shared_ptr mqtt_cb; + }; } // 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/lifecycle_manager.cpp index b618c35..8e556be 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/lifecycle_manager.cpp @@ -13,7 +13,7 @@ LifecycleManager::LifecycleManager() : rclcpp_lifecycle::LifecycleNode("lifecycl device_path_ = this->declare_parameter("device_path", "/dev/ttyUSB0"); baudrate_ = this->declare_parameter("baudrate", 115200); - communication_type_ = this->declare_parameter("communication_type", "serial"); // placeholder default serial or param mqtt + communication_type_ = this->declare_parameter("comm_t", "serial"); // placeholder default serial or param mqtt // hardware interface instance hw_interface = std::make_shared(); @@ -39,11 +39,20 @@ void LifecycleManager::publish_imu_data(const std::string& data) { // placeholde rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleManager::on_configure(const rclcpp_lifecycle::State&) { + RCLCPP_INFO(this->get_logger(), "configuring lifecycle..."); - if (!hw_interface->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; + if (communication_type_ == "mqtt") { + // RCLCPP_INFO(this->get_logger(), "Setting up MQTT communication..."); + // hw_interface->mqtt_listener(); + + } else { + RCLCPP_INFO(this->get_logger(), "Using serial communication."); + + if (!hw_interface->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; + } } return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; @@ -53,9 +62,23 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleManager::on_activate(const rclcpp_lifecycle::State&) { RCLCPP_INFO(this->get_logger(), "activating lifecycle..."); - if (!hw_interface->is_device_open()) { - RCLCPP_ERROR(this->get_logger(), "Hardware device is not open during activation."); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + + if (communication_type_ == "mqtt") { + RCLCPP_INFO(this->get_logger(), "Setting up MQTT communication..."); + hw_interface->mqtt_listener(); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + + } else { + RCLCPP_INFO(this->get_logger(), "Using serial communication."); + if (!hw_interface->is_device_open()) { + RCLCPP_ERROR(this->get_logger(), "Hardware device is not open during activation."); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + } + if (!hw_interface->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; + } } RCLCPP_INFO(this->get_logger(), "Hardware device is open, starting data read..."); @@ -71,7 +94,13 @@ LifecycleManager::on_deactivate(const rclcpp_lifecycle::State&) { RCLCPP_INFO(this->get_logger(), "deactivating lifecycle..."); - hw_interface->close_device(); + if (communication_type_ == "mqtt") + { + hw_interface->close_mqtt_conn(); + } else { + hw_interface->close_device(); + } + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; }