commander API hangs with amcl/get_state service not available

[david-wb]

Bug report

Required Info:

• Operating System:
  • Ubuntu 22.04
• ROS2 Version:
  • Iron
• Version or commit hash:
  • 1.2.6
• DDS implementation:
  • I don't know. How do I check?

Steps to reproduce issue

I am launching a vanila nav2 stack with ros-ion/gazebo-fortress. Everything starts up properly and I can run waypoint following via Rviz. However, when I try to run waypoint following using the commander API it complains that the amcl/get_state service is unavailable.

Here is what I am doing with the commander API.

nav = BasicNavigator()
nav.waitUntilNav2Active()
p = PoseStamped()
p.pose.position.x = 1
p.pose.position.y = 1
p.pose.position.z = 0
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1
nav.followWaypoints([p])

Here are my nav2 params:

amcl:
  ros__parameters:
    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

bt_navigator:
  ros__parameters:
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    bt_loop_duration: 10
    default_server_timeout: 20
    action_server_result_timeout: 900.0
    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: 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_plugins: ["progress_checker"]
    goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
    controller_plugins: ["FollowPath"]
    use_realtime_priority: false

    # 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.26
      max_vel_y: 0.0
      max_vel_theta: 1.0
      min_speed_xy: 0.0
      max_speed_xy: 0.26
      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

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      robot_radius: 0.22
      plugins: ["static_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      always_send_full_costmap: True

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      robot_radius: 0.22
      resolution: 0.3
      track_unknown_space: true
      plugins: ["static_layer", "inflation_layer"]
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
        enabled: true
        subscribe_to_updates: true
        transform_tolerance: 0.1
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55
      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: "config/map.yaml"

map_saver:
  ros__parameters:
    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
    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: 10.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_link
    transform_tolerance: 0.1
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: 0.4
    rotational_acc_lim: 3.2

waypoint_follower:
  ros__parameters:
    loop_rate: 20
    stop_on_failure: false
    action_server_result_timeout: 900.0
    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: 20.0
    scale_velocities: False
    feedback: "OPEN_LOOP"
    max_velocity: [0.26, 0.0, 1.0]
    min_velocity: [-0.26, 0.0, -1.0]
    max_accel: [2.5, 0.0, 3.2]
    max_decel: [-2.5, 0.0, -3.2]
    odom_topic: "odom"
    odom_duration: 0.1
    deadband_velocity: [0.0, 0.0, 0.0]
    velocity_timeout: 1.0

lifecycle_manager:
  ros__parameters:
    autostart: true
    node_names:
      [
        "controller_server",
        "planner_server",
        "behavior_server",
        "bt_navigator",
        "waypoint_follower",
      ]
    bond_timeout: 4.0
    attempt_respawn_reconnection: true
    bond_respawn_max_duration: 10.0

And here is my launch.py

import os

import xacro
from ament_index_python.packages import \
    get_package_share_directory  # type: ignore
from launch_ros.actions import Node

from launch import LaunchDescription
from launch.actions import (EmitEvent, ExecuteProcess,
                            IncludeLaunchDescription, LogInfo,
                            RegisterEventHandler, TimerAction)
from launch.event_handlers import OnProcessExit, OnProcessIO, OnProcessStart
from launch.events import Shutdown
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import EnvironmentVariable

def generate_launch_description():  # type: ignore
    pkg_share = get_package_share_directory("namoros")
    model_path = os.path.join(pkg_share, "..")
    plugin_path = get_package_share_directory("namoros")
    default_rviz_config_path = os.path.join(pkg_share, "nav2_default_view.rviz")

    gazebo = ExecuteProcess(
        cmd=["ign", "gazebo", "--render-engine", "ogre", "-v", "3", "namo_world.sdf"],
        output="screen",
        additional_env={
            "GZ_SIM_SYSTEM_PLUGIN_PATH": plugin_path,
            "IGN_GAZEBO_RESOURCE_PATH": model_path,
        },
    )
    namoros = Node(
        package="namoros",
        namespace="namoros",
        executable="namoros",
        name="namoros_main",
    )

    # Process the URDF file
    pkg_path = os.path.join(get_package_share_directory("namoros"))
    xacro_file = os.path.join(pkg_path, "models/differential_robot/model.urdf")
    robot_description_config = xacro.process_file(xacro_file)  # type: ignore
    # Create a robot_state_publisher node
    params = {  # type: ignore
        "robot_description": robot_description_config.toxml(),  # type: ignore
        "use_sim_time": False,
    }
    node_robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        output="screen",
        parameters=[params],  # type: ignore
        arguments=[model_path],
    )
    # Run the spawner node from the gazebo_ros package. The entity name doesn't really matter if you only have a single robot.
    spawn_entity = Node(
        package="ros_gz_sim",
        executable="create",
        arguments=["-topic", "robot_description", "-entity", "namoros", "-z", "2"],
        output="screen",
    )
    rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="screen",
        arguments=["-d", default_rviz_config_path],
    )
    static_tf = Node(
        package="tf2_ros",
        executable="static_transform_publisher",
        name="static_transform_publisher",
        output="screen",
        arguments=["--frame-id", "map", "--child-frame-id", "odom"],
    )
    bringup_share = get_package_share_directory("nav2_bringup")
    bringup = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(bringup_share + "/launch/bringup_launch.py"),
        launch_arguments={
            "autostart": "True",
            "params_file": os.path.join(pkg_share, "config/nav2_params.yaml"),
            #'map': os.path.join(pkg_share, "config/map.yaml"),
        }.items(),
    )
    return LaunchDescription(
        [
            gazebo,
            Node(
                package="ros_gz_bridge",
                executable="parameter_bridge",
                arguments=[
                    "--ros-args",
                    "-p",
                    "config_file:=ros_gz_bridge.yaml",
                ],
                output="screen",
            ),
            RegisterEventHandler(
                OnProcessIO(
                    target_action=gazebo,
                    on_stdout=lambda event: LogInfo(
                        msg='Gazebo: "{}"'.format(event.text.decode().strip())
                    ),
                )
            ),
            RegisterEventHandler(
                OnProcessStart(
                    target_action=gazebo,
                    on_start=[
                        LogInfo(msg="Gazebo started, starting namoros"),
                        TimerAction(period=5.0, actions=[namoros]),
                    ],
                )
            ),
            RegisterEventHandler(
                OnProcessExit(
                    target_action=gazebo,
                    on_exit=[
                        LogInfo(
                            msg=(
                                EnvironmentVariable(name="USER"),
                                " closed the Gazebo window",
                            )
                        ),
                        EmitEvent(event=Shutdown(reason="Window closed")),
                    ],
                )
            ),
            node_robot_state_publisher,
            spawn_entity,
            static_tf,
            bringup,
            rviz_node,
        ]
    )  # type: ignore

