Need help to solve Ros2 Navigation rosject problems, please

Course Support ROS2 Navigation
, , ,
1

[very long post] [a lot of questions and problems]
Hello there,

source /opt/ros/foxy/setup.bash
cd ~/ros2_ws
colcon build --packages-select project_localization
source ~/ros2_ws/install/setup.bash
ros2 launch project_localization localization.launch.py

localization.launch.py

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    
    nav2_yaml = os.path.join(get_package_share_directory('project_localization'), 'config', 'amcl_config.yaml')
    map_file = os.path.join(get_package_share_directory('project_mapping'), 'maps', 'turtlebot_area.yaml')

    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=[nav2_yaml]
        ),

        Node(
            package='nav2_lifecycle_manager',
            executable='lifecycle_manager',
            name='lifecycle_manager_localization',
            output='screen',
            parameters=[{'use_sim_time': True},
                        {'autostart': True},
                        {'node_names': ['map_server', 'amcl']}]
        )
    ])

I get

user:~/ros2_ws$ ros2 launch project_localization localization.launch.py
[INFO] [launch]: All log files can be found below /home/user/.ros/log/2023-09-27-12-34-06-015666-4_xterm-5230
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [map_server-1]: process started with pid [5255]
[INFO] [amcl-2]: process started with pid [5257]
[INFO] [lifecycle_manager-3]: process started with pid [5259]
[lifecycle_manager-3] [INFO] [1695818047.133908262] [lifecycle_manager_localization]: Creating
[lifecycle_manager-3] [INFO] [1695818047.320844877] [lifecycle_manager_localization]: Creating and initializing lifecycle service clients
[lifecycle_manager-3] [INFO] [1695818047.329914336] [lifecycle_manager_localization]: Starting managed nodes bringup...
[lifecycle_manager-3] [INFO] [1695818047.329964701] [lifecycle_manager_localization]: Configuring map_server
[amcl-2] [INFO] [1695818047.616186365] [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] [1695818047.616348102] [amcl]: Creating
[map_server-1] [INFO] [1695818048.135659223] [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] [1695818048.135825770] [map_server]: Creating
[map_server-1] [INFO] [1695818048.136092978] [map_server]: Configuring
[map_server-1] [INFO] [map_io]: Loading yaml file: /home/user/ros2_ws/install/project_mapping/share/project_mapping/maps/turtlebot_area.yaml
[map_server-1] [DEBUG] [map_io]: resolution: 0.05
[map_server-1] [DEBUG] [map_io]: origin[0]: -1.26
[map_server-1] [DEBUG] [map_io]: origin[1]: -1.84
[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/project_mapping/share/project_mapping/maps/turtlebot_area.pgm
[map_server-1] [DEBUG] [map_io]: Read map /home/user/ros2_ws/install/project_mapping/share/project_mapping/maps/turtlebot_area.pgm: 55 X 50 map @ 0.05 m/cell
[lifecycle_manager-3] [INFO] [1695818048.717614246] [lifecycle_manager_localization]: Configuring amcl
[amcl-2] [INFO] [1695818048.718628685] [amcl]: Configuring
[amcl-2] [INFO] [1695818048.718741088] [amcl]: initTransforms
[amcl-2] [INFO] [1695818048.796532068] [amcl]: initPubSub
[amcl-2] [INFO] [1695818048.804674516] [amcl]: Subscribed to map topic.
[lifecycle_manager-3] [INFO] [1695818048.897780252] [lifecycle_manager_localization]: Activating map_server
[map_server-1] [INFO] [1695818048.898814067] [map_server]: Activating
[lifecycle_manager-3] [INFO] [1695818048.899875442] [lifecycle_manager_localization]: Activating amcl
[amcl-2] [INFO] [1695818048.901178731] [amcl]: Received a 55 X 50 map @ 0.050 m/pix
[amcl-2] [INFO] [1695818048.901764809] [amcl]: Activating
[amcl-2] [WARN] [1695818048.902753679] [amcl]: Publishing the particlecloud as geometry_msgs/PoseArray msg is deprecated, will be published as nav2_msgs/ParticleCloud in the future
[lifecycle_manager-3] [INFO] [1695818048.903968085] [lifecycle_manager_localization]: Managed nodes are active
[amcl-2] [INFO] [1695818049.010629955] [amcl]: createLaserObject
[amcl-2] [WARN] [1695818051.618533520] [amcl]: ACML cannot publish a pose or update the transform. Please set the initial pose...
[amcl-2] [WARN] [1695818054.596964995] [amcl]: ACML cannot publish a pose or update the transform. Please set the initial pose...
[amcl-2] [INFO] [1695818054.710959150] [amcl]: initialPoseReceived
[amcl-2] [WARN] [1695818054.711103009] [amcl]: Failed to transform initial pose in time (Lookup would require extrapolation into the future.Requested time 1695818054.710452 but the latest data is at time 189.783000, when looking up transform from frame [odom] to frame [base_footprint])
[amcl-2] [INFO] [1695818054.711146271] [amcl]: Setting pose (189.749000): -0.024 0.011 -0.030

Is it normal to have this?

[WARN] [1695818576.010740766] [amcl]: Failed to transform initial pose in time (Lookup would require extrapolation into the future.Requested time 1695818576.004997 but the latest data is at time 553.209000, when looking up transform from frame [odom] to frame [base_footprint])

When starting as expected the particles ar spead

image

when moving they concentrate

image

Is it normal that in straight line the points spead more again?

image

amcl_config.yaml

amcl:
  ros__parameters:
    use_sim_time: True
    alpha1: 0.2
    alpha2: 0.2
    alpha3: 0.2
    alpha4: 0.2
    alpha5: 0.2
    base_frame_id: "base_footprint"
    beam_skip_distance: 0.5
    beam_skip_error_threshold: 0.9
    beam_skip_threshold: 0.3
    do_beamskip: false
    global_frame_id: "map"
    lambda_short: 0.1
    laser_likelihood_max_dist: 2.0
    laser_max_range: 3.5
    laser_min_range: 0.1
    laser_model_type: "likelihood_field"
    max_beams: 60
    max_particles: 10000
    min_particles: 1000
    odom_frame_id: "odom"
    pf_err: 0.05
    pf_z: 0.99
    recovery_alpha_fast: 0.0
    recovery_alpha_slow: 0.0
    resample_interval: 1
    robot_model_type: "differential"
    save_pose_rate: 0.5
    sigma_hit: 0.2
    tf_broadcast: true
    transform_tolerance: 0.5
    update_min_a: 0.05
    update_min_d: 0.1
    z_hit: 0.5
    z_max: 0.05
    z_rand: 0.5
    z_short: 0.05

except from this the localization seems to work pretty well and doesn’t have problem with the fact to stop it (ctrl-C) and restart it.

The problems come with the path planning package

source /opt/ros/foxy/setup.bash
cd ~/ros2_ws
colcon build --packages-select project_path_planning
source ~/ros2_ws/install/setup.bash

ros2 launch project_path_planning pathplanner.launch.py

pathplanner.launch.py

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():

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

    
    return LaunchDescription([     
        Node(
            package='nav2_controller',
            executable='controller_server',
            name='controller_server',
            output='screen',
            parameters=[controller_yaml]),

        Node(
            package='nav2_planner',
            executable='planner_server',
            name='planner_server',
            output='screen',
            parameters=[planner_yaml]),
            
        Node(
            package='nav2_recoveries',
            executable='recoveries_server',
            name='recoveries_server',
            parameters=[recovery_yaml],
            output='screen'),

        Node(
            package='nav2_bt_navigator',
            executable='bt_navigator',
            name='bt_navigator',
            output='screen',
            parameters=[bt_navigator_yaml]),

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

I get the following launching, giving in rviz a goal and then when finished a second one

user:~/ros2_ws$ ros2 launch project_path_planning pathplanner.launch.py[INFO] [launch]: All log files can be found below /home/user/.ros/log/2023-09-27-12-59-37-567429-4_xterm-12034
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [controller_server-1]: process started with pid [12059]
[INFO] [planner_server-2]: process started with pid [12061]
[INFO] [recoveries_server-3]: process started with pid [12063]
[INFO] [bt_navigator-4]: process started with pid [12065]
[INFO] [lifecycle_manager-5]: process started with pid [12067]
[lifecycle_manager-5] [INFO] [1695819578.639631503] [lifecycle_manager_pathplanner]: Creating
[lifecycle_manager-5] [INFO] [1695819578.727039032] [lifecycle_manager_pathplanner]: Creating and initializing lifecycle service clients
[lifecycle_manager-5] [INFO] [1695819578.773307518] [lifecycle_manager_pathplanner]: Starting managed nodes bringup...
[lifecycle_manager-5] [INFO] [1695819578.774254045] [lifecycle_manager_pathplanner]: Configuring planner_server
[controller_server-1] [INFO] [1695819579.363613371] [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.
[recoveries_server-3] [INFO] [1695819579.437622052] [recoveries_server]:
[recoveries_server-3]   recoveries_server lifecycle node launched.
[recoveries_server-3]   Waiting on external lifecycle transitions to activate
[recoveries_server-3]   See https://design.ros2.org/articles/node_lifecycle.html for more information.
[bt_navigator-4] [INFO] [1695819579.443781076] [bt_navigator]:
[bt_navigator-4]        bt_navigator lifecycle node launched.
[bt_navigator-4]        Waiting on external lifecycle transitions to activate
[bt_navigator-4]        See https://design.ros2.org/articles/node_lifecycle.html for more information.
[controller_server-1] [INFO] [1695819579.450222121] [controller_server]: Creating controller server
[bt_navigator-4] [INFO] [1695819579.447080356] [bt_navigator]: Creating
[planner_server-2] [INFO] [1695819579.510251556] [planner_server]:
[planner_server-2]      planner_server lifecycle node launched.
[planner_server-2]      Waiting on external lifecycle transitions to activate
[planner_server-2]      See https://design.ros2.org/articles/node_lifecycle.html for more information.
[planner_server-2] [INFO] [1695819579.570956245] [planner_server]: Creating
[planner_server-2] [INFO] [1695819579.769121459] [global_costmap.global_costmap]:
[planner_server-2]      global_costmap lifecycle node launched.
[planner_server-2]      Waiting on external lifecycle transitions to activate
[planner_server-2]      See https://design.ros2.org/articles/node_lifecycle.html for more information.
[planner_server-2] [INFO] [1695819579.788147715] [global_costmap.global_costmap]: Creating Costmap
[controller_server-1] [INFO] [1695819579.883466538] [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] [1695819579.893751613] [local_costmap.local_costmap]: Creating Costmap
[planner_server-2] [INFO] [1695819579.977713979] [planner_server]: Configuring
[planner_server-2] [INFO] [1695819579.977769794] [global_costmap.global_costmap]: Configuring
[planner_server-2] [INFO] [1695819580.003653469] [global_costmap.global_costmap]: Using plugin "static_layer"
[planner_server-2] [INFO] [1695819580.426172710] [global_costmap.global_costmap]: Subscribing to the map topic (/map) with transient local durability
[planner_server-2] [INFO] [1695819580.437577433] [global_costmap.global_costmap]: Initialized plugin "static_layer"
[planner_server-2] [INFO] [1695819580.437650730] [global_costmap.global_costmap]: Using plugin "obstacle_layer"
[planner_server-2] [INFO] [1695819580.442824386] [global_costmap.global_costmap]: Subscribed to Topics:
[planner_server-2] [INFO] [1695819580.442895836] [global_costmap.global_costmap]: Initialized plugin "obstacle_layer"
[planner_server-2] [INFO] [1695819580.442922623] [global_costmap.global_costmap]: Using plugin "inflation_layer"
[planner_server-2] [INFO] [1695819580.443234291] [global_costmap.global_costmap]: Initialized plugin "inflation_layer"
[planner_server-2] [INFO] [1695819580.666167874] [planner_server]: Created global planner plugin GridBased of type nav2_navfn_planner/NavfnPlanner
[planner_server-2] [INFO] [1695819580.666260276] [planner_server]: Configuring plugin GridBased of type NavfnPlanner
[planner_server-2] [INFO] [1695819580.995970706] [planner_server]: Planner Server has GridBased  planners available.
[lifecycle_manager-5] [INFO] [1695819581.038478946] [lifecycle_manager_pathplanner]: Configuring controller_server
[controller_server-1] [INFO] [1695819581.039473804] [controller_server]: Configuring controller interface
[controller_server-1] [INFO] [1695819581.039626955] [controller_server]: Controller frequency set to 5.0000Hz
[controller_server-1] [INFO] [1695819581.039661681] [local_costmap.local_costmap]: Configuring
[controller_server-1] [INFO] [1695819581.054866198] [local_costmap.local_costmap]: Using plugin "static_layer"
[controller_server-1] [INFO] [1695819581.099859251] [local_costmap.local_costmap]: Subscribing to the map topic (/map) with transient local durability
[controller_server-1] [INFO] [1695819581.107947986] [local_costmap.local_costmap]: Initialized plugin "static_layer"
[controller_server-1] [INFO] [1695819581.107998253] [local_costmap.local_costmap]: Using plugin "obstacle_layer"
[controller_server-1] [INFO] [1695819581.108169929] [local_costmap.local_costmap]: Subscribed to Topics:
[controller_server-1] [INFO] [1695819581.108232340] [local_costmap.local_costmap]: Initialized plugin "obstacle_layer"
[controller_server-1] [INFO] [1695819581.108248017] [local_costmap.local_costmap]: Using plugin "inflation_layer"
[controller_server-1] [INFO] [1695819581.108540332] [local_costmap.local_costmap]: Initialized plugin "inflation_layer"
[planner_server-2] [INFO] [1695819581.122888437] [global_costmap.global_costmap]: StaticLayer: Resizing costmap to 55 X 50 at 0.050000 m/pix
[controller_server-1] [INFO] [1695819581.122921974] [local_costmap.local_costmap]: StaticLayer: Resizing costmap to 55 X 50 at 0.050000 m/pix
[controller_server-1] [INFO] [1695819581.301456471] [controller_server]: Created progress_checker : progress_checker of type nav2_controller::SimpleProgressChecker
[controller_server-1] [INFO] [1695819581.308265674] [controller_server]: Created goal_checker : goal_checker of type nav2_controller::SimpleGoalChecker
[controller_server-1] [INFO] [1695819581.617071973] [controller_server]: Created controller : FollowPath of type dwb_core::DWBLocalPlanner
[controller_server-1] [INFO] [1695819581.617374758] [controller_server]: Setting transform_tolerance to 0.200000
[controller_server-1] [INFO] [1695819582.167502549] [controller_server]: Using critic "RotateToGoal" (dwb_critics::RotateToGoalCritic)
[controller_server-1] [INFO] [1695819582.179993933] [controller_server]: Critic plugin initialized
[controller_server-1] [INFO] [1695819582.195718109] [controller_server]: Using critic "Oscillation" (dwb_critics::OscillationCritic)
[controller_server-1] [INFO] [1695819582.203753140] [controller_server]: Critic plugin initialized
[controller_server-1] [INFO] [1695819582.204985826] [controller_server]: Using critic "BaseObstacle" (dwb_critics::BaseObstacleCritic)
[controller_server-1] [INFO] [1695819582.214933220] [controller_server]: Critic plugin initialized
[controller_server-1] [INFO] [1695819582.222937446] [controller_server]: Using critic "GoalAlign" (dwb_critics::GoalAlignCritic)
[controller_server-1] [INFO] [1695819582.243359485] [controller_server]: Critic plugin initialized
[controller_server-1] [INFO] [1695819582.255268704] [controller_server]: Using critic "PathAlign" (dwb_critics::PathAlignCritic)
[controller_server-1] [INFO] [1695819582.268452940] [controller_server]: Critic plugin initialized
[controller_server-1] [INFO] [1695819582.273345961] [controller_server]: Using critic "PathDist" (dwb_critics::PathDistCritic)
[controller_server-1] [INFO] [1695819582.285736215] [controller_server]: Critic plugin initialized
[controller_server-1] [INFO] [1695819582.291096512] [controller_server]: Using critic "GoalDist" (dwb_critics::GoalDistCritic)
[controller_server-1] [INFO] [1695819582.296165467] [controller_server]: Critic plugin initialized
[controller_server-1] [INFO] [1695819582.296231600] [controller_server]: Controller Server has FollowPath  controllers available.
[lifecycle_manager-5] [INFO] [1695819582.397046039] [lifecycle_manager_pathplanner]: Configuring recoveries_server
[recoveries_server-3] [INFO] [1695819582.397510759] [recoveries_server]: Configuring
[recoveries_server-3] [INFO] [1695819582.437351125] [recoveries_server]: Creating recovery plugin spin of type nav2_recoveries/Spin
[recoveries_server-3] [INFO] [1695819582.546638473] [recoveries_server]: Configuring spin
[recoveries_server-3] [INFO] [1695819582.555868906] [recoveries_server]: Creating recovery plugin backup of type nav2_recoveries/BackUp
[recoveries_server-3] [INFO] [1695819582.821622502] [recoveries_server]: Configuring backup
[recoveries_server-3] [INFO] [1695819582.848021049] [recoveries_server]: Creating recovery plugin wait of type nav2_recoveries/Wait
[recoveries_server-3] [INFO] [1695819582.998001527] [recoveries_server]: Configuring wait
[lifecycle_manager-5] [INFO] [1695819583.021042693] [lifecycle_manager_pathplanner]: Configuring bt_navigator
[bt_navigator-4] [INFO] [1695819583.021538671] [bt_navigator]: Configuring
[bt_navigator-4] [INFO] [1695819584.073187443] [bt_navigator_rclcpp_node]: Waiting for "compute_path_to_pose" action server
[bt_navigator-4] [INFO] [1695819584.095292687] [bt_navigator_rclcpp_node]: "ComputePathToPose" BtActionNode initialized
[bt_navigator-4] [INFO] [1695819584.115711059] [bt_navigator_rclcpp_node]: Waiting for "global_costmap/clear_entirely_global_costmap" service
[bt_navigator-4] [INFO] [1695819584.119039746] [bt_navigator_rclcpp_node]: "ClearGlobalCostmap-Context" BtServiceNode initialized
[bt_navigator-4] [INFO] [1695819584.171289209] [bt_navigator_rclcpp_node]: Waiting for "follow_path" action server
[bt_navigator-4] [INFO] [1695819584.175312259] [bt_navigator_rclcpp_node]: "FollowPath" BtActionNode initialized
[bt_navigator-4] [INFO] [1695819584.186917001] [bt_navigator_rclcpp_node]: Waiting for "local_costmap/clear_entirely_local_costmap" service
[bt_navigator-4] [INFO] [1695819584.197435235] [bt_navigator_rclcpp_node]: "ClearLocalCostmap-Context" BtServiceNode initialized
[bt_navigator-4] [INFO] [1695819584.219540799] [bt_navigator_rclcpp_node]: Waiting for "local_costmap/clear_entirely_local_costmap" service
[bt_navigator-4] [INFO] [1695819584.223358278] [bt_navigator_rclcpp_node]: "ClearLocalCostmap-Subtree" BtServiceNode initialized
[bt_navigator-4] [INFO] [1695819584.239954899] [bt_navigator_rclcpp_node]: Waiting for "global_costmap/clear_entirely_global_costmap" service
[bt_navigator-4] [INFO] [1695819584.240911411] [bt_navigator_rclcpp_node]: "ClearGlobalCostmap-Subtree" BtServiceNode initialized
[bt_navigator-4] [INFO] [1695819584.304143406] [bt_navigator_rclcpp_node]: Waiting for "spin" action server
[bt_navigator-4] [INFO] [1695819584.305188798] [bt_navigator_rclcpp_node]: "Spin" BtActionNode initialized
[bt_navigator-4] [INFO] [1695819584.419414957] [bt_navigator_rclcpp_node]: Waiting for "wait" action server
[bt_navigator-4] [INFO] [1695819584.420381044] [bt_navigator_rclcpp_node]: "Wait" BtActionNode initialized
[lifecycle_manager-5] [INFO] [1695819584.430855538] [lifecycle_manager_pathplanner]: Activating planner_server
[planner_server-2] [INFO] [1695819584.431856858] [planner_server]: Activating
[planner_server-2] [INFO] [1695819584.431905778] [global_costmap.global_costmap]: Activating
[planner_server-2] [INFO] [1695819584.431929881] [global_costmap.global_costmap]: Checking transform
[planner_server-2] [INFO] [1695819584.432029496] [global_costmap.global_costmap]: start
[planner_server-2] [INFO] [1695819584.483759106] [planner_server]: Activating plugin GridBased of type NavfnPlanner
[lifecycle_manager-5] [INFO] [1695819584.493933214] [lifecycle_manager_pathplanner]: Activating controller_server
[controller_server-1] [INFO] [1695819584.494676947] [controller_server]: Activating
[controller_server-1] [INFO] [1695819584.494743044] [local_costmap.local_costmap]: Activating
[controller_server-1] [INFO] [1695819584.494768978] [local_costmap.local_costmap]: Checking transform
[controller_server-1] [INFO] [1695819584.494903670] [local_costmap.local_costmap]: start
[lifecycle_manager-5] [INFO] [1695819584.547954012] [lifecycle_manager_pathplanner]: Activating recoveries_server
[recoveries_server-3] [INFO] [1695819584.550521668] [recoveries_server]: Activating
[recoveries_server-3] [INFO] [1695819584.550560222] [recoveries_server]: Activating spin
[recoveries_server-3] [INFO] [1695819584.550584745] [recoveries_server]: Activating backup
[recoveries_server-3] [INFO] [1695819584.550599707] [recoveries_server]: Activating wait
[lifecycle_manager-5] [INFO] [1695819584.556497739] [lifecycle_manager_pathplanner]: Activating bt_navigator
[lifecycle_manager-5] [INFO] [1695819584.559320322] [lifecycle_manager_pathplanner]: Managed nodes are active
[bt_navigator-4] [INFO] [1695819584.557614291] [bt_navigator]: Activating

[bt_navigator-4] [INFO] [1695819711.067383936] [bt_navigator]: Begin navigating from current location to (0.87, -1.00)
[bt_navigator-4] [ERROR] [1695819711.112090088] [bt_navigator]: Actionserver failed while executing action callback: "send_goal failed"
[controller_server-1] [INFO] [1695819711.116510070] [controller_server]: Received a goal, begin computing control effort.
[bt_navigator-4] [WARN] [1695819711.116761738] [bt_navigator]: [navigate_to_pose] [ActionServer] Aborting handle.
[controller_server-1] [INFO] [1695819726.133269484] [controller_server]: Reached the goal!

image
image

Is this error nomal?
[bt_navigator-4] [ERROR] [1695819711.112090088] [bt_navigator]: Actionserver failed while executing action callback: “send_goal failed”

after the second one i get an error and nothing happens exept the path in rviz.

[bt_navigator-4] [INFO] [1695819825.259601650] [bt_navigator]: Begin navigating from current location to (-0.02, -1.05)
[ERROR] [bt_navigator-4]: process has died [pid 12065, exit code -11, cmd '/opt/ros/foxy/lib/nav2_bt_navigator/bt_navigator --ros-args -r __node:=bt_navigator --params-file /home/user/ros2_ws/install/project_path_planning/share/project_path_planning/config/bt_navigator.yaml'].

image

It doesn’t always do this because just after for example just launching again I was able to do it more than 15 times even if some time it doesn’t move and tells me it moved like the following

[bt_navigator-4] [INFO] [1695820341.180784958] [bt_navigator]: Begin navigating from current location to (-0.09, -1.06)
[bt_navigator-4] [INFO] [1695820341.252795107] [bt_navigator]: Navigation succeeded

after having the following error it stopped moving

[bt_navigator-4] [INFO] [1695820596.072455459] [bt_navigator]: Begin navigating from current location to (0.43, -1.15)
[bt_navigator-4] [ERROR] [1695820596.095409652] [bt_navigator]: Actionserver failed while executing action callback: "send_goal failed"
[bt_navigator-4] [WARN] [1695820596.159450482] [bt_navigator]: [navigate_to_pose] [ActionServer] Aborting handle.
[bt_navigator-4] [INFO] [1695820634.607902262] [bt_navigator]: Begin navigating from current location to (-0.20, -1.10)
[bt_navigator-4] [INFO] [1695820644.135158051] [bt_navigator]: Received goal preemption request
[bt_navigator-4] [INFO] [1695820644.135261802] [bt_navigator]: Begin navigating from current location to (-0.09, -1.09)
[bt_navigator-4] [INFO] [1695820652.614209121] [bt_navigator]: Received goal preemption request
[bt_navigator-4] [INFO] [1695820652.614307884] [bt_navigator]: Begin navigating from current location to (-0.20, -1.09)
[bt_navigator-4] [INFO] [1695820670.016338304] [bt_navigator]: Received goal preemption request
[bt_navigator-4] [INFO] [1695820670.016469814] [bt_navigator]: Begin navigating from current location to (-0.23, -1.12)

image

After CTRL-C I get that shows againg that there is a probleme with BT_navigator

^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)
[lifecycle_manager-5] [INFO] [1695820834.870707846] [rclcpp]: signal_handler(signal_value=2)
[INFO] [planner_server-2]: process has finished cleanly [pid 14518]
[recoveries_server-3] [INFO] [1695820834.870758029] [rclcpp]: signal_handler(signal_value=2)
[controller_server-1] [INFO] [1695820834.870793874] [rclcpp]: signal_handler(signal_value=2)
[planner_server-2] [INFO] [1695820834.870813910] [rclcpp]: signal_handler(signal_value=2)
[bt_navigator-4] [INFO] [1695820834.874832189] [rclcpp]: signal_handler(signal_value=2)
[bt_navigator-4] [ERROR] [1695820834.877860412] [bt_navigator]: Actionserver failed while executing action callback: "Failed to create interrupt guard condition in Executor constructor: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at /tmp/binarydeb/ros-foxy-rcl-1.1.11/src/rcl/guard_condition.c:67"
[bt_navigator-4] [WARN] [1695820834.877875962] [bt_navigator]: [navigate_to_pose] [ActionServer] Aborting handle.
[bt_navigator-4] [INFO] [1695820834.879979693] [bt_navigator]: Destroying
[bt_navigator-4] terminate called after throwing an instance of 'rclcpp::exceptions::RCLError'
[bt_navigator-4]   what():  Failed to create interrupt guard conditionin Executor constructor: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at /tmp/binarydeb/ros-foxy-rcl-1.1.11/src/rcl/guard_condition.c:67
[lifecycle_manager-5] [INFO] [1695820834.882125908] [lifecycle_manager_pathplanner]: Destroying
[bt_navigator-4] [PublisherZMQ] Server quitting.
[bt_navigator-4] [PublisherZMQ] just died. Exeption Context was terminated
[bt_navigator-4] [PublisherZMQ] Publisher quitting.
[bt_navigator-4] [PublisherZMQ] just died. Exeption Context was terminated
[INFO] [recoveries_server-3]: process has finished cleanly [pid 14520]
[INFO] [lifecycle_manager-5]: process has finished cleanly [pid 14525]
[INFO] [controller_server-1]: process has finished cleanly [pid 14516]
[ERROR] [bt_navigator-4]: process has died [pid 14522, exit code -6, cmd '/opt/ros/foxy/lib/nav2_bt_navigator/bt_navigator --ros-args -r __node:=bt_navigator --params-file /home/user/ros2_ws/install/project_path_planning/share/project_path_planning/config/bt_navigator.yaml'].

This time When I launch pathplanner again It doesn’t work

^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)
[lifecycle_manager-5] [INFO] [1695820834.870707846] [rclcpp]: signal_handler(signal_value=2)
[INFO] [planner_server-2]: process has finished cleanly [pid 14518]
[recoveries_server-3] [INFO] [1695820834.870758029] [rclcpp]: signal_handler(signal_value=2)
[controller_server-1] [INFO] [1695820834.870793874] [rclcpp]: signal_handler(signal_value=2)
[planner_server-2] [INFO] [1695820834.870813910] [rclcpp]: signal_handler(signal_value=2)
[bt_navigator-4] [INFO] [1695820834.874832189] [rclcpp]: signal_handler(signal_value=2)
[bt_navigator-4] [ERROR] [1695820834.877860412] [bt_navigator]: Actionserver failed while executing action callback: "Failed to create interrupt guard condition in Executor constructor: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at /tmp/binarydeb/ros-foxy-rcl-1.1.11/src/rcl/guard_condition.c:67"
[bt_navigator-4] [WARN] [1695820834.877875962] [bt_navigator]: [navigate_to_pose] [ActionServer] Aborting handle.
[bt_navigator-4] [INFO] [1695820834.879979693] [bt_navigator]: Destroying
[bt_navigator-4] terminate called after throwing an instance of 'rclcpp::exceptions::RCLError'
[bt_navigator-4]   what():  Failed to create interrupt guard conditionin Executor constructor: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at /tmp/binarydeb/ros-foxy-rcl-1.1.11/src/rcl/guard_condition.c:67
[lifecycle_manager-5] [INFO] [1695820834.882125908] [lifecycle_manager_pathplanner]: Destroying
[bt_navigator-4] [PublisherZMQ] Server quitting.
[bt_navigator-4] [PublisherZMQ] just died. Exeption Context was terminated
[bt_navigator-4] [PublisherZMQ] Publisher quitting.
[bt_navigator-4] [PublisherZMQ] just died. Exeption Context was terminated
[INFO] [recoveries_server-3]: process has finished cleanly [pid 14520]
[INFO] [lifecycle_manager-5]: process has finished cleanly [pid 14525]
[INFO] [controller_server-1]: process has finished cleanly [pid 14516]
[ERROR] [bt_navigator-4]: process has died [pid 14522, exit code -6, cmd '/opt/ros/foxy/lib/nav2_bt_navigator/bt_navigator --ros-args -r __node:=bt_navigator --params-file /home/user/ros2_ws/install/project_path_planning/share/project_path_planning/config/bt_navigator.yaml'].

relaunching the localization and then the path planner it restarts working like before.

It seems that my problems come frome the bt-navigator

2

bt_navigator.yaml

# Foxy Version
bt_navigator:
  ros__parameters:
    use_sim_time: True
    global_frame: map
    robot_base_frame: base_footprint
    odom_topic: /odom
    enable_groot_monitoring: True
    groot_zmq_publisher_port: 1666
    groot_zmq_server_port: 1667
    default_bt_xml_filename: "/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

behavior.xml


<!-- Foxy Version -->
<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>

An other problem I get with path planner is the following, where the problem comes from planner server and the localization seems to have stopped on RVIZ

[planner_server-2] [INFO] [1695822004.317905606] [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 canTransformargument target_frame - frame does not exist

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

at this point everything as stopped working
even the gazebo simulated robot

gazebo shell:

[Msg] Loading world file [/home/user/simulation_ws/install/turtlebot3_gazebo/share/turtlebot3_gazebo/worlds/realrobotlab_v1.world]
[gzserver-1] [Wrn] [Publisher.cc:136] Queue limit reached for topic /gazebo/default/physics/contacts, deleting message. This warning is printed only once.

Sorry for the long post but I already spent 2 trying to end the rosject. Everything is done but when I test in sim I always end having the move of the robot stopped or the sim crashed so I can’t hevent test correctly to find if I have a problem or why the robot sometimes turns into a wall
At least it seems I managed to get rid of the Control loop missed its desired rate of 10.0000Hz error i got
following this other topic
but I don’t find the problems remaining

Thank you in advande for your time

regards,
Pablo

3

Other case

Here for example it doesn’t turn to follow his trajectory. Then it stopped
image

Launching it again it restarts and goes to the destination this time

Then when I try the to send the robot back to his start position it starts turning on himself to be in the right angle to follow the calulated path
image
but after being in position it turn on the opposite side and stays blocked. after some tries it doesn’t want to move to goal anymore.
image

[bt_navigator-4] [INFO] [1695823395.695536322] [bt_navigator]: Begin navigating from current location to (-0.57, -1.27)
[controller_server-1] [INFO] [1695823395.728638315] [controller_server]: Received a goal, begin computing control effort.
[controller_server-1] [INFO] [1695823396.930936911] [controller_server]: Passing new path to controller.
[bt_navigator-4] [ERROR] [1695823397.773775100] [bt_navigator]: Actionserver failed while executing action callback: "send_goal failed"
[bt_navigator-4] [WARN] [1695823397.773839391] [bt_navigator]: [navigate_to_pose] [ActionServer] Aborting handle.
[controller_server-1] [ERROR] [1695823410.930549296] [controller_server]: Failed to make progress
[controller_server-1] [WARN] [1695823410.930751057] [controller_server_rclcpp_node]: [follow_path] [ActionServer] Aborting handle.
[bt_navigator-4] [INFO] [1695823470.699267574] [bt_navigator]: Begin navigating from current location to (-0.57, -1.27)
[controller_server-1] [INFO] [1695823470.796092462] [local_costmap.local_costmap]: Received request to clear entirely the local_costmap
[controller_server-1] [INFO] [1695823470.806670359] [controller_server]: Received a goal, begin computing control effort.
[controller_server-1] [INFO] [1695823486.028139748] [controller_server]: Reached the goal!
[bt_navigator-4] [INFO] [1695823486.058404660] [bt_navigator]: Navigation succeeded
[bt_navigator-4] [INFO] [1695823499.394633216] [bt_navigator]: Begin navigating from current location to (0.08, 0.09)
[controller_server-1] [INFO] [1695823499.421031801] [controller_server]: Received a goal, begin computing control effort.
[controller_server-1] [INFO] [1695823499.625448935] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1695823500.622150683] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1695823501.622403957] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1695823502.624769869] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1695823503.624798696] [controller_server]: Passing new path to controller.
[bt_navigator-4] [ERROR] [1695823504.597763979] [bt_navigator]: Actionserver failed while executing action callback: "send_goal failed"
[bt_navigator-4] [WARN] [1695823504.597822712] [bt_navigator]: [navigate_to_pose] [ActionServer] Aborting handle.
[controller_server-1] [INFO] [1695823504.622255881] [controller_server]: Passing new path to controller.
[controller_server-1] [ERROR] [1695823515.022178288] [controller_server]: Failed to make progress
[controller_server-1] [WARN] [1695823515.022387022] [controller_server_rclcpp_node]: [follow_path] [ActionServer] Aborting handle.
[bt_navigator-4] [INFO] [1695823843.552878987] [bt_navigator]: Begin navigating from current location to (0.08, 0.09)
[controller_server-1] [INFO] [1695823843.564391889] [local_costmap.local_costmap]: Received request to clear entirely the local_costmap
[controller_server-1] [INFO] [1695823843.569236104] [controller_server]: Received a goal, begin computing control effort.
[bt_navigator-4] [ERROR] [1695823843.596134718] [bt_navigator]: Actionserver failed while executing action callback: "send_goal failed"
[bt_navigator-4] [WARN] [1695823843.596195984] [bt_navigator]: [navigate_to_pose] [ActionServer] Aborting handle.
[controller_server-1] [INFO] [1695823843.792857103] [controller_server]: Passing new path to controller.
[controller_server-1] [ERROR] [1695823858.570347861] [controller_server]: Failed to make progress
[controller_server-1] [WARN] [1695823858.570561766] [controller_server_rclcpp_node]: [follow_path] [ActionServer] Aborting handle.

