Path planning error - ROS2 Navigation course

Course Support ROS2 Navigation
1

Hi everyone
I’m trying to run the path planning system for the final project.
Nodes seem to start in a proper way:

[planner_server-4] [INFO] [1659458628.243002277] [global_costmap.global_costmap]: start
[planner_server-4] [INFO] [1659458628.298515465] [planner_server]: Activating plugin GridBased of type NavfnPlanner
[lifecycle_manager-7] [INFO] [1659458628.299315896] [lifecycle_manager_pathplanner]: Activating controller_server
[controller_server-3] [INFO] [1659458628.299667729] [controller_server]: Activating
[controller_server-3] [INFO] [1659458628.299748777] [local_costmap.local_costmap]: Activating
[controller_server-3] [INFO] [1659458628.299778689] [local_costmap.local_costmap]: Checking transform
[controller_server-3] [INFO] [1659458628.299901549] [local_costmap.local_costmap]: start
[lifecycle_manager-7] [INFO] [1659458628.399684284] [lifecycle_manager_pathplanner]: Activating recoveries_server
[recoveries_server-5] [INFO] [1659458628.400233953] [recoveries_server]: Activating
[recoveries_server-5] [INFO] [1659458628.400294647] [recoveries_server]: Activating spin
[recoveries_server-5] [INFO] [1659458628.400319038] [recoveries_server]: Activating backup
[recoveries_server-5] [INFO] [1659458628.400337537] [recoveries_server]: Activating wait
[lifecycle_manager-7] [INFO] [1659458628.401484026] [lifecycle_manager_pathplanner]: Activating bt_navigator
[bt_navigator-6] [INFO] [1659458628.401778170] [bt_navigator]: Activating
[lifecycle_manager-7] [INFO] [1659458628.402386041] [lifecycle_manager_pathplanner]: Managed nodes are active

Nevertheless, when sending Nav2 Goals through Rviz2, I get the next errors:

[rviz2-8] [INFO] [1659458663.932603475] [rviz2]: Setting goal pose: Frame:map, Position(0.637056, -0.733013, 0), Orientation(0, 0, -0.924357, 0.381528) = Angle: -2.35869
[bt_navigator-6] [INFO] [1659458663.936166614] [bt_navigator]: Begin navigating from current location to (0.64, -0.73)
[bt_navigator-6] [ERROR] [1659458663.937610264] [bt_navigator]: Action server failed while executing action callback: "One of the children of a DecoratorNode or ControlNode is nullptr"
[bt_navigator-6] [WARN] [1659458663.938555277] [bt_navigator]: [navigate_to_pose] [ActionServer] Aborting handle.

The configuration of my BT node is the following:

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/project_path_planning/config/behavior.xml"
    plugin_lib_names:
    - nav2_compute_path_to_pose_action_bt_node
    - nav2_follow_path_action_bt_node
    - nav2_back_up_action_bt_node
    - nav2_spin_action_bt_node
    - nav2_wait_action_bt_node
    - nav2_clear_costmap_service_bt_node
    - nav2_is_stuck_condition_bt_node
    - nav2_goal_reached_condition_bt_node
    - nav2_goal_updated_condition_bt_node
    - nav2_initial_pose_received_condition_bt_node
    - nav2_reinitialize_global_localization_service_bt_node
    - nav2_rate_controller_bt_node
    - nav2_distance_controller_bt_node
    - nav2_speed_controller_bt_node
    - nav2_truncate_path_action_bt_node
    - nav2_goal_updater_node_bt_node
    - nav2_recovery_node_bt_node
    - nav2_pipeline_sequence_bt_node
    - nav2_round_robin_node_bt_node
    - nav2_transform_available_condition_bt_node
    - nav2_time_expired_condition_bt_node
    - nav2_distance_traveled_condition_bt_node

The BT is defined by using the same XML that is provided in the course:

<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>

I don’t know what could be the problem.
Thanks in advance again!

2

Hi again
I changed the default_nav_to_pose_bt_xml tag to default_bt_xml_filename the goal seems to be reached.
Nevertheless, I get the messages

