From 2dfa8f1c942f265d095e19db54d831668f5f42b7 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: Mon, 13 Apr 2026 17:20:32 +1000 Subject: [PATCH] Add foxglove_bridge configuration and settings support --- src/iiwa_bringup/launch/iiwa.launch.py | 41 +++-- src/iiwa_config/config/setting.yaml | 28 ++++ src/iiwa_utils/iiwa_utils/setting_loader.py | 162 ++++++++++++++------ 3 files changed, 163 insertions(+), 68 deletions(-) diff --git a/src/iiwa_bringup/launch/iiwa.launch.py b/src/iiwa_bringup/launch/iiwa.launch.py index 09cddf1..7bbb205 100644 --- a/src/iiwa_bringup/launch/iiwa.launch.py +++ b/src/iiwa_bringup/launch/iiwa.launch.py @@ -1,3 +1,5 @@ +from dataclasses import asdict + from launch import LaunchDescription from launch.actions import ( DeclareLaunchArgument, @@ -18,12 +20,19 @@ from moveit_configs_utils import MoveItConfigsBuilder from iiwa_utils import converter, setting_loader +def _foxglove_params(fg, use_sim_time: bool) -> dict: + params = asdict(fg) + params.pop("enabled") + params["use_sim_time"] = use_sim_time + + return params + + def _runtime_setup(context, *args, **kwargs): setup = [] # Настройка параметров simulate = LaunchConfiguration("simulate").perform(context) in ("true", "1", "yes") - foxglove = LaunchConfiguration("foxglove").perform(context) in ("true", "1", "yes") settings = setting_loader.build_settings( settings_path=LaunchConfiguration("setting").perform(context), @@ -137,7 +146,7 @@ def _runtime_setup(context, *args, **kwargs): ] ) ), - launch_arguments=controller_args.items(), + launch_arguments={k: str(v) for k, v in controller_args.items()}.items(), ) # Moveit launch @@ -201,19 +210,16 @@ def _runtime_setup(context, *args, **kwargs): shutdown_on_rviz_exit ] - if foxglove: - pass + if settings.foxglove.enabled: + foxglove_bridge = Node( + package="foxglove_bridge", + executable="foxglove_bridge", + output="screen", + name="foxglove_bridge", + parameters=[_foxglove_params(settings.foxglove, use_sim_time)] + ) - # PythonLaunchDescriptionSource( - # PathJoinSubstitution( - # [ - # FindPackageShare("iiwa_bringup"), - # "launch", - # "supported", - # "controllers.launch.py", - # ] - # ) - # ), + setup += [foxglove_bridge] return setup @@ -238,19 +244,12 @@ def generate_launch_description(): description="Путь к файлу настроек", ) - declare_foxglove = DeclareLaunchArgument( - name="foxglove", - default_value="false", - description="true = запустить foxglove стриминг" - ) - runtime_setup = OpaqueFunction(function=_runtime_setup) return LaunchDescription([ declare_simulate, declare_rviz, declare_setting, - declare_foxglove, runtime_setup, ]) diff --git a/src/iiwa_config/config/setting.yaml b/src/iiwa_config/config/setting.yaml index d50cc52..39ff7e8 100644 --- a/src/iiwa_config/config/setting.yaml +++ b/src/iiwa_config/config/setting.yaml @@ -28,3 +28,31 @@ controller: moveit_controllers: pkg://iiwa_config/config/moveit/moveit_controllers.yaml moveit_cpp: pkg://iiwa_config/config/moveit/moveit_cpp.yaml + +foxglove: + enabled: true # Запускать ли foxglove_bridge вместе с роботом + port: 8765 # WebSocket-порт, к которому подключается Foxglove Studio (по умолчанию 8765) + debug: false # Включить подробное логирование bridge-процесса + address: 0.0.0.0 # Адрес, на котором слушает сервер. 0.0.0.0 — все интерфейсы, 127.0.0.1 — только локально + tls: false # Включить TLS-шифрование соединения (нужны certfile + keyfile) + certfile: "" # Путь к SSL-сертификату (нужен только при tls: true)Путь к SSL-сертификату (нужен только при tls: true) + keyfile: "" # Путь к SSL-сертификату (нужен только при tls: true) + topic_whitelist: ['.*'] # Regex-список топиков, которые bridge публикует клиенту + param_whitelist: ['.*'] # Regex-список ROS-параметров, видимых клиенту + service_whitelist: ['.*'] # Regex-список сервисов, доступных клиенту + client_topic_whitelist: ['.*'] # Regex-список топиков, в которые клиент может публиковать (clientPublish) + min_qos_depth: 1 # Минимальная глубина QoS-очереди при подписке bridge на топик + max_qos_depth: 10 # Максимальная глубина QoS-очереди + num_threads: 0 # Число потоков исполнения. 0 = автоматически по числу CPU + send_buffer_limit: 10000000 # Максимальный размер буфера отправки в байтах (защита от OOM при медленном клиенте) + use_sim_time: false # Использовать симуляционное время /clock вместо системного + capabilities: # Список возможностей, открытых клиенту + - clientPublish + - parameters + - parametersSubscribe + - services + - connectionGraph + - assets + include_hidden: false # Показывать клиенту скрытые топики и сервисы (начинаются с _) + asset_uri_allowlist: ['^package://(?:[-\w%]+/)*[-\w%.]+\.(?:dae|fbx|glb|gltf|jpeg|jpg|mtl|obj|png|stl|tif|tiff|urdf|webp|xacro)$'] # Regex-список URI вида package://..., из которых bridge разрешает отдавать файлы-ассеты (URDF, mesh и т.п.) + ignore_unresponsive_param_nodes: true # Не падать, если нода не отвечает на запросы параметров (защита от зависания при старте) \ No newline at end of file diff --git a/src/iiwa_utils/iiwa_utils/setting_loader.py b/src/iiwa_utils/iiwa_utils/setting_loader.py index 0190afb..e0cfd2d 100644 --- a/src/iiwa_utils/iiwa_utils/setting_loader.py +++ b/src/iiwa_utils/iiwa_utils/setting_loader.py @@ -1,6 +1,6 @@ import os from dataclasses import dataclass, fields, is_dataclass -from typing import Any, Dict, Type, TypeVar +from typing import Any, Dict, List, Optional, Type, TypeVar import yaml from ament_index_python.packages import get_package_share_directory @@ -35,6 +35,7 @@ class DigitalTwinCfg: webots: WebotsCfg rviz: RvizCfg + @dataclass(frozen=True) class MoveitCfg: srdf: str @@ -52,6 +53,30 @@ class ControllerCfg: moveit: MoveitCfg +@dataclass(frozen=True) +class FoxgloveCfg: + enabled: bool # Запускать ли foxglove_bridge + port: int # WebSocket-порт (обычно 8765) + address: str # Адрес прослушивания (0.0.0.0 = все интерфейсы) + debug: bool # Подробное логирование bridge + tls: bool # Включить TLS-шифрование + certfile: str # Путь к SSL-сертификату (при tls=true) + keyfile: str # Путь к приватному ключу SSL (при tls=true) + topic_whitelist: List[str] # Regex топиков, публикуемых клиенту + param_whitelist: List[str] # Regex ROS-параметров, видимых клиенту + service_whitelist: List[str] # Regex сервисов, доступных клиенту + client_topic_whitelist: List[str] # Regex топиков, в которые клиент может писать + min_qos_depth: int # Минимальная глубина QoS-очереди + max_qos_depth: int # Максимальная глубина QoS-очереди + num_threads: int # Потоки bridge (0 = авто по CPU) + send_buffer_limit: int # Лимит буфера отправки в байтах + use_sim_time: bool # Использовать /clock вместо системного времени + capabilities: List[str] # Возможности, открытые клиенту + include_hidden: bool # Показывать скрытые топики/сервисы + asset_uri_allowlist: List[str] # Разрешённые package://-URI для отдачи ассетов + ignore_unresponsive_param_nodes: bool # Не падать при зависших param-нодах + + class SettingsError(RuntimeError): pass @@ -61,10 +86,9 @@ class Settings: robot: RobotCfg digital_twin: DigitalTwinCfg controller: ControllerCfg + foxglove: FoxgloveCfg def to_dict(self) -> Dict[str, Any]: - """Serialize dataclass tree -> dict (recursive).""" - def _convert(obj: Any) -> Any: if is_dataclass(obj): return {f.name: _convert(getattr(obj, f.name)) for f in fields(obj)} @@ -90,6 +114,7 @@ class Settings: return build_settings(settings_path=settings_path, check_files=check_files) +# Helpers def load_yaml(path: str) -> Dict[str, Any]: path = os.path.abspath(path) if not os.path.exists(path): @@ -102,29 +127,18 @@ def load_yaml(path: str) -> Dict[str, Any]: def resolve_path(value: str, settings_dir: str) -> str: - """ - Supported: - - pkg:/// - - /abs/path - - ./relative or relative (relative to settings.yaml dir) - Returns absolute normalized path. - """ if not isinstance(value, str) or not value.strip(): raise SettingsError("empty path value in settings") - value = value.strip() - if value.startswith("pkg://"): - rest = value[len("pkg://") :] + rest = value[len("pkg://"):] if "/" not in rest: raise SettingsError(f"bad pkg uri: {value} (need pkg:///)") pkg, rel = rest.split("/", 1) share = get_package_share_directory(pkg) return os.path.normpath(os.path.join(share, rel)) - if os.path.isabs(value): return os.path.normpath(value) - return os.path.normpath(os.path.join(settings_dir, value)) @@ -141,6 +155,72 @@ def assert_file(path: str, key: str) -> None: raise SettingsError(f"path for '{key}' is not a file: {path}") +# Foxglove defaults +_FOXGLOVE_DEFAULTS: Dict[str, Any] = { + "enabled": False, + "port": 8765, + "address": "0.0.0.0", + "debug": False, + "tls": False, + "certfile": "", + "keyfile": "", + "topic_whitelist": [".*"], + "param_whitelist": [".*"], + "service_whitelist": [".*"], + "client_topic_whitelist": [".*"], + "min_qos_depth": 1, + "max_qos_depth": 10, + "num_threads": 0, + "send_buffer_limit": 10_000_000, + "use_sim_time": False, + "capabilities": [ + "clientPublish", + "parameters", + "parametersSubscribe", + "services", + "connectionGraph", + "assets", + ], + "include_hidden": False, + "asset_uri_allowlist": [ + r"^package://(?:[-\w%]+/)*[-\w%.]+\.(?:dae|fbx|glb|gltf|jpeg|jpg|mtl|obj|png|stl|tif|tiff|urdf|webp|xacro)$" + ], + "ignore_unresponsive_param_nodes": True, +} + + +def _parse_foxglove(raw: Optional[Dict[str, Any]]) -> FoxgloveCfg: + """Парсит секцию foxglove, подставляя дефолты для пропущенных полей.""" + if raw is None: + raw = {} + + def get(key: str) -> Any: + return raw.get(key, _FOXGLOVE_DEFAULTS[key]) + + return FoxgloveCfg( + enabled=bool(get("enabled")), + port=int(get("port")), + address=str(get("address")), + debug=bool(get("debug")), + tls=bool(get("tls")), + certfile=str(get("certfile")), + keyfile=str(get("keyfile")), + topic_whitelist=list(get("topic_whitelist")), + param_whitelist=list(get("param_whitelist")), + service_whitelist=list(get("service_whitelist")), + client_topic_whitelist=list(get("client_topic_whitelist")), + min_qos_depth=int(get("min_qos_depth")), + max_qos_depth=int(get("max_qos_depth")), + num_threads=int(get("num_threads")), + send_buffer_limit=int(get("send_buffer_limit")), + use_sim_time=bool(get("use_sim_time")), + capabilities=list(get("capabilities")), + include_hidden=bool(get("include_hidden")), + asset_uri_allowlist=list(get("asset_uri_allowlist")), + ignore_unresponsive_param_nodes=bool(get("ignore_unresponsive_param_nodes")), + ) + + def build_settings(settings_path: str, check_files: bool = True) -> Settings: settings_path = os.path.abspath(settings_path) settings_dir = os.path.dirname(settings_path) @@ -166,64 +246,52 @@ def build_settings(settings_path: str, check_files: bool = True) -> Settings: world=resolve_path(str(require(webots_raw, "world")), settings_dir), transform=str(require(webots_raw, "transform")), rotation=str(require(webots_raw, "rotation")), - controller_timer=int(require(webots_raw, "controller_timer")), + controller_timer=str(int(require(webots_raw, "controller_timer"))), ) - - rviz = RvizCfg(config=resolve_path(str(require(rviz_raw, "config")), settings_dir)) - - digital_twin = DigitalTwinCfg( - webots=webots, - rviz=rviz + rviz = RvizCfg( + config=resolve_path(str(require(rviz_raw, "config")), settings_dir) ) + digital_twin = DigitalTwinCfg(webots=webots, rviz=rviz) - # controller, moveit + # controller + moveit ctrl_raw = require(raw, "controller") moveit_raw = require(ctrl_raw, "moveit") moveit = MoveitCfg( srdf=resolve_path(str(require(moveit_raw, "srdf")), settings_dir), kinematics=resolve_path(str(require(moveit_raw, "kinematics")), settings_dir), - joint_limits=resolve_path( - str(require(moveit_raw, "joint_limits")), settings_dir - ), + joint_limits=resolve_path(str(require(moveit_raw, "joint_limits")), settings_dir), pilz_limits=resolve_path(str(require(moveit_raw, "pilz_limits")), settings_dir), - initial_positions=resolve_path( - str(require(moveit_raw, "initial_positions")), settings_dir - ), - moveit_controllers=resolve_path( - str(require(moveit_raw, "moveit_controllers")), settings_dir - ), + initial_positions=resolve_path(str(require(moveit_raw, "initial_positions")), settings_dir), + moveit_controllers=resolve_path(str(require(moveit_raw, "moveit_controllers")), settings_dir), moveit_cpp=resolve_path(str(require(moveit_raw, "moveit_cpp")), settings_dir), ) - controller = ControllerCfg( - controller_path=resolve_path( - str(require(ctrl_raw, "controller_path")), settings_dir - ), + controller_path=resolve_path(str(require(ctrl_raw, "controller_path")), settings_dir), moveit=moveit, ) - s = Settings(robot=robot, digital_twin=digital_twin, controller=controller) + # foxglove + foxglove = _parse_foxglove(raw.get("foxglove")) + + s = Settings( + robot=robot, + digital_twin=digital_twin, + controller=controller, + foxglove=foxglove, + ) if check_files: 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") assert_file(s.controller.controller_path, "controller.controller_path") - assert_file(s.controller.moveit.srdf, "controller.moveit.srdf") assert_file(s.controller.moveit.kinematics, "controller.moveit.kinematics") assert_file(s.controller.moveit.joint_limits, "controller.moveit.joint_limits") assert_file(s.controller.moveit.pilz_limits, "controller.moveit.pilz_limits") - assert_file( - s.controller.moveit.initial_positions, "controller.moveit.initial_positions" - ) - assert_file( - s.controller.moveit.moveit_controllers, - "controller.moveit.moveit_controllers", - ) + assert_file(s.controller.moveit.initial_positions, "controller.moveit.initial_positions") + assert_file(s.controller.moveit.moveit_controllers, "controller.moveit.moveit_controllers") assert_file(s.controller.moveit.moveit_cpp, "controller.moveit.moveit_cpp") return s - -