浏览代码

增加G29ros2的驱动 初步测试完成

wangyingjie 2 周之前
当前提交
49173063ca

+ 20 - 0
g29_ros2_feedback_drive/.vscode/c_cpp_properties.json

@@ -0,0 +1,20 @@
+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${default}",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "/opt/ros/humble/include/**",
+        "/usr/include/**"
+      ],
+      "name": "ROS",
+      "intelliSenseMode": "gcc-x64",
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "gnu11",
+      "cppStandard": "c++14"
+    }
+  ],
+  "version": 4
+}

+ 15 - 0
g29_ros2_feedback_drive/.vscode/settings.json

@@ -0,0 +1,15 @@
+{
+     "ros.distro": "humble",
+     "python.autoComplete.extraPaths": [
+          "/opt/ros/humble/lib/python3.10/site-packages",
+          "/opt/ros/humble/local/lib/python3.10/dist-packages"
+     ],
+     "python.analysis.extraPaths": [
+          "/opt/ros/humble/lib/python3.10/site-packages",
+          "/opt/ros/humble/local/lib/python3.10/dist-packages"
+     ],
+     "files.associations": {
+          "thread": "cpp",
+          "chrono": "cpp"
+     }
+}

+ 137 - 0
g29_ros2_feedback_drive/Readme.md

@@ -0,0 +1,137 @@
+<!--
+ * @Author: wangyingjie 2778809626@qq.com
+ * @Date: 2025-05-22 11:00:44
+ * @LastEditors: wangyingjie 2778809626@qq.com
+ * @LastEditTime: 2025-05-22 17:25:49
+ * @FilePath: /g29_ros2_feedback/Readme.md
+ * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
+-->
+
+# 罗技G29数据发布
+
+该项目是读取罗技G29控制器的数据,并发布为ROS2话题。
+
+编译和运行方法:
+```bash
+cd g29_ros2_feedback_bag
+colcon build
+source install/setup.bash
+ros2 run g29_ros2_feedback g29_ros2_feedback_node 
+```
+
+数据发布话题为`/g29_feedback`
+
+示例如下
+```bash
+heartbeat: 77
+axis0: -332
+axis1: 32767
+axis2: 32767
+axis3: 32767
+axis4: 0
+axis5: 0
+button0: false
+button1: false
+button2: false
+button3: false
+button4: false
+button5: false
+button6: false
+button7: false
+button8: false
+button9: false
+button10: false
+button11: false
+button12: false
+button13: false
+button14: true
+button15: false
+button16: false
+button17: false
+button18: false
+button19: false
+button20: false
+button21: false
+button22: false
+button23: false
+button24: false
+
+```
+
+数据会一直发布,其中`heartbeat`为心跳包,每秒发布一次会增加,在0~100之间循环
+
+
+
+
+
+方向盘: axis0 -32767 - 32767 (逆时针为负,逆时针终值为-32767,顺时针终值为32767)
+离合: axis1 -32767 - 32767 (踩到底为-32767,正常抬起为32767)
+油门: axis2 -32767 - 32767 (踩到底为-32767,正常抬起为32767)
+刹车: axis3 -32767 - 32767 (踩到底为-32767,正常抬起为32767)
+方向键左: axis4 -32767 / 0
+方向键右: axis4 32767 / 0
+方向键上: axis5 -32767 / 0
+方向键下: axis5 32767 / 0
+
+button0: X 键  0/1
+button1: 正方形 键  0/1
+button2: O 键  0/1
+button3: 三角 键  0/1
+button4: 右拨片  0/1
+button5: 左拨片  0/1
+button6: R2 键  0/1
+button7: L2 键  0/1
+button8: 三条竖线的发光键  0/1
+button9: 三条横线的键  0/1
+button10: R3 键  0/1
+button11: L3 键  0/1
+button12: 一档  0/1
+button13: 二挡  0/1
+button14: 三挡  0/1
+button15: 四挡  0/1
+button16: 五档  0/1
+button17: 六档  0/1
+button18: 按下去倒挡  0/1
+button19: + 键  0/1
+button20: - 键  0/1
+button21: 红色齿轮顺时针  0/1
+button22: 红色齿轮逆时针  0/1
+button23: 红色齿轮中心的箭头  0/1
+button24: 帆船标志键  0/1
+
+
+button_mapping = {const std::vector<int> &} {length 25, capacity 32}
+ [0] = {int} 288 [0x120]
+ [1] = {int} 289 [0x121]
+ [2] = {int} 290 [0x122]
+ [3] = {int} 291 [0x123]
+ [4] = {int} 292 [0x124]
+ [5] = {int} 293 [0x125]
+ [6] = {int} 294 [0x126]
+ [7] = {int} 295 [0x127]
+ [8] = {int} 296 [0x128]
+ [9] = {int} 297 [0x129]
+ [10] = {int} 298 [0x12a]
+ [11] = {int} 299 [0x12b]
+ [12] = {int} 300 [0x12c]
+ [13] = {int} 301 [0x12d]
+ [14] = {int} 302 [0x12e]
+ [15] = {int} 303 [0x12f]
+ [16] = {int} 704 [0x2c0]
+ [17] = {int} 705 [0x2c1]
+ [18] = {int} 706 [0x2c2]
+ [19] = {int} 707 [0x2c3]
+ [20] = {int} 708 [0x2c4]
+ [21] = {int} 709 [0x2c5]
+ [22] = {int} 710 [0x2c6]
+ [23] = {int} 711 [0x2c7]
+ [24] = {int} 712 [0x2c8]
+ 
+
+axis_mapping = {const std::vector<int> &} {length 6, capacity 8}
+ [0] = {int} 0 [0x0]
+ [1] = {int} 1 [0x1]
+ [2] = {int} 2 [0x2]
+ [3] = {int} 5 [0x5]
+ [4] = {int} 16 [0x10]
+ [5] = {int} 17 [0x11]

+ 29 - 0
g29_ros2_feedback_drive/src/g29_msg/CMakeLists.txt

@@ -0,0 +1,29 @@
+cmake_minimum_required(VERSION 3.8)
+project(g29_msg)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+
+# new add---------------------
+find_package(geometry_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+rosidl_generate_interfaces(${PROJECT_NAME} 
+  "msg/G29Msg.msg" 
+  DEPENDENCIES std_msgs geometry_msgs
+  )
+# new add---------------------
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  set(ament_cmake_copyright_FOUND TRUE)
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

+ 37 - 0
g29_ros2_feedback_drive/src/g29_msg/msg/G29Msg.msg

@@ -0,0 +1,37 @@
+# G29Msg.msg
+
+int32 heartbeat # 心跳信号,用于检测连接状态
+# 轴数据,包含方向盘、离合、油门、刹车和方向键的状态
+int32 axis0  # 方向盘轴 (范围:-32767 到 32767) (逆时针为负,逆时针终值为-32767,顺时针终值为32767)
+int32 axis1  # 离合轴 (范围:-32767 到 32767) (踩到底为-32767,正常抬起为32767)
+int32 axis2  # 油门轴 (范围:-32767 到 32767) (踩到底为-32767,正常抬起为32767)
+int32 axis3  # 刹车轴 (范围:-32767 到 32767) (踩到底为-32767,正常抬起为32767))
+int32 axis4  # 方向键左 / 右 (左键按下为 -32767,右键按下为 32767, 否则为 0)
+int32 axis5  # 方向键上 / 下 (上键按下为 -32767,下键按下为 32767, 否则为 0)
+
+# 按钮状态,0 为未按下,1 为按下
+bool button0   # X 键
+bool button1   # 正方形 键
+bool button2   # O 键
+bool button3   # 三角 键
+bool button4   # 右拨片
+bool button5   # 左拨片
+bool button6   # R2 键
+bool button7   # L2 键
+bool button8   # 三条竖线的发光键
+bool button9   # 三条横线的键
+bool button10  # R3 键
+bool button11  # L3 键
+bool button12  # 一档
+bool button13  # 二挡
+bool button14  # 三挡
+bool button15  # 四挡
+bool button16  # 五档
+bool button17  # 六档
+bool button18  # 按下去倒挡
+bool button19  # + 键
+bool button20  # - 键
+bool button21  # 红色齿轮顺时针
+bool button22  # 红色齿轮逆时针
+bool button23  # 红色齿轮中心的箭头
+bool button24  # 帆船标志键

+ 26 - 0
g29_ros2_feedback_drive/src/g29_msg/package.xml

@@ -0,0 +1,26 @@
+<?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>g29_msg</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="wub@todo.todo">wub</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>std_msgs</depend>
+  <depend>rclcpp</depend>
+
+  <depend>geometry_msgs</depend>
+  <build_depend>rosidl_default_generators</build_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

+ 57 - 0
g29_ros2_feedback_drive/src/g29_ros2_feedback/CMakeLists.txt

@@ -0,0 +1,57 @@
+cmake_minimum_required(VERSION 3.8)
+project(g29_ros2_feedback) 
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+# new add---------------------
+find_package(g29_msg REQUIRED)
+# new add---------------------
+# 查找 X11 库
+find_package(X11 REQUIRED)
+
+foreach(dirname ${CMAKE_CURRENT_SOURCE_DIR}/src)
+    file(GLOB_RECURSE SRC RELATIVE ${CMAKE_CURRENT_SOURCE_DIR}
+            "${dirname}/*.h"
+            "${dirname}/*.hpp"
+            "${dirname}/*.c"
+            "${dirname}/*.cpp"
+            )
+endforeach()
+
+add_executable(${PROJECT_NAME}_node ${SRC})
+
+target_include_directories(${PROJECT_NAME}_node PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+  ${X11_INCLUDE_DIR}  # 添加 X11 头文件路径
+  )
+target_compile_features(${PROJECT_NAME}_node PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+ament_target_dependencies(
+  ${PROJECT_NAME}_node
+  "std_msgs"
+  "rclcpp"
+  g29_msg
+)
+
+# 显式链接 X11 库
+target_link_libraries(${PROJECT_NAME}_node
+  ${X11_LIBRARIES}   # 添加 X11 库
+)
+
+install(TARGETS ${PROJECT_NAME}_node
+  DESTINATION lib/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  set(ament_cmake_copyright_FOUND TRUE)
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

+ 23 - 0
g29_ros2_feedback_drive/src/g29_ros2_feedback/package.xml

@@ -0,0 +1,23 @@
+<?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>g29_ros2_feedback</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="wub@todo.todo">wub</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>std_msgs</depend>
+  <depend>rclcpp</depend>
+
+  <depend>g29_msg</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

+ 746 - 0
g29_ros2_feedback_drive/src/g29_ros2_feedback/src/evdev_helper.cpp

@@ -0,0 +1,746 @@
+/*
+**  Xbox360 USB Gamepad Userspace Driver
+**  Copyright (C) 2008 Ingo Ruhnke <grumbel@gmail.com>
+**
+**  This program is free software: you can redistribute it and/or modify
+**  it under the terms of the GNU General Public License as published by
+**  the Free Software Foundation, either version 3 of the License, or
+**  (at your option) any later version.
+**
+**  This program is distributed in the hope that it will be useful,
+**  but WITHOUT ANY WARRANTY; without even the implied warranty of
+**  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+**  GNU General Public License for more details.
+**
+**  You should have received a copy of the GNU General Public License
+**  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include <X11/Xlib.h>
+#include <linux/input.h>
+#include <iostream>
+#include <sstream>
+#include <stdexcept>
+#include <map>
+#include "evdev_helper.hpp"
+
+template <class Enum>
+class EnumBox
+{
+protected:
+  std::string name;                        // 枚举名称
+  std::map<Enum, std::string> enum2string; // 枚举值到字符串的映射
+  std::map<std::string, Enum> string2enum; // 字符串到枚举值的映射
+
+protected:
+  // 构造函数,初始化枚举名称
+  EnumBox(const std::string &name)
+      : name(name)
+  {
+  }
+
+  // 添加一个枚举值及其对应的字符串表示
+  void add(Enum i, const std::string &name)
+  {
+    enum2string[i] = name;
+    string2enum[name] = i;
+  }
+
+public:
+  // 将字符串转换为对应的枚举值
+  Enum operator[](const std::string &str) const
+  {
+    typename std::map<std::string, Enum>::const_iterator i = string2enum.find(str);
+    if (i == string2enum.end())
+    {
+      std::istringstream in(str);
+      Enum tmp;
+      in >> tmp;
+      if (in.fail())
+      {
+        std::ostringstream out;
+        out << "无法将 '" << str << "' 转换为枚举 " << name << std::endl;
+        throw std::runtime_error(out.str());
+      }
+      else
+      {
+        return tmp;
+      }
+    }
+    else
+    {
+      return i->second;
+    }
+  }
+
+  // 将枚举值转换为对应的字符串表示
+  std::string operator[](Enum v) const
+  {
+    typename std::map<Enum, std::string>::const_iterator i = enum2string.find(v);
+    if (i == enum2string.end())
+    {
+      // 如果无法进行符号转换,则直接将整数转换为字符串
+      std::ostringstream out;
+      out << v;
+      return out.str();
+    }
+    else
+    {
+      return i->second;
+    }
+  }
+};
+
+// 定义一个相对事件枚举类,继承自EnumBox<int>
+class EvDevRelEnum : public EnumBox<int>
+{
+public:
+  // 构造函数初始化相对事件枚举类
+  EvDevRelEnum()
+      : EnumBox<int>("EV_REL")
+  {
+    // 下面的注释代码展示了如何从Linux输入头文件中提取REL定义,并自动生成add函数调用
+    // 这段代码是 Ruby 代码,用于生成add函数调用,以保持代码的自文档化特性
+    // File.new("/usr/include/linux/input.h")
+    //  .grep(/^#define REL/)
+    //  .each{|i| name = i.split[1]; puts "add(%s,%s\"%s\");" % [name, " " * (20-name.length), name] };
+
+    // 添加相对事件枚举值和对应的名称
+    add(REL_X, "REL_X");
+    add(REL_Y, "REL_Y");
+    add(REL_Z, "REL_Z");
+    add(REL_RX, "REL_RX");
+    add(REL_RY, "REL_RY");
+    add(REL_RZ, "REL_RZ");
+    add(REL_HWHEEL, "REL_HWHEEL");
+    add(REL_DIAL, "REL_DIAL");
+    add(REL_WHEEL, "REL_WHEEL");
+    add(REL_MISC, "REL_MISC");
+  }
+} evdev_rel_names;
+
+class EvDevAbsEnum : public EnumBox<int>
+{
+public:
+EvDevAbsEnum()
+    : EnumBox<int>("EV_ABS")
+{
+    // File.new("/usr/include/linux/input.h")
+    //  .grep(/^#define ABS/)
+    //  .each{|i| name = i.split[1]; puts "add(%s,%s\"%s\");" % [name, " " * (20-name.length), name] };
+
+    add(ABS_X, "ABS_X");             // X轴绝对坐标(常用于鼠标/手柄X轴)
+    add(ABS_Y, "ABS_Y");             // Y轴绝对坐标(常用于鼠标/手柄Y轴)
+    add(ABS_Z, "ABS_Z");             // Z轴绝对坐标(如三维控制器)
+    add(ABS_RX, "ABS_RX");           // 绕X轴旋转角度(摇杆RX轴)
+    add(ABS_RY, "ABS_RY");           // 绕Y轴旋转角度(摇杆RY轴)
+    add(ABS_RZ, "ABS_RZ");           // 绕Z轴旋转角度(摇杆RZ轴)
+    add(ABS_THROTTLE, "ABS_THROTTLE"); // 油门控制轴(飞行模拟器等)
+    add(ABS_RUDDER, "ABS_RUDDER");   // 方向舵轴(飞行模拟器方向控制)
+    add(ABS_WHEEL, "ABS_WHEEL");     // 转向轮角度(方向盘设备使用)
+    add(ABS_GAS, "ABS_GAS");         // 加速踏板(油门)
+    add(ABS_BRAKE, "ABS_BRAKE");     // 制动踏板
+    add(ABS_HAT0X, "ABS_HAT0X");     // 第0方向帽(D-Pad)X轴
+    add(ABS_HAT0Y, "ABS_HAT0Y");     // 第0方向帽(D-Pad)Y轴
+    add(ABS_HAT1X, "ABS_HAT1X");     // 第1方向帽X轴
+    add(ABS_HAT1Y, "ABS_HAT1Y");     // 第1方向帽Y轴
+    add(ABS_HAT2X, "ABS_HAT2X");     // 第2方向帽X轴
+    add(ABS_HAT2Y, "ABS_HAT2Y");     // 第2方向帽Y轴
+    add(ABS_HAT3X, "ABS_HAT3X");     // 第3方向帽X轴
+    add(ABS_HAT3Y, "ABS_HAT3Y");     // 第3方向帽Y轴
+    add(ABS_PRESSURE, "ABS_PRESSURE"); // 压力传感器输入(如触控笔压力)
+    add(ABS_DISTANCE, "ABS_DISTANCE"); // 距离测量轴
+    add(ABS_TILT_X, "ABS_TILT_X");   // X轴倾斜度(如重力感应)
+    add(ABS_TILT_Y, "ABS_TILT_Y");   // Y轴倾斜度
+    add(ABS_TOOL_WIDTH, "ABS_TOOL_WIDTH"); // 工具宽度(绘图软件识别笔尖粗细)
+    add(ABS_VOLUME, "ABS_VOLUME");   // 音量调节轴
+    add(ABS_MISC, "ABS_MISC");       // 杂项轴(保留用于未分类输入)
+}
+} evdev_abs_names;
+
+class EvDevBtnEnum : public EnumBox<int>
+{
+public:
+  EvDevBtnEnum()
+      : EnumBox<int>("EV_KEY")
+  {
+    // File.new("/usr/include/linux/input.h")
+    //  .grep(/^#define (BTN|KEY)/)
+    //  .each{|i| name = i.split[1]; puts "add(%s,%s\"%s\");" % [name, " " * (20-name.length), name] };
+    add(KEY_RESERVED, "KEY_RESERVED");
+    add(KEY_ESC, "KEY_ESC");
+    add(KEY_1, "KEY_1");
+    add(KEY_2, "KEY_2");
+    add(KEY_3, "KEY_3");
+    add(KEY_4, "KEY_4");
+    add(KEY_5, "KEY_5");
+    add(KEY_6, "KEY_6");
+    add(KEY_7, "KEY_7");
+    add(KEY_8, "KEY_8");
+    add(KEY_9, "KEY_9");
+    add(KEY_0, "KEY_0");
+    add(KEY_MINUS, "KEY_MINUS");
+    add(KEY_EQUAL, "KEY_EQUAL");
+    add(KEY_BACKSPACE, "KEY_BACKSPACE");
+    add(KEY_TAB, "KEY_TAB");
+    add(KEY_Q, "KEY_Q");
+    add(KEY_W, "KEY_W");
+    add(KEY_E, "KEY_E");
+    add(KEY_R, "KEY_R");
+    add(KEY_T, "KEY_T");
+    add(KEY_Y, "KEY_Y");
+    add(KEY_U, "KEY_U");
+    add(KEY_I, "KEY_I");
+    add(KEY_O, "KEY_O");
+    add(KEY_P, "KEY_P");
+    add(KEY_LEFTBRACE, "KEY_LEFTBRACE");
+    add(KEY_RIGHTBRACE, "KEY_RIGHTBRACE");
+    add(KEY_ENTER, "KEY_ENTER");
+    add(KEY_LEFTCTRL, "KEY_LEFTCTRL");
+    add(KEY_A, "KEY_A");
+    add(KEY_S, "KEY_S");
+    add(KEY_D, "KEY_D");
+    add(KEY_F, "KEY_F");
+    add(KEY_G, "KEY_G");
+    add(KEY_H, "KEY_H");
+    add(KEY_J, "KEY_J");
+    add(KEY_K, "KEY_K");
+    add(KEY_L, "KEY_L");
+    add(KEY_SEMICOLON, "KEY_SEMICOLON");
+    add(KEY_APOSTROPHE, "KEY_APOSTROPHE");
+    add(KEY_GRAVE, "KEY_GRAVE");
+    add(KEY_LEFTSHIFT, "KEY_LEFTSHIFT");
+    add(KEY_BACKSLASH, "KEY_BACKSLASH");
+    add(KEY_Z, "KEY_Z");
+    add(KEY_X, "KEY_X");
+    add(KEY_C, "KEY_C");
+    add(KEY_V, "KEY_V");
+    add(KEY_B, "KEY_B");
+    add(KEY_N, "KEY_N");
+    add(KEY_M, "KEY_M");
+    add(KEY_COMMA, "KEY_COMMA");
+    add(KEY_DOT, "KEY_DOT");
+    add(KEY_SLASH, "KEY_SLASH");
+    add(KEY_RIGHTSHIFT, "KEY_RIGHTSHIFT");
+    add(KEY_KPASTERISK, "KEY_KPASTERISK");
+    add(KEY_LEFTALT, "KEY_LEFTALT");
+    add(KEY_SPACE, "KEY_SPACE");
+    add(KEY_CAPSLOCK, "KEY_CAPSLOCK");
+    add(KEY_F1, "KEY_F1");
+    add(KEY_F2, "KEY_F2");
+    add(KEY_F3, "KEY_F3");
+    add(KEY_F4, "KEY_F4");
+    add(KEY_F5, "KEY_F5");
+    add(KEY_F6, "KEY_F6");
+    add(KEY_F7, "KEY_F7");
+    add(KEY_F8, "KEY_F8");
+    add(KEY_F9, "KEY_F9");
+    add(KEY_F10, "KEY_F10");
+    add(KEY_NUMLOCK, "KEY_NUMLOCK");
+    add(KEY_SCROLLLOCK, "KEY_SCROLLLOCK");
+    add(KEY_KP7, "KEY_KP7");
+    add(KEY_KP8, "KEY_KP8");
+    add(KEY_KP9, "KEY_KP9");
+    add(KEY_KPMINUS, "KEY_KPMINUS");
+    add(KEY_KP4, "KEY_KP4");
+    add(KEY_KP5, "KEY_KP5");
+    add(KEY_KP6, "KEY_KP6");
+    add(KEY_KPPLUS, "KEY_KPPLUS");
+    add(KEY_KP1, "KEY_KP1");
+    add(KEY_KP2, "KEY_KP2");
+    add(KEY_KP3, "KEY_KP3");
+    add(KEY_KP0, "KEY_KP0");
+    add(KEY_KPDOT, "KEY_KPDOT");
+    add(KEY_ZENKAKUHANKAKU, "KEY_ZENKAKUHANKAKU");
+    add(KEY_102ND, "KEY_102ND");
+    add(KEY_F11, "KEY_F11");
+    add(KEY_F12, "KEY_F12");
+    add(KEY_RO, "KEY_RO");
+    add(KEY_KATAKANA, "KEY_KATAKANA");
+    add(KEY_HIRAGANA, "KEY_HIRAGANA");
+    add(KEY_HENKAN, "KEY_HENKAN");
+    add(KEY_KATAKANAHIRAGANA, "KEY_KATAKANAHIRAGANA");
+    add(KEY_MUHENKAN, "KEY_MUHENKAN");
+    add(KEY_KPJPCOMMA, "KEY_KPJPCOMMA");
+    add(KEY_KPENTER, "KEY_KPENTER");
+    add(KEY_RIGHTCTRL, "KEY_RIGHTCTRL");
+    add(KEY_KPSLASH, "KEY_KPSLASH");
+    add(KEY_SYSRQ, "KEY_SYSRQ");
+    add(KEY_RIGHTALT, "KEY_RIGHTALT");
+    add(KEY_LINEFEED, "KEY_LINEFEED");
+    add(KEY_HOME, "KEY_HOME");
+    add(KEY_UP, "KEY_UP");
+    add(KEY_PAGEUP, "KEY_PAGEUP");
+    add(KEY_LEFT, "KEY_LEFT");
+    add(KEY_RIGHT, "KEY_RIGHT");
+    add(KEY_END, "KEY_END");
+    add(KEY_DOWN, "KEY_DOWN");
+    add(KEY_PAGEDOWN, "KEY_PAGEDOWN");
+    add(KEY_INSERT, "KEY_INSERT");
+    add(KEY_DELETE, "KEY_DELETE");
+    add(KEY_MACRO, "KEY_MACRO");
+    add(KEY_MUTE, "KEY_MUTE");
+    add(KEY_VOLUMEDOWN, "KEY_VOLUMEDOWN");
+    add(KEY_VOLUMEUP, "KEY_VOLUMEUP");
+    add(KEY_POWER, "KEY_POWER");
+    add(KEY_KPEQUAL, "KEY_KPEQUAL");
+    add(KEY_KPPLUSMINUS, "KEY_KPPLUSMINUS");
+    add(KEY_PAUSE, "KEY_PAUSE");
+    add(KEY_KPCOMMA, "KEY_KPCOMMA");
+    add(KEY_HANGEUL, "KEY_HANGEUL");
+    add(KEY_HANGUEL, "KEY_HANGUEL");
+    add(KEY_HANJA, "KEY_HANJA");
+    add(KEY_YEN, "KEY_YEN");
+    add(KEY_LEFTMETA, "KEY_LEFTMETA");
+    add(KEY_RIGHTMETA, "KEY_RIGHTMETA");
+    add(KEY_COMPOSE, "KEY_COMPOSE");
+    add(KEY_STOP, "KEY_STOP");
+    add(KEY_AGAIN, "KEY_AGAIN");
+    add(KEY_PROPS, "KEY_PROPS");
+    add(KEY_UNDO, "KEY_UNDO");
+    add(KEY_FRONT, "KEY_FRONT");
+    add(KEY_COPY, "KEY_COPY");
+    add(KEY_OPEN, "KEY_OPEN");
+    add(KEY_PASTE, "KEY_PASTE");
+    add(KEY_FIND, "KEY_FIND");
+    add(KEY_CUT, "KEY_CUT");
+    add(KEY_HELP, "KEY_HELP");
+    add(KEY_MENU, "KEY_MENU");
+    add(KEY_CALC, "KEY_CALC");
+    add(KEY_SETUP, "KEY_SETUP");
+    add(KEY_SLEEP, "KEY_SLEEP");
+    add(KEY_WAKEUP, "KEY_WAKEUP");
+    add(KEY_FILE, "KEY_FILE");
+    add(KEY_SENDFILE, "KEY_SENDFILE");
+    add(KEY_DELETEFILE, "KEY_DELETEFILE");
+    add(KEY_XFER, "KEY_XFER");
+    add(KEY_PROG1, "KEY_PROG1");
+    add(KEY_PROG2, "KEY_PROG2");
+    add(KEY_WWW, "KEY_WWW");
+    add(KEY_MSDOS, "KEY_MSDOS");
+    add(KEY_COFFEE, "KEY_COFFEE");
+    add(KEY_SCREENLOCK, "KEY_SCREENLOCK");
+    add(KEY_DIRECTION, "KEY_DIRECTION");
+    add(KEY_CYCLEWINDOWS, "KEY_CYCLEWINDOWS");
+    add(KEY_MAIL, "KEY_MAIL");
+    add(KEY_BOOKMARKS, "KEY_BOOKMARKS");
+    add(KEY_COMPUTER, "KEY_COMPUTER");
+    add(KEY_BACK, "KEY_BACK");
+    add(KEY_FORWARD, "KEY_FORWARD");
+    add(KEY_CLOSECD, "KEY_CLOSECD");
+    add(KEY_EJECTCD, "KEY_EJECTCD");
+    add(KEY_EJECTCLOSECD, "KEY_EJECTCLOSECD");
+    add(KEY_NEXTSONG, "KEY_NEXTSONG");
+    add(KEY_PLAYPAUSE, "KEY_PLAYPAUSE");
+    add(KEY_PREVIOUSSONG, "KEY_PREVIOUSSONG");
+    add(KEY_STOPCD, "KEY_STOPCD");
+    add(KEY_RECORD, "KEY_RECORD");
+    add(KEY_REWIND, "KEY_REWIND");
+    add(KEY_PHONE, "KEY_PHONE");
+    add(KEY_ISO, "KEY_ISO");
+    add(KEY_CONFIG, "KEY_CONFIG");
+    add(KEY_HOMEPAGE, "KEY_HOMEPAGE");
+    add(KEY_REFRESH, "KEY_REFRESH");
+    add(KEY_EXIT, "KEY_EXIT");
+    add(KEY_MOVE, "KEY_MOVE");
+    add(KEY_EDIT, "KEY_EDIT");
+    add(KEY_SCROLLUP, "KEY_SCROLLUP");
+    add(KEY_SCROLLDOWN, "KEY_SCROLLDOWN");
+    add(KEY_KPLEFTPAREN, "KEY_KPLEFTPAREN");
+    add(KEY_KPRIGHTPAREN, "KEY_KPRIGHTPAREN");
+    add(KEY_NEW, "KEY_NEW");
+    add(KEY_REDO, "KEY_REDO");
+    add(KEY_F13, "KEY_F13");
+    add(KEY_F14, "KEY_F14");
+    add(KEY_F15, "KEY_F15");
+    add(KEY_F16, "KEY_F16");
+    add(KEY_F17, "KEY_F17");
+    add(KEY_F18, "KEY_F18");
+    add(KEY_F19, "KEY_F19");
+    add(KEY_F20, "KEY_F20");
+    add(KEY_F21, "KEY_F21");
+    add(KEY_F22, "KEY_F22");
+    add(KEY_F23, "KEY_F23");
+    add(KEY_F24, "KEY_F24");
+    add(KEY_PLAYCD, "KEY_PLAYCD");
+    add(KEY_PAUSECD, "KEY_PAUSECD");
+    add(KEY_PROG3, "KEY_PROG3");
+    add(KEY_PROG4, "KEY_PROG4");
+    add(KEY_SUSPEND, "KEY_SUSPEND");
+    add(KEY_CLOSE, "KEY_CLOSE");
+    add(KEY_PLAY, "KEY_PLAY");
+    add(KEY_FASTFORWARD, "KEY_FASTFORWARD");
+    add(KEY_BASSBOOST, "KEY_BASSBOOST");
+    add(KEY_PRINT, "KEY_PRINT");
+    add(KEY_HP, "KEY_HP");
+    add(KEY_CAMERA, "KEY_CAMERA");
+    add(KEY_SOUND, "KEY_SOUND");
+    add(KEY_QUESTION, "KEY_QUESTION");
+    add(KEY_EMAIL, "KEY_EMAIL");
+    add(KEY_CHAT, "KEY_CHAT");
+    add(KEY_SEARCH, "KEY_SEARCH");
+    add(KEY_CONNECT, "KEY_CONNECT");
+    add(KEY_FINANCE, "KEY_FINANCE");
+    add(KEY_SPORT, "KEY_SPORT");
+    add(KEY_SHOP, "KEY_SHOP");
+    add(KEY_ALTERASE, "KEY_ALTERASE");
+    add(KEY_CANCEL, "KEY_CANCEL");
+    add(KEY_BRIGHTNESSDOWN, "KEY_BRIGHTNESSDOWN");
+    add(KEY_BRIGHTNESSUP, "KEY_BRIGHTNESSUP");
+    add(KEY_MEDIA, "KEY_MEDIA");
+    add(KEY_SWITCHVIDEOMODE, "KEY_SWITCHVIDEOMODE");
+    add(KEY_KBDILLUMTOGGLE, "KEY_KBDILLUMTOGGLE");
+    add(KEY_KBDILLUMDOWN, "KEY_KBDILLUMDOWN");
+    add(KEY_KBDILLUMUP, "KEY_KBDILLUMUP");
+    add(KEY_SEND, "KEY_SEND");
+    add(KEY_REPLY, "KEY_REPLY");
+    add(KEY_FORWARDMAIL, "KEY_FORWARDMAIL");
+    add(KEY_SAVE, "KEY_SAVE");
+    add(KEY_DOCUMENTS, "KEY_DOCUMENTS");
+    add(KEY_BATTERY, "KEY_BATTERY");
+    add(KEY_BLUETOOTH, "KEY_BLUETOOTH");
+    add(KEY_WLAN, "KEY_WLAN");
+    add(KEY_UWB, "KEY_UWB");
+    add(KEY_UNKNOWN, "KEY_UNKNOWN");
+    add(KEY_VIDEO_NEXT, "KEY_VIDEO_NEXT");
+    add(KEY_VIDEO_PREV, "KEY_VIDEO_PREV");
+    add(KEY_BRIGHTNESS_CYCLE, "KEY_BRIGHTNESS_CYCLE");
+    add(KEY_BRIGHTNESS_ZERO, "KEY_BRIGHTNESS_ZERO");
+    add(KEY_DISPLAY_OFF, "KEY_DISPLAY_OFF");
+    add(KEY_WIMAX, "KEY_WIMAX");
+    add(BTN_MISC, "BTN_MISC");
+    add(BTN_0, "BTN_0");
+    add(BTN_1, "BTN_1");
+    add(BTN_2, "BTN_2");
+    add(BTN_3, "BTN_3");
+    add(BTN_4, "BTN_4");
+    add(BTN_5, "BTN_5");
+    add(BTN_6, "BTN_6");
+    add(BTN_7, "BTN_7");
+    add(BTN_8, "BTN_8");
+    add(BTN_9, "BTN_9");
+    add(BTN_MOUSE, "BTN_MOUSE");
+    add(BTN_LEFT, "BTN_LEFT");
+    add(BTN_RIGHT, "BTN_RIGHT");
+    add(BTN_MIDDLE, "BTN_MIDDLE");
+    add(BTN_SIDE, "BTN_SIDE");
+    add(BTN_EXTRA, "BTN_EXTRA");
+    add(BTN_FORWARD, "BTN_FORWARD");
+    add(BTN_BACK, "BTN_BACK");
+    add(BTN_TASK, "BTN_TASK");
+    add(BTN_JOYSTICK, "BTN_JOYSTICK");
+    add(BTN_TRIGGER, "BTN_TRIGGER"); // 0 buttons
+    add(BTN_THUMB, "BTN_THUMB");
+    add(BTN_THUMB2, "BTN_THUMB2");
+    add(BTN_TOP, "BTN_TOP");
+    add(BTN_TOP2, "BTN_TOP2");
+    add(BTN_PINKIE, "BTN_PINKIE");
+    add(BTN_BASE, "BTN_BASE");
+    add(BTN_BASE2, "BTN_BASE2");
+    add(BTN_BASE3, "BTN_BASE3");
+    add(BTN_BASE4, "BTN_BASE4");
+    add(BTN_BASE5, "BTN_BASE5");
+    add(BTN_BASE6, "BTN_BASE6"); // 11 buttons
+    add(BTN_DEAD, "BTN_DEAD");
+    add(BTN_GAMEPAD, "BTN_GAMEPAD");
+    add(BTN_A, "BTN_A");
+    add(BTN_B, "BTN_B");
+    add(BTN_C, "BTN_C");
+    add(BTN_X, "BTN_X");
+    add(BTN_Y, "BTN_Y");
+    add(BTN_Z, "BTN_Z");
+    add(BTN_TL, "BTN_TL");
+    add(BTN_TR, "BTN_TR");
+    add(BTN_TL2, "BTN_TL2");
+    add(BTN_TR2, "BTN_TR2");
+    add(BTN_SELECT, "BTN_SELECT");
+    add(BTN_START, "BTN_START");
+    add(BTN_MODE, "BTN_MODE");
+    add(BTN_THUMBL, "BTN_THUMBL");
+    add(BTN_THUMBR, "BTN_THUMBR");
+    add(BTN_DIGI, "BTN_DIGI");
+    add(BTN_TOOL_PEN, "BTN_TOOL_PEN");
+    add(BTN_TOOL_RUBBER, "BTN_TOOL_RUBBER");
+    add(BTN_TOOL_BRUSH, "BTN_TOOL_BRUSH");
+    add(BTN_TOOL_PENCIL, "BTN_TOOL_PENCIL");
+    add(BTN_TOOL_AIRBRUSH, "BTN_TOOL_AIRBRUSH");
+    add(BTN_TOOL_FINGER, "BTN_TOOL_FINGER");
+    add(BTN_TOOL_MOUSE, "BTN_TOOL_MOUSE");
+    add(BTN_TOOL_LENS, "BTN_TOOL_LENS");
+    add(BTN_TOUCH, "BTN_TOUCH");
+    add(BTN_STYLUS, "BTN_STYLUS");
+    add(BTN_STYLUS2, "BTN_STYLUS2");
+    add(BTN_TOOL_DOUBLETAP, "BTN_TOOL_DOUBLETAP");
+    add(BTN_TOOL_TRIPLETAP, "BTN_TOOL_TRIPLETAP");
+    add(BTN_WHEEL, "BTN_WHEEL");
+    add(BTN_GEAR_DOWN, "BTN_GEAR_DOWN");
+    add(BTN_GEAR_UP, "BTN_GEAR_UP");
+    add(KEY_OK, "KEY_OK");
+    add(KEY_SELECT, "KEY_SELECT");
+    add(KEY_GOTO, "KEY_GOTO");
+    add(KEY_CLEAR, "KEY_CLEAR");
+    add(KEY_POWER2, "KEY_POWER2");
+    add(KEY_OPTION, "KEY_OPTION");
+    add(KEY_INFO, "KEY_INFO");
+    add(KEY_TIME, "KEY_TIME");
+    add(KEY_VENDOR, "KEY_VENDOR");
+    add(KEY_ARCHIVE, "KEY_ARCHIVE");
+    add(KEY_PROGRAM, "KEY_PROGRAM");
+    add(KEY_CHANNEL, "KEY_CHANNEL");
+    add(KEY_FAVORITES, "KEY_FAVORITES");
+    add(KEY_EPG, "KEY_EPG");
+    add(KEY_PVR, "KEY_PVR");
+    add(KEY_MHP, "KEY_MHP");
+    add(KEY_LANGUAGE, "KEY_LANGUAGE");
+    add(KEY_TITLE, "KEY_TITLE");
+    add(KEY_SUBTITLE, "KEY_SUBTITLE");
+    add(KEY_ANGLE, "KEY_ANGLE");
+    add(KEY_ZOOM, "KEY_ZOOM");
+    add(KEY_MODE, "KEY_MODE");
+    add(KEY_KEYBOARD, "KEY_KEYBOARD");
+    add(KEY_SCREEN, "KEY_SCREEN");
+    add(KEY_PC, "KEY_PC");
+    add(KEY_TV, "KEY_TV");
+    add(KEY_TV2, "KEY_TV2");
+    add(KEY_VCR, "KEY_VCR");
+    add(KEY_VCR2, "KEY_VCR2");
+    add(KEY_SAT, "KEY_SAT");
+    add(KEY_SAT2, "KEY_SAT2");
+    add(KEY_CD, "KEY_CD");
+    add(KEY_TAPE, "KEY_TAPE");
+    add(KEY_RADIO, "KEY_RADIO");
+    add(KEY_TUNER, "KEY_TUNER");
+    add(KEY_PLAYER, "KEY_PLAYER");
+    add(KEY_TEXT, "KEY_TEXT");
+    add(KEY_DVD, "KEY_DVD");
+    add(KEY_AUX, "KEY_AUX");
+    add(KEY_MP3, "KEY_MP3");
+    add(KEY_AUDIO, "KEY_AUDIO");
+    add(KEY_VIDEO, "KEY_VIDEO");
+    add(KEY_DIRECTORY, "KEY_DIRECTORY");
+    add(KEY_LIST, "KEY_LIST");
+    add(KEY_MEMO, "KEY_MEMO");
+    add(KEY_CALENDAR, "KEY_CALENDAR");
+    add(KEY_RED, "KEY_RED");
+    add(KEY_GREEN, "KEY_GREEN");
+    add(KEY_YELLOW, "KEY_YELLOW");
+    add(KEY_BLUE, "KEY_BLUE");
+    add(KEY_CHANNELUP, "KEY_CHANNELUP");
+    add(KEY_CHANNELDOWN, "KEY_CHANNELDOWN");
+    add(KEY_FIRST, "KEY_FIRST");
+    add(KEY_LAST, "KEY_LAST");
+    add(KEY_AB, "KEY_AB");
+    add(KEY_NEXT, "KEY_NEXT");
+    add(KEY_RESTART, "KEY_RESTART");
+    add(KEY_SLOW, "KEY_SLOW");
+    add(KEY_SHUFFLE, "KEY_SHUFFLE");
+    add(KEY_BREAK, "KEY_BREAK");
+    add(KEY_PREVIOUS, "KEY_PREVIOUS");
+    add(KEY_DIGITS, "KEY_DIGITS");
+    add(KEY_TEEN, "KEY_TEEN");
+    add(KEY_TWEN, "KEY_TWEN");
+    add(KEY_VIDEOPHONE, "KEY_VIDEOPHONE");
+    add(KEY_GAMES, "KEY_GAMES");
+    add(KEY_ZOOMIN, "KEY_ZOOMIN");
+    add(KEY_ZOOMOUT, "KEY_ZOOMOUT");
+    add(KEY_ZOOMRESET, "KEY_ZOOMRESET");
+    add(KEY_WORDPROCESSOR, "KEY_WORDPROCESSOR");
+    add(KEY_EDITOR, "KEY_EDITOR");
+    add(KEY_SPREADSHEET, "KEY_SPREADSHEET");
+    add(KEY_GRAPHICSEDITOR, "KEY_GRAPHICSEDITOR");
+    add(KEY_PRESENTATION, "KEY_PRESENTATION");
+    add(KEY_DATABASE, "KEY_DATABASE");
+    add(KEY_NEWS, "KEY_NEWS");
+    add(KEY_VOICEMAIL, "KEY_VOICEMAIL");
+    add(KEY_ADDRESSBOOK, "KEY_ADDRESSBOOK");
+    add(KEY_MESSENGER, "KEY_MESSENGER");
+    add(KEY_DISPLAYTOGGLE, "KEY_DISPLAYTOGGLE");
+    add(KEY_SPELLCHECK, "KEY_SPELLCHECK");
+    add(KEY_LOGOFF, "KEY_LOGOFF");
+    add(KEY_DOLLAR, "KEY_DOLLAR");
+    add(KEY_EURO, "KEY_EURO");
+    add(KEY_FRAMEBACK, "KEY_FRAMEBACK");
+    add(KEY_FRAMEFORWARD, "KEY_FRAMEFORWARD");
+    add(KEY_CONTEXT_MENU, "KEY_CONTEXT_MENU");
+#ifdef KEY_MEDIA_REPEAT
+    add(KEY_MEDIA_REPEAT, "KEY_MEDIA_REPEAT");
+#endif
+    add(KEY_DEL_EOL, "KEY_DEL_EOL");
+    add(KEY_DEL_EOS, "KEY_DEL_EOS");
+    add(KEY_INS_LINE, "KEY_INS_LINE");
+    add(KEY_DEL_LINE, "KEY_DEL_LINE");
+    add(KEY_FN, "KEY_FN");
+    add(KEY_FN_ESC, "KEY_FN_ESC");
+    add(KEY_FN_F1, "KEY_FN_F1");
+    add(KEY_FN_F2, "KEY_FN_F2");
+    add(KEY_FN_F3, "KEY_FN_F3");
+    add(KEY_FN_F4, "KEY_FN_F4");
+    add(KEY_FN_F5, "KEY_FN_F5");
+    add(KEY_FN_F6, "KEY_FN_F6");
+    add(KEY_FN_F7, "KEY_FN_F7");
+    add(KEY_FN_F8, "KEY_FN_F8");
+    add(KEY_FN_F9, "KEY_FN_F9");
+    add(KEY_FN_F10, "KEY_FN_F10");
+    add(KEY_FN_F11, "KEY_FN_F11");
+    add(KEY_FN_F12, "KEY_FN_F12");
+    add(KEY_FN_1, "KEY_FN_1");
+    add(KEY_FN_2, "KEY_FN_2");
+    add(KEY_FN_D, "KEY_FN_D");
+    add(KEY_FN_E, "KEY_FN_E");
+    add(KEY_FN_F, "KEY_FN_F");
+    add(KEY_FN_S, "KEY_FN_S");
+    add(KEY_FN_B, "KEY_FN_B");
+    add(KEY_BRL_DOT1, "KEY_BRL_DOT1");
+    add(KEY_BRL_DOT2, "KEY_BRL_DOT2");
+    add(KEY_BRL_DOT3, "KEY_BRL_DOT3");
+    add(KEY_BRL_DOT4, "KEY_BRL_DOT4");
+    add(KEY_BRL_DOT5, "KEY_BRL_DOT5");
+    add(KEY_BRL_DOT6, "KEY_BRL_DOT6");
+    add(KEY_BRL_DOT7, "KEY_BRL_DOT7");
+    add(KEY_BRL_DOT8, "KEY_BRL_DOT8");
+    add(KEY_BRL_DOT9, "KEY_BRL_DOT9");
+    add(KEY_BRL_DOT10, "KEY_BRL_DOT10");
+    add(KEY_MIN_INTERESTING, "KEY_MIN_INTERESTING");
+  }
+} evdev_btn_names;
+
+class Keysym2Keycode
+{
+public:
+  // Map KeySym to kernel keycode
+  std::map<KeySym, int> mapping;
+
+  Keysym2Keycode()
+  {
+    // std::cout << "Initing Keysym2Keycode" << std::endl;
+
+    Display *dpy = XOpenDisplay(NULL);
+    if (!dpy)
+    {
+      throw std::runtime_error("Keysym2Keycode: Couldn't open X11 display");
+    }
+    else
+    {
+      process_keymap(dpy);
+      XCloseDisplay(dpy);
+    }
+  }
+
+  void process_keymap(Display *dpy)
+  {
+    int min_keycode, max_keycode;
+    XDisplayKeycodes(dpy, &min_keycode, &max_keycode);
+
+    int num_keycodes = max_keycode - min_keycode + 1;
+    int keysyms_per_keycode;
+    KeySym *keymap = XGetKeyboardMapping(dpy, min_keycode,
+                                         num_keycodes,
+                                         &keysyms_per_keycode);
+
+    for (int i = 0; i < num_keycodes; ++i)
+    {
+      if (keymap[i * keysyms_per_keycode] != NoSymbol)
+      {
+        KeySym keysym = keymap[i * keysyms_per_keycode];
+        // FIXME: Duplicate entries confuse the conversion
+        // std::map<KeySym, int>::iterator it = mapping.find(keysym);
+        // if (it != mapping.end())
+        //   std::cout << "Duplicate keycode: " << i << std::endl;
+        mapping[keysym] = i;
+      }
+    }
+
+    XFree(keymap);
+  }
+};
+
+int xkeysym2keycode(const std::string &name)
+{
+  static Keysym2Keycode sym2code;
+
+  KeySym keysym = XStringToKeysym(name.substr(3).c_str());
+
+  if (keysym == NoSymbol)
+  {
+    throw std::runtime_error("xkeysym2keycode: Couldn't convert name '" + name + "' to xkeysym");
+  }
+
+  std::map<KeySym, int>::iterator i = sym2code.mapping.find(keysym);
+  if (i == sym2code.mapping.end())
+  {
+    throw std::runtime_error("xkeysym2keycode: Couldn't convert xkeysym '" + name + "' to evdev keycode");
+  }
+  else
+  {
+    if (0)
+      std::cout << name << " -> " << keysym << " -> " << XKeysymToString(keysym)
+                << " -> " << btn2str(i->second) << "(" << i->second << ")" << std::endl;
+    return i->second;
+  }
+}
+
+bool str2event(const std::string &name, int &type, int &code)
+{
+  if (name == "void" || name == "none")
+  {
+    type = -1;
+    code = -1;
+    return true;
+  }
+  else if (name.compare(0, 3, "REL") == 0)
+  {
+    type = EV_REL;
+    code = evdev_rel_names[name];
+    return true;
+  }
+  else if (name.compare(0, 3, "ABS") == 0)
+  {
+    type = EV_ABS;
+    code = evdev_abs_names[name];
+    return true;
+  }
+  else if (name.compare(0, 2, "XK") == 0)
+  {
+    type = EV_KEY;
+    code = xkeysym2keycode(name);
+    return true;
+  }
+  else if (name.compare(0, 2, "JS") == 0)
+  {
+    int int_value = 0;
+    std::istringstream(name.substr(3)) >> int_value;
+
+    type = EV_KEY;
+    code = BTN_JOYSTICK + int_value;
+
+    return true;
+  }
+  else if (name.compare(0, 3, "KEY") == 0 ||
+           name.compare(0, 3, "BTN") == 0)
+  {
+    type = EV_KEY;
+    code = evdev_btn_names[name];
+    return true;
+  }
+  else
+  {
+    return false;
+  }
+}
+
+std::string btn2str(int i)
+{
+  return evdev_btn_names[i];
+}
+
+std::string abs2str(int i)
+{
+  return evdev_abs_names[i];
+}
+
+std::string rel2str(int i)
+{
+  return evdev_rel_names[i];
+}
+
+/* EOF */