Expected behavior

The commander API should initialize and robot should follow waypoints.

Actual behavior

The nav.waitForNav2Active() call hangs.

[namoros-8] [INFO] [1711542259.476309392] [namoros.namoros_main]: amcl/get_state service not available, waiting...
[namoros-8] [INFO] [1711542260.580838297] [namoros.namoros_main]: amcl/get_state service not available, waiting...
[namoros-8] [INFO] [1711542261.599272221] [namoros.namoros_main]: amcl/get_state service not available, waiting...
[namoros-8] [INFO] [1711542262.654907867] [namoros.namoros_main]: amcl/get_state service not available, waiting...
[namoros-8] [INFO] [1711542263.684143388] [namoros.namoros_main]: amcl/get_state service not available, waiting...

Additional information

The AMCL node is active:

$ ros2 lifecycle get /amcl
active [3]

I can call the /amcl/get_state service manually, but it takes a few seconds to respond:

$ ros2 service call /amcl/get_state lifecycle_msgs/srv/GetState
requester: making request: lifecycle_msgs.srv.GetState_Request()

response:
lifecycle_msgs.srv.GetState_Response(current_state=lifecycle_msgs.msg.State(id=3, label='active'))

If I comment out the call to nav.waitForNav2Active(), then it hangs at

[namoros-8] [INFO] [1711544599.059104336] [namoros.namoros_main]: 'FollowWaypoints' action server not available, waiting...

I gets even weirder. I can send waypoints using the ros2 cli. So this definitely seems to be an issue with the commander API itself.

$ ros2 action send_goal /follow_waypoints nav2_msgs/action/FollowWaypoints "{number_of_loops: 1, goal_index: 0, poses: [{header: {stamp: {sec: 0, nanosec: 0}, frame_id: 'map'}, pose: {position: {x: 0.0, y: 0.0, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}]}"
Waiting for an action server to become available...
Sending goal:
     number_of_loops: 1
goal_index: 0
poses:
- header:
    stamp:
      sec: 0
      nanosec: 0
    frame_id: map
  pose:
    position:
      x: 0.0
      y: 0.0
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0

Goal accepted with ID: 5dc48dd3d07d4df79ae11bc37da40826

[SteveMacenski]

Dont cross post with Stack Exchange.

[david-wb]

@SteveMacenski I posted here with additional context because I think there is a bug related to namespacing. Please read my comments in the "Additional Information" section above.

I think this line from the docs might have something to do with it.