From ac4c6c28161317aa1a0741c432924feb515c41ec Mon Sep 17 00:00:00 2001
From: Robofish <1683502971@qq.com>
Date: Wed, 9 Apr 2025 00:53:41 +0800
Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0ps2=E6=89=8B=E6=9F=84?=
 =?UTF-8?q?=E4=BB=A3=E7=A0=81?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/ps2_controller/CMakeLists.txt             |  22 ++++
 src/ps2_controller/package.xml                |  20 +++
 src/ps2_controller/src/ps2_reader.cpp         | 123 ++++++++++++++++++
 src/rc_msgs/msg/Ps2Data.msg                   |   5 +
 .../src/unitree_motor_serial_driver.cpp       |  10 +-
 5 files changed, 173 insertions(+), 7 deletions(-)
 create mode 100644 src/ps2_controller/CMakeLists.txt
 create mode 100644 src/ps2_controller/package.xml
 create mode 100644 src/ps2_controller/src/ps2_reader.cpp

diff --git a/src/ps2_controller/CMakeLists.txt b/src/ps2_controller/CMakeLists.txt
new file mode 100644
index 0000000..c9e2c8a
--- /dev/null
+++ b/src/ps2_controller/CMakeLists.txt
@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 3.5)
+project(ps2_controller)
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+# Find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rc_msgs REQUIRED)  # 添加对 rc_msgs 的依赖
+
+add_executable(ps2_reader src/ps2_reader.cpp)
+
+ament_target_dependencies(ps2_reader rclcpp rc_msgs)  # 添加 rc_msgs 依赖
+
+install(TARGETS
+  ps2_reader
+  DESTINATION lib/${PROJECT_NAME})
+
+ament_package()
\ No newline at end of file
diff --git a/src/ps2_controller/package.xml b/src/ps2_controller/package.xml
new file mode 100644
index 0000000..93ef87f
--- /dev/null
+++ b/src/ps2_controller/package.xml
@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>ps2_controller</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="1683502971@qq.com">robofish</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rc_msgs</depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/src/ps2_controller/src/ps2_reader.cpp b/src/ps2_controller/src/ps2_reader.cpp
new file mode 100644
index 0000000..9635192
--- /dev/null
+++ b/src/ps2_controller/src/ps2_reader.cpp
@@ -0,0 +1,123 @@
+#include <rclcpp/rclcpp.hpp>
+#include <linux/joystick.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <iostream>
+#include <string>
+#include "rc_msgs/msg/ps2_data.hpp"  // 引入 Ps2Data 消息类型
+
+class PS2ControllerNode : public rclcpp::Node
+{
+public:
+    PS2ControllerNode() : Node("ps2_controller_node")
+    {
+        this->declare_parameter<std::string>("device", "/dev/input/js0");
+        device_path_ = this->get_parameter("device").as_string();
+
+        joystick_fd_ = open(device_path_.c_str(), O_RDONLY | O_NONBLOCK);
+        if (joystick_fd_ < 0)
+        {
+            RCLCPP_ERROR(this->get_logger(), "Failed to open device: %s", device_path_.c_str());
+            return;
+        }
+
+        RCLCPP_INFO(this->get_logger(), "Connected to device: %s", device_path_.c_str());
+
+        ps2_data_ = std::make_shared<rc_msgs::msg::Ps2Data>();
+        publisher_ = this->create_publisher<rc_msgs::msg::Ps2Data>("ps2_data", 10);
+
+        timer_ = this->create_wall_timer(
+            std::chrono::milliseconds(10),  // 100Hz
+            std::bind(&PS2ControllerNode::publishData, this));
+    }
+
+    ~PS2ControllerNode()
+    {
+        if (joystick_fd_ >= 0)
+        {
+            close(joystick_fd_);
+        }
+    }
+
+private:
+    void publishData()
+    {
+        readJoystick();
+        publisher_->publish(*ps2_data_);
+    }
+
+    void readJoystick()
+    {
+        struct js_event event;
+        while (read(joystick_fd_, &event, sizeof(event)) > 0)
+        {
+            switch (event.type & ~JS_EVENT_INIT)
+            {
+            case JS_EVENT_BUTTON:
+                handleButtonEvent(event);
+                break;
+            case JS_EVENT_AXIS:
+                handleAxisEvent(event);
+                break;
+            default:
+                break;
+            }
+        }
+    }
+
+    void handleButtonEvent(const js_event &event)
+    {
+        switch (event.number)
+        {
+        case 10: ps2_data_->select = event.value; break;
+        case 11: ps2_data_->start = event.value; break;
+        case 6: ps2_data_->l1 = event.value; break;
+        case 8: ps2_data_->l2 = event.value; break;
+        case 7: ps2_data_->r1 = event.value; break;
+        case 9: ps2_data_->r2 = event.value; break;
+        case 3: ps2_data_->x = event.value; break;
+        case 4: ps2_data_->y = event.value; break;
+        case 0: ps2_data_->a = event.value; break;
+        case 1: ps2_data_->b = event.value; break;
+        default:
+            break;
+        }
+    }
+
+    void handleAxisEvent(const js_event &event)
+    {
+        constexpr float MAX_AXIS_VALUE = 32767.0f;  // 最大轴值
+        constexpr float MIN_AXIS_VALUE = -32768.0f; // 最小轴值
+
+        auto normalize = [](int value) -> float {
+            return static_cast<float>(value) / MAX_AXIS_VALUE;
+        };
+
+        switch (event.number)
+        {
+        case 0: ps2_data_->lx = normalize(event.value); break;
+        case 1: ps2_data_->ly = normalize(event.value); break;
+        case 2: ps2_data_->rx = normalize(event.value); break;
+        case 3: ps2_data_->ry = normalize(event.value); break;
+        case 6: ps2_data_->left_right = normalize(event.value); break;
+        case 7: ps2_data_->up_down = normalize(event.value); break;
+        default:
+            break;
+        }
+    }
+
+    std::string device_path_;
+    int joystick_fd_;
+    rclcpp::TimerBase::SharedPtr timer_;
+    rclcpp::Publisher<rc_msgs::msg::Ps2Data>::SharedPtr publisher_;
+    std::shared_ptr<rc_msgs::msg::Ps2Data> ps2_data_;
+};
+
+int main(int argc, char *argv[])
+{
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<PS2ControllerNode>();
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+    return 0;
+}
\ No newline at end of file
diff --git a/src/rc_msgs/msg/Ps2Data.msg b/src/rc_msgs/msg/Ps2Data.msg
index 2b52c1c..0761847 100644
--- a/src/rc_msgs/msg/Ps2Data.msg
+++ b/src/rc_msgs/msg/Ps2Data.msg
@@ -15,6 +15,11 @@ bool r2
 # 四种模式
 uint8 mode # 0:手柄控制 1:键盘控制 2:自瞄 3:手动瞄准
 
