search

ros-navigation / navigation2

ROS 2 Navigation Framework and System
https://nav2.org/
Other
2.58k stars 1.29k forks source link

Ubuntu/MacOS: nav2 goal is 3 feet away in straight line, but device executes ~10 spins and fails to reach goal #1050

Closed oneattosecond closed 5 years ago

oneattosecond commented 5 years ago

Bug report

Required Info:

Steps to reproduce issue

  1. ros2 launch turtlebot3_navigation2 navigation2.launch.py map:=/home/user/maps/map.yaml
  2. set initial pose accurately via rviz UI
  3. give TB3 a goal 3 feet away in a straight line with no obstacles

Expected behavior

TB3 should drive (ideally in a straight-ish line) for 3 feet then stop

Actual behavior

TB3 spins in random semi-circles about 10 times, moves 2 feet over and 2 feet up (away from goal) then gives up.

ubuntu_nav2_failed_console_log.txt

SteveMacenski commented 5 years ago

@oneattosecond we'd appreciate it if you only posted the section of the log showing the issue, and in markdown format. Linked files are not searchable for people looking for answers and long files like this make it less likely that we're able to help

oneattosecond commented 5 years ago

@SteveMacenski understood... unfortunately I don't know what the issue is & hence I can't point out what section the failure is in...

SteveMacenski commented 5 years ago

Well, I can tell you its probably not the 5ish warnings that make up the majority of the file, because they're from rviz, even some clean up is nice.

Also asking someone to open your file containing... who knows what on an internet website is a little dubious. For all I know, you're an elite hacker trying to infiltrate robotics company's servers

oneattosecond commented 5 years ago

@SteveMacenski I'm quite accustomed to people asking for more logs, not less... here are the 140 lines that look most relevant:

[rviz2-9] [INFO] [rviz2]: Setting goal: Frame:map, Position(-0.0104192, 0.0275195, 0), Orientation(0, 0, -0.706825, 0.707388) = Angle: -1.57

[amcl-3] [INFO] [amcl]: Setting pose (1566336945.987594): -0.010 0.028 -1.570 [amcl-3] [WARN] [amcl]: Waiting for map.... [amcl-3] [INFO] [amcl]: Received a 96 X 83 map @ 0.050 m/pix [amcl-3] [amcl-3] [INFO] [amcl]: Setting pose (1566336947.589513): -0.010 0.028 -1.570

[lifecycle_manager-1] [INFO] [lifecycle_manager]: Configuring and activating dwb_controller

[rviz2-9] [INFO] [rviz2]: Trying to create a map of size 96 x 83 using 1 swatches [dwb_controller-5] [INFO] [local_costmap.local_costmap]: Using plugin "obstacle_layer" [dwb_controller-5] [INFO] [local_costmap.local_costmap]: Subscribed to Topics: scan [dwb_controller-5] [INFO] [local_costmap.local_costmap]: Using plugin "inflation_layer" [world_model-4] [WARN] [global_costmap.global_costmap]: Transform timeout. Current time: 1566336949.8710, global_pose stamp: 1566336949.5694, tolerance: 0.3000, difference: 0.3017 [world_model-4] [WARN] [global_costmap.global_costmap]: Transform timeout. Current time: 1566336950.0707, global_pose stamp: 1566336949.7061, tolerance: 0.3000, difference: 0.3646 [world_model-4] [WARN] [global_costmap.global_costmap]: Transform timeout. Current time: 1566336950.2707, global_pose stamp: 1566336949.9664, tolerance: 0.3000, difference: 0.3043 [dwb_controller-5] [INFO] [dwb_controller]: Setting transform_tolerance to 0.200000 [dwb_controller-5] [INFO] [dwb_controller]: Using critic "RotateToGoal" (dwb_critics::RotateToGoalCritic) [dwb_controller-5] [INFO] [dwb_controller]: Critic plugin initialized [dwb_controller-5] [INFO] [dwb_controller]: Using critic "Oscillation" (dwb_critics::OscillationCritic) [dwb_controller-5] [INFO] [dwb_controller]: Critic plugin initialized [dwb_controller-5] [INFO] [dwb_controller]: Using critic "BaseObstacle" (dwb_critics::BaseObstacleCritic) [dwb_controller-5] [INFO] [dwb_controller]: Critic plugin initialized [dwb_controller-5] [INFO] [dwb_controller]: Using critic "GoalAlign" (dwb_critics::GoalAlignCritic) [dwb_controller-5] [INFO] [dwb_controller]: Critic plugin initialized [dwb_controller-5] [INFO] [dwb_controller]: Using critic "PathAlign" (dwb_critics::PathAlignCritic) [dwb_controller-5] [INFO] [dwb_controller]: Critic plugin initialized [dwb_controller-5] [INFO] [dwb_controller]: Using critic "PathDist" (dwb_critics::PathDistCritic) [dwb_controller-5] [INFO] [dwb_controller]: Critic plugin initialized [dwb_controller-5] [INFO] [dwb_controller]: Using critic "GoalDist" (dwb_critics::GoalDistCritic) [dwb_controller-5] [INFO] [dwb_controller]: Critic plugin initialized [dwb_controller-5] [INFO] [dwb_controller]: Controller frequency set to 20.0000Hz

