Unit 4.3: Cannot send navigation goal

I added the launch script and config files by following 4.1. All nodes seem to activate, and in the end I get this in console:

[bt_navigator-5] [INFO] [1707415518.353879790] [bt_navigator]: Creating bond (bt_navigator) to lifecycle manager.
[lifecycle_manager-7] [INFO] [1707415518.461199636] [lifecycle_manager]: Server bt_navigator connected with bond.
[lifecycle_manager-7] [INFO] [1707415518.461251594] [lifecycle_manager]: Managed nodes are active
[lifecycle_manager-7] [INFO] [1707415518.461266892] [lifecycle_manager]: Creating bond timer...

then I tried to send a goal:

ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose "pose: {header: {frame_id: map}, pose: {position: {x: 1.52, y: 1.92, z: 0.0}, orientation:{x: 0.0, y: 0.0, z: 0, w: 1.0000000}}}"

it would fail with the following message:

[bt_navigator-5] [INFO] [1707415525.165466582] [bt_navigator]: Begin navigating from current location (-0.01, 0.02) to (1.52, 1.92)
[controller_server-4] [INFO] [1707415525.186354882] [controller_server]: Received a goal, begin computing control effort.
[controller_server-4] [WARN] [1707415525.186419281] [controller_server]: No goal checker was specified in parameter 'current_goal_checker'. Server will use only plugin loaded general_goal_checker . This warning will appear once.
[ERROR] [controller_server-4]: process has died [pid 12045, exit code -11, cmd '/opt/ros/humble/lib/nav2_controller/controller_server --ros-args -r __node:=controller_server --params-file /home/user/ros2_ws/install/path_planner_server/share/path_planner_server/config/controller.yaml'].
[bt_navigator-5] [WARN] [1707415526.236515204] [bt_navigator_navigate_to_pose_rclcpp_node]: Timed out while waiting for action server to acknowledge goal request for follow_path
[bt_navigator-5] [WARN] [1707415526.257053897] [bt_navigator_navigate_to_pose_rclcpp_node]: Node timed out while executing service call to local_costmap/clear_entirely_local_costmap.
[bt_navigator-5] [WARN] [1707415526.277512796] [bt_navigator_navigate_to_pose_rclcpp_node]: Node timed out while executing service call to local_costmap/clear_entirely_local_costmap.
[bt_navigator-5] [ERROR] [1707415526.277726366] [bt_navigator]: Goal failed
[bt_navigator-5] [WARN] [1707415526.277749086] [bt_navigator]: [navigate_to_pose] [ActionServer] Aborting handle.
[lifecycle_manager-7] [INFO] [1707415529.261411222] [lifecycle_manager]: Have not received a heartbeat from controller_server.
[lifecycle_manager-7] [ERROR] [1707415529.261476029] [lifecycle_manager]: CRITICAL FAILURE: SERVER controller_server IS DOWN after not receiving a heartbeat for 4000 ms. Shutting down related nodes.
[lifecycle_manager-7] [INFO] [1707415529.261496243] [lifecycle_manager]: Terminating bond timer...
[lifecycle_manager-7] [INFO] [1707415529.261510595] [lifecycle_manager]: Resetting managed nodes...
[lifecycle_manager-7] [INFO] [1707415529.261525540] [lifecycle_manager]: Deactivating bt_navigator
[bt_navigator-5] [INFO] [1707415529.261817496] [bt_navigator]: Deactivating
[bt_navigator-5] [INFO] [1707415529.261865003] [bt_navigator]: Destroying bond (bt_navigator) to lifecycle manager.
[lifecycle_manager-7] [INFO] [1707415529.366030475] [lifecycle_manager]: Deactivating recoveries_server
[behavior_server-6] [INFO] [1707415529.366823742] [recoveries_server]: Deactivating
[behavior_server-6] [INFO] [1707415529.366890985] [recoveries_server]: Destroying bond (recoveries_server) to lifecycle manager.
[lifecycle_manager-7] [INFO] [1707415529.470986337] [lifecycle_manager]: Deactivating controller_server

Appreciate any help troubleshooting. Thanks in advance!

I can see in your log that the controller manager has died. This is not normal. Because the controller manager died, the navigation system cannot control the robot to follow the path.

But with so little information you provided it is not possible to figure out why the controller died. Some other things you need to include in order to debug that problem:

  1. Does the path planner show a path in the rviz when you set a goal? (show an rviz screenshot of it)
  2. What is the full log of the terminal where you launch the path planner server and the controller server?
  3. Show the launch files and config files of the path planner and the controller servers

I think with that we can have an idea of what may be happening

Hi Ricardo,

Thanks for helping me troubleshoot!

  1. Does the path planner show a path in the rviz when you set a goal? (show an rviz screenshot of it)
    I can see a path in RViz

  2. What is the full log of the terminal where you launch the path planner server and the controller server?
    I launched all nav2_xxx node in one terminal, here is the full log

