PCB Odometry interface (hope that work :) )

TODO.md

@@ -0,0 +1 @@
+- rework position system with that https://docs.ros2.org/latest/api/geometry_msgs/index-msg.html

src/modelec/CMakeLists.txt

@@ -31,9 +31,9 @@ target_include_directories(serial_listener PUBLIC
 )
 
 # USB Arduino Logic Processor Node
-add_executable(odometry_logic_processor src/odometry_logic_processor.cpp)
-ament_target_dependencies(odometry_logic_processor rclcpp std_msgs modelec_interface)
-target_include_directories(odometry_logic_processor PUBLIC
+add_executable(pcb_odo_interface src/pcb_odo_interface.cpp)
+ament_target_dependencies(pcb_odo_interface rclcpp std_msgs modelec_interface)
+target_include_directories(pcb_odo_interface PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>
 )
@@ -119,7 +119,7 @@ endif()
 # Install targets
 install(TARGETS
   serial_listener
-  odometry_logic_processor
+  pcb_odo_interface
   pca9685_controller
   gamecontroller_listener
   move_controller

src/modelec/include/modelec/odometry_logic_processor.hpp

@@ -1,19 +0,0 @@
-#pragma once
-
-#include <rclcpp/rclcpp.hpp>
-#include <std_msgs/msg/string.hpp>
-#include <modelec/utils.hpp>
-#include <modelec_interface/msg/odometry_data.hpp>
-
-namespace Modelec {
-  class odometry_logic_processor : public rclcpp::Node {
-  public:
-    odometry_logic_processor();
-  private:
-    void processData(const std::string &data);
-
-    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscriber_;
-    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_to_odometry_;
-    rclcpp::Publisher<modelec_interface::msg::OdometryData>::SharedPtr publisher_odometry_;
-  };
-}

src/modelec/include/modelec/pcb_alim_interface.hpp

@@ -18,10 +18,11 @@ private:
     void PCBCallback(const std_msgs::msg::String::SharedPtr msg);
 
     void SendToPCB(const std::string &data);
+    void SendToPCB(const std::string& order, const std::string& elem, const std::string& data, const std::string& value = std::string());
 
-    void GetData(const std::string &elem, const std::string& value);
+    void GetData(const std::string &elem, const std::string& data, const std::string& value = std::string());
 
-    void SendOrder(const std::string &elem, const std::string& data, const std::string& value);
+    void SendOrder(const std::string &elem, const std::string& data, const std::string& value = std::string());
 
     // get data
     void GetEmergencyStopButtonState();

src/modelec/include/modelec/pcb_odo_interface.h

@@ -0,0 +1,100 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <queue>
+#include <future>
+#include <mutex>
+
+#include <std_msgs/msg/string.hpp>
+#include <modelec_interface/msg/odometry_pos.hpp>
+#include <modelec_interface/msg/odometry_speed.hpp>
+#include <modelec_interface/msg/odometry_to_f.hpp>
+#include <modelec_interface/msg/odometry_waypoint_reach.hpp>
+#include <modelec_interface/msg/odometry_add_waypoint.hpp>
+
+#include <modelec_interface/srv/odometry_position.hpp>
+#include <modelec_interface/srv/odometry_speed.hpp>
+#include <modelec_interface/srv/odometry_to_f.hpp>
+
+namespace Modelec {
+
+class PCBOdoInterface : public rclcpp::Node {
+public:
+    PCBOdoInterface();
+    ~PCBOdoInterface() override = default;
+
+    struct OdometryData {
+        long x;
+        long y;
+        long theta;
+    };
+
+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);
+
+    rclcpp::Publisher<modelec_interface::msg::OdometryPos>::SharedPtr odo_pos_publisher_;
+    rclcpp::Publisher<modelec_interface::msg::OdometrySpeed>::SharedPtr odo_speed_publisher_;
+    rclcpp::Publisher<modelec_interface::msg::OdometryToF>::SharedPtr odo_tof_publisher_;
+    rclcpp::Publisher<modelec_interface::msg::OdometryWaypointReach>::SharedPtr odo_waypoint_reach_publisher_;
+
+    rclcpp::Subscription<modelec_interface::msg::OdometryAddWaypoint>::SharedPtr odo_add_waypoint_subscriber_;
+
+    void AddWaypointCallback(const modelec_interface::msg::OdometryAddWaypoint::SharedPtr msg) const;
+
+    rclcpp::Subscription<modelec_interface::msg::OdometryPos>::SharedPtr odo_set_pos_subscriber_;
+
+    void SetPosCallback(const modelec_interface::msg::OdometryPos::SharedPtr msg) const;
+
+    rclcpp::Service<modelec_interface::srv::OdometryToF>::SharedPtr get_tof_service_;
+    rclcpp::Service<modelec_interface::srv::OdometrySpeed>::SharedPtr get_speed_service_;
+    rclcpp::Service<modelec_interface::srv::OdometryPosition>::SharedPtr get_position_service_;
+
+    std::queue<std::promise<long>> tof_promises_;
+    std::mutex tof_mutex_;
+
+    std::queue<std::promise<OdometryData>> speed_promises_;
+    std::mutex speed_mutex_;
+
+    std::queue<std::promise<OdometryData>> pos_promises_;
+    std::mutex pos_mutex_;
+
+    void HandleGetTof(const std::shared_ptr<modelec_interface::srv::OdometryToF::Request> request,
+                      std::shared_ptr<modelec_interface::srv::OdometryToF::Response> response);
+
+    void HandleGetSpeed(const std::shared_ptr<modelec_interface::srv::OdometrySpeed::Request> request,
+                        std::shared_ptr<modelec_interface::srv::OdometrySpeed::Response> response);
+
+    void HandleGetPosition(const std::shared_ptr<modelec_interface::srv::OdometryPosition::Request> request,
+                        std::shared_ptr<modelec_interface::srv::OdometryPosition::Response> response);
+
+    void ResolveToFRequest(long distance);
+    void ResolveSpeedRequest(const OdometryData& speed);
+    void ResolvePositionRequest(const OdometryData& position);
+
+public:
+    void SendToPCB(const std::string &data) const;
+    void SendToPCB(const std::string& order, const std::string& elem, const std::vector<std::string>& data = {}) const;
+
+    void GetData(const std::string& elem, const std::vector<std::string>& data = {}) const;
+
+    void SendOrder(const std::string &elem, const std::vector<std::string> &data = {}) const;
+
+    void GetPos() const;
+
+    void GetSpeed() const;
+
+    void GetToF(const int &tof) const;
+
+    void SetRobotPos(const modelec_interface::msg::OdometryPos::SharedPtr msg) const;
+    void SetRobotPos(long x, long y, long theta) const;
+
+    void AddWaypoint(modelec_interface::msg::OdometryAddWaypoint::SharedPtr msg) const;
+    void AddWaypoint(int index, bool IsStopPoint, long x, long y, long theta) const;
+
+};
+
+} // Modelec

src/modelec/launch/modelec.launch.yml

@@ -1,14 +1,14 @@
 launch:
 
 - node:
-    pkg: modelec
+    pkg: 'modelec'
     exec: "serial_listener"
     name: "serial_listener"
 
 - node:
     pkg: 'modelec'
-    exec: 'usb_arduino_logic_processor'
-    name: 'usb_arduino_logic_processor'
+    exec: 'pcb_odo_interface'
+    name: 'pcb_odo_interface'
 
 - node:
     pkg: 'modelec'

src/modelec/launch/modelec_gamecontroller.launch.yml

@@ -1,14 +1,14 @@
 launch:
 
 - node:
-    pkg: modelec
+    pkg: 'modelec'
     exec: "serial_listener"
     name: "serial_listener"
 
 - node:
-    pkg: 'modelec'
-    exec: 'usb_arduino_logic_processor'
-    name: 'usb_arduino_logic_processor'
+      pkg: 'modelec'
+      exec: 'pcb_odo_interface'
+      name: 'pcb_odo_interface'
 
 - node:
     pkg: 'modelec'

src/modelec/launch/modelec_gamecontroller_remote_server.launch.yml

@@ -1,14 +1,14 @@
 launch:
 
 - node:
-    pkg: modelec
+    pkg: 'modelec'
     exec: "serial_listener"
     name: "serial_listener"
 
 - node:
-    pkg: 'modelec'
-    exec: 'usb_arduino_logic_processor'
-    name: 'usb_arduino_logic_processor'
+      pkg: 'modelec'
+      exec: 'pcb_odo_interface'
+      name: 'pcb_odo_interface'
 
 - node:
     pkg: 'modelec'

src/modelec/launch/test.modelec.launch.yml

@@ -1,7 +1,7 @@
 launch:
 
 - node:
-    pkg: modelec
+    pkg: 'modelec'
     exec: "serial_listener"
     name: "serial_listener"
 
@@ -9,3 +9,8 @@ launch:
     pkg: 'modelec'
     exec: 'usb_arduino_logic_processor'
     name: 'usb_arduino_logic_processor'
+
+- node:
+      pkg: 'modelec'
+      exec: 'pcb_odo_interface'
+      name: 'pcb_odo_interface'

src/modelec/src/odometry_logic_processor.cpp

@@ -1,72 +0,0 @@
-#include "modelec/odometry_logic_processor.hpp"
-#include "modelec_interface/srv/add_serial_listener.hpp"
-#include <vector>
-
-Modelec::odometry_logic_processor::odometry_logic_processor() : Node("usb_logic_processor") {
-  publisher_odometry_ = this->create_publisher<modelec_interface::msg::OdometryData>("odomertry_data", 10);
-
-  // Service to create a new serial listener
-  auto request = std::make_shared<modelec_interface::srv::AddSerialListener::Request>();
-  request->name = "odometry";
-  request->bauds = 115200;
-  request->serial_port = "/dev/pts/6";
-  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 (auto res = result.get()) {
-      if (res->success) {
-
-        RCLCPP_INFO(this->get_logger(), "Publisher: %s", res->publisher.c_str());
-        RCLCPP_INFO(this->get_logger(), "Subscriber: %s", res->subscriber.c_str());
-
-        subscriber_ = this->create_subscription<std_msgs::msg::String>(
-          res->publisher, 10, [this](std_msgs::msg::String::SharedPtr msg) {
-              processData(msg->data);
-            });
-      
-        publisher_to_odometry_ = this->create_publisher<std_msgs::msg::String>(res->subscriber, 10);
-      } else {
-        RCLCPP_ERROR(this->get_logger(), "Failed to add serial listener");
-      }
-    } else {
-      RCLCPP_ERROR(this->get_logger(), "Failed to ask for a serial listener");
-    }
-  } else {
-    RCLCPP_ERROR(this->get_logger(), "Service call failed");
-  }
-}
-
-void Modelec::odometry_logic_processor::processData(const std::string &data) {
-  auto processed_data = "Processed: " + data;
-  RCLCPP_INFO(this->get_logger(), "%s", processed_data.c_str());
-  auto msg = modelec_interface::msg::OdometryData();
-  std::vector<std::string> d = split(data, ':');
-
-  if (d.size() == 3) {
-    msg.x = std::stol(d[0]);
-    msg.y = std::stol(d[1]);
-    msg.theta = std::stol(d[2]);
-  }
-  else {
-    msg.x = 0;
-    msg.y = 0;
-    msg.theta = 0;
-  }
-
-  publisher_odometry_->publish(msg);
-}
-
-int main(int argc, char ** argv)
-{
-  rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<Modelec::odometry_logic_processor>());
-  rclcpp::shutdown();
-  return 0;
-}