+ 68 - 0
g29_ros2_feedback_drive/src/g29_ros2_feedback/src/evdev_helper.hpp

@@ -0,0 +1,68 @@
+/*
+**  Xbox360 USB Gamepad Userspace Driver
+**  Copyright (C) 2008 Ingo Ruhnke <grumbel@gmail.com>
+**
+**  This program is free software: you can redistribute it and/or modify
+**  it under the terms of the GNU General Public License as published by
+**  the Free Software Foundation, either version 3 of the License, or
+**  (at your option) any later version.
+**
+**  This program is distributed in the hope that it will be useful,
+**  but WITHOUT ANY WARRANTY; without even the implied warranty of
+**  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+**  GNU General Public License for more details.
+**
+**  You should have received a copy of the GNU General Public License
+**  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef HEADER_EVDEV_HELPER_HPP
+#define HEADER_EVDEV_HELPER_HPP
+
+#include <string>
+
+/**
+ * 将字符串转换为事件类型和代码
+ * 该函数根据输入的字符串名称,将对应的事件类型和代码通过引用参数返回
+ * 主要用于解析事件名称字符串,以便在程序中以标准化的方式处理不同的输入事件
+ * 
+ * @param name 事件名称的字符串表示
+ * @param type 事件类型,通过引用返回,如按键事件、绝对值事件等
+ * @param code 事件代码,通过引用返回,具体含义依赖于事件类型
+ * @return bool 表示转换是否成功,成功则为true,否则为false
+ */
+bool str2event(const std::string& name, int& type, int& code);
+
+/**
+ * 将按钮事件代码转换为字符串
+ * 根据输入的按钮事件代码,返回其对应的字符串表示
+ * 用于将按钮事件代码映射到人类可读的字符串,便于日志记录或界面显示
+ * 
+ * @param i 按钮事件代码
+ * @return std::string 按钮事件的字符串表示
+ */
+std::string btn2str(int i);
+
+/**
+ * 将绝对值事件代码转换为字符串
+ * 根据输入的绝对值事件代码,返回其对应的字符串表示
+ * 用于将绝对值事件代码映射到人类可读的字符串,便于日志记录或界面显示
+ * 
+ * @param i 绝对值事件代码
+ * @return std::string 绝对值事件的字符串表示
+ */
+std::string abs2str(int i);
+
+/**
+ * 将相对值事件代码转换为字符串
+ * 根据输入的相对值事件代码,返回其对应的字符串表示
+ * 用于将相对值事件代码映射到人类可读的字符串,便于日志记录或界面显示
+ * 
+ * @param i 相对值事件代码
+ * @return std::string 相对值事件的字符串表示
+ */
+std::string rel2str(int i);
+
+#endif
+
+/* EOF */

