diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py
index 59a8599b03..a844498ab8 100644
--- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py
+++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py
@@ -389,8 +389,12 @@ def getPathThroughPoses(self, start, goals, planner_id='', use_start=False):
 
         return self.result_future.result().result.path
 
-    def smoothPath(self, path, smoother_id='', max_duration=2.0, check_for_collision=False):
-        """Send a `SmoothPath` action request."""
+    def _smoothPathImpl(self, path, smoother_id='', max_duration=2.0, check_for_collision=False):
+        """
+        Send a `SmoothPath` action request.
+
+        Internal implementation to get the full result, not just the path.
+        """
         self.debug("Waiting for 'SmoothPath' action server")
         while not self.smoother_client.wait_for_server(timeout_sec=1.0):
             self.info("'SmoothPath' action server not available, waiting...")
@@ -417,7 +421,16 @@ def smoothPath(self, path, smoother_id='', max_duration=2.0, check_for_collision
             self.warn(f'Getting path failed with status code: {self.status}')
             return None
 
-        return self.result_future.result().result.path
+        return self.result_future.result().result
+
+    def smoothPath(self, path, smoother_id='', max_duration=2.0, check_for_collision=False):
+        """Send a `SmoothPath` action request."""
+        rtn = self._smoothPathImpl(
+            self, path, smoother_id='', max_duration=2.0, check_for_collision=False)
+        if not rtn:
+            return None
+        else:
+            return rtn.path
 
     def changeMap(self, map_filepath):
         """Change the current static map in the map server."""
diff --git a/tools/smoother_benchmarking/README.md b/tools/smoother_benchmarking/README.md
new file mode 100644
index 0000000000..65ea8a4c9d
--- /dev/null
+++ b/tools/smoother_benchmarking/README.md
@@ -0,0 +1,98 @@
+# Planners Smoothing Benchmark
+
+This experiment runs a set with randomly generated goals for objective benchmarking.
+
+Bechmarking scripts require the following python packages to be installed:
+
+```
+pip install transforms3d
+pip install seaborn
+pip install tabulate
+```
+
+To use the suite, modify the Nav2 bringup parameters `nav2_params.yaml` to include selected path planner:
+
+```
+planner_server:
+  ros__parameters:
+    expected_planner_frequency: 20.0
+    planner_plugins: ["SmacHybrid"]
+    SmacHybrid:
+      plugin: "nav2_smac_planner/SmacPlannerHybrid"
+      tolerance: 0.5
+      motion_model_for_search: "DUBIN" # default, non-reverse motion
+      smooth_path: false # should be disabled for experiment
+      analytic_expansion_max_length: 0.3 # decreased to avoid robot jerking
+```
+
+... and path smoothers for benchmark:
+
+```
+ smoother_server:
+   ros__parameters:
+    smoother_plugins: ["simple_smoother", "constrained_smoother"]
+    simple_smoother:
+      plugin: "nav2_smoother::SimpleSmoother"
+    constrained_smoother:
+      plugin: "nav2_constrained_smoother/ConstrainedSmoother"
+      w_smooth: 100000.0 # tuned
+```
+
+Set global costmap, path planner and smoothers parameters to those desired in `nav2_params.yaml`.
+Inside of `metrics.py`, you can change reference path planner / path smoothers to use.
+
+For the benchmarking purposes, the clarification of execution time may be made for planner and smoother servers, to reduce impacts caused by other system actions outside of the planning / smoothing algorithm (optional):
+
+```
+diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp
+index c7a90bcb..6f93edbf 100644
+--- a/nav2_planner/src/planner_server.cpp
++++ b/nav2_planner/src/planner_server.cpp
+@@ -381,7 +381,10 @@ void PlannerServer::computePlanThroughPoses()
+       }
+ 
+       // Get plan from start -> goal
++      auto planning_start = steady_clock_.now();
+       nav_msgs::msg::Path curr_path = getPlan(curr_start, curr_goal, goal->planner_id);
++      auto planning_duration = steady_clock_.now() - planning_start;
++      result->planning_time = planning_duration;
+ 
+       if (!validatePath<ActionThroughPoses>(curr_goal, curr_path, goal->planner_id)) {
+         throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + "generated a empty path");
+@@ -398,7 +401,7 @@ void PlannerServer::computePlanThroughPoses()
+     publishPlan(result->path);
+ 
+     auto cycle_duration = steady_clock_.now() - start_time;
+-    result->planning_time = cycle_duration;
++    // result->planning_time = cycle_duration;
+ 
+     if (max_planner_duration_ && cycle_duration.seconds() > max_planner_duration_) {
+       RCLCPP_WARN(
+diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp
+index ada1f664..610e9512 100644
+--- a/nav2_smoother/src/nav2_smoother.cpp
++++ b/nav2_smoother/src/nav2_smoother.cpp
+@@ -253,8 +253,6 @@ bool SmootherServer::findSmootherId(
+ 
+ void SmootherServer::smoothPlan()
+ {
+-  auto start_time = steady_clock_.now();
+-
+   RCLCPP_INFO(get_logger(), "Received a path to smooth.");
+ 
+   auto result = std::make_shared<Action::Result>();
+@@ -271,6 +269,8 @@ void SmootherServer::smoothPlan()
+     // Perform smoothing
+     auto goal = action_server_->get_current_goal();
+     result->path = goal->path;
++
++    auto start_time = steady_clock_.now();
+     result->was_completed = smoothers_[current_smoother_]->smooth(
+       result->path, goal->max_smoothing_duration);
+     result->smoothing_duration = steady_clock_.now() - start_time;
+```
+
+Then execute the benchmarking:
+
+- `ros2 launch ./smoother_benchmark_bringup.py` to launch the nav2 stack and path smoothers benchmarking
+- `python3 ./process_data.py` to take the metric files and process them into key results (and plots)
diff --git a/tools/smoother_benchmarking/maps/smoothers_world.pgm b/tools/smoother_benchmarking/maps/smoothers_world.pgm
new file mode 100644
index 0000000000..a79c5cd79e
Binary files /dev/null and b/tools/smoother_benchmarking/maps/smoothers_world.pgm differ
diff --git a/tools/smoother_benchmarking/maps/smoothers_world.yaml b/tools/smoother_benchmarking/maps/smoothers_world.yaml
new file mode 100644
index 0000000000..c093166203
--- /dev/null
+++ b/tools/smoother_benchmarking/maps/smoothers_world.yaml
@@ -0,0 +1,6 @@
+image: smoothers_world.pgm
+resolution: 0.050000
+origin: [0.0, 0.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
diff --git a/tools/smoother_benchmarking/metrics.py b/tools/smoother_benchmarking/metrics.py
new file mode 100644
index 0000000000..6b576b08a9
--- /dev/null
+++ b/tools/smoother_benchmarking/metrics.py
@@ -0,0 +1,157 @@
+#! /usr/bin/env python3
+# Copyright (c) 2022 Samsung R&D Institute Russia
+# Copyright (c) 2022 Joshua Wallace
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from geometry_msgs.msg import PoseStamped
+from nav2_simple_commander.robot_navigator import BasicNavigator
+import rclpy
+
+import math
+import os
+import pickle
+import numpy as np
+
+from random import seed
+from random import randint
+from random import uniform
+
+from transforms3d.euler import euler2quat
+
+
+# Note: Map origin is assumed to be (0,0)
+
+def getPlannerResults(navigator, initial_pose, goal_pose, planner):
+    return navigator._getPathImpl(initial_pose, goal_pose, planner, use_start=True)
+
+def getSmootherResults(navigator, path, smoothers):
+    smoothed_results = []
+    for smoother in smoothers:
+        smoothed_result = navigator._smoothPathImpl(path, smoother)
+        if smoothed_result is not None:
+            smoothed_results.append(smoothed_result)
+        else:
+            print(smoother, " failed to smooth the path")
+            return None
+    return smoothed_results
+
+def getRandomStart(costmap, max_cost, side_buffer, time_stamp, res):
+    start = PoseStamped()
+    start.header.frame_id = 'map'
+    start.header.stamp = time_stamp
+    while True:
+        row = randint(side_buffer, costmap.shape[0]-side_buffer)
+        col = randint(side_buffer, costmap.shape[1]-side_buffer)
+
+        if costmap[row, col] < max_cost:
+            start.pose.position.x = col*res
+            start.pose.position.y = row*res
+
+            yaw = uniform(0, 1) * 2*math.pi
+            quad = euler2quat(0.0, 0.0, yaw)
+            start.pose.orientation.w = quad[0]
+            start.pose.orientation.x = quad[1]
+            start.pose.orientation.y = quad[2]
+            start.pose.orientation.z = quad[3]
+            break
+    return start
+
+def getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res):
+    goal = PoseStamped()
+    goal.header.frame_id = 'map'
+    goal.header.stamp = time_stamp
+    while True:
+        row = randint(side_buffer, costmap.shape[0]-side_buffer)
+        col = randint(side_buffer, costmap.shape[1]-side_buffer)
+
+        start_x = start.pose.position.x
+        start_y = start.pose.position.y
+        goal_x = col*res
+        goal_y = row*res
+        x_diff = goal_x - start_x
+        y_diff = goal_y - start_y
+        dist = math.sqrt(x_diff ** 2 + y_diff ** 2)
+
+        if costmap[row, col] < max_cost and dist > 3.0:
+            goal.pose.position.x = goal_x
+            goal.pose.position.y = goal_y
+
+            yaw = uniform(0, 1) * 2*math.pi
+            quad = euler2quat(0.0, 0.0, yaw)
+            goal.pose.orientation.w = quad[0]
+            goal.pose.orientation.x = quad[1]
+            goal.pose.orientation.y = quad[2]
+            goal.pose.orientation.z = quad[3]
+            break
+    return goal
+
+def main():
+    rclpy.init()
+
+    navigator = BasicNavigator()
+
+    # Wait for planner and smoother to fully activate
+    print("Waiting for planner and smoother servers to activate")
+    navigator.waitUntilNav2Active('smoother_server', 'planner_server')
+
+    # Get the costmap for start/goal validation
+    costmap_msg = navigator.getGlobalCostmap()
+    costmap = np.asarray(costmap_msg.data)
+    costmap.resize(costmap_msg.metadata.size_y, costmap_msg.metadata.size_x)
+
+    planner = 'SmacHybrid'
+    smoothers = ['simple_smoother', 'constrained_smoother']
+    max_cost = 210
+    side_buffer = 10
+    time_stamp = navigator.get_clock().now().to_msg()
+    results = []
+    seed(33)
+
+    random_pairs = 100
+    i = 0
+    res = costmap_msg.metadata.resolution
+    while i < random_pairs:
+        print("Cycle: ", i, "out of: ", random_pairs)
+        start = getRandomStart(costmap, max_cost, side_buffer, time_stamp, res)
+        goal = getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res)
+        print("Start", start)
+        print("Goal", goal)
+        result = getPlannerResults(navigator, start, goal, planner)
+        if result is not None:
+            smoothed_results = getSmootherResults(navigator, result.path, smoothers)
+            if smoothed_results is not None:
+              results.append(result)
+              results.append(smoothed_results)
+              i += 1
+        else:
+            print(planner, " planner failed to produce the path")
+
+    print("Write Results...")
+    benchmark_dir = os.getcwd()
+    with open(os.path.join(benchmark_dir, 'results.pickle'), 'wb') as f:
+        pickle.dump(results, f, pickle.HIGHEST_PROTOCOL)
+
+    with open(os.path.join(benchmark_dir, 'costmap.pickle'), 'wb') as f:
+        pickle.dump(costmap_msg, f, pickle.HIGHEST_PROTOCOL)
+
+    smoothers.insert(0, planner)
+    with open(os.path.join(benchmark_dir, 'methods.pickle'), 'wb') as f:
+        pickle.dump(smoothers, f, pickle.HIGHEST_PROTOCOL)
+    print("Write Complete")
+
+    exit(0)
+
+
+if __name__ == '__main__':
+    main()
diff --git a/tools/smoother_benchmarking/process_data.py b/tools/smoother_benchmarking/process_data.py
new file mode 100644
index 0000000000..edc0dad811
--- /dev/null
+++ b/tools/smoother_benchmarking/process_data.py
@@ -0,0 +1,302 @@
+#! /usr/bin/env python3
+# Copyright (c) 2022 Samsung R&D Institute Russia
+# Copyright (c) 2022 Joshua Wallace
+# Copyright (c) 2021 RoboTech Vision
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import numpy as np
+import math
+
+import os
+import pickle
+
+import seaborn as sns
+import matplotlib.pylab as plt
+from tabulate import tabulate
+
+
+def getPaths(results):
+    paths = []
+    for i in range(len(results)):
+        if (i % 2) == 0:
+            # Append non-smoothed path
+            paths.append(results[i].path)
+        else:
+            # Append smoothed paths array
+            for result in results[i]:
+                paths.append(result.path)
+    return paths
+
+
+def getTimes(results):
+    times = []
+    for i in range(len(results)):
+        if (i % 2) == 0:
+            # Append non-smoothed time
+            times.append(results[i].planning_time.nanosec/1e09 + results[i].planning_time.sec)
+        else:
+            # Append smoothed times array
+            for result in results[i]:
+                times.append(result.smoothing_duration.nanosec/1e09 + result.smoothing_duration.sec)
+    return times
+
+
+def getMapCoordsFromPaths(paths, resolution):
+    coords = []
+    for path in paths:
+        x = []
+        y = []
+        for pose in path.poses:
+            x.append(pose.pose.position.x/resolution)
+            y.append(pose.pose.position.y/resolution)
+        coords.append(x)
+        coords.append(y)
+    return coords
+
+
+def getPathLength(path):
+    path_length = 0
+    x_prev = path.poses[0].pose.position.x
+    y_prev = path.poses[0].pose.position.y
+    for i in range(1, len(path.poses)):
+        x_curr = path.poses[i].pose.position.x
+        y_curr = path.poses[i].pose.position.y
+        path_length = path_length + math.sqrt((x_curr-x_prev)**2 + (y_curr-y_prev)**2)
+        x_prev = x_curr
+        y_prev = y_curr
+    return path_length
+
+# Path smoothness calculations
+def getSmoothness(pt_prev, pt, pt_next):
+    d1 = pt - pt_prev
+    d2 = pt_next - pt
+    delta = d2 - d1
+    return np.dot(delta, delta)
+
+def getPathSmoothnesses(paths):
+    smoothnesses = []
+    pm0 = np.array([0.0, 0.0])
+    pm1 = np.array([0.0, 0.0])
+    pm2 = np.array([0.0, 0.0])
+    for path in paths:
+        smoothness = 0.0
+        for i in range(2, len(path.poses)):
+            pm0[0] = path.poses[i].pose.position.x
+            pm0[1] = path.poses[i].pose.position.y
+            pm1[0] = path.poses[i-1].pose.position.x
+            pm1[1] = path.poses[i-1].pose.position.y
+            pm2[0] = path.poses[i-2].pose.position.x
+            pm2[1] = path.poses[i-2].pose.position.y
+            smoothness += getSmoothness(pm2, pm1, pm0)
+        smoothnesses.append(smoothness)
+    return smoothnesses
+
+# Curvature calculations
+def arcCenter(pt_prev, pt, pt_next):
+    cusp_thresh = -0.7
+
+    d1 = pt - pt_prev
+    d2 = pt_next - pt
+
+    d1_norm = d1 / np.linalg.norm(d1)
+    d2_norm = d2 / np.linalg.norm(d2)
+    cos_angle = np.dot(d1_norm, d2_norm)
+
+    if cos_angle < cusp_thresh:
+        # cusp case
+        d2 = -d2
+        pt_next = pt + d2
+
+    det = d1[0] * d2[1] - d1[1] * d2[0]
+    if abs(det) < 1e-4:  # straight line
+        return (float('inf'), float('inf'))
+
+    # circle center is at the intersection of mirror axes of the segments:
+    # http://paulbourke.net/geometry/circlesphere/
+    # line intersection:
+    # https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection#Intersection%20of%20two%20lines
+    mid1 = (pt_prev + pt) / 2
+    mid2 = (pt + pt_next) / 2
+    n1 = (-d1[1], d1[0])
+    n2 = (-d2[1], d2[0])
+    det1 = (mid1[0] + n1[0]) * mid1[1] - (mid1[1] + n1[1]) * mid1[0]
+    det2 = (mid2[0] + n2[0]) * mid2[1] - (mid2[1] + n2[1]) * mid2[0]
+    center = np.array([(det1 * n2[0] - det2 * n1[0]) / det, (det1 * n2[1] - det2 * n1[1]) / det])
+    return center
+
+def getPathCurvatures(paths):
+    curvatures = []
+    pm0 = np.array([0.0, 0.0])
+    pm1 = np.array([0.0, 0.0])
+    pm2 = np.array([0.0, 0.0])
+    for path in paths:
+        radiuses = []
+        for i in range(2, len(path.poses)):
+            pm0[0] = path.poses[i].pose.position.x
+            pm0[1] = path.poses[i].pose.position.y
+            pm1[0] = path.poses[i-1].pose.position.x
+            pm1[1] = path.poses[i-1].pose.position.y
+            pm2[0] = path.poses[i-2].pose.position.x
+            pm2[1] = path.poses[i-2].pose.position.y
+            center = arcCenter(pm2, pm1, pm0)
+            if center[0] != float('inf'):
+              turning_rad = np.linalg.norm(pm1 - center);
+              radiuses.append(turning_rad)
+        curvatures.append(np.average(radiuses))
+    return curvatures
+
+def plotResults(costmap, paths):
+    coords = getMapCoordsFromPaths(paths, costmap.metadata.resolution)
+    data = np.asarray(costmap.data)
+    data.resize(costmap.metadata.size_y, costmap.metadata.size_x)
+    data = np.where(data <= 253, 0, data)
+
+    plt.figure(3)
+    ax = sns.heatmap(data, cmap='Greys', cbar=False)
+    for i in range(0, len(coords), 2):
+        ax.plot(coords[i], coords[i+1], linewidth=0.7)
+    plt.axis('off')
+    ax.set_aspect('equal', 'box')
+    plt.show()
+
+
+def averagePathCost(paths, costmap, num_of_planners):
+    coords = getMapCoordsFromPaths(paths, costmap.metadata.resolution)
+    data = np.asarray(costmap.data)
+    data.resize(costmap.metadata.size_y, costmap.metadata.size_x)
+
+    average_path_costs = []
+    for i in range(num_of_planners):
+        average_path_costs.append([])
+
+    k = 0
+    for i in range(0, len(coords), 2):
+        costs = []
+        for j in range(len(coords[i])):
+            costs.append(data[math.floor(coords[i+1][j])][math.floor(coords[i][j])])
+        average_path_costs[k % num_of_planners].append(sum(costs)/len(costs))
+        k += 1
+
+    return average_path_costs
+
+
+def maxPathCost(paths, costmap, num_of_planners):
+    coords = getMapCoordsFromPaths(paths, costmap.metadata.resolution)
+    data = np.asarray(costmap.data)
+    data.resize(costmap.metadata.size_y, costmap.metadata.size_x)
+
+    max_path_costs = []
+    for i in range(num_of_planners):
+        max_path_costs.append([])
+
+    k = 0
+    for i in range(0, len(coords), 2):
+        max_cost = 0
+        for j in range(len(coords[i])):
+            cost = data[math.floor(coords[i+1][j])][math.floor(coords[i][j])]
+            if max_cost < cost:
+                max_cost = cost
+        max_path_costs[k % num_of_planners].append(max_cost)
+        k += 1
+
+    return max_path_costs
+
+
+def main():
+    # Read the data
+    benchmark_dir = os.getcwd()
+    print("Read data")
+    with open(os.path.join(benchmark_dir, 'results.pickle'), 'rb') as f:
+        results = pickle.load(f)
+
+    with open(os.path.join(benchmark_dir, 'methods.pickle'), 'rb') as f:
+        smoothers = pickle.load(f)
+    planner = smoothers[0]
+    del smoothers[0]
+    methods_num = len(smoothers) + 1
+
+    with open(os.path.join(benchmark_dir, 'costmap.pickle'), 'rb') as f:
+        costmap = pickle.load(f)
+
+    # Paths (planner and smoothers)
+    paths = getPaths(results)
+    path_lengths = []
+
+    for path in paths:
+        path_lengths.append(getPathLength(path))
+    path_lengths = np.asarray(path_lengths)
+    total_paths = len(paths)
+
+    # [planner, smoothers] path lenghth in a row
+    path_lengths.resize((int(total_paths/methods_num), methods_num))
+    # [planner, smoothers] path length in a column
+    path_lengths = path_lengths.transpose()
+
+    # Times
+    times = getTimes(results)
+    times = np.asarray(times)
+    times.resize((int(total_paths/methods_num), methods_num))
+    times = np.transpose(times)
+
+    # Costs
+    average_path_costs = np.asarray(averagePathCost(paths, costmap, methods_num))
+    max_path_costs = np.asarray(maxPathCost(paths, costmap, methods_num))
+
+    # Smoothness
+    smoothnesses = getPathSmoothnesses(paths)
+    smoothnesses = np.asarray(smoothnesses)
+    smoothnesses.resize((int(total_paths/methods_num), methods_num))
+    smoothnesses = np.transpose(smoothnesses)
+
+    # Curvatures
+    curvatures = getPathCurvatures(paths)
+    curvatures = np.asarray(curvatures)
+    curvatures.resize((int(total_paths/methods_num), methods_num))
+    curvatures = np.transpose(curvatures)
+
+    # Generate table
+    planner_table = [['Planner',
+                      'Time (s)',
+                      'Path length (m)',
+                      'Average cost',
+                      'Max cost',
+                      'Path smoothness (x100)',
+                      'Average turning rad (m)']]
+    # for path planner
+    planner_table.append([planner,
+                          np.average(times[0]),
+                          np.average(path_lengths[0]),
+                          np.average(average_path_costs[0]),
+                          np.average(max_path_costs[0]),
+                          np.average(smoothnesses[0]) * 100,
+                          np.average(curvatures[0])])
+    # for path smoothers
+    for i in range(1, methods_num):
+        planner_table.append([smoothers[i-1],
+                              np.average(times[i]),
+                              np.average(path_lengths[i]),
+                              np.average(average_path_costs[i]),
+                              np.average(max_path_costs[i]),
+                              np.average(smoothnesses[i]) * 100,
+                              np.average(curvatures[i])])
+
+    # Visualize results
+    print(tabulate(planner_table))
+    plotResults(costmap, paths)
+
+    exit(0)
+
+
+if __name__ == '__main__':
+    main()
diff --git a/tools/smoother_benchmarking/smoother_benchmark_bringup.py b/tools/smoother_benchmarking/smoother_benchmark_bringup.py
new file mode 100644
index 0000000000..9b0ed744de
--- /dev/null
+++ b/tools/smoother_benchmarking/smoother_benchmark_bringup.py
@@ -0,0 +1,94 @@
+# Copyright (c) 2022 Samsung Research America
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
+    benchmark_dir = os.getcwd()
+    metrics_py = os.path.join(benchmark_dir, 'metrics.py')
+    config = os.path.join(get_package_share_directory('nav2_bringup'), 'params', 'nav2_params.yaml')
+    map_file = os.path.join(benchmark_dir, 'maps', 'smoothers_world.yaml')
+    lifecycle_nodes = ['map_server', 'planner_server', 'smoother_server']
+
+    static_transform_one = Node(
+        package = 'tf2_ros',
+        executable = 'static_transform_publisher',
+        output = 'screen',
+        arguments = ["0", "0", "0", "0", "0", "0", "base_link", "map"])
+
+    static_transform_two = Node(
+        package = 'tf2_ros',
+        executable = 'static_transform_publisher',
+        output = 'screen',
+        arguments = ["0", "0", "0", "0", "0", "0", "base_link", "odom"])
+
+    start_map_server_cmd = Node(
+        package='nav2_map_server',
+        executable='map_server',
+        name='map_server',
+        output='screen',
+        parameters=[{'use_sim_time': True},
+                    {'yaml_filename': map_file},
+                    {'topic_name': "map"}])
+
+    start_planner_server_cmd = Node(
+        package='nav2_planner',
+        executable='planner_server',
+        name='planner_server',
+        output='screen',
+        parameters=[config])
+
+    start_smoother_server_cmd = Node(
+        package='nav2_smoother',
+        executable='smoother_server',
+        name='smoother_server',
+        output='screen',
+        parameters=[config])
+
+    start_lifecycle_manager_cmd = Node(
+        package='nav2_lifecycle_manager',
+        executable='lifecycle_manager',
+        name='lifecycle_manager',
+        output='screen',
+        parameters=[{'use_sim_time': True},
+                    {'autostart': True},
+                    {'node_names': lifecycle_nodes}])
+
+    rviz_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')),
+        launch_arguments={'namespace': '',
+                          'use_namespace': 'False'}.items())
+
+    metrics_cmd = ExecuteProcess(
+        cmd=['python3', '-u', metrics_py],
+        cwd=[benchmark_dir], output='screen')
+
+    ld = LaunchDescription()
+    ld.add_action(static_transform_one)
+    ld.add_action(static_transform_two)
+    ld.add_action(start_map_server_cmd)
+    ld.add_action(start_planner_server_cmd)
+    ld.add_action(start_smoother_server_cmd)
+    ld.add_action(start_lifecycle_manager_cmd)
+    ld.add_action(rviz_cmd)
+    ld.add_action(metrics_cmd)
+    return ld