Constrained smoother

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. It optimizes for path length, smoothness, distance from obstacles, and curvature in a large Ceres-based optimization program.

Important

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

`reversing_enabled`

Type: bool Default: true: Whether to detect forward/reverse direction and cusps. Should be set to false for paths without orientations assigned

`path_downsampling_factor`

Type: int Default: 1: Every n-th node of the path is taken for optimization. Useful for speed-up

`path_upsampling_factor`

Type: int Default: 1

Upsampling factor for refining.

0 - path remains downsampled (see path_downsampling_factor)
1 - path is upsampled back to original granularity using cubic bezier
2... - more upsampling

`keep_start_orientation`

Type: bool Default: true: Whether to prevent the start orientation from being smoothed

`keep_goal_orientation`

Type: bool Default: true: Whether to prevent the goal orientation from being smoothed

`minimum_turning_radius`

Type: double Default: 0.4: 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`

Type: double Default: 30.0: Weight to enforce minimum_turning_radius

`w_dist`

Type: double Default: 0.0: Weight to bind path to original as optional replacement for cost weight

`w_smooth`

Type: double Default: 2000000.0: Weight to maximize smoothness of path

`w_cost`

Type: double Default: 0.015: Weight to steer robot away from collision and cost

`w_cost_cusp_multiplier`

Type: double Default: 3.0

Option to use higher weight during forward/reverse direction change, helping optimizer to converge or add an extra obstacle avoidance at these problematic segments. Following image depicts improvement of the path with w_cost_cusp_multiplier (green) compared to one without it (purple). Original path has cyan color.

`cusp_zone_length`

Type: double Default: 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)

`cost_check_points`

Type: vector<double> Default: []

Points in robot frame to grab costmap values from. Format: [x1, y1, weight1, x2, y2, weight2, ...].

Important

Requires much higher number of optimizer iterations to actually improve the path. Use only if you really need it (highly elongated/asymmetric robots)

Following image depicts how two cost check points can be used to improve cost awareness of a rectangular robot.

`optimizer.max_iterations`

Type: int Default: 100: Maximum number of optimizer iterations

`optimizer.debug_optimizer`

Type: bool Default: false: Whether to print optimizer debug info

`optimizer.linear_solver_type`

Type: string Default: "SPARSE_NORMAL_CHOLESKY": Linear solver type to be used by optimizer. Valid values are "SPARSE_NORMAL_CHOLESKY" and "DENSE_QR"

`optimizer.gradient_tol`

Type: double Default: 1e-10: Gradient tolerance optimization termination criterion

`optimizer.fn_tol`

Type: double Default: 1e-7: Function tolerance optimization termination criterion

`optimizer.param_tol`

Type: double Default: 1e-15: Parameter tolerance optimization termination criterion

Example

smoother_server:
  ros__parameters:
    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: 0.5                  # weight to enforce minimum_turning_radius
      w_dist: 0.0                   # weight to bind path to original as optional replacement for cost weight
      w_smooth: 3.0e+3              # weight to maximize smoothness of path
      w_cost: 0.000045              # 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: 50.0
        fn_tol: 1.0e-15
        param_tol: 1.0e-20