ROS 2下navigation 2 stack的构建

系列文章目录

思岚激光雷达rplidar从ROS 1到ROS 2的移植
订阅rviz2的导航目标位置消息“/goal_pose”
为Navigation 2创建自定义behavior tree plugin
打断behavior tree的异步动作节点，并执行其他节点动作

---

前言

项目整个迁移到ROS 2架构，其中涉及到了navigation stack的迁移构建，把过程写出来，希望能提供帮助和借鉴

---

一. ROS 2的navigation变化

相比于ROS Navigation stack，ROS 2 Navigation 2 stack引入了Behavior Tree作为机器人的逻辑流程控制，并对整个navigation stack做出了适应性的修改，同时更强调plugin的组合架构。ROS 2 Navigation 2 stack的组织框架为下图：

ROS 2 Navigation 2相比于ROS Navigation的变化如下，详细内容参看官网链接：
https://navigation.ros.org/about/ros1_comparison.html#ros1-comparison

移植的packages：
amcl: 移植为 nav2_amcl
map_server: 移植为 nav2_map_server
nav2_planner: 用 N planner plugins，即插件代替global_planner
nav2_controller: 用 N controller plugins，即插件代替local_planner
Navfn: 移植为 nav2_navfn_planner
DWB: 用 nav2_dwb_controller 代替 DWA
nav_core: 移植为 nav2_core
costmap_2d: 移植为 nav2_costmap_2d

新增的packages：
nav2_bt_navigator: 代替 move_base 的状态机机制
nav2_lifecycle_manager: 管理服务程序的生命周期
nav2_waypoint_follower: 用于多点导航功能
nav2_system_tests: 教程相关的测试案例
nav2_rviz_plugins: rviz中于navigation相关控制的插件
nav2_experimental: 试验代码（未正式发布），包括了深度学习相关的控制器
navigation2_behavior_trees: ROS action servers使用Behavior tree的接口

二. 本说明的环境配置

1.系统

ubuntu 20.04
ROS foxy
苇航智能NaviBOT A0 package

2.安装navigation 2

在终端输入命令：

sudo apt install ros-foxy-navigation2
sudo apt install ros-foxy-nav2-bringup
sudo apt install ~nros-foxy-turtlebot3*
sudo apt install ~nros-foxy-gazebo-*

NOTE：如果ubuntu及ROS版本很新，会出现turtlebot3的相应deb还未发布，可以使用下载源码编译安装的方式，参考官网链接：
https://emanual.robotis.com/docs/en/platform/turtlebot3/ros2_setup/

三. turtlebot3 navigation simulation

turtlebot3是关于navigation最好的示例，通过查看turtlebot3的navigation运行过程，以此指导navigation 2 stack的构建，以及自主开发导航机器人驱动package

1. 运行navigation simulation

turtlebot3的navigation simulation需要gazebo的配合，gazebo的初次使用需要加入GAZEBO_MODEL_PATH，打开新终端，输入命令：

echo 'export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:~/turtlebot3_ws/src/turtlebot3/turtlebot3_simulations/turtlebot3_gazebo/models' >> ~/.bashrc
source ~/.bashrc

导出turtlebot3的型号，苇航智能使用“burger”，在终端继续输入命令：

export TURTLEBOT3_MODEL=burger

运行gazebo加载导航的环境，包括地图以及机器人信息，在终端继续输入命令：

ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

NOTE：第一次运行时，可能会需要下载环境模型，但国内由于墙的原因下载会失败，如果有条件可以使用proxy，比如proxychains，如下图：

运行后如下图：

然后运行导航，打开一个新终端，再次导出turtlebot3的型号，输入命令：

export TURTLEBOT3_MODEL=burger
ros2 launch turtlebot3_navigation2 navigation2.launch.py use_sim_time:=True map:=/home/whi/turtlebot3_ws/install/turtlebot3_navigation2/share/turtlebot3_navigation2/map/map.yaml

运行后将启动rviz，并显示上述命令所指定的地图，如下图：

此时，导航处于未开启状态，需要通过给定initial position来激活导航，点击“2D Pose Estimate”在地图上选择一个initial position，此时，导航将开启，如下图：

TIP：关于turtlebot3 simulation的更多使用，可以参考官网链接：
https://emanual.robotis.com/docs/en/platform/turtlebot3/ros2_simulation/#ros-2-simulation

2. 查看turtlebot3 navigation的msgs及tf frames

msgs：
在终端输入命令：

