From ec40feb6f256443c6ab64610c32692c72b4f74fc 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: Sat, 22 Nov 2025 21:43:03 +1000 Subject: [PATCH] =?UTF-8?q?=D0=A1=D0=BE=D0=B7=D0=B4=D0=B0=D0=BD=20=D0=BE?= =?UTF-8?q?=D0=B1=D1=89=D0=B8=D0=B9=20=D1=84=D0=B0=D0=B9=D0=BB=20=D0=B7?= =?UTF-8?q?=D0=B0=D0=BF=D1=83=D1=81=D0=BA=D0=B0=20`digital=5Ftwin.launch.p?= =?UTF-8?q?y`,=20=D1=87=D0=B5=D1=80=D0=B5=D0=B7=20=D0=BA=D0=BE=D1=82=D0=BE?= =?UTF-8?q?=D1=80=D1=8B=D0=B9=20=D0=B2=20=D0=B4=D0=B0=D0=BB=D1=8C=D0=BD?= =?UTF-8?q?=D0=B5=D0=B9=D1=88=D0=B5=D0=BC=20=D0=B1=D1=83=D0=B4=D0=B5=D1=82?= =?UTF-8?q?=20=D0=B7=D0=B0=D0=BF=D1=83=D1=81=D0=BA=D0=B0=D1=82=D1=8C=D1=81?= =?UTF-8?q?=D1=8F=20=D0=B2=D0=B5=D1=81=D1=8C=20=D1=86=D0=B8=D1=84=D1=80?= =?UTF-8?q?=D0=BE=D0=B2=D0=BE=D0=B9=20=D0=B4=D0=B2=D0=BE=D0=B9=D0=BD=D0=B8?= =?UTF-8?q?=D0=BA.=20=D0=94=D0=BE=D0=B1=D0=B0=D0=B2=D0=BB=D0=B5=D0=BD?= =?UTF-8?q?=D0=B0=20=D0=BF=D0=B0=D0=BF=D0=BA=D0=B0=20`supported`,=20=D0=B2?= =?UTF-8?q?=20=D0=BA=D0=BE=D1=82=D0=BE=D1=80=D0=BE=D0=B9=20=D0=B1=D1=83?= =?UTF-8?q?=D0=B4=D1=83=D1=82=20=D1=80=D0=B0=D0=B7=D0=BC=D0=B5=D1=89=D0=B0?= =?UTF-8?q?=D1=82=D1=8C=D1=81=D1=8F=20=D1=84=D0=B0=D0=B9=D0=BB=D1=8B=20?= =?UTF-8?q?=D0=B7=D0=B0=D0=BF=D1=83=D1=81=D0=BA=D0=B0=20=D0=BF=D0=BE=D0=B4?= =?UTF-8?q?=D0=BA=D0=BB=D1=8E=D1=87=D0=B0=D0=B5=D0=BC=D1=8B=D0=B5=20=D0=B2?= =?UTF-8?q?=20=D0=BE=D1=81=D0=BD=D0=BE=D0=B2=D0=BD=D0=BE=D0=BC=20=D1=84?= =?UTF-8?q?=D0=B0=D0=B9=D0=BB=D0=B5.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Переименован контроллер на `iiwa_arm_controller` Добавлен пакет `iiwa_bringup.utils`, в котором будут распологаться все вспомогательные функции, для папки `launch` В файле `README`, были добавлены подсказки для автора --- README.md | 20 ++ src/iiwa_bringup/__init__.py | 0 src/iiwa_bringup/config/iiwa_controller.yaml | 4 +- .../launch/digital_twin.launch.py | 156 +++++++++++ .../launch/supported/rviz.launch.py | 46 ++++ .../supported/webots_controllers.launch.py | 51 ++++ .../launch/supported/webots_spawn.launch.py | 90 +++++++ src/iiwa_bringup/launch/webots.launch.py | 245 ------------------ src/iiwa_bringup/setup.py | 86 +++--- src/iiwa_bringup/utils/__init__.py | 0 src/iiwa_bringup/utils/converter.py | 12 + 11 files changed, 415 insertions(+), 295 deletions(-) create mode 100644 src/iiwa_bringup/__init__.py create mode 100644 src/iiwa_bringup/launch/digital_twin.launch.py create mode 100644 src/iiwa_bringup/launch/supported/rviz.launch.py create mode 100644 src/iiwa_bringup/launch/supported/webots_controllers.launch.py create mode 100644 src/iiwa_bringup/launch/supported/webots_spawn.launch.py delete mode 100644 src/iiwa_bringup/launch/webots.launch.py create mode 100644 src/iiwa_bringup/utils/__init__.py create mode 100644 src/iiwa_bringup/utils/converter.py diff --git a/README.md b/README.md index dcf70c2..1fc89c2 100644 --- a/README.md +++ b/README.md @@ -8,4 +8,24 @@ sudo apt install -y ros-${ROS_DISTRO}-webots-ros2 \ ros-${ROS_DISTRO}-ros2-control \ ros-${ROS_DISTRO}-ros2-controllers \ ros-${ROS_DISTRO}-moveit-* \ +``` + +Установка moveit2 (внимательно проверяй) +```bash +sudo apt install -y build-essential \ + cmake \ + git \ + python3-colcon-common-extensions \ + python3-flake8 \ + python3-rosdep \ + python3-setuptools \ + python3-vcstool \ + wget + +git clone https://github.com/moveit/moveit2.git +vcs import --recursive < moveit2/moveit2.repos +sudo apt remove ros-$ROS_DISTRO-moveit* +rosdep install -r --from-paths ./src/ --ignore-src --rosdistro $ROS_DISTRO --os=ubuntu:noble -y + +colcon build --mixin release ``` \ No newline at end of file diff --git a/src/iiwa_bringup/__init__.py b/src/iiwa_bringup/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/iiwa_bringup/config/iiwa_controller.yaml b/src/iiwa_bringup/config/iiwa_controller.yaml index 39a751d..66dc021 100644 --- a/src/iiwa_bringup/config/iiwa_controller.yaml +++ b/src/iiwa_bringup/config/iiwa_controller.yaml @@ -5,11 +5,11 @@ controller_manager: joint_state_broadcaster: type: "joint_state_broadcaster/JointStateBroadcaster" - joint_trajectory_controller: + iiwa_arm_controller: type: "joint_trajectory_controller/JointTrajectoryController" -joint_trajectory_controller: +iiwa_arm_controller: ros__parameters: joints: - joint1 diff --git a/src/iiwa_bringup/launch/digital_twin.launch.py b/src/iiwa_bringup/launch/digital_twin.launch.py new file mode 100644 index 0000000..6fe3b3a --- /dev/null +++ b/src/iiwa_bringup/launch/digital_twin.launch.py @@ -0,0 +1,156 @@ +from iiwa_bringup.utils import converter + + +from launch_ros.actions import Node +from launch import LaunchDescription +from launch_ros.substitutions import FindPackageShare +from launch.substitutions import PathJoinSubstitution, LaunchConfiguration +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import (IncludeLaunchDescription, + DeclareLaunchArgument, + OpaqueFunction) + + +PACKAGE = "iiwa_bringup" +DESCRIPTION_PKG = "iiwa_description" + + +def _runtime_setup(context, *args, **kwatgs): + setup = [] + + model_path = LaunchConfiguration('model').perform(context) + robot_name = LaunchConfiguration('robot_name').perform(context) + world_path = LaunchConfiguration('world').perform(context) + rviz_status = LaunchConfiguration('rviz').perform(context).lower() in ['true', '1', 'yes'] + + transform = LaunchConfiguration('transform').perform(context) + rotation = LaunchConfiguration('rotation').perform(context) + timer = LaunchConfiguration('controller_timer').perform(context) + + robot_description = converter.load_robot_description(model_path=model_path, + robot_name=robot_name) + + rsp_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", + parameters=[{ + "robot_description": robot_description, + "use_sim_time": False + }] + ) + + webots_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare(PACKAGE), + 'launch', + 'supported', 'webots_spawn.launch.py' + ]) + ), + launch_arguments={ + 'robot_name': robot_name, + 'model': model_path, + 'world': world_path, + 'transform': transform, + 'rotation': rotation, + 'controller_timer': timer + }.items() + ) + + + setup += [rsp_node, webots_launch] + + + if rviz_status: + rviz_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([FindPackageShare(PACKAGE), + "launch", "supported", 'rviz.launch.py' + ])), + launch_arguments={'package_name': PACKAGE}.items() + ) + + setup.append(rviz_launch) + + return setup + +def generate_launch_description(): + + # Объявление аргументов командной строки + declare_model_arg = DeclareLaunchArgument( + name="model", + default_value=PathJoinSubstitution([ + FindPackageShare(DESCRIPTION_PKG), + "urdf", + "iiwa7.urdf.xacro" + ]), + description="Path to robot xacro or urdf file (used to build robot_description)." + ) + + declare_robot_name_arg = DeclareLaunchArgument( + name="robot_name", + default_value="iiwa7", + description="Robot name (used for TF and naming spawned robot)." + ) + + declare_world_arg = DeclareLaunchArgument( + name="world", + default_value=PathJoinSubstitution([ + FindPackageShare(DESCRIPTION_PKG), + "worlds", + "iiwa.wbt" + ]), + description="Path to the Webots world (.wbt) to launch." + ) + + declare_controller_arg = DeclareLaunchArgument( + name="controller", + default_value=PathJoinSubstitution([ + FindPackageShare(PACKAGE), + "config", + "iiwa_controller.yaml" + ]), + description="Path to controllers YAML file (used by spawner and controller_manager)." + ) + + declare_transform_arg = DeclareLaunchArgument( + name="transform", + default_value="-0.25 0 0.79", + description="Translation applied when spawning the robot in the Webots world (x y z)." + ) + + declare_rotation_arg = DeclareLaunchArgument( + name="rotation", + default_value="0 0 1 0", + description="Rotation (axis-angle) applied when spawning the robot in the Webots world." + ) + + declare_rviz_arg = DeclareLaunchArgument( + name="rviz", + default_value="0", + description="If true|1|yes then launch RViz and joint_state_publisher_gui instead of controllers." + ) + + declare_controller_manager_arg = DeclareLaunchArgument( + name="controller_timer", + default_value="50", + description="Timeout (seconds) for controller_manager spawners (--controller-manager-timeout)." + ) + + runtime_setup = OpaqueFunction( + function=_runtime_setup + ) + + return LaunchDescription([ + declare_model_arg, + declare_robot_name_arg, + declare_world_arg, + declare_controller_arg, + declare_transform_arg, + declare_rotation_arg, + declare_rviz_arg, + declare_controller_manager_arg, + runtime_setup + ]) \ No newline at end of file diff --git a/src/iiwa_bringup/launch/supported/rviz.launch.py b/src/iiwa_bringup/launch/supported/rviz.launch.py new file mode 100644 index 0000000..fc282ec --- /dev/null +++ b/src/iiwa_bringup/launch/supported/rviz.launch.py @@ -0,0 +1,46 @@ +from launch_ros.actions import Node +from launch.events import Shutdown +from launch import LaunchDescription +from launch.event_handlers import OnProcessExit +from launch_ros.substitutions import FindPackageShare +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.actions import RegisterEventHandler, EmitEvent, DeclareLaunchArgument + + +def generate_launch_description(): + + declare_package_arg = DeclareLaunchArgument( + 'package_name', + default_value='iiwa_bringup', + description='Package name where RViz config is stored' + ) + + package_name = LaunchConfiguration('package_name') + + rviz_config = PathJoinSubstitution([ + FindPackageShare(package_name), + 'config', + 'rviz_iiwa.rviz' + ]) + + rviz = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', rviz_config], + output='log' + ) + + # При закрытии rviz закрываем все + shutdown_on_rviz_exit = RegisterEventHandler( + OnProcessExit( + target_action=rviz, + on_exit=[EmitEvent(event=Shutdown())] + ) + ) + + return LaunchDescription([ + declare_package_arg, + rviz, + shutdown_on_rviz_exit + ]) diff --git a/src/iiwa_bringup/launch/supported/webots_controllers.launch.py b/src/iiwa_bringup/launch/supported/webots_controllers.launch.py new file mode 100644 index 0000000..f07810f --- /dev/null +++ b/src/iiwa_bringup/launch/supported/webots_controllers.launch.py @@ -0,0 +1,51 @@ +from launch_ros.actions import Node +from webots_ros2_driver.urdf_spawner import URDFSpawner + +from launch import LaunchDescription +from launch.actions import OpaqueFunction +from launch.substitutions import LaunchConfiguration +from iiwa_bringup.utils import converter + + +def _setup_controllers(context, *args, **kwargs): + model_path = LaunchConfiguration('model').perform(context) + robot_name = LaunchConfiguration('robot_name').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=model_path, + robot_name=robot_name) + + tmo = ['--controller-manager-timeout', str(timer)] + + jsb = Node( + package='controller_manager', + executable='spawner', + output='screen', + arguments=['joint_state_broadcaster'] + tmo, + parameters=[{'use_sim_time': False}], + ) + + jtc = Node( + package='controller_manager', + executable='spawner', + output='screen', + arguments=['iiwa_arm_controller'] + tmo, + parameters=[{'use_sim_time': False}], + ) + + spawner_urdf = URDFSpawner( + name=robot_name, + robot_description=robot_description, + translation=transform, + rotation=rotation + ) + + return [jsb, jtc, spawner_urdf] + + +def generate_launch_description(): + return LaunchDescription([ + OpaqueFunction(function=_setup_controllers) + ]) \ No newline at end of file diff --git a/src/iiwa_bringup/launch/supported/webots_spawn.launch.py b/src/iiwa_bringup/launch/supported/webots_spawn.launch.py new file mode 100644 index 0000000..a92feb6 --- /dev/null +++ b/src/iiwa_bringup/launch/supported/webots_spawn.launch.py @@ -0,0 +1,90 @@ +from launch.events import Shutdown +from launch import LaunchDescription +from launch.event_handlers import OnProcessExit +from launch.substitutions import LaunchConfiguration +from launch_ros.substitutions import FindPackageShare +from launch.event_handlers import OnProcessExit, OnProcessStart +from launch.substitutions import PathJoinSubstitution, LaunchConfiguration +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import (IncludeLaunchDescription, + OpaqueFunction, + TimerAction, + RegisterEventHandler, + EmitEvent) + + +from webots_ros2_driver.webots_launcher import WebotsLauncher +from webots_ros2_driver.webots_controller import WebotsController + + +def _spawn_setup(context, *args, **kwargs): + model_path = LaunchConfiguration('model').perform(context) + 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) + + webots = WebotsLauncher(world=world_path, + ros2_supervisor=True) + + driver = WebotsController( + robot_name=robot_name, + parameters=[ + { + "robot_description": model_path, + "use_sim_time": False, + "set_robot_state_publisher": False + }, + LaunchConfiguration('controller').perform(context) + ], + respawn=True + ) + + controllers_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('iiwa_bringup'), + 'launch', + 'supported', + 'webots_controllers.launch.py' + ]) + ), + launch_arguments={ + 'robot_name': robot_name, + 'model': model_path, + 'transform': transform, + 'rotation': rotation, + 'controller_timer': timer + }.items() + ) + + spawn_on_driver_start = RegisterEventHandler( + event_handler=OnProcessStart( + target_action=driver, + on_start=lambda evt, ctx: [ + TimerAction(period=5.0, + actions=[controllers_launch] + ) + ] + ) + ) + + shutdown_on_webots_exit = RegisterEventHandler( + OnProcessExit( + target_action=webots, + on_exit=[EmitEvent(event=Shutdown())] + ) + ) + + return [webots, + webots._supervisor, + driver, + spawn_on_driver_start, + shutdown_on_webots_exit] + + +def generate_launch_description(): + return LaunchDescription([ + OpaqueFunction(function=_spawn_setup) + ]) \ No newline at end of file diff --git a/src/iiwa_bringup/launch/webots.launch.py b/src/iiwa_bringup/launch/webots.launch.py deleted file mode 100644 index ea033d4..0000000 --- a/src/iiwa_bringup/launch/webots.launch.py +++ /dev/null @@ -1,245 +0,0 @@ -import xacro -from pathlib import Path -from launch.events import Shutdown -from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - RegisterEventHandler, - OpaqueFunction, - EmitEvent, - TimerAction -) -from launch.event_handlers import OnProcessExit, OnProcessStart -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.substitutions import FindPackageShare -from launch_ros.actions import Node -from webots_ros2_driver.webots_launcher import WebotsLauncher -from webots_ros2_driver.urdf_spawner import URDFSpawner -from webots_ros2_driver.webots_controller import WebotsController - - -def _runtime_controller(robot_name: str, - robot_description_str: str, - transform_proto: str = "0 0 0", - rotation_proto: str = "0 0 1 0", - controller_manager_timer: int = 50): - tmo = ['--controller-manager-timeout', str(controller_manager_timer)] - - jsb = Node( - package='controller_manager', - executable='spawner', - output='screen', - arguments=['joint_state_broadcaster'] + tmo, - parameters=[{'use_sim_time': False}], - ) - - jtc = Node( - package='controller_manager', - executable='spawner', - output='screen', - arguments=['joint_trajectory_controller'] + tmo, # название с yaml файла - parameters=[{'use_sim_time': False}], - ) - - spawner_urdf = URDFSpawner( - name=robot_name, - robot_description=robot_description_str, - translation=transform_proto, - rotation=rotation_proto - ) - - return [jsb, jtc, spawner_urdf] - - -def _runtime_setup(context, *args, **kwargs): - # Получение параметров от пользователя при запуске launch - model = LaunchConfiguration('model').perform(context) - robot_name = LaunchConfiguration('robot_name').perform(context) - transform_proto = LaunchConfiguration('transform_proto').perform(context) - rotation_proto = LaunchConfiguration('rotation_proto').perform(context) - world_path = LaunchConfiguration('world').perform(context) - rviz_status = LaunchConfiguration('rviz').perform(context).lower() in ['true', '1', 'yes'] - controller_manager = LaunchConfiguration('controller_manager').perform(context) - - # Загрузка описания робота - model_path = Path(model) - suffix = model_path.suffix.lower() - if suffix == ".xacro": - robot_description_str = xacro.process_file(model, mappings={'name': str(robot_name)}).toxml() - elif suffix==".urdf": - robot_description_str = Path(model).read_text(encoding="utf-8") - else: - raise FileNotFoundError(f"Поддерживаются форматы файла: xacro/urdf") - - # Запуск узлов - webots = WebotsLauncher(world=world_path, - ros2_supervisor=True) - - rsp_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - name="robot_state_publisher", - output="screen", - parameters=[{ - "robot_description": robot_description_str, - "use_sim_time": False - }] - ) - - # работа с драйверами - driver = WebotsController( - robot_name=robot_name, - parameters=[ - { - "robot_description": model, - "use_sim_time": False, - "set_robot_state_publisher": False - }, - LaunchConfiguration('controller').perform(context) - ], - respawn=True - ) - - # Во время запуска драйвера спавниться робот и ros2_controllers - spawn_on_driver_start = RegisterEventHandler( - event_handler=OnProcessStart( - target_action=driver, - on_start=lambda evt, ctx: [ - TimerAction(period=2.0, - actions=_runtime_controller(robot_name=robot_name, - robot_description_str=robot_description_str, - transform_proto=transform_proto, - rotation_proto=rotation_proto, - controller_manager_timer=controller_manager)) - ] - ) - ) - - # При закрытии webots закрываем все - shutdown_on_webots_exit = RegisterEventHandler( - OnProcessExit( - target_action=webots, - on_exit=[EmitEvent(event=Shutdown())] - ) - ) - - setup = [webots, - webots._supervisor, - rsp_node, - driver, - spawn_on_driver_start, - shutdown_on_webots_exit - ] - - - # Настройка rviz - if rviz_status: - - rviz_config = PathJoinSubstitution([ - FindPackageShare(kwargs['package_name']), - 'config', - 'rviz_iiwa.rviz' - ]) - - rviz = Node( - package='rviz2', - executable='rviz2', - name='rviz2', - arguments=['-d', rviz_config], - output='log' - ) - - # При закрытии rviz закрываем все - shutdown_on_rviz_exit = RegisterEventHandler( - OnProcessExit( - target_action=rviz, - on_exit=[EmitEvent(event=Shutdown())] - ) - ) - - setup += [rviz, shutdown_on_rviz_exit] - - return setup - - -def generate_launch_description(): - package_name="iiwa_bringup" - description_pkg = "iiwa_description" - - # Объявление аргументов командной строки - declare_model_arg = DeclareLaunchArgument( - name="model", - default_value=PathJoinSubstitution([ - FindPackageShare(description_pkg), - "urdf", - "iiwa7.urdf.xacro" - ]), - description="Path to robot xacro or urdf file (used to build robot_description)." - ) - - declare_robot_name_arg = DeclareLaunchArgument( - name="robot_name", - default_value="iiwa7", - description="Robot name (used for TF and naming spawned robot)." - ) - - declare_world_arg = DeclareLaunchArgument( - name="world", - default_value=PathJoinSubstitution([ - FindPackageShare(description_pkg), - "worlds", - "iiwa.wbt" - ]), - description="Path to the Webots world (.wbt) to launch." - ) - - declare_controller_arg = DeclareLaunchArgument( - name="controller", - default_value=PathJoinSubstitution([ - FindPackageShare(package_name), - "config", - "iiwa_controller.yaml" - ]), - description="Path to controllers YAML file (used by spawner and controller_manager)." - ) - - declare_transform_arg = DeclareLaunchArgument( - name="transform_proto", - default_value="-0.25 0 0.79", - description="Translation applied when spawning the robot in the Webots world (x y z)." - ) - - declare_rotation_arg = DeclareLaunchArgument( - name="rotation_proto", - default_value="0 0 1 0", - description="Rotation (axis-angle) applied when spawning the robot in the Webots world." - ) - - declare_rviz_arg = DeclareLaunchArgument( - name="rviz", - default_value="0", - description="If true|1|yes then launch RViz and joint_state_publisher_gui instead of controllers." - ) - - declare_controller_manager_arg = DeclareLaunchArgument( - name="controller_manager", - default_value="50", - description="Timeout (seconds) for controller_manager spawners (--controller-manager-timeout)." - ) - - runtime_setup = OpaqueFunction( - function=_runtime_setup, - kwargs={'package_name': package_name} - ) - - return LaunchDescription([ - declare_model_arg, - declare_robot_name_arg, - declare_world_arg, - declare_controller_arg, - declare_transform_arg, - declare_rotation_arg, - declare_rviz_arg, - declare_controller_manager_arg, - runtime_setup, - ]) diff --git a/src/iiwa_bringup/setup.py b/src/iiwa_bringup/setup.py index fb96316..0e278f9 100644 --- a/src/iiwa_bringup/setup.py +++ b/src/iiwa_bringup/setup.py @@ -1,65 +1,55 @@ 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 +from setuptools import setup package_name = 'iiwa_bringup' -resource_intsall_root = f"share/{package_name}/resource" -launch_intsall_root = f"share/{package_name}/launch" -config_intsall_root = f"share/{package_name}/config" +# Собираем список пакетов: основной + вложенные (если они есть). +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') -data_files = [ - ('share/' + package_name, ['package.xml']), -] - -data_files += data_files_from_tree('resource', resource_intsall_root) -data_files += data_files_from_tree('launch', launch_intsall_root) -data_files += data_files_from_tree('config', config_intsall_root) - +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=find_packages(exclude=['test']), + packages=packages, + package_dir=package_dir, + include_package_data=True, data_files=data_files, install_requires=['setuptools'], - zip_safe=True, + zip_safe=False, maintainer='Grabar Daniil', maintainer_email='grabardm@ml-dev.ru', - description='TODO: Package description', + description='iiwa bringup package', license='Apache-2.0', - extras_require={ - 'test': [ - 'pytest', - ], - }, - entry_points={ - 'console_scripts': [ - ], - }, ) diff --git a/src/iiwa_bringup/utils/__init__.py b/src/iiwa_bringup/utils/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/iiwa_bringup/utils/converter.py b/src/iiwa_bringup/utils/converter.py new file mode 100644 index 0000000..4ccc3e2 --- /dev/null +++ b/src/iiwa_bringup/utils/converter.py @@ -0,0 +1,12 @@ +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(f"Supported file formats: xacro/urdf") \ No newline at end of file