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

Changed logic for when to resize layered costmap in static layer #792

Merged
merged 2 commits into from Jun 18, 2019
Merged

Changed logic for when to resize layered costmap in static layer #792

merged 2 commits into from Jun 18, 2019

Conversation

ghost
Copy link

@ghost ghost commented Sep 7, 2018

-Now the master layered costmap should no longer get resized by static layer when
isSizeLocked returns true

fixes #768

-Now the master layered costmap should no longer get resized when
isSizeLocked returns true
@ghost
Copy link
Author

ghost commented Dec 5, 2018

The issue solved by this pull request has also been solved in #795. Please advice as to the steps to get either of these pull requests approved.

Copy link
Contributor

@aaronhoy aaronhoy left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The logic LGTM. Let's fix up the formatting though. I would suggest

  if (!layered_costmap_->isRolling() && 
      !layered_costmap_->isSizeLocked() &&
      (master->getSizeInCellsX() != size_x ||
       master->getSizeInCellsY() != size_y ||
       master->getResolution() != new_map->info.resolution ||
       master->getOriginX() != new_map->info.origin.position.x ||
       master->getOriginY() != new_map->info.origin.position.y))

costmap_2d/plugins/static_layer.cpp Outdated Show resolved Hide resolved
costmap_2d/plugins/static_layer.cpp Outdated Show resolved Hide resolved
costmap_2d/plugins/static_layer.cpp Outdated Show resolved Hide resolved
@ghost
Copy link
Author

ghost commented Dec 10, 2018

aaronhoy, thank you for suggesting corrections, appreciate the pointers.

Copy link
Contributor

@aaronhoy aaronhoy left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

LGTM

@DLu DLu added costmap_2d kinetic PRs for kinetic-devel labels Feb 26, 2019
@DLu DLu merged commit 69ff9ba into ros-planning:kinetic-devel Jun 18, 2019
DLu added a commit that referenced this pull request Jun 18, 2019
* Changed logic for when to resize layered costmap in static layer

-Now the master layered costmap should no longer get resized when
isSizeLocked returns true

* Fixing format for if loop
@mechwiz
Copy link

mechwiz commented Jul 17, 2019

Hi,
I'm not sure if this is a separate issue or not but I noticed that with this change, the costmap never gets resized now after the first resizing occurs. This means that my global costmap (which I have set to be on the static layer with an inflation layer on top of it) is not being updated correctly with new occupancy grid messages during mapping. I would prefer the costmap to match the size and position of the map topic its subscribed to and to grow as the occupancy grid map does. I believe this is because in the following code, the size_locked parameter is set to true. Hence, when the isSizeLocked() function gets called next, it will be true (since no where else is that parameter updated as far as I can tell). Before the change mentioned earlier in the chain, the !layered_costmap_->isSizeLocked() always got resolved to false as part of being part of that big OR list (which was ok since the !layered_costmap_->isRolling() would still be true and the code block within the if-statement would fire, but now since it's part of the AND statement above, the costmap resizing never occurs.

