Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Costmap not being published when using custom params #2541

Closed
marcomasa opened this issue Aug 30, 2021 · 6 comments
Closed

Costmap not being published when using custom params #2541

marcomasa opened this issue Aug 30, 2021 · 6 comments

Comments

@marcomasa
Copy link

Bug report

Required Info:

  • Operating System: Ubuntu 20.04

  • ROS2 Version: Galactic (apt-get)

  • Version or commit hash:
    ros-galactic-nav2-amcl 1.0.6-1focal.20210701.002235
    ros-galactic-nav2-behavior-tree 1.0.6-1focal.20210701.002952
    ros-galactic-nav2-bringup 1.0.6-1focal.20210701.015912
    ros-galactic-nav2-bt-navigator 1.0.6-1focal.20210701.004934
    ros-galactic-nav2-common 1.0.6-1focal.20210630.232638
    ros-galactic-nav2-controller 1.0.6-1focal.20210701.004401
    ros-galactic-nav2-core 1.0.6-1focal.20210701.004136
    ros-galactic-nav2-costmap-2d 1.0.6-1focal.20210701.002823
    ros-galactic-nav2-dwb-controller 1.0.6-1focal.20210701.005251
    ros-galactic-nav2-lifecycle-manager 1.0.6-1focal.20210701.002253
    ros-galactic-nav2-map-server 1.0.6-1focal.20210701.002229
    ros-galactic-nav2-msgs 1.0.6-1focal.20210630.235639
    ros-galactic-nav2-navfn-planner 1.0.6-1focal.20210701.004410
    ros-galactic-nav2-planner 1.0.6-1focal.20210701.004411
    ros-galactic-nav2-recoveries 1.0.6-1focal.20210701.004929
    ros-galactic-nav2-regulated-pure-pursuit-controller 1.0.6-1focal.20210701.004418
    ros-galactic-nav2-rviz-plugins 1.0.6-1focal.20210701.014516
    ros-galactic-nav2-smac-planner 1.0.6-1focal.20210701.004430
    ros-galactic-nav2-util 1.0.6-1focal.20210701.001550
    ros-galactic-nav2-voxel-grid 1.0.6-1focal.20210701.002338
    ros-galactic-nav2-waypoint-follower 1.0.6-1focal.20210701.004452

  • DDS implementation: default

Steps to reproduce issue

I am trying to get nav2 working with a custom simulation setup.
I modified the launchfiles from the turtlebot example and used them in the waffle sim.
Then i got my custom model loaded in gazebo & rviz2 and made a slam of my custom world.

After adjusting AMCL and the global planner plugin (SMAC),
i cannot get the costmaps to show in rviz2.
Weird thing about this is, it seems its just not published, bc sometimes i can still get the robot to navigate.

nav_params.yaml

amcl:
  ros__parameters:
    use_sim_time: True
    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: 600
    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: "omnidirectional"
    save_pose_rate: 0.5
    sigma_hit: 0.2
    tf_broadcast: true
    transform_tolerance: 1.0
    update_min_a: 0.01
    update_min_d: 0.01
    z_hit: 0.5
    z_max: 0.05
    z_rand: 0.5
    z_short: 0.05
    scan_topic: scan
    set_initial_pose: True
    initial_pose: '{0.6, 0.5, 0.0, 0.0}'

amcl_map_client:
  ros__parameters:
    use_sim_time: True

amcl_rclcpp_node:
  ros__parameters:
    use_sim_time: True

bt_navigator:
  ros__parameters:
    use_sim_time: True
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    bt_loop_duration: 10
    default_server_timeout: 20
    enable_groot_monitoring: False
    groot_zmq_publisher_port: 1666
    groot_zmq_server_port: 1667
    # '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.
    
    #default_nav_to_pose_bt_xml: replace/with/path/to/bt.xml
    #default_nav_through_poses_bt_xml: replace/with/path/to/bt.xml
    
    #bt_xml_filename: "navigate_w_replanning_and_recovery.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

bt_navigator_rclcpp_node:
  ros__parameters:
    use_sim_time: True

