add humble-navigation2
This commit is contained in:
X-lanni
@@ -0,0 +1,17 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
|
||||
project(nav2_common NONE)
|
||||
|
||||
find_package(ament_cmake_core REQUIRED)
|
||||
find_package(ament_cmake_python REQUIRED)
|
||||
|
||||
ament_python_install_package(nav2_common)
|
||||
|
||||
ament_package(
|
||||
CONFIG_EXTRAS "nav2_common-extras.cmake"
|
||||
)
|
||||
|
||||
install(
|
||||
DIRECTORY cmake
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
@@ -0,0 +1,57 @@
|
||||
# Copyright 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.
|
||||
|
||||
#
|
||||
# Standard Nav2 project setup
|
||||
#
|
||||
# @public
|
||||
#
|
||||
macro(nav2_package)
|
||||
if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES)
|
||||
message(STATUS "Setting build type to Release as none was specified.")
|
||||
set(CMAKE_BUILD_TYPE "Release" CACHE
|
||||
STRING "Choose the type of build." FORCE)
|
||||
# Set the possible values of build type for cmake-gui
|
||||
set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS
|
||||
"Debug" "Release" "MinSizeRel" "RelWithDebInfo")
|
||||
endif()
|
||||
|
||||
# Default to C++14
|
||||
if(NOT CMAKE_CXX_STANDARD)
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
endif()
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wdeprecated -fPIC)
|
||||
endif()
|
||||
|
||||
option(COVERAGE_ENABLED "Enable code coverage" FALSE)
|
||||
if(COVERAGE_ENABLED)
|
||||
add_compile_options(--coverage)
|
||||
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} --coverage")
|
||||
set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} --coverage")
|
||||
endif()
|
||||
|
||||
# Defaults for Microsoft C++ compiler
|
||||
if(MSVC)
|
||||
# https://blog.kitware.com/create-dlls-on-windows-without-declspec-using-new-cmake-export-all-feature/
|
||||
set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
|
||||
|
||||
# Enable Math Constants
|
||||
# https://docs.microsoft.com/en-us/cpp/c-runtime-library/math-constants?view=vs-2019
|
||||
add_compile_definitions(
|
||||
_USE_MATH_DEFINES
|
||||
)
|
||||
endif()
|
||||
endmacro()
|
||||
@@ -0,0 +1,17 @@
|
||||
# Copyright 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.
|
||||
|
||||
set(AMENT_BUILD_CONFIGURATION_KEYWORD_SEPARATOR ":")
|
||||
|
||||
include("${nav2_common_DIR}/nav2_package.cmake")
|
||||
@@ -0,0 +1,18 @@
|
||||
# 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.
|
||||
|
||||
from .has_node_params import HasNodeParams
|
||||
from .rewritten_yaml import RewrittenYaml
|
||||
from .replace_string import ReplaceString
|
||||
from .parse_multirobot_pose import ParseMultiRobotPose
|
||||
@@ -0,0 +1,60 @@
|
||||
# Copyright (c) 2021 PAL Robotics S.L.
|
||||
#
|
||||
# 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 typing import List
|
||||
from typing import Text
|
||||
|
||||
import yaml
|
||||
import launch
|
||||
|
||||
import sys # delete this
|
||||
|
||||
class HasNodeParams(launch.Substitution):
|
||||
"""
|
||||
Substitution that checks if a param file contains parameters for a node
|
||||
|
||||
Used in launch system
|
||||
"""
|
||||
|
||||
def __init__(self,
|
||||
source_file: launch.SomeSubstitutionsType,
|
||||
node_name: Text) -> None:
|
||||
super().__init__()
|
||||
"""
|
||||
Construct the substitution
|
||||
|
||||
:param: source_file the parameter YAML file
|
||||
:param: node_name the name of the node to check
|
||||
"""
|
||||
|
||||
from launch.utilities import normalize_to_list_of_substitutions # import here to avoid loop
|
||||
self.__source_file = normalize_to_list_of_substitutions(source_file)
|
||||
self.__node_name = node_name
|
||||
|
||||
@property
|
||||
def name(self) -> List[launch.Substitution]:
|
||||
"""Getter for name."""
|
||||
return self.__source_file
|
||||
|
||||
def describe(self) -> Text:
|
||||
"""Return a description of this substitution as a string."""
|
||||
return ''
|
||||
|
||||
def perform(self, context: launch.LaunchContext) -> Text:
|
||||
yaml_filename = launch.utilities.perform_substitutions(context, self.name)
|
||||
data = yaml.safe_load(open(yaml_filename, 'r'))
|
||||
|
||||
if self.__node_name in data.keys():
|
||||
return "True"
|
||||
return "False"
|
||||
@@ -0,0 +1,82 @@
|
||||
# Copyright (c) 2023 LG Electronics.
|
||||
#
|
||||
# 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 yaml
|
||||
import sys
|
||||
from typing import Text, Dict
|
||||
|
||||
|
||||
class ParseMultiRobotPose():
|
||||
"""
|
||||
Parsing argument using sys module
|
||||
"""
|
||||
|
||||
def __init__(self, target_argument: Text):
|
||||
"""
|
||||
Parse arguments for multi-robot's pose
|
||||
|
||||
for example,
|
||||
`ros2 launch nav2_bringup bringup_multirobot_launch.py
|
||||
robots:="robot1={x: 1.0, y: 1.0, yaw: 0.0};
|
||||
robot2={x: 1.0, y: 1.0, z: 1.0, roll: 0.0, pitch: 1.5707, yaw: 1.5707}"`
|
||||
|
||||
`target_argument` shall be 'robots'.
|
||||
Then, this will parse a string value for `robots` argument.
|
||||
|
||||
Each robot name which is corresponding to namespace and pose of it will be separted by `;`.
|
||||
The pose consists of x, y and yaw with YAML format.
|
||||
|
||||
:param: target argument name to parse
|
||||
"""
|
||||
self.__args: Text = self.__parse_argument(target_argument)
|
||||
|
||||
def __parse_argument(self, target_argument: Text) -> Text:
|
||||
"""
|
||||
get value of target argument
|
||||
"""
|
||||
if len(sys.argv) > 4:
|
||||
argv = sys.argv[4:]
|
||||
for arg in argv:
|
||||
if arg.startswith(target_argument + ":="):
|
||||
return arg.replace(target_argument + ":=", "")
|
||||
return ""
|
||||
|
||||
def value(self) -> Dict:
|
||||
"""
|
||||
get value of target argument
|
||||
"""
|
||||
args = self.__args
|
||||
parsed_args = list() if len(args) == 0 else args.split(';')
|
||||
multirobots = dict()
|
||||
for arg in parsed_args:
|
||||
key_val = arg.strip().split('=')
|
||||
if len(key_val) != 2:
|
||||
continue
|
||||
key = key_val[0].strip()
|
||||
val = key_val[1].strip()
|
||||
robot_pose = yaml.safe_load(val)
|
||||
if 'x' not in robot_pose:
|
||||
robot_pose['x'] = 0.0
|
||||
if 'y' not in robot_pose:
|
||||
robot_pose['y'] = 0.0
|
||||
if 'z' not in robot_pose:
|
||||
robot_pose['z'] = 0.0
|
||||
if 'roll' not in robot_pose:
|
||||
robot_pose['roll'] = 0.0
|
||||
if 'pitch' not in robot_pose:
|
||||
robot_pose['pitch'] = 0.0
|
||||
if 'yaw' not in robot_pose:
|
||||
robot_pose['yaw'] = 0.0
|
||||
multirobots[key] = robot_pose
|
||||
return multirobots
|
||||
@@ -0,0 +1,87 @@
|
||||
# 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.
|
||||
|
||||
from typing import Dict
|
||||
from typing import List
|
||||
from typing import Text
|
||||
from typing import Optional
|
||||
import tempfile
|
||||
import launch
|
||||
|
||||
class ReplaceString(launch.Substitution):
|
||||
"""
|
||||
Substitution that replaces strings on a given file.
|
||||
|
||||
Used in launch system
|
||||
"""
|
||||
|
||||
def __init__(self,
|
||||
source_file: launch.SomeSubstitutionsType,
|
||||
replacements: Dict,
|
||||
condition: Optional[launch.Condition] = None) -> None:
|
||||
super().__init__()
|
||||
|
||||
from launch.utilities import normalize_to_list_of_substitutions # import here to avoid loop
|
||||
self.__source_file = normalize_to_list_of_substitutions(source_file)
|
||||
self.__replacements = {}
|
||||
for key in replacements:
|
||||
self.__replacements[key] = normalize_to_list_of_substitutions(replacements[key])
|
||||
self.__condition = condition
|
||||
|
||||
@property
|
||||
def name(self) -> List[launch.Substitution]:
|
||||
"""Getter for name."""
|
||||
return self.__source_file
|
||||
|
||||
@property
|
||||
def condition(self) -> Optional[launch.Condition]:
|
||||
"""Getter for condition."""
|
||||
return self.__condition
|
||||
|
||||
def describe(self) -> Text:
|
||||
"""Return a description of this substitution as a string."""
|
||||
return ''
|
||||
|
||||
def perform(self, context: launch.LaunchContext) -> Text:
|
||||
yaml_filename = launch.utilities.perform_substitutions(context, self.name)
|
||||
if self.__condition is None or self.__condition.evaluate(context):
|
||||
output_file = tempfile.NamedTemporaryFile(mode='w', delete=False)
|
||||
replacements = self.resolve_replacements(context)
|
||||
try:
|
||||
input_file = open(yaml_filename, 'r')
|
||||
self.replace(input_file, output_file, replacements)
|
||||
except Exception as err: # noqa: B902
|
||||
print('ReplaceString substitution error: ', err)
|
||||
finally:
|
||||
input_file.close()
|
||||
output_file.close()
|
||||
return output_file.name
|
||||
else:
|
||||
return yaml_filename
|
||||
|
||||
def resolve_replacements(self, context):
|
||||
resolved_replacements = {}
|
||||
for key in self.__replacements:
|
||||
resolved_replacements[key] = launch.utilities.perform_substitutions(context, self.__replacements[key])
|
||||
return resolved_replacements
|
||||
|
||||
def replace(self, input_file, output_file, replacements):
|
||||
for line in input_file:
|
||||
for key, value in replacements.items():
|
||||
if isinstance(key, str) and isinstance(value, str):
|
||||
if key in line:
|
||||
line = line.replace(key, value)
|
||||
else:
|
||||
raise TypeError('A provided replacement pair is not a string. Both key and value should be strings.')
|
||||
output_file.write(line)
|
||||
@@ -0,0 +1,190 @@
|
||||
# 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.
|
||||
|
||||
from typing import Dict
|
||||
from typing import List
|
||||
from typing import Text
|
||||
from typing import Optional
|
||||
|
||||
import yaml
|
||||
import tempfile
|
||||
import launch
|
||||
|
||||
|
||||
class DictItemReference:
|
||||
def __init__(self, dictionary, key):
|
||||
self.dictionary = dictionary
|
||||
self.dictKey = key
|
||||
|
||||
def key(self):
|
||||
return self.dictKey
|
||||
|
||||
def setValue(self, value):
|
||||
self.dictionary[self.dictKey] = value
|
||||
|
||||
|
||||
class RewrittenYaml(launch.Substitution):
|
||||
"""
|
||||
Substitution that modifies the given YAML file.
|
||||
|
||||
Used in launch system
|
||||
"""
|
||||
|
||||
def __init__(self,
|
||||
source_file: launch.SomeSubstitutionsType,
|
||||
param_rewrites: Dict,
|
||||
root_key: Optional[launch.SomeSubstitutionsType] = None,
|
||||
key_rewrites: Optional[Dict] = None,
|
||||
convert_types = False) -> None:
|
||||
super().__init__()
|
||||
"""
|
||||
Construct the substitution
|
||||
|
||||
:param: source_file the original YAML file to modify
|
||||
:param: param_rewrites mappings to replace
|
||||
:param: root_key if provided, the contents are placed under this key
|
||||
:param: key_rewrites keys of mappings to replace
|
||||
:param: convert_types whether to attempt converting the string to a number or boolean
|
||||
"""
|
||||
|
||||
from launch.utilities import normalize_to_list_of_substitutions # import here to avoid loop
|
||||
self.__source_file = normalize_to_list_of_substitutions(source_file)
|
||||
self.__param_rewrites = {}
|
||||
self.__key_rewrites = {}
|
||||
self.__convert_types = convert_types
|
||||
self.__root_key = None
|
||||
for key in param_rewrites:
|
||||
self.__param_rewrites[key] = normalize_to_list_of_substitutions(param_rewrites[key])
|
||||
if key_rewrites is not None:
|
||||
for key in key_rewrites:
|
||||
self.__key_rewrites[key] = normalize_to_list_of_substitutions(key_rewrites[key])
|
||||
if root_key is not None:
|
||||
self.__root_key = normalize_to_list_of_substitutions(root_key)
|
||||
|
||||
@property
|
||||
def name(self) -> List[launch.Substitution]:
|
||||
"""Getter for name."""
|
||||
return self.__source_file
|
||||
|
||||
def describe(self) -> Text:
|
||||
"""Return a description of this substitution as a string."""
|
||||
return ''
|
||||
|
||||
def perform(self, context: launch.LaunchContext) -> Text:
|
||||
yaml_filename = launch.utilities.perform_substitutions(context, self.name)
|
||||
rewritten_yaml = tempfile.NamedTemporaryFile(mode='w', delete=False)
|
||||
param_rewrites, keys_rewrites = self.resolve_rewrites(context)
|
||||
data = yaml.safe_load(open(yaml_filename, 'r'))
|
||||
self.substitute_params(data, param_rewrites)
|
||||
self.substitute_keys(data, keys_rewrites)
|
||||
if self.__root_key is not None:
|
||||
root_key = launch.utilities.perform_substitutions(context, self.__root_key)
|
||||
if root_key:
|
||||
data = {root_key: data}
|
||||
yaml.dump(data, rewritten_yaml)
|
||||
rewritten_yaml.close()
|
||||
return rewritten_yaml.name
|
||||
|
||||
def resolve_rewrites(self, context):
|
||||
resolved_params = {}
|
||||
for key in self.__param_rewrites:
|
||||
resolved_params[key] = launch.utilities.perform_substitutions(context, self.__param_rewrites[key])
|
||||
resolved_keys = {}
|
||||
for key in self.__key_rewrites:
|
||||
resolved_keys[key] = launch.utilities.perform_substitutions(context, self.__key_rewrites[key])
|
||||
return resolved_params, resolved_keys
|
||||
|
||||
def substitute_params(self, yaml, param_rewrites):
|
||||
# substitute leaf-only parameters
|
||||
for key in self.getYamlLeafKeys(yaml):
|
||||
if key.key() in param_rewrites:
|
||||
raw_value = param_rewrites[key.key()]
|
||||
key.setValue(self.convert(raw_value))
|
||||
|
||||
# substitute total path parameters
|
||||
yaml_paths = self.pathify(yaml)
|
||||
for path in yaml_paths:
|
||||
if path in param_rewrites:
|
||||
# this is an absolute path (ex. 'key.keyA.keyB.val')
|
||||
rewrite_val = self.convert(param_rewrites[path])
|
||||
yaml_keys = path.split('.')
|
||||
yaml = self.updateYamlPathVals(yaml, yaml_keys, rewrite_val)
|
||||
|
||||
|
||||
def updateYamlPathVals(self, yaml, yaml_key_list, rewrite_val):
|
||||
for key in yaml_key_list:
|
||||
if key == yaml_key_list[-1]:
|
||||
yaml[key] = rewrite_val
|
||||
break
|
||||
key = yaml_key_list.pop(0)
|
||||
if isinstance(yaml, list):
|
||||
yaml[int(key)] = self.updateYamlPathVals(yaml[int(key)], yaml_key_list, rewrite_val)
|
||||
else:
|
||||
yaml[key] = self.updateYamlPathVals(yaml.get(key, {}), yaml_key_list, rewrite_val)
|
||||
return yaml
|
||||
|
||||
def substitute_keys(self, yaml, key_rewrites):
|
||||
if len(key_rewrites) != 0:
|
||||
for key in list(yaml.keys()):
|
||||
val = yaml[key]
|
||||
if key in key_rewrites:
|
||||
new_key = key_rewrites[key]
|
||||
yaml[new_key] = yaml[key]
|
||||
del yaml[key]
|
||||
if isinstance(val, dict):
|
||||
self.substitute_keys(val, key_rewrites)
|
||||
|
||||
def getYamlLeafKeys(self, yamlData):
|
||||
try:
|
||||
for key in yamlData.keys():
|
||||
for k in self.getYamlLeafKeys(yamlData[key]):
|
||||
yield k
|
||||
yield DictItemReference(yamlData, key)
|
||||
except AttributeError:
|
||||
return
|
||||
|
||||
def pathify(self, d, p=None, paths=None, joinchar='.'):
|
||||
if p is None:
|
||||
paths = {}
|
||||
self.pathify(d, "", paths, joinchar=joinchar)
|
||||
return paths
|
||||
pn = p
|
||||
if p != "":
|
||||
pn += joinchar
|
||||
if isinstance(d, dict):
|
||||
for k in d:
|
||||
v = d[k]
|
||||
self.pathify(v, str(pn) + str(k), paths, joinchar=joinchar)
|
||||
elif isinstance(d, list):
|
||||
for idx, e in enumerate(d):
|
||||
self.pathify(e, pn + str(idx), paths, joinchar=joinchar)
|
||||
else:
|
||||
paths[p] = d
|
||||
|
||||
def convert(self, text_value):
|
||||
if self.__convert_types:
|
||||
# try converting to int or float
|
||||
try:
|
||||
return float(text_value) if '.' in text_value else int(text_value)
|
||||
except ValueError:
|
||||
pass
|
||||
|
||||
# try converting to bool
|
||||
if text_value.lower() == "true":
|
||||
return True
|
||||
if text_value.lower() == "false":
|
||||
return False
|
||||
|
||||
# nothing else worked so fall through and return text
|
||||
return text_value
|
||||
@@ -0,0 +1,25 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>nav2_common</name>
|
||||
<version>1.1.18</version>
|
||||
<description>Common support functionality used throughout the navigation 2 stack</description>
|
||||
<maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<depend>launch</depend>
|
||||
<depend>launch_ros</depend>
|
||||
<depend>osrf_pycommon</depend>
|
||||
<depend>rclpy</depend>
|
||||
<depend>python3-yaml</depend>
|
||||
|
||||
<buildtool_depend>ament_cmake_core</buildtool_depend>
|
||||
|
||||
<build_depend>ament_cmake_python</build_depend>
|
||||
|
||||
<buildtool_export_depend>ament_cmake_core</buildtool_export_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
Reference in New Issue
Block a user