Bt_navigator stuck waiting for "spin" action server in rosject Section III

anaam.mostafiz · August 23, 2023, 12:40am

For the ROS2 Navigation rosject Section III, I launched localization, set the 2D Pose Estimate in rviz, and then launched path planning. At first, the bt_navigator would fail and the process would abort, so I changed my bt_navigator.yaml and behavior.xml files to account for Foxy, according to this forum discussion.

That fixed that error, but now it appears the bt_navigator is continously waiting for “spin” action server when path planning is launched. If I try to set the 2D Goal Pose, nothing happens. What should I do?

Terminal output:

ros2 launch project_path_planning pathplanner.launch.py
[INFO] [launch]: All log files can be found below /home/user/.ros/log/2023-08-23-00-09-52-120502-1_xterm-8623
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [planner_server-1]: process started with pid [8648]
[INFO] [controller_server-2]: process started with pid [8650]
[INFO] [bt_navigator-3]: process started with pid [8652]
[INFO] [recoveries_server-4]: process started with pid [8656]
[INFO] [lifecycle_manager-5]: process started with pid [8672]
[planner_server-1] [INFO] [1692749392.807985160] [planner_server]:
[planner_server-1]      planner_server lifecycle node launched.
[planner_server-1]      Waiting on external lifecycle transitions to activate
[planner_server-1]      See https://design.ros2.org/articles/node_lifecycle.html for more information.
[planner_server-1] [INFO] [1692749392.811292995] [planner_server]: Creating
[bt_navigator-3] [INFO] [1692749392.907553431] [bt_navigator]:
[bt_navigator-3]        bt_navigator lifecycle node launched.
[bt_navigator-3]        Waiting on external lifecycle transitions to activate
[bt_navigator-3]        See https://design.ros2.org/articles/node_lifecycle.html for more information.
[bt_navigator-3] [INFO] [1692749392.907719746] [bt_navigator]: Creating
[controller_server-2] [INFO] [1692749392.917745877] [controller_server]:
[controller_server-2]   controller_server lifecycle node launched.
[controller_server-2]   Waiting on external lifecycle transitions to activate
[controller_server-2]   See https://design.ros2.org/articles/node_lifecycle.html for more information.
[controller_server-2] [INFO] [1692749393.005446987] [controller_server]: Creating controller server
[recoveries_server-4] [INFO] [1692749393.019974768] [recoveries_server]:
[recoveries_server-4]   recoveries_server lifecycle node launched.
[recoveries_server-4]   Waiting on external lifecycle transitions to activate
[recoveries_server-4]   See https://design.ros2.org/articles/node_lifecycle.html for more information.
[planner_server-1] [INFO] [1692749393.048721467] [global_costmap.global_costmap]:
[planner_server-1]      global_costmap lifecycle node launched.
[planner_server-1]      Waiting on external lifecycle transitions to activate
[planner_server-1]      See https://design.ros2.org/articles/node_lifecycle.html for more information.
[planner_server-1] [INFO] [1692749393.049540410] [global_costmap.global_costmap]: Creating Costmap
[lifecycle_manager-5] [INFO] [1692749393.121145959] [lifecycle_manager_planning]: Creating
[lifecycle_manager-5] [INFO] [1692749393.322029088] [lifecycle_manager_planning]: Creating and initializing lifecycle service clients
[controller_server-2] [INFO] [1692749393.419480903] [local_costmap.local_costmap]:
[controller_server-2]   local_costmap lifecycle node launched.
[controller_server-2]   Waiting on external lifecycle transitions to activate
[controller_server-2]   See https://design.ros2.org/articles/node_lifecycle.html for more information.
[controller_server-2] [INFO] [1692749393.420740116] [local_costmap.local_costmap]: Creating Costmap
[lifecycle_manager-5] [INFO] [1692749393.435825725] [lifecycle_manager_planning]: Starting managed nodes bringup...
[lifecycle_manager-5] [INFO] [1692749393.435866065] [lifecycle_manager_planning]: Configuring planner_server
[planner_server-1] [INFO] [1692749393.504490104] [planner_server]: Configuring
[planner_server-1] [INFO] [1692749393.504562423] [global_costmap.global_costmap]: Configuring
[planner_server-1] [INFO] [1692749393.510602632] [global_costmap.global_costmap]: Using plugin "static_layer"
[planner_server-1] [INFO] [1692749393.519691240] [global_costmap.global_costmap]: Subscribing to the map topic (/map) with transient local durability
[planner_server-1] [INFO] [1692749393.522738747] [global_costmap.global_costmap]: Initialized plugin "static_layer"
[planner_server-1] [INFO] [1692749393.523783425] [global_costmap.global_costmap]: Using plugin "obstacle_layer"
[planner_server-1] [INFO] [1692749393.526735545] [global_costmap.global_costmap]: Subscribed to Topics:
[planner_server-1] [INFO] [1692749393.527297117] [global_costmap.global_costmap]: Initialized plugin "obstacle_layer"
[planner_server-1] [INFO] [1692749393.527805262] [global_costmap.global_costmap]: Using plugin "inflation_layer"
[planner_server-1] [INFO] [1692749393.530311382] [global_costmap.global_costmap]: Initialized plugin "inflation_layer"
[planner_server-1] [INFO] [1692749393.533160479] [global_costmap.global_costmap]: StaticLayer: Resizing costmap to 57 X51 at 0.050000 m/pix
[planner_server-1] [INFO] [1692749393.705563547] [planner_server]: Created global planner plugin GridBased of type nav2_navfn_planner/NavfnPlanner
[planner_server-1] [INFO] [1692749393.705629197] [planner_server]: Configuring plugin GridBased of type NavfnPlanner
[planner_server-1] [INFO] [1692749393.734080716] [planner_server]: Planner Server has GridBased  planners available.
[lifecycle_manager-5] [INFO] [1692749393.819738253] [lifecycle_manager_planning]: Configuring controller_server
[controller_server-2] [INFO] [1692749393.821186229] [controller_server]: Configuring controller interface
[controller_server-2] [INFO] [1692749393.821344813] [controller_server]: Controller frequency set to 10.0000Hz
[controller_server-2] [INFO] [1692749393.821379795] [local_costmap.local_costmap]: Configuring
[controller_server-2] [INFO] [1692749393.829523069] [local_costmap.local_costmap]: Using plugin "static_layer"
[controller_server-2] [INFO] [1692749393.837697769] [local_costmap.local_costmap]: Subscribing to the map topic (/map) with transient local durability
[controller_server-2] [INFO] [1692749393.906454567] [local_costmap.local_costmap]: Initialized plugin "static_layer"
[controller_server-2] [INFO] [1692749393.906511985] [local_costmap.local_costmap]: Using plugin "obstacle_layer"
[controller_server-2] [INFO] [1692749393.911151189] [local_costmap.local_costmap]: Subscribed to Topics:
[controller_server-2] [INFO] [1692749393.911253323] [local_costmap.local_costmap]: Initialized plugin "obstacle_layer"
[controller_server-2] [INFO] [1692749393.911282224] [local_costmap.local_costmap]: Using plugin "inflation_layer"
[controller_server-2] [INFO] [1692749393.913902801] [local_costmap.local_costmap]: Initialized plugin "inflation_layer"
[controller_server-2] [INFO] [1692749393.920488069] [local_costmap.local_costmap]: StaticLayer: Resizing costmap to 57 X 51 at 0.050000 m/pix
[controller_server-2] [INFO] [1692749393.944730148] [controller_server]: Created progress_checker : progress_checker oftype nav2_controller::SimpleProgressChecker
[controller_server-2] [INFO] [1692749393.945422602] [controller_server]: Created goal_checker : goal_checker of type nav2_controller::SimpleGoalChecker
[controller_server-2] [INFO] [1692749394.011760147] [controller_server]: Created controller : FollowPath of type dwb_core::DWBLocalPlanner
[controller_server-2] [INFO] [1692749394.015263375] [controller_server]: Setting transform_tolerance to 0.200000
[controller_server-2] [INFO] [1692749394.104384976] [controller_server]: Using critic "RotateToGoal" (dwb_critics::RotateToGoalCritic)
[controller_server-2] [INFO] [1692749394.104578019] [controller_server]: Criticplugin initialized
[controller_server-2] [INFO] [1692749394.104651139] [controller_server]: Using critic "Oscillation" (dwb_critics::OscillationCritic)
[controller_server-2] [INFO] [1692749394.104777816] [controller_server]: Criticplugin initialized
[controller_server-2] [INFO] [1692749394.104839555] [controller_server]: Using critic "BaseObstacle" (dwb_critics::BaseObstacleCritic)
[controller_server-2] [INFO] [1692749394.104901079] [controller_server]: Criticplugin initialized
[controller_server-2] [INFO] [1692749394.104954281] [controller_server]: Using critic "GoalAlign" (dwb_critics::GoalAlignCritic)
[controller_server-2] [INFO] [1692749394.105087894] [controller_server]: Criticplugin initialized
[controller_server-2] [INFO] [1692749394.105148252] [controller_server]: Using critic "PathAlign" (dwb_critics::PathAlignCritic)
[controller_server-2] [INFO] [1692749394.105251807] [controller_server]: Criticplugin initialized
[controller_server-2] [INFO] [1692749394.105317257] [controller_server]: Using critic "PathDist" (dwb_critics::PathDistCritic)
[controller_server-2] [INFO] [1692749394.105421765] [controller_server]: Criticplugin initialized
[controller_server-2] [INFO] [1692749394.105476413] [controller_server]: Using critic "GoalDist" (dwb_critics::GoalDistCritic)
[controller_server-2] [INFO] [1692749394.105581424] [controller_server]: Criticplugin initialized
[controller_server-2] [INFO] [1692749394.105606622] [controller_server]: Controller Server has FollowPath  controllers available.
[lifecycle_manager-5] [INFO] [1692749394.227040671] [lifecycle_manager_planning]: Configuring bt_navigator
[bt_navigator-3] [INFO] [1692749394.227412866] [bt_navigator]: Configuring
[bt_navigator-3] [INFO] [1692749394.508170917] [bt_navigator_rclcpp_node]: Waiting for "compute_path_to_pose" action server
[bt_navigator-3] [INFO] [1692749394.508532506] [bt_navigator_rclcpp_node]: "ComputePathToPose" BtActionNode initialized
[bt_navigator-3] [INFO] [1692749394.510834218] [bt_navigator_rclcpp_node]: Waiting for "global_costmap/clear_entirely_global_costmap" service
[bt_navigator-3] [INFO] [1692749394.511477779] [bt_navigator_rclcpp_node]: "ClearGlobalCostmap-Context" BtServiceNode initialized
[bt_navigator-3] [INFO] [1692749394.523551124] [bt_navigator_rclcpp_node]: Waiting for "follow_path" action server
[bt_navigator-3] [INFO] [1692749394.523751525] [bt_navigator_rclcpp_node]: "FollowPath" BtActionNode initialized
[bt_navigator-3] [INFO] [1692749394.528367190] [bt_navigator_rclcpp_node]: Waiting for "local_costmap/clear_entirely_local_costmap" service
[bt_navigator-3] [INFO] [1692749394.528554760] [bt_navigator_rclcpp_node]: "ClearLocalCostmap-Context" BtServiceNode initialized
[bt_navigator-3] [INFO] [1692749394.532585051] [bt_navigator_rclcpp_node]: Waiting for "local_costmap/clear_entirely_local_costmap" service
[bt_navigator-3] [INFO] [1692749394.532791306] [bt_navigator_rclcpp_node]: "ClearLocalCostmap-Subtree" BtServiceNode initialized
[bt_navigator-3] [INFO] [1692749394.536389477] [bt_navigator_rclcpp_node]: Waiting for "global_costmap/clear_entirely_global_costmap" service
[bt_navigator-3] [INFO] [1692749394.536744183] [bt_navigator_rclcpp_node]: "ClearGlobalCostmap-Subtree" BtServiceNode initialized
[bt_navigator-3] [INFO] [1692749394.606288659] [bt_navigator_rclcpp_node]: Waiting for "spin" action server

