0
$\begingroup$

I have a problem with the local costmap and global costmap concerning the position of the sensors, the error generated is

[local_costmap.local_costmap]: Sensor origin at (441107297.00, 3412589829.87) is out of map bounds (441670478.35, 3416986938.70) to (441670498.33, 3416987758.68). The costmap cannot raytrace for it.

Let me describe my application:

it is a rover capable of moving omnidirectionally in outdoor environments, it uses 3 main sources of odometry, 1st type: odometry from wheels, imu with heading, gps with active RTK. in addition I have two lidars that can cover the front and rear of the robot and so there is 360° coverage of the machine in motion.

For the setup I took my cue from the official demo in Nav2_gps_waypoint, adapting a few parameters so as not to use Gazebo but the real world I obtained the following configuration:

# GPS WPF CHANGES: 
# - amcl params where removed. They are not needed because global localization is provided
#   by an ekf node on robot_localization fusing gps data with local odometry sources
# - static layer is removed from both costmaps, in this tutorial we assume there is no map
#   of the environment
# - global costmap is set to be rolling to allow the robot to traverse big environment by
#   following successive relatively close waypoints that fit in a smaller rolling costmap

bt_navigator:
  ros__parameters:
    global_frame: map
    robot_base_frame: base_footprint
    odom_topic: /odom
    bt_loop_duration: 10
    default_server_timeout: 20
    navigators: ["navigate_to_pose", "navigate_through_poses"]
    navigate_to_pose:
      plugin: "nav2_bt_navigator/NavigateToPoseNavigator"
    navigate_through_poses:
      plugin: "nav2_bt_navigator/NavigateThroughPosesNavigator"
    # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
    # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
    # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
    # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
    plugin_lib_names:
      - nav2_compute_path_to_pose_action_bt_node
      - nav2_compute_path_through_poses_action_bt_node
      - nav2_smooth_path_action_bt_node
      - nav2_follow_path_action_bt_node
      - nav2_spin_action_bt_node
      - nav2_wait_action_bt_node
      - nav2_assisted_teleop_action_bt_node
      - nav2_back_up_action_bt_node
      - nav2_drive_on_heading_bt_node
      - nav2_clear_costmap_service_bt_node
      - nav2_is_stuck_condition_bt_node
      - nav2_goal_reached_condition_bt_node
      - nav2_goal_updated_condition_bt_node
      - nav2_globally_updated_goal_condition_bt_node
      - nav2_is_path_valid_condition_bt_node
      - nav2_are_error_codes_active_condition_bt_node
      - nav2_would_a_controller_recovery_help_condition_bt_node
      - nav2_would_a_planner_recovery_help_condition_bt_node
      - nav2_would_a_smoother_recovery_help_condition_bt_node
      - nav2_initial_pose_received_condition_bt_node
      - nav2_reinitialize_global_localization_service_bt_node
      - nav2_rate_controller_bt_node
      - nav2_distance_controller_bt_node
      - nav2_speed_controller_bt_node
      - nav2_truncate_path_action_bt_node
      - nav2_truncate_path_local_action_bt_node
      - nav2_goal_updater_node_bt_node
      - nav2_recovery_node_bt_node
      - nav2_pipeline_sequence_bt_node
      - nav2_round_robin_node_bt_node
      - nav2_transform_available_condition_bt_node
      - nav2_time_expired_condition_bt_node
      - nav2_path_expiring_timer_condition
      - nav2_distance_traveled_condition_bt_node
      - nav2_single_trigger_bt_node
      - nav2_goal_updated_controller_bt_node
      - nav2_is_battery_low_condition_bt_node
      - nav2_navigate_through_poses_action_bt_node
      - nav2_navigate_to_pose_action_bt_node
      - nav2_remove_passed_goals_action_bt_node
      - nav2_planner_selector_bt_node
      - nav2_controller_selector_bt_node
      - nav2_goal_checker_selector_bt_node
      - nav2_controller_cancel_bt_node
      - nav2_path_longer_on_approach_bt_node
      - nav2_wait_cancel_bt_node
      - nav2_spin_cancel_bt_node
      - nav2_back_up_cancel_bt_node
      - nav2_assisted_teleop_cancel_bt_node
      - nav2_drive_on_heading_cancel_bt_node
      - nav2_is_battery_charging_condition_bt_node
    error_code_names:
      - compute_path_error_code
      - follow_path_error_code

