✨ feat: Support joy_ps4 control

README.md

@@ -143,6 +143,17 @@ world:=rmul_2024
 
   - `world` : 仿真世界名，关联栅格地图的读取。可选参数 `rmul_2024` or `rmuc_2024`。
 
+3.4 ps4 手柄控制
+
+![ps4](https://gmedia.playstation.com/is/image/SIEPDC/ps4-accessories-ds4-glacier-white-screen-01-en-28oct20?$1600px$)
+
+默认情况下，PS4 手柄控制已开启。
+
+左肩键：安全按键，按下后才会发布控制指令到 `cmd_vel`  
+右肩键：加速按键，按下后会使速度控制指令变为原先的两倍  
+左摇杆：发布线速度  
+右摇杆：发布加速度
+
 ## TODO
 
 - [x] 优化 lidar_odom 和 odom 的关系。目前可以看到机器人底盘实际上是沉在地图下的，雷达与地图高度重合

pb2025_nav_bringup/config/simulation/nav2_params.yaml

@@ -306,3 +306,35 @@ velocity_smoother:
     odom_duration: 0.1
     deadband_velocity: [0.0, 0.0, 0.0]
     velocity_timeout: 1.0
+
+teleop_twist_joy_node:
+  ros__parameters:
+    axis_linear:
+      x: 1                        # 左摇杆的水平轴
+      y: 0                        # 左摇杆的垂直轴
+      z: -1                       # Disable
+    scale_linear:
+      x: 1.5
+      y: 1.5
+      z: 0.0
+    scale_linear_turbo:
+      x: 3.0
+      y: 3.0
+      z: 0.0
+
+    axis_angular:
+      roll: -1                    # Disable
+      pitch: -1                   # Disable
+      yaw: 3                      # 右摇杆的水平轴
+    scale_angular:
+      roll: 0.0
+      pitch: 0.0
+      yaw: 3.0
+    scale_angular_turbo:
+      roll: 0.0
+      pitch: 0.0
+      yaw: 6.0
+
+    require_enable_button: true
+    enable_button: 4              # L1 shoulder button
+    enable_turbo_button: 5        # R1 shoulder button

pb2025_nav_bringup/launch/joy_teleop_launch.py

@@ -0,0 +1,115 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.conditions import IfCondition
+from launch.actions import DeclareLaunchArgument, GroupAction
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node, PushRosNamespace
+from launch_ros.descriptions import ParameterFile
+from nav2_common.launch import RewrittenYaml
+
+
+def generate_launch_description():
+    # Get the package directory
+    bringup_dir = get_package_share_directory("pb2025_nav_bringup")
+
+    # Create the launch configuration variables
+    namespace = LaunchConfiguration("namespace")
+    use_namespace = LaunchConfiguration("use_namespace")
+    joy_vel = LaunchConfiguration("joy_vel")
+    joy_dev = LaunchConfiguration("joy_dev")
+    joy_config_file = LaunchConfiguration("joy_config_file")
+    publish_stamped_twist = LaunchConfiguration("publish_stamped_twist")
+
+    configured_params = ParameterFile(
+        RewrittenYaml(
+            source_file=joy_config_file,
+            root_key=namespace,
+            param_rewrites={},
+            convert_types=True),
+        allow_substs=True)
+
+    # Declare the launch arguments
+    declare_namespace_cmd = DeclareLaunchArgument(
+        "namespace",
+        default_value="",
+        description="Top-level namespace",
+    )
+
+    declare_use_namespace_cmd = DeclareLaunchArgument(
+        "use_namespace",
+        default_value="false",
+        description="Whether to apply a namespace to the navigation stack",
+    )
+
+    declare_joy_vel_cmd = DeclareLaunchArgument(
+        "joy_vel",
+        default_value="cmd_vel",
+        description="The topic to publish velocity commands to",
+    )
+
+    declare_joy_config_file_cmd = DeclareLaunchArgument(
+        "joy_config_file",
+        default_value=os.path.join(bringup_dir, "config", "simulation", "nav2_params.yaml"),
+        description="The joystick configuration file path",
+    )
+
+    declare_joy_dev_cmd = DeclareLaunchArgument(
+        "joy_dev", default_value="0", description="The joystick device ID"
+    )
+
+    declare_publish_stamped_twist_cmd = DeclareLaunchArgument(
+        "publish_stamped_twist",
+        default_value="false",
+        description="Whether to publish stamped twist messages",
+    )
+
+    bringup_cmd_group = GroupAction(
+        [
+            PushRosNamespace(condition=IfCondition(use_namespace), namespace=namespace),
+            Node(
+                package="joy",
+                executable="joy_node",
+                name="joy_node",
+                output="screen",
+                parameters=[
+                    {
+                        "device_id": joy_dev,
+                        "deadzone": 0.3,
+                        "autorepeat_rate": 20.0,
+                    }
+                ],
+            ),
+            Node(
+                package="teleop_twist_joy",
+                executable="teleop_node",
+                name="teleop_twist_joy_node",
+                output="screen",
+                parameters=[
+                    configured_params,
+                    {
+                        "publish_stamped_twist": publish_stamped_twist,
+                    },
+                ],
+                remappings=[("/cmd_vel", joy_vel)],
+            ),
+        ]
+    )
+
+    # Create the launch description and populate
+    ld = LaunchDescription()
+
+    # Declare the launch options
+    ld.add_action(declare_namespace_cmd)
+    ld.add_action(declare_use_namespace_cmd)
+    ld.add_action(declare_joy_vel_cmd)
+    ld.add_action(declare_joy_config_file_cmd)
+    ld.add_action(declare_joy_dev_cmd)
+    ld.add_action(declare_publish_stamped_twist_cmd)
+
+    # Add the actions to launch the nodes
+    ld.add_action(bringup_cmd_group)
+
+    return ld

pb2025_nav_bringup/launch/rm_sentry_simulation_launch.py

@@ -159,6 +159,16 @@ def generate_launch_description():
         }.items(),
     )
 
+    joy_teleop_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(os.path.join(launch_dir, "joy_teleop_launch.py")),
+        launch_arguments={
+            "namespace": namespace,
+            "use_namespace": use_namespace,
+            "use_sim_time": use_sim_time,
+            "joy_config_file": params_file,
+        }.items(),
+    )
+
     ld = LaunchDescription()
 
     # Declare the launch options
@@ -178,6 +188,7 @@ def generate_launch_description():
     ld.add_action(start_velodyne_convert_tool)
     ld.add_action(start_point_lio_node)
     ld.add_action(bringup_cmd)
+    ld.add_action(joy_teleop_cmd)
     ld.add_action(rviz_cmd)
 
     return ld

pb2025_nav_bringup/package.xml

@@ -14,6 +14,7 @@
   <depend>nav2_common</depend>
   <depend>slam_toolbox</depend>
   <depend>small_gicp_relocalization</depend>
+  <depend>teleop_twist_joy</depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>