ros2 run rqt_graph rqt_graph

打开后同时勾选“tf”，如下图：

tf frames：
在终端输入命令：

ros2 run tf2_tools view_frames.py

运行完毕提示“Generating graph…”，并在所运行命令的目录生成“frames.pdf”文件，如下图：

上述过程生成的是导航模式下turtlebot3的tf tree，可以看到tr tree完整的结构为“map->odom->base_footprint->base_link->xxx”，如下图：

由此可以推断：在非导航模式，即只运行turtlebot3，则应该生成的tf tree为“odom->base_footprint->base_link->xxx”，该tf tree也是自主开发机器人驱动时需要满足的tf tree结构

3. 查看turtlebot3的description

和ROS 1一样，在ROS 2中也有robot_state_publisher，该node的作用是发布机器人的描述，包括关节信息、关节间的tf，同时订阅joint_state以接收动态关节的信息，关于robot_state_publisher的详细信息可以参考官网链接：
http://wiki.ros.org/robot_state_publisher
https://index.ros.org/r/robot_state_publisher/

turtlebot3 bringup中有“turtlebot3_state_publisher.launch.py”launch文件，用于运行robot_state_publisher。可以运行该文件查看turtlebot3的机器人信息，tf tree的关系

打开一个新终端，输入命令：

export TURTLEBOT3_MODEL=burger
ros2 launch turtlebot3_bringup turtlebot3_state_publisher.launch.py

运行后将显示发布机器人信息，如下图：

使用rviz2来可视化机器人的信息

打开一个新终端，输入命令：

ros2 run rviz2 rviz2

在运行的rviz2中，加入“RobotModel”，如下图：

然后在“RobotModel”中展开“Description Topic”，并选择“/robot_description”，如下图：

此时将显示turtlebot3的model，但为白色没有任何渲染，如下图：

这是因为“Fixed Frame”当前为“map”的原因，因为目前机器人为静态状态，所以没有map->odom->base_footprint->base_link->xxx的tf tree，所以需要调整fixed frame，比如选择为base_footprint，此时将完整显示turtlebot3模型的完整渲染，如下图：

通过选择不同的Fixed Frame，能够帮助查看各个静态frame或者link之间的static tf关系，如下图中为分别选择了base_link和base_scan后的显示：

该关系通过机器人的urdf模型文件设置，即该文件中指定parent和child的片段，如下图：

该设置表明从base_link到base_scan需要在x方向移动-0.032m，在z方向移动0.172m，这个静态transfer关系还能通过查看消息/tf_static确认

打开一个新终端，输入命令：

ros2 topic list

可以看到，当前robot_state_publiser发布了如下消息，其中就包括静态link之间的tf_static消息，如下图：

在终端继续输入命令：

ros2 topic echo /tf_static

查看消息内容，对应base_link和base_scan部分的translation数据即为上述urdf中的设置数据，如下图：

4. turtlebot3的navigation package

无论是在ROS 1还是ROS 2，turtlebot3的navigation都是一个独立的package存在，这是因为在ROS架构下的message的发布和订阅机制，得益于该机制，导航过程可以是完全运行于机器人本身，也可以运行于远程的机器，特别是当机器人本身处理能力有局限的时候。该机制在ROS 1和ROS 2的区别就是ROS 1使用了TCP协议，而在ROS 2下，因为引入了DDS中间层，协议变为了UDP

下面例子为raspberry PI（IP：192.168.10.116）接收来自PC（IP：192.168.10.111）发布的tf_static消息，如下图：

四. 创建navigation package

充分利用ROS分布式架构带来的优势，对navigation单独创建一个package，这样既能满足在机器人本地运行导航，也能通过远程的方式运行导航

首先，创建package。在终端输入命令：

cd ~/dev_ws/src
ros2 pkg create --build-type ament_cmake motion_ctrl_nav

创建package后，修改其package.xml中的版本、维护者、版权等信息

其次，建立rviz目录，用于存储运行rviz导航时的configuration文件；建立launch目录，用于存储运行导航时的launch文件；建立param目录，用于存储导航控制的配置参数如下图：

最后，编辑motion_ctrl_nav的CMakeLists文件，加入rivz、launch、param的安装目录，便于后续launch的调用。打开其CMakeLists.txt增加如下语句：

install(DIRECTORY
  launch
  param
  rviz
  DESTINATION share/${PROJECT_NAME}
)

如下图：

五. 建立导航控制参数