[dwb_controller-5] [INFO] [local_costmap.local_costmap]: Checking transform

[lifecycle_manager-1] [INFO] [lifecycle_manager]: Configuring and activating navfn_planner

[rviz2-9] [INFO] [rviz2]: Trying to create a map of size 60 x 60 using 1 swatches

[lifecycle_manager-1] [INFO] [lifecycle_manager]: Configuring and activating bt_navigator

[world_model-4] [WARN] [global_costmap.global_costmap]: Transform timeout. Current time: 1566336952.1017, global_pose stamp: 1566336951.6813, tolerance: 0.3000, difference: 0.4204 [world_model-4] [WARN] [global_costmap.global_costmap]: Transform timeout. Current time: 1566336952.7018, global_pose stamp: 1566336952.2422, tolerance: 0.3000, difference: 0.4596 [bt_navigator-8] [INFO] [bt_navigator_client_node]: Waiting for "ComputePathToPose" action server [bt_navigator-8] [INFO] [bt_navigator_client_node]: "ComputePathToPose" BtActionNode initialized [bt_navigator-8] [INFO] [bt_navigator_client_node]: Waiting for "FollowPath" action server [bt_navigator-8] [INFO] [bt_navigator_client_node]: "FollowPath" BtActionNode initialized [world_model-4] [WARN] [global_costmap.global_costmap]: Transform timeout. Current time: 1566336957.5863, global_pose stamp: 1566336957.1592, tolerance: 0.3000, difference: 0.4271 [world_model-4] [WARN] [global_costmap.global_costmap]: Transform timeout. Current time: 1566336957.5864, global_pose stamp: 1566336957.1592, tolerance: 0.3000, difference: 0.4272 [bt_navigator-8] [INFO] [bt_navigator_client_node]: Waiting for "Spin" action server [bt_navigator-8] [INFO] [bt_navigator_client_node]: "Spin" BtActionNode initialized

