From fa39991bcf2be65e14b505584898da22ff5230eb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=94=D0=B0=D0=BD=D0=B8=D0=B8=D0=BB=20=D0=93=D1=80=D0=B0?= =?UTF-8?q?=D0=B1=D0=B0=D1=80=D1=8C?= Date: Thu, 25 Dec 2025 21:46:40 +1000 Subject: [PATCH] =?UTF-8?q?=D0=9F=D0=B5=D1=80=D0=B5=D1=80=D0=B0=D0=B1?= =?UTF-8?q?=D0=BE=D1=82=D0=B0=D0=BD=D0=B0=20=D0=B2=D1=81=D1=8F=20=D1=81?= =?UTF-8?q?=D1=82=D1=80=D1=83=D0=BA=D1=82=D1=83=D1=80=D0=B0=20=D0=B7=D0=B0?= =?UTF-8?q?=D0=BF=D1=83=D1=81=D0=BA=D0=B0=20=D1=84=D0=B0=D0=B9=D0=BB=D0=BE?= =?UTF-8?q?=D0=B2.=20=D0=A2=D0=B5=D0=BF=D0=B5=D1=80=D1=8C=20=D0=B2=20`iiwa?= =?UTF-8?q?=5Fbringup`=20=D1=85=D1=80=D0=B0=D0=BD=D1=8F=D1=82=D1=81=D1=8F?= =?UTF-8?q?=20=D1=82=D0=BE=D0=BB=D1=8C=D0=BA=D0=BE=20`launch`=20=D1=84?= =?UTF-8?q?=D0=B0=D0=B9=D0=BB=D1=8B.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Все конфигурационные файлы переехали в пакет `iiwa_config`. также там появился общий конфигурационный файл, который упращает запуск `digital_twin.launch.py` Добавлен дополнительный пакет `iiwa_utils`, в который переехал весь код с папки `iiwa_bringup/utils`, а также все тестовые ноды переехали с `iiwa_object_spawner` в этот пакет Пакеты `iiwa_bringup` и `iiwa_description` переведены на `ament_cmake` для дальнейшего удобсва - пакеты не будут содержать код для реализации --- README.md | 16 +- src/iiwa_bringup/CMakeLists.txt | 15 ++ .../launch/digital_twin.launch.py | 224 +++++------------ .../supported/webots_controllers.launch.py | 21 +- .../launch/supported/webots_spawn.launch.py | 21 +- src/iiwa_bringup/package.xml | 18 +- src/iiwa_bringup/setup.cfg | 4 - src/iiwa_bringup/setup.py | 58 ----- src/iiwa_bringup/utils/__init__.py | 0 src/iiwa_bringup/utils/converter.py | 12 - src/iiwa_config/CMakeLists.txt | 17 ++ .../LICENSE | 0 .../config/moveit}/iiwa7.srdf | 0 .../config/moveit}/iiwa_controller.yaml | 0 .../config/moveit}/initial_positions.yaml | 0 .../config/moveit}/joint_limits.yaml | 0 .../config/moveit}/kinematics.yaml | 0 .../config/moveit}/moveit_controllers.yaml | 0 .../config/moveit/moveit_cpp.yaml} | 0 .../config/moveit}/pilz_cartesian_limits.yaml | 0 .../config/rviz/rviz_moveit.rviz} | 10 +- src/iiwa_config/config/setting.yaml | 29 +++ src/iiwa_config/package.xml | 18 ++ src/iiwa_description/CMakeLists.txt | 16 ++ src/iiwa_description/package.xml | 17 +- src/iiwa_description/setup.cfg | 4 - src/iiwa_description/setup.py | 70 ------ src/iiwa_description/urdf/iiwa7.urdf.xacro | 5 +- .../iiwa_object_spawner/__init__.py | 0 .../resource/iiwa_object_spawner | 0 src/iiwa_object_spawner/setup.cfg | 4 - src/iiwa_utils/LICENSE | 202 ++++++++++++++++ .../iiwa_utils}/__init__.py | 0 src/iiwa_utils/iiwa_utils/converter.py | 25 ++ .../iiwa_utils}/motion_planing_test.py | 0 .../iiwa_utils}/object_spawner.py | 0 src/iiwa_utils/iiwa_utils/setting_loader.py | 226 ++++++++++++++++++ .../package.xml | 9 +- .../resource/iiwa_utils} | 0 src/iiwa_utils/setup.cfg | 4 + .../setup.py | 12 +- 41 files changed, 680 insertions(+), 377 deletions(-) create mode 100644 src/iiwa_bringup/CMakeLists.txt delete mode 100644 src/iiwa_bringup/setup.cfg delete mode 100644 src/iiwa_bringup/setup.py delete mode 100644 src/iiwa_bringup/utils/__init__.py delete mode 100644 src/iiwa_bringup/utils/converter.py create mode 100644 src/iiwa_config/CMakeLists.txt rename src/{iiwa_object_spawner => iiwa_config}/LICENSE (100%) rename src/{iiwa_bringup/config => iiwa_config/config/moveit}/iiwa7.srdf (100%) rename src/{iiwa_bringup/config => iiwa_config/config/moveit}/iiwa_controller.yaml (100%) rename src/{iiwa_bringup/config => iiwa_config/config/moveit}/initial_positions.yaml (100%) rename src/{iiwa_bringup/config => iiwa_config/config/moveit}/joint_limits.yaml (100%) rename src/{iiwa_bringup/config => iiwa_config/config/moveit}/kinematics.yaml (100%) rename src/{iiwa_bringup/config => iiwa_config/config/moveit}/moveit_controllers.yaml (100%) rename src/{iiwa_bringup/config/motion_planing.yaml => iiwa_config/config/moveit/moveit_cpp.yaml} (100%) rename src/{iiwa_bringup/config => iiwa_config/config/moveit}/pilz_cartesian_limits.yaml (100%) rename src/{iiwa_bringup/config/rviz_iiwa.rviz => iiwa_config/config/rviz/rviz_moveit.rviz} (98%) create mode 100644 src/iiwa_config/config/setting.yaml create mode 100644 src/iiwa_config/package.xml create mode 100644 src/iiwa_description/CMakeLists.txt delete mode 100644 src/iiwa_description/setup.cfg delete mode 100644 src/iiwa_description/setup.py delete mode 100644 src/iiwa_object_spawner/iiwa_object_spawner/__init__.py delete mode 100644 src/iiwa_object_spawner/resource/iiwa_object_spawner delete mode 100644 src/iiwa_object_spawner/setup.cfg create mode 100644 src/iiwa_utils/LICENSE rename src/{iiwa_bringup => iiwa_utils/iiwa_utils}/__init__.py (100%) create mode 100644 src/iiwa_utils/iiwa_utils/converter.py rename src/{iiwa_object_spawner/iiwa_object_spawner => iiwa_utils/iiwa_utils}/motion_planing_test.py (100%) rename src/{iiwa_object_spawner/iiwa_object_spawner => iiwa_utils/iiwa_utils}/object_spawner.py (100%) create mode 100644 src/iiwa_utils/iiwa_utils/setting_loader.py rename src/{iiwa_object_spawner => iiwa_utils}/package.xml (64%) rename src/{iiwa_bringup/resource/iiwa_bringup => iiwa_utils/resource/iiwa_utils} (100%) create mode 100644 src/iiwa_utils/setup.cfg rename src/{iiwa_object_spawner => iiwa_utils}/setup.py (60%) diff --git a/README.md b/README.md index 1dcd090..2fce1cb 100644 --- a/README.md +++ b/README.md @@ -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 diff --git a/src/iiwa_bringup/CMakeLists.txt b/src/iiwa_bringup/CMakeLists.txt new file mode 100644 index 0000000..c3ec90d --- /dev/null +++ b/src/iiwa_bringup/CMakeLists.txt @@ -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() diff --git a/src/iiwa_bringup/launch/digital_twin.launch.py b/src/iiwa_bringup/launch/digital_twin.launch.py index bf155af..1a087a3 100644 --- a/src/iiwa_bringup/launch/digital_twin.launch.py +++ b/src/iiwa_bringup/launch/digital_twin.launch.py @@ -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 / where 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, ] ) diff --git a/src/iiwa_bringup/launch/supported/webots_controllers.launch.py b/src/iiwa_bringup/launch/supported/webots_controllers.launch.py index ffee1ab..490bade 100644 --- a/src/iiwa_bringup/launch/supported/webots_controllers.launch.py +++ b/src/iiwa_bringup/launch/supported/webots_controllers.launch.py @@ -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", diff --git a/src/iiwa_bringup/launch/supported/webots_spawn.launch.py b/src/iiwa_bringup/launch/supported/webots_spawn.launch.py index 23d1e59..d722dfb 100644 --- a/src/iiwa_bringup/launch/supported/webots_spawn.launch.py +++ b/src/iiwa_bringup/launch/supported/webots_spawn.launch.py @@ -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(), ) diff --git a/src/iiwa_bringup/package.xml b/src/iiwa_bringup/package.xml index a11c4fd..1e5d3a3 100644 --- a/src/iiwa_bringup/package.xml +++ b/src/iiwa_bringup/package.xml @@ -2,18 +2,18 @@ iiwa_bringup - 0.0.1 - TODO: Package description - Grabar Daniil + 0.2.0 + Файлы запуска проектов + daniel Apache-2.0 - ament_copyright - ament_flake8 - ament_pep257 - ament_xmllint - python3-pytest + ament_cmake + iiwa_utils + + ament_lint_auto + ament_lint_common - ament_python + ament_cmake diff --git a/src/iiwa_bringup/setup.cfg b/src/iiwa_bringup/setup.cfg deleted file mode 100644 index 863ba40..0000000 --- a/src/iiwa_bringup/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/iiwa_bringup -[install] -install_scripts=$base/lib/iiwa_bringup diff --git a/src/iiwa_bringup/setup.py b/src/iiwa_bringup/setup.py deleted file mode 100644 index 6fbd6d6..0000000 --- a/src/iiwa_bringup/setup.py +++ /dev/null @@ -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", -) diff --git a/src/iiwa_bringup/utils/__init__.py b/src/iiwa_bringup/utils/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/src/iiwa_bringup/utils/converter.py b/src/iiwa_bringup/utils/converter.py deleted file mode 100644 index eb8e5a5..0000000 --- a/src/iiwa_bringup/utils/converter.py +++ /dev/null @@ -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") \ No newline at end of file diff --git a/src/iiwa_config/CMakeLists.txt b/src/iiwa_config/CMakeLists.txt new file mode 100644 index 0000000..9347beb --- /dev/null +++ b/src/iiwa_config/CMakeLists.txt @@ -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() diff --git a/src/iiwa_object_spawner/LICENSE b/src/iiwa_config/LICENSE similarity index 100% rename from src/iiwa_object_spawner/LICENSE rename to src/iiwa_config/LICENSE diff --git a/src/iiwa_bringup/config/iiwa7.srdf b/src/iiwa_config/config/moveit/iiwa7.srdf similarity index 100% rename from src/iiwa_bringup/config/iiwa7.srdf rename to src/iiwa_config/config/moveit/iiwa7.srdf diff --git a/src/iiwa_bringup/config/iiwa_controller.yaml b/src/iiwa_config/config/moveit/iiwa_controller.yaml similarity index 100% rename from src/iiwa_bringup/config/iiwa_controller.yaml rename to src/iiwa_config/config/moveit/iiwa_controller.yaml diff --git a/src/iiwa_bringup/config/initial_positions.yaml b/src/iiwa_config/config/moveit/initial_positions.yaml similarity index 100% rename from src/iiwa_bringup/config/initial_positions.yaml rename to src/iiwa_config/config/moveit/initial_positions.yaml diff --git a/src/iiwa_bringup/config/joint_limits.yaml b/src/iiwa_config/config/moveit/joint_limits.yaml similarity index 100% rename from src/iiwa_bringup/config/joint_limits.yaml rename to src/iiwa_config/config/moveit/joint_limits.yaml diff --git a/src/iiwa_bringup/config/kinematics.yaml b/src/iiwa_config/config/moveit/kinematics.yaml similarity index 100% rename from src/iiwa_bringup/config/kinematics.yaml rename to src/iiwa_config/config/moveit/kinematics.yaml diff --git a/src/iiwa_bringup/config/moveit_controllers.yaml b/src/iiwa_config/config/moveit/moveit_controllers.yaml similarity index 100% rename from src/iiwa_bringup/config/moveit_controllers.yaml rename to src/iiwa_config/config/moveit/moveit_controllers.yaml diff --git a/src/iiwa_bringup/config/motion_planing.yaml b/src/iiwa_config/config/moveit/moveit_cpp.yaml similarity index 100% rename from src/iiwa_bringup/config/motion_planing.yaml rename to src/iiwa_config/config/moveit/moveit_cpp.yaml diff --git a/src/iiwa_bringup/config/pilz_cartesian_limits.yaml b/src/iiwa_config/config/moveit/pilz_cartesian_limits.yaml similarity index 100% rename from src/iiwa_bringup/config/pilz_cartesian_limits.yaml rename to src/iiwa_config/config/moveit/pilz_cartesian_limits.yaml diff --git a/src/iiwa_bringup/config/rviz_iiwa.rviz b/src/iiwa_config/config/rviz/rviz_moveit.rviz similarity index 98% rename from src/iiwa_bringup/config/rviz_iiwa.rviz rename to src/iiwa_config/config/rviz/rviz_moveit.rviz index 110d571..9ebb619 100644 --- a/src/iiwa_bringup/config/rviz_iiwa.rviz +++ b/src/iiwa_config/config/rviz/rviz_moveit.rviz @@ -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: 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 diff --git a/src/iiwa_config/config/setting.yaml b/src/iiwa_config/config/setting.yaml new file mode 100644 index 0000000..1185f36 --- /dev/null +++ b/src/iiwa_config/config/setting.yaml @@ -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 + diff --git a/src/iiwa_config/package.xml b/src/iiwa_config/package.xml new file mode 100644 index 0000000..1c5761d --- /dev/null +++ b/src/iiwa_config/package.xml @@ -0,0 +1,18 @@ + + + + iiwa_config + 0.1.0 + Пакет с настройками цифрового двойника и физического робота + daniel + Apache-2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/iiwa_description/CMakeLists.txt b/src/iiwa_description/CMakeLists.txt new file mode 100644 index 0000000..4244aa3 --- /dev/null +++ b/src/iiwa_description/CMakeLists.txt @@ -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() diff --git a/src/iiwa_description/package.xml b/src/iiwa_description/package.xml index cb68974..0cb5ab6 100644 --- a/src/iiwa_description/package.xml +++ b/src/iiwa_description/package.xml @@ -2,18 +2,17 @@ iiwa_description - 0.0.1 - TODO: Package description - Grabar Daniil + 0.0.0 + Описание робота в urdf/xacro а также описание webots мира + daniel Apache-2.0 - ament_copyright - ament_flake8 - ament_pep257 - ament_xmllint - python3-pytest + ament_cmake + + ament_lint_auto + ament_lint_common - ament_python + ament_cmake diff --git a/src/iiwa_description/setup.cfg b/src/iiwa_description/setup.cfg deleted file mode 100644 index 669d2c3..0000000 --- a/src/iiwa_description/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/iiwa_description -[install] -install_scripts=$base/lib/iiwa_description diff --git a/src/iiwa_description/setup.py b/src/iiwa_description/setup.py deleted file mode 100644 index 07abe67..0000000 --- a/src/iiwa_description/setup.py +++ /dev/null @@ -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": [], - }, -) diff --git a/src/iiwa_description/urdf/iiwa7.urdf.xacro b/src/iiwa_description/urdf/iiwa7.urdf.xacro index 0915e38..c61742a 100644 --- a/src/iiwa_description/urdf/iiwa7.urdf.xacro +++ b/src/iiwa_description/urdf/iiwa7.urdf.xacro @@ -3,11 +3,10 @@ + default="$(find iiwa_config)/config/moveit/initial_positions.yaml"/> - + value="${xacro.load_yaml('$(arg initial_positions_file)')['initial_positions']}"/> diff --git a/src/iiwa_object_spawner/iiwa_object_spawner/__init__.py b/src/iiwa_object_spawner/iiwa_object_spawner/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/src/iiwa_object_spawner/resource/iiwa_object_spawner b/src/iiwa_object_spawner/resource/iiwa_object_spawner deleted file mode 100644 index e69de29..0000000 diff --git a/src/iiwa_object_spawner/setup.cfg b/src/iiwa_object_spawner/setup.cfg deleted file mode 100644 index 1dd0f86..0000000 --- a/src/iiwa_object_spawner/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/iiwa_object_spawner -[install] -install_scripts=$base/lib/iiwa_object_spawner diff --git a/src/iiwa_utils/LICENSE b/src/iiwa_utils/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/src/iiwa_utils/LICENSE @@ -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. diff --git a/src/iiwa_bringup/__init__.py b/src/iiwa_utils/iiwa_utils/__init__.py similarity index 100% rename from src/iiwa_bringup/__init__.py rename to src/iiwa_utils/iiwa_utils/__init__.py diff --git a/src/iiwa_utils/iiwa_utils/converter.py b/src/iiwa_utils/iiwa_utils/converter.py new file mode 100644 index 0000000..67dce82 --- /dev/null +++ b/src/iiwa_utils/iiwa_utils/converter.py @@ -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}") diff --git a/src/iiwa_object_spawner/iiwa_object_spawner/motion_planing_test.py b/src/iiwa_utils/iiwa_utils/motion_planing_test.py similarity index 100% rename from src/iiwa_object_spawner/iiwa_object_spawner/motion_planing_test.py rename to src/iiwa_utils/iiwa_utils/motion_planing_test.py diff --git a/src/iiwa_object_spawner/iiwa_object_spawner/object_spawner.py b/src/iiwa_utils/iiwa_utils/object_spawner.py similarity index 100% rename from src/iiwa_object_spawner/iiwa_object_spawner/object_spawner.py rename to src/iiwa_utils/iiwa_utils/object_spawner.py diff --git a/src/iiwa_utils/iiwa_utils/setting_loader.py b/src/iiwa_utils/iiwa_utils/setting_loader.py new file mode 100644 index 0000000..0364725 --- /dev/null +++ b/src/iiwa_utils/iiwa_utils/setting_loader.py @@ -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:/// + - /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, 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 diff --git a/src/iiwa_object_spawner/package.xml b/src/iiwa_utils/package.xml similarity index 64% rename from src/iiwa_object_spawner/package.xml rename to src/iiwa_utils/package.xml index 93d80d7..05c6e26 100644 --- a/src/iiwa_object_spawner/package.xml +++ b/src/iiwa_utils/package.xml @@ -1,16 +1,15 @@ - iiwa_object_spawner - 0.0.1 - TODO: Package description - Даниил Грабарь + iiwa_utils + 0.1.0 + Вспомогательные файлы для работы всей системы + daniel Apache-2.0 ament_copyright ament_flake8 ament_pep257 - ament_xmllint python3-pytest diff --git a/src/iiwa_bringup/resource/iiwa_bringup b/src/iiwa_utils/resource/iiwa_utils similarity index 100% rename from src/iiwa_bringup/resource/iiwa_bringup rename to src/iiwa_utils/resource/iiwa_utils diff --git a/src/iiwa_utils/setup.cfg b/src/iiwa_utils/setup.cfg new file mode 100644 index 0000000..cde6e1f --- /dev/null +++ b/src/iiwa_utils/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/iiwa_utils +[install] +install_scripts=$base/lib/iiwa_utils diff --git a/src/iiwa_object_spawner/setup.py b/src/iiwa_utils/setup.py similarity index 60% rename from src/iiwa_object_spawner/setup.py rename to src/iiwa_utils/setup.py index 2f18014..d782b25 100644 --- a/src/iiwa_object_spawner/setup.py +++ b/src/iiwa_utils/setup.py @@ -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" ], }, )