WSL上のROS 2ワークスペースは、Visual Studio CodeのRemote - WSL接続で直接開けます。Linux側のlaunch、YAML、Python、C++を編集し、VS Code統合ターミナルでビルドやトピック・TFの確認まで行えるため、シミュレーションの設定変更とデバッグを同じ環境で完結できます。

今回は、このVisual Studio Code環境において、Gazebo上のUnitree Go2にLivox MID360相当のLiDARを搭載し、FAST-LIOで3D点群地図を保存した後、Nav2で自律走行を行うまでの実験を行いましたので、そのときの手順を整理して紹介します。

3D点群地図の保存にはFAST-LIOを使用します。一方、今回のGazeboシミュレーションでNav2が使用する2D occupancy gridは、Gazeboワールドに配置した障害物の座標から生成した固定地図です。自己位置にはGazeboのground-truthオドメトリを使用するため、Gazebo画面とRViz2のGo2位置を同じ座標系で扱えます。

ここで使用するCHAMP（不要な略語へ展開せず、四足歩行ロボット向けの制御フレームワーク）は、Go2の歩行を担当するソフトウェアです。Nav2から受け取った /cmd_vel の速度指令を歩容計算と関節軌道へ変換し、各脚を動かしてロボットを歩行させます。本記事では、Nav2が経路を計画し、CHAMPがその経路に沿ってGo2を実際に歩かせる役割分担になっています。

なお、本記事はWSL2（Ubuntu 22.04 LTS）、ROS 2 Humble、Unitree Go2（CHAMPコントローラ）、FAST-LIO、Livox-SDK2がセットアップされた環境（~/ros2_ws/src 配下に unitree_ros2、unitree-go2-ros2-gazebo、livox_ros_driver2、fast_lio リポジトリが配置済み）を前提としています。

今回の記事の流れ

WSL上のワークスペースをVisual Studio Codeで開く
必要パッケージのインストールとソースコードの準備
- 障害物ワールドの準備
- ワークスペースのビルド
- FAST-LIOによる3D点群地図の保存
Nav2用固定2D地図とウェイポイントの生成
Ground-truthオドメトリでGazeboとNav2を起動する
初期位置設定とRViz2表示
4地点の自律巡回
トラブルシューティング

1. WSL上のワークスペースをVisual Studio Codeで開く

Windows側にVisual Studio CodeとRemote Development拡張機能を導入しておきます。WSLのUbuntuターミナルから、ROS 2ワークスペースへ移動して以下を実行します。

cd ~/ros2_ws
code .

初回はVS Code Serverの導入を待つと、ウィンドウ左下に WSL: Ubuntu-22.04 と表示され、エクスプローラーに ros2_ws の内容が表示されます。

この状態では、WSL側のパスをそのまま開いて編集できます。統合ターミナルもWSL上のbashとして実行されるため、以降の source、colcon build、ros2 topic echo、ros2 launch はVS Code内から実行できます。

2. 必要パッケージのインストールとソースコードの準備

2.1 必須パッケージのインストール

ROS 2 Humbleの環境で、Nav2関連パッケージを導入します。

sudo apt update
sudo apt install -y \
  ros-humble-navigation2 \
  ros-humble-nav2-bringup \
    ros-humble-nav2-map-server \
    ros-humble-pointcloud-to-laserscan \
    ros-humble-slam-toolbox

2.2 Go2 URDFとLiDARの準備（`velodyne.xacro` & `laser.xacro`）

FAST-LIOの3D点群用には velodyne.xacro（/livox/lidar）、Nav2の2Dコストマップ用にはGo2の水平2Dレーザー laser.xacro（front_laser、720サンプル、最大6m）を使用します。

go2_description/xacro/robot.xacro から両方のセンサー定義が読み込まれていることを確認します。

<xacro:include filename="$(find go2_description)/xacro/gazebo.xacro"/>
<xacro:include filename="$(find go2_description)/xacro/laser.xacro"/>
<xacro:include filename="$(find go2_description)/xacro/velodyne.xacro"/>

laser.xacro では、horizontal の samples を 720、range の max を 6.0、プラグインのリマッピング先を /scan に設定しておきます。

