0
$\begingroup$

Rosanswers logo

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
 selection_alternative_time_cost: False
 
 roadmap_graph_no_samples: 15
 roadmap_graph_area_width: 5
 h_signature_prescaler: 0.5
 h_signature_threshold: 0.1
 obstacle_keypoint_offset: 0.1
 obstacle_heading_threshold: 0.45
 visualize_hc_graph: False

launch file

<launch>
  <master auto="start"/>

  <!-- Run the map server, right now I don't have a map -->

  <!--- Run AMCL, do I need to run AMCL? My UAV provides it own internal odometry-->
  <!--- We load ACML here with diff=true to support our differential drive robot -->
  <include file="$(find amcl)/examples/amcl_diff.launch" />

  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <rosparam file="costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="local_costmap_params.yaml" command="load" />
    <rosparam file="global_costmap_params.yaml" command="load" />
    <rosparam file="base_local_planner_params.yaml" command="load" />
  </node>
</launch>

Finally, something that deviates from the traditional navigation stack. My quadrotor has velocity based controls, but they're not that great. I tried running on the default parameters from the robot setup from the navigation stack tutorial and quadrotor keeps making large sweeping arc motions that deviated from the trajectory using velocity commands. Now the quadrotor has very good position based navigation via waypoints. Is it possible to follow the trajectory base on the path generated by the teb_local_planner instead of using cmd_vel?


Originally posted by uwleahcim on ROS Answers with karma: 101 on 2016-07-03

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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.


Originally posted by croesmann with karma: 2531 on 2016-07-12

This answer was ACCEPTED on the original site

Post score: 0


Original comments

Comment by uwleahcim on 2016-07-12:
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

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.