一、目标

搭载Robosense 16线激光雷达和6轴IMU传感器的履带机器人，通过使用ros的navigation自主导航框架完成点到点的自主导航。

二、总体方案设计

2.1 功能架构设计

Navigation导航框架如下图所示，其主要完成了全局路径规划器、局部路径规划器、全局代价地图、局部代价地图和行为决策（recovery_behaviors）的实现。

由上图可知，为实现自主导航功能，还需要地图、感知、定位、控制这四个模块。

2.2 地图模块

地图模块需要提供全局地图，以便全局路径规划器进行全局路径规划。

全局地图一般为二维占据栅格地图，可由图像(jpg等格式)表示，其中白色表示自由，黑色表示占据，灰色代表未知区域。

navigation框架提供了一个可选的地图模块package：map_server。

该package通过读取地图配置文件和地图，然后发布/map话题，以供全局规划器进行全局路径规划

地图配置文件为.yaml文件，主要由6个参数描述：

• 1.地图路径
• 2.地图分辨率（单位：米）
• 3.地图坐标系原点（x，y，theta）
• 4.是否翻转（黑色占据，白色自由 -> 黑色自由，白色占据）
• 5.占据阈值
• 6.自由阈值

image: result.jpg
resolution: 0.20000
origin: [-61, -149.5, 0.000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

地图为.jpg文件，其中白色表示自由，黑色表示占据，一般需要提前让机器人用SLAM算法为工作区域建立二维栅格地图。。

地图模块实践操作：

• 1.建图

生成二维地图和yaml配置文件

在实际工程中一般用3D SLAM算法先建立三维点云地图，然后设置地图分辨率参数，占据/自由阈值，将其投影为二维栅格地图，同时输出三维点云地图中坐标原点对应二维栅格地图中的像素坐标。

在使用地图模块时，需要分清楚机器人在地图坐标系下的坐标和定位坐标系下坐标的区别。

机器人的二维栅格地图坐标系为像素坐标系，图片的左顶点为原点，图片宽度为x，图片高度为y，定位数据通过地图分辨率转换为像素坐标。

• 2.定位

若在定位时，使用二维地图对应的三维点云地图进行定位，则二维地图坐标系与三维点云地图坐标系完成对应，机器人在三维点云地图坐标系下的定位坐标可直接作为二维地图坐标系下坐标。

若在定位时，未使用二维地图对应的三维点云地图进行定位，而是使用其它方式，如RTK、UWB等作为定位数据，则需要在建图的同时记录三维点云地图中坐标原点对应的RTK、UWB值和原点对应二维栅格地图中的像素坐标。机器人在二维地图坐标系下的坐标为，实时的RTK、UWB值 减去 三维点云地图中坐标原点对应的RTK、UWB值。

2.3 定位模块

定位方式有多种：①轮速里程计+imu；②激光+imu；③GPS(RTK)+激光+imu；④GPS(RTK)+激光+imu+轮速里程计；⑤相机+imu；⑥GPS(RTK)+相机+imu；⑦GPS(RTK)+相机+imu+轮速里程计；⑧GPS(RTK)+相机+激光+imu+轮速里程计；⑨...

以上不同定位方式的定位精度和成本不同，其中⑧的成本最高，定位效果最好；③的应用最广泛，不依赖特定的平台，同时环境适应性很强。其它定位方式与具体的场景有关，如室内环境、轮式平台等。

Navigation要求定位模块发出/odom的Topic名称，消息类型是nav_msgs/Odometry，包含机器人的位姿和速度。

GPS(RTK)设备不断的重新接收和计算世界坐标系中机器人的位姿，但连续两帧的GPS(RTK)坐标是不连续的，可能会发生跳变。这意味着在map坐标系中机器人的姿态再两帧之间会发生离散的跳变。

GPS(RTK)作为长期的全局坐标是很有用的，但是位姿的跳变极大影响运动规划和控制模块的性能，所以一般不直接使用GPS(RTK)坐标进行运动规划和控制。

机器人的里程计坐标是能够保证连续的，这意味着在里程计坐标系中的机器人的姿态总是平滑变化，没有跳变。因此适用于进行运动规划和控制。

激光和imu组合成激光里程计，然后通过GPS(RTK)进行校正是常用的定位方法。

定位模块实践操作：
1.让负责定位的同学发出里程计坐标，Topic的名称是/odom，消息类型是nav_msgs/Odometry，包含机器人的位姿和速度。

2.如果定位模块中有tf广播，需要注释掉，避免和自己写的tf冲突。

3.定位模块发出的里程计坐标的topic的frame_id要绑定到odom固定坐标系下（或者直接绑定到map下），不能绑定到base_link坐标系下。

2.4 感知模块

Navigation使用的sensro sources是激光雷达点云信息，话题名称是/scan，数据类型是LaserScan。

单线激光雷达可以室内满足要求，但无法在室外环境下为机器人提供足够的环境信息。通常使用多线激光雷达在室外环境下进行环境感知。

在Navigation中，采用多线激光雷达在室外环境下进行环境感知，需要将多线激光雷达数据格式转换为LaserScan数据类型。

推荐用pointcloud_to_laserscan包进行数据转换。该包是将多线激光雷达投影为二维数据，并不是只提取多线激光雷达中的一条线。

<group ns="bigrobot2">
  <node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan">
        <rosparam>
            <remap from="cloud_in" to="/lidar_cloud_origin"/>
            target_frame: bigrobot2/base_footprint # Leave disabled to output scan in pointcloud frame
            transform_tolerance: 0.01
            min_height: -0.4
            max_height: 1.0

            angle_min: -3.1415926 # -M_PI
            angle_max: 3.1415926 # M_PI
            angle_increment: 0.003 # 0.17degree
            scan_time: 0.1
            range_min: 0.1
            range_max: 100
            use_inf: true
            inf_epsilon: 1.0

            # Concurrency level, affects number of pointclouds queued for processing and number of threads used
            # 0 : Detect number of cores
            # 1 : Single threaded
            # 2->inf : Parallelism level
            concurrency_level: 1
        </rosparam>
    </node>
</group>

注意1： target_frame: bigrobot2/base_footprint 这个参数要求转换前的点云frame_id为bigrobot2/base_footprint，而该点云是直接读取速腾激光雷达点云topic，所以需要在速腾激光雷达点云的配置文件中直接修改发出点云topic的frame_id为bigrobot2/base_footprint。

注意2：rqt_graph显示pointcloud_to_laserscan没有订阅上points_raw，是因为没有节点订阅 scan。必须有节点订阅scan，pointcloud_to_laserscan节点才会订阅points_raw

感知模块实践操作

• 1.修改速腾激光雷达驱动配置文件，修改雷达发出点云topic的frame_id为bigrobot2/ lidar_cloud_origin
• 2.修改pointcloud_to_laserscan的launch文件中的target_frame为已有tf 树上的frame_id。
  注意是否需要remap相应的topic，可以通过rosnode info pointcloud_to_laserscan_node查看发出和接收的topic。
  remap和group的规则：先remap，再group。
  remap的规则：<remap from="old_topic" to="new_topic">

2.5 tf树

此模块非常重要！

需要自己创建节点进行tf树的广播，以便激光雷达点云数据、里程计的topic绑定对应的frame_id，必须确保这些topic绑定到对应的frame_id，否则Navigation无法正确运行。

Navigation是通过ROS中的tf 树来维护机器人中不同数据之间的坐标变换，如将激光雷达点云从激光雷达坐标系(base_laser)变换到车体坐标系(base_link)。

一个机器人通常tf 树的结构如下:

tf 树

每个坐标系都有一个父坐标系和任意子坐标系，map坐标系是odom坐标系的父，odom坐标系是base_link的父，每个坐标系只能有一个父类。

map下可以有多个定位分支odom，gps(uwb)，fusion_localization，这些分支与map原点可以设定是重合的或者由固定坐标转换，采用不同定位方式得到的定位结果。可以根据实际定位效果，把base_link绑定在不同的定位坐标系下。

base_link可以绑定在不同分支下

①world->map->gps(uwb)->base_link->base_laser
②world->map->odom->base_link->base_laser
③world->map->fusion_localization->base_link->base_laser

注意，如果slam定位模块中有tf广播，需要注释掉，避免和navigation的tf冲突，ROS只允许一个tf树存在。

Rviz中的fixed_frame也是根据tf树构建的坐标树显示相应的topic。

坐标系简析

• 1.世界坐标(earth)

固定坐标系（fixed frame），用来描述某点在地球上的绝对位置，通常用经纬度和高度，或者WGS84表示。

引入世界坐标系的背景是：
1.可以将机器人在地图中的坐标通过earth直接转换为经纬高。
2.机器人建图时会因为地图过大(TB甚至PB数量级）无法一次性完成建图，需要将地图分为多个子图（自动驾驶地图就是由多个子图组成），可以利用经纬高完成不同的子图拼接。

如果只有一张地图，一般而言，世界坐标系原点与地图坐标系原点重合

• 2.map坐标系
  固定坐标系（fixed frame），地图坐标系

作为长期的全局参考是很有用的，但是跳变使得对于本地传感和执行器来说，其实是一个不好的参考坐标。

• 3.里程计坐标系(odom)

odom 坐标系是一个固定坐标系（fixed frame）。

odom和map坐标系再机器人运动开始是重合的。但是，随着时间的推移是不重合的，而出现的偏差就是里程计的累积误差。

那map–>odom的tf怎么得到?就是在一些校正传感器合作校正的package比如amcl会给出一个位置估计（localization），这可以得到map–>base_link的tf，所以估计位置和里程计位置的偏差也就是odom与map的坐标系偏差。所以，如果你的odom计算没有错误，那么map–>odom的tf就是0。

• 4.车体坐标系(base_link)

该base_link坐标刚性地连接到移动机器人基座。base_link可以安装在基座中的任意方位；对于每个硬件平台，在基座上的不同地方都会提供一个明显的参考点。

机器人本体坐标系，与机器人中心重合，当然有些机器人(PR 2)是base_footprint,其实是一个意思。

• 5.激光雷达的坐标系(base_laser)
  与激光雷达的安装点有关，其与base_link的tf为固定的。

2.6 规划模块

如果以播放bag包形式运行move_base，costmap会报错，costmap的tolorence时间超过1s阈值，此时需要设置时间为仿真时间。

首先启动bag包的播放，让其发布/clock话题，这样其他包就会从这个包的时间戳读时间

1 设置采用仿真时间
rosparam set /use_sim_time true

2 仿真时间来源为bag的时间戳
rosbag play --pause --clock -k a.bag
--pause：停止播放（按空格取消）
--clock：以bag时间为clock加，使用仿真时间
-k, --keep-alive：包的数据结束后，仍然提供时间服务，避免一些程序读时间产生bug

2.7 控制模块

三、Navigation配置

3.1 move_base.launch

navigation实际使用的核心在于参数配置文件!

Navigation由于参数配置文件不同，在实际使用过程中的导航效果也不同，本节将从launch文件与配置参数文件进行分析，move_base.launch为启动文件，启动的节点包含map_server、move_base以及pointcloud_to_laserscan。

该launch文件不包含定位节点，需要额外单独启动，详情参考另一篇博客navigation定位未加链接！

3.2 map_server节点配置参数

move_base.launch首先启动map_server节点，加载了104andtest.yaml的地图配置文件，并设置map_server发布的map的frame_id为map。

<?xml version="1.0"?>
<launch>
  <node name="map_server" pkg="map_server" type="map_server" args="$(find move_control)/maps/104andtest.yaml">
    <param name="frame_id" value="map"/>
  </node>

104andtest.yaml文件如下,主要是制定了地图文件和地图参数如原点,分辨率等.

image: 104andtest.pgm
resolution: 0.050000
origin: [-141.372849, -52.350185, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

104andtest.pgm为slam建立的二维地图,类似

3.3 pointcloud_to_laserscan节点配置参数

接着启动pointcloud_to_laserscan节点

  <node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan">

    <remap from="/cloud_in" to="/points_raw"/>
    <rosparam>
        target_frame: velodyne # Leave disabled to output scan in pointcloud frame
        <!-- outputxx_frame: base_link -->
        transform_tolerance: 0.01
        min_height: -0.4
        max_height: 1.0

        angle_min: -3.1415926 # -M_PI
        angle_max: 3.1415926 # M_PI
        angle_increment: 0.003 # 0.17degree
        scan_time: 0.1
        range_min: 0.1
        range_max: 100
        use_inf: true
        inf_epsilon: 1.0

        # Concurrency level, affects number of pointclouds queued for processing and number of threads used
        # 0 : Detect number of cores
        # 1 : Single threaded
        # 2->inf : Parallelism level
        concurrency_level: 1
    </rosparam>
<remap from="/scan" to="/scan"/>
</node>

该节点需要订阅雷达点云信息,因为话题名称不匹配,需要remap订阅的话题名称<remap from="/cloud_in" to="/points_raw"/>

根据tf关系决定是否调整输出的点云信息的target_frame``,target_frame决定了点云是挂在哪个tf坐标系下的。

min_height:为-0.4,若为0,则会把地面当成障碍物,出现一圈环形障碍物
max_height:为1.0,高于机器人本体的高度即可,低于该高度的点云都会被投影下来
scan_time:为发出点云的间隔时间,0.1s,对应10Hz
range_min:点云距离雷达最小的距离
range_max:点云距离雷达最大的距离
use_inf:设置为true,若为false,可能导致costmap地图更新会有问题,与costmap_2D的障碍层的inf_is_valid参数相关联,对于无穷远的点要求其值为inf

3.4 move_base节点配置参数

启动核心节点move_base

<launch>
...
   <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">

     <param name="controller_frequency" value="10.0"/>
     <param name="controller_patiente" value="15.0"/>
     <param name="planner_frequnency" value="0.0"/>
     <param name="planner_patience" value="5.0" />

     <rosparam command="load" file="$(find move_control)/config/global_planner_params.yaml"/>
     <param name="base_global_planner" value="global_planner/GlobalPlanner"/>

     <rosparam command="load" file="$(find move_control)/config/teb_local_planner_params.yaml" />
     <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />

     <rosparam file="$(find move_control)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
     <rosparam file="$(find move_control)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
     <rosparam file="$(find move_control)/config/local_costmap_params.yaml" command="load" />
     <rosparam file="$(find move_control)/config/global_costmap_params.yaml" command="load" />
     <rosparam file="$(find move_control)/config/costmap_converter_params.yaml" command="load" />
 
</launch>

controller_frequency:以 Hz 为单位运行控制回路并向地盘发送速度命令的速率。
controller_patiente:在执行空间清理操作之前，控制器将等待多长时间（以秒为单位）而没有收到有效的控制。
planner_patience:在执行空间清理操作之前，规划器将在几秒钟内等待多长时间以尝试找到有效规划结果。
planner_frequnency:以 Hz 为单位运行全局规划循环的速率。如果频率设置为 0.0，则全局规划器将仅在收到新目标或本地规划器报告其路径被阻塞时运行。该参数是Navigation 1.6.0 中的新功能

3.4.1 base_global_planner节点配置参数

global_planner/GlobalPlanner参数为navigation自带的全局规划器,包含多种规划算法.

     <rosparam command="load" file="$(find move_control)/config/global_planner_params.yaml"/>
     <param name="base_global_planner" value="global_planner/GlobalPlanner"/>

global_planner_params.yaml文件如下,其中use_dijkstra若为false,则采用A*算法.

GlobalPlanner:
  allow_unknown: false #true
  default_tolerance: 0.2 # If goal in obstacle, plan to the closest point in radius default_tolerance, default 0.0
  visualize_potential: false 
  use_dijkstra: true #If true, use dijkstra's algorithm. Otherwise, A*.
  use_quadratic: true
  use_grid_path: false
  old_navfn_behavior: false                     # Exactly mirror behavior of navfn, use defaults for other boolean parameters, default false

3.4.2 base_local_planner节点配置参数

teb_local_planner/TebLocalPlannerROS为teb局部路径规划器,需要额外安装,可以参考teb规划器未加链接

     <rosparam command="load" file="$(find move_control)/config/teb_local_planner_params.yaml" />
     <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />

teb_local_planner_params.yaml文件如下

TebLocalPlannerROS:

 odom_topic: odom
    
 # Trajectory
  
 teb_autosize: True
 dt_ref: 0.34
 dt_hysteresis: 0.15
 max_samples: 500
 global_plan_overwrite_orientation: True
 allow_init_with_backwards_motion: False # false
 max_global_plan_lookahead_dist: 1.5
 global_plan_viapoint_sep: -1
 global_plan_prune_distance: 1
 exact_arc_length: True
 feasibility_check_no_poses: 5
 publish_feedback: False
    
 # Robot
         
 max_vel_x: 2.0 # 0.8
 max_vel_x_backwards: 0.8
 max_vel_y: 0.0
 max_vel_theta: 0.6
 acc_lim_x: 0.4 # 0.2
 acc_lim_theta: 0.6
 min_turning_radius: 0.0 # diff-drive robot (can turn on place!)

 footprint_model:
#   type: "polygon"
#   vertices: [[-1.223, 0.42], [-1.223, -0.42], [0.3, -0.42], [0.3, 0.42]]
   type: "two_circles"
   front_offset: 0.0
   front_radius: 0.28
   rear_offset: 0.3  
   rear_radius: 0.25

 # GoalTolerance
    
 xy_goal_tolerance: 0.2
 yaw_goal_tolerance: 0.1
 free_goal_vel: False
 complete_global_plan: True
    
 # Obstacles
    
 min_obstacle_dist: 0.1 # 0.25 # This value must also include our robot radius, since footprint_model is set to "point".
 inflation_dist: 0.2 # 0.6
 include_costmap_obstacles: True
 costmap_obstacles_behind_robot_dist: 1.5
 obstacle_poses_affected: 15

 dynamic_obstacle_inflation_dist: 0.2 # 0.6
 include_dynamic_obstacles: True

 costmap_converter_plugin: ""
 costmap_converter_spin_thread: True
 costmap_converter_rate: 5

 # Optimization
    
 no_inner_iterations: 5
 no_outer_iterations: 4
 optimization_activate: True
 optimization_verbose: False
 penalty_epsilon: 0.001 # 0.1
 obstacle_cost_exponent: 4
 weight_max_vel_x: 2
 weight_max_vel_theta: 1
 weight_acc_lim_x: 1
 weight_acc_lim_theta: 1
 weight_kinematics_nh: 1000
 weight_kinematics_forward_drive: 1
 weight_kinematics_turning_radius: 1
 weight_optimaltime: 1 # must be > 0
 weight_shortest_path: 0
 weight_obstacle: 100
 weight_inflation: 0.2
 weight_dynamic_obstacle: 10
 weight_dynamic_obstacle_inflation: 0.2
 weight_viapoint: 1
 weight_adapt_factor: 2

 # Homotopy Class Planner

 enable_homotopy_class_planning: True
 enable_multithreading: True
 max_number_classes: 4
 selection_cost_hysteresis: 1.0
 selection_prefer_initial_plan: 0.9
 selection_obst_cost_scale: 100.0
 selection_alternative_time_cost: False
 
 roadmap_graph_no_samples: 15
 roadmap_graph_area_width: 5
 roadmap_graph_area_length_scale: 1.0
 h_signature_prescaler: 0.5
 h_signature_threshold: 0.1
 obstacle_heading_threshold: 0.45
 switching_blocking_period: 0.0
 viapoints_all_candidates: True
 delete_detours_backwards: True
 max_ratio_detours_duration_best_duration: 3.0
 visualize_hc_graph: False
 visualize_with_time_as_z_axis_scale: False

# Recovery
 
 shrink_horizon_backup: True
 shrink_horizon_min_duration: 10
 oscillation_recovery: True
 oscillation_v_eps: 0.1
 oscillation_omega_eps: 0.1
 oscillation_recovery_min_duration: 10
 oscillation_filter_duration: 10

teb的参数可分为七类:
Trajectory:局部轨迹的参数,如轨迹长度,间隔时间,采样数量,是否允许倒退等
Robot:机器人最大速度,加速度和机器人外形尺寸
GoalTolerance:到达目标位置的误差参数
Obstacles:障碍物距离与膨胀参数问题!!!与costmap中的障碍物膨胀有什么区别
Optimization:目标函数权重参数,优化求解器参数
Homotopy Class Planner:同伦类规划器,teb中两种类实例化局部规划器,teb与Homotopy Class Planner.后者是一系列规划器的组合.参考
Recovery:类似navigation的recovery模块,陷入困境时脱困使用.

teb各个参数具体含义及调整可以参考这篇文章

3.4.3 costmap节点配置参数

navigation采用costmap_2D作为地图层,通过加载不同参数生成全局地图和局部地图.

     <rosparam file="$(find move_control)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
     <rosparam file="$(find move_control)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
     <rosparam file="$(find move_control)/config/local_costmap_params.yaml" command="load" />
     <rosparam file="$(find move_control)/config/global_costmap_params.yaml" command="load" />
     <rosparam file="$(find move_control)/config/costmap_converter_params.yaml" command="load" />

通过这些参数文件在ROS的参数服务器上注册了global_costmap/xx与local_costmap/xx等参数,move_base中会调用costmap_2D的API接口,根据这些参数实例化两个costmap对象,即global_costmap和local_costmap.

costmap_common_params.yaml文件,其中参数是全局地图和局部地图共有的参数.通过设置namespace参数ns="global_costmap"与ns="local_costmap"将global_costmap和local_costmap相同的参数一同初始化,避免改同一个参数,要修改两次.

当然也可以不用costmap_common_params.yaml文件,把这些参数全部放到global_costmap_params.yaml和local_costmap_params.yaml文件中.

costmap_common_params.yaml文件如下

robot_radius: 0.3
obstacle_layer:
  enabled: true
  combination_method: 1
  track_unknown_space: true
  obstacle_range: 6
  raytrace_range: 6.1
  observation_sources: scan
  scan: 
    sensor_frame: velodyne
    data_type: LaserScan
    topic: scan
    marking: true
    clearing: true
    inf_is_valid: true
    expected_update_rate: 0

# inflation_layer:
#   enabled:              true
#   cost_scaling_factor:  10.0 # 0.1
#   inflation_radius:     0.8 # 0.01 

static_layer:
  enabled:              true

global_costmap和local_costmap的机器人尺寸为robot_radius,两者都包含static_layer和obstacle_layer,而inflation_layer层的配置参数则由global_costmap和local_costmap分别配置,其中障碍物层参数解析可见添加链接.

global_costmap_params.yaml文件如下,

global_costmap:
  global_frame: map
  robot_base_frame: base_link
  update_frequency: 5.0
  publish_frequency: 5.0
  static_map: true
  rolling_window: false
  resolution: 0.1
  robot_radius: 0.3
  transform_tolerance: 1.0
  map_type: costmap
  plugins:
    - {name: static_layer, type: "costmap_2d::StaticLayer"}
    - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
  inflation_layer:
    enabled:              true
    cost_scaling_factor:  10.0 # 0.1
    inflation_radius:     0.4 # 0.01 

所有参数都是global_costmap:的二级子参数,类似global_costmap/global_frame,参数解析添加链接

local_costmap_params.yaml文件如下

local_costmap:
   global_frame: map
   robot_base_frame: base_link
   update_frequency: 10.0
   publish_frequency: 10.0
   static_map: false
   rolling_window: true
   width: 6.0
   height: 6.0
   resolution: 0.1
   robot_radius: 0.2
   transform_tolerance: 1.0
   plugins:
    - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
   inflation_layer:
      enabled:              true
      cost_scaling_factor:  10.0 # 0.1
      inflation_radius:     0.2 # 0.01

参数解析添加链接

costmap_converter_params.yaml文件如下

###########################################################################################
## NOTE: Costmap conversion is experimental. Its purpose is to combine many point        ##
## obstales into clusters, computed in a separate thread in order to improve the overall ## 
## efficiency of local planning. However, the implemented conversion algorithms are in a ##
## very early stage of development. Contributions are welcome!                           ##
###########################################################################################

TebLocalPlannerROS:

  ## Costmap converter plugin   
#costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH"
  costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSRANSAC"
#costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSMCCH"
#costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSConcaveHull"
  costmap_converter_spin_thread: True
  costmap_converter_rate: 5
 
 
  ## Configure plugins (namespace move_base/costmap_to_lines or move_base/costmap_to_polygons)
  ## costmap_converter/CostmapToLinesDBSRANSAC, costmap_converter/CostmapToLinesDBSMCCH, costmap_converter/CostmapToPolygonsDBSMCCH
  costmap_converter/CostmapToLinesDBSRANSAC:
    cluster_max_distance: 0.4
    cluster_min_pts: 2
    ransac_inlier_distance: 0.15
    ransac_min_inliers: 10
    ransac_no_iterations: 1500
    ransac_remainig_outliers: 3
    ransac_convert_outlier_pts: True
    ransac_filter_remaining_outlier_pts: False
    convex_hull_min_pt_separation: 0.1

costmap_converter诚如代码中的note所说,还处于very eary stage,早期不稳定版本,该参数不是必须的!

costmap_converter是teb作者提供了一个插件用于转换代价地图，原始的代价地图是由栅格地图中单元格组成，用于表示障碍物，但单元格占用的计算资源较大，故采用插件将单元格转换成点，线，多边形表示。

可以[参考]这篇文章了解convert(https://blog.csdn.net/weixin_42621524/article/details/120721270)

地图拼接，新地图坐标系原点，相对旧地图坐标系的变换矩阵。

把odom发布的cpp文件放到autoware中.

6。问题解决
注意pointcloud_to_laserscan是接收到cloud in的 topic后才会创建去订阅
remap不成功，在cloudin前面加了个/，不知道能否起作用
https://blog.csdn.net/qq_23670601/article/details/88529739

参考：
坐标系：https://blog.csdn.net/lovely_yoshino/article/details/99699321