[ WARN] Too many anomalous beams! Is the robot lost? Skipping map change detection...

[Nick-Kou]

Thank you for the great work. After attempting to run this node on my custom robot in a simulated AWS small warehouse world with added clutter, it appears as though the map updates do not process and instead I receive a ROS warning message stating: "Too many anomalous beams! Is the robot lost? Skipping map change detection..." After changing "max_anomalous_beam_fraction" to a larger value, almost all the occupied cells in the occupancy grid map were being erased and I was not getting the correct expected behaviour as shown in the videos presented. In simulation my robot is modelled as a tricycle kinematic model and equipped with 1 rplidar using the libgazebo_ros_laser.so plugin with a 360 degree field of view. I greatly appreciate any assistance you can provide.

[elisastefanini]

Hi Nick-Kou, sorry for the late response.
could you tell me what you use as a localization algorithm?
The others parameters that depend on the type of robot are: min_linear_displacement and min_angular_displacement, you can try to increase them. In addition, the parameter max_trusted_angular_velocity refers to the maximum angular velocity of the robot that allows the map updating. If your localization system is not good enough, probably you should set this value with a lower one.
Moreover, you can try to change the max_beam_range parameter, this depends on the size of the room, the bigger it is, the higher it should be.
Let me know if something change!
Thank you

[Nick-Kou]

Hi, thanks for your response. I am using AMCL for localization as shown in the original paper. In simulation I use ground truth odom to ensure it is reliable and to avoid that as a potential root cause. I have reverted back to the default parameters in this repository and modified the ones you mentioned above. For the first experiment I set the min_linear_displacement to 0.5, increased the beam range to 30.0, and increased the min angular displacement to 55.0. In this experiment I was receiving a combination of the 2 warnings for anomalous beams and going over the trusted velocity. Occasionally I also observed the following message:

[ INFO] [1679537695.042245970, 2308.547000000]: Total: 0.118000s (find: 0.012000s, anom: 0.063000s, normal: 0.038000s, plot: 0.005000s). Mean: 0.128455s; max: 0.227000s
[ INFO] [1679537695.150864369, 2308.630000000]: Service successfully called.
However in rviz by viewing the /processed_map topic it appears as though no map updates were performed at all even though I applied significant changes in the gazebo world environment.

In the second experiment I then increased the max_trusted_angular_velocity to 25 to avoid that warning and decreased the min angular displacement to 5.0 and I was only receiving "Too many anomalous beams warning" as well as the occasional info message I mentioned above, however still yet to see any actual updates in rviz. Although I am not sure if this is also relevant I would like to add that I noticed in your simulation videos the 2 wheel differential drive robot is used and the map updates would occur when sharp 90 degree turns in place were made. My robot is a tricycle and is unable to make such turns. Would this in any way impact the expected behaviour of the map updating algorithm?

[elisastefanini]

We never tried the algorithm with a tricycle but the robot kinematic should not affect the algorithm because it works only on position and orientation displacement.
Could you provide me with a video of the simulation?
If you change the parameter debug_visualization to true you should visualize on a marker topic the changed cell in the map and understand better if there are some changes.

[Nick-Kou]

Lifelong_mapping_test.zip

Thank you for your help. I have attached 2 images. One of them is the original world I performed mapping in and the second image is a depiction of the world where I am conducting the test as shown in the mp4 video attached. In this test I am running localization using amcl and navigating my robot around while viewing the processed_map with debug visualisation set to true. After zooming in very closely I can see faint red lines but I am not sure if this is the expected behaviour because usually obstacles on the grid should be represented as dark cells. Also we can see in terminal the warn messages ""Too many anomalous beams! Is the robot lost? Skipping map change detection..."" occurring frequently.

The parameters I am using in this test are as follows:

update_map_online: true
init_buffers: false
min_angle_increment: 0.1 #0.5 # expressed in degrees, it determines the considered beams number
range_1_pair: 0.2
pairing_distance_1: 0.15
range_2_pair: 10.0
pairing_distance_2: 0.28
pairing_distance_saturation: 0.3

