Fix for the seg-fault that occurred when RPP was tested at high speed #2474

padhupradheep · 2021-07-23T13:26:44Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#2332
Primary OS tested on	Ubuntu
Robotic platform tested on	mp-400 from Neobotix

Description of contribution in a few bullet points

Fixed the segfault issue that occurred when the robot was set to drive at high speed using RPP.

Description of documentation updates required from your changes

NA

Future work that may be required in bullet points

NA

For Maintainers:

Check that any new parameters added are updated in navigation.ros.org
Check that any significant change is added to the migration guide
Check that any new functions have Doxygen added
Check that any new features have test coverage
Check that any new plugins is added to the plugins page
If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

SteveMacenski

Good first stab, only a couple of simple tweaks and this can be merged

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

...troller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp

SteveMacenski · 2021-07-23T17:43:49Z

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

@@ -450,7 +456,13 @@ bool RegulatedPurePursuitController::inCollision(const double & x, const double
 double RegulatedPurePursuitController::costAtPose(const double & x, const double & y)
 {
  unsigned int mx, my;
-  costmap_->worldToMap(x, y, mx, my);
+  auto convertWorldToMap = costmap_->worldToMap(x, y, mx, my);
+  if (!convertWorldToMap) {


This function is only used when the x,y represents our robot pose, this should never be out of bounds.

So I think these changes in this function can be reverted?

I think it is not a good idea. If you remember the second issue that I brought up in #2332 , these changes actually solves that issue.

This function is only used when the x,y represents our robot pose, this should never be out of bounds.

Explaining it further: With the parameters set in that issue, the current position of the robot can sometimes be outside of the local costmap. Then again, we have a seg fault. In order to avoid it, we check for the bounds.

Please give me an example flow of data that this would prevent? What I see is:

costAtPose is only used as an argument for applyConstraints()

the pose given to applyConstraints is pose, which in that function I track to the argument pose https://github.com/ros-planning/navigation2/blob/8595b870dd66ef709101786a13eb86a5fc889e76/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp#L243

that pose represents the robot's current pose -- so there's no look ahead applied and my assumption is that at all times the robot should be on the costmap. If the local costmap is rolling, the robot should always be in the center. If the local costmap is static and the robot is outside of it, that's a bad configuration which all algorithms will fail.

The point you bring up about the lookahead distance being off the costmap is a good point, but where specifically does that apply here? I decoupled in the code the lookahead distance / carrot from the collision checking. We never actually check the validity of the carrot or lookahead distance, only the output velocity command for the time simulation you have covered in the other function

that pose represents the robot's current pose -- so there's no look ahead applied and my assumption is that at all times the robot should be on the costmap.

Okay, I totally agree with the data flow you have mentioned above this point. But, the actual behavior is other way round to the actual assumption you are defining. It is always not the case, there are instance where the current position of the robot can be outside the local costmap. Even before conducting this experiment, I was also under the same assumption, but by setting the robot go at high speed, the rolling costmap will take sometime to catch up with the robot (maybe because of the update rate). Example:

Note that, this works only with the fix I have made above. Without the fix, it ends up in a segfault as follows:

Maybe, I'm observing it in totally different way, that might be not correct. It is a bit tricky but an interesting one!

That seems.... bad. I know you set it to 1x1 to prove a point, but what if this was a more normal size like 5x5 or a more reasonable update rate like 5hz? We should cover the crash, you bring up a good point, but those are some extraordinarily poor configurations 😆. I think we just need to change the logging error message for this context to be more specific and clear about what this issue is: robot is off the costmap due to poor configuration. This shouldn't be throttled like before, we should complain every single iteration that happens as a critical issue since there is no possible collision avoidance at all in this situation

Edit: Actually, we should outright fail. That's a massive safety issue, log a FATAL message and throw an exception. The other issue is less severe if we can only collision check as far in the future as the costmap's size, this is an outright non-stop-robot-collision maker.

SteveMacenski · 2021-08-16T19:11:53Z

Any update?

padhupradheep · 2021-08-17T13:03:14Z

Any update?

Expect a commit for the above reviews, on this friday.

SteveMacenski · 2021-08-20T18:10:24Z

Pull in master or rebase to get CI turning over, has to do with a stale branch from changes we made to sync with Rolling

SteveMacenski

Mostly just wording changes to be more explicit

SteveMacenski · 2021-08-20T18:11:51Z

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

+      "The dimensions of the costmap is smaller, the controller failed and so the robot cannot proceed further");
+    throw nav2_core::PlannerException(
+            "RegulatedPurePursuitController detected poor configuration of local costmap!");
+    return std::numeric_limits<double>::max();


Nothing after a throw is executed, so you can remove this return line.

SteveMacenski · 2021-08-20T18:12:12Z

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

-  costmap_->worldToMap(x, y, mx, my);
+
+  if (!costmap_->worldToMap(x, y, mx, my)) {
+    RCLCPP_FATAL_ONCE(


I would not make this a ONCE, this should be a FATAL warning every single time it gets triggered

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

…uit_controller.cpp Co-authored-by: Steve Macenski <[email protected]>

padhupradheep · 2021-08-20T20:16:09Z

created new PR #2526

Fix for the seg-fault that occurred when RPP was tested at high speed

8595b87

SteveMacenski requested changes Jul 23, 2021

View reviewed changes

review update

fb21fc3

SteveMacenski requested changes Aug 20, 2021

View reviewed changes

padhupradheep and others added 3 commits August 20, 2021 21:59

Update nav2_regulated_pure_pursuit_controller/src/regulated_pure_purs…

d836e48

…uit_controller.cpp Co-authored-by: Steve Macenski <[email protected]>

Update nav2_regulated_pure_pursuit_controller/src/regulated_pure_purs…

a72b840

…uit_controller.cpp Co-authored-by: Steve Macenski <[email protected]>

Update nav2_regulated_pure_pursuit_controller/src/regulated_pure_purs…

9dc6641

…uit_controller.cpp Co-authored-by: Steve Macenski <[email protected]>

padhupradheep closed this Aug 20, 2021

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Fix for the seg-fault that occurred when RPP was tested at high speed #2474

Fix for the seg-fault that occurred when RPP was tested at high speed #2474

padhupradheep commented Jul 23, 2021 •

edited by SteveMacenski

Loading

SteveMacenski left a comment

SteveMacenski Jul 23, 2021

SteveMacenski Jul 23, 2021

padhupradheep Jul 26, 2021 •

edited

Loading

SteveMacenski Jul 26, 2021 •

edited

Loading

padhupradheep Jul 27, 2021 •

edited

Loading

SteveMacenski Jul 27, 2021 •

edited

Loading

SteveMacenski commented Aug 16, 2021

padhupradheep commented Aug 17, 2021

SteveMacenski commented Aug 20, 2021

SteveMacenski left a comment

SteveMacenski Aug 20, 2021

SteveMacenski Aug 20, 2021

padhupradheep commented Aug 20, 2021

Fix for the seg-fault that occurred when RPP was tested at high speed #2474

Fix for the seg-fault that occurred when RPP was tested at high speed #2474

Conversation

padhupradheep commented Jul 23, 2021 • edited by SteveMacenski Loading

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Future work that may be required in bullet points

For Maintainers:

SteveMacenski left a comment

Choose a reason for hiding this comment

SteveMacenski Jul 23, 2021

Choose a reason for hiding this comment

SteveMacenski Jul 23, 2021

Choose a reason for hiding this comment

padhupradheep Jul 26, 2021 • edited Loading

Choose a reason for hiding this comment

SteveMacenski Jul 26, 2021 • edited Loading

Choose a reason for hiding this comment

padhupradheep Jul 27, 2021 • edited Loading

Choose a reason for hiding this comment

SteveMacenski Jul 27, 2021 • edited Loading

Choose a reason for hiding this comment

SteveMacenski commented Aug 16, 2021

padhupradheep commented Aug 17, 2021

SteveMacenski commented Aug 20, 2021

SteveMacenski left a comment

Choose a reason for hiding this comment

SteveMacenski Aug 20, 2021

Choose a reason for hiding this comment

SteveMacenski Aug 20, 2021

Choose a reason for hiding this comment

padhupradheep commented Aug 20, 2021

padhupradheep commented Jul 23, 2021 •

edited by SteveMacenski

Loading

padhupradheep Jul 26, 2021 •

edited

Loading

SteveMacenski Jul 26, 2021 •

edited

Loading

padhupradheep Jul 27, 2021 •

edited

Loading

SteveMacenski Jul 27, 2021 •

edited

Loading