写在前面
安装nav2
nav2介绍
nav2实践
- map
- - 地图保存
  - 地图读取
- 状态估计(TF变换)
- - 所需TF坐标
  - - `base_link -> sensor frames`
    - `odom -> base_link`
    - - Laser Scan Matcher for ROS2
    - `map -> odom`
    - - `AMCL`使用
      - `AMCL`参数配置文件`amcl_config.yaml`
效果
过程中的知识点
- 一、launch文件编写格式及方法
- - 编写ROS2的launch文件
  - - 1.创建launch文件
    - 2.编写launch文件
    - 3.运行launch文件
    - - 3.1使用python启动launch
      - 3.2使用C++启动launch
    - 4.启动launch文件
- 二、ROS坐标系
- - 官方描述
  - 自己理解
- 三、urdf文件
- - 语法规则
  - xacro
结语

写在前面

前面一段时间在ros2和cartographer中摸爬滚打后，终于把图给建出来了，下一步应该就是根据建好的图利用nav2来进行导航了。

本篇博客主要讲解如果加载地图，并用AMCL算法实现定位，导航的部分放到下一部分当中。

环境

Ubuntu 22.04
ROS2 humble
激光雷达：镭神激光雷达M10P 网口版

安装nav2

参考官网安装
https://navigation.ros.org/getting_started/index.html

sudo apt install ros-<ros2-distro>-navigation2
sudo apt install ros-<ros2-distro>-nav2-bringup

其中，ros2-distro代表了发布的ros2版本，例如安装humble版就是sudo apt install ros-humble-navigation2

nav2介绍

由于nav2算法涵盖的知识点还是比较多的，这里推荐直接参考官方文档说明或者小鱼翻译的nav2中文版，贴个网址出来，可以学习一下nav2算法包含了一些什么东西。

1.nav2官网

2.《动手学ROS2》10.7 Nav2导航框架介绍与安装

这里是引用小鱼博客中的介绍介绍nav2是怎么实现导航的

nav2架构图见后文，已经粘出

小鱼对nav2架构进行解释，可以简单分为一大三小四个服务。
一大：
BT Navigator Server 导航行为树服务，通过这个大的服务来进行下面三个小服务组织和调用。
三小：
Planner Server，规划服务器，其任务是计算完成一些目标函数的路径。根据所选的命名法和算法，该路径也可以称为路线。说白了就是在地图上找路。
Controller Server，控制服务器，在ROS 1中也被称为局部规划器，是我们跟随全局计算路径或完成局部任务的方法。说白了就是根据找的路控制机器人走路。
Recovery Server，恢复服务器，恢复器是容错系统的支柱。恢复器的目标是处理系统的未知状况或故障状况并自主处理这些状况。说白了就是机器人可能出现意外的时候想办法让其正常，比如走着走着掉坑如何爬出来。
通过规划路径、控制机器人沿着路径运动、遇到问题自主恢复三者进行不断切换完成机器人的自主导航
原文链接：https://blog.csdn.net/qq_27865227/article/details/125051385

我这篇博客偏向于用nav2包进行应用，因此重点偏向于对实践过程中的记录

nav2实践

Nav2具有下列工具：
● 加载、提供和存储地图的工具（地图服务器Map Server）
● 在地图上定位机器人的工具 (AMCL)
● 避开障碍物从A点移动到B点的路径规划工具（Nav2 Planner）
● 跟随路径过程中控制机器人的工具（Nav2 Controller）
● 将传感器数据转换为机器人世界中的成本地图表达的工具（Nav2 Costmap 2D）
● 使用行为树构建复杂机器人行为的工具（Nav2 行为树和BT Navigator）
● 发生故障时计算恢复行为的工具（Nav2 Recoveries）
● 跟随顺导航点的工具（Nav2 Waypoint Follower）
● 管理服务器生命周期的工具和看门狗(Nav2 Lifecycle Manager)
● 启用用户自定义算法和行为的插件（Nav2 Core）

引用自https://navigation.ros.org/

与小鱼讲解nav2的博客教程相互结合进行学习
Nav2架构图
我们可以总结出来，需要启动nav2需要以下几个部分