<horizontal>
    <samples>720</samples>
    <resolution>1.000000</resolution>
    <min_angle>0.000000</min_angle>
    <max_angle>6.283185307179586</max_angle>
</horizontal>
<range>
    <min>0.120000</min>
    <max>6.0</max>
    <resolution>0.01</resolution>
</range>
<plugin name="gazebo_ros_front_laserscan" filename="libgazebo_ros_ray_sensor.so">
    <ros>
        <remapping>~/out:=/scan</remapping>
    </ros>
    <output_type>sensor_msgs/LaserScan</output_type>
    <frame_name>front_laser</frame_name>
</plugin>

この設定変更には次の理由があります。

水平方向の samples は、元の 40 サンプルでは粗い分解能となり、障害物の輪郭が飛び飛びになって2D地図の形状を十分に再現できなかったため 720 にしました。360度を約0.5度刻みで測定できます。
出力先を /scan に固定したのは、Gazeboの水平レーザーをそのまま slam_toolbox、AMCL、Nav2のコストマップで利用し、3D点群からの追加投影処理をなくすためです。3D LiDARを pointcloud_to_laserscan で投影する方式では、床面やTFの影響で2D地図が安定しませんでした。
laser.xacro の最大距離は元の 3.5m から 6.0m に変更しました。今回の屋内ワールドで障害物内側の巡回検証に必要な範囲を確保しつつ、遠距離の不要な観測を取り込まないための実験上の値です。すべての環境に必須の値ではありません。

2.3 FAST-LIO設定（`mid360.yaml`）

FAST-LIOのトピック名と3D PCD保存先を設定します。編集対象のファイルは以下の通りです。

~/ros2_ws/src/fast_lio/config/mid360.yaml

common、preprocess、mapping、pcd_save セクションを環境に合わせて設定します。

map_file_path: "/home/<user>/ros2_ws/src/go2_nav2_fastlio/maps/indoor_room.pcd"

common:
    lid_topic:  "/livox/lidar"
    imu_topic:  "/imu/data"

preprocess:
    lidar_type: 2
    scan_line:  32
    timestamp_unit: 2

mapping:
    extrinsic_T: [ 0.2, 0.0, 0.08 ]

pcd_save:
    pcd_save_en: true
    interval: -1

map_file_path の <user> 部分は自身のユーザー名に置き換えます。

lidar_type: 2 は、Gazebo側でVelodyne互換のレーザープラグインから点群を出力しているためです。extrinsic_T は、velodyne.xacro の base_link から livox_frame への固定オフセット [0.2, 0.0, 0.08] と一致させます。これにより、FAST-LIOの点群・推定位置とロボットモデルのセンサー位置を同じ配置で扱えます。

pcd_save_en: true は /map_save サービスによる保存を有効にする設定です。interval: -1 は走行中の全フレームを1つの indoor_room.pcd にまとめ、今回の3D点群地図を1ファイルで扱うために使用しています。長時間走行ではメモリ使用量が増えるため、別の実験では適切な分割間隔を設定してください。

2.4 CHAMPコントローラのタイムアウトとground-truthモード追加

CHAMPの速度指令保持による連続走行を防ぐため、quadruped_controller にタイムアウト処理を追加します。また、ground-truthオドメトリ使用時にCHAMPのEKF TFを無効化できるよう use_ground_truth_odom 引数を追加します。

cmd_vel_timeout: 0.5 は、teleopやNav2からの速度指令が途絶えた後も前回の速度が保持され続けた問題への対策です。0.5秒間新しい指令が届かなければ速度をゼロにし、停止指令の取りこぼしによる継続走行を防ぎます。

最終的な自律巡回でも、CHAMPは歩行制御としてそのまま使用します。一方、オドメトリ推定だけはGazeboのground-truthへ置き換えます。実験中、CHAMPのEKFが推定する /odom はGazebo上のGo2の実際の移動量と大きくずれました。この推定値をNav2へ渡すと、固定地図上の自己位置とGazebo上の実位置がずれ、経路計画や障害物回避の基準も不正確になります。

