"Queue is Full" Error

[retrodorian]

Hi there, I am using the following configuration on my robot:
-Raspberry Pi4 (4GB RAM)
-Teensy4.1
-Ubuntu 22.04
-ROS2 Humble

I am using the humble branch of the linorobot2_repository and linorobot2_hardware (I did not chose them manually, the installation process is followed exactly). At first, I had faced some issues with the hardware side, the firmware could not uploaded properly to the teensy4.1. However I managed to solved the problem with switching to the @hippo5329 's hardware fork. Now I have a different issue. when I start the navigation, I am getting these errors:
Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist
[rvizz-2] [INFO] [1738951374.343872793] [rvizz]: Message Filter dropping message: frame 'odom' at time 1738951372.251 for reason 'discarding message because the queue is full'
[rvizz-2] [INFO] [1738951374.343872793] [rvizz]: Message Filter dropping message: frame 'odom' at time 1738951372.251 for reason 'discarding message because the queue is full'
[rvizz-2] [INFO] [1738951374.343872793] [rvizz]: Message Filter dropping message: frame 'odom' at time 1738951372.251 for reason 'discarding message because the queue is full'

[component_container_isolated-1] [INFO] [1738951374.727089274] [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.

What should I do? Thank you beforehand.

[hippo5329]

Have you walk through nav2 tutorial and slam tutorial with simulation on tb3/tb4?

Next, you should run slam with your robot,

https://github.com/hippo5329/linorobot2_hardware/wiki#nav2-slam

[retrodorian]

Thank you for your quick response. Yes actually I do some simulations with tb3. I do not face this problem with ubuntu20.04 and ros2 foxy. I am getting the same error only at humble branch. I am using 2 different SD cards in order to not create a conflict between anything. Also the hardware side is not working very well with the main branch or your branch at humble. I can upload the firmware to teensy41 with lots of failed trails at every time. Do you have an idea why this problem occurs. I do not face any problem related to hardware side at foxy also.

[hippo5329]

Maybe you can install native ubuntu to a partition or external USB disk on your laptop. And try out on your laptop.

I do not have pi 4. I have only pi 5. Did you add external power to teensy41? You should no use external power on teensy. You should power teensy via USB.

I do not use teensy. Or you can get a pico to replace teensy.

[retrodorian]

Again, thank you for your quick response, I am not using any external power source to teensy. I am powering the motors externally. The encoders, imu and bts7960 is powered by teensy. The ground is common of course. Also I tested the upload process both with and without wiring. The issues are same, I am getting "Error 1" most of the time. But when I successfully upload the firmware, I can connect via microROS and I can read the IMU data (I tested with mpu6050 and I will test with 9250.). Also when I send some data via teleop, the teensy orange led is blinking as it should.

I controlled hz with ros2 topic hz /imu/data, and it is 50.0 +-0.5 I believe it should be just fine.

Is the teensy related with the error output of the nav2 stack? If yes, why I do not face the same problem at foxy? I do getting different errors at foxy, but I can do some nav2 goal operation also. To me, It is not related to teensy.

Thank you for your interest.

[hippo5329]

Are you able to run the nav2 slam as the wiki link above?

[grassjelly]

have you tried switching to cyclone dds?

[retrodorian]

Thank you both @hippo5329 and @grassjelly for your response.
I had tried and created a successful map with Linorobot's slam operation at both foxy and humble. I managed to do some navigation operation with Linorobot's navigation at foxy (not every time, after some time I got lots of error) but I could not make any navigation at humble with Linorobot's navigation.

Also I tried cyclone dds at humble but I am getting the same error "queue is full", I had not tried it at foxy yet.

Also I got an ice tower active cooler and heatsink from geekpi and I will test the foxy setup with overclock to see is it related with performance issue. Because when I check the the status of the pi from htop, all 4 cpu core's are at %99 - %100 when the crashes are happening. and I can clearly see the controller_server and planner_server misses the desired rate of 20.0000 Hz at foxy setup. Maybe the cyclone dds and overclock can solve this issue. I had tried to decrease the 20.0000 to 15.0000 gradually (by 1.0) and when I do this the robot behavior at navigation operation is not accurate and it always misses the path, the localization is also got corrupted. What you think, do you have any idea that I can try? (as I mentioned, these are the outputs of the foxy setup, I could not do any navigation on humble)

Other errors I got at foxy:

[WARN] [1737702882.822527732] [amcl]: Failed to transform initial pose in time (Lookup would require extrapolation into the future. Requested time 1737702882.821602 but the latest data is at time 1737702882.777000, when looking up transform from frame [base_footprint] to frame odom]).
-[INFO] [1737702882.8226656721 [amcl]: Setting pose (1737702882.822662): 6.022 3.020 - 0.019
[rviz2-10] Start navigation
[rviz2-101 ][ERROR] [1737702904.319517746] [rviz2]: FollowWaypoints action server is not available. Is the initial pose set?

-controller_server-4][WARN] [1737702107.230180978] [DWBLocalPlanner]: A scoring function failed to prepare
controller_server-4]
[ERROR] [1737702107.230275551] [GoalDistCritic]: None of the points of the global plan were in the local costmap.
controller_server-4]
[WARN] [1737702107.230297884] [DWBLocalPlanner]: A scoring function
controller_server-4]
[ERROR] [1737702107.262903250] [DWBLocalPlanner]: No valid trajectories out of 419!
controller_server-4]
controller_server-4]
[ERROR] [1737702107.264442277] [controller_server]: No valid trajectories out of 419!
controller_server-4]
[WARN] [1737702107.266292866] [controller_server_rclcpP_node]: [follow_path] [ActionServer] Aborting