[controller_server-3] [WARN] [1659526861.043168858] [controller_server]: Control loop missed its desired rate of 10.0000Hz
[controller_server-3] [WARN] [1659526861.143213501] [controller_server]: Control loop missed its desired rate of 10.0000Hz
[bt_navigator-6] [ERROR] [1659526861.546625929] [bt_navigator]: Action server failed while executing action callback: "send_goal failed"
[bt_navigator-6] [WARN] [1659526861.546698208] [bt_navigator]: [navigate_to_pose] [ActionServer] Aborting handle.
[controller_server-3] [INFO] [1659526861.636290263] [controller_server]: Passing new path to controller.
[controller_server-3] [WARN] [1659526864.940740861] [controller_server]: Control loop missed its desired rate of 10.0000Hz
[controller_server-3] [WARN] [1659526867.038560374] [controller_server]: Control loop missed its desired rate of 10.0000Hz
[controller_server-3] [INFO] [1659526869.547421058] [controller_server]: Reached the goal!

What does the error Action server failed while executing action callback: "send_goal failed" means? and the warning Control loop missed its desired rate of 10.0000Hz?

Furthermore, from time to time I get the next error from Rviz2 ¿?

[rviz2-8] rviz2: /tmp/binarydeb/ros-foxy-rviz-ogre-vendor-8.2.3/obj-x86_64-linux-gnu/ogre-v1.12.1-prefix/src/ogre-v1.12.1/OgreMain/src/OgreNode.cpp:626: void Ogre::Node::setScale(const Vector3&): Assertion `!inScale.isNaN() && "Invalid vector supplied as parameter"' failed.
[ERROR] [rviz2-8]: process has died [pid 20084, exit code -6, cmd '/opt/ros/foxy/lib/rviz2/rviz2 -d /home/user/ros2_ws/install/project_path_planning/share/project_path_planning/config/project_path_planning_rviz.rviz --ros-args -r __node:=rviz2 --params-file /tmp/launch_params_wootv_05'].

Thanks again!!

3

Laura, it happens that the course is teaching you the Nav2 for Galactic, but the project is based on Foxy. This means that some things we teach for Galactic are not available for Foxy.

For instance, the default bt XML file parameter has been changed from default_bt_xml_filename in Foxy to default_nav_to_pose_bt_xml in Galactic.

Also, there are some plugin_lib_names that do not exist in Foxy.

Those differences between ROS2 versions can produce strange errors. So let me provide you with a Foxy example of config files that you can use to compare with yours.

For the BT Navigator

bt_navigator:
  ros__parameters:
    use_sim_time: True
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    enable_groot_monitoring: True
    groot_zmq_publisher_port: 1666
    groot_zmq_server_port: 1667
    default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
    plugin_lib_names:
    - nav2_compute_path_to_pose_action_bt_node
    - nav2_follow_path_action_bt_node
    - nav2_back_up_action_bt_node
    - nav2_spin_action_bt_node
    - nav2_wait_action_bt_node
    - nav2_clear_costmap_service_bt_node
    - nav2_is_stuck_condition_bt_node
    - nav2_goal_reached_condition_bt_node
    - nav2_goal_updated_condition_bt_node
    - nav2_initial_pose_received_condition_bt_node
    - nav2_reinitialize_global_localization_service_bt_node
    - nav2_rate_controller_bt_node
    - nav2_distance_controller_bt_node
    - nav2_speed_controller_bt_node
    - nav2_truncate_path_action_bt_node
    - nav2_goal_updater_node_bt_node
    - nav2_recovery_node_bt_node
    - nav2_pipeline_sequence_bt_node
    - nav2_round_robin_node_bt_node
    - nav2_transform_available_condition_bt_node
    - nav2_time_expired_condition_bt_node
    - nav2_distance_traveled_condition_bt_node

Behavior XML:

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

The error Action server failed while executing action callback: "send_goal failed", according to this post by the Nav2 creator, that is an error that only happens on Nav2 for Foxy, so it is solved in Galactic. Do not worry about it, since the error is not actually preventing the robot to reach the goal

About the error Control loop missed its desired rate of 10.0000Hz this means that the controller server cannot compute everything at 10Hz, as you indicated in the controller.yaml file parameter controller_frequency: 10.0.
This is due to the computer not being able to handle all the load. This is an important error message because it means two things:

  1. Either you requested a too fast control loop (your robot may not require that high control speed). If that is the case, you can just decrease the value of the parameter and it will be ok
  2. Given the speed at which your robot moves, you need at least that control frequency, otherwise the robot can crash. In that case, you need to decrease both the controller_frequency parameter but also the maximum speeds at which your robot can move, so, by decreasing the control loop frequency, your robot will not crash. In this case you need to play with the following parameters of the controller.yaml 
      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

About the error

[rviz2-8] rviz2: /tmp/binarydeb/ros-foxy-rviz-ogre-vendor-8.2.3/obj-x86_64-linux-gnu/ogre-v1.12.1-prefix/src/ogre-v1.12.1/OgreMain/src/OgreNode.cpp:626: void Ogre::Node::setScale(const Vector3&): Assertion `!inScale.isNaN() && "Invalid vector supplied as parameter"' failed.

