ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
2

Nav2 planner_server's compute_path_to_pose hangs indefinitely

asked 2022-05-24 15:22:30 -0500

hermanoid gravatar image

We're attempting to implement a basic nav2 stack on our robot. We haven't included obstacle finding or any mapping at all. Our node setup contains the nav2_bt_navigator node, nav2_planner, and nav2_controller, plus a number of peripherals:

  • Real-time localization is provided by integrating our wheel encoders. We do not have a robot_localization node running yet, so this odom value is fed directly into tf and the /odom topic.
  • Global localization is provided using a proprietary VSLAM node. We do the necessary math such that the tf tree looks like: map -> odom -> base_link
  • Motor output is fed into our Odrive motor driver node. This bit hasn't been relevant yet because no such motor output is being produced.
  • We provide a completely blank static map. The idea is to get the robot to move with no concern for obstacles before we add those in.
  • All obstacle detection, real-time mapping, etc., is disconnected.

We generally duplicated the example configuration file from the nav2_bringuppackage. It's too long to include in this post, so I put it here: https://pastebin.com/FXaPuEFS

And the relevant bits of our launch file:

param_substitutions = {"use_sim_time": "false", "yaml_filename": "turtlebot3_world.yaml"}

configured_params = RewrittenYaml(source_file=params_file, root_key=namespace, param_rewrites=param_substitutions, convert_types=True)
lifecycle_nodes = ["controller_server", "planner_server", "bt_navigator"]
load_nodes = GroupAction(
    actions=[
        PushRosNamespace(condition=IfCondition(use_namespace), namespace=namespace),
        Node(
            package="nav2_controller",
            executable="controller_server",
            output="screen",
            respawn=use_respawn,
            respawn_delay=2.0,
            parameters=[configured_params],
            remappings=remappings,
            # arguments=['--ros-args', '--log-level', 'debug']
        ),
        Node(
            package="nav2_planner",
            executable="planner_server",
            name="planner_server",
            output="screen",
            respawn=use_respawn,
            respawn_delay=2.0,
            parameters=[configured_params],
            # prefix=['xterm -e gdb -ex run --args'],
            remappings=remappings,
            arguments=['--ros-args', '--log-level', 'debug']
        ),
        Node(
            package="nav2_bt_navigator",
            executable="bt_navigator",
            name="bt_navigator",
            output="screen",
            respawn=use_respawn,
            respawn_delay=2.0,
            parameters=[configured_params],
            remappings=remappings,
            # arguments=['--ros-args', '--log-level', 'debug']
        ),
        # Node(
        #     package="nav2_waypoint_follower",
        #     executable="waypoint_follower",
        #     name="waypoint_follower",
        #     output="screen",
        #     respawn=use_respawn,
        #     respawn_delay=2.0,
        #     parameters=[configured_params],
        #     remappings=remappings,
        #     # arguments=['--ros-args', '--log-level', 'debug']
        # ),
        Node(
            package="nav2_lifecycle_manager",
            executable="lifecycle_manager",
            name="lifecycle_manager_navigation",
            output="screen",
            parameters=[{"use_sim_time": use_sim_time}, {"autostart": autostart}, {"node_names": lifecycle_nodes}],
            # arguments=['--ros-args', '--log-level', 'debug']
        ),
    ]
)

The whole system starts (and activates) successfully. We've tried sending goal poses through RVIZ. bt_navigator will recognize our actions:

[bt_navigator-8] [INFO] [1653423291.874302388] [bt_navigator]: Begin navigating from current location to (1.85, 3.77)

But then... nothing happens. We used Groot on the bt tree and found that it seems to hang on the planner_server. We manually called the planner server like this:

ros2 action send_goal -f /compute_path_to_pose nav2_msgs/action/ComputePathToPose "{pose: {pose: {position: {x: 5, y: 5, z: 0}, orientation: {x: 0, y: 0, z: 0, w: 1}}, header: {frame_id: 0}}}"
Waiting for an action server to become available...
Sending goal:
     pose:
  header:
    stamp:
      sec: 0
      nanosec: 0
    frame_id: '0'
  pose:
    position:
      x: 5.0
      y: 5.0
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
planner_id: ''

[INFO] [1653422755.926925127] [_ros2cli_send_goal_nav2_msgs_action_ComputePathToPose]: sent goal sequence number 1 for action /compute_path_to_pose
[INFO] [1653422755.927586914] [_ros2cli_send_goal_nav2_msgs_action_ComputePathToPose]: added goal sequence number 1 for action /compute_path_to_pose

But, after the goal state is ... (more)

edit retag flag offensive close merge delete

Comments

Hello,

Any news on this issue ?
I have the same problem.

I had nav2 working but then I went from Foxy to Galactic and this happened.
I've also prefixed all the frames of my robot by its name and use a namespace (in order to support multi robot in the future), maybe it has something to do with that...

Acwok gravatar image Acwok  ( 2022-06-03 08:34:16 -0500 )edit
1

Basically, no! I'll submit an answer to this question with what I do know.

hermanoid gravatar image hermanoid  ( 2022-06-03 11:11:41 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2022-06-03 11:30:35 -0500

hermanoid gravatar image

We did end up solving this problem, but not in a way that allows me to point to what the solution was. After fighting this issue for a very long time, we took a different approach and started with the turtlebot3 simulation that ships with nav2. We gradually moved pieces of our physical robot into the simulation until we completely turned off the simulation and somehow everything was working.

Assuming I was right to assume that the controller_server was stuck on that wait_for_costmap(...)function I mentioned in my original question: I'm guessing the solution can be found in the difference between our old, not working configuration and the new one that does work. That diff can be found here: https://www.diffchecker.com/gdBaSp4O

The key bit (I'm guessing) is that, in the working version, we omit the voxel_layer that relies on a laser scan (which our robot doesn't have). The broken version was probably waiting indefinitely for a scan. This, of course, means that our robot cannot perceive obstacles, but we plan on replacing that piece with a depth-camera based obstacle detector.

You can see that our launch file basically didn't change: https://www.diffchecker.com/O9FGdWuY

I can't guarantee that this was the breaking difference (especially because I remember trying to turn off the voxel layer when originally fighting this issue), but it's the best I've got.

I hope this helps someone in the future!

edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2022-05-24 15:22:30 -0500

Seen: 291 times

Last updated: Jun 03 '22