Nav2 robot out of bounds (3D environment, almost 2D). Recovery mode problem.
Hi!
I am trying to navigate a robot in a 3D environment using 2D navigation. The enviornment has some smooth ups and downs above and below the costmap built.
It works pretty fine, however, when the robot moves out of the plane I keep getting [controller_server-1] [WARN] [1658242715.771246989] [local_costmap.local_costmap]: Sensor origin: (1.42, -0.13, -0.03), out of map bounds. The costmap can't raytrace for it.
The robot continues moving in a normal way, but if it gets stuck, the behavior tree doesn't enter into the recovery mode (It stays in the FollowPath branch although I have already received
[bt_navigator-4] [ERROR] [1658242701.988860407] [bt_navigator]: Action server failed while executing action callback: "send_goal failed"
[bt_navigator-4] [WARN] [1658242701.988931893] [bt_navigator]: [navigate_to_pose] [ActionServer] Aborting handle.
I don't know if there is a way to fix this, but I have seen other projects where they do this (but their robot doesn't require the recovery mode).
I suppose it is because of the ClearEntireCostmap action (in this case with the clear entirely local costmap server) branch of the behavior tree (I am using the default navigate_w_replanning_and_recovery). For this reason I tried to get rid of those branches creating a new behavior tree, and although I changed the root of the BT in the yaml file, when monitoring it with Groot I still see the previous BT (I cleared the previous BT, closed and opened Groot and even changed the publisher and server port).
However, I don't get , if this is the problem, if the ClearEntireCostmap is failing, shouldn't it be returning a "Failure" and so the BT should jump to the RecoveryFallback?
Thank you in advance for your help!