さらに、CHAMPのEKFとGazeboのground-truthが同じ base_link 系のTFを配信すると、TFツリーの親子関係が競合します。そのため use_ground_truth_odom:=true でCHAMP側のEKF TFを無効化し、Gazeboの /odom/ground_truth を ground_truth_odom.py でNav2用の /odom と odom -> base_footprint -> base_link へ中継します。つまり、歩行制御はCHAMP、自己位置はGazebo ground-truthという役割分担です。

`champ_base/include/quadruped_controller.h`

// 速度指令途絶用のタイムアウト管理メンバー
rclcpp::Time last_cmd_vel_time_;
double cmd_vel_timeout_;

`champ_base/src/quadruped_controller.cpp`

// コンストラクタ内
this->declare_parameter("cmd_vel_timeout", 0.5);
cmd_vel_timeout_ = this->get_parameter("cmd_vel_timeout").as_double();
last_cmd_vel_time_ = clock_.now();

// controlLoop_() 内
if ((clock_.now() - last_cmd_vel_time_).seconds() > cmd_vel_timeout_)
{
    req_vel_.linear.x = 0.0;
    req_vel_.linear.y = 0.0;
    req_vel_.angular.z = 0.0;
}

// cmdVelCallback_() 内
last_cmd_vel_time_ = clock_.now();

`champ_bringup/launch/bringup.launch.py`

declare_use_ground_truth_odom = DeclareLaunchArgument(
    "use_ground_truth_odom",
    default_value="false",
    description="Disable CHAMP EKF TF when Gazebo ground truth is used",
)

base_to_footprint_ekf = Node(
    package="robot_localization",
    executable="ekf_node",
    name="base_to_footprint_ekf",
    output="screen",
    condition=UnlessCondition(LaunchConfiguration("use_ground_truth_odom")),
    # ...
)

footprint_to_odom_ekf = Node(
    package="robot_localization",
    executable="ekf_node",
    name="footprint_to_odom_ekf",
    output="screen",
    condition=UnlessCondition(LaunchConfiguration("use_ground_truth_odom")),
    # ...
)

`go2_config/launch/gazebo.launch.py`

declare_use_ground_truth_odom = DeclareLaunchArgument(
    "use_ground_truth_odom",
    default_value="false",
    description="Disable CHAMP EKF TF for Gazebo ground-truth navigation",
)

# bringup_ld の launch_arguments に追加
"use_ground_truth_odom": LaunchConfiguration("use_ground_truth_odom"),

2.5 自作パッケージ `go2_nav2_fastlio` の構成

Nav2統合とウェイポイント巡回を行う go2_nav2_fastlio パッケージを作成・配置します。

ディレクトリ構成

~/ros2_ws/src/go2_nav2_fastlio/
├── package.xml
├── setup.py
├── setup.cfg
├── config/
│   └── indoor_room_waypoints.yaml
├── launch/
│   └── navigation.launch.py
├── maps/
│   ├── indoor_room_ground_truth.pgm
│   └── indoor_room_ground_truth.yaml
└── go2_nav2_fastlio/
    ├── __init__.py
    ├── create_indoor_room_map.py
    ├── ground_truth_odom.py
    ├── send_goal.py
    ├── run_waypoint_tour.py
    ├── set_initial_pose.py
    └── waypoint_markers.py

`package.xml`

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>go2_nav2_fastlio</name>
  <version>0.1.0</version>
    <description>Unitree Go2 (CHAMP) + FAST-LIO + Nav2 integration: mapping, navigation params, and waypoint runner.</description>
  <maintainer email="user@example.com">yoossh</maintainer>
  <license>Apache-2.0</license>

  <exec_depend>rclpy</exec_depend>
  <exec_depend>geometry_msgs</exec_depend>
  <exec_depend>nav_msgs</exec_depend>
    <exec_depend>pointcloud_to_laserscan</exec_depend>
    <exec_depend>slam_toolbox</exec_depend>
  <exec_depend>tf2_ros</exec_depend>
  <exec_depend>nav2_bringup</exec_depend>
  <exec_depend>nav2_map_server</exec_depend>
  <exec_depend>visualization_msgs</exec_depend>

    <test_depend>ament_copyright</test_depend>
    <test_depend>ament_flake8</test_depend>
    <test_depend>ament_pep257</test_depend>
    <test_depend>python3-pytest</test_depend>

  <export>
    <build_type>ament_python</build_type>
  </export>
