Navigation goals not working with laser, sonar and slam_toolbox

edit

Hi there,

my platform is an "almost turtlebot4": we have a create3 platform with a lidar and an array of 7 ultrasound sensors. For navigating we use the turtlebot4 stack (Ros-Galactic). More specifically we can

CONFIG1. build a map with slam toolbox launching the following configuration:

ros2 launch turtlebot4_navigation nav_bringup.launch.py slam:=async  base_frame:=base_link params_file:=my_robot.yaml

CONFIG2. correctly (and by that I mean navigating using both laser and sonar) navigate the built map (previously obtained by CONFIG1) with the following command:

ros2 launch turtlebot4_navigation nav_bringup.launch.py slam:=off localization:=true  base_frame:=base_link map:=$PWD/the_map_create_at_point1.yaml params_file:=my_robot.yaml

CONFIG3: temporarily obtain the desired behavior until a pose estimate is issued by RVIZ2

ros2 launch turtlebot4_navigation nav_bringup.launch.py slam:=synclocalization:=true  base_frame:=base_link map:=$PWD/the_map_create_at_point1.yaml params_file:=my_robot.yaml

However, when using CONFIG1 (with the slam parameter either set to async or sync) I can not launch navigation goals. The controller server ĺog shows the following (notice that the tf clearly states the relation between the following frames and it is the same in all configs):

LOG_ERROR:

"Range sensor layer can't transform from odom to sonarX_frame", where X=[1,7]

In CONFIG2 there are no problems in launching navigation goals (but obviously we have the drawback of not building a map). Interestingly if I mix slam and localization (configuration CONFIG3)  I can see the robot building a map and accepting navigation goals. This works until I set pose by the RVIZ2 interface (at that moment, if I get it right, amcl and slam_toolbox will concurrently update the robot position and hence the flickering..).

Notice that with CONFIG1, the goal navigation can be issued if I remove the sonar layer. Looking a LOG_ERROR above, is it a problem of slam toolbox not handling other sensors than laser scanner  or most likely I did not understand which configuration would give me the 3 features right below I am looking for?

• F1. build a map
• F2. issue navigation goals
• F3. navigate taking into account scan and sonar readings (in terms of planning / obstacle avoidance)

You find attached the robot configuration file where I have added the sonar layer, here below a "summary" of the config

• CONFIG1: it builds the map in any case BUT navigation goals work only if I remove the "sonar_layer" from my_robot_config.yaml.
• CONFIG2: it allows the robot to navigate taking into account both laser and sonar readings
• CONFIG3: it gives me F1+F2+F3 till I set a robot pose in RVIZ, at that point the robot position estimates start to flicker (probably due to concurrent update).

Thanks

File: my_robot_config.yaml ( I am reporting only the part with the sonar layer as copying the entire file does not work in terms for formatting)

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      use_sim_time: True
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      robot_radius: 0.22
      plugins: ["obstacle_layer", "sonar_layer", "inflation_layer" ]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius ...