ROS 2: Nav2でGPS, IMU, LiDARを使ってGPS Waypoint Followerを動かす- gazebo編

宇宙系のロボット開発サークルで制御の開発をしています。

アメリカで行われる火星探査機の世界大会UniversityRoverChallengeに出場した際、Navigation2を使ってGPSポイントを巡るような探査機型ロボットの制御開発をしていたのでその備忘録になります。

本記事では、Nav2のtutorialにあるnav2_gps_waypoint_follower_demoをベースに、自分たちのURDFを用いながらgazebo上で複数点設定したGNSSポイントを巡れるようなパッケージを紹介します。

今回取り扱うパッケージはnav_rover_controlという名前で公開しています。
https://github.com/SoraKarimata/navigation_sim_tutorial/tree/main/nav_rover_control

1. URDFにpluginを追加

開発環境

項目	Version
Ubuntu	22.04
ROS 2	Iron
CPU	Intel Core i7-12700H
GPU	Intel Arc A550M
Memory	16GB

gazebo上でロボットのモデルを表示、動作させるためにはURDF形式のファイルを用いる必要があります。今回は4輪独立ステアを搭載したローバーを用いて進めていきます。URDFの作成について詳しくはこの記事を参考にしてください。

1.1 はじめに

全体像として、最初にnav2のtutorialにあるgps waypoint follower demoとこの記事で扱うパッケージについて説明します。
まず、ベースとして使っているnav2のtutorialは、turtlebot3で主にGPS、IMU、LiDARからのセンサデータを用いてナヴィゲーションを行っています。GNSSからのデータをOdometryとして用いることでグローバルマップ上においてロボットの自己位置を反映することができます。そのため、例えば砂漠など大規模なフィールドでも目標のGNSS座標まで障害物を避けながら経路を生成することができます。
この記事ではこのtutorialのモデルを自分たちの任意のモデルに置き換えて、gazebo上で設定した目標座標までローバーを自律走行させる流れを紹介します。

1.2 Install

最初に必要なパッケージをダウンロードしていきます。環境によって他に必要な依存パッケージがあるかもしれないので、エラーが出たときには必要なパッケージをインストールしてください。

gazebo, ros2_controlのインストール

sudo apt install ros-iron-ros2-control
sudo apt install ros-iron-ros2-controllers
sudo apt install gazebo
sudo apt install ros-iron-gazebo-ros-pkgs
sudo apt install ros-iron-gazebo-ros2-control

navigation2のインストール

sudo apt install ros-iron-navigation2
sudo apt install ros-iron-nav2-bringup

robot localizationのインストール

sudo apt install ros-$ROS_DISTRO-robot-localization
sudo apt install ros-$ROS_DISTRO-mapviz
sudo apt install ros-$ROS_DISTRO-mapviz-plugins
sudo apt install ros-$ROS_DISTRO-tile-map

1.2 ros2_control

ここからURDFにpluginを追加していきます。URDFについてはこの記事で使用したURDFをカスタマイズして使用します。
最初はURDF内で設定したjointが司令を受けて可動できるようにros2_controlの設定を新しく追加していきます。

新しくURDF内に追加

