multiple pointcloud2 topics for Navigation Stack with teb_local_planner

edit

I'm trying to setup move_base to work with an quadrotor with 4 stereo cameras sets under four different namespace (/front, /right, /back, and /left)

I'm currently using stereo_image_proc to generate pointcloud2 topics, which generates four separate topics:

/front/points2
/right/points2
/back/points2
/left/points2

I like to use all 4 topics for obstacle detection in my navigation stack with the teb_local_planner. Can the navigation stack handle multi point clouds or should I merge them into one topic?

Does following costmap setup look correct?

costmap_common_parameters.yaml

robot_radius: 0.5

transform_tolerance: 0.2
map_type: costmap

obstacle_layer:
 enabled: true
 obstacle_range: 3.0
 raytrace_range: 4.0
 max_obstacle_height: 2.5 # I have it set just below door height
 min_obstacle_height: 1.5  # I have it set above my min flight height
 inflation_radius: 0.2
 track_unknown_space: true
 combination_method: 1

observation_sources: point1 point2 point3 point4 laser1 laser2

point1: {sensor_frame: front_camera, data_type: PointCloud, topic: /front/points2, marking: true, clearing: true}
point2: {sensor_frame: right_camera, data_type: PointCloud, topic: /right/points2, marking: true, clearing: true}
point3: {sensor_frame: back_camera,  data_type: PointCloud, topic: /back/points2,  marking: true, clearing: true}
point4: {sensor_frame: left_camera,  data_type: PointCloud, topic: /left/points2,  marking: true, clearing: true}
laser1: {sensor_frame: base_link,  data_type: LaserScan, topic: /scan1,  marking: true, clearing: true}
laser2: {sensor_frame: base_link,  data_type: LaserScan, topic: /scan2,  marking: true, clearing: 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.

global_costmap_params.yaml

global_costmap:
  global_frame: /map
  robot_base_frame: base_link
  update_frequency: 1.0
  publish_frequency: 0.5
  static_map: true

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

local_costmap_params.yaml

local_costmap:
  global_frame: /map
  robot_base_frame: base_link
  update_frequency: 5.0
  publish_frequency: 2.0
  static_map: false
  rolling_window: true
  width: 5.5
  height: 5.5
  resolution: 0.1
  transform_tolerance: 0.5

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

Since my robot is a quadrotor, It is also has omni-directional movement like a holonomic robot. So I need a planner setup that does y movements as well.

teh_local_planner_params.yaml

TebLocalPlannerROS:

 # Trajectory

 teb_autosize: True
 dt_ref: 0.3
 dt_hysteresis: 0.1
 global_plan_overwrite_orientation: True
 max_global_plan_lookahead_dist: 3.0
 feasibility_check_no_poses: 5

 # Robot

 max_vel_x: 3.0
 max_vel_x_backwards: 3.0
 max_vel_y: 3.0
 max_vel_theta: 1.5
 acc_lim_x: 3.0
 acc_lim_y: 3.0
 acc_lim_theta: 3.0
 min_turning_radius: 0.0 # omni-drive robot (can turn on place!)

 footprint_model:
   type: "point"

 # GoalTolerance

 xy_goal_tolerance: 0.2
 yaw_goal_tolerance: 0.1
 free_goal_vel: False

 # Obstacles

 min_obstacle_dist: 0.7 # This value must also include our robot radius, since footprint_model is set to "point".
 include_costmap_obstacles: True
 costmap_obstacles_behind_robot_dist: 1.0
 obstacle_poses_affected: 30
 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
 weight_max_vel_x: 2
 weight_max_vel_y: 2
 weight_max_vel_theta: 1
 weight_acc_lim_x: 1
 weight_acc_lim_y: 1
 weight_acc_lim_theta: 1
 weight_kinematics_nh: 1 # WE HAVE A HOLONOMIC ROBOT, JUST ADD A SMALL PENALTY
 weight_kinematics_forward_drive: 1
 weight_kinematics_turning_radius: 1
 weight_optimaltime: 1
 weight_obstacle: 50

 # Homotopy Class Planner

 enable_homotopy_class_planning: True
 enable_multithreading: True
 simple_exploration: False
 max_number_classes: 4
 selection_cost_hysteresis: 1.0
 selection_obst_cost_scale: 1.0 ...