首先，创建param_NaviBOT_A0.yaml的空文件，用于存储导航控制参数，如下图：

NOTE：从复用性考虑，这里文件加入了苇航智能机器人硬件相关的型号

然后，复制如下内容至该文件：

amcl:
  ros__parameters:
    use_sim_time: False
    alpha1: 0.2
    alpha2: 0.2
    alpha3: 0.2
    alpha4: 0.2
    alpha5: 0.2
    base_frame_id: "base_footprint"
    beam_skip_distance: 0.5
    beam_skip_error_threshold: 0.9
    beam_skip_threshold: 0.3
    do_beamskip: false
    global_frame_id: "map"
    lambda_short: 0.1
    laser_likelihood_max_dist: 2.0
    laser_max_range: 100.0
    laser_min_range: -1.0
    laser_model_type: "likelihood_field"
    max_beams: 60
    max_particles: 2000
    min_particles: 500
    odom_frame_id: "odom"
    pf_err: 0.05
    pf_z: 0.99
    recovery_alpha_fast: 0.0
    recovery_alpha_slow: 0.0
    resample_interval: 1
    robot_model_type: "differential"
    save_pose_rate: 0.5
    sigma_hit: 0.2
    tf_broadcast: true
    transform_tolerance: 1.0
    update_min_a: 0.2
    update_min_d: 0.25
    z_hit: 0.5
    z_max: 0.05
    z_rand: 0.5
    z_short: 0.05

amcl_map_client:
  ros__parameters:
    use_sim_time: False

amcl_rclcpp_node:
  ros__parameters:
    use_sim_time: False

bt_navigator:
  ros__parameters:
    use_sim_time: False
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
    plugin_lib_names:
    - nav2_compute_path_to_pose_action_bt_node
    - nav2_follow_path_action_bt_node
    - nav2_back_up_action_bt_node
    - nav2_spin_action_bt_node
    - nav2_wait_action_bt_node
    - nav2_clear_costmap_service_bt_node
    - nav2_is_stuck_condition_bt_node
    - nav2_goal_reached_condition_bt_node
    - nav2_goal_updated_condition_bt_node
    - nav2_initial_pose_received_condition_bt_node
    - nav2_reinitialize_global_localization_service_bt_node
    - nav2_rate_controller_bt_node
    - nav2_distance_controller_bt_node
    - nav2_speed_controller_bt_node
    - nav2_recovery_node_bt_node
    - nav2_pipeline_sequence_bt_node
    - nav2_round_robin_node_bt_node
    - nav2_transform_available_condition_bt_node
    - nav2_time_expired_condition_bt_node
    - nav2_distance_traveled_condition_bt_node

bt_navigator_rclcpp_node:
  ros__parameters:
    use_sim_time: False

controller_server:
  ros__parameters:
    use_sim_time: False
    controller_frequency: 10.0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    controller_plugins: ["FollowPath"]

    # DWB parameters
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      debug_trajectory_details: True
      min_vel_x: 0.0
      min_vel_y: 0.0
      max_vel_x: 0.22
      max_vel_y: 0.0
      max_vel_theta: 1.0
      min_speed_xy: 0.0
      max_speed_xy: 0.22
      min_speed_theta: 0.0
      # Add high threshold velocity for turtlebot 3 issue.
      # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
      acc_lim_x: 2.5
      acc_lim_y: 0.0
      acc_lim_theta: 3.2
      decel_lim_x: -2.5
      decel_lim_y: 0.0
      decel_lim_theta: -3.2
      vx_samples: 20
      vy_samples: 5
      vtheta_samples: 20
      sim_time: 1.7
      linear_granularity: 0.05
      angular_granularity: 0.025
      transform_tolerance: 0.2
      xy_goal_tolerance: 0.25
      trans_stopped_velocity: 0.25
      short_circuit_trajectory_evaluation: True
      stateful: True
      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
      BaseObstacle.scale: 0.02
      PathAlign.scale: 32.0
      PathAlign.forward_point_distance: 0.1
      GoalAlign.scale: 24.0
      GoalAlign.forward_point_distance: 0.1
      PathDist.scale: 32.0
      GoalDist.scale: 24.0
      RotateToGoal.scale: 32.0
      RotateToGoal.slowing_factor: 5.0
      RotateToGoal.lookahead_time: -1.0

