无人机避障——2D栅格地图pgm格式文件路径规划代码详解

代码和测试效果请看上一篇博客：

无人机避障——使用三维PCD点云生成的2D栅格地图PGM做路径规划-CSDN博客

 更换模型文件.dae：

部分模型文件可以从这里下载：

https://github.com/ethz-asl/rotors_simulator/wiki

将原先代码中的car.dae文件更换为无人机.dae文件

然后对urdf文件进行修改：

<?xml version="1.0"?>
<robot name="car">
  <link name="ground_link">
    <visual name="base_visual">
      <origin xyz="0 0 10" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://hybrid_a_star/model/car_and_UAV_model/firefly.dae" scale="8 8 8"/>
      </geometry>
    </visual>
  </link>
</robot>

测试结果： 

代码分析： 

run_hybrid_a_star.launch文件：

<launch>
    <!-- 定义一个名为fixed_frame的参数，其默认值为"world"，这个参数可能在后续节点启动或配置中会用到，
         通常用于指定某个固定的参考坐标系 -->
    <arg name="fixed_frame" default="world"/>

    <!-- 设置一个名为robot_description的参数，其值通过读取指定的URDF文件来获取。这里使用$(find hybrid_a_star)
         来查找名为hybrid_a_star的功能包路径，然后拼接上/model/car_and_UAV_model/default.urdf来确定具体要读取的URDF文件，
         该文件可能描述了机器人（在这里可能是车辆模型）的结构等信息 -->
    <param name="robot_description" textfile="$(find hybrid_a_star)/model/car_and_UAV_model/default.urdf"/>

    <!-- 以下这部分是被注释掉的两个节点启动设置。这里原本可能是打算启动joint_state_publisher和robot_state_publisher节点，
         但当前未被使用（可能是在某些特定场景或调试阶段才需要启动它们）。
         joint_state_publisher节点通常用于发布机器人关节状态信息，
         robot_state_publisher节点一般用于根据机器人的关节状态等信息来发布机器人的状态信息到tf树中 -->
    <!--这两个节点没有使用
    <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher"/>
    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher"/>
    -->

    <!-- 启动rviz节点，rviz是ROS中用于可视化的工具。这里通过指定args参数来加载特定的配置文件，
         同样使用$(find hybrid_a_star)来查找hybrid_a_star功能包路径，然后拼接上/rviz/display_hybrid_astar.rviz
         确定要加载的rviz配置文件，该配置文件会定义在rviz中如何显示相关的信息，比如机器人模型、地图等 -->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find hybrid_a_star)/rviz/display_hybrid_astar.rviz"/>

    <!-- 这是一个对下面即将启动的static_transform_publisher节点的注释说明。
         static_transform_publisher节点用于发布一个静态的坐标变换，
         这里给出了其参数的含义：x y z yaw pitch roll frame_id child_frame_id period_in_ms，
         分别表示在x、y、z方向上的平移量，绕yaw、pitch、roll轴的旋转角度，
         以及父坐标系frame_id、子坐标系child_frame_id和发布变换的周期（单位为毫秒） -->
    <!--static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period_in_ms-->
    <node pkg="tf" type="static_transform_publisher" name="link1_broadcaster"
          args="0 0 0 0 0 0 world map 100"/>

    <!-- 启动map_server节点，map_server用于加载和发布地图信息。
         这里通过args参数指定要加载的地图配置文件，使用$(find hybrid_a_star)找到hybrid_a_star功能包路径，
         再拼接上/maps/map.yaml确定具体要加载的地图配置文件，该文件中应该包含了地图的相关参数，比如地图分辨率、尺寸等 -->
    <node pkg="map_server" type="map_server" name="map_server" args="$(find hybrid_a_star)/maps/map.yaml"/>

    <!-- 启动名为run_hybrid_astar的节点，该节点属于hybrid_a_star功能包，执行的可执行文件类型为run_hybrid_astar。
         并且设置了一些参数，这些参数可能用于配置路径规划相关的策略或属性，以下是对每个参数的具体说明： -->
    <node pkg="hybrid_a_star" type="run_hybrid_astar" name="run_hybrid_astar" output="screen">
        <!-- 设置规划器的转向角度参数，值为15.0度，可能用于控制车辆在路径规划时的最大转向角度 -->
        <param name="planner/steering_angle" value="15.0"/>
        <!-- 设置转向角度离散化数量参数，值为1，可能与转向角度的离散化处理方式有关，比如将连续的转向角度划分成几个离散的区间 -->
        <param name="planner/steering_angle_discrete_num" value="1"/>
        <!-- 设置车辆的轴距参数，值为2.0，轴距是车辆前后轴之间的距离，在路径规划中可能会影响车辆转弯等操作的计算 -->
        <param name="planner/wheel_base" value="2.0"/>
        <!-- 设置路径规划中线段长度参数，值为1.6，可能用于定义在规划路径时每一段线段的长度 -->
        <param name="planner/segment_length" value="1.6"/>
        <!-- 设置线段长度离散化数量参数，值为8，与线段长度的离散化处理相关，比如将路径划分成若干个离散长度的线段 -->
        <param name="planner/segment_length_discrete_num" value="8"/>
        <!-- 设置转向惩罚参数，值为1.5，可能在路径规划算法中，当车辆进行转向操作时会根据这个参数来增加一定的成本，
             以影响最终路径的选择，使得路径更倾向于减少不必要的转向 -->
        <param name="planner/steering_penalty" value="1.5"/>
        <!-- 设置倒车惩罚参数，值为3.0，在路径规划中如果车辆需要倒车，可能会根据这个参数增加较高的成本，
             促使规划出的路径尽量减少倒车操作 -->
        <param name="planner/reversing_penalty" value="3.0"/>
        <!-- 设置转向变化惩罚参数，值为2.0，当车辆在路径上的转向角度发生变化时，可能会根据这个参数增加一定的成本，
             用于优化路径，使得转向变化更加平滑或者符合某种策略 -->
        <param name="planner/steering_change_pparam" name="planner/steering_change_penalty" value="2.0"/>
        
        <!-- 设置短距离参数，值为5.0，可能与某种短距离相关的操作或判断有关，具体含义可能需要结合具体的路径规划算法来确定 -->
        
        <param name="planner/shot_distance" value="5.0"/>
    </node>

