骨架机器人P1——配置Nav2的需求记录

2023/12/5：

目标如下：

1. 帮助新用户设置使用新机器人的 Navigation2
2. 帮助自定义机器人的用户正确设置他们的机器人，以在 ROS/Navigation2 中使用
3. 作为有经验读者的检查清单、模板或参考样例
4. 提供示例，可以在诸如 Gazebo 或 RViz 这样的仿真器/工具上运行，引导读者进行 Nav2 设置过程，即使没有实体机器人也能进行学习（但是这一部分要设置为实体机器人，也就是骨架机器人）。

首先将涉及以下主题（这个只是基础部分，更为详细的要看 Configuration Guide ）：

1. 介绍 TF2 并设置您的机器人 URDF
2. 设置机器人里程计的传感器来源
3. 设置机器人感知的传感器来源
4. 为您的机器人配置圆形或任意形状的足迹
5. 选择并设置用于机器人导航任务的规划器和控制器导航插件
6. 生命周期节点管理，以便轻松启动其他相关传感器或节点

TF2：

        这些变换允许 Nav2 将来自各种来源（如传感器和里程计）的信息进行解释，将其转换为可用的坐标框架。TF2 使用树结构来保证任意两个坐标系之间只有一条单一的遍历路径，假设树中的所有边都是从父节点指向子节点的。

        TF2并不是一个文件，而是一个ROS中的软件包，它提供了一系列的工具、数据结构和功能，帮助在机器人系统中管理坐标系。它可以通过ROS消息系统发布坐标变换信息，并允许系统中的各个组件进行坐标系转换和数据配准。TF2可以通过ROS话题或服务等方式发布坐标变换信息，让系统中的不同部分了解彼此的相对位置和方向。        

        Nav2 需要在 ROS 中发布以下转换关系：

        map => odom：通常由处理定位和地图等工作的不同 ROS 包（如 AMCL）提供

        odom => base_link：通常由我们的里程计系统使用轮子编码器等传感器发布，这通常是通过里程计传感器（IMU、轮子编码器、VIO 等）的传感器融合计算出来的，使用了 robot_localization 包。

        base_link => base_laser（传感器基准坐标系）：Nav2 使用这个变换树来正确关联来自传感器或其他感兴趣的坐标系的信息和机器人的其余部分。这两个坐标系之间的变换通常通过机器人状态发布器（Robot State Publisher）和通用机器人描述文件（URDF）提供给 Nav2。

设置URDF文件base_link => base_laser：

        1、将创建一个简单的差动驱动机器人的统一机器人描述格式（Unified Robot Description Format，URDF）文件。

        2、将设置机器人状态发布器（robot state publisher）

        3、将向机器人URDF文件添加一些运动学属性

        每个传感器都有自己的坐标系，将利用机器人状态发布器（Robot State Publisher）软件包来发布变换。

        机器人状态发布器是ROS 2的一个软件包，与tf2软件包交互以发布可以直接从机器人的几何结构推断出的所有必要变换。向它提供正确的URDF，它将自动处理变换的发布。

        统一机器人描述格式（URDF）是表示机器人模型的XML文件。URDF可以用来定义机器人的物理特性。URDF的另一个重要特性是它还支持Xacro（XML宏），可以帮助你创建更短、更可读的XML，以便定义复杂的机器人。URDF代表统一机器人描述格式（Unified Robot Description Format），是ROS中用于描述机器人模型的XML文件格式。它定义了机器人的几何结构、连接关系和传感器信息等，可用于模拟、可视化和控制机器人。

         举个例子，如果你有一个机器人模型的URDF文件描述了机器人的基本结构，包括车轮、臂和传感器的连接关系，机器人状态发布器可以根据这个URDF自动发布tf2所需的所有变换，如车轮到基座的变换、臂到基座的变换以及传感器到基座的变换等。

设置机器人的测距系统odom => base_link：

        将机器人的测距系统（odometry system）与 Nav2 集成