ares8_rover.urdf

 <ros2_control name="GazeboSystem" type="system">
    <hardware>
        <plugin>gazebo_ros2_control/GazeboSystem</plugin>
    </hardware>

    <joint name="lf_tire">
        <command_interface name="velocity">
            <param name="min">-3.14</param>
            <param name="max">3.14</param>
        </command_interface>
        <state_interface name="position">
            <param name="initial_value">0</param>
        </state_interface>
        <state_interface name="velocity"/>
    </joint>

    <joint name="rf_tire">
        <command_interface name="velocity">
            <param name="min">-3.14</param>
            <param name="max">3.14</param>
        </command_interface>
        <state_interface name="position">
            <param name="initial_value">0</param>
        </state_interface>
        <state_interface name="velocity"/>
    </joint>

    <joint name="lb_tire">
        <command_interface name="velocity">
            <param name="min">-3.14</param>
            <param name="max">3.14</param>
        </command_interface>
        <state_interface name="position">
            <param name="initial_value">0</param>
        </state_interface>
        <state_interface name="velocity"/>
    </joint>

    <joint name="rb_tire">
        <command_interface name="velocity">
            <param name="min">-3.14</param>
            <param name="max">3.14</param>
        </command_interface>
        <state_interface name="position">
            <param name="initial_value">0</param>
        </state_interface>
        <state_interface name="velocity"/>
    </joint>

    <joint name="lf_str">
        <command_interface name="position">
            <param name="min">-3.14</param>
            <param name="max">3.14</param>
        </command_interface>
        <state_interface name="position">
            <param name="initial_value">0</param>
        </state_interface>
        <state_interface name="velocity"/>
    </joint>

    <joint name="rf_str">
        <command_interface name="position">
            <param name="min">-3.14</param>
            <param name="max">3.14</param>
        </command_interface>
        <state_interface name="position">
            <param name="initial_value">0</param>
        </state_interface>
        <state_interface name="velocity"/>
    </joint>

    <joint name="lb_str">
        <command_interface name="position">
            <param name="min">-3.14</param>
            <param name="max">3.14</param>
        </command_interface>
        <state_interface name="position">
            <param name="initial_value">0</param>
        </state_interface>
        <state_interface name="velocity"/>
    </joint>

    <joint name="rb_str">
        <command_interface name="position">
            <param name="min">-3.14</param>
            <param name="max">3.14</param>
        </command_interface>
        <state_interface name="position">
            <param name="initial_value">0</param>
        </state_interface>
        <state_interface name="velocity"/>
    </joint>
 </ros2_control>

 <gazebo>
    <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
        <parameters>/home/karisora/ros2_ws/install/nav_rover_control/share/nav_rover_control/config/ares8_rover.yaml</parameters>
    </plugin>
 </gazebo>

ares8_rover.yamlは自分が作成するファイル名と絶対パスに置き換えてください。ares8_rover.yamlの内容については2.3で詳しく説明します。

1.3 URDF sensor plugin

ナビゲーションをするためにIMUとLiDAR、GPSデータが必要なため、URDF内に新しくgazebo上で動くセンサをpluginとして追加します。
IMUやLiDARのlinkを設定してローバー本体であるbase_linkとjointで接続します。

ares8_rover.urdf

<link name="imu_link">
  <inertial>
    <mass value="0.01"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
  </inertial>
</link>

<joint name="imu_joint" type="fixed">
    <parent link="base_link"/>
    <child link="imu_link"/>
    <origin xyz="0 0 0.1" rpy="0 0 0"/>
</joint>

<link name="gps_link"/>


<joint name="gps_joint" type="fixed">
    <parent link="base_link"/>
    <child link="gps_link"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
</joint>

<link name="lidar_link">
    <inertial>
        <origin xyz="0 0 4" rpy="0 0 0"/>
        <mass value="0.1"/>
        <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
    </inertial>
</link>

<joint name="lidar_joint" type="fixed">
    <parent link="base_link"/>
    <child link="lidar_link"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
</joint>

センサー系のpluginを追加しgazebo上でIMUやLiDARが使えるようにします。

ares8_rover.urdf