buffer_size: 10
counters_change_threshold: 7
#counters_change_hysteresis: 0

scan_update_interval: 0.3 #1.0 # min time that has to elapse between two scans processing
min_linear_displacement: 0.5 #0.5 # min distance that the robot has to travel between two scans processing
min_angular_displacement: 5.0 # expressed in degrees

max_beam_range: 30.0 #15.0 # measurements greater than this will be discarded
max_range_tol: 0.8 # virtual beams will measure no more than "max_beam_range" + "max_range_tol"

virtual_to_measured_beam_ratio: 1 #3 # determines the number of virtual beams
min_search_window_halfwidth_degrees: 4.0 # expressed in degrees

max_trusted_angular_velocity: 25 # expressed in degrees/s
min_idle_duration: 1.2 #1.5 # min idle state time induced by a quick rotation
min_idle_distance: 1.6 #1.8

final_chunk_skip_fraction: 0.02 # final fraction of the beam that is skipped when doing ray casting for anomalous beams analysis
min_chunk_skip_length: 0.15 # min skipped length
skip_fraction_false_positive: 0.02 # final fraction of the beam that is skipped when doing ray casting for false positive detection
N_skip_anomalous: 2
max_anomalous_beam_fraction: 0.8
range_1_search: 0.0
radius_1: 0.10
range_2_search: 10.0
radius_2: 0.2

[elisastefanini]

From the video, there is some problem with localisation, and this may be the reason for the algorithm's warning. However, the objects that you used in the environments there aren't so big, the laser can detect only the "legs", which are small on the map. I suggest you first use some boxes or other big objects just to understand how the algorithm works and set your custom parameters. If you want, try your robot in this example environment, it is smaller than the one that I used in the paper but you should clearly see the changes in the map.
toy_example_test.zip

[Nick-Kou]

I used to toy0 map for AMCL localization, spawned my robot in toy_example_1.world, defaulted back to the original parameters.yaml provided in this repository, and navigated around the environment manually to observe an possible changes. As you can see from the video attached below, in both rviz and gazebo my robot is not facing any localization issues as we can see almost the same exact movement I perform in the gazebo world itself is being reflected in rviz. The warn issue still appears and no map changes occur.

Life_long_video_test.zip

Can this potentially be an issue with my lidar plugin in gazebo? Should I attempt to try this node in the real world instead?

[elisastefanini]

I don't think it's a lidar plugin problem, hoverer, I used the gpu-ray one.
From the video, in the RVIZ visualization, when the robot is moving, why the current laser measurement didn't overlap on the wall? I see that the robot in RVIZ is in the same position as in Gazebo, but what about laser measurements? It is for this reason that the system thinks that the robot is lost. Another thing that I noticed is in the shell, the messages about the service call. The system update the map autonomously and publishes it on the topic \processed_map.
The right behaviour is the one in the video attached.
toy_test_github.zip

[Nick-Kou]

Disregard the "[ INFO] [1680059043.093572061, 348.591000000]: Service successfully called." this is from another node that I am launching during the process. I re-conducted the experiment using slamtoolbox. I mapped toy0 world and then localized within toy1 world and the video is shown below. In the video (specifically at 1 minute and 8 seconds) we can see the red laser scan of the robot overlapping the walls correctly, however it is the orange marker in rviz which comes from the "visualize_on_rviz" value="true"/> parameter set in the node "map_change_detection_node" that is incorrect and seems to be translating with respect to the position of the robot. Can this maybe due to a bug in the source code you pushed to github compared to the one on your local machine?

slamtoolbox_lifelong_mapping.mp4

Thank you for all your assistance and feedback I really appreciate it.

[elisastefanini]

I've checked the code and they are the same. The marker topic on Rviz derived from the processed lidar measurements in the fixed frame that you set in the param file.
As you can see from the scan_handler.cpp code, the system is waiting for the transformation from robot base frame link to the map fixed frame to compute the hit points in the fixed frame from the laser measurements using the global robot pose, x,y, and theta. I've checked all the frame name in the code, and they are general.