odom => base_link 变换

        robot_localization会发布 odom => base_link 变换

        里程计系统根据机器人的运动提供了一个局部准确的姿态和速度估计。

        odom坐标系（局部坐标系）及其相关的转换使用机器人的里程计系统发布定位信息，这些信息连续性强，但随着时间或距离的增加会变得不太准确（这取决于传感器的模式和漂移情况）。尽管如此，机器人仍然可以利用这些信息来导航其周围环境（例如避免碰撞）。为了始终获得准确的里程计信息，map坐标系（全局坐标系）提供了全局准确的信息，用于校正odom坐标系。

        odom坐标系通过 odom => base_link 转换连接到系统的其余部分和Nav2。这个转换由tf2发布器或者类似robot_localization的框架发布，后者还提供了额外的功能。

nav_msgs/Odometry 消息

        除了所需的 odom => base_link 转换，Nav2还要求发布 nav_msgs/Odometry 消息，因为这个消息提供了机器人的速度信息。

# 这表示空间中位置和速度的估计。
# 此消息中的姿势应在由 header.frame_id 给出的坐标系中指定。
# 此消息中的扭转应在由 child_frame_id 给出的坐标系中指定。
 
# 包括姿势父级的框架 ID。
std_msgs/Header header
 
# 姿势指向的框架 ID。扭转在此坐标系中。
string child_frame_id
 
# 估计的姿势，通常相对于固定的世界坐标系。
geometry_msgs/PoseWithCovariance pose
 
# 相对于 child_frame_id 的估计线性和角速度。
geometry_msgs/TwistWithCovariance twist

        这段代码描述了一个 ROS 消息格式，用于表示在自由空间中的位置和速度估计。消息包含了估计的姿势和扭转，其中姿势通常是相对于固定的世界坐标系，而扭转则是相对于指定的子坐标系。消息中包含了头部信息（header）以及姿势和扭转信息。

        设置物理机器人的导航系统的里程计取决于机器人配备的哪些里程计传感器。设置里程计的主要目的是通过传感器获取里程计信息，并将其转换到机器人的基本坐标系（例如，base_link）中。在这个过程中，会进行坐标系之间的变换，以便正确地将局部坐标系中的位置和姿态信息映射到机器人的基本坐标系中，并通过 ROS 2 发布 nav_msgs/Odometry 消息和 odom => base_link 变换。

（

        使用ros2_control中的diff_drive_controller是一种更简单且标准的方法来处理轮式编码器所提供的信息，并将其转换为里程计信息。这个控制器能够接收来自cmd_vel话题的运动控制指令，并据此计算里程计数据，然后以nav_msgs/Odometry消息的形式发布到odom话题上。

        这种做法使得底层的里程计数据处理更加标准化和易于管理。通过使用ros2_control中的这种控制器，你可以避免手动编写里程计算算法，而是依赖于现有的、被广泛验证的控制器，从而更方便地集成到Nav2导航系统中。

）

        Nav2 需要发布 nav_msgs/Odometry 消息和 odom => base_link 变换，这应该是设置里程计系统时的目标。

向URDF文件添加IMU：

在我们的URDF文件中在</robot>之前添加以下行：（在现实的机器人里面，可能不需要<gazebo>的部分，以下的代码仅供参考）

<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>
 
 <gazebo reference="imu_link">
  <sensor name="imu_sensor" type="imu">
   <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
      <ros>
        <namespace>/demo</namespace>
        <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>
</gazebo>

        类似的在gazebo模拟中：/demo/imu主题发布的是sensor_msgs/Imu类型的消息，而/demo/odom主题发布的是nav_msgs/Odometry类型的消息。这些主题发布的信息来自于 gazebo 模拟的 IMU 传感器和差分驱动器。

总结：

1. TF2 设置：

   • 确保正确设置了各个坐标系之间的变换关系，比如 map => odom、odom => base_link 和 base_link => base_laser。这些变换关系通过 ROS 的消息系统发布，通常需要依赖不同的 ROS 包如 AMCL、robot_localization 等。

2. URDF 文件设置：

   • 创建并设置统一机器人描述格式（URDF）文件，描述机器人的结构、连接关系和传感器信息。
   • 使用机器人状态发布器（Robot State Publisher）自动发布 URDF 中定义的各种变换，如传感器到基座的变换等。