user:~/ros2_ws$ ros2 launch path_planner_server path_planner.launch.py
[INFO] [launch]: All log files can be found below /home/user/.ros/log/2024-02-09-17-59-54-920877-1_xterm-3296
[INFO] [launch]: Default logging verbosity is set to INFO
[WARNING] [launch_ros.actions.node]: Parameter file path is not a file: /home/user/ros2_ws/install/path_planner_server/share/path_planner_server/config/amcl_config_initialized.yaml
[INFO] [map_server-1]: process started with pid [3297]
[INFO] [amcl-2]: process started with pid [3299]
[INFO] [planner_server-3]: process started with pid [3301]
[INFO] [controller_server-4]: process started with pid [3303]
[INFO] [bt_navigator-5]: process started with pid [3306]
[INFO] [behavior_server-6]: process started with pid [3340]
[INFO] [lifecycle_manager-7]: process started with pid [3343]
[controller_server-4] [INFO] [1707501595.400705461] [controller_server]:
[controller_server-4]   controller_server lifecycle node launched.
[controller_server-4]   Waiting on external lifecycle transitions to activate
[controller_server-4]   See https://design.ros2.org/articles/node_lifecycle.html for more information.
[controller_server-4] [INFO] [1707501595.418945807] [controller_server]: Creating controller server
[amcl-2] [INFO] [1707501595.487999419] [amcl]:
[amcl-2]        amcl lifecycle node launched.
[amcl-2]        Waiting on external lifecycle transitions to activate
[amcl-2]        See https://design.ros2.org/articles/node_lifecycle.html for more information.
[amcl-2] [INFO] [1707501595.498057725] [amcl]: Creating
[bt_navigator-5] [INFO] [1707501595.539389830] [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] [1707501595.549318666] [bt_navigator]: Creating
[planner_server-3] [INFO] [1707501595.609784467] [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.
[planner_server-3] [INFO] [1707501595.613455502] [planner_server]: Creating[map_server-1] [INFO] [1707501595.678956217] [map_server]:
[map_server-1]  map_server lifecycle node launched.
[map_server-1]  Waiting on external lifecycle transitions to activate
[map_server-1]  See https://design.ros2.org/articles/node_lifecycle.html for more information.
[map_server-1] [INFO] [1707501595.685340839] [map_server]: Creating
[lifecycle_manager-7] [INFO] [1707501595.694843864] [lifecycle_manager]: Creating
[lifecycle_manager-7] [INFO] [1707501595.701554470] [lifecycle_manager]: Creating and initializing lifecycle service clients
[controller_server-4] [INFO] [1707501595.704734535] [local_costmap.local_costmap]:
[controller_server-4]   local_costmap lifecycle node launched.
[controller_server-4]   Waiting on external lifecycle transitions to activate
[controller_server-4]   See https://design.ros2.org/articles/node_lifecycle.html for more information.
[controller_server-4] [INFO] [1707501595.711097030] [local_costmap.local_costmap]: Creating Costmap
[behavior_server-6] [INFO] [1707501595.779190828] [recoveries_server]:
[behavior_server-6]     recoveries_server lifecycle node launched.
[behavior_server-6]     Waiting on external lifecycle transitions to activate
[behavior_server-6]     See https://design.ros2.org/articles/node_lifecycle.html for more information.
[planner_server-3] [INFO] [1707501595.820074281] [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] [1707501595.823408095] [global_costmap.global_costmap]: Creating Costmap
[lifecycle_manager-7] [INFO] [1707501596.603634705] [lifecycle_manager]: Starting managed nodes bringup...
[lifecycle_manager-7] [INFO] [1707501596.603696096] [lifecycle_manager]: Configuring map_server
[map_server-1] [INFO] [1707501596.604069603] [map_server]: Configuring
[map_server-1] [INFO] [map_io]: Loading yaml file: /home/user/ros2_ws/install/path_planner_server/share/path_planner_server/config/turtlebot_area.yaml
[map_server-1] [DEBUG] [map_io]: resolution: 0.05
[map_server-1] [DEBUG] [map_io]: origin[0]: -4.33
[map_server-1] [DEBUG] [map_io]: origin[1]: -1.85
[map_server-1] [DEBUG] [map_io]: origin[2]: 0
[map_server-1] [DEBUG] [map_io]: free_thresh: 0.25
[map_server-1] [DEBUG] [map_io]: occupied_thresh: 0.65
[map_server-1] [DEBUG] [map_io]: mode: trinary
[map_server-1] [DEBUG] [map_io]: negate: 0
[map_server-1] [INFO] [map_io]: Loading image_file: /home/user/ros2_ws/install/path_planner_server/share/path_planner_server/config/turtlebot_area.pgm
[map_server-1] [DEBUG] [map_io]: Read map /home/user/ros2_ws/install/path_planner_server/share/path_planner_server/config/turtlebot_area.pgm: 148 X 144 map @ 0.05 m/cell
[lifecycle_manager-7] [INFO] [1707501596.619084983] [lifecycle_manager]: Configuring amcl
[amcl-2] [INFO] [1707501596.619750310] [amcl]: Configuring
[amcl-2] [INFO] [1707501596.619870968] [amcl]: initTransforms
[amcl-2] [INFO] [1707501596.638364099] [amcl]: initPubSub
[amcl-2] [INFO] [1707501596.647574623] [amcl]: Subscribed to map topic.
[lifecycle_manager-7] [INFO] [1707501596.653246121] [lifecycle_manager]: Configuring planner_server
[planner_server-3] [INFO] [1707501596.653872541] [planner_server]: Configuring
[planner_server-3] [INFO] [1707501596.653921016] [global_costmap.global_costmap]: Configuring
[planner_server-3] [INFO] [1707501596.670277871] [global_costmap.global_costmap]: Using plugin "static_layer"
[planner_server-3] [INFO] [1707501596.679353716] [global_costmap.global_costmap]: Subscribing to the map topic (/map) with transient local durability
[planner_server-3] [INFO] [1707501596.683835285] [global_costmap.global_costmap]: Initialized plugin "static_layer"
[planner_server-3] [INFO] [1707501596.684718846] [global_costmap.global_costmap]: Using plugin "obstacle_layer"
[planner_server-3] [INFO] [1707501596.688579379] [global_costmap.global_costmap]: Subscribed to Topics:
[planner_server-3] [INFO] [1707501596.688674372] [global_costmap.global_costmap]: Initialized plugin "obstacle_layer"
[planner_server-3] [INFO] [1707501596.688701360] [global_costmap.global_costmap]: Using plugin "inflation_layer"
[planner_server-3] [INFO] [1707501596.692971751] [global_costmap.global_costmap]: Initialized plugin "inflation_layer"
[planner_server-3] [INFO] [1707501596.710833501] [planner_server]: Created global planner plugin GridBased of type nav2_navfn_planner/NavfnPlanner
[planner_server-3] [INFO] [1707501596.710893119] [planner_server]: Configuring plugin GridBased of type NavfnPlanner
[planner_server-3] [INFO] [1707501596.713833102] [planner_server]: Planner Server has GridBased  planners available.
[lifecycle_manager-7] [INFO] [1707501596.738303897] [lifecycle_manager]: Configuring controller_server
[controller_server-4] [INFO] [1707501596.738727869] [controller_server]: Configuring controller interface
[controller_server-4] [INFO] [1707501596.739030908] [controller_server]: getting goal checker plugins..
[controller_server-4] [INFO] [1707501596.739181406] [controller_server]: Controller frequency set to 10.0000Hz
[controller_server-4] [INFO] [1707501596.739486974] [local_costmap.local_costmap]: Configuring
[controller_server-4] [INFO] [1707501596.776107474] [local_costmap.local_costmap]: Using plugin "static_layer"
[controller_server-4] [INFO] [1707501596.794638641] [local_costmap.local_costmap]: Subscribing to the map topic (/map) with transient local durability
[controller_server-4] [INFO] [1707501596.797662063] [local_costmap.local_costmap]: Initialized plugin "static_layer"
[controller_server-4] [INFO] [1707501596.797703418] [local_costmap.local_costmap]: Using plugin "obstacle_layer"
[controller_server-4] [INFO] [1707501596.801384280] [local_costmap.local_costmap]: Subscribed to Topics:
[controller_server-4] [INFO] [1707501596.801462635] [local_costmap.local_costmap]: Initialized plugin "obstacle_layer"
[controller_server-4] [INFO] [1707501596.801482991] [local_costmap.local_costmap]: Using plugin "inflation_layer"
[controller_server-4] [INFO] [1707501596.804698030] [local_costmap.local_costmap]: Initialized plugin "inflation_layer"
[controller_server-4] [INFO] [1707501596.826305301] [controller_server]: Created progress_checker : progress_checker of type nav2_controller::SimpleProgressChecker
[controller_server-4] [INFO] [1707501596.830725877] [controller_server]: Created goal checker : general_goal_checker of type nav2_controller::SimpleGoalChecker
[controller_server-4] [INFO] [1707501596.832908360] [controller_server]: Controller Server has general_goal_checker  goal checkers available.
[controller_server-4] [INFO] [1707501596.835238635] [controller_server]: Created controller : FollowPath of type dwb_core::DWBLocalPlanner
[controller_server-4] [INFO] [1707501596.840090578] [controller_server]: Setting transform_tolerance to 0.200000
[controller_server-4] [INFO] [1707501596.860869595] [controller_server]: Using critic "RotateToGoal" (dwb_critics::RotateToGoalCritic)
[controller_server-4] [INFO] [1707501596.861357810] [controller_server]: Critic plugin initialized
[controller_server-4] [INFO] [1707501596.861790320] [controller_server]: Using critic "Oscillation" (dwb_critics::OscillationCritic)
[controller_server-4] [INFO] [1707501596.862275085] [controller_server]: Critic plugin initialized
[controller_server-4] [INFO] [1707501596.862413119] [controller_server]: Using critic "BaseObstacle" (dwb_critics::BaseObstacleCritic)
[controller_server-4] [INFO] [1707501596.862638893] [controller_server]: Critic plugin initialized
[controller_server-4] [INFO] [1707501596.862768724] [controller_server]: Using critic "GoalAlign" (dwb_critics::GoalAlignCritic)
[controller_server-4] [INFO] [1707501596.863017320] [controller_server]: Critic plugin initialized
[controller_server-4] [INFO] [1707501596.863097149] [controller_server]: Using critic "PathAlign" (dwb_critics::PathAlignCritic)
[controller_server-4] [INFO] [1707501596.863273814] [controller_server]: Critic plugin initialized
[controller_server-4] [INFO] [1707501596.863353833] [controller_server]: Using critic "PathDist" (dwb_critics::PathDistCritic)
[controller_server-4] [INFO] [1707501596.863533997] [controller_server]: Critic plugin initialized
[controller_server-4] [INFO] [1707501596.863660971] [controller_server]: Using critic "GoalDist" (dwb_critics::GoalDistCritic)
[controller_server-4] [INFO] [1707501596.863846478] [controller_server]: Critic plugin initialized
[controller_server-4] [INFO] [1707501596.863869182] [controller_server]: Controller Server has FollowPath  controllers available.
[lifecycle_manager-7] [INFO] [1707501596.883294856] [lifecycle_manager]: Configuring recoveries_server
[behavior_server-6] [INFO] [1707501596.883668803] [recoveries_server]: Configuring
[behavior_server-6] [INFO] [1707501596.903023759] [recoveries_server]: Creating behavior plugin spin of type nav2_behaviors/Spin
[behavior_server-6] [INFO] [1707501596.904401415] [recoveries_server]: Configuring spin
[behavior_server-6] [INFO] [1707501596.924011253] [recoveries_server]: Creating behavior plugin backup of type nav2_behaviors/BackUp
[behavior_server-6] [INFO] [1707501596.925090159] [recoveries_server]: Configuring backup
[behavior_server-6] [INFO] [1707501596.935728971] [recoveries_server]: Creating behavior plugin drive_on_heading of type nav2_behaviors/DriveOnHeading
[behavior_server-6] [INFO] [1707501596.937316330] [recoveries_server]: Configuring drive_on_heading
[behavior_server-6] [INFO] [1707501596.950619999] [recoveries_server]: Creating behavior plugin wait of type nav2_behaviors/Wait
[behavior_server-6] [INFO] [1707501596.951709625] [recoveries_server]: Configuring wait
[lifecycle_manager-7] [INFO] [1707501596.987560854] [lifecycle_manager]: Configuring bt_navigator
[bt_navigator-5] [INFO] [1707501596.987981754] [bt_navigator]: Configuring
[lifecycle_manager-7] [INFO] [1707501597.150785667] [lifecycle_manager]: Activating map_server
[map_server-1] [INFO] [1707501597.151087182] [map_server]: Activating
[map_server-1] [INFO] [1707501597.151365834] [map_server]: Creating bond (map_server) to lifecycle manager.
[planner_server-3] [INFO] [1707501597.151532674] [global_costmap.global_costmap]: StaticLayer: Resizing costmap to 148 X 144 at 0.050000 m/pix
[amcl-2] [INFO] [1707501597.151596857] [amcl]: Received a 148 X 144 map @ 0.050 m/pix
[controller_server-4] [INFO] [1707501597.152708919] [local_costmap.local_costmap]: StaticLayer: Resizing costmap to 148 X 144 at 0.050000 m/pix
[lifecycle_manager-7] [INFO] [1707501597.259431882] [lifecycle_manager]: Server map_server connected with bond.
[lifecycle_manager-7] [INFO] [1707501597.259498653] [lifecycle_manager]: Activating amcl
[amcl-2] [INFO] [1707501597.259890172] [amcl]: Activating
[amcl-2] [INFO] [1707501597.259965110] [amcl]: Creating bond (amcl) to lifecycle manager.
[amcl-2] [INFO] [1707501597.335431934] [amcl]: createLaserObject
[lifecycle_manager-7] [INFO] [1707501597.370518920] [lifecycle_manager]: Server amcl connected with bond.
[lifecycle_manager-7] [INFO] [1707501597.370586068] [lifecycle_manager]: Activating planner_server
[planner_server-3] [INFO] [1707501597.370953082] [planner_server]: Activating
[planner_server-3] [INFO] [1707501597.371013677] [global_costmap.global_costmap]: Activating
[planner_server-3] [INFO] [1707501597.371031297] [global_costmap.global_costmap]: Checking transform
...
[planner_server-3] [INFO] [1707501609.871082509] [global_costmap.global_costmap]: Timed out waiting for transform from base_link to map to become available, tf error: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist
[planner_server-3] [INFO] [1707501610.371134776] [global_costmap.global_costmap]: Timed out waiting for transform from base_link to map to become available, tf error: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist
[planner_server-3] [INFO] [1707501610.871132010] [global_costmap.global_costmap]: Timed out waiting for transform from base_link to map to become available, tf error: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist
[amcl-2] [INFO] [1707501610.901736185] [amcl]: initialPoseReceived
[amcl-2] [WARN] [1707501610.901872218] [amcl]: Failed to transform initial pose in time (Lookup would require extrapolation into the future.  Requested time 1707501610.901797 but the latest data is at time 930.308000, when looking up transform from frame [base_footprint] to frame [odom])
[amcl-2] [INFO] [1707501610.901904327] [amcl]: Setting pose (1707501610.901904): -0.491 -0.330 0.034
[planner_server-3] [INFO] [1707501611.371195939] [global_costmap.global_costmap]: Timed out waiting for transform from base_link to map to become available, tf error: Lookup would require extrapolation into the past.  Requested time 930.784000 but the earliest data is at time 931.475000, when looking up transform from frame [base_link] to frame [map]
[planner_server-3] [INFO] [1707501611.871165035] [global_costmap.global_costmap]: Timed out waiting for transform from base_link to map to become available, tf error: Lookup would require extrapolation into the past.  Requested time 931.260000 but the earliest data is at time 931.475000, when looking up transform from frame [base_link] to frame [map]
[planner_server-3] [INFO] [1707501612.371215581] [global_costmap.global_costmap]: start
[planner_server-3] [INFO] [1707501612.621413907] [planner_server]: Activating plugin GridBased of type NavfnPlanner
[planner_server-3] [INFO] [1707501612.623378915] [planner_server]: Creating bond (planner_server) to lifecycle manager.
[lifecycle_manager-7] [INFO] [1707501612.730711628] [lifecycle_manager]: Server planner_server connected with bond.
[lifecycle_manager-7] [INFO] [1707501612.730784195] [lifecycle_manager]: Activating controller_server
[controller_server-4] [INFO] [1707501612.731695988] [controller_server]: Activating
[controller_server-4] [INFO] [1707501612.731764522] [local_costmap.local_costmap]: Activating
[controller_server-4] [INFO] [1707501612.731831227] [local_costmap.local_costmap]: Checking transform
[controller_server-4] [INFO] [1707501612.731954295] [local_costmap.local_costmap]: start
[controller_server-4] [INFO] [1707501612.982554610] [controller_server]: Creating bond (controller_server) to lifecycle manager.
[lifecycle_manager-7] [INFO] [1707501613.089796565] [lifecycle_manager]: Server controller_server connected with bond.
[lifecycle_manager-7] [INFO] [1707501613.089906774] [lifecycle_manager]: Activating recoveries_server
[behavior_server-6] [INFO] [1707501613.090304082] [recoveries_server]: Activating
[behavior_server-6] [INFO] [1707501613.090358027] [recoveries_server]: Activating spin
[behavior_server-6] [INFO] [1707501613.090380759] [recoveries_server]: Activating backup
[behavior_server-6] [INFO] [1707501613.090398914] [recoveries_server]: Activating drive_on_heading
[behavior_server-6] [INFO] [1707501613.090434340] [recoveries_server]: Activating wait
[behavior_server-6] [INFO] [1707501613.090455804] [recoveries_server]: Creating bond (recoveries_server) to lifecycle manager.
[lifecycle_manager-7] [INFO] [1707501613.206860396] [lifecycle_manager]: Server recoveries_server connected with bond.
[lifecycle_manager-7] [INFO] [1707501613.206925311] [lifecycle_manager]: Activating bt_navigator
[bt_navigator-5] [INFO] [1707501613.208340689] [bt_navigator]: Activating
[bt_navigator-5] [INFO] [1707501613.322902828] [bt_navigator]: Creating bond (bt_navigator) to lifecycle manager.
[lifecycle_manager-7] [INFO] [1707501613.434849794] [lifecycle_manager]: Server bt_navigator connected with bond.
[lifecycle_manager-7] [INFO] [1707501613.434925624] [lifecycle_manager]: Managed nodes are active
[lifecycle_manager-7] [INFO] [1707501613.434947785] [lifecycle_manager]: Creating bond timer...
[bt_navigator-5] [INFO] [1707501627.862130912] [bt_navigator]: Begin navigating from current location (-0.51, -0.27) to (-1.26, 0.27)
[controller_server-4] [INFO] [1707501627.895075244] [controller_server]: Received a goal, begin computing control effort.
[controller_server-4] [WARN] [1707501627.895165950] [controller_server]: No goal checker was specified in parameter 'current_goal_checker'. Server will use only plugin loaded general_goal_checker . This warning will appear once.
[ERROR] [controller_server-4]: process has died [pid 3303, exit code -11, cmd '/opt/ros/humble/lib/nav2_controller/controller_server --ros-args -r __node:=controller_server --params-file /home/user/ros2_ws/install/path_planner_server/share/path_planner_server/config/controller.yaml'].
[bt_navigator-5] [WARN] [1707501628.933365417] [bt_navigator_navigate_to_pose_rclcpp_node]: Timed out while waiting for action server to acknowledge goal request for follow_path
[bt_navigator-5] [WARN] [1707501628.953997361] [bt_navigator_navigate_to_pose_rclcpp_node]: Node timed out while executing service call to local_costmap/clear_entirely_local_costmap.
[bt_navigator-5] [WARN] [1707501628.974466530] [bt_navigator_navigate_to_pose_rclcpp_node]: Node timed out while executing service call to local_costmap/clear_entirely_local_costmap.
[bt_navigator-5] [ERROR] [1707501628.974730902] [bt_navigator]: Goal failed
[bt_navigator-5] [WARN] [1707501628.974764335] [bt_navigator]: [navigate_to_pose] [ActionServer] Aborting handle.
[lifecycle_manager-7] [INFO] [1707501632.035103009] [lifecycle_manager]: Have not received a heartbeat from controller_server.
[lifecycle_manager-7] [ERROR] [1707501632.035169450] [lifecycle_manager]: CRITICAL FAILURE: SERVER controller_server IS DOWN after not receiving a heartbeat for 4000 ms. Shutting down related nodes.
[lifecycle_manager-7] [INFO] [1707501632.035189232] [lifecycle_manager]: Terminating bond timer...
[lifecycle_manager-7] [INFO] [1707501632.035205298] [lifecycle_manager]: Resetting managed nodes...
[lifecycle_manager-7] [INFO] [1707501632.035219878] [lifecycle_manager]: Deactivating bt_navigator
[bt_navigator-5] [INFO] [1707501632.036869287] [bt_navigator]: Deactivating
[bt_navigator-5] [INFO] [1707501632.036948923] [bt_navigator]: Destroying bond (bt_navigator) to lifecycle manager.
[lifecycle_manager-7] [INFO] [1707501632.146769323] [lifecycle_manager]: Deactivating recoveries_server
[behavior_server-6] [INFO] [1707501632.147179695] [recoveries_server]: Deactivating
[behavior_server-6] [INFO] [1707501632.147271150] [recoveries_server]: Destroying bond (recoveries_server) to lifecycle manager.
[lifecycle_manager-7] [INFO] [1707501632.254529178] [lifecycle_manager]: Deactivating controller_server
  1. Show the launch files and config files of the path planner and the controller servers

Launch File:

import os
from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch_ros.actions import Node

path_planner_server_config_dir = os.path.join(get_package_share_directory('path_planner_server'), 'config')
map_file = os.path.join(path_planner_server_config_dir, "turtlebot_area.yaml")
amcl_config_file = os.path.join(path_planner_server_config_dir, "amcl_config_initialized.yaml")
planner_config_file = os.path.join(path_planner_server_config_dir, "planner_server.yaml")
controller_config_file = os.path.join(path_planner_server_config_dir, "controller.yaml")
bt_navigator_config_file = os.path.join(path_planner_server_config_dir, "bt_navigator.yaml")
recovery_config_file = os.path.join(path_planner_server_config_dir, "behavior.yaml")

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='nav2_map_server',
            executable='map_server',
            name='map_server',
            output='screen',
            parameters=[{'use_sim_time': True},
                        {"yaml_filename":map_file}],
        ),
        Node(
            package='nav2_amcl',
            executable='amcl',
            name='amcl',
            output='screen',
            parameters=[amcl_config_file],
        ),
        Node(
            package='nav2_planner',
            executable='planner_server',
            name='planner_server',
            output='screen',
            parameters=[planner_config_file],
            # remappings= [('/tf', 'tf'),
            #              ('/tf_static', 'tf_static')]
        ),
        Node(
            package='nav2_controller',
            executable='controller_server',
            name='controller_server',
            output='screen',
            parameters=[controller_config_file],
            # remappings= [('/tf', 'tf'),
            #              ('/tf_static', 'tf_static')]
        ),
        Node(
            package='nav2_bt_navigator',
            executable='bt_navigator',
            name='bt_navigator',
            output='screen',
            parameters=[bt_navigator_config_file],
        ),
        Node(
            package='nav2_behaviors',
            executable='behavior_server',
            name='recoveries_server',
            output='screen',
            parameters=[recovery_config_file],
        ),
        Node(
            package='nav2_lifecycle_manager',
            executable='lifecycle_manager',
            name='lifecycle_manager',
            output="screen",
            parameters=[{"use_sim_time": True},
                        {"autostart": True},
                        {"node_names": ["map_server", 
                                        "amcl", 
                                        "planner_server", 
                                        "controller_server",
                                        "recoveries_server",
                                        "bt_navigator"]}]
        )
    ])

