Skip to content

Fix SpeedController#3378

Merged
SteveMacenski merged 2 commits intoros-navigation:mainfrom
borongyuan:main
Jan 21, 2023
Merged

Fix SpeedController#3378
SteveMacenski merged 2 commits intoros-navigation:mainfrom
borongyuan:main

Conversation

@borongyuan
Copy link
Contributor

@borongyuan borongyuan commented Jan 20, 2023

  • Using blackboard get() method that does not throw error
  • Share odom_smoother on the blackboard

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

Basic Info

Info Please fill out this column
Ticket(s) this addresses
Primary OS tested on Ubuntu
Robotic platform tested on gazebo simulation

Description of contribution in a few bullet points

The SpeedController is broken since (#2271).
There are two get() methods in blackboard. One of them throws an error if val doesn't exist. Obviously goal and goals cannot exist on blackboard at the same time. So we need to use another get() method like in GoalUpdatedController.
Now the BT Navigator already has an instance of OdomSmoother, we can't create another one in SpeedController, otherwise we won't get the correct data. A simple fix is to share odom_smoother on blackboard.

Description of documentation updates required from your changes

Since odom_smoother is shared from BT Navigator, we cannot set filter_duration in SpeedController. So I removed this parameter. The default value of 0.3 works well in most cases, so this should not affect usage. But after these changes are confirmed, we need to update the documentation.

Future work that may be required in bullet points

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

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

Signed-off-by: Borong Yuan <yuanborong@hotmail.com>
@mergify
Copy link
Contributor

mergify bot commented Jan 20, 2023

@borongyuan, please properly fill in PR template in the future. @SteveMacenski, use this instead.

  • 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

@SteveMacenski
Copy link
Member

SteveMacenski commented Jan 20, 2023 •
edited
Loading

Check CI. This PR has linting, test coverage, and XML invalidation errors. You can ignore the single trigger failure and Theta*, that's unrelated. But the others are caused by this

SteveMacenski
SteveMacenski approved these changes Jan 20, 2023
View reviewed changes
Copy link
Member

@SteveMacenski SteveMacenski left a comment

Choose a reason for hiding this comment

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

Otherwise after CI is happy with your changes, this all looks good to me

@borongyuan
Fix SpeedController linting and test errors
ea5e292 
Signed-off-by: Borong Yuan <yuanborong@hotmail.com>
@SteveMacenski SteveMacenski merged commit ff958bb into ros-navigation:main Jan 21, 2023
@SteveMacenski
Copy link
Member

SteveMacenski commented Jan 21, 2023

Thanks for the fix!

SteveMacenski pushed a commit that referenced this pull request Feb 10, 2023
@borongyuan @SteveMacenski
Fix SpeedController (#3378)
106e968 
* 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>
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
@borongyuan
Fix SpeedController (ros-navigation#3378)
3b49488 
* 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>
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>
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.

2 participants

@borongyuan @SteveMacenski