[lifecycle_manager-1] [INFO] [lifecycle_manager]: The system is active [bt_navigator-8] [INFO] [bt_navigator]: Begin navigating from current location to (-0.30, -1.15) [dwb_controller-5] [INFO] [dwb_controller]: Received a goal, begin following path [dwb_controller-5] [WARN] [dwb_controller]: Control loop missed its desired rate of 20.0000Hz [dwb_controller-5] [ERROR] [dwb_controller]: Failed to make progress [dwb_controller-5] [INFO] [local_costmap.local_costmap]: Received request to clear entirely the local_costmap [world_model-4] [INFO] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap [recoveries_node-7] [INFO] [recoveries]: Attempting Spin [recoveries_node-7] [INFO] [recoveries]: Currently only supported spinning by a fixed amount [recoveries_node-7] [INFO] [recoveries]: Spin running... [recoveries_node-7] [INFO] [recoveries]: Spin running... [recoveries_node-7] [INFO] [recoveries]: Spin running... [recoveries_node-7] [INFO] [recoveries]: Spin running... [recoveries_node-7] [INFO] [recoveries]: Spin running... [recoveries_node-7] [INFO] [recoveries]: Spin running... [recoveries_node-7] [INFO] [recoveries]: Spin completed successfully [dwb_controller-5] [INFO] [dwb_controller]: Received a goal, begin following path [dwb_controller-5] [ERROR] [dwb_controller]: Failed to make progress [dwb_controller-5] [INFO] [local_costmap.local_costmap]: Received request to clear entirely the local_costmap [world_model-4] [INFO] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap [recoveries_node-7] [INFO] [recoveries]: Attempting Spin [recoveries_node-7] [INFO] [recoveries]: Currently only supported spinning by a fixed amount [recoveries_node-7] [INFO] [recoveries]: Spin running... [recoveries_node-7] [INFO] [recoveries]: Spin running... [recoveries_node-7] [INFO] [recoveries]: Spin running... [recoveries_node-7] [INFO] [recoveries]: Spin running... [recoveries_node-7] [INFO] [recoveries]: Spin running... [recoveries_node-7] [INFO] [recoveries]: Spin running... [recoveries_node-7] [INFO] [recoveries]: Spin completed successfully [dwb_controller-5] [INFO] [dwb_controller]: Received a goal, begin following path [dwb_controller-5] [ERROR] [dwb_controller]: Failed to make progress [dwb_controller-5] [INFO] [local_costmap.local_costmap]: Received request to clear entirely the local_costmap [world_model-4] [INFO] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap [recoveries_node-7] [INFO] [recoveries]: Attempting Spin [recoveries_node-7] [INFO] [recoveries]: Currently only supported spinning by a fixed amount [recoveries_node-7] [INFO] [recoveries]: Spin running... [recoveries_node-7] [INFO] [recoveries]: Spin running... [recoveries_node-7] [INFO] [recoveries]: Spin running... [recoveries_node-7] [INFO] [recoveries]: Spin running... [recoveries_node-7] [INFO] [recoveries]: Spin running... [recoveries_node-7] [INFO] [recoveries]: Spin running... [recoveries_node-7] [INFO] [recoveries]: Spin completed successfully [dwb_controller-5] [INFO] [dwb_controller]: Received a goal, begin following path [dwb_controller-5] [WARN] [dwb_controller]: Control loop missed its desired rate of 20.0000Hz [dwb_controller-5] [ERROR] [dwb_controller]: Failed to make progress [dwb_controller-5] [INFO] [local_costmap.local_costmap]: Received request to clear entirely the local_costmap [world_model-4] [INFO] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap [recoveries_node-7] [INFO] [recoveries]: Attempting Spin [recoveries_node-7] [INFO] [recoveries]: Currently only supported spinning by a fixed amount [recoveries_node-7] [INFO] [recoveries]: Spin running... [recoveries_node-7] [INFO] [recoveries]: Spin running... [recoveries_node-7] [INFO] [recoveries]: Spin running... [recoveries_node-7] [INFO] [recoveries]: Spin running... [recoveries_node-7] [INFO] [recoveries]: Spin running... [recoveries_node-7] [INFO] [recoveries]: Spin running... [recoveries_node-7] [INFO] [recoveries]: Spin completed successfully [dwb_controller-5] [INFO] [dwb_controller]: Received a goal, begin following path [dwb_controller-5] [ERROR] [dwb_controller]: Failed to make progress [dwb_controller-5] [INFO] [local_costmap.local_costmap]: Received request to clear entirely the local_costmap [world_model-4] [INFO] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap [recoveries_node-7] [INFO] [recoveries]: Attempting Spin [recoveries_node-7] [INFO] [recoveries]: Currently only supported spinning by a fixed amount [recoveries_node-7] [INFO] [recoveries]: Spin running... [recoveries_node-7] [INFO] [recoveries]: Spin running... [recoveries_node-7] [INFO] [recoveries]: Spin running... [recoveries_node-7] [INFO] [recoveries]: Spin running... [recoveries_node-7] [INFO] [recoveries]: Spin running... [recoveries_node-7] [INFO] [recoveries]: Spin running... [recoveries_node-7] [INFO] [recoveries]: Spin completed successfully [dwb_controller-5] [INFO] [dwb_controller]: Received a goal, begin following path [dwb_controller-5] [WARN] [dwb_controller]: Control loop missed its desired rate of 20.0000Hz [dwb_controller-5] [WARN] [dwb_controller]: Control loop missed its desired rate of 20.0000Hz [dwb_controller-5] [ERROR] [dwb_controller]: Failed to make progress [dwb_controller-5] [INFO] [local_costmap.local_costmap]: Received request to clear entirely the local_costmap [world_model-4] [INFO] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap [recoveries_node-7] [INFO] [recoveries]: Attempting Spin [recoveries_node-7] [INFO] [recoveries]: Currently only supported spinning by a fixed amount [recoveries_node-7] [INFO] [recoveries]: Spin running... [recoveries_node-7] [INFO] [recoveries]: Spin running... [recoveries_node-7] [INFO] [recoveries]: Spin running... [recoveries_node-7] [INFO] [recoveries]: Spin running... [recoveries_node-7] [INFO] [recoveries]: Spin running... [recoveries_node-7] [INFO] [recoveries]: Spin completed successfully [bt_navigator-8] [ERROR] [bt_navigator]: Navigation failed