+ 492 - 0
g29_ros2_feedback_drive/src/g29_ros2_feedback/src/joystick.cpp

@@ -0,0 +1,492 @@
+/*
+**  jstest-gtk - A graphical joystick tester
+**  Copyright (C) 2009 Ingo Ruhnke <grumbel@gmail.com>
+**
+**  This program is free software: you can redistribute it and/or modify
+**  it under the terms of the GNU General Public License as published by
+**  the Free Software Foundation, either version 3 of the License, or
+**  (at your option) any later version.
+**
+**  This program is distributed in the hope that it will be useful,
+**  but WITHOUT ANY WARRANTY; without even the implied warranty of
+**  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+**  GNU General Public License for more details.
+**
+**  You should have received a copy of the GNU General Public License
+**  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include <assert.h>
+#include <math.h>
+#include <algorithm>
+#include <iostream>
+#include <stdexcept>
+#include <stdint.h>
+#include <string.h>
+#include <errno.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <sstream>
+#include <unistd.h>
+#include <linux/joystick.h>
+#include "evdev_helper.hpp"
+#include "joystick.h"
+
+// 构造函数 Joystick_pub,用于初始化一个 Joystick_pub 对象
+// 参数 filename_ 是 joystick 设备文件的路径
+Joystick_pub::Joystick_pub(const std::string &filename, const std::string &node_name)
+    : filename(filename), Node(node_name) // 初始化成员变量 filename
+{
+    heartbeat = 0;
+    // 尝试以只读模式打开 joystick 设备文件
+    if ((fd = open(filename.c_str(), O_RDONLY | O_NONBLOCK)) < 0)
+    {
+        // 如果打开失败,抛出 runtime_error 异常,并附带错误信息
+        std::ostringstream str;
+        str << filename << ": " << strerror(errno);
+        throw std::runtime_error(str.str());
+    }
+    else
+    {
+        // 获取设备上的轴数和按钮数
+        uint8_t num_axis = 0;                  // 存储轴数量
+        uint8_t num_button = 0;                // 存储按钮数量
+        ioctl(fd, JSIOCGAXES, &num_axis);      // 使用 ioctl 获取轴数量
+        ioctl(fd, JSIOCGBUTTONS, &num_button); // 使用 ioctl 获取按钮数量
+        axis_count = num_axis;                 // 设置 axis_count 成员变量
+        button_count = num_button;             // 设置 button_count 成员变量
+
+        // 获取设备名称
+        char name_c_str[1024]; // 临时存储设备名称的字符数组
+        if (ioctl(fd, JSIOCGNAME(sizeof(name_c_str)), name_c_str) < 0)
+        {
+            // 如果获取名称失败,抛出 runtime_error 异常,并附带错误信息
+            std::ostringstream str;
+            str << filename << ": " << strerror(errno);
+            throw std::runtime_error(str.str());
+        }
+        else
+        {
+            // 获取成功,保存原始名称
+            orig_name = name_c_str;
+            name = orig_name; // 保存设备名称
+        }
+
+        // 初始化轴状态向量,大小等于轴的数量
+        axis_state.resize(axis_count);
+        button_state.reserve(button_count);
+    }
+
+    // 获取并保存初始校准数据
+    orig_calibration_data = get_calibration();
+
+    // 输出设备信息
+    std::cout << "Joystick_pub device info:" << std::endl;
+    std::cout << "Name: " << name << std::endl;
+    std::cout << "Number of Axes: " << axis_count << std::endl;
+    std::cout << "Number of Buttons: " << button_count << std::endl;
+
+    g29_publisher = this->create_publisher<g29_msg::msg::G29Msg>("g29_feedback", 10);
+    g29_pub_timer = this->create_wall_timer(std::chrono::milliseconds(10), std::bind(&Joystick_pub::on_in_publish, this));
+}
+
+Joystick_pub::~Joystick_pub()
+{
+    // connection.disconnect();
+    close(fd);
+}
+
+bool Joystick_pub::on_in_publish()
+{
+    update();  // 读取并更新摇杆状态
+    publish(); // 发布摇杆按钮
+    return true;
+}
+
+/**
+ * 更新摇杆状态
+ *
+ * 从摇杆设备文件中读取事件,并根据事件更新摇杆的状态
+ * 如果读取事件失败或发生错误,将抛出异常
+ */
+void Joystick_pub::update()
+{
+    // 定义一个 js_event 结构体来存储读取的事件
+    struct js_event event;
+    // 从摇杆设备文件中读取事件
+    ssize_t len = read(fd, &event, sizeof(event));
+    heartbeat++;
+    if (heartbeat > 100)
+    {
+        heartbeat = 0;
+    }
+    // 如果读取长度为负值,则读取失败,抛出异常
+    if (len < 0)
+    {
+        return;
+        // 构建错误信息
+        // std::ostringstream str;
+        // str << filename << ": " << strerror(errno);
+        // // 抛出运行时错误异常
+        // throw std::runtime_error(str.str());
+    }
+    // 如果读取长度与事件大小一致,则读取成功
+    else if (len == sizeof(event))
+    {
+        // 如果事件类型包含轴事件
+        if (event.type & JS_EVENT_AXIS)
+        {
+            axis_update(event.number, event.value);
+        }
+        // 如果事件类型包含按钮事件
+        else if (event.type & JS_EVENT_BUTTON)
+        {
+            // 调用按钮移动的处理函数
+            button_update(event.number, event.value);
+        }
+    }
+    // 如果读取长度既不为负值也不为事件大小,则发生未知错误,抛出异常
+    else
+    {
+        throw std::runtime_error("Joystick_pub::update(): unknown read error");
+    }
+}
+
+void Joystick_pub::publish()
+{
+    auto message = g29_msg::msg::G29Msg();
+
+    message.heartbeat = heartbeat;
+    message.axis0 = axis_state[0];
+    message.axis1 = axis_state[1];
+    message.axis2 = axis_state[2];
+    message.axis3 = axis_state[3];
+    message.axis4 = axis_state[4];
+    message.axis5 = axis_state[5];
+
+    message.button0 = button_state[0];
+    message.button1 = button_state[1];
+    message.button2 = button_state[2];
+    message.button3 = button_state[3];
+    message.button4 = button_state[4];
+    message.button5 = button_state[5];
+    message.button6 = button_state[6];
+    message.button7 = button_state[7];
+    message.button8 = button_state[8];
+    message.button9 = button_state[9];
+    message.button10 = button_state[10];
+    message.button11 = button_state[11];
+    message.button12 = button_state[12];
+    message.button13 = button_state[13];
+    message.button14 = button_state[14];
+    message.button15 = button_state[15];
+    message.button16 = button_state[16];
+    message.button17 = button_state[17];
+    message.button18 = button_state[18];
+    message.button19 = button_state[19];
+    message.button20 = button_state[20];
+    message.button21 = button_state[21];
+    message.button22 = button_state[22];
+    message.button23 = button_state[23];
+    message.button24 = button_state[24];
+
+    g29_publisher->publish(message);
+}
+
+// 轴移动的处理函数
+void Joystick_pub::axis_update(int axis, int value)
+{
+    // 更新轴的状态
+    axis_state[axis] = value;
+    // 调用轴移动的处理函数
+    // std::cout << "Axis " << axis << " moved to value: " << axis_state[axis] << std::endl;
+}
+
+// 按钮移动的处理函数
+void Joystick_pub::button_update(int button, int value)
+{
+    // 处理按钮事件
+    button_state[button] = value;
+    // std::cout << "Button " << button << " moved to value: " << button_state[button] << std::endl;
+}
+
+Joystick_pub::CalibrationData corr2cal(const struct js_corr &corr_)
+{
+    struct js_corr corr = corr_;
+
+    Joystick_pub::CalibrationData data;
+
+    if (corr.type)
+    {
+        data.calibrate = true;
+        data.invert = (corr.coef[2] < 0 && corr.coef[3] < 0);
+        data.center_min = corr.coef[0];
+        data.center_max = corr.coef[1];
+
+        if (data.invert)
+        {
+            corr.coef[2] = -corr.coef[2];
+            corr.coef[3] = -corr.coef[3];
+        }
+
+        // Need to use double and rint(), since calculation doesn't end
+        // up on clean integer positions (i.e. 0.9999 can happen)
+        data.range_min = rint(data.center_min - ((32767.0 * 16384) / corr.coef[2]));
+        data.range_max = rint((32767.0 * 16384) / corr.coef[3] + data.center_max);
+    }
+    else
+    {
+        data.calibrate = false;
+        data.invert = false;
+        data.center_min = 0;
+        data.center_max = 0;
+        data.range_min = 0;
+        data.range_max = 0;
+    }
+
+    return data;
+}
+
+std::vector<Joystick_pub::CalibrationData> Joystick_pub::get_calibration()
+{
+    std::vector<struct js_corr> corr(get_axis_count());
+
+    if (ioctl(fd, JSIOCGCORR, &*corr.begin()) < 0)
+    {
+        std::ostringstream str;
+        str << filename << ": " << strerror(errno);
+        throw std::runtime_error(str.str());
+    }
+    else
+    {
+        std::vector<CalibrationData> data;
+        std::transform(corr.begin(), corr.end(), std::back_inserter(data), corr2cal);
+        return data;
+    }
+}
+
+struct js_corr cal2corr(const Joystick_pub::CalibrationData &data)
+{
+    struct js_corr corr;
+
+    if (data.calibrate &&
+        (data.center_min - data.range_min) != 0 &&
+        (data.range_max - data.center_max) != 0)
+    {
+        corr.type = 1;
+        corr.prec = 0;
+        corr.coef[0] = data.center_min;
+        corr.coef[1] = data.center_max;
+
+        corr.coef[2] = (32767 * 16384) / (data.center_min - data.range_min);
+        corr.coef[3] = (32767 * 16384) / (data.range_max - data.center_max);
+
+        if (data.invert)
+        {
+            corr.coef[2] = -corr.coef[2];
+            corr.coef[3] = -corr.coef[3];
+        }
+    }
+    else
+    {
+        corr.type = 0;
+        corr.prec = 0;
+        memset(corr.coef, 0, sizeof(corr.coef));
+    }
+
+    return corr;
+}
+
+void Joystick_pub::set_calibration(const std::vector<CalibrationData> &data)
+{
+    std::vector<struct js_corr> corr;
+
+    std::transform(data.begin(), data.end(), std::back_inserter(corr), cal2corr);
+
+    if (ioctl(fd, JSIOCSCORR, &*corr.begin()) < 0)
+    {
+        std::ostringstream str;
+        str << filename << ": " << strerror(errno);
+        throw std::runtime_error(str.str());
+    }
+}
+
+void Joystick_pub::clear_calibration()
+{
+    std::vector<CalibrationData> data;
+
+    for (int i = 0; i < get_axis_count(); ++i)
+    {
+        CalibrationData cal;
+
+        cal.calibrate = false;
+        cal.invert = false;
+        cal.center_min = 0;
+        cal.center_max = 0;
+        cal.range_min = 0;
+        cal.range_max = 0;
+
+        data.push_back(cal);
+    }
+
+    set_calibration(data);
+}
+
+void Joystick_pub::reset_calibration()
+{
+    set_calibration(orig_calibration_data);
+}
+
+std::vector<int> Joystick_pub::get_button_mapping()
+{
+    uint16_t btnmap[KEY_MAX - BTN_MISC + 1];
+    if (ioctl(fd, JSIOCGBTNMAP, btnmap) < 0)
+    {
+        std::ostringstream str;
+        str << filename << ": " << strerror(errno);
+        throw std::runtime_error(str.str());
+    }
+    else
+    {
+        std::vector<int> mapping;
+        std::copy(btnmap, btnmap + button_count, std::back_inserter(mapping));
+        return mapping;
+    }
+}
+
+std::vector<int> Joystick_pub::get_axis_mapping()
+{
+    uint8_t axismap[ABS_MAX + 1];
+    if (ioctl(fd, JSIOCGAXMAP, axismap) < 0)
+    {
+        std::ostringstream str;
+        str << filename << ": " << strerror(errno);
+        throw std::runtime_error(str.str());
+    }
+    else
+    {
+        std::vector<int> mapping;
+        std::copy(axismap, axismap + axis_count, std::back_inserter(mapping));
+        return mapping;
+    }
+}
+
+void Joystick_pub::set_button_mapping(const std::vector<int> &mapping)
+{
+    assert((int)mapping.size() == button_count);
+
+    uint16_t btnmap[KEY_MAX - BTN_MISC + 1];
+    memset(btnmap, 0, sizeof(btnmap));
+    std::copy(mapping.begin(), mapping.end(), btnmap);
+
+    if (0)
+        for (int i = 0; i < button_count; ++i)
+        {
+            std::cout << i << " -> " << btnmap[i] << std::endl;
+        }
+
+    if (ioctl(fd, JSIOCSBTNMAP, btnmap) < 0)
+    {
+        std::ostringstream str;
+        str << filename << ": " << strerror(errno);
+        throw std::runtime_error(str.str());
+    }
+}
+
+int Joystick_pub::get_axis_state(int id)
+{
+    if (id >= 0 && id < (int)axis_state.size())
+    {
+        return axis_state[id];
+    }
+    else
+    {
+        return 0;
+    }
+}
+
+int Joystick_pub::get_button_state(int id)
+{
+    if (id >= 0 && id < get_button_count())
+    {
+        return button_state[id];
+    }
+    else
+    {
+        return 0;
+    }
+}
+
+void Joystick_pub::set_axis_mapping(const std::vector<int> &mapping)
+{
+    assert((int)mapping.size() == axis_count);
+
+    uint8_t axismap[ABS_MAX + 1];
+
+    std::copy(mapping.begin(), mapping.end(), axismap);
+
+    if (ioctl(fd, JSIOCSAXMAP, axismap) < 0)
+    {
+        std::ostringstream str;
+        str << filename << ": " << strerror(errno);
+        throw std::runtime_error(str.str());
+    }
+}
+
+void Joystick_pub::correct_calibration(const std::vector<int> &mapping_old, const std::vector<int> &mapping_new)
+{
+    int axes[ABS_MAX + 1]; // axes[name] -> old_idx
+    for (std::vector<int>::const_iterator i = mapping_old.begin(); i != mapping_old.end(); ++i)
+    {
+        axes[*i] = i - mapping_old.begin();
+    }
+
+    std::vector<CalibrationData> callib_old = get_calibration();
+    std::vector<CalibrationData> callib_new;
+    for (std::vector<int>::const_iterator i = mapping_new.begin(); i != mapping_new.end(); ++i)
+    {
+        callib_new.push_back(callib_old[axes[*i]]);
+    }
+
+    set_calibration(callib_new);
+}
+
+std::string Joystick_pub::get_evdev() const
+{
+    // See /usr/share/doc/linux-doc-2.6.28/devices.txt.gz
+    for (int i = 0; i < 32; ++i)
+    {
+        std::ostringstream out;
+        out << "/dev/input/event" << i;
+
+        int evdev_fd;
+        if ((evdev_fd = open(out.str().c_str(), O_RDONLY)) < 0)
+        {
+            // ignore
+        }
+        else
+        {
+            char evdev_name[256];
+            if (ioctl(evdev_fd, EVIOCGNAME(sizeof(evdev_name)), evdev_name) < 0)
+            {
+                std::cout << out.str() << ": " << strerror(errno) << std::endl;
+            }
+            else
+            {
+                if (orig_name == evdev_name)
+                {
+                    // Found a device that matches, so return it
+                    close(evdev_fd);
+                    return out.str();
+                }
+            }
+
+            close(evdev_fd);
+        }
+    }
+
+    throw std::runtime_error("couldn't find evdev for " + filename);
+}
+
+/* EOF */