+bool x
+bool y
+bool a
+bool b
+
 bool select
 bool start
 
diff --git a/src/unitree_motor_serial_driver/src/unitree_motor_serial_driver.cpp b/src/unitree_motor_serial_driver/src/unitree_motor_serial_driver.cpp
index f93a059..38f978d 100644
--- a/src/unitree_motor_serial_driver/src/unitree_motor_serial_driver.cpp
+++ b/src/unitree_motor_serial_driver/src/unitree_motor_serial_driver.cpp
@@ -14,7 +14,7 @@ namespace unitree_motor_serial_driver
     
         bool connection_success = false;
         int attempts = 0;
-    
+        timer_count_ = 0;
         // 其余代码保持不变
         timer_ = this->create_wall_timer(
             std::chrono::microseconds(1000), std::bind(&MotorControlNode::control_motor, this));
@@ -56,15 +56,11 @@ void MotorControlNode::control_motor()
         cmd.dq = motor[i].cmd.dq;
         cmd.tau = motor[i].cmd.tau;
 
-        if (timer_count_ > 100)
+        if (timer_count_ = 100)
         {
             Motor_Ctrl_Offline(&motor[i]);
-            timer_count_ = 0;
-        }
-        else
-        {
-            timer_count_++;
         }
+        timer_count_++;
 
         // 添加异常处理以防止程序崩溃
         try {