<gazebo reference="imu_link">
    <sensor name="imu_sensor" type="imu">
        <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
            <ros>
                <remapping>~/out:=imu</remapping>
            </ros>
            <initial_orientation_as_reference>false</initial_orientation_as_reference>
        </plugin>
        <always_on>true</always_on>
        <update_rate>100</update_rate>
        <visualize>true</visualize>
        <imu>
            <angular_velocity>
                <x>
                    <noise type="gaussian">
                        <mean>0.0</mean>
                        <stddev>2e-4</stddev>
                        <bias_mean>0.0000075</bias_mean>
                        <bias_stddev>0.0000008</bias_stddev>
                    </noise>
                </x>
                <y>
                    <noise type="gaussian">
                        <mean>0.0</mean>
                        <stddev>2e-4</stddev>
                        <bias_mean>0.0000075</bias_mean>
                        <bias_stddev>0.0000008</bias_stddev>
                    </noise>
                </y>
                <z>
                    <noise type="gaussian">
                        <mean>0.0</mean>
                        <stddev>2e-4</stddev>
                        <bias_mean>0.0000075</bias_mean>
                        <bias_stddev>0.0000008</bias_stddev>
                    </noise>
                </z>
            </angular_velocity>
            <linear_acceleration>
                <x>
                    <noise type="gaussian">
                        <mean>0.0</mean>
                        <stddev>1.7e-2</stddev>
                        <bias_mean>0.1</bias_mean>
                        <bias_stddev>0.001</bias_stddev>
                    </noise>
                </x>
                <y>
                    <noise type="gaussian">
                        <mean>0.0</mean>
                        <stddev>1.7e-2</stddev>
                        <bias_mean>0.1</bias_mean>
                        <bias_stddev>0.001</bias_stddev>
                    </noise>
                </y>
                <z>
                    <noise type="gaussian">
                        <mean>0.0</mean>
                        <stddev>1.7e-2</stddev>
                        <bias_mean>0.1</bias_mean>
                        <bias_stddev>0.001</bias_stddev>
                    </noise>
                </z>
            </linear_acceleration>
        </imu>
    </sensor>
    <sensor name="my_gps" type="gps">
        <always_on>true</always_on>
        <update_rate>10</update_rate>
        <plugin name="my_gps_plugin" filename="libgazebo_ros_gps_sensor.so">
            <ros>
                <remapping>~/out:=gps/fix</remapping>
            </ros>
        </plugin>
    </sensor>
    <sensor name="gazebo_lidar" type="ray">
        <always_on>true</always_on>
        <visualize>true</visualize>
        <update_rate>60</update_rate>
        <plugin filename="libgazebo_ros_ray_sensor.so" name="gazebo_lidar">
            <ros>
                <remapping>~/out:=scan</remapping>
            </ros>
            <output_type>sensor_msgs/LaserScan</output_type>
            <frame_name>lidar_link</frame_name>
        </plugin>
        <ray>
            <scan>
                <horizontal>
                    <samples>360</samples>
                    <resolution>1.000000</resolution>
                    <min_angle>-3.140000</min_angle>
                    <max_angle>3.140000</max_angle>
                </horizontal>
            </scan>
            <range>
                <min>1.0</min>
                <max>6.0</max>
                <resolution>0.015</resolution>
            </range>
            <noise>
                <type>gaussian</type>
                <mean>0.0</mean>
                <stddev>0.01</stddev>
            </noise>
        </ray>
    </sensor>
</gazebo>

1.4 URDF

最終的にこのようなURDFの形になっています。摩擦係数やlinkの質量、慣性モーメントなど細かいパラメータも変更しています。
https://github.com/SoraKarimata/navigation_sim_tutorial/blob/main/nav_rover_control/urdf/ares8_rover.urdf

2. ファイルの追加・編集

基本的にはNav2のtutorialにあるnav2_gps_waypoint_follower_demoをそのままベースとして使っています。

2.1 launchファイル

自分たちのローバーモデルをgazebo上にスポーンさせるares8_rover.launch.pyを作成します。
nav2や他のlaunchファイルを起動させるgps_waypoint_follower.launch.pyの内容を編集します。

gps_waypoint_follower.launch.py

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