again, apologies, I have seen others attach files to GitHub issues and thought that was expected over here as well.

SteveMacenski commented 5 years ago

Got it, maybe others have opinions, but attaching files I view as less useful than inline of the actual issue so that when people google a similar error, the ticket comes up. That's saved me more times than I can count. Plus it could be a text file with some malware embedded. But this helps alot.

Ugh, ros2 logging doesnt have timestamps 

[dwb_controller-5] [INFO] [dwb_controller]: Received a goal, begin following path
[dwb_controller-5] [ERROR] [dwb_controller]: Failed to make progress

clearly its the collision checker in dwb telling us that its not moving so we're executing recoveries. I'd put some prints to tell if its the timeout or is_robot_moved_enough that's triggering. I'm hoping its the timeout, because then its probably just an issue with sim time on your machine settings.

crdelsey commented 5 years ago

Based on the timestamps, it looks like you are running on a real robot and not in simulation. Is that correct?

oneattosecond commented 5 years ago

it looks like you are running on a real robot and not in simulation

How did you generate your map? Did you drive the robot around using teleop and cartographer?

In Rviz, do the /plan and /local_plan plans look reasonable?

SteveMacenski commented 5 years ago

Did you change configs? Also can you show up a gif/video of you doing this in rviz so we can see the costmaps and where you’re plotting to?

yathartha3 commented 5 years ago

One thing to check would be to make sure that you haven't set the robot velocity too low. It's the progress checker that is complaining about "Failed to make progress". As Steve said: _'put some prints to tell if its the timeout or is_robot_movedenough that's triggering. I'm hoping its the timeout, because then its probably just an issue with sim time on your machine settings.'

mlherd commented 5 years ago

@oneattosecond A few things to make sure;

Can you visualize the global costmap using Rviz? Can you visualize the global path using Rviz? Can you share the map or a screenshot of it with us? How fast does your robot move? Is is less than 30 cm per 10 seconds?

PS: Recently, Turtlebot 3 released a new version for Dashing and updated their e-Manuel (Yesterday). I haven't tested it with Navigation 2 yet.

oneattosecond commented 5 years ago

I'll update to the latest TB3 sources and report back answers to above questions later today, thanks for looking...

oneattosecond commented 5 years ago

unfortunately, I am hitting a crash in TB3 on latest sources (tried both Mac and Ubuntu) so I can't move to latest TB3 version just yet. https://github.com/ROBOTIS-GIT/turtlebot3/issues/479

@mlherd I'm fairly certain time is synchronized between machines, and the TB3 launch script should handle turning off SIM time and autostart, no?

Yes I can visualize the global cost map and global path using rviz. Once I bring my TB3 back to life by rolling back to Raspbian on RPi and recompiling older TB3 repos, I'll grab a screenshot.

