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.

Sign up for GitHub

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

Jump to bottom

Do only consider enabled costmap plugins/filters in the isCurrent() method of a LayeredCostmap #3356

Merged
merged 2 commits into from
Jan 10, 2023
Merged

Do only consider enabled costmap plugins/filters in the isCurrent() method of a LayeredCostmap #3356

merged 2 commits into from
Jan 10, 2023

Conversation

milidam
Copy link
Contributor

@milidam milidam commented Jan 10, 2023 •
edited by SteveMacenski
Loading

When clearing entirely a costmap (see the clear_entirely_* service), its layers are reset. This sets their current_ attribute to False, until they are updated (updateCosts()). However, for disabled plugins/filters, the updateCosts() is bypassed so that the current_ attribute is never reset to True, and the costmap never becomes "current" again; because of that the controller_server used to get stuck in an endless loop (see while (!costmap_ros_->isCurrent()) in ControllerServer::computeControl()).

This patch fixes that by adding a condition for not considering disabled plugins/filters in the LayeredCostmap::isCurrent().

Basic Info

Info Please fill out this column
Ticket(s) this addresses #3353
Primary OS tested on
Robotic platform tested on

Description of contribution in a few bullet points

Description of documentation updates required from your changes

N/A

Future work that may be required in bullet points

N/A

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 features OR changes to existing behaviors are reflected in the tuning 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

@milidam
Do only consider enabled costmap plugins/filters in the isCurrent() m…
e2e3737 
…ethod of a LayeredCostmap

When clearing entirely a costmap (see the `clear_entirely_*` service),
its layers are reset. This sets their `current_` attribute to False,
until they are updated (`updateCosts()`). However, for disabled
plugins/filters, the `updateCosts()` is bypassed so that the `current_`
attribute is never reset to True, and the costmap never becomes
"current" again; because of that the controller_server used to get stuck
in an endless loop (see `while (!costmap_ros_->isCurrent())`
in `ControllerServer::computeControl()`).

This patch fixes that by adding a condition for not considering disabled
plugins/filters in the `LayeredCostmap::isCurrent()`.
@SteveMacenski
Copy link
Member

SteveMacenski commented Jan 10, 2023 •
edited
Loading

/opt/overlay_ws/src/navigation2/nav2_costmap_2d/src/layered_costmap.cpp:259:67: error: ‘using element_type = class nav2_costmap_2d::Layer’ {aka ‘class nav2_costmap_2d::Layer’} has no member named ‘isEnabled’

This does not build. I think you need to make that API :-)

@ros-navigation ros-navigation deleted a comment from mergify bot Jan 10, 2023
@ros-navigation ros-navigation deleted a comment from mergify bot Jan 10, 2023
@SteveMacenski SteveMacenski merged commit 9e070b7 into ros-navigation:main Jan 10, 2023
@milidam
Copy link
Contributor Author

milidam commented Jan 11, 2023

@SteveMacenski thanks for merging the commit.
Would it be possible to backport it to the humble branch as well?

@SteveMacenski
Copy link
Member

Yes, this will be backported in a few weeks when I run a new sync

