Constrained Smoother tuning by AlexeyMerzlyakov · Pull Request #3341 · ros-navigation/navigation2

AlexeyMerzlyakov · 2022-12-27T15:32:50Z

During the benchmarking of Path Smoothers, it was discovered abnormal behavior of Constrained Smoother. This behavior appeared on some experiments with SmacHybridA* planner and Constrained Smoother, where the path smoothed by Constrained Smoother could have some “wobblings” or “oscillations” like depicted below:

The analysis is being performed and attached to current PR in a CS_path_oscillation_analysis_v7.pdf file.
As the results of analysis and during the numerous tests and benchmarks under different environments (on small and large maps, narrow- and wide-spaced) it was found and proved the optimal set of the residual coefficients causing Constrained Smoother to produce the rational path without oscillation effects:

Residual coefficient	Standard value change
w_smooth	15,000 → 2,000,000
w_cost	0.015
w_curve	30.0
w_dist	0.0

Thus, for best Constrained Smoother production experience is suggested to update default w_smooth value from 15K to 2M.

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	N/A
Primary OS tested on	Ubuntu 20.04
Robotic platform tested on	TB3 Gazebo simulation, Path Smoothers benchmarking, colcon tests, lcov

Description of contribution in a few bullet points

Updated defauly w_smooth value from 15K -> to 2M
Tuned test_constrained_smoother for new w_smooth value:
- testingObstacleAvoidance: cost avoidance improvement value was decreased, as expected from params ratio change
- testingObstacleAvoidanceNearCusps: test seems to be tuned for the 15K w_smooth value - leaved untouched
- testingDownsamplingUpsampling: slight smoothness improvement change
- testingStartGoalOrientations: slight change of smoothness improvement in normal orientation, ~2x change in smoothness improvement and angles in reversed path orientation
Fixed dist calculation formula in test_constrained_smoother causing incorrect exponent factor calculation, which causes overflows in costs making incorrect default costmap for the test (although, having no effect on final test results)
Fixed incorrect integer ROS-parameter setting while it is required floating-point one for some cases in test_constrained_smoother test. This issue leaded to w_smooth coefficient was not being updated in these cases.

Description of documentation updates required from your changes

w_smooth default value should be updated at the CS configuration guide page, if this change to be agreed and accepted

Future work that may be required in bullet points

(optionally and might be not necessary) Re-work test_constrained_smoother -> testingObstacleAvoidanceNearCusps testcase for new w_smooth = 2M value

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 · 2022-12-27T15:33:23Z

@AlexeyMerzlyakov, 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 · 2023-01-02T20:23:19Z

nav2_constrained_smoother/include/nav2_constrained_smoother/options.hpp

    node->get_parameter(local_name + "w_dist", distance_weight);
    nav2_util::declare_parameter_if_not_declared(
-      node, local_name + "w_smooth", rclcpp::ParameterValue(15000.0));
+      node, local_name + "w_smooth", rclcpp::ParameterValue(2000000.0));


Docs update to match changes on the website?

Sure, if you are OK with w_smooth 2M new value (and also with this change in general), I'll submit a new doc update PR.

Done: ros-navigation/docs.nav2.org#385. Seems to be mentioned only on two places, there is no tutorials or something else needed to be updated.

* 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>

* 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>

Update default w_smooth to 2M

ca86452

SteveMacenski requested changes Jan 2, 2023

View reviewed changes

AlexeyMerzlyakov mentioned this pull request Jan 12, 2023

Increase default w_smooth to 2M for Constrained Smoother ros-navigation/docs.nav2.org#385

Merged

SteveMacenski merged commit 96fcf48 into ros-navigation:main Jan 12, 2023

SteveMacenski pushed a commit that referenced this pull request Feb 10, 2023

Update default w_smooth to 2M (#3341)

76a2106

andrewlycas pushed a commit to StratomInc/navigation2 that referenced this pull request Feb 23, 2023

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

2dbd752

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Constrained Smoother tuning#3341

Constrained Smoother tuning#3341
SteveMacenski merged 1 commit intoros-navigation:mainfrom
AlexeyMerzlyakov:cs-tuning

AlexeyMerzlyakov commented Dec 27, 2022

Uh oh!

mergify bot commented Dec 27, 2022

Uh oh!

SteveMacenski Jan 2, 2023 •

edited

Loading

Uh oh!

AlexeyMerzlyakov Jan 9, 2023

Uh oh!

SteveMacenski Jan 9, 2023

Uh oh!

AlexeyMerzlyakov Jan 12, 2023

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

Conversation

AlexeyMerzlyakov commented Dec 27, 2022

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Future work that may be required in bullet points

For Maintainers:

Uh oh!

mergify bot commented Dec 27, 2022

Uh oh!

SteveMacenski Jan 2, 2023 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

AlexeyMerzlyakov Jan 9, 2023

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Jan 9, 2023

Choose a reason for hiding this comment

Uh oh!

AlexeyMerzlyakov Jan 12, 2023

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

SteveMacenski Jan 2, 2023 •

edited

Loading