I really appreciate both of your interest and help! Thank you so much again.

[hippo5329]

How much does your robot weight? Please run test_acc to find the max acc and vel with full load. It would be better to add a battery sensor to your teensy adc pin. Please post photos of your wiring. It is recommended to getting started with a small light-weight robot.

[retrodorian]

@hippo5329 thank you so much! I will gather all information and add here in 2 days (I don't have the access to the lab atm). Should I use the combination of the hardware fork of yours and the @grassjelly linorobot2_ws (for humble) or should I start from scratch with your wiki?. I think there should be no any issue with the combination of your hardware and linorobot2_ws (for humble). Also I did not understand the relation between the "queue is full" error and the robots weight because I could not even initialize the navigation launch, the map is not published and I can not see it at rviz due to the error. But of course I will test all the mentioned issues at humble.

[hippo5329]

There can be multiple sources of errors in your configuration.

1. Try navigation in nav2 instead of linorobot2.

https://github.com/hippo5329/linorobot2_hardware/wiki#nav2-navigate-with-a-map

2. micro-SD card issue. What is the write speed your micro-SD? You should have fast write speed micro-SD. It better to get A2 type. Pi 5 is much faster than Pi 4 in both CPU performance and SD access.

[hippo5329]

My forks of linorbot2_hardware and linorobot2 are my development branches. I would submit pull requests to upstream linorobot when I think appropriate. They should work the same. You can use or mix both.

[hippo5329]

You may install linorobot2 with

cd ~
mkdir -p linorobot2_ws/src
cd linorobot2_ws/src
git clone https://github.com/Slamtec/sllidar_ros2.git
git clone https://github.com/linorobot/linorobot2.git
cd linorobot2
touch linorobot2_gazebo/COLCON_IGNORE
git remote add hippo https://github.com/hippo5329/linorobot2.git
git fetch hippo
cd ../..
rosdep install --from-paths src --ignore-src -r -y
colcon build --symlink-install

Then add the env to .bashrc

export LINOROBOT2_BASE=2wd
export LINOROBOT2_LASER_SENSOR=a1
export LINOROBOT2_DEPTH_SENSOR=
source $HOME/linorobot2_ws/install/setup.bash

This assume micro-ros agent installed in ~/uros_ws as in my wiki. You may later check out my branch to test.

[retrodorian]

thank you so much @hippo5329 . You are the best!

As you suggested, I am suspected about the write and read rates of the sd card that I use. My sd card specs:
SanDisk Ultra 32 GB 120 MB/s A1 Class 10 UHS-I.

I saw that it is even suggested to use an external usb SSD rather than a SD card. At first, It seemed a little over kill but maybe that is the issue. I will test the foxy setup since I can initialize navigation with overclock and decide whether should I upgrade my sd card or not.

I also will test directly nav2.

I really appreciate your interest and help! Thank you so much again.

[hippo5329]

Write speed is more important. You should skip the read speed and look for better write speed. Sandisk Ultra is not fast in write speed. PNY 64GB/256GB Premier-X Class 10 U3 V30 get much better write speed.

Some USB SSD are really fast on USB 3.0/3.2 port. Don't use USB 2.0 for mass storage. They are not so expensive now. Last month, I bought a Silicon Power 250GB MS70 USB 3.2 with read/write speed at 1000M/800M. It was on sale for $20.

To use USB SSD on Pi 4/5, you will need to add "usb_max_current_enable=1" to config.txt.