新建~/unitree_elevation_ws/src/elevation_ws/src/elevation_mapping/elevation_mapping_demos/config/robots/real_d435_config.yaml
# ====================== 核心坐标系配置（必改） ====================== map_frame_id: "odom" # 手持/台架测试建议用 odom（由 static TF 关联到相机） robot_base_frame_id: "camera_link" # 以相机为机器人基座（无机身里程计时更稳妥） sensor_frame_id: "camera_depth_optical_frame" # 点云frame_id（与真实相机一致） track_point_frame_id: "camera_link" # 跟踪点坐标系（与相机一致） track_point_x: 0.0 # 跟踪点X偏移（躯干中心） track_point_y: 0.0 # 跟踪点Y偏移 track_point_z: 0.0 # 跟踪点Z偏移 # ====================== 机器人位姿与时间参数 ====================== robot_pose_with_covariance_topic: "" # 躯干位姿话题（替代轮式的base_footprint_pose） robot_pose_cache_size: 200 # 缓存位姿数量（适配500Hz控制频率） min_update_rate: 10.0 # 地图更新频率（匹配LiDAR的10Hz） time_tolerance: 0.2 # 时间容忍度（秒，适配足式运动延迟） time_offset_for_point_cloud: 0.0 # 点云时间偏移（根据硬件校准） # ====================== 地图基础属性 ====================== length_in_x: 2.0 # 地图X方向长度（5m，适配足式导航范围） length_in_y: 2.0 # 地图Y方向长度（5m） position_x: 0.0 # 地图中心X偏移 position_y: 0.0 # 地图中心Y偏移 resolution: 0.08 # 地图分辨率（5cm，兼顾精度与实时性） min_variance: 0.001 # 最小高度方差（容忍轻微抖动） max_variance: 0.06 # 最大高度方差（适配足式颠簸） mahalanobis_distance_threshold: 3.0 # 马氏距离阈值（调大容忍抖动） multi_height_noise: 0.001 # 多高度噪声（保持与原始文件一致） surface_normal_positive_axis: z # 表面法向正方向（Z轴） fused_map_publishing_rate: 10.0 # 融合地图发布频率（高于轮式，适配动态场景） # ====================== 动态清理参数（解决腿部拖影） ====================== enable_visibility_cleanup: true # 开启可见性清理（替代map_acquisition/visibility_cleanup_enabled） visibility_cleanup_rate: 10.0 # 可见性清理频率（10Hz，匹配LiDAR） scanning_duration: 0.1 # 扫描持续时间（秒，匹配10Hz LiDAR） max_time_without_update: 2.0 # 最大无更新时间（秒，超时清理无效点） ray_tracing_enabled: true # 开启射线追踪（精准剔除遮挡点） # ====================== 高程范围配置 ====================== min_height: -0.5 # 最小高程（低于地面0.5m，适配凹凸地形） max_height: 1.0 # 最大高程（高于地面1.0m，适配台阶/障碍物） # ====================== 子图初始化（四足无需初始子图） ====================== initialize_elevation_map: false # 关闭初始子图（足式运动范围动态变化） initialization_method: 0 # 初始化方法（无效，保持默认） length_in_x_init_submap: 1.6 # 初始子图X长度（保留默认，实际不启用） length_in_y_init_submap: 1.6 # 初始子图Y长度（保留默认） init_submap_height_offset: 0.01 # 初始子图高度偏移（保留默认） init_submap_variance: 0.001 # 初始子图方差（保留默认） target_frame_init_submap: "trunk" # 初始子图坐标系（与躯干一致）新建/home/zy/unitree_elevation_ws/src/elevation_ws/src/elevation_mapping/elevation_mapping_demos/launch/real_world_mapping.launch
<launch> <include file="$(find realsense2_camera)/launch/rs_camera.launch"> <arg name="filters" value="pointcloud,spatial,temporal"/> <arg name="depth_width" value="640"/> <arg name="depth_height" value="480"/> <arg name="depth_fps" value="15"/> </include> <node pkg="tf" type="static_transform_publisher" name="map_to_camera" args="0 0 1.0 0 0.5 0 odom camera_link 100" /> <node pkg="nodelet" type="nodelet" name="voxel_grid_filter" args="standalone pcl/VoxelGrid" output="screen"> <remap from="~input" to="/camera/depth/color/points"/> <remap from="~output" to="/points_downsampled"/> <rosparam> filter_field_name: z filter_limit_min: 0.2 filter_limit_max: 5.0 leaf_size: 0.05 </rosparam> </node> <node pkg="elevation_mapping" type="elevation_mapping" name="elevation_mapping" output="screen"> <rosparam command="load" file="$(find elevation_mapping_demos)/config/robots/real_d435_config.yaml"/> <rosparam command="load" file="$(find elevation_mapping_demos)/config/postprocessing/postprocessor_pipeline.yaml"/> <rosparam subst_value="true"> input_sources: front: type: pointcloud topic: /points_downsampled queue_size: 1 publish_on_update: true sensor_processor: type: perfect min_point_distance: 0.3 max_point_distance: 2.5 </rosparam> </node> <node pkg="rviz" type="rviz" name="rviz" args="-d $(find elevation_mapping_demos)/rviz/turtlebot3_waffle_demo.rviz"/> </launch>cd ~/unitree_elevation_ws
catkin_make
source ~/unitree_elevation_ws/devel/setup.bashroslaunch elevation_mapping_demos real_world_mapping.launch
