Nav2 planner_server's compute_path_to_pose hangs indefinitely
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_bringup
package. 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 ...
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...
Basically, no! I'll submit an answer to this question with what I do know.