search

ros-planning / navigation

ROS Navigation stack. Code for finding where the robot is and how it can get somewhere else.
2.3k stars 1.79k forks source link

DWA Fails at the end of the path #147

Open DLu opened 10 years ago

DLu commented 10 years ago

Discussed here: http://answers.ros.org/question/94536/testing-dwa-local-planner-approaches-goal-stops-keeps-swinging-left-right/#96786

Temporary fix is here: https://github.com/ros-planning/navigation/blob/hydro-devel/dwa_local_planner/src/dwa_planner.cpp#L176 Set cheat_factor to 2 to avoid the problem, but doesn't address the fundamental problem.

The fundamental problem stems from the weight on the alignment cost being set incorrectly (see https://github.com/ros-planning/navigation/commit/6a72c61 for one fix)

DLu commented 10 years ago

Hey Hendrik, Just to confirm, did you change the value of cheat_factor? -DL!!

On Thu, Nov 28, 2013 at 7:19 PM, Hendrik Wiese notifications@github.comwrote:

I don't know what it is but it's still happening to my (simulated) robot. I'm not able to test it on a real robot because we don't have one yet. But that shouldn't make a difference, should it?

I pulled the latest hydro-devel branch and recompiled the whole navigation stack. And it still happens. Do I have to configure something to see your bug fix working? Or should it work out of the box?

— Reply to this email directly or view it on GitHubhttps://github.com/ros-planning/navigation/issues/147#issuecomment-29491420 .

HWiese1980 commented 10 years ago

Ah, no, I didn't. That might be the cause. What would be a reasonable value and where do I have to set it?

DLu commented 10 years ago

$MOVE_BASE_NAMESPACE/DWAPlannerROS/cheat_factor 2

On Wed, Dec 4, 2013 at 4:21 PM, Hendrik Wiese notifications@github.comwrote:

Ah, no, I didn't. That might be the cause. What would be a reasonable value and where do I have to set it?

— Reply to this email directly or view it on GitHubhttps://github.com/ros-planning/navigation/issues/147#issuecomment-29851873 .

HWiese1980 commented 10 years ago

Alright. Thanks, I'll give it a try and report back.

HWiese1980 commented 10 years ago

Okay, I have set the cheat_factor to 2. It still depends on the value of sim_time (1.5 works, 3.0 doesn't) and the xy_goal_tolerance (higher tolerance works, lower doesn't). Higher values for the cheat_factor don't appear to have bigger impact on this behavior.

DLu commented 10 years ago

Ok. As implied by the name, cheat_factor isn't a real fix. Is your code using this fix? https://github.com/ros-planning/navigation/pull/154

On Thu, Dec 5, 2013 at 6:05 AM, Hendrik Wiese notifications@github.comwrote:

Okay, I have set the cheat_factor to 2. It still depends on the value of sim_time (1.5 works, 3.0 doesn't) and the xy_goal_tolerance (higher tolerance works, lower doesn't). Higher values for the cheat_factor don't appear to have bigger impact on this behavior.

— Reply to this email directly or view it on GitHubhttps://github.com/ros-planning/navigation/issues/147#issuecomment-29892329 .

HWiese1980 commented 10 years ago

Yeah, that's what I thought. I'm on hydro-devel HEAD revision. If the fix hasn't yet been merged into it, I don't use it. I'll check it out and report back.

//edit: alright, it obviously is merged, part of the head revision and thus part of my stack as well. So, yes, my code uses #154

DLu commented 10 years ago

Hey all, I spent the weekend looking at this. Can you try this branch please? https://github.com/DLu/navigation/tree/hydro_end_path_fix -DL!!

On Fri, Dec 6, 2013 at 1:28 PM, Hendrik Wiese notifications@github.comwrote:

Yeah, that's what I thought. I'm on hydro-devel HEAD revision. If the fix hasn't yet been merged into it, I don't use it. I'll check it out and report back.

— Reply to this email directly or view it on GitHubhttps://github.com/ros-planning/navigation/issues/147#issuecomment-30021894 .

HWiese1980 commented 10 years ago

Oh dear, I hope it's not completely my fault in the end... having busted your whole weekend for nothing would be a pretty unpleasant thought. I'll try it and, again, report back.

//edit: Well, the first thing to mention is that I can't compile dwa_planner.cpp due to an undeclared variable _porient_scale__ in line 279. I couldn't find it anywhere, neither something that looks at least similar. Where does it come from?

DLu commented 10 years ago

Dear David, Please test-compile your code before making other people try it. -David!!

Seriously though, try it again after you pull the latest version. And don't worry, it juuuuuuust happens that your problem was something I was looking at for my thesis anyway.

HWiese1980 commented 10 years ago

Alright, phew, that takes a load off my mind. :-)

