complete rework of serial IO + pcb alim interface

src/modelec/CMakeLists.txt

@@ -22,10 +22,10 @@ find_package(modelec_interface REQUIRED)
 find_library(WIRINGPI_LIB wiringPi)
 
 # USB Arduino Listener Node
-add_executable(usb_arduino_listener src/usb_arduino_listener.cpp)
-ament_target_dependencies(usb_arduino_listener rclcpp std_msgs)
-target_link_libraries(usb_arduino_listener Boost::system)
-target_include_directories(usb_arduino_listener PUBLIC
+add_executable(serial_listener src/multiple_serial_listener.cpp)
+ament_target_dependencies(serial_listener rclcpp std_msgs modelec_interface)
+target_link_libraries(serial_listener Boost::system)
+target_include_directories(serial_listener PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>
 )
@@ -38,6 +38,14 @@ target_include_directories(usb_arduino_logic_processor PUBLIC
   $<INSTALL_INTERFACE:include>
 )
 
+# USB Arduino Logic Processor Node
+add_executable(pcb_alim_interface src/pcb_alim_interface.cpp)
+ament_target_dependencies(pcb_alim_interface rclcpp std_msgs modelec_interface)
+target_include_directories(pcb_alim_interface PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
 # PCA9685 Listener Node
 add_executable(pca9685_controller src/pca9685_controller.cpp)
 ament_target_dependencies(pca9685_controller rclcpp std_msgs modelec_interface)
@@ -110,7 +118,7 @@ endif()
 
 # Install targets
 install(TARGETS
-  usb_arduino_listener
+  serial_listener
   usb_arduino_logic_processor
   pca9685_controller
   gamecontroller_listener

src/modelec/include/modelec/multiple_serial_listener.hpp

@@ -0,0 +1,62 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include <boost/asio.hpp>
+#include <std_msgs/msg/string.hpp>
+#include <modelec_interface/srv/add_serial_listener.hpp>
+
+#define MAX_MESSAGE_LEN 1048
+#define READ_REFRESH_RATE 100 //ms
+
+namespace Modelec
+{
+class MultipleSerialListener : public rclcpp::Node
+{
+public:
+    MultipleSerialListener();
+    ~MultipleSerialListener() override;
+
+private:
+
+    int default_max_message_len_ = MAX_MESSAGE_LEN;
+    int read_refresh_rate_ = READ_REFRESH_RATE;
+
+    class SerialListener {
+    private:
+        bool status_;
+        int bauds_;
+        std::string serial_port_;
+        int max_message_len_;
+        boost::asio::streambuf read_buffer_;
+        boost::asio::io_service& io_;
+
+    public:
+        std::string name_;
+        boost::asio::serial_port port_;
+
+        rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
+        rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscriber_;
+
+        SerialListener(const std::string& name, int bauds, const std::string& serial_port, int max_message_len, boost::asio::io_service& io);
+
+        void SetMaxMessageLen(int max_message_len) { max_message_len_ = max_message_len; }
+        bool IsOk() const { return status_; }
+        void asyncRead();
+        void asyncWrite(const std::string& data);
+    };
+
+    std::map<std::string, std::shared_ptr<SerialListener>> serial_listeners;
+
+    boost::asio::io_service io;
+    rclcpp::TimerBase::SharedPtr timer;
+
+    rclcpp::Service<modelec_interface::srv::AddSerialListener>::SharedPtr add_serial_listener_service_;
+    void add_serial_listener(const std::shared_ptr<modelec_interface::srv::AddSerialListener::Request> request, std::shared_ptr<modelec_interface::srv::AddSerialListener::Response> response);
+
+    void readData(const SerialListener& listener);
+
+    OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_;
+    rcl_interfaces::msg::SetParametersResult onParameterChange(const std::vector<rclcpp::Parameter> &parameters);
+    void updateConnection();
+};
+}  // namespace Modelec

src/modelec/include/modelec/pcb_alim_interface.hpp

@@ -0,0 +1,55 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/string.hpp>
+
+namespace Modelec
+{
+class PCBAlimInterface : public rclcpp::Node
+{
+public:
+    PCBAlimInterface();
+
+private:
+
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pcb_publisher_;
+    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr pcb_subscriber_;
+
+    void PCBCallback(const std_msgs::msg::String::SharedPtr msg);
+
+    void SendToPCB(const std::string &data);
+
+    void GetData(const std::string &elem, const std::string& value);
+
+    void SendOrder(const std::string &elem, const std::string& data, const std::string& value);
+
+    // get data
+    void GetEmergencyStopButtonState();
+    void GetEntryVoltage(int entry);
+    void GetEntryCurrent(int entry);
+    void GetEntryState(int entry);
+    void GetEntryIsValide(int entry);
+    void GetPCBTemperature();
+    
+    void GetOutput5VState();
+    void GetOutput5VVoltage();
+    void GetOutput5VCurrent();
+
+    void GetOutput5V1State();
+    void GetOutput5V1Voltage();
+    void GetOutput5V1Current();
+
+    void GetOutput12VState();
+    void GetOutput12VVoltage();
+    void GetOutput12VCurrent();
+
+    void GetOutput24VState();
+    void GetOutput24VVoltage();
+    void GetOutput24VCurrent();
+
+    void SetSoftwareEmergencyStop(bool state);
+    void Set5VState(bool state);
+    void Set12VState(bool state);
+    void Set24VState(bool state);
+};
+}  // namespace Modelec

src/modelec/include/modelec/usb_arduino_listener.hpp

@@ -1,40 +0,0 @@
-#pragma once
-
-#include <rclcpp/rclcpp.hpp>
-#include <std_msgs/msg/string.hpp>
-#include <boost/asio.hpp>
-
-#define SERIAL_PORT "/dev/ttyUSB0"
-#define MAX_MESSAGE_LEN 1048
-#define BAUDS 115200 //vitesse des données (bit/sec)
-
-namespace Modelec {
-
-class USBListener : public rclcpp::Node {
-public:
-    USBListener();
-    ~USBListener() override;
-
-private:
-    int bauds = BAUDS;
-    std::string serial_port = SERIAL_PORT;
-    int max_message_len = MAX_MESSAGE_LEN;
-
-    boost::asio::io_service io;
-    boost::asio::serial_port port;
-    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher;
-    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscriber;
-    rclcpp::TimerBase::SharedPtr timer;
-
-    void readData();
-
-    OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_;
-    rcl_interfaces::msg::SetParametersResult onParameterChange(const std::vector<rclcpp::Parameter> &parameters);
-    void updateConnection();
-
-    void write_to_arduino(const std::string &data);
-
-    bool initializeConnection();
-};
-
-}

src/modelec/include/modelec/usb_arduino_listener_newlib.hpp

@@ -1,39 +0,0 @@
-#pragma once
-
-#include <rclcpp/rclcpp.hpp>
-#include <std_msgs/msg/string.hpp>
-#include <wiringSerial.h>
-
-#define SERIAL_PORT "/dev/pts/6"
-#define MAX_MESSAGE_LEN 1048
-#define BAUDS 115200 //vitesse des données (bit/sec)
-
-namespace Modelec {
-
-class USBListener : public rclcpp::Node {
-public:
-    USBListener();
-    ~USBListener() override;
-
-private:
-    int bauds = BAUDS;
-    std::string serial_port = SERIAL_PORT;
-    int max_message_len = MAX_MESSAGE_LEN;
-
-    int fd;
-    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher;
-    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscriber;
-    rclcpp::TimerBase::SharedPtr timer;
-
-    void readData();
-
-    OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_;
-    rcl_interfaces::msg::SetParametersResult onParameterChange(const std::vector<rclcpp::Parameter> &parameters);
-    void updateConnection();
-
-    void write_to_arduino(const std::string &data);
-
-    bool initializeConnection();
-};
-
-}

src/modelec/launch/modelec.launch.yml

@@ -2,14 +2,19 @@ launch:
 
 - node:
     pkg: modelec
-    exec: "usb_arduino_listener"
-    name: "usb_arduino_listener"
+    exec: "serial_listener"
+    name: "serial_listener"
 
 - node:
     pkg: 'modelec'
     exec: 'usb_arduino_logic_processor'
     name: 'usb_arduino_logic_processor'
 
+- node:
+    pkg: 'modelec'
+    exec: 'pcb_alim_interface'
+    name: 'pcb_alim_interface'
+
 - node:
     pkg: 'modelec'
     exec: 'pca9685_controller'

src/modelec/launch/modelec_gamecontroller.launch.yml

@@ -2,14 +2,19 @@ launch:
 
 - node:
     pkg: modelec
-    exec: "usb_arduino_listener"
-    name: "usb_arduino_listener"
+    exec: "serial_listener"
+    name: "serial_listener"
 
 - node:
     pkg: 'modelec'
     exec: 'usb_arduino_logic_processor'
     name: 'usb_arduino_logic_processor'
 
+- node:
+    pkg: 'modelec'
+    exec: 'pcb_alim_interface'
+    name: 'pcb_alim_interface'
+
 - node:
     pkg: 'modelec'
     exec: 'pca9685_controller'

src/modelec/launch/modelec_gamecontroller_remote_server.launch.yml

@@ -2,14 +2,19 @@ launch:
 
 - node:
     pkg: modelec
-    exec: "usb_arduino_listener"
-    name: "usb_arduino_listener"
+    exec: "serial_listener"
+    name: "serial_listener"
 
 - node:
     pkg: 'modelec'
     exec: 'usb_arduino_logic_processor'
     name: 'usb_arduino_logic_processor'
 
+- node:
+    pkg: 'modelec'
+    exec: 'pcb_alim_interface'
+    name: 'pcb_alim_interface'
+
 - node:
     pkg: 'modelec'
     exec: 'pca9685_controller'

src/modelec/src/gamecontroller_listener.cpp

@@ -1,5 +1,6 @@
 #include "modelec/gamecontroller_listener.hpp"
 #include "modelec/utils.hpp"
+#include <modelec_interface/srv/add_serial_listener.hpp>
 
 namespace Modelec {
     ControllerListener::ControllerListener() : Node("controller_listener")
@@ -11,7 +12,31 @@ namespace Modelec {
 
         servo_publisher_ = this->create_publisher<ServoMode>("arm_control", 10);
 
-        arduino_publisher_ = this->create_publisher<std_msgs::msg::String>("send_to_arduino", 10);
+          // Service to create a new serial listener
+        auto request = std::make_shared<modelec_interface::srv::AddSerialListener::Request>();
+        request->name = "arduino";
+        request->bauds = 115200;
+        request->serial_port = "/dev/ttyACM0";
+        auto client = this->create_client<modelec_interface::srv::AddSerialListener>("add_serial_listener");
+        while (!client->wait_for_service(std::chrono::seconds(1))) {
+            if (!rclcpp::ok()) {
+            RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting.");
+            return;
+            }
+            RCLCPP_INFO(this->get_logger(), "service not available, waiting again...");
+        }
+        auto result = client->async_send_request(request);
+        if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) == rclcpp::FutureReturnCode::SUCCESS) {
+            if (!result.get()->status) {
+            RCLCPP_ERROR(this->get_logger(), "Failed to add serial listener");
+            } else {
+            RCLCPP_INFO(this->get_logger(), "Added serial listener");
+            }
+        } else {
+            RCLCPP_ERROR(this->get_logger(), "Service call failed");
+        }
+
+        arduino_publisher_ = this->create_publisher<std_msgs::msg::String>(result.get()->subscriber, 10);
 
         clear_pca_publisher_ = this->create_publisher<std_msgs::msg::Empty>("clear_pca9685", 10);
 

src/modelec/src/multiple_serial_listener.cpp

@@ -0,0 +1,131 @@
+#include <modelec/multiple_serial_listener.hpp>
+
+namespace Modelec {
+    MultipleSerialListener::MultipleSerialListener() : Node("multiple_serial_listener"), io() {
+        add_serial_listener_service_ = create_service<modelec_interface::srv::AddSerialListener>("add_serial_listener", std::bind(&MultipleSerialListener::add_serial_listener, this, std::placeholders::_1, std::placeholders::_2));
+        timer = create_wall_timer(std::chrono::milliseconds(READ_REFRESH_RATE), [this]() {
+            for (auto &listener : serial_listeners) {
+                listener.second->asyncRead();
+            }
+        });
+
+        parameter_callback_handle_ = this->add_on_set_parameters_callback(
+            [this](const std::vector<rclcpp::Parameter> &parameters) {
+                return onParameterChange(parameters);
+            });
+    }
+
+    MultipleSerialListener::~MultipleSerialListener() {
+        for (auto &listener : serial_listeners) {
+            listener.second->port_.close();
+        }
+    }
+
+    void MultipleSerialListener::add_serial_listener(const std::shared_ptr<modelec_interface::srv::AddSerialListener::Request> request, std::shared_ptr<modelec_interface::srv::AddSerialListener::Response> response) {
+        if (serial_listeners.find(request->name) != serial_listeners.end()) {
+            response->status = true;
+            response->publisher = serial_listeners[request->name]->publisher_->get_topic_name();
+            response->subscriber = serial_listeners[request->name]->subscriber_->get_topic_name();
+            return;
+        }
+
+        auto listener = std::make_shared<SerialListener>(request->name, request->bauds, request->serial_port, MAX_MESSAGE_LEN, io);
+
+        if (!listener->IsOk()) {
+            response->status = false;
+            return;
+        }
+
+        listener->publisher_ = create_publisher<std_msgs::msg::String>("raw_data/" + listener->name_, 10);
+        listener->subscriber_ = create_subscription<std_msgs::msg::String>( 
+            "send_to_serial/" + listener->name_, 10, [listener](std_msgs::msg::String::SharedPtr msg) {
+                listener->asyncWrite(msg->data);
+            });
+
+        serial_listeners.insert({request->name, listener});
+
+        response->status = true;
+        response->publisher = listener->publisher_->get_topic_name();
+        response->subscriber = listener->subscriber_->get_topic_name();
+    }
+
+    rcl_interfaces::msg::SetParametersResult MultipleSerialListener::onParameterChange(const std::vector<rclcpp::Parameter> &parameters) {
+        rcl_interfaces::msg::SetParametersResult result;
+        result.successful = true;
+
+        for (const auto &parameter : parameters) {
+            if (parameter.get_name() == "bauds" || parameter.get_name() == "serial_port" || parameter.get_name() == "max_message_len") {
+                updateConnection();
+            }
+        }
+
+        return result;
+    }
+
+    void MultipleSerialListener::updateConnection() {
+        for (auto &listener : serial_listeners) {
+            listener.second->SetMaxMessageLen(get_parameter("max_message_len").as_int());
+        }
+
+        // change the timer refresh rate
+        read_refresh_rate_ = get_parameter("read_refresh_rate").as_int();
+        timer->cancel();
+        timer = create_wall_timer(std::chrono::milliseconds(read_refresh_rate_), [this]() {
+            for (auto &listener : serial_listeners) {
+                listener.second->asyncRead();
+            }
+        });
+    }
+
+    MultipleSerialListener::SerialListener::SerialListener(const std::string& name, int bauds, const std::string& serial_port, int max_message_len, boost::asio::io_service& io)
+        : bauds_(bauds), serial_port_(serial_port), max_message_len_(max_message_len), io_(io), name_(name), port_(io)
+    {
+        try {
+            port_.open(serial_port_);
+            port_.set_option(boost::asio::serial_port_base::baud_rate(bauds_));
+            status_ = true;
+        } catch (boost::system::system_error &e) {
+            RCLCPP_ERROR(rclcpp::get_logger("MultipleSerialListener"), "Failed to open serial port: %s", e.what());
+            status_ = false;
+            return;
+        }
+        asyncRead();  // Start asynchronous reading
+    }
+
+    void MultipleSerialListener::SerialListener::asyncRead() {
+        if (!status_) return;
+
+        boost::asio::async_read_until(port_, read_buffer_, '\n', 
+            [this](boost::system::error_code ec, std::size_t) {
+                if (!ec) {
+                    std::istream is(&read_buffer_);
+                    std::string data;
+                    std::getline(is, data);
+                    std_msgs::msg::String msg;
+                    msg.data = data;
+                    publisher_->publish(msg);
+                } else {
+                    RCLCPP_ERROR(rclcpp::get_logger("SerialListener"), "Read error: %s", ec.message().c_str());
+                }
+                asyncRead(); // Continue reading asynchronously
+            });
+    }
+
+    void MultipleSerialListener::SerialListener::asyncWrite(const std::string& data) {
+        if (!status_) return;
+
+        boost::asio::async_write(port_, boost::asio::buffer(data),
+            [](boost::system::error_code ec, std::size_t) {
+                if (ec) {
+                    RCLCPP_ERROR(rclcpp::get_logger("SerialListener"), "Write error: %s", ec.message().c_str());
+                }
+            });
+    }
+}
+
+int main(int argc, char **argv) {
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<Modelec::MultipleSerialListener>());
+    rclcpp::shutdown();
+    return 0;
+}

src/modelec/src/pcb_alim_interface.cpp

@@ -0,0 +1,162 @@
+#include <modelec/pcb_alim_interface.hpp>
+#include <modelec_interface/srv/add_serial_listener.hpp>
+
+namespace Modelec
+{
+    PCBAlimInterface::PCBAlimInterface() : Node("pcb_alim_interface")
+    {
+        // Service to create a new serial listener
+        auto request = std::make_shared<modelec_interface::srv::AddSerialListener::Request>();
+        request->name = "pcb_alim";
+        request->bauds = 115200;
+        request->serial_port = "/dev/serial0"; // TODO : check the real serial port
+        auto client = this->create_client<modelec_interface::srv::AddSerialListener>("add_serial_listener");
+        while (!client->wait_for_service(std::chrono::seconds(1))) {
+            if (!rclcpp::ok()) {
+            RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting.");
+            return;
+            }
+            RCLCPP_INFO(this->get_logger(), "service not available, waiting again...");
+        }
+        auto result = client->async_send_request(request);
+        if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) == rclcpp::FutureReturnCode::SUCCESS) {
+            if (!result.get()->status) {
+            RCLCPP_ERROR(this->get_logger(), "Failed to add serial listener");
+            } else {
+            RCLCPP_INFO(this->get_logger(), "Added serial listener");
+            }
+        } else {
+            RCLCPP_ERROR(this->get_logger(), "Service call failed");
+        }
+
+        pcb_publisher_ = this->create_publisher<std_msgs::msg::String>(result.get()->subscriber, 10);
+        pcb_subscriber_ = this->create_subscription<std_msgs::msg::String>(
+            result.get()->publisher, 10,
+        std::bind(&PCBAlimInterface::PCBCallback, this, std::placeholders::_1));
+    }
+
+    void PCBAlimInterface::PCBCallback(const std_msgs::msg::String::SharedPtr msg)
+    {
+        RCLCPP_INFO(this->get_logger(), "Received message: '%s'", msg->data.c_str());
+    }
+
+    void PCBAlimInterface::SendToPCB(const std::string &data)
+    {
+        auto message = std_msgs::msg::String();
+        message.data = data;
+        pcb_publisher_->publish(message);
+    }
+
+    void PCBAlimInterface::GetData(const std::string &elem, const std::string& value)
+    {
+        SendToPCB("GET;" + elem + ";" + value);
+    }
+
+    void PCBAlimInterface::SendOrder(const std::string &elem, const std::string& data, const std::string& value)
+    {
+        SendToPCB("SET;" + elem + ";" + data + ";" + value);
+    }
+
+    void PCBAlimInterface::GetEmergencyStopButtonState()
+    {
+        GetData("BAU", "STATE");
+    }
+
+    void PCBAlimInterface::GetEntryVoltage(int entry) {
+        GetData("IN" +  std::to_string(entry), "VOLT");
+    }
+
+    void PCBAlimInterface::GetEntryCurrent(int entry) {
+        GetData("IN" +  std::to_string(entry), "AMP");
+    }
+
+    void PCBAlimInterface::GetEntryState(int entry) {
+        GetData("IN" +  std::to_string(entry), "STATE");
+    }
+
+    void PCBAlimInterface::GetEntryIsValide(int entry) {
+        GetData("IN" +  std::to_string(entry), "VALID");
+    }
+
+    void PCBAlimInterface::GetPCBTemperature() {
+        GetData("TEMP", "CELS");
+    }
+
+    
+    void PCBAlimInterface::GetOutput5VState() {
+        GetData("OUT5V", "STATE");
+    }
+    
+    void PCBAlimInterface::GetOutput5VVoltage() {
+        GetData("OUT5V", "VOLT");
+    }
+    
+    void PCBAlimInterface::GetOutput5VCurrent() {
+        GetData("OUT5V", "AMP");
+    }
+
+
+    void PCBAlimInterface::GetOutput5V1State() {
+        GetData("OUT5V1", "STATE");
+    }
+    
+    void PCBAlimInterface::GetOutput5V1Voltage() {
+        GetData("OUT5V1", "VOLT");
+    }
+    
+    void PCBAlimInterface::GetOutput5V1Current() {
+        GetData("OUT5V1", "AMP");
+    }
+
+
+    
+    void PCBAlimInterface::GetOutput12VState() {
+        GetData("OUT12V", "STATE");
+    }
+    
+    void PCBAlimInterface::GetOutput12VVoltage() {
+        GetData("OUT12V", "VOLT");
+    }
+    
+    void PCBAlimInterface::GetOutput12VCurrent() {
+        GetData("OUT12V", "AMP");
+    }
+
+
+    void PCBAlimInterface::GetOutput24VState() {
+        GetData("OUT24V", "STATE");
+    }
+    
+    void PCBAlimInterface::GetOutput24VVoltage() {
+        GetData("OUT24V", "VOLT");
+    }
+    
+    void PCBAlimInterface::GetOutput24VCurrent() {
+        GetData("OUT24V", "AMP");
+    }
+
+
+    void PCBAlimInterface::SetSoftwareEmergencyStop(bool state) {
+        SendOrder("EMG", "STATE", state ? "1" : "0");
+    }
+    
+    void PCBAlimInterface::Set5VState(bool state) {
+        SendOrder("OUT5V", "STATE", state ? "1" : "0");
+    }
+    
+    void PCBAlimInterface::Set12VState(bool state) {
+        SendOrder("OUT12V", "STATE", state ? "1" : "0");
+    }
+    
+    void PCBAlimInterface::Set24VState(bool state) {
+        SendOrder("OUT24V", "STATE", state ? "1" : "0");
+    }
+} // namespace Modelec
+
+int main(int argc, char **argv)
+{
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<Modelec::PCBAlimInterface>());
+    rclcpp::shutdown();
+    return 0;
+}