When navigating yesterday, it was moving generally slowly but I don't think it was less than 30 cm (~12 inches) in 10 seconds. It would sometimes speed up quite fast then get stop after a few seconds and decide to spin.

mlherd commented 5 years ago

@oneattosecond

@mkhansen-intel and I tested the new TB3 release today. I followed the setup instructions in their e-manual.

In my experience, new TB3 version works much better and more reliable than the earlier version. I would recommend you using the latest release. Just letting you know that I have an Intel NUC running Ubuntu 18.04 on the robot. I haven't tested it with Raspberry Pi or MacOS yet.

This is how we tested things;

Test1: (success-ish) It works fine most of the time. The robot spins and tries to correct its pose especially when it is very close to the goal pose. Nothing crashes and the robot keeps moving when sending new goal poses.

Test2: (success) This works fine.

Test3: (Success) This seems to work fine. I didn't test this much since this is not an ideal use case.

Test4: (success) This works fine as well. We tested the robot in a fairly large and complex map.

The default value for use_sim_time should be false unless you set it to true. I don't know why, but the networking was more reliable when using Opensplice on the Robot PC and FastRTPS on the remote PC.

Yesterday, I observed a very similar spinning problem after I deliberately lowered the robot's speed to 0.1. Today, I didn't have the robot spinning issue.

mlherd commented 5 years ago

@oneattosecond This is the order I follow when I install, build, and launch everything. Navigation2-Setup-Flowchart.pdf Navigation2-Setup

crdelsey commented 5 years ago

local_plan keeps changing to try to steer towards /plan, but almost never overlaps with /plan

In a good run, local_plan should be more or less aligned with the plan and wriggling back and forth a small amount.

You mentioned you are running on a Raspberry Pi, and I believe I saw some errors in you log about DWB missing its deadline Control loop missed its desired rate of 20.0000Hz

We haven't done a lot of testing on a low-end computer like the Raspberry Pi yet. It's possible you're running into performance problems. If you see a lot of the "Control loop missed its desired rate" messages, that would be a problem.

To check what the effective loop rate of the controller is, I'd suggest running

ros2 topic hz /cmd_vel

This should be 20Hz, but I'd say anything over 5Hz is probably OK for the control loop on the turtlebot.

I'd also recommend running top to see how much overall CPU is being used as well as if individual processes are consuming a lot.

oneattosecond commented 5 years ago

@mlherd can you document your HW setup in a little more detail? I'm just using the stock TB3 Waffle Pi with no HW modifications, which I was hoping would work out of the box with Ubuntu...

@crdelsey thanks I'll take a look as soon as I resurrect my TB3, bouncing between multiple experiments will come back after some delay

oneattosecond commented 5 years ago

yeah I'm getting ~9.8 Hz from /cmd_vel and (using older TB3 support package) 2 of 4 Raspberry Pi CPU's are pegged running MicroXRCE, top shows 50% idle on Raspberry Pi. Ubuntu ("remote PC") side is running closer to 90% busy (10% idle) in this configuration.

I'll keep looking and hopefully get the newer TB3 packages working tomorrow so as to not waste time on older software

routiful commented 5 years ago

@oneattosecond The new version of firmware make that communicate between raspberry pi 3 and remote pc is more reliable than older version. I have been commented your issue in TurtleBot3 repo.

mlherd commented 5 years ago

@oneattosecond Yesterday and Today, I tested Turtlebot 3 using both Navigation2 Master and Dashing branches. I wasn't able to reproduce the spinning issue. I tested the robot in a fairly large and complex area by sending several different goal positions.

Let me know if you still have this problem.

I can create a diagram showing our HW setup on the robot if that is what you mean by saying HW setup.

mlherd commented 5 years ago

@routiful Thanks for your hard work! I can confirm that the new firmware works more reliably.

oneattosecond commented 5 years ago

@mlherd yes if you would mind sharing, can you show how your NUC is connected to TB3? Considering whether to copy your setup or stick with stock TB3 Waffle Pi.

I'm currently stuck with latest TB3 firmware, I cannot echo IMU topic from TB3 --> Ubuntu side. Will follow up separately, just posting that I am still trying to get up and going.

