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

Add foxglove_bridge configuration and settings support

Browse Source
This commit is contained in:
Даниил Грабарь
2026-04-13 17:20:32 +10:00
parent d585c4f0b9
commit 2dfa8f1c94
3 changed files with 163 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/iiwa_bringup/launch/iiwa.launch.py
+20 -21
View File
@@ -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,
])
src/iiwa_config/config/setting.yaml
+28
View File
@@ -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 # Не падать, если нода не отвечает на запросы параметров (защита от зависания при старте)
src/iiwa_utils/iiwa_utils/setting_loader.py
+115 -47
View File
@@ -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://<pkg>/<rel>
- /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>/<path>)")
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