From 93f6c2ad3e4919c6d91ac8269b4ed237b8c59368 Mon Sep 17 00:00:00 2001
From: acki <ackimixs@protonmail.com>
Date: Wed, 17 Dec 2025 17:27:44 +0100
Subject: [PATCH] joy

---
 src/modelec_com/CMakeLists.txt                |  3 +-
 .../include/modelec_com/pcb_odo_interface.hpp | 18 ++++--------
 src/modelec_com/src/pcb_odo_interface.cpp     | 29 +++++++++++++++++++
 3 files changed, 36 insertions(+), 14 deletions(-)

diff --git a/src/modelec_com/CMakeLists.txt b/src/modelec_com/CMakeLists.txt
index 2e65e3d..5e16b00 100644
--- a/src/modelec_com/CMakeLists.txt
+++ b/src/modelec_com/CMakeLists.txt
@@ -9,6 +9,7 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
 find_package(Boost REQUIRED COMPONENTS system)
 find_package(ament_index_cpp REQUIRED)
 find_package(fmt)
@@ -18,7 +19,7 @@ find_package(modelec_utils REQUIRED)
 
 # PCB Odometry Node
 add_executable(pcb_odo_interface src/pcb_odo_interface.cpp src/serial_listener.cpp)
-ament_target_dependencies(pcb_odo_interface rclcpp std_msgs modelec_interfaces ament_index_cpp)
+ament_target_dependencies(pcb_odo_interface rclcpp std_msgs sensor_msgs modelec_interfaces ament_index_cpp)
 target_link_libraries(pcb_odo_interface Boost::system modelec_utils::utils modelec_utils::config)
 target_include_directories(pcb_odo_interface PUBLIC
         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
diff --git a/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp b/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp
index 213380d..2622810 100644
--- a/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp
+++ b/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp
@@ -4,12 +4,8 @@
 
 #include <rclcpp/rclcpp.hpp>
 
-#include <queue>
-#include <mutex>
 #include <future>
 
-#include <std_msgs/msg/string.hpp>
-
 #include <modelec_interfaces/msg/odometry_pos.hpp>
 #include <modelec_interfaces/msg/odometry_speed.hpp>
 #include <modelec_interfaces/msg/odometry_to_f.hpp>
@@ -18,18 +14,11 @@
 #include <modelec_interfaces/msg/odometry_start.hpp>
 #include <modelec_interfaces/msg/odometry_pid.hpp>
 
-#include <modelec_interfaces/srv/odometry_position.hpp>
-#include <modelec_interfaces/srv/odometry_speed.hpp>
-#include <modelec_interfaces/srv/odometry_to_f.hpp>
-#include <modelec_interfaces/srv/add_serial_listener.hpp>
-#include <modelec_interfaces/srv/odometry_start.hpp>
-#include <modelec_interfaces/srv/odometry_get_pid.hpp>
-#include <modelec_interfaces/srv/odometry_set_pid.hpp>
-#include <modelec_interfaces/srv/odometry_add_waypoint.hpp>
-
 #include <std_msgs/msg/bool.hpp>
 #include <std_msgs/msg/empty.hpp>
 
+#include <sensor_msgs/msg/joy.hpp>
+
 namespace Modelec
 {
     class PCBOdoInterface : public rclcpp::Node, public SerialListener
@@ -67,6 +56,7 @@ namespace Modelec
         rclcpp::Subscription<modelec_interfaces::msg::OdometryWaypoints>::SharedPtr odo_add_waypoints_subscriber_;
         rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr odo_set_pos_subscriber_;
         rclcpp::Subscription<modelec_interfaces::msg::OdometryPid>::SharedPtr odo_set_pid_subscriber_;
+        rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr cmd_vel_subscriber_;
 
         void AddWaypointCallback(const modelec_interfaces::msg::OdometryWaypoint::SharedPtr msg);
         void AddWaypointsCallback(const modelec_interfaces::msg::OdometryWaypoints::SharedPtr msg);
@@ -104,5 +94,7 @@ namespace Modelec
         void GetPID();
         void SetPID(const modelec_interfaces::msg::OdometryPid::SharedPtr msg);
         void SetPID(std::string name, float p, float i, float d, std::optional<float> min = std::nullopt, std::optional<float> max = std::nullopt);
+
+        void SetMotor(int left, int right);
     };
 } // namespace Modelec
diff --git a/src/modelec_com/src/pcb_odo_interface.cpp b/src/modelec_com/src/pcb_odo_interface.cpp
index 8345e7d..50a9e3b 100644
--- a/src/modelec_com/src/pcb_odo_interface.cpp
+++ b/src/modelec_com/src/pcb_odo_interface.cpp
@@ -69,6 +69,25 @@ namespace Modelec
                 SetPIDCallback(msg);
             });
 
+        cmd_vel_subscriber_ = this->create_subscription<sensor_msgs::msg::Joy>(
+            "cmd_vel", 30,
+            [this](const sensor_msgs::msg::Joy::SharedPtr msg)
+            {
+                double forward = msg->axes[1];
+                double turn = msg->axes[3];
+
+                if (fabs(forward) < 0.05) forward = 0.0;
+                if (fabs(turn) < 0.05) turn = 0.0;
+
+                int left  = static_cast<int>(forward * 626 - turn * 626);
+                int right = static_cast<int>(forward * 626 + turn * 626);
+
+                left  = std::max(-626, std::min(626, left));
+                right = std::max(-626, std::min(626, right));
+
+                SetMotor(left, right);
+            });
+
         start_odo_sub_ = this->create_subscription<std_msgs::msg::Bool>(
             "odometry/start", 10,
             [this](const std_msgs::msg::Bool::SharedPtr msg)
@@ -381,6 +400,16 @@ namespace Modelec
 
         SendOrder("PID", data);
     }
+
+    void PCBOdoInterface::SetMotor(int left, int right)
+    {
+        std::vector<std::string> data = {
+            std::to_string(left),
+            std::to_string(right)
+        };
+
+        SendOrder("MOTOR", data);
+    }
 } // Modelec
 
 #ifndef MODELEC_COM_TESTING