@SteveMacenski SteveMacenski mentioned this pull request Jan 12, 2023
Closed
SteveMacenski pushed a commit that referenced this pull request Feb 10, 2023
@milidam @SteveMacenski
Do only consider enabled costmap plugins/filters in the isCurrent() m…
c223668 
…ethod of a LayeredCostmap (#3356)

* Do only consider enabled costmap plugins/filters in the isCurrent() method of a LayeredCostmap

When clearing entirely a costmap (see the `clear_entirely_*` service),
its layers are reset. This sets their `current_` attribute to False,
until they are updated (`updateCosts()`). However, for disabled
plugins/filters, the `updateCosts()` is bypassed so that the `current_`
attribute is never reset to True, and the costmap never becomes
"current" again; because of that the controller_server used to get stuck
in an endless loop (see `while (!costmap_ros_->isCurrent())`
in `ControllerServer::computeControl()`).

This patch fixes that by adding a condition for not considering disabled
plugins/filters in the `LayeredCostmap::isCurrent()`.

* Add forgotten Layer::isEnabled() definition
SteveMacenski added a commit that referenced this pull request Feb 10, 2023
@SteveMacenski @andriimaistruk
@milidam @malban @AlexeyMerzlyakov @HovorunB @DylanDeCoeyer-Quimesis @borongyuan @austin-InDro @muttistefano
Humble Sync 5 (Feb 10) (#3405)
040c585 
* Std::string doesn't have such constructor, therefore wrong exception happens when waiting for action server is over (#3333)

* Update bt_action_node.hpp

* Update bt_cancel_action_node.hpp

* Update bt_action_node.hpp

* Update bt_cancel_action_node.hpp

* Do only consider enabled costmap plugins/filters in the isCurrent() method of a LayeredCostmap (#3356)

* Do only consider enabled costmap plugins/filters in the isCurrent() method of a LayeredCostmap

When clearing entirely a costmap (see the `clear_entirely_*` service),
its layers are reset. This sets their `current_` attribute to False,
until they are updated (`updateCosts()`). However, for disabled
plugins/filters, the `updateCosts()` is bypassed so that the `current_`
attribute is never reset to True, and the costmap never becomes
"current" again; because of that the controller_server used to get stuck
in an endless loop (see `while (!costmap_ros_->isCurrent())`
in `ControllerServer::computeControl()`).

This patch fixes that by adding a condition for not considering disabled
plugins/filters in the `LayeredCostmap::isCurrent()`.

* Add forgotten Layer::isEnabled() definition

* Export ompl include directories from nav2_smac_planner. (#3358)

* Update default w_smooth to 2M (#3341)

* Fix nav2_velocity_smoother for sim time (#3360)

* Fix nav2_velocity_smoother for sim time

* Update nav2_velocity_smoother/src/velocity_smoother.cpp

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* catch general exceptions (#3373)

* catch general exceptions

* Update controller_server.cpp

* nav2_tree_nodes: rename "service_name" input port for action nodes (#3370)

Classes inheriting from nav2_behavior_tree::BtActionNode use
"server_name" as input port. The "service_name" input port is reserved
to sub-classes of nav2_behavior_tree::BtServiceNode. There seemed to be
a confusion in the nav2_tree_nodes.xml file.

* Fixing Theta* test failures (#3380)

* adding printouts since can't see locally

* one more to isolate

* request in bounds

* remove prints

* Fix SpeedController (#3378)

* Fix SpeedController
* Using blackboard get() method that does not throw error
* Share odom_smoother on the blackboard

Signed-off-by: Borong Yuan <yuanborong@hotmail.com>

* Fix SpeedController linting and test errors

Signed-off-by: Borong Yuan <yuanborong@hotmail.com>

Signed-off-by: Borong Yuan <yuanborong@hotmail.com>

* node_name change during initialization (#3387)

* Added missing conditions to nav2_tree_nodes (#3401)

* Add WouldAControllerRecoveryHelp and WouldAPlannerRecoveryHelp to nav2_tree_nodes

* Add WouldAControllerRecoveryHelp and WouldAPlannerRecoveryHelp...... to nav2_tree_nodes

* Update nav2_tree_nodes.xml

---------

Co-authored-by: blanker <you@example.com>

* Fix multi-robot launch issues (#3383)

* Fix multi-robot launch issues:
* Use full name of composable nodes container
* Correct and update BT plugins and error codes list
* Fix and update local and global costmap used plugins parameters

* Do not publish voxel map for multi-robot case

* Update controller_server.cpp

* Update nav2_smoother.cpp

---------

Signed-off-by: Borong Yuan <yuanborong@hotmail.com>
Co-authored-by: Andrii Maistruk <71632363+andriimaistruk@users.noreply.github.com>
Co-authored-by: milidam <milidam@users.noreply.github.com>
Co-authored-by: Marc Alban <marcalban@gmail.com>
Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com>
Co-authored-by: HovorunB <87417416+HovorunB@users.noreply.github.com>
Co-authored-by: DylanDeCoeyer-Quimesis <102609575+DylanDeCoeyer-Quimesis@users.noreply.github.com>
Co-authored-by: Borong Yuan <yuanborong@hotmail.com>
Co-authored-by: Austin Greisman <92941098+austin-InDro@users.noreply.github.com>
Co-authored-by: muttistefano <mutti.stefano.jp@gmail.com>
Co-authored-by: blanker <you@example.com>
andrewlycas pushed a commit to StratomInc/navigation2 that referenced this pull request Feb 23, 2023
@milidam
Do only consider enabled costmap plugins/filters in the isCurrent() m…
ee36ef4 
…ethod of a LayeredCostmap (ros-navigation#3356)

* Do only consider enabled costmap plugins/filters in the isCurrent() method of a LayeredCostmap

When clearing entirely a costmap (see the `clear_entirely_*` service),
its layers are reset. This sets their `current_` attribute to False,
until they are updated (`updateCosts()`). However, for disabled
plugins/filters, the `updateCosts()` is bypassed so that the `current_`
attribute is never reset to True, and the costmap never becomes
"current" again; because of that the controller_server used to get stuck
in an endless loop (see `while (!costmap_ros_->isCurrent())`
in `ControllerServer::computeControl()`).

This patch fixes that by adding a condition for not considering disabled
plugins/filters in the `LayeredCostmap::isCurrent()`.

* Add forgotten Layer::isEnabled() definition
hyunseok-yang pushed a commit to lge-ros2/navigation2 that referenced this pull request Mar 6, 2023
@SteveMacenski @andriimaistruk
@milidam @malban @AlexeyMerzlyakov @HovorunB @DylanDeCoeyer-Quimesis @borongyuan @austin-InDro @muttistefano @hyunseok-yang
Humble Sync 5 (Feb 10) (ros-navigation#3405)
322b1b2 
* Std::string doesn't have such constructor, therefore wrong exception happens when waiting for action server is over (ros-navigation#3333)

* Update bt_action_node.hpp

* Update bt_cancel_action_node.hpp

* Update bt_action_node.hpp

* Update bt_cancel_action_node.hpp

* Do only consider enabled costmap plugins/filters in the isCurrent() method of a LayeredCostmap (ros-navigation#3356)

* Do only consider enabled costmap plugins/filters in the isCurrent() method of a LayeredCostmap

When clearing entirely a costmap (see the `clear_entirely_*` service),
its layers are reset. This sets their `current_` attribute to False,
until they are updated (`updateCosts()`). However, for disabled
plugins/filters, the `updateCosts()` is bypassed so that the `current_`
attribute is never reset to True, and the costmap never becomes
"current" again; because of that the controller_server used to get stuck
in an endless loop (see `while (!costmap_ros_->isCurrent())`
in `ControllerServer::computeControl()`).

This patch fixes that by adding a condition for not considering disabled
plugins/filters in the `LayeredCostmap::isCurrent()`.

* Add forgotten Layer::isEnabled() definition

* Export ompl include directories from nav2_smac_planner. (ros-navigation#3358)

* Update default w_smooth to 2M (ros-navigation#3341)

* Fix nav2_velocity_smoother for sim time (ros-navigation#3360)

* Fix nav2_velocity_smoother for sim time

* Update nav2_velocity_smoother/src/velocity_smoother.cpp

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* catch general exceptions (ros-navigation#3373)

* catch general exceptions

* Update controller_server.cpp

* nav2_tree_nodes: rename "service_name" input port for action nodes (ros-navigation#3370)

Classes inheriting from nav2_behavior_tree::BtActionNode use
"server_name" as input port. The "service_name" input port is reserved
to sub-classes of nav2_behavior_tree::BtServiceNode. There seemed to be
a confusion in the nav2_tree_nodes.xml file.

* Fixing Theta* test failures (ros-navigation#3380)

* adding printouts since can't see locally

* one more to isolate

* request in bounds

* remove prints

* Fix SpeedController (ros-navigation#3378)

* Fix SpeedController
* Using blackboard get() method that does not throw error
* Share odom_smoother on the blackboard

Signed-off-by: Borong Yuan <yuanborong@hotmail.com>

* Fix SpeedController linting and test errors

Signed-off-by: Borong Yuan <yuanborong@hotmail.com>

Signed-off-by: Borong Yuan <yuanborong@hotmail.com>

* node_name change during initialization (ros-navigation#3387)

* Added missing conditions to nav2_tree_nodes (ros-navigation#3401)

* Add WouldAControllerRecoveryHelp and WouldAPlannerRecoveryHelp to nav2_tree_nodes

* Add WouldAControllerRecoveryHelp and WouldAPlannerRecoveryHelp...... to nav2_tree_nodes

* Update nav2_tree_nodes.xml

---------

Co-authored-by: blanker <you@example.com>

* Fix multi-robot launch issues (ros-navigation#3383)

* Fix multi-robot launch issues:
* Use full name of composable nodes container
* Correct and update BT plugins and error codes list
* Fix and update local and global costmap used plugins parameters

* Do not publish voxel map for multi-robot case

* Update controller_server.cpp

* Update nav2_smoother.cpp

---------

Signed-off-by: Borong Yuan <yuanborong@hotmail.com>
Co-authored-by: Andrii Maistruk <71632363+andriimaistruk@users.noreply.github.com>
Co-authored-by: milidam <milidam@users.noreply.github.com>
Co-authored-by: Marc Alban <marcalban@gmail.com>
Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com>
Co-authored-by: HovorunB <87417416+HovorunB@users.noreply.github.com>
Co-authored-by: DylanDeCoeyer-Quimesis <102609575+DylanDeCoeyer-Quimesis@users.noreply.github.com>
Co-authored-by: Borong Yuan <yuanborong@hotmail.com>
Co-authored-by: Austin Greisman <92941098+austin-InDro@users.noreply.github.com>
Co-authored-by: muttistefano <mutti.stefano.jp@gmail.com>
Co-authored-by: blanker <you@example.com>
@SteveMacenski SteveMacenski mentioned this pull request Jun 26, 2024
Closed
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers

@SteveMacenski SteveMacenski SteveMacenski approved these changes

Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.

LayeredCostmap isCurrent() does not consider whether the plugins are enabled or not
2 participants
@milidam @SteveMacenski