Controller Config File:

controller_server:
  ros__parameters:
    use_sim_time: True
    controller_frequency: 10.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_plugin: "progress_checker"
    goal_checker_plugins: ["general_goal_checker"] 
    controller_plugins: ["FollowPath"]

    # Progress checker parameters
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 10.0
      
    # Goal checker parameters
    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
      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

Planner Config File:

planner_server:
  ros__parameters:
    expected_planner_frequency: 10.0
    use_sim_time: True
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.5
      use_astar: false
      allow_unknown: true

Hi,

I have to write in advance, that my controller also fails and my code worked some weeks (3?) ago.

Please, paste all the content of all the config files.
Namely: recovery, amcl and bt_navigator.
Also please, the xml for bt navigator.

I suspect, that your map is not well aligned to reality.
I see, that the wall of the “temple” is not parallel with the grid. Mine also was not parallel. I advise you to do mapping again:
When you create a map, navigate slowly and go straight or turn but not at the same time. Try to move along all the possible paths and parallel with the grid lines. Do it until your map will be full.

Another thing is that the order of lifecycle managed nodes are important. This is the order in the material:
‘map_server’,
‘amcl’,
‘controller_server’,
‘planner_server’,
‘recoveries_server’,
‘bt_navigator’

Well, I have my nodes in this order and my controller server also fails, but maybe.