I've tried it and it looks like it's gotten worse again. The robot stops close to its goal, swings left and right for a while and kind of jumps ahead for a short distance only to go dancing again... before your last changes it at least worked with a low sim_time and a for my setup relatively high goal tolerance.

Just to make sure it isn't caused by anything you simply cannot think of because it's too unusual, here some facts about my setup:

DWAPlannerROS: acc_lim_x: 0.5 # measured with rqt_plot acc_lim_y: 0.0 acc_lim_th: 6.5 # measured with rqt_plot max_trans_vel: 0.4 min_trans_vel: 0.05 max_vel_x: 0.6 min_vel_x: -0.4 max_vel_y: 0.0 min_vel_y: 0.0 max_rot_vel: 1.0 min_rot_vel: 0.02 latch_xy_goal_tolerance: true publish_cost_grid_pc: true cheat_factor: 2

Hope I haven't forgotten any important information that might be causing this behavior...

Anyway, I really appreciate your help and effort! I like your planner most because it is up to now the only one that appears to allow my robot to approach its goal backwards (among other things).

DLu commented 10 years ago

Dang it. Okay, try one more thing (that I really should have mentioned the first time): Same branch but set DWAPlannerROS/scaled_path_factor to 0.75.

On Tue, Dec 10, 2013 at 5:41 PM, Hendrik Wiese notifications@github.comwrote:

Alright, phew, that takes a load off my mind. :-)

I've tried it and it looks like it's gotten worse again. The robot stops close to its goal, swings left and right for a while and kind of jumps ahead for a short distance only to go dancing again... before your last changes it at least worked with a low sim_time and a for my setup relatively high goal tolerance.

Just to make sure it isn't caused by anything you simply cannot think of because it's too unusual, here some facts about my setup:

  • I'm still running a simulated robot in a V-REP environment (the real robot still isn't finished yet)
  • It's a two-wheeled differential drive robot only moving in a plane (no ramps or uneven terrain), so only x and y coordinates matter, z is always 0.00
  • The V-REP simulator provides ROS with a fully simulated odometry/localization
  • I also simulate laser scans which keep the robot's obstacle awareness up-to-date
  • I'm sending goals using RViz 2D Nav Goal user interface
  • all parameters for the DWA local planner are default, exceptions see below
  • ideal cartesian precision is about 0.001m, but since that's utopistic I'm content with 0.05m in the first place (optimizations might follow later)
  • desired polar precision is 1 degree

DWAPlannerROS: acc_lim_x: 0.5 # measured with rqt_plot acc_lim_y: 0.0 acc_lim_th: 6.5 # measured with rqt_plot max_trans_vel: 0.4 min_trans_vel: 0.05 max_vel_x: 0.6 min_vel_x: -0.4 max_vel_y: 0.0 min_vel_y: 0.0 max_rot_vel: 1.0 min_rot_vel: 0.02 latch_xy_goal_tolerance: true publish_cost_grid_pc: true cheat_factor: 2

Hope I haven't forgotten any important information that might be causing this behavior...

Anyway, I really appreciate your help and effort! I like your planner most because it is up to now the only one that appears to allow my robot to approach its goal backwards (among other things).

— Reply to this email directly or view it on GitHubhttps://github.com/ros-planning/navigation/issues/147#issuecomment-30280421 .

HWiese1980 commented 10 years ago

Okay, that works a lot better. However the robot's behavior is still heavily depending on the value of sim_time. With a value of 2 seconds it successfully reaches its goal... but there are other disadvantages to that near static obstacles. I'll continue watching it tomorrow (well, today, that is, as it's already around 2:30am here; guess I'll go to bed now...) and report back... thanks again and good night!

