宇树机器人 G1 导航仿真：地图转换与参数配置

1.3、下载 PCD 转 PGM 工具

# 进入工作空间源码目录
cd ~/ws_loc/src
# 克隆 PCD 转栅格地图工具
git clone https://github.com/Hinson-A/pcd2pgm_package.git

二、编译与错误修复

2.1 首次编译尝试

cd ~/ws_loc
catkin_make

2.2 解决编译错误

如果编译报错提示缺少 include 目录，执行：

# 创建缺失的目录
cd ~/ws_loc/src/ros_navigation_humanoid
mkdir -p include
# 重新编译
cd ~/ws_loc
catkin_make

2.3 更新环境变量

source devel/setup.bash

三、地图文件准备与转换

3.1 复制地图文件

# 假设已有建好的点云地图
cp ~/ws_loc/maps/map_1.pcd ~/ws_loc/src/ros_navigation_humanoid/maps/

3.2 修改转换启动文件

编辑 ~/ws_loc/src/ros_navigation_humanoid/launch/conv_pcd2pgm.launch：

<?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.3 执行 PCD 到 PGM 转换

# 转换指定名称的地图文件
roslaunch ros_navigation_humanoid conv_pcd2pgm.launch file_name:=map_1

3.4 或者根据下面的步骤来将 pcd 转换到 pgm

四、栅格地图编辑

4.1 使用 KolourPaint 编辑 PGM

# 打开生成的 PGM 文件进行编辑
kolourpaint ~/ws_loc/src/ros_navigation_humanoid/maps/map_1.pgm

白色区域代表可通行，黑色为障碍，灰色为未知。

五、机器人模型展示

5.1 启动 URDF 模型显示

roslaunch ros_navigation_humanoid static_display.launch

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

6.1 修改全局代价地图参数

roscd ros_navigation_humanoid/move_base_config
nano global_costmap_params.yaml

修改内容：

# 将机器人基座坐标系从 pelvis 改为 base_link
robot_base_frame: base_link
# 原为：pelvis

6.2 修改局部代价地图参数

nano local_costmap_params.yaml

修改内容：

robot_base_frame: base_link
# 原为：pelvis

6.3 修改局部规划器参数

nano base_local_planner_params.yaml

修改内容：

# 启用全向移动（适用于轮式机器人）
holonomic_robot: true
# 原为：false

6.4 修改 RViz 仿真节点

roscd ros_navigation_humanoid/src
nano rviz_sim.cpp

修改内容：

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

七、重新编译与验证

7.1 重新编译修改后的代码

cd ~/ws_loc
catkin_make

7.2 启动完整导航仿真

终端 1 - 启动 ROS 核心：

cd ~/ws_loc
roscore

终端 2 - 启动 RViz 仿真：

cd ~/ws_loc
roslaunch ros_navigation_humanoid rviz_sim.launch

终端 3 - 发布静态坐标变换：

# 发布 base_link 到 pelvis 的静态 TF（维持原有 TF 树结构）
rosrun tf static_transform_publisher 0 0 0 0 0 0 base_link pelvis 100

这样就可以给导航点让机器人仿真导航过去了。

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

8.1 修改 bag 播放启动文件

编辑 bag_play.launch，修改 bag 文件路径：

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

8.2 播放 rosbag

roslaunch g1_ros1_nav bag_play.launch

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

9.1 下载步态规划相关包

cd ~/ws_loc/src
# 下载人形机器人消息定义
git clone https://github.com/ahornung/humanoid_msgs.git
# 下载人形机器人导航包
git clone https://github.com/ROBOTIS-GIT/humanoid_navigation.git

9.2 提取必要组件

# 只需要 footstep_planner 和 gridmap_2d
cp -r humanoid_navigation/footstep_planner .
cp -r humanoid_navigation/gridmap_2d .

9.3 编译步态规划包

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

9.4 启动步态规划

roslaunch ros_navigation_humanoid foot_planner.launch