// Update the size of the layered costmap (and all layers, including this one)
ROS_INFO("Resizing costmap to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
layered_costmap_->resizeMap(size_x, size_y, new_map->info.resolution, new_map->info.origin.position.x,
new_map->info.origin.position.y, true);

To test this theory, I set it to false, and my costmap updated correctly.

The log file shown below (taken with the size_locked parameter set to true) demonstrates this. I output the value of layered_costmap_->isSizeLocked() before the if-statement occurs as shown here:

// resize costmap if size, resolution or origin do not match
Costmap2D* master = layered_costmap_->getCostmap();
ROS_INFO("isSizeLocked: %d ", layered_costmap_->isSizeLocked());

The log file is here:

�[0m[ INFO] [1563377834.115638646, 1558644104.569229850]: Using plugin "static"�[0m
�[0m[ INFO] [1563377834.131965174, 1558644104.589358265]: Requesting the map...�[0m
�[0m[ INFO] [1563377834.533728097, 1558644104.992478528]: isSizeLocked: 0 �[0m
�[0m[ INFO] [1563377834.533757795, 1558644104.992478528]: Resizing costmap to 296 X 87 at 0.050000 m/pix�[0m
�[0m[ INFO] [1563377834.634133428, 1558644105.093436164]: Received a 296 X 87 map at 0.050000 m/pix�[0m
�[0m[ INFO] [1563377834.642749935, 1558644105.093436164]: Using plugin "inflation"�[0m
�[0m[ INFO] [1563377835.494782038, 1558644105.952308143]: isSizeLocked: 1 �[0m
�[0m[ INFO] [1563377835.494809855, 1558644105.952308143]: Resizing static layer to 296 X 87 at 0.050000 m/pix�[0m
�[0m[ INFO] [1563377836.515285845, 1558644106.972099552]: isSizeLocked: 1 �[0m
�[0m[ INFO] [1563377836.515333581, 1558644106.972099552]: Resizing static layer to 296 X 88 at 0.050000 m/pix�[0m
�[0m[ INFO] [1563377837.542664283, 1558644107.993611563]: isSizeLocked: 1 �[0m
�[0m[ INFO] [1563377837.542702069, 1558644107.993611563]: Resizing static layer to 297 X 88 at 0.050000 m/pix�[0m
�[0m[ INFO] [1563377838.586958524, 1558644109.043587034]: isSizeLocked: 1 �[0m
�[0m[ INFO] [1563377838.586997319, 1558644109.043587034]: Resizing static layer to 297 X 87 at 0.050000 m/pix�[0m
�[0m[ INFO] [1563377839.628621333, 1558644110.082591416]: isSizeLocked: 1 �[0m
�[0m[ INFO] [1563377840.710439168, 1558644111.162096268]: isSizeLocked: 1 �[0m
�[0m[ INFO] [1563377841.772654922, 1558644112.223333363]: isSizeLocked: 1 �[0m
�[0m[ INFO] [1563377842.839620531, 1558644113.293791071]: isSizeLocked: 1 �[0m
�[0m[ INFO] [1563377842.839647095, 1558644113.293791071]: Resizing static layer to 297 X 87 at 0.050000 m/pix�[0m
�[0m[ INFO] [1563377843.918805933, 1558644114.375617727]: isSizeLocked: 1 �[0m
�[0m[ INFO] [1563377843.918829805, 1558644114.375617727]: Resizing static layer to 297 X 87 at 0.050000 m/pix�[0m
�[0m[ INFO] [1563377845.012703208, 1558644115.464880366]: isSizeLocked: 1 �[0m
�[0m[ INFO] [1563377845.012729928, 1558644115.464880366]: Resizing static layer to 321 X 88 at 0.050000 m/pix�[0m

The log file after setting the parameter to false is shown below:

�[0m[ INFO] [1563378953.352665745, 1558644104.578246365]: Using plugin "static"�[0m
�[0m[ INFO] [1563378953.367412953, 1558644104.598365363]: Requesting the map...�[0m
�[0m[ INFO] [1563378953.873352085, 1558644105.104326012]: isSizeLocked: 0 �[0m
�[0m[ INFO] [1563378953.873409095, 1558644105.104326012]: Resizing costmap to 296 X 87 at 0.050000 m/pix�[0m
�[0m[ INFO] [1563378953.973668381, 1558644105.205502219]: Received a 296 X 87 map at 0.050000 m/pix�[0m
�[0m[ INFO] [1563378953.980215700, 1558644105.205502219]: Using plugin "inflation"�[0m
�[0m[ INFO] [1563378954.805175406, 1558644106.033145695]: isSizeLocked: 0 �[0m
�[0m[ INFO] [1563378954.805208989, 1558644106.033145695]: Resizing costmap to 296 X 87 at 0.050000 m/pix�[0m
�[0m[ INFO] [1563378955.827636544, 1558644107.053472605]: isSizeLocked: 0 �[0m
�[0m[ INFO] [1563378955.827663562, 1558644107.053472605]: Resizing costmap to 296 X 88 at 0.050000 m/pix�[0m
�[0m[ INFO] [1563378956.860063354, 1558644108.083633575]: isSizeLocked: 0 �[0m
�[0m[ INFO] [1563378956.860090997, 1558644108.083633575]: Resizing costmap to 297 X 87 at 0.050000 m/pix�[0m
�[0m[ INFO] [1563378957.903980301, 1558644109.134484380]: isSizeLocked: 0 �[0m
�[0m[ INFO] [1563378958.951264515, 1558644110.175823230]: isSizeLocked: 0 �[0m
�[0m[ INFO] [1563378960.032215248, 1558644111.256436674]: isSizeLocked: 0 �[0m
�[0m[ INFO] [1563378961.100435385, 1558644112.327652012]: isSizeLocked: 0 �[0m
�[0m[ INFO] [1563378962.177538244, 1558644113.408071297]: isSizeLocked: 0 �[0m
�[0m[ INFO] [1563378962.177703361, 1558644113.408071297]: Resizing costmap to 297 X 87 at 0.050000 m/pix�[0m
�[0m[ INFO] [1563378963.281821808, 1558644114.510617325]: isSizeLocked: 0 �[0m
�[0m[ INFO] [1563378963.281879404, 1558644114.510617325]: Resizing costmap to 297 X 87 at 0.050000 m/pix�[0m
�[0m[ INFO] [1563378964.372661397, 1558644115.602424481]: isSizeLocked: 0 �[0m
�[0m[ INFO] [1563378964.372687153, 1558644115.602424481]: Resizing costmap to 321 X 88 at 0.050000 m/pix�[0m
�[0m[ INFO] [1563378965.492818412, 1558644116.723105063]: isSizeLocked: 0 �[0m
�[0m[ INFO] [1563378965.492847135, 1558644116.723105063]: Resizing costmap to 321 X 90 at 0.050000 m/pix�[0m
�[0m[ INFO] [1563378966.646462455, 1558644117.873629736]: isSizeLocked: 0 �[0m
�[0m[ INFO] [1563378966.646485179, 1558644117.873629736]: Resizing costmap to 321 X 92 at 0.050000 m/pix�[0m

I'm wondering if anything can be done to fix this issue - I'm assuming setting that parameter to false is not a good long-term solution for those people who want the size locked. I do realize that this is really only an issue for doing mapping since in localization, the map is static from the getgo and is not changing. Maybe the correct solution is just to use a rolling-window for my global costmap.

EDIT

It seems that this "temporary fix" of changing the size_locked parameter to false actually breaks the system when I want to load a pre-made map such as for localization purposes. The global costmap does not seem to load at all. In order for me to get it to load, I have to launch the map server, kill it, and then launch it again (which effectively publishes the map twice), which is not ideal.
I'll give this some more thought about what is exactly happening, but in the meantime the code that existed before the commit works perfectly for both situations.

LeroyR pushed a commit to CentralLabFacilities/navigation that referenced this pull request Aug 27, 2019
@wakingking77
Copy link

Hi,
I'm not sure if this is a separate issue or not but I noticed that with this change, the costmap never gets resized now after the first resizing occurs. This means that my global costmap (which I have set to be on the static layer with an inflation layer on top of it) is not being updated correctly with new occupancy grid messages during mapping. I would prefer the costmap to match the size and position of the map topic its subscribed to and to grow as the occupancy grid map does. I believe this is because in the following code, the size_locked parameter is set to true. Hence, when the isSizeLocked() function gets called next, it will be true (since no where else is that parameter updated as far as I can tell). Before the change mentioned earlier in the chain, the !layered_costmap_->isSizeLocked() always got resolved to false as part of being part of that big OR list (which was ok since the !layered_costmap_->isRolling() would still be true and the code block within the if-statement would fire, but now since it's part of the AND statement above, the costmap resizing never occurs.

// Update the size of the layered costmap (and all layers, including this one)
ROS_INFO("Resizing costmap to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
layered_costmap_->resizeMap(size_x, size_y, new_map->info.resolution, new_map->info.origin.position.x,
new_map->info.origin.position.y, true);

To test this theory, I set it to false, and my costmap updated correctly.

The log file shown below (taken with the size_locked parameter set to true) demonstrates this. I output the value of layered_costmap_->isSizeLocked() before the if-statement occurs as shown here:

// resize costmap if size, resolution or origin do not match
Costmap2D* master = layered_costmap_->getCostmap();
ROS_INFO("isSizeLocked: %d ", layered_costmap_->isSizeLocked());

The log file is here:

�[0m[ INFO] [1563377834.115638646, 1558644104.569229850]: Using plugin "static"�[0m
�[0m[ INFO] [1563377834.131965174, 1558644104.589358265]: Requesting the map...�[0m
�[0m[ INFO] [1563377834.533728097, 1558644104.992478528]: isSizeLocked: 0 �[0m
�[0m[ INFO] [1563377834.533757795, 1558644104.992478528]: Resizing costmap to 296 X 87 at 0.050000 m/pix�[0m
�[0m[ INFO] [1563377834.634133428, 1558644105.093436164]: Received a 296 X 87 map at 0.050000 m/pix�[0m
�[0m[ INFO] [1563377834.642749935, 1558644105.093436164]: Using plugin "inflation"�[0m
�[0m[ INFO] [1563377835.494782038, 1558644105.952308143]: isSizeLocked: 1 �[0m
�[0m[ INFO] [1563377835.494809855, 1558644105.952308143]: Resizing static layer to 296 X 87 at 0.050000 m/pix�[0m
�[0m[ INFO] [1563377836.515285845, 1558644106.972099552]: isSizeLocked: 1 �[0m
�[0m[ INFO] [1563377836.515333581, 1558644106.972099552]: Resizing static layer to 296 X 88 at 0.050000 m/pix�[0m
�[0m[ INFO] [1563377837.542664283, 1558644107.993611563]: isSizeLocked: 1 �[0m
�[0m[ INFO] [1563377837.542702069, 1558644107.993611563]: Resizing static layer to 297 X 88 at 0.050000 m/pix�[0m
�[0m[ INFO] [1563377838.586958524, 1558644109.043587034]: isSizeLocked: 1 �[0m
�[0m[ INFO] [1563377838.586997319, 1558644109.043587034]: Resizing static layer to 297 X 87 at 0.050000 m/pix�[0m
�[0m[ INFO] [1563377839.628621333, 1558644110.082591416]: isSizeLocked: 1 �[0m
�[0m[ INFO] [1563377840.710439168, 1558644111.162096268]: isSizeLocked: 1 �[0m
�[0m[ INFO] [1563377841.772654922, 1558644112.223333363]: isSizeLocked: 1 �[0m
�[0m[ INFO] [1563377842.839620531, 1558644113.293791071]: isSizeLocked: 1 �[0m
�[0m[ INFO] [1563377842.839647095, 1558644113.293791071]: Resizing static layer to 297 X 87 at 0.050000 m/pix�[0m
�[0m[ INFO] [1563377843.918805933, 1558644114.375617727]: isSizeLocked: 1 �[0m
�[0m[ INFO] [1563377843.918829805, 1558644114.375617727]: Resizing static layer to 297 X 87 at 0.050000 m/pix�[0m
�[0m[ INFO] [1563377845.012703208, 1558644115.464880366]: isSizeLocked: 1 �[0m
�[0m[ INFO] [1563377845.012729928, 1558644115.464880366]: Resizing static layer to 321 X 88 at 0.050000 m/pix�[0m

The log file after setting the parameter to false is shown below:

�[0m[ INFO] [1563378953.352665745, 1558644104.578246365]: Using plugin "static"�[0m
�[0m[ INFO] [1563378953.367412953, 1558644104.598365363]: Requesting the map...�[0m
�[0m[ INFO] [1563378953.873352085, 1558644105.104326012]: isSizeLocked: 0 �[0m
�[0m[ INFO] [1563378953.873409095, 1558644105.104326012]: Resizing costmap to 296 X 87 at 0.050000 m/pix�[0m
�[0m[ INFO] [1563378953.973668381, 1558644105.205502219]: Received a 296 X 87 map at 0.050000 m/pix�[0m
�[0m[ INFO] [1563378953.980215700, 1558644105.205502219]: Using plugin "inflation"�[0m
�[0m[ INFO] [1563378954.805175406, 1558644106.033145695]: isSizeLocked: 0 �[0m
�[0m[ INFO] [1563378954.805208989, 1558644106.033145695]: Resizing costmap to 296 X 87 at 0.050000 m/pix�[0m
�[0m[ INFO] [1563378955.827636544, 1558644107.053472605]: isSizeLocked: 0 �[0m
�[0m[ INFO] [1563378955.827663562, 1558644107.053472605]: Resizing costmap to 296 X 88 at 0.050000 m/pix�[0m
�[0m[ INFO] [1563378956.860063354, 1558644108.083633575]: isSizeLocked: 0 �[0m
�[0m[ INFO] [1563378956.860090997, 1558644108.083633575]: Resizing costmap to 297 X 87 at 0.050000 m/pix�[0m
�[0m[ INFO] [1563378957.903980301, 1558644109.134484380]: isSizeLocked: 0 �[0m
�[0m[ INFO] [1563378958.951264515, 1558644110.175823230]: isSizeLocked: 0 �[0m
�[0m[ INFO] [1563378960.032215248, 1558644111.256436674]: isSizeLocked: 0 �[0m
�[0m[ INFO] [1563378961.100435385, 1558644112.327652012]: isSizeLocked: 0 �[0m
�[0m[ INFO] [1563378962.177538244, 1558644113.408071297]: isSizeLocked: 0 �[0m
�[0m[ INFO] [1563378962.177703361, 1558644113.408071297]: Resizing costmap to 297 X 87 at 0.050000 m/pix�[0m
�[0m[ INFO] [1563378963.281821808, 1558644114.510617325]: isSizeLocked: 0 �[0m
�[0m[ INFO] [1563378963.281879404, 1558644114.510617325]: Resizing costmap to 297 X 87 at 0.050000 m/pix�[0m
�[0m[ INFO] [1563378964.372661397, 1558644115.602424481]: isSizeLocked: 0 �[0m
�[0m[ INFO] [1563378964.372687153, 1558644115.602424481]: Resizing costmap to 321 X 88 at 0.050000 m/pix�[0m
�[0m[ INFO] [1563378965.492818412, 1558644116.723105063]: isSizeLocked: 0 �[0m
�[0m[ INFO] [1563378965.492847135, 1558644116.723105063]: Resizing costmap to 321 X 90 at 0.050000 m/pix�[0m
�[0m[ INFO] [1563378966.646462455, 1558644117.873629736]: isSizeLocked: 0 �[0m
�[0m[ INFO] [1563378966.646485179, 1558644117.873629736]: Resizing costmap to 321 X 92 at 0.050000 m/pix�[0m

I'm wondering if anything can be done to fix this issue - I'm assuming setting that parameter to false is not a good long-term solution for those people who want the size locked. I do realize that this is really only an issue for doing mapping since in localization, the map is static from the getgo and is not changing. Maybe the correct solution is just to use a rolling-window for my global costmap.

EDIT

It seems that this "temporary fix" of changing the size_locked parameter to false actually breaks the system when I want to load a pre-made map such as for localization purposes. The global costmap does not seem to load at all. In order for me to get it to load, I have to launch the map server, kill it, and then launch it again (which effectively publishes the map twice), which is not ideal.
I'll give this some more thought about what is exactly happening, but in the meantime the code that existed before the commit works perfectly for both situations.

thank you very much. I will flow your advice to modify my code, and I hope it will work

GodCed added a commit to GodCed/navigation that referenced this pull request Jan 8, 2020
* clear area in layer for melodic

* Added publishZeroVelocity() before starting planner (ros-planning#751)

Edit for Issue ros-planning#750

* Changed logic for when to resize layered costmap in static layer (ros-planning#792)

* Changed logic for when to resize layered costmap in static layer

-Now the master layered costmap should no longer get resized when
isSizeLocked returns true

* Fixing format for if loop

* Provide different negative values for unknown and out-of-map costs (ros-planning#833)

* Add force_updating and affected_maps parameters to tailor clear costmaps recovey (ros-planning#838)

behavior

* Costmap_2d plugin universal parameters and pre-hydro warnings (ros-planning#738)

* Comment and description clarification

* Renamed resetOldParameters to loadOldParameters

* Upscaled pre-hydro parameter info message to warning and added costmap-name

* Warn user when static_map or map_type is set but not used while plugins are used

* Added function that copies parent parameters inside each layer (makes it possible to set a global inflation_radius)

* use parameter magic

* Fixes ros-planning#782 (ros-planning#892)

* Drop Parameter Magic (ros-planning#893)

* Fix typo in amcl_laser model header (ros-planning#918)

* Cherry pick ros-planning#914 (ros-planning#919)

* update changelogs

* 1.16.3

Co-authored-by: Steven Macenski <stevenmacenski@gmail.com>
Co-authored-by: SUNIL SULANIA <sunilsulania9192@gmail.com>
Co-authored-by: David V. Lu!! <davidvlu@gmail.com>
Co-authored-by: Jorge Santos Simón <jsantossimon@gmail.com>
Co-authored-by: Martin Ganeff <martinganeff@gmail.com>
Co-authored-by: Hadi Tabatabaee <hadi.tab@gmail.com>
Co-authored-by: Michael Ferguson <mfergs7@gmail.com>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
costmap_2d kinetic PRs for kinetic-devel
Projects
None yet
Development

Successfully merging this pull request may close these issues.

5 participants