3. 机器人的测距系统 odom => base_link 设置：

   • 整合机器人的测距系统（odometry system）与 Nav2，确保发布 odom => base_link 变换和 nav_msgs/Odometry 消息，提供机器人的速度信息。

4. 添加 IMU 到 URDF 文件：

   • 在 URDF 文件中添加 IMU 相关的信息，确保在实际机器人上将 IMU 数据发布到对应的话题，比如 sensor_msgs/Imu 类型的消息。

5. ROS 控制器（可选）：

   • 使用 ros2_control 中的 diff_drive_controller 简化轮式编码器提供的信息转换为里程计信息的处理。这个控制器能够接收运动控制指令，并计算里程计数据发布到相应的话题上，如 nav_msgs/Odometry。

2023/12/6：

        于12/5日做出的机器人，可以实际上在gazebo上面跑一跑。

robot_localization融合里程计信息

        目前：来自IMU传感器和差分驱动器（小车内部自带利用速度和转向来计算里程计）发布的主题没有订阅者

Type: sensor_msgs/msg/Imu
Publisher count: 1
Subscription count: 0
 
Type: nav_msgs/msg/Odometry
Publisher count: 1
Subscription count: 0

        所以说将创建一个 robot_localization 节点，该节点将订阅这两个主题。然后，它将使用这两个主题发布的消息为 Nav2 提供融合的、本地精确和平滑的里程计信息。robot_localization包用于从N个里程计传感器输入的数据中提供融合的、本地精确且平滑的里程计信息。

        当向robot_localization提供多个来源的数据时，它能够通过状态估计节点融合传感器提供的里程计信息。这些节点利用扩展卡尔曼滤波器（ekf_node）或无迹卡尔曼滤波器（ukf_node）来实现这种融合。此外，该包还实现了一个navsat_transform_node，在使用GPS时将地理坐标转换为机器人的世界坐标系。navsat_transform_node是robot_localization包中的一个节点，用于处理GPS数据并将其转换为机器人世界坐标系中的地理坐标。

        如果在其配置中启用了，robot_localization包通过odometry/filtered和accel/filtered主题发布融合的传感器数据。此外，它还可以在/tf主题上发布odom => base_link的变换。

        如果机器人只能提供一个里程计来源，使用robot_localization除了平滑外，影响可能不大。

        例如：它将使用在/demo/Imu上（IMU发布的里程计）发布的sensor_msgs/Imu消息和在/demo/odom（机器人本身发布的里程计）上发布的nav_msgs/Odometry消息，然后在odometry/filtered、accel/filtered和/tf主题上发布数据。

        现在配置robot_localization包，使用扩展卡尔曼滤波器（ekf_node）来融合里程信息并发布odom => base_link的变换。

        使用一个YAML文件来指定ekf_node的参数。在您项目的根目录下创建一个名为config的目录，并创建一个名为ekf.yaml的文件。将以下代码复制到ekf.yaml文件中。

# ekf_filter_node 节点的参数设置
ekf_filter_node:
    ros__parameters:
        # 滤波器输出位置估计的频率（单位：赫兹）。请注意，滤波器在接收到来自输入数据的至少一条消息之前不会开始计算。
        # 之后，它将以此处指定的频率连续运行，无论是否收到更多的测量。如果未指定，默认为 30。
        frequency: 30.0

        # ekf_localization_node 和 ukf_localization_node 都使用 3D 全向运动模型。
        # 如果此参数设置为 true，则您的状态估计中将不使用 3D 信息。
        # 如果您在平面环境中操作并希望忽略地面平面的微小变化（例如，IMU 可能检测到的效果），请使用此参数。
        # 如果未指定，默认为 false。
        two_d_mode: false

        # 是否发布加速度状态。如果未指定，默认为 false。
        publish_acceleration: true

        # 是否通过 /tf 话题广播转换。如果未指定，默认为 true。
        publish_tf: true

        # 设置地图帧、里程计帧和基准坐标帧以适应您系统的适当帧名称。
        # 如果您的系统没有地图帧，请删除它，并确保 "world_frame" 设置为 odom_frame 的值。
        # ...
        map_frame: map              # 默认为 "map"（如果未指定）
        odom_frame: odom            # 默认为 "odom"（如果未指定）
        base_link_frame: base_link  # 默认为 "base_link"（如果未指定）
        world_frame: odom           # 默认为 odom_frame 的值（如果未指定）

        # 使用的 odom0 话题及其配置，用于指定要融合的数据维度。
        odom0: demo/odom
        odom0_config: [true, true, true,
                       false, false, false,
                       false, false, false,
                       false, false, true,
                       false, false, false]

        # 使用的 imu0 话题及其配置，用于指定要融合的数据维度。
        imu0: demo/imu
        imu0_config: [false, false, false,
                      true, true, true,
                      false, false, false,
                      false, false, false,
                      false, false, false]

        这个配置里面，会定义频率、二维模式、是否发布加速度、是否发布tf、地图坐标系、里程计坐标系、基础坐标系和世界坐标系的参数值。 here 可以看到一个示例。

        上述使用的是odom0和imu0，将odom0的值设置为demo/odom，这是发布nav_msgs/Odometry的主题。将imu0的值设置为发布sensor_msgs/Imu的主题，即demo/imu。

        使用_config参数指定要由过滤器使用的传感器值。此参数的值顺序为x、y、z、roll、pitch、yaw、vx、vy、vz、vroll、vpitch、vyaw、ax、ay、az。在示例中，odom0_config中的所有值都设置为false，除了第1、2、3和12个条目，这意味着过滤器只会使用odom0的x、y、z和vyaw值。（也就是_config参数可以指定要使用相应传感器的什么信息，odom0 使用 x、y、z 和 vyaw，imu0 仅使用 roll、pitch 和 yaw。）

将 ekf_node 添加到launch文件中

        把 ekf.yaml ，ekf_node 文件添加到launch文件中。打开 launch/display.launch.py 文件，在 `return launch.LaunchDescription([` 行之前粘贴以下行。

robot_localization_node = launch_ros.actions.Node(
       package='robot_localization',
       executable='ekf_node',
       name='ekf_filter_node',
       output='screen',
       parameters=[os.path.join(pkg_share, 'config/ekf.yaml'), {'use_sim_time': LaunchConfiguration('use_sim_time')}]
)

        接下来，在 `return launch.LaunchDescription([` 块内添加以下启动参数。

launch.actions.DeclareLaunchArgument(name='use_sim_time', default_value='True',
                                            description='Flag to enable use_sim_time'),

        最后，在 `rviz_node` 行上方添加 `robot_localization_node`，以启动机器人定位节点。

      robot_state_publisher_node,
      spawn_entity,
      robot_localization_node,
      rviz_node
])

        将 `robot_localization` 依赖项添加到包定义中。打开 `package.xml`，并在最后一个 `<exec_depend>` 标签下面添加以下行。

<exec_depend>robot_localization</exec_depend>

        最后，打开 CMakeLists.txt，并在 install(DIRECTORY...) 中追加 config 目录，如下面的片段所示。

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

验证是否配置成功：

        验证一下系统中是否存在odometry/filtered、accel/filtered和/tf这些话题。打开一个新的终端并执行：

ros2 topic list

        通过执行以下命令再次检查这些话题的订阅者计数，应该看到/demo/imu和/demo/odom现在各自都有1个订阅者。

ros2 topic info /demo/imu
ros2 topic info /demo/odom

        要验证ekf_filter_node（ekf_filter_node是一个用于消息融合的节点，它订阅了多个传感器发布的消息，然后使用扩展卡尔曼滤波器（Extended Kalman Filter，EKF）对这些消息进行融合。通过接收来自不同传感器的数据，它尝试将这些数据整合到一个更准确和连贯的状态估计中）是否是这些话题的订阅者，请执行：

ros2 node info /ekf_filter_node

        会看到以下类似的输出：

