Waiting for service map_server/get_state on nav2 application

[chivas1000]

Hi, I’m modifying the carter_sim_elbrus.launch.py example in nvblox_nav2 to do navigation in my robot,
The application is ok at DP1.1 release before
but for DP2.0, after the configurinig is done, the terminal keep popping “waiting for server state”

Error Output:
[lifecycle_manager-10] [INFO] [1666756876.922715146] [lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-1] [INFO] [1666756876.922716862] [lifecycle_manager_map]: Waiting for service map_server/get_state...
[lifecycle_manager-1] [INFO] [1666756878.923016306] [lifecycle_manager_map]: Waiting for service map_server/get_state...
[lifecycle_manager-10] [INFO] [1666756878.923042882] [lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-1] [INFO] [1666756880.923268323] [lifecycle_manager_map]: Waiting for service map_server/get_state...
[lifecycle_manager-10] [INFO] [1666756880.923292501] [lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-10] [INFO] [1666756882.923515151] [lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-1] [INFO] [1666756882.923517381] [lifecycle_manager_map]: Waiting for service map_server/get_state...
[lifecycle_manager-1] [INFO] [1666756884.923782959] [lifecycle_manager_map]: Waiting for service map_server/get_state...
[lifecycle_manager-10] [INFO] [1666756884.923803742] [lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[nvblox_node-11] [INFO] [1666756885.119232012] [nvblox_node]: RGB frame statistics:
[nvblox_node-11] avg=66.713883, min=55.955423, max=84.688353, std_dev=5.970719, count=13211
[nvblox_node-11] [INFO] [1666756885.463369156] [nvblox_node]: Depth frame statistics:
[nvblox_node-11] avg=36.677327, min=13.215166, max=126.695587, std_dev=10.915946, count=24039
[nvblox_node-11] [INFO] [1666756885.463661005] [nvblox_node]: Timing statistics:
[nvblox_node-11] NVBlox Timing
[nvblox_node-11] -----------
[nvblox_node-11] color/integrate

But the nvblox and vslam seem working as expect

so maybe is that the navigation stack doesn’t working since there are no availiable map input even though the nvblox does produced the map, so I assume that the nvblox has output esdf map but map server didn’t receive it

My launch files:

carter_sim_elbrus_new.launch.py
(for this the I didn't modified the map server related lines)

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import IncludeLaunchDescription
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import ComposableNodeContainer
from launch_ros.actions import Node
from launch_ros.descriptions import ComposableNode

def generate_launch_description():

#simulation time, this would not be use in the our application
use_sim_time = LaunchConfiguration('use_sim_time', default='False')


#costmap param, need to be modify
param_dir = LaunchConfiguration(
    'params_file',
    default=os.path.join(
        get_package_share_directory(
            'nvblox_nav2'), 'params', 'carter_nav2_new.yaml'
    ),
)

#nvblox params, if the output is not good, tune later
nvblox_param_dir = LaunchConfiguration(
    'nvblox_params_file', default=os.path.join(
        get_package_share_directory('nvblox_nav2'),
        'params', 'nvblox.yaml'),)

nav2_bringup_launch_dir = os.path.join(
    get_package_share_directory('nav2_bringup'), 'launch')

rviz_config_dir = os.path.join(get_package_share_directory(
    'nvblox_nav2'), 'config', 'carter_nvblox_nav2.rviz')

lifecycle_nodes = ['map_server']

#setting up vslam node, to be modify, we use infrared image here
visual_slam_node = ComposableNode(
    name='visual_slam_node',
    package='isaac_ros_visual_slam',
    plugin='isaac_ros::visual_slam::VisualSlamNode',
    remappings=[('/stereo_camera/left/camera_info', '/camera/infra1/camera_info'),
                ('/stereo_camera/right/camera_info', '/camera/infra2/camera_info'),
                ('/stereo_camera/left/image', '/camera/realsense_splitter_node/output/infra_1'),
                ('/stereo_camera/right/image', '/camera/realsense_splitter_node/output/infra_2')],
    parameters=[{
        'image_qos': 'SENSOR_DATA',
        'enable_rectified_pose': True,
        'denoise_input_images': True,
        'rectified_images': True,
        'enable_debug_mode': False,
        'debug_dump_path': '/tmp/elbrus',
        'left_camera_frame': 'camera_infra1_frame',
        'right_camera_frame': 'camera_infra2_frame',
        'map_frame': 'map',
        'fixed_frame': 'odom',
        'odom_frame': 'odom',
        'base_frame': 'base_link',
        #these two does not have reference
        'current_smooth_frame': 'base_link_smooth',
        'current_rectified_frame': 'base_link_rectified',
        'enable_observations_view': True,
        'enable_landmarks_view': True,
        'enable_reading_slam_internals': True,
        'enable_slam_visualization': True,
        'enable_localization_n_mapping': True,
        'use_sim_time': use_sim_time
    }]
)
visual_slam_launch_container = ComposableNodeContainer(
    name='visual_slam_launch_container',
    namespace='',
    package='rclcpp_components',
    executable='component_container',
    composable_node_descriptions=[
        visual_slam_node
    ],
    output='screen'
)
#now vslam should be set

return LaunchDescription(
    [
        #this lacks the realsense node, would add it in later(on the above)
        #here we don't run the realsense at edge
        #realsense_container,
        #adding TF joints to the camera and baselink
        #base_link_tf_node,
        #loading args of params file
        DeclareLaunchArgument(
            'params_file', default_value=param_dir,
            description='Full path to param file to load'),
            
            
        #is it simulation, in our application it is not, it is false
        DeclareLaunchArgument(
            'use_sim_time', default_value='False',
            description='Use simulation (Omniverse Isaac Sim) clock if true'),
        #rviz args, leave it
        DeclareLaunchArgument(
            'run_rviz', default_value='true',
            description='Whether to start RVIZ'),
            
            
        #running map_server
        DeclareLaunchArgument(
            'run_nav2', default_value='true',
            description='Whether to run nav2'),
        Node(
            package='nav2_lifecycle_manager', executable='lifecycle_manager',
            name='lifecycle_manager_map', output='screen',
            parameters=[{'use_sim_time': use_sim_time},
                        {'autostart': True},
                        {'node_names': lifecycle_nodes}],
            condition=IfCondition(
                LaunchConfiguration('run_nav2'))
        ),
        
        	#bringup rviz
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                os.path.join(nav2_bringup_launch_dir, 'rviz_launch.py')),
            launch_arguments={'namespace': '', 'use_namespace': 'False',
                              'autostart': 'True',
                              'rviz_config': rviz_config_dir}.items(),
            condition=IfCondition(LaunchConfiguration('run_rviz')),),
            
            #setting up nav2 stack and bring up, points to carter_nav2_new.yaml
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                os.path.join(
                    nav2_bringup_launch_dir, 'navigation_launch.py')),
            launch_arguments={'params_file': param_dir,
            		   'use_sim_time': use_sim_time,
                              'autostart': 'True'}.items(),
            condition=IfCondition(
                LaunchConfiguration('run_nav2'))
        ),
        
        
        #bringing up nvblox, modify the camera input
        Node(
            package='nvblox_ros', executable='nvblox_node',
            parameters=[nvblox_param_dir, {'use_sim_time': use_sim_time,
                                           'global_frame': 'odom'}],
            output='screen',
            remappings=[('depth/image', '/camera/realsense_splitter_node/output/depth'),
                        ('depth/camera_info', '/camera/depth/camera_info'),
                        ('color/image', '/camera/color/image_raw'),
                        ('color/camera_info', '/camera/color/camera_info'), ]),
        #bringing up vslam
        visual_slam_launch_container,
        

    ])