Can you check your ROS version?

printenv ROS_DISTRO

There are log files in ~/.ros, if you check them maybe you can find a line with error.

I tried to use a very small config file for controller, no luck.
I also try to debug controller with gdb with adding this line:

prefix=['xterm -e gdb -ex run --args']

to my node setup in the launch file,
, no relevant info in the log. Maybe the issue is in a plugin and gdb can not see that?

[New Thread 0x7fa3b1d0f640 (LWP 23478)]
[INFO] [1707581208.900833051] [local_costmap.local_costmap]: start
[INFO] [1707581209.151050499] [controller_server]: Creating bond (controller_server) to lifecycle manager.
[New Thread 0x7fa3b146f640 (LWP 23565)]
[INFO] [1707581234.042660466] [controller_server]: Received a goal, begin computing control effort.
[WARN] [1707581234.042742637] [controller_server]: No goal checker was specified in parameter 'current_goal_checker'. Server will use only plugin loaded goal_checker . This warning will appear once.

Thread 17 "controller_serv" received signal SIGSEGV, Segmentation fault.
[Switching to Thread 0x7fa3b146f640 (LWP 23565)]
0x00007fa3b253f3bb in costmap_queue::CostmapQueue::enqueueCell(unsigned int, unsigned int, unsigned int, unsigned int, unsigned int) () from /opt/ros/humble/lib/libdwb_critics.so

