search

ros-planning / navigation

ROS Navigation stack. Code for finding where the robot is and how it can get somewhere else.
2.34k stars 1.79k forks source link

global costmap not built @ kinetic #743

Closed ghost closed 6 years ago

ghost commented 6 years ago

I am facing an issue with building a global costmap. While I am getting the warning in the terminal about map bounts The origin for the sensor at (-0.06, 0.01) is out of map bounds. So, the costmap cannot raytrace for it. the global cost map looks like this: costmap_issue

The local costmap looks fine: costmap_issue2

How can I avoid this issue? Is there probably a problem with the robot_base_frame?

I work with the branch "kinetic-devel"

I discovered this thread, but its not clear to me where and why which changes have been made. :( https://github.com/ros-planning/navigation/issues/148

Just in case, here is the output of 

rosparam get /move_base
TebLocalPlannerROS:
  acc_lim_theta: 0.5
  acc_lim_x: 0.5
  acc_lim_y: 0.5
  allow_init_with_backwards_motion: false
  alternative_time_cost: false
  cmd_angle_instead_rotvel: false
  costmap_converter_plugin: ''
  costmap_converter_rate: 5
  costmap_converter_spin_thread: true
  costmap_obstacles_behind_robot_dist: 1.0
  dt_hysteresis: 0.1
  dt_ref: 0.3
  dynamic_obstacle_inflation_dist: 0.6
  enable_homotopy_class_planning: true
  enable_multithreading: true
  exact_arc_length: false
  feasibility_check_no_poses: 5
  footprint_model:
    front_offset: 0.2
    front_radius: 0.2
    line_end: [0.3, 0.0]
    line_start: [-0.3, 0.0]
    radius: 0.2
    rear_offset: 0.2
    rear_radius: 0.2
    type: point
    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]
  force_reinit_new_goal_dist: 1.0
  free_goal_vel: false
  global_plan_overwrite_orientation: true
  global_plan_viapoint_sep: -0.1
  h_signature_prescaler: 0.5
  h_signature_threshold: 0.1
  include_costmap_obstacles: true
  include_dynamic_obstacles: false
  inflation_dist: 0.6
  is_footprint_dynamic: false
  legacy_obstacle_association: false
  map_frame: /map
  max_global_plan_lookahead_dist: 3.0
  max_number_classes: 4
  max_vel_theta: 0.3
  max_vel_x: 0.4
  max_vel_x_backwards: 0.2
  max_vel_y: 0.0
  min_obstacle_dist: 0.6
  min_turning_radius: 0.0
  no_inner_iterations: 5
  no_outer_iterations: 4
  obstacle_association_cutoff_factor: 5.0
  obstacle_association_force_inclusion_factor: 1.5
  obstacle_heading_threshold: 0.45
  obstacle_keypoint_offset: 0.1
  obstacle_poses_affected: 30
  odom_topic: odom
  optimization_activate: true
  optimization_verbose: false
  oscillation_recovery: true
  penalty_epsilon: 0.1
  publish_feedback: false
  roadmap_graph_area_length_scale: 1.0
  roadmap_graph_area_width: 5.0
  roadmap_graph_no_samples: 15
  selection_alternative_time_cost: false
  selection_cost_hysteresis: 1.0
  selection_obst_cost_scale: 100.0
  selection_prefer_initial_plan: 0.95
  selection_viapoint_cost_scale: 1.0
  shrink_horizon_backup: true
  simple_exploration: false
  teb_autosize: true
  via_points_ordered: false
  viapoints_all_candidates: true
  visualize_hc_graph: false
  visualize_with_time_as_z_axis_scale: 0.0
  weight_acc_lim_theta: 1.0
  weight_acc_lim_x: 1.0
  weight_acc_lim_y: 1.0
  weight_adapt_factor: 2.0
  weight_dynamic_obstacle: 10.0
  weight_dynamic_obstacle_inflation: 0.1
  weight_inflation: 0.1
  weight_kinematics_forward_drive: 1.0
  weight_kinematics_nh: 1000.0
  weight_kinematics_turning_radius: 1.0
  weight_max_vel_theta: 1.0
  weight_max_vel_x: 2.0
  weight_max_vel_y: 2.0
  weight_obstacle: 50.0
  weight_optimaltime: 1.0
  weight_viapoint: 1.0
  wheelbase: 1.0
  xy_goal_tolerance: 0.05
  yaw_goal_tolerance: 0.1