+ 236 - 0
g29_ros2_feedback_drive/src/g29_ros2_feedback/src/joystick.h

@@ -0,0 +1,236 @@
+/*
+**  jstest-gtk - A graphical joystick tester
+**  Copyright (C) 2009 Ingo Ruhnke <grumbel@gmail.com>
+**
+**  This program is free software: you can redistribute it and/or modify
+**  it under the terms of the GNU General Public License as published by
+**  the Free Software Foundation, either version 3 of the License, or
+**  (at your option) any later version.
+**
+**  This program is distributed in the hope that it will be useful,
+**  but WITHOUT ANY WARRANTY; without even the implied warranty of
+**  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+**  GNU General Public License for more details.
+**
+**  You should have received a copy of the GNU General Public License
+**  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef HEADER_JSTEST_GTK_JOYSTICK_HPP
+#define HEADER_JSTEST_GTK_JOYSTICK_HPP
+
+#include <linux/joystick.h>
+#include "joystick_description.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+#include "g29_msg/msg/g29_msg.hpp"
+
+// 声明前向引用,减少编译依赖
+class XMLReader;
+class XMLWriter;
+
+/**
+ * Joystick_pub 类封装了与游戏手柄相关的操作,包括读取手柄事件、校准、
+ * 设置轴和按钮的映射等。它还提供了加载和保存手柄配置的功能。
+ */
+class Joystick_pub : public rclcpp::Node
+{
+public:
+    /**
+     * 校准数据结构,用于存储手柄轴的校准信息。
+     */
+    struct CalibrationData
+    {
+        bool calibrate; // 是否进行校准
+        bool invert;    // 是否反转轴的方向
+        int center_min; // 中心位置的最小值
+        int center_max; // 中心位置的最大值
+        int range_min;  // 轴的最小值
+        int range_max;  // 轴的最大值
+    };
+
+private:
+
+    int fd; // 文件描述符,用于与设备通信
+    std::string filename;  // 设备文件名
+    std::string orig_name; // 原始设备名称
+    std::string name;      // 手柄名称
+    int axis_count;        // 轴的数量
+    int button_count;      // 按钮的数量
+    int heartbeat;
+    std::vector<int> axis_state;                        // 轴的状态
+    std::vector<int> button_state;                      // 按钮的状态
+    std::vector<CalibrationData> orig_calibration_data; // 原始校准数据
+
+    rclcpp::Publisher<g29_msg::msg::G29Msg>::SharedPtr g29_publisher;
+    rclcpp::TimerBase::SharedPtr g29_pub_timer;
+
+public:
+    /**
+     * 构造函数,初始化Joystick_pub对象。
+     * @param filename 设备文件名。
+     */
+    Joystick_pub(const std::string &filename, const std::string &node_name = "g29_publisher_node");
+
+    /**
+     * 析构函数,释放资源。
+     */
+    ~Joystick_pub();
+
+    /**
+     * 获取文件描述符。
+     * @return 文件描述符。
+     */
+    int get_fd() const { return fd; }
+
+    /**
+     * 更新手柄状态。
+     */
+    void update();
+
+    /**
+     * 接收USB数据,并用ros2消息发布
+     */
+    bool on_in_publish();
+
+    void publish();
+
+    /**
+     * 获取设备文件名。
+     * @return 设备文件名。
+     */
+    std::string get_filename() const { return filename; }
+
+    /**
+     * 获取手柄名称。
+     * @return 手柄名称。
+     */
+    std::string get_name() const { return name; }
+
+    /**
+     * 获取轴的数量。
+     * @return 轴的数量。
+     */
+    int get_axis_count() const { return axis_count; }
+
+    /**
+     * 获取按钮的数量。
+     * @return 按钮的数量。
+     */
+    int get_button_count() const { return button_count; }
+
+    // 轴移动的处理函数
+    void axis_update(int axis, int value);
+
+    // 按钮移动的处理函数
+    void button_update(int button, int value);
+
+    /**
+     * 获取指定轴的状态。
+     * @param id 轴的标识符。
+     * @return 轴的状态。
+     */
+    int get_axis_state(int id);
+
+    /**
+     * 获取指定按钮的状态。
+     * @param id 按钮的标识符。
+     * @return 按钮的状态。
+     */
+    int get_button_state(int id);
+
+    /**
+     * 获取系统中所有手柄的描述信息。
+     * @return 手柄描述信息列表。
+     */
+    static std::vector<JoystickDescription> get_joysticks();
+
+    /**
+     * 获取校准数据。
+     * @return 校准数据列表。
+     */
+    std::vector<CalibrationData> get_calibration();
+
+    /**
+     * 设置校准数据。
+     * @param data 校准数据列表。
+     */
+    void set_calibration(const std::vector<CalibrationData> &data);
+
+    /**
+     * 重置校准数据到默认值。
+     */
+    void reset_calibration();
+
+    /**
+     * 清除所有校准数据,注意这将意味着使用原始USB输入值,
+     * 而不是缩放后的值。
+     */
+    void clear_calibration();
+
+    /**
+     * 获取按钮映射。
+     * @return 按钮映射列表。
+     */
+    std::vector<int> get_button_mapping();
+
+    /**
+     * 获取轴映射。
+     * @return 轴映射列表。
+     */
+    std::vector<int> get_axis_mapping();
+
+    /**
+     * 设置按钮映射。
+     * @param mapping 按钮映射列表。
+     */
+    void set_button_mapping(const std::vector<int> &mapping);
+
+    /**
+     * 设置轴映射。
+     * @param mapping 轴映射列表。
+     */
+    void set_axis_mapping(const std::vector<int> &mapping);
+
+    /**
+     * 在重新映射轴后修正校准数据。
+     * @param mapping_old 旧的轴映射。
+     * @param mapping_new 新的轴映射。
+     */
+    void correct_calibration(const std::vector<int> &mapping_old, const std::vector<int> &mapping_new);
+
+    /**
+     * 将手柄配置写入XML。
+     * @param out XML写入器。
+     */
+    void write(XMLWriter &out);
+
+    /**
+     * 从XML加载手柄配置。
+     * @param reader XML读取器。
+     */
+    void load(const XMLReader &reader);
+
+    /**
+     * 获取此手柄设备基于的evdev设备文件名。这个调用只是一个猜测,
+     * 不保证是相同的设备文件,但对于我们的用途来说已经足够。
+     * @return evdev设备文件名。
+     */
+    std::string get_evdev() const;
+
+private:
+    /**
+     * 禁止拷贝构造函数,防止对象拷贝。
+     */
+    Joystick_pub(const Joystick_pub &);
+
+    /**
+     * 禁止赋值操作符,防止对象赋值。
+     */
+    Joystick_pub &operator=(const Joystick_pub &);
+};
+
+#endif
+
+/* EOF */

