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

Fix nav2_velocity_smoother for sim time #3360

Merged
merged 2 commits into from
Jan 13, 2023
Merged

Fix nav2_velocity_smoother for sim time #3360

merged 2 commits into from
Jan 13, 2023

Conversation

HovorunB
Copy link
Contributor

@HovorunB HovorunB commented Jan 12, 2023 •
edited
Loading

Basic Info

Info Fix nav2_velocity_smoother for sim time
Ticket(s) this addresses
Primary OS tested on Ubuntu
Robotic platform tested on Gazebo simulation

Description of contribution in a few bullet points

If sim time is used: now() might be close to 0 on startup and last_command_time_ is defined 0 making the condition now() - last_command_time_ > velocity_timeout_ False, which assumes that velocity command was received recently and then leads to an error when trying to access command_'s properties ( command_->linear.x )
So i added check for command_ being null before proceeding with smoothing

Description of documentation updates required from your changes

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

@mergify
Copy link
Contributor

mergify bot commented Jan 12, 2023

@HovorunB, 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

@HovorunB @SteveMacenski
Update nav2_velocity_smoother/src/velocity_smoother.cpp
c930a17 
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
AlexeyMerzlyakov
AlexeyMerzlyakov approved these changes Jan 13, 2023
View reviewed changes
Copy link
Collaborator

@AlexeyMerzlyakov AlexeyMerzlyakov left a comment

Choose a reason for hiding this comment

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

The change LGTM: the main logic is comes from smootherTimer() callback, so we need to wait until first message to be received.

@AlexeyMerzlyakov AlexeyMerzlyakov merged commit c841417 into ros-navigation:main Jan 13, 2023
SteveMacenski added a commit that referenced this pull request Feb 10, 2023
@HovorunB @SteveMacenski
Fix nav2_velocity_smoother for sim time (#3360)
739df47 
* Fix nav2_velocity_smoother for sim time

* Update nav2_velocity_smoother/src/velocity_smoother.cpp

Co-authored-by: Steve Macenski <stevenmacenski@gmail.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
@HovorunB @SteveMacenski
Fix nav2_velocity_smoother for sim time (ros-navigation#3360)
c07a6fe 
* Fix nav2_velocity_smoother for sim time

* Update nav2_velocity_smoother/src/velocity_smoother.cpp

Co-authored-by: Steve Macenski <stevenmacenski@gmail.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

@AlexeyMerzlyakov AlexeyMerzlyakov AlexeyMerzlyakov 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.
3 participants
@HovorunB @SteveMacenski @AlexeyMerzlyakov