DLu commented 10 years ago

Ok. Sorry to hear of your troubles. You should get some sleep, and then tell me the approximate shapes of the paths you're testing, including goal orientation. Or possibly even record the local and global paths into a bag if you have time.

-DL!!

On Tue, Dec 10, 2013 at 7:36 PM, Hendrik Wiese notifications@github.comwrote:

Okay, that works a lot better. However the robot's behavior is still heavily depending on the value of sim_time. With a value of 2 seconds it successfully reaches its goal... but there are other disadvantages to that near static obstacles. I'll continue watching it tomorrow (well, today, that is, as it's already around 2:30am here; guess I'll go to bed now...) and report back... thanks again and good night!

— Reply to this email directly or view it on GitHubhttps://github.com/ros-planning/navigation/issues/147#issuecomment-30286615 .

HWiese1980 commented 10 years ago

Sorry, didn't have the time to do that, yesterday, but today I'm on it again. So, here comes a bag file (Dropbox) that will hopefully shed some light on what's going on in your planner on my system. I've performed some usual tasks, sending goals, waiting for the robot to arrive (or start dancing) and meanwhile changing _simtime through dynamic reconfigure between working and not so well working values. You'll hopefully be able to see what's happening. In case you need any further data, just tell me which other topics you need. I've recorded the whole environment into a big bag file, just filtered the topics relevant to your planner into the uploaded bag file for it to remain conveniently small. So it's all there.

HWiese1980 commented 10 years ago

Hey David, just curious, have you alread had the time and inclination to take a look at my bag file?

DLu commented 10 years ago

I took an initial look while trying to visualize the paths in Rviz, but that wasn't working for some reason. Are the models you're using proprietary, or can you share your whole simulation setup?

On Sat, Dec 14, 2013 at 1:40 PM, Hendrik Wiese notifications@github.comwrote:

Hey David, just curious, have you alread had the time and inclination to take a look at my bag file?

— Reply to this email directly or view it on GitHubhttps://github.com/ros-planning/navigation/issues/147#issuecomment-30585082 .

HWiese1980 commented 10 years ago

Hmm, strange... well, the whole simulation is available from our GitLab at http://git.rchh.org/public. However it's pretty complicated to set up. There is a ReadMe that should explain how to do it but I'm not sure if it covers everything needed. Might be a good opportunity to prove that. You can read it here: ReadMe. If you need any assistance setting it up, I'll certainly try my very best.

//edit: you should be able to clone the repository even with just guest access using the public urls instead of the git@rchh.org ones.

DLu commented 10 years ago

Actually, I think I figured out the bag. Can you add TF and the goals to the bag?

On Sun, Dec 15, 2013 at 2:47 AM, Hendrik Wiese notifications@github.comwrote:

Hmm, strange... well, the whole simulation is available from our GitLab at http://git.rchh.org/public. However it's pretty complicated to set up. There is a ReadMe that should explain how to do it but I'm not sure if it covers everything needed. Might be a good opportunity to prove that. You can read it here: ReadMehttp://rchh.org:81/software/otter4_documentation/tree/master. If you need any assistance setting it up, I'll certainly try my very best.

— Reply to this email directly or view it on GitHubhttps://github.com/ros-planning/navigation/issues/147#issuecomment-30600601 .

HWiese1980 commented 10 years ago

But of course. Here you are: bag file with DWA planner, TF and goals

HWiese1980 commented 10 years ago

Don't mean to rush you... but... have you been able to make use of the new bag file?

DLu commented 10 years ago

Were you recording the trajectory cloud as well when you made this bag?

On Tue, Dec 17, 2013 at 1:14 PM, Hendrik Wiese notifications@github.comwrote:

Don't mean to rush you... but... have you been able to make use of the new bag file?

— Reply to this email directly or view it on GitHubhttps://github.com/ros-planning/navigation/issues/147#issuecomment-30780832 .

HWiese1980 commented 10 years ago

