Add Path Smoothers Benchmarking suite (#3236)

* Add Path Smoothers Benchmarking suite

* Meet review items

* Update tools/smoother_benchmarking/README.md

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Move optional performance patch to the end of README

* Fix README

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

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."""

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)

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

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()