With teleop the robot still moves. But the localization freezes.
I then try to restart the localisation but never shows the map in rviz or asks me to set initial pose so I have to restart rviz

[lifecycle_manager-3] [INFO] [1695824390.761100053] [lifecycle_manager_localization]: Managed nodes are active

Other times it just doesn’t follow the green line and ends hitting a wall

Why does the robot has that strange behavior? Why does everything blocks like that ?

4

Your post is super lengthy. I think I understand what you mean but try and summarize it in a couple of sentences.

What is happening to the robot? What are you expecting to happen?

From what I gather, your nav2 stack works, but sometimes the goal fails and the robot doesn’t get there. That could be for a lot of different reasons.

My guess here is that the global/local costmaps configured are preventing some of the goals from succeeding. You should add them to rviz2, otherwise it is very hard to debug.

Another thing that might be going on is errors in the communication because of the underlying RMW implementation. Since this rosject is Foxy, you can try setting a different implementation in every terminal where you launch your nodes. It’s this environment variable before launching anything (or you can also set it in your .bashrc:

export RMW_IMPLEMENTATION=rwm_cyclonedds_cpp

5

Hello,

yes sorry I understand it is too long and maybe also messy.

This is the most common error I get relaunching my path planer after having rebuilt the package after a change in one of the config files. This also occurs when the path planner craches with one component dying.

[lifecycle_manager-5] [INFO] [1695978888.060298396] [lifecycle_manager_pathplanner]: Activating planner_server
[planner_server-2] [INFO] [1695978888.060608627] [planner_server]: Activating
[planner_server-2] [INFO] [1695978888.060649142] [global_costmap.global_costmap]: Activating
[planner_server-2] [INFO] [1695978888.060666834] [global_costmap.global_costmap]: Checking transform
[planner_server-2] [INFO] [1695978888.060696893] [global_costmap.global_costmap]: Timed out waiting for transform from base_link to map to become available, tf error: Invalid frame ID "base_link" passed to canTransform argument source_frame - frame does not exist

It seems that it even the gazebo sim crashes (because i cant teleop the robot and it doesn’t publishes) . So to make everything work again i have to restart everything.

The rviz communication seems also interrupted because it doesn’t receives the map from the restarted map server and the map server doesn’t receives the initial pose estimate it is asking when i put it in rviz.

It seems strange that i cant modify a config file without having to relaunch everything.

My costmaps look like this maybe I should leave more space reducing the inflation.
image

I tried to export what you told me but it doesn’t work

user:~/ros2_ws$ export RMW_IMPLEMENTATION=rwm_cyclonedds_cpp
user:~/ros2_ws$ source /opt/ros/foxy/setup.bash
user:~/ros2_ws$ cd ~/ros2_ws
user:~/ros2_ws$ colcon build --packages-select project_localization
Starting >>> project_localization
Finished <<< project_localization [3.28s]

Summary: 1 package finished [3.88s]
user:~/ros2_ws$ source ~/ros2_ws/install/setup.bash
user:~/ros2_ws$ ros2 launch project_localization localization.launch.py
[INFO] [launch]: All log files can be found below /home/user/.ros/log/2023-09-29-09-06-03-698836-3_xterm-27089
[INFO] [launch]: Default logging verbosity is set to INFO
[ERROR] [1695978363.870848664] [rcl]: Error getting RMW implementation identifier / RMW implementation not installed (expected identifier of 'rwm_cyclonedds_cpp'), with error message 'failed to find shared library 'rwm_cyclonedds_cpp', at /tmp/binarydeb/ros-foxy-rmw-implementation-1.0.2/src/functions.cpp:73', exiting with 1., at /tmp/binarydeb/ros-foxy-rcl-1.1.11/src/rcl/rmw_implementation_identifier_check.c:139
6

It works better now (I don’t know why the colors changed)
image

The fact that it does stops on the destination is because of the particles?

I also have the case where it tells me the navigation was done with success but it just didn’t do anything
edit: after sending 4 times goal it finally moved
image

7

So… what is the summary? What are the answers to the questions I asked? You go straight into talking about your errors, and what I asked in order to help you was:

try and summarize it in a couple of sentences.

What is happening to the robot? What are you expecting to happen?

If you can’t teleop the robot, then it can be either the simulation (for which a restart is the correct fix) or the terminal, so you can restart sim or restart terminal.

You state:

This is the most common error I get relaunching…

But if you look closely, the terminal output you shared is not an error, it is an INFO stream

  • It is strange that you have to restart everything after changing a config file. Try separating your launches in order to only kill the nodes you are changing the config for.

  • While debugging, you will have to restart rviz2 a lot, yes.

  • Your costmaps seem huge. Do you see why the robot would have a difficult time finding a solution to the goal sent? What about the robot footprint? Is it too big for the robot?

  • I see that the RMW implementation I mentioned isn’t installed in this rosject. But I really don’t think that’s the issue here, so you can ignore it.

You are almost there. It sounds like you understand your system and just have to tune it.

If you want to continue to get help, please remember to be clearer in your questions, and summarized. We can’t mentally jump into your problem the same way you are doing it, so it is really hard to understand with a long post like this.

Try answering my questions and you might even get ideas of what you need to do yourself.

8

Sorry, it was a little bit complicated

The summary:

-Is thetranslation spread normal compared to the rotation that concentrates the particles? Reducing the translation noise I finally reduced it.

-I was worried about this message. But I ended up finding an answer

[bt_navigator-4] [ERROR] [1695819711.112090088] [bt_navigator]: Actionserver failed while executing action callback: "send_goal failed"
  • A lot of times when I give a goal I get, And I have to launch everything again. So I have a problem with BT-navigator
[bt_navigator-4] [INFO] [1695819825.259601650] [bt_navigator]: Begin navigating from current location to (-0.02, -1.05)
[ERROR] [bt_navigator-4]: process has died [pid 12065, exit code -11, cmd '/opt/ros/foxy/lib/nav2_bt_navigator/bt_navigator --ros-args -r __node:=bt_navigator --params-file /home/user/ros2_ws/install/project_path_planning/share/project_path_planning/config/bt_navigator.yaml'].
  • I also often get a problem of transform
[planner_server-2] [INFO] [1695822004.317905606] [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 canTransformargument target_frame - frame does not exist

  • I had a problem of blocking behavior of the robot but you were right it certainly was because of theglobal and local costmaps where the inflation was too big for small sized zone.

  • The robot never goes to the end of the path and sometimes really ends far. I suppose it is because of the particles orbecause of the accepted margin where it is considered arrived to the goal.

    image
    image305×348 22.3 KB

Following your last message

  • I know I have to restart when I get the Info message. I will try separating the the launch files
[planner_server-2] [INFO] [1695978888.060696893] [global_costmap.global_costmap]: Timed out waiting for transform from base_link to map to become available, tf error: Invalid frame ID "base_link" passed to canTransform argument source_frame - frame does not exist
  • As I thought it only shows the data frome different topics, I didn’t thought it would have an impact requiring to restart it. I juste used to click reset to erase the previous launch remaining data
  • The foot print is the one from the course. But the inflation was too big it works better now even if sometimes I have to send it multiple time before having it to start moving. Sometimes it is strange it tell me navigation succeed but it didn’t move to de goal

I will still test

Thank you for you patience

9

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