From d733863f2e0337837a7a949df263a55868f0d605 Mon Sep 17 00:00:00 2001 From: LihanChen2004 <757003373@qq.com> Date: Mon, 13 Jan 2025 15:42:19 +0800 Subject: [PATCH] =?UTF-8?q?=E2=9C=A8=20feat:=20Add=20submodule=20pb=5Ftele?= =?UTF-8?q?op=5Ftwist=5Fjoy?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitmodules | 3 + README.md | 5 +- .../config/reality/nav2_params.yaml | 57 ++--- .../config/simulation/nav2_params.yaml | 57 ++--- .../launch/joy_teleop_launch.py | 12 +- pb2025_nav_bringup/package.xml | 2 +- .../rviz/nav2_default_view.rviz | 221 ++++++++++++++++-- pb_teleop_twist_joy | 1 + 8 files changed, 278 insertions(+), 80 deletions(-) create mode 160000 pb_teleop_twist_joy diff --git a/.gitmodules b/.gitmodules index 99868d2..3e40780 100644 --- a/.gitmodules +++ b/.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 diff --git a/README.md b/README.md index 1c13db0..e40dd6e 100644 --- a/README.md +++ b/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 diff --git a/pb2025_nav_bringup/config/reality/nav2_params.yaml b/pb2025_nav_bringup/config/reality/nav2_params.yaml index 4aca9b7..0145ab5 100644 --- a/pb2025_nav_bringup/config/reality/nav2_params.yaml +++ b/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 diff --git a/pb2025_nav_bringup/config/simulation/nav2_params.yaml b/pb2025_nav_bringup/config/simulation/nav2_params.yaml index 2f31fc2..e87d673 100644 --- a/pb2025_nav_bringup/config/simulation/nav2_params.yaml +++ b/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 diff --git a/pb2025_nav_bringup/launch/joy_teleop_launch.py b/pb2025_nav_bringup/launch/joy_teleop_launch.py index 03243a3..9fbda18 100644 --- a/pb2025_nav_bringup/launch/joy_teleop_launch.py +++ b/pb2025_nav_bringup/launch/joy_teleop_launch.py @@ -101,9 +101,9 @@ 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, @@ -111,7 +111,11 @@ def generate_launch_description(): "publish_stamped_twist": publish_stamped_twist, }, ], - remappings=[("/cmd_vel", joy_vel)], + remappings=[ + ("/cmd_vel", joy_vel), + ("/tf", "tf"), + ("/tf_static", "tf_static"), + ], ), ] ) diff --git a/pb2025_nav_bringup/package.xml b/pb2025_nav_bringup/package.xml index 65d7204..9e6230e 100644 --- a/pb2025_nav_bringup/package.xml +++ b/pb2025_nav_bringup/package.xml @@ -18,8 +18,8 @@ navigation2 nav2_common spatio_temporal_voxel_layer - teleop_twist_joy pb2025_sentry_nav + pb_teleop_twist_joy livox_ros_driver2 point_lio terrain_analysis diff --git a/pb2025_nav_bringup/rviz/nav2_default_view.rviz b/pb2025_nav_bringup/rviz/nav2_default_view.rviz index e6675ff..f9819eb 100644 --- a/pb2025_nav_bringup/rviz/nav2_default_view.rviz +++ b/pb2025_nav_bringup/rviz/nav2_default_view.rviz @@ -5,8 +5,10 @@ Panels: Property Tree Widget: Expanded: - /Global Options1 + - /Camera1 + - /Camera1/Visibility1 Splitter Ratio: 0.5833333134651184 - Tree Height: 850 + Tree Height: 441 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -41,6 +43,114 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order + armor_0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + armor_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + armor_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + armor_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + armor_support_frame_0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + armor_support_frame_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + armor_support_frame_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + armor_support_frame_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + chassis: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_industrial_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_industrial_camera_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + front_left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_mid360: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_rplidar_a2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gimbal_pitch: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gimbal_yaw: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + light_indicator: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + speed_monitor: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true Mass Properties: Inertia: false Mass: false @@ -54,22 +164,62 @@ Visualization Manager: Frame Timeout: 15 Frames: All Enabled: false + armor_0: + Value: true + armor_1: + Value: true + armor_2: + Value: true + armor_3: + Value: true + armor_support_frame_0: + Value: true + armor_support_frame_1: + Value: true + armor_support_frame_2: + Value: true + armor_support_frame_3: + Value: true + base_footprint: + Value: true + chassis: + Value: true + front_industrial_camera: + Value: true + front_industrial_camera_optical_frame: + Value: true + front_mid360: + Value: true + front_rplidar_a2: + Value: true + gimbal_pitch: + Value: true + light_indicator: + Value: true + map: + Value: true + odom: + Value: true + speed_monitor: + Value: true Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true Show Names: false Tree: - {} + map: + odom: + {} Update Interval: 0 Value: true - Class: rviz_default_plugins/Axes Enabled: false Length: 1 Name: GimbalYawFake - Radius: 0.1 + Radius: 0.10000000149011612 Reference Frame: gimbal_yaw_fake - Value: true + Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -103,7 +253,7 @@ Visualization Manager: Value: rplidar_a2/scan Use Fixed Frame: true Use rainbow: true - Value: true + Value: false - Alpha: 0.20000000298023224 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -270,7 +420,7 @@ Visualization Manager: Position Transformer: XYZ Selectable: true Size (Pixels): 3 - Size (m): 0.05 + Size (m): 0.05000000074505806 Style: Boxes Topic: Depth: 5 @@ -310,13 +460,13 @@ Visualization Manager: Reliability Policy: Reliable Value: local_plan Value: true - - Alpha: 0.8 + - Alpha: 0.800000011920929 Class: rviz_default_plugins/PointStamped Color: 204; 41; 204 Enabled: true History Length: 1 Name: Lookahead Point - Radius: 0.2 + Radius: 0.20000000298023224 Topic: Depth: 5 Durability Policy: Volatile @@ -352,6 +502,41 @@ Visualization Manager: Reliability Policy: Reliable Value: waypoints Value: true + - Class: rviz_default_plugins/Camera + Enabled: true + Far Plane Distance: 100 + Image Rendering: background and overlay + Name: Camera + Overlay Alpha: 0.5 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /red_standard_robot1/front_camera/image + Value: true + Visibility: + Controller: + Local Costmap: true + Local Plan: true + Lookahead Point: true + Polygon: true + STVL Layer Voxels: true + Value: true + GimbalYawFake: true + Global Planner: + Global Costmap: true + Path: true + Polygon: true + Value: true + LaserScan: true + Map: true + RobotModel: false + Sensor Scan: true + TF: false + Value: true + WayPoints: true + Zoom Factor: 1 Enabled: true Global Options: Background Color: 48; 48; 48 @@ -384,25 +569,27 @@ Visualization Manager: Class: rviz_default_plugins/Orbit Distance: 31.64445686340332 Enable Stereo Rendering: - Stereo Eye Separation: 0.06 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 8 - Y: 0 - Z: 0 + X: 9.899405479431152 + Y: -1.6569770574569702 + Z: 0.006887615658342838 Focal Shape Fixed Size: true - Focal Shape Size: 0.05 + Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View - Near Clip Distance: 0.01 + Near Clip Distance: 0.009999999776482582 Pitch: 1.5671751499176025 Target Frame: Value: Orbit (rviz_default_plugins) - Yaw: 3.14 + Yaw: 3.140000104904175 Saved: ~ Window Geometry: + Camera: + collapsed: false Displays: collapsed: false Height: 1376 @@ -410,13 +597,13 @@ Window Geometry: Hide Right Dock: false Navigation 2: collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000015600000506fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000038f000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003201000003d2000001710000012600ffffff000000010000010f00000506fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000506000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003810000050600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000015600000506fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d00000386000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e00200032000000003d000005060000013900fffffffb0000000c00430061006d00650072006103000003200000001b0000040700000260000000010000010f00000506fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000506000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000040d0000050600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Tool Properties: collapsed: false Views: collapsed: false - Width: 1245 + Width: 1037 X: 70 Y: 27 diff --git a/pb_teleop_twist_joy b/pb_teleop_twist_joy new file mode 160000 index 0000000..bd14179 --- /dev/null +++ b/pb_teleop_twist_joy @@ -0,0 +1 @@ +Subproject commit bd141797075235e7ba0b403483e780caf19795da