diff --git a/tools/testing_environment/costmap.pickle b/tools/testing_environment/costmap.pickle new file mode 100644 index 00000000000..6fa609d3c67 Binary files /dev/null and b/tools/testing_environment/costmap.pickle differ diff --git a/tools/testing_environment/debug_launch.launch.py b/tools/testing_environment/debug_launch.launch.py index 5b0d488e88b..4ec4f931a8e 100644 --- a/tools/testing_environment/debug_launch.launch.py +++ b/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( diff --git a/tools/testing_environment/debug_script.py b/tools/testing_environment/debug_script.py index d7cfd34147e..3107b0e2074 100644 --- a/tools/testing_environment/debug_script.py +++ b/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,24 +48,28 @@ 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 = [ @@ -67,14 +77,33 @@ def main(): ] 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__': diff --git a/tools/testing_environment/planners.pickle b/tools/testing_environment/planners.pickle new file mode 100644 index 00000000000..cd5df74732c Binary files /dev/null and b/tools/testing_environment/planners.pickle differ diff --git a/tools/testing_environment/process_data.py b/tools/testing_environment/process_data.py new file mode 100644 index 00000000000..299997774db --- /dev/null +++ b/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() diff --git a/tools/testing_environment/results.pickle b/tools/testing_environment/results.pickle new file mode 100644 index 00000000000..fb9776f6e75 Binary files /dev/null and b/tools/testing_environment/results.pickle differ