/ekf_filter_node:  # EKF节点的名称

  Subscribers:  # 订阅器
    /demo/imu: sensor_msgs/msg/Imu  # 订阅 IMU 数据
    /demo/odom: nav_msgs/msg/Odometry  # 订阅里程计数据
    /parameter_events: rcl_interfaces/msg/ParameterEvent  # 订阅参数事件
    /set_pose: geometry_msgs/msg/PoseWithCovarianceStamped  # 接收位置设置命令

  Publishers:  # 发布器
    /accel/filtered: geometry_msgs/msg/AccelWithCovarianceStamped  # 发布滤波后的加速度信息
    /diagnostics: diagnostic_msgs/msg/DiagnosticArray  # 发布诊断信息
    /odometry/filtered: nav_msgs/msg/Odometry  # 发布滤波后的里程计信息
    /parameter_events: rcl_interfaces/msg/ParameterEvent  # 发布参数事件
    /rosout: rcl_interfaces/msg/Log  # ROS 输出
    /tf: tf2_msgs/msg/TFMessage  # 发布变换信息

  Service Servers:  # 服务服务器
    ...  # 在这里描述每个服务服务器提供的服务

        可以看到ekf_filter_node已订阅了/demo/imu和/demo/odom。还可以看到ekf_filter_node在odometry/filtered、accel/filtered和/tf主题上发布消息。

        可以使用tf2_echo实用程序验证robot_localization是否发布了odom => base_link的变换。在单独的命令行终端中运行以下命令：

ros2 run tf2_ros tf2_echo odom base_link

        会看到类似以下内容的连续输出。

At time 8.842000000
- Translation: [0.003, -0.000, 0.127]
- Rotation: in Quaternion [-0.000, 0.092, 0.003, 0.996]
At time 9.842000000
- Translation: [0.002, -0.000, 0.127]
- Rotation: in Quaternion [-0.000, 0.092, 0.003, 0.996]

sensor_msgs包：基本传感器设置

        传感器的读数可以用于地图绘制、定位和感知任务。移动机器人配备了多种传感器，使它们能够观察和感知周围环境。ROS提供了sensor_msgs包，定义了通用的传感器接口。在接下来将介绍一些导航中常用的消息，即sensor_msgs/LaserScan、sensor_msgs/PointCloud2、sensor_msgs/Range和sensor_msgs/Image。

1. sensor_msgs/LaserScan：

   • 用于表示激光雷达扫描数据。
   • 包含激光雷达传感器扫描的角度、距离和强度信息。
   • 通常用于表示激光雷达沿一条直线扫描的结果，提供了障碍物的距离信息。
   • 这条消息代表来自平面激光雷达的单次扫描。它在slam_toolbox和nav2_amcl中用于定位和建图，或在nav2_costmap_2d中用于感知。

2. sensor_msgs/PointCloud2：

   • 用于表示点云数据。这可能来自三维激光雷达、二维激光雷达、深度摄像头或其他设备。
   • 它是一个三维点集合，每个点包含了位置（x、y、z坐标）、颜色、法线等信息。
   • 通过这些点的集合，可以表示环境中物体的形状、位置和表面信息。

3. sensor_msgs/Range：

   • 用于表示一维测距传感器（如超声波传感器或红外线传感器）的数据。它发射能量并报告在测量距离处沿弧线有效的一个范围读数。
   • 提供了物体与传感器之间的距离信息，通常用于测量某个方向上的距离。