I think, I recorded everything. I didn't set any filters. Yet, I'm not really sure about what the trajectory cloud is. If it's a regular topic then it should be recorded as well. If you name other topics I'll happily add them to the uploaded bag file. Easiest would probably be to upload the whole unfiltered bag file, but it's pretty large and I guess you won't need our robot specific, custom topics and messages. So... here's a list of all the recorded topics in the original bag file: rosbag info ...

DLu commented 10 years ago

Hey Hendrik, Can you email me directly at davidvlu@gmail so we can try to debug this offline? -David!!

On Tue, Dec 17, 2013 at 3:00 PM, Hendrik Wiese notifications@github.comwrote:

I think, I recorded everything. I didn't set any filters. Yet, I'm not really sure about what the trajectory cloud is. If it's a regular topic then it should be recorded as well. If you name other topics I'll happily add them to the uploaded bag file. Easiest would probably be to upload the whole unfiltered bag file, but it's pretty large and I guess you won't need our robot specific, custom topics and messages. So... here's a list of all the recorded topics in the original bag file: rosbag info ...http://pastebin.com/8utb0qn5

— Reply to this email directly or view it on GitHubhttps://github.com/ros-planning/navigation/issues/147#issuecomment-30790417 .

meiqua commented 6 years ago

We also faced the problem recently. After seeing your trial and going through the code, I'm quite sure that it's because the latch_xy_goal_tolerance is not working well.
latch_xy_goal_tolerance is designed to prevent robot moving once reaching the goal position. If we close it, the robot may go away from the goal then start local planning again during the in place rotating stage. So we can see the robot dancing sometimes. To check this, you can set latch_xy_goal_tolerance to false and should see the robot dancing in the same way.
The problem is from global planning. We don't want the robot being latched if we get a new goal, so when there is a new global plan, move_base will reset Latching. However, the global planning is never closed before we reach the goal position and orientation. During the last rotating stage, the orientation haven't been reached, so latching is reset.
An easy way to fix this bug is checking the global plan's goal before resetting latching in function DWAPlannerROS::setPlan. If latch_xy_goal_tolerance is what we want and global plan's goal is not changed (so we need to record an old goal), don't reset the latching.

zhengzh commented 5 years ago

@meiqua Do you solve the problem? Could you send me a patch, thanks!

meiqua commented 5 years ago

@meiqua Do you solve the problem? Could you send me a patch, thanks!

@zhengzh I figure out this problem just after our course project ended, so I haven't tested it yet... But you can try it out if you face the problem too. There is another quick dirty way, just set planner_frequency in move_base params to zero, then global planner will only work when we get a new goal or local planner fails, so latching won't be reset too often.

zhengzh commented 5 years ago

@meiqua Yes, I tested it, it worked! Just check whether the goal is changed before resetting the latching. here is my code.

meiqua commented 5 years ago

@zhengzh nice!

RalphKai commented 5 years ago

Hi, I have a similar problem. But my robot is unable to rotate in-place. but it passed the goal and only stop around 1 sec. and keep moving forward. Is there any suggestion

meiqua commented 5 years ago

@RalphKai Maybe your robot has not received a 0 cmd? When reaching the goal, topic cmd_vel stops publishing, but the last cmd your robot received is not 0, so it keeps going. If it's the reason, you can just make your robot stop if not receiving anything.

RalphKai commented 5 years ago

@meiqua thanks for reply! I have checked the cmd from dwa. It's still sending forward command to my driver. Because my robot is carlike robot, so it have some limitation to move exactly the goal's pose. After I set the xy_goal_tolerance larger then the problem fixed. But I think it's still very weird, It should be able to move backward and modify its plan until it get the goal. However, I haven't see it plan the reverse motion from dwa_local_planner. Any suggestion will be helpful. Thanks!

meiqua commented 5 years ago

@RalphKai dwa has some velocity limitation parameters:

~/min_vel_x (double, default: 0.0) The minimum x velocity for the robot in m/s, negative for backwards motion.

RalphKai commented 5 years ago

@meiqua thank you very much. It works fine now~

Deric-W commented 2 years ago

Hi, I had a similar problem without using a global planner on a differential wheeled robot. I managed to fix it by setting vx_samples to 10 (default 3) and vy_samples to 0 (default 10). Maybe this helps other people with the same problem.