controller_server_rclcpp_node:
  ros__parameters:
    use_sim_time: False

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      use_sim_time: False
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      robot_radius: 0.105
      plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: pointcloud
        pointcloud:
          topic: /intel_realsense_r200_depth/points
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "PointCloud2"
      static_layer:
        map_subscribe_transient_local: True
      always_send_full_costmap: True
  local_costmap_client:
    ros__parameters:
      use_sim_time: False
  local_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: False

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      use_sim_time: False
      robot_radius: 0.105
      resolution: 0.05
      plugins: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: pointcloud
        pointcloud:
          topic: /intel_realsense_r200_depth/points
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "PointCloud2"
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
      always_send_full_costmap: True
  global_costmap_client:
    ros__parameters:
      use_sim_time: False
  global_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: False

map_server:
  ros__parameters:
    use_sim_time: False
    yaml_filename: "turtlebot3_world.yaml"

map_saver:
  ros__parameters:
    use_sim_time: False
    save_map_timeout: 5000
    free_thresh_default: 0.25
    occupied_thresh_default: 0.65

planner_server:
  ros__parameters:
    expected_planner_frequency: 20.0
    use_sim_time: False
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.5
      use_astar: false
      allow_unknown: true

planner_server_rclcpp_node:
  ros__parameters:
    use_sim_time: False

recoveries_server:
  ros__parameters:
    costmap_topic: local_costmap/costmap_raw
    footprint_topic: local_costmap/published_footprint
    cycle_frequency: 10.0
    recovery_plugins: ["spin", "backup", "wait"]
    spin:
      plugin: "nav2_recoveries/Spin"
    backup:
      plugin: "nav2_recoveries/BackUp"
    wait:
      plugin: "nav2_recoveries/Wait"
    global_frame: odom
    robot_base_frame: base_link
    transform_timeout: 0.1
    use_sim_time: False
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: 0.4
    rotational_acc_lim: 3.2

robot_state_publisher:
  ros__parameters:
    use_sim_time: False

仔细观察上述文件，并对比ROS 1中navigation的构建，会发现ROS 2把导航参数都集中到一个文件管理，更加直观，维护也更方便

六. 建立导航launch文件

首先，创建“navigation_local_NaviBOT_A0.launch.py”和“navigation_remote_NaviBOT_A0.launch.py”两个空文件，用于存储导航控制参数。其中第一个用于本地导航运行，第二个用于远程导航运行，如下图：

NOTE：从复用性考虑，这里文件加入了苇航机器人硬件相关的型号；同时建立的为运行在机器人本地的导航方式，添加了local属性

然后，复制如下内容至文件“navigation_local_NaviBOT_A0.launch.py”：

# Copyright 2019 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
# Author: Xinjue Zou

import os

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


def generate_launch_description():
    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
    
    map_dir = LaunchConfiguration(
        'map',
        default='/home/whi/dev_ws/map.yaml')
    
    params_dir = '/home/whi/dev_ws/src/motion_ctrl_nav/param/param_NaviBOT_A0.yaml'

    nav2_launch_file_dir = os.path.join(get_package_share_directory('nav2_bringup'), 'launch')

    rviz_config_dir = os.path.join(
        get_package_share_directory('motion_ctrl_nav'),
        'rviz',
        'nav_view.rviz')
        
    motion_ctrl_pkg_dir = LaunchConfiguration('motion_ctrl_pkg_dir',
        default=os.path.join(get_package_share_directory('motion_ctrl'), 'launch'))

    return LaunchDescription([
        DeclareLaunchArgument(
            'map',
            default_value='/home/whi/dev_ws/map.yaml',
            description='Full path to map file to load'),

        DeclareLaunchArgument(
            'params_file',
            default_value=params_dir,
            description='Full path to param file to load'),

        DeclareLaunchArgument(
            'use_sim_time',
            default_value='false',
            description='Use simulation (Gazebo) clock if true'),
            
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource([motion_ctrl_pkg_dir, '/NaviBOT_A0.launch.py'])
        ),

        IncludeLaunchDescription(
            PythonLaunchDescriptionSource([nav2_launch_file_dir, '/bringup_launch.py']),
            launch_arguments={
                'map': map_dir,
                'use_sim_time': use_sim_time,
                'params_file': params_dir}.items(),
        ),

        Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            arguments=['-d', rviz_config_dir],
            parameters=[{'use_sim_time': use_sim_time}],
            output='screen'),
])

NOTE：上述内容中参数目录params_dir直接使用了motion_ctrl_nav package中param目录的绝对地址，这是为了方便导航参数的调整，也可以使用“get_package_share_directory”，但这样就需要每次修改导航参数后，再次编译motion_ctrl_nav package

