From 37da67abe585ce7cdca2126a54ce6dcf8a0248a8 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, 29 Nov 2025 15:51:45 +1000 Subject: [PATCH] =?UTF-8?q?=D0=BE=D1=82=D1=84=D0=BE=D1=80=D0=BC=D0=B0?= =?UTF-8?q?=D1=82=D0=B8=D1=80=D0=BE=D0=B2=D0=B0=D0=BD=D1=8B=20=D0=B4=D0=BE?= =?UTF-8?q?=D0=BA=D1=83=D0=BC=D0=B5=D0=BD=D1=82=D1=8B=20=D1=81=D0=BE=D0=B3?= =?UTF-8?q?=D0=BB=D0=B0=D1=81=D0=BD=D0=BE=20=D0=BD=D0=BE=D1=82=D0=B0=D1=86?= =?UTF-8?q?=D0=B8=D0=B8=20pep?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/digital_twin.launch.py | 158 +++++++++--------- src/iiwa_bringup/launch/rviz_iiwa.launch.py | 93 ++++++----- .../launch/supported/rviz.launch.py | 48 +++--- .../supported/webots_controllers.launch.py | 51 +++--- .../launch/supported/webots_spawn.launch.py | 100 ++++++----- src/iiwa_bringup/setup.py | 41 ++--- src/iiwa_description/setup.py | 45 ++--- .../iiwa_object_spawner/object_spawner.py | 32 ++-- 8 files changed, 276 insertions(+), 292 deletions(-) diff --git a/src/iiwa_bringup/launch/digital_twin.launch.py b/src/iiwa_bringup/launch/digital_twin.launch.py index 6fe3b3a..8f1ed80 100644 --- a/src/iiwa_bringup/launch/digital_twin.launch.py +++ b/src/iiwa_bringup/launch/digital_twin.launch.py @@ -1,15 +1,15 @@ -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.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + OpaqueFunction, +) from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.actions import (IncludeLaunchDescription, - DeclareLaunchArgument, - OpaqueFunction) +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from iiwa_bringup.utils import converter PACKAGE = "iiwa_bringup" DESCRIPTION_PKG = "iiwa_description" @@ -18,139 +18,137 @@ 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) + 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", + ] - robot_description = converter.load_robot_description(model_path=model_path, - robot_name=robot_name) + 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 - }] + parameters=[{"robot_description": robot_description, "use_sim_time": False}], ) - + webots_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( - PathJoinSubstitution([ - FindPackageShare(PACKAGE), - 'launch', - 'supported', 'webots_spawn.launch.py' - ]) + 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() + "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() + 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)." + 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)." + 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." + 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)." + 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)." + 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." + 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." + 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)." + description="Timeout (seconds) for controller_manager spawners (--controller-manager-timeout).", ) - runtime_setup = OpaqueFunction( - function=_runtime_setup - ) + 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 + 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/launch/rviz_iiwa.launch.py b/src/iiwa_bringup/launch/rviz_iiwa.launch.py index 6de869d..24bb95a 100644 --- a/src/iiwa_bringup/launch/rviz_iiwa.launch.py +++ b/src/iiwa_bringup/launch/rviz_iiwa.launch.py @@ -1,81 +1,82 @@ -from launch_ros.actions import Node from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, EmitEvent, RegisterEventHandler -from launch_ros.substitutions import FindPackageShare -from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution -from launch.events import Shutdown from launch.event_handlers import OnProcessExit +from launch.events import Shutdown +from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - package_name="iiwa_bringup" + package_name = "iiwa_bringup" description_package_name = "iiwa_description" declare_model_arg = DeclareLaunchArgument( name="model", - default_value=PathJoinSubstitution([ - FindPackageShare(description_package_name), - "urdf", - "iiwa7.urdf.xacro" - ]), - description="Path to the robot URDF/Xacro file" + default_value=PathJoinSubstitution( + [FindPackageShare(description_package_name), "urdf", "iiwa7.urdf.xacro"] + ), + description="Path to the robot URDF/Xacro file", ) declare_robot_name_arg = DeclareLaunchArgument( name="robot_name", default_value="iiwa7", - description="Robot name fro the TF tree" + description="Robot name fro the TF tree", ) - robot_description = Command([ - "xacro ", LaunchConfiguration('model'), - " robot_name:=", LaunchConfiguration("robot_name") - ]) + robot_description = Command( + [ + "xacro ", + LaunchConfiguration("model"), + " robot_name:=", + LaunchConfiguration("robot_name"), + ] + ) robot_state_publisher = Node( package="robot_state_publisher", executable="robot_state_publisher", name="robot_state_publisher", output="both", - parameters=[{ - "robot_description": robot_description, - "use_sim_time": False, - }] + parameters=[ + { + "robot_description": robot_description, + "use_sim_time": False, + } + ], ) joint_state_publisher_gui = Node( - package='joint_state_publisher_gui', - executable='joint_state_publisher_gui', - name='joint_state_publisher_gui', - parameters=[{"use_sim_time": False}] + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + name="joint_state_publisher_gui", + parameters=[{"use_sim_time": False}], ) - rviz_config = PathJoinSubstitution([ - FindPackageShare(package_name), - 'config', - 'rviz_iiwa.rviz' - ]) + rviz_config = PathJoinSubstitution( + [FindPackageShare(package_name), "config", "rviz_iiwa.rviz"] + ) rviz = Node( - package='rviz2', - executable='rviz2', - name='rviz2', - arguments=['-d', rviz_config], - output='log' + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["-d", rviz_config], + output="log", ) shutdown_on_rviz_exit = RegisterEventHandler( - OnProcessExit( - target_action=rviz, - on_exit=[EmitEvent(event=Shutdown())] - ) + OnProcessExit(target_action=rviz, on_exit=[EmitEvent(event=Shutdown())]) ) - return LaunchDescription([ - declare_model_arg, - declare_robot_name_arg, - robot_state_publisher, - joint_state_publisher_gui, - rviz, - shutdown_on_rviz_exit - ]) \ No newline at end of file + return LaunchDescription( + [ + declare_model_arg, + declare_robot_name_arg, + robot_state_publisher, + joint_state_publisher_gui, + rviz, + shutdown_on_rviz_exit, + ] + ) diff --git a/src/iiwa_bringup/launch/supported/rviz.launch.py b/src/iiwa_bringup/launch/supported/rviz.launch.py index fc282ec..7318deb 100644 --- a/src/iiwa_bringup/launch/supported/rviz.launch.py +++ b/src/iiwa_bringup/launch/supported/rviz.launch.py @@ -1,46 +1,36 @@ -from launch_ros.actions import Node -from launch.events import Shutdown from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, EmitEvent, RegisterEventHandler from launch.event_handlers import OnProcessExit -from launch_ros.substitutions import FindPackageShare +from launch.events import Shutdown from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch.actions import RegisterEventHandler, EmitEvent, DeclareLaunchArgument - +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + def generate_launch_description(): - declare_package_arg = DeclareLaunchArgument( - 'package_name', - default_value='iiwa_bringup', - description='Package name where RViz config is stored' + "package_name", + default_value="iiwa_bringup", + description="Package name where RViz config is stored", ) - package_name = LaunchConfiguration('package_name') + package_name = LaunchConfiguration("package_name") - rviz_config = PathJoinSubstitution([ - FindPackageShare(package_name), - 'config', - 'rviz_iiwa.rviz' - ]) + rviz_config = PathJoinSubstitution( + [FindPackageShare(package_name), "config", "rviz_iiwa.rviz"] + ) rviz = Node( - package='rviz2', - executable='rviz2', - name='rviz2', - arguments=['-d', rviz_config], - output='log' + 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())] - ) + OnProcessExit(target_action=rviz, on_exit=[EmitEvent(event=Shutdown())]) ) - return LaunchDescription([ - declare_package_arg, - rviz, - shutdown_on_rviz_exit - ]) + 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 index f07810f..0353b31 100644 --- a/src/iiwa_bringup/launch/supported/webots_controllers.launch.py +++ b/src/iiwa_bringup/launch/supported/webots_controllers.launch.py @@ -1,51 +1,50 @@ -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 launch_ros.actions import Node +from webots_ros2_driver.urdf_spawner import URDFSpawner + 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) + 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)] + 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}], + 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}], + 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 + rotation=rotation, ) return [jsb, jtc, spawner_urdf] def generate_launch_description(): - return LaunchDescription([ - OpaqueFunction(function=_setup_controllers) - ]) \ No newline at end of file + return LaunchDescription([OpaqueFunction(function=_setup_controllers)]) diff --git a/src/iiwa_bringup/launch/supported/webots_spawn.launch.py b/src/iiwa_bringup/launch/supported/webots_spawn.launch.py index e6829ba..4299a7c 100644 --- a/src/iiwa_bringup/launch/supported/webots_spawn.launch.py +++ b/src/iiwa_bringup/launch/supported/webots_spawn.launch.py @@ -1,90 +1,84 @@ -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.actions import ( + EmitEvent, + IncludeLaunchDescription, + OpaqueFunction, + RegisterEventHandler, + TimerAction, +) from launch.event_handlers import OnProcessExit, OnProcessStart -from launch.substitutions import PathJoinSubstitution, LaunchConfiguration +from launch.events import Shutdown 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 launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare from webots_ros2_driver.webots_controller import WebotsController +from webots_ros2_driver.webots_launcher import WebotsLauncher 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) + 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) - webots = WebotsLauncher(world=world_path, - ros2_supervisor=True) - driver = WebotsController( robot_name=robot_name, parameters=[ { "robot_description": model_path, "use_sim_time": True, - "set_robot_state_publisher": False + "set_robot_state_publisher": False, }, - LaunchConfiguration('controller').perform(context) + LaunchConfiguration("controller").perform(context), ], - respawn=True + respawn=True, ) controllers_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( - PathJoinSubstitution([ - FindPackageShare('iiwa_bringup'), - 'launch', - 'supported', - 'webots_controllers.launch.py' - ]) + 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() + "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] - ) - ] + TimerAction(period=5.0, actions=[controllers_launch]) + ], ) ) shutdown_on_webots_exit = RegisterEventHandler( - OnProcessExit( - target_action=webots, - on_exit=[EmitEvent(event=Shutdown())] - ) + OnProcessExit(target_action=webots, on_exit=[EmitEvent(event=Shutdown())]) ) - return [webots, - webots._supervisor, - driver, - spawn_on_driver_start, - shutdown_on_webots_exit] + 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 + return LaunchDescription([OpaqueFunction(function=_spawn_setup)]) diff --git a/src/iiwa_bringup/setup.py b/src/iiwa_bringup/setup.py index 0e278f9..6fbd6d6 100644 --- a/src/iiwa_bringup/setup.py +++ b/src/iiwa_bringup/setup.py @@ -1,55 +1,58 @@ import os + from setuptools import setup -package_name = 'iiwa_bringup' +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') +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: '.', + package_name: ".", } -if f'{package_name}.utils' in packages: - package_dir[f'{package_name}.utils'] = os.path.join('.', 'utils') +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'} + 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 + 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') + +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', + version="0.0.1", packages=packages, package_dir=package_dir, include_package_data=True, data_files=data_files, - install_requires=['setuptools'], + install_requires=["setuptools"], zip_safe=False, - maintainer='Grabar Daniil', - maintainer_email='grabardm@ml-dev.ru', - description='iiwa bringup package', - license='Apache-2.0', + maintainer="Grabar Daniil", + maintainer_email="grabardm@ml-dev.ru", + description="iiwa bringup package", + license="Apache-2.0", ) diff --git a/src/iiwa_description/setup.py b/src/iiwa_description/setup.py index cd782de..07abe67 100644 --- a/src/iiwa_description/setup.py +++ b/src/iiwa_description/setup.py @@ -1,8 +1,10 @@ import os + from setuptools import find_packages, setup -EXCLUDES = {'.DS_Store', 'Thumbs.db'} -EXCLUDE_DIRS = {'__pycache__', '.pytest_cache', '.git', '.idea'} +EXCLUDES = {".DS_Store", "Thumbs.db"} +EXCLUDE_DIRS = {"__pycache__", ".pytest_cache", ".git", ".idea"} + def data_files_from_tree(src_dir: str, dst_root: str) -> list: """ @@ -20,12 +22,13 @@ def data_files_from_tree(src_dir: str, dst_root: str) -> list: continue rel = os.path.relpath(root, src_dir) - dst_dir = os.path.join(dst_root, rel) if rel != '.' else dst_root + 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' + +package_name = "iiwa_description" resource_intsall_root = f"share/{package_name}/resource" meshes_intsall_root = f"share/{package_name}/meshes" @@ -34,36 +37,34 @@ worlds_intsall_root = f"share/{package_name}/worlds" protos_intsall_root = f"share/{package_name}/protos" - data_files = [ - ('share/' + package_name, ['package.xml']), + ("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) +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']), + version="0.0.1", + packages=find_packages(exclude=["test"]), data_files=data_files, - install_requires=['setuptools'], + install_requires=["setuptools"], zip_safe=True, - maintainer='Grabar Daniil', - maintainer_email='grabardm@ml-dev.ru', - description='TODO: Package description', - license='Apache-2.0', + maintainer="Grabar Daniil", + maintainer_email="grabardm@ml-dev.ru", + description="TODO: Package description", + license="Apache-2.0", extras_require={ - 'test': [ - 'pytest', + "test": [ + "pytest", ], }, entry_points={ - 'console_scripts': [ - ], + "console_scripts": [], }, ) diff --git a/src/iiwa_object_spawner/iiwa_object_spawner/object_spawner.py b/src/iiwa_object_spawner/iiwa_object_spawner/object_spawner.py index 1966a28..5e6c27f 100644 --- a/src/iiwa_object_spawner/iiwa_object_spawner/object_spawner.py +++ b/src/iiwa_object_spawner/iiwa_object_spawner/object_spawner.py @@ -2,25 +2,25 @@ # /Ros2Supervisor/spawn_urdf_robot -import rclpy from random import randint + +import rclpy +from rclpy.executors import ExternalShutdownException from rclpy.node import Node from rclpy.task import Future -from rclpy.executors import ExternalShutdownException from webots_ros2_msgs.srv import SpawnNodeFromString class ObjectSpawner(Node): def __init__(self): super().__init__("object_spawner") - # self._object_count: int = randint(1, 5) - self._object_count: int = 1 + self._object_count: int = randint(1, 5) self._spawned_count: int = 0 self._call_in_progress: bool = False - - self.cli = self.create_client(SpawnNodeFromString, - "/Ros2Supervisor/spawn_node_from_string") + self.cli = self.create_client( + SpawnNodeFromString, "/Ros2Supervisor/spawn_node_from_string" + ) while not self.cli.wait_for_service(timeout_sec=10): self.get_logger().warning( @@ -31,21 +31,18 @@ class ObjectSpawner(Node): def timer_callback(self): if self._spawned_count >= self._object_count: - self.get_logger().info( - f"All {self._object_count} objects spawned" - ) + self.get_logger().info(f"All {self._object_count} objects spawned") self._timer.cancel() return - + if self._call_in_progress: return - - data = "Solid { name \"test_box2\" translation 0 1 0.5 children [ Shape { appearance PBRAppearance { baseColor 0.901961 0.380392 0 } geometry Box { size 0.1 0.1 0.1 } } ] boundingObject Box { size 0.1 0.1 0.1 } physics Physics { } }" - req = SpawnNodeFromString.Request(data=data, - check_fields=True) + data = 'Solid { name "test_box2" translation 0 1 0.5 children [ Shape { appearance PBRAppearance { baseColor 0.901961 0.380392 0 } geometry Box { size 0.1 0.1 0.1 } } ] boundingObject Box { size 0.1 0.1 0.1 } physics Physics { } }' + + req = SpawnNodeFromString.Request(data=data, check_fields=True) self.get_logger().info( - f"Calling spawn service #{self._spawned_count+1}/{self._object_count}" + f"Calling spawn service #{self._spawned_count + 1}/{self._object_count}" ) self._call_in_progress = True future = self.cli.call_async(req) @@ -70,5 +67,6 @@ def main(args=None): except (KeyboardInterrupt, ExternalShutdownException): pass -if __name__ == '__main__': + +if __name__ == "__main__": main()