Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[Bug] [Global Planner] Lethal cost is always 1 unit smaller than user setting #1093

Closed
siferati opened this issue Jan 22, 2021 · 4 comments · May be fixed by #1240
Closed

[Bug] [Global Planner] Lethal cost is always 1 unit smaller than user setting #1093

siferati opened this issue Jan 22, 2021 · 4 comments · May be fixed by #1240

Comments

@siferati
Copy link

Shouldn't the line below read c < lethal_cost_ instead of c < lethal_cost_ - 1?

As it stands, a non-lethal cell with cost lethal_cost_ - 1 is treated as lethal. This means the lethal cost is actually 1 unit smaller than the user setting (i.e. if user sets lethal cost to 100, the actual lethal cost is 99).

if (c < lethal_cost_ - 1 || (unknown_ && c==255)) {

If this is indeed a bug and not-intended, I can make a quick PR to fix it.

@ZJUTongYang
Copy link

I think there is no bug. In the costmap (not the occupancy grid), value 254 marks only physical obstacles. Value 253 marks the C-space obstacles, i.e., the robot will definitely hit obstacles when it stays here. So it is reasonable to also avoid expanding nodes into the C-space obstacles.

However, I noticed that your lethal cost is 100. The range of cost value changes from [0, 255] (costmap_2d) to [-1, 100] (nav_msgs/occupancyGrid), so I'm not sure whether there is a bug when you input the map in a form of occupancy grid.

@siferati
Copy link
Author

siferati commented Jan 28, 2021

The code I linked above is working with the cost values of the costmap [0, 255] and not the occupancy grid [-1, 100], so I think we can ignore the latter for the problem at hand. The lethal_cost of 100 was merely an example I gave in the range [0, 255].

So it is reasonable to also avoid expanding nodes into the C-space obstacles.

Yes, I agree with this. Isn't it because of this reason that the default value of lethal_cost is set to 253 and not 254?

However, if you take a look at the snippet I linked above, you will notice that when using the default lethal_value 253, the actual lethal_value would be 252 instead - cells with cost 252 would not be expanded.

Basically, lethal_cost is always 1 unit smaller than the user / default setting because of the < sign. It should either be <=, or lethal_cost_ - 1 should be replaced with lethal_cost_.

@mikeferguson
Copy link
Contributor

I think the second comment here is correct - If you take a look at cost_values.h, "lethal - 1" is INSCRIBED_INFLATED_OBSTACLE - those are points that are also not valid (the inscribed footprint is in collision, so the cell is invalid at ANY orientation).

@siferati
Copy link
Author

I think the second comment here is correct - If you take a look at cost_values.h, "lethal - 1" is INSCRIBED_INFLATED_OBSTACLE - those are points that are also not valid (the inscribed footprint is in collision, so the cell is invalid at ANY orientation).

@mikeferguson Those values are applied to the costmap, not the global planner.
The lethal_cost_ parameter of the global planner does not have the same meaning as the costmap_2d::LETHAL_OBSTACLE constant, that's the whole point of the parameter being dynamic reconfigurable.

And that's also why the global planner defaults this value to 253 (i.e. costmap_2d::INSCRIBED_INFLATED_OBSTACLE) rather than defaulting it to 254 (i.e. costmap_2d::LETHAL_OBSTACLE). Because for the global planner, a cell of inscribed cost is indeed a lethal cell -- the robot can not traverse it so it shouldn't plan through that cell.

gen.add("lethal_cost", int_t, 0, "Lethal Cost", 253, 1, 255)

However, as it stands, the global planner will also treat cells of cost 252 as lethal, which is obviously wrong.

I fixed this issue in #1240

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
3 participants