map：地图，用来了解环境
Sensor Data：传感器数据传入到规划服务器(Planner Server)，用来找路
状态估计(TF变换)：告知不同坐标系的变换规则，建立map到机器人的关系,并确定机器人位于什么位置(nav2默认使用了AMCL(自适应蒙特卡洛定位)算法)
BT：机器人的行为决策

因此，我们如果要使用nav2，其实就是启动一个个nav2中的这些小组件，然后让nav2能够依据这些信息来导航。

map

我们在导航前，都要进行的一步就是建图，建图方法我之前采用了cartographer，可以参考我前一篇博客
ROS2+cartorgrapher+激光雷达建图并保存

这里贴一些指令

地图保存

在建图完毕之后，启动一个终端，需要用到nav2_map_server的功能包中的map_saver_cli保存地图

ros2 run nav2_map_server map_saver_cli -f map_name

这样地图就会以map_name的名字保存到你运行该指令的文件位置当中。
地图分为两部分.pgm和.yaml两个文件。

地图读取

与地图的保存类似，我们需要用到nav2_map_sever功能包中的map_server功能包来加载地图，并在map主题上提供静态地图

由于在导航的过程中需要启动的节点特别多，因此，我将所有的节点启动放到了一个launch文件中，launch文件的编写规则可以查看这篇博客的最后一节

针对地图读取的launch文件，思路就是写清楚map的文件路径，并启动map_server节点

注：nav2中节点都是由生命周期节点控制的，因此，还需要设置生命周期节点让该节点启动。
生命周期节点有助于确定ROS系统启动和关闭的状态是否正常。

map_file = os.path.join(get_package_share_directory('功能包目录'), 'map', 'map.yaml')
mapserver_node = launch_ros.actions.Node(
        package='nav2_map_server',
        executable='map_server',
        name='map_server',
        output='screen',
        parameters=[{'use_sim_time': False}, 
                    {'yaml_filename':map_file}]
    )
lifecycle_node = launch_ros.actions.Node(
        package='nav2_lifecycle_manager',
        executable='lifecycle_manager',
        name='lifecycle_manager_mapper',
        output='screen',
        parameters=[{'use_sim_time': False},
                    {'autostart': True},
                    {'node_names': ['map_server']}]  
    )

生命周期管理器要处理的节点是使用node_names参数设置的。 node_names参数接受一个有序的节点列表，以通过生命周期转换来启动。
Lifecycle Manager的另外两个参数是autostart 和 bond_timeout。

如果您想在启动时将转换节点设置为 Active状态，请将autostart 设置为true。否则，您将需要手动触发 Lifecycle Manager 以升级系统。
bond_timeout设置等待时间，以决定如果节点没有响应，何时向下转换所有节点

需要注意的是，这里的文件路径是功能包路径\map\map.yaml，在map目录下还存放了图片信息map.pgm

状态估计(TF变换)

所需TF坐标

根据ROS社区的项目标准，导航的项目里面需要提供两个主要的坐标转换，分别是map -> odom和odom -> base_link。

坐标变换的相关学习可以同样参考我前面一篇博客

在官网中提到，nav2没有必要一定使用激光雷达来实现导航，也可以使用其他传感器来进行，但是无论是使用激光雷达或者是其他传感器，都必须要遵循这个标准 ——REP 105，坐标系相关点也可以参考本博客最后一章节。

简而言之： 我们需要建立起以下坐标系之间的关系map -> odom -> base_link -> [sensor frames]

为了便于理解，我是从下面往上写一下TF坐标系变化的关系。其中包括参数文件的配置，这都是整套系统所必须的。

`base_link -> sensor frames`

sensor frames是我们机器人上面各个传感器的坐标及其位置关系。而base_link是与机器人底盘刚性连接的一个参考坐标系。可以在你底盘刚性连接上的任意一个地方，但其原点一般定义为机器人的旋转中心。

我们要建立base_link -> sensor frames的关系，就需要用到urdf文件了。

那什么是urdf文件呢

urdf文件就是用来描述机器人模型的一个文件，你可以编辑urdf文件来创造一个你自己的机器人。在urdf文件中，会描述各个坐标系之间的关系。具体的urdf语法规则我放到了本博客的最后一节当中，可以当作参考。

由于我自己项目用的小车是使用的两轮差速运动的小车，因此，我直接引用了官方的urdf文件，直接更改其中参数。

