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.

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

Nav2 Stalling on Multiple Robots? #4350

Closed
WallerJonathan opened this issue May 16, 2024 · 9 comments
Closed

Nav2 Stalling on Multiple Robots? #4350

WallerJonathan opened this issue May 16, 2024 · 9 comments

Comments

@WallerJonathan
Copy link

Bug report

Required Info:

  • Operating System:
    • Ubuntu 22.04 Jammy Jellyfish
  • ROS2 Version:
    • ROS2 Humble
  • Version or commit hash:
    • Latest Version
  • DDS implementation:
    • Default

Steps to reproduce issue

This is the code I am currently looking at (everything else in the file is not run yet)

def main():
    rclpy.init()

    naviturt = BasicNavigator()

    # Set initial pose
    xStart = float(input("Starting x position (float): "))
    yStart = float(input("Starting y position (float): "))
    init_pose = naviturt.getPoseStamped([xStart, yStart], TurtleBot4Directions.NORTH)
    naviturt.setInitialPose(init_pose)

    # Wait until Nav2 starts up
    naviturt.waitUntilNav2Active()
  1. Make into a build
  2. Use namespacing to make the first robot (I am using the turtlebot4 gazebo simulator for this) ros2 launch turtlebot4_ignition_bringup turtlebot4_ignition.launch.py namespace:=/robot1 localization:=true rviz:=true
  3. Use namespacing to make the second robot (again I am using the turtlebot4 gazebo simulator for this) ros2 launch turtlebot4_ignition_bringup turtlebot4_spawn.launch.py namespace:=/robot2 localization:=true rviz:=true
  4. Run the build file; I ran ros2 run my_nav my_nav_py_node --ros-args -r __ns:=/robot2

Expected behavior

I expect the second robot to wait until nav2 is active, then start with the tasks

Actual behavior

It freezes up while waiting for Nav2. The output runs forever unless you use a keyboard interrupt:

Starting y position (float): 0
[INFO] [1715887737.224566729] [robot2.basic_navigator]: Publishing Initial Pose
[INFO] [1715887740.263714789] [robot2.basic_navigator]: bt_navigator/get_state service not available, waiting...
[INFO] [1715887741.267756369] [robot2.basic_navigator]: bt_navigator/get_state service not available, waiting...
[INFO] [1715887742.272033874] [robot2.basic_navigator]: bt_navigator/get_state service not available, waiting...
[INFO] [1715887743.276700730] [robot2.basic_navigator]: bt_navigator/get_state service not available, waiting...
[INFO] [1715887744.280826984] [robot2.basic_navigator]: bt_navigator/get_state service not available, waiting...
[INFO] [1715887745.285144201] [robot2.basic_navigator]: bt_navigator/get_state service not available, waiting...
[INFO] [1715887746.289668470] [robot2.basic_navigator]: bt_navigator/get_state service not available, waiting...
[INFO] [1715887747.293806072] [robot2.basic_navigator]: bt_navigator/get_state service not available, waiting...
[INFO] [1715887748.298023824] [robot2.basic_navigator]: bt_navigator/get_state service not available, waiting...
[INFO] [1715887749.301946025] [robot2.basic_navigator]: bt_navigator/get_state service not available, waiting...
[INFO] [1715887750.306116626] [robot2.basic_navigator]: bt_navigator/get_state service not available, waiting...
[INFO] [1715887751.310751901] [robot2.basic_navigator]: bt_navigator/get_state service not available, waiting...
[INFO] [1715887752.315370022] [robot2.basic_navigator]: bt_navigator/get_state service not available, waiting...
[INFO] [1715887753.319347344] [robot2.basic_navigator]: bt_navigator/get_state service not available, waiting...
[INFO] [1715887754.324028083] [robot2.basic_navigator]: bt_navigator/get_state service not available, waiting...
^CTraceback (most recent call last):
  File "/home/biorobotics/turtlebot4_ws/install/my_nav/lib/my_nav/my_nav_py_node", line 33, in <module>
    sys.exit(load_entry_point('my-nav', 'console_scripts', 'my_nav_py_node')())
  File "/home/biorobotics/turtlebot4_ws/build/my_nav/my_nav/my_nav_py_node.py", line 72, in main
    naviturt.waitUntilNav2Active()
  File "/opt/ros/humble/lib/python3.10/site-packages/nav2_simple_commander/robot_navigator.py", line 316, in waitUntilNav2Active
    self._waitForNodeToActivate(navigator)
  File "/opt/ros/humble/lib/python3.10/site-packages/nav2_simple_commander/robot_navigator.py", line 537, in _waitForNodeToActivate
    while not state_client.wait_for_service(timeout_sec=1.0):
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/client.py", line 177, in wait_for_service
    time.sleep(sleep_time)