def generate_launch_description():
    # Get the launch directory
    bringup_dir = get_package_share_directory('nav2_bringup')
    gps_wpf_dir = get_package_share_directory(
        "nav_rover_control")
    launch_dir = os.path.join(gps_wpf_dir, 'launch')
    params_dir = os.path.join(gps_wpf_dir, "config")
    nav2_params = os.path.join(params_dir, "nav2_no_map_params.yaml")
    configured_params = RewrittenYaml(
        source_file=nav2_params, root_key="", param_rewrites="", convert_types=True
    )
    use_rviz = LaunchConfiguration('use_rviz')
    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
    urdf_file_name = 'ares8_rover.urdf'

    print('urdf_file_name : {}'.format(urdf_file_name))

    urdf = os.path.join(
        get_package_share_directory('nav_rover_control'),
        'urdf',
        urdf_file_name)

    with open(urdf, 'r') as infp:
        robot_desc = infp.read()

    declare_use_rviz_cmd = DeclareLaunchArgument(
        'use_rviz',
        default_value='True',
        description='Whether to start RVIZ')

    gazebo_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(launch_dir, 'ares8_rover.launch.py'))
    )

    robot_state_publisher_node = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='robot_state_publisher',
        output='screen',
        parameters=[{
            'use_sim_time': use_sim_time,
            'robot_description': robot_desc,
        }],
        arguments=[urdf]
    )

    robot_localization_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(launch_dir, 'dual_ekf_navsat.launch.py'))
    )

    navigation2_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(bringup_dir, "launch", "navigation_launch.py")
        ),
        launch_arguments={
            "use_sim_time": "True",
            "params_file": configured_params,
            "autostart": "True",
        }.items(),
    )

    rover_control = Node(
        package='nav_rover_control',
        executable='rover_control',
        output='screen',
        parameters=[configured_params, {'use_sim_time': True}],
        name='rover_control_node'
    )

    static_transform_publisher = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        name='static_transform_publisher',
        output='screen',
        arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom']
    )

    rviz_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(bringup_dir, "launch", 'rviz_launch.py')),
        condition=IfCondition(use_rviz)
    )

    ld = LaunchDescription()
    # simulator launch
    ld.add_action(gazebo_cmd)
    ld.add_action(robot_state_publisher_node)
    # robot localization launch
    ld.add_action(robot_localization_cmd)
    ld.add_action(static_transform_publisher)
    # navigation2 launch
    ld.add_action(navigation2_cmd)
    # rover controller launch
    ld.add_action(rover_control)
    # rviz launch
    ld.add_action(declare_use_rviz_cmd)
    ld.add_action(rviz_cmd)

    return ld

ares8_rover.launch.py

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler
from launch.event_handlers import OnProcessExit
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
import xacro
import os

def generate_launch_description():
    # Constants for paths to different files and folders
    gazebo_pkg_name = 'nav_rover_control'
    robot_name_in_model = 'ares8_rover'
    urdf_file_path = 'urdf/ares8_rover.urdf'
    world_file_path = 'worlds/basic.world'
    # Pose where we want to spawn the robot
    spawn_x_val = '0.0'
    spawn_y_val = '0.0'
    spawn_z_val = '0.0'
    spawn_yaw_val = '0.0'

    gazebo_pkg_share = FindPackageShare(package=gazebo_pkg_name).find(gazebo_pkg_name)
    world_path = os.path.join(gazebo_pkg_share, world_file_path)

    gazebo = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
            get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']),
            launch_arguments={'world': world_path}.items(),

    )

    urdf_model_path = os.path.join(gazebo_pkg_share, urdf_file_path)

    doc = xacro.parse(open(urdf_model_path))
    xacro.process_doc(doc)


    spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
                        arguments=['-topic', 'robot_description',
                                   '-entity', robot_name_in_model,
                                   '-x', spawn_x_val,
                                   '-y', spawn_y_val,
                                   '-z', spawn_z_val,
                                   '-Y', spawn_yaw_val,
                                   ],
                        output='screen')

    load_joint_state_controller = ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'joint_state_broadcaster'],
        output='screen'
    )

    load_tire_controller = ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller',  '--set-state', 'active', 'tire_controller'],
        output='screen'
    )

    load_str_controller = ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller',  '--set-state', 'active', 'str_controller'],
        output='screen'
    )


    nodes = [
        # Gazebo
        RegisterEventHandler(
            event_handler=OnProcessExit(
                target_action=spawn_entity,
                on_exit=[load_joint_state_controller],
            )
        ),
        RegisterEventHandler(
            event_handler=OnProcessExit(
                target_action=load_joint_state_controller,
                on_exit=[
                    load_tire_controller,
                    load_str_controller,
                ],
            )
        ),
        gazebo,
        spawn_entity,
    ]

    return LaunchDescription(nodes)

2.2 pythonファイル

