m5p3nc3r/agv_pro_ros2
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

add humble-navigation2

Browse Source
This commit is contained in:
X-lanni
2025-05-27 19:03:40 +08:00
parent 974abb5e1e
commit e74ec539c2
1280 changed files with 204114 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
navigation2/tools/smoother_benchmarking/README.md
+98
View File
@@ -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)
navigation2/tools/smoother_benchmarking/maps/smoothers_world.pgm
BIN
View File
Binary file not shown.
navigation2/tools/smoother_benchmarking/maps/smoothers_world.yaml
+6
View File
@@ -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
navigation2/tools/smoother_benchmarking/metrics.py
+157
View File
@@ -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()
navigation2/tools/smoother_benchmarking/process_data.py
+302
View File
@@ -0,0 +1,302 @@
#! /usr/bin/env python3
# Copyright (c) 2022 Samsung R&D Institute Russia
# Copyright (c) 2022 Joshua Wallace
# Copyright (c) 2021 RoboTech Vision
#
# 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
import pickle
import seaborn as sns
import matplotlib.pylab as plt
from tabulate import tabulate
def getPaths(results):
paths = []
for i in range(len(results)):
if (i % 2) == 0:
# Append non-smoothed path
paths.append(results[i].path)
else:
# Append smoothed paths array
for result in results[i]:
paths.append(result.path)
return paths
def getTimes(results):
times = []
for i in range(len(results)):
if (i % 2) == 0:
# Append non-smoothed time
times.append(results[i].planning_time.nanosec/1e09 + results[i].planning_time.sec)
else:
# Append smoothed times array
for result in results[i]:
times.append(result.smoothing_duration.nanosec/1e09 + result.smoothing_duration.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
# Path smoothness calculations
def getSmoothness(pt_prev, pt, pt_next):
d1 = pt - pt_prev
d2 = pt_next - pt
delta = d2 - d1
return np.dot(delta, delta)
def getPathSmoothnesses(paths):
smoothnesses = []
pm0 = np.array([0.0, 0.0])
pm1 = np.array([0.0, 0.0])
pm2 = np.array([0.0, 0.0])
for path in paths:
smoothness = 0.0
for i in range(2, len(path.poses)):
pm0[0] = path.poses[i].pose.position.x
pm0[1] = path.poses[i].pose.position.y
pm1[0] = path.poses[i-1].pose.position.x
pm1[1] = path.poses[i-1].pose.position.y
pm2[0] = path.poses[i-2].pose.position.x
pm2[1] = path.poses[i-2].pose.position.y
smoothness += getSmoothness(pm2, pm1, pm0)
smoothnesses.append(smoothness)
return smoothnesses
# Curvature calculations
def arcCenter(pt_prev, pt, pt_next):
cusp_thresh = -0.7
d1 = pt - pt_prev
d2 = pt_next - pt
d1_norm = d1 / np.linalg.norm(d1)
d2_norm = d2 / np.linalg.norm(d2)
cos_angle = np.dot(d1_norm, d2_norm)
if cos_angle < cusp_thresh:
# cusp case
d2 = -d2
pt_next = pt + d2
det = d1[0] * d2[1] - d1[1] * d2[0]
if abs(det) < 1e-4: # straight line
return (float('inf'), float('inf'))
# circle center is at the intersection of mirror axes of the segments:
# http://paulbourke.net/geometry/circlesphere/
# line intersection:
# https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection#Intersection%20of%20two%20lines
mid1 = (pt_prev + pt) / 2
mid2 = (pt + pt_next) / 2
n1 = (-d1[1], d1[0])
n2 = (-d2[1], d2[0])
det1 = (mid1[0] + n1[0]) * mid1[1] - (mid1[1] + n1[1]) * mid1[0]
det2 = (mid2[0] + n2[0]) * mid2[1] - (mid2[1] + n2[1]) * mid2[0]
center = np.array([(det1 * n2[0] - det2 * n1[0]) / det, (det1 * n2[1] - det2 * n1[1]) / det])
return center
def getPathCurvatures(paths):
curvatures = []
pm0 = np.array([0.0, 0.0])
pm1 = np.array([0.0, 0.0])
pm2 = np.array([0.0, 0.0])
for path in paths:
radiuses = []
for i in range(2, len(path.poses)):
pm0[0] = path.poses[i].pose.position.x
pm0[1] = path.poses[i].pose.position.y
pm1[0] = path.poses[i-1].pose.position.x
pm1[1] = path.poses[i-1].pose.position.y
pm2[0] = path.poses[i-2].pose.position.x
pm2[1] = path.poses[i-2].pose.position.y
center = arcCenter(pm2, pm1, pm0)
if center[0] != float('inf'):
turning_rad = np.linalg.norm(pm1 - center);
radiuses.append(turning_rad)
curvatures.append(np.average(radiuses))
return curvatures
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():
# Read the data
benchmark_dir = os.getcwd()
print("Read data")
with open(os.path.join(benchmark_dir, 'results.pickle'), 'rb') as f:
results = pickle.load(f)
with open(os.path.join(benchmark_dir, 'methods.pickle'), 'rb') as f:
smoothers = pickle.load(f)
planner = smoothers[0]
del smoothers[0]
methods_num = len(smoothers) + 1
with open(os.path.join(benchmark_dir, 'costmap.pickle'), 'rb') as f:
costmap = pickle.load(f)
# Paths (planner and smoothers)
paths = getPaths(results)
path_lengths = []
for path in paths:
path_lengths.append(getPathLength(path))
path_lengths = np.asarray(path_lengths)
total_paths = len(paths)
# [planner, smoothers] path lenghth in a row
path_lengths.resize((int(total_paths/methods_num), methods_num))
# [planner, smoothers] path length in a column
path_lengths = path_lengths.transpose()
# Times
times = getTimes(results)
times = np.asarray(times)
times.resize((int(total_paths/methods_num), methods_num))
times = np.transpose(times)
# Costs
average_path_costs = np.asarray(averagePathCost(paths, costmap, methods_num))
max_path_costs = np.asarray(maxPathCost(paths, costmap, methods_num))
# Smoothness
smoothnesses = getPathSmoothnesses(paths)
smoothnesses = np.asarray(smoothnesses)
smoothnesses.resize((int(total_paths/methods_num), methods_num))
smoothnesses = np.transpose(smoothnesses)
# Curvatures
curvatures = getPathCurvatures(paths)
curvatures = np.asarray(curvatures)
curvatures.resize((int(total_paths/methods_num), methods_num))
curvatures = np.transpose(curvatures)
# Generate table
planner_table = [['Planner',
'Time (s)',
'Path length (m)',
'Average cost',
'Max cost',
'Path smoothness (x100)',
'Average turning rad (m)']]
# for path planner
planner_table.append([planner,
np.average(times[0]),
np.average(path_lengths[0]),
np.average(average_path_costs[0]),
np.average(max_path_costs[0]),
np.average(smoothnesses[0]) * 100,
np.average(curvatures[0])])
# for path smoothers
for i in range(1, methods_num):
planner_table.append([smoothers[i-1],
np.average(times[i]),
np.average(path_lengths[i]),
np.average(average_path_costs[i]),
np.average(max_path_costs[i]),
np.average(smoothnesses[i]) * 100,
np.average(curvatures[i])])
# Visualize results
print(tabulate(planner_table))
plotResults(costmap, paths)
exit(0)
if __name__ == '__main__':
main()
navigation2/tools/smoother_benchmarking/smoother_benchmark_bringup.py
+94
View File
@@ -0,0 +1,94 @@
# 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 ExecuteProcess, 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')
benchmark_dir = os.getcwd()
metrics_py = os.path.join(benchmark_dir, 'metrics.py')
config = os.path.join(get_package_share_directory('nav2_bringup'), 'params', 'nav2_params.yaml')
map_file = os.path.join(benchmark_dir, 'maps', 'smoothers_world.yaml')
lifecycle_nodes = ['map_server', 'planner_server', 'smoother_server']
static_transform_one = Node(
package = 'tf2_ros',
executable = 'static_transform_publisher',
output = 'screen',
arguments = ["0", "0", "0", "0", "0", "0", "base_link", "map"])
static_transform_two = Node(
package = 'tf2_ros',
executable = 'static_transform_publisher',
output = 'screen',
arguments = ["0", "0", "0", "0", "0", "0", "base_link", "odom"])
start_map_server_cmd = 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"}])
start_planner_server_cmd = Node(
package='nav2_planner',
executable='planner_server',
name='planner_server',
output='screen',
parameters=[config])
start_smoother_server_cmd = Node(
package='nav2_smoother',
executable='smoother_server',
name='smoother_server',
output='screen',
parameters=[config])
start_lifecycle_manager_cmd = Node(
package='nav2_lifecycle_manager',
executable='lifecycle_manager',
name='lifecycle_manager',
output='screen',
parameters=[{'use_sim_time': True},
{'autostart': True},
{'node_names': lifecycle_nodes}])
rviz_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')),
launch_arguments={'namespace': '',
'use_namespace': 'False'}.items())
metrics_cmd = ExecuteProcess(
cmd=['python3', '-u', metrics_py],
cwd=[benchmark_dir], output='screen')
ld = LaunchDescription()
ld.add_action(static_transform_one)
ld.add_action(static_transform_two)
ld.add_action(start_map_server_cmd)
ld.add_action(start_planner_server_cmd)
ld.add_action(start_smoother_server_cmd)
ld.add_action(start_lifecycle_manager_cmd)
ld.add_action(rviz_cmd)
ld.add_action(metrics_cmd)
return ld