src/modelec/src/usb_arduino_listener.cpp

@@ -1,137 +0,0 @@
-#include "modelec/usb_arduino_listener.hpp"
-
-namespace Modelec {
-  USBListener::USBListener() : Node("usb_listener"), io(), port(io) {
-    
-    // Declare and initialize parameters
-    bauds = this->declare_parameter<int>("bauds", BAUDS);
-    serial_port = this->declare_parameter<std::string>("serial_port", SERIAL_PORT);
-    max_message_len = this->declare_parameter<int>("max_message_len", MAX_MESSAGE_LEN);
-
-    // Initialize topic publisher
-    publisher = this->create_publisher<std_msgs::msg::String>("arduino_raw_data", 10);
-
-    subscriber = this->create_subscription<std_msgs::msg::String>(
-      "send_to_arduino", 10, [this](std_msgs::msg::String::SharedPtr msg) {
-        write_to_arduino(msg->data);
-      });
-
-    // Open usb port
-    if (!initializeConnection()) {
-      RCLCPP_ERROR(this->get_logger(), "Failed to initialize the serial port, shutting down.");
-      rclcpp::shutdown();
-    }
-
-    // Start reading periodically
-    timer = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&USBListener::readData, this));
-
-    // Set a callback for parameter updates
-    parameter_callback_handle_ = this->add_on_set_parameters_callback(
-      [this](const std::vector<rclcpp::Parameter> &parameters) {
-        return onParameterChange(parameters);
-      });
-  }
-
-  bool USBListener::initializeConnection() {
-    try {
-      // Try to open the serial port
-      port.open(serial_port);
-      port.set_option(boost::asio::serial_port_base::baud_rate(bauds));
-      RCLCPP_INFO(this->get_logger(), "Serial port '%s' opened successfully", serial_port.c_str());
-      return true;
-    } catch (boost::system::system_error &e) {
-      RCLCPP_ERROR(this->get_logger(), "Error opening serial port '%s': %s", serial_port.c_str(), e.what());
-      return false;
-    }
-  }
-
-  void USBListener::readData() {
-    if (!port.is_open()) {
-      return;  // Port is not open, so exit
-    }
-
-    std::vector<char> data(max_message_len);
-    try {
-      // Attempt to read data from the serial port
-      size_t len = port.read_some(boost::asio::buffer(data.data(), max_message_len));
-      if (len > 0) {
-        // Prepare and publish the message
-        auto msg = std_msgs::msg::String();
-        msg.data = std::string(data.begin(), data.begin() + len);
-        publisher->publish(msg);
-      }
-    } catch (boost::system::system_error &e) {
-      RCLCPP_ERROR(this->get_logger(), "Error reading from serial port: %s", e.what());
-    }
-  }
-
-  USBListener::~USBListener() {
-    // Cleanup resources and close port
-    parameter_callback_handle_.reset();
-    if (port.is_open()) {
-      port.close();
-    }
-  }
-
-  rcl_interfaces::msg::SetParametersResult USBListener::onParameterChange(const std::vector<rclcpp::Parameter> &parameters) {
-    rcl_interfaces::msg::SetParametersResult result;
-    result.successful = true;
-
-    for (const auto &parameter : parameters) {
-      if (parameter.get_name() == "bauds" || parameter.get_name() == "serial_port" || parameter.get_name() == "max_message_len") {
-        updateConnection();
-      }
-    }
-
-    return result;
-  }
-
-  void USBListener::updateConnection() {
-    // Retrieve the current parameters
-    int new_bauds = this->get_parameter("bauds").as_int();
-    std::string new_serial_port = this->get_parameter("serial_port").as_string();
-    int new_max_message_len = this->get_parameter("max_message_len").as_int();
-
-    // Check if the parameters have changed
-    if (new_bauds == bauds && new_serial_port == serial_port && new_max_message_len == max_message_len) {
-      RCLCPP_DEBUG(this->get_logger(), "Connection parameters have not changed.");
-      return;  // No changes, no need to update the connection
-    }
-
-    // Update the connection parameters
-    bauds = new_bauds;
-    serial_port = new_serial_port;
-    max_message_len = new_max_message_len;
-
-    // Reopen the connection with updated parameters
-    try {
-      if (port.is_open()) {
-        port.close();  // Close the current port before reopening
-      }
-
-      if (!initializeConnection()) {
-        RCLCPP_ERROR(this->get_logger(), "Failed to reopen the serial port, shutting down.");
-        rclcpp::shutdown();  // Graceful shutdown if port opening fails
-      }
-
-    } catch (boost::system::system_error &e) {
-      RCLCPP_ERROR(this->get_logger(), "Error during connection update: %s", e.what());
-      rclcpp::shutdown();
-    }
-  }
-
-  void USBListener::write_to_arduino(const std::string &data) {
-    try {
-      boost::asio::write(port, boost::asio::buffer(data));
-    } catch (boost::system::system_error &e) {
-      RCLCPP_ERROR(this->get_logger(), "Error writing to serial port: %s", e.what());
-    }
-  }
-}
-
-int main(int argc, char **argv) {
-  rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<Modelec::USBListener>());
-  rclcpp::shutdown();
-  return 0;
-}

