lio_sam+move_base导航仿真实现

在仿真中使用lio_sam

参考https://py1995.blog.csdn.net/article/details/137286327?spm=1001.2014.3001.5502

保存三维点云地图PCD

point_cloud转laser_scan

参考https://py1995.blog.csdn.net/article/details/137539651?spm=1001.2014.3001.5502

三维点云转二维栅格地图

在线实时转（不建议，全局地图应为静态图层，实时更新会导致动态物体在全局地图上出现虚影）
参考https://py1995.blog.csdn.net/article/details/137521887?spm=1001.2014.3001.5502

以PCD形式加载地图，并以地图的格式发布map话题，参考https://github.com/linzs-online/robot_gazebo/tree/main/pcd2pgm

move_base参数配置

主要参数文件来自这里https://github.com/wh200720041/warehouse_simulation_toolkit/tree/master/param

参考坐标系向lio_sam配置对齐，在lio_sam中，坐标系为

在move_base设置为

move_base的输入话题有三个：/odom，/map, /scan，前两个做重映射即可，后面一个在pointcloud_to_laserscan包中发布了。

local_costmap_params.yaml

local_costmap:
  global_frame: odom_est
  robot_base_frame: base_link_est
  update_frequency: 10.0
  publish_frequency: 10.0
  rolling_window: true
  width: 5.5
  height: 5.5
  resolution: 0.05
  transform_tolerance: 0.5

  plugins:
    - { name: static_layer, type: "costmap_2d::StaticLayer" }
    - { name: obstacle_layer, type: "costmap_2d::ObstacleLayer" }

costmap_common_params.yaml

---
#---standard pioneer footprint---
#---(in meters)---
# robot_radius: 0.3
# footprint_padding: 0.05
footprint: [[0.5, 0.3], [0.5, -0.3], [-0.5, -0.3], [-0.5, 0.3]]

transform_tolerance: 0.2

always_send_full_costmap: true

obstacle_layer:
  enabled: true
  obstacle_range: 3.0
  raytrace_range: 5.0
  track_unknown_space: true
  combination_method: 1

  observation_sources: laser_scan_sensor
  laser_scan_sensor:
    { data_type: LaserScan, topic: scan, marking: true, clearing: true, inf_is_valid: true }

inflation_layer:
  enabled: true
  cost_scaling_factor: 10.0 # exponential rate at which the obstacle cost drops off (default: 10)
  inflation_radius: 0.5 # max. distance from an obstacle at which costs are incurred for planning paths (default: 0.55)

static_layer:
  enabled: true
  map_topic: "/map"

global_costmap_params.yaml

global_costmap:
  global_frame: map
  robot_base_frame: base_link_est
  update_frequency: 1.0
  publish_frequency: 0.5

  transform_tolerance: 0.5
  plugins:
    - { name: static_layer, type: "costmap_2d::StaticLayer" }
    - { name: obstacle_layer, type: "costmap_2d::ObstacleLayer" }
    - { name: inflation_layer, type: "costmap_2d::InflationLayer" }

teb_local_planner_params.yaml

TebLocalPlannerROS:
  odom_topic: odom

  # Trajectory
  teb_autosize: True
  dt_ref: 0.3
  dt_hysteresis: 0.1
  max_samples: 500
  global_plan_overwrite_orientation: True
  allow_init_with_backwards_motion: False
  max_global_plan_lookahead_dist: 3.0
  global_plan_viapoint_sep: -1
  global_plan_prune_distance: 1
  exact_arc_length: False
  feasibility_check_no_poses: 5
  publish_feedback: False

  # Robot
  max_vel_x: 0.55 # m/s
  max_vel_x_backwards: 0.2 # m/s
  max_vel_y: 0.0
  max_vel_theta: 0.3 # rad/s
  acc_lim_x: 0.3 # m/s^2
  acc_lim_theta: 0.5 # rad/s^2
  min_turning_radius: 0.0 # diff-drive robot (can turn on place!)

  # footprint_model:
  #   type: "polygon"
  #   # radius: 0.2 # for type "circular"
  #   # line_start: [-0.3, 0.0] # for type "line"
  #   # line_end: [0.3, 0.0] # for type "line"
  #   # front_offset: 0.2 # for type "two_circles"
  #   # front_radius: 0.2 # for type "two_circles"
  #   # rear_offset: 0.2 # for type "two_circles"
  #   # rear_radius: 0.2 # for type "two_circles"
  #   vertices: [ [0.25, -0.05], [0.18, -0.05], [0.18, -0.18], [-0.19, -0.18], [-0.25, 0], [-0.19, 0.18], [0.18, 0.18], [0.18, 0.05], [0.25, 0.05] ] # for type "polygon"
  
  footprint: [[0.5, 0.175], [0.5, -0.175], [-0.5, -0.175], [-0.5, 0.175]]

  # GoalTolerance
  xy_goal_tolerance: 0.2
  yaw_goal_tolerance: 0.1
  free_goal_vel: False
  complete_global_plan: True

  # Obstacles
  min_obstacle_dist: 0.4 # This value must also include our robot radius, since footprint_model is set to "point".
  inflation_dist: 0.6
  include_costmap_obstacles: True
  costmap_obstacles_behind_robot_dist: 1.5
  obstacle_poses_affected: 15

  dynamic_obstacle_inflation_dist: 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.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: 50
  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: False
  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

launch