Реализован спавн робота с контроллерами в виртуальной среде webots
This commit is contained in:
Даниил Грабарь
@@ -0,0 +1,30 @@
|
||||
controller_manager:
|
||||
ros__parameters:
|
||||
update_rate: 31
|
||||
|
||||
joint_state_broadcaster:
|
||||
type: "joint_state_broadcaster/JointStateBroadcaster"
|
||||
|
||||
joint_trajectory_controller:
|
||||
type: "joint_trajectory_controller/JointTrajectoryController"
|
||||
|
||||
|
||||
joint_trajectory_controller:
|
||||
ros__parameters:
|
||||
joints:
|
||||
- joint1
|
||||
- joint2
|
||||
- joint3
|
||||
- joint4
|
||||
- joint5
|
||||
- joint6
|
||||
- joint7
|
||||
|
||||
command_interfaces:
|
||||
- position
|
||||
|
||||
state_interfaces:
|
||||
- position
|
||||
|
||||
allow_partial_joints_goal: false
|
||||
interpolate_from_desired_state: true
|
||||
@@ -0,0 +1,170 @@
|
||||
# TODO: код на будущее
|
||||
|
||||
import os
|
||||
import re
|
||||
from pathlib import Path
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import (
|
||||
DeclareLaunchArgument,
|
||||
RegisterEventHandler,
|
||||
OpaqueFunction,
|
||||
)
|
||||
from launch.event_handlers import OnProcessExit
|
||||
from launch.substitutions import Command, 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
|
||||
|
||||
|
||||
def _patch_proto_controller(proto_path: str):
|
||||
pattern = r'(field\s+SFString\s+controller\s+)"void"'
|
||||
replacement = r'\1"exec"'
|
||||
proto = Path(proto_path)
|
||||
text = proto.read_text(encoding='utf-8')
|
||||
new_text, count = re.subn(pattern, replacement, text)
|
||||
|
||||
if count != 0:
|
||||
proto.write_text(new_text, encoding='utf-8')
|
||||
|
||||
|
||||
def _after_xacro(context, *args, **kwargs):
|
||||
"""Выполняется после завершения xacro2proto: патчим proto и возвращаем actions (webots, rsp)."""
|
||||
proto_path = kwargs['proto_path']
|
||||
# проверим существование
|
||||
if not Path(proto_path).exists():
|
||||
raise RuntimeError(f"Expected proto file not found: {proto_path}")
|
||||
|
||||
# Патчим
|
||||
_patch_proto_controller(proto_path)
|
||||
|
||||
# создаём действия, которые будут запущены ПОСЛЕ xacro2proto
|
||||
world_path = LaunchConfiguration('world').perform(context)
|
||||
webots = WebotsLauncher(world=world_path,
|
||||
ros2_supervisor=True)
|
||||
|
||||
# robot_state_publisher можно запускать после webots (или вместе)
|
||||
robot_description_sub: Command = kwargs['robot_description_sub']
|
||||
robot_description_str = robot_description_sub.perform(context)
|
||||
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": True
|
||||
}]
|
||||
)
|
||||
|
||||
# вернуть список действий, которые launch затем подключит
|
||||
return [ webots, rsp_node ]
|
||||
|
||||
def _runtime_setup(context, *args, **kwargs):
|
||||
# Получаем значения
|
||||
model_path = LaunchConfiguration('model').perform(context)
|
||||
protos_path = LaunchConfiguration('protos_path').perform(context)
|
||||
robot_name = LaunchConfiguration('robot_name').perform(context)
|
||||
tool_slot = LaunchConfiguration('tool_slot').perform(context)
|
||||
transform_proto = LaunchConfiguration('transform_proto').perform(context)
|
||||
rotation_proto = LaunchConfiguration('rotation_proto').perform(context)
|
||||
|
||||
|
||||
|
||||
# убедимся, что директория существует
|
||||
os.makedirs(protos_path, exist_ok=True)
|
||||
|
||||
proto_output = f"{protos_path}/{robot_name}.proto" # <- явное .proto
|
||||
|
||||
xacro2proto_node = Node(
|
||||
package="webots_ros2_importer",
|
||||
executable="xacro2proto",
|
||||
name="xacro2proto",
|
||||
output="screen",
|
||||
arguments=[
|
||||
"--input", str(model_path),
|
||||
"--output", proto_output,
|
||||
"--tool-slot", str(tool_slot),
|
||||
"--translation", str(transform_proto).replace(",", " "),
|
||||
"--rotation", str(rotation_proto).replace(",", " "),
|
||||
]
|
||||
)
|
||||
|
||||
# обработчик: после завершения xacro2proto запускаем OpaqueFunction, который патчит и запускает webots+rsp
|
||||
after_xacro = RegisterEventHandler(
|
||||
OnProcessExit(
|
||||
target_action=xacro2proto_node,
|
||||
on_exit=[
|
||||
OpaqueFunction(
|
||||
function=_after_xacro,
|
||||
kwargs={
|
||||
'proto_path': proto_output,
|
||||
'robot_description_sub': kwargs['robot_description_sub']
|
||||
}
|
||||
)
|
||||
]
|
||||
)
|
||||
)
|
||||
|
||||
return [ xacro2proto_node, after_xacro ]
|
||||
|
||||
def generate_launch_description():
|
||||
description_pkg = "iiwa_description"
|
||||
|
||||
declare_model_arg = DeclareLaunchArgument(
|
||||
name="model",
|
||||
default_value=PathJoinSubstitution([
|
||||
FindPackageShare(description_pkg),
|
||||
"urdf",
|
||||
"iiwa7.urdf.xacro"
|
||||
]),
|
||||
)
|
||||
|
||||
declare_robot_name_arg = DeclareLaunchArgument(name="robot_name", default_value="iiwa7")
|
||||
declare_world_arg = DeclareLaunchArgument(
|
||||
name="world",
|
||||
default_value=PathJoinSubstitution([
|
||||
FindPackageShare(description_pkg),
|
||||
"worlds",
|
||||
"simple_world.wbt"
|
||||
]),
|
||||
)
|
||||
declare_proto_arg = DeclareLaunchArgument(
|
||||
name="protos_path",
|
||||
default_value=PathJoinSubstitution([
|
||||
FindPackageShare(description_pkg),
|
||||
"protos"
|
||||
]),
|
||||
)
|
||||
declare_transform_arg = DeclareLaunchArgument(
|
||||
name="transform_proto",
|
||||
default_value="0 0 0"
|
||||
)
|
||||
declare_rotation_arg = DeclareLaunchArgument(
|
||||
name="rotation_proto",
|
||||
default_value="0 0 1 0"
|
||||
)
|
||||
declare_tool_arg = DeclareLaunchArgument(name="tool_slot", default_value="link7_ee")
|
||||
|
||||
|
||||
robot_description_cmd = Command([
|
||||
"xacro ", LaunchConfiguration("model"),
|
||||
" robot_name:=", LaunchConfiguration("robot_name")
|
||||
])
|
||||
|
||||
runtime_setup = OpaqueFunction(
|
||||
function=_runtime_setup,
|
||||
kwargs={'robot_description_sub': robot_description_cmd}
|
||||
)
|
||||
|
||||
ld = LaunchDescription([
|
||||
declare_model_arg,
|
||||
declare_robot_name_arg,
|
||||
declare_world_arg,
|
||||
declare_proto_arg,
|
||||
declare_transform_arg,
|
||||
declare_rotation_arg,
|
||||
declare_tool_arg,
|
||||
runtime_setup,
|
||||
])
|
||||
return ld
|
||||
@@ -0,0 +1,245 @@
|
||||
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,
|
||||
])
|
||||
@@ -27,6 +27,7 @@ def data_files_from_tree(src_dir: str, dst_root: str) -> list:
|
||||
|
||||
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"
|
||||
|
||||
@@ -35,6 +36,7 @@ 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)
|
||||
|
||||
|
||||
Reference in New Issue
Block a user