src/modelec/src/usb_arduino_listener_newlib.cpp

@@ -1,136 +0,0 @@
-#include "modelec/usb_arduino_listener_newlib.hpp"
-#include <wiringPi.h>
-
-namespace Modelec {
-  USBListener::USBListener() : Node("usb_listener") {
-    
-    // Declare and initialize parameters
-    bauds = this->declare_parameter<int>("bauds", BAUDS);
-    serial_port = this->declare_parameter<std::string>("serial_port", SERIAL_PORT);
-    max_message_len = this->declare_parameter<int>("max_message_len", MAX_MESSAGE_LEN);
-
-    // Initialize topic publisher
-    publisher = this->create_publisher<std_msgs::msg::String>("arduino_raw_data", 10);
-
-    subscriber = this->create_subscription<std_msgs::msg::String>(
-      "send_to_arduino", 10, [this](std_msgs::msg::String::SharedPtr msg) {
-        write_to_arduino(msg->data);
-      });
-
-    // Open usb port
-    if (!initializeConnection()) {
-      RCLCPP_ERROR(this->get_logger(), "Failed to initialize the serial port, shutting down.");
-      rclcpp::shutdown();
-    }
-
-    // Start reading periodically
-    timer = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&USBListener::readData, this));
-
-    // Set a callback for parameter updates
-    parameter_callback_handle_ = this->add_on_set_parameters_callback(
-      [this](const std::vector<rclcpp::Parameter> &parameters) {
-        return onParameterChange(parameters);
-      });
-  }
-
-  bool USBListener::initializeConnection() {
-    try {
-      // Try to open the serial port
-      fd = serialOpen(serial_port.c_str(), bauds);
-      if (fd < 0) {
-        RCLCPP_ERROR(this->get_logger(), "Error opening serial port '%s'", serial_port.c_str());
-        return false;
-      }
-
-      if (wiringPiSetup() == -1) {
-        RCLCPP_ERROR(this->get_logger(), "Error setting up wiringPi");
-        return false;
-      }
-
-      RCLCPP_INFO(this->get_logger(), "Serial port '%s' opened successfully", serial_port.c_str());
-      return true;
-    } catch (std::exception &e) {
-      RCLCPP_ERROR(this->get_logger(), "Error opening serial port '%s': %s", serial_port.c_str(), e.what());
-      return false;
-    }
-  }
-
-  void USBListener::readData() {
-    std::vector<char> data(max_message_len);
-
-    // Read data from the serial port
-    while (serialDataAvail(fd) > 0) {
-      data.push_back(serialGetchar(fd));
-    }
-
-    // Publish the received data
-    std_msgs::msg::String msg;
-    msg.data = std::string(data.begin(), data.end());
-    publisher->publish(msg);
-
-    RCLCPP_DEBUG(this->get_logger(), "Received data: %s", data.data());
-  }
-
-  USBListener::~USBListener() {
-    // Cleanup resources and close port
-    parameter_callback_handle_.reset();
-    serialClose(fd);
-  }
-
-  rcl_interfaces::msg::SetParametersResult USBListener::onParameterChange(const std::vector<rclcpp::Parameter> &parameters) {
-    rcl_interfaces::msg::SetParametersResult result;
-    result.successful = true;
-
-    for (const auto &parameter : parameters) {
-      if (parameter.get_name() == "bauds" || parameter.get_name() == "serial_port" || parameter.get_name() == "max_message_len") {
-        updateConnection();
-      }
-    }
-
-    return result;
-  }
-
-  void USBListener::updateConnection() {
-    // Retrieve the current parameters
-    int new_bauds = this->get_parameter("bauds").as_int();
-    std::string new_serial_port = this->get_parameter("serial_port").as_string();
-    int new_max_message_len = this->get_parameter("max_message_len").as_int();
-
-    // Check if the parameters have changed
-    if (new_bauds == bauds && new_serial_port == serial_port && new_max_message_len == max_message_len) {
-      RCLCPP_DEBUG(this->get_logger(), "Connection parameters have not changed.");
-      return;  // No changes, no need to update the connection
-    }
-
-    // Update the connection parameters
-    bauds = new_bauds;
-    serial_port = new_serial_port;
-    max_message_len = new_max_message_len;
-
-    // Reopen the connection with updated parameters
-    try {
-      serialClose(fd);
-
-      if (!initializeConnection()) {
-        RCLCPP_ERROR(this->get_logger(), "Failed to reopen the serial port, shutting down.");
-        rclcpp::shutdown();  // Graceful shutdown if port opening fails
-      }
-
-    } catch (std::exception &e) {
-      RCLCPP_ERROR(this->get_logger(), "Error updating connection parameters: %s", e.what());
-    }
-  }
-
-  void USBListener::write_to_arduino(const std::string &data) {
-    serialPuts(fd, data.c_str());
-    serialFlush(fd);
-    RCLCPP_INFO(this->get_logger(), "Sent data: %s", data.c_str());
-  }
-}
-
-int main(int argc, char **argv) {
-  rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<Modelec::USBListener>());
-  rclcpp::shutdown();
-  return 0;
-}

