Add Path Smoothers Benchmarking suite

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,121 @@
+# 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 perform the benchmarking, the following changes should be made for planner and smoother servers:
+
+```
+diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp
+index c7a90bcb..510bb6ea 100644
+--- a/nav2_planner/src/planner_server.cpp
++++ b/nav2_planner/src/planner_server.cpp
+@@ -285,7 +285,7 @@ bool PlannerServer::getStartPose(
+   typename std::shared_ptr<const typename T::Goal> goal,
+   geometry_msgs::msg::PoseStamped & start)
+ {
+-  if (goal->use_start) {
++  if (true) {
+     start = goal->start;
+   } else if (!costmap_ros_->getRobotPose(start)) {
+     return false;
+@@ -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;
+```
+
+To use the suite, modify the Nav2 bringup parameters 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"
+      holonomic: False # should be disabled to utilize motion/rotation model
+    constrained_smoother:
+      plugin: "nav2_constrained_smoother/ConstrainedSmoother"
+      w_smooth: 100000.0 # tuned
+```
+
+Then enable SmoothPath sequence for selected path planner, as shown for default `navigate_to_pose_w_replanning_and_recovery.xml` behavior tree:
+
+```
+       <PipelineSequence name="NavigateWithReplanning">
+         <RateController hz="1.0">
+           <RecoveryNode number_of_retries="1" name="ComputePathToPose">
+-            <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
++            <Sequence name="ComputeAndSmoothPath">
++              <ComputePathToPose goal="{goal}" path="{path}" planner_id="SmacHybrid"/>
++              <SmoothPath unsmoothed_path="{path}" smoothed_path="{path}"/>
++            </Sequence>
+             <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
+           </RecoveryNode>
+         </RateController>
+```
+
+Inside of `metrics.py`, you can modify the map or reference planner / path smoothers to use. Set global costmap, path planner and smoothers settings to those desired for benchmarking and execute it:
+
+- `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,173 @@
+#! /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
+
+import time
+
+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)
+
+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
+        #time.sleep(10)
+    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()
+
+    # Set our experiment's initial pose
+    initial_pose = PoseStamped()
+    initial_pose.header.frame_id = 'map'
+    initial_pose.header.stamp = navigator.get_clock().now().to_msg()
+    initial_pose.pose.position.x = 1.0
+    initial_pose.pose.position.y = 1.0
+    initial_pose.pose.orientation.z = 0.0
+    initial_pose.pose.orientation.w = 1.0
+    navigator.setInitialPose(initial_pose)
+
+    # Wait for navigation to fully activate
+    print("Waiting for nav2 to activate")
+    navigator.waitUntilNav2Active()
+
+    # 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: 'NavFn', 'SmacLattice', 'SmacHybrid', 'ThetaStarPlanner'
+    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:
+            print("MAO: result = ", result)
+            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...")
+    nav2_planner_metrics_dir = os.getcwd()
+    with open(os.path.join(nav2_planner_metrics_dir, 'results.pickle'), 'wb') as f:
+        pickle.dump(results, f, pickle.HIGHEST_PROTOCOL)
+
+    with open(os.path.join(nav2_planner_metrics_dir, 'costmap.pickle'), 'wb') as f:
+        pickle.dump(costmap_msg, f, pickle.HIGHEST_PROTOCOL)
+
+    smoothers.insert(0, planner)
+    with open(os.path.join(nav2_planner_metrics_dir, 'methods.pickle'), 'wb') as f:
+        pickle.dump(smoothers, f, pickle.HIGHEST_PROTOCOL)
+    print("Write Complete")
+
+    navigator.lifecycleShutdown()
+    exit(0)
+
+
+if __name__ == '__main__':
+    main()