grabardm/lightweight-cobot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 1 Wiki Activity

отформатированы документы согласно нотации pep

Browse Source
This commit is contained in:
Даниил Грабарь
2025-11-29 15:51:45 +10:00
parent b36bd0e98b
commit 37da67abe5
8 changed files with 276 additions and 292 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/iiwa_bringup/launch/digital_twin.launch.py
+67 -69
View File
@@ -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.launch_description_sources import PythonLaunchDescriptionSource
from launch.actions import (IncludeLaunchDescription,
from launch.actions import (
DeclareLaunchArgument,
OpaqueFunction)
IncludeLaunchDescription,
OpaqueFunction,
)
from launch.launch_description_sources import PythonLaunchDescriptionSource
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,132 +18,129 @@ 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']
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)
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)
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([
PathJoinSubstitution(
[
FindPackageShare(PACKAGE),
'launch',
'supported', 'webots_spawn.launch.py'
])
"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():
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([
return LaunchDescription(
[
declare_model_arg,
declare_robot_name_arg,
declare_world_arg,
@@ -152,5 +149,6 @@ def generate_launch_description():
declare_rotation_arg,
declare_rviz_arg,
declare_controller_manager_arg,
runtime_setup
])
runtime_setup,
]
)
src/iiwa_bringup/launch/rviz_iiwa.launch.py
+39 -38
View File
@@ -1,10 +1,10 @@
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():
@@ -13,69 +13,70 @@ def generate_launch_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=[{
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([
return LaunchDescription(
[
declare_model_arg,
declare_robot_name_arg,
robot_state_publisher,
joint_state_publisher_gui,
rviz,
shutdown_on_rviz_exit
])
shutdown_on_rviz_exit,
]
)
src/iiwa_bringup/launch/supported/rviz.launch.py
+18 -28
View File
@@ -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])
src/iiwa_bringup/launch/supported/webots_controllers.launch.py
+24 -25
View File
@@ -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)
robot_description = converter.load_robot_description(
model_path=model_path, robot_name=robot_name
)
tmo = ['--controller-manager-timeout', str(timer)]
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)
])
return LaunchDescription([OpaqueFunction(function=_setup_controllers)])
src/iiwa_bringup/launch/supported/webots_spawn.launch.py
+43 -49
View File
@@ -1,32 +1,29 @@
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,
from launch.actions import (
EmitEvent,
IncludeLaunchDescription,
OpaqueFunction,
TimerAction,
RegisterEventHandler,
EmitEvent)
from webots_ros2_driver.webots_launcher import WebotsLauncher
TimerAction,
)
from launch.event_handlers import OnProcessExit, OnProcessStart
from launch.events import Shutdown
from launch.launch_description_sources import PythonLaunchDescriptionSource
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,
@@ -34,57 +31,54 @@ def _spawn_setup(context, *args, **kwargs):
{
"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,
return [
webots,
webots._supervisor,
driver,
spawn_on_driver_start,
shutdown_on_webots_exit]
shutdown_on_webots_exit,
]
def generate_launch_description():
return LaunchDescription([
OpaqueFunction(function=_spawn_setup)
])
return LaunchDescription([OpaqueFunction(function=_spawn_setup)])
src/iiwa_bringup/setup.py
+22 -19
View File
@@ -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",
)
src/iiwa_description/setup.py
+23 -22
View File
@@ -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": [],
},
)
src/iiwa_object_spawner/iiwa_object_spawner/object_spawner.py
+12 -14
View File
@@ -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,19 +31,16 @@ 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 { } }"
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)
req = SpawnNodeFromString.Request(data=data, check_fields=True)
self.get_logger().info(
f"Calling spawn service #{self._spawned_count + 1}/{self._object_count}"
)
@@ -70,5 +67,6 @@ def main(args=None):
except (KeyboardInterrupt, ExternalShutdownException):
pass
if __name__ == '__main__':
if __name__ == "__main__":
main()