<?xml version="1.0"?>
<robot name="sam_bot" xmlns:xacro="http://ros.org/wiki/xacro">

  <!-- Define robot constants -->
  <xacro:property name="base_width" value="0.31"/>
  <xacro:property name="base_length" value="0.42"/>
  <xacro:property name="base_height" value="0.18"/>

  <xacro:property name="wheel_radius" value="0.10"/>
  <xacro:property name="wheel_width" value="0.04"/>
  <xacro:property name="wheel_ygap" value="0.025"/>
  <xacro:property name="wheel_zoff" value="0.05"/>
  <xacro:property name="wheel_xoff" value="0.12"/>

  <xacro:property name="caster_xoff" value="0.14"/>

  <!-- Define some commonly used intertial properties  -->
  <xacro:macro name="box_inertia" params="m w h d">
    <inertial>
      <origin xyz="0 0 0" rpy="${pi/2} 0 ${pi/2}"/>      
      <mass value="${m}"/>
      <inertia ixx="${(m/12) * (h*h + d*d)}" ixy="0.0" ixz="0.0" iyy="${(m/12) * (w*w + d*d)}" iyz="0.0" izz="${(m/12) * (w*w + h*h)}"/>
    </inertial>
  </xacro:macro>

  <xacro:macro name="cylinder_inertia" params="m r h">
    <inertial>
      <origin xyz="0 0 0" rpy="${pi/2} 0 0" />      
      <mass value="${m}"/>
      <inertia ixx="${(m/12) * (3*r*r + h*h)}" ixy = "0" ixz = "0" iyy="${(m/12) * (3*r*r + h*h)}" iyz = "0" izz="${(m/2) * (r*r)}"/> 
    </inertial>
  </xacro:macro>

  <xacro:macro name="sphere_inertia" params="m r">
    <inertial>
      <mass value="${m}"/>
      <inertia ixx="${(2/5) * m * (r*r)}" ixy="0.0" ixz="0.0" iyy="${(2/5) * m * (r*r)}" iyz="0.0" izz="${(2/5) * m * (r*r)}"/>
    </inertial>
  </xacro:macro>
  
  <!-- Robot Base -->
  <link name="base_link">
    <visual>
      <geometry>
        <box size="${base_length} ${base_width} ${base_height}"/>
      </geometry>
      <material name="Cyan">
        <color rgba="0 1.0 1.0 1.0"/>
      </material>
    </visual>

    <collision>
      <geometry>
        <box size="${base_length} ${base_width} ${base_height}"/>
      </geometry>
    </collision>

    <xacro:box_inertia m="15" w="${base_width}" d="${base_length}" h="${base_height}"/>
  </link>
  
   <!-- Robot Footprint -->
  <link name="base_footprint">
  	<xacro:box_inertia m="0" w="0" d="0" h="0"/>
  </link>

  <joint name="base_joint" type="fixed">
    <parent link="base_link"/>
    <child link="base_footprint"/>
    <origin xyz="0.0 0.0 ${-(wheel_radius+wheel_zoff)}" rpy="0 0 0"/>
  </joint>

  <!-- Wheels -->
  <xacro:macro name="wheel" params="prefix x_reflect y_reflect">
    <link name="${prefix}_link">
      <visual>
        <origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
        <geometry>
            <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
        </geometry>
        <material name="Gray">
          <color rgba="0.5 0.5 0.5 1.0"/>
        </material>
      </visual>

      <collision>
        <origin xyz="0 0 0" rpy="${pi/2} 0 0"/> 
        <geometry>
          <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
        </geometry>
      </collision>

      <xacro:cylinder_inertia m="0.5" r="${wheel_radius}" h="${wheel_width}"/>
    </link>

    <joint name="${prefix}_joint" type="continuous">
      <parent link="base_link"/>
      <child link="${prefix}_link"/>
      <origin xyz="${x_reflect*wheel_xoff} ${y_reflect*(base_width/2+wheel_ygap)} ${-wheel_zoff}" rpy="0 0 0"/>
      <axis xyz="0 1 0"/>
    </joint>
  </xacro:macro>

  <xacro:wheel prefix="drivewhl_l" x_reflect="-1" y_reflect="1" />
  <xacro:wheel prefix="drivewhl_r" x_reflect="-1" y_reflect="-1" />

  <link name="front_caster">
    <visual>
      <geometry>
        <sphere radius="${(wheel_radius+wheel_zoff-(base_height/2))}"/>
      </geometry>
      <material name="Cyan">
        <color rgba="0 1.0 1.0 1.0"/>
      </material>
    </visual>

    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <sphere radius="${(wheel_radius+wheel_zoff-(base_height/2))}"/>
      </geometry>
    </collision>

    <xacro:sphere_inertia m="0.5" r="${(wheel_radius+wheel_zoff-(base_height/2))}"/>
  </link>

  <joint name="caster_joint" type="fixed">
    <parent link="base_link"/>
    <child link="front_caster"/>
    <origin xyz="${caster_xoff} 0.0 ${-(base_height/2)}" rpy="0 0 0"/>
  </joint>

  <link name="imu_link">
    <visual>
      <geometry>
        <box size="0.1 0.1 0.1"/>
      </geometry>
    </visual>
    
    <collision>
      <geometry>
        <box size="0.1 0.1 0.1"/>
      </geometry>
    </collision>
      
    <xacro:box_inertia m="0.1" w="0.1" d="0.1" h="0.1"/>
  </link>
    
  <joint name="imu_joint" type="fixed">
    <parent link="base_link"/>
    <child link="imu_link"/>
    <origin xyz="0 0 0.01"/>
  </joint>
    

 <link name="laser_link">
    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="0.125"/>
      <inertia ixx="0.001"  ixy="0"  ixz="0" iyy="0.001" iyz="0" izz="0.001" />
    </inertial>

    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
         <cylinder radius="0.0508" length="0.055"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
         <cylinder radius="0.0508" length="0.055"/>
      </geometry>
    </visual>
  </link>
    
  <joint name="laser_joint" type="fixed">
    <parent link="base_link"/>
    <child link="laser_link"/>
    <origin xyz="0 0 0.12" rpy="0 0 0"/>
  </joint>
    
  <link name="camera_link">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.015 0.130 0.022"/>
      </geometry>
    </visual>

    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.015 0.130 0.022"/>
      </geometry>
    </collision>

    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="0.035"/>
      <inertia ixx="0.001"  ixy="0"  ixz="0" iyy="0.001" iyz="0" izz="0.001" />
    </inertial>
  </link>
  
  <joint name="camera_joint" type="fixed">
    <parent link="base_link"/>
    <child link="camera_link"/>
    <origin xyz="0.215 0 0.05" rpy="0 0 0"/>
  </joint>

  <link name="camera_depth_frame"/>

  <joint name="camera_depth_joint" type="fixed">
    <origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}"/>
    <parent link="camera_link"/>
    <child link="camera_depth_frame"/>
  </joint>