+ 47 - 0
g29_ros2_feedback_drive/src/g29_ros2_feedback/src/joystick_description.hpp

@@ -0,0 +1,47 @@
+/*
+**  jstest-gtk - A graphical joystick tester
+**  Copyright (C) 2009 Ingo Ruhnke <grumbel@gmail.com>
+**
+**  This program is free software: you can redistribute it and/or modify
+**  it under the terms of the GNU General Public License as published by
+**  the Free Software Foundation, either version 3 of the License, or
+**  (at your option) any later version.
+**
+**  This program is distributed in the hope that it will be useful,
+**  but WITHOUT ANY WARRANTY; without even the implied warranty of
+**  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+**  GNU General Public License for more details.
+**
+**  You should have received a copy of the GNU General Public License
+**  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef HEADER_JSTEST_GTK_JOYSTICK_DESCRIPTION_HPP
+#define HEADER_JSTEST_GTK_JOYSTICK_DESCRIPTION_HPP
+
+#include <string>
+#include <vector>
+
+class JoystickDescription
+{
+private:
+public:
+  std::string filename;
+  std::string name;
+  int axis_count;
+  int button_count;
+
+  JoystickDescription(const std::string& filename_,
+                      const std::string& name_,
+                      int axis_count_,
+                      int button_count_)
+    : filename(filename_),
+      name(name_),
+      axis_count(axis_count_),
+      button_count(button_count_)
+  {}
+};
+
+#endif
+
+/* EOF */

