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/bt2img.py
Executable
+197
View File
@@ -0,0 +1,197 @@
#!/usr/bin/python3
# Copyright (c) 2019 Intel Corporation
#
# 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.
# This tool converts a behavior tree XML file to a PNG image. Run bt2img.py -h
# for instructions
import argparse
import xml.etree.ElementTree
import graphviz # pip3 install graphviz
control_nodes = [
"Fallback",
"Parallel",
"ReactiveFallback",
"ReactiveSequence",
"Sequence",
"SequenceStar",
"BlackboardCheckInt",
"BlackboardCheckDouble",
"BlackboardCheckString",
"ForceFailure",
"ForceSuccess",
"Inverter",
"Repeat",
"Subtree",
"Timeout",
"RecoveryNode",
"PipelineSequence",
"RoundRobin",
"Control",
]
action_nodes = [
"AlwaysFailure",
"AlwaysSuccess",
"SetBlackboard",
"ComputePathToPose",
"FollowPath",
"BackUp",
"Spin",
"Wait",
"ClearEntireCostmap",
"ReinitializeGlobalLocalization",
"Action",
]
condition_nodes = [
"IsStuck",
"GoalReached",
"initialPoseReceived",
"GoalUpdated",
"DistanceTraveled",
"TimeExpired",
"TransformAvailable",
"Condition",
]
decorator_nodes = [
"Decorator",
"RateController",
"DistanceController",
"SpeedController",
]
subtree_nodes = [
"SubTree",
]
global xml_tree
def main():
global xml_tree
args = parse_command_line()
xml_tree = xml.etree.ElementTree.parse(args.behavior_tree)
root_tree_name = find_root_tree_name(xml_tree)
behavior_tree = find_behavior_tree(xml_tree, root_tree_name)
dot = convert2dot(behavior_tree)
if args.legend:
legend = make_legend()
legend.format = 'png'
legend.render(args.legend)
dot.format = 'png'
if args.save_dot:
print(f'Saving dot to {args.save_dot}')
args.save_dot.write(dot.source)
dot.render(args.image_out, view=args.display)
def parse_command_line():
parser = argparse.ArgumentParser(description='Convert a behavior tree XML file to an image')
parser.add_argument('--behavior_tree', required=True,
help='the behavior tree XML file to convert to an image')
parser.add_argument('--image_out', required=True,
help='The name of the output image file. Leave off the .png extension')
parser.add_argument('--display', action="store_true",
help='If specified, opens the image in the default viewer')
parser.add_argument('--save_dot', type=argparse.FileType('w'),
help='Saves the intermediate dot source to the specified file')
parser.add_argument('--legend',
help='Generate a legend image as well')
return parser.parse_args()
def find_root_tree_name(xml_tree):
return xml_tree.getroot().get('main_tree_to_execute')
def find_behavior_tree(xml_tree, tree_name):
trees = xml_tree.findall('BehaviorTree')
if len(trees) == 0:
raise RuntimeError("No behavior trees were found in the XML file")
for tree in trees:
if tree_name == tree.get('ID'):
return tree
raise RuntimeError(f'No behavior tree for name {tree_name} found in the XML file')
# Generate a dot description of the root of the behavior tree.
def convert2dot(behavior_tree):
dot = graphviz.Digraph()
root = behavior_tree
parent_dot_name = str(hash(root))
dot.node(parent_dot_name, root.get('ID'), shape='box')
convert_subtree(dot, root, parent_dot_name)
return dot
# Recursive function. We add the children to the dot file, and then recursively
# call this function on the children. Nodes are given an ID that is the hash
# of the node to ensure each is unique.
def convert_subtree(dot, parent_node, parent_dot_name):
if parent_node.tag == "SubTree":
add_sub_tree(dot, parent_dot_name, parent_node)
else:
add_nodes(dot, parent_dot_name, parent_node)
def add_sub_tree(dot, parent_dot_name, parent_node):
root_tree_name = parent_node.get('ID')
dot.node(parent_dot_name, root_tree_name, shape='box')
behavior_tree = find_behavior_tree(xml_tree, root_tree_name)
convert_subtree(dot, behavior_tree, parent_dot_name)
def add_nodes(dot, parent_dot_name, parent_node):
for node in list(parent_node):
label = make_label(node)
dot.node(str(hash(node)), label, color=node_color(node.tag), style='filled', shape='box')
dot_name = str(hash(node))
dot.edge(parent_dot_name, dot_name)
convert_subtree(dot, node, dot_name)
# The node label contains the:
# type, the name if provided, and the parameters.
def make_label(node):
label = '< <table border="0" cellspacing="0" cellpadding="0">'
label += '<tr><td align="text"><i>' + node.tag + '</i></td></tr>'
name = node.get('name')
if name:
label += '<tr><td align="text"><b>' + name + '</b></td></tr>'
for (param_name, value) in node.items():
label += '<tr><td align="left"><sub>' + param_name + '=' + value + '</sub></td></tr>'
label += '</table> >'
return label
def node_color(type):
if type in control_nodes:
return "chartreuse4"
if type in action_nodes:
return "cornflowerblue"
if type in condition_nodes:
return "yellow2"
if type in decorator_nodes:
return "darkorange1"
if type in subtree_nodes:
return "darkorchid1"
#else it's unknown
return "grey"
# creates a legend which can be provided with the other images.
def make_legend():
legend = graphviz.Digraph(graph_attr={'rankdir': 'LR'})
legend.attr(label='Legend')
legend.node('Unknown', shape='box', style='filled', color="grey")
legend.node('Action', 'Action Node', shape='box', style='filled', color="cornflowerblue")
legend.node('Condition', 'Condition Node', shape='box', style='filled', color="yellow2")
legend.node('Control', 'Control Node', shape='box', style='filled', color="chartreuse4")
return legend
if __name__ == '__main__':
main()
navigation2/tools/code_coverage_report.bash
Executable
+80
View File
@@ -0,0 +1,80 @@
#!/bin/bash
if [ ! -d build ]; then
echo "Please run this script from the root of your workspace."
echo "Expected directory hierarchy is:"
echo "example_ws"
echo " - build"
echo " - - package_a"
echo " - - package_b"
exit 1
fi
set -e
LCOVDIR=lcov
PWD=`pwd`
COVERAGE_REPORT_VIEW="genhtml"
for opt in "$@" ; do
case "$opt" in
clean)
rm -rf install build log $LCOVDIR
exit 0
;;
codecovio)
COVERAGE_REPORT_VIEW=codecovio
;;
genhtml)
COVERAGE_REPORT_VIEW=genhtml
;;
ci)
COVERAGE_REPORT_VIEW=ci
;;
esac
done
set -o xtrace
mkdir -p ${LCOVDIR}
# Ignore certain packages:
# - messages, which are auto generated files
# - system tests, which are themselves all test artifacts
# - rviz plugins, which are not used for real navigation
EXCLUDE_PACKAGES=$(
colcon list \
--names-only \
--packages-select-regex \
".*_msgs" \
".*_tests" \
".*_rviz.*" \
| xargs)
INCLUDE_PACKAGES=$(
colcon list \
--names-only \
--packages-ignore \
$EXCLUDE_PACKAGES \
| xargs)
# Capture executed code data.
fastcov --lcov \
-d build \
--exclude test/ $EXCLUDE_PACKAGES thirdparty/ \
--include $INCLUDE_PACKAGES \
--process-gcno \
--validate-sources \
--dump-statistic \
--output ${LCOVDIR}/total_coverage.info
if [ $COVERAGE_REPORT_VIEW = codecovio ]; then
curl -s https://codecov.io/bash > codecov
codecov_version=$(grep -o 'VERSION=\"[0-9\.]*\"' codecov | cut -d'"' -f2)
shasum -a 512 -c <(curl -s "https://raw.githubusercontent.com/codecov/codecov-bash/${codecov_version}/SHA512SUM" | grep -w "codecov")
bash codecov \
-f ${LCOVDIR}/total_coverage.info \
-R src/navigation2
elif [ $COVERAGE_REPORT_VIEW = genhtml ]; then
genhtml ${LCOVDIR}/total_coverage.info \
--output-directory ${LCOVDIR}/html
fi
navigation2/tools/ctest_retry.bash
Executable
+61
View File
@@ -0,0 +1,61 @@
#!/bin/bash
usage() {
echo "ctest_retry.bash [options]"
echo "Reruns ctest until the test passes or it has run 3 times (by default)."
echo " -r <number> Retry the test up to <number> times [Default: 3]"
echo " -d <path> Execute ctest from the given path [Default: Current working directory]"
echo " -t <testname> Execute only <testname> [Default: execute all tests available in <path> ]"
echo " -h displays this usage summary"
exit 0
}
RETRIES=3
TESTDIR=.
SPECIFIC_TEST=""
# Check Options
while getopts ":hr:d:t:" opt; do
case "$opt" in
h)
usage
;;
r)
RETRIES=$OPTARG
;;
d)
TESTDIR=$OPTARG
;;
t)
SPECIFIC_TEST="-R $OPTARG"
;;
\?)
echo "Invalid option: -$OPTARG" >&2
usage
exit 1
;;
:)
echo "Option -$OPTARG requires an argument." >&2
exit 1
;;
esac
done
total=$RETRIES
cd $TESTDIR
export RCUTILS_LOGGING_BUFFERED_STREAM=1
export RCUTILS_LOGGING_USE_STDOUT=1
echo "Retrying Ctest up to " $total " times."
for ((i=1;i<=total;i++))
do
ctest -V $SPECIFIC_TEST
result=$?
if [ "$result" == "0" ] # if ctest succeeded, then exit the retry loop
then
echo "Test succeeded on try " $i
exit 0
fi
done
echo "Test failed " $total " times."
exit $result
navigation2/tools/distro.Dockerfile
+101
View File
@@ -0,0 +1,101 @@
# syntax=docker/dockerfile:experimental
# Use experimental buildkit for faster builds
# https://github.com/moby/buildkit/blob/master/frontend/dockerfile/docs/experimental.md
# Use `--progress=plain` to use plane stdout for docker build
#
# Example build command:
# export DOCKER_BUILDKIT=1
# export FROM_IMAGE="ros:humble"
# export OVERLAY_MIXINS="release ccache lld"
# docker build -t nav2:humble \
# --build-arg FROM_IMAGE \
# --build-arg OVERLAY_MIXINS \
# -f distro.Dockerfile ../
ARG FROM_IMAGE=ros:humble
ARG OVERLAY_WS=/opt/overlay_ws
# multi-stage for caching
FROM $FROM_IMAGE AS cacher
# copy overlay source
ARG OVERLAY_WS
WORKDIR $OVERLAY_WS/src
RUN echo "\
repositories: \n\
ros-planning/navigation2: \n\
type: git \n\
url: https://github.com/ros-planning/navigation2.git \n\
version: ${ROS_DISTRO}-devel \n\
" > ../overlay.repos
RUN vcs import ./ < ../overlay.repos && \
find ./ -name ".git" | xargs rm -rf
# COPY ./ ./ros-planning/navigation2
# copy manifests for caching
WORKDIR /opt
RUN mkdir -p /tmp/opt && \
find ./ -name "package.xml" | \
xargs cp --parents -t /tmp/opt && \
find ./ -name "COLCON_IGNORE" | \
xargs cp --parents -t /tmp/opt || true
# multi-stage for building
FROM $FROM_IMAGE AS builder
ARG DEBIAN_FRONTEND=noninteractive
# edit apt for caching
RUN cp /etc/apt/apt.conf.d/docker-clean /etc/apt/ && \
echo 'Binary::apt::APT::Keep-Downloaded-Packages "true";' \
> /etc/apt/apt.conf.d/docker-clean
# install CI dependencies
RUN --mount=type=cache,target=/var/cache/apt \
--mount=type=cache,target=/var/lib/apt \
apt-get update && apt-get install -q -y \
ccache \
lcov \
lld \
&& rosdep update
# install overlay dependencies
ARG OVERLAY_WS
WORKDIR $OVERLAY_WS
COPY --from=cacher /tmp/$OVERLAY_WS/src ./src
RUN --mount=type=cache,target=/var/cache/apt \
--mount=type=cache,target=/var/lib/apt \
. /opt/ros/$ROS_DISTRO/setup.sh && \
apt-get update && rosdep install -q -y \
--from-paths src \
--ignore-src \
&& rm -rf /var/lib/apt/lists/*
# build overlay source
COPY --from=cacher $OVERLAY_WS/src ./src
ARG OVERLAY_MIXINS="release ccache lld"
RUN --mount=type=cache,target=/root/.ccache \
. /opt/ros/$ROS_DISTRO/setup.sh && \
colcon build \
--symlink-install \
--mixin $OVERLAY_MIXINS
# restore apt for docker
RUN mv /etc/apt/docker-clean /etc/apt/apt.conf.d/ && \
rm -rf /var/lib/apt/lists/
# source overlay from entrypoint
ENV OVERLAY_WS $OVERLAY_WS
RUN sed --in-place \
's|^source .*|source "$OVERLAY_WS/install/setup.bash"|' \
/ros_entrypoint.sh
# test overlay build
ARG RUN_TESTS
ARG FAIL_ON_TEST_FAILURE
RUN if [ -n "$RUN_TESTS" ]; then \
. install/setup.sh && \
colcon test && \
colcon test-result \
|| ([ -z "$FAIL_ON_TEST_FAILURE" ] || exit 1) \
fi
navigation2/tools/planner_benchmarking/100by100_10.pgm
BIN
View File
Binary file not shown.
navigation2/tools/planner_benchmarking/100by100_10.yaml
+6
View File
@@ -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
navigation2/tools/planner_benchmarking/100by100_15.pgm
BIN
View File
Binary file not shown.
navigation2/tools/planner_benchmarking/100by100_15.yaml
+6
View File
@@ -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
navigation2/tools/planner_benchmarking/100by100_20.pgm
BIN
View File
Binary file not shown.
navigation2/tools/planner_benchmarking/100by100_20.yaml
+6
View File
@@ -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
navigation2/tools/planner_benchmarking/README.md
+30
View File
@@ -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)
navigation2/tools/planner_benchmarking/metrics.py
+150
View File
@@ -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()
navigation2/tools/planner_benchmarking/planning_benchmark_bringup.py
+73
View File
@@ -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())
])
navigation2/tools/planner_benchmarking/process_data.py
+173
View File
@@ -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()
navigation2/tools/run_sanitizers
Executable
+41
View File
@@ -0,0 +1,41 @@
#!/bin/bash
# colcon sanitizer plugin instructions at
# https://github.com/colcon/colcon-sanitizer-reports/blob/master/README.rst
# To use this script, make sure you have the colcon plugins installed according
# to the instructions above. Then build ros2_ws and navstack_dependencies_ws.
# Source the navstack_dependencies_ws and then cd to the nav2_ws.
#
# Run this script by invoking navigation2/tools/run_sanitizers
# Afterwards, there should be two files in the root of the workspace:
# - sanitizer_report-asan.csv
# - sanitizer_report-tsan.csv
clean_workspace() {
rm -rf build install log
}
build_with_sanitizer() {
colcon build --build-base=build-$1 --install-base=install-$1 \
--cmake-args -DOSRF_TESTING_TOOLS_CPP_DISABLE_MEMORY_TOOLS=ON \
-DCMAKE_BUILD_TYPE=Debug \
--mixin $1 \
--symlink-install
}
test_with_sanitizer() {
colcon test --build-base=build-$1 --install-base=install-$1 --retest-until-pass 3 \
--event-handlers sanitizer_report+
}
clean_workspace
build_with_sanitizer asan-gcc
build_with_sanitizer tsan
test_with_sanitizer asan-gcc
mv sanitizer_report.csv sanitizer_report-asan.csv
test_with_sanitizer tsan
mv sanitizer_report.csv sanitizer_report-tsan.csv
navigation2/tools/run_test_suite.bash
Executable
+28
View File
@@ -0,0 +1,28 @@
#!/bin/bash
set -ex
SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" # gets the directory of this script
# Skip flaky tests. Nav2 system tests will be run later.
colcon test --packages-skip nav2_system_tests nav2_behaviors
# run the stable tests in nav2_behaviors
colcon test --packages-select nav2_behaviors --ctest-args --exclude-regex "test_recoveries"
# run the linters in nav2_system_tests. They only need to be run once.
colcon test --packages-select nav2_system_tests --ctest-args --exclude-regex "test_.*" # run the linters
# Each of the `colcon test` lines above runs tests on independent sets of packages.
# As a result the test logs of each line won't overwrite the others. The single
# call to `colcon test-result` will look through all packages and report any errors
# that happened in any of the `colcon test` lines above.
colcon test-result --verbose
# $SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_localization$
# $SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_planner_costmaps$
# $SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_planner_random$
# $SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_bt_navigator$
# $SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_bt_navigator_with_dijkstra$
$SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_dynamic_obstacle$
# $SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_multi_robot$
navigation2/tools/skip_keys.txt
+7
View File
@@ -0,0 +1,7 @@
console_bridge
fastcdr
fastrtps
libopensplice69
rti-connext-dds-5.3.1
slam_toolbox
urdfdom_headers
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
navigation2/tools/source.Dockerfile
+247
View File
@@ -0,0 +1,247 @@
# syntax=docker/dockerfile:experimental
# Use experimental buildkit for faster builds
# https://github.com/moby/buildkit/blob/master/frontend/dockerfile/docs/experimental.md
# Use `--progress=plain` to use plane stdout for docker build
#
# Example build command:
# This determines which version of the ROS2 code base to pull
# export ROS2_BRANCH=main
# export DOCKER_BUILDKIT=1
# docker build \
# --tag nav2:source \
# --file source.Dockerfile ../
#
# Use `--no-cache` to break the local docker build cache.
# Use `--pull` to pull the latest parent image from the remote registry.
# Use `--target=<stage_name>` to build stages not used for final stage.
#
# We're only building on top of a ros2 devel image to get the basics
# prerequisites installed such as the apt source, rosdep, etc. We don't want to
# actually use any of the ros release packages. Instead we are going to build
# everything from source in one big workspace.
ARG FROM_IMAGE=osrf/ros2:devel
ARG UNDERLAY_WS=/opt/underlay_ws
ARG OVERLAY_WS=/opt/overlay_ws
# multi-stage for caching
FROM $FROM_IMAGE AS cacher
# clone ros2 source
ARG ROS2_BRANCH=master
ARG ROS2_REPO=https://github.com/ros2/ros2.git
WORKDIR $ROS2_WS/src
RUN git clone $ROS2_REPO -b $ROS2_BRANCH && \
vcs import ./ < ros2/ros2.repos && \
find ./ -name ".git" | xargs rm -rf
# clone underlay source
ARG UNDERLAY_WS
WORKDIR $UNDERLAY_WS/src
COPY ./tools/underlay.repos ../
RUN vcs import ./ < ../underlay.repos && \
find ./ -name ".git" | xargs rm -rf
# copy overlay source
ARG OVERLAY_WS
WORKDIR $OVERLAY_WS/src
COPY ./ ./ros-planning/navigation2
RUN colcon list --names-only | cat >> /opt/packages.txt
# remove skiped packages
WORKDIR /opt
RUN find ./ \
-name "AMENT_IGNORE" -o \
-name "CATKIN_IGNORE" -o \
-name "COLCON_IGNORE" \
| xargs dirname | xargs rm -rf || true && \
colcon list --paths-only \
--packages-skip-up-to \
$(cat packages.txt | xargs) \
| xargs rm -rf
# copy manifests for caching
RUN mkdir -p /tmp/opt && \
find ./ -name "package.xml" | \
xargs cp --parents -t /tmp/opt
# multi-stage for ros2 dependencies
FROM $FROM_IMAGE AS ros2_depender
ARG DEBIAN_FRONTEND=noninteractive
# edit apt for caching
RUN cp /etc/apt/apt.conf.d/docker-clean /etc/apt/ && \
echo 'Binary::apt::APT::Keep-Downloaded-Packages "true";' \
> /etc/apt/apt.conf.d/docker-clean
# install packages
RUN --mount=type=cache,target=/var/cache/apt \
--mount=type=cache,target=/var/lib/apt \
apt-get update && apt-get install -q -y \
ccache \
libasio-dev \
libtinyxml2-dev \
lld \
&& rosdep update
ENV ROS_VERSION=2 \
ROS_PYTHON_VERSION=3
# install ros2 dependencies
WORKDIR $ROS2_WS
COPY --from=cacher /tmp/$ROS2_WS ./
COPY ./tools/skip_keys.txt /tmp/
RUN --mount=type=cache,target=/var/cache/apt \
--mount=type=cache,target=/var/lib/apt \
apt-get update && rosdep install -q -y \
--from-paths src \
--ignore-src \
--skip-keys " \
$(cat /tmp/skip_keys.txt | xargs) \
"
# multi-stage for building ros2
FROM ros2_depender AS ros2_builder
# build ros2 source
COPY --from=cacher $ROS2_WS ./
ARG ROS2_MIXINS="release ccache lld"
RUN --mount=type=cache,target=/root/.ccache \
colcon build \
--symlink-install \
--mixin $ROS2_MIXINS
# multi-stage for testing ros2
FROM ros2_builder AS ros2_tester
# test overlay build
ARG RUN_TESTS
ARG FAIL_ON_TEST_FAILURE
RUN if [ -n "$RUN_TESTS" ]; then \
. install/setup.sh && \
colcon test && \
colcon test-result \
|| ([ -z "$FAIL_ON_TEST_FAILURE" ] || exit 1) \
fi
# multi-stage for underlay dependencies
FROM ros2_depender AS underlay_depender
# copy manifests for caching
COPY --from=cacher /tmp/$ROS2_WS $ROS2_WS
# install underlay dependencies
ARG UNDERLAY_WS
WORKDIR $UNDERLAY_WS
COPY --from=cacher /tmp/$UNDERLAY_WS ./
RUN --mount=type=cache,target=/var/cache/apt \
--mount=type=cache,target=/var/lib/apt \
apt-get update && rosdep install -q -y \
--from-paths src \
$ROS2_WS/src \
--ignore-src \
--skip-keys " \
$(cat /tmp/skip_keys.txt | xargs) \
"
# multi-stage for building underlay
FROM underlay_depender AS underlay_builder
# copy workspace for caching
COPY --from=ros2_builder $ROS2_WS $ROS2_WS
# build underlay source
COPY --from=cacher $UNDERLAY_WS ./
ARG UNDERLAY_MIXINS="release ccache lld"
RUN --mount=type=cache,target=/root/.ccache \
. $ROS2_WS/install/setup.sh && \
colcon build \
--symlink-install \
--mixin $UNDERLAY_MIXINS
# multi-stage for testing underlay
FROM underlay_builder AS underlay_tester
# test overlay build
ARG RUN_TESTS
ARG FAIL_ON_TEST_FAILURE
RUN if [ -n "$RUN_TESTS" ]; then \
. install/setup.sh && \
colcon test && \
colcon test-result \
|| ([ -z "$FAIL_ON_TEST_FAILURE" ] || exit 1) \
fi
# multi-stage for overlay dependencies
FROM underlay_depender AS overlay_depender
# copy manifests for caching
COPY --from=cacher /tmp/$ROS2_WS $ROS2_WS
COPY --from=cacher /tmp/$UNDERLAY_WS $UNDERLAY_WS
# install overlay dependencies
ARG OVERLAY_WS
WORKDIR $OVERLAY_WS
COPY --from=cacher /tmp/$OVERLAY_WS ./
RUN --mount=type=cache,target=/var/cache/apt \
--mount=type=cache,target=/var/lib/apt \
apt-get update && rosdep install -q -y \
--from-paths src \
$ROS2_WS/src \
$UNDERLAY_WS/src \
--ignore-src \
--skip-keys " \
$(cat /tmp/skip_keys.txt | xargs) \
"
# multi-stage for building overlay
FROM overlay_depender AS overlay_builder
# copy workspace for caching
COPY --from=ros2_builder $ROS2_WS $ROS2_WS
COPY --from=underlay_builder $UNDERLAY_WS $UNDERLAY_WS
# build overlay source
COPY --from=cacher $OVERLAY_WS ./
ARG OVERLAY_MIXINS="release ccache lld"
RUN --mount=type=cache,target=/root/.ccache \
. $UNDERLAY_WS/install/setup.sh && \
colcon build \
--symlink-install \
--mixin $OVERLAY_MIXINS
# multi-stage for testing overlay
FROM overlay_builder AS overlay_tester
# test overlay build
ARG RUN_TESTS
ARG FAIL_ON_TEST_FAILURE
RUN if [ -n "$RUN_TESTS" ]; then \
. install/setup.sh && \
colcon test && \
colcon test-result \
|| ([ -z "$FAIL_ON_TEST_FAILURE" ] || exit 1) \
fi
# multi-stage for testing workspaces
FROM overlay_builder AS workspaces_tester
# copy workspace test results
COPY --from=ros2_tester $ROS2_WS/log $ROS2_WS/log
COPY --from=underlay_tester $UNDERLAY_WS/log $UNDERLAY_WS/log
COPY --from=overlay_tester $OVERLAY_WS/log $OVERLAY_WS/log
# multi-stage for shipping overlay
FROM overlay_builder AS overlay_shipper
# restore apt for docker
RUN mv /etc/apt/docker-clean /etc/apt/apt.conf.d/ && \
rm -rf /var/lib/apt/lists/
# source overlay from entrypoint
ENV UNDERLAY_WS $UNDERLAY_WS
ENV OVERLAY_WS $OVERLAY_WS
RUN sed --in-place \
's|^source .*|source "$OVERLAY_WS/install/setup.bash"|' \
/ros_entrypoint.sh
navigation2/tools/underlay.repos
+29
View File
@@ -0,0 +1,29 @@
repositories:
# BehaviorTree/BehaviorTree.CPP:
# type: git
# url: https://github.com/BehaviorTree/BehaviorTree.CPP.git
# version: master
# ros/angles:
# type: git
# url: https://github.com/ros/angles.git
# version: ros2
# ros-simulation/gazebo_ros_pkgs:
# type: git
# url: https://github.com/ros-simulation/gazebo_ros_pkgs.git
# version: ros2
# ros-perception/vision_opencv:
# type: git
# url: https://github.com/ros-perception/vision_opencv.git
# version: ros2
# ros/bond_core:
# type: git
# url: https://github.com/ros/bond_core.git
# version: ros2
# ompl/ompl:
# type: git
# url: https://github.com/ompl/ompl.git
# version: main
# ros-simulation/gazebo_ros_pkgs:
# type: git
# url: https://github.com/ros-simulation/gazebo_ros_pkgs.git
# version: ros2
navigation2/tools/update_bt_diagrams.bash
Executable
+14
View File
@@ -0,0 +1,14 @@
#!/bin/bash
# Run this from the root of the workspace to update these behavior_tree images
# in the doc directory of the nav2_bt_navigator package
navigation2/tools/bt2img.py \
--behavior_tree navigation2/nav2_bt_navigator/behavior_trees/navigate_w_replanning.xml \
--image_out navigation2/nav2_bt_navigator/doc/simple_parallel \
--legend navigation2/nav2_bt_navigator/doc/legend
navigation2/tools/bt2img.py \
--behavior_tree navigation2/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml \
--image_out navigation2/nav2_bt_navigator/doc/parallel_w_recovery
navigation2/tools/bt2img.py \
--behavior_tree navigation2/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml \
--image_out navigation2/nav2_bt_navigator/doc/parallel_through_poses_w_recovery