</robot>

同样我们需要把urdf文件放到launch文件中启动

default_model_path = os.path.join(pkg_share, 'src/description/sam_bot_description.urdf')

robot_state_publisher_node = launch_ros.actions.Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description': Command(['xacro ', LaunchConfiguration('model')])}]
    )
    joint_state_publisher_node = launch_ros.actions.Node(
        package='joint_state_publisher',
        executable='joint_state_publisher',
        name='joint_state_publisher',
    )
return launch.LaunchDescription([
        launch.actions.DeclareLaunchArgument(name='model', default_value=default_model_path, 
 							description='Absolute path to robot urdf file')，
        joint_state_publisher_node,
        robot_state_publisher_node,
])

`odom -> base_link`

建立了机器人的基本模型之后，我们就需要建立机器人参考坐标base_link到odom之间的关系。关于对odom坐标系的理解，可以查看本博客最后一章节的内容。

odom坐标系的数据来源一般是IMU、激光雷达或者车轮编码器，由于在我自己的项目上，车轮需要更换，因此暂时还没用车轮编码器写好发布odom的功能包。但我在网上找到了可以将激光雷达数据转换为odom的ROS2的包——Laser Scan Matcher for ROS2

Laser Scan Matcher for ROS2

由于我在源里面并没有找到这个包，因此通过从GitHub仓库中clone的功能包直接安装的方式，这个功能包已经支持ros2 humble了

使用这个功能包，需要使用csm功能包——ros2_csm_eigen
然后下载转换激光雷达数据的包 ——Laser Scan Matcher for ROS2

在上面的两个GitHub地址中，直接clone下来，并放入到工作区/src中，使用colcon build编译

