Source code on Github.
A smoother plugin for nav2_smoother based on the original deprecated smoother in nav2_smac_planner and put into operational state by RoboTech Vision. Suitable for applications which need planned global path to be pushed away from obstacles and/or for Reeds-Shepp motion models.
Important note: Constrained smoother uses a rather heavy optimization algorithm and thus is suggested to be used on a periodically truncated path. TruncatePathLocal BT Node can be used for achieving a proper path length and DistanceController BT Node can be used for achieving periodicity.
Following image depicts how Constrained Smoother can improve quality of an input path (cyan, generated by an outdated version of Smac Planner, intentionally not configured optimally to highlight the power of the smoother), increasing its smoothness and distance from obstacles. Resulting path is marked by green color. Note: last few path poses are not smoothed since TruncatePathLocal is used on this path.
Smoother Server Parameters¶
smoother_server: ros__parameters: use_sim_time: True smoother_plugins: ["SmoothPath"] SmoothPath: plugin: "nav2_constrained_smoother/ConstrainedSmoother" reversing_enabled: true # whether to detect forward/reverse direction and cusps. Should be set to false for paths without orientations assigned path_downsampling_factor: 3 # every n-th node of the path is taken. Useful for speed-up path_upsampling_factor: 1 # 0 - path remains downsampled, 1 - path is upsampled back to original granularity using cubic bezier, 2... - more upsampling keep_start_orientation: true # whether to prevent the start orientation from being smoothed keep_goal_orientation: true # whether to prevent the gpal orientation from being smoothed minimum_turning_radius: 0.40 # minimum turning radius the robot can perform. Can be set to 0.0 (or w_curve can be set to 0.0 with the same effect) for diff-drive/holonomic robots w_curve: 30.0 # weight to enforce minimum_turning_radius w_dist: 0.0 # weight to bind path to original as optional replacement for cost weight w_smooth: 15000.0 # weight to maximize smoothness of path w_cost: 0.015 # weight to steer robot away from collision and cost # Parameters used to improve obstacle avoidance near cusps (forward/reverse movement changes) w_cost_cusp_multiplier: 3.0 # option to use higher weight during forward/reverse direction change which is often accompanied with dangerous rotations cusp_zone_length: 2.5 # length of the section around cusp in which nodes use w_cost_cusp_multiplier (w_cost rises gradually inside the zone towards the cusp point, whose costmap weight eqals w_cost*w_cost_cusp_multiplier) # Points in robot frame to grab costmap values from. Format: [x1, y1, weight1, x2, y2, weight2, ...] # IMPORTANT: Requires much higher number of iterations to actually improve the path. Uncomment only if you really need it (highly elongated/asymmetric robots) # cost_check_points: [-0.185, 0.0, 1.0] optimizer: max_iterations: 70 # max iterations of smoother debug_optimizer: false # print debug info gradient_tol: 5e3 fn_tol: 1.0e-15 param_tol: 1.0e-20