controller_server:
  ros__parameters:
    use_sim_time: True
    controller_frequency: 20.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: ["general_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
    #precise_goal_checker:
    #  plugin: "nav2_controller::SimpleGoalChecker"
    #  xy_goal_tolerance: 0.25
    #  yaw_goal_tolerance: 0.25
    #  stateful: True
    general_goal_checker:
      stateful: True
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.25
      yaw_goal_tolerance: 0.25
    # DWB parameters
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      debug_trajectory_details: True
      min_vel_x: 0.0
      min_vel_y: 0.0
      max_vel_x: 0.8
      max_vel_y: 0.8
      max_vel_theta: 1.0
      min_speed_xy: 0.0
      max_speed_xy: 0.8
      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: 0.0
      acc_lim_theta: 3.2
      decel_lim_x: -2.5
      decel_lim_y: 0.0
      decel_lim_theta: -3.2
      vx_samples: 20
      vy_samples: 5
      vtheta_samples: 20
      sim_time: 1.7
      linear_granularity: 0.05
      angular_granularity: 0.025
      transform_tolerance: 0.2
      xy_goal_tolerance: 0.25
      trans_stopped_velocity: 0.25
      short_circuit_trajectory_evaluation: True
      stateful: True
      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
      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
      
controller_server_rclcpp_node:
  ros__parameters:
    use_sim_time: True

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: True
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.025
      #robot_radius: 0.22
      footprint: '[ [0.24, 0.2], [0.24, -0.2], [-0.24, -0.2], [-0.24, 0.2] ]'
      plugins: ["voxel_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 0.025
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      always_send_full_costmap: True
  local_costmap_client:
    ros__parameters:
      use_sim_time: True
  local_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: True

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      use_sim_time: True
      #robot_radius: 0.22
      footprint: '[[0.24,0.2],[0.24,-0.2],[-0.24,-0.2],[-0.24,0.2]]'
      resolution: 0.025
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      obstacle_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"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55
      always_send_full_costmap: True
  global_costmap_client:
    ros__parameters:
      use_sim_time: True
  global_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: True

map_server:
  ros__parameters:
    use_sim_time: True
    yaml_filename: "traktor_map.yaml"

map_saver:
  ros__parameters:
    use_sim_time: True
    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: True
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_smac_planner/SmacPlanner"
      tolerance: 0.5                     # tolerance for planning if unable to reach exact pose, in meters, for 2D node
      downsample_costmap: false           # whether or not to downsample the map
      downsampling_factor: 1          # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm)
      allow_unknown: false                # allow traveling in unknown space
      max_iterations: 1000000             # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable
      max_on_approach_iterations: 1000    # maximum number of iterations to attempt to reach goal once in tolerance, 2D only
      max_planning_time: 3.5              # max time in s for planner to plan, smooth, and upsample. Will scale maximum smoothing and upsampling times based on remaining time after planning.
      motion_model_for_search: "DUBIN"    # 2D Moore, Von Neumann; Hybrid Dubin, Redds-Shepp; State Lattice set internally
      cost_travel_multiplier: 2.0         # For 2D: Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*.
      angle_quantization_bins: 64         # For Hybrid/Lattice nodes: Number of angle bins for search, must be 1 for 2D node (no angle search)
      analytic_expansion_ratio: 3.5       # For Hybrid/Lattice nodes: The ratio to attempt analytic expansions during search for final approach.
      minimum_turning_radius: 0.0       # For Hybrid/Lattice nodes: minimum turning radius in m of path / vehicle
      reverse_penalty: 2.1                # For Reeds-Shepp model: penalty to apply if motion is reversing, must be => 1
      change_penalty: 0.15                # For Hybrid/Lattice nodes: penalty to apply if motion is changing directions, must be >= 0
      non_straight_penalty: 1.50          # For Hybrid/Lattice nodes: penalty to apply if motion is non-straight, must be => 1
      cost_penalty: 1.7                   # For Hybrid/Lattice nodes: penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable.
      lattice_filepath: ""                # For Lattice node: the filepath to the state lattice graph
      lookup_table_size: 20               # For Hybrid/Lattice nodes: Size of the dubin/reeds-sheep distance window to cache, in meters.
      cache_obstacle_heuristic: True      # For Hybrid/Lattice nodes: Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static.
      smoother:
        max_iterations: 1000
        w_smooth: 0.3
        w_data: 0.2
        tolerance: 1e-10

planner_server_rclcpp_node:
  ros__parameters:
    use_sim_time: True

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: true
    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: True

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

Expected behavior

I would expect the costmap to be published and then subscribed/visualized by rviz2.

Actual behavior

Clean startup?

[INFO] [launch]: All log files can be found below /home/evocortex/.ros/log/2021-08-30-23-18-45-324104-sim2-100541
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [controller_server-1]: process started with pid [100543]
[INFO] [planner_server-2]: process started with pid [100545]
[INFO] [recoveries_server-3]: process started with pid [100547]
[INFO] [bt_navigator-4]: process started with pid [100549]
[INFO] [waypoint_follower-5]: process started with pid [100551]
[INFO] [lifecycle_manager-6]: process started with pid [100553]
[planner_server-2] 1630358325.767119 [0] planner_se: using network interface enp5s0 (udp/10.0.7.21) selected arbitrarily from: enp5s0, docker0
[bt_navigator-4] 1630358325.767301 [0] bt_navigat: using network interface enp5s0 (udp/10.0.7.21) selected arbitrarily from: enp5s0, docker0
[waypoint_follower-5] 1630358325.768459 [0] waypoint_f: using network interface enp5s0 (udp/10.0.7.21) selected arbitrarily from: enp5s0, docker0
[lifecycle_manager-6] 1630358325.770640 [0] lifecycle_: using network interface enp5s0 (udp/10.0.7.21) selected arbitrarily from: enp5s0, docker0
[recoveries_server-3] 1630358325.770677 [0] recoveries: using network interface enp5s0 (udp/10.0.7.21) selected arbitrarily from: enp5s0, docker0
[controller_server-1] 1630358325.771237 [0] controller: using network interface enp5s0 (udp/10.0.7.21) selected arbitrarily from: enp5s0, docker0
[lifecycle_manager-6] [INFO] [1630358325.776176077] [lifecycle_manager_navigation]: Creating
[recoveries_server-3] [INFO] [1630358325.778302597] [recoveries_server]: 
[recoveries_server-3] 	recoveries_server lifecycle node launched. 
[recoveries_server-3] 	Waiting on external lifecycle transitions to activate
[recoveries_server-3] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[bt_navigator-4] [INFO] [1630358325.778605605] [bt_navigator]: 
[bt_navigator-4] 	bt_navigator lifecycle node launched. 
[bt_navigator-4] 	Waiting on external lifecycle transitions to activate
[bt_navigator-4] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[bt_navigator-4] [INFO] [1630358325.778676639] [bt_navigator]: Creating
[controller_server-1] [INFO] [1630358325.779164213] [controller_server]: 
[controller_server-1] 	controller_server lifecycle node launched. 
[controller_server-1] 	Waiting on external lifecycle transitions to activate
[controller_server-1] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[lifecycle_manager-6] [INFO] [1630358325.781670825] [lifecycle_manager_navigation]: Creating and initializing lifecycle service clients
[controller_server-1] [INFO] [1630358325.781704739] [controller_server]: Creating controller server
[planner_server-2] [INFO] [1630358325.785159760] [planner_server]: 
[planner_server-2] 	planner_server lifecycle node launched. 
[planner_server-2] 	Waiting on external lifecycle transitions to activate
[planner_server-2] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[lifecycle_manager-6] [INFO] [1630358325.785320642] [lifecycle_manager_navigation]: Starting managed nodes bringup...
[lifecycle_manager-6] [INFO] [1630358325.785351550] [lifecycle_manager_navigation]: Configuring controller_server
[planner_server-2] [INFO] [1630358325.786238143] [planner_server]: Creating
[controller_server-1] [INFO] [1630358325.790139512] [local_costmap.local_costmap]: 
[controller_server-1] 	local_costmap lifecycle node launched. 
[controller_server-1] 	Waiting on external lifecycle transitions to activate
[controller_server-1] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[controller_server-1] [INFO] [1630358325.790581571] [local_costmap.local_costmap]: Creating Costmap
[waypoint_follower-5] [INFO] [1630358325.791299648] [waypoint_follower]: 
[waypoint_follower-5] 	waypoint_follower lifecycle node launched. 
[waypoint_follower-5] 	Waiting on external lifecycle transitions to activate
[waypoint_follower-5] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[waypoint_follower-5] [INFO] [1630358325.793637124] [waypoint_follower]: Creating
[planner_server-2] [INFO] [1630358325.798265877] [global_costmap.global_costmap]: 
[planner_server-2] 	global_costmap lifecycle node launched. 
[planner_server-2] 	Waiting on external lifecycle transitions to activate
[planner_server-2] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[planner_server-2] [INFO] [1630358325.798787285] [global_costmap.global_costmap]: Creating Costmap
[controller_server-1] [INFO] [1630358325.820999404] [controller_server]: Configuring controller interface
[controller_server-1] [INFO] [1630358325.821503059] [controller_server]: getting goal checker plugins..
[controller_server-1] [INFO] [1630358325.821715337] [controller_server]: Controller frequency set to 20.0000Hz
[controller_server-1] [INFO] [1630358325.821740274] [local_costmap.local_costmap]: Configuring
[controller_server-1] [INFO] [1630358325.825867697] [local_costmap.local_costmap]: Using plugin "voxel_layer"
[controller_server-1] [INFO] [1630358325.830157395] [local_costmap.local_costmap]: Subscribed to Topics: scan
[controller_server-1] [INFO] [1630358325.836920521] [local_costmap.local_costmap]: Initialized plugin "voxel_layer"
[controller_server-1] [INFO] [1630358325.836950618] [local_costmap.local_costmap]: Using plugin "inflation_layer"
[controller_server-1] [INFO] [1630358325.838637332] [local_costmap.local_costmap]: Initialized plugin "inflation_layer"
[controller_server-1] [INFO] [1630358325.843466752] [controller_server]: Created progress_checker : progress_checker of type nav2_controller::SimpleProgressChecker
[controller_server-1] [INFO] [1630358325.848151320] [controller_server]: Created goal checker : general_goal_checker of type nav2_controller::SimpleGoalChecker
[controller_server-1] [INFO] [1630358325.851614948] [controller_server]: Controller Server has general_goal_checker  goal checkers available.
[controller_server-1] [INFO] [1630358325.853490527] [controller_server]: Created controller : FollowPath of type dwb_core::DWBLocalPlanner
[controller_server-1] [INFO] [1630358325.855359373] [controller_server]: Setting transform_tolerance to 0.200000
[controller_server-1] [INFO] [1630358325.867706535] [controller_server]: Using critic "RotateToGoal" (dwb_critics::RotateToGoalCritic)
[controller_server-1] [INFO] [1630358325.868465028] [controller_server]: Critic plugin initialized
[controller_server-1] [INFO] [1630358325.868646088] [controller_server]: Using critic "Oscillation" (dwb_critics::OscillationCritic)
[controller_server-1] [INFO] [1630358325.869576904] [controller_server]: Critic plugin initialized
[controller_server-1] [INFO] [1630358325.869727306] [controller_server]: Using critic "BaseObstacle" (dwb_critics::BaseObstacleCritic)
[controller_server-1] [INFO] [1630358325.870054881] [controller_server]: Critic plugin initialized
[controller_server-1] [INFO] [1630358325.870247041] [controller_server]: Using critic "GoalAlign" (dwb_critics::GoalAlignCritic)
[controller_server-1] [INFO] [1630358325.870816189] [controller_server]: Critic plugin initialized
[controller_server-1] [INFO] [1630358325.870976480] [controller_server]: Using critic "PathAlign" (dwb_critics::PathAlignCritic)
[controller_server-1] [INFO] [1630358325.871478171] [controller_server]: Critic plugin initialized
[controller_server-1] [INFO] [1630358325.871667205] [controller_server]: Using critic "PathDist" (dwb_critics::PathDistCritic)
[controller_server-1] [INFO] [1630358325.872029976] [controller_server]: Critic plugin initialized
[controller_server-1] [INFO] [1630358325.872190717] [controller_server]: Using critic "GoalDist" (dwb_critics::GoalDistCritic)
[controller_server-1] [INFO] [1630358325.872557335] [controller_server]: Critic plugin initialized
[controller_server-1] [INFO] [1630358325.872577112] [controller_server]: Controller Server has FollowPath  controllers available.
[lifecycle_manager-6] [INFO] [1630358325.875450222] [lifecycle_manager_navigation]: Configuring planner_server
[planner_server-2] [INFO] [1630358325.875612787] [planner_server]: Configuring
[planner_server-2] [INFO] [1630358325.875641361] [global_costmap.global_costmap]: Configuring
[planner_server-2] [INFO] [1630358325.879136237] [global_costmap.global_costmap]: Using plugin "static_layer"
[planner_server-2] [INFO] [1630358325.883220439] [global_costmap.global_costmap]: Subscribing to the map topic (/map) with transient local durability
[planner_server-2] [INFO] [1630358325.883722892] [global_costmap.global_costmap]: Initialized plugin "static_layer"
[planner_server-2] [INFO] [1630358325.883740996] [global_costmap.global_costmap]: Using plugin "obstacle_layer"
[planner_server-2] [INFO] [1630358325.884414790] [global_costmap.global_costmap]: Subscribed to Topics: scan
[planner_server-2] [INFO] [1630358325.886326476] [global_costmap.global_costmap]: StaticLayer: Resizing costmap to 549 X 412 at 0.025000 m/pix
[planner_server-2] [INFO] [1630358325.887184466] [global_costmap.global_costmap]: Initialized plugin "obstacle_layer"
[planner_server-2] [INFO] [1630358325.887202990] [global_costmap.global_costmap]: Using plugin "inflation_layer"
[planner_server-2] [INFO] [1630358325.888469116] [global_costmap.global_costmap]: Initialized plugin "inflation_layer"
[planner_server-2] [INFO] [1630358325.901900192] [planner_server]: Created global planner plugin GridBased of type nav2_smac_planner/SmacPlanner
[planner_server-2] [INFO] [1630358325.905684401] [planner_server]: Configured plugin GridBased of type SmacPlanner with tolerance 0.50, maximum iterations 1000000, max on approach iterations 2147483647, and not allowing unknown traversal. Using motion model: Dubin.
[planner_server-2] [INFO] [1630358325.905729525] [planner_server]: Planner Server has GridBased  planners available.
[lifecycle_manager-6] [INFO] [1630358325.909526989] [lifecycle_manager_navigation]: Configuring recoveries_server
[recoveries_server-3] [INFO] [1630358325.909804350] [recoveries_server]: Configuring
[recoveries_server-3] [INFO] [1630358325.914151174] [recoveries_server]: Creating recovery plugin spin of type nav2_recoveries/Spin
[recoveries_server-3] [INFO] [1630358325.915132205] [recoveries_server]: Configuring spin
[recoveries_server-3] [INFO] [1630358325.917386283] [recoveries_server]: Creating recovery plugin backup of type nav2_recoveries/BackUp
[recoveries_server-3] [INFO] [1630358325.918133876] [recoveries_server]: Configuring backup
[recoveries_server-3] [INFO] [1630358325.919680827] [recoveries_server]: Creating recovery plugin wait of type nav2_recoveries/Wait
[recoveries_server-3] [INFO] [1630358325.920304517] [recoveries_server]: Configuring wait
[lifecycle_manager-6] [INFO] [1630358325.922243314] [lifecycle_manager_navigation]: Configuring bt_navigator
[bt_navigator-4] [INFO] [1630358325.922390711] [bt_navigator]: Configuring
[bt_navigator-4] [WARN] [1630358325.947081741] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic.
[lifecycle_manager-6] [INFO] [1630358325.954611105] [lifecycle_manager_navigation]: Configuring waypoint_follower
[waypoint_follower-5] [INFO] [1630358325.954793738] [waypoint_follower]: Configuring
[waypoint_follower-5] [INFO] [1630358325.961933212] [waypoint_follower]: Created waypoint_task_executor : wait_at_waypoint of type nav2_waypoint_follower::WaitAtWaypoint
[lifecycle_manager-6] [INFO] [1630358325.962511276] [lifecycle_manager_navigation]: Activating controller_server
[controller_server-1] [INFO] [1630358325.962623777] [controller_server]: Activating
[controller_server-1] [INFO] [1630358325.962647271] [local_costmap.local_costmap]: Activating
[controller_server-1] [INFO] [1630358325.962657220] [local_costmap.local_costmap]: Checking transform
[controller_server-1] [INFO] [1630358325.962775322] [local_costmap.local_costmap]: start
[controller_server-1] [INFO] [1630358326.013255380] [controller_server]: Creating bond (controller_server) to lifecycle manager.
[lifecycle_manager-6] [INFO] [1630358326.116728362] [lifecycle_manager_navigation]: Server controller_server connected with bond.
[lifecycle_manager-6] [INFO] [1630358326.116791992] [lifecycle_manager_navigation]: Activating planner_server
[planner_server-2] [INFO] [1630358326.117043403] [planner_server]: Activating
[planner_server-2] [INFO] [1630358326.117082977] [global_costmap.global_costmap]: Activating
[planner_server-2] [INFO] [1630358326.117097785] [global_costmap.global_costmap]: Checking transform
[planner_server-2] [INFO] [1630358326.117174669] [global_costmap.global_costmap]: Timed out waiting for transform from base_link to map to become available, tf error: Lookup would require extrapolation into the past.  Requested time 1593.432000 but the earliest data is at time 1594.237000, when looking up transform from frame [base_link] to frame [map]
[planner_server-2] [INFO] [1630358326.617218776] [global_costmap.global_costmap]: Timed out waiting for transform from base_link to map to become available, tf error: Lookup would require extrapolation into the past.  Requested time 1593.882000 but the earliest data is at time 1594.237000, when looking up transform from frame [base_link] to frame [map]
[planner_server-2] [INFO] [1630358327.117257673] [global_costmap.global_costmap]: start
[planner_server-2] [INFO] [1630358327.167846997] [planner_server]: Activating plugin GridBased of type SmacPlanner
[planner_server-2] [INFO] [1630358327.167940482] [planner_server]: Creating bond (planner_server) to lifecycle manager.
[lifecycle_manager-6] [INFO] [1630358327.270327096] [lifecycle_manager_navigation]: Server planner_server connected with bond.
[lifecycle_manager-6] [INFO] [1630358327.270375487] [lifecycle_manager_navigation]: Activating recoveries_server
[recoveries_server-3] [INFO] [1630358327.270691209] [recoveries_server]: Activating
[recoveries_server-3] [INFO] [1630358327.270736003] [recoveries_server]: Activating spin
[recoveries_server-3] [INFO] [1630358327.270752123] [recoveries_server]: Activating backup
[recoveries_server-3] [INFO] [1630358327.270763625] [recoveries_server]: Activating wait
[recoveries_server-3] [INFO] [1630358327.270775928] [recoveries_server]: Creating bond (recoveries_server) to lifecycle manager.
[lifecycle_manager-6] [INFO] [1630358327.373286805] [lifecycle_manager_navigation]: Server recoveries_server connected with bond.
[lifecycle_manager-6] [INFO] [1630358327.373378047] [lifecycle_manager_navigation]: Activating bt_navigator
[bt_navigator-4] [INFO] [1630358327.373691505] [bt_navigator]: Activating
[bt_navigator-4] [INFO] [1630358327.395543498] [bt_navigator]: Creating bond (bt_navigator) to lifecycle manager.
[lifecycle_manager-6] [INFO] [1630358327.497265225] [lifecycle_manager_navigation]: Server bt_navigator connected with bond.
[lifecycle_manager-6] [INFO] [1630358327.497347749] [lifecycle_manager_navigation]: Activating waypoint_follower
[waypoint_follower-5] [INFO] [1630358327.497708356] [waypoint_follower]: Activating
[waypoint_follower-5] [INFO] [1630358327.497756216] [waypoint_follower]: Creating bond (waypoint_follower) to lifecycle manager.
[lifecycle_manager-6] [INFO] [1630358327.599928267] [lifecycle_manager_navigation]: Server waypoint_follower connected with bond.
[lifecycle_manager-6] [INFO] [1630358327.599983761] [lifecycle_manager_navigation]: Managed nodes are active
[lifecycle_manager-6] [INFO] [1630358327.600001254] [lifecycle_manager_navigation]: Creating bond timer...

Rviz2 not showing the costmaps.

rviz_screen2

Additional information

I played around with rviz2 QoS settings as i had troubles with that earlier.
Seems to not change anything regarding the costmap publishing..

ros2 topic info /local_costmap/costmap --verbose 
Type: nav_msgs/msg/OccupancyGrid

Publisher count: 1

Node name: local_costmap
Node namespace: /local_costmap
Topic type: nav_msgs/msg/OccupancyGrid
Endpoint type: PUBLISHER
GID: 48.11.10.01.3f.5b.71.18.50.7a.a5.79.00.00.74.03.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RELIABLE
  Durability: TRANSIENT_LOCAL
  Lifespan: 9223372036854775807 nanoseconds
  Deadline: 9223372036854775807 nanoseconds
  Liveliness: AUTOMATIC
  Liveliness lease duration: 9223372036854775807 nanoseconds

Subscription count: 1

Node name: rviz
Node namespace: /
Topic type: nav_msgs/msg/OccupancyGrid
Endpoint type: SUBSCRIPTION
GID: 86.49.10.01.34.f8.b9.fa.99.f0.6b.ad.00.00.3f.04.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RELIABLE
  Durability: TRANSIENT_LOCAL
  Lifespan: 9223372036854775807 nanoseconds
  Deadline: 9223372036854775807 nanoseconds
  Liveliness: AUTOMATIC
  Liveliness lease duration: 9223372036854775807 nanoseconds

So the QoS settings are matching i guess,
but i try to find out the frequency nothing happens...

ros2 topic hz /local_costmap/costmap
1630363248.914410 [0]       ros2: using network interface enp5s0 (udp/10.0.7.21) selected arbitrarily from: enp5s0, docker0
@SteveMacenski
Copy link
Member

SteveMacenski commented Aug 31, 2021 •
edited

Please ask on ROS answers / search through issue trackers for rviz / QoS. This is not a problem with nav2 but perhaps your setup if it is navigating correctly.

@myxiaojun
Copy link

i have the same problem of "Rviz2 not showing the costmaps", @marcomasa do you find a way to solve this problem?

@marcomasa
Copy link
Author

@myxiaojun you need to launch everything in one launchfile.
Idk why, but starting everything seperately does NOT work for me..
You also need to check your QoS settings, but that does not help if you dont start everything at once (see turtle nav examples)

@myxiaojun
Copy link

myxiaojun commented Dec 8, 2021 •
edited

I use "ros2 launch turtlebot3_bringup robot.launch.py" to launch robot, use "ros2 launch turtlebot3_navigation2 navigation2.launch.py map:=$HOME/map.yaml" to launch nav2.. but the costmaps still not show. And my rviz qos is the same as the picture you posted...

@marcomasa
Copy link
Author

marcomasa commented Dec 9, 2021 •
edited

I think u need to launch rviz with the nav stack in a single file.
I dont fully understand the new launchfile backend, but i believe some values are not shared when
launching from seperate files (this is also kinda the intention of the new system?)

Maybe its because of the Lifecycle Nodes and the autostart params.
I guess the publishers do not recognize RVIZ as subscriber in your / our case.

@myxiaojun
Copy link

I once start nav2 by "ros2 launch turtlebot3_navigation2 navigation2.launch.py use_sim_time:=True map:=/mnt/ros2/src/map.yaml".
and I solved this problem by delete param "use_sim_time:=True".

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
3 participants
@SteveMacenski @marcomasa @myxiaojun