✨ feat: Add submodule pb_teleop_twist_joy

.gitmodules

@@ -13,3 +13,6 @@
 [submodule "terrain_analysis"]
 	path = terrain_analysis
 	url = https://github.com/SMBU-PolarBear-Robotics-Team/terrain_analysis.git
+[submodule "pb_teleop_twist_joy"]
+	path = pb_teleop_twist_joy
+	url = https://github.com/SMBU-PolarBear-Robotics-Team/pb_teleop_twist_joy.git

README.md

@@ -186,10 +186,7 @@
 
 默认情况下，PS4 手柄控制已开启。键位映射关系详见 [nav2_params.yaml](./pb2025_nav_bringup/config/simulation/nav2_params.yaml) 中的 `teleop_twist_joy_node` 部分。
 
-左肩键：安全按键，按下后才会发布控制指令到 `cmd_vel`
-右肩键：加速按键，按下后会使速度控制指令变为原先的两倍
-左摇杆：发布线速度
-右摇杆：发布角速度
+![teleop_twist_joy.gif](https://raw.githubusercontent.com/LihanChen2004/picx-images-hosting/master/teleop_twist_joy.5j4aav3v3p.gif)
 
 ## TODO
 

pb2025_nav_bringup/config/reality/nav2_params.yaml

@@ -151,7 +151,7 @@ bt_navigator:
     robot_base_frame: gimbal_yaw_fake
     odom_topic: odometry
     bt_loop_duration: 10
-    default_server_timeout: 20
+    default_server_timeout: 100
     wait_for_service_timeout: 1000
     default_nav_to_pose_bt_xml: $(find-pkg-share pb2025_nav_bringup)/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml # or $(find-pkg-share my_package)/behavior_tree/my_nav_to_pose_bt.xml
     default_nav_through_poses_bt_xml: $(find-pkg-share pb2025_nav_bringup)/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml # or $(find-pkg-share my_package)/behavior_tree/my_nav_through_poses_bt.xml
@@ -512,34 +512,37 @@ velocity_smoother:
     deadband_velocity: [0.0, 0.0, 0.0]
     velocity_timeout: 1.0
 
-teleop_twist_joy_node:
+pb_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
+    robot_base_frame: gimbal_yaw
+    control_mode: auto_control    # Option: auto_control, manual_control
 
     require_enable_button: true
     enable_button: 4              # L1 shoulder button
     enable_turbo_button: 5        # R1 shoulder button
+
+    axis_chassis:
+      x: 1                        # Left thumb stick vertical
+      y: 0                        # Left thumb stick horizontal
+      yaw: 6                      # button_left and button_right
+    scale_chassis:
+      x: 2.5
+      y: 2.5
+      yaw: 3.0
+    scale_chassis_turbo:
+      x: 4.0
+      y: 4.0
+      yaw: 6.0
+
+    axis_gimbal:
+      roll: -1                    # Disable
+      pitch: 4                    # Right thumb stick vertical
+      yaw: 3                      # Right thumb stick horizontal
+    scale_gimbal:
+      roll: 0.0
+      pitch: -0.5
+      yaw: 2.0
+    scale_gimbal_turbo:
+      roll: 0.0
+      pitch: -1.0
+      yaw: 3.0

pb2025_nav_bringup/config/simulation/nav2_params.yaml

@@ -151,7 +151,7 @@ bt_navigator:
     robot_base_frame: gimbal_yaw_fake
     odom_topic: odometry
     bt_loop_duration: 10
-    default_server_timeout: 20
+    default_server_timeout: 100
     wait_for_service_timeout: 1000
     default_nav_to_pose_bt_xml: $(find-pkg-share pb2025_nav_bringup)/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml # or $(find-pkg-share my_package)/behavior_tree/my_nav_to_pose_bt.xml
     default_nav_through_poses_bt_xml: $(find-pkg-share pb2025_nav_bringup)/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml # or $(find-pkg-share my_package)/behavior_tree/my_nav_through_poses_bt.xml
@@ -512,34 +512,37 @@ velocity_smoother:
     deadband_velocity: [0.0, 0.0, 0.0]
     velocity_timeout: 1.0
 
-teleop_twist_joy_node:
+pb_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
+    robot_base_frame: gimbal_yaw
+    control_mode: auto_control    # Option: auto_control, manual_control
 
     require_enable_button: true
     enable_button: 4              # L1 shoulder button
     enable_turbo_button: 5        # R1 shoulder button
+
+    axis_chassis:
+      x: 1                        # Left thumb stick vertical
+      y: 0                        # Left thumb stick horizontal
+      yaw: 6                      # button_left and button_right
+    scale_chassis:
+      x: 2.5
+      y: 2.5
+      yaw: 3.0
+    scale_chassis_turbo:
+      x: 4.0
+      y: 4.0
+      yaw: 6.0
+
+    axis_gimbal:
+      roll: -1                    # Disable
+      pitch: 4                    # Right thumb stick vertical
+      yaw: 3                      # Right thumb stick horizontal
+    scale_gimbal:
+      roll: 0.0
+      pitch: -0.5
+      yaw: 2.0
+    scale_gimbal_turbo:
+      roll: 0.0
+      pitch: -1.0
+      yaw: 3.0

pb2025_nav_bringup/launch/joy_teleop_launch.py

@@ -101,17 +101,21 @@ def generate_launch_description():
                 ],
             ),
             Node(
-                package="teleop_twist_joy",
-                executable="teleop_node",
-                name="teleop_twist_joy_node",
+                package="pb_teleop_twist_joy",
+                executable="pb_teleop_twist_joy_node",
+                name="pb_teleop_twist_joy_node",
                 output="screen",
                 parameters=[
                     configured_params,
                     {
                         "publish_stamped_twist": publish_stamped_twist,
                     },
                 ],
-                remappings=[("/cmd_vel", joy_vel)],
+                remappings=[
+                    ("/cmd_vel", joy_vel),
+                    ("/tf", "tf"),
+                    ("/tf_static", "tf_static"),
+                ],
             ),
         ]
     )

pb2025_nav_bringup/package.xml

@@ -18,8 +18,8 @@
   <exec_depend>navigation2</exec_depend>
   <exec_depend>nav2_common</exec_depend>
   <exec_depend>spatio_temporal_voxel_layer</exec_depend>
-  <exec_depend>teleop_twist_joy</exec_depend>
   <exec_depend>pb2025_sentry_nav</exec_depend>
+  <exec_depend>pb_teleop_twist_joy</exec_depend>
   <exec_depend>livox_ros_driver2</exec_depend>
   <exec_depend>point_lio</exec_depend>
   <exec_depend>terrain_analysis</exec_depend>