4. sensor_msgs/Image：

   • 用于表示图像数据。  这代表来自RGB或深度相机的传感器读数，对应于RGB或深度数值。
   • 可以包含彩色或灰度图像，以及图像的元数据，如分辨率、编码方式等。
   • 通常用于从摄像头或其他视觉传感器中获取图像数据。

        除了sensor_msgs包，还有radar_msgs和vision_msgs的标准接口也值得关注。radar_msgs定义了用于雷达特定传感器的消息，而vision_msgs包定义了计算机视觉中使用的消息，例如目标检测、分割和其他机器学习模型。此包支持的消息包括vision_msgs/Classification2D、vision_msgs/Classification3D、vision_msgs/Detection2D和vision_msgs/Detection3D等。

 要了解更多信息，请参阅sensor_msgs、radar_msgs和 vision_msgs的API文档。

        骨架机器人的传感器可能已经有为其编写的ROS驱动程序（例如，连接到传感器、将数据填充到消息中并发布这些消息供机器人使用的ROS节点），这些驱动程序遵循sensor_msgs包中的标准接口。sensor_msgs包使得你可以轻松地使用来自不同制造商的许多不同传感器。通用软件包如Nav2可以读取这些标准化的消息并执行与传感器硬件无关的任务。（也就是说骨架机器人上面用到的传感器，都会遵循sensor_msgs包中的标准接口发布主题信息。例如，Nav2（导航系统）不需要了解特定传感器的详细规范或通信协议，它只需要订阅并接收标准化的sensor_msgs消息，无论这些消息来自激光雷达、相机、距离传感器还是其他设备。这样，Nav2能够处理这些消息，执行导航任务，并对机器人采取适当的行动，而不需要关注传感器硬件的细节。这种与传感器硬件无关的设计使得机器人系统更具通用性和可扩展性。）

sensor_msgs/LaserScan

 sensor_msgs/PointCloud2

sensor_msgs/Range

sensor_msgs/Image

总结：

        robot_localization可以融合传感器信息，平滑路径。

        使用一个YAML文件来指定ekf_node的参数（robot_localization包使用扩展卡尔曼滤波器来融合里程信息并发布odom => base_link的变换）

        YAML文件需要包括一系列参数，这些参数将被传递给 ekf_filter_node 节点。在这个例子中，ekf_filter_node 节点的参数如下：

• frequency: 滤波器输出位置估计的频率。
• two_d_mode: 是否启用二维模式，用于在平面环境中忽略三维信息。
• publish_acceleration: 是否发布加速度状态。
• publish_tf: 是否在 /tf 话题上广播变换信息。
• map_frame, odom_frame, base_link_frame, world_frame: 不同坐标系的名称。
• odom0: 使用的里程计话题。
• odom0_config: 里程计的配置，用于指定哪些维度的数据应该被融合。
• imu0: 使用的 IMU 话题。
• imu0_config: IMU 的配置，指定哪些维度的数据应该被融合。

        这些参数和它们的值是根据系统的需求和传感器的特性进行设置的。因此，一个YAML文件用来指定 ekf_filter_node 节点的参数，提供了对滤波器行为和使用的传感器进行配置的方法。

        最后完成了ekf.yaml文件的配置（配置需要什么里程计，imu，差分驱动器等等，以及这些传感器都需要什么里程计信息）之后， 

2023/12/7：

在骨架机器人上添加基本的传感器设置

        模仿在模拟机器人上设置传感器，大多数这些步骤仍然需要为设置 URDF 框架。