KeyboardInterrupt
[ros2run]: Interrupt

Additional information

I am thinking this may be a namespacing problem? The thing I can't figure out is why it seems to work sometimes with the first robot spawned but never the second. Also, I do not think this is a turtlebot4 specific problem, but if it is please let me know!

Thanks!

@SteveMacenski
Copy link
Member

SteveMacenski commented May 16, 2024

[robot2.basic_navigator]: bt_navigator/get_state service not available

Almost certainly. You can see that bt_navigator isn't namespaced. What if you pass into waitUntilNav2Active the namespaced navigator/localizer types? They are used in the implementation.


    def waitUntilNav2Active(self, navigator='bt_navigator', localizer='amcl'):
        """Block until the full navigation system is up and running."""
        if localizer != 'robot_localization':  # non-lifecycle node
            self._waitForNodeToActivate(localizer)
        if localizer == 'amcl':
            self._waitForInitialPose()
        self._waitForNodeToActivate(navigator)
        self.info('Nav2 is ready for use!')
        return

If that fixes things, then we could store the namespace in the constructor and use it here automatically. Should be easy:

self.namespace = namespace 

... 

 self._waitForNodeToActivate(self.namespace + '/' + localizer)
 self._waitForNodeToActivate(self.namespace + '/' + navigator)

@WallerJonathan
Copy link
Author

Thanks for the reply!

Unfortunately I don't think it worked (unless I did it wrong). Here is the modified code:

def main():
    rclpy.init()

    naviturt = BasicNavigator()

    # Set initial pose
    xStart = float(input("Starting x position (float): "))
    yStart = float(input("Starting y position (float): "))
    init_pose = naviturt.getPoseStamped([xStart, yStart], TurtleBot4Directions.NORTH)
    naviturt.setInitialPose(init_pose)

    # Wait until Nav2 starts up
    naviturt.waitUntilNav2Active(navigator='/robot2/bt_navigator', localizer='/robot2/amcl')

One of my runs gave me this as the terminal output:

ubuntu@unity:~/tb4_ws$ ros2 run my_nav my_nav_py_node --ros-args -r __ns:=/robot2
Starting x position (float): 0
Starting y position (float): 1
[INFO] [1715949631.921310175] [robot2.basic_navigator]: Publishing Initial Pose
[INFO] [1715949634.957696762] [robot2.basic_navigator]: /robot2/bt_navigator/get_state service not available, waiting...
[INFO] [1715949635.961778416] [robot2.basic_navigator]: /robot2/bt_navigator/get_state service not available, waiting...
[INFO] [1715949636.965818513] [robot2.basic_navigator]: /robot2/bt_navigator/get_state service not available, waiting...
[INFO] [1715949637.969824188] [robot2.basic_navigator]: /robot2/bt_navigator/get_state service not available, waiting...
[INFO] [1715949638.974061734] [robot2.basic_navigator]: /robot2/bt_navigator/get_state service not available, waiting...
[INFO] [1715949639.977766634] [robot2.basic_navigator]: /robot2/bt_navigator/get_state service not available, waiting...
[INFO] [1715949640.981943741] [robot2.basic_navigator]: /robot2/bt_navigator/get_state service not available, waiting...
[INFO] [1715949641.986084632] [robot2.basic_navigator]: /robot2/bt_navigator/get_state service not available, waiting...
[INFO] [1715949642.990370890] [robot2.basic_navigator]: /robot2/bt_navigator/get_state service not available, waiting...
[INFO] [1715949643.995334885] [robot2.basic_navigator]: /robot2/bt_navigator/get_state service not available, waiting...
[INFO] [1715949644.999524696] [robot2.basic_navigator]: /robot2/bt_navigator/get_state service not available, waiting...
^CTraceback (most recent call last):
  File "/home/ubuntu/tb4_ws/install/my_nav/lib/my_nav/my_nav_py_node", line 33, in <module>
    sys.exit(load_entry_point('my-nav', 'console_scripts', 'my_nav_py_node')())
  File "/home/ubuntu/tb4_ws/build/my_nav/my_nav/my_nav_py_node.py", line 72, in main
    naviturt.waitUntilNav2Active(navigator='/robot2/bt_navigator', localizer='/robot2/amcl')
  File "/opt/ros/humble/lib/python3.10/site-packages/nav2_simple_commander/robot_navigator.py", line 316, in waitUntilNav2Active
    self._waitForNodeToActivate(navigator)
  File "/opt/ros/humble/lib/python3.10/site-packages/nav2_simple_commander/robot_navigator.py", line 537, in _waitForNodeToActivate
    while not state_client.wait_for_service(timeout_sec=1.0):
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/client.py", line 177, in wait_for_service
    time.sleep(sleep_time)