</launch>

主要是map_server和hybrid_a_star节点对读取地图做路径规划起作用！！ 

<node pkg="map_server" type="map_server" name="map_server" args="$(find hybrid_a_star)/maps/map.yaml"/>

map_server节点会将加载并处理好的地图数据发布到 ROS 的话题（topic）上，通常发布的话题名称为/map。其他的 ROS 节点（比如路径规划节点、导航节点等）可以通过订阅这个话题来获取地图信息，从而在其自身的功能实现中利用这些地图数据。例如，路径规划节点在规划机器人从起点到终点的最优路径时，就需要知道地图的布局，包括哪里有障碍物、哪里是可通行区域等，通过订阅/map话题就可以从map_server节点获取到这些必要的地图信息。 

map_server处理过程

• PGM 文件开头有特定的文件头格式，包含了关于这幅图像的一些基本信息，如文件格式标识（确认是 PGM 格式）、图像的宽度和高度（即地图的尺寸）、灰度值的最大范围等。map_server首先会读取这些文件头信息，以便后续正确解析图像数据。例如，知道了图像的宽度和高度，就可以确定要读取多少个像素的数据来完整获取地图信息。
• 根据文件头获取到的图像尺寸信息，map_server会逐行逐列地读取 PGM 文件中的每个像素的灰度值。在读取过程中，依据事先约定好的灰度值与地图属性的对应关系（如上述白色表示可通行、黑色表示障碍物）来判断每个像素位置对应的地图区域是可通行还是存在障碍物。
• 比如，如果读取到一个像素的灰度值接近 255（假设是 8 位灰度图像，最大灰度值为 255），那么根据约定，这个像素所对应的地图位置就会被判定为可通行区域；如果读取到的灰度值接近 0，就会被判定为障碍物所在位置。
• 在解析完所有像素数据后，map_server会将判断好的地图通行和障碍信息以合适的方式存储在内存中。通常会采用二维数组等数据结构来存储，其中数组的每个元素对应着地图上的一个位置，元素的值则表示该位置是可通行、障碍物还是其他状态（如未知区域等）。例如，可能会用 0 表示障碍物，1 表示可通行区域等，这样其他需要使用地图的节点（如路径规划节点）就可以方便地从这个存储结构中获取到所需的地图信息。