+ 22 - 0
g29_ros2_feedback_drive/src/g29_ros2_feedback/src/main.cpp

@@ -0,0 +1,22 @@
+/*
+ * @Author: wangyingjie 2778809626@qq.com
+ * @Date: 2025-05-22 11:00:44
+ * @LastEditors: wangyingjie 2778809626@qq.com
+ * @LastEditTime: 2025-05-22 15:37:06
+ * @FilePath: /g29_ros2_feedback/src/g29_ros2_feedback/src/main.cpp
+ * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
+ */
+
+#include "joystick.h"
+
+int main(int argc, char *argv[])
+{
+
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<Joystick_pub>("/dev/input/js0");
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+
+    return 0;
+}
+

+ 14 - 0
readme.md

@@ -0,0 +1,14 @@
+<!--
+ * @Author: wangyingjie 2778809626@qq.com
+ * @Date: 2025-05-22 17:32:41
+ * @LastEditors: wangyingjie 2778809626@qq.com
+ * @LastEditTime: 2025-05-22 17:34:38
+ * @FilePath: \undefinedd:\7_work\1_nzzn\2_code\6_drive\Drive\readme.md
+ * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
+-->
+
+# 驱动项目
+
+## g29_ros2_feedback_drive
+
+该项目是罗技G29手柄的ros2驱动程序,用于将手柄的输入转换为ROS2消息,并发布到ROS2话题中。