nav2では/cmd_velというtopic名でTwist型で司令が送られます。このパッケージでは/cmd_velからのTwistデータを速度と角度に変換しそれぞれをタイヤの速度とステアリングの角度データとして新しくローバーの操作司令としてpublishしています。今は条件分けしてかなり雑な制御でローバーを動かしていますが、実機の方ではもう少しきれいに制御しています。

rover_control.py

#!/usr/bin/python3
import rclpy
import numpy as np
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray
from geometry_msgs.msg import Twist

class Commander(Node):

    def __init__(self):
        super().__init__('commander')
        timer_period = 0.01  # 100Hz
        self.pos = np.array([0.0, 0.0, 0.0, 0.0])
        # lf_str, rf_str, lb_str, rb_str

        self.vel = np.array([0.0, 0.0, 0.0, 0.0])
        # lf_tire, rf_tire, lb_tire, rb_tire

        self.pub_pos = self.create_publisher(Float64MultiArray, '/str_controller/commands', 10)
        self.pub_vel = self.create_publisher(Float64MultiArray, '/tire_controller/commands', 10)
        self.sub = self.create_subscription(Twist, '/cmd_vel', self.cmd_vel_callback, 10)
        self.latest_twist = Twist()
        self.timer = self.create_timer(timer_period, self.timer_callback)

    def cmd_vel_callback(self, msg: Twist):
        self.latest_twist = msg

    def timer_callback(self):
        linear_velocity = self.latest_twist.linear.x
        angular_velocity = self.latest_twist.angular.z

        if(linear_velocity > 0.1):
            linear_velocity = 5
        else:
            linear_velocity = 0

        if angular_velocity > 0.3:
            self.pos[0] = -0.5 * 1.2
            self.pos[1] = 0.5
            self.pos[2] = -0.5 * 1.2
            self.pos[3] = 0.5

            self.vel[0] = 5
            self.vel[1] = -5
            self.vel[2] = 5
            self.vel[3] = -5
        elif angular_velocity < -0.3:
            self.pos[0] = 0.5
            self.pos[1] = -0.5 * 1.2
            self.pos[2] = 0.5
            self.pos[3] = -0.5 * 1.2

            self.vel[0] = 5
            self.vel[1] = -5
            self.vel[2] = 5
            self.vel[3] = -5
        else:
            self.pos[0] = 0
            self.pos[1] = 0
            self.pos[2] = 0
            self.pos[3] = 0

            self.vel[0] = linear_velocity
            self.vel[1] = -linear_velocity
            self.vel[2] = linear_velocity
            self.vel[3] = -linear_velocity

        pos_array = Float64MultiArray(data=self.pos.tolist())
        vel_array = Float64MultiArray(data=self.vel.tolist())
        self.pub_pos.publish(pos_array)
        self.pub_vel.publish(vel_array)
        self.vel[:] = 0
        self.pos[:] = 0

def main(args=None):
    rclpy.init(args=args)
    commander = Commander()
    try:
        rclpy.spin(commander)
    except KeyboardInterrupt:
        pass
    commander.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

2.3 yamlファイル

ros2_controlでの設定をares8_rover.yamlとして保存しています。ジョイントやインターフェースの設定をこの中で記述しています。もし別のロボットモデルでシミュレーションを行うならこのyamlの設定も更新する必要があります。
またnav2_no_map_params.yamlとdual_ekf_navsat_paramsはデフォルト値から変更してodom_topicをbase_linkに変更したり、その他細かいパラメータ等を変更しているので詳しくはリンクから参照してください。

2.4 worldファイル

gazeboのワールドデータの設定です。ここでは初期地点のGNSSの設定や高度などの設定を行うことができます。このworldデータではフラットなワールドですが、町中や砂漠のような環境のモデルを用意すれば様々な環境でもシミュレーションすることができます（処理重め）。

basic.world

