<?xml version="1.0"?>
<launch>
  <!-- 
    
    前面4个节点的代码文件在hdl_graph_slam/apps/...   是在ROS中注册的插件

    prefiltering_nodelet:               ！！！！ 点云滤波  ！！！！
        包（pkg）: nodelet
        类型（type）: nodelet
        名称（name）: prefiltering_nodelet
        作用：加载并执行hdl_graph_slam包中的PrefilteringNodelet，用于对激光雷达点云数据进行预处理，包括距离滤波、下采样和离群点移除。

    scan_matching_odometry_nodelet:     ！！！！帧间匹配，也就是激光里程计（前端里程计） ！！！！
        包（pkg）: nodelet
        类型（type）: nodelet
        名称（name）: scan_matching_odometry_nodelet
        作用：加载并执行hdl_graph_slam包中的ScanMatchingOdometryNodelet，用于通过扫描匹配进行里程计估计。

    floor_detection_nodelet (条件启动，取决于 enable_floor_detection 参数):           ！！！！地平面检测 ！！！！
        包（pkg）: nodelet
        类型（type）: nodelet
        名称（name）: floor_detection_nodelet
        作用：加载并执行hdl_graph_slam包中的FloorDetectionNodelet，用于检测地面平面。

    hdl_graph_slam_nodelet:             ！！！！后端概率图构建  ！！！！
        包（pkg）: nodelet
        类型（type）: nodelet
        名称（name）: hdl_graph_slam_nodelet
        作用：加载并执行hdl_graph_slam包中的HdlGraphSlamNodelet，用于执行基于图的SLAM（Simultaneous Localization and Mapping）。

    lidar2base_publisher:
        包（pkg）: tf
        类型（type）: static_transform_publisher
        作用：发布从base_link到velodyne（激光雷达）的静态坐标变换。这里的参数0 0 0 0 0 0表示没有平移和旋转，即激光雷达与base_link坐标系重合。

    velodyne_nodelet_manager:
        包（pkg）: nodelet
        类型（type）: nodelet
        名称（name）: velodyne_nodelet_manager
        作用：作为一个节点管理器，用于管理其他节点（nodelet）。

    map2odom_publisher:
        包（pkg）: hdl_graph_slam
        类型（type）: map2odom_publisher.py
        名称（name）: map2odom_publisher
        作用：这是一个Python脚本，用于发布从地图到里程计的变换。
  -->

  <!-- 使用仿真时间 -->
  <param name="use_sim_time" value="true" />
  <!-- 回放rosbag文件 -->
  <!-- /home/mj/hdl_slam/hdl_400.bag -->
  
  <node pkg="rosbag" type="play" name="player" output="screen" args="--clock /home/mj/hdl_slam/hdl_400.bag"/>
  <node pkg="hdl_graph_slam" type="laser2cloud" name="laser2cloud" output="log" />

  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find hdl_graph_slam)/rviz/hdl_graph_slam.rviz"/>  