git clone https://github.com/AlexKaravaev/ros2_laser_scan_matcher.git
git clone https://github.com/AlexKaravaev/csm.git

colcon build

该功能包需要订阅的话题
/scan (sensor_msgs/LaserScan)
/tf (tf2_msgs/TFMessage)

该功能包发布的话题
/tf (tf2_msgs/TFMessage) 发布odom->base_link转换关系
/odom (nav_msgs/Odometry) 可选项，功能包中有一个参数(Parameter)publish_odom，设置名字即为发布odom的topic，如果该参数为空，则不会发布odom坐标
即代码laser_scan_matcher.cpp的68行：add_parameter(“publish_odom”, rclcpp::ParameterValue(std::string(“odom”))

# 运行
ros2 run ros2_laser_scan_matcher laser_scan_matcher

无论使用什么传感器，最后只要能建立起odom坐标系即可。

`map -> odom`

现在我们已经建立了odom坐标系，下一步就是需要知道机器人在地图中的哪一个位置了，而这一步的实现，就需要使用到一些算法了，nav2中使用的是AMCL算法

参考算法讲解(讲的挺清晰明了的)
ROS 2D导航原理系列（二）|自适应蒙特卡罗定位AMCL

`AMCL`使用

参考博客：
ROS2极简总结-Nav2-地图和自适应蒙特卡洛定位

AMCL需要订阅以下的topic，因此，这些topic一定要存在的

激光扫描：/scan (sensor_msgs/LaserScan) (由激光雷达发布)
TF: 将 odom -> base_link
初始姿势：/initialpose (geometry_msgs/PoseWithCovarianceStamped)
地图：/map（nav_msgs/OccupancyGrid）

算法经过计算之后，就会建立起map和odom之间的关系，这时候，用rviz2就可以看到机器人的经过算法计算之后的效果啦。

`AMCL`参数配置文件`amcl_config.yaml`

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_link"
    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: 8000
    min_particles: 200
    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
    set_initial_pose: true
    initial_pose: 
        x: -0.0119032
        y: -0.00386167
        yaw: -0.0354927

launch文件中的启动节点

nav2_yaml = os.path.join(get_package_share_directory('sam_bot_description'), 'config', 'amcl_config.yaml')

amcl_node = launch_ros.actions.Node(
        package='nav2_amcl',
        executable='amcl',
        name='amcl',
        output='screen',
        parameters=[nav2_yaml]
    )
# 生命循环节点也同样需要启动
lifecycle_node = launch_ros.actions.Node(
        package='nav2_lifecycle_manager',
        executable='lifecycle_manager',
        name='lifecycle_manager_mapper',
        output='screen',
        parameters=[{'use_sim_time': False},
                    {'autostart': True},
                    {'node_names': ['amcl']}]  
    )

效果

激光雷达！你怎么了！醒醒！

过程中的知识点

一、launch文件编写格式及方法

由于

节点之间一般会存在相互依赖关系
一次启动多个节点会比较麻烦

launch文件就诞生了。它允许我们可以同时和配置多个包含ROS2节点的可执行文件，在ROS2中可以使用python来写launch文件

编写ROS2的launch文件

编写launch文件可以有三种方式，python、yaml、xml这三种方式，但是官方推荐的是使用python格式，因为python是一种编程语言，可以使用python的一些库来进行一些工作

一般的命名方式是xxx.launch.py

1.创建launch文件

在工作区下面建立launch目录(与src并列)，并创建xxx.launch.py文件

mkdir launch
cd launch 
touch xxx.launch.py

2.编写launch文件

首先在文件开头import两个模块

from launch import LaunchDescription
from launch_ros.actions import Node

然后编写启动描述。launch文件其实就是需要将所有描述放到启动描述(generate_launch_description)中,这个名字必须是这个。ROS2会对其进行识别

def generate_launch_description():
    Node_1 = Node(
        package='package-name', #节点所在的功能包
        namespace='package-namespace', #命名空间。如果存在同名节点，这一选项会有用.使节点名称前增加命名空间前缀，命名空间不同使系统允许两个相同节点名和主题名不冲突。如果没有唯一的命名空间，当topic消息相同时就无法区分是哪个节点的。
        executable='execute-name/script-name.py', #表示要运行的可执行文件名或脚本名字.py
        parameters=[{'parameter-name': parameter-value}], #参数
        arguments=['-xxx', xxx,  '-xxx', xxx ], #启动参数
        output='screen', #用于将话题信息打印到屏幕
        name='node-name' #表示启动后的节点名，可以没有
        remappings=[ #重映射,将默认节点属性（如节点名称、主题名称、服务名称等），重映射为其它名称。
            ('/xxx/xxx-new', '/xxx/xxx-old'),
        ]
   ),
    Node_2 = Node(
        package="Name_2",
        executable='',
        name=''
   )
   return LaunchDescription([
	Node_1,
    Node_2
   ])

连接路径

使用join

import os
...
#文件
file-name = 'example-file.xxx'
#字符串前加`f`表示可以在字符串里面使用用花括号括起来的变量和表达式，如定义好的`file-name`
file-path = os.path.join(package-path, f'example-folder/{file-name}')
#或者使用逗号隔开
file-path = os.path.join(package-path, 'example-folder', file-name)

#路径
dir-path = os.path.join(package-path, 'example-folder/')

launch文件嵌套

假设已经存在很多的单独的launch文件用于启动不同的功能，如果需要同时启动这些launch文件，可以使用IncludeLaunchDescription在launch文件中嵌套启动launch文件，这样可以提高复用率。

需要添加以下两个头文件

from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

使用IncludeLaunchDescription嵌套launch文件，其中同样可以使用上文所述的传递参数。

将以下代码放入到generate_launch_description函数当中，并在return的时候填入下文件中的another-launch

another-launch = IncludeLaunchDescription(
    PythonLaunchDescriptionSource(
    	os.path.join(launch_file_dir, 'launch-file-name.launch.py')
    ),
    launch_arguments={'arg-name': example-arg}.items()
)

原文链接：http://www.robotsfan.com/posts/7a5950c4.html

3.运行launch文件

3.1使用python启动launch

在setup.py中编写以下内容

from setuptools import setup
from glob import glob
import os

setup(
    name=package_name,
    version='0.0.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/*.launch.py')),
    ],
    },
)