controller_server:
  ros__parameters:
    controller_frequency: 10.0
    min_x_velocity_threshold: 0.04
    min_y_velocity_threshold: 0.04
    min_xy_velocity_threshold: 0.04
    min_theta_velocity_threshold: 0.04
    failure_tolerance: 0.3
    progress_checker_plugins: ["progress_checker"]
    goal_checker_plugins: ["precise_goal_checker"] # "precise_goal_checker"
    controller_plugins: ["FollowPath"]

    # Progress checker parameters
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.05
      movement_time_allowance: 30.0
    # Goal checker parameters
    precise_goal_checker:
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.05
      yaw_goal_tolerance: 6.28
      stateful: False
    #general_goal_checker:
    #  stateful: True
    #  plugin: "nav2_controller::SimpleGoalChecker"
    #  xy_goal_tolerance: 0.05
    #  yaw_goal_tolerance: 6.28
    # DWB parameters
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      debug_trajectory_details: True
      min_vel_x: -0.1
      max_vel_x: 0.1

      min_vel_y: -0.1
      max_vel_y: 0.1

      min_vel_theta: -0.1
      max_vel_theta: 0.1

      min_speed_xy: -0.1
      max_speed_xy: 0.1

      min_speed_theta: 0.0
      # Add high threshold velocity for turtlebot 3 issue.
      # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
      
      acc_lim_x: 2.5
      acc_lim_y: 2.5
      acc_lim_theta: 0.0
      
      decel_lim_x: -2.5
      decel_lim_y: -2.5
      decel_lim_theta: 0.0
      
      vx_samples: 20
      vy_samples: 20
      vtheta_samples: 20

      sim_time: 1.7
      linear_granularity: 0.05
      angular_granularity: 0.025
      transform_tolerance: 0.2
      xy_goal_tolerance: 0.05 #0.25
      trans_stopped_velocity: 0.25
      short_circuit_trajectory_evaluation: True
      stateful: True
      critics: ["Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] #"RotateToGoal", 
      BaseObstacle.scale: 0.02
      PathAlign.scale: 32.0
      PathAlign.forward_point_distance: 0.1
      GoalAlign.scale: 24.0
      GoalAlign.forward_point_distance: 0.1
      PathDist.scale: 32.0
      GoalDist.scale: 24.0
      #RotateToGoal.scale: 32.0
      #RotateToGoal.slowing_factor: 5.0
      #RotateToGoal.lookahead_time: -1.0

# GPS WPF CHANGE: Remove static layer
local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_footprint
      rolling_window: true
      width: 20
      height: 820
      resolution: 0.05
      origin_x: 1.0
      robot_radius: 1.25
      track_unknown_space: true
      plugins: ["obstacle_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 10.0 #3.0
        inflation_radius: 0.25
      #voxel_layer:
      #  plugin: "nav2_costmap_2d::VoxelLayer"
      #  enabled: True
      #  publish_voxel_map: True
      #  origin_z: 0.0
      #  z_resolution: 0.05
      #  z_voxels: 16
      #  max_obstacle_height: 2.0
      #  mark_threshold: 0
      #  observation_sources: scan_front scan_rear
      #  scan_front:
      #    topic: /scan_front
      #    max_obstacle_height: 2.0
      #    clearing: True
      #    marking: True
      #    data_type: "LaserScan"
      #    raytrace_max_range: 5.0
      #    raytrace_min_range: 1.5
      #    obstacle_max_range: 5.0
      #    obstacle_min_range: 1.5
      #  scan_rear:
      #   topic: /scan_rear
      #   max_obstacle_height: 2.0
      #   clearing: True
      #   marking: True
      #   data_type: "LaserScan"
      #   raytrace_max_range: 5.0
      #   raytrace_min_range: 1.5
      #   obstacle_max_range: 5.0
      #  obstacle_min_range: 1.5
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan_front scan_rear
        scan_front:
          topic: /scan_front
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 10.0
          raytrace_min_range: 1.5
          obstacle_max_range: 9.5
          obstacle_min_range: 1.5
          # outdoors there will probably be more inf points
          inf_is_valid: true
        scan_rear:
          topic: /scan_rear
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 10.0
          raytrace_min_range: 1.5
          obstacle_max_range: 9.5
          obstacle_min_range: 1.5
          # outdoors there will probably be more inf points
          inf_is_valid: true
      always_send_full_costmap: True

# GPS WPF CHANGE: Remove static layer
# GPS WPF CHANGE: Set rolling global costmap with 50x50 size. See note below
global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_footprint
      robot_radius: 1.25
      resolution: 0.1
      # When using GPS navigation you will potentially traverse huge environments which are not practical to
      # fit on a big static costmap. Thus it is recommended to use a rolling global costmap large enough to 
      # contain each pair of successive waypoints. See: https://github.com/ros-planning/navigation2/issues/2174
      
      rolling_window: True
      width: 70
      height: 70
      track_unknown_space: true
      # no static map
      plugins: ["obstacle_layer", "inflation_layer"]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan_front scan_rear
        scan_front:
          topic: /scan_front
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 10.0
          raytrace_min_range: 1.5
          obstacle_max_range: 9.5
          obstacle_min_range: 1.5
          # outdoors there will probably be more inf points
          inf_is_valid: true
        scan_rear:
          topic: /scan_rear
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 10.0
          raytrace_min_range: 1.5
          obstacle_max_range: 9.5
          obstacle_min_range: 1.5
          # outdoors there will probably be more inf points
          inf_is_valid: true
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 10.0 #3.0
        inflation_radius: 0.20
      always_send_full_costmap: True

# The yaml_filename does not need to be specified since it going to be set by defaults in launch.
# If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py
# file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used.
# map_server:
#   ros__parameters:
#     yaml_filename: ""

map_saver:
  ros__parameters:
    use_sim_time: False
    save_map_timeout: 5.0
    free_thresh_default: 0.25
    occupied_thresh_default: 0.65
    map_subscribe_transient_local: True
    yaml_filename: "/reiwa/rover_ws/src/nav2_gps_waypoint_follower_demo/maps/mymap_empty.png"

planner_server:
  ros__parameters:
    expected_planner_frequency: 20.0 #1.0 #20.0
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.5
      use_astar: false
      allow_unknown: true

smoother_server:
  ros__parameters:
    smoother_plugins: ["simple_smoother"]
    simple_smoother:
      plugin: "nav2_smoother::SimpleSmoother"
      tolerance: 1.0e-10
      max_its: 1000
      do_refinement: True

behavior_server:
  ros__parameters:
    local_costmap_topic: local_costmap/costmap_raw
    global_costmap_topic: global_costmap/costmap_raw
    local_footprint_topic: local_costmap/published_footprint
    global_footprint_topic: global_costmap/published_footprint
    cycle_frequency: 5.0
    behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
    spin:
      plugin: "nav2_behaviors/Spin"
    backup:
      plugin: "nav2_behaviors/BackUp"
    drive_on_heading:
      plugin: "nav2_behaviors/DriveOnHeading"
    wait:
      plugin: "nav2_behaviors/Wait"
    assisted_teleop:
      plugin: "nav2_behaviors/AssistedTeleop"
    local_frame: odom
    global_frame: map
    robot_base_frame: base_footprint
    transform_tolerance: 0.1
    simulate_ahead_time: 2.0
    max_rotational_vel: 0.08
    min_rotational_vel: 0.04
    rotational_acc_lim: 3.04

waypoint_follower:
  ros__parameters:
    loop_rate: 20
    stop_on_failure: false
    waypoint_task_executor_plugin: "wait_at_waypoint"
    wait_at_waypoint:
      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
      enabled: True
      waypoint_pause_duration: 200

velocity_smoother:
  ros__parameters:
    smoothing_frequency: 0.1
    scale_velocities: False
    feedback: "OPEN_LOOP"
    max_velocity: [0.46, 0.46, 1.0]
    min_velocity: [-0.46, -0.46, -1.0]
    max_accel: [2.5, 2.5, 3.2]
    max_decel: [-2.5, -2.5, -3.2]
    odom_topic: "odom"
    odom_duration: 0.1
    deadband_velocity: [0.0, 0.0, 0.0]
    velocity_timeout: 1.0

I can't understand why in some circumstances the warnings (local_costmap and global_costmap out of sensor bound) disappear and allow me to navigate, while in other circumstances they are persistent and block the progress of the programme.

What can I do?

$\endgroup$
1
  • $\begingroup$ This is admittedly a system-level problem that is difficult for me to provide generic here's-your-problem advise for without myself digging into your full setup and experimenting. If you'd like some professional support from Open Navigation on this, let us know (opennav.org). It covers potentially more ground than you'd want to share publicly and would take a non-trivial amount of our time to look into that wouldn't be proportionate for Stack Exchange. It seems that something is misconfigured or there's some cross talking that needs to be addressed, which isn't immediately obvious $\endgroup$ Commented Mar 20 at 22:19

0

Your Answer

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

Browse other questions tagged or ask your own question.