aggressive_reset: {reset_distance: 1.84}
base_global_planner: navfn/NavfnROS
base_local_planner: teb_local_planner/TebLocalPlannerROS
clearing_rotation_allowed: true
conservative_reset: {reset_distance: 0.1}
conservative_reset_dist: 0.1
controller_frequency: 3.0
controller_patience: 1.0
global_costmap:
  cost_scaling_factor: 0.5
  footprint: '[[0.5,-0.5],[0.5,0.5],[-0.5,0.5],[-0.5,-0.5]]'
  footprint_padding: 0.01
  global_frame: /map
  height: 10
  inflation_layer: {cost_scaling_factor: 0.5, enabled: true, inflate_unknown: false,
    inflation_radius: 0.5}
  inflation_radius: 0.5
  lethal_cost_threshold: 10
  map_type: costmap
  observation_sources: scan
  obstacle_layer:
    combination_method: 1
    enabled: true
    footprint_clearing_enabled: true
    max_obstacle_height: 2.0
    observation_sources: scan
    obstacle_range: 8.5
    raytrace_range: 8.5
    scan: {clearing: true, data_type: LaserScan, marking: true, topic: scan}
  obstacle_range: 8.5
  origin_x: 0.0
  origin_y: 0.0
  plugins:
  - {name: obstacle_layer, type: 'costmap_2d::ObstacleLayer'}
  - {name: inflation_layer, type: 'costmap_2d::InflationLayer'}
  publish_frequency: 0.1
  raytrace_range: 8.5
  resolution: 0.05
  robot_base_frame: /base_footprint
  robot_radius: 0.46
  scan: {clearing: true, data_type: LaserScan, marking: true, topic: scan}
  static_map: false
  transform_tolerance: 1.0
  update_frequency: 2.0
  width: 10
local_costmap:
  cost_scaling_factor: 0.5
  footprint: '[[0.5,-0.5],[0.5,0.5],[-0.5,0.5],[-0.5,-0.5]]'
  footprint_padding: 0.01
  global_frame: /map
  height: 4
  inflation_layer: {cost_scaling_factor: 0.5, enabled: true, inflate_unknown: false,
    inflation_radius: 0.5}
  inflation_radius: 0.5
  map_type: costmap
  observation_sources: scan
  obstacle_layer:
    combination_method: 1
    enabled: true
    footprint_clearing_enabled: true
    max_obstacle_height: 2.0
    observation_sources: scan
    obstacle_range: 8.5
    raytrace_range: 8.5
    scan: {clearing: true, data_type: LaserScan, marking: true, topic: scan}
  obstacle_range: 8.5
  origin_x: 0.0
  origin_y: 0.0
  plugins:
  - {name: obstacle_layer, type: 'costmap_2d::ObstacleLayer'}
  - {name: inflation_layer, type: 'costmap_2d::InflationLayer'}
  publish_frequency: 0.5
  raytrace_range: 8.5
  resolution: 0.05
  robot_base_frame: /base_footprint
  robot_radius: 0.46
  rolling_window: true
  scan: {clearing: true, data_type: LaserScan, marking: true, topic: scan}
  static_map: false
  transform_tolerance: 1.0
  update_frequency: 2.0
  width: 4
max_planning_retries: -1
oscillation_distance: 0.2
oscillation_timeout: 10.0
planner_frequency: 2.0
planner_patience: 1.0
recovery_behavior_enabled: true
restore_defaults: false
shutdown_costmaps: false
mikeferguson commented 6 years ago

The error message is pretty clear -- you're not on the map. The global costmap is only as big as the map is -- and since your map appears to be entirely in the positive quadrant (this is pretty typical in ROS), you can't expect any global costmap updates to be in the negative quadrants. Localize your robot to a location not at 0,0 and you should see updates.

ghost commented 6 years ago

Hmm, sorry what you say sound to me like the root source of the problem but the issue is: that actually this step (move the costmap) is not performed well, when using move_base node.

Interestingly, when I use standard turtlebot gmapping launch files, everything seems to be ok. However, when cartographer is responsible for slam, then the move base fails to shift the global cost map correctly.

What I also have observed: using default turtlebot files, the global costmap has a position (-10, -10, 0) which is all zero with cartographer.

The TFs (map->odom->base_footprint->base_link) seem all to be ok.

So probably this is not the right place to place the questions since it not an issue with the low-level costmap2d implementation. Would be great if someone can give me a hint where to look further.

Thanks!

ghost commented 6 years ago

solved. the issue was that the move_base module requires the same TF nodes as in the turtlebot example.. So I added imu_link and base_footprint with zero translation and identity rotation...

tunai commented 6 years ago

Hi @mojovski ,

I see you just solved this problem 10 hours ago. Can you please expand on your solution? I am new to the navigation stack and I have been playing with the parameters of my TurtleBot3 navigation module to improve the navigation performance, and I believe I screwed something up. Up until yesterday everything was fine, now I keep getting the "the origin of the sensor is out of bounds" error, as you were. I just replaced all the param files on the navigation folder for the original ones. I believe my problem might be on the RViz displays and the topics they are listening to.

So long story short, can you be more specific on your solution? Thank you!