carter_nav2_new.yaml
(for this I only modified the tolerance and threshold to fit my own robot, the map form or name didn't changed)

bt_navigator:
ros__parameters:
use_sim_time: True
global_frame: map
robot_base_frame: base_link
odom_topic: /odom
bt_loop_duration: 20
default_server_timeout: 40
# '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_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_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_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_drive_on_heading_cancel_bt_node

bt_navigator_rclcpp_node:
ros__parameters:
use_sim_time: True

controller_server:
ros__parameters:
use_sim_time: True
controller_frequency: 10.0
min_x_velocity_threshold: 0.001
min_y_velocity_threshold: 0.05
min_theta_velocity_threshold: 0.001
#change the failure_tolerance for tunning
failure_tolerance: 2
progress_checker_plugin: "progress_checker"
goal_checker_plugin: ["general_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
general_goal_checker:
  plugin: "nav2_controller::SimpleGoalChecker"
  #changed goal tolerance for tunning
  xy_goal_tolerance: 0.4
  yaw_goal_tolerance: 0.4
  stateful: True
# 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.2
  max_vel_y: 0.0
  max_vel_theta: 0.2
  min_speed_xy: 0.0
  max_speed_xy: 0.2
  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: 1.2
  decel_lim_x: -2.5
  decel_lim_y: 0.0
  decel_lim_theta: -1.2
  vx_samples: 30
  vy_samples: 10
  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: 1.0
  RotateToGoal.lookahead_time: -1.0

controller_server_rclcpp_node:
ros__parameters:
use_sim_time: True

local_costmap:
local_costmap:
ros__parameters:
update_frequency: 10.0
publish_frequency: 10.0
global_frame: map
robot_base_frame: base_link
use_sim_time: True
rolling_window: True
width: 10
height: 10
resolution: 0.05
robot_radius: 0.4
plugins: ["nvblox_layer"]
nvblox_layer:
plugin: "nvblox::nav2::NvbloxCostmapLayer"
enabled: True
max_obstacle_distance: 1.0
inflation_distance: 0.4
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: 10.0
publish_frequency: 10.0
global_frame: map
robot_base_frame: base_link
use_sim_time: True
rolling_window: True
width: 200
height: 200
robot_radius: 0.4
resolution: 0.1
origin_x: -100.0
origin_y: -100.0
plugins: ["nvblox_layer"]
nvblox_layer:
plugin: "nvblox::nav2::NvbloxCostmapLayer"
enabled: True
max_obstacle_distance: 1.0
inflation_distance: 0.4
always_send_full_costmap: True
global_costmap_client:
ros__parameters:
use_sim_time: True
global_costmap_rclcpp_node:
ros__parameters:
use_sim_time: True

planner_server:
ros__parameters:
expected_planner_frequency: 10.0
use_sim_time: True
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: True

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

behavior_server:
ros__parameters:
costmap_topic: local_costmap/costmap_raw
footprint_topic: local_costmap/published_footprint
cycle_frequency: 5.0
behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"]
spin:
plugin: "nav2_behaviors/Spin"
backup:
plugin: "nav2_behaviors/BackUp"
drive_on_heading:
plugin: "nav2_behaviors/DriveOnHeading"
wait:
plugin: "nav2_behaviors/Wait"
global_frame: map
robot_base_frame: base_link
transform_tolerance: 0.2
use_sim_time: true
simulate_ahead_time: 2.0
max_rotational_vel: 0.2
min_rotational_vel: 0.05
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

has anyone ran into this problem? and any advises are appreciated.
Thanks

[chivas1000]

Sorry, the problem should be in my modifided carter_nav2.yaml that I tweaked with tolerance time and radius and acc_lim, or maybe some syntax problem, Since I use default yaml the map successfully received.

[helenol]

Since the default yaml is working, I'm not sure how we can help here. Nothing you've changed stands out at something that should cause this issue, but this seems like a nav2 issue rather than an nvblox one.
Sorry we couldn't help more!