Thak you in advance
Péter

Hi Peter,

My ROS_DISTRO is humble, and the line of error is:

[ERROR] [controller_server-3]: process has died [pid 7502, exit code -11, cmd '/opt/ros/humble/lib/nav2_controller/controller_server --ros-args -r __node:=controller_server --params-file /home/user/ros2_ws/install/path_planner_server/share/path_planner_server/config/controller.yaml'].

As for the config files (yaml and xml), I simply copied them from the course notbook, and didn’t make any change.

I am not sure what to try next.

Hi @chenlu.han,
My exception is exactly as what you have. Because my code worked some weeks ago maybe other things changed (ros upgrade?).

I have no clue. I literally did 2-3 times everything from zero.

As for the bt_navigator.yaml:
There is a path in that. Please check it twice if it is ok for you:
default_nav_to_pose_bt_xml: “/home/user/ros2_ws/src/path_planner/config/behavior.xml”

There is one thing that I do not understand:
Some of the config files needs robot_base_frame: base_link but others uses
base_footprint instead of base_link, like in amcl_config.yaml:
base_frame_id: “base_footprint” . I do not know which one should I use and why they are different.

Hope @rtellez will help.

Péter

Checking this right now.
There was no update, so the code should work.

Let me have a look

Hi Chenlu and Peter I can certify that there is no problem making the navigation work as specified in the course:

