Waypoint Skipping issue on JETSON XAVIER NX
Hi, So I was running nav2 of Xavier. The problem I faced was that after completing a few waypoints, it skipped some waypoints. For example if the waypoints are 1 -> 2 -> 3 -> 4 ->5, it will sometime follow 1 -> 2 -> 3 -> 5. The waypoints were not given through Rviz but were given through an action server. The behavior happened after 70 waypoints but mostly happened every time.
This happened in both foxy and rolling.
Also, the following did not happen when running on machines with higher compute capacity like NUC, or i7gen laptops.
If anyone else faced the same issue, kindly respond.
Thanks
Asked by root-robot on 2021-12-15 04:30:20 UTC
Comments
Logs when this happens would be useful to see what is happening. Also, do you see this happening when goals 1,2,3 succeeded, any failed? The waypoint follower uses the async API for the action, so I could believe that there's something odd going on, but it would be hard to debug without logging and a couple of iterations if you're open to it.
Asked by stevemacenski on 2021-12-16 01:29:42 UTC
The first thing I would try to do is add some logging in the resultCallback() to give some info about what's going on there. Log when the resultCallback triggers what the goal_id is of the result (e.g.
result.goal_id).If we see repeats (which could be happening if you have low compute left over do DDS retries for a reliable connection), then this is an easy solution that we just need to use the
future_goal_handle_to make sure that the result is from the currently processing goal.The other thing I would do off the bat is move https://github.com/ros-planning/navigation2/blob/main/nav2_waypoint_follower/src/waypoint_follower.cpp#L208 to before the
send_goal_optionsdeclaration line. I doubt that is what is causing your issue, but it could cause an issue in general for rare async things.Let me know how those go, if your logs / this doesn't give any progress, lets open a ticket in Nav2 and chat there.
Asked by stevemacenski on 2021-12-16 01:38:28 UTC
Sure stevemacenksi will do that and let you know the updates.
Asked by root-robot on 2021-12-16 09:16:04 UTC
Also yes 1,2,3 waypoints always succeed. It generally happens after 70 waypoints or so. But it happens repeatedly.
Asked by root-robot on 2021-12-16 09:22:23 UTC
As far as normal logs are concerned, it states that the waypoint has been reached, and then starts moving to the new goal. But in reality, the waypoint has not been reached. Meaning the order it goes in is correct but the goal checker gives the result even before reaching the goal.
I will post the logs very soon.
Asked by root-robot on 2021-12-16 09:33:57 UTC
I tried to add loads to my system and I couldn't replicate, but certainly doesn't mean it doesn't happen. Let me know how that goes. I have a development environment setup to play with that as I hear from you.
Asked by stevemacenski on 2021-12-16 19:00:13 UTC
Hey so was trying to replicate the issue in rolling but was unable to do it as I kept running into another issue. The link to the issue is here. Take a look at it once. When I will debug it I think i will be able to reproduce the skipping error.
Thanks
Asked by root-robot on 2021-12-17 09:31:27 UTC