Add camera spawning functionality and update configuration files
This commit is contained in:
Даниил Грабарь
@@ -1,3 +1,4 @@
|
||||
import json
|
||||
from dataclasses import asdict
|
||||
|
||||
from launch import LaunchDescription
|
||||
@@ -16,8 +17,10 @@ from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
from moveit_configs_utils import MoveItConfigsBuilder
|
||||
from webots_ros2_driver.webots_controller import WebotsController
|
||||
|
||||
from iiwa_utils import converter, setting_loader
|
||||
from iiwa_utils.camera_spawner import load_camera_config, build_ros_urdf # type: ignore
|
||||
|
||||
|
||||
def _foxglove_params(fg, use_sim_time: bool) -> dict:
|
||||
@@ -112,7 +115,7 @@ def _runtime_setup(context, *args, **kwargs):
|
||||
)
|
||||
|
||||
setup += [webots_launch]
|
||||
|
||||
|
||||
# Controller launch
|
||||
if simulate:
|
||||
controller_args = {
|
||||
@@ -126,38 +129,34 @@ def _runtime_setup(context, *args, **kwargs):
|
||||
"controller_timer": str(settings.digital_twin.webots.controller_timer),
|
||||
}
|
||||
|
||||
# TODO пример работы, необходимо правильно интегрировать в структуру проекта и удалить из-за избыточности
|
||||
if settings.digital_twin.webots.cameras:
|
||||
# Спавн камеры
|
||||
camera_spawner_node = Node(
|
||||
package="iiwa_utils",
|
||||
executable="camera_spawner",
|
||||
name="camera_spawner",
|
||||
output="screen",
|
||||
parameters=[{
|
||||
"camera_configs": json.dumps(settings.digital_twin.webots.cameras)
|
||||
}],
|
||||
)
|
||||
setup.append(camera_spawner_node)
|
||||
|
||||
# example_camera_urdf = """<?xml version="1.0"?>
|
||||
# <robot name="example_camera_robot">
|
||||
# <link name="example_camera_link"/>
|
||||
# <webots>
|
||||
# <device reference="example_camera" type="Camera">
|
||||
# <ros>
|
||||
# <topicName>/example_camera/image_raw</topicName>
|
||||
# <updateRate>30</updateRate>
|
||||
# <alwaysOn>True</alwaysOn>
|
||||
# <frameName>example_camera_link</frameName>
|
||||
# </ros>
|
||||
# </device>
|
||||
# </webots>
|
||||
# </robot>
|
||||
# """
|
||||
# WebotsController для каждой камеры
|
||||
for cam_path in settings.digital_twin.webots.cameras:
|
||||
cam_cfg = load_camera_config(cam_path)
|
||||
urdf = build_ros_urdf(cam_cfg)
|
||||
|
||||
# from webots_ros2_driver.webots_controller import WebotsController
|
||||
|
||||
# example_camera_driver = WebotsController(
|
||||
# robot_name="example_camera_robot",
|
||||
# parameters=[
|
||||
# {
|
||||
# "robot_description": example_camera_urdf,
|
||||
# "use_sim_time": True,
|
||||
# "set_robot_state_publisher": False,
|
||||
# }
|
||||
# ],
|
||||
# respawn=True,
|
||||
# )
|
||||
# setup += [example_camera_driver]
|
||||
camera_controller = WebotsController(
|
||||
robot_name=f"{cam_cfg.name}_robot",
|
||||
parameters=[{
|
||||
"robot_description": urdf,
|
||||
"use_sim_time": True,
|
||||
"set_robot_state_publisher": False,
|
||||
}],
|
||||
respawn=True,
|
||||
)
|
||||
setup.append(camera_controller)
|
||||
|
||||
else:
|
||||
controller_args = {
|
||||
|
||||
@@ -1,4 +1,52 @@
|
||||
# Пример использования параметров камеры
|
||||
name: D455_TOP
|
||||
translation: "-0.25 0 0.79"
|
||||
rotation: "0 0 1 0"
|
||||
# Имя камеры - используется как:
|
||||
# - имя Robot-ноды в Webots: "{name}_robot"
|
||||
# - имя Camera-девайса: "{name}"
|
||||
# - имя RangeFinder-девайса: "{name}_depth"
|
||||
# - префикс ROS2-топиков
|
||||
|
||||
name: d455_top
|
||||
# Положение в сцене (x y z)
|
||||
translation: "-0.25 0 1.5"
|
||||
# Ориентация (ось_x ось_y ось_z угол_рад)
|
||||
rotation: "0 0 1 0"
|
||||
|
||||
# Блок camera
|
||||
# Если блок отсутствует — Camera-девайс не создаётся
|
||||
# Поля соответствуют документации Webots:
|
||||
# https://cyberbotics.com/doc/reference/camera
|
||||
camera:
|
||||
width: 640
|
||||
height: 480
|
||||
fieldOfView: 1.047 # ~60° в радианах
|
||||
near: 0.1
|
||||
far: 0.0 # 0 = без ограничений
|
||||
exposure: 1.0
|
||||
antiAliasing: false
|
||||
ambientOcclusionRadius: 0.0
|
||||
bloomThreshold: -1.0 # -1 = отключён
|
||||
noise: 0.0
|
||||
motionBlur: 0.0
|
||||
|
||||
# Параметры публикации в ROS2 (не часть Webots-ноды, обрабатываются отдельно)
|
||||
ros:
|
||||
topic: /d455_top/image_raw
|
||||
update_rate: 30
|
||||
|
||||
|
||||
# Блок range_finder
|
||||
# Если блок отсутствует - RangeFinder-девайс не создаётся
|
||||
# Поля соответствуют документации Webots:
|
||||
# https://cyberbotics.com/doc/reference/rangefinder
|
||||
# range_finder:
|
||||
# width: 640
|
||||
# height: 480
|
||||
# fieldOfView: 1.047
|
||||
# minRange: 0.1
|
||||
# maxRange: 5.0
|
||||
# resolution: -1.0 # -1 = без ограничений по дискретизации
|
||||
# noise: 0.0
|
||||
|
||||
# # Параметры публикации в ROS2
|
||||
# ros:
|
||||
# topic: /d455_top/depth/image_raw
|
||||
# update_rate: 15
|
||||
@@ -14,6 +14,9 @@ digital_twin:
|
||||
rotation: "0 0 1 0"
|
||||
controller_timer: "50"
|
||||
|
||||
cameras:
|
||||
- pkg://iiwa_config/config/cameras/d455_top.yaml
|
||||
|
||||
rviz:
|
||||
config: pkg://iiwa_config/config/rviz/rviz_moveit.rviz
|
||||
|
||||
|
||||
@@ -1,106 +0,0 @@
|
||||
<sdf version='1.10'>
|
||||
<world name='empty'>
|
||||
<physics name='1ms' type='ignored'>
|
||||
<max_step_size>0.001</max_step_size>
|
||||
<real_time_factor>1</real_time_factor>
|
||||
<real_time_update_rate>1000</real_time_update_rate>
|
||||
</physics>
|
||||
<plugin name='gz::sim::systems::Physics' filename='gz-sim-physics-system'/>
|
||||
<plugin name='gz::sim::systems::UserCommands' filename='gz-sim-user-commands-system'/>
|
||||
<plugin name='gz::sim::systems::SceneBroadcaster' filename='gz-sim-scene-broadcaster-system'/>
|
||||
<plugin name='gz::sim::systems::Contact' filename='gz-sim-contact-system'/>
|
||||
|
||||
<plugin filename="WorldControl" name="World control">
|
||||
<gz-gui>
|
||||
<title>Kuka iiwa world</title>
|
||||
<property type="string" key="state">floating</property>
|
||||
<anchors target="3D View">
|
||||
<line own="left" target="left"/>
|
||||
<line own="bottom" target="bottom"/>
|
||||
</anchors>
|
||||
</gz-gui>
|
||||
|
||||
<play_pause>true</play_pause>
|
||||
<step>false</step>
|
||||
<start_paused>false</start_paused>
|
||||
</plugin>
|
||||
|
||||
<gravity>0 0 -9.8000000000000007</gravity>
|
||||
<magnetic_field>5.5644999999999998e-06 2.2875799999999999e-05 -4.2388400000000002e-05</magnetic_field>
|
||||
<atmosphere type='adiabatic'/>
|
||||
<scene>
|
||||
<ambient>0.400000006 0.400000006 0.400000006 1</ambient>
|
||||
<background>0.699999988 0.699999988 0.699999988 1</background>
|
||||
<shadows>true</shadows>
|
||||
</scene>
|
||||
<model name='ground_plane'>
|
||||
<static>true</static>
|
||||
<link name='link'>
|
||||
<collision name='collision'>
|
||||
<geometry>
|
||||
<plane>
|
||||
<normal>0 0 1</normal>
|
||||
<size>100 100</size>
|
||||
</plane>
|
||||
</geometry>
|
||||
<surface>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<bounce/>
|
||||
<contact/>
|
||||
</surface>
|
||||
</collision>
|
||||
<visual name='visual'>
|
||||
<geometry>
|
||||
<plane>
|
||||
<normal>0 0 1</normal>
|
||||
<size>100 100</size>
|
||||
</plane>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>0.800000012 0.800000012 0.800000012 1</ambient>
|
||||
<diffuse>0.800000012 0.800000012 0.800000012 1</diffuse>
|
||||
<specular>0.800000012 0.800000012 0.800000012 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<inertial>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<mass>1</mass>
|
||||
<inertia>
|
||||
<ixx>1</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>1</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>1</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<enable_wind>false</enable_wind>
|
||||
</link>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<self_collide>false</self_collide>
|
||||
</model>
|
||||
|
||||
<light name='sun' type='directional'>
|
||||
<pose>0 0 10 0 0 0</pose>
|
||||
<cast_shadows>true</cast_shadows>
|
||||
<intensity>1</intensity>
|
||||
<direction>-0.5 0.10000000000000001 -0.90000000000000002</direction>
|
||||
<diffuse>0.800000012 0.800000012 0.800000012 1</diffuse>
|
||||
<specular>0.200000003 0.200000003 0.200000003 1</specular>
|
||||
<attenuation>
|
||||
<range>1000</range>
|
||||
<linear>0.01</linear>
|
||||
<constant>0.90000000000000002</constant>
|
||||
<quadratic>0.001</quadratic>
|
||||
</attenuation>
|
||||
<spot>
|
||||
<inner_angle>0</inner_angle>
|
||||
<outer_angle>0</outer_angle>
|
||||
<falloff>0</falloff>
|
||||
</spot>
|
||||
</light>
|
||||
</world>
|
||||
</sdf>
|
||||
@@ -17,14 +17,3 @@ TexturedBackgroundLight {
|
||||
RectangleArena {
|
||||
floorSize 5 5
|
||||
}
|
||||
|
||||
Robot {
|
||||
translation -1.26 0 0.77
|
||||
children [
|
||||
Camera {
|
||||
name "example_camera"
|
||||
}
|
||||
]
|
||||
name "example_camera_robot"
|
||||
controller "<extern>"
|
||||
}
|
||||
|
||||
@@ -1,56 +1,306 @@
|
||||
from dataclasses import dataclass
|
||||
"""
|
||||
Документация Webots:
|
||||
Camera: https://cyberbotics.com/doc/reference/camera
|
||||
RangeFinder: https://cyberbotics.com/doc/reference/rangefinder
|
||||
"""
|
||||
|
||||
import json
|
||||
import os
|
||||
from dataclasses import dataclass
|
||||
from typing import Any, Dict, List, Optional
|
||||
|
||||
import yaml
|
||||
import rclpy
|
||||
from rclpy.executors import ExternalShutdownException
|
||||
from rclpy.node import Node
|
||||
from rclpy.task import Future
|
||||
from webots_ros2_msgs.srv import SpawnNodeFromString
|
||||
from rclpy.executors import ExternalShutdownException
|
||||
|
||||
|
||||
_SKIP_FIELDS = {"ros"}
|
||||
_CAMERA_BOOL_FIELDS = {"antiAliasing"}
|
||||
_RANGE_FINDER_BOOL_FIELDS: set = set()
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CameraSpawnParams:
|
||||
camera_name: str
|
||||
class RosCfg:
|
||||
topic: str
|
||||
update_rate: int
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CameraDeviceCfg:
|
||||
webots_fields: Dict[str, Any]
|
||||
ros: RosCfg
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class RangeFinderDeviceCfg:
|
||||
webots_fields: Dict[str, Any]
|
||||
ros: RosCfg
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CameraConfig:
|
||||
name: str
|
||||
translation: str
|
||||
rotation: str
|
||||
|
||||
camera: Optional[CameraDeviceCfg]
|
||||
range_finder: Optional[RangeFinderDeviceCfg]
|
||||
|
||||
|
||||
def _parse_device_block(
|
||||
raw: Optional[Dict[str, Any]],
|
||||
bool_fields: set,
|
||||
default_topic: str,
|
||||
default_rate: int,
|
||||
) -> Optional[Dict[str, Any]]:
|
||||
"""
|
||||
Парсит блок camera или range_finder из YAML.
|
||||
Возвращает None если блок отсутствует.
|
||||
"""
|
||||
if raw is None:
|
||||
return None
|
||||
|
||||
ros_raw = raw.get("ros", {})
|
||||
ros = RosCfg(
|
||||
topic=str(ros_raw.get("topic", default_topic)),
|
||||
update_rate=int(ros_raw.get("update_rate", default_rate)),
|
||||
)
|
||||
|
||||
webots_fields: Dict[str, Any] = {}
|
||||
for key, value in raw.items():
|
||||
if key in _SKIP_FIELDS:
|
||||
continue
|
||||
if key in bool_fields:
|
||||
webots_fields[key] = "TRUE" if value else "FALSE"
|
||||
else:
|
||||
webots_fields[key] = value
|
||||
|
||||
return {"webots_fields": webots_fields, "ros": ros}
|
||||
|
||||
|
||||
def load_camera_config(path: str) -> CameraConfig:
|
||||
"""Загружает camera YAML и возвращает CameraConfig."""
|
||||
path = os.path.abspath(path)
|
||||
if not os.path.exists(path):
|
||||
raise FileNotFoundError(f"Camera config not found: {path}")
|
||||
|
||||
with open(path, "r", encoding="utf-8") as f:
|
||||
raw: Dict[str, Any] = yaml.safe_load(f) or {}
|
||||
|
||||
name = str(raw["name"])
|
||||
translation = str(raw["translation"])
|
||||
rotation = str(raw["rotation"])
|
||||
|
||||
cam_raw = _parse_device_block(
|
||||
raw.get("camera"),
|
||||
_CAMERA_BOOL_FIELDS,
|
||||
default_topic=f"/{name}/image_raw",
|
||||
default_rate=30,
|
||||
)
|
||||
rf_raw = _parse_device_block(
|
||||
raw.get("range_finder"),
|
||||
_RANGE_FINDER_BOOL_FIELDS,
|
||||
default_topic=f"/{name}/depth/image_raw",
|
||||
default_rate=15,
|
||||
)
|
||||
|
||||
camera = (
|
||||
CameraDeviceCfg(webots_fields=cam_raw["webots_fields"], ros=cam_raw["ros"])
|
||||
if cam_raw is not None else None
|
||||
)
|
||||
range_finder = (
|
||||
RangeFinderDeviceCfg(webots_fields=rf_raw["webots_fields"], ros=rf_raw["ros"])
|
||||
if rf_raw is not None else None
|
||||
)
|
||||
|
||||
return CameraConfig(
|
||||
name=name,
|
||||
translation=translation,
|
||||
rotation=rotation,
|
||||
camera=camera,
|
||||
range_finder=range_finder,
|
||||
)
|
||||
|
||||
|
||||
|
||||
def _build_device_proto(device_type: str, device_name: str, fields: Dict[str, Any]) -> str:
|
||||
"""Строит PROTO-строку для Camera или RangeFinder."""
|
||||
parts = [f'name "{device_name}"']
|
||||
for key, value in fields.items():
|
||||
if isinstance(value, str) and value not in ("TRUE", "FALSE"):
|
||||
parts.append(f'{key} "{value}"')
|
||||
else:
|
||||
parts.append(f'{key} {value}')
|
||||
return f'{device_type} {{ {" ".join(parts)} }}'
|
||||
|
||||
|
||||
def build_robot_proto(cfg: CameraConfig) -> str:
|
||||
"""
|
||||
Строит Webots PROTO-строку Robot-ноды, содержащей Camera и/или RangeFinder.
|
||||
Результат передаётся в SpawnNodeFromString.Request.data.
|
||||
"""
|
||||
children: List[str] = []
|
||||
|
||||
if cfg.camera is not None:
|
||||
children.append(
|
||||
_build_device_proto("Camera", cfg.name, cfg.camera.webots_fields)
|
||||
)
|
||||
|
||||
if cfg.range_finder is not None:
|
||||
children.append(
|
||||
_build_device_proto("RangeFinder", f"{cfg.name}_depth", cfg.range_finder.webots_fields)
|
||||
)
|
||||
|
||||
children_str = " ".join(children)
|
||||
|
||||
return (
|
||||
f'Robot {{'
|
||||
f' name "{cfg.name}_robot"'
|
||||
f' translation {cfg.translation}'
|
||||
f' rotation {cfg.rotation}'
|
||||
f' children [ {children_str} ]'
|
||||
f' controller "<extern>"'
|
||||
f' }}'
|
||||
)
|
||||
|
||||
|
||||
def build_ros_urdf(cfg: CameraConfig) -> str:
|
||||
"""
|
||||
Строит URDF-строку для WebotsController.
|
||||
Описывает ROS2-интерфейс Camera и/или RangeFinder.
|
||||
"""
|
||||
devices: List[str] = []
|
||||
|
||||
if cfg.camera is not None:
|
||||
ros = cfg.camera.ros
|
||||
devices.append(
|
||||
f' <device reference="{cfg.name}" type="Camera">\n'
|
||||
f' <ros>\n'
|
||||
f' <topicName>{ros.topic}</topicName>\n'
|
||||
f' <updateRate>{ros.update_rate}</updateRate>\n'
|
||||
f' <alwaysOn>True</alwaysOn>\n'
|
||||
f' <frameName>{cfg.name}_link</frameName>\n'
|
||||
f' </ros>\n'
|
||||
f' </device>'
|
||||
)
|
||||
|
||||
if cfg.range_finder is not None:
|
||||
ros = cfg.range_finder.ros
|
||||
devices.append(
|
||||
f' <device reference="{cfg.name}_depth" type="RangeFinder">\n'
|
||||
f' <ros>\n'
|
||||
f' <topicName>{ros.topic}</topicName>\n'
|
||||
f' <updateRate>{ros.update_rate}</updateRate>\n'
|
||||
f' <alwaysOn>True</alwaysOn>\n'
|
||||
f' <frameName>{cfg.name}_link</frameName>\n'
|
||||
f' </ros>\n'
|
||||
f' </device>'
|
||||
)
|
||||
|
||||
devices_str = "\n".join(devices)
|
||||
|
||||
return (
|
||||
f'<?xml version="1.0"?>\n'
|
||||
f'<robot name="{cfg.name}_robot">\n'
|
||||
f' <link name="{cfg.name}_link"/>\n'
|
||||
f' <webots>\n'
|
||||
f'{devices_str}\n'
|
||||
f' </webots>\n'
|
||||
f'</robot>'
|
||||
)
|
||||
|
||||
|
||||
class CameraSpawner(Node):
|
||||
"""
|
||||
Спавнит камеры в Webots через /Ros2Supervisor/spawn_node_from_string.
|
||||
|
||||
def __init__(self, camera_params: CameraSpawnParams):
|
||||
Параметры ROS2 ноды:
|
||||
camera_configs (string): JSON-массив абсолютных путей до camera YAML файлов.
|
||||
"""
|
||||
|
||||
def __init__(self):
|
||||
super().__init__("camera_spawner")
|
||||
|
||||
self._camera_params = camera_params
|
||||
self._urdf_template = self._generate_camera_urdf()
|
||||
self._proto_template = self._generate_camera_proto()
|
||||
self.declare_parameter("camera_configs", "[]")
|
||||
configs_json = (
|
||||
self.get_parameter("camera_configs")
|
||||
.get_parameter_value()
|
||||
.string_value
|
||||
)
|
||||
config_paths: List[str] = json.loads(configs_json)
|
||||
|
||||
self._pending: List[CameraConfig] = []
|
||||
for path in config_paths:
|
||||
try:
|
||||
cfg = load_camera_config(path)
|
||||
self._pending.append(cfg)
|
||||
self.get_logger().info(f"Loaded camera config: '{cfg.name}' from {path}")
|
||||
except Exception as e:
|
||||
self.get_logger().error(f"Failed to load camera config '{path}': {e}")
|
||||
|
||||
if not self._pending:
|
||||
self.get_logger().info("No cameras to spawn.")
|
||||
return
|
||||
|
||||
self._in_flight = False
|
||||
|
||||
self._client = self.create_client(
|
||||
SpawnNodeFromString, "/Ros2Supervisor/spawn_node_from_string"
|
||||
)
|
||||
|
||||
self.get_logger().info("Waiting for /Ros2Supervisor/spawn_node_from_string...")
|
||||
while not self._client.wait_for_service(timeout_sec=10.0):
|
||||
self.get_logger().warning(
|
||||
"Service /Ros2Supervisor/spawn_node_from_string not available, retrying..."
|
||||
)
|
||||
|
||||
self._timer = self.create_timer(0.1, self._tick)
|
||||
|
||||
def _tick(self):
|
||||
if not self._pending:
|
||||
self.get_logger().info("All cameras spawned.")
|
||||
self._timer.cancel()
|
||||
return
|
||||
|
||||
if self._in_flight:
|
||||
return
|
||||
|
||||
cfg = self._pending[0]
|
||||
proto = build_robot_proto(cfg)
|
||||
|
||||
self.get_logger().info(f"Spawning camera '{cfg.name}'...")
|
||||
self.get_logger().debug(f"Proto:\n{proto}")
|
||||
|
||||
req = SpawnNodeFromString.Request(data=proto, check_fields=True)
|
||||
self._in_flight = True
|
||||
future = self._client.call_async(req)
|
||||
future.add_done_callback(lambda f: self._on_spawned(f, cfg))
|
||||
|
||||
def _on_spawned(self, future: Future, cfg: CameraConfig):
|
||||
try:
|
||||
response = future.result()
|
||||
self.get_logger().info(f"Camera '{cfg.name}' spawned: {response}")
|
||||
except Exception as e:
|
||||
self.get_logger().error(f"Failed to spawn camera '{cfg.name}': {e}")
|
||||
finally:
|
||||
self._pending.pop(0)
|
||||
self._in_flight = False
|
||||
|
||||
|
||||
|
||||
# self.cli = self.create_client(
|
||||
# SpawnNodeFromString, "/Ros2Supervisor/spawn_node_from_string"
|
||||
# )
|
||||
def main(args=None):
|
||||
try:
|
||||
rclpy.init(args=args)
|
||||
node = CameraSpawner()
|
||||
rclpy.spin(node)
|
||||
except (KeyboardInterrupt, ExternalShutdownException):
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
# while not self.cli.wait_for_service(timeout_sec=10):
|
||||
# self.get_logger().warning(
|
||||
# "service /Ros2Supervisor/spawn_node_from_string not available, waiting again..."
|
||||
# )
|
||||
|
||||
# data = 'Camera { name "camera" translation 0 0 1.5 rotation 1 0 0 -1.5708 }'
|
||||
# req = SpawnNodeFromString.Request(data=data, check_fields=True)
|
||||
# self.get_logger().info(f"Calling spawn service for camera")
|
||||
# future = self.cli.call_async(req)
|
||||
# future.add_done_callback(self._response_callback)
|
||||
|
||||
# def _response_callback(self, future: Future):
|
||||
# try:
|
||||
# response = future.result()
|
||||
# self.get_logger().info("Camera spawned successfully")
|
||||
# except Exception as e:
|
||||
# self.get_logger().error(f"Service call failed: {e}")
|
||||
|
||||
def _generate_camera_proto(self) -> str:
|
||||
...
|
||||
|
||||
def _generate_camera_urdf(self) -> str:
|
||||
...
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
||||
@@ -23,6 +23,7 @@ class WebotsCfg:
|
||||
transform: str
|
||||
rotation: str
|
||||
controller_timer: str
|
||||
cameras: List[str]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
@@ -240,13 +241,17 @@ def build_settings(settings_path: str, check_files: bool = True) -> Settings:
|
||||
# digital_twin
|
||||
dt_raw = require(raw, "digital_twin")
|
||||
webots_raw = require(dt_raw, "webots")
|
||||
cameras_raw = webots_raw.get("cameras", [])
|
||||
rviz_raw = require(dt_raw, "rviz")
|
||||
|
||||
cameras = [resolve_path(str(c), settings_dir) for c in cameras_raw]
|
||||
|
||||
webots = WebotsCfg(
|
||||
world=resolve_path(str(require(webots_raw, "world")), settings_dir),
|
||||
transform=str(require(webots_raw, "transform")),
|
||||
rotation=str(require(webots_raw, "rotation")),
|
||||
controller_timer=str(int(require(webots_raw, "controller_timer"))),
|
||||
cameras=cameras,
|
||||
)
|
||||
rviz = RvizCfg(
|
||||
config=resolve_path(str(require(rviz_raw, "config")), settings_dir)
|
||||
@@ -282,6 +287,9 @@ def build_settings(settings_path: str, check_files: bool = True) -> Settings:
|
||||
)
|
||||
|
||||
if check_files:
|
||||
for i, cam_path in enumerate(s.digital_twin.webots.cameras):
|
||||
assert_file(cam_path, f"digital_twin.webots.cameras[{i}]")
|
||||
|
||||
assert_file(s.robot.description, "robot.description")
|
||||
assert_file(s.digital_twin.webots.world, "digital_twin.webots.world")
|
||||
assert_file(s.digital_twin.rviz.config, "digital_twin.rviz.config")
|
||||
|
||||
@@ -19,8 +19,9 @@ setup(
|
||||
license='Apache-2.0',
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'object_spawner = iiwa_utils.object_spawner:main',
|
||||
"motion_planning_test = iiwa_utils.motion_planing_test:main"
|
||||
"object_spawner = iiwa_utils.object_spawner:main",
|
||||
"motion_planning_test = iiwa_utils.motion_planing_test:main",
|
||||
"camera_spawner = iiwa_utils.camera_spawner:main",
|
||||
],
|
||||
},
|
||||
)
|
||||
|
||||
Reference in New Issue
Block a user