Let me go step by step to see if I can clarify some points. But first, I want to thank Peter very much for his help and comments trying to help Chenlu (even if some points were not 100% correct, check below):

  1. Peter mentioned that the line to specify the path to the behavior.xml inside the bt_navigator.yaml file file should be this:

default_nav_to_pose_bt_xml: “/home/user/ros2_ws/src/path_planner/config/behavior.xml”

THAT IS NOT CORRECT

the correct line is:
default_nav_to_pose_bt_xml: "/home/user/ros2_ws/src/path_planner_server/config/behavior.xml"

The reason why Peter wrote it wrong is because it was wrong in the course material (we confused him). This error was corrected a couple of weeks ago. Sorry Peter!

  1. Peter mentioned that the order in the lifecycle_manager is very important (inside the pathplanner.launch.py), and he is right. However, the order he specifies is not correct. The order should be:
{'node_names': ['planner_server',
                          'controller_server', 
                         'bt_navigator',
                         'recoveries_server']}])
  1. Chenlu, why are you launching everything from a single launch file? That is not the way we are teaching that, and there is a reason: now it is a lot more difficult to find the errors. You should only attempt to put everything in a single launch file, WHEN EVERYTHING WORKS SEPARATELY (that is the final exercise of Unit 5).

So go and separate the files in different launch files for each system.

  1. Your separated files should look like the ones below.
    I overviewed your files and they look correct to me, except that they don’t have the parameters for local and global costmaps. I didn’t check each file line by line.