然后，复制如下内容至文件“navigation_remote_NaviBOT_A0.launch.py”：

# Copyright 2019 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
# Author: Xinjue Zou

import os

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


def generate_launch_description():
    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
    
    map_dir = LaunchConfiguration(
        'map',
        default='/home/whi/dev_ws/map.yaml')
    
    params_dir = '/home/whi/dev_ws/src/motion_ctrl_nav/param/param_NaviBOT_A0.yaml'

    nav2_launch_file_dir = os.path.join(get_package_share_directory('nav2_bringup'), 'launch')

    rviz_config_dir = os.path.join(
        get_package_share_directory('motion_ctrl_nav'),
        'rviz',
        'nav_view.rviz')

    return LaunchDescription([
        DeclareLaunchArgument(
            'map',
            default_value='/home/whi/dev_ws/map.yaml',
            description='Full path to map file to load'),

        DeclareLaunchArgument(
            'params_file',
            default_value=params_dir,
            description='Full path to param file to load'),

        DeclareLaunchArgument(
            'use_sim_time',
            default_value='false',
            description='Use simulation (Gazebo) clock if true'),

        IncludeLaunchDescription(
            PythonLaunchDescriptionSource([nav2_launch_file_dir, '/bringup_launch.py']),
            launch_arguments={
                'map': map_dir,
                'use_sim_time': use_sim_time,
                'params_file': params_dir}.items(),
        ),

        Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            arguments=['-d', rviz_config_dir],
            parameters=[{'use_sim_time': use_sim_time}],
            output='screen'),
])

NOTE：remote的launch文件和local的相比，少了嵌套调用NaviBOT_A0.launch.py的过程

NOTE：上述两个文件都有和苇航智能机器人驱动相关的代码，请根据实际情况做适当的修改

七. 编译navigation package

在终端输入命令：

colcon build --packages-select motion_ctrl_nav

编译motion_ctrl_nav，编译完成后将在workspace的install目录下生成motion_ctrl_nav目录以及其下share\launch以及share\param，如下图：

八. 实现base_footprint到base_link的translation

苇航智能的机器人驱动没有用代码的方式实现base_footprint到base_link之间的translation,通过ros2 run命令直接运行苇航智能的机器人驱动后的tr tree如下图，可以看到只有odom到base_footprint的tf tree：

苇航智能采用了static transform的方式实现base_footprint与base_link之间的关联。和ROS 1一样，在ROS 2中static transform可以在launch文件中实现，比如思岚激光雷达做逆时针旋转180度的静态translation：