src/modelec/src/usb_arduino_logic_processor.cpp

@@ -1,11 +1,38 @@
 #include "modelec/usb_aruido_logic_processor.hpp"
+#include "modelec_interface/srv/add_serial_listener.hpp"
 #include <vector>
+#include <modelec_interface/srv/add_serial_listener.hpp>
 
 using namespace Modelec;
 
 LogicProcessor::LogicProcessor() : Node("usb_logic_processor") {
+
+  // Service to create a new serial listener
+  auto request = std::make_shared<modelec_interface::srv::AddSerialListener::Request>();
+  request->name = "arduino";
+  request->bauds = 115200;
+  request->serial_port = "/dev/ttyACM0";
+  auto client = this->create_client<modelec_interface::srv::AddSerialListener>("add_serial_listener");
+  while (!client->wait_for_service(std::chrono::seconds(1))) {
+    if (!rclcpp::ok()) {
+      RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting.");
+      return;
+    }
+    RCLCPP_INFO(this->get_logger(), "service not available, waiting again...");
+  }
+  auto result = client->async_send_request(request);
+  if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) == rclcpp::FutureReturnCode::SUCCESS) {
+    if (!result.get()->status) {
+      RCLCPP_ERROR(this->get_logger(), "Failed to add serial listener");
+    } else {
+      RCLCPP_INFO(this->get_logger(), "Added serial listener");
+    }
+  } else {
+    RCLCPP_ERROR(this->get_logger(), "Service call failed");
+  }
+
     subscriber_ = this->create_subscription<std_msgs::msg::String>(
-        "arduino_raw_data", 10, [this](std_msgs::msg::String::SharedPtr msg) {
+        result.get()->publisher, 10, [this](std_msgs::msg::String::SharedPtr msg) {
             processData(msg->data);
         });
     publisher_ = this->create_publisher<modelec_interface::msg::ArduinoData>("arduino_processed_data", 10);

src/modelec/worlds/gazebo.world

@@ -0,0 +1,300 @@
+<?xml version="1.0" ?>
+<sdf version="1.8">
+    <world name="Moving_robot">
+        <physics name="1ms" type="ignored">
+            <max_step_size>0.001</max_step_size>
+            <real_time_factor>1.0</real_time_factor>
+        </physics>
+        <plugin
+            filename="gz-sim-physics-system"
+            name="gz::sim::systems::Physics">
+        </plugin>
+        <plugin
+            filename="gz-sim-user-commands-system"
+            name="gz::sim::systems::UserCommands">
+        </plugin>
+        <plugin
+            filename="gz-sim-scene-broadcaster-system"
+            name="gz::sim::systems::SceneBroadcaster">
+        </plugin>
+
+        <light type="directional" name="sun">
+            <cast_shadows>true</cast_shadows>
+            <pose>0 0 10 0 0 0</pose>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.2 0.2 0.2 1</specular>
+            <attenuation>
+                <range>1000</range>
+                <constant>0.9</constant>
+                <linear>0.01</linear>
+                <quadratic>0.001</quadratic>
+            </attenuation>
+            <direction>-0.5 0.1 -0.9</direction>
+        </light>
+
+        <model name="ground_plane">
+            <static>true</static>
+            <link name="link">
+                <collision name="collision">
+                <geometry>
+                    <plane>
+                    <normal>0 0 1</normal>
+                    </plane>
+                </geometry>
+                </collision>
+                <visual name="visual">
+                <geometry>
+                    <plane>
+                    <normal>0 0 1</normal>
+                    <size>100 100</size>
+                    </plane>
+                </geometry>
+                <material>
+                    <ambient>0.8 0.8 0.8 1</ambient>
+                    <diffuse>0.8 0.8 0.8 1</diffuse>
+                    <specular>0.8 0.8 0.8 1</specular>
+                </material>
+                </visual>
+            </link>
+        </model>
+
+        <model name='vehicle_blue' canonical_link='chassis'>
+            <pose relative_to='world'>0 0 0 0 0 0</pose>   <!--the pose is relative to the world by default-->
+
+            <link name='chassis'>
+                <pose relative_to='__model__'>0.5 0 0.4 0 0 0</pose>
+                <inertial> <!--inertial properties of the link mass, inertia matix-->
+                    <mass>1.14395</mass>
+                    <inertia>
+                        <ixx>0.126164</ixx>
+                        <ixy>0</ixy>
+                        <ixz>0</ixz>
+                        <iyy>0.416519</iyy>
+                        <iyz>0</iyz>
+                        <izz>0.481014</izz>
+                    </inertia>
+                </inertial>
+                <visual name='visual'>
+                    <geometry>
+                        <box>
+                            <size>2.0 1.0 0.5</size> <!--question: this size is in meter-->
+                        </box>
+                    </geometry>
+                    <!--let's add color to our link-->
+                    <material>
+                        <ambient>0.0 0.0 1.0 1</ambient>
+                        <diffuse>0.0 0.0 1.0 1</diffuse>
+                        <specular>0.0 0.0 1.0 1</specular>
+                    </material>
+                </visual>
+                <collision name='collision'> <!--todo: describe why we need the collision-->
+                    <geometry>
+                        <box>
+                            <size>2.0 1.0 0.5</size>
+                        </box>
+                    </geometry>
+                </collision>
+            </link>
+
+            <!--let's build the left wheel-->
+            <link name='left_wheel'>
+                <pose relative_to="chassis">-0.5 0.6 0 -1.5707 0 0</pose> <!--angles are in radian-->
+                <inertial>
+                    <mass>2</mass>
+                    <inertia>
+                        <ixx>0.145833</ixx>
+                        <ixy>0</ixy>
+                        <ixz>0</ixz>
+                        <iyy>0.145833</iyy>
+                        <iyz>0</iyz>
+                        <izz>0.125</izz>
+                    </inertia>
+                </inertial>
+                <visual name='visual'>
+                    <geometry>
+                        <cylinder>
+                            <radius>0.4</radius>
+                            <length>0.2</length>
+                        </cylinder>
+                    </geometry>
+                    <material>
+                        <ambient>1.0 0.0 0.0 1</ambient>
+                        <diffuse>1.0 0.0 0.0 1</diffuse>
+                        <specular>1.0 0.0 0.0 1</specular>
+                    </material>
+                </visual>
+                <collision name='collision'>
+                    <geometry>
+                        <cylinder>
+                            <radius>0.4</radius>
+                            <length>0.2</length>
+                        </cylinder>
+                    </geometry>
+                </collision>
+            </link>
+
+            <!--copy and paste for right wheel but change position-->
+            <link name='right_wheel'>
+                <pose relative_to="chassis">-0.5 -0.6 0 -1.5707 0 0</pose> <!--angles are in radian-->
+                <inertial>
+                    <mass>1</mass>
+                    <inertia>
+                        <ixx>0.145833</ixx>
+                        <ixy>0</ixy>
+                        <ixz>0</ixz>
+                        <iyy>0.145833</iyy>
+                        <iyz>0</iyz>
+                        <izz>0.125</izz>
+                    </inertia>
+                </inertial>
+                <visual name='visual'>
+                    <geometry>
+                        <cylinder>
+                            <radius>0.4</radius>
+                            <length>0.2</length>
+                        </cylinder>
+                    </geometry>
+                    <material>
+                        <ambient>1.0 0.0 0.0 1</ambient>
+                        <diffuse>1.0 0.0 0.0 1</diffuse>
+                        <specular>1.0 0.0 0.0 1</specular>
+                    </material>
+                </visual>
+                <collision name='collision'>
+                    <geometry>
+                        <cylinder>
+                            <radius>0.4</radius>
+                            <length>0.2</length>
+                        </cylinder>
+                    </geometry>
+                </collision>
+            </link>
+
+            <frame name="caster_frame" attached_to='chassis'>
+                <pose>0.8 0 -0.2 0 0 0</pose>
+            </frame>
+
+            <!--caster wheel-->
+            <link name='caster'>
+                <pose relative_to='caster_frame'/>
+                <inertial>
+                    <mass>1</mass>
+                    <inertia>
+                        <ixx>0.1</ixx>
+                        <ixy>0</ixy>
+                        <ixz>0</ixz>
+                        <iyy>0.1</iyy>
+                        <iyz>0</iyz>
+                        <izz>0.1</izz>
+                    </inertia>
+                </inertial>
+                <visual name='visual'>
+                    <geometry>
+                        <sphere>
+                            <radius>0.2</radius>
+                        </sphere>
+                    </geometry>
+                    <material>
+                        <ambient>0.0 1 0.0 1</ambient>
+                        <diffuse>0.0 1 0.0 1</diffuse>
+                        <specular>0.0 1 0.0 1</specular>
+                    </material>
+                </visual>
+                <collision name='collision'>
+                    <geometry>
+                        <sphere>
+                            <radius>0.2</radius>
+                        </sphere>
+                    </geometry>
+                </collision>
+            </link>
+
+
+            <!--connecting these links together using joints-->
+            <joint name='left_wheel_joint' type='revolute'> <!--continous joint is not supported yet-->
+                <pose relative_to='left_wheel'/>
+                <parent>chassis</parent>
+                <child>left_wheel</child>
+                <axis>
+                    <xyz expressed_in='__model__'>0 1 0</xyz> <!--can be defined as any frame or even arbitrary frames-->
+                    <limit>
+                        <lower>-1.79769e+308</lower>    <!--negative infinity-->
+                        <upper>1.79769e+308</upper>     <!--positive infinity-->
+                    </limit>
+                </axis>
+            </joint>
+
+            <joint name='right_wheel_joint' type='revolute'>
+                <pose relative_to='right_wheel'/>
+                <parent>chassis</parent>
+                <child>right_wheel</child>
+                <axis>
+                    <xyz expressed_in='__model__'>0 1 0</xyz>
+                    <limit>
+                        <lower>-1.79769e+308</lower>    <!--negative infinity-->
+                        <upper>1.79769e+308</upper>     <!--positive infinity-->
+                    </limit>
+                </axis>
+            </joint>
+
+            <!--different type of joints ball joint--> <!--defult value is the child-->
+            <joint name='caster_wheel' type='ball'>
+                <parent>chassis</parent>
+                <child>caster</child>
+            </joint>
+
+            <!--diff drive plugin-->
+            <plugin
+                filename="gz-sim-diff-drive-system"
+                name="gz::sim::systems::DiffDrive">
+                <left_joint>left_wheel_joint</left_joint>
+                <right_joint>right_wheel_joint</right_joint>
+                <wheel_separation>1.2</wheel_separation>
+                <wheel_radius>0.4</wheel_radius>
+                <odom_publish_frequency>1</odom_publish_frequency>
+                <topic>cmd_vel</topic>
+            </plugin>
+        </model>
+
+        <!-- Moving Left-->
+        <plugin filename="gz-sim-triggered-publisher-system"
+                name="gz::sim::systems::TriggeredPublisher">
+            <input type="gz.msgs.Int32" topic="/keyboard/keypress">
+                <match field="data">16777234</match>
+            </input>
+            <output type="gz.msgs.Twist" topic="/cmd_vel">
+                linear: {x: 0.0}, angular: {z: 0.5}
+            </output>
+        </plugin>
+        <!-- Moving Forward-->
+        <plugin filename="gz-sim-triggered-publisher-system"
+                name="gz::sim::systems::TriggeredPublisher">
+            <input type="gz.msgs.Int32" topic="/keyboard/keypress">
+                <match field="data">16777235</match>
+            </input>
+            <output type="gz.msgs.Twist" topic="/cmd_vel">
+                linear: {x: 0.5}, angular: {z: 0.0}
+            </output>
+        </plugin>
+        <!-- Moving Right-->
+        <plugin filename="gz-sim-triggered-publisher-system"
+                name="gz::sim::systems::TriggeredPublisher">
+            <input type="gz.msgs.Int32" topic="/keyboard/keypress">
+                <match field="data">16777236</match>
+            </input>
+            <output type="gz.msgs.Twist" topic="/cmd_vel">
+                linear: {x: 0.0}, angular: {z: -0.5}
+            </output>
+        </plugin>
+        <!-- Moving Backward-->
+        <plugin filename="gz-sim-triggered-publisher-system"
+                name="gz::sim::systems::TriggeredPublisher">
+            <input type="gz.msgs.Int32" topic="/keyboard/keypress">
+                <match field="data">16777237</match>
+            </input>
+            <output type="gz.msgs.Twist" topic="/cmd_vel">
+                linear: {x: -0.5}, angular: {z: 0.0}
+            </output>
+        </plugin>
+    </world>
+</sdf>

src/modelec_interface/CMakeLists.txt

@@ -22,6 +22,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/Tirette.srv"
   "srv/NewButton.srv"
   "srv/Button.srv"
+  "srv/AddSerialListener.srv"
   DEPENDENCIES std_msgs
 )
 

src/modelec_interface/srv/AddSerialListener.srv

@@ -0,0 +1,7 @@
+string name
+string serial_port
+int64 bauds
+---
+bool status
+string publisher
+string subscriber