So it is now your turn: compare your separated files with mine. If they are different, you will get where the error is. If they are not, then both should work.

Let me know what happens, and where was the error in your files.

planner_server.yaml

planner_server:
  ros__parameters:
    expected_planner_frequency: 10.0
    use_sim_time: True
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.5
      use_astar: false
      allow_unknown: true

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      use_sim_time: True
      robot_radius: 0.15
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.35
      always_send_full_costmap: True

controller.yaml

planner_server:
  ros__parameters:
    expected_planner_frequency: 10.0
    use_sim_time: True
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.5
      use_astar: false
      allow_unknown: true

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      use_sim_time: True
      robot_radius: 0.15
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.35
      always_send_full_costmap: True

recovery.yaml

behavior_server:
  ros__parameters:
    costmap_topic: local_costmap/costmap_raw
    footprint_topic: local_costmap/published_footprint
    cycle_frequency: 10.0
    behavior_plugins: ["spin", "backup", "wait"]
    spin:
      plugin: "nav2_behaviors/Spin"
    backup:
      plugin: "nav2_behaviors/BackUp"
    wait:
      plugin: "nav2_behaviors/Wait"
    global_frame: odom
    robot_base_frame: base_link
    transform_timeout: 0.1
    use_sim_time: true
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: 0.4
    rotational_acc_lim: 3.2