<!-- 线程池   用另外一个线程实现后端 -->



  <!-- arguments -->
  <arg name="nodelet_manager" default="velodyne_nodelet_manager" />
  <arg name="enable_floor_detection" default="true" />
  <arg name="enable_gps" default="false" />
  <arg name="enable_imu_acc" default="false" />
  <arg name="enable_imu_ori" default="false" />

  <arg name="points_topic" default="/velodyne_points" />
  <arg name="map_frame_id" default="map" />
  <arg name="lidar_odom_frame_id" default="odom" />

  <!-- optional arguments -->
  <arg name="enable_robot_odometry_init_guess" default="false" />
  <arg name="robot_odom_frame_id" default="robot_odom" />

  <!-- transformation between lidar and base_link -->
  <node pkg="tf" type="static_transform_publisher" name="lidar2base_publisher" args="0 0 0 0 0 0 base_link velodyne 10" />

  <!-- in case you use velodyne_driver, comment out the following line -->
  <node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager)" args="manager" output="screen"/>

  <!-- prefiltering_nodelet -->
  <node pkg="nodelet" type="nodelet" name="prefiltering_nodelet" args="load hdl_graph_slam/PrefilteringNodelet $(arg nodelet_manager)">
    <remap from="/velodyne_points" to="$(arg points_topic)" />
    <!-- in case base_link_frame is blank, mapping will be performed in the lidar frame -->
    <param name="base_link_frame" value="base_link" />
    <!-- distance filter -->
    <param name="use_distance_filter" value="true" />
    <param name="distance_near_thresh" value="0.1" />
    <param name="distance_far_thresh" value="100.0" />
    <!-- NONE, VOXELGRID, or APPROX_VOXELGRID -->
    <param name="downsample_method" value="VOXELGRID" />
    <param name="downsample_resolution" value="0.1" />
    <!-- NONE, RADIUS, or STATISTICAL -->
    <param name="outlier_removal_method" value="RADIUS" />
    <param name="statistical_mean_k" value="30" />
    <param name="statistical_stddev" value="1.2" />
    <param name="radius_radius" value="0.5" />
    <param name="radius_min_neighbors" value="2" />
  </node>

  <!-- scan_matching_odometry_nodelet -->
  <node pkg="nodelet" type="nodelet" name="scan_matching_odometry_nodelet" args="load hdl_graph_slam/ScanMatchingOdometryNodelet $(arg nodelet_manager)" output="screen">
    <param name="points_topic" value="$(arg points_topic)" />
    <param name="odom_frame_id" value="$(arg lidar_odom_frame_id)" />
    <param name="robot_odom_frame_id" value="$(arg robot_odom_frame_id)" />
    <param name="keyframe_delta_trans" value="1.0" />
    <param name="keyframe_delta_angle" value="1.0" />
    <param name="keyframe_delta_time" value="10000.0" />
    <param name="transform_thresholding" value="false" />
    <param name="enable_robot_odometry_init_guess" value="$(arg enable_robot_odometry_init_guess)" />
    <param name="max_acceptable_trans" value="1.0" />
    <param name="max_acceptable_angle" value="1.0" />
    <param name="downsample_method" value="NONE" />
    <param name="downsample_resolution" value="0.1" />
    <!-- ICP, GICP, NDT, GICP_OMP, NDT_OMP, FAST_GICP(recommended), or FAST_VGICP -->
    <param name="registration_method" value="FAST_GICP" />
    <param name="reg_num_threads" value="0" />
    <param name="reg_transformation_epsilon" value="0.1"/>
    <param name="reg_maximum_iterations" value="64"/>
    <param name="reg_max_correspondence_distance" value="2.0"/>
    <param name="reg_max_optimizer_iterations" value="20"/>
    <param name="reg_use_reciprocal_correspondences" value="false"/>
    <param name="reg_correspondence_randomness" value="20"/>
    <param name="reg_resolution" value="1.0" />
    <param name="reg_nn_search_method" value="DIRECT7" />
  </node>

  <!-- floor_detection_nodelet -->
  <node pkg="nodelet" type="nodelet" name="floor_detection_nodelet" args="load hdl_graph_slam/FloorDetectionNodelet $(arg nodelet_manager)" if="$(arg enable_floor_detection)">
    <param name="points_topic" value="$(arg points_topic)" />
    <param name="tilt_deg" value="0.0" />
    <param name="sensor_height" value="2.0" />
    <param name="height_clip_range" value="1.0" />
    <param name="floor_pts_thresh" value="512" />
    <param name="use_normal_filtering" value="true" />
    <param name="normal_filter_thresh" value="20.0" />
  </node>

  <!-- hdl_graph_slam_nodelet -->
  <node pkg="nodelet" type="nodelet" name="hdl_graph_slam_nodelet" args="load hdl_graph_slam/HdlGraphSlamNodelet $(arg nodelet_manager)" output="screen">
    <param name="points_topic" value="$(arg points_topic)" />
    <!-- frame settings -->
    <param name="map_frame_id" value="$(arg map_frame_id)" />
    <param name="odom_frame_id" value="$(arg lidar_odom_frame_id)" />
    <!-- optimization params -->
    <!-- typical solvers: gn_var, gn_fix6_3, gn_var_cholmod, lm_var, lm_fix6_3, lm_var_cholmod, ... -->
    <param name="g2o_solver_type" value="lm_var_cholmod" />
    <param name="g2o_solver_num_iterations" value="512" />
    <!-- constraint switches -->
    <param name="enable_gps" value="$(arg enable_gps)" />
    <param name="enable_imu_acceleration" value="$(arg enable_imu_acc)" />
    <param name="enable_imu_orientation" value="$(arg enable_imu_ori)" />
    <!-- keyframe registration params -->
    <param name="max_keyframes_per_update" value="10" />
    <param name="keyframe_delta_trans" value="2.0" />
    <param name="keyframe_delta_angle" value="2.0" />
    <!-- fix first node for optimization stability -->
    <param name="fix_first_node" value="true"/>
    <param name="fix_first_node_stddev" value="10 10 10 1 1 1"/>
    <param name="fix_first_node_adaptive" value="true"/>
    <!-- loop closure params -->
    <param name="distance_thresh" value="15.0" />
    <param name="accum_distance_thresh" value="25.0" />
    <param name="min_edge_interval" value="15.0" />
    <param name="fitness_score_thresh" value="2.5" />
    <!-- scan matching params -->
    <param name="registration_method" value="FAST_GICP" />
    <param name="reg_num_threads" value="0" />
    <param name="reg_transformation_epsilon" value="0.1"/>
    <param name="reg_maximum_iterations" value="64"/>
    <param name="reg_max_correspondence_distance" value="2.0"/>
    <param name="reg_max_optimizer_iterations" value="20"/>
    <param name="reg_use_reciprocal_correspondences" value="false"/>
    <param name="reg_correspondence_randomness" value="20"/>
    <param name="reg_resolution" value="1.0" />
    <param name="reg_nn_search_method" value="DIRECT7" />
    <!-- edge params -->
    <!-- GPS -->
    <param name="gps_edge_robust_kernel" value="NONE" />
    <param name="gps_edge_robust_kernel_size" value="1.0" />
    <param name="gps_edge_stddev_xy" value="20.0" />
    <param name="gps_edge_stddev_z" value="5.0" />
    <!-- IMU orientation -->
    <param name="imu_orientation_edge_robust_kernel" value="NONE" />
    <param name="imu_orientation_edge_stddev" value="1.0" />
    <!-- IMU acceleration (gravity vector) -->
    <param name="imu_acceleration_edge_robust_kernel" value="NONE" />
    <param name="imu_acceleration_edge_stddev" value="1.0" />
    <!-- ground plane -->
    <param name="floor_edge_robust_kernel" value="NONE" />
    <param name="floor_edge_stddev" value="10.0" />
    <!-- scan matching -->
    <!-- robust kernels: NONE, Cauchy, DCS, Fair, GemanMcClure, Huber, PseudoHuber, Saturated, Tukey, Welsch -->
    <param name="odometry_edge_robust_kernel" value="NONE" />
    <param name="odometry_edge_robust_kernel_size" value="1.0" />
    <param name="loop_closure_edge_robust_kernel" value="Huber" />
    <param name="loop_closure_edge_robust_kernel_size" value="1.0" />
    <param name="use_const_inf_matrix" value="false" />
    <param name="const_stddev_x" value="0.5" />
    <param name="const_stddev_q" value="0.1" />
    <param name="var_gain_a" value="20.0" />
    <param name="min_stddev_x" value="0.1" />
    <param name="max_stddev_x" value="5.0" />
    <param name="min_stddev_q" value="0.05" />
    <param name="max_stddev_q" value="0.2" />
    <!-- update params -->
    <param name="graph_update_interval" value="3.0" />
    <param name="map_cloud_update_interval" value="10.0" />
    <param name="map_cloud_resolution" value="0.05" />
  </node>

  <node pkg="hdl_graph_slam" type="map2odom_publisher.py" name="map2odom_publisher"  output="screen"/>
</launch>