lx/Go2_navigation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: 初始化 Go2 导航代码与配置

Browse Source
This commit is contained in:
龙正扬应用学生后端
2026-10-14 11:08:44 +08:00
parent 8e16ecceba
commit 9dab37e8c8
1258 changed files with 1936673 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

0
unitree_slam/config/.gitkeep Executable file
View File

25
unitree_slam/config/clean_log.sh Executable file
View File

@@ -0,0 +1,25 @@
#!/bin/bash
# 检查是否提供了路径参数
if [ $# -ne 1 ]; then
echo "Usage: $0 <directory_path>"
echo "Example: $0 /path/to/directory"
exit 1
fi
# 检查提供的路径是否存在
if [ ! -d "$1" ]; then
echo "Error: Directory '$1' does not exist"
exit 1
fi
# 显示将要执行的操作
echo "Will delete files older than 7 days in directory: $1"
# 使用find命令查找并删除超过7天的文件
# -type f: 只查找文件
# -mtime +7: 查找修改时间超过7天的文件
# -exec rm -f {} \;: 对每个找到的文件执行rm命令
find "$1" -type f -mtime +7 -exec rm -f {} \;
echo "Cleanup completed!"

12
unitree_slam/config/cyclonedds.xml Executable file
View File

@@ -0,0 +1,12 @@
<CycloneDDS>
<Domain>
<General>
<Interfaces>
<NetworkInterface name="wlp0s20f3" priority="default" multicast="default"/>
</Interfaces>
<!-- <Internal>-->
<!-- <MinimumSocketReceiveBufferSize>20MB</MinimumSocketReceiveBufferSize>-->
<!-- </Internal>-->
</General>
</Domain>
</CycloneDDS>

14
unitree_slam/config/dds_config.json Executable file
View File

@@ -0,0 +1,14 @@
{
"DomainId": 0,
"Config": "/home/unitree/graph_pid_ws/src/slam_server_interfaces/config/cyclonedds.xml",
"Participant":
{
"Qos":
{
"Reliability":
{
"Kind": "RELIABLE_RELIABILITY_QOS"
}
}
}
}

128
unitree_slam/config/gridmap_config/config.yaml Executable file
View File

@@ -0,0 +1,128 @@
B2:
grid_map:
revolution: 0.1 # 分辨率 （m）
wide: 8.0 # 宽度 （m）
T_Dog2lidar: [0.415 ,0.0, 0.04,0.0 ,0.0, 0.0] # xyz rpy （m）
lidar_height: 0.7 # 高度 （m）
point_cloud_topic: "rt/rslidar_points"
#odom_topic: "rt/dog_odom" # base odom (not lidar link)
odom_topic: "rt/lio_sam_ros2/mapping/re_location_odometry"
gridmap_topic: "rt/gridmap"
## 碰撞检测 为了保证安全，应当设置比机器人大一点，触发会急停
collision_x_range: [-0.3, 0.4, 10] # x_min x_max resolution
collision_y_range: [-0.2, 0.2, 5] # y_min y_max resolution
## 预警范围 需要大于碰撞检测范围，进行潜在碰撞的预警
warning_x_range: [-0.65, 0.7 , 10] # x_min x_max resolution
warning_y_range: [-0.3, 0.3, 6] # y_min y_max resolution
## 预警范围 会根据当前速度推测未来机器人位置，进行潜在碰撞的预警
predict_num: 3 #预测未来碰撞风险 步数
predict_time: 2.0 #预测未来碰撞时间
## 日志路径
log_path: "logs/gridmap/"
B2_W:
grid_map:
revolution: 0.1 # 分辨率 （m）
wide: 8.0 # 宽度 （m）
T_Dog2lidar: [0.415 ,0.0, 0.04,0.0 ,0.0, 0.0] # xyz rpy （m）
lidar_height: 0.8 # 高度 （m）
point_cloud_topic: "rt/rslidar_points"
#odom_topic: "rt/dog_odom" # base odom (not lidar link)
odom_topic: "rt/lio_sam_ros2/mapping/re_location_odometry"
gridmap_topic: "rt/gridmap"
## 碰撞检测 为了保证安全，应当设置比机器人大一点，触发会急停
collision_x_range: [-0.3, 0.4, 10] # x_min x_max resolution
collision_y_range: [-0.2, 0.2, 5] # y_min y_max resolution
## 预警范围 需要大于碰撞检测范围，进行潜在碰撞的预警
warning_x_range: [-0.35, 0.7 , 10] # x_min x_max resolution
warning_y_range: [-0.3, 0.3, 6] # y_min y_max resolution
## 预警范围 会根据当前速度推测未来机器人位置，进行潜在碰撞的预警
predict_num: 3 #预测未来碰撞风险 步数
predict_time: 2.0 #预测未来碰撞时间
## 日志路径
log_path: "logs/gridmap/"
G1:
grid_map:
revolution: 0.1 # 分辨率 （m）
wide: 4.0 # 宽度 （m）
T_Dog2lidar: [0.0 ,0.0, 0.0 ,3.1415926 ,0.0, 0.0] # xyz rpy （m）
lidar_height: 1.28 # 高度 （m）
point_cloud_topic: "rt/utlidar/cloud_livox_mid360"
#odom_topic: "rt/dog_odom" # base odom (not lidar link)
odom_topic: "rt/unitree/slam_relocation/odom"
gridmap_topic: "rt/gridmap"
## 碰撞检测 为了保证安全，应当设置比机器人大一点，触发会急停
collision_x_range: [-0.1, 0.2, 4] # x_min x_max resolution
collision_y_range: [-0.05, 0.05, 3] # y_min y_max resolution
## 预警范围 需要大于碰撞检测范围，进行潜在碰撞的预警
warning_x_range: [-0.0, 0.6 , 5] # x_min x_max resolution
warning_y_range: [-0.4, 0.4, 5] # y_min y_max resolution
## 预警范围 会根据当前速度推测未来机器人位置，进行潜在碰撞的预警
predict_num: 3 #预测未来碰撞风险 步数
predict_time: 2.0 #预测未来碰撞时间
## 日志路径
log_path: "logs/gridmap/"
Go2_W:
grid_map:
revolution: 0.05 # 分辨率 （m）
wide: 6.0 # 宽度 （m）
T_Dog2lidar: [0.0 ,0.0, 0.0 ,0.0 ,0.226, 0.0] # xyz rpy （m）
lidar_height: 0.4 # 高度 （m）
point_cloud_topic: "rt/unitree/slam_lidar/points"
#odom_topic: "rt/dog_odom" # base odom (not lidar link)
odom_topic: "rt/unitree/slam_relocation/odom"
gridmap_topic: "rt/gridmap"
## 碰撞检测 为了保证安全，应当设置比机器人大一点，触发会急停
collision_x_range: [-0.2, 0.2, 5] # x_min x_max resolution
collision_y_range: [-0.05, 0.05, 3] # y_min y_max resolution
## 预警范围 需要大于碰撞检测范围，进行潜在碰撞的预警
warning_x_range: [-0.2, 0.3 , 5] # x_min x_max resolution
warning_y_range: [-0.05, 0.05, 3] # y_min y_max resolution
## 预警范围 会根据当前速度推测未来机器人位置，进行潜在碰撞的预警
predict_num: 3 #预测未来碰撞风险 步数
predict_time: 2.0 #预测未来碰撞时间
## 日志路径
log_path: "logs/gridmap/"
Go2:
grid_map:
revolution: 0.1 # 分辨率 （m）
wide: 8.0 # 宽度
# T_Dog2lidar: [ 0.1701 ,0.0, 0.0908 ,0.0 ,0.0 , 1.57 ] # xyz rpy （m）xt16
# T_Dog2lidar: [ 0.0 ,0.0, 0.0 ,0.0 ,0.226 , 0.0 ] # xyz rpy （m）mid360
T_Dog2lidar: [0.1870, 0.0, 0.0803, 0.0, 0.226893, 0.0]
lidar_height: 0.4 # 高度 （m）
point_cloud_topic: "rt/unitree/slam_lidar/points"
#odom_topic: "rt/dog_odom" # base odom (not lidar link)
odom_topic: "rt/unitree/slam_relocation/odom"
gridmap_topic: "rt/gridmap"
## 碰撞检测 为了保证安全，应当设置比机器人大一点，触发会急停
collision_x_range: [-0.2, 0.2, 5] # x_min x_max resolution
collision_y_range: [-0.05, 0.05, 3] # y_min y_max resolution
## 预警范围 需要大于碰撞检测范围，进行潜在碰撞的预警
warning_x_range: [-0.2, 0.3 , 5] # x_min x_max resolution
warning_y_range: [-0.05, 0.05, 3] # y_min y_max resolution
## 预警范围 会根据当前速度推测未来机器人位置，进行潜在碰撞的预警
predict_num: 3 #预测未来碰撞风险 步数
predict_time: 2.0 #预测未来碰撞时间
## 日志路径
log_path: "logs/gridmap/"

9
unitree_slam/config/gridmap_config/demo.json Executable file
View File

@@ -0,0 +1,9 @@
{
"type": "collision",
"data": "{}",
"errorCode": 421,
"info": "There are obstacles nearby, please be careful",
"nanosec": 631140099,
"sec": 1749028957
}

402
unitree_slam/config/gridmap_config/ele.rviz Executable file
View File

@@ -0,0 +1,402 @@
Panels:
- Class: rviz_common/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /TF1/Frames1
- /ele_cloud1
- /ele_cloud1/Autocompute Value Bounds1
- /Odometry1
- /Odometry1/Shape1
Splitter Ratio: 0.5
Tree Height: 641
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
Expanded:
- /2D Goal Pose1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz_common/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz_common/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: Coll
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz_default_plugins/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Class: rviz_default_plugins/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: false
Marker Scale: 1
Name: TF
Show Arrows: false
Show Axes: true
Show Names: true
Tree:
{}
Update Interval: 0
Value: true
- Class: rviz_common/Group
Displays:
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 237; 51; 59
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: Coll
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /collision_clouds
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 46; 194; 126
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: SafeBase
Position Transformer: XYZ
Selectable: true
Size (Pixels): 8
Size (m): 0.009999999776482582
Style: Points
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /safe_clouds
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 255; 120; 0
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: warring
Position Transformer: XYZ
Selectable: true
Size (Pixels): 15
Size (m): 0.009999999776482582
Style: Points
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /warning_clouds
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: pre_cloud
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Points
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /pre_safe_clouds
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 245; 194; 17
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: pre_warring
Position Transformer: XYZ
Selectable: true
Size (Pixels): 8
Size (m): 0.009999999776482582
Style: Points
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /pre_collision_clouds
Use Fixed Frame: true
Use rainbow: true
Value: true
Enabled: true
Name: coll
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 0.514559268951416
Min Value: -0.05097384750843048
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 255; 255; 255
Color Transformer: AxisColor
Decay Time: 1
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: ele_cloud
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Points
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /ele_clouds
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 0.699999988079071
Class: rviz_default_plugins/Map
Color Scheme: costmap
Draw Behind: false
Enabled: true
Name: Map
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /global_map
Update Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /global_map_updates
Use Timestamp: false
Value: true
- Angle Tolerance: 0.10000000149011612
Class: rviz_default_plugins/Odometry
Covariance:
Orientation:
Alpha: 0.5
Color: 255; 255; 127
Color Style: Unique
Frame: Local
Offset: 1
Scale: 1
Value: true
Position:
Alpha: 0.30000001192092896
Color: 204; 51; 204
Scale: 1
Value: true
Value: true
Enabled: true
Keep: 1
Name: Odometry
Position Tolerance: 0.10000000149011612
Shape:
Alpha: 1
Axes Length: 0.5
Axes Radius: 0.05000000074505806
Color: 255; 25; 0
Head Length: 0.30000001192092896
Head Radius: 0.10000000149011612
Shaft Length: 1
Shaft Radius: 0.05000000074505806
Value: Axes
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /unitree/slam_relocation/odom
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: map
Frame Rate: 30
Name: root
Tools:
- Class: rviz_default_plugins/Interact
Hide Inactive Objects: true
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
- Class: rviz_default_plugins/FocusCamera
- Class: rviz_default_plugins/Measure
Line color: 128; 128; 0
- Class: rviz_default_plugins/SetInitialPose
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /initialpose
- Class: rviz_default_plugins/SetGoal
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /goal_pose
- Class: rviz_default_plugins/PublishPoint
Single click: true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /clicked_point
Transformation:
Current:
Class: rviz_default_plugins/TF
Value: true
Views:
Current:
Angle: -0.8699986338615417
Class: rviz_default_plugins/TopDownOrtho
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Scale: 97.70134735107422
Target Frame: base_link
Value: TopDownOrtho (rviz_default_plugins)
X: 0.13703224062919617
Y: 1.097522258758545
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 986
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd0000000400000000000001560000030cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000030c000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000030cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000030c000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006480000006efc0100000002fb0000000800540069006d00650100000000000006480000005800fffffffb0000000800540069006d00650100000000000004500000000000000000000003d70000030c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1608
X: 72
Y: 27

63
unitree_slam/config/mid360/mid360_config.json Executable file
View File

@@ -0,0 +1,63 @@
{
"lidar_summary_info" : {
"lidar_type": 8
},
"Mid360s": {
"lidar_net_info" : {
"cmd_data_port": 56100,
"push_msg_port": 56200,
"point_data_port": 56300,
"imu_data_port": 56400,
"log_data_port": 56500
},
"host_net_info" : {
"cmd_data_ip" : "192.168.123.18",
"cmd_data_port": 56101,
"push_msg_ip": "192.168.123.18",
"push_msg_port": 56201,
"point_data_ip": "192.168.123.18",
"point_data_port": 56301,
"imu_data_ip" : "192.168.123.18",
"imu_data_port": 56401,
"log_data_ip" : "",
"log_data_port": 56501
}
},
"MID360": {
"lidar_net_info" : {
"cmd_data_port": 56100,
"push_msg_port": 56200,
"point_data_port": 56300,
"imu_data_port": 56400,
"log_data_port": 56500
},
"host_net_info" : {
"cmd_data_ip" : "192.168.123.18",
"cmd_data_port": 56101,
"push_msg_ip": "192.168.123.18",
"push_msg_port": 56201,
"point_data_ip": "192.168.123.18",
"point_data_port": 56301,
"imu_data_ip" : "192.168.123.18",
"imu_data_port": 56401,
"log_data_ip" : "",
"log_data_port": 56501
}
},
"lidar_configs" : [
{
"ip" : "192.168.123.20",
"pcl_data_type" : 1,
"pattern_mode" : 0,
"extrinsic_parameter" : {
"roll": 0.0,
"pitch": 0.0,
"yaw": 0.0,
"x": 0,
"y": 0,
"z": 0
}
}
]
}

5
unitree_slam/config/mid360/user_config.yaml Executable file
View File

@@ -0,0 +1,5 @@
network: "eth0"
lidar_frequency: 20 # int
lidar_frame: "livox_frame" # string
lidar_topic: "rt/unitree/slam_lidar/points" # string rt/....
imu_topic: "rt/unitree/slam_lidar/imu" # string rt/....

107
unitree_slam/config/pl_mapping/mid360.yaml Executable file
View File

@@ -0,0 +1,107 @@
G1:
imu_topic: "rt/utlidar/imu_livox_mid360" #/livox/imu rt/utlidar/imu_livox_mid360
lidar_topic: "rt/utlidar/cloud_livox_mid360"
imu_acc_multi: 10.0 #10.0
imu_gyro_multi: 1.0
filter_num: 8 #8
lidar_scan_min_range: 0.25
lidar_scan_max_range: 30.0
map_frame: "map"
body_frame: "livox_frame"
pub_point_topic: "rt/unitree/slam_mapping/points"
pub_odom_topic: "rt/unitree/slam_mapping/odom"
imu_init_num: 100
vel_cov: 20.0
acc_cov: 50000.0
gyro_cov: 1000.0
b_acc_cov: 0.0001
b_gyro_cov: 0.0001
lidar_meas_cov_inv: 100.0
imu_meas_acc_cov_inv: 1.0
imu_meas_gyro_cov_inv: 10.0
satu_acc: 20.0
satu_gyro: 35.0
scan_resolution: 0.3 #0.3
map_resolution: 0.3
near_search_num: 5.0
move_thresh: 1.5
det_range: 60.0
cube_len: 300.0
pose_imu_lidar: [1.0, 0.0, 0.0, 0.0, -0.011, -0.02329, 0.04412]
save_pcd: true
save_pcd_res: 0.15
dist_sq_threshold: 2.0
plane_threshold: 0.1
ground_ang: 75.0 # degree
Go2:
imu_topic: "rt/unitree/slam_lidar/imu" #/livox/imu rt/utlidar/imu_livox_mid360
lidar_topic: "rt/unitree/slam_lidar/points"
imu_acc_multi: 10.0 #10.0
imu_gyro_multi: 1.0
filter_num: 25 #8
lidar_scan_min_range: 0.25
lidar_scan_max_range: 30.0
map_frame: "map"
body_frame: "livox_frame"
pub_point_topic: "rt/unitree/slam_mapping/points"
pub_odom_topic: "rt/unitree/slam_mapping/odom"
imu_init_num: 100
vel_cov: 20.0
acc_cov: 50000.0
gyro_cov: 1000.0
b_acc_cov: 0.0001
b_gyro_cov: 0.0001
lidar_meas_cov_inv: 100.0
imu_meas_acc_cov_inv: 1.0
imu_meas_gyro_cov_inv: 10.0
satu_acc: 20.0
satu_gyro: 35.0
scan_resolution: 0.3 #0.3
map_resolution: 0.3
near_search_num: 5.0
move_thresh: 1.5
det_range: 60.0
cube_len: 300.0
pose_imu_lidar: [1.0, 0.0, 0.0, 0.0, -0.011, -0.02329, 0.04412]
save_pcd: true
save_pcd_res: 0.15
dist_sq_threshold: 2.0
plane_threshold: 0.1
ground_ang: 75.0 # degree
Go2_W:
imu_topic: "rt/unitree/slam_lidar/imu" #/livox/imu rt/utlidar/imu_livox_mid360
lidar_topic: "rt/unitree/slam_lidar/points"
imu_acc_multi: 10.0 #10.0
imu_gyro_multi: 1.0
filter_num: 25 #8
lidar_scan_min_range: 0.25
lidar_scan_max_range: 30.0
map_frame: "map"
body_frame: "livox_frame"
pub_point_topic: "rt/unitree/slam_mapping/points"
pub_odom_topic: "rt/unitree/slam_mapping/odom"
imu_init_num: 100
vel_cov: 20.0
acc_cov: 50000.0
gyro_cov: 1000.0
b_acc_cov: 0.0001
b_gyro_cov: 0.0001
lidar_meas_cov_inv: 100.0
imu_meas_acc_cov_inv: 1.0
imu_meas_gyro_cov_inv: 10.0
satu_acc: 20.0
satu_gyro: 35.0
scan_resolution: 0.3 #0.3
map_resolution: 0.3
near_search_num: 5.0
move_thresh: 1.5
det_range: 60.0
cube_len: 300.0
pose_imu_lidar: [1.0, 0.0, 0.0, 0.0, -0.011, -0.02329, 0.04412]
save_pcd: true
save_pcd_res: 0.15
dist_sq_threshold: 2.0
plane_threshold: 0.1
ground_ang: 75.0 # degree

107
unitree_slam/config/pl_mapping/xt16.yaml Executable file
View File

@@ -0,0 +1,107 @@
G1:
imu_topic: "rt/dog_imu_raw" #/livox/imu rt/utlidar/imu_livox_mid360
lidar_topic: "rt/rslidar_points"
imu_acc_multi: 1.0 #10.0
imu_gyro_multi: 1.0
filter_num: 4 #8
lidar_scan_min_range: 0.25
lidar_scan_max_range: 20.0
map_frame: "map"
body_frame: "rslidar"
pub_point_topic: "rt/unitree/slam_mapping/points"
pub_odom_topic: "rt/unitree/slam_mapping/odom"
imu_init_num: 100
vel_cov: 20.0
acc_cov: 500.0
gyro_cov: 1000.0
b_acc_cov: 0.0001
b_gyro_cov: 0.0001
lidar_meas_cov_inv: 100.0
imu_meas_acc_cov_inv: 10.0
imu_meas_gyro_cov_inv: 10.0
satu_acc: 20.0
satu_gyro: 35.0
scan_resolution: 0.4 #0.3
map_resolution: 0.4
near_search_num: 5.0
move_thresh: 1.5
det_range: 60.0
cube_len: 300.0
pose_imu_lidar: [0.707, 0.0, 0.0, 0.707, 0.1710, 0, 0.0908]
save_pcd: true
save_pcd_res: 0.15
dist_sq_threshold: 2.0
plane_threshold: 0.1
ground_ang: 75.0 # degree
Go2:
imu_topic: "rt/dog_imu_raw" #/livox/imu rt/utlidar/imu_livox_mid360
lidar_topic: "rt/unitree/slam_lidar/points"
imu_acc_multi: 1.0 #10.0
imu_gyro_multi: 1.0
filter_num: 35 #8
lidar_scan_min_range: 0.25
lidar_scan_max_range: 20.0
map_frame: "map"
body_frame: "rslidar"
pub_point_topic: "rt/unitree/slam_mapping/points"
pub_odom_topic: "rt/unitree/slam_mapping/odom"
imu_init_num: 100
vel_cov: 20.0
acc_cov: 500.0
gyro_cov: 1000.0
b_acc_cov: 0.0001
b_gyro_cov: 0.0001
lidar_meas_cov_inv: 100.0
imu_meas_acc_cov_inv: 0.5
imu_meas_gyro_cov_inv: 0.5
satu_acc: 20.0
satu_gyro: 35.0
scan_resolution: 0.4 #0.3
map_resolution: 0.4
near_search_num: 5.0
move_thresh: 1.5
det_range: 60.0
cube_len: 300.0
pose_imu_lidar: [0.707, 0.0, 0.0, 0.707, 0.1710, 0.0, 0.0908]
save_pcd: true
save_pcd_res: 0.15
dist_sq_threshold: 2.0
plane_threshold: 0.1
ground_ang: 75.0 # degree
Go2_W:
imu_topic: "rt/dog_imu_raw" #/livox/imu rt/utlidar/imu_livox_mid360
lidar_topic: "rt/unitree/slam_lidar/points"
imu_acc_multi: 1.0 #10.0
imu_gyro_multi: 1.0
filter_num: 35 #8
lidar_scan_min_range: 0.25
lidar_scan_max_range: 20.0
map_frame: "map"
body_frame: "rslidar"
pub_point_topic: "rt/unitree/slam_mapping/points"
pub_odom_topic: "rt/unitree/slam_mapping/odom"
imu_init_num: 100
vel_cov: 20.0
acc_cov: 500.0
gyro_cov: 1000.0
b_acc_cov: 0.0001
b_gyro_cov: 0.0001
lidar_meas_cov_inv: 100.0
imu_meas_acc_cov_inv: 0.5
imu_meas_gyro_cov_inv: 0.5
satu_acc: 20.0
satu_gyro: 35.0
scan_resolution: 0.4 #0.3
map_resolution: 0.4
near_search_num: 5.0
move_thresh: 1.5
det_range: 60.0
cube_len: 300.0
pose_imu_lidar: [0.707, 0.0, 0.0, 0.707, 0.1710, 0.0, 0.0908]
save_pcd: true
save_pcd_res: 0.15
dist_sq_threshold: 2.0
plane_threshold: 0.1
ground_ang: 75.0 # degree

149
unitree_slam/config/pl_relocation/mid360.yaml Executable file
View File

@@ -0,0 +1,149 @@
G1:
imu_topic: "rt/utlidar/imu_livox_mid360" #/livox/imu
lidar_topic: "rt/utlidar/cloud_livox_mid360"
map_frame: "map"
body_frame: "livox_frame"
pub_point_topic: "rt/unitree/slam_relocation/points"
pub_odom_topic: "rt/unitree/slam_relocation/odom"
pub_global_pcd_topic: "rt/unitree/slam_relocation/global_map"
pub_local_pcd_topic: "rt/unitree/slam_relocation/local_map"
imu_acc_multi: 1.0 #10.0
imu_gyro_multi: 1.0
filter_num: 9 #
lidar_scan_min_range: 0.25
lidar_scan_max_range: 20.0 # 30.0
imu_init_num: 100
vel_cov: 20.0
acc_cov: 50000.0
gyro_cov: 1000.0
b_acc_cov: 0.0001
b_gyro_cov: 0.0001
lidar_meas_cov_inv: 100.0
imu_meas_acc_cov_inv: 1.0
imu_meas_gyro_cov_inv: 10.0
satu_acc: 20.0
satu_gyro: 35.0
scan_resolution: 0.3 #0.15
map_resolution: 0.3
near_search_num: 5.0
move_thresh: 1.5
det_range: 10.0
pose_imu_lidar: [1.0, 0.0, 0.0, 0.0, -0.011, -0.02329, 0.04412] # [qw,qx,qy,qz,tx,ty,tz ]
icp_score: 0.03
cube_len: 300.0
speed_max: 1.5 #m/s
plane_threshold: 0.1 #0.1
dist_sq_threshold: 1.5 #0.1
init_local_map_res: 0.15
gridResolutionX: 30
gridResolutionY: 30
gridResolutionZ: 30
nearbyMethod: NEARBY26
re_position_tollerance: 0.1
re_angle_tollerance: 0.2
re_score_tollerance: 0.02
map_thresh: 10.0
time_thresh: 1.0
pos_thresh: 0.5
angle_thresh: 0.2
ground_ang: 70.0 # degree
Go2:
imu_topic: "rt/unitree/slam_lidar/imu" #/livox/imu
lidar_topic: "rt/unitree/slam_lidar/points"
map_frame: "map"
body_frame: "livox_frame"
pub_point_topic: "rt/unitree/slam_relocation/points"
pub_odom_topic: "rt/unitree/slam_relocation/odom"
pub_global_pcd_topic: "rt/unitree/slam_relocation/global_map"
pub_local_pcd_topic: "rt/unitree/slam_relocation/local_map"
imu_acc_multi: 10.0 #10.0
imu_gyro_multi: 1.0
filter_num: 35 #
lidar_scan_min_range: 0.25
lidar_scan_max_range: 20.0 # 30.0
imu_init_num: 100
vel_cov: 20.0
acc_cov: 50000.0
gyro_cov: 1000.0
b_acc_cov: 0.0001
b_gyro_cov: 0.0001
lidar_meas_cov_inv: 100.0
imu_meas_acc_cov_inv: 1.0
imu_meas_gyro_cov_inv: 10.0
satu_acc: 20.0
satu_gyro: 35.0
scan_resolution: 0.3 #0.15
map_resolution: 0.3
near_search_num: 5.0
move_thresh: 1.5
det_range: 10.0
pose_imu_lidar: [1.0, 0.0, 0.0, 0.0, 0.1870, 0.0, 0.0803] # [qw,qx,qy,qz,tx,ty,tz ]
icp_score: 0.03
cube_len: 300.0
speed_max: 2.5 #m/s
plane_threshold: 0.1 #0.1
dist_sq_threshold: 0.1 #0.1
init_local_map_res: 0.15
gridResolutionX: 30
gridResolutionY: 30
gridResolutionZ: 30
nearbyMethod: NEARBY26
re_position_tollerance: 0.1
re_angle_tollerance: 0.2
re_score_tollerance: 0.02
map_thresh: 10.0
time_thresh: 1.0
pos_thresh: 0.5
angle_thresh: 0.2
ground_ang: 60.0 # degree
Go2_W:
imu_topic: "rt/unitree/slam_lidar/imu" #/livox/imu
lidar_topic: "rt/unitree/slam_lidar/points"
map_frame: "map"
body_frame: "livox_frame"
pub_point_topic: "rt/unitree/slam_relocation/points"
pub_odom_topic: "rt/unitree/slam_relocation/odom"
pub_global_pcd_topic: "rt/unitree/slam_relocation/global_map"
pub_local_pcd_topic: "rt/unitree/slam_relocation/local_map"
imu_acc_multi: 10.0 #10.0
imu_gyro_multi: 1.0
filter_num: 35 #
lidar_scan_min_range: 0.25
lidar_scan_max_range: 20.0 # 30.0
imu_init_num: 100
vel_cov: 20.0
acc_cov: 50000.0
gyro_cov: 1000.0
b_acc_cov: 0.0001
b_gyro_cov: 0.0001
lidar_meas_cov_inv: 100.0
imu_meas_acc_cov_inv: 1.0
imu_meas_gyro_cov_inv: 10.0
satu_acc: 20.0
satu_gyro: 35.0
scan_resolution: 0.3 #0.15
map_resolution: 0.3
near_search_num: 5.0
move_thresh: 1.5
det_range: 10.0
pose_imu_lidar: [1.0, 0.0, 0.0, 0.0, 0.1870, 0.0, 0.0803] # [qw,qx,qy,qz,tx,ty,tz ]
icp_score: 0.05 #0.03
cube_len: 300.0
speed_max: 2.5 #m/s
plane_threshold: 0.1 #0.1
dist_sq_threshold: 0.1 #0.1
init_local_map_res: 0.15
gridResolutionX: 30
gridResolutionY: 30
gridResolutionZ: 30
nearbyMethod: NEARBY26
re_position_tollerance: 0.1
re_angle_tollerance: 0.2
re_score_tollerance: 0.02
map_thresh: 10.0
time_thresh: 1.0
pos_thresh: 0.5
angle_thresh: 0.2
ground_ang: 60.0 # degree

149
unitree_slam/config/pl_relocation/xt16.yaml Executable file
View File

@@ -0,0 +1,149 @@
G1:
imu_topic: "rt/dog_imu_raw" #/livox/imu
lidar_topic: "rt/rslidar_points"
map_frame: "map"
body_frame: "rslidar"
pub_point_topic: "rt/unitree/slam_relocation/points"
pub_odom_topic: "rt/unitree/slam_relocation/odom"
pub_global_pcd_topic: "rt/unitree/slam_relocation/global_map"
pub_local_pcd_topic: "rt/unitree/slam_relocation/local_map"
imu_acc_multi: 1.0 #10.0
imu_gyro_multi: 1.0
filter_num: 4 #
lidar_scan_min_range: 0.25
lidar_scan_max_range: 15.0 # 30.0
imu_init_num: 100
vel_cov: 20.0
acc_cov: 500.0
gyro_cov: 1000.0
b_acc_cov: 0.0001
b_gyro_cov: 0.0001
lidar_meas_cov_inv: 100.0
imu_meas_acc_cov_inv: 10.0
imu_meas_gyro_cov_inv: 10.0
satu_acc: 20.0
satu_gyro: 35.0
scan_resolution: 0.3 #0.15
map_resolution: 0.3
near_search_num: 5.0
move_thresh: 1.5
det_range: 60.0
pose_imu_lidar: [0.707, 0.0, 0.0, 0.707, 0.1710, 0.0, 0.0908] # [qw,qx,qy,qz,tx,ty,tz ]
icp_score: 0.03
cube_len: 300.0
speed_max: 2.5 #m/s
plane_threshold: 0.1 #0.1
dist_sq_threshold: 0.1 #0.1
init_local_map_res: 0.15
gridResolutionX: 15
gridResolutionY: 15
gridResolutionZ: 15
nearbyMethod: NEARBY26
re_position_tollerance: 3.0
re_angle_tollerance: 0.2
re_score_tollerance: 0.2
map_thresh: 10.0
time_thresh: 1.0
pos_thresh: 0.5
angle_thresh: 0.2
ground_ang: 75.0 # degree
Go2:
imu_topic: "rt/dog_imu_raw" #/livox/imu
lidar_topic: "rt/unitree/slam_lidar/points"
map_frame: "map"
body_frame: "rslidar"
pub_point_topic: "rt/unitree/slam_relocation/points"
pub_odom_topic: "rt/unitree/slam_relocation/odom"
pub_global_pcd_topic: "rt/unitree/slam_relocation/global_map"
pub_local_pcd_topic: "rt/unitree/slam_relocation/local_map"
imu_acc_multi: 1.0 #10.0
imu_gyro_multi: 1.0
filter_num: 45 #
lidar_scan_min_range: 0.25
lidar_scan_max_range: 15.0 # 30.0
imu_init_num: 100
vel_cov: 20.0
acc_cov: 500.0
gyro_cov: 1000.0
b_acc_cov: 0.0001
b_gyro_cov: 0.0001
lidar_meas_cov_inv: 100.0
imu_meas_acc_cov_inv: 0.5
imu_meas_gyro_cov_inv: 0.5
satu_acc: 20.0
satu_gyro: 35.0
scan_resolution: 0.3 #0.15
map_resolution: 0.3
near_search_num: 5.0
move_thresh: 1.5
det_range: 60.0
pose_imu_lidar: [0.707, 0.0, 0.0, 0.707, 0.1710, 0.0, 0.0908] # [qw,qx,qy,qz,tx,ty,tz ]
icp_score: 0.03
cube_len: 300.0
speed_max: 2.5 #m/s
plane_threshold: 0.1 #0.1
dist_sq_threshold: 0.1 #0.1
init_local_map_res: 0.15
gridResolutionX: 15
gridResolutionY: 15
gridResolutionZ: 15
nearbyMethod: NEARBY26
re_position_tollerance: 3.0
re_angle_tollerance: 0.2
re_score_tollerance: 0.2
map_thresh: 10.0
time_thresh: 1.0
pos_thresh: 0.5
angle_thresh: 0.2
ground_ang: 75.0 # degree
Go2_W:
imu_topic: "rt/dog_imu_raw" #/livox/imu
lidar_topic: "rt/unitree/slam_lidar/points"
map_frame: "map"
body_frame: "rslidar"
pub_point_topic: "rt/unitree/slam_relocation/points"
pub_odom_topic: "rt/unitree/slam_relocation/odom"
pub_global_pcd_topic: "rt/unitree/slam_relocation/global_map"
pub_local_pcd_topic: "rt/unitree/slam_relocation/local_map"
imu_acc_multi: 1.0 #10.0
imu_gyro_multi: 1.0
filter_num: 45 #
lidar_scan_min_range: 0.25
lidar_scan_max_range: 15.0 # 30.0
imu_init_num: 100
vel_cov: 20.0
acc_cov: 500.0
gyro_cov: 1000.0
b_acc_cov: 0.0001
b_gyro_cov: 0.0001
lidar_meas_cov_inv: 100.0
imu_meas_acc_cov_inv: 0.5
imu_meas_gyro_cov_inv: 0.5
satu_acc: 20.0
satu_gyro: 35.0
scan_resolution: 0.3 #0.15
map_resolution: 0.3
near_search_num: 5.0
move_thresh: 1.5
det_range: 60.0
pose_imu_lidar: [0.707, 0.0, 0.0, 0.707, 0.1710, 0.0, 0.0908] # [qw,qx,qy,qz,tx,ty,tz ]
icp_score: 0.03
cube_len: 300.0
speed_max: 2.5 #m/s
plane_threshold: 0.1 #0.1
dist_sq_threshold: 0.1 #0.1
init_local_map_res: 0.15
gridResolutionX: 15
gridResolutionY: 15
gridResolutionZ: 15
nearbyMethod: NEARBY26
re_position_tollerance: 3.0
re_angle_tollerance: 0.2
re_score_tollerance: 0.2
map_thresh: 10.0
time_thresh: 1.0
pos_thresh: 0.5
angle_thresh: 0.2
ground_ang: 75.0 # degree

20
unitree_slam/config/planner_config/param.yaml Executable file
View File

@@ -0,0 +1,20 @@
CurvatureSamplingInterval: 0.2 # 曲率离散点采样间隔 pid 巡迹使用
# Planner 参数设置
Planner/pub_rate: 40.0 # 节点发布频率
Planner/forwardDis: 1.0 # 0.6 # 搜索的最佳追踪点到当前位置的最远距离
Planner/transform_tolerance: 0.3 # 是否到达目标附近的容忍度
accurateThresholdRotate_x: 0.1
accurateThresholdRotate_y: 0.1
accurateThresholdRotate_yaw: 0.15
accurateThresholdAdjust_x: 0.05
accurateThresholdAdjust_y: 0.05
accurateThresholdAdjust_yaw: 0.05
accurateThresholdKeeping_x: 0.05
accurateThresholdKeeping_y: 0.05
accurateThresholdKeeping_yaw: 0.15

311
unitree_slam/config/planner_config/pose_ctrl_param.yaml Executable file
View File

@@ -0,0 +1,311 @@
B2:
pid:
ctrl_rate: 50 #hz
# x方向
PID_x_Limmax: 0.2
PID_x_Vmax: 1.8
PID_x_KP: 2.5
PID_x_KI: 0.1
PID_x_KD: 0.0
# y方向
PID_y_Limmax: 0.2
PID_y_Vmax: 0.7
PID_y_KP: 3.5
PID_y_KI: 0.1
PID_y_KD: 0.0
# yaw
PID_yaw_Limmax: 0.2
PID_yaw_Vmax: 0.8
PID_yaw_KP: 2.5
PID_yaw_KI: 0.1
PID_yaw_KD: 0.0
# about threshold
# x方向
threshold_x: 0.02
# y方向
threshold_y: 0.02
# yaw
threshold_yaw: 0.02
ctrl:
CurvatureSamplingInterval: 0.2 # 曲率离散点采样间隔 pid 巡迹使用
# Planner 参数设置
Planner/pub_rate: 40.0 # 节点发布频率
Planner/forwardDis: 0.60 # 0.6 # 搜索的最佳追踪点到当前位置的最远距离
Planner/transform_tolerance: 0.3 # 是否到达目标附近的容忍度
accurateThresholdRotate_x: 0.1
accurateThresholdRotate_y: 0.1
accurateThresholdRotate_yaw: 0.15
accurateThresholdAdjust_x: 0.08
accurateThresholdAdjust_y: 0.08
accurateThresholdAdjust_yaw: 0.1
accurateThresholdKeeping_x: 0.05
accurateThresholdKeeping_y: 0.05
accurateThresholdKeeping_yaw: 0.15
B2_W:
pid:
ctrl_rate: 50 #hz
# x方向
PID_x_Limmax: 0.2
PID_x_Vmax: 2.5
PID_x_KP: 2.25
PID_x_KI: 0.1
PID_x_KD: 0.0
# y方向
PID_y_Limmax: 0.2
PID_y_Vmax: 0.5
PID_y_KP: 2.5
PID_y_KI: 0.1
PID_y_KD: 0.0
# yaw
PID_yaw_Limmax: 0.2
PID_yaw_Vmax: 1.0
PID_yaw_KP: 2.5
PID_yaw_KI: 0.1
PID_yaw_KD: 0.0
# about threshold
# x方向
threshold_x: 0.02
# y方向
threshold_y: 0.02
# yaw
threshold_yaw: 0.02
ctrl:
CurvatureSamplingInterval: 0.2 # 曲率离散点采样间隔 pid 巡迹使用
# Planner 参数设置
Planner/pub_rate: 40.0 # 节点发布频率
Planner/forwardDis: 1.0 # 0.6 # 搜索的最佳追踪点到当前位置的最远距离
Planner/transform_tolerance: 0.3 # 是否到达目标附近的容忍度
accurateThresholdRotate_x: 0.2
accurateThresholdRotate_y: 0.2
accurateThresholdRotate_yaw: 0.25
accurateThresholdAdjust_x: 0.1
accurateThresholdAdjust_y: 0.1
accurateThresholdAdjust_yaw: 0.1
accurateThresholdKeeping_x: 0.05
accurateThresholdKeeping_y: 0.05
accurateThresholdKeeping_yaw: 0.15
G1:
pid:
# about pid
ctrl_rate: 50 #hz
# x方向
PID_x_Limmax: 0.2
PID_x_Vmax: 0.8
PID_x_KP: 1.8
PID_x_KI: 0.0
PID_x_KD: 0.0
# y方向
PID_y_Limmax: 0.2
PID_y_Vmax: 0.8
PID_y_KP: 1.5
PID_y_KI: 0.1
PID_y_KD: 0.0
# yaw
PID_yaw_Limmax: 0.2
PID_yaw_Vmax: 0.85
PID_yaw_KP: 2.0
PID_yaw_KI: 0.1
PID_yaw_KD: 0.1
# about threshold
# x方向
threshold_x: 0.02
# y方向
threshold_y: 0.02
# yaw
threshold_yaw: 0.02
ctrl:
CurvatureSamplingInterval: 0.2 # 曲率离散点采样间隔 pid 巡迹使用
# Planner 参数设置
Planner/pub_rate: 40.0 # 节点发布频率
Planner/forwardDis: 0.6 # 0.8 # 搜索的最佳追踪点到当前位置的最远距离
Planner/transform_tolerance: 0.2 # 是否到达目标附近的容忍度
accurateThresholdRotate_x: 0.2
accurateThresholdRotate_y: 0.2
accurateThresholdRotate_yaw: 0.2
accurateThresholdAdjust_x: 0.25
accurateThresholdAdjust_y: 0.25
accurateThresholdAdjust_yaw: 0.25
accurateThresholdKeeping_x: 0.15
accurateThresholdKeeping_y: 0.15
accurateThresholdKeeping_yaw: 0.15
Go2:
pid:
ctrl_rate: 50 #hz
# x方向
PID_x_Limmax: 0.2
PID_x_Vmax: 1.8
PID_x_KP: 2.5
PID_x_KI: 0.1
PID_x_KD: 0.0
# y方向
PID_y_Limmax: 0.2
PID_y_Vmax: 0.7
PID_y_KP: 3.5
PID_y_KI: 0.1
PID_y_KD: 0.0
# yaw
PID_yaw_Limmax: 0.2
PID_yaw_Vmax: 1.8
PID_yaw_KP: 8.0
PID_yaw_KI: 0.5
PID_yaw_KD: 0.0
# about threshold
# x方向
threshold_x: 0.02
# y方向
threshold_y: 0.02
# yaw
threshold_yaw: 0.02
ctrl:
CurvatureSamplingInterval: 0.2 # 曲率离散点采样间隔 pid 巡迹使用
# Planner 参数设置
Planner/pub_rate: 40.0 # 节点发布频率
Planner/forwardDis: 1.0 # 0.6 # 搜索的最佳追踪点到当前位置的最远距离
Planner/transform_tolerance: 0.3 # 是否到达目标附近的容忍度
accurateThresholdRotate_x: 0.1
accurateThresholdRotate_y: 0.1
accurateThresholdRotate_yaw: 0.15
accurateThresholdAdjust_x: 0.1
accurateThresholdAdjust_y: 0.1
accurateThresholdAdjust_yaw: 0.15
accurateThresholdKeeping_x: 0.15
accurateThresholdKeeping_y: 0.15
accurateThresholdKeeping_yaw: 0.15
Go2_W:
pid:
ctrl_rate: 50 #hz
# x方向
PID_x_Limmax: 0.2
PID_x_Vmax: 1.8
PID_x_KP: 2.5
PID_x_KI: 0.1
PID_x_KD: 0.0
# y方向
PID_y_Limmax: 0.2
PID_y_Vmax: 0.7
PID_y_KP: 3.5
PID_y_KI: 0.1
PID_y_KD: 0.0
# yaw
PID_yaw_Limmax: 0.2
PID_yaw_Vmax: 0.8
PID_yaw_KP: 2.0
PID_yaw_KI: 0.1
PID_yaw_KD: 0.0
# about threshold
# x方向
threshold_x: 0.02
# y方向
threshold_y: 0.02
# yaw
threshold_yaw: 0.02
ctrl:
CurvatureSamplingInterval: 0.2 # 曲率离散点采样间隔 pid 巡迹使用
# Planner 参数设置
Planner/pub_rate: 40.0 # 节点发布频率
Planner/forwardDis: 1.0 # 0.6 # 搜索的最佳追踪点到当前位置的最远距离
Planner/transform_tolerance: 0.3 # 是否到达目标附近的容忍度
accurateThresholdRotate_x: 0.1
accurateThresholdRotate_y: 0.1
accurateThresholdRotate_yaw: 0.15
accurateThresholdAdjust_x: 0.1
accurateThresholdAdjust_y: 0.1
accurateThresholdAdjust_yaw: 0.15
accurateThresholdKeeping_x: 0.15
accurateThresholdKeeping_y: 0.15
accurateThresholdKeeping_yaw: 0.15
H1:
pid:
ctrl_rate: 50 #hz
# x方向
PID_x_Limmax: 0.2
PID_x_Vmax: 1.8
PID_x_KP: 2.5
PID_x_KI: 0.1
PID_x_KD: 1.1
# y方向
PID_y_Limmax: 0.2
PID_y_Vmax: 0.7
PID_y_KP: 3.5
PID_y_KI: 0.1
PID_y_KD: 1.1
# yaw
PID_yaw_Limmax: 0.2
PID_yaw_Vmax: 0.8
PID_yaw_KP: 2.0
PID_yaw_KI: 0.1
PID_yaw_KD: 0.1
# about threshold
# x方向
threshold_x: 0.02
# y方向
threshold_y: 0.02
# yaw
threshold_yaw: 0.02
ctrl:
CurvatureSamplingInterval: 0.2 # 曲率离散点采样间隔 pid 巡迹使用
# Planner 参数设置
Planner/pub_rate: 40.0 # 节点发布频率
Planner/forwardDis: 1.0 # 0.6 # 搜索的最佳追踪点到当前位置的最远距离
Planner/transform_tolerance: 0.3 # 是否到达目标附近的容忍度
accurateThresholdRotate_x: 0.1
accurateThresholdRotate_y: 0.1
accurateThresholdRotate_yaw: 0.15
accurateThresholdAdjust_x: 0.15
accurateThresholdAdjust_y: 0.15
accurateThresholdAdjust_yaw: 0.15
accurateThresholdKeeping_x: 0.05
accurateThresholdKeeping_y: 0.05
accurateThresholdKeeping_yaw: 0.15

25
unitree_slam/config/recharge_config/aruco_config.yaml Executable file
View File

@@ -0,0 +1,25 @@
intrinsic_matrix:
- 486.969
- 0.2502
- 659.2783
- 0
- 488.012
- 356.3946
- 0
- 0
- 1
distCoeffs:
- 0.020209098720387
- -0.071085536994343
- 0.018253077133880
- 0.00
- 0.00
# aruco_size: 0.1715 #0.1075
aruco_size: 0.1075 #0.1075
aruco2center_dis: 1.05
camera2dog_dis: 0.50

7
unitree_slam/config/recharge_config/cylinder_config.yaml Executable file
View File

@@ -0,0 +1,7 @@
lidar_topic: "rt/rslidar_points"
high_intensity_topic: "rt/high_intensity"
chargingWidth: 0.40
chargingLength: 0.40
intensity_threshold: 150
dis_chargePt2Center: 0.38

11
unitree_slam/config/slam_interfaces_server_config/cyclonedds.xml Executable file
View File

@@ -0,0 +1,11 @@
<?xml version="1.0" encoding="UTF-8" ?>
<CycloneDDS xmlns="https://cdds.io/config" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:schemaLocation="https://cdds.io/config https://raw.githubusercontent.com/eclipse-cyclonedds/cyclonedds/master/etc/cyclonedds.xsd">
<Domain Id="any">
<General>
<Interfaces>
<NetworkInterface name="enp2s0" priority="default" multicast="default" />
</Interfaces>
<AllowMulticast>spdp</AllowMulticast>
</General>
</Domain>
</CycloneDDS>

8
unitree_slam/config/slam_interfaces_server_config/dds_parameter.json Executable file
View File

@@ -0,0 +1,8 @@
{
"Participant":
{
"DomainId": 0,
"Config": "/unitree/etc/cyclonedds.xml",
"Config_": "../config/slam_interfaces_server_config/cyclonedds.xml"
}
}

54
unitree_slam/config/slam_interfaces_server_config/param.yaml Executable file
View File

@@ -0,0 +1,54 @@
B2:
network_interface: "eth0" #eth0/enp2s0/wlxfc221c3f66c8
lidar_type: "robosense32" #robosense32/xt16
lidar_ysn: "ARMCP540031377" #47PGO440010032 AW3HFG53903HFI51
lidar_ip: "192.168.124.20" #192.168.123.20/192.168.124.20
odom_topic: "rt/dog_odom" #
imu_topic: "rt/dog_imu_raw" #
time_out_seconds: 60 #
B2_W:
network_interface: "eth0" #eth0/enp2s0/wlxfc221c3f66c8
lidar_type: "robosense32" #robosense32/xt16
lidar_ysn: "47PGO440010032" #47PGO440010032 AW3HFG53903HFI51
lidar_ip: "192.168.124.20" #192.168.123.20/192.168.124.20
odom_topic: "rt/dog_odom" #
imu_topic: "rt/dog_imu_raw" #
time_out_seconds: 60 #
Go2:
network_interface: "eth0" #eth0/enp2s0/wlxfc221c3f66c8
lidar_type: "mid360" #mid360/xt16
lidar_ysn: "DUPFS540031377" #YSN for lidar SN ARMCP540031377
lidar_ip: "192.168.123.20" #192.168.123.20/192.168.124.20
odom_topic: "rt/dog_odom" #
imu_topic: "rt/dog_imu_raw" #
time_out_seconds: 30 #
Go2_W:
network_interface: "eth0" #eth0/enp2s0/wlxfc221c3f66c8
lidar_type: "mid360" #mid360/xt16
lidar_ysn: "DUPFS540031377" #YSN for lidar SN ARMCP540031377
lidar_ip: "192.168.123.20" #192.168.123.20/192.168.124.20
odom_topic: "rt/dog_odom" #
imu_topic: "rt/dog_imu_raw" #
time_out_seconds: 30 #
G1:
network_interface: "eth0" #eth0/enp2s0/wlxfc221c3f66c8
lidar_type: "mid360" #mid360
lidar_ysn: "47PGO440010032" #47PGO440010032 AW3HFG53903HFI51
lidar_ip: "192.168.123.120" #192.168.123.20/192.168.124.20
odom_topic: "rt/dog_odom" #
imu_topic: "rt/dog_imu_raw" #
time_out_seconds: 30 #
H1:
network_interface: "eth0" #eth0/enp2s0/wlxfc221c3f66c8
lidar_type: "mid360" #mid360
lidar_ysn: "47PGO440010032" #47PGO440010032 AW3HFG53903HFI51
lidar_ip: "192.168.123.20" #192.168.123.20/192.168.124.20
odom_topic: "rt/dog_odom" #
imu_topic: "rt/dog_imu_raw" #
time_out_seconds: 30 #

54
unitree_slam/config/slam_interfaces_server_config/param.yaml.orig Executable file
View File

@@ -0,0 +1,54 @@
B2:
network_interface: "eth0" #eth0/enp2s0/wlxfc221c3f66c8
lidar_type: "robosense32" #robosense32/xt16
lidar_ysn: "ARMCP540031377" #47PGO440010032 AW3HFG53903HFI51
lidar_ip: "192.168.124.20" #192.168.123.20/192.168.124.20
odom_topic: "rt/dog_odom" #
imu_topic: "rt/dog_imu_raw" #
time_out_seconds: 60 #
B2_W:
network_interface: "eth0" #eth0/enp2s0/wlxfc221c3f66c8
lidar_type: "robosense32" #robosense32/xt16
lidar_ysn: "47PGO440010032" #47PGO440010032 AW3HFG53903HFI51
lidar_ip: "192.168.124.20" #192.168.123.20/192.168.124.20
odom_topic: "rt/dog_odom" #
imu_topic: "rt/dog_imu_raw" #
time_out_seconds: 60 #
Go2:
network_interface: "eth0" #eth0/enp2s0/wlxfc221c3f66c8
lidar_type: "mid360" #mid360/xt16
lidar_ysn: "ARMCP540031377" #47PGO440010032 AW3HFG53903HFI51
lidar_ip: "192.168.123.20" #192.168.123.20/192.168.124.20
odom_topic: "rt/dog_odom" #
imu_topic: "rt/dog_imu_raw" #
time_out_seconds: 30 #
Go2_W:
network_interface: "eth0" #eth0/enp2s0/wlxfc221c3f66c8
lidar_type: "mid360" #mid360/xt16
lidar_ysn: "ARMCP540031377" #47PGO440010032 AW3HFG53903HFI51
lidar_ip: "192.168.123.20" #192.168.123.20/192.168.124.20
odom_topic: "rt/dog_odom" #
imu_topic: "rt/dog_imu_raw" #
time_out_seconds: 30 #
G1:
network_interface: "eth0" #eth0/enp2s0/wlxfc221c3f66c8
lidar_type: "mid360" #mid360
lidar_ysn: "47PGO440010032" #47PGO440010032 AW3HFG53903HFI51
lidar_ip: "192.168.123.120" #192.168.123.20/192.168.124.20
odom_topic: "rt/dog_odom" #
imu_topic: "rt/dog_imu_raw" #
time_out_seconds: 30 #
H1:
network_interface: "eth0" #eth0/enp2s0/wlxfc221c3f66c8
lidar_type: "mid360" #mid360
lidar_ysn: "47PGO440010032" #47PGO440010032 AW3HFG53903HFI51
lidar_ip: "192.168.123.20" #192.168.123.20/192.168.124.20
odom_topic: "rt/dog_odom" #
imu_topic: "rt/dog_imu_raw" #
time_out_seconds: 30 #

17
unitree_slam/config/xt16/PandarXT-16.csv Executable file
View File

@@ -0,0 +1,17 @@
Channel,Elevation,Azimuth
1,15,0
2,13,0
3,11,0
4,9,0
5,7,0
6,5,0
7,3,0
8,1,0
9,-1,0
10,-3,0
11,-5,0
12,-7,0
13,-9,0
14,-11,0
15,-13,0
16,-15,0
1 Channel Elevation Azimuth
2 1 15 0
3 2 13 0
4 3 11 0
5 4 9 0
6 5 7 0
7 6 5 0
8 7 3 0
9 8 1 0
10 9 -1 0
11 10 -3 0
12 11 -5 0
13 12 -7 0
14 13 -9 0
15 14 -11 0
16 15 -13 0
17 16 -15 0

4
unitree_slam/config/xt16/user_config.yaml Executable file
View File

@@ -0,0 +1,4 @@
network: "eth0"
ip_address: "192.168.123.20"
lidar_frame: "rslidar" # string
lidar_topic: "rt/unitree/slam_lidar/points" # string rt/....