KeyboardInterrupt
[ros2run]: Interrupt

I ran the exact same commands again and I got a slightly different output that looks like this:

ubuntu@unity:~/tb4_ws$ ros2 run my_nav my_nav_py_node --ros-args -r __ns:=/robot2
Starting x position (float): 0
Starting y position (float): 1
[INFO] [1715950555.372274157] [robot2.basic_navigator]: Publishing Initial Pose
[INFO] [1715950556.437064355] [robot2.basic_navigator]: /robot2/amcl/get_state service not available, waiting...
[INFO] [1715950557.439939156] [robot2.basic_navigator]: /robot2/amcl/get_state service not available, waiting...
[INFO] [1715950561.209691728] [robot2.basic_navigator]: robot2/bt_navigator/get_state service not available, waiting...
[INFO] [1715950562.213891973] [robot2.basic_navigator]: robot2/bt_navigator/get_state service not available, waiting...
[INFO] [1715950563.218917243] [robot2.basic_navigator]: robot2/bt_navigator/get_state service not available, waiting...
[INFO] [1715950564.223296188] [robot2.basic_navigator]: robot2/bt_navigator/get_state service not available, waiting...
[INFO] [1715950565.227448403] [robot2.basic_navigator]: robot2/bt_navigator/get_state service not available, waiting...
^CTraceback (most recent call last):
  File "/home/ubuntu/tb4_ws/install/my_nav/lib/my_nav/my_nav_py_node", line 33, in <module>
    sys.exit(load_entry_point('my-nav', 'console_scripts', 'my_nav_py_node')())
  File "/home/ubuntu/tb4_ws/build/my_nav/my_nav/my_nav_py_node.py", line 72, in main
    naviturt.waitUntilNav2Active(navigator='/robot2/bt_navigator', localizer='/robot2/amcl')
  File "/opt/ros/humble/lib/python3.10/site-packages/nav2_simple_commander/robot_navigator.py", line 316, in waitUntilNav2Active
    self._waitForNodeToActivate(navigator)
  File "/opt/ros/humble/lib/python3.10/site-packages/nav2_simple_commander/robot_navigator.py", line 537, in _waitForNodeToActivate
    while not state_client.wait_for_service(timeout_sec=1.0):
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/client.py", line 177, in wait_for_service
    time.sleep(sleep_time)
KeyboardInterrupt
[ros2run]: Interrupt

In addition, I tried just running the nav2 that is provided with turtlebot4 and gives a similar error, although not the same:

ubunutu@unity:~/tb4_ws$ ros2 launch turtlebot4_navigation nav2.launch.py namespace:=robot2
[INFO] [launch]: All log files can be found below /home/biorobotics/.ros/log/2024-05-17-08-41-43-502371-unity-22609
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [controller_server-1]: process started with pid [22610]
[INFO] [smoother_server-2]: process started with pid [22612]
[INFO] [planner_server-3]: process started with pid [22614]
[INFO] [behavior_server-4]: process started with pid [22616]
[INFO] [bt_navigator-5]: process started with pid [22618]
[INFO] [waypoint_follower-6]: process started with pid [22620]
[INFO] [velocity_smoother-7]: process started with pid [22622]
[INFO] [lifecycle_manager-8]: process started with pid [22624]
[waypoint_follower-6] [INFO] [1715949704.072797658] [robot2.waypoint_follower]: 
[waypoint_follower-6] 	waypoint_follower lifecycle node launched. 
[waypoint_follower-6] 	Waiting on external lifecycle transitions to activate
[waypoint_follower-6] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[waypoint_follower-6] [INFO] [1715949704.074010626] [robot2.waypoint_follower]: Creating
[lifecycle_manager-8] [INFO] [1715949704.081933475] [robot2.lifecycle_manager_navigation]: Creating
[lifecycle_manager-8] [INFO] [1715949704.093525886] [robot2.lifecycle_manager_navigation]: Creating and initializing lifecycle service clients
[smoother_server-2] [INFO] [1715949704.105257981] [robot2.smoother_server]: 
[smoother_server-2] 	smoother_server lifecycle node launched. 
[smoother_server-2] 	Waiting on external lifecycle transitions to activate
[smoother_server-2] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[bt_navigator-5] [INFO] [1715949704.106290988] [robot2.bt_navigator]: 
[bt_navigator-5] 	bt_navigator lifecycle node launched. 
[bt_navigator-5] 	Waiting on external lifecycle transitions to activate
[bt_navigator-5] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[bt_navigator-5] [INFO] [1715949704.106528548] [robot2.bt_navigator]: Creating
[controller_server-1] [INFO] [1715949704.112317575] [robot2.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.
[smoother_server-2] [INFO] [1715949704.113637505] [robot2.smoother_server]: Creating smoother server
[controller_server-1] [INFO] [1715949704.119748823] [robot2.controller_server]: Creating controller server
[planner_server-3] [INFO] [1715949704.120412991] [robot2.planner_server]: 
[planner_server-3] 	planner_server lifecycle node launched. 
[planner_server-3] 	Waiting on external lifecycle transitions to activate
[planner_server-3] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[velocity_smoother-7] [INFO] [1715949704.121958829] [robot2.velocity_smoother]: 
[velocity_smoother-7] 	velocity_smoother lifecycle node launched. 
[velocity_smoother-7] 	Waiting on external lifecycle transitions to activate
[velocity_smoother-7] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[planner_server-3] [INFO] [1715949704.125715936] [robot2.planner_server]: Creating
[controller_server-1] [INFO] [1715949704.164391694] [robot2.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] [1715949704.165389304] [robot2.local_costmap.local_costmap]: Creating Costmap
[planner_server-3] [INFO] [1715949704.179529802] [robot2.global_costmap.global_costmap]: 
[planner_server-3] 	global_costmap lifecycle node launched. 
[planner_server-3] 	Waiting on external lifecycle transitions to activate
[planner_server-3] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[planner_server-3] [INFO] [1715949704.180798565] [robot2.global_costmap.global_costmap]: Creating Costmap
[behavior_server-4] [INFO] [1715949704.189889908] [robot2.behavior_server]: 
[behavior_server-4] 	behavior_server lifecycle node launched. 
[behavior_server-4] 	Waiting on external lifecycle transitions to activate
[behavior_server-4] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[lifecycle_manager-8] [INFO] [1715949706.101525973] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-8] [INFO] [1715949708.102117002] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-8] [INFO] [1715949710.102655912] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-8] [INFO] [1715949712.103353380] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)
[lifecycle_manager-8] [INFO] [1715949712.562085570] [rclcpp]: signal_handler(signum=2)
[lifecycle_manager-8] [INFO] [1715949712.562433550] [robot2.lifecycle_manager_navigation]: Running Nav2 LifecycleManager rcl preshutdown (lifecycle_manager_navigation)
[controller_server-1] [INFO] [1715949712.562125186] [rclcpp]: signal_handler(signum=2)
[controller_server-1] [INFO] [1715949712.562411448] [robot2.local_costmap.local_costmap]: Running Nav2 LifecycleNode rcl preshutdown (local_costmap)
[controller_server-1] [INFO] [1715949712.562516437] [robot2.local_costmap.local_costmap]: Destroying bond (local_costmap) to lifecycle manager.
[bt_navigator-5] [INFO] [1715949712.562149642] [rclcpp]: signal_handler(signum=2)
[bt_navigator-5] [INFO] [1715949712.562501418] [robot2.bt_navigator]: Running Nav2 LifecycleNode rcl preshutdown (bt_navigator)
[waypoint_follower-6] [INFO] [1715949712.562149732] [rclcpp]: signal_handler(signum=2)
[waypoint_follower-6] [INFO] [1715949712.562504614] [robot2.waypoint_follower]: Running Nav2 LifecycleNode rcl preshutdown (waypoint_follower)
[planner_server-3] [INFO] [1715949712.562145875] [rclcpp]: signal_handler(signum=2)
[planner_server-3] [INFO] [1715949712.562543769] [robot2.global_costmap.global_costmap]: Running Nav2 LifecycleNode rcl preshutdown (global_costmap)
[velocity_smoother-7] [INFO] [1715949712.562201360] [rclcpp]: signal_handler(signum=2)
[velocity_smoother-7] [INFO] [1715949712.562607880] [robot2.velocity_smoother]: Running Nav2 LifecycleNode rcl preshutdown (velocity_smoother)
[velocity_smoother-7] [INFO] [1715949712.562934098] [robot2.velocity_smoother]: Destroying bond (velocity_smoother) to lifecycle manager.
[smoother_server-2] [INFO] [1715949712.562196360] [rclcpp]: signal_handler(signum=2)
[smoother_server-2] [INFO] [1715949712.562618129] [robot2.smoother_server]: Running Nav2 LifecycleNode rcl preshutdown (smoother_server)
[smoother_server-2] [INFO] [1715949712.562997017] [robot2.smoother_server]: Destroying bond (smoother_server) to lifecycle manager.
[behavior_server-4] [INFO] [1715949712.562201179] [rclcpp]: signal_handler(signum=2)
[behavior_server-4] [INFO] [1715949712.562549169] [robot2.behavior_server]: Running Nav2 LifecycleNode rcl preshutdown (behavior_server)
[behavior_server-4] [INFO] [1715949712.562865488] [robot2.behavior_server]: Destroying bond (behavior_server) to lifecycle manager.[lifecycle_manager-8] [INFO] [1715949714.118333685] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-8] [INFO] [1715949714.168452972] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-8] [INFO] [1715949714.218587447] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-8] [INFO] [1715949714.268731121] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-8] [INFO] [1715949712.562600366] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-8] 
[lifecycle_manager-8] >>> [rcutils|error_handling.c:108] rcutils_set_error_state()
[lifecycle_manager-8] This error state is being overwritten:
[lifecycle_manager-8] 
[lifecycle_manager-8]   'rcl node's context is invalid, at ./src/rcl/node.c:428'
[lifecycle_manager-8] 
[lifecycle_manager-8] with this new error message:
[lifecycle_manager-8] 
[lifecycle_manager-8]   'publisher's context is invalid, at ./src/rcl/publisher.c:389'
[lifecycle_manager-8] 
[lifecycle_manager-8] rcutils_reset_error() should be called after error handling to avoid this.
[lifecycle_manager-8] <<<
[lifecycle_manager-8] Failed to publish log message to rosout: publisher's context is invalid, at ./src/rcl/publisher.c:389
[lifecycle_manager-8] [INFO] [1715949712.562657043] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-8] 
[lifecycle_manager-8] >>> [rcutils|error_handling.c:108] rcutils_set_error_state()
[lifecycle_manager-8] This error state is being overwritten:
[lifecycle_manager-8] 
[lifecycle_manager-8]   'rcl node's context is invalid, at ./src/rcl/node.c:428'
[lifecycle_manager-8] 
[lifecycle_manager-8] with this new error message:
[lifecycle_manager-8] 
[lifecycle_manager-8]   'publisher's context is invalid, at ./src/rcl/publisher.c:389'
[lifecycle_manager-8] 
[lifecycle_manager-8] rcutils_reset_error() should be called after error handling to avoid this.
[lifecycle_manager-8] <<<
[lifecycle_manager-8] Failed to publish log message to rosout: publisher's context is invalid, at ./src/rcl/publisher.c:389
[lifecycle_manager-8] [INFO] [1715949712.562873704] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[controller_server-1] [INFO] [1715949712.562534511] [robot2.controller_server]: Running Nav2 LifecycleNode rcl preshutdown (controller_server)
[controller_server-1] [INFO] [1715949712.562554799] [robot2.controller_server]: Destroying bond (controller_server) to lifecycle manager.
[bt_navigator-5] [INFO] [1715949712.562682471] [robot2.bt_navigator]: Destroying bond (bt_navigator) to lifecycle manager.
[bt_navigator-5] [INFO] [1715949712.568448294] [robot2.bt_navigator]: Destroying
[waypoint_follower-6] [INFO] [1715949712.562676229] [robot2.waypoint_follower]: Destroying bond (waypoint_follower) to lifecycle manager.
[waypoint_follower-6] [INFO] [1715949712.569206771] [robot2.waypoint_follower]: Destroying
[planner_server-3] [INFO] [1715949712.562875517] [robot2.global_costmap.global_costmap]: Destroying bond (global_costmap) to lifecycle manager.
[planner_server-3] [INFO] [1715949712.562913208] [robot2.planner_server]: Running Nav2 LifecycleNode rcl preshutdown (planner_server)
[planner_server-3] [INFO] [1715949712.562957602] [robot2.planner_server]: Destroying bond (planner_server) to lifecycle manager.
[velocity_smoother-7] [INFO] [1715949712.570233856] [robot2.velocity_smoother]: Destroying
[smoother_server-2] [INFO] [1715949712.570508947] [robot2.smoother_server]: Destroying
[controller_server-1] [INFO] [1715949712.574436438] [robot2.local_costmap.local_costmap]: Destroying
[behavior_server-4] [INFO] [1715949712.576164973] [robot2.behavior_server]: Destroying
[planner_server-3] [INFO] [1715949712.578551714] [robot2.global_costmap.global_costmap]: Destroying
[lifecycle_manager-8] [INFO] [1715949712.613007037] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[controller_server-1] [INFO] [1715949712.646183988] [robot2.controller_server]: Destroying
[lifecycle_manager-8] [INFO] [1715949712.663171038] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-8] [INFO] [1715949712.714197634] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[planner_server-3] [INFO] [1715949712.751755113] [robot2.planner_server]: Destroying
[lifecycle_manager-8] [INFO] [1715949712.764654791] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-8] [INFO] [1715949712.814797211] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[INFO] [waypoint_follower-6]: process has finished cleanly [pid 22620]
[INFO] [controller_server-1]: process has finished cleanly [pid 22610]
[INFO] [velocity_smoother-7]: process has finished cleanly [pid 22622]
[INFO] [behavior_server-4]: process has finished cleanly [pid 22616]
[lifecycle_manager-8] [INFO] [1715949712.864954029] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[INFO] [smoother_server-2]: process has finished cleanly [pid 22612]
[INFO] [bt_navigator-5]: process has finished cleanly [pid 22618]
[lifecycle_manager-8] [INFO] [1715949712.915097963] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[INFO] [planner_server-3]: process has finished cleanly [pid 22614]
[lifecycle_manager-8] [INFO] [1715949712.965215486] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-8] [INFO] [1715949713.015332800] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-8] [INFO] [1715949713.065461514] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-8] [INFO] [1715949713.115586121] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-8] [INFO] [1715949713.165744652] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-8] [INFO] [1715949713.215867395] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-8] [INFO] [1715949713.265998054] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-8] [INFO] [1715949713.316118132] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-8] [INFO] [1715949713.366249803] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-8] [INFO] [1715949713.416391111] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-8] [INFO] [1715949713.466531118] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-8] [INFO] [1715949713.516695109] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-8] [INFO] [1715949713.566819797] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-8] [INFO] [1715949713.616951968] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-8] [INFO] [1715949713.667090101] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-8] [INFO] [1715949713.717244123] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-8] [INFO] [1715949713.767395772] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-8] [INFO] [1715949713.817525859] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-8] [INFO] [1715949713.867663180] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-8] [INFO] [1715949713.917792355] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-8] [INFO] [1715949713.967934686] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-8] [INFO] [1715949714.018075303] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[lifecycle_manager-8] [INFO] [1715949714.068208316] [robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...
[ERROR] [lifecycle_manager-8]: process[lifecycle_manager-8] failed to terminate '10.0' seconds after receiving 'SIGTERM', escalating to 'SIGKILL'
[INFO] [lifecycle_manager-8]: sending signal 'SIGKILL' to process[lifecycle_manager-8]
[ERROR] [lifecycle_manager-8]: process has died [pid 22624, exit code -9, cmd '/opt/ros/humble/lib/nav2_lifecycle_manager/lifecycle_manager --ros-args --log-level info --ros-args -r __node:=lifecycle_manager_navigation -r __ns:=/robot2 --params-file /tmp/launch_params_r341kn8k --params-file /tmp/launch_params_ibsnqp1y --params-file /tmp/launch_params_9r493_pm -r /robot2/global_costmap/scan:=/robot2/scan -r /robot2/local_costmap/scan:=/robot2/scan'].

Thanks for the help!

@WallerJonathan
Copy link
Author

I did not mean to close this, that is my bad!

@SteveMacenski
Copy link
Member

SteveMacenski commented May 17, 2024

I can't comment on the TB4 stuff - but did you try this with our multirobot bringup? The TB4 package might not be working properly... What's your ros2 topic list look like - is everything properly namespaced and nothing in the root namespace? That could explain why its not finding your <namespace>/bt_navigator/get_state service, if there's no <namepace>-ed server

Waiting for service controller_server/get_state...

This is making me think its just another instance of a similar problem elsewhere in launch. I don't totally put it above a Nav2 problem, but this looks like a launch issue with TB4 config files not respecting namespaces in the lifecycle manager's node list

@WallerJonathan
Copy link
Author

WallerJonathan commented May 20, 2024

Running ros2 topic list | nl gives me a list of topics that has what I believe I need:

ubuntu@unity:~$ ros2 topic list | nl
     1	/clock
     2	/diagnostics
     3	/parameter_events
     4	/robot1/amcl/transition_event
     5	/robot1/amcl_pose
     6	/robot1/battery_state
     7	/robot1/behavior_server/transition_event
     8	/robot1/behavior_tree_log
     9	/robot1/bond
    10	/robot1/bt_navigator/transition_event
    11	/robot1/bumper_contact
    12	/robot1/clicked_point
    13	/robot1/cmd_audio
    14	/robot1/cmd_lightring
    15	/robot1/cmd_vel
    16	/robot1/cmd_vel_nav
    17	/robot1/cmd_vel_teleop
    18	/robot1/controller_server/transition_event
    19	/robot1/cost_cloud
    20	/robot1/diffdrive_controller/cmd_vel_unstamped
    21	/robot1/diffdrive_controller/transition_event
    22	/robot1/dock_status
    23	/robot1/downsampled_costmap
    24	/robot1/downsampled_costmap_updates
    25	/robot1/dynamic_joint_states
    26	/robot1/evaluation
    27	/robot1/function_calls
    28	/robot1/global_costmap/costmap
    29	/robot1/global_costmap/costmap_raw
    30	/robot1/global_costmap/costmap_updates
    31	/robot1/global_costmap/footprint
    32	/robot1/global_costmap/global_costmap/transition_event
    33	/robot1/global_costmap/published_footprint
    34	/robot1/global_costmap/voxel_marked_cloud
    35	/robot1/goal_pose
    36	/robot1/hazard_detection
    37	/robot1/initialpose
    38	/robot1/interface_buttons
    39	/robot1/ip
    40	/robot1/ir_intensity
    41	/robot1/ir_opcode
    42	/robot1/joint_state_broadcaster/transition_event
    43	/robot1/joint_states
    44	/robot1/joy
    45	/robot1/kidnap_status
    46	/robot1/local_costmap/clearing_endpoints
    47	/robot1/local_costmap/costmap
    48	/robot1/local_costmap/costmap_raw
    49	/robot1/local_costmap/costmap_updates
    50	/robot1/local_costmap/footprint
    51	/robot1/local_costmap/local_costmap/transition_event
    52	/robot1/local_costmap/published_footprint
    53	/robot1/local_costmap/voxel_grid
    54	/robot1/local_costmap/voxel_marked_cloud
    55	/robot1/local_plan
    56	/robot1/map
    57	/robot1/map_server/transition_event
    58	/robot1/map_updates
    59	/robot1/marker
    60	/robot1/mobile_base/sensors/bumper_pointcloud
    61	/robot1/mouse
    62	/robot1/oakd/rgb/preview/camera_info
    63	/robot1/oakd/rgb/preview/depth
    64	/robot1/oakd/rgb/preview/depth/points
    65	/robot1/oakd/rgb/preview/image_raw
    66	/robot1/odom
    67	/robot1/particle_cloud
    68	/robot1/plan
    69	/robot1/plan_smoothed
    70	/robot1/planner_server/transition_event
    71	/robot1/preempt_teleop
    72	/robot1/received_global_plan
    73	/robot1/robot_description
    74	/robot1/scan
    75	/robot1/sim_ground_truth_dock_pose
    76	/robot1/sim_ground_truth_pose
    77	/robot1/slip_status
    78	/robot1/smoother_server/transition_event
    79	/robot1/speed_limit
    80	/robot1/standard_dock_description
    81	/robot1/stop_status
    82	/robot1/tf
    83	/robot1/tf_static
    84	/robot1/transformed_global_plan
    85	/robot1/velocity_smoother/transition_event
    86	/robot1/waypoint_follower/transition_event
    87	/robot1/waypoints
    88	/robot1/wheel_status
    89	/robot1/wheel_ticks
    90	/robot1/wheel_vels
    91	/robot2/amcl/transition_event
    92	/robot2/amcl_pose
    93	/robot2/battery_state
    94	/robot2/behavior_server/transition_event
    95	/robot2/bond
    96	/robot2/bt_navigator/transition_event
    97	/robot2/bumper_contact
    98	/robot2/clicked_point
    99	/robot2/cmd_audio
   100	/robot2/cmd_lightring
   101	/robot2/cmd_vel
   102	/robot2/controller_server/transition_event
   103	/robot2/diffdrive_controller/cmd_vel_unstamped
   104	/robot2/diffdrive_controller/transition_event
   105	/robot2/dock_status
   106	/robot2/downsampled_costmap
   107	/robot2/downsampled_costmap_updates
   108	/robot2/dynamic_joint_states
   109	/robot2/function_calls
   110	/robot2/global_costmap/costmap
   111	/robot2/global_costmap/costmap_updates
   112	/robot2/global_costmap/global_costmap/transition_event
   113	/robot2/global_costmap/voxel_marked_cloud
   114	/robot2/hazard_detection
   115	/robot2/initialpose
   116	/robot2/interface_buttons
   117	/robot2/ip
   118	/robot2/ir_intensity
   119	/robot2/ir_opcode
   120	/robot2/joint_states
   121	/robot2/joy
   122	/robot2/kidnap_status
   123	/robot2/local_costmap/costmap
   124	/robot2/local_costmap/costmap_updates
   125	/robot2/local_costmap/local_costmap/transition_event
   126	/robot2/local_costmap/published_footprint
   127	/robot2/local_costmap/voxel_marked_cloud
   128	/robot2/local_plan
   129	/robot2/map
   130	/robot2/map_server/transition_event
   131	/robot2/map_updates
   132	/robot2/mobile_base/sensors/bumper_pointcloud
   133	/robot2/mouse
   134	/robot2/odom
   135	/robot2/particle_cloud
   136	/robot2/plan
   137	/robot2/planner_server/transition_event
   138	/robot2/robot_description
   139	/robot2/scan
   140	/robot2/sim_ground_truth_dock_pose
   141	/robot2/sim_ground_truth_pose
   142	/robot2/slip_status
   143	/robot2/smoother_server/transition_event
   144	/robot2/standard_dock_description
   145	/robot2/stop_status
   146	/robot2/tf
   147	/robot2/tf_static
   148	/robot2/waypoint_follower/transition_event
   149	/robot2/waypoints
   150	/robot2/wheel_status
   151	/robot2/wheel_ticks
   152	/robot2/wheel_vels
   153	/rosout

In addition, when I run ros2 service list I can see the services active (shortened so it is easy to read):

/robot1/bt_navigator/get_state
/robot1/controller_server/get_state
/robot2/bt_navigator/get_state
/robot2/controller_server/get_state

I agree that it is probably something to do with Turtlebot4 and not as much NAV2. An interesting thing is that about ten or so topics are not seen in /robot2 including 16 /robot1/cmd_vel_nav; could this possibly be part of the problem?

Again, thanks for the help.

@SteveMacenski
Copy link
Member

SteveMacenski commented May 20, 2024

In that case, this line:

[robot2.lifecycle_manager_navigation]: Waiting for service controller_server/get_state...

Points to the likely issue being that their eq of https://github.com/ros-navigation/navigation2/blob/main/nav2_bringup/launch/navigation_launch.py#L43 isn't respecting namespaces. Its looking for controller_server/get_state, not <robot>/controller_server/get_state. There could be something else going on here, but that's where the lifecycle manager gets its list of nodes from.

I think you should file this with the TB4 folks. The namespace issue in the wait until active seems like a good direct contribution back to Nav2 however, if you wanted to open a PR to add using self.namespace! That's something we can do here.

@SteveMacenski
Copy link
Member

Any update?

@WallerJonathan
Copy link
Author

WallerJonathan commented May 29, 2024

A few updates so far:

It seems that the TB4 team has other work at the moment, and they are not able to answer the tickets.

I decided to look into this more myself as the project I am working on requires multiple NAV2 modules working at once. I found that using the bringup_launch.py from the nav2_bringup package allows me to bring up multiple modules at the same time! It seems their launch file may have some issues, although I am not sure what.

The problem I have now run into is that the local costmap appears blank from Nav2. It seems to be publishing a blank costmap. I checked through #3034, but their solution does not seem to work in my case. I also tried changing the parameter file to one that works with TB4, but that doesn't seem to change anything.

Nav2 still works on both robots, it is just missing the costmap which means the robots can't see dynamic obstacles. Luckily, this is not a huge problem at the moment, but it is something I need to fix in a week or so. I am not sure what code/terminal outputs you might need, so just let me know and I will get back to you.

As always, thanks for your help!

@SteveMacenski
Copy link
Member

Great, I think that means we can close this? The issues are with the TB4 launch files and our stuff here works. Happy to respond here though if things come up!

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

2 participants