3.2使用C++启动launch

在CmakeList.txt中添加一句

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

4.启动launch文件

ros2 launch file_name file_name.launch.py

二、ROS坐标系

官方描述

原文链接：

ROS坐标系统，常见的坐标系及含义

ROS-REP-105

1.base_link
base_link坐标系和机器人的底盘直接连接。其具体位置和方向都是任意的。对于不同的机器人平台，底盘上会有不同的参考点。不过ROS也给了推荐的坐标系取法。

x 轴指向机器人前方
y 轴指向机器人左方
z 轴指向机器人上方

2.odom
odom是一个固定在环境中的坐标系也就是world-fixed。它的原点和方向不会随着机器人运动而改变。但是odom的位置可以随着机器人的运动漂移。漂移导致odom不是一个很有用的长期的全局坐标。然而机器人的odom坐标必须保证是连续变化的。也就是在odom坐标系下机器人的位置必须是连续变化的，不能有突变和跳跃。
在一般使用中odom坐标系是通过里程计信息计算出来的。比如轮子的编码器或者视觉里程计算法或者陀螺仪和加速度计。odom是一个短期的局域的精确坐标系。但是却是一个比较差的长期大范围坐标。

3.map
map和odom一样是一个固定在环境中的世界坐标系。map的z轴是向上的。机器人在map坐标系下的坐标不应该随着时间漂移。但是map坐标系下的坐标并不需要保证连续性。也就是说在map坐标系下机器人的坐标可以在任何时间发生跳跃变化。
一般来说map坐标系的坐标是通过传感器的信息不断的计算更新而来。比如激光雷达，视觉定位等等。因此能够有效的减少累积误差，但是也导致每次坐标更新可能会产生跳跃。
map坐标系是一个很有用的长期全局坐标系。但是由于坐标会跳跃改变，这是一个比较差的局部坐标系（不适合用于避障和局部操作）。

而在开放环境中，我们需要定义一个全球坐标系

默认的方向要采用 x轴向东，y轴向北，z轴向上
如果没有特殊说明的话z轴为零的地方应该在WGS84椭球上(WGS84椭球是一个全球定位坐标。大致上也就是z代表水平面高度)
如果在开发中这个约定不能完全保证，也要求尽量满足。比如对于没有GPS，指南针等传感器的机器人，仍然可以保证坐标系z轴向上的约定。如果有指南针传感器，这样就能保证x和y轴的初始化方向。