Node(
    package='tf2_ros',
    node_executable='static_transform_publisher',
    node_name='base_to_laser',
    arguments=['0', '0', '0.125', ‘3.14’, '0', '0',
               'base_link', 'laser_frame],
output='screen'),

但该方式不直观，不便于维护时的理解，以及模块的调整，因此，苇航智能使用了robot_state_publisher发布机器人信息的方式实现base_footprint到base_link之间的关联。robot_state_publisher的详细信息可以参考官网链接：
http://wiki.ros.org/robot_state_publisher
https://index.ros.org/r/robot_state_publisher/

1. frame坐标基准的定义

苇航智能规定base_footprint坐标基准为地面、base_link坐标基准为驱动轮的轴心，详细坐标关系如下图：

2. 建立机器人的urdf信息

在苇航智能机器人驱动motion_ctrl package的根目录下，建立“urdf”文件夹，同时建立空白文件“NaviBOT_A0.urdf”，如下图：

复制如下内容至该文件：

机器人信息文件urdf建立后，需要建立一个launch来运行robot_state_publisher，也为后续导航包调用提供嵌套的launch。在motion_ctrl package的launch目录下建立空白文件“NaviBOT_A0_state_publisher.launch.py”如下图：

复制如下内容至该文件：

#!/usr/bin/env python3
#
# Copyright 2020 Wheel Hub Intelligent CO., LTD.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
# Authors: Xinjue Zou

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
    
    urdf_file_name = 'NaviBOT_A0.urdf'
    print("urdf_file_name : {}".format(urdf_file_name))

    urdf = LaunchConfiguration('urdf',
        default=os.path.join(get_package_share_directory('motion_ctrl'), 'urdf', urdf_file_name))

    return LaunchDescription([
        DeclareLaunchArgument(
            'use_sim_time',
            default_value='false',
            description='Use simulation (Gazebo) clock if true'),

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

编译package motion_ctrl，编译完成后将在workspace的“install//share/”目录下生成urdf目录及文件NaviBOT_A0.urdf

NOTE：motion_ctrl的CMakeLists文件需要添加对应的install内容，才能在install//share目录下生成上述的urdf及其文件，可以参照章节“创建navigation package”中install的内容

编译后，通过运行launch文件“NaviBOT_A0_state_publisher.launch.py”，并使用rviz来可视化机器人模型

在终端输入命令：

ros2 launch motion_ctrl NaviBOT_A0_state_publisher.launch.py

运行机器人的state publisher，发布模型信息，以及static transform。打开新终端，输入命令：

ros2 run rviz2 rviz2

运行rviz，启动后添加“RobotModel”，并选择“Description Topic”为“/robot_description”，“Fixed Frame”为“base_link”，将显示机器人的模型，如下图：

TIP：可以尝试改变Fixed Frame查看各个frames之间的基准关系

最后，使用robot_state_publisher最终目的为获得base_footprint到base_link的tf tree，因此，在新终端中，输入命令：

ros2 topic list

查看是否有/tf_static主题的消息，如下图：

再通过命令：

ros2 topic echo /tf_static

查看该消息发布的，在urdf中各个frame间的translation，可以看到base_footprint到base_link之间的translation，如下图：

NOTE： 本章节内容提到了苇航智能的机器人驱动package，但超出本说明范围，不展开说明

九. 检查tf tree

前面提到navigation需要有完整结构为“map->odom->base_footprint->base_link->xxx”的tf tree，在开始运行导航之前，首先检查是否满足该要求

在终端中输入命令：

ros2 launch motion_ctrl NaviBOT_A0.launch.py

以苇航智能机器人驱动的launch文件为例，打开新终端，输入命令：

ros2 run tf2_tools view_frames.py

等待运行完毕后，将在运行命令的目录下生成名为“frames.pdf”文件，查看是否有完备tr tree，如下图：

NOTE：上述命令“ros2 launch motion_ctrl NaviBOT_A0.launch.py”中的py文件为苇航智能机器人驱动package的launch文件

十. 尝试运行

完成上述步骤后，navigation已经构建完毕，可以尝试运行，查看是否遗留其他问题

打开新终端，输入命令：

sudo su
. install/setup.bash
ros2 launch motion_ctrl_nav navigation_NaviBOT_A0.launch.py map:=/home/whi/dev_ws/whiMap_h.yaml

NOTE：因为苇航智能的机器人驱动package需要root权限，因此进入了su用户，同时需要给定导航地图的绝对路径

一切顺利的话将开启rivz，如下图：

因为这里使用了navigation2默认的rviz配置，所以同样需要首先指定initial position来激活导航，点击“2D Pose Estimate”给定机器人initial position后将激活导航，如下图：

十一. 远程运行

得益于ROS的消息机制，ROS 1和ROS 2都能远程运行导航，而让机器人本身只运行运动驱动、lidar雷达、以及发送必要的odom和tf tree，这样可以借助远程高性能的机器提高导航的运算频率

1. 机器人端

打开新终端，输入命令：

cd ~/dev_ws
sudo su
. install/setup.bash
ros2 launch motion_ctrl NaviBOT_A0.launch.py

运行机器人驱动、lidar、发布odom和tf tree，如下图：

2. 远程PC端

首先，需要将导航需要的地图拷贝到PC的workspace下，比如dev_ws，同时将motion_ctrl_nav package拷贝到PC workspace的src目录下，分别如下图：

然后，打开新终端，输入命令：

cd ~/dev_ws
colcon build

编译motion_ctrl_nav package，编译完成后，继续在终端输入命令：

ros2 launch motion_ctrl_nav navigation_remote_NaviBOT_A0.launch.py map:=/home/whi/dev_ws/whiMap_h.yaml

远程PC将打开rviz并显示导航地图，如下图：

和运行local一样，给定机器人的initial position以激活导航，指定导航目标点，机器人将被远程PC导航控制的开始运行，其中机器人IP：192.168.10.116，远程PC IP：192.168.10.111，如下图：

---

总结

至此，ROS 2的navigation 2 stack已经构建完毕，本说明着重关注navigation stack本身，其中涉及到部分苇航智能机器人驱动package的内容，但不影响navigation构建的过程和思路

如果本说明有错误和不明确的地方，欢迎指正，共同交流，[email protected]