添加激光雷达传感器：

        打开 URDF 文件，路径为 src/description/*****.urdf，然后在 </robot> 标签之前粘贴以下行。

<link name="lidar_link">  # 定义名为 lidar_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="lidar_joint" type="fixed">  # 定义名为 lidar_joint 的固定类型关节
  <parent link="base_link"/>  # 父链接为 base_link
  <child link="lidar_link"/>  # 子链接为 lidar_link
  <origin xyz="0 0 0.12" rpy="0 0 0"/>  # 关节的起始点坐标和旋转角度
</joint>
 
<gazebo reference="lidar_link">  # 在Gazebo中参考 lidar_link
  <sensor name="lidar" type="ray">  # 定义名为 lidar 的射线类型传感器
    <always_on>true</always_on>  # 传感器始终开启
    <visualize>true</visualize>  # 可视化传感器信息
    <update_rate>5</update_rate>  # 更新频率为 5Hz
    <ray>
      <scan>  # 定义激光扫描参数
        <horizontal>
          <samples>360</samples>  # 360个激光样本
          <resolution>1.000000</resolution>  # 分辨率为 1.0
          <min_angle>0.000000</min_angle>  # 最小角度
          <max_angle>6.280000</max_angle>  # 最大角度
        </horizontal>
      </scan>
      <range>  # 定义范围参数
        <min>0.120000</min>  # 最小范围
        <max>3.5</max>  # 最大范围
        <resolution>0.015000</resolution>  # 范围分辨率
      </range>
      <noise>  # 定义传感器噪声
        <type>gaussian</type>  # 噪声类型
        <mean>0.0</mean>  # 均值
        <stddev>0.01</stddev>  # 标准差
      </noise>
    </ray>
    <plugin name="scan" filename="libgazebo_ros_ray_sensor.so">  # 定义 Gazebo 插件
      <ros>
        <remapping>~/out:=scan</remapping>  # 重新映射输出消息
      </ros>
      <output_type>sensor_msgs/LaserScan</output_type>  # 输出消息类型为激光扫描
      <frame_name>lidar_link</frame_name>  # 坐标系名称为 lidar_link
    </plugin>
  </sensor>
</gazebo>

        这里面的<gazebo></gazebo>是用于使用 Gazebo 的插件，以备后用。

        "gazebo_ros_ray_sensor" 是 Gazebo 仿真环境中的一个 ROS 插件，用于模拟光线传感器，例如激光雷达。该插件允许在 Gazebo 中模拟激光雷达传感器的行为，并将其与 ROS 中的话题进行连接，以便发布激光雷达传感器扫描数据。通过这个插件，可以在仿真环境中模拟出激光雷达的功能，以便进行仿真测试和开发

添加深度摄像头：

        在激光雷达传感器的 </gazebo> 标签之后，粘贴以下行。

<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"/>  # 父链接为 base_link
  <child link="camera_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"/>  # 父链接为 camera_link
  <child link="camera_depth_frame"/>  # 子链接为 camera_depth_frame
</joint>
 
<gazebo reference="camera_link">  # 在 Gazebo 中参考 camera_link
  <sensor name="depth_camera" type="depth">  # 深度相机传感器类型为深度
    <visualize>true</visualize>  # 可视化传感器信息
    <update_rate>30.0</update_rate>  # 更新频率为 30.0 Hz
    <camera name="camera">  # 相机参数
      <horizontal_fov>1.047198</horizontal_fov>  # 水平视场角
      <image>
        <width>640</width>  # 图像宽度
        <height>480</height>  # 图像高度
        <format>R8G8B8</format>  # 图像格式
      </image>
      <clip>  # 裁剪范围
        <near>0.05</near>  # 近处距离
        <far>3</far>  # 远处距离
      </clip>
    </camera>
    <plugin name="depth_camera_controller" filename="libgazebo_ros_camera.so">  # 深度相机控制插件
      <baseline>0.2</baseline>  # 基线
      <alwaysOn>true</alwaysOn>  # 总是开启
      <updateRate>0.0</updateRate>  # 更新频率为 0.0
      <frame_name>camera_depth_frame</frame_name>  # 坐标系名称为 camera_depth_frame
      <pointCloudCutoff>0.5</pointCloudCutoff>  # 点云截断
      <pointCloudCutoffMax>3.0</pointCloudCutoffMax>  # 点云最大截断
      <distortionK1>0</distortionK1>  # 畸变参数
      <distortionK2>0</distortionK2>
      <distortionK3>0</distortionK3>
      <distortionT1>0</distortionT1>
      <distortionT2>0</distortionT2>
      <CxPrime>0</CxPrime>
      <Cx>0</Cx>
      <Cy>0</Cy>
      <focalLength>0</focalLength>
      <hackBaseline>0</hackBaseline>
    </plugin>
  </sensor>
</gazebo>

        与激光雷达传感器类似，创建了一个名为 camera_link 的链接，该链接将被 gazebo_ros_camera 插件引用作为传感器的连接位置。

        还创建了一个名为 camera_depth_frame 的链接，它连接到 camera_link 上，并将设置为深度相机插件的 <frame_name>。

        还配置了插件，使其会将 sensor_msgs/Image 和 sensor_msgs/PointCloud2 消息发布到 /depth_camera/image_raw 和 /depth_camera/points 话题上。

        最后，还设置了深度相机的其他基本配置属性。

slam_toolbox定位包：

        现在有一个机器人及其传感器设置，可以使用获得的传感器信息构建环境地图，并对地图中的机器人进行定位。

        slam_toolbox包是ROS2中用于在潜在的大型地图中进行二维同时定位与建图（SLAM）的一组工具和功能。它还是Nav2中官方支持的SLAM库之一，在需要在机器人设置上使用SLAM的情况下建议使用该软件包。

        定位也可以通过nav2_amcl软件包来实现。

        slam_toolbox和nav2_amcl都使用激光扫描传感器的信息来感知机器人的环境。因此，为了验证它们能够访问激光扫描传感器的读数，我们必须确保它们已经订阅了发布sensor_msgs/LaserScan消息的正确主题。

nav2_params.yaml配置机器人的足迹：

        为 Nav2 使用的导航算法配置机器人的足迹。足迹是机器人在地面投影时的二维形状，主要由 Nav2 在规划过程中用于避免碰撞。

        足迹是通过全局和局部代价地图中的 footprint 或 robot_radius 参数进行设置

        在 footprint 参数中定义的值是一个有序的二维点向量，用于定义机器人在 base_link 坐标系中的足迹。向量中的第一个和最后一个点将连接为最后一条线段，以封闭足迹的形状。作为替代方案，您还可以使用 robot_radius 参数，在此情况下将自动生成以 base_link 为中心的圆形足迹。如果在配置中同时定义了 footprint 和 robot_radius 参数，则使用 footprint。（footprint是多边形的足迹，robot_radius是圆形足迹）

        当机器人相对于其环境非常小，以至于不需要精确的碰撞避免时，使用局部代价地图的 robot_radius（圆形）。但一般来说，局部轨迹规划器应该使用机器人的实际足迹多边形。（对于骨架机器人来说，应用场景我觉得应该使用圆形足迹就可以）

         在config目录下，创建一个名为nav2_params.yaml的新文件。然后，复制config/nav2_params.yaml的内容并粘贴到新创建的文件中。config/nav2_params.yaml的内容是从Nav2的默认配置文件中复制的，但是对footprint和robot_radius参数进行了更改，以匹配sam_bot的形状。

        以下是来自nav2_params.yaml的代码片段，定义了本地costmap的足迹。在这个配置文件中，本地costmap的footprint参数已经设置为一个矩形足迹。这个矩形框以base_link框架为中心。

resolution: 0.05
footprint: "[ [0.21, 0.195], [0.21, -0.195], [-0.21, -0.195], [-0.21, 0.195] ]"
plugins: ["voxel_layer", "inflation_layer"]

        对于全局costmap，设置了robot_radius参数，以创建一个圆形足迹，与骨架机器人的尺寸相匹配，并以base_link为中心。下面是已修改的参数代码片段。

use_sim_time: True
robot_radius: 0.3
resolution: 0.05

算法插件：

        导航算法通过在 ROS 动作服务器上运行的插件来实现，在 Nav2 中实现。这些服务器包括规划器、控制器和行为服务器（等等）。

        这些服务器可以实现一个或多个算法插件，每个插件都有自己的配置，专门为特定的操作或机器人状态进行了定制。

        规划器和控制器服务器托管一个或多个插件的地图，其中特定的插件将用于特定的环境、场景或任务。例如，控制器服务器可以有一个用于在长廊中沿中间行走的路径的插件，然后在拥挤的地方避免动态障碍物的另一个插件。根据机器人的任务选择执行哪种规划算法可以通过导航系统或应用程序服务器的行为树来完成。（控制器和规划器可以托管多个插件，每个插件可以用于不同的环境、场景或任务。当机器人遇到特定情景或任务时，导航系统会依据预设的策略或条件来选择适当的规划器或控制器插件。）

总结：

        添加新的传感器，一般要设置 URDF 框架。这个 URDF 框架里面包括了传感器的可视化（visual，包括传感器几何形状，碰撞（collision），惯性（inertial，包括原点和旋转角度、质量、惯性矩阵）），相机关节（连接base_link和传感器link的关节）。还可以为后续的工作，设置gazebo的参数。

        footprint是多边形的足迹，robot_radius是圆形足迹