src/modelec/src/pcb_alim_interface.cpp

@@ -11,28 +11,37 @@ namespace Modelec
         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;
+        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()->success) {
-            RCLCPP_ERROR(this->get_logger(), "Failed to add serial listener");
-            } else {
-            RCLCPP_INFO(this->get_logger(), "Added serial listener");
+        if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) ==
+            rclcpp::FutureReturnCode::SUCCESS)
+        {
+            if (!result.get()->success)
+            {
+                RCLCPP_ERROR(this->get_logger(), "Failed to add serial listener");
             }
-        } else {
+            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));
+            std::bind(&PCBAlimInterface::PCBCallback, this, std::placeholders::_1));
     }
 
     void PCBAlimInterface::PCBCallback(const std_msgs::msg::String::SharedPtr msg)
@@ -40,21 +49,36 @@ namespace Modelec
         RCLCPP_INFO(this->get_logger(), "Received message: '%s'", msg->data.c_str());
     }
 
-    void PCBAlimInterface::SendToPCB(const std::string &data)
+    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)
+    void PCBAlimInterface::SendToPCB(const std::string& order, const std::string& elem, const std::string& data,
+        const std::string& value)
     {
-        SendToPCB("GET;" + elem + ";" + value);
+        std::string command = order + ";" + elem;
+        if (!data.empty())
+        {
+            command += ";" + data;
+        }
+        if (!value.empty())
+        {
+            command += ";" + value;
+        }
+        SendToPCB(command);
     }
 
