🚔D.20 Constrained smoother

သူက အတားအဆီး ကန့်သတ်ချက်တွေကို smooth လုပ်ပေးတဲ့ကောင်။ ဒါပေမဲ့ compute power လိုတာမို့ အပိုင်းလိုက် လမ်းကြောင်းတွေအတွက်ပဲသုံးသင့်တယ်။

ဒါကြောင့် TruncatePathLocal BT Node ကို လမ်းကြောင်းအရှည်ကိုရရှိရန်အတွက်အသုံးပြုနိုင်ပြီး DistanceController BT Node ကို အပိုင်းလိုက် လမ်းကြောင်း ရရှိရန်အတွက်အသုံးပြုနိုင်ပါလို့ဆိုပါတယ်။

ပုံမှာဆိုရင် global path က ကြည်ပြာရောင်ဖြစ်ပြီး အစိမ်းရောင် path ကတော့ သူ့ရဲ့ ရလာဒ်ဖြစ်ပါတယ်။

source from nav2 doc

Name	Description	Type	Default
reversing_enabled	Whether to detect forward/reverse direction and cusps. Should be set to false for paths without orientations assigned	bool	TRUE
path_downsampling_factor	Every n-th node of the path is taken for optimization. Useful for speed-up	int	1
path_upsampling_factor	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	int	1
keep_start_orientation	Whether to prevent the start orientation from being smoothed	bool	TRUE
keep_goal_orientation	Whether to prevent the goal orientation from being smoothed	bool	TRUE
minimum_turning_radius	Minimum turning radius the robot can perform. Can be set to 0.0 (or w_curve can be set to 0.0 withe the same effect) for diff-drive/holonomic robots. (robot လှည့်နိုင်သော အနည်းဆုံး အချင်းဝက်)	double	0.4
w_curve	Weight to enforce minimum_turning_radius	double	30
w_dist	Weight to bind path to original as optional replacement for cost weight	double	0.0
w_smooth	Weight to maximize smoothness of path	double	2000000.0
w_cost	Weight to steer robot away from collision and cost	double	0.015
w_cost_cusp_multipler		double	0.3

w_cost_cusp_multiplier

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. (ရှေ့နောက် direction ပြောင်းလဲချိန်တွင် higher weight သုံးရန်ရွေးချယ်ခွင့်, ပြဿနာရှိသောအပိုင်းများတွင် အပိုအတားဆီးများ ပေါင်းထည့်ခြင်းသို့မဟုတ် ရှောင်ရှားရန်တွက် optimizer ကို ကူညီရန်. အောက်ပါပုံသည် w_cost_cusp_multiplier ဖြင့်တိုးတက်လာသော လမ်းကြောင်း (အစိမ်းရောင်) နှင့် ၎င်းကိုအသုံးမပြုထားသော (ခရမ်းရောင်) ကိုနှုိင်းယှဥ်ပြထားသည်။ မူလ လမ်းကြောင်းသည် စိမ်းပြာရောင်ဖြစ်သည်။

cusp_zone_length	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)	double	2.5
cost_check_points		array of double	[ ]

cost_check_points

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	Maximum number of optimizer iterations	int	100
optimizer.debug_optimizer	Whether to print optimizer debug info	bool	FALSE
optimizer.linear_solver_type	Linear solver type to used by optimizer. Valid values are SPARSE_NORMAL_CHOLESKY and DENSE_QR	string	"SPARSE_NORMAL _CHOLESKY"
optimizer.gradient_tol	Gradient tolerance optimization termination criterion	bool	1E-10
optimizer.fn_tol	Function tolerance optimization termination criterion	bool	1E-07
optimizer.param_tol	Parameter tolerance optimizatioin termination criterion	bool	1E-15

နမူနာ

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: 2000000.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