Smoothness metrics update (ros-navigation#3294)

* Update metrics for path smoothness

* Support Savitzky-Golay smoother

Author: AlexeyMerzlyakov

tools/smoother_benchmarking/README.md

@@ -30,12 +30,14 @@ planner_server:
 ```
  smoother_server:
    ros__parameters:
-    smoother_plugins: ["simple_smoother", "constrained_smoother"]
+    smoother_plugins: ["simple_smoother", "constrained_smoother", "sg_smoother"]
     simple_smoother:
       plugin: "nav2_smoother::SimpleSmoother"
     constrained_smoother:
       plugin: "nav2_constrained_smoother/ConstrainedSmoother"
       w_smooth: 100000.0 # tuned
+    sg_smoother:
+      plugin: "nav2_smoother::SavitzkyGolaySmoother"
 ```
 
 Set global costmap, path planner and smoothers parameters to those desired in `nav2_params.yaml`.

tools/smoother_benchmarking/metrics.py

@@ -111,7 +111,7 @@ def main():
     costmap.resize(costmap_msg.metadata.size_y, costmap_msg.metadata.size_x)
 
     planner = 'SmacHybrid'
-    smoothers = ['simple_smoother', 'constrained_smoother']
+    smoothers = ['simple_smoother', 'constrained_smoother', 'sg_smoother']
     max_cost = 210
     side_buffer = 10
     time_stamp = navigator.get_clock().now().to_msg()

tools/smoother_benchmarking/process_data.py

@@ -82,7 +82,7 @@ def getSmoothness(pt_prev, pt, pt_next):
     d1 = pt - pt_prev
     d2 = pt_next - pt
     delta = d2 - d1
-    return np.dot(delta, delta)
+    return np.linalg.norm(delta)
 
 def getPathSmoothnesses(paths):
     smoothnesses = []