</package>

`setup.py`

from setuptools import setup
import os
from glob import glob

package_name = 'go2_nav2_fastlio'

setup(
    name=package_name,
    version='0.1.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        (os.path.join('share', package_name, 'launch'), glob('launch/*.py')),
        (os.path.join('share', package_name, 'config'), glob('config/*.yaml')),
        (os.path.join('share', package_name, 'maps'), glob('maps/*')),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='yoossh',
    maintainer_email='user@example.com',
    description='Unitree Go2 (CHAMP) + FAST-LIO + Nav2 integration.',
    license='Apache-2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'create_indoor_room_map = go2_nav2_fastlio.create_indoor_room_map:main',
            'ground_truth_odom = go2_nav2_fastlio.ground_truth_odom:main',
            'run_waypoint_tour = go2_nav2_fastlio.run_waypoint_tour:main',
            'send_goal = go2_nav2_fastlio.send_goal:main',
            'set_initial_pose = go2_nav2_fastlio.set_initial_pose:main',
            'waypoint_markers = go2_nav2_fastlio.waypoint_markers:main',
        ],
    },
)

`setup.cfg`

[develop]
script_dir=$base/lib/go2_nav2_fastlio

[install]
install_scripts=$base/lib/go2_nav2_fastlio

`go2_nav2_fastlio/ground_truth_odom.py`

p3d_base_controller プラグイン（gazebo.xacro）が配信する /odom/ground_truth（親: world、子: base_link）を受信し、Nav2が要求する /odom トピックと odom -> base_footprint -> base_link のTFへ変換・中継します。

import math
import rclpy
from geometry_msgs.msg import TransformStamped
from nav_msgs.msg import Odometry
from rclpy.node import Node
from tf2_ros import TransformBroadcaster

class GroundTruthOdomRelay(Node):
    def __init__(self):
        super().__init__("ground_truth_odom_relay")
        self._odom_publisher = self.create_publisher(Odometry, "/odom", 10)
        self._tf_broadcaster = TransformBroadcaster(self)
        self._subscription = self.create_subscription(
            Odometry, "/odom/ground_truth", self._callback, 10
        )

    def _callback(self, message):
        yaw = math.atan2(
            2.0 * (message.pose.pose.orientation.w * message.pose.pose.orientation.z + message.pose.pose.orientation.x * message.pose.pose.orientation.y),
            1.0 - 2.0 * (message.pose.pose.orientation.y * message.pose.pose.orientation.y + message.pose.pose.orientation.z * message.pose.pose.orientation.z),
        )

        odom = Odometry()
        odom.header.stamp = message.header.stamp
        odom.header.frame_id = "odom"
        odom.child_frame_id = "base_footprint"
        odom.pose.pose.position.x = message.pose.pose.position.x
        odom.pose.pose.position.y = message.pose.pose.position.y
        odom.pose.pose.orientation.z = math.sin(yaw / 2.0)
        odom.pose.pose.orientation.w = math.cos(yaw / 2.0)
        odom.twist = message.twist
        self._odom_publisher.publish(odom)

        odom_to_footprint = TransformStamped()
        odom_to_footprint.header = odom.header
        odom_to_footprint.child_frame_id = "base_footprint"
        odom_to_footprint.transform.translation.x = odom.pose.pose.position.x
        odom_to_footprint.transform.translation.y = odom.pose.pose.position.y
        odom_to_footprint.transform.rotation = odom.pose.pose.orientation

        footprint_to_base = TransformStamped()
        footprint_to_base.header.stamp = message.header.stamp
        footprint_to_base.header.frame_id = "base_footprint"
        footprint_to_base.child_frame_id = "base_link"
        footprint_to_base.transform.translation.z = message.pose.pose.position.z
        footprint_to_base.transform.rotation.w = 1.0

        self._tf_broadcaster.sendTransform([odom_to_footprint, footprint_to_base])

def main(args=None):
    rclpy.init(args=args)
    node = GroundTruthOdomRelay()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

`launch/navigation.launch.py`

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

def generate_launch_description():
    pkg_share = get_package_share_directory("go2_nav2_fastlio")
    nav2_share = get_package_share_directory("nav2_bringup")

    default_map = os.path.join(pkg_share, "maps", "indoor_room_ground_truth.yaml")
    nav2_params = os.path.join(nav2_share, "params", "nav2_params.yaml")

    map_yaml = LaunchConfiguration("map")
    use_sim_time = LaunchConfiguration("use_sim_time")

    declare_map = DeclareLaunchArgument(
        "map", default_value=default_map, description="Absolute path to map YAML"
    )
    declare_use_sim_time = DeclareLaunchArgument(
        "use_sim_time", default_value="true", description="Use Gazebo clock"
    )

    ground_truth_odom = Node(
        package="go2_nav2_fastlio",
        executable="ground_truth_odom",
        name="ground_truth_odom_relay",
        output="screen",
        parameters=[{"use_sim_time": use_sim_time}],
    )

    nav2 = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(nav2_share, "launch", "bringup_launch.py")
        ),
        launch_arguments={
            "map": map_yaml,
            "params_file": nav2_params,
            "use_sim_time": use_sim_time,
            "autostart": "true",
            "use_composition": "False",
        }.items(),
    )

    return LaunchDescription([
        declare_map,
        declare_use_sim_time,
        ground_truth_odom,
        nav2,
    ])

`go2_nav2_fastlio/set_initial_pose.py`

use_sim_time:=true のパラメータを自動処理し、/initialpose トピックへ5回連続で初期姿勢メッセージを送信して自動終了するスクリプトです。

import math
import rclpy
from geometry_msgs.msg import PoseWithCovarianceStamped
from rclpy.node import Node

class InitialPosePublisher(Node):
    def __init__(self):
        super().__init__("go2_initial_pose_publisher")
        self.declare_parameter("x", 0.0)
        self.declare_parameter("y", 0.0)
        self.declare_parameter("yaw", 0.0)
        self._publisher = self.create_publisher(PoseWithCovarianceStamped, "/initialpose", 10)
        self._publish_count = 0
        self._timer = self.create_timer(0.5, self._publish)

    def _publish(self):
        if self._publisher.get_subscription_count() == 0:
            self.get_logger().info("Waiting for an AMCL initial-pose subscriber.")
            return

        yaw = self.get_parameter("yaw").value
        message = PoseWithCovarianceStamped()
        message.header.frame_id = "map"
        message.header.stamp = self.get_clock().now().to_msg()
        message.pose.pose.position.x = self.get_parameter("x").value
        message.pose.pose.position.y = self.get_parameter("y").value
        message.pose.pose.orientation.z = math.sin(yaw / 2.0)
        message.pose.pose.orientation.w = math.cos(yaw / 2.0)
        message.pose.covariance[0] = 0.05
        message.pose.covariance[7] = 0.05
        message.pose.covariance[35] = 0.02
        self._publisher.publish(message)
        self._publish_count += 1
        self.get_logger().info("Published initial pose %d/5 x=%.3f y=%.3f yaw=%.3f" % (self._publish_count, message.pose.pose.position.x, message.pose.pose.position.y, yaw))
        if self._publish_count == 5:
            self._timer.cancel()
            rclpy.shutdown()

def main(args=None):
    rclpy.init(args=args)
    node = InitialPosePublisher()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

`go2_nav2_fastlio/run_waypoint_tour.py`

indoor_room_waypoints.yaml から4地点を読み込み、NavigateToPose アクションを順番に呼び出して目標到達ごとに次の地点へ進むスクリプトです。

import math
import os
import rclpy
import yaml
from ament_index_python.packages import get_package_share_directory
from geometry_msgs.msg import PoseStamped
from nav2_msgs.action import NavigateToPose
from rclpy.action import ActionClient
from rclpy.node import Node

class WaypointTourRunner(Node):
    def __init__(self):
        super().__init__("go2_waypoint_tour_runner")
        self._client = ActionClient(self, NavigateToPose, "navigate_to_pose")
        self._waypoints = self._load_waypoints()
        self._current_index = 0
        self._send_next_waypoint()

    def _load_waypoints(self):
        config_path = os.path.join(
            get_package_share_directory("go2_nav2_fastlio"),
            "config",
            "indoor_room_waypoints.yaml",
        )
        with open(config_path, "r", encoding="utf-8") as config_file:
            return yaml.safe_load(config_file)["waypoints"]

    def _send_next_waypoint(self):
        if self._current_index == len(self._waypoints):
            self.get_logger().info("Waypoint tour completed.")
            rclpy.shutdown()
            return

        if not self._client.wait_for_server(timeout_sec=30.0):
            self.get_logger().error("Nav2 action server navigate_to_pose is unavailable.")
            rclpy.shutdown()
            return

        waypoint = self._waypoints[self._current_index]
        goal = NavigateToPose.Goal()
        goal.pose = PoseStamped()
        goal.pose.header.frame_id = "map"
        goal.pose.header.stamp = self.get_clock().now().to_msg()
        goal.pose.pose.position.x = waypoint["x"]
        goal.pose.pose.position.y = waypoint["y"]
        goal.pose.pose.orientation.z = math.sin(waypoint["yaw"] / 2.0)
        goal.pose.pose.orientation.w = math.cos(waypoint["yaw"] / 2.0)
        self.get_logger().info("Waypoint %d/%d: %s (%.2f, %.2f)" % (self._current_index + 1, len(self._waypoints), waypoint["name"], waypoint["x"], waypoint["y"]))
        future = self._client.send_goal_async(goal)
        future.add_done_callback(self._goal_response_callback)

    def _goal_response_callback(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().error("Nav2 rejected waypoint %d." % (self._current_index + 1))
            rclpy.shutdown()
            return

        result_future = goal_handle.get_result_async()
        result_future.add_done_callback(self._result_callback)

    def _result_callback(self, future):
        result = future.result()
        if result.status == 4:
            self.get_logger().info("Reached waypoint %d." % (self._current_index + 1))
            self._current_index += 1
            self._send_next_waypoint()
        else:
            self.get_logger().error("Waypoint %d finished with action status %d." % (self._current_index + 1, result.status))
            rclpy.shutdown()

def main(args=None):
    rclpy.init(args=args)
    node = WaypointTourRunner()
    if rclpy.ok():
        rclpy.spin(node)
    node.destroy_node()

2.6 indoor_room.worldへ障害物を追加する

FAST-LIOで保存する3D点群に障害物を含めるため、PCD保存を始める前にワールドへ障害物を追加します。編集対象は次のファイルです。

~/ros2_ws/src/unitree-go2-ros2-gazebo/robots/configs/go2_config/worlds/indoor_room.world

バックアップを作成します。

cd ~/ros2_ws/src/unitree-go2-ros2-gazebo/robots/configs/go2_config/worlds
cp indoor_room.world indoor_room.world.before_nav2_obstacles

<world> 要素内の <state> より前へ、次の5つの <include> を追加します。既存のモデル名と重複しないことを確認してください。

<include>
    <uri>model://cinder_block</uri>
    <name>wall_block_1</name>
    <pose>2 1.5 0 0 0 0</pose>
</include>
<include>
    <uri>model://cinder_block</uri>
    <name>wall_block_2</name>
    <pose>2 -1.5 0 0 0 0</pose>
</include>
<include>
    <uri>model://cabinet</uri>
    <name>cabinet_1</name>
    <pose>-2 2 0 0 0 1.5708</pose>
</include>
<include>
    <uri>model://bookshelf</uri>
    <name>bookshelf_1</name>
    <pose>-2 -2 0 0 0 0</pose>
</include>
<include>
    <uri>model://wooden_case</uri>
    <name>wooden_case_1</name>
    <pose>0 3 0 0 0 0</pose>
</include>

2.7 ワークスペースをビルドする

障害物ワールドと自作パッケージの準備後にビルドします。

cd ~/ros2_ws
source /opt/ros/humble/setup.bash

colcon build --packages-select \
    champ_base \
    champ_bringup \
    go2_description \
    go2_config \
    go2_nav2_fastlio \
    fast_lio \
    livox_ros_driver2 \
    --symlink-install

source ~/ros2_ws/install/setup.bash

2.8 FAST-LIOで3D点群地図を保存する

Gazebo、FAST-LIO、キーボード操作ノードを起動します。world:= には、障害物を追加したワールドを指定してください。

# ターミナル1: Gazebo + Go2
ros2 launch go2_config gazebo.launch.py \
    world:=$HOME/ros2_ws/src/unitree-go2-ros2-gazebo/robots/configs/go2_config/worlds/indoor_room.world

# ターミナル2: FAST-LIO
ros2 launch fast_lio mapping.launch.py \
    config_file:=mid360.yaml \
    use_sim_time:=true

# ターミナル3: キーボード操作
ros2 run teleop_twist_keyboard teleop_twist_keyboard

障害物の周囲を走行し、次のサービスでPCDを保存します。

ros2 service call /map_save std_srvs/srv/Trigger "{}"
ls -lh ~/ros2_ws/src/go2_nav2_fastlio/maps/indoor_room.pcd

indoor_room.pcd を確認したら、次のNav2手順へ進む前に全ターミナルで Ctrl-C を押してFAST-LIOとGazeboを終了します。

3. Nav2用の固定地図とウェイポイントを生成する

indoor_room.world の障害物座標から、Gazebo world座標と完全一致する固定2D地図（indoor_room_ground_truth.pgm および .yaml）を生成します。

この固定地図へ切り替えたのは、FAST-LIOとCHAMPの推定オドメトリを使ったSLAM地図では、走行中のオドメトリ誤差によって障害物が放射状に歪み、Gazeboの実座標と一致しなかったためです。create_indoor_room_map.py では、モデル寸法に0.15mの安全余裕を加えて占有セルを作り、Go2のフットプリントが障害物へ近づきすぎないようにしています。

source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash

ros2 run go2_nav2_fastlio create_indoor_room_map

障害物に囲まれた内側を安全に巡回する4地点は、indoor_room_waypoints.yaml で定義します。

順番	名前	x	y	yaw
1	inner_southwest	-1.0	-1.0	0.0
2	inner_southeast	1.2	-1.0	1.571
3	inner_northeast	1.2	0.8	3.051
4	inner_northwest	-1.0	1.0	-1.571

各地点および地点間の直線経路は、固定地図の障害物セルから0.5m以上の安全距離を確保しています。

ウェイポイントは、保存したSLAM地図の自由領域をそのまま採用するのではなく、Gazeboワールドの障害物配置と固定地図の占有セルを基準に再選定しました。これにより、地図の歪みや未知領域の影響を避け、4地点間の経路も同じ座標系で検証できます。

4. Ground-truthオドメトリでGazeboとNav2を起動する

Nav2走行時は、Gazeboをground-truthオドメトリモードで起動します。

# ターミナル1: Gazebo + Go2
cd ~/ros2_ws
source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash

ros2 launch go2_config gazebo.launch.py \
  world:=$HOME/ros2_ws/src/unitree-go2-ros2-gazebo/robots/configs/go2_config/worlds/indoor_room.world \
  use_ground_truth_odom:=true

別ターミナルでオドメトリの出力（header.frame_id: world、child_frame_id: base_link）を確認します。

source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash

ros2 topic echo /odom/ground_truth --once

確認後、Nav2を起動します。

# ターミナル2: Nav2
cd ~/ros2_ws
source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash

ros2 launch go2_nav2_fastlio navigation.launch.py

navigation.launch.py は、Gazeboの /odom/ground_truth をNav2用 /odom および odom -> base_footprint -> base_link のTFへ中継します。

5. 初期位置を設定してRViz2表示を行う

Gazebo起動直後のGo2は原点付近に位置するため、以下のコマンドで初期位置を設定します。

# ターミナル3: 初期位置設定
source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash

ros2 run go2_nav2_fastlio set_initial_pose --ros-args \
  -p use_sim_time:=true \
  -p x:=0.0 \
  -p y:=0.0 \
  -p yaw:=0.0

Published initial pose 1/5 〜 5/5 が出力されてコマンドが自動終了した後、Nav2の起動完了を確認します。

ros2 lifecycle get /bt_navigator

active [3] が表示されたらRViz2を起動します。

# ターミナル4: RViz2
source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash

rviz2 -d /opt/ros/humble/share/nav2_bringup/rviz/nav2_default_view.rviz

Fixed Frame は map を使用します。コマンドで初期位置を設定済みのため、RViz2上で 2D Pose Estimate を行う必要はありません。

別ターミナルでウェイポイント表示ノードを起動し、RViz2の Add ボタンから MarkerArray を追加して Topic に /go2_waypoints を指定すると、緑の矢印とラベルで4地点が表示されます。

ros2 run go2_nav2_fastlio waypoint_markers

6. 4地点の自律巡回を実行する

まず1地点（inner_southwest）だけの到達を確認できます。

ros2 run go2_nav2_fastlio send_goal --ros-args \
  -p waypoint:=inner_southwest

1地点の正常到達を確認後、4地点の自動巡回を実行します。

ros2 run go2_nav2_fastlio run_waypoint_tour

run_waypoint_tour は NavigateToPose アクションを4回逐次実行します。各地点で到達を確認してから次の目標へ進むため、進捗ログを追いながら安全に巡回できます。

[INFO] [go2_waypoint_tour_runner]: Waypoint 1/4: inner_southwest (-1.00, -1.00)
[INFO] [go2_waypoint_tour_runner]: Reached waypoint 1.
[INFO] [go2_waypoint_tour_runner]: Waypoint 2/4: inner_southeast (1.20, -1.00)
[INFO] [go2_waypoint_tour_runner]: Reached waypoint 2.
...
[INFO] [go2_waypoint_tour_runner]: Waypoint tour completed.

1つ目の inner_southwest ウェイポイントに向かう様子（RViz2）

1つ目の inner_southwest ウェイポイントに向かう様子（Gazebo）

2つ目の inner_southeast ウェイポイントに向かう様子（RViz2）

2つ目の inner_southeast ウェイポイントに向かう様子（Gazebo）

4つ目の inner_northwest ウェイポイントに向かう様子（RViz2）

4つ目の inner_northwest ウェイポイントに向かう様子（Gazebo）

途中で想定外の動きが見られた場合は、RViz2画面左下の Navigation2 パネルにある Pause ボタンをクリックすることで、即座に走行を停止できます。

7. トラブルシューティング

初期位置設定前の警告ログ
AMCL cannot publish a pose... や Timed out waiting for transform from base_link to map は、初期位置を設定する前によく出力される待機メッセージです。set_initial_pose を実行すると解消されます。
Nav2ノードが unconfigured のまま止まる場合
lifecycle_manager_navigation が planner_server の応答を待つ間にタイムアウトした可能性があります。負荷を抑えるため、他ターミナルでの不要なログ出力や監視コマンドを一度止め、Nav2だけを起動し直します。
3D点群保存からNav2移行時のトピック・TF競合
FAST-LIOを使った3D PCD保存のプロセスが動いたままNav2を起動すると、/scan やオドメトリのTFが重複して自律走行が暴走する原因になります。必ずFAST-LIO環境を Ctrl-C で停止してからNav2を起動します。

まとめ

今回は、Gazebo上のUnitree Go2において、FAST-LIOによる3D点群地図（PCD）の保存と、Gazeboワールド座標に一致する固定2D地図・ground-truthオドメトリを用いたNav2による4地点自動巡回を行いました。

FAST-LIOの /map_save サービスで3D点群を .pcd として保存
Gazeboワールドの障害物座標から精度高い固定2D地図を生成
/odom/ground_truth をNav2用の /odom および odom -> base_footprint -> base_link のTFへ中継
障害物に囲まれた内側の4地点を NavigateToPose で1地点ずつ順次巡回
cmd_vel_timeout=0.5 秒の設定により、速度指令が途絶えた場合の安全停止を確保

最後までお読みいただき、ありがとうございました。