Переработана вся структура запуска файлов. Теперь в iiwa_bringup хранятся только launch файлы.
Все конфигурационные файлы переехали в пакет `iiwa_config`. также там появился общий конфигурационный файл, который упращает запуск `digital_twin.launch.py` Добавлен дополнительный пакет `iiwa_utils`, в который переехал весь код с папки `iiwa_bringup/utils`, а также все тестовые ноды переехали с `iiwa_object_spawner` в этот пакет Пакеты `iiwa_bringup` и `iiwa_description` переведены на `ament_cmake` для дальнейшего удобсва - пакеты не будут содержать код для реализации
This commit is contained in:
Даниил Грабарь
@@ -1,6 +1,18 @@
|
||||
Подмена файла по пути обязательна: `/opt/ros/rolling/lib/webots_ros2_driver/ros2_supervisor.py`
|
||||
## Описание пакетов
|
||||
|
||||
> необходимо `warn` заменить на `warning` в логере
|
||||
|Наименование|описание|
|
||||
|---|---|
|
||||
|iiwa_bringup|Все необходимые файлы запуска расположены в этом пакете|
|
||||
|iiwa_config|Файлы конфигурации и основной `setting.yam` файл расположены внутри этого пакета|
|
||||
|iiwa_controller|Самописный контроллер на физического робота|
|
||||
|iiwa_description|urdf/xacro файлы, а также все 3D объекты и webots миры находятся в этом пакете|
|
||||
|iiwa_utils|Вспомогательные модули или функции для рабоы всей системы|
|
||||
|
||||
|
||||
|
||||
> Вроде уже не обязательно
|
||||
Подмена файла по пути обязательна: `/opt/ros/rolling/lib/webots_ros2_driver/ros2_supervisor.py`
|
||||
необходимо `warn` заменить на `warning` в логере
|
||||
|
||||
```bash
|
||||
|
||||
|
||||
@@ -0,0 +1,15 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(iiwa_bringup)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
install(
|
||||
DIRECTORY launch
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_package()
|
||||
@@ -15,45 +15,22 @@ from launch_ros.actions import Node
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
from moveit_configs_utils import MoveItConfigsBuilder
|
||||
|
||||
from iiwa_bringup.utils import converter
|
||||
|
||||
|
||||
PACKAGE = "iiwa_bringup"
|
||||
DESCRIPTION_PKG = "iiwa_description"
|
||||
|
||||
|
||||
def _arg(name: str, default, desc: str):
|
||||
"""Short helper to declare a launch argument."""
|
||||
return DeclareLaunchArgument(name=name, default_value=default, description=desc)
|
||||
|
||||
|
||||
def _share_file(pkg_arg_name: str, *relpath: str):
|
||||
"""Join <share>/<relpath...> where <share> is FindPackageShare(LaunchConfiguration(pkg_arg_name))."""
|
||||
return PathJoinSubstitution([FindPackageShare(LaunchConfiguration(pkg_arg_name)), *relpath])
|
||||
from iiwa_utils import converter, setting_loader
|
||||
|
||||
|
||||
def _runtime_setup(context, *args, **kwatgs):
|
||||
setup = []
|
||||
|
||||
robot_name = LaunchConfiguration("robot_name").perform(context)
|
||||
world_path = LaunchConfiguration("world").perform(context)
|
||||
transform = LaunchConfiguration("transform").perform(context)
|
||||
rotation = LaunchConfiguration("rotation").perform(context)
|
||||
timer = LaunchConfiguration("controller_timer").perform(context)
|
||||
|
||||
xacro_file = LaunchConfiguration("xacro_file").perform(context)
|
||||
srdf_file = LaunchConfiguration("srdf_file").perform(context)
|
||||
|
||||
# ros2_controllers_file = LaunchConfiguration("controller").perform(context)
|
||||
initial_positions_file = LaunchConfiguration("initial_positions_file").perform(context)
|
||||
|
||||
kinematics_yaml = LaunchConfiguration("kinematics_yaml").perform(context)
|
||||
joint_limits_yaml = LaunchConfiguration("joint_limits_yaml").perform(context)
|
||||
pilz_limits_yaml = LaunchConfiguration("pilz_limits_yaml").perform(context)
|
||||
moveit_controllers_yaml = LaunchConfiguration("moveit_controllers_yaml").perform(context)
|
||||
settings = setting_loader.build_settings(
|
||||
settings_path=LaunchConfiguration("setting").perform(context), check_files=True
|
||||
)
|
||||
|
||||
robot_description = converter.load_robot_description(
|
||||
model_path=xacro_file, robot_name=robot_name
|
||||
model_path=settings.digital_twin.description,
|
||||
robot_name=settings.robot.name,
|
||||
xacro_args={
|
||||
"initial_positions_file": settings.controller.moveit.initial_positions
|
||||
},
|
||||
)
|
||||
|
||||
rsp_node = Node(
|
||||
@@ -61,15 +38,14 @@ def _runtime_setup(context, *args, **kwatgs):
|
||||
executable="robot_state_publisher",
|
||||
name="robot_state_publisher",
|
||||
output="screen",
|
||||
parameters=[{"robot_description": robot_description,
|
||||
"use_sim_time": False}],
|
||||
parameters=[{"robot_description": robot_description, "use_sim_time": False}],
|
||||
)
|
||||
|
||||
webots_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
PathJoinSubstitution(
|
||||
[
|
||||
FindPackageShare(PACKAGE),
|
||||
FindPackageShare("iiwa_bringup"),
|
||||
"launch",
|
||||
"supported",
|
||||
"webots_spawn.launch.py",
|
||||
@@ -77,26 +53,31 @@ def _runtime_setup(context, *args, **kwatgs):
|
||||
)
|
||||
),
|
||||
launch_arguments={
|
||||
"robot_name": robot_name,
|
||||
"xacro_file": xacro_file,
|
||||
"world": world_path,
|
||||
"transform": transform,
|
||||
"rotation": rotation,
|
||||
"controller_timer": timer,
|
||||
"robot_name": str(settings.robot.name),
|
||||
"description": str(settings.digital_twin.description),
|
||||
"world": str(settings.digital_twin.webots.world),
|
||||
"transform": str(settings.digital_twin.webots.transform),
|
||||
"rotation": str(settings.digital_twin.webots.rotation),
|
||||
"controller_timer": str(settings.digital_twin.webots.controller_timer),
|
||||
"controller": str(settings.controller.controller_path),
|
||||
"initial_positions_file": str(settings.controller.moveit.initial_positions),
|
||||
}.items(),
|
||||
)
|
||||
|
||||
moveit_configs = (
|
||||
MoveItConfigsBuilder("iiwa7", package_name=LaunchConfiguration("config_pkg").perform(context))
|
||||
.robot_description(file_path=xacro_file, mappings={
|
||||
"initial_positions_file": initial_positions_file,
|
||||
})
|
||||
.robot_description_semantic(file_path=srdf_file)
|
||||
.robot_description_kinematics(file_path=kinematics_yaml)
|
||||
.joint_limits(file_path=joint_limits_yaml)
|
||||
.pilz_cartesian_limits(file_path=pilz_limits_yaml)
|
||||
.trajectory_execution(file_path=moveit_controllers_yaml)
|
||||
.moveit_cpp(file_path="/home/daniel/dev/ros2_iiwa7/src/iiwa_bringup/config/motion_planing.yaml") # TODO: сделать подстановочным значением
|
||||
MoveItConfigsBuilder("iiwa7", package_name="iiwa_config")
|
||||
.robot_description(
|
||||
file_path=settings.digital_twin.description,
|
||||
mappings={
|
||||
"initial_positions_file": settings.controller.moveit.initial_positions
|
||||
},
|
||||
)
|
||||
.robot_description_semantic(file_path=settings.controller.moveit.srdf)
|
||||
.robot_description_kinematics(file_path=settings.controller.moveit.kinematics)
|
||||
.joint_limits(file_path=settings.controller.moveit.joint_limits)
|
||||
.pilz_cartesian_limits(file_path=settings.controller.moveit.pilz_limits)
|
||||
.trajectory_execution(file_path=settings.controller.moveit.moveit_controllers)
|
||||
.moveit_cpp(file_path=settings.controller.moveit.moveit_cpp)
|
||||
.to_moveit_configs()
|
||||
)
|
||||
|
||||
@@ -106,29 +87,25 @@ def _runtime_setup(context, *args, **kwatgs):
|
||||
output="screen",
|
||||
parameters=[
|
||||
moveit_configs.to_dict(),
|
||||
robot_description,
|
||||
{"robot_description": robot_description},
|
||||
],
|
||||
)
|
||||
|
||||
# TODO: не забудь поменять правильное название и имя пакета
|
||||
moveit_py_node = Node(
|
||||
name="moveit_py",
|
||||
package="iiwa_object_spawner",
|
||||
package="iiwa_utils",
|
||||
executable="motion_planning_test",
|
||||
output="both",
|
||||
parameters=[moveit_configs.to_dict()],
|
||||
)
|
||||
|
||||
rviz_config = PathJoinSubstitution(
|
||||
[FindPackageShare(PACKAGE), "config", "rviz_iiwa.rviz"]
|
||||
)
|
||||
|
||||
rviz_launch = Node(
|
||||
condition=IfCondition(LaunchConfiguration("rviz")),
|
||||
package="rviz2",
|
||||
executable="rviz2",
|
||||
name="rviz2",
|
||||
arguments=["-d", rviz_config],
|
||||
arguments=["-d", settings.digital_twin.rviz.config],
|
||||
output="log",
|
||||
parameters=[
|
||||
moveit_configs.robot_description,
|
||||
@@ -140,133 +117,42 @@ def _runtime_setup(context, *args, **kwatgs):
|
||||
)
|
||||
|
||||
shutdown_on_rviz_exit = RegisterEventHandler(
|
||||
OnProcessExit(target_action=rviz_launch,
|
||||
on_exit=[EmitEvent(event=Shutdown())])
|
||||
OnProcessExit(target_action=rviz_launch, on_exit=[EmitEvent(event=Shutdown())])
|
||||
)
|
||||
|
||||
setup += [rsp_node,
|
||||
webots_launch,
|
||||
move_group,
|
||||
moveit_py_node,
|
||||
rviz_launch,
|
||||
shutdown_on_rviz_exit
|
||||
]
|
||||
setup += [
|
||||
rsp_node,
|
||||
webots_launch,
|
||||
move_group,
|
||||
moveit_py_node,
|
||||
rviz_launch,
|
||||
shutdown_on_rviz_exit,
|
||||
]
|
||||
|
||||
return setup
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
# Объявление аргументов командной строки
|
||||
declare_description_pkg = _arg(
|
||||
"description_pkg",
|
||||
DESCRIPTION_PKG,
|
||||
"Package containing URDF/Xacro (and optionally SRDF/worlds).",
|
||||
)
|
||||
declare_config_pkg = _arg(
|
||||
"config_pkg",
|
||||
PACKAGE,
|
||||
"Package containing MoveIt/ros2_control config YAML files (config/*).",
|
||||
declare_rviz = DeclareLaunchArgument(
|
||||
name="rviz",
|
||||
default_value="0",
|
||||
description="If true|1|yes then launch RViz/MoveIt branch (instead of controllers branch)",
|
||||
)
|
||||
|
||||
declare_robot_name = _arg(
|
||||
"robot_name",
|
||||
"iiwa7",
|
||||
"Robot name (used for TF and naming).",
|
||||
declacre_setting = DeclareLaunchArgument(
|
||||
name="setting",
|
||||
default_value=PathJoinSubstitution(
|
||||
[FindPackageShare("iiwa_config"), "config", "setting.yaml"]
|
||||
),
|
||||
description="Absolute path to settings file",
|
||||
)
|
||||
declare_world = _arg(
|
||||
"world",
|
||||
_share_file("description_pkg", "worlds", "iiwa.wbt"),
|
||||
"Path to Webots world (.wbt).",
|
||||
)
|
||||
declare_controller = _arg(
|
||||
"controller",
|
||||
_share_file("config_pkg", "config", "iiwa_controller.yaml"),
|
||||
"Controllers YAML (spawner/controller_manager).",
|
||||
)
|
||||
declare_transform = _arg(
|
||||
"transform",
|
||||
"-0.25 0 0.79",
|
||||
"Spawn translation in Webots (x y z).",
|
||||
)
|
||||
declare_rotation = _arg(
|
||||
"rotation",
|
||||
"0 0 1 0",
|
||||
"Spawn rotation axis-angle in Webots.",
|
||||
)
|
||||
declare_rviz = _arg(
|
||||
"rviz",
|
||||
"0",
|
||||
"If true|1|yes then launch RViz/MoveIt branch (instead of controllers branch).",
|
||||
)
|
||||
declare_controller_timer = _arg(
|
||||
"controller_timer",
|
||||
"50",
|
||||
"Timeout (seconds) for controller_manager spawners.",
|
||||
)
|
||||
|
||||
# URDF/SRDF
|
||||
declare_xacro_file = _arg(
|
||||
"xacro_file",
|
||||
_share_file("description_pkg", "urdf", "iiwa7.urdf.xacro"),
|
||||
"Xacro used by MoveIt robot_description.",
|
||||
)
|
||||
declare_srdf_file = _arg(
|
||||
"srdf_file",
|
||||
_share_file("config_pkg", "config", "iiwa7.srdf"),
|
||||
"SRDF path.",
|
||||
)
|
||||
|
||||
# YAML конфиги
|
||||
declare_initial_positions = _arg(
|
||||
"initial_positions_file",
|
||||
_share_file("config_pkg", "config", "initial_positions.yaml"),
|
||||
"initial_positions.yaml passed into xacro arg initial_positions_file.",
|
||||
)
|
||||
declare_kinematics_yaml = _arg(
|
||||
"kinematics_yaml",
|
||||
_share_file("config_pkg", "config", "kinematics.yaml"),
|
||||
"MoveIt kinematics.yaml",
|
||||
)
|
||||
declare_joint_limits_yaml = _arg(
|
||||
"joint_limits_yaml",
|
||||
_share_file("config_pkg", "config", "joint_limits.yaml"),
|
||||
"MoveIt joint_limits.yaml",
|
||||
)
|
||||
declare_pilz_limits_yaml = _arg(
|
||||
"pilz_limits_yaml",
|
||||
_share_file("config_pkg", "config", "pilz_cartesian_limits.yaml"),
|
||||
"Pilz cartesian limits yaml.",
|
||||
)
|
||||
declare_moveit_controllers_yaml = _arg(
|
||||
"moveit_controllers_yaml",
|
||||
_share_file("config_pkg", "config", "moveit_controllers.yaml"),
|
||||
"MoveIt controllers (trajectory_execution / simple_controller_manager).",
|
||||
)
|
||||
|
||||
|
||||
runtime_setup = OpaqueFunction(function=_runtime_setup)
|
||||
|
||||
return LaunchDescription(
|
||||
[
|
||||
declare_description_pkg,
|
||||
declare_config_pkg,
|
||||
|
||||
declare_robot_name,
|
||||
declare_world,
|
||||
declare_controller,
|
||||
declare_transform,
|
||||
declare_rotation,
|
||||
declare_rviz,
|
||||
declare_controller_timer,
|
||||
|
||||
declare_xacro_file,
|
||||
declare_srdf_file,
|
||||
declare_initial_positions,
|
||||
declare_kinematics_yaml,
|
||||
declare_joint_limits_yaml,
|
||||
declare_pilz_limits_yaml,
|
||||
declare_moveit_controllers_yaml,
|
||||
|
||||
declacre_setting,
|
||||
runtime_setup,
|
||||
]
|
||||
)
|
||||
|
||||
@@ -4,23 +4,26 @@ from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
from webots_ros2_driver.urdf_spawner import URDFSpawner
|
||||
|
||||
|
||||
from iiwa_bringup.utils import converter
|
||||
from iiwa_utils import converter
|
||||
|
||||
|
||||
def _setup_controllers(context, *args, **kwargs):
|
||||
xacro_file = LaunchConfiguration("xacro_file").perform(context)
|
||||
robot_name = LaunchConfiguration("robot_name").perform(context)
|
||||
description = LaunchConfiguration("description").perform(context)
|
||||
transform = LaunchConfiguration("transform").perform(context)
|
||||
rotation = LaunchConfiguration("rotation").perform(context)
|
||||
timer = LaunchConfiguration("controller_timer").perform(context)
|
||||
|
||||
robot_description = converter.load_robot_description(
|
||||
model_path=xacro_file, robot_name=robot_name
|
||||
controller_timer = LaunchConfiguration("controller_timer").perform(context)
|
||||
initial_positions_file = LaunchConfiguration("initial_positions_file").perform(
|
||||
context
|
||||
)
|
||||
|
||||
tmo = ["--controller-manager-timeout", str(timer)]
|
||||
robot_description = converter.load_robot_description(
|
||||
model_path=description,
|
||||
robot_name=robot_name,
|
||||
xacro_args={"initial_positions_file": initial_positions_file},
|
||||
)
|
||||
|
||||
tmo = ["--controller-manager-timeout", str(controller_timer)]
|
||||
|
||||
jsb = Node(
|
||||
package="controller_manager",
|
||||
|
||||
@@ -16,24 +16,28 @@ from webots_ros2_driver.webots_launcher import WebotsLauncher
|
||||
|
||||
|
||||
def _spawn_setup(context, *args, **kwargs):
|
||||
xacro_file = LaunchConfiguration("xacro_file").perform(context)
|
||||
robot_name = LaunchConfiguration("robot_name").perform(context)
|
||||
world_path = LaunchConfiguration("world").perform(context)
|
||||
description = LaunchConfiguration("description").perform(context)
|
||||
world = LaunchConfiguration("world").perform(context)
|
||||
transform = LaunchConfiguration("transform").perform(context)
|
||||
rotation = LaunchConfiguration("rotation").perform(context)
|
||||
timer = LaunchConfiguration("controller_timer").perform(context)
|
||||
controller_timer = LaunchConfiguration("controller_timer").perform(context)
|
||||
controller = LaunchConfiguration("controller").perform(context)
|
||||
initial_positions_file = LaunchConfiguration("initial_positions_file").perform(
|
||||
context
|
||||
)
|
||||
|
||||
webots = WebotsLauncher(world=world_path, ros2_supervisor=True)
|
||||
webots = WebotsLauncher(world=world, ros2_supervisor=True)
|
||||
|
||||
driver = WebotsController(
|
||||
robot_name=robot_name,
|
||||
parameters=[
|
||||
{
|
||||
"robot_description": xacro_file,
|
||||
"robot_description": description,
|
||||
"use_sim_time": False,
|
||||
"set_robot_state_publisher": False,
|
||||
},
|
||||
LaunchConfiguration("controller").perform(context),
|
||||
controller,
|
||||
],
|
||||
respawn=True,
|
||||
)
|
||||
@@ -51,10 +55,11 @@ def _spawn_setup(context, *args, **kwargs):
|
||||
),
|
||||
launch_arguments={
|
||||
"robot_name": robot_name,
|
||||
"xacro_file": xacro_file,
|
||||
"description": description,
|
||||
"transform": transform,
|
||||
"rotation": rotation,
|
||||
"controller_timer": timer,
|
||||
"controller_timer": controller_timer,
|
||||
"initial_positions_file": initial_positions_file,
|
||||
}.items(),
|
||||
)
|
||||
|
||||
|
||||
@@ -2,18 +2,18 @@
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>iiwa_bringup</name>
|
||||
<version>0.0.1</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="grabardm@ml-dev.ru">Grabar Daniil</maintainer>
|
||||
<version>0.2.0</version>
|
||||
<description>Файлы запуска проектов</description>
|
||||
<maintainer email="grabardm@ml-dev.ru">daniel</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>ament_xmllint</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<exec_depend>iiwa_utils</exec_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
|
||||
@@ -1,4 +0,0 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/iiwa_bringup
|
||||
[install]
|
||||
install_scripts=$base/lib/iiwa_bringup
|
||||
@@ -1,58 +0,0 @@
|
||||
import os
|
||||
|
||||
from setuptools import setup
|
||||
|
||||
package_name = "iiwa_bringup"
|
||||
|
||||
# Собираем список пакетов: основной + вложенные (если они есть).
|
||||
packages = [package_name]
|
||||
# Если есть подпапка utils с __init__.py — зарегистрируем её как iiwa_bringup.utils
|
||||
if os.path.isdir("utils") and os.path.isfile(os.path.join("utils", "__init__.py")):
|
||||
packages.append(f"{package_name}.utils")
|
||||
|
||||
# Соответствие имени пакета -> директория на диске.
|
||||
# iiwa_bringup -> текущая папка '.'
|
||||
# iiwa_bringup.utils -> ./utils
|
||||
package_dir = {
|
||||
package_name: ".",
|
||||
}
|
||||
if f"{package_name}.utils" in packages:
|
||||
package_dir[f"{package_name}.utils"] = os.path.join(".", "utils")
|
||||
|
||||
|
||||
def data_files_from_tree(src_dir: str, dst_root: str) -> list:
|
||||
entries = []
|
||||
if not os.path.isdir(src_dir):
|
||||
return entries
|
||||
EXCLUDE_DIRS = {".git", "__pycache__", ".pytest_cache", ".idea"}
|
||||
EXCLUDES = {".DS_Store", "Thumbs.db"}
|
||||
for root, dirs, files in os.walk(src_dir):
|
||||
dirs[:] = [d for d in dirs if d not in EXCLUDE_DIRS]
|
||||
file_list = [os.path.join(root, f) for f in files if f not in EXCLUDES]
|
||||
if not file_list:
|
||||
continue
|
||||
rel = os.path.relpath(root, src_dir)
|
||||
dst_dir = os.path.join(dst_root, rel) if rel != "." else dst_root
|
||||
entries.append((dst_dir, file_list))
|
||||
return entries
|
||||
|
||||
|
||||
data_files = [(f"share/{package_name}", ["package.xml"])]
|
||||
data_files += data_files_from_tree("launch", f"share/{package_name}/launch")
|
||||
data_files += data_files_from_tree("config", f"share/{package_name}/config")
|
||||
data_files += data_files_from_tree("resource", f"share/{package_name}/resource")
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version="0.0.1",
|
||||
packages=packages,
|
||||
package_dir=package_dir,
|
||||
include_package_data=True,
|
||||
data_files=data_files,
|
||||
install_requires=["setuptools"],
|
||||
zip_safe=False,
|
||||
maintainer="Grabar Daniil",
|
||||
maintainer_email="grabardm@ml-dev.ru",
|
||||
description="iiwa bringup package",
|
||||
license="Apache-2.0",
|
||||
)
|
||||
@@ -1,12 +0,0 @@
|
||||
import xacro
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
def load_robot_description(model_path: Path, robot_name: str) -> str:
|
||||
suffix = Path(model_path).suffix.lower()
|
||||
if suffix == ".xacro":
|
||||
return xacro.process_file(model_path, mappings={'name': str(robot_name)}).toxml()
|
||||
elif suffix == ".urdf":
|
||||
return Path(model_path).read_text(encoding="utf-8")
|
||||
else:
|
||||
raise FileNotFoundError("Supported file formats: xacro/urdf")
|
||||
@@ -0,0 +1,17 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(iiwa_config)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
install(
|
||||
DIRECTORY config
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
|
||||
ament_package()
|
||||
+5
-5
@@ -271,7 +271,7 @@ Visualization Manager:
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz_default_plugins/Orbit
|
||||
Distance: 3.753772020339966
|
||||
Distance: 3.119210720062256
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
@@ -286,10 +286,10 @@ Visualization Manager:
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 0.6753981113433838
|
||||
Pitch: 0.570398211479187
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 5.888577938079834
|
||||
Yaw: 5.948578357696533
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
"":
|
||||
@@ -311,5 +311,5 @@ Window Geometry:
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1200
|
||||
X: 414
|
||||
Y: 160
|
||||
X: 60
|
||||
Y: 60
|
||||
@@ -0,0 +1,29 @@
|
||||
robot:
|
||||
name: "iiwa7"
|
||||
ip: "192.168.21.144"
|
||||
port: 3000
|
||||
|
||||
digital_twin:
|
||||
webots:
|
||||
world: pkg://iiwa_description/worlds/iiwa.wbt
|
||||
transform: "-0.25 0 0.79"
|
||||
rotation: "0 0 1 0"
|
||||
controller_timer: "50"
|
||||
|
||||
rviz:
|
||||
config: pkg://iiwa_config/config/rviz/rviz_moveit.rviz
|
||||
|
||||
description: pkg://iiwa_description/urdf/iiwa7.urdf.xacro
|
||||
|
||||
controller:
|
||||
controller_path: pkg://iiwa_config/config/moveit/iiwa_controller.yaml
|
||||
|
||||
moveit:
|
||||
srdf: pkg://iiwa_config/config/moveit/iiwa7.srdf
|
||||
kinematics: pkg://iiwa_config/config/moveit/kinematics.yaml
|
||||
joint_limits: pkg://iiwa_config/config/moveit/joint_limits.yaml
|
||||
pilz_limits: pkg://iiwa_config/config/moveit/pilz_cartesian_limits.yaml
|
||||
initial_positions: pkg://iiwa_config/config/moveit/initial_positions.yaml
|
||||
moveit_controllers: pkg://iiwa_config/config/moveit/moveit_controllers.yaml
|
||||
moveit_cpp: pkg://iiwa_config/config/moveit/moveit_cpp.yaml
|
||||
|
||||
@@ -0,0 +1,18 @@
|
||||
<?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>iiwa_config</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Пакет с настройками цифрового двойника и физического робота</description>
|
||||
<maintainer email="grabardm@ml-dev.ru">daniel</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,16 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(iiwa_description)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
install(
|
||||
DIRECTORY resource worlds urdf
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_package()
|
||||
@@ -2,18 +2,17 @@
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>iiwa_description</name>
|
||||
<version>0.0.1</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="grabardm@ml-dev.ru">Grabar Daniil</maintainer>
|
||||
<version>0.0.0</version>
|
||||
<description>Описание робота в urdf/xacro а также описание webots мира</description>
|
||||
<maintainer email="grabardm@ml-dev.ru">daniel</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>ament_xmllint</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
|
||||
@@ -1,4 +0,0 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/iiwa_description
|
||||
[install]
|
||||
install_scripts=$base/lib/iiwa_description
|
||||
@@ -1,70 +0,0 @@
|
||||
import os
|
||||
|
||||
from setuptools import find_packages, setup
|
||||
|
||||
EXCLUDES = {".DS_Store", "Thumbs.db"}
|
||||
EXCLUDE_DIRS = {"__pycache__", ".pytest_cache", ".git", ".idea"}
|
||||
|
||||
|
||||
def data_files_from_tree(src_dir: str, dst_root: str) -> list:
|
||||
"""
|
||||
Собирает data_files в формате, подходящем для setuptools.
|
||||
Возвращает список пар (dst_path, [file1, file2, ...]).
|
||||
"""
|
||||
entries = []
|
||||
|
||||
for root, dirs, files in os.walk(src_dir):
|
||||
# исключаем служебные директории
|
||||
dirs[:] = [d for d in dirs if d not in EXCLUDE_DIRS]
|
||||
|
||||
file_list = [os.path.join(root, f) for f in files if f not in EXCLUDES]
|
||||
if not file_list:
|
||||
continue
|
||||
|
||||
rel = os.path.relpath(root, src_dir)
|
||||
dst_dir = os.path.join(dst_root, rel) if rel != "." else dst_root
|
||||
entries.append((dst_dir, file_list))
|
||||
|
||||
return entries
|
||||
|
||||
|
||||
package_name = "iiwa_description"
|
||||
|
||||
resource_intsall_root = f"share/{package_name}/resource"
|
||||
meshes_intsall_root = f"share/{package_name}/meshes"
|
||||
urdf_intsall_root = f"share/{package_name}/urdf"
|
||||
worlds_intsall_root = f"share/{package_name}/worlds"
|
||||
protos_intsall_root = f"share/{package_name}/protos"
|
||||
|
||||
|
||||
data_files = [
|
||||
("share/" + package_name, ["package.xml"]),
|
||||
]
|
||||
|
||||
data_files += data_files_from_tree("resource", resource_intsall_root)
|
||||
data_files += data_files_from_tree("meshes", meshes_intsall_root)
|
||||
data_files += data_files_from_tree("urdf", urdf_intsall_root)
|
||||
data_files += data_files_from_tree("worlds", worlds_intsall_root)
|
||||
data_files += data_files_from_tree("protos", protos_intsall_root)
|
||||
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version="0.0.1",
|
||||
packages=find_packages(exclude=["test"]),
|
||||
data_files=data_files,
|
||||
install_requires=["setuptools"],
|
||||
zip_safe=True,
|
||||
maintainer="Grabar Daniil",
|
||||
maintainer_email="grabardm@ml-dev.ru",
|
||||
description="TODO: Package description",
|
||||
license="Apache-2.0",
|
||||
extras_require={
|
||||
"test": [
|
||||
"pytest",
|
||||
],
|
||||
},
|
||||
entry_points={
|
||||
"console_scripts": [],
|
||||
},
|
||||
)
|
||||
@@ -3,11 +3,10 @@
|
||||
|
||||
<!-- Аргукменты -->
|
||||
<xacro:arg name="initial_positions_file"
|
||||
default="$(find iiwa_bringup)/config/initial_positions.yaml"/>
|
||||
default="$(find iiwa_config)/config/moveit/initial_positions.yaml"/>
|
||||
|
||||
<xacro:property name="initial_positions"
|
||||
value="${xacro.load_yaml('$(arg initial_positions_file)')['initial_positions']}"/>
|
||||
|
||||
value="${xacro.load_yaml('$(arg initial_positions_file)')['initial_positions']}"/>
|
||||
|
||||
<xacro:include filename="$(find iiwa_description)/urdf/params.xacro"/>
|
||||
<xacro:include filename="$(find iiwa_description)/urdf/macros.xacro"/>
|
||||
|
||||
@@ -1,4 +0,0 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/iiwa_object_spawner
|
||||
[install]
|
||||
install_scripts=$base/lib/iiwa_object_spawner
|
||||
@@ -0,0 +1,202 @@
|
||||
|
||||
Apache License
|
||||
Version 2.0, January 2004
|
||||
http://www.apache.org/licenses/
|
||||
|
||||
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
|
||||
|
||||
1. Definitions.
|
||||
|
||||
"License" shall mean the terms and conditions for use, reproduction,
|
||||
and distribution as defined by Sections 1 through 9 of this document.
|
||||
|
||||
"Licensor" shall mean the copyright owner or entity authorized by
|
||||
the copyright owner that is granting the License.
|
||||
|
||||
"Legal Entity" shall mean the union of the acting entity and all
|
||||
other entities that control, are controlled by, or are under common
|
||||
control with that entity. For the purposes of this definition,
|
||||
"control" means (i) the power, direct or indirect, to cause the
|
||||
direction or management of such entity, whether by contract or
|
||||
otherwise, or (ii) ownership of fifty percent (50%) or more of the
|
||||
outstanding shares, or (iii) beneficial ownership of such entity.
|
||||
|
||||
"You" (or "Your") shall mean an individual or Legal Entity
|
||||
exercising permissions granted by this License.
|
||||
|
||||
"Source" form shall mean the preferred form for making modifications,
|
||||
including but not limited to software source code, documentation
|
||||
source, and configuration files.
|
||||
|
||||
"Object" form shall mean any form resulting from mechanical
|
||||
transformation or translation of a Source form, including but
|
||||
not limited to compiled object code, generated documentation,
|
||||
and conversions to other media types.
|
||||
|
||||
"Work" shall mean the work of authorship, whether in Source or
|
||||
Object form, made available under the License, as indicated by a
|
||||
copyright notice that is included in or attached to the work
|
||||
(an example is provided in the Appendix below).
|
||||
|
||||
"Derivative Works" shall mean any work, whether in Source or Object
|
||||
form, that is based on (or derived from) the Work and for which the
|
||||
editorial revisions, annotations, elaborations, or other modifications
|
||||
represent, as a whole, an original work of authorship. For the purposes
|
||||
of this License, Derivative Works shall not include works that remain
|
||||
separable from, or merely link (or bind by name) to the interfaces of,
|
||||
the Work and Derivative Works thereof.
|
||||
|
||||
"Contribution" shall mean any work of authorship, including
|
||||
the original version of the Work and any modifications or additions
|
||||
to that Work or Derivative Works thereof, that is intentionally
|
||||
submitted to Licensor for inclusion in the Work by the copyright owner
|
||||
or by an individual or Legal Entity authorized to submit on behalf of
|
||||
the copyright owner. For the purposes of this definition, "submitted"
|
||||
means any form of electronic, verbal, or written communication sent
|
||||
to the Licensor or its representatives, including but not limited to
|
||||
communication on electronic mailing lists, source code control systems,
|
||||
and issue tracking systems that are managed by, or on behalf of, the
|
||||
Licensor for the purpose of discussing and improving the Work, but
|
||||
excluding communication that is conspicuously marked or otherwise
|
||||
designated in writing by the copyright owner as "Not a Contribution."
|
||||
|
||||
"Contributor" shall mean Licensor and any individual or Legal Entity
|
||||
on behalf of whom a Contribution has been received by Licensor and
|
||||
subsequently incorporated within the Work.
|
||||
|
||||
2. Grant of Copyright License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
copyright license to reproduce, prepare Derivative Works of,
|
||||
publicly display, publicly perform, sublicense, and distribute the
|
||||
Work and such Derivative Works in Source or Object form.
|
||||
|
||||
3. Grant of Patent License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
(except as stated in this section) patent license to make, have made,
|
||||
use, offer to sell, sell, import, and otherwise transfer the Work,
|
||||
where such license applies only to those patent claims licensable
|
||||
by such Contributor that are necessarily infringed by their
|
||||
Contribution(s) alone or by combination of their Contribution(s)
|
||||
with the Work to which such Contribution(s) was submitted. If You
|
||||
institute patent litigation against any entity (including a
|
||||
cross-claim or counterclaim in a lawsuit) alleging that the Work
|
||||
or a Contribution incorporated within the Work constitutes direct
|
||||
or contributory patent infringement, then any patent licenses
|
||||
granted to You under this License for that Work shall terminate
|
||||
as of the date such litigation is filed.
|
||||
|
||||
4. Redistribution. You may reproduce and distribute copies of the
|
||||
Work or Derivative Works thereof in any medium, with or without
|
||||
modifications, and in Source or Object form, provided that You
|
||||
meet the following conditions:
|
||||
|
||||
(a) You must give any other recipients of the Work or
|
||||
Derivative Works a copy of this License; and
|
||||
|
||||
(b) You must cause any modified files to carry prominent notices
|
||||
stating that You changed the files; and
|
||||
|
||||
(c) You must retain, in the Source form of any Derivative Works
|
||||
that You distribute, all copyright, patent, trademark, and
|
||||
attribution notices from the Source form of the Work,
|
||||
excluding those notices that do not pertain to any part of
|
||||
the Derivative Works; and
|
||||
|
||||
(d) If the Work includes a "NOTICE" text file as part of its
|
||||
distribution, then any Derivative Works that You distribute must
|
||||
include a readable copy of the attribution notices contained
|
||||
within such NOTICE file, excluding those notices that do not
|
||||
pertain to any part of the Derivative Works, in at least one
|
||||
of the following places: within a NOTICE text file distributed
|
||||
as part of the Derivative Works; within the Source form or
|
||||
documentation, if provided along with the Derivative Works; or,
|
||||
within a display generated by the Derivative Works, if and
|
||||
wherever such third-party notices normally appear. The contents
|
||||
of the NOTICE file are for informational purposes only and
|
||||
do not modify the License. You may add Your own attribution
|
||||
notices within Derivative Works that You distribute, alongside
|
||||
or as an addendum to the NOTICE text from the Work, provided
|
||||
that such additional attribution notices cannot be construed
|
||||
as modifying the License.
|
||||
|
||||
You may add Your own copyright statement to Your modifications and
|
||||
may provide additional or different license terms and conditions
|
||||
for use, reproduction, or distribution of Your modifications, or
|
||||
for any such Derivative Works as a whole, provided Your use,
|
||||
reproduction, and distribution of the Work otherwise complies with
|
||||
the conditions stated in this License.
|
||||
|
||||
5. Submission of Contributions. Unless You explicitly state otherwise,
|
||||
any Contribution intentionally submitted for inclusion in the Work
|
||||
by You to the Licensor shall be under the terms and conditions of
|
||||
this License, without any additional terms or conditions.
|
||||
Notwithstanding the above, nothing herein shall supersede or modify
|
||||
the terms of any separate license agreement you may have executed
|
||||
with Licensor regarding such Contributions.
|
||||
|
||||
6. Trademarks. This License does not grant permission to use the trade
|
||||
names, trademarks, service marks, or product names of the Licensor,
|
||||
except as required for reasonable and customary use in describing the
|
||||
origin of the Work and reproducing the content of the NOTICE file.
|
||||
|
||||
7. Disclaimer of Warranty. Unless required by applicable law or
|
||||
agreed to in writing, Licensor provides the Work (and each
|
||||
Contributor provides its Contributions) on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
|
||||
implied, including, without limitation, any warranties or conditions
|
||||
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
|
||||
PARTICULAR PURPOSE. You are solely responsible for determining the
|
||||
appropriateness of using or redistributing the Work and assume any
|
||||
risks associated with Your exercise of permissions under this License.
|
||||
|
||||
8. Limitation of Liability. In no event and under no legal theory,
|
||||
whether in tort (including negligence), contract, or otherwise,
|
||||
unless required by applicable law (such as deliberate and grossly
|
||||
negligent acts) or agreed to in writing, shall any Contributor be
|
||||
liable to You for damages, including any direct, indirect, special,
|
||||
incidental, or consequential damages of any character arising as a
|
||||
result of this License or out of the use or inability to use the
|
||||
Work (including but not limited to damages for loss of goodwill,
|
||||
work stoppage, computer failure or malfunction, or any and all
|
||||
other commercial damages or losses), even if such Contributor
|
||||
has been advised of the possibility of such damages.
|
||||
|
||||
9. Accepting Warranty or Additional Liability. While redistributing
|
||||
the Work or Derivative Works thereof, You may choose to offer,
|
||||
and charge a fee for, acceptance of support, warranty, indemnity,
|
||||
or other liability obligations and/or rights consistent with this
|
||||
License. However, in accepting such obligations, You may act only
|
||||
on Your own behalf and on Your sole responsibility, not on behalf
|
||||
of any other Contributor, and only if You agree to indemnify,
|
||||
defend, and hold each Contributor harmless for any liability
|
||||
incurred by, or claims asserted against, such Contributor by reason
|
||||
of your accepting any such warranty or additional liability.
|
||||
|
||||
END OF TERMS AND CONDITIONS
|
||||
|
||||
APPENDIX: How to apply the Apache License to your work.
|
||||
|
||||
To apply the Apache License to your work, attach the following
|
||||
boilerplate notice, with the fields enclosed by brackets "[]"
|
||||
replaced with your own identifying information. (Don't include
|
||||
the brackets!) The text should be enclosed in the appropriate
|
||||
comment syntax for the file format. We also recommend that a
|
||||
file or class name and description of purpose be included on the
|
||||
same "printed page" as the copyright notice for easier
|
||||
identification within third-party archives.
|
||||
|
||||
Copyright [yyyy] [name of copyright owner]
|
||||
|
||||
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.
|
||||
@@ -0,0 +1,25 @@
|
||||
from pathlib import Path
|
||||
from typing import Dict, Optional, Union
|
||||
|
||||
import xacro
|
||||
|
||||
|
||||
def load_robot_description(
|
||||
model_path: Union[str, Path],
|
||||
robot_name: str,
|
||||
xacro_args: Optional[Dict[str, str]] = None,
|
||||
) -> str:
|
||||
model_path = Path(model_path)
|
||||
suffix = model_path.suffix.lower()
|
||||
|
||||
if suffix == ".xacro":
|
||||
mappings = {"name": str(robot_name)}
|
||||
if xacro_args:
|
||||
mappings.update({k: str(v) for k, v in xacro_args.items()})
|
||||
|
||||
return xacro.process_file(str(model_path), mappings=mappings).toxml()
|
||||
|
||||
if suffix == ".urdf":
|
||||
return model_path.read_text(encoding="utf-8")
|
||||
|
||||
raise FileNotFoundError(f"Supported file formats: .xacro/.urdf, got: {model_path}")
|
||||
@@ -0,0 +1,226 @@
|
||||
import os
|
||||
from dataclasses import dataclass, fields, is_dataclass
|
||||
from typing import Any, Dict, Type, TypeVar
|
||||
|
||||
import yaml
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
T = TypeVar("T")
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class RobotCfg:
|
||||
name: str
|
||||
ip: str
|
||||
port: int
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class WebotsCfg:
|
||||
world: str
|
||||
transform: str
|
||||
rotation: str
|
||||
controller_timer: str
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class RvizCfg:
|
||||
config: str
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class DigitalTwinCfg:
|
||||
webots: WebotsCfg
|
||||
rviz: RvizCfg
|
||||
description: str
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class MoveitCfg:
|
||||
srdf: str
|
||||
kinematics: str
|
||||
joint_limits: str
|
||||
pilz_limits: str
|
||||
initial_positions: str
|
||||
moveit_controllers: str
|
||||
moveit_cpp: str
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ControllerCfg:
|
||||
controller_path: str
|
||||
moveit: MoveitCfg
|
||||
|
||||
|
||||
class SettingsError(RuntimeError):
|
||||
pass
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Settings:
|
||||
robot: RobotCfg
|
||||
digital_twin: DigitalTwinCfg
|
||||
controller: ControllerCfg
|
||||
|
||||
def to_dict(self) -> Dict[str, Any]:
|
||||
"""Serialize dataclass tree -> dict (recursive)."""
|
||||
|
||||
def _convert(obj: Any) -> Any:
|
||||
if is_dataclass(obj):
|
||||
return {f.name: _convert(getattr(obj, f.name)) for f in fields(obj)}
|
||||
if isinstance(obj, dict):
|
||||
return {k: _convert(v) for k, v in obj.items()}
|
||||
if isinstance(obj, (list, tuple)):
|
||||
return [_convert(x) for x in obj]
|
||||
return obj
|
||||
|
||||
return _convert(self)
|
||||
|
||||
def to_yaml(self) -> str:
|
||||
return yaml.safe_dump(self.to_dict(), sort_keys=False, allow_unicode=True)
|
||||
|
||||
def save_yaml(self, path: str) -> None:
|
||||
path = os.path.abspath(path)
|
||||
os.makedirs(os.path.dirname(path), exist_ok=True)
|
||||
with open(path, "w", encoding="utf-8") as f:
|
||||
f.write(self.to_yaml())
|
||||
|
||||
@classmethod
|
||||
def from_yaml(cls: Type[T], settings_path: str, check_files: bool = True) -> T:
|
||||
return build_settings(settings_path=settings_path, check_files=check_files)
|
||||
|
||||
|
||||
def load_yaml(path: str) -> Dict[str, Any]:
|
||||
path = os.path.abspath(path)
|
||||
if not os.path.exists(path):
|
||||
raise SettingsError(f"settings yaml not found: {path}")
|
||||
with open(path, "r", encoding="utf-8") as f:
|
||||
data = yaml.safe_load(f) or {}
|
||||
if not isinstance(data, dict):
|
||||
raise SettingsError("settings yaml root must be a mapping (dict)")
|
||||
return data
|
||||
|
||||
|
||||
def resolve_path(value: str, settings_dir: str) -> str:
|
||||
"""
|
||||
Supported:
|
||||
- pkg://<pkg>/<rel>
|
||||
- /abs/path
|
||||
- ./relative or relative (relative to settings.yaml dir)
|
||||
Returns absolute normalized path.
|
||||
"""
|
||||
if not isinstance(value, str) or not value.strip():
|
||||
raise SettingsError("empty path value in settings")
|
||||
|
||||
value = value.strip()
|
||||
|
||||
if value.startswith("pkg://"):
|
||||
rest = value[len("pkg://") :]
|
||||
if "/" not in rest:
|
||||
raise SettingsError(f"bad pkg uri: {value} (need pkg://<pkg>/<path>)")
|
||||
pkg, rel = rest.split("/", 1)
|
||||
share = get_package_share_directory(pkg)
|
||||
return os.path.normpath(os.path.join(share, rel))
|
||||
|
||||
if os.path.isabs(value):
|
||||
return os.path.normpath(value)
|
||||
|
||||
return os.path.normpath(os.path.join(settings_dir, value))
|
||||
|
||||
|
||||
def require(d: Dict[str, Any], key: str) -> Any:
|
||||
if key not in d:
|
||||
raise SettingsError(f"missing key '{key}'")
|
||||
return d[key]
|
||||
|
||||
|
||||
def assert_file(path: str, key: str) -> None:
|
||||
if not os.path.exists(path):
|
||||
raise SettingsError(f"file for '{key}' does not exist: {path}")
|
||||
if not os.path.isfile(path):
|
||||
raise SettingsError(f"path for '{key}' is not a file: {path}")
|
||||
|
||||
|
||||
def build_settings(settings_path: str, check_files: bool = True) -> Settings:
|
||||
settings_path = os.path.abspath(settings_path)
|
||||
settings_dir = os.path.dirname(settings_path)
|
||||
|
||||
raw = load_yaml(settings_path)
|
||||
|
||||
# robot
|
||||
robot_raw = require(raw, "robot")
|
||||
robot = RobotCfg(
|
||||
name=str(require(robot_raw, "name")),
|
||||
ip=str(require(robot_raw, "ip")),
|
||||
port=int(require(robot_raw, "port")),
|
||||
)
|
||||
|
||||
# digital_twin
|
||||
dt_raw = require(raw, "digital_twin")
|
||||
webots_raw = require(dt_raw, "webots")
|
||||
rviz_raw = require(dt_raw, "rviz")
|
||||
|
||||
webots = WebotsCfg(
|
||||
world=resolve_path(str(require(webots_raw, "world")), settings_dir),
|
||||
transform=str(require(webots_raw, "transform")),
|
||||
rotation=str(require(webots_raw, "rotation")),
|
||||
controller_timer=int(require(webots_raw, "controller_timer")),
|
||||
)
|
||||
|
||||
rviz = RvizCfg(config=resolve_path(str(require(rviz_raw, "config")), settings_dir))
|
||||
|
||||
digital_twin = DigitalTwinCfg(
|
||||
webots=webots,
|
||||
rviz=rviz,
|
||||
description=resolve_path(str(require(dt_raw, "description")), settings_dir),
|
||||
)
|
||||
|
||||
# controller, moveit
|
||||
ctrl_raw = require(raw, "controller")
|
||||
moveit_raw = require(ctrl_raw, "moveit")
|
||||
|
||||
moveit = MoveitCfg(
|
||||
srdf=resolve_path(str(require(moveit_raw, "srdf")), settings_dir),
|
||||
kinematics=resolve_path(str(require(moveit_raw, "kinematics")), settings_dir),
|
||||
joint_limits=resolve_path(
|
||||
str(require(moveit_raw, "joint_limits")), settings_dir
|
||||
),
|
||||
pilz_limits=resolve_path(str(require(moveit_raw, "pilz_limits")), settings_dir),
|
||||
initial_positions=resolve_path(
|
||||
str(require(moveit_raw, "initial_positions")), settings_dir
|
||||
),
|
||||
moveit_controllers=resolve_path(
|
||||
str(require(moveit_raw, "moveit_controllers")), settings_dir
|
||||
),
|
||||
moveit_cpp=resolve_path(str(require(moveit_raw, "moveit_cpp")), settings_dir),
|
||||
)
|
||||
|
||||
controller = ControllerCfg(
|
||||
controller_path=resolve_path(
|
||||
str(require(ctrl_raw, "controller_path")), settings_dir
|
||||
),
|
||||
moveit=moveit,
|
||||
)
|
||||
|
||||
s = Settings(robot=robot, digital_twin=digital_twin, controller=controller)
|
||||
|
||||
if check_files:
|
||||
assert_file(s.digital_twin.webots.world, "digital_twin.webots.world")
|
||||
assert_file(s.digital_twin.rviz.config, "digital_twin.rviz.config")
|
||||
assert_file(s.digital_twin.description, "digital_twin.description")
|
||||
assert_file(s.controller.controller_path, "controller.controller_path")
|
||||
|
||||
assert_file(s.controller.moveit.srdf, "controller.moveit.srdf")
|
||||
assert_file(s.controller.moveit.kinematics, "controller.moveit.kinematics")
|
||||
assert_file(s.controller.moveit.joint_limits, "controller.moveit.joint_limits")
|
||||
assert_file(s.controller.moveit.pilz_limits, "controller.moveit.pilz_limits")
|
||||
assert_file(
|
||||
s.controller.moveit.initial_positions, "controller.moveit.initial_positions"
|
||||
)
|
||||
assert_file(
|
||||
s.controller.moveit.moveit_controllers,
|
||||
"controller.moveit.moveit_controllers",
|
||||
)
|
||||
assert_file(s.controller.moveit.moveit_cpp, "controller.moveit.moveit_cpp")
|
||||
|
||||
return s
|
||||
@@ -1,16 +1,15 @@
|
||||
<?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>iiwa_object_spawner</name>
|
||||
<version>0.0.1</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="grabardm@ml-dev.ru">Даниил Грабарь</maintainer>
|
||||
<name>iiwa_utils</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Вспомогательные файлы для работы всей системы</description>
|
||||
<maintainer email="grabardm@ml-dev.ru">daniel</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>ament_xmllint</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
@@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/iiwa_utils
|
||||
[install]
|
||||
install_scripts=$base/lib/iiwa_utils
|
||||
@@ -1,10 +1,10 @@
|
||||
from setuptools import find_packages, setup
|
||||
|
||||
package_name = 'iiwa_object_spawner'
|
||||
package_name = 'iiwa_utils'
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version='0.0.1',
|
||||
version='0.0.0',
|
||||
packages=find_packages(exclude=['test']),
|
||||
data_files=[
|
||||
('share/ament_index/resource_index/packages',
|
||||
@@ -13,14 +13,14 @@ setup(
|
||||
],
|
||||
install_requires=['setuptools'],
|
||||
zip_safe=True,
|
||||
maintainer='Даниил Грабарь',
|
||||
maintainer='daniel',
|
||||
maintainer_email='grabardm@ml-dev.ru',
|
||||
description='TODO: Package description',
|
||||
description='Вспомогательные файлы для работы всей системы',
|
||||
license='Apache-2.0',
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'object_spawner = iiwa_object_spawner.object_spawner:main',
|
||||
"motion_planning_test = iiwa_object_spawner.motion_planing_test:main"
|
||||
'object_spawner = iiwa_utils.object_spawner:main',
|
||||
"motion_planning_test = iiwa_utils.motion_planing_test:main"
|
||||
],
|
||||
},
|
||||
)
|
||||
Reference in New Issue
Block a user