adding results

tools/testing_environment/debug_launch.launch.py

@@ -45,7 +45,7 @@ def generate_launch_description():
                 executable='planner_server',
                 name='planner_server',
                 output='screen',
-                prefix=['xterm -bg black -fg white -e gdb -ex run --args'],
+                # prefix=['xterm -bg black -fg white -e gdb -ex run --args'],
                 parameters=[config],
             ),
             Node(

tools/testing_environment/debug_script.py

@@ -5,7 +5,11 @@
 from rcl_interfaces.srv import SetParameters
 from rclpy.parameter import Parameter
 from time import sleep
-
+from transforms3d.euler import euler2quat
+import os
+import pickle
+
+import numpy as np
 
 
 def main():
@@ -14,24 +18,26 @@ def main():
     navigator = BasicNavigator()
 
     planner_param_cli = navigator.create_client(SetParameters, '/planner_server/set_parameters')
-
-
-#     [rviz2-6] [INFO] [1707801843.417743672] [rviz2]: Setting estimate pose: Frame:map, Position(-1.90312, 0.068757, 0), Orientation(0, 0, -0.693387, 0.720566) = Angle: -1.53236
-# [rviz2-6] Start navigation
-# [rviz2-6] [ERROR] [1707801854.817011406] [rviz_navigation_dialog_action_client]: navigate_to_pose action server is not available. Is the initial pose set?
-# [rviz2-6] [INFO] [1707801860.095115277] [rviz2]: Setting estimate pose: Frame:map, Position(1.675, 0.00936884, 0), Orientation(0, 0, 0.703077, 0.711114) = Angle: 1.55943
-
-
+    print("created client")
+    # 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)
 
     start = PoseStamped()
     start.header.frame_id = 'map'
     start.header.stamp = navigator.get_clock().now().to_msg()
     start.pose.position.x = -1.90312
     start.pose.position.y = 0.068757
-    start.pose.orientation.w = 0.720566
-    start.pose.orientation.x = 0.0
-    start.pose.orientation.y = 0.0
-    start.pose.orientation.z = -0.693387
+    quad = euler2quat(0.0, 0.0, -1.53236)
+    start.pose.orientation.w = quad[0]
+    start.pose.orientation.x = quad[1]
+    start.pose.orientation.y = quad[2]
+    start.pose.orientation.z = quad[3]
+
+
+
+
 
 
     print("Start position: ", start.pose.position.x, ", ", start.pose.position.y, ", ", start.pose.position.z)
@@ -42,39 +48,62 @@ def main():
     goal.header.frame_id = 'map'
     goal.header.stamp = navigator.get_clock().now().to_msg()
     goal.pose.position.x = 1.675
-    goal.pose.position.y = 0
-    goal.pose.orientation.w = 0.711114
-    goal.pose.orientation.x = 0.0
-    goal.pose.orientation.y = 0.0
-    goal.pose.orientation.z = 0.703077
+    goal.pose.position.y = 0.0
+    quad = euler2quat(0.0, 0.0, 1.55943)
+    goal.pose.orientation.w = quad[0]
+    goal.pose.orientation.x = quad[1]
+    goal.pose.orientation.y = quad[2]
+    goal.pose.orientation.z = quad[3]
 
 
     print("Goal position: ", goal.pose.position.x, ", ", goal.pose.position.y, ", ", goal.pose.position.z)
     print("Goal orientation: ", goal.pose.orientation.x, ", ", goal.pose.orientation.y, ", ", goal.pose.orientation.z, ", ", goal.pose.orientation.w)
+    result = []
     results = []
     # generate path
-    goal_headings = ["DEFAULT", "BIDIRECTIONAL", "ALL_DIRECTION"]
+    goal_headings = ["DEFAULT" , "BIDIRECTIONAL", "ALL_DIRECTION"]
     planners = ['SmacHybrid', 'Smac2d', 'SmacLattice']
+    planners_goal_headings = ["DEFAULT_SmacHybrid" , "DEFAULT_Smac2d", "DEFAULT_SmacLattice", "BIDIRECTIONAL_SmacHybrid", "BIDIRECTIONAL_Smac2d", "BIDIRECTIONAL_SmacLattice", "ALL_DIRECTION_SmacHybrid", "ALL_DIRECTION_Smac2d", "ALL_DIRECTION_SmacLattice"]
     for goal_heading in goal_headings:
         print("Goal heading: ", goal_heading)      
         for planner in planners:
             print("Planner: ", planner)
             if(planner != 'Smac2d'):
+                print("Setting goal heading for ", planner)
                 # set parameters planner
                 req = SetParameters.Request()
                 req.parameters = [ 
                     Parameter(planner + '.goal_heading', Parameter.Type.STRING, goal_heading).to_parameter_msg()
                 ]
                 future = planner_param_cli.call_async(req)
                 rclpy.spin_until_future_complete(navigator, future)
+                # if future.results() is None:
+                #     print('Service call failed %r' % (future.exception(),))
+                #     return
+                # print('Service call result %r' % (future.results().result,))
+            print("Getting path from ", planner)
             path = navigator._getPathImpl(start, goal, planner, use_start=True)
             if path is not None and path.error_code == 0:
                 print("Path found by ", planner)
-                results.append(path)
+                result.append(path)
             else:
                 print(planner, 'planner failed to produce the path')
         print("\n")
+        # sleep(5)
     print("Done")
+    print('Write Results...')
+    results.append(result)
+    with open(os.getcwd() + '/results.pickle', 'wb+') as f:
+        pickle.dump(results, f, pickle.HIGHEST_PROTOCOL)
+
+    with open(os.getcwd() + '/costmap.pickle', 'wb+') as f:
+        pickle.dump(costmap_msg, f, pickle.HIGHEST_PROTOCOL)
+
+    with open(os.getcwd() + '/planners.pickle', 'wb+') as f:
+        pickle.dump(planners_goal_headings, f, pickle.HIGHEST_PROTOCOL)
+    print('Write Complete')
+    exit(0)
+
 
 
 if __name__ == '__main__':

tools/testing_environment/process_data.py

@@ -0,0 +1,187 @@
+#! /usr/bin/env python3
+# Copyright 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.
+
+import math
+import os
+import pickle
+
+import matplotlib.pylab as plt
+import numpy as np
+import seaborn as sns
+from tabulate import tabulate
+
+
+def getPaths(results):
+    paths = []
+    for result in results:
+        for path in result:
+            paths.append(path.path)
+    return paths
+
+
+def getTimes(results):
+    times = []
+    for result in results:
+        for time in result:
+            times.append(time.planning_time.nanosec / 1e09 + time.planning_time.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
+
+
+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():
+
+    print('Read data')
+    with open(os.getcwd() + '/results.pickle', 'rb') as f:
+        results = pickle.load(f)
+
+    with open(os.getcwd() + '/planners.pickle', 'rb') as f:
+        planners = pickle.load(f)
+
+    with open(os.getcwd() + '/costmap.pickle', 'rb') as f:
+        costmap = pickle.load(f)
+
+    paths = getPaths(results)
+    path_lengths = []
+
+    for path in paths:
+        path_lengths.append(getPathLength(path))
+    path_lengths = np.asarray(path_lengths)
+    total_paths = len(paths)
+
+    path_lengths.resize((int(total_paths / len(planners)), len(planners)))
+    path_lengths = path_lengths.transpose()
+
+    times = getTimes(results)
+    times = np.asarray(times)
+    times.resize((int(total_paths / len(planners)), len(planners)))
+    times = np.transpose(times)
+
+    # Costs
+    average_path_costs = np.asarray(averagePathCost(paths, costmap, len(planners)))
+    max_path_costs = np.asarray(maxPathCost(paths, costmap, len(planners)))
+
+    # Generate table
+    planner_table = [
+        [
+            'Planner',
+            'Average path length (m)',
+            'Average Time (s)',
+            'Average cost',
+            'Max cost',
+        ]
+    ]
+
+    for i in range(0, len(planners)):
+        planner_table.append(
+            [
+                planners[i],
+                np.average(path_lengths[i]),
+                np.average(times[i]),
+                np.average(average_path_costs[i]),
+                np.average(max_path_costs[i]),
+            ]
+        )
+
+    # Visualize results
+    print(tabulate(planner_table))
+    plotResults(costmap, paths)
+
+
+if __name__ == '__main__':
+    main()