<?xml version="1.0" ?>
<sdf version="1.6">
  <world name="default">
    <!-- A global light source -->
    <scene>
      <ambient>0.95 0.95 0.95 1</ambient>
      <background>0.3 0.3 0.3 1</background>
      <shadows>true</shadows>
      <sky>
        <clouds>
          <speed>3</speed>
        </clouds>
      </sky>
    </scene>
    <include>
      <uri>model://sun</uri>
    </include>
    <include>
      <uri>model://ground_plane</uri>
      <pose>0 0 0 0 0 0</pose>
    </include>
    <spherical_coordinates>
      <!-- currently gazebo has a bug: instead of outputing lat, long, altitude in ENU
      (x = East, y = North and z = Up) as the default configurations, it's outputting (-E)(-N)U,
      therefore we rotate the default frame 180 so that it would go back to ENU 
      see: https://github.com/osrf/gazebo/issues/2022 --> 
      <surface_model>EARTH_WGS84</surface_model>
      <latitude_deg>38.42390688</latitude_deg>
      <longitude_deg>-110.7852678</longitude_deg>
      <elevation>488.0</elevation>
      <heading_deg>180</heading_deg>
    </spherical_coordinates>
    
    <physics type="ode">
      <real_time_update_rate>100.0</real_time_update_rate>
      <max_step_size>0.01</max_step_size>
      <real_time_factor>1</real_time_factor>
      <ode>
        <solver>
          <type>quick</type>
          <iters>150</iters>
          <precon_iters>0</precon_iters>
          <sor>1.400000</sor>
          <use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling>
        </solver>
        <constraints>
          <cfm>0.00001</cfm>
          <erp>0.2</erp>
          <contact_max_correcting_vel>2000.000000</contact_max_correcting_vel>
          <contact_surface_layer>0.01000</contact_surface_layer>
        </constraints>
      </ode>
    </physics>
  </world>
</sdf>

2.5 setup.py

nav_rover_control内に新しく作ったrover_control.pyのpythonファイルが参照できるようにconsole_scripts内に追加します。

3. gazebo上でのシミュレーション

3.1 gps_waypoint_follower.launch.py

このnav_rover_controlを任意のwsでbuildした後、下のコマンドを実行することでシミュレータを起動することができます。

ros2 launch nav_rover_control gps_waypoint_follower.launch.py

起動後Nav2 Goalの矢印で目標地点を与えるとローバーがその地点まで移動します。

3.2 logged_waypoint_follower.py

nav_rover_controlのconfig内にあるdemo_waypoints.yamlでローバーが巡りたい複数のwaypointを設定することができます。下のコマンドを実行することでここに書かれたwaypointを順番通りにローバーが巡ってくれます。
demo_waypoints.yamlは絶対パスなのでそれぞれの環境で合うように書き換えてください。

ros2 run nav_rover_control logged_waypoint_follower /home/karisora/ros2_ws/src/nav_rover_control/config/demo_waypoints.yaml

目標GPSの座標が遠いとき(数十メートル)、時々pathの生成に失敗してabortすることがあります。原因については検証中ですが、もし何回も失敗する場合はこのfollower.py内に復帰処理や目標GPS座標の変更などを行ってください。

4. パッケージ構造

4.1 rqt_graph

ここではセンサデータのノード関係について見ていきます。まずGPSデータは/gps/fixというtopic名で生データがpublishされ拡張カルマンフィルタを通して/odometry/globalに渡されています。/imuも同様にekf_filterを介して値が渡されます。フィルタを通すことでノイズなどがあってもある程度正確な状態推定を行うことができます。
これらの設定はnav2_no_map_params.yamlとdual_ekf_navsat_params内で行っています。

/cmd_velから受け取ったTwistデータをrover_control_nodeでそれぞれタイヤの速度データ/tire_controllerとステアリングの角度データ/str_controllerに変換してデータをpublishしていることがわかります。

4.2 tf_tree

参考にtf_treeの図も載せました。utmはnav2を立ち上げたときに設定されるロボットの初期位置のGNSS座標になります。この座標は予めnav2_no_map_params.yaml内で設定することも可能です。

4.3 最後に

今回はgazebo上で、nav2を使いgps waypoint followerを動かすところまでを紹介しました。ここから更にパッケージの編集と追加をすることで同じパッケージで実機ローバーを動かすことができます。次回はその方法について紹介します。