search

ros-navigation / navigation2

ROS 2 Navigation Framework and System
https://nav2.org/
Other
2.58k stars 1.29k forks source link

Ghost Obstacles in Local Costmap using Gazebo Simulations #4043

Closed mich1342 closed 10 months ago

mich1342 commented 10 months ago

Bug report

Required Info:

navigation yaml file: code that can be copy-pasted is preferred -->

amcl:
  ros__parameters:
    use_sim_time: False
    alpha1: 0.2
    alpha2: 0.2
    alpha3: 0.2
    alpha4: 0.2
    alpha5: 0.2
    base_frame_id: "base_footprint"
    beam_skip_distance: 0.5
    beam_skip_error_threshold: 0.9
    beam_skip_threshold: 0.3
    do_beamskip: false
    global_frame_id: "map"
    lambda_short: 0.1
    laser_likelihood_max_dist: 2.0
    laser_max_range: 100.0
    laser_min_range: -1.0
    laser_model_type: "likelihood_field"
    max_beams: 60
    max_particles: 2000
    min_particles: 500
    odom_frame_id: "odom"
    pf_err: 0.05
    pf_z: 0.99
    recovery_alpha_fast: 0.0
    recovery_alpha_slow: 0.0
    resample_interval: 1
    robot_model_type: "nav2_amcl::DifferentialMotionModel"
    save_pose_rate: 0.5
    sigma_hit: 0.2
    tf_broadcast: true
    transform_tolerance: 1.0
    update_min_a: 0.2
    update_min_d: 0.25
    z_hit: 0.5
    z_max: 0.05
    z_rand: 0.5
    z_short: 0.05
    scan_topic: scan

amcl_map_client:
  ros__parameters:
    use_sim_time: False

amcl_rclcpp_node:
  ros__parameters:
    use_sim_time: False

bt_navigator:
  ros__parameters:
    use_sim_time: False
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    bt_loop_duration: 10
    default_server_timeout: 20
    enable_groot_monitoring: True
    groot_zmq_publisher_port: 1666
    groot_zmq_server_port: 1667
    default_nav_to_pose_bt_xml: $(find-pkg-share nav2_bt_navigator)/behavior_trees/basic_navigation_talosbot.xml
    plugin_lib_names:
      - nav2_compute_path_to_pose_action_bt_node
      - nav2_compute_path_through_poses_action_bt_node
      - nav2_follow_path_action_bt_node
      - nav2_back_up_action_bt_node
      - nav2_spin_action_bt_node
      - nav2_wait_action_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_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_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_distance_traveled_condition_bt_node
      - nav2_single_trigger_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_smooth_path_action_bt_node

bt_navigator_rclcpp_node:
  ros__parameters:
    use_sim_time: False

controller_server:
  ros__parameters:
    use_sim_time: False
    controller_frequency: 30.0
    # min_x_velocity_threshold: 0.001
    # min_y_velocity_threshold: 0.5
    # min_theta_velocity_threshold: 0.001
    # failure_tolerance: 0.3
    # progress_checker_plugin: ["progress_checker"]
    # goal_checker_plugins: ["goal_checker"] # "precise_goal_checker"
    # controller_plugins: ["FollowPath"]

    # # Progress checker parameters
    # progress_checker:
    #   plugin: "nav2_controller::SimpleProgressChecker"
    #   required_movement_radius: 0.5
    #   movement_time_allowance: 10.0

    # # Goal checker parameters
    # goal_checker:
    #   stateful: True
    #   plugin: "nav2_controller::SimpleGoalChecker"
    #   xy_goal_tolerance: 0.25
    #   yaw_goal_tolerance: 0.25
    # # DWB parameters
    FollowPath:
      plugin: "nav2_mppi_controller::MPPIController"
      time_steps: 56
      model_dt: 0.05
      batch_size: 2000
      vx_std: 0.3
      vy_std: 0.0
      wz_std: 0.4
      vx_max: 0.3
      vx_min: -0.2
      vy_max: 0.0
      wz_max: 0.5
      iteration_count: 1
      prune_distance: 1.7
      transform_tolerance: 0.1
      temperature: 0.3
      gamma: 0.015
      motion_model: "DiffDrive"
      visualize: false
      reset_period: 1.0 # (only in Humble)
      regenerate_noises: false
      TrajectoryVisualizer:
        trajectory_step: 5
        time_step: 3
      AckermannConstraints:
        min_turning_r: 0.5
      critics:
        [
          "ConstraintCritic",
          "ObstaclesCritic",
          "GoalCritic",
          "GoalAngleCritic",
          "PathAlignCritic",
          "PathFollowCritic",
          "PathAngleCritic",
          "PreferForwardCritic",
        ]
      ConstraintCritic:
        enabled: true
        cost_power: 1
        cost_weight: 4.0
      GoalCritic:
        enabled: true
        cost_power: 1
        cost_weight: 5.0
        threshold_to_consider: 1.4
      GoalAngleCritic:
        enabled: true
        cost_power: 1
        cost_weight: 3.0
        threshold_to_consider: 0.5
      PreferForwardCritic:
        enabled: true
        cost_power: 1
        cost_weight: 5.0
        threshold_to_consider: 0.5
      ObstaclesCritic:
        enabled: true
        cost_power: 1
        repulsion_weight: 1.5
        critical_weight: 20.0
        consider_footprint: true
        collision_cost: 10000.0
        collision_margin_distance: 0.1
        near_goal_distance: 0.5
        inflation_radius: 0.55 # (only in Humble)
        cost_scaling_factor: 10.0 # (only in Humble)
      PathAlignCritic:
        enabled: true
        cost_power: 1
        cost_weight: 14.0
        max_path_occupancy_ratio: 0.05
        trajectory_point_step: 3
        threshold_to_consider: 0.5
        offset_from_furthest: 20
        use_path_orientations: false
      PathFollowCritic:
        enabled: true
        cost_power: 1
        cost_weight: 5.0
        offset_from_furthest: 5
        threshold_to_consider: 1.4
      PathAngleCritic:
        enabled: true
        cost_power: 1
        cost_weight: 2.0
        offset_from_furthest: 4
        threshold_to_consider: 0.5
        max_angle_to_furthest: 1.0
        mode: 0
      TwirlingCritic:
        enabled: true
        twirling_cost_power: 1
        twirling_cost_weight: 10.0