-    void PCBAlimInterface::SendOrder(const std::string &elem, const std::string& data, const std::string& value)
+    void PCBAlimInterface::GetData(const std::string& elem, const std::string& data, const std::string& value)
     {
-        SendToPCB("SET;" + elem + ";" + data + ";" + value);
+        SendToPCB("GET", elem, data, value);
+    }
+
+    void PCBAlimInterface::SendOrder(const std::string& elem, const std::string& data, const std::string& value)
+    {
+        SendToPCB("SET", elem, data, value);
     }
 
     void PCBAlimInterface::GetEmergencyStopButtonState()
@@ -62,98 +86,118 @@ namespace Modelec
         GetData("BAU", "STATE");
     }
 
-    void PCBAlimInterface::GetEntryVoltage(int entry) {
-        GetData("IN" +  std::to_string(entry), "VOLT");
+    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::GetEntryCurrent(int entry)
+    {
+        GetData("IN" + std::to_string(entry), "AMP");
     }
 
-    void PCBAlimInterface::GetEntryState(int entry) {
-        GetData("IN" +  std::to_string(entry), "STATE");
+    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::GetEntryIsValide(int entry)
+    {
+        GetData("IN" + std::to_string(entry), "VALID");
     }
 
-    void PCBAlimInterface::GetPCBTemperature() {
+    void PCBAlimInterface::GetPCBTemperature()
+    {
         GetData("TEMP", "CELS");
     }
 
-    
-    void PCBAlimInterface::GetOutput5VState() {
+
+    void PCBAlimInterface::GetOutput5VState()
+    {
         GetData("OUT5V", "STATE");
     }
-    
-    void PCBAlimInterface::GetOutput5VVoltage() {
+
+    void PCBAlimInterface::GetOutput5VVoltage()
+    {
         GetData("OUT5V", "VOLT");
     }
-    
-    void PCBAlimInterface::GetOutput5VCurrent() {
+
+    void PCBAlimInterface::GetOutput5VCurrent()
+    {
         GetData("OUT5V", "AMP");
     }
 
 
-    void PCBAlimInterface::GetOutput5V1State() {
+    void PCBAlimInterface::GetOutput5V1State()
+    {
         GetData("OUT5V1", "STATE");
     }
-    
-    void PCBAlimInterface::GetOutput5V1Voltage() {
+
+    void PCBAlimInterface::GetOutput5V1Voltage()
+    {
         GetData("OUT5V1", "VOLT");
     }
-    
-    void PCBAlimInterface::GetOutput5V1Current() {
+
+    void PCBAlimInterface::GetOutput5V1Current()
+    {
         GetData("OUT5V1", "AMP");
     }
 
 
-    
-    void PCBAlimInterface::GetOutput12VState() {
+    void PCBAlimInterface::GetOutput12VState()
+    {
         GetData("OUT12V", "STATE");
     }
-    
-    void PCBAlimInterface::GetOutput12VVoltage() {
+
+    void PCBAlimInterface::GetOutput12VVoltage()
+    {
         GetData("OUT12V", "VOLT");
     }
-    
-    void PCBAlimInterface::GetOutput12VCurrent() {
+
+    void PCBAlimInterface::GetOutput12VCurrent()
+    {
         GetData("OUT12V", "AMP");
     }
 
 
-    void PCBAlimInterface::GetOutput24VState() {
+    void PCBAlimInterface::GetOutput24VState()
+    {
         GetData("OUT24V", "STATE");
     }
-    
-    void PCBAlimInterface::GetOutput24VVoltage() {
+
+    void PCBAlimInterface::GetOutput24VVoltage()
+    {
         GetData("OUT24V", "VOLT");
     }
-    
-    void PCBAlimInterface::GetOutput24VCurrent() {
+
+    void PCBAlimInterface::GetOutput24VCurrent()
+    {
         GetData("OUT24V", "AMP");
     }
 
 
-    void PCBAlimInterface::SetSoftwareEmergencyStop(bool state) {
+    void PCBAlimInterface::SetSoftwareEmergencyStop(bool state)
+    {
         SendOrder("EMG", "STATE", state ? "1" : "0");
     }
-    
-    void PCBAlimInterface::Set5VState(bool state) {
+
+    void PCBAlimInterface::Set5VState(bool state)
+    {
         SendOrder("OUT5V", "STATE", state ? "1" : "0");
     }
-    
-    void PCBAlimInterface::Set12VState(bool state) {
+
+    void PCBAlimInterface::Set12VState(bool state)
+    {
         SendOrder("OUT12V", "STATE", state ? "1" : "0");
     }
-    
-    void PCBAlimInterface::Set24VState(bool state) {
+
+    void PCBAlimInterface::Set24VState(bool state)
+    {
         SendOrder("OUT24V", "STATE", state ? "1" : "0");
     }
 } // namespace Modelec
 
-int main(int argc, char **argv)
+int main(int argc, char** argv)
 {
     rclcpp::init(argc, argv);
     rclcpp::spin(std::make_shared<Modelec::PCBAlimInterface>());

src/modelec/src/pcb_odo_interface.cpp

@@ -0,0 +1,348 @@
+#include <modelec/pcb_odo_interface.h>
+#include <modelec/utils.hpp>
+#include <modelec_interface/srv/add_serial_listener.hpp>
+
+namespace Modelec
+{
+    PCBOdoInterface::PCBOdoInterface() : Node("pcb_odo_interface")
+    {
+        // Service to create a new serial listener
+        auto request = std::make_shared<modelec_interface::srv::AddSerialListener::Request>();
+        request->name = "pcb_odo";
+        request->bauds = 115200;
+        request->serial_port = "/dev/serial1"; // 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()->success)
+            {
+                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(&PCBOdoInterface::PCBCallback, this, std::placeholders::_1));
+
+        odo_pos_publisher_ = this->create_publisher<modelec_interface::msg::OdometryPos>(
+            "odometry/position", 10);
+
+        odo_speed_publisher_ = this->create_publisher<modelec_interface::msg::OdometrySpeed>(
+            "odometry/speed", 10);
+
+        odo_tof_publisher_ = this->create_publisher<modelec_interface::msg::OdometryToF>(
+            "odometry/tof", 10);
+
+        odo_waypoint_reach_publisher_ = this->create_publisher<modelec_interface::msg::OdometryWaypointReach>(
+            "odometry/waypoint-reach", 10);
+
+        odo_add_waypoint_subscriber_ = this->create_subscription<modelec_interface::msg::OdometryAddWaypoint>(
+            "odometry/add-waypoint", 10,
+            std::bind(&PCBOdoInterface::AddWaypointCallback, this, std::placeholders::_1));
+
+        odo_set_pos_subscriber_ = this->create_subscription<modelec_interface::msg::OdometryPos>(
+            "odometry/set-position", 10,
+            std::bind(&PCBOdoInterface::SetPosCallback, this, std::placeholders::_1));
+
+        // Services
+        get_tof_service_ = create_service<modelec_interface::srv::OdometryToF>(
+            "odometry/tof",
+            std::bind(&PCBOdoInterface::HandleGetTof, this, std::placeholders::_1, std::placeholders::_2));
+
+        get_speed_service_ = create_service<modelec_interface::srv::OdometrySpeed>(
+            "odometry/speed",
+            std::bind(&PCBOdoInterface::HandleGetSpeed, this, std::placeholders::_1, std::placeholders::_2));
+
+        get_position_service_ = create_service<modelec_interface::srv::OdometryPosition>(
+            "odometry/position",
+            std::bind(&PCBOdoInterface::HandleGetPosition, this, std::placeholders::_1, std::placeholders::_2));
+    }
+
+    void PCBOdoInterface::PCBCallback(const std_msgs::msg::String::SharedPtr msg)
+    {
+        std::vector<std::string> tokens = split(msg->data, ';');
+        if (tokens.size() < 2)
+        {
+            RCLCPP_ERROR(this->get_logger(), "Invalid message format: %s", msg->data.c_str());
+            return;
+        }
+
+        if (tokens[0] == "SET")
+        {
+            if (tokens[1] == "POS")
+            {
+                long x = std::stol(tokens[2]);
+                long y = std::stol(tokens[3]);
+                long theta = std::stol(tokens[4]);
+
+                auto message = modelec_interface::msg::OdometryPos();
+                message.x = x;
+                message.y = y;
+                message.theta = theta;
+
+                odo_pos_publisher_->publish(message);
+                ResolvePositionRequest({ x, y, theta });
+            }
+            else if (tokens[1] == "SPEED")
+            {
+                long x = std::stol(tokens[2]);
+                long y = std::stol(tokens[3]);
+                long theta = std::stol(tokens[4]);
+
+                auto message = modelec_interface::msg::OdometrySpeed();
+                message.x = x;
+                message.y = y;
+                message.theta = theta;
+
+                odo_speed_publisher_->publish(message);
+                ResolveSpeedRequest({ x, y, theta });
+            }
+            else if (tokens[1] == "DIST")
+            {
+                int n = std::stoi(tokens[2]);
+                long dist = std::stol(tokens[3]);
+
+                auto message = modelec_interface::msg::OdometryToF();
+                message.n = n;
+                message.distance = dist;
+
+                odo_tof_publisher_->publish(message);
+                ResolveToFRequest(dist);
+            }
+            else if (tokens[1] == "WAYPOINT")
+            {
+                int id = std::stoi(tokens[2]);
+
+                auto message = modelec_interface::msg::OdometryWaypointReach();
+                message.id = id;
+
+                odo_waypoint_reach_publisher_->publish(message);
+            }
+        }
+        else if (tokens[0] == "OK")
+        {
+            RCLCPP_INFO(this->get_logger(), "PCB response: %s", msg->data.c_str());
+        }
+        else if (tokens[0] == "KO")
+        {
+            RCLCPP_ERROR(this->get_logger(), "PCB error: %s", msg->data.c_str());
+        }
+    }
+
+    void PCBOdoInterface::AddWaypointCallback(const modelec_interface::msg::OdometryAddWaypoint::SharedPtr msg) const
+    {
+        AddWaypoint(msg);
+    }
+
+    void PCBOdoInterface::SetPosCallback(const modelec_interface::msg::OdometryPos::SharedPtr msg) const
+    {
+        SetRobotPos(msg);
+    }
+
+    void PCBOdoInterface::HandleGetTof(const std::shared_ptr<modelec_interface::srv::OdometryToF::Request> request,
+                                       std::shared_ptr<modelec_interface::srv::OdometryToF::Response> response)
+    {
+        std::promise<long> promise;
+        auto future = promise.get_future();
+        {
+            std::lock_guard<std::mutex> lock(tof_mutex_);
+            tof_promises_.push(std::move(promise));
+        }
+
+        GetToF(request->n);
+
+        response->distance = future.get();
+    }
+
+    void PCBOdoInterface::HandleGetSpeed(const std::shared_ptr<modelec_interface::srv::OdometrySpeed::Request>,
+        std::shared_ptr<modelec_interface::srv::OdometrySpeed::Response> response)
+    {
+        std::promise<OdometryData> promise;
+        auto future = promise.get_future();
+        {
+            std::lock_guard<std::mutex> lock(speed_mutex_);
+            speed_promises_.push(std::move(promise));
+        }
+
+        GetSpeed();
+
+        OdometryData result = future.get();
+
+        response->x = result.x;
+        response->y = result.y;
+        response->theta = result.theta;
+    }
+
+    void PCBOdoInterface::HandleGetPosition(
+        const std::shared_ptr<modelec_interface::srv::OdometryPosition::Request>,
+        std::shared_ptr<modelec_interface::srv::OdometryPosition::Response> response)
+    {
+        std::promise<OdometryData> promise;
+        auto future = promise.get_future();
+        {
+            std::lock_guard<std::mutex> lock(pos_mutex_);
+            pos_promises_.push(std::move(promise));
+        }
+
+        GetPos();
+
+        OdometryData result = future.get();
+
+        response->x = result.x;
+        response->y = result.y;
+        response->theta = result.theta;
+    }
+
+    void PCBOdoInterface::ResolveToFRequest(const long distance)
+    {
+        std::lock_guard<std::mutex> lock(tof_mutex_);
+        if (!tof_promises_.empty())
+        {
+            std::promise<long> promise = std::move(tof_promises_.front());
+            tof_promises_.pop();
+            promise.set_value(distance);
+        }
+        else
+        {
+            RCLCPP_WARN(this->get_logger(), "Received ToF response but no promise waiting");
+        }
+    }
+
+    void PCBOdoInterface::ResolveSpeedRequest(const OdometryData& speed)
+    {
+        std::lock_guard<std::mutex> lock(speed_mutex_);
+        if (!speed_promises_.empty())
+        {
+            std::promise<OdometryData> promise = std::move(speed_promises_.front());
+            speed_promises_.pop();
+            promise.set_value(speed);
+        }
+        else
+        {
+            RCLCPP_WARN(this->get_logger(), "Received Speed response but no promise waiting");
+        }
+    }
+
+    void PCBOdoInterface::ResolvePositionRequest(const OdometryData& position)
+    {
+        std::lock_guard<std::mutex> lock(pos_mutex_);
+        if (!pos_promises_.empty())
+        {
+            std::promise<OdometryData> promise = std::move(pos_promises_.front());
+            pos_promises_.pop();
+            promise.set_value(position);
+        }
+        else
+        {
+            RCLCPP_WARN(this->get_logger(), "Received Position response but no promise waiting");
+        }
+    }
+
+    void PCBOdoInterface::SendToPCB(const std::string& data) const
+    {
+        auto message = std_msgs::msg::String();
+        message.data = data;
+        pcb_publisher_->publish(message);
+    }
+
+    void PCBOdoInterface::SendToPCB(const std::string& order, const std::string& elem,
+        const std::vector<std::string>& data) const
+    {
+        std::string command = order + ";" + elem;
+        for (const auto& d : data)
+        {
+            command += ";" + d;
+        }
+
+        SendToPCB(command);
+    }
+
+    void PCBOdoInterface::GetData(const std::string& elem, const std::vector<std::string>& data) const
+    {
+        SendToPCB("GET", elem, data);
+    }
+
+    void PCBOdoInterface::SendOrder(const std::string& elem, const std::vector<std::string>& data) const
+    {
+        SendToPCB("SET", elem, data);
+    }
+
+    void PCBOdoInterface::GetPos() const
+    {
+        GetData("POS");
+    }
+
+    void PCBOdoInterface::GetSpeed() const
+    {
+        GetData("SPEED");
+    }
+
+    void PCBOdoInterface::GetToF(const int& tof) const
+    {
+        GetData("DIST", { std::to_string(tof) });
+    }
+
+    void PCBOdoInterface::SetRobotPos(const modelec_interface::msg::OdometryPos::SharedPtr msg) const
+    {
+        SetRobotPos(msg->x, msg->y, msg->theta);
+    }
+
+    void PCBOdoInterface::SetRobotPos(const long x, const long y, const long theta) const
+    {
+        std::vector<std::string> data = {
+            std::to_string(x),
+            std::to_string(y),
+            std::to_string(theta)
+        };
+
+        SendOrder("POS", data);
+    }
+
+    void PCBOdoInterface::AddWaypoint(
+        const modelec_interface::msg::OdometryAddWaypoint::SharedPtr msg) const
+    {
+        AddWaypoint(msg->id, msg->is_end, msg->x, msg->y, msg->theta);
+    }
+
+    void PCBOdoInterface::AddWaypoint(const int index, const bool IsStopPoint, const long x, const long y, const long theta) const
+    {
+        std::vector<std::string> data = {
+            std::to_string(index),
+            std::to_string(IsStopPoint),
+            std::to_string(x),
+            std::to_string(y),
+            std::to_string(theta)
+        };
+
+        SendOrder("WAYPOINT", data);
+    }
+} // Modelec
+
+
+int main(int argc, char** argv)
+{
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<Modelec::PCBOdoInterface>());
+    rclcpp::shutdown();
+    return 0;
+}

src/modelec_interface/CMakeLists.txt

@@ -11,11 +11,18 @@ find_package(std_msgs REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
-  "msg/OdometryData.msg"
+  "msg/Odometry/OdometryPos.msg"
+  "msg/Odometry/OdometrySpeed.msg"
+  "msg/Odometry/OdometryToF.msg"
+  "msg/Odometry/OdometryWaypointReach.msg"
+  "msg/Odometry/OdometryAddWaypoint.msg"
   "msg/PCA9685Servo.msg"
   "msg/ServoMode.msg"
   "msg/Solenoid.msg"
   "msg/Button.msg"
+  "srv/Odometry/OdometryPosition.srv"
+  "srv/Odometry/OdometrySpeed.srv"
+  "srv/Odometry/OdometryToF.srv"
   "srv/AddServoMotor.srv"
   "srv/AddSolenoid.srv"
   "srv/Tirette.srv"

src/modelec_interface/msg/Odometry/OdometryAddWaypoint.msg

@@ -1,3 +1,5 @@
+int8 id
+bool is_end
 int64 x
 int64 y
 int64 theta

src/modelec_interface/msg/Odometry/OdometryPos.msg

@@ -0,0 +1,3 @@
+int64 x
+int64 y
+int64 theta

src/modelec_interface/msg/Odometry/OdometrySpeed.msg

@@ -0,0 +1,3 @@
+int64 x
+int64 y
+int64 theta

src/modelec_interface/msg/Odometry/OdometryToF.msg

@@ -0,0 +1,2 @@
+int8 n
+int64 distance

src/modelec_interface/msg/Odometry/OdometryWaypointReach.msg

@@ -0,0 +1 @@
+int8 id

src/modelec_interface/srv/Odometry/OdometryPosition.srv

@@ -0,0 +1,4 @@
+---
+int64 x
+int64 y
+int64 theta

src/modelec_interface/srv/Odometry/OdometrySpeed.srv

@@ -0,0 +1,4 @@
+---
+int64 x
+int64 y
+int64 theta

src/modelec_interface/srv/Odometry/OdometryToF.srv

@@ -0,0 +1,3 @@
+int8 n
+---
+int64 distance