bt_navigator.yaml:

# Foxy Version
bt_navigator:
  ros__parameters:
    use_sim_time: True
    global_frame: map
    robot_base_frame: base_footprint # base_link
    odom_topic: /odom
    enable_groot_monitoring: True
    groot_zmq_publisher_port: 1666
    groot_zmq_server_port: 1667
    default_bt_xml_filename: "/home/user/ros2_ws/src/project_path_planning/config/behavior.xml"
    plugin_lib_names:
    - nav2_compute_path_to_pose_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

behavior.xml:

<!--
  This Behavior Tree replans the global path periodically at 1 Hz, and has
  recovery actions. Obtained from the official Nav2 package
-->


<!-- Foxy Version -->
<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
      <PipelineSequence name="NavigateWithReplanning">
        <RateController hz="1.0">
          <RecoveryNode number_of_retries="1" name="ComputePathToPose">
            <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
            <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
          </RecoveryNode>
        </RateController>
        <RecoveryNode number_of_retries="1" name="FollowPath">
          <FollowPath path="{path}" controller_id="FollowPath"/>
          <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
        </RecoveryNode>
      </PipelineSequence>
      <ReactiveFallback name="RecoveryFallback">
        <GoalUpdated/>
        <SequenceStar name="RecoveryActions">
          <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
          <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
          <Spin spin_dist="1.57"/>
          <Wait wait_duration="5"/>
        </SequenceStar>
      </ReactiveFallback>
    </RecoveryNode>
  </BehaviorTree>
