Regulated Purepursuit is slow #2332
Comments
|
What do you mean "runs too slowly"? I can get this to run at 1khz easily on my 7th gen i5. Please provide much more detail about exactly what your issue is, what you have tried to solve it, and any relevant compute / robot platform details. Also provide your actual config file. |
|
@vimalrajayyappan can you give more context / info? |
|
@vimalrajayyappan we'll have to close this ticket in about a week if we don't receive a response |
|
Hi, While running the experiments with the pure pursuit in January, I experienced an effect that made the robot move slow, even if I configured the params to move faster. Maybe it is what @vimalrajayyappan describes. I will try to remember the reason and the fix. I remember something related to the existence of the Best |
|
Yes, exactly friend :). Kindly help @fmrico and @SteveMacenski :). |
|
Happy to fix an issue if you guys can narrow it down a bit - so far I'm not seeing enough to really start debugging Edit; Things to make sure that you've set your acceleration profiles so you can accelerate enough to get up to your maximum desired speed in a reasonable period of time. If you disable To @fmrico comment, if your odometry in is slow, then it will cause problems because we use the current speed to determine the kinematic max accel / decel to use. OR if it didn't get your |
|
I did a short testing by changing the acceleration limits. I think the point made by @SteveMacenski seems to be vaild. Various acceleration limits are showing different behaviors. I have made a small plot showing the difference The first image on the top is the RPP with acceleration limit set to 2.5 and the second image is when the acceleration limit is set to around 7.0. There is a clear difference.
Hope this helps. Edit: I did the test using the robot MPO-700 found in neo_simulation-2 package. |
|
Yeah, if your acceleration limits are too small, then the RPP will cap the speed updates by the kinematic feasibility via (v_new = v_old + a * dt) which will ask the robot to execute a slower velocity -- thusly your odometry would be slower as the execution of that task. But eventually you should be able to achieve that speed, it would just take longer. If you're seeing that you can't at all even achieve that speed, that's a different issue. @padhupradheep did you try with the params @vimalrajayyappan provided? His max speed is something like @vimalrajayyappan, Pradheep is interested in working on this problem on your behalf, but some more information about what exactly you're seeing or reproduction instructions are necessary to make effective use of his limited time. Please answer the questions in my first comment in this ticket to provide the details needed to analyze the problem. |
|
Some more findings:
[ERROR] [controller_server-4]: process has died [pid 21214, exit code -11, cmd '/opt/ros/foxy/lib/nav2_controller/controller_server --ros-args --params-file /tmp/tmps7dkfreq -r /tf:=tf -r /tf_static:=tf_static'].
This above causes irrespective to the maximum acceleration or the velocity that has been set (Note, we have a good processor and neo_local_planner works well at 100 Hz). Also we have odom running at 100 Hz FYI, I used the same parameters given above by @vimalrajayyappan. |
Really? Now that is interesting. Did you try to get a traceback (this tutorial would help)? I've definitely tested at > 20hz, that's how I found I could get it to run at 1 khz so maybe your param isn't being read in? |
|
|
That's really odd, #2437 is seeing that too -- which may or not be related. But that doesn't necessarily feel related to the initial ticket that if you set the param of speed > 2.5 m/s it doesn't reach those speeds, not that it crashes. That appears to be an unrelated issue that also should be addressed. On what branch are you testing this? The only recent change I can find that makes me even somewhat suspect is c616cf0, maybe worth testing reverting that if you're on the |
Note that the crash happens exactly at the point when the robot reaches 2.5 m/s. Also, @vimalrajayyappan stated that:
But he did not exactly give the details on what speed it was running at, for the given parameters.
I'm testing it on Foxy but I will do checkout the suspicious commit. |
|
Well both the ticket issuer and Francisco mention that it just didn't go as fast as they wanted, they would have mentioned the crash if the crash was the issue. Again, its semantics, they're both problems and honestly the crash is a more serious problem anyway. Given Galactic binaries are available, I'd be curious if you saw it there. You can also run that on 20.04 so it should be an easy transition to test. #2437 (comment) tested without those commits already and it was still occurring. It might be good to sanity check them, but I think we can tentatively assume its neither of those commits. It might be safe to try to address this problem assuming that its a standalone issue in RPP that we're trying to access a cell that is out of bounds for some reason. Edit: Oooooh I think I know what this might be. To verify: print what the mx/my are and what the costmap size is https://github.com/ros-planning/navigation2/blob/main/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp#L407. While its running, visualize the collision check arc points in rviz. What I think is happening is that since the costmap is a rolling costmap of finite size and if you set the robot's max speed to 10m/s (lets say) then if you set the look ahead collision checking time to 1 second, then it will try to look for collisions 10m away at the most. But if your rolling costmap size is only 5m, then it would go out of bounds and seg fault. What I think we should do about that (assuming that I am correct) is to print a throttled warning to every 30 seconds that you've configured your costmap too small to safely collision check the full distance away at your high speeds, proceed at your own caution. We can prevent the seg fault by simply checking if That still doesn't answer the question of why it couldn't go faster than 2.5 m/s in particular, unless that's the magic number of roughly the lookahead time * rolling costmap half size. |
|
Costmap size was indeed causing the problem. I updated the height and the width of the costmap. Now, there is no more issue. I gave it a run by setting the desired velocity to 8.0 m/s, you can see the plot below on how it works:
We just need to do the the costmap size checks and leave out a warning to avoid a Seg fault. |
|
Awesome!! Is that something you can open a PR on? I think it should be separate of your reversing PR (and that way you get credit for 2 merge commits 😉 ) to keep things isolated. I can totally understand why that caused the crash, not entirely sure why that caused the speed limit issues, but I won't argue with it 😄 |
I will gladly do it !
That's for another day! :D |
|
Why don't we just have this as a feature to the Costmaps rather than having it "local" to the RPP ? By that way, if we or someone else plan to write a local planner in the future, this functionality might be already handy. They could just call this function from the costmap to check for it. Edit: I believe, we already have this functionality in the Costmap API. While we convert the robots pose to map frame here. On further investigating the function here, we can just have a boolean that could check if the robot goes beyond the costmap bounds. By this way, we can see if the robot is going beyond the dimensions of the given cost map and just leave out the warning at that point. |
Here is a another scenario, where the controller tends to seg fault. The height and the width of my local costmap is 1x1. Even if we take care of the collision check scenario (as explained above). Here I have a lookahead distance certainly greater than that of the local costmap. This is another part which needs further investigation and needs to be taken care of as well! Note that the above parameters works well, if the size of the local costmap was something greater than that of the lookahead distance. Edit: Also we need to know that, irrespective of the Lookahead Distance, the carrot pose will be at the edge of the local costmap. With these parameters the segfault happens at the point when the robot goes beyond the carrot pose. The reason for the robot going beyond the carrot could be caused because the carrot continues to respect the local costmap, which apparently gets updated at a slower rate and also because of the fact that it moves relatively slower than that of the robot. Edit-2: Okay, now I have to disagree with the above edit, for the point where I say:
The rest of the points in the Edit still stays valid. It is not necessary for the pose of the carrot to be at the edge of the given local costmap. Sorry for the confusion! |
That could be one way to do it, as we're querying points just checking bounds. Typically I don't use that API because bound-checking has a non-trivial performance hit when you're querying ALOT of nodes, but in this context I think that's the best solution. This controller was running in excess of 1 Khz in my testing, we can certainly handle a little performance hit for the sake of code readability and we're not querying on the order of thousands or millions of cells 😄 The second problem you brought up could be handled the same way, using the bound respecting API for getting info from the costmap |
|
Merged! |
FollowPath:
plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
desired_linear_vel: 7.2
max_linear_accel: 2.5
max_linear_decel: 2.5
lookahead_dist: 1.8 #0.6
min_lookahead_dist: 1.0 #0.3
max_lookahead_dist: 2.0 #0.9
lookahead_time: 3.0 #1.5
rotate_to_heading_angular_vel: 1.8
transform_tolerance: 0.1
use_velocity_scaled_lookahead_dist: false
min_approach_linear_velocity: 0.05
use_approach_linear_velocity_scaling: true
max_allowed_time_to_collision: 1.0
use_regulated_linear_velocity_scaling: true
use_cost_regulated_linear_velocity_scaling: false
regulated_linear_scaling_min_radius: 0.9
regulated_linear_scaling_min_speed: 0.25
use_rotate_to_heading: true
rotate_to_heading_min_angle: 0.785
max_angular_accel: 3.2
The tuning parameters mentioned above are the one I'm using. It may look wierd, but I tried giving the max values possible to check any reflecting changes. But I couldnt find any. The purepursuit is running at its slow speed as it used to be. Am I missing anything/ Can you please help me on that?
The text was updated successfully, but these errors were encountered: