宇树 G1 机器人导航仿真：地图构建与参数配置实战

3. 下载 PCD 转 PGM 工具

我们需要将点云地图转换为栅格地图，需额外引入转换工具：

cd ~/ws_loc/src
git clone https://github.com/Hinson-A/pcd2pgm_package.git

二、编译与错误修复

1. 首次编译尝试

直接在工作空间根目录执行编译命令：

cd ~/ws_loc
catkin_make

2. 解决编译错误

如果遇到缺少 include 目录的报错，通常是因为源码结构未完全初始化。创建缺失目录后重试：

cd ~/ws_loc/src/ros_navigation_humanoid
mkdir -p include
cd ~/ws_loc
catkin_make

3. 更新环境变量

编译成功后务必刷新环境变量，否则无法识别新包：

source devel/setup.bash

三、地图文件准备与转换

1. 复制地图文件

假设你已经通过激光雷达建图生成了 .pcd 文件，将其复制到导航包的 maps 目录下：

cp ~/ws_loc/maps/map_1.pcd ~/ws_loc/src/ros_navigation_humanoid/maps/

2. 修改转换启动文件

编辑 conv_pcd2pgm.launch 文件，确保参数配置正确。这里我调整了部分阈值参数以适应 G1 机器人的尺寸：

<?xml version="1.0"?>
<launch>
  <!-- 添加可自定义的地图名参数 -->
  <arg name="file_name" default="map" />
  <arg name="file_directory" default="$(find ros_navigation_humanoid)/maps/" />
  
  <node pkg="pcd2pgm" name="pcd2pgm" type="pcd2pgm" output="screen">
    <!-- 使用私有参数语法 -->
    <param name="~file_name" value="$(arg file_name)" />
    <param name="~file_directory" value="$(arg file_directory)" />
    <param name="~map_topic_name" value="map" />
    <!-- 转换参数配置 -->
    <param name="~thre_z_min" value="0.2" />
    <param name="~thre_z_max" value="0.8" />
    <param name="~flag_pass_through" value="0" />
    <param name="~thre_radius" value="0.5" />
    <param name="~thres_point_count" value="10" />
    <param name="~map_resolution" value="0.05" />
  </node>
  <!-- 启动 RViz 查看转换结果 -->
  <node pkg="rviz" name="rviz_sim" type="rviz" args="-d $(find ros_navigation_humanoid)/rviz/conv_pcd2pgm.rviz" output="screen"/>
</launch>

3. 执行 PCD 到 PGM 转换

运行 launch 文件进行转换，指定你的地图名称：

roslaunch ros_navigation_humanoid conv_pcd2pgm.launch file_name:=map_1

或者你也可以参考下图中的步骤手动操作（此处省略截图）。

四、栅格地图编辑

1. 使用 KolourPaint 编辑 PGM

转换完成后，生成的 .pgm 文件可能包含噪点或需要修正区域。使用 KolourPaint 打开编辑：

kolourpaint ~/ws_loc/src/ros_navigation_humanoid/maps/map_1.pgm

注意： 白色区域代表可通行，黑色为障碍不可走，灰色为未知区域。根据实际环境清理不必要的障碍物标记。

五、机器人模型展示

1. 启动 URDF 模型显示

先确认机器人模型能正常加载，这有助于后续调试 TF 变换：

roslaunch ros_navigation_humanoid static_display.launch

六、导航配置修改（双足→轮式适配）

G1 虽然是双足机器人，但在导航仿真中常需适配通用导航栈参数。以下是关键修改点：

1. 修改全局代价地图参数

找到 move_base_config/global_costmap_params.yaml，将基座坐标系从 pelvis 改为 base_link：

robot_base_frame: base_link # 原为 pelvis

2. 修改局部代价地图参数

同样修改 local_costmap_params.yaml：

robot_base_frame: base_link # 原为 pelvis

3. 修改局部规划器参数

如果希望启用全向移动能力（适用于轮式模式），需开启 holonomic_robot：

holonomic_robot: true # 原为 false

4. 修改 RViz 仿真节点代码

C++ 源码中也需要同步修改 TF 子坐标系：

// 将 TF 子坐标系改为 base_link
map_trans.child_frame_id = "base_link"; // 原为 "pelvis"

七、重新编译与验证

1. 重新编译修改后的代码

代码变动后必须重新编译：

cd ~/ws_loc
catkin_make

2. 启动完整导航仿真

分三个终端窗口依次启动服务，顺序不能乱：

终端 1 - 启动 ROS 核心：

cd ~/ws_loc
roscore

终端 2 - 启动 RViz 仿真：

cd ~/ws_loc
roslaunch ros_navigation_humanoid rviz_sim.launch

终端 3 - 发布静态坐标变换： 为了维持原有的 TF 树结构，需要发布 base_link 到 pelvis 的静态变换：

rosrun tf static_transform_publisher 0 0 0 0 0 0 base_link pelvis 100

此时即可下发导航目标，观察机器人仿真路径规划效果。

八、回放 Rosbag 验证机器人运动（可选）

如果有录制的真实数据，可以通过回放 Bag 包来验证运动控制逻辑。

1. 修改 Bag 播放启动文件

编辑 bag_play.launch，填入你的 bag 文件绝对路径：

<param name="bag_file_path" value="/your/path/to/bagfile.bag" />

2. 播放 Rosbag

roslaunch g1_ros1_nav bag_play.launch

九、步态规划配置（可选，保留双足功能）

如果你需要保留双足行走的步态规划能力，还需引入额外的消息定义和规划包。

1. 下载步态规划相关包

cd ~/ws_loc/src
git clone https://github.com/ahornung/humanoid_msgs.git
git clone https://github.com/ROBOTIS-GIT/humanoid_navigation.git

2. 提取必要组件

只需复制核心的规划模块即可：

cp -r humanoid_navigation/footstep_planner .
cp -r humanoid_navigation/gridmap_2d .

3. 编译步态规划包

cd ~/ws_loc
catkin build
source devel/setup.bash

4. 启动步态规划

roslaunch ros_navigation_humanoid foot_planner.launch