</root>

pathplanner.launch.py:

from launch import LaunchDescription
from launch_ros.actions import Node
import os
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():

    nav2_yaml = os.path.join(get_package_share_directory('project_path_planning'),'config','planner_server.yaml')
    controller_yaml = os.path.join(get_package_share_directory('project_path_planning'),'config','controller.yaml')
    bt_navigator_yaml = os.path.join(get_package_share_directory('project_path_planning'),'config','bt_navigator.yaml')
    recovery_yaml = os.path.join(get_package_share_directory('project_path_planning'),'config','recovery.yaml')

    return LaunchDescription([
    
        Node(
            package='nav2_planner',
            executable='planner_server',
            name='planner_server',
            output='screen',
            parameters=[nav2_yaml]
        ),

        Node(
            package='nav2_controller',
            executable='controller_server',
            name = 'controller_server',
            output='screen',
            parameters=[controller_yaml]
        ),

        Node(
            package='nav2_bt_navigator',
            executable='bt_navigator',
            name='bt_navigator',
            output='screen',
            parameters=[bt_navigator_yaml]
        ),

        Node(
            package='nav2_recoveries',
            executable='recoveries_server',
            name='recoveries_server',
            parameters=[recovery_yaml],
            output='screen'
        ),

        Node(
            package='nav2_lifecycle_manager',
            executable='lifecycle_manager',
            name='lifecycle_manager_planning',
            output='screen',
            parameters=[{'use_sim_time': True},
                        {'autostart': True},
                        {'node_names': ['planner_server','controller_server','bt_navigator','recoveries_server']}]
        ),
    
    ])

