add humble-navigation2
This commit is contained in:
X-lanni
Binary file not shown.
@@ -0,0 +1,6 @@
|
||||
image: 100by100_10.pgm
|
||||
resolution: 0.05
|
||||
origin: [0.0, 0.000000, 0.000000]
|
||||
negate: 1
|
||||
occupied_thresh: 0.65
|
||||
free_thresh: 0.196
|
||||
Binary file not shown.
@@ -0,0 +1,6 @@
|
||||
image: 100by100_15.pgm
|
||||
resolution: 0.05
|
||||
origin: [0.0, 0.000000, 0.000000]
|
||||
negate: 1
|
||||
occupied_thresh: 0.65
|
||||
free_thresh: 0.196
|
||||
Binary file not shown.
@@ -0,0 +1,6 @@
|
||||
image: 100by100_20.pgm
|
||||
resolution: 0.05
|
||||
origin: [0.0, 0.000000, 0.000000]
|
||||
negate: 1
|
||||
occupied_thresh: 0.65
|
||||
free_thresh: 0.196
|
||||
@@ -0,0 +1,30 @@
|
||||
# Planning Benchmark
|
||||
|
||||
This experiment runs a set of planners over randomly generated maps, with randomly generated goals for objective benchmarking.
|
||||
|
||||
To use, modify the Nav2 bringup parameters to include the planners of interest:
|
||||
|
||||
```
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
expected_planner_frequency: 20.0
|
||||
use_sim_time: True
|
||||
planner_plugins: ["SmacHybrid", "Smac2d", "SmacLattice", "Navfn", "ThetaStar"]
|
||||
SmacHybrid:
|
||||
plugin: "nav2_smac_planner/SmacPlannerHybrid"
|
||||
Smac2d:
|
||||
plugin: "nav2_smac_planner/SmacPlanner2D"
|
||||
SmacLattice:
|
||||
plugin: "nav2_smac_planner/SmacPlannerLattice"
|
||||
Navfn:
|
||||
plugin: "nav2_navfn_planner/NavfnPlanner"
|
||||
ThetaStar:
|
||||
plugin: "nav2_theta_star_planner/ThetaStarPlanner"
|
||||
```
|
||||
|
||||
Set global costmap settings to those desired for benchmarking. The global map will be automatically set in the script. Inside of `metrics.py`, you can modify the map or set of planners to use.
|
||||
|
||||
Launch the benchmark via `ros2 launch ./planning_benchmark_bringup.py` to launch the planner and map servers, then run each script in this directory:
|
||||
|
||||
- `metrics.py` to capture data in `.pickle` files.
|
||||
- `process_data.py` to take the metric files and process them into key results (and plots)
|
||||
@@ -0,0 +1,150 @@
|
||||
#! /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.
|
||||
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from nav2_simple_commander.robot_navigator import BasicNavigator
|
||||
import rclpy
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
import math
|
||||
import os
|
||||
import pickle
|
||||
import glob
|
||||
import time
|
||||
import numpy as np
|
||||
|
||||
from random import seed
|
||||
from random import randint
|
||||
from random import uniform
|
||||
|
||||
from transforms3d.euler import euler2quat
|
||||
|
||||
|
||||
def getPlannerResults(navigator, initial_pose, goal_pose, planners):
|
||||
results = []
|
||||
for planner in planners:
|
||||
path = navigator._getPathImpl(initial_pose, goal_pose, planner, use_start=True)
|
||||
if path is not None:
|
||||
results.append(path)
|
||||
else:
|
||||
return results
|
||||
return 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 map to use, other options: 100by100_15, 100by100_10
|
||||
map_path = os.getcwd() + '/' + glob.glob('**/100by100_20.yaml', recursive=True)[0]
|
||||
navigator.changeMap(map_path)
|
||||
time.sleep(2)
|
||||
|
||||
# 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)
|
||||
|
||||
planners = ['Navfn', 'ThetaStar', 'SmacHybrid', 'Smac2d', 'SmacLattice']
|
||||
max_cost = 210
|
||||
side_buffer = 100
|
||||
time_stamp = navigator.get_clock().now().to_msg()
|
||||
results = []
|
||||
seed(33)
|
||||
|
||||
random_pairs = 100
|
||||
res = costmap_msg.metadata.resolution
|
||||
i = 0
|
||||
while len(results) != 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, planners)
|
||||
if len(result) == len(planners):
|
||||
results.append(result)
|
||||
i = i + 1
|
||||
else:
|
||||
print("One of the planners was invalid")
|
||||
|
||||
print("Write Results...")
|
||||
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, f, pickle.HIGHEST_PROTOCOL)
|
||||
print("Write Complete")
|
||||
exit(0)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,73 @@
|
||||
# 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 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')
|
||||
config = os.path.join(get_package_share_directory('nav2_bringup'), 'params', 'nav2_params.yaml')
|
||||
map_file = os.path.join(nav2_bringup_dir, 'maps', 'turtlebot3_world.yaml')
|
||||
lifecycle_nodes = ['map_server', 'planner_server']
|
||||
|
||||
return LaunchDescription([
|
||||
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"}]),
|
||||
|
||||
Node(
|
||||
package='nav2_planner',
|
||||
executable='planner_server',
|
||||
name='planner_server',
|
||||
output='screen',
|
||||
parameters=[config]),
|
||||
|
||||
Node(
|
||||
package = 'tf2_ros',
|
||||
executable = 'static_transform_publisher',
|
||||
output = 'screen',
|
||||
arguments = ["0", "0", "0", "0", "0", "0", "base_link", "map"]),
|
||||
|
||||
Node(
|
||||
package = 'tf2_ros',
|
||||
executable = 'static_transform_publisher',
|
||||
output = 'screen',
|
||||
arguments = ["0", "0", "0", "0", "0", "0", "base_link", "odom"]),
|
||||
|
||||
Node(
|
||||
package='nav2_lifecycle_manager',
|
||||
executable='lifecycle_manager',
|
||||
name='lifecycle_manager',
|
||||
output='screen',
|
||||
parameters=[{'use_sim_time': True},
|
||||
{'autostart': True},
|
||||
{'node_names': lifecycle_nodes}]),
|
||||
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')),
|
||||
launch_arguments={'namespace': '',
|
||||
'use_namespace': 'False'}.items())
|
||||
|
||||
])
|
||||
@@ -0,0 +1,173 @@
|
||||
#! /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 numpy as np
|
||||
import math
|
||||
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
import pickle
|
||||
|
||||
import seaborn as sns
|
||||
import matplotlib.pylab as plt
|
||||
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()
|
||||
Reference in New Issue
Block a user