controller_server_rclcpp_node:
  ros__parameters:
    use_sim_time: False

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      use_sim_time: False
      rolling_window: true
      width: 4
      height: 4
      resolution: 0.05
      # robot_radius: 0.22
      footprint: "[[0.6, 0.35], [0.6, -0.35], [-0.6, -0.35], [-0.6, 0.35]]"
      plugins: ["voxel2d_layer", "voxel3d_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 5.00
        # inflation_radius: 0.55
        inflation_radius: 10.00
      voxel2d_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: False
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: scan base_scan
        # observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
        base_scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
      voxel3d_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: False
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        mark_threshold: 1
        observation_sources: realsense zed oakd
        min_obstacle_height: 0.2
        max_obstacle_height: 0.5
        realsense:
          topic: /camera/depth/color/points
          raytrace_min_range: 0.5
          min_obstacle_height: 0.1
          max_obstacle_height: 0.5
          clearing: True
          marking: True
          data_type: "PointCloud2"
        zed:
          topic: /zed/point_cloud/cloud_registered
          raytrace_min_range: 0.5
          min_obstacle_height: 0.1
          max_obstacle_height: 0.5
          clearing: True
          marking: True
          data_type: "PointCloud2"
        oakd:
          topic: /stereo/points
          raytrace_min_range: 0.5
          min_obstacle_height: 0.0
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "PointCloud2"
      static_layer:
        map_subscribe_transient_local: True
      always_send_full_costmap: True
  local_costmap_client:
    ros__parameters:
      use_sim_time: False
  local_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: False

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      use_sim_time: False
      # robot_radius: 0.22
      footprint: "[[0.6, 0.35], [0.6, -0.35], [-0.6, -0.35], [-0.6, 0.35]]"
      resolution: 0.05
      track_unknown_space: true
      plugins:
        ["static_layer", "voxel2d_layer", "voxel3d_layer", "inflation_layer"]
      voxel2d_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
      voxel3d_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: False
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        mark_threshold: 1
        observation_sources: realsense zed oakd
        min_obstacle_height: 0.2
        max_obstacle_height: 2.0
        realsense:
          topic: /camera/depth/color/points
          raytrace_min_range: 0.5
          min_obstacle_height: 0.1
          max_obstacle_height: 0.5
          clearing: True
          marking: True
          data_type: "PointCloud2"
        zed:
          topic: /zed/point_cloud/cloud_registered
          raytrace_min_range: 0.5
          min_obstacle_height: 0.0
          max_obstacle_height: 0.5
          clearing: True
          marking: True
          data_type: "PointCloud2"
        oakd:
          topic: /stereo/points
          raytrace_min_range: 0.5
          min_obstacle_height: 0.0
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "PointCloud2"
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 4.5
        # inflation_radius: 0.55
        inflation_radius: 10.00
      always_send_full_costmap: True
  global_costmap_client:
    ros__parameters:
      use_sim_time: False
  global_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: False

map_server:
  ros__parameters:
    use_sim_time: False
    yaml_filename: "turtlebot3_world.yaml"

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

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

planner_server_rclcpp_node:
  ros__parameters:
    use_sim_time: False

recoveries_server:
  ros__parameters:
    costmap_topic: local_costmap/costmap_raw
    footprint_topic: local_costmap/published_footprint
    cycle_frequency: 10.0
    recovery_plugins: ["spin", "backup", "wait"]
    spin:
      plugin: "nav2_recoveries/Spin"
    backup:
      plugin: "nav2_recoveries/BackUp"
    wait:
      plugin: "nav2_recoveries/Wait"
    global_frame: odom
    robot_base_frame: base_link
    transform_timeout: 0.1
    use_sim_time: False
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: 0.4
    rotational_acc_lim: 3.2

robot_state_publisher:
  ros__parameters:
    use_sim_time: False

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
smoother_server:
  ros__parameters:
    costmap_topic: global_costmap/costmap_raw
    footprint_topic: global_costmap/published_footprint
    robot_base_frame: base_link
    transform_timeout: 0.1
    smoother_plugins: ["savitzky_golay_smoother"]
    savitzky_golay_smoother:
      plugin: "nav2_smoother::SavitzkyGolaySmoother"
      do_refinement: True
      refinement_num: 2

Expected behavior

There should no ghosting obstacles in the local costmap

Actual behavior

There are ghost obstacles in the local costmap layers as could be seen in the following images image

SteveMacenski commented 10 months ago

It seems to me that you likely have some sensors not properly configured. I don't see any errors in the config file so that would point to me to look at the URDF and SDF files to make sure the sensors are properly setup. What happens if you use the TB3 in the demo?

You could start to debug this by enabling / disabling different sensor sources in the costmap to see which are causing your issue, then track it down from there. The fact that its a straight line, right near a bunch of other obstacle straight-lines makes me think you have a sensor URDF or simulation plugin located in a wonky way mirroring some obstacles in an incorrect location.

mich1342 commented 10 months ago

Sure, will try your suggestions and get back to you soon. Thank you for your responses.

mich1342 commented 10 months ago

Hi @SteveMacenski,

Just found the issues in my IMU URDF coordinates. Thank you, I will close this issues.