From 065820c2e452429647a86fca2d226ae874552a17 Mon Sep 17 00:00:00 2001
From: acki <ackimixs@protonmail.com>
Date: Mon, 7 Apr 2025 16:20:25 -0400
Subject: [PATCH] waypoint service

---
 .../include/modelec/pcb_odo_interface.h       |  9 +++++
 src/modelec/src/pcb_odo_interface.cpp         | 37 +++++++++++++++++++
 src/modelec_interface/CMakeLists.txt          |  1 +
 .../srv/Odometry/OdometryAddWaypoint.srv      |  7 ++++
 4 files changed, 54 insertions(+)
 create mode 100644 src/modelec_interface/srv/Odometry/OdometryAddWaypoint.srv

diff --git a/src/modelec/include/modelec/pcb_odo_interface.h b/src/modelec/include/modelec/pcb_odo_interface.h
index 8921d1b..7a31d79 100644
--- a/src/modelec/include/modelec/pcb_odo_interface.h
+++ b/src/modelec/include/modelec/pcb_odo_interface.h
@@ -22,6 +22,7 @@
 #include <modelec_interface/srv/odometry_start.hpp>
 #include <modelec_interface/srv/odometry_get_pid.hpp>
 #include <modelec_interface/srv/odometry_set_pid.hpp>
+#include <modelec_interface/srv/odometry_add_waypoint.hpp>
 
 namespace Modelec {
 
@@ -73,6 +74,7 @@ private:
     rclcpp::Service<modelec_interface::srv::OdometryStart>::SharedPtr set_start_service_;
     rclcpp::Service<modelec_interface::srv::OdometryGetPid>::SharedPtr get_pid_service_;
     rclcpp::Service<modelec_interface::srv::OdometrySetPid>::SharedPtr set_pid_service_;
+    rclcpp::Service<modelec_interface::srv::OdometryAddWaypoint>::SharedPtr add_waypoint_service_;
 
     // Promises and mutexes to synchronize service responses asynchronously
     std::queue<std::promise<long>> tof_promises_;
@@ -93,6 +95,9 @@ private:
     std::queue<std::promise<bool>> set_pid_promises_;
     std::mutex set_pid_mutex_;
 
+    std::queue<std::promise<bool>> add_waypoint_promises_;
+    std::mutex add_waypoint_mutex_;
+
     // Service handlers using async wait with promises
     void HandleGetTof(const std::shared_ptr<modelec_interface::srv::OdometryToF::Request> request,
                       std::shared_ptr<modelec_interface::srv::OdometryToF::Response> response);
@@ -112,6 +117,9 @@ private:
     void HandleSetPID(const std::shared_ptr<modelec_interface::srv::OdometrySetPid::Request> request,
                            std::shared_ptr<modelec_interface::srv::OdometrySetPid::Response> response);
 
+    void HandleAddWaypoint(const std::shared_ptr<modelec_interface::srv::OdometryAddWaypoint::Request> request,
+                           std::shared_ptr<modelec_interface::srv::OdometryAddWaypoint::Response> response);
+
     // Resolving methods called by subscriber callback
     void ResolveToFRequest(long distance);
     void ResolveSpeedRequest(const OdometryData& speed);
@@ -119,6 +127,7 @@ private:
     void ResolveStartRequest(bool start);
     void ResolveGetPIDRequest(const PIDData& pid);
     void ResolveSetPIDRequest(bool success);
+    void ResolveAddWaypointRequest(bool success);
 
 public:
     void SendToPCB(const std::string &data) const;
diff --git a/src/modelec/src/pcb_odo_interface.cpp b/src/modelec/src/pcb_odo_interface.cpp
index ce8e30d..342599a 100644
--- a/src/modelec/src/pcb_odo_interface.cpp
+++ b/src/modelec/src/pcb_odo_interface.cpp
@@ -117,6 +117,10 @@ namespace Modelec
         set_pid_service_ = create_service<modelec_interface::srv::OdometrySetPid>(
             "odometry/set_pid",
             std::bind(&PCBOdoInterface::HandleSetPID, this, std::placeholders::_1, std::placeholders::_2));
+
+        add_waypoint_service_ = create_service<modelec_interface::srv::OdometryAddWaypoint>(
+            "odometry/add_waypoint",
+            std::bind(&PCBOdoInterface::HandleAddWaypoint, this, std::placeholders::_1, std::placeholders::_2));
     }
 
     PCBOdoInterface::~PCBOdoInterface()
@@ -211,6 +215,11 @@ namespace Modelec
                 bool start = std::stoi(tokens[2]);
                 ResolveStartRequest(start);
             }
+            else if (tokens[1] == "WAYPOINT")
+            {
+                bool success = true;
+                ResolveAddWaypointRequest(success);
+            }
             else
             {
                 RCLCPP_INFO(this->get_logger(), "PCB response: %s", msg->data.c_str());
@@ -346,6 +355,22 @@ namespace Modelec
         response->success = result;
     }
 
+    void PCBOdoInterface::HandleAddWaypoint(
+        const std::shared_ptr<modelec_interface::srv::OdometryAddWaypoint::Request> request,
+        std::shared_ptr<modelec_interface::srv::OdometryAddWaypoint::Response> response)
+    {
+        std::promise<bool> promise;
+        auto future = promise.get_future();
+
+        {
+            std::lock_guard<std::mutex> lock(add_waypoint_mutex_);
+            add_waypoint_promises_.push(std::move(promise));
+        }
+        AddWaypoint(request->id, request->is_end, request->x, request->y, request->theta);
+        bool result = future.get();
+        response->success = result;
+    }
+
     void PCBOdoInterface::ResolveToFRequest(const long distance)
     {
         std::lock_guard<std::mutex> lock(tof_mutex_);
@@ -418,6 +443,18 @@ namespace Modelec
         }
     }
 
+    void PCBOdoInterface::ResolveAddWaypointRequest(bool success)
+    {
+        std::lock_guard<std::mutex> lock(add_waypoint_mutex_);
+        if (!add_waypoint_promises_.empty()) {
+            auto promise = std::move(add_waypoint_promises_.front());
+            add_waypoint_promises_.pop();
+            promise.set_value(success);
+        } else {
+            RCLCPP_DEBUG(get_logger(), "No pending Add Waypoint request to resolve.");
+        }
+    }
+
     void PCBOdoInterface::SendToPCB(const std::string& data) const
     {
         auto message = std_msgs::msg::String();
diff --git a/src/modelec_interface/CMakeLists.txt b/src/modelec_interface/CMakeLists.txt
index f1fb66d..c76da7b 100644
--- a/src/modelec_interface/CMakeLists.txt
+++ b/src/modelec_interface/CMakeLists.txt
@@ -27,6 +27,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/Odometry/OdometrySpeed.srv"
   "srv/Odometry/OdometryToF.srv"
   "srv/Odometry/OdometryStart.srv"
+  "srv/Odometry/OdometryAddWaypoint.srv"
   "srv/Odometry/PID/OdometryGetPid.srv"
   "srv/Odometry/PID/OdometrySetPid.srv"
   "srv/AddServoMotor.srv"
diff --git a/src/modelec_interface/srv/Odometry/OdometryAddWaypoint.srv b/src/modelec_interface/srv/Odometry/OdometryAddWaypoint.srv
new file mode 100644
index 0000000..e24f578
--- /dev/null
+++ b/src/modelec_interface/srv/Odometry/OdometryAddWaypoint.srv
@@ -0,0 +1,7 @@
+int8 id
+bool is_end
+int64 x
+int64 y
+int64 theta
+---
+bool success
\ No newline at end of file