Do not worry, is not preventing you from using Rviz.
Rviz is not that stable, so you should experience some crashes from time to time. That is why you must save your Rviz configuration.

3 Likes
4

Thanks @rtellez!!
I would also like to know: is there any restriction in Galactic in order to merged localization (map_server, amcl) and path planning nodes (controller_server, planner_server, recoveries_server, bt_navigator) in one launch file?
I’m trying to do it. The nodes seem to start in a proper way but when I send a goal I get the errors:

[lifecycle_manager-7] [INFO] [1659688377.725010021] [lifecycle_manager_pathplanner]: Managed nodes are active
[lifecycle_manager-7] [INFO] [1659688377.725027042] [lifecycle_manager_pathplanner]: Creating bond timer...
[rviz2-8] [INFO] [1659688378.733459152] [rviz2]: Trying to create a map of size 125 x 116 using 1 swatches
[rviz2-8] [ERROR] [1659688378.878381432] [rviz2]: Vertex Program:rviz/glsl120/indexed_8bit_image.vert Fragment Program:rviz/glsl120/indexed_8bit_image.frag GLSL link result : 
[rviz2-8] active samplers with a different type refer to the same texture image unit
[rviz2-8] [INFO] [1659688378.959849227] [rviz2]: Trying to create a map of size 125 x 116 using 1 swatches
[rviz2-8] [INFO] [1659688389.930625585] [rviz2]: Setting goal pose: Frame:map, Position(2.56423, -0.102569, 0), Orientation(0, 0, -0.0379031, 0.999281) = Angle: -0.0758243
[bt_navigator-6] [INFO] [1659688389.937629322] [bt_navigator]: Begin navigating from current location to (2.56, -0.10)
[controller_server-3] [INFO] [1659688390.019221668] [controller_server]: Received a goal, begin computing control effort.
[controller_server-3] [WARN] [1659688390.021076197] [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.
[rviz2-8] [INFO] [1659688391.073878551] [rviz2]: Message Filter dropping message: frame 'base_scan' at time 32.639 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[rviz2-8] [INFO] [1659688391.074016055] [rviz2]: Message Filter dropping message: frame 'base_scan' at time 32.639 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[bt_navigator-6] [WARN] [1659688391.082604178] [bt_navigatornavigate_to_pose_rclcpp_node]: Timed out while waiting for action server to acknowledge goal request for follow_path
[bt_navigator-6] [WARN] [1659688391.112063942] [bt_navigatornavigate_to_pose_rclcpp_node]: Node timed out while executing service call to local_costmap/clear_entirely_local_costmap.
[bt_navigator-6] [WARN] [1659688391.145681575] [bt_navigatornavigate_to_pose_rclcpp_node]: Node timed out while executing service call to local_costmap/clear_entirely_local_costmap.
[recoveries_server-5] [INFO] [1659688391.165261180] [recoveries_server]: Attempting spin
[recoveries_server-5] [INFO] [1659688391.183151511] [recoveries_server]: Turning 1.57 for spin recovery.
[recoveries_server-5] [ERROR] [1659688391.287616232] [recoveries_server]: Pose Goes Off Grid.
[recoveries_server-5] [WARN] [1659688391.287689439] [recoveries_server]: Collision Ahead - Exiting Spin
[recoveries_server-5] [WARN] [1659688391.287709051] [recoveries_server]: spin failed
[recoveries_server-5] [WARN] [1659688391.287739566] [recoveries_server]: [spin] [ActionServer] Aborting handle.
[recoveries_server-5] [INFO] [1659688391.325273311] [recoveries_server]: Attempting wait
[rviz2-8] [INFO] [1659688391.347848420] [rviz2]: Message Filter dropping message: frame 'base_scan' at time 32.839 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[rviz2-8] [INFO] [1659688391.354766366] [rviz2]: Message Filter dropping message: frame 'base_scan' at time 32.839 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[ERROR] [controller_server-3]: process has died [pid 151556, exit code -11, cmd '/opt/ros/galactic/lib/nav2_controller/controller_server --ros-args -r __node:=controller_server --params-file /home/laura/turtlebot3_ws/install/nav_through_poses/share/nav_through_poses/config/controller.yaml'].
[lifecycle_manager-7] [INFO] [1659688394.125922880] [lifecycle_manager_pathplanner]: Have not received a heartbeat from controller_server.

It has not sense because I don’t have problems whan launching them from different packages ¿?
Thanks again