宇树机器人 G1 二次开发：导航仿真与地图转换教程

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

四、栅格地图编辑

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

6.2 修改局部代价地图参数

nano local_costmap_params.yaml

修改内容：

robot_base_frame: base_link

6.3 修改局部规划器参数

nano base_local_planner_params.yaml

修改内容：

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

6.4 修改 RViz 仿真节点

roscd ros_navigation_humanoid/src
nano rviz_sim.cpp

修改内容：

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

七、重新编译与验证

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