自己理解

参考博客：
ROS中odom、map坐标系的理解

针对map和base_link这两个坐标系其实是好理解的，map就是地图坐标系，与机器人所处的世界坐标重合，而base_link就是机器人的本体坐标系，一般定义为机器人的旋转中心。不太好理解的是这一个odom，即里程计坐标系，这里把自己的理解写一下。

小车需要实现对自己位置的感知，就需要建立起base_link到map这两个坐标系之间的变换，而这个变换，是通过里程计来实现的，比如说：你的里程计说：你向北走了10cm，那么如果你知道你在地图上初始位置，你就知道你在地图上的位置应该到哪了。

里程计(里程计可以来自许多数据源，包括激光雷达、车轮编码器和IMU)可以计算出机器人到底移动了多少的距离，即机器人的实际移动距离。

但是里程计会存在一个问题，也就是漂移问题，无论是IMU还是激光雷达还是车轮编码器，虽然在短暂时间上的位置定位精准的，但是随着时间的增长，这些传感器是会存在累计误差的，比如IMU的漂移问题，车轮的打滑、空转问题等等。因此我们认为的里程计获得的位置，是在odom坐标系中的位置，而不是在map中的位置。也可以理解为map坐标系和base_link坐标系之间是存在一个偏移量的。

对于ROS系统，有提供一些功能包来减少偏移量，官网给出了的一个就是robot_localization，可以将N个传感器融合，尽量解决掉偏移的问题。

最终想要获取小车在map中的位置，就需要amcl之类的方法得到odom坐标系与map坐标系的误差，然后由base_link在odom中的位置，计算出base_link在map中的位置

三、urdf文件

urdf文件的形式标签化、XML树状结构、连杆层次结构
简而言之，就是利用XML树状结构和一些标签，按照连杆层次结构，建立起来了一个属于你自己独有的机器人

语法规则

原文博客链接
ROS机器人建模与仿真（一）——URDF模型的建立和改进
这里将语法摘出，便于以后对照查看
参考博客
官方urdf教程
官方xacro教程

1.1 < robot > 标签
< robot > 是完整机器人模型的最顶层标签，< link > 和 < joint > 标签都必须包含在< robot >标签内。一个完整的机器人模型由一系列的< link > 和 < joint > 组成。即给自己的robot进行命名
< robot >标签语法如下：

<robot name = "<name of the robot>">
	<link> -------</link>
	<link> -------</link>
	<joint>-------</joint>
	<joint>-------</joint>
</robot>

1.2 < link > 标签
< link >标签用于描述机器人某个刚体部分的外观和物理属性，包括尺寸（size）、颜色（color）、形状（shape）、惯性矩阵（inertial matrix）、碰撞参数（collision properties）等。

< link > 标签URDF 描述语法如下：

< visual >用于描述机器人link部分的外观参数
< inertial >标签用于描述link的惯性参数，
< collision>标签用于描述link的碰撞属性。一般来说，检测碰撞的link区域大于外观可视的区域，也就是说有一定的安全空间

<link name = "<link name>"> 
<inertial> ------------</inertial>
	<visual>-------------</visual>
	<collision>--------- </collision>
</link>

1.3 < joint >标签
< joint >标签用于描述机器人关节的运动学和动力学属性，包括关节运动的位置和速度限制。机器人关节的主要作用是连接两个刚体link，这两个link分别称为 parent link 和 child link。

< joint >标签的描述语法如下：

必须指定joint的parent link 和 child link，即该关节连接哪两个刚体
< calibration > : 关节的参考位置，用来校准关节的绝对位置
< dynamics > : 用于描述关节的物理属性，例如阻尼值、物理经摩擦力等，经常在运动学仿真中用到。
< limit > : 用于描述运动的一些极限值，包括关节运动的上下限位置、速度限制、力矩限制等。
< mimic > : 用于描述该关节与已有关节的关系。
< safety_controller > : 用于描述安全控制器的参数。

<joint name="<name of the joint>">
	<parent link = "parent_link" />
	<child  link = "child_link" />
	<calibration ---- />
	<dynamics damping ---- />
	<limit effort ---- />
</joint>