Ask Your Question

multiple pointcloud2 topics for Navigation Stack with teb_local_planner

asked 2016-07-03 00:14:18 -0500

uwleahcim gravatar image

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:


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?


robot_radius: 0.5

transform_tolerance: 0.2
map_type: costmap

 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}

  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_frame: /map
  robot_base_frame: base_link
  update_frequency: 1.0
  publish_frequency: 0.5
  static_map: true

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


  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

   - {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.



 # 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!)

   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_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 ...
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted

answered 2016-07-12 14:30:12 -0500

croesmann gravatar image

updated 2016-07-12 14:31:15 -0500

In general, adding multiple sources is fine and should be preferred over fusing them manually. After having a brief look at your configuration, I recognized that your observation_sources are not part of the obstacle_layer param namespace. This could be the reason why they are ignored. See here and here (-> part of the obstacle layer).

The planner supports holonomic movements since kinetic (are you already running kinetic)? Otherwise the source code is backwards compatible by compiling it from source (up to now).

Regarding your second question: If you just need waypoints you can probably subscribe to the local_plan topic. If you also need temporal information, you could describe to the teb_feedback topic (but you need to turn it on by setting parameter publish_feedback to true. But both messages/topics are only published while navigation is active.

PS: the navigation stack is specialized to motions in the 2d plane. So I guess, you are planning for a fixed height.

edit flag offensive delete link more


Yes, I'm currently planning on performing 2D navigation for now.

As for the namespace issue, how do I got about making the observation_sources part of the obstacle_layer param namespace? According to link two, I should put the obs under the plugins, correct? Update: I added "obstacle_layer:" works

uwleahcim gravatar image uwleahcim  ( 2016-07-12 19:51:21 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower


Asked: 2016-07-03 00:14:18 -0500

Seen: 1,072 times

Last updated: Jul 12 '16