setup.py:

from setuptools import setup
import os
from glob import glob

package_name = 'project_path_planning'

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
        (os.path.join('share', package_name, 'config'), glob('config/*')),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='user',
    maintainer_email='user@todo.todo',
    description='TODO: Package description',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
        ],
    },
)

rtellez · August 23, 2023, 12:40pm

Hi Anaam,
the error message indicates that the bt_navigator is being configured by the lifecycle_manager_planning, but it never achieves to end the configuration because it is waiting for a spin action server.

That spin is provided by the recoveries_server node. The problem is that, given the order you have indicated in the pathplanner.launch.py launch file (at the lifecycle_manager_planning section), you are indicating that the lifecycle_manager_planning has to initialize the recoveries_server only after the bt_navigator has finished initialization.

Since the bt_navigator needs the recoveries_server to be ready before starting, you have caused a deadlock situation (the bt_navigator cannot finish initialization until recoveries_server initializes, and recoveries_server cannot initialize until bt_behavior finishes).

The solution is very simple: change the order of the recoveries_server putting it before the bt_navigator in the lifecycle_manager_planning node launch config.

Remember always that order matters in the list of nodes that the lifecycle_manager has to manage

system · August 28, 2023, 12:41pm

This topic was automatically closed 5 days after the last reply. New replies are no longer allowed.