bt_navigator.yaml

bt_navigator:
  ros__parameters:
    use_sim_time: True
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    bt_loop_duration: 10
    default_server_timeout: 20
    default_nav_to_pose_bt_xml: "/home/user/ros2_ws/src/path_planner_server/config/behavior.xml"
    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_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_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_drive_on_heading_cancel_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
-->
<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 service_name="global_costmap/clear_entirely_global_costmap"/>
          </RecoveryNode>
        </RateController>
        <RecoveryNode number_of_retries="1" name="FollowPath">
          <FollowPath path="{path}" controller_id="FollowPath"/>
          <ClearEntireCostmap service_name="local_costmap/clear_entirely_local_costmap"/>
        </RecoveryNode>
      </PipelineSequence>
      <SequenceStar name="RecoveryActions">
        <ClearEntireCostmap service_name="local_costmap/clear_entirely_local_costmap"/>
        <ClearEntireCostmap service_name="global_costmap/clear_entirely_global_costmap"/>
        <Spin spin_dist="1.57"/>
        <Wait wait_duration="5"/>
      </SequenceStar>
    </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

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

def generate_launch_description():
    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_behaviors',
            executable='behavior_server',
            name='recoveries_server',
            output='screen',
            parameters=[recovery_yaml]),

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

Hi Ricardo,

Thanks for the explanation! I was indeed missing the parameters for “local_costmap” in controller.yaml and “global_costmap” in planner_server.yaml. Everything seems to be working now, with those 2 sections added in.

It was not directly obvious from the course notebook that we need to add those 2 sections into the provided yaml templates. Maybe you can consider putting a note there to information future students?

Also, the controller.yaml code pasted above is the same as planner_server.yaml. So instead, I am using:

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: ["voxel_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.70
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      always_send_full_costmap: True

from navigation2/nav2_bringup/params/nav2_params.yaml at main · ros-planning/navigation2 · GitHub

Not sure if that is ideal, but seems to work.

Thank you, this was exactly the root cause of my issue also.
I added both global and local map configuration to the planner server configuration.

But (as it is clearly stated in the material),
local costmap config → controller config
global costmap config → planner config

And this is logical. For path planning, we need global costmap. For “dynamic obstacle” avoidance, we need a good path (wich was created based on the static map) and the local costmap.

Maybe, for path planning, we also can take into consideration local costmap to not going into a direction which is impossible because of some dynamic obstacle.

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