mlherd commented 5 years ago

@oneattosecond

Block Diagram.pdf Block Diagram

oneattosecond commented 5 years ago

@mlherd thank you!

mkhansenbot commented 5 years ago

@oneattosecond - haven't heard back from you on this, is this resolved for you now?

oneattosecond commented 5 years ago

@mkhansen-intel I haven't been able to test, been having more basic problems with TB3 on Ubuntu. I'll come back to this shortly.

oneattosecond commented 5 years ago

still haven't gotten back to Ubuntu, but once unblocked on https://github.com/ros-planning/navigation2/issues/1049 I saw similar / same behavior on Mac. Also a lot of "no path between X and Y" type of messages scrolling past

After a bunch of fiddling, I changed below params and getting better results on Mac. So this might just be TB3 default params aren't ideal for an obstacle cluttered room?

 inflation_layer.cost_scaling_factor: 0.5 (was 3.0)
 inflation_layer.inflation_radius: 0.1 (was unset / default)
mkhansenbot commented 5 years ago

We run our TB3 in our lab without difficulty. I don't know how cluttered your space is, I'd have to see the map that was generated. Is the problem still that DWB is reporting that you 'failed to make progress'?

mkhansenbot commented 5 years ago

Is this running on the Raspberry Pi?

So one thing you could try is changing the parameters file to increase the timeout for the progress check.

https://github.com/ros-planning/navigation2/blob/7c473cdadb71355fcdc8dd73bd4bbf40e75141ac/nav2_dwb_controller/dwb_controller/src/progress_checker.cpp#L23

If you set the "movement_timeallowance" to something larger than 10s, you won't trigger this error so quickly. Maybe set it to 60s?

oneattosecond commented 5 years ago

I'm stuck on Ubuntu due to unrelated issues, and have things working pretty well on MacOS with tuning of parameters. I'm going to close this issue with many thanks to the folks who've commented and given helpful suggestions. Will file new issues in future as needed.

mkhansenbot commented 5 years ago

I'm glad things are working on MacOS! Hooray! That's pretty great to hear since we haven't tested that.

mlherd commented 5 years ago

@oneattosecond Could you share how you fixed the issue, so other MacOS users can benefit from your solution? What parameters did you tune? Are they the ones that you mentioned in your previous comment or are there any other parameters? Thanks.

oneattosecond commented 5 years ago

@mlherd two main obstacles I encountered on MacOS (w/r/t navigation, had to hack around other things in TB3 support packages):

https://github.com/ros-planning/navigation2/issues/1049 <-- makecontext/getcontext corrupting stack. Rearranging headers solves this, but I still don't know why it works, just that bt_navigator stops crashing with header order changes.

obstacles have very high cost <-- I placed a cup on the floor and TB3 gives it a 6 foot cone of shame where it won't go anywhere near it. Trouble is, this was in a 10 foot x 11 foot room with a few obstacles (mostly in corners), so there was very little space TB3 was willing to navigate to, everywhere was marked as high cost and/or fatal cost. I tuned the parameters mentioned earlier (inflation_layer.cost_scaling_factor, inflation_layer.inflation_radius) as the default parameters were far too large for my use cases. I also had difficulty navigating in a larger room (10 foot wide hallway) and tuning these parameters improved every room I tested so far.

It isn't perfect (some collisions with wall, some occasional spinning in complex spaces) but it's significantly better at this point.

Sorry to clutter a Ubuntu ticket with MacOS details, but here we are. Thanks all for the many suggestions!

oneattosecond commented 5 years ago

Last comment, updated title to reflect similar behavior was seen on MacOS and Ubuntu.

Also worth mentioning https://github.com/googlecartographer/cartographer/issues/1626 which causes Cartographer to crash very often on MacOS

in summary, what I did to get MacOS to work with TB3

  1. modify TB3 support packages to compile/link on MacOS (many small CMake changes)
  2. fix UB in cartographer so it doesn't crash regularly
  3. tune nav2 parameters for inflation / obstacle avoidance
  4. header file re-arranging to prevent stack corruption on nav2 MacOS
  5. ???
  6. profit