From d1714a7fa010bc55d98b1aa5c367632123ea47da Mon Sep 17 00:00:00 2001
From: acki <ackimixs@protonmail.com>
Date: Tue, 6 May 2025 13:25:11 -0400
Subject: [PATCH] add more config

---
 src/modelec_com/CMakeLists.txt             | 13 +++++++------
 src/modelec_com/src/pcb_alim_interface.cpp | 15 ++++++++++++---
 src/modelec_com/src/pcb_odo_interface.cpp  | 15 ++++++++++++---
 src/modelec_strat/data/config.xml          | 14 ++++++++++++++
 4 files changed, 45 insertions(+), 12 deletions(-)

diff --git a/src/modelec_com/CMakeLists.txt b/src/modelec_com/CMakeLists.txt
index e502b9f..d6867bc 100644
--- a/src/modelec_com/CMakeLists.txt
+++ b/src/modelec_com/CMakeLists.txt
@@ -12,6 +12,7 @@ find_package(std_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(Boost REQUIRED COMPONENTS system)
+find_package(ament_index_cpp REQUIRED)
 
 find_package(modelec_interfaces REQUIRED)
 find_package(modelec_utils REQUIRED)
@@ -19,8 +20,8 @@ find_library(WIRINGPI_LIB wiringPi)
 
 # USB Arduino Listener Node
 add_executable(serial_listener src/multiple_serial_listener.cpp)
-ament_target_dependencies(serial_listener rclcpp std_msgs modelec_interfaces)
-target_link_libraries(serial_listener Boost::system modelec_utils::utils)
+ament_target_dependencies(serial_listener rclcpp std_msgs modelec_interfaces ament_index_cpp)
+target_link_libraries(serial_listener Boost::system modelec_utils::utils modelec_utils::config)
 target_include_directories(serial_listener PUBLIC
         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
         $<INSTALL_INTERFACE:include>
@@ -28,8 +29,8 @@ target_include_directories(serial_listener PUBLIC
 
 # USB Arduino Logic Processor Node
 add_executable(pcb_odo_interface src/pcb_odo_interface.cpp)
-ament_target_dependencies(pcb_odo_interface rclcpp std_msgs modelec_interfaces)
-target_link_libraries(pcb_odo_interface modelec_utils::utils)
+ament_target_dependencies(pcb_odo_interface rclcpp std_msgs modelec_interfaces ament_index_cpp)
+target_link_libraries(pcb_odo_interface modelec_utils::utils modelec_utils::config)
 target_include_directories(pcb_odo_interface PUBLIC
         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
         $<INSTALL_INTERFACE:include>
@@ -37,8 +38,8 @@ target_include_directories(pcb_odo_interface PUBLIC
 
 # 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_interfaces)
-target_link_libraries(pcb_alim_interface modelec_utils::utils)
+ament_target_dependencies(pcb_alim_interface rclcpp std_msgs modelec_interfaces ament_index_cpp)
+target_link_libraries(pcb_alim_interface modelec_utils::utils modelec_utils::config)
 target_include_directories(pcb_alim_interface PUBLIC
         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
         $<INSTALL_INTERFACE:include>
diff --git a/src/modelec_com/src/pcb_alim_interface.cpp b/src/modelec_com/src/pcb_alim_interface.cpp
index dbf6747..5e35371 100644
--- a/src/modelec_com/src/pcb_alim_interface.cpp
+++ b/src/modelec_com/src/pcb_alim_interface.cpp
@@ -1,16 +1,25 @@
 #include <modelec_com/pcb_alim_interface.hpp>
 #include <modelec_utils/utils.hpp>
 #include <modelec_interfaces/srv/add_serial_listener.hpp>
+#include <ament_index_cpp/get_package_share_directory.hpp>
+#include <modelec_utils/config.hpp>
 
 namespace Modelec
 {
     PCBAlimInterface::PCBAlimInterface() : Node("pcb_alim_interface")
     {
         // Service to create a new serial listener
+        std::string config_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml";
+        if (!Config::load(config_path))
+        {
+            RCLCPP_ERROR(get_logger(), "Failed to load config file: %s", config_path.c_str());
+        }
+
         auto request = std::make_shared<modelec_interfaces::srv::AddSerialListener::Request>();
-        request->name = "pcb_alim";
-        request->bauds = 115200;
-        request->serial_port = "/dev/pts/12"; // TODO : check the real serial port
+        request->name = Config::get<std::string>("config.usb.pcb.pcb_alim.name", "pcb_odo");
+        request->bauds = Config::get<int>("config.usb.pcb.pcb_alim.baudrate", 115200);
+        request->serial_port = Config::get<std::string>("config.usb.pcb.pcb_alim.port", "/dev/ttyUSB0");
+
         auto client = this->create_client<modelec_interfaces::srv::AddSerialListener>("add_serial_listener");
         while (!client->wait_for_service(std::chrono::seconds(1)))
         {
diff --git a/src/modelec_com/src/pcb_odo_interface.cpp b/src/modelec_com/src/pcb_odo_interface.cpp
index adab14c..ea2acec 100644
--- a/src/modelec_com/src/pcb_odo_interface.cpp
+++ b/src/modelec_com/src/pcb_odo_interface.cpp
@@ -1,16 +1,25 @@
 #include <modelec_com/pcb_odo_interface.hpp>
 #include <modelec_utils/utils.hpp>
 #include <modelec_interfaces/srv/add_serial_listener.hpp>
+#include <ament_index_cpp/get_package_share_directory.hpp>
+#include <modelec_utils/config.hpp>
 
 namespace Modelec
 {
     PCBOdoInterface::PCBOdoInterface() : Node("pcb_odo_interface")
     {
+        std::string config_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml";
+        if (!Config::load(config_path))
+        {
+            RCLCPP_ERROR(get_logger(), "Failed to load config file: %s", config_path.c_str());
+        }
+
         // Service to create a new serial listener
         auto request = std::make_shared<modelec_interfaces::srv::AddSerialListener::Request>();
-        request->name = "pcb_odo";
-        request->bauds = 115200;
-        request->serial_port = "/dev/pts/10"; // TODO : check the real serial port
+        request->name = Config::get<std::string>("config.usb.pcb.pcb_odo.name", "pcb_odo");
+        request->bauds = Config::get<int>("config.usb.pcb.pcb_odo.baudrate", 115200);
+        request->serial_port = Config::get<std::string>("config.usb.pcb.pcb_odo.port", "/dev/ttyUSB0");
+
         auto client = this->create_client<modelec_interfaces::srv::AddSerialListener>("add_serial_listener");
         while (!client->wait_for_service(std::chrono::seconds(1)))
         {
diff --git a/src/modelec_strat/data/config.xml b/src/modelec_strat/data/config.xml
index 68fe8f0..e480e2d 100644
--- a/src/modelec_strat/data/config.xml
+++ b/src/modelec_strat/data/config.xml
@@ -39,4 +39,18 @@
         <blue x="100" y="1600" theta="-1.5708" />
         <yellow x="2650" y="1600" theta="-1.5708" />
     </spawn>
+    <usb>
+        <pcb>
+            <pcb_odo>
+                <name>pcb_odo</name>
+                <port>/dev/pts/11</port>
+                <baud_rate>115200</baud_rate>
+            </pcb_odo>
+            <pcb_alim>
+                <name>pcb_alim</name>
+                <port>/dev/pts/12</port>
+                <baud_rate>115200</baud_rate>
+            </pcb_alim>
+        </pcb>
+    </usb>
 </config>