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

Merge branch 'dev'

Browse Source
This commit is contained in:
Даниил Грабарь
2026-06-16 13:16:45 +10:00
parent c66de51526 ddfbbda7db
commit 1a1ee6f027
318 changed files with 4435 additions and 45917 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+2
View File
@@ -10,3 +10,5 @@ __pycache__
*.egg-info
**.FCBak
CLAUDE.md
README.md
+14 -24
View File
@@ -201,30 +201,14 @@ ros2 service call /iiwa/stop std_srvs/srv/Trigger "{}"
Примеры использования `test_motion_sequence`:
```bash
# Просто выполнить последовательность без записи
ros2 run iiwa_utils test_motion_sequence \
--ros-args -p n_iterations:=3 \
-p delay_between_iterations:=5.0
# Записать все доступные топики в bag
ros2 run iiwa_utils test_motion_sequence \
--ros-args -p n_iterations:=5 \
-p delay_between_iterations:=5.0 \
-p bag_path:=/tmp/iiwa_session
# Записать конкретные топики
ros2 run iiwa_utils test_motion_sequence \
--ros-args -p n_iterations:=5 \
-p delay_between_iterations:=5.0 \
-p bag_path:=/tmp/iiwa_session \
-p topics:="['/joint_states', '/d455_top/color/image_raw', '/tf']"
# Использовать свой конфиг поз
ros2 run iiwa_utils test_motion_sequence \
--ros-args -p config_path:=/path/to/my_config.json \
-p n_iterations:=1 \
-p delay_between_iterations:=3.0 \
-p bag_path:=/tmp/iiwa_session
ros2 run iiwa_planning motion_sequence_runner \
--ros-args \
-p config_path:=/path/to/config.json \
-p n_iterations:=3 \
-p delay_between_iterations:=5.0 \
-p bag_path:=/tmp/my_bag \
-p joints_action:=my_ns/move_to_joints \
-p pose_action:=my_ns/move_to_pose
```
@@ -307,3 +291,9 @@ docker run -it --rm --network host evilfisru/lwa:jazzy-lwa7-noble
# dev - src остаётся для отладки
`docker build --build-arg BUILD_TYPE=dev -t my-image .`
```
```
# URL для доступа к MCP LLM
http://localhost:8007/mcp/mcp
```
cobot-setting.yaml
+10 -1
View File
@@ -2,7 +2,6 @@ robot:
name: "iiwa7"
ip: "192.170.10.2"
port: 30200
command_mode: "position" # torque, position
fri_cycle_ms: 10 # период FRI-цикла: 5 мс (200 Гц) или 10 мс (100 Гц)
joint_position_tau: 0.04 # EMA фильтр позиций [с]: сглаживает команды перед отправкой в FRI
joint_velocity_tau: 0.01 # EMA фильтр скорости [с]: убирает выбросы конечных разностей
@@ -37,6 +36,9 @@ controller:
moveit_cpp: pkg://iiwa_config/config/moveit/moveit_cpp.yaml
tool:
active: "patron" # Активный инструмент: none | patron | ... (из tools.yaml)
planning:
pose_link: "tcp" # TCP-линк для декартовых целей
planning_group: "iiwa_arm" # Группа планирования из SRDF
@@ -44,6 +46,13 @@ planning:
default_planner: "ompl" # Планировщик по умолчанию
planning_attempts: 3 # Число попыток планирования
web:
enabled: true
host: "0.0.0.0"
port: 8007
endpoints: pkg://iiwa_config/config/api_endpoints.yaml
joint_limits: pkg://iiwa_config/config/moveit/joint_limits.yaml
foxglove:
enabled: true # Запускать ли foxglove_bridge вместе с роботом
port: 8765 # WebSocket-порт, к которому подключается Foxglove Studio (по умолчанию 8765)
cobot/cli.py
+11 -2
View File
@@ -1,5 +1,4 @@
import argparse
import sys
# Import each command module so we can register its subparser.
# Импортируем каждый модуль команды, чтобы зарегистрировать его подпарсер.
@@ -96,7 +95,17 @@ def main():
_register_commands(subparsers)
args = parser.parse_args()
args.func(args)
# Install one SIGINT handler + atexit cleanup so a single Ctrl-C tears down
# any running subprocesses (builds, ros2 launch, docker) cleanly.
# Устанавливаем один обработчик SIGINT + очистку atexit, чтобы один Ctrl-C
# аккуратно завершал все запущенные подпроцессы (сборку, ros2 launch, docker).
from cobot import process, privilege
process.install_signal_handlers()
try:
args.func(args)
finally:
privilege.stop_keepalive()
def _register_commands(subparsers):
cobot/commands/clean.py
+28 -77
View File
@@ -3,11 +3,10 @@ from __future__ import annotations
import argparse
import shutil
from pathlib import Path
from typing import List, Optional
from typing import List
from textual.app import App
from cobot.tui import SCREEN_CSS, LogScreen, MultiPickScreen
from cobot import ui
from cobot.ui import header, done
_PROJECT_DIR = Path(__file__).parent.parent.parent
@@ -19,80 +18,21 @@ _DIR_MAP = {
}
def _task_clean(screen: LogScreen, dirs: List[str]) -> None:
"""Delete the selected top-level directories.
Удаляет выбранные директории верхнего уровня.
def _clean(dirs: List[str]) -> None:
"""Delete the selected top-level directories, printing the outcome of each.
Удаляет выбранные директории верхнего уровня, печатая результат по каждой.
"""
try:
screen.write("[bold]Cleaning build artifacts[/bold]\n")
total = len(dirs)
for i, label in enumerate(dirs):
if screen.is_stopped():
return
screen.set_progress(i / total * 100, f"Removing {label}...")
path = _DIR_MAP[label]
if path.exists():
shutil.rmtree(path)
screen.write(f"[green][ok][/green] Removed {label}")
else:
screen.write(f"[dim]Not found: {label}[/dim]")
if not screen.is_stopped():
screen.set_progress(100, "Done")
screen.write("\n[green]Done.[/green]")
screen.finish(True)
except Exception as exc:
if not screen.is_stopped():
screen.write(f"\n[red]Error:[/red] {exc}")
screen.finish(False)
class _CleanApp(App[None]):
"""Clean wizard: lets the user pick which directories to delete, then removes them.
Мастер очистки: позволяет выбрать директории для удаления, затем удаляет их.
"""
CSS = SCREEN_CSS
def __init__(self, all_dirs: bool):
super().__init__()
# True = skip the question and delete everything right away.
# True = пропустить вопрос и сразу удалить всё.
self._all_dirs = all_dirs
def on_mount(self) -> None:
if self._all_dirs:
self._start(_DIR_OPTIONS)
header("Очистка артефактов сборки")
removed = False
for label in dirs:
path = _DIR_MAP[label]
if path.exists():
shutil.rmtree(path)
ui.info(f" [green]✓[/green] Удалено {label}")
removed = True
else:
self._ask_dirs()
def _ask_dirs(self) -> None:
self.push_screen(
MultiPickScreen(
"clean",
"Which directories to delete?",
_DIR_OPTIONS,
note="Space — toggle · Enter — confirm",
),
self._got_dirs,
)
def _got_dirs(self, dirs: Optional[List[str]]) -> None:
if not dirs:
self.exit()
return
self._start(dirs)
def _start(self, dirs: List[str]) -> None:
self.push_screen(
LogScreen(
"Cleaning",
lambda s: _task_clean(s, dirs),
show_progress=True,
),
lambda _: self.exit(),
)
ui.info(f" [dim]Нет:[/dim] {label}")
done(True, "Очищено" if removed else "Нечего удалять")
def register(subparsers: argparse._SubParsersAction) -> None:
@@ -114,4 +54,15 @@ def run(args: argparse.Namespace) -> None:
"""Entry point for the clean command.
Точка входа для команды clean.
"""
_CleanApp(all_dirs=(getattr(args, "target", None) == "all")).run()
if getattr(args, "target", None) == "all":
_clean(_DIR_OPTIONS)
return
dirs = ui.multiselect(
"Какие директории удалить?",
_DIR_OPTIONS,
note="Space — отметить · Enter — подтвердить",
)
if not dirs:
return
_clean(dirs)
cobot/commands/delete.py
+104 -210
View File
@@ -4,110 +4,82 @@ import argparse
import shutil
import subprocess
from pathlib import Path
from typing import Optional
from typing import Callable
from textual.app import App
from cobot.tui import SCREEN_CSS, LogScreen, PickScreen
from cobot import ui
from cobot import privilege
from cobot.ui import done
from cobot.process import StepProgress
_PROJECT_DIR = Path(__file__).parent.parent.parent
Log = Callable[[str], None]
# Stop and remove all Docker containers whose name contains "lwc".
# Останавливаем и удаляем все Docker-контейнеры, чьё имя содержит "lwc".
def _stop_docker_containers(write) -> None:
def _stop_docker_containers(log: Log) -> None:
"""Stop and force-remove all Docker containers whose name contains "lwc".
Останавливает и принудительно удаляет все Docker-контейнеры с "lwc" в имени.
"""
write("[cyan][*][/cyan] Stopping Docker containers...")
log("[cyan]▸[/cyan] Остановка Docker-контейнеров...")
result = subprocess.run(
["docker", "ps", "-a", "--filter", "name=lwc", "--format", "{{.Names}}"],
capture_output=True, text=True,
)
containers = [c for c in result.stdout.strip().splitlines() if c]
if not containers:
write("[dim]No project containers found.[/dim]")
log("[dim]Контейнеры проекта не найдены.[/dim]")
return
for name in containers:
subprocess.run(["docker", "rm", "-f", name], capture_output=True)
write(f"[green][ok][/green] Removed container: {name}")
log(f"[green]✓[/green] Удалён контейнер: {name}")
# Remove all Docker images whose repository or tag contains "lwc".
# Удаляем все Docker-образы, репозиторий или тег которых содержит "lwc".
def _remove_docker_images(write) -> None:
def _remove_docker_images(log: Log) -> None:
"""Force-remove all local Docker images whose name or tag contains "lwc".
Принудительно удаляет все локальные Docker-образы с "lwc" в имени или теге.
"""
write("[cyan][*][/cyan] Removing Docker images...")
log("[cyan]▸[/cyan] Удаление Docker-образов...")
result = subprocess.run(
["docker", "images", "--format", "{{.Repository}}:{{.Tag}}"],
capture_output=True, text=True,
)
project_images = [
img for img in result.stdout.strip().splitlines()
if "lwc" in img.lower()
]
project_images = [img for img in result.stdout.strip().splitlines() if "lwc" in img.lower()]
if not project_images:
write("[dim]No project images found.[/dim]")
log("[dim]Образы проекта не найдены.[/dim]")
return
for img in project_images:
subprocess.run(["docker", "rmi", "-f", img], capture_output=True)
write(f"[green][ok][/green] Removed image: {img}")
log(f"[green]✓[/green] Удалён образ: {img}")
# Remove the Docker volume that stores the Webots asset cache.
# Удаляем Docker volume с кэшем ассетов Webots.
def _remove_webots_volume(write) -> None:
"""Remove the lwc-webots-cache Docker volume if it exists. Skips silently if absent.
Удаляет Docker volume lwc-webots-cache если он существует. Молча пропускает если отсутствует.
def _remove_webots_volume(log: Log) -> None:
"""Remove the lwc-webots-cache Docker volume if it exists.
Удаляет Docker volume lwc-webots-cache если он существует.
"""
result = subprocess.run(
["docker", "volume", "inspect", "lwc-webots-cache"],
capture_output=True,
)
result = subprocess.run(["docker", "volume", "inspect", "lwc-webots-cache"], capture_output=True)
if result.returncode != 0:
write("[dim]Webots cache volume not found, skipping.[/dim]")
log("[dim]Volume кэша Webots не найден, пропускаем.[/dim]")
return
subprocess.run(["docker", "volume", "rm", "lwc-webots-cache"], capture_output=True)
write("[green][ok][/green] Removed Docker volume: lwc-webots-cache")
log("[green]✓[/green] Удалён Docker volume: lwc-webots-cache")
# Remove ROS2 Jazzy packages via apt and clean up the source line from shell configs.
# Uses the official removal commands to also unregister the ROS2 apt repository.
# Удаляем пакеты ROS2 Jazzy через apt и очищаем строку source из конфигов оболочки.
# Используем официальные команды удаления, которые также снимают регистрацию apt-репозитория ROS2.
def _remove_ros2(write) -> None:
"""Remove all ros-jazzy-* packages, the ros2-apt-source package, and the ROS2 source
line from .bashrc / .zshrc. Does nothing if /opt/ros/jazzy is not present.
Удаляет все пакеты ros-jazzy-*, пакет ros2-apt-source и строку source ROS2 из
.bashrc / .zshrc. Ничего не делает если /opt/ros/jazzy отсутствует.
def _remove_ros2(log: Log) -> None:
"""Remove all ros-jazzy-* packages, the ros2-apt-source, and the ROS2 source line.
Удаляет все пакеты ros-jazzy-*, ros2-apt-source и строку source ROS2 из конфигов.
"""
write("[cyan][*][/cyan] Removing ROS2 Jazzy packages...")
log("[cyan]▸[/cyan] Удаление пакетов ROS2 Jazzy...")
if not Path("/opt/ros/jazzy").exists():
write("[dim]ROS2 Jazzy not found, skipping.[/dim]")
log("[dim]ROS2 Jazzy не найден, пропускаем.[/dim]")
else:
# Remove all ros-jazzy-* packages matched by the apt regex pattern ~n<name>.
# Удаляем все пакеты ros-jazzy-* по regex-паттерну apt ~n<имя>.
subprocess.run(
["sudo", "apt", "remove", "-y", "~nros-jazzy-*"],
capture_output=True,
)
subprocess.run(["sudo", "apt", "autoremove", "-y"], capture_output=True)
write("[green][ok][/green] ROS2 Jazzy packages removed")
subprocess.run(privilege.sudo(["apt", "remove", "-y", "~nros-jazzy-*"]), capture_output=True)
subprocess.run(privilege.sudo(["apt", "autoremove", "-y"]), capture_output=True)
log("[green]✓[/green] Пакеты ROS2 Jazzy удалены")
subprocess.run(privilege.sudo(["apt", "remove", "-y", "ros2-apt-source"]), capture_output=True)
subprocess.run(privilege.sudo(["apt", "update", "-qq"]), capture_output=True)
subprocess.run(privilege.sudo(["apt", "autoremove", "-y"]), capture_output=True)
log("[green]✓[/green] apt-репозиторий ROS2 удалён")
# Remove the ROS2 apt source package that added the repository.
# Удаляем пакет apt-источника ROS2, который добавил репозиторий.
subprocess.run(
["sudo", "apt", "remove", "-y", "ros2-apt-source"],
capture_output=True,
)
subprocess.run(["sudo", "apt", "update", "-qq"], capture_output=True)
subprocess.run(["sudo", "apt", "autoremove", "-y"], capture_output=True)
write("[green][ok][/green] ROS2 apt repository removed")
# Clean up the source line that local-setup added to the shell config.
# Очищаем строку source, добавленную local-setup в конфиг оболочки.
source_line = "source /opt/ros/jazzy/setup.bash"
for rc_name in [".bashrc", ".zshrc"]:
rc = Path.home() / rc_name
@@ -116,32 +88,24 @@ def _remove_ros2(write) -> None:
content = rc.read_text()
if source_line not in content:
continue
# Remove the whole block that was added by local-setup, not just the single line.
# Удаляем весь блок добавленный local-setup, а не только одну строку.
new_content = content.replace(f"\n# ROS2 Jazzy\n{source_line}\n", "\n")
new_content = new_content.replace(source_line, "")
rc.write_text(new_content)
write(f"[green][ok][/green] Cleaned up ~/{rc_name}")
log(f"[green]✓[/green] Очищен ~/{rc_name}")
# Remove Webots from the system via apt.
# Удаляем Webots из системы через apt.
def _remove_webots(write) -> None:
"""Remove the webots package via apt, run autoremove, and clean up WEBOTS_HOME
from .bashrc / .zshrc. Skips if webots is not found on PATH.
Удаляет пакет webots через apt, запускает autoremove и очищает WEBOTS_HOME из
.bashrc / .zshrc. Пропускает если webots не найден в PATH.
def _remove_webots(log: Log) -> None:
"""Remove the webots package and clean WEBOTS_HOME from shell configs.
Удаляет пакет webots и очищает WEBOTS_HOME из конфигов оболочки.
"""
write("[cyan][*][/cyan] Removing Webots...")
log("[cyan]▸[/cyan] Удаление Webots...")
if not shutil.which("webots"):
write("[dim]Webots not found, skipping.[/dim]")
log("[dim]Webots не найден, пропускаем.[/dim]")
return
subprocess.run(["sudo", "apt", "remove", "-y", "webots"], capture_output=True)
subprocess.run(["sudo", "apt", "autoremove", "-y"], capture_output=True)
write("[green][ok][/green] Webots removed")
subprocess.run(privilege.sudo(["apt", "remove", "-y", "webots"]), capture_output=True)
subprocess.run(privilege.sudo(["apt", "autoremove", "-y"]), capture_output=True)
log("[green]✓[/green] Webots удалён")
# Remove the WEBOTS_HOME block that install_webots.sh added to shell configs.
# Удаляем блок WEBOTS_HOME, добавленный install_webots.sh в конфиги оболочки.
for rc_name in [".bashrc", ".zshrc"]:
rc = Path.home() / rc_name
if not rc.exists():
@@ -154,154 +118,65 @@ def _remove_webots(write) -> None:
new_content = new_content.replace("# Webots\n", "")
if new_content != content:
rc.write_text(new_content)
write(f"[green][ok][/green] Cleaned WEBOTS_HOME from ~/{rc_name}")
log(f"[green]✓[/green] Очищен WEBOTS_HOME из ~/{rc_name}")
# Uninstall the cobot CLI from the uv tool store.
# Удаляем cobot CLI из хранилища инструментов uv.
def _uninstall_cobot(write) -> None:
def _uninstall_cobot(log: Log) -> None:
"""Uninstall the lightweight-cobot package from the uv tool store.
Удаляет пакет lightweight-cobot из хранилища инструментов uv.
"""
write("[cyan][*][/cyan] Uninstalling cobot CLI...")
result = subprocess.run(
["uv", "tool", "uninstall", "lightweight-cobot"],
capture_output=True, text=True,
)
log("[cyan]▸[/cyan] Удаление cobot CLI...")
result = subprocess.run(["uv", "tool", "uninstall", "lightweight-cobot"],
capture_output=True, text=True)
if result.returncode == 0:
write("[green][ok][/green] cobot uninstalled")
log("[green]✓[/green] cobot удалён")
else:
write(f"[yellow]Warning:[/yellow] {result.stderr.strip() or 'could not uninstall cobot'}")
log(f"[yellow]Предупреждение:[/yellow] {result.stderr.strip() or 'не удалось удалить cobot'}")
# Delete the entire project directory from disk.
# Удаляем всю директорию проекта с диска.
def _remove_project_dir(write) -> None:
"""Recursively delete the entire project directory (_PROJECT_DIR) from disk.
Рекурсивно удаляет всю директорию проекта (_PROJECT_DIR) с диска.
def _remove_project_dir(log: Log) -> None:
"""Recursively delete the entire project directory from disk.
Рекурсивно удаляет всю директорию проекта с диска.
"""
write(f"[cyan][*][/cyan] Removing project directory...")
try:
shutil.rmtree(_PROJECT_DIR)
write(f"[green][ok][/green] Removed {_PROJECT_DIR}")
except Exception as exc:
write(f"[red]Failed:[/red] {exc}")
raise
log("[cyan]▸[/cyan] Удаление директории проекта...")
shutil.rmtree(_PROJECT_DIR)
log(f"[green]✓[/green] Удалено {_PROJECT_DIR}")
# Run all deletion steps in order.
# Progress ranges are split evenly across the active steps so the bar always reaches 100%.
# Выполняем все шаги удаления по порядку.
# Диапазоны прогресса делятся равномерно между активными шагами, чтобы бар всегда доходил до 100%.
def _task_delete(screen: LogScreen, remove_ros: bool, remove_webots: bool) -> None:
"""Worker function that runs inside LogScreen. Runs all deletion steps in order:
containers -> images -> ROS2 (optional) -> Webots (optional) -> cobot CLI -> project dir.
Рабочая функция внутри LogScreen. Выполняет все шаги удаления по порядку:
контейнеры -> образы -> ROS2 (опционально) -> Webots (опционально) -> cobot CLI -> директория.
def _delete(remove_ros: bool, remove_webots: bool) -> None:
"""Run all deletion steps in order, with progress split across the active steps.
Выполняет все шаги удаления по порядку, распределяя прогресс между активными шагами.
"""
try:
screen.set_progress(0, "Stopping containers...")
_stop_docker_containers(screen.write)
_remove_webots_volume(screen.write)
ok, fail_msg = True, ""
with StepProgress("Удаление проекта") as p:
try:
p.set(0, "Остановка контейнеров...")
_stop_docker_containers(p.raw)
_remove_webots_volume(p.raw)
screen.set_progress(20, "Removing Docker images...")
_remove_docker_images(screen.write)
p.set(20, "Удаление Docker-образов...")
_remove_docker_images(p.raw)
pct = 40
if remove_ros:
screen.set_progress(pct, "Removing ROS2 Jazzy...")
_remove_ros2(screen.write)
pct = 65
pct = 40
if remove_ros:
p.set(pct, "Удаление ROS2 Jazzy...")
_remove_ros2(p.raw)
pct = 65
if remove_webots:
p.set(pct, "Удаление Webots...")
_remove_webots(p.raw)
pct = 75
if remove_webots:
screen.set_progress(pct, "Removing Webots...")
_remove_webots(screen.write)
pct = 75
p.set(pct, "Удаление cobot CLI...")
_uninstall_cobot(p.raw)
screen.set_progress(pct, "Uninstalling cobot CLI...")
_uninstall_cobot(screen.write)
p.set(88, "Удаление директории проекта...")
_remove_project_dir(p.raw)
p.set(100, "Готово")
except Exception as exc:
ok, fail_msg = False, str(exc)
screen.set_progress(88, "Removing project directory...")
_remove_project_dir(screen.write)
if not screen.is_stopped():
screen.set_progress(100, "Done")
screen.write("\n[green]Project fully removed.[/green]")
screen.finish(True)
except Exception as exc:
if not screen.is_stopped():
screen.write(f"\n[red]Error:[/red] {exc}")
screen.finish(False)
# Multi-step confirmation wizard before anything is deleted.
# Shows extra questions only when the relevant software is actually installed.
# Многошаговый мастер подтверждения перед удалением.
# Дополнительные вопросы показываются только если соответствующее ПО действительно установлено.
class _DeleteApp(App[None]):
"""Deletion wizard that asks for confirmation, then optionally asks about ROS2 and Webots,
then launches LogScreen running _task_delete.
Мастер удаления: просит подтверждение, затем опционально спрашивает про ROS2 и Webots,
затем запускает LogScreen с _task_delete.
"""
CSS = SCREEN_CSS
def on_mount(self) -> None:
self.push_screen(
PickScreen(
"Confirm deletion",
"This will permanently delete the project, Docker images and containers. Are you sure?",
["No, cancel", "Yes, delete everything"],
"No, cancel",
),
self._on_confirm,
)
def _on_confirm(self, choice: Optional[str]) -> None:
if choice is None or choice.startswith("No"):
self.exit()
return
self.push_screen(
PickScreen(
"ROS2 Jazzy",
"Also remove ROS2 Jazzy from the system?",
["No, keep ROS2", "Yes, remove ROS2 Jazzy"],
"No, keep ROS2",
),
self._on_ros_choice,
)
def _on_ros_choice(self, choice: Optional[str]) -> None:
remove_ros = choice is not None and choice.startswith("Yes")
# Only ask about Webots if it is actually installed on this machine.
# Спрашиваем про Webots только если он действительно установлен на этой машине.
if shutil.which("webots"):
self.push_screen(
PickScreen(
"Webots",
"Also remove Webots from the system?",
["No, keep Webots", "Yes, remove Webots"],
"No, keep Webots",
),
lambda c: self._on_webots_choice(c, remove_ros),
)
else:
self._start_deletion(remove_ros, remove_webots=False)
def _on_webots_choice(self, choice: Optional[str], remove_ros: bool) -> None:
remove_webots = choice is not None and choice.startswith("Yes")
self._start_deletion(remove_ros, remove_webots)
def _start_deletion(self, remove_ros: bool, remove_webots: bool) -> None:
self.push_screen(
LogScreen(
"Deleting project",
lambda s: _task_delete(s, remove_ros, remove_webots),
show_progress=True,
),
lambda _: self.exit(),
)
done(ok, "Проект полностью удалён" if ok else fail_msg)
def register(subparsers: argparse._SubParsersAction) -> None:
@@ -313,4 +188,23 @@ def register(subparsers: argparse._SubParsersAction) -> None:
def run(args: argparse.Namespace) -> None:
_DeleteApp().run()
ui.header("Удаление проекта", "контейнеры, образы, опционально ROS2/Webots")
if not ui.confirm(
"Это безвозвратно удалит проект, Docker-образы и контейнеры. Продолжить?",
default=False,
):
return
remove_ros = ui.confirm("Также удалить ROS2 Jazzy из системы?", default=False)
remove_webots = False
if shutil.which("webots"):
remove_webots = ui.confirm("Также удалить Webots из системы?", default=False)
# apt removals need root — acquire sudo once before starting.
# Удаление через apt требует root — получаем sudo один раз перед началом.
if (remove_ros or remove_webots) and not privilege.ensure_sudo():
return
_delete(remove_ros, remove_webots)
cobot/commands/doc_setup.py
+124 -248
View File
@@ -7,47 +7,38 @@ import shutil
import subprocess
import sys
from pathlib import Path
from typing import Callable, Optional
from textual.app import App
from cobot.tui import SCREEN_CSS, InputScreen, LogScreen
from cobot import process, ui
from cobot.ui import done
from cobot.process import StepProgress
_PROJECT_DIR = Path(__file__).parent.parent.parent
# The documentation source lives inside the project. We mount it into the container so
# MkDocs can pick up live edits without rebuilding the image.
# Исходники документации находятся внутри проекта. Монтируем директорию в контейнер, чтобы
# MkDocs мог подхватывать изменения вживую без пересборки образа.
# The documentation source lives inside the project; it is mounted into the container
# so MkDocs picks up live edits without rebuilding the image.
# Исходники документации находятся внутри проекта; директория монтируется в контейнер,
# чтобы MkDocs подхватывал изменения вживую без пересборки образа.
_DOC_DIR = _PROJECT_DIR / "doc" / "lwc-doc"
_IMAGE_NAME = "lwc-docs"
_CONTAINER_NAME = "lwc-docs"
_DEFAULT_PORT = "8000"
Write = Callable[[str], None]
# Thin wrapper around docker so we do not repeat ["docker", ...] everywhere.
# Тонкая обёртка вокруг docker, чтобы не повторять ["docker", ...] везде.
def _docker(*args: str, capture: bool = False) -> subprocess.CompletedProcess:
"""Run a docker subcommand. Pass capture=True to capture stdout/stderr instead of printing.
Запускает подкоманду docker. capture=True перехватывает stdout/stderr вместо вывода на экран.
"""Run a docker subcommand.
Запускает подкоманду docker.
"""
return subprocess.run(["docker", *args], capture_output=capture, text=True)
# Check whether the docs container is currently running.
# Проверяем, запущен ли сейчас контейнер с документацией.
def _is_running() -> bool:
"""Return True if the lwc-docs container is currently running.
Возвращает True если контейнер lwc-docs в данный момент запущен.
Возвращает True если контейнер lwc-docs запущен.
"""
r = _docker("ps", "--filter", f"name={_CONTAINER_NAME}", "--format", "{{.Names}}", capture=True)
return _CONTAINER_NAME in r.stdout
# Check whether the docs Docker image has already been built.
# Проверяем, был ли уже собран Docker-образ для документации.
def _image_exists() -> bool:
"""Return True if the lwc-docs Docker image exists locally.
Возвращает True если Docker-образ lwc-docs существует локально.
@@ -55,259 +46,132 @@ def _image_exists() -> bool:
return bool(_docker("images", "-q", _IMAGE_NAME, capture=True).stdout.strip())
# Build the MkDocs Docker image. Only needs to run once.
# Progress comes from parsing "Step X/Y" lines in the docker build output.
# Собираем Docker-образ MkDocs. Нужно сделать только один раз.
# Прогресс получаем, парся строки "Step X/Y" из вывода docker build.
def _build_docs_image(
write: Write,
on_progress: Optional[Callable[[float], None]] = None,
register_proc: Optional[Callable] = None,
) -> bool:
"""Build the lwc-docs Docker image from the doc/lwc-doc directory. Returns True on success.
Собирает Docker-образ lwc-docs из директории doc/lwc-doc. Возвращает True при успехе.
def _build_docs_image(p: StepProgress, lo: float, hi: float) -> bool:
"""Build the lwc-docs image, mapping "Step X/Y" to the lo..hi progress slice.
Собирает образ lwc-docs, отображая "Step X/Y" на участок lo..hi прогресса.
"""
write("[cyan][*][/cyan] Building documentation image (runs once)...")
# DOCKER_BUILDKIT=0 gives us "Step X/Y" lines that we can parse for progress.
# DOCKER_BUILDKIT=0 даёт нам строки "Step X/Y", которые можно парсить для прогресса.
p.raw("[cyan]▸[/cyan] Сборка образа документации (один раз)...")
env = {**os.environ, "DOCKER_BUILDKIT": "0"}
proc = subprocess.Popen(
["docker", "build", "-t", _IMAGE_NAME, str(_DOC_DIR)],
stdout=subprocess.PIPE, stderr=subprocess.STDOUT, text=True, env=env,
)
if register_proc:
register_proc(proc)
for line in proc.stdout:
s = line.rstrip()
def on_line(s: str) -> None:
if s:
write(s)
if on_progress:
m = re.match(r"Step (\d+)/(\d+) :", line)
if m:
step, total = int(m.group(1)), int(m.group(2))
on_progress(step / total * 100)
proc.wait()
if proc.returncode in (-9, -15):
p.log(s)
m = re.match(r"Step (\d+)/(\d+) :", s)
if m:
step, total = int(m.group(1)), int(m.group(2))
p.set(lo + step / total * (hi - lo), f"шаг {step}/{total}")
rc = process.stream(["docker", "build", "-t", _IMAGE_NAME, str(_DOC_DIR)],
env=env, on_line=on_line)
if rc != 0:
p.raw("[red]Сборка образа не удалась.[/red]")
return False
if proc.returncode == 0:
write("[green][ok][/green] Documentation image ready")
return True
write("[red]Image build failed.[/red]")
return False
p.raw("[green]✓[/green] Образ документации готов")
return True
# Start the docs server. Builds the image first if it does not exist yet.
# Запускаем сервер документации. Сначала собирает образ, если он ещё не существует.
def _task_up(screen: LogScreen, port: str) -> None:
"""Worker function for the "up" action. Builds the image if missing, then starts the container.
Рабочая функция для действия "up". Собирает образ если отсутствует, затем запускает контейнер.
def _start_container(p: StepProgress, port: str) -> bool:
"""Start the MkDocs container on the given port. Returns True on success.
Запускает контейнер MkDocs на заданном порту. Возвращает True при успехе.
"""
try:
if _is_running():
screen.write(f"[green]Docs already running at:[/green] http://localhost:{port}")
screen.write(" Stop with: [bold]cobot doc-setup down[/bold]")
if not screen.is_stopped():
screen.finish(True)
return
p.set(90, "Запуск сервера MkDocs...")
p.raw("[cyan]▸[/cyan] Запуск сервера MkDocs...")
result = _docker(
"run", "-d", "--name", _CONTAINER_NAME, "--rm",
"-p", f"{port}:8000",
"-v", f"{_DOC_DIR}:/docs",
_IMAGE_NAME, "serve", "--dev-addr=0.0.0.0:8000",
capture=True,
)
if result.returncode != 0:
p.raw(f"[red]Не удалось запустить контейнер.[/red]\n{result.stderr}")
return False
return True
if not _DOC_DIR.exists():
screen.write(f"[red]Doc directory not found:[/red] {_DOC_DIR}")
if not screen.is_stopped():
screen.finish(False)
return
def _task_up(port: str) -> None:
"""Build the image if missing, then start the docs container.
Собирает образ если отсутствует, затем запускает контейнер документации.
"""
if _is_running():
ui.info(f"[green]Документация уже запущена:[/green] http://localhost:{port}")
ui.note("Остановить: cobot doc-setup down")
return
if not _DOC_DIR.exists():
ui.error(f"Директория документации не найдена: {_DOC_DIR}")
return
ok, fail_msg = True, ""
with StepProgress("Сервер документации") as p:
if not _image_exists():
screen.set_progress(0, "Building documentation image...")
ok = _build_docs_image(
screen.write,
on_progress=lambda p: screen.set_progress(p * 0.85, "Building documentation image..."),
register_proc=screen.set_proc,
)
if screen.is_stopped():
return
if not ok:
screen.finish(False)
return
p.set(0, "Сборка образа документации...")
if not _build_docs_image(p, 0, 85):
ok, fail_msg = False, "Сборка образа не удалась"
else:
screen.write("[dim]Documentation image already built, skipping.[/dim]")
p.log("Образ документации уже собран, пропускаем.")
if ok and not _start_container(p, port):
ok, fail_msg = False, "Не удалось запустить контейнер"
if ok:
p.set(100, "Сервер запущен")
if screen.is_stopped():
return
screen.set_progress(88, "Starting MkDocs server...")
screen.write("\n[cyan][*][/cyan] Starting MkDocs server...")
result = _docker(
"run", "-d", "--name", _CONTAINER_NAME, "--rm",
"-p", f"{port}:8000",
# Mount the docs directory so edits appear live without restarting the container.
# Монтируем директорию с документацией, чтобы изменения появлялись сразу без перезапуска.
"-v", f"{_DOC_DIR}:/docs",
_IMAGE_NAME, "serve", "--dev-addr=0.0.0.0:8000",
capture=True,
)
if screen.is_stopped():
return
if result.returncode != 0:
screen.write(f"[red]Failed to start container.[/red]\n{result.stderr}")
screen.finish(False)
return
screen.set_progress(100, "Server running")
screen.write(f"\n[green]Docs running at:[/green] http://localhost:{port}")
screen.write(" Edit files in [bold]doc/lwc-doc/docs/[/bold] — reloads automatically.")
screen.write(" Stop with: [bold]cobot doc-setup down[/bold]")
if not screen.is_stopped():
screen.finish(True)
except Exception as exc:
if not screen.is_stopped():
screen.write(f"[red]Error:[/red] {exc}")
screen.finish(False)
if ok:
done(True, f"Документация доступна: http://localhost:{port}")
ui.note("Правьте файлы в doc/lwc-doc/docs/ — перезагрузка автоматическая.")
ui.note("Остановить: cobot doc-setup down")
else:
done(False, fail_msg)
# Stop the running docs container.
# Останавливаем работающий контейнер с документацией.
def _task_down(screen: LogScreen) -> None:
"""Worker function for the "down" action. Stops the lwc-docs container if it is running.
Рабочая функция для действия "down". Останавливает контейнер lwc-docs если он запущен.
def _task_down() -> None:
"""Stop the lwc-docs container if it is running.
Останавливает контейнер lwc-docs если он запущен.
"""
try:
if not _is_running():
screen.write("[yellow]Docs container is not running.[/yellow]")
if not screen.is_stopped():
screen.finish(True)
return
screen.set_progress(30, "Stopping container...")
screen.write("[cyan][*][/cyan] Stopping documentation server...")
if not _is_running():
ui.info("[yellow]Контейнер документации не запущен.[/yellow]")
return
with StepProgress("Сервер документации") as p:
p.set(30, "Остановка контейнера...")
p.raw("[cyan]▸[/cyan] Остановка сервера документации...")
_docker("stop", _CONTAINER_NAME)
if screen.is_stopped():
return
screen.set_progress(100, "Done")
screen.write("[green][ok][/green] Container stopped.")
screen.finish(True)
except Exception as exc:
if not screen.is_stopped():
screen.write(f"[red]Error:[/red] {exc}")
screen.finish(False)
p.set(100, "Готово")
done(True, "Контейнер остановлен")
# Stop the container, remove the old image, rebuild it, and start a new container.
# Останавливаем контейнер, удаляем старый образ, пересобираем и запускаем новый контейнер.
def _task_rebuild(screen: LogScreen, port: str) -> None:
"""Worker function for the "rebuild" action. Stops the container, removes the old image,
rebuilds it, and starts a fresh container on the given port.
Рабочая функция для действия "rebuild". Останавливает контейнер, удаляет старый образ,
пересобирает его и запускает новый контейнер на указанном порту.
def _task_rebuild(port: str) -> None:
"""Stop the container, remove the old image, rebuild it, and start a fresh container.
Останавливает контейнер, удаляет старый образ, пересобирает и запускает новый контейнер.
"""
try:
ok, fail_msg = True, ""
with StepProgress("Сервер документации — пересборка") as p:
if _is_running():
screen.set_progress(5, "Stopping container...")
screen.write("[cyan][*][/cyan] Stopping existing container...")
p.set(5, "Остановка контейнера...")
_docker("stop", _CONTAINER_NAME)
if screen.is_stopped():
return
screen.write("[green][ok][/green] Stopped.")
p.raw("[green]✓[/green] Остановлен.")
if _image_exists():
screen.set_progress(15, "Removing old image...")
screen.write("[cyan][*][/cyan] Removing old image...")
p.set(15, "Удаление старого образа...")
_docker("rmi", "-f", _IMAGE_NAME)
if screen.is_stopped():
return
screen.write("[green][ok][/green] Image removed.")
p.raw("[green]✓[/green] Образ удалён.")
p.set(20, "Сборка образа документации...")
if not _build_docs_image(p, 20, 88):
ok, fail_msg = False, "Сборка образа не удалась"
if ok and not _start_container(p, port):
ok, fail_msg = False, "Не удалось запустить контейнер"
if ok:
p.set(100, "Сервер запущен")
screen.set_progress(20, "Building documentation image...")
ok = _build_docs_image(
screen.write,
on_progress=lambda p: screen.set_progress(20 + p * 0.68, "Building documentation image..."),
register_proc=screen.set_proc,
)
if screen.is_stopped():
return
if not ok:
screen.finish(False)
return
screen.set_progress(90, "Starting MkDocs server...")
screen.write("\n[cyan][*][/cyan] Starting MkDocs server...")
result = _docker(
"run", "-d", "--name", _CONTAINER_NAME, "--rm",
"-p", f"{port}:8000",
"-v", f"{_DOC_DIR}:/docs",
_IMAGE_NAME, "serve", "--dev-addr=0.0.0.0:8000",
capture=True,
)
if screen.is_stopped():
return
if result.returncode != 0:
screen.write(f"[red]Failed to start container.[/red]\n{result.stderr}")
screen.finish(False)
return
screen.set_progress(100, "Server running")
screen.write(f"\n[green]Docs running at:[/green] http://localhost:{port}")
screen.write(" Stop with: [bold]cobot doc-setup down[/bold]")
if not screen.is_stopped():
screen.finish(True)
except Exception as exc:
if not screen.is_stopped():
screen.write(f"[red]Error:[/red] {exc}")
screen.finish(False)
if ok:
done(True, f"Документация доступна: http://localhost:{port}")
ui.note("Остановить: cobot doc-setup down")
else:
done(False, fail_msg)
# One app handles all three actions (up/down/rebuild) by branching in on_mount.
# Одно приложение обрабатывает все три действия (up/down/rebuild), разветвляясь в on_mount.
class _DocApp(App[None]):
"""Documentation server app. Handles "up", "down", and "rebuild" actions by branching
in on_mount to the appropriate LogScreen task.
Приложение сервера документации. Обрабатывает действия "up", "down" и "rebuild",
разветвляясь в on_mount к соответствующей задаче LogScreen.
def _normalize_port(value: str) -> str:
"""Return a numeric port string, falling back to the default when invalid.
Возвращает числовой порт, откатываясь на значение по умолчанию при ошибке.
"""
CSS = SCREEN_CSS
def __init__(self, action: str):
super().__init__()
self._action = action
def on_mount(self) -> None:
if self._action == "down":
self.push_screen(
LogScreen("Documentation server", _task_down, show_progress=True),
lambda _: self.exit(),
)
elif self._action == "rebuild":
self.push_screen(
InputScreen("Step 1 of 1", "Port to serve documentation on:", _DEFAULT_PORT),
self._got_port_rebuild,
)
else:
self.push_screen(
InputScreen("Step 1 of 1", "Port to serve documentation on:", _DEFAULT_PORT),
self._got_port_up,
)
def _got_port_up(self, port: Optional[str]) -> None:
if port is None:
self.exit()
return
# Use the default port if the user cleared the input or typed something that is not a number.
# Используем порт по умолчанию если пользователь очистил ввод или написал не число.
p = (port.strip() or _DEFAULT_PORT) if port.isdigit() or not port.strip() else _DEFAULT_PORT
self.push_screen(
LogScreen("Documentation server", lambda s: _task_up(s, p), show_progress=True),
lambda _: self.exit(),
)
def _got_port_rebuild(self, port: Optional[str]) -> None:
if port is None:
self.exit()
return
p = (port.strip() or _DEFAULT_PORT) if port.isdigit() or not port.strip() else _DEFAULT_PORT
self.push_screen(
LogScreen("Documentation server — rebuild", lambda s: _task_rebuild(s, p), show_progress=True),
lambda _: self.exit(),
)
value = (value or "").strip()
return value if value.isdigit() else _DEFAULT_PORT
def register(subparsers: argparse._SubParsersAction) -> None:
@@ -324,9 +188,21 @@ def register(subparsers: argparse._SubParsersAction) -> None:
def run(args: argparse.Namespace) -> None:
if not shutil.which("docker"):
from rich.console import Console
Console().print("[red]Error:[/red] Docker is not installed or not on PATH.")
ui.error("Docker не установлен или отсутствует в PATH.")
sys.exit(1)
action = getattr(args, "action", "up")
_DocApp(action).run()
if action == "down":
_task_down()
return
port_v = ui.text("Порт для сервера документации:", _DEFAULT_PORT)
if port_v is None:
return
port = _normalize_port(port_v)
if action == "rebuild":
_task_rebuild(port)
else:
_task_up(port)
cobot/commands/docker_setup.py
+126 -310
View File
@@ -4,15 +4,14 @@ import argparse
import os
import re
import shutil
import subprocess
import sys
from dataclasses import dataclass
from pathlib import Path
from typing import Callable, List, Optional
from typing import List, Optional
from textual.app import App
from cobot.tui import SCREEN_CSS, InputScreen, LogScreen, PickScreen
from cobot import process, ui
from cobot.ui import done
from cobot.process import StepProgress
_PROJECT_DIR = Path(__file__).parent.parent.parent
_DOCKER_DIR = _PROJECT_DIR / "docker"
@@ -27,8 +26,8 @@ _DEFAULT_PREFIX = "lwc-local"
_CONTROLLER_CHAIN = ["ros-core", "ros-base", "ros-iiwa7"]
_WEBOTS_CHAIN = ["ros-core", "ros-base", "ros-iiwa7-webots"]
# Maps each image to the image it is built FROM. None means it starts from scratch (base Ubuntu).
# Сопоставляет каждый образ с тем, на основе которого он собирается. None - начинает с нуля (базовый Ubuntu).
# Maps each image to the image it is built FROM. None means it starts from base Ubuntu.
# Сопоставляет каждый образ с тем, на основе которого он собирается. None - базовый Ubuntu.
_IMAGE_PARENT: dict[str, str | None] = {
"ros-core": None,
"ros-base": "ros-core",
@@ -36,15 +35,11 @@ _IMAGE_PARENT: dict[str, str | None] = {
"ros-iiwa7-webots": "ros-base",
}
# These images need the full project source as Docker build context because they copy source files.
# Эти образы требуют полный исходный код проекта как контекст сборки, потому что копируют файлы.
# These images need the full project source as Docker build context.
# Эти образы требуют полный исходный код проекта как контекст сборки.
_NEEDS_PROJECT_CTX = {"ros-iiwa7", "ros-iiwa7-webots"}
Write = Callable[[str], None]
# All the choices the user makes in the wizard are stored here before we start the actual build.
# Все выборы пользователя в мастере хранятся здесь перед началом фактической сборки.
@dataclass
class _Config:
ros_version: str
@@ -55,29 +50,14 @@ class _Config:
hub_repo: str
# Build one Docker image and stream its output to the log.
# Progress is tracked by parsing "Step X/Y" lines that Docker prints during the build.
# Собирает один Docker-образ и транслирует его вывод в лог.
# Прогресс отслеживается по строкам "Step X/Y", которые Docker печатает во время сборки.
def _build_image(
name: str,
tag: str,
dockerfile: Path,
ctx: Path,
write: Write,
on_progress: Optional[Callable[[float], None]] = None,
parent_tag: Optional[str] = None,
build_type: str = "release",
register_proc: Optional[Callable] = None,
) -> bool:
"""Build a single Docker image from a Dockerfile and stream its output line by line.
Returns True on success, False if the build failed or was cancelled.
Собирает один Docker-образ из Dockerfile и транслирует вывод построчно.
Возвращает True при успехе, False если сборка завершилась ошибкой или была отменена.
def _build_image(name: str, tag: str, dockerfile: Path, ctx: Path, p: StepProgress,
lo: float, hi: float, parent_tag: Optional[str], build_type: str) -> bool:
"""Build a single Docker image, streaming output and mapping "Step X/Y" to the
lo..hi slice of the progress bar. Returns True on success.
Собирает один Docker-образ, транслируя вывод и отображая "Step X/Y" на участок
lo..hi прогресс-бара. Возвращает True при успехе.
"""
write(f"[cyan][*][/cyan] Building [bold]{name}[/bold]...")
# DOCKER_BUILDKIT=0 gives us "Step X/Y" lines in the output which we parse for progress.
# DOCKER_BUILDKIT=0 даёт нам строки "Step X/Y" в выводе, которые мы парсим для прогресса.
p.raw(f"[cyan]▸[/cyan] Сборка [bold]{name}[/bold]...")
env = {**os.environ, "DOCKER_BUILDKIT": "0"}
cmd = [
"docker", "build", "-t", tag, "-f", str(dockerfile),
@@ -87,315 +67,109 @@ def _build_image(
cmd += ["--build-arg", f"IMAGE={parent_tag}"]
cmd.append(str(ctx))
proc = subprocess.Popen(
cmd, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, text=True, env=env,
)
if register_proc:
register_proc(proc)
for line in proc.stdout:
s = line.rstrip()
def on_line(s: str) -> None:
if s:
write(s)
if on_progress:
m = re.match(r"Step (\d+)/(\d+) :", line)
if m:
step, total = int(m.group(1)), int(m.group(2))
on_progress(step / total * 100)
proc.wait()
if proc.returncode in (-9, -15):
p.log(s)
m = re.match(r"Step (\d+)/(\d+) :", s)
if m:
step, total = int(m.group(1)), int(m.group(2))
p.set(lo + step / total * (hi - lo), f"{name}: шаг {step}/{total}")
rc = process.stream(cmd, env=env, on_line=on_line)
if rc in (-9, -15):
return False
if proc.returncode == 0:
write(f"[green][ok][/green] {name}")
if rc == 0:
p.raw(f"[green]✓[/green] {name}")
return True
write(f"[red]Build failed:[/red] {tag}")
p.raw(f"[red]Сборка не удалась:[/red] {tag}")
return False
# Pull a Docker image from Hub and track progress by counting downloaded layers.
# Скачиваем Docker-образ с Hub и отслеживаем прогресс по количеством скачанных слоёв.
def _pull_image(
name: str,
tag: str,
write: Write,
on_progress: Optional[Callable[[float], None]] = None,
register_proc: Optional[Callable] = None,
) -> bool:
"""Pull a Docker image from Docker Hub and report layer-by-layer progress.
Returns True on success, False if the pull failed or was cancelled.
Скачивает Docker-образ с Docker Hub и сообщает о прогрессе по слоям.
Возвращает True при успехе, False если скачивание завершилось ошибкой или было отменено.
def _pull_image(name: str, tag: str, p: StepProgress, lo: float, hi: float) -> bool:
"""Pull a Docker image, tracking progress by counting completed layers.
Скачивает Docker-образ, отслеживая прогресс по числу завершённых слоёв.
"""
write(f"[cyan][*][/cyan] Pulling [bold]{name}[/bold] ({tag})...")
if on_progress:
on_progress(5)
proc = subprocess.Popen(
["docker", "pull", tag],
stdout=subprocess.PIPE, stderr=subprocess.STDOUT, text=True,
)
if register_proc:
register_proc(proc)
p.raw(f"[cyan]▸[/cyan] Скачивание [bold]{name}[/bold] ({tag})...")
layers_total = 0
layers_done = 0
for line in proc.stdout:
s = line.rstrip()
def on_line(s: str) -> None:
nonlocal layers_total, layers_done
if s:
write(s)
# Count layers as they appear and mark them done when Docker confirms they are pulled.
# Считаем слои по мере их появления и отмечаем завершёнными когда Docker подтверждает скачивание.
if "Pulling fs layer" in line or "Waiting" in line:
p.log(s)
if "Pulling fs layer" in s or "Waiting" in s:
layers_total += 1
elif "Pull complete" in line or "Already exists" in line:
elif "Pull complete" in s or "Already exists" in s:
layers_done += 1
if on_progress and layers_total > 0:
on_progress(5 + layers_done / layers_total * 90)
proc.wait()
if proc.returncode in (-9, -15):
if layers_total > 0:
p.set(lo + layers_done / layers_total * (hi - lo), f"{name}: слои")
rc = process.stream(["docker", "pull", tag], on_line=on_line)
if rc in (-9, -15):
return False
if proc.returncode == 0:
write(f"[green][ok][/green] {name}")
if on_progress:
on_progress(100)
if rc == 0:
p.raw(f"[green]✓[/green] {name}")
return True
write(f"[red]Pull failed:[/red] {tag}")
p.raw(f"[red]Скачивание не удалось:[/red] {tag}")
return False
# The actual work - either build or pull all images depending on what the user chose.
# Each image gets its own slice of the progress bar so the overall bar advances smoothly.
# Основная работа - собираем или скачиваем все образы в зависимости от выбора пользователя.
# Каждый образ получает свой кусок прогресс-бара, чтобы общий бар двигался равномерно.
def _task_execute(screen: LogScreen, cfg: _Config) -> None:
"""Worker function that runs inside LogScreen. Builds or pulls all images in the chain
defined by the user's choices and updates the progress bar after each image.
Рабочая функция, выполняемая внутри LogScreen. Собирает или скачивает все образы из цепочки
согласно выбору пользователя и обновляет прогресс-бар после каждого образа.
def _execute(cfg: _Config) -> None:
"""Build or pull all images in the chain selected by the user's choices.
Собирает или скачивает все образы из цепочки, выбранной пользователем.
"""
try:
chain = _WEBOTS_CHAIN if cfg.variant == "webots" else _CONTROLLER_CHAIN
n = len(chain)
chain = _WEBOTS_CHAIN if cfg.variant == "webots" else _CONTROLLER_CHAIN
n = len(chain)
ok = True
fail_msg = ""
if cfg.source == "build":
screen.write(
f"[bold]Building {n} image(s) — "
f"ROS {cfg.ros_version} — {cfg.build_type}[/bold]\n"
)
if cfg.source == "build":
title = f"Сборка {n} образ(ов) — ROS {cfg.ros_version} — {cfg.build_type}"
with StepProgress(title) as p:
for i, name in enumerate(chain):
if screen.is_stopped():
return
lo = i / n * 100
hi = (i + 1) / n * 100
screen.set_progress(lo, f"Image {i + 1}/{n}: building {name}...")
lo, hi = i / n * 100, (i + 1) / n * 100
p.set(lo, f"Образ {i + 1}/{n}: {name}")
tag = f"{cfg.image_prefix}:{name}-{cfg.ros_version}"
dockerfile = _DOCKER_DIR / cfg.ros_version / name / "Dockerfile"
if not dockerfile.exists():
screen.write(f"[red]Dockerfile not found:[/red] {dockerfile}")
if not screen.is_stopped():
screen.finish(False)
return
ok, fail_msg = False, f"Dockerfile не найден: {dockerfile}"
break
ctx = _PROJECT_DIR if name in _NEEDS_PROJECT_CTX else dockerfile.parent
parent_name = _IMAGE_PARENT.get(name)
parent_tag = (
f"{cfg.image_prefix}:{parent_name}-{cfg.ros_version}"
if parent_name else None
)
ok = _build_image(
name, tag, dockerfile, ctx, screen.write,
on_progress=lambda p, lo=lo, hi=hi: screen.set_progress(
lo + p * (hi - lo) / 100, f"Image {i + 1}/{n}: building {name}..."
),
parent_tag=parent_tag,
build_type=cfg.build_type,
register_proc=screen.set_proc,
)
if screen.is_stopped():
return
if not ok:
screen.finish(False)
return
screen.set_progress(hi)
if not screen.is_stopped():
screen.set_progress(100, "All images built")
screen.write(
f"\n[green]Done.[/green] "
f"Images tagged [bold]{cfg.image_prefix}:<name>-{cfg.ros_version}[/bold]."
)
else:
short = "webots" if cfg.variant == "webots" else "iiwa"
suffix = "-dev" if cfg.build_type == "dev" else ""
full_ref = f"{cfg.hub_repo}:{short}-{cfg.ros_version}{suffix}"
screen.write(
f"[bold]Pulling from {cfg.hub_repo} — "
f"ROS {cfg.ros_version} — {cfg.build_type}[/bold]\n"
)
screen.set_progress(0, f"Pulling {full_ref}...")
ok = _pull_image(
short, full_ref, screen.write,
on_progress=lambda p: screen.set_progress(p, f"Pulling {full_ref}..."),
register_proc=screen.set_proc,
)
if screen.is_stopped():
return
if not ok:
screen.finish(False)
return
screen.set_progress(100, "Pull complete")
screen.write(f"\n[green]Done.[/green] Image ready: [bold]{full_ref}[/bold].")
if not screen.is_stopped():
screen.finish(True)
except Exception as exc:
if not screen.is_stopped():
screen.write(f"\n[red]Error:[/red] {exc}")
screen.finish(False)
parent_tag = (f"{cfg.image_prefix}:{parent_name}-{cfg.ros_version}"
if parent_name else None)
if not _build_image(name, tag, dockerfile, ctx, p, lo, hi,
parent_tag, cfg.build_type):
ok, fail_msg = False, f"Сборка образа {name} не удалась"
break
if ok:
p.set(100, "Готово")
done(ok, f"Образы готовы: {cfg.image_prefix}:<name>-{cfg.ros_version}"
if ok else fail_msg)
else:
short = "webots" if cfg.variant == "webots" else "iiwa"
suffix = "-dev" if cfg.build_type == "dev" else ""
full_ref = f"{cfg.hub_repo}:{short}-{cfg.ros_version}{suffix}"
with StepProgress(f"Скачивание из {cfg.hub_repo} — ROS {cfg.ros_version}") as p:
if _pull_image(short, full_ref, p, 0, 100):
p.set(100, "Готово")
else:
ok, fail_msg = False, f"Скачивание {full_ref} не удалось"
done(ok, f"Образ готов: {full_ref}" if ok else fail_msg)
# Scan the docker/ directory for subdirectories named after ROS versions (e.g. jazzy).
# If nothing is found we fall back to "jazzy" so the wizard still works.
# Сканируем директорию docker/ на наличие поддиректорий с именами версий ROS (например jazzy).
# Если ничего не найдено, используем "jazzy" по умолчанию, чтобы мастер всё равно работал.
def _discover_versions() -> List[str]:
"""Return a sorted list of ROS versions found in docker/. Jazzy is placed first.
Falls back to ["jazzy"] if the directory does not exist or is empty.
Возвращает отсортированный список версий ROS найденных в docker/. Jazzy идёт первым.
Возвращает ["jazzy"] если директория не существует или пуста.
"""Return ROS versions found in docker/ (jazzy first). Falls back to ["jazzy"].
Возвращает версии ROS из docker/ (jazzy первым). По умолчанию ["jazzy"].
"""
if not _DOCKER_DIR.exists():
return ["jazzy"]
dirs = sorted(d.name for d in _DOCKER_DIR.iterdir() if d.is_dir())
# Put jazzy first so it is the pre-selected default in the wizard.
# Ставим jazzy первым, чтобы он был предвыбранным по умолчанию в мастере.
if "jazzy" in dirs:
dirs = ["jazzy"] + [d for d in dirs if d != "jazzy"]
return dirs or ["jazzy"]
# Multi-step wizard that collects all build options before starting the actual image build.
# Многошаговый мастер, который собирает все параметры сборки перед запуском фактической сборки образа.
class _Wizard(App[None]):
"""Five-step wizard: ROS version -> source (pull/build) -> variant -> build type -> repo/prefix.
Collects all options, then hands off to LogScreen which runs _task_execute.
Пятишаговый мастер: версия ROS -> источник (pull/build) -> вариант -> тип сборки -> репо/префикс.
Собирает все параметры, затем передаёт управление LogScreen который запускает _task_execute.
"""
CSS = SCREEN_CSS
def __init__(self, versions: List[str], default_version: str = "jazzy"):
super().__init__()
self.versions = versions
self.default_version = default_version
self._state: dict = {}
def on_mount(self) -> None:
self._ask_version()
def _ask_version(self) -> None:
self.push_screen(
PickScreen("Step 1 of 5", "Select ROS version:", self.versions, self.default_version),
self._got_version,
)
def _got_version(self, v: Optional[str]) -> None:
if v is None:
self.exit()
return
self._state["ros_version"] = v
self.push_screen(
PickScreen(
"Step 2 of 5", "Source:",
["Pull from Docker Hub", "Build locally"],
"Pull from Docker Hub",
),
self._got_source,
)
def _got_source(self, v: Optional[str]) -> None:
if v is None:
self.exit()
return
self._state["source"] = "build" if v == "Build locally" else "pull"
self.push_screen(
PickScreen(
"Step 3 of 5",
"What to install:",
[
"Controller only — ros-core, ros-base, ros-iiwa7",
"Controller with Webots — ros-core, ros-base, ros-iiwa7-webots",
],
"Controller only — ros-core, ros-base, ros-iiwa7",
),
self._got_variant,
)
def _got_variant(self, v: Optional[str]) -> None:
if v is None:
self.exit()
return
self._state["variant"] = "webots" if v.startswith("Controller with Webots") else "controller"
self.push_screen(
PickScreen("Step 4 of 5", "Build type:", ["release", "dev"], "release"),
self._got_build_type,
)
def _got_build_type(self, v: Optional[str]) -> None:
if v is None:
self.exit()
return
self._state["build_type"] = v or "release"
# Pull needs a Hub repo name, build needs a local image prefix.
# Для pull нужно имя репозитория на Hub, для build - локальный префикс образов.
if self._state["source"] == "pull":
self.push_screen(
InputScreen("Step 5 of 5", "Docker Hub repository:", _DEFAULT_HUB_REPO),
self._got_hub_repo,
)
else:
self.push_screen(
InputScreen("Step 5 of 5", "Image prefix:", _DEFAULT_PREFIX),
self._got_prefix,
)
def _got_hub_repo(self, v: Optional[str]) -> None:
if v is None:
self.exit()
return
self._state["hub_repo"] = v
self._finish()
def _got_prefix(self, v: Optional[str]) -> None:
if v is None:
self.exit()
return
self._state["image_prefix"] = v
self._finish()
def _finish(self) -> None:
# Assemble the config and hand it off to the log screen that does the actual work.
# Собираем конфиг и передаём его экрану лога, который выполняет фактическую работу.
s = self._state
cfg = _Config(
ros_version=s["ros_version"],
variant=s["variant"],
source=s["source"],
build_type=s["build_type"],
image_prefix=s.get("image_prefix", _DEFAULT_PREFIX),
hub_repo=s.get("hub_repo", _DEFAULT_HUB_REPO),
)
title = (
f"Building Docker images — ROS {cfg.ros_version}"
if cfg.source == "build"
else f"Pulling Docker image — ROS {cfg.ros_version}"
)
self.push_screen(
LogScreen(title, lambda screen: _task_execute(screen, cfg), show_progress=True),
lambda _: self.exit(),
)
def register(subparsers: argparse._SubParsersAction) -> None:
p = subparsers.add_parser("docker-setup", help="Build or pull Docker images for KUKA iiwa7")
p.set_defaults(func=run)
@@ -403,10 +177,52 @@ def register(subparsers: argparse._SubParsersAction) -> None:
def run(args: argparse.Namespace) -> None:
if not shutil.which("docker"):
from rich.console import Console
Console().print("[red]Error:[/red] Docker is not installed or not on PATH.")
ui.error("Docker не установлен или отсутствует в PATH.")
sys.exit(1)
ui.header("Настройка Docker", "сборка или скачивание образов KUKA iiwa7")
versions = _discover_versions()
default = "jazzy" if "jazzy" in versions else versions[0]
_Wizard(versions=versions, default_version=default).run()
ros_version = ui.select("Версия ROS:", versions, default)
if ros_version is None:
return
src = ui.select("Источник:", ["Скачать с Docker Hub", "Собрать локально"],
"Скачать с Docker Hub")
if src is None:
return
source = "build" if src == "Собрать локально" else "pull"
variant_v = ui.select(
"Что установить:",
["Только контроллер — ros-core, ros-base, ros-iiwa7",
"Контроллер с Webots — ros-core, ros-base, ros-iiwa7-webots"],
"Только контроллер — ros-core, ros-base, ros-iiwa7",
)
if variant_v is None:
return
variant = "webots" if variant_v.startswith("Контроллер с Webots") else "controller"
build_type = ui.select("Тип сборки:", ["release", "dev"], "release")
if build_type is None:
return
image_prefix, hub_repo = _DEFAULT_PREFIX, _DEFAULT_HUB_REPO
if source == "pull":
v = ui.text("Репозиторий Docker Hub:", _DEFAULT_HUB_REPO)
if v is None:
return
hub_repo = v
else:
v = ui.text("Префикс образов:", _DEFAULT_PREFIX)
if v is None:
return
image_prefix = v
cfg = _Config(
ros_version=ros_version, variant=variant, source=source,
build_type=build_type, image_prefix=image_prefix, hub_repo=hub_repo,
)
_execute(cfg)
cobot/commands/local_setup.py
+249 -370
View File
@@ -5,11 +5,10 @@ import os
import shutil
import subprocess
from pathlib import Path
from typing import Callable, List, Optional
from textual.app import App
from cobot.tui import SCREEN_CSS, LogScreen, PickScreen
from cobot import process, ui
from cobot import privilege
from cobot.ui import done, header
from cobot.commands.docker_setup import run as _docker_setup
# Root directory of the project, used as the working directory for colcon builds.
@@ -28,9 +27,12 @@ _WEBOTS_VERSION = "2025a"
# Директория с shell-скриптами, используемыми этой командой.
_SCRIPTS_DIR = _PROJECT_DIR / "scripts"
# Type alias for the callable used to write a line to the TUI log screen.
# Псевдоним типа для функции записи строки в лог TUI.
Write = Callable[[str], None]
# apt packages that must exist before rosdep can install the pip-based keys
# (python3-pip / dev / venv). Their absence is what produced the "pip is not
# installed" failure in the screenshots.
# apt-пакеты, необходимые до того как rosdep сможет установить pip-зависимости.
# Именно их отсутствие давало ошибку "pip is not installed" на скриншотах.
_APT_PREREQS = ["python3-pip", "python3-dev", "python3-venv"]
# OS and tool detection helpers
@@ -38,9 +40,7 @@ Write = Callable[[str], None]
def _detect_ubuntu_2404() -> bool:
"""Return True if the current OS is Ubuntu 24.04 (Noble).
Reads /etc/os-release and checks the ID and VERSION_ID fields.
Возвращает True, если текущая ОС - Ubuntu 24.04 (Noble).
Читает /etc/os-release и проверяет поля ID и VERSION_ID.
"""
path = Path("/etc/os-release")
if not path.exists():
@@ -55,7 +55,6 @@ def _detect_ubuntu_2404() -> bool:
def _detect_ros2() -> bool:
"""Return True if ROS2 Jazzy is already installed under /opt/ros/jazzy.
Возвращает True, если ROS2 Jazzy уже установлен в /opt/ros/jazzy.
"""
return Path(f"/opt/ros/{_DISTRO}").is_dir()
@@ -63,7 +62,6 @@ def _detect_ros2() -> bool:
def webots_installed() -> bool:
"""Return True if the Webots binary is available on PATH.
Возвращает True, если бинарный файл Webots доступен в PATH.
"""
return shutil.which("webots") is not None
@@ -77,8 +75,6 @@ def _ros2_env() -> dict:
os.environ if the setup file does not exist yet.
Формирует словарь окружения с переменными ROS2, полученными из setup.bash.
Запускает /opt/ros/jazzy/setup.bash в подпроцессе, перехватывает все
экспортированные переменные и объединяет их с копией os.environ.
Возвращает чистый os.environ если файл setup.bash ещё не существует.
"""
setup = Path(f"/opt/ros/{_DISTRO}/setup.bash")
@@ -96,15 +92,13 @@ def _ros2_env() -> dict:
# CMake's find_package(Python3) ignores PATH and uses its own search logic,
# so we must pin it explicitly to the system Python where catkin_pkg is installed.
# PATH reordering alone is not enough.
# CMake игнорирует PATH при поиске Python через find_package(Python3),
# поэтому явно указываем системный Python, где установлен catkin_pkg.
# Одного изменения PATH недостаточно.
env["Python3_EXECUTABLE"] = "/usr/bin/python3"
env["PYTHON_EXECUTABLE"] = "/usr/bin/python3"
# Also keep PATH clean so other tools (rosdep, colcon itself) use system Python.
# Заодно чистим PATH чтобы другие инструменты тоже использовали системный Python.
# Keep PATH clean so other tools (rosdep, colcon itself) use system Python.
# Чистим PATH чтобы другие инструменты тоже использовали системный Python.
_SYSTEM_PATHS = ["/usr/bin", "/usr/local/bin"]
existing = env.get("PATH", "").split(":")
env["PATH"] = ":".join(
@@ -113,374 +107,277 @@ def _ros2_env() -> dict:
return env
# Subprocess runner helpers
# Вспомогательные функции для запуска подпроцессов
def _run_logged(
cmd: List[str],
write: Write,
env: dict | None = None,
cwd=None,
register_proc: Callable | None = None,
) -> None:
"""Run a command and stream every non-empty output line to the TUI log.
Raises RuntimeError if the process exits with a non-zero code (SIGKILL is
treated as a normal cancellation and does not raise).
Запускает команду и передаёт каждую непустую строку вывода в лог TUI.
Выбрасывает RuntimeError если процесс завершился с ненулевым кодом
(SIGKILL считается нормальной отменой и не вызывает исключение).
# Bash-script runner that understands PROGRESS:<pct>:<label> markers
# Запуск bash-скриптов с поддержкой маркеров PROGRESS:<pct>:<метка>
def _run_script(script: Path, title: str) -> int:
"""Run a shell script, streaming its output to a live log and advancing the
progress bar from PROGRESS:<pct>:<label> markers (which are not echoed raw).
Returns the script exit code.
Запускает shell-скрипт, транслируя вывод в живой лог и продвигая прогресс-бар по
маркерам PROGRESS:<pct>:<метка> (сами маркеры не печатаются). Возвращает код возврата.
"""
proc = subprocess.Popen(
cmd,
stdout=subprocess.PIPE,
stderr=subprocess.STDOUT,
text=True,
env=env or os.environ,
cwd=cwd,
)
if register_proc:
register_proc(proc)
for line in proc.stdout:
s = line.rstrip()
if s:
write(s)
proc.wait()
if proc.returncode not in (0, -9):
raise RuntimeError(f"Command failed: {cmd[0]}")
if not script.exists():
header(title)
ui.error(f"Скрипт не найден: {script}")
done(False, "Скрипт отсутствует")
return 1
with process.StepProgress(title) as p:
def on_line(s: str) -> None:
if s.startswith("PROGRESS:"):
parts = s.split(":", 2)
try:
p.set(float(parts[1]), parts[2] if len(parts) > 2 else "")
except (ValueError, IndexError):
pass
return
if s:
p.log(s)
rc = process.stream(["bash", str(script)], cwd=str(_PROJECT_DIR), on_line=on_line)
ok = rc in (0, -9, -15)
done(ok, "Готово" if ok else f"Скрипт завершился с кодом {rc}")
return rc
# Tasks - long-running functions executed inside a LogScreen background thread
# Задачи - долгие функции, выполняемые в фоновом потоке внутри LogScreen
def _run_script(script: Path, screen: LogScreen) -> None:
"""Run a shell script, stream its output to the TUI log, and parse
PROGRESS:<pct>:<label> markers to update the progress bar.
Raises RuntimeError if the script exits with a non-zero code.
Запускает shell-скрипт, транслирует вывод в лог TUI и разбирает маркеры
PROGRESS:<pct>:<метка> для обновления прогресс-бара.
Выбрасывает RuntimeError если скрипт завершился с ненулевым кодом.
"""
proc = subprocess.Popen(
["bash", str(script)],
stdout=subprocess.PIPE, stderr=subprocess.STDOUT,
text=True, cwd=_PROJECT_DIR,
)
screen.set_proc(proc)
for line in proc.stdout:
s = line.rstrip()
if s.startswith("PROGRESS:"):
# Format emitted by scripts: PROGRESS:<pct>:<label>
# Формат, выводимый скриптами: PROGRESS:<pct>:<метка>
parts = s.split(":", 2)
try:
screen.set_progress(float(parts[1]), parts[2] if len(parts) > 2 else "")
except (ValueError, IndexError):
pass
elif s:
screen.write(s)
proc.wait()
if proc.returncode not in (0, -9):
raise RuntimeError(f"Script failed (exit {proc.returncode}): {script.name}")
def _task_install(screen: LogScreen, pkg: str) -> None:
"""Run the ROS2 Jazzy installation shell script for the chosen variant (desktop / ros-base).
The script emits PROGRESS: markers so the bar advances during installation.
def install_ros2(pkg: str) -> bool:
"""Run the ROS2 Jazzy install shell script for the chosen variant (desktop / ros-base).
Запускает shell-скрипт установки ROS2 Jazzy для выбранного варианта (desktop / ros-base).
Скрипт выводит маркеры PROGRESS:, чтобы прогресс-бар обновлялся во время установки.
"""
script = _SCRIPTS_DIR / f"setup_ros2_{pkg.replace('-', '_')}.sh"
rc = _run_script(script, f"Установка ROS2 {_DISTRO} ({pkg})")
return rc in (0, -9, -15)
def install_webots() -> bool:
"""Run the Webots installation shell script.
Запускает shell-скрипт установки Webots.
"""
script = _SCRIPTS_DIR / "install_webots.sh"
rc = _run_script(script, f"Установка Webots {_WEBOTS_VERSION}")
return rc in (0, -9, -15)
# Build prerequisites
# Предусловия сборки
def _missing_apt_prereqs() -> list[str]:
"""Return the subset of _APT_PREREQS that is not currently installed via dpkg.
Возвращает подмножество _APT_PREREQS, которое сейчас не установлено через dpkg.
"""
missing = []
for pkg in _APT_PREREQS:
r = subprocess.run(
["dpkg-query", "-W", "-f=${Status}", pkg],
capture_output=True, text=True,
)
if "install ok installed" not in r.stdout:
missing.append(pkg)
return missing
def _ensure_root_pip_break(p: process.StepProgress) -> None:
"""Let root's pip override PEP 668, scoped to /root/.config/pip/pip.conf.
rosdep installs the pip-based rosdep keys (fastapi, uvicorn, multipart, fastmcp)
as root via sudo; on Ubuntu 24.04 that is blocked by PEP 668 unless break-system-
packages is allowed. Writing root's pip config is idempotent, reversible (just
delete the file), and does not touch the user's own pip configuration.
Разрешает pip от root обходить PEP 668, ограничиваясь /root/.config/pip/pip.conf.
rosdep ставит pip-зависимости от root через sudo; на Ubuntu 24.04 это блокируется
PEP 668, пока не разрешён break-system-packages. Запись конфига pip от root
идемпотентна, обратима (удалить файл) и не трогает пользовательский pip.
"""
snippet = (
"mkdir -p /root/.config/pip && "
"( grep -qs 'break-system-packages' /root/.config/pip/pip.conf || "
"printf '[global]\\nbreak-system-packages = true\\n' "
">> /root/.config/pip/pip.conf )"
)
process.stream(privilege.sudo(["bash", "-c", snippet]), on_line=p.log)
# rosdep calls `pip install -U <pkg>` which upgrades every transitive dependency,
# including packages installed by apt that have no pip RECORD file, causing an
# uninstall failure. Pre-installing with --ignore-installed creates pip RECORD
# files for all transitive deps so the subsequent rosdep upgrade succeeds.
process.stream(
privilege.sudo(["pip3", "install", "--break-system-packages",
"--ignore-installed", "fastmcp"]),
on_line=p.log,
)
def _register_rosdep_source(p: process.StepProgress, env: dict) -> None:
"""Register the project's local rosdep.yaml as a rosdep source and run rosdep update.
Only re-writes / updates when the source file is missing or out of date.
Регистрирует локальный rosdep.yaml проекта как источник rosdep и запускает rosdep update.
Перезаписывает/обновляет только если файл-источник отсутствует или устарел.
"""
rosdep_yaml = _PROJECT_DIR / "rosdep.yaml"
if not rosdep_yaml.exists():
return
sources_list = Path("/etc/ros/rosdep/sources.list.d/50-kuka-local.list")
entry = f"yaml file://{rosdep_yaml}\n"
try:
# "desktop" -> setup_ros2_desktop.sh, "ros-base" -> setup_ros2_ros_base.sh
script = _SCRIPTS_DIR / f"setup_ros2_{pkg.replace('-', '_')}.sh"
if not script.exists():
screen.write(f"[red]Script not found:[/red] {script}")
screen.finish(False)
return
screen.set_progress(0, "Starting installation...")
_run_script(script, screen)
if not screen.is_stopped():
screen.set_progress(100, "Done")
screen.write(f"\n[green]ROS2 {_DISTRO} ({pkg}) installed successfully.[/green]")
screen.finish(True)
except Exception as exc:
if not screen.is_stopped():
screen.write(f"\n[red]Error:[/red] {exc}")
screen.finish(False)
current = sources_list.read_text() if sources_list.exists() else ""
except Exception:
current = ""
if current == entry:
return
snippet = (
"mkdir -p /etc/ros/rosdep/sources.list.d && "
f"printf '%s\\n' 'yaml file://{rosdep_yaml}' > {sources_list}"
)
process.stream(privilege.sudo(["bash", "-c", snippet]), on_line=p.log)
p.log(f"Зарегистрирован локальный источник rosdep: {rosdep_yaml}")
process.stream(["rosdep", "update"], env=env, cwd=str(_PROJECT_DIR), on_line=p.log)
def _task_build(screen: LogScreen) -> None:
"""Build the project workspace using rosdep and colcon.
Step 1 - runs "rosdep install --from-paths src" to pull in all package
dependencies declared in the src/ directory.
Step 2 - runs "colcon build --symlink-install" to compile every package.
Both commands receive a copy of os.environ extended with the sourced ROS2
setup so that ament CMake macros and ROS2 packages are visible even if the
user has not yet sourced setup.bash in this terminal session.
Собирает рабочее пространство проекта с помощью rosdep и colcon.
Шаг 1 - запускает "rosdep install --from-paths src" для установки всех
зависимостей пакетов, объявленных в директории src/.
Шаг 2 - запускает "colcon build --symlink-install" для компиляции каждого пакета.
Обе команды получают копию os.environ с подключённым окружением ROS2, так что
макросы ament CMake и пакеты ROS2 видны даже если пользователь ещё не выполнил
source setup.bash в этой сессии терминала.
def _count_colcon_packages(env: dict) -> int:
"""Count colcon packages under src/ so the build bar can show X / total.
Считает пакеты colcon в src/, чтобы бар сборки показывал X / всего.
"""
try:
env = _ros2_env()
if not shutil.which("colcon") and not Path(f"/opt/ros/{_DISTRO}/bin/colcon").exists():
screen.write("[red]colcon not found.[/red]")
screen.write(f"Source ROS2 first: [bold]source /opt/ros/{_DISTRO}/setup.bash[/bold]")
screen.finish(False)
return
screen.set_progress(0, "Installing dependencies...")
screen.write("[bold]Step 1 / 2 - rosdep install[/bold]\n")
_run_logged(
["rosdep", "install", "--from-paths", "src", "-i", "-r", "-y"],
screen.write,
env=env,
cwd=_PROJECT_DIR,
register_proc=screen.set_proc,
)
if screen.is_stopped():
return
screen.set_progress(30, "Building...")
list_result = subprocess.run(
["colcon", "list", "--base-paths", "src"], capture_output=True, text=True,
cwd=_PROJECT_DIR, env=env,
)
total = max(len([l for l in list_result.stdout.splitlines() if l.strip()]), 1)
screen.write(f"\n[bold]Step 2 / 2 - colcon build ({total} packages)[/bold]\n")
built = 0
def _track(line: str) -> None:
"""Update the progress bar each time colcon finishes a package.
Обновляет прогресс-бар каждый раз, когда colcon завершает пакет.
"""
nonlocal built
screen.write(line)
if "Finished <<<" in line or "Failed <<<" in line:
built += 1
screen.set_progress(
30 + built / total * 70,
f"{built} / {total} packages done",
)
_run_logged(
["colcon", "build", "--base-paths", "src"],
_track,
env=env,
cwd=_PROJECT_DIR,
register_proc=screen.set_proc,
)
if not screen.is_stopped():
screen.set_progress(100, "Build complete")
screen.write("\nActivate workspace: [bold]source install/setup.bash[/bold]")
screen.finish(True)
except Exception as exc:
if not screen.is_stopped():
screen.write(f"\n[red]Error:[/red] {exc}")
screen.finish(False)
r = subprocess.run(
["colcon", "list", "--base-paths", "src"],
capture_output=True, text=True, cwd=str(_PROJECT_DIR), env=env,
)
return max(len([l for l in r.stdout.splitlines() if l.strip()]), 1)
def _task_install_webots(screen: LogScreen) -> None:
"""Run the Webots installation shell script, streaming output and progress to the TUI.
def build_workspace() -> bool:
"""Build the workspace: apt prerequisites -> rosdep install -> colcon build.
Запускает shell-скрипт установки Webots, транслируя вывод и прогресс в TUI.
Step 1 guarantees python3-pip/dev/venv and allows root pip under PEP 668 so the
pip-based rosdep keys install cleanly. Step 2 runs rosdep install with
PIP_BREAK_SYSTEM_PACKAGES=1. Step 3 compiles every package with live progress.
Собирает workspace: apt-предусловия -> rosdep install -> colcon build.
Шаг 1 гарантирует python3-pip/dev/venv и разрешает pip от root под PEP 668. Шаг 2
запускает rosdep install с PIP_BREAK_SYSTEM_PACKAGES=1. Шаг 3 компилирует все пакеты.
"""
try:
script = _SCRIPTS_DIR / "install_webots.sh"
if not script.exists():
screen.write(f"[red]Script not found:[/red] {script}")
screen.finish(False)
return
screen.set_progress(0, "Starting Webots installation...")
_run_script(script, screen)
if not screen.is_stopped():
screen.set_progress(100, "Done")
screen.write(f"\n[green]Webots {_WEBOTS_VERSION} installed successfully.[/green]")
screen.finish(True)
except Exception as exc:
if not screen.is_stopped():
screen.write(f"\n[red]Error:[/red] {exc}")
screen.finish(False)
env = _ros2_env()
env["PIP_BREAK_SYSTEM_PACKAGES"] = "1"
if not shutil.which("colcon") and not Path(f"/opt/ros/{_DISTRO}/bin/colcon").exists():
header("Сборка проекта")
ui.error("colcon не найден.")
ui.note(f"Сначала выполните: source /opt/ros/{_DISTRO}/setup.bash")
done(False, "colcon недоступен")
return False
# TUI application - orchestrates screens and user choices
# TUI приложение - управляет экранами и выборами пользователя
class _LocalSetupApp(App[Optional[str]]):
"""Main TUI application for the local-setup command.
ok = True
fail_msg = ""
Guides the user through: install ROS2 choice, OS check, version choice,
installation log, build log, and optional Webots installation.
Returns "docker" if the user opts for Docker setup, None otherwise.
Главное TUI приложение для команды local-setup.
Проводит пользователя через: выбор установки ROS2, проверку ОС, выбор версии,
лог установки, лог сборки и опциональную установку Webots.
Возвращает "docker" если пользователь выбирает Docker, иначе None.
"""
CSS = SCREEN_CSS
def on_mount(self) -> None:
self.push_screen(
PickScreen(
"local-setup",
"Install ROS2 Jazzy?",
["Yes, install", "No, exit"],
"Yes, install",
),
self._on_install_choice,
)
def _on_install_choice(self, choice: Optional[str]) -> None:
"""Handle the initial yes/no choice to install ROS2.
Обрабатывает начальный выбор да/нет для установки ROS2.
"""
if not choice or choice.startswith("No"):
self.exit(None)
return
if not _detect_ubuntu_2404():
self.push_screen(
PickScreen(
"Unsupported OS",
"Ubuntu 24.04 not detected. Set up the environment via Docker instead?",
["Yes, run docker-setup", "No, exit"],
"Yes, run docker-setup",
),
self._on_docker_choice,
with process.StepProgress("Сборка проекта") as p:
# --- Шаг 1/3: системные зависимости pip (apt) ---
p.raw("[bold]Шаг 1/3 — системные зависимости pip (apt)[/bold]")
p.set(0, "Проверка python3-pip / dev / venv...")
missing = _missing_apt_prereqs()
if missing:
p.log(f"Установка: {', '.join(missing)}")
process.stream(privilege.sudo(["apt-get", "update", "-q"]), env=env, on_line=p.log)
rc = process.stream(
privilege.sudo(["apt-get", "install", "-y", *missing]),
env=env, on_line=p.log,
)
if rc not in (0, -9, -15):
ok, fail_msg = False, "Не удалось установить apt-зависимости"
else:
self.push_screen(
PickScreen(
"ROS2 version",
"Which ROS2 Jazzy variant do you want to install?",
["Desktop (full install, includes GUI tools)", "Base (minimal, no GUI)"],
"Desktop (full install, includes GUI tools)",
),
self._on_version_choice,
p.log("python3-pip / dev / venv уже установлены")
if ok:
_ensure_root_pip_break(p)
# --- Шаг 2/3: rosdep install ---
if ok:
p.set(10, "rosdep install...")
p.raw("\n[bold]Шаг 2/3 — rosdep install[/bold]")
_register_rosdep_source(p, env)
rc = process.stream(
["rosdep", "install", "--from-paths", "src", "-i", "-r", "-y"],
env=env, cwd=str(_PROJECT_DIR), on_line=p.log,
)
if rc not in (0, -9, -15):
ok, fail_msg = False, "rosdep install завершился с ошибкой"
def _on_docker_choice(self, choice: Optional[str]) -> None:
"""Exit the app signalling whether docker-setup should be launched.
Завершает приложение, сигнализируя нужно ли запустить docker-setup.
"""
self.exit("docker" if choice and choice.startswith("Yes") else None)
# --- Шаг 3/3: colcon build ---
if ok:
total = _count_colcon_packages(env)
p.set(30, f"0 / {total} пакетов")
p.raw(f"\n[bold]Шаг 3/3 — colcon build ({total} пакетов)[/bold]")
built = 0
def _on_version_choice(self, choice: Optional[str]) -> None:
"""Start the installation log screen for the chosen ROS2 variant.
Запускает экран лога установки для выбранного варианта ROS2.
"""
if not choice:
self.exit(None)
return
pkg = "desktop" if choice.startswith("Desktop") else "ros-base"
self.push_screen(
LogScreen(
f"Installing ROS2 Jazzy ({pkg})",
lambda s: _task_install(s, pkg),
show_progress=True,
),
lambda _: self._after_install(),
)
def _on_build(s: str) -> None:
nonlocal built
if s:
p.log(s)
if "Finished <<<" in s or "Failed <<<" in s:
built += 1
p.set(30 + built / total * 70, f"{built} / {total} пакетов")
def _after_install(self) -> None:
"""After installation, ask whether to build the project workspace now.
После установки спрашивает, нужно ли собрать рабочее пространство прямо сейчас.
"""
self.push_screen(
PickScreen(
"Build",
"Build the project workspace now?\n(runs rosdep install + colcon build)",
["Yes, build now", "No, skip"],
"Yes, build now",
),
self._on_build_choice,
)
def _on_build_choice(self, choice: Optional[str]) -> None:
"""Start the build log screen or skip directly to the Webots prompt.
Запускает экран сборки или пропускает к вопросу про Webots.
"""
if not choice or choice.startswith("No"):
self._after_build()
return
self.push_screen(
LogScreen("Building project", _task_build, show_progress=True),
lambda _: self._after_build(),
)
def _after_build(self) -> None:
"""After the build, offer to install Webots if it is not already present.
После сборки предлагает установить Webots если он ещё не установлен.
"""
if webots_installed():
self.exit(None)
return
self.push_screen(
PickScreen(
"Webots",
f"Install Webots {_WEBOTS_VERSION} simulator?",
[f"Yes, install Webots {_WEBOTS_VERSION}", "No, skip"],
"No, skip",
),
self._on_webots_choice,
)
def _on_webots_choice(self, choice: Optional[str]) -> None:
"""Start the Webots installer or exit depending on the user choice.
Запускает установщик Webots или завершает работу в зависимости от выбора.
"""
if choice and choice.startswith("Yes"):
subprocess.run(["sudo", "-v"], check=False)
self.push_screen(
LogScreen(
f"Installing Webots {_WEBOTS_VERSION}",
_task_install_webots,
show_progress=True,
),
lambda _: self.exit(None),
rc = process.stream(
["colcon", "build", "--base-paths", "src"],
env=env, cwd=str(_PROJECT_DIR), on_line=_on_build,
)
else:
self.exit(None)
if rc not in (0, -9, -15):
ok, fail_msg = False, "colcon build завершился с ошибкой"
else:
p.set(100, "Готово")
done(ok, "Сборка завершена" if ok else fail_msg)
if ok:
ui.note("Активируйте окружение: source install/setup.bash")
return ok
class WebotsInstallApp(App[bool]):
"""Standalone TUI app for installing Webots, used by the run command.
Launched by run.py when the user starts a local simulation but Webots
is not installed yet.
Отдельное TUI приложение для установки Webots, используемое командой run.
Запускается из run.py когда пользователь запускает локальную симуляцию,
но Webots ещё не установлен.
# Interactive flow
# Интерактивный сценарий
def run(args: argparse.Namespace) -> None:
"""Guide the user through installing ROS2 Jazzy and building the workspace.
Проводит пользователя через установку ROS2 Jazzy и сборку workspace.
"""
header("Локальная установка", "ROS2 Jazzy + сборка проекта")
CSS = SCREEN_CSS
choice = ui.select("Установить ROS2 Jazzy?", ["Да, установить", "Нет, выход"],
"Да, установить")
if not choice or choice.startswith("Нет"):
return
def on_mount(self) -> None:
self.push_screen(
LogScreen(
f"Installing Webots {_WEBOTS_VERSION}",
_task_install_webots,
show_progress=True,
),
self.exit,
# Acquire sudo once, up front, with the masked prompt + keep-alive thread.
# Получаем sudo один раз, заранее, с маскированным вводом + keep-alive потоком.
if not privilege.ensure_sudo():
return
if not _detect_ubuntu_2404():
v = ui.select(
"Ubuntu 24.04 не обнаружена. Настроить окружение через Docker?",
["Да, запустить docker-setup", "Нет, выход"],
"Да, запустить docker-setup",
)
if v and v.startswith("Да"):
_docker_setup(args)
return
variant = ui.select(
"Какой вариант ROS2 Jazzy установить?",
["Desktop (полный, с GUI-инструментами)", "Base (минимальный, без GUI)"],
"Desktop (полный, с GUI-инструментами)",
)
if not variant:
return
pkg = "desktop" if variant.startswith("Desktop") else "ros-base"
if not install_ros2(pkg):
return
if ui.confirm("Собрать workspace сейчас? (rosdep install + colcon build)", default=True):
build_workspace()
if not webots_installed():
if ui.confirm(f"Установить симулятор Webots {_WEBOTS_VERSION}?", default=False):
install_webots()
# Entry point - registered as the "local-setup" subcommand
# Точка входа - зарегистрирована как подкоманда "local-setup"
def register(subparsers: argparse._SubParsersAction) -> None:
"""Register the local-setup subcommand with the CLI argument parser.
Регистрирует подкоманду local-setup в парсере аргументов командной строки.
"""
p = subparsers.add_parser(
@@ -488,21 +385,3 @@ def register(subparsers: argparse._SubParsersAction) -> None:
help="Install ROS2 Jazzy natively and build the project with colcon",
)
p.set_defaults(func=run)
def run(args: argparse.Namespace) -> None:
"""Entry point for the local-setup command.
Pre-caches the sudo token while the terminal is in normal mode so that
subsequent sudo calls inside the Textual TUI do not hang waiting for
a password prompt that the user cannot see.
Точка входа для команды local-setup.
Предварительно кеширует sudo-токен пока терминал в обычном режиме, чтобы
последующие вызовы sudo внутри Textual TUI не зависали ожидая запрос пароля,
который пользователь не может увидеть.
"""
subprocess.run(["sudo", "-v"], check=False)
result = _LocalSetupApp().run()
if result == "docker":
_docker_setup(args)
cobot/commands/rebuild.py
+63 -138
View File
@@ -5,154 +5,63 @@ import subprocess
from pathlib import Path
from typing import List, Optional
from textual.app import App
from cobot.tui import SCREEN_CSS, InputScreen, LogScreen, PickScreen
from cobot import ui, process
from cobot.commands.local_setup import _ros2_env
_PROJECT_DIR = Path(__file__).parent.parent.parent
def _task_rebuild(screen: LogScreen, packages: List[str], symlink: bool) -> None:
"""Run colcon build for the selected packages (or all if packages is empty).
Streams output to the log and tracks per-package progress.
Запускает colcon build для выбранных пакетов (или всех если packages пуст).
Транслирует вывод в лог и отслеживает прогресс по каждому пакету.
def _count_packages(packages: List[str], env: dict) -> int:
"""Count how many colcon packages will be built so we can show X / total progress.
Считает количество пакетов colcon для отображения прогресса X / всего.
"""
try:
env = _ros2_env()
# Count packages so we can show X / total progress.
# Считаем пакеты чтобы показывать X / всего в прогрессе.
list_cmd = ["colcon", "list", "--base-paths", "src"]
if packages:
list_cmd += ["--packages-select"] + packages
list_result = subprocess.run(
list_cmd, capture_output=True, text=True,
cwd=_PROJECT_DIR, env=env,
)
total = max(len([l for l in list_result.stdout.splitlines() if l.strip()]), 1)
pkg_label = " ".join(packages) if packages else "all packages"
symlink_label = " --symlink-install" if symlink else ""
screen.write(f"[bold]colcon build{symlink_label} — {pkg_label}[/bold]\n")
screen.set_progress(0, f"0 / {total} packages done")
built = 0
cmd = ["colcon", "build", "--base-paths", "src"]
if symlink:
cmd.append("--symlink-install")
if packages:
cmd += ["--packages-select"] + packages
proc = subprocess.Popen(
cmd,
stdout=subprocess.PIPE, stderr=subprocess.STDOUT,
text=True, cwd=_PROJECT_DIR, env=env,
)
screen.set_proc(proc)
for line in proc.stdout:
s = line.rstrip()
if s:
screen.write(s)
if "Finished <<<" in line or "Failed <<<" in line:
built += 1
screen.set_progress(
built / total * 100,
f"{built} / {total} packages done",
)
proc.wait()
if screen.is_stopped():
return
if proc.returncode not in (0, -9):
screen.write("\n[red]Build failed.[/red]")
screen.finish(False)
return
screen.set_progress(100, "Done")
screen.write("\n[green]Build complete.[/green]")
screen.finish(True)
except Exception as exc:
if not screen.is_stopped():
screen.write(f"\n[red]Error:[/red] {exc}")
screen.finish(False)
list_cmd = ["colcon", "list", "--base-paths", "src"]
if packages:
list_cmd += ["--packages-select"] + packages
result = subprocess.run(list_cmd, capture_output=True, text=True,
cwd=_PROJECT_DIR, env=env)
return max(len([l for l in result.stdout.splitlines() if l.strip()]), 1)
class _RebuildApp(App[None]):
"""Rebuild wizard: optionally asks for packages and symlink flag, then runs colcon.
Мастер пересборки: опционально спрашивает пакеты и флаг symlink, затем запускает colcon.
def _rebuild(packages: List[str], symlink: bool) -> None:
"""Run colcon build for the selected packages (or all), streaming live output
with a per-package progress bar.
Запускает colcon build для выбранных пакетов (или всех), транслируя живой вывод
с прогресс-баром по пакетам.
"""
env = _ros2_env()
total = _count_packages(packages, env)
CSS = SCREEN_CSS
pkg_label = " ".join(packages) if packages else "все пакеты"
symlink_label = " --symlink-install" if symlink else ""
def __init__(self, packages: Optional[List[str]], symlink: Optional[bool]):
super().__init__()
# None means "ask the user interactively".
# None означает "спросить пользователя интерактивно".
self._packages = packages
self._symlink = symlink
cmd = ["colcon", "build", "--base-paths", "src"]
if symlink:
cmd.append("--symlink-install")
if packages:
cmd += ["--packages-select"] + packages
def on_mount(self) -> None:
if self._packages is None:
self._ask_packages()
elif self._symlink is None:
self._ask_symlink()
else:
self._start()
built = 0
def _ask_packages(self) -> None:
self.push_screen(
InputScreen(
"rebuild",
"Packages to rebuild (space-separated, leave empty for all):",
"",
note="Example: iiwa_controller iiwa_bringup",
),
self._got_packages,
)
def _parse(line: str):
nonlocal built
if "Finished <<<" in line or "Failed <<<" in line:
built += 1
return (built / total * 100, f"{built} / {total} пакетов")
return None
def _got_packages(self, value: Optional[str]) -> None:
if value is None:
self.exit()
return
self._packages = value.split() if value.strip() else []
if self._symlink is None:
self._ask_symlink()
else:
self._start()
def _ask_symlink(self) -> None:
self.push_screen(
PickScreen(
"rebuild",
"Use --symlink-install?",
["Yes", "No"],
"Yes",
),
self._got_symlink,
)
def _got_symlink(self, value: Optional[str]) -> None:
if value is None:
self.exit()
return
self._symlink = value == "Yes"
self._start()
def _start(self) -> None:
self.push_screen(
LogScreen(
"Rebuilding packages",
lambda s: _task_rebuild(s, self._packages, self._symlink),
show_progress=True,
),
lambda _: self.exit(),
)
rc = process.run_step(
f"colcon build{symlink_label} — {pkg_label}",
cmd,
env=env,
cwd=str(_PROJECT_DIR),
total=100.0,
parse_progress=_parse,
success_msg="Сборка завершена",
fail_msg="Сборка завершилась с ошибкой",
)
if rc in (0, -9, -15):
ui.note("Активируйте окружение: source install/setup.bash")
def register(subparsers: argparse._SubParsersAction) -> None:
@@ -186,7 +95,23 @@ def run(args: argparse.Namespace) -> None:
"""Entry point for the rebuild command.
Точка входа для команды rebuild.
"""
# Convert empty list to None so the TUI asks interactively.
# Преобразуем пустой список в None, чтобы TUI спросил интерактивно.
packages = args.packages if args.packages else None
_RebuildApp(packages, args.symlink).run()
packages: Optional[List[str]] = args.packages if args.packages else None
if packages is None:
value = ui.text(
"Какие пакеты пересобрать? (через пробел, пусто — все)",
"",
note="Пример: iiwa_controller iiwa_bringup",
)
if value is None:
return
packages = value.split() if value.strip() else []
symlink = args.symlink
if symlink is None:
choice = ui.select("Использовать --symlink-install?", ["Да", "Нет"], "Да")
if choice is None:
return
symlink = choice == "Да"
_rebuild(packages, symlink)
cobot/commands/robot_setup.py
+207 -199
View File
@@ -2,52 +2,78 @@ from __future__ import annotations
import argparse
import sys
from dataclasses import dataclass, field
from dataclasses import dataclass
from pathlib import Path
from typing import Any, Callable, List, Optional, Tuple
from typing import Any, List, Optional
from ruamel.yaml import YAML
from textual.app import App, ComposeResult
from textual.binding import Binding
from textual.screen import Screen
from textual.widgets import Footer, Static
from cobot.tui import SCREEN_CSS, InputScreen, PickScreen
from cobot import ui
_PROJECT_DIR = Path(__file__).parent.parent.parent
_CONFIG_PATH = _PROJECT_DIR / "cobot-setting.yaml"
# Use ruamel.yaml instead of PyYAML so comments and formatting in the config file are preserved.
_TOOLS_YAML = _PROJECT_DIR / "src" / "iiwa_config" / "config" / "tools.yaml"
_TOOL_ACTIVE_XACRO = (
_PROJECT_DIR / "src" / "iiwa_description" / "urdf" / "tools" / "tool_active.xacro"
)
_SRDF_PATH = _PROJECT_DIR / "src" / "iiwa_config" / "config" / "moveit" / "iiwa7.srdf"
# Use ruamel.yaml instead of PyYAML so comments and formatting in the config are preserved.
# Используем ruamel.yaml вместо PyYAML, чтобы комментарии и форматирование в конфиге сохранялись.
_yaml = YAML()
_yaml.preserve_quotes = True
# One question inside a configuration block.
# A field can either show a pick list (options) or a free-text input (no options).
# Один вопрос внутри блока конфигурации.
# Поле может показывать список вариантов (options) или поле для ввода текста (без options).
@dataclass
class _Field:
key: str # dot-separated path within the block, e.g. "webots.world"
question: str
default: Any
note: str = ""
options: Optional[List[str]] = None # if set - PickScreen, else - InputScreen
options: Optional[List[str]] = None # if set - select(), else - text()
def label(self) -> str:
return self.key.split(".")[-1]
# A group of related fields shown together under one "Configure X?" question.
# Группа связанных полей, показываемая вместе под одним вопросом "Настроить X?".
@dataclass
class _Block:
yaml_key: str # top-level key in cobot-setting.yaml
title: str # shown in "Configure <title>?" prompt
title: str # shown in "Настроить <title>?" prompt
fields: List[_Field]
def _load_tools_registry() -> dict:
"""Читает tools.yaml и возвращает словарь инструментов."""
if not _TOOLS_YAML.exists():
return {}
_y = YAML()
with open(_TOOLS_YAML, encoding="utf-8") as f:
data = _y.load(f)
return dict(data.get("tools", {}))
def _build_tool_block() -> Optional[_Block]:
"""Строит блок выбора инструмента из реестра tools.yaml.
Возвращает None если реестр недоступен."""
registry = _load_tools_registry()
if not registry:
return None
options = list(registry.keys())
labels = " | ".join(
f"{name}: {registry[name].get('label', '')}" for name in options
)
return _Block(
yaml_key="tool",
title="Инструмент / Захват",
fields=[
_Field("active", "Выберите активный инструмент:", options[0],
note=labels, options=options),
],
)
# All configuration blocks. Each block maps to a top-level key in cobot-setting.yaml.
# Все блоки конфигурации. Каждый блок соответствует ключу верхнего уровня в cobot-setting.yaml.
_BLOCKS: List[_Block] = [
@@ -55,78 +81,83 @@ _BLOCKS: List[_Block] = [
yaml_key="foxglove",
title="Foxglove bridge",
fields=[
_Field("enabled", "Enable Foxglove bridge?", "true",
note="Start foxglove_bridge alongside the robot node",
_Field("enabled", "Включить Foxglove bridge?", "true",
note="Запускать foxglove_bridge вместе с узлом робота",
options=["true", "false"]),
_Field("port", "WebSocket port:", "8765",
note="Port Foxglove Studio connects to (default 8765)"),
_Field("address", "Listen address:", "0.0.0.0",
note="0.0.0.0 = all interfaces, 127.0.0.1 = localhost only",
_Field("port", "Порт WebSocket:", "8765",
note="Порт, к которому подключается Foxglove Studio (по умолчанию 8765)"),
_Field("address", "Адрес прослушивания:", "0.0.0.0",
note="0.0.0.0 = все интерфейсы, 127.0.0.1 = только localhost",
options=["0.0.0.0", "127.0.0.1"]),
_Field("use_sim_time", "Use simulation time (/clock)?", "false",
note="Subscribe to /clock instead of using wall time",
_Field("use_sim_time", "Использовать симуляционное время (/clock)?", "false",
note="Подписываться на /clock вместо системного времени",
options=["false", "true"]),
_Field("debug", "Enable verbose bridge logging?", "false",
_Field("debug", "Подробное логирование bridge?", "false",
options=["false", "true"]),
_Field("num_threads", "Executor threads (0 = auto):", "0"),
_Field("num_threads", "Потоки executor (0 = авто):", "0"),
],
),
_Block(
yaml_key="web",
title="Веб-сервер (FastAPI)",
fields=[
_Field("enabled", "Включить веб-сервер?", "true",
note="Запускать FastAPI-сервер для HTTP/WebSocket-управления",
options=["true", "false"]),
_Field("host", "Адрес прослушивания:", "0.0.0.0",
note="0.0.0.0 = все интерфейсы, 127.0.0.1 = только localhost",
options=["0.0.0.0", "127.0.0.1"]),
_Field("port", "Порт HTTP:", "8007",
note="Порт FastAPI-сервера (по умолчанию 8007)"),
],
),
_Block(
yaml_key="planning",
title="MoveIt planning",
title="MoveIt планирование",
fields=[
_Field("pose_link", "TCP link name:", "tcp",
note="Link used as the end-effector for Cartesian goals (defined in URDF/SRDF)"),
_Field("planning_group", "Planning group:", "iiwa_arm",
note="MoveIt planning group as defined in the SRDF"),
_Field("default_frame", "Default reference frame:", "base_link"),
_Field("default_planner", "Default planner:", "ompl",
_Field("planning_group", "Группа планирования:", "iiwa_arm",
note="Группа планирования MoveIt из SRDF"),
_Field("default_frame", "Система отсчёта по умолчанию:", "base_link"),
_Field("default_planner", "Планировщик по умолчанию:", "ompl",
options=["ompl", "pilz_industrial_motion_planner", "chomp"]),
_Field("planning_attempts", "Planning attempts:", "3"),
_Field("planning_attempts", "Попыток планирования:", "3"),
],
),
_Block(
yaml_key="digital_twin",
title="Digital twin (Webots / RViz)",
title="Цифровой двойник (Webots / RViz)",
fields=[
_Field("webots.transform", "Robot transform in Webots scene (x y z, metres):", "-0.25 0 0.79"),
_Field("webots.rotation", "Robot rotation in Webots scene (ax ay az angle):", "0 0 1 0"),
_Field("webots.controller_timer", "Webots controller step timer (ms):", "50"),
_Field("webots.transform", "Трансформ робота в сцене Webots (x y z, метры):", "-0.25 0 0.79"),
_Field("webots.rotation", "Поворот робота в сцене Webots (ax ay az угол):", "0 0 1 0"),
_Field("webots.controller_timer", "Шаг таймера контроллера Webots (мс):", "50"),
],
),
_Block(
yaml_key="robot",
title="Robot connection",
title="Подключение робота",
fields=[
_Field("name", "Robot model name:", "iiwa7"),
_Field("ip", "Robot IP address:", "192.170.10.2",
note="IP of the KUKA controller on the FRI network interface"),
_Field("port", "FRI port:", "30200"),
_Field("command_mode", "Command mode:", "position",
note="position = joint position control, torque = joint torque control",
options=["position", "torque"]),
_Field("fri_cycle_ms", "FRI cycle time (ms):", "10",
note="5 ms = 200 Hz, 10 ms = 100 Hz",
_Field("name", "Имя модели робота:", "iiwa7"),
_Field("ip", "IP-адрес робота:", "192.170.10.2",
note="IP контроллера KUKA на сетевом интерфейсе FRI"),
_Field("port", "Порт FRI:", "30200"),
_Field("fri_cycle_ms", "Цикл FRI (мс):", "10",
note="5 мс = 200 Гц, 10 мс = 100 Гц",
options=["10", "5"]),
_Field("active_controller", "Active ROS controller:", "jtc",
_Field("active_controller", "Активный ROS-контроллер:", "jtc",
note="jtc = JointTrajectoryController (MoveIt), forward = ForwardCommandController",
options=["jtc", "forward"]),
_Field("joint_position_tau", "Position EMA filter tau (s):", "0.04",
note="Smooths position commands before sending to FRI"),
_Field("joint_velocity_tau", "Velocity EMA filter tau (s):", "0.01",
note="Removes spikes from finite-difference velocity estimation"),
_Field("joint_position_tau", "EMA tau фильтра положения (с):", "0.04",
note="Сглаживает команды положения перед отправкой в FRI"),
_Field("joint_velocity_tau", "EMA tau фильтра скорости (с):", "0.01",
note="Убирает выбросы из оценки скорости конечной разностью"),
],
),
]
# Try to keep the original YAML type (bool, int, float) when saving a value back.
# Trying to preserve type prevents "true" from becoming a plain string in the YAML file.
# Пытаемся сохранить исходный тип YAML (bool, int, float) при записи значения обратно.
# Сохранение типа предотвращает превращение "true" в обычную строку в YAML-файле.
def _coerce(value: str, original: Any) -> Any:
"""Convert a string value to match the type of the original YAML value (bool, int, float, str).
Преобразует строковое значение к типу исходного значения YAML (bool, int, float, str).
"""Convert a string value to match the type of the original YAML value.
Преобразует строковое значение к типу исходного значения YAML.
"""
if isinstance(original, bool):
return value.lower() == "true"
@@ -143,11 +174,9 @@ def _coerce(value: str, original: Any) -> Any:
return value
# Read a value from a nested YAML mapping using a dot-separated key like "webots.transform".
# Читаем значение из вложенного YAML-словаря по ключу с точками, например "webots.transform".
def _get_nested(mapping: Any, path: str) -> Any:
"""Return the value at a dot-separated path inside a nested YAML mapping, or None if missing.
Возвращает значение по пути с точками внутри вложенного YAML-словаря, или None если отсутствует.
"""Return the value at a dot-separated path inside a nested YAML mapping, or None.
Возвращает значение по пути с точками внутри вложенного YAML-словаря, или None.
"""
keys = path.split(".")
cur = mapping
@@ -158,169 +187,148 @@ def _get_nested(mapping: Any, path: str) -> Any:
return cur
# Write a value into a nested YAML mapping using a dot-separated key.
# Записываем значение в вложенный YAML-словарь по ключу с точками.
def _infer(value: str) -> Any:
"""Infer a bool / int / float / str from a raw string when there is no original
value to match the type against (i.e. the key is new in the config).
Выводит bool / int / float / str из строки, когда нет исходного значения для
сопоставления типа (т.е. ключ новый в конфиге).
"""
low = value.strip().lower()
if low in ("true", "false"):
return low == "true"
try:
return int(value)
except ValueError:
pass
try:
return float(value)
except ValueError:
pass
return value
def _set_nested(mapping: Any, path: str, value: Any) -> None:
"""Set the value at a dot-separated path inside a nested YAML mapping, coercing type to match.
Устанавливает значение по пути с точками во вложенном YAML-словаре, приводя тип к исходному.
"""Set the value at a dot-separated path, creating missing intermediate maps.
If the leaf key already exists its type is preserved via _coerce; otherwise the
type is inferred from the string with _infer. This makes the wizard tolerant of
configs that do not yet contain every field (e.g. an older cobot-setting.yaml).
Устанавливает значение по пути с точками, создавая отсутствующие промежуточные
словари. Если конечный ключ уже есть — тип сохраняется через _coerce; иначе тип
выводится из строки через _infer. Это делает мастер устойчивым к конфигам, где
ещё нет всех полей (например, более старый cobot-setting.yaml).
"""
keys = path.split(".")
cur = mapping
for k in keys[:-1]:
if k not in cur or cur[k] is None:
cur[k] = {}
cur = cur[k]
original = cur[keys[-1]]
cur[keys[-1]] = _coerce(value, original)
leaf = keys[-1]
if leaf in cur and cur[leaf] is not None:
cur[leaf] = _coerce(value, cur[leaf])
else:
cur[leaf] = _infer(value)
# Shown after all blocks have been configured to confirm the file was saved.
# Показывается после настройки всех блоков для подтверждения сохранения файла.
class _SavedScreen(Screen[None]):
"""Confirmation screen shown after all configuration blocks are saved. Press Enter to close.
Экран подтверждения, показываемый после сохранения всех блоков конфигурации. Enter для закрытия.
"""
BINDINGS = [Binding("enter,escape", "close", "Close")]
def compose(self) -> ComposeResult:
yield Static("Done", id="step")
yield Static(f"Configuration saved to {_CONFIG_PATH.name}", id="question")
yield Static("Press Enter to close.", id="note")
yield Footer()
def action_close(self) -> None:
self.dismiss(None)
# The main configuration wizard. Goes through each block in order.
# For each block it first asks "Configure X?" then steps through all its fields.
# Главный мастер конфигурации. Проходит по каждому блоку по порядку.
# Для каждого блока сначала спрашивает "Настроить X?" а затем проходит по всем его полям.
class _Wizard(App[None]):
"""Configuration wizard that iterates over all _BLOCKS. For each block it asks
"Configure X?" and if confirmed steps through every field with PickScreen or InputScreen.
Saves to cobot-setting.yaml when all blocks are done and shows _SavedScreen.
Мастер конфигурации, проходящий по всем _BLOCKS. Для каждого блока спрашивает
"Настроить X?" и при подтверждении проходит по всем полям через PickScreen или InputScreen.
Сохраняет в cobot-setting.yaml по завершении и показывает _SavedScreen.
"""
CSS = SCREEN_CSS
def __init__(self, data: Any):
super().__init__()
self._data = data
self._blocks = list(_BLOCKS)
self._block_idx = 0
self._field_idx = 0
self._current_block: Optional[_Block] = None
self._pending_fields: List[_Field] = []
def on_mount(self) -> None:
self._next_block()
def _next_block(self) -> None:
if self._block_idx >= len(self._blocks):
# All blocks done - save and show the confirmation screen.
# Все блоки пройдены - сохраняем и показываем экран подтверждения.
_save_config(self._data)
self.push_screen(_SavedScreen(), lambda _: self.exit())
return
block = self._blocks[self._block_idx]
total = len(self._blocks)
step = f"Block {self._block_idx + 1} of {total}"
self.push_screen(
PickScreen(
step,
f"Configure {block.title}?",
["Yes", "No"],
"Yes",
),
lambda v: self._got_block_choice(v, block),
)
def _got_block_choice(self, v: Optional[str], block: _Block) -> None:
if v is None:
self.exit()
return
self._block_idx += 1
if v == "Yes":
self._current_block = block
self._pending_fields = list(block.fields)
self._field_idx = 0
self._next_field()
else:
# Skip all fields in this block and jump to the next block.
# Пропускаем все поля этого блока и переходим к следующему.
self._next_block()
def _next_field(self) -> None:
if not self._pending_fields:
self._next_block()
return
f = self._pending_fields[0]
block = self._current_block
total_blocks = len(self._blocks)
block_num = self._block_idx # already incremented
self._field_idx += 1
field_num = self._field_idx
total_fields = len(block.fields)
step = f"Block {block_num} of {total_blocks} - Field {field_num} of {total_fields}"
# Resolve current value from loaded YAML as the pre-filled default
yaml_val = _get_nested(self._data[block.yaml_key], f.key)
current = str(yaml_val) if yaml_val is not None else f.default
if f.options:
# Make the current value the default selection
default_opt = current if current in f.options else f.options[0]
screen = PickScreen(step, f.question, f.options, default_opt, note=f.note)
else:
screen = InputScreen(step, f.question, current, note=f.note)
self.push_screen(screen, lambda v, _f=f: self._got_field(v, _f))
def _got_field(self, v: Optional[str], f: _Field) -> None:
if v is None:
self.exit()
return
block = self._current_block
_set_nested(self._data[block.yaml_key], f.key, v)
# Remove the field we just handled and move on to the next one.
# Удаляем только что обработанное поле и переходим к следующему.
self._pending_fields.pop(0)
self._next_field()
# Load the config file preserving all comments and key order.
# Загружаем конфиг-файл, сохраняя все комментарии и порядок ключей.
def _load_config() -> Any:
"""Load cobot-setting.yaml with ruamel.yaml, preserving comments and key order.
Загружает cobot-setting.yaml с помощью ruamel.yaml, сохраняя комментарии и порядок ключей.
"""Load cobot-setting.yaml preserving comments and key order.
Загружает cobot-setting.yaml, сохраняя комментарии и порядок ключей.
"""
with open(_CONFIG_PATH, "r", encoding="utf-8") as fh:
return _yaml.load(fh)
# Write the modified config back to disk preserving comments and formatting.
# Записываем изменённый конфиг обратно на диск, сохраняя комментарии и форматирование.
def _save_config(data: Any) -> None:
"""Write the modified YAML data back to cobot-setting.yaml, preserving comments.
Записывает изменённые данные YAML обратно в cobot-setting.yaml, сохраняя комментарии.
"""Write the modified YAML back to cobot-setting.yaml, preserving comments.
Записывает изменённый YAML обратно в cobot-setting.yaml, сохраняя комментарии.
"""
with open(_CONFIG_PATH, "w", encoding="utf-8") as fh:
_yaml.dump(data, fh)
def _run_wizard(data: Any, blocks: List[_Block]) -> bool:
"""Walk every block: ask "Configure X?", and if yes step through its fields.
Returns True if the user completed the wizard (config was saved), False on cancel.
Проходит по каждому блоку: спрашивает "Настроить X?", и если да — проходит по полям.
Возвращает True если мастер завершён (конфиг сохранён), False при отмене.
"""
total = len(blocks)
for bi, block in enumerate(blocks):
ui.header(f"Блок {bi + 1}/{total}", block.title)
if not ui.confirm(f"Настроить {block.title}?", default=True):
continue
for fi, f in enumerate(block.fields):
yaml_val = _get_nested(data[block.yaml_key], f.key)
current = str(yaml_val) if yaml_val is not None else str(f.default)
step_note = f"Поле {fi + 1}/{len(block.fields)}"
note = f"{step_note}\n{f.note}" if f.note else step_note
if f.options:
default_opt = current if current in f.options else f.options[0]
value = ui.select(f.question, f.options, default_opt, note=note)
else:
value = ui.text(f.question, current, note=note)
if value is None:
ui.info("[yellow]Отменено.[/yellow]")
return False
_set_nested(data[block.yaml_key], f.key, value)
_save_config(data)
ui.done(True, f"Конфигурация сохранена в {_CONFIG_PATH.name}")
return True
def register(subparsers: argparse._SubParsersAction) -> None:
p = subparsers.add_parser("robot-setup", help="Configure cobot-setting.yaml interactively")
p.set_defaults(func=run)
def run(args: argparse.Namespace) -> None:
ui.header("Настройка робота", "cobot-setting.yaml")
if not _CONFIG_PATH.exists():
from rich.console import Console
Console().print(f"[red]Config not found:[/red] {_CONFIG_PATH}")
ui.error(f"Конфиг не найден: {_CONFIG_PATH}")
sys.exit(1)
data = _load_config()
_Wizard(data).run()
# Убеждаемся что секция tool существует в данных (для старых конфигов)
if "tool" not in data:
data["tool"] = {"active": "patron"}
tool_block = _build_tool_block()
blocks = ([tool_block] if tool_block else []) + list(_BLOCKS)
if not _run_wizard(data, blocks):
return
# Применяем выбранный инструмент: перезаписываем tool_active.xacro и iiwa7.srdf
active_tool = str(data["tool"].get("active", "patron"))
if not _TOOLS_YAML.exists():
ui.info(f"[yellow]tools.yaml не найден ({_TOOLS_YAML}) — пропуск применения инструмента[/yellow]")
return
try:
registry = _load_tools_registry()
if active_tool not in registry:
raise ValueError(f"Неизвестный инструмент '{active_tool}'. Доступны: {', '.join(registry)}")
tool_cfg = dict(registry[active_tool])
sys.path.insert(0, str(_PROJECT_DIR / "src" / "iiwa_utils"))
from iiwa_utils.tool_manager import apply_tool
apply_tool(tool_cfg=tool_cfg, xacro_out_path=_TOOL_ACTIVE_XACRO, srdf_path=_SRDF_PATH)
# Синхронизируем planning.pose_link с tcp_link выбранного инструмента
tcp_link = tool_cfg.get("tcp_link", "link_ee")
if "planning" in data:
data["planning"]["pose_link"] = tcp_link
_save_config(data)
ui.info(
f"[green]✓[/green] Инструмент [bold]{active_tool}[/bold] применён: "
f"tool_active.xacro, iiwa7.srdf обновлены, "
f"planning.pose_link → [bold]{tcp_link}[/bold]."
)
except Exception as exc:
ui.error(f"Не удалось применить инструмент: {exc}")
cobot/commands/run.py
+85 -319
View File
@@ -2,17 +2,21 @@ from __future__ import annotations
import argparse
import os
import signal
import shutil
import socket
import subprocess
from pathlib import Path
from typing import Callable, List, Optional
from typing import List, Optional
from textual.app import App
from cobot.tui import SCREEN_CSS, LogScreen, PickScreen, RunScreen
from cobot.commands.local_setup import webots_installed, WebotsInstallApp, _WEBOTS_VERSION
from cobot import process, ui
from cobot import privilege
from cobot.ui import done, header
from cobot.commands.local_setup import (
_WEBOTS_VERSION,
build_workspace,
install_webots,
webots_installed,
)
_PROJECT_DIR = Path(__file__).parent.parent.parent
_CONFIG_PATH = _PROJECT_DIR / "cobot-setting.yaml"
@@ -32,15 +36,11 @@ _CONTAINER_CONTROLLER = "lwc-controller"
_CONTAINER_WEBOTS = "lwc-webots"
# Named Docker volume that stores the Webots asset cache between container runs.
# Without it Webots re-downloads all 3D assets from the internet on every launch.
# Именованный Docker volume для хранения кэша ассетов Webots между запусками контейнера.
# Без него Webots заново скачивает все 3D-ассеты из интернета при каждом запуске.
_WEBOTS_CACHE_VOLUME = "lwc-webots-cache"
# Candidates checked in order - for the controller the webots image is a valid fallback
# because it already contains all controller packages too.
# Кандидаты проверяются по порядку - для контроллера образ webots является допустимым запасным,
# так как он уже содержит все пакеты контроллера.
# Candidates checked in order - for the controller the webots image is a valid fallback.
# Кандидаты проверяются по порядку - для контроллера образ webots является допустимым запасным.
_CONTROLLER_IMAGES = [
"lwc-local:ros-iiwa7-jazzy",
"evilfisru/lwc:iiwa-jazzy",
@@ -56,56 +56,16 @@ _WEBOTS_IMAGES = [
]
# A minimal app that asks one question and exits immediately with the chosen value.
# We need a full App because Textual screens cannot run outside one.
# Минимальное приложение, которое задаёт один вопрос и сразу выходит с выбранным значением.
# Нам нужен полноценный App, потому что экраны Textual не могут работать вне него.
class _Ask(App[Optional[str]]):
"""Minimal one-question Textual app. Pushes a PickScreen and exits with the chosen value.
Минимальное однвопросное Textual-приложение. Открывает PickScreen и завершается с выбранным значением.
"""
CSS = SCREEN_CSS
def __init__(self, step: str, question: str, options: List[str], default: str):
super().__init__()
self._step = step
self._question = question
self._options = options
self._default = default
def on_mount(self) -> None:
self.push_screen(
PickScreen(self._step, self._question, self._options, self._default),
self.exit,
)
def _ask(step: str, question: str, options: List[str], default: str) -> Optional[str]:
"""Show a single-choice PickScreen and return the selected value, or None on Escape.
Показывает PickScreen с одним выбором и возвращает выбранное значение или None при Escape.
"""
# Returns None when the user pressed Escape to cancel.
# Возвращает None когда пользователь нажал Escape для отмены.
return _Ask(step, question, options, default).run()
def _detect_webots_home() -> str:
"""Return the WEBOTS_HOME path for the locally installed Linux Webots.
Validates that the candidate directory contains the Linux 'webots' binary
Возвращает путь WEBOTS_HOME для локально установленного Linux Webots.
"""Return the WEBOTS_HOME path for the locally installed Linux Webots, or "".
Возвращает путь WEBOTS_HOME для локально установленного Linux Webots, или "".
"""
def _is_linux_webots(home: str) -> bool:
# Accept only directories that have the Linux 'webots' binary directly inside.
# Принимаем только директории с Linux-бинарником 'webots' напрямую внутри.
return (Path(home) / "webots").is_file()
if "WEBOTS_HOME" in os.environ:
home = os.environ["WEBOTS_HOME"]
if _is_linux_webots(home):
return home
return ""
return home if _is_linux_webots(home) else ""
if _WEBOTS_DEFAULT_HOME.is_dir() and _is_linux_webots(str(_WEBOTS_DEFAULT_HOME)):
return str(_WEBOTS_DEFAULT_HOME)
webots_bin = shutil.which("webots")
@@ -116,10 +76,8 @@ def _detect_webots_home() -> str:
return ""
# Detect the GPU type so we can pass the right flags to docker run for Webots rendering.
# Определяем тип GPU, чтобы передать нужные флаги в docker run для рендеринга Webots.
def _detect_gpu() -> str:
"""Return "nvidia", "mesa", or "software" based on what GPU drivers are available.
"""Return "nvidia", "mesa", or "software" based on available GPU drivers.
Возвращает "nvidia", "mesa" или "software" в зависимости от доступных драйверов GPU.
"""
if shutil.which("nvidia-smi"):
@@ -130,11 +88,9 @@ def _detect_gpu() -> str:
return "software"
# List all Docker images currently available on this machine.
# Получаем список всех Docker-образов доступных на этой машине.
def _docker_images() -> set:
"""Return the set of "repository:tag" strings for all locally available Docker images.
Возвращает множество строк "репозиторий:тег" для всех локально доступных Docker-образов.
"""Return the set of "repository:tag" strings for all local Docker images.
Возвращает множество строк "репозиторий:тег" для всех локальных Docker-образов.
"""
r = subprocess.run(
["docker", "images", "--format", "{{.Repository}}:{{.Tag}}"],
@@ -143,11 +99,9 @@ def _docker_images() -> set:
return set(r.stdout.strip().splitlines())
# Return the first image from the candidates list that is already present locally.
# Возвращаем первый образ из списка кандидатов, который уже присутствует локально.
def _find_image(candidates: List[str]) -> Optional[str]:
"""Return the first candidate image that exists locally, or None if none are available.
Возвращает первый образ-кандидат, присутствующий локально, или None если ни один не найден.
"""Return the first candidate image that exists locally, or None.
Возвращает первый образ-кандидат, присутствующий локально, или None.
"""
available = _docker_images()
for img in candidates:
@@ -156,82 +110,13 @@ def _find_image(candidates: List[str]) -> Optional[str]:
return None
# Build the ROS2 project locally with colcon. Used when launching in local mode
# and the install/ directory does not exist yet.
# Собираем ROS2-проект локально с помощью colcon. Используется при запуске в локальном режиме,
# если директория install/ ещё не существует.
def _task_build(screen: LogScreen) -> None:
"""Worker function that runs inside LogScreen. Counts packages, then runs colcon build
with release mixin and updates progress as each package finishes.
Рабочая функция внутри LogScreen. Подсчитывает пакеты, запускает colcon build с mixin release
и обновляет прогресс по мере завершения каждого пакета.
"""
try:
screen.write("[bold]Building project with colcon[/bold]\n")
# Count packages first so we can show X/total progress.
# Сначала считаем пакеты, чтобы показывать X/всего в прогрессе.
list_proc = subprocess.run(
["bash", "-c", f"source {_JAZZY_DIR}/setup.bash && colcon list"],
capture_output=True, text=True, cwd=_PROJECT_DIR,
)
total = max(len([l for l in list_proc.stdout.splitlines() if l.strip()]), 1)
screen.set_progress(0, f"0 / {total} packages done")
built = 0
proc = subprocess.Popen(
["bash", "-c",
f"source {_JAZZY_DIR}/setup.bash && colcon build --mixin release"],
stdout=subprocess.PIPE, stderr=subprocess.STDOUT,
text=True, cwd=_PROJECT_DIR,
)
for line in proc.stdout:
s = line.rstrip()
if s:
screen.write(s)
# colcon prints "Finished <<<" or "Failed <<<" when each package is done.
# colcon печатает "Finished <<<" или "Failed <<<" когда каждый пакет готов.
if "Finished <<<" in line or "Failed <<<" in line:
built += 1
screen.set_progress(built / total * 100, f"{built} / {total} packages done")
proc.wait()
if proc.returncode != 0:
screen.write("\n[red]Build failed.[/red]")
screen.finish(False)
return
screen.set_progress(100, "Build complete")
screen.write("\n[green]Build successful.[/green]")
screen.finish(True)
except Exception as exc:
screen.write(f"\n[red]Error:[/red] {exc}")
screen.finish(False)
class _BuildApp(App[bool]):
"""Minimal app that opens a LogScreen running _task_build and exits with the build result.
Минимальное приложение, открывающее LogScreen с _task_build и завершающееся с результатом сборки.
"""
CSS = SCREEN_CSS
def on_mount(self) -> None:
self.push_screen(
LogScreen("Building project before launch", _task_build, show_progress=True),
self.exit,
)
# Start the ROS2 launch file directly on this machine without Docker.
# Uses start_new_session so we can kill the whole process group with one signal.
# Запускаем launch-файл ROS2 напрямую на этой машине без Docker.
# Используем start_new_session, чтобы можно было убить всю группу процессов одним сигналом.
def _task_run_local(screen: RunScreen, mode: str) -> None:
"""Worker function that runs inside RunScreen. Launches iiwa.launch.py locally by sourcing
ROS2 and install/setup.bash, then streams output until the process exits or is stopped.
Рабочая функция внутри RunScreen. Запускает iiwa.launch.py локально через source ROS2 и
install/setup.bash, затем транслирует вывод до завершения процесса или его остановки.
# Local (non-Docker) launch
# Локальный (не Docker) запуск
def _run_local(mode: str) -> None:
"""Launch iiwa.launch.py natively. The whole ros2 launch tree runs in its own
session so a single Ctrl-C tears down every node cleanly.
Запускает iiwa.launch.py нативно. Всё дерево ros2 launch работает в своей сессии,
поэтому один Ctrl-C аккуратно завершает каждый узел.
"""
config = str(_CONFIG_PATH)
ros_cmd = f"ros2 launch iiwa_bringup iiwa.launch.py setting:={config}"
@@ -248,51 +133,28 @@ def _task_run_local(screen: RunScreen, mode: str) -> None:
f"{ros_cmd}"
)
label = "Webots simulator" if mode == "webots" else "Controller"
screen.write(f"[bold]Launching {label} (local)[/bold]")
screen.write(f"[dim]{ros_cmd}[/dim]")
label = "симулятор Webots" if mode == "webots" else "контроллер"
header(f"Запуск: {label} (локально)")
ui.note(ros_cmd)
if webots_home:
screen.write(f"[dim]WEBOTS_HOME: {webots_home}[/dim]")
screen.write("")
ui.note(f"WEBOTS_HOME: {webots_home}")
ui.note("Нажмите Ctrl-C чтобы остановить")
proc = subprocess.Popen(
rc = process.stream(
["bash", "-c", full_cmd],
stdout=subprocess.PIPE, stderr=subprocess.STDOUT,
text=True, cwd=_PROJECT_DIR,
start_new_session=True,
cwd=str(_PROJECT_DIR),
new_session=True,
)
screen.set_proc(proc)
# Kill the entire session so all ROS2 nodes are terminated together.
# ros2 launch puts each node in its own process group (setpgrp), so killpg on the
# bash pgid only reaches bash/launch itself. All nodes share the session started
# with start_new_session=True, so pkill -s reaches every one of them.
# Убиваем всю сессию, чтобы все ROS2-узлы завершились вместе.
# ros2 launch помещает каждый узел в отдельную группу процессов (setpgrp), поэтому
# killpg по pgid bash достигает только bash/launch. Все узлы разделяют сессию,
# созданную через start_new_session=True, поэтому pkill -s достигает каждого из них.
sid = os.getsid(proc.pid)
screen.set_kill_fn(lambda: subprocess.run(
["pkill", "-TERM", "-s", str(sid)], capture_output=True
))
for line in proc.stdout:
s = line.rstrip()
if s:
screen.write(s)
proc.wait()
screen.finish(stopped=screen._stopped)
done(rc in (0, -2, -15, 130), "Остановлено")
# Start the ROS2 launch file inside a Docker container.
# For Webots mode we also forward X11 and GPU access so the simulator window can appear on screen.
# Запускаем launch-файл ROS2 внутри Docker-контейнера.
# Для режима Webots также пробрасываем X11 и доступ к GPU, чтобы окно симулятора появилось на экране.
def _task_run_docker(screen: RunScreen, image: str, mode: str, gpu: str) -> None:
"""Worker function that runs inside RunScreen. Builds the docker run command with the
appropriate GPU/X11 flags for Webots, then streams container output until stopped or exited.
Рабочая функция внутри RunScreen. Формирует команду docker run с нужными флагами GPU/X11
для Webots, затем транслирует вывод контейнера до остановки или завершения.
# Docker launch
# Запуск в Docker
def _run_docker(image: str, mode: str, gpu: str) -> None:
"""Launch iiwa.launch.py inside a Docker container, forwarding X11/GPU for Webots.
The container is stopped with ``docker kill`` on Ctrl-C.
Запускает iiwa.launch.py внутри Docker-контейнера, пробрасывая X11/GPU для Webots.
Контейнер останавливается через ``docker kill`` по Ctrl-C.
"""
container = _CONTAINER_WEBOTS if mode == "webots" else _CONTAINER_CONTROLLER
@@ -303,8 +165,8 @@ def _task_run_docker(screen: RunScreen, image: str, mode: str, gpu: str) -> None
if mode == "webots":
ros_cmd += " simulate:=1"
# Remove any stale container with the same name left from a previous run.
# Удаляем устаревший контейнер с таким же именем, оставшийся от предыдущего запуска.
# Remove any stale container with the same name from a previous run.
# Удаляем устаревший контейнер с таким же именем от предыдущего запуска.
subprocess.run(["docker", "rm", "-f", container], capture_output=True)
cmd = [
@@ -316,15 +178,11 @@ def _task_run_docker(screen: RunScreen, image: str, mode: str, gpu: str) -> None
]
if mode == "webots":
# Allow the container to open windows on the host display.
# Разрешаем контейнеру открывать окна на дисплее хоста.
subprocess.run(["xhost", "+local:docker"], capture_output=True)
cmd += [
"-e", f"DISPLAY={os.environ.get('DISPLAY', ':0')}",
"-e", "QT_X11_NO_MITSHM=1",
"-v", "/tmp/.X11-unix:/tmp/.X11-unix:rw",
# Persist the Webots asset cache so it is not re-downloaded on every launch.
# Сохраняем кэш ассетов Webots, чтобы он не скачивался заново при каждом запуске.
"-v", f"{_WEBOTS_CACHE_VOLUME}:/root/.cache/Cyberbotics/Webots",
]
if gpu == "nvidia":
@@ -334,19 +192,13 @@ def _task_run_docker(screen: RunScreen, image: str, mode: str, gpu: str) -> None
"-e", "NVIDIA_DRIVER_CAPABILITIES=graphics,utility,compute",
]
elif gpu == "mesa":
# Pass through the DRI device for Intel/AMD hardware acceleration.
# Пробрасываем DRI-устройство для аппаратного ускорения Intel/AMD.
cmd += ["--device", "/dev/dri"]
else:
# No GPU found - fall back to software rendering via llvmpipe.
# GPU не найден - используем программный рендеринг через llvmpipe.
cmd += [
"-e", "LIBGL_ALWAYS_SOFTWARE=1",
"-e", "GALLIUM_DRIVER=llvmpipe",
]
# Mount the config file so the container uses our local cobot-setting.yaml.
# Монтируем конфиг-файл, чтобы контейнер использовал наш локальный cobot-setting.yaml.
if _CONFIG_PATH.exists():
cmd += ["-v", f"{_CONFIG_PATH}:{_CONFIG_IN_CONTAINER}:ro"]
@@ -355,170 +207,87 @@ def _task_run_docker(screen: RunScreen, image: str, mode: str, gpu: str) -> None
_GPU_LABELS = {
"nvidia": "NVIDIA GPU",
"mesa": "Intel/AMD DRI (Mesa)",
"software": "Software rendering (llvmpipe)",
"software": "Программный рендеринг (llvmpipe)",
}
label = "Webots simulator" if mode == "webots" else "Controller"
screen.write(f"[bold]Launching {label} in Docker[/bold]")
screen.write(f"[dim]Image: {image}[/dim]")
label = "симулятор Webots" if mode == "webots" else "контроллер"
header(f"Запуск: {label} в Docker")
ui.note(f"Образ: {image}")
if mode == "webots":
screen.write(f"[dim]GPU: {_GPU_LABELS.get(gpu, gpu)}[/dim]")
screen.write("")
ui.note(f"GPU: {_GPU_LABELS.get(gpu, gpu)}")
ui.note("Нажмите Ctrl-C чтобы остановить")
proc = subprocess.Popen(
rc = process.stream(
cmd,
stdout=subprocess.PIPE, stderr=subprocess.STDOUT,
text=True,
kill_fn=lambda: subprocess.run(["docker", "kill", container], capture_output=True),
)
screen.set_proc(proc)
# Use docker kill instead of proc.terminate() so the container is stopped immediately.
# Terminating only the docker CLI process leaves the container itself running.
# Используем docker kill вместо proc.terminate(), чтобы контейнер остановился немедленно.
# Завершение только процесса docker CLI оставляет сам контейнер работающим.
screen.set_kill_fn(lambda: subprocess.run(["docker", "kill", container], capture_output=True))
for line in proc.stdout:
s = line.rstrip()
if s:
screen.write(s)
proc.wait()
screen.finish(stopped=screen._stopped)
done(rc in (0, -2, -15, 130), "Остановлено")
# Wraps a RunScreen in an App so it can be launched with .run().
# Оборачивает RunScreen в App, чтобы его можно было запустить через .run().
class _RunApp(App[None]):
"""Minimal app that wraps a RunScreen so it can be started with .run().
Минимальное приложение, оборачивающее RunScreen чтобы его можно было запустить через .run().
"""
CSS = SCREEN_CSS
def __init__(self, title: str, task: Callable):
super().__init__()
self._title = title
self._run_fn = task
def on_mount(self) -> None:
self.push_screen(RunScreen(self._title, self._run_fn), lambda _: self.exit())
# Guide the user through launching locally - asks what to run, checks prerequisites,
# installs Webots and builds the project if needed, then launches.
# Ведёт пользователя через локальный запуск - спрашивает что запустить, проверяет
# предварительные условия, устанавливает Webots и собирает проект при необходимости, затем запускает.
def _local_flow(args: argparse.Namespace) -> None:
"""Interactive flow for local (non-Docker) launch. Checks Webots, ROS2, and build state,
offers to install/build missing pieces, then starts RunScreen.
Интерактивный сценарий для локального (не Docker) запуска. Проверяет Webots, ROS2 и состояние
сборки, предлагает установить/собрать недостающее, затем запускает RunScreen.
"""Interactive flow for local launch: check Webots/ROS2/build, then run.
Интерактивный сценарий локального запуска: проверка Webots/ROS2/сборки, затем запуск.
"""
mode_v = _ask(
"Run local",
"What do you want to launch?",
["Controller", "Webots simulator"],
"Controller",
mode_v = ui.select(
"Что запустить?",
["Контроллер", "Симулятор Webots"],
"Контроллер",
)
if mode_v is None:
return
mode = "webots" if mode_v == "Webots simulator" else "controller"
mode = "webots" if mode_v == "Симулятор Webots" else "controller"
# Check Webots installed (local mode only)
if mode == "webots" and not webots_installed():
v = _ask(
"Webots not found",
f"Webots {_WEBOTS_VERSION} is not installed. Install it now?",
[f"Yes, install Webots {_WEBOTS_VERSION}", "No, cancel"],
f"Yes, install Webots {_WEBOTS_VERSION}",
)
if v is None or v.startswith("No"):
if not ui.confirm(f"Webots {_WEBOTS_VERSION} не установлен. Установить сейчас?",
default=True):
return
ok = WebotsInstallApp().run()
if not ok:
if not privilege.ensure_sudo() or not install_webots():
return
# Check ROS2 Jazzy
if not _JAZZY_DIR.is_dir():
v = _ask(
"ROS2 not found",
"ROS2 Jazzy is not installed. Run local-setup now?",
["Yes, run local-setup", "No, cancel"],
"Yes, run local-setup",
)
if v and v.startswith("Yes"):
if ui.confirm("ROS2 Jazzy не установлен. Запустить local-setup?", default=True):
from cobot.commands.local_setup import run as _local_setup
_local_setup(args)
return
# Check project built
if not (_INSTALL_DIR / "setup.bash").exists():
v = _ask(
"Project not built",
"The project has not been built yet. Build it now?",
["Yes, build now", "No, cancel"],
"Yes, build now",
)
if v is None or v.startswith("No"):
if not ui.confirm("Проект ещё не собран. Собрать сейчас?", default=True):
return
ok = _BuildApp().run()
if not ok:
if not privilege.ensure_sudo() or not build_workspace():
return
label = "Webots simulator" if mode == "webots" else "Controller"
_RunApp(f"Running {label} — local", lambda s: _task_run_local(s, mode)).run()
_run_local(mode)
# Guide the user through launching in Docker - asks what to run, finds a suitable image,
# detects the GPU for Webots, and launches.
# Ведёт пользователя через запуск в Docker - спрашивает что запустить, ищет подходящий образ,
# определяет GPU для Webots и запускает.
def _docker_flow(args: argparse.Namespace) -> None:
"""Interactive flow for Docker launch. Finds the best available image, detects the GPU
for Webots mode, then starts RunScreen with the docker run task.
Интерактивный сценарий для запуска в Docker. Находит лучший доступный образ, определяет GPU
для режима Webots, затем запускает RunScreen с задачей docker run.
"""Interactive flow for Docker launch: pick an image, detect GPU, then run.
Интерактивный сценарий запуска в Docker: выбор образа, определение GPU, затем запуск.
"""
if not shutil.which("docker"):
from rich.console import Console
Console().print("[red]Error:[/red] Docker is not installed or not on PATH.")
ui.error("Docker не установлен или отсутствует в PATH.")
return
mode_v = _ask(
"Run in Docker",
"What do you want to launch?",
["Controller", "Webots simulator"],
"Controller",
mode_v = ui.select(
"Что запустить?",
["Контроллер", "Симулятор Webots"],
"Контроллер",
)
if mode_v is None:
return
mode = "webots" if mode_v == "Webots simulator" else "controller"
mode = "webots" if mode_v == "Симулятор Webots" else "controller"
candidates = _WEBOTS_IMAGES if mode == "webots" else _CONTROLLER_IMAGES
image = _find_image(candidates)
if image is None:
# No image available - offer to run docker-setup to get one.
# Образ не найден - предлагаем запустить docker-setup чтобы его получить.
what = "Webots" if mode == "webots" else "controller or Webots"
v = _ask(
"No image found",
f"No Docker image found for {what}. Run docker-setup now?",
["Yes, run docker-setup", "No, cancel"],
"Yes, run docker-setup",
)
if v and v.startswith("Yes"):
what = "Webots" if mode == "webots" else "контроллера или Webots"
if ui.confirm(f"Docker-образ для {what} не найден. Запустить docker-setup?",
default=True):
from cobot.commands.docker_setup import run as _docker_setup
_docker_setup(args)
return
# Only detect GPU for Webots - the controller does not need a display.
# GPU определяем только для Webots - контроллеру дисплей не нужен.
gpu = _detect_gpu() if mode == "webots" else "software"
label = "Webots simulator" if mode == "webots" else "Controller"
_RunApp(
f"Running {label} — Docker",
lambda s: _task_run_docker(s, image, mode, gpu),
).run()
_run_docker(image, mode, gpu)
def register(subparsers: argparse._SubParsersAction) -> None:
@@ -544,17 +313,14 @@ def run(args: argparse.Namespace) -> None:
elif mode == "docker":
_docker_flow(args)
else:
# No mode given - ask the user how they want to run.
# Режим не указан - спрашиваем пользователя как он хочет запустить.
v = _ask(
"Run",
"How do you want to run the project?",
["Local (native ROS2)", "Docker"],
"Local (native ROS2)",
v = ui.select(
"Как запустить проект?",
["Локально (нативный ROS2)", "Docker"],
"Локально (нативный ROS2)",
)
if v is None:
return
if v.startswith("Local"):
if v.startswith("Локально"):
_local_flow(args)
else:
_docker_flow(args)
cobot/commands/setup.py
+14 -59
View File
@@ -1,7 +1,6 @@
import argparse
from typing import List, Optional
from textual.app import App
from cobot import ui
# Import each sub-command's run() so we can call them in sequence.
# Импортируем run() каждой подкоманды, чтобы вызывать их по порядку.
@@ -9,40 +8,6 @@ from cobot.commands.doc_setup import run as _doc_setup
from cobot.commands.docker_setup import run as _docker_setup
from cobot.commands.local_setup import run as _local_setup
from cobot.commands.robot_setup import run as _robot_setup
from cobot.tui import SCREEN_CSS, PickScreen
# A minimal Textual app that asks a single question and exits with the chosen value.
# We need this because Textual screens cannot run outside of an App context.
# Минимальное Textual-приложение, которое задаёт один вопрос и выходит с выбранным значением.
# Нам это нужно, потому что экраны Textual не могут работать вне контекста приложения.
class _Ask(App[Optional[str]]):
"""Minimal one-question Textual app. Pushes a PickScreen and exits with the chosen value.
Минимальное однвопросное Textual-приложение. Открывает PickScreen и завершается с выбранным значением.
"""
CSS = SCREEN_CSS
def __init__(self, step: str, question: str, options: List[str], default: str):
super().__init__()
self._step = step
self._question = question
self._options = options
self._default = default
def on_mount(self) -> None:
self.push_screen(
PickScreen(self._step, self._question, self._options, self._default),
self.exit,
)
def _ask(step: str, question: str, options: List[str], default: str) -> Optional[str]:
"""Show a PickScreen and return the selected value, or None if the user pressed Escape.
Показывает PickScreen и возвращает выбранное значение или None если пользователь нажал Escape.
"""
# Returns None if the user pressed Escape to cancel the whole wizard.
# Возвращает None если пользователь нажал Escape для отмены всего мастера.
return _Ask(step, question, options, default).run()
def register(subparsers):
@@ -54,44 +19,34 @@ def register(subparsers):
def run(args: argparse.Namespace) -> None:
"""Run the three-step first-time setup wizard: doc server -> build env -> robot config.
Запускает трёхшаговый мастер первоначальной настройки: сервер документации -> среда сборки -> конфиг.
"""Run the three-step first-time setup wizard: docs -> build env -> robot config.
Запускает трёхшаговый мастер первичной настройки: документация -> среда сборки -> конфиг.
"""
ui.header("Первичная настройка", "3 шага")
# Step 1 - documentation server.
# Шаг 1 - сервер документации.
v = _ask("Step 1 of 3", "Set up the documentation server?", ["Yes", "No"], "Yes")
if v is None:
return
if v == "Yes":
if ui.confirm("Шаг 1/3 — настроить сервер документации?", default=True):
_doc_setup(args)
# Step 2 - build environment: local ROS2 or Docker.
# Шаг 2 - среда сборки: локальный ROS2 или Docker.
v = _ask(
"Step 2 of 3",
"How do you want to set up the build environment?",
env_choice = ui.select(
"Шаг 2/3 — как настроить среду сборки?",
[
"local-setup — install ROS2 Jazzy on this machine and build with colcon",
"docker-setup — build a Docker image with ROS2 Jazzy pre-installed",
"local-setup — установить ROS2 Jazzy на эту машину и собрать через colcon",
"docker-setup — собрать Docker-образ с предустановленным ROS2 Jazzy",
],
"local-setup — install ROS2 Jazzy on this machine and build with colcon",
"local-setup — установить ROS2 Jazzy на эту машину и собрать через colcon",
)
if v is None:
if env_choice is None:
return
if v.startswith("local"):
if env_choice.startswith("local"):
_local_setup(args)
else:
_docker_setup(args)
# Step 3 - robot parameters in cobot-setting.yaml.
# Шаг 3 - параметры робота в cobot-setting.yaml.
v = _ask(
"Step 3 of 3",
"Configure robot parameters (cobot-setting.yaml)?",
["Yes", "No"],
"Yes",
)
if v is None:
return
if v == "Yes":
if ui.confirm("Шаг 3/3 — настроить параметры робота (cobot-setting.yaml)?", default=True):
_robot_setup(args)
cobot/commands/update.py
+55 -119
View File
@@ -4,134 +4,70 @@ import argparse
import subprocess
from pathlib import Path
from textual.app import App
from cobot.tui import SCREEN_CSS, LogScreen
from cobot import process, ui
from cobot.ui import done
from cobot.process import StepProgress
_PROJECT_DIR = Path(__file__).parent.parent.parent
# Pull the latest commits from the remote and reinstall the cobot CLI in one go.
# Progress bar: fetch (0-30%), pull (30-80%), reinstall (80-100%).
# Скачиваем последние коммиты с удалённого репозитория и переустанавливаем cobot CLI за один раз.
# Прогресс-бар: fetch (0-30%), pull (30-80%), переустановка (80-100%).
def _task_update(screen: LogScreen) -> None:
"""Worker function that runs inside LogScreen. Fetches the current branch, shows incoming
commits, pulls changes, then reinstalls the cobot CLI via uv tool install --editable.
Рабочая функция, выполняемая внутри LogScreen. Получает текущую ветку, показывает входящие
коммиты, вытягивает изменения, затем переустанавливает cobot CLI через uv tool install --editable.
def _git(*args: str) -> str:
"""Run a git command in the project dir and return its stripped stdout.
Запускает git-команду в директории проекта и возвращает обрезанный stdout.
"""
try:
# Find out which branch we are on so we can fetch and pull the right one.
# Определяем на какой ветке мы находимся, чтобы делать fetch и pull нужной ветки.
branch = subprocess.check_output(
["git", "rev-parse", "--abbrev-ref", "HEAD"],
cwd=_PROJECT_DIR, text=True,
).strip()
screen.write(f"[cyan][*][/cyan] Branch: [bold]{branch}[/bold]")
# Fetch (0 → 30 %)
screen.set_progress(0, "Fetching from remote...")
screen.write("[cyan][*][/cyan] Fetching from remote...")
fetch_proc = subprocess.Popen(
["git", "fetch", "origin"],
cwd=_PROJECT_DIR, stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True,
)
screen.set_proc(fetch_proc)
fetch_out, fetch_err = fetch_proc.communicate()
if screen.is_stopped():
return
if fetch_proc.returncode not in (0, -9):
screen.write(f"[red]Fetch failed:[/red] {fetch_err.strip()}")
screen.finish(False)
return
screen.set_progress(30)
# Count how many commits the remote is ahead of us.
# Считаем сколько коммитов нас опережает удалённый репозиторий.
behind = subprocess.check_output(
["git", "rev-list", f"HEAD..origin/{branch}", "--count"],
cwd=_PROJECT_DIR, text=True,
).strip()
if behind == "0":
if not screen.is_stopped():
screen.set_progress(100, "Already up to date")
screen.write("[green][ok][/green] Already up to date.")
screen.finish(True)
return
# Show which commits are coming in so the user knows what changed.
# Показываем какие коммиты приходят, чтобы пользователь знал что изменилось.
screen.write(f"\n[bold]{behind} new commit(s):[/bold]")
log_lines = subprocess.check_output(
["git", "log", f"HEAD..origin/{branch}", "--oneline"],
cwd=_PROJECT_DIR, text=True,
).strip().splitlines()
for line in log_lines:
screen.write(f" [dim]{line}[/dim]")
# Pull (30 → 80 %)
screen.set_progress(30, "Pulling changes...")
screen.write("\n[cyan][*][/cyan] Pulling changes...")
pull_proc = subprocess.Popen(
["git", "pull", "origin", branch],
stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True, cwd=_PROJECT_DIR,
)
screen.set_proc(pull_proc)
pull_out, pull_err = pull_proc.communicate()
if screen.is_stopped():
return
if pull_proc.returncode not in (0, -9):
for line in (pull_out + pull_err).splitlines():
if line.strip():
screen.write(line)
screen.write("[red]Pull failed.[/red]")
screen.finish(False)
return
screen.set_progress(80)
# Reinstall (80 → 100 %)
# Reinstall so the cobot binary picks up any new dependencies from pyproject.toml.
# Переустанавливаем, чтобы бинарник cobot подхватил новые зависимости из pyproject.toml.
screen.set_progress(80, "Reinstalling cobot CLI...")
screen.write("\n[cyan][*][/cyan] Reinstalling cobot CLI...")
reinstall_proc = subprocess.Popen(
["uv", "tool", "install", "--editable", str(_PROJECT_DIR)],
stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True,
)
screen.set_proc(reinstall_proc)
reinstall_out, reinstall_err = reinstall_proc.communicate()
if screen.is_stopped():
return
if reinstall_proc.returncode in (0, -9):
screen.write("[green][ok][/green] cobot reinstalled")
else:
screen.write(f"[yellow]Warning:[/yellow] reinstall failed — {reinstall_err.strip()}")
if not screen.is_stopped():
screen.set_progress(100, "Done")
screen.write("\n[green]Project updated successfully.[/green]")
screen.finish(True)
except Exception as exc:
if not screen.is_stopped():
screen.write(f"\n[red]Error:[/red] {exc}")
screen.finish(False)
return subprocess.check_output(["git", *args], cwd=_PROJECT_DIR, text=True).strip()
class _UpdateApp(App[None]):
"""Minimal Textual app that opens a LogScreen running _task_update and exits when it closes.
Минимальное Textual-приложение, открывающее LogScreen с _task_update и завершающееся при закрытии.
def _update() -> None:
"""Fetch the current branch, show incoming commits, pull, and reinstall the cobot CLI.
Progress: fetch (0-30 %), pull (30-80 %), reinstall (80-100 %).
Получает текущую ветку, показывает входящие коммиты, делает pull и переустанавливает CLI.
Прогресс: fetch (0-30 %), pull (30-80 %), переустановка (80-100 %).
"""
ok, fail_msg = True, ""
with StepProgress("Обновление проекта") as p:
try:
branch = _git("rev-parse", "--abbrev-ref", "HEAD")
p.raw(f"[cyan]▸[/cyan] Ветка: [bold]{branch}[/bold]")
CSS = SCREEN_CSS
p.set(0, "Получение с удалённого репозитория...")
rc = process.stream(["git", "fetch", "origin"], cwd=str(_PROJECT_DIR), on_line=p.log)
if rc not in (0, -9, -15):
done(False, "git fetch завершился с ошибкой")
return
p.set(30)
def on_mount(self) -> None:
self.push_screen(
LogScreen("Updating project", _task_update, show_progress=True),
lambda _: self.exit(),
)
behind = _git("rev-list", f"HEAD..origin/{branch}", "--count")
if behind == "0":
p.set(100, "Уже актуально")
done(True, "Уже актуальная версия")
return
p.raw(f"\n[bold]{behind} новых коммит(ов):[/bold]")
for line in _git("log", f"HEAD..origin/{branch}", "--oneline").splitlines():
p.log(line)
p.set(30, "Применение изменений...")
rc = process.stream(["git", "pull", "origin", branch],
cwd=str(_PROJECT_DIR), on_line=p.log)
if rc not in (0, -9, -15):
done(False, "git pull завершился с ошибкой")
return
p.set(80)
p.set(80, "Переустановка cobot CLI...")
p.raw("\n[cyan]▸[/cyan] Переустановка cobot CLI...")
rc = process.stream(["uv", "tool", "install", "--editable", str(_PROJECT_DIR)],
on_line=p.log)
if rc in (0, -9, -15):
p.raw("[green]✓[/green] cobot переустановлен")
else:
p.raw("[yellow]Предупреждение:[/yellow] переустановка не удалась")
p.set(100, "Готово")
except subprocess.CalledProcessError as exc:
ok, fail_msg = False, str(exc)
done(ok, "Проект обновлён" if ok else fail_msg)
def register(subparsers: argparse._SubParsersAction) -> None:
@@ -140,4 +76,4 @@ def register(subparsers: argparse._SubParsersAction) -> None:
def run(args: argparse.Namespace) -> None:
_UpdateApp().run()
_update()
cobot/privilege.py
+176
View File
@@ -0,0 +1,176 @@
from __future__ import annotations
import subprocess
import sys
import threading
from typing import List, Optional, Sequence
from cobot import ui
from cobot.ui import console
# How often the keep-alive thread refreshes the sudo timestamp (seconds).
# Sudo's default timeout is 15 min; 60 s gives a huge safety margin.
# Как часто поток keep-alive обновляет токен sudo (секунды).
# Таймаут sudo по умолчанию 15 мин; 60 с даёт большой запас.
_KEEPALIVE_INTERVAL = 60
# Module-level state: whether sudo has been primed and the keep-alive thread.
# Состояние уровня модуля: прогрет ли sudo и поток keep-alive.
_primed = False
_keepalive_thread: Optional[threading.Thread] = None
_keepalive_stop = threading.Event()
def _have_valid_timestamp() -> bool:
"""Return True if a non-interactive ``sudo -n -v`` succeeds (cached token valid).
Возвращает True, если ``sudo -n -v`` проходит без запроса (токен закеширован и валиден).
"""
return subprocess.run(
["sudo", "-n", "-v"],
stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL,
).returncode == 0
def _read_masked_password(prompt: str) -> Optional[str]:
"""Read a password character-by-character, echoing a ``★`` for each one.
Backspace deletes the last char. Enter submits. Ctrl-C / Esc cancels (None).
Falls back to getpass when stdin is not a TTY.
Читает пароль посимвольно, отображая ``★`` за каждый символ.
Backspace удаляет последний символ. Enter — подтвердить. Ctrl-C / Esc — отмена (None).
Откатывается на getpass, если stdin не является TTY.
"""
if not sys.stdin.isatty():
import getpass
try:
return getpass.getpass(prompt)
except (EOFError, KeyboardInterrupt):
return None
sys.stdout.write(prompt)
sys.stdout.flush()
chars: List[str] = []
while True:
kind, ch = ui._read_key()
if kind == "enter":
sys.stdout.write("\n")
sys.stdout.flush()
return "".join(chars)
if kind in ("esc", "interrupt"):
sys.stdout.write("\n")
sys.stdout.flush()
return None
if kind == "backspace":
if chars:
chars.pop()
# Erase one mask glyph: move back, overwrite with space, move back.
# Стираем один символ маски: назад, пробел, снова назад.
sys.stdout.write("\b \b")
sys.stdout.flush()
continue
# Space and any printable char are part of the password.
# Пробел и любой печатный символ — часть пароля.
if kind == "space":
chars.append(" ")
sys.stdout.write("★")
sys.stdout.flush()
elif kind == "char" and ch.isprintable():
chars.append(ch)
sys.stdout.write("★")
sys.stdout.flush()
def _validate_password(password: str) -> bool:
"""Feed the password to ``sudo -S -v`` to validate it and cache the timestamp.
Передаёт пароль в ``sudo -S -v`` для проверки и кеширования токена.
"""
proc = subprocess.run(
["sudo", "-S", "-v"],
input=password + "\n",
stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL,
text=True,
)
return proc.returncode == 0
def _keepalive_loop() -> None:
"""Refresh the sudo timestamp periodically until the process exits.
Периодически обновляет токен sudo, пока процесс не завершится.
"""
while not _keepalive_stop.wait(_KEEPALIVE_INTERVAL):
subprocess.run(
["sudo", "-n", "-v"],
stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL,
)
def _start_keepalive() -> None:
global _keepalive_thread
if _keepalive_thread is not None and _keepalive_thread.is_alive():
return
_keepalive_stop.clear()
_keepalive_thread = threading.Thread(target=_keepalive_loop, daemon=True)
_keepalive_thread.start()
def ensure_sudo() -> bool:
"""Make sure we hold a valid sudo timestamp, asking for the password once.
If a valid cached timestamp already exists (e.g. the user ran sudo recently),
no password is asked. Otherwise the user is prompted up to 3 times with a masked
input. On success a keep-alive thread is started. Returns True if sudo is ready.
Гарантирует наличие валидного токена sudo, спрашивая пароль один раз.
Если валидный токен уже есть (например, пользователь недавно вызывал sudo), пароль
не спрашивается. Иначе пользователю предлагается до 3 попыток с маскированным вводом.
При успехе запускается поток keep-alive. Возвращает True, если sudo готов.
"""
global _primed
if _primed and _have_valid_timestamp():
return True
if _have_valid_timestamp():
_primed = True
_start_keepalive()
return True
console.print(
"\n[bold]Для установки/удаления системных пакетов нужны права root.[/bold]"
)
console.print(
"[dim]Пароль спросим один раз и будем держать сессию sudo активной "
"до конца операции.[/dim]"
)
for attempt in range(3):
password = _read_masked_password(" [sudo] пароль: ")
if password is None:
console.print("[yellow]Отменено.[/yellow]")
return False
if _validate_password(password):
del password
_primed = True
_start_keepalive()
console.print("[green]✓ sudo активирован[/green]")
return True
remaining = 2 - attempt
if remaining > 0:
console.print(f"[red]Неверный пароль.[/red] Осталось попыток: {remaining}")
else:
console.print("[red]Неверный пароль. Превышено число попыток.[/red]")
return False
def sudo(cmd: Sequence[str]) -> List[str]:
"""Prefix a command with ``sudo -n`` (non-interactive; token already cached).
Префиксует команду ``sudo -n`` (неинтерактивно; токен уже закеширован).
"""
return ["sudo", "-n", *cmd]
def stop_keepalive() -> None:
"""Stop the keep-alive thread. Safe to call even if it was never started.
Останавливает поток keep-alive. Безопасно вызывать, даже если он не запускался.
"""
_keepalive_stop.set()
cobot/process.py
+332
View File
@@ -0,0 +1,332 @@
from __future__ import annotations
import atexit
import os
import signal
import subprocess
import threading
from typing import Callable, Dict, List, Optional, Sequence
from rich.progress import BarColumn, Progress, SpinnerColumn, TextColumn
from cobot.ui import console, done, header
# Type of an optional callback invoked for every streamed output line.
# Тип опционального колбэка, вызываемого для каждой строки потокового вывода.
LineHook = Callable[[str], None]
# Registry of all live subprocesses, so the signal handler can kill them on exit.
# Maps pid -> Popen. Guarded by a lock because procs start/finish in helper calls.
# Реестр всех живых подпроцессов, чтобы обработчик сигнала мог убить их при выходе.
# Сопоставляет pid -> Popen. Защищён блокировкой, т.к. процессы создаются/завершаются в хелперах.
_procs: Dict[int, subprocess.Popen] = {}
_procs_lock = threading.Lock()
_handlers_installed = False
def _register(proc: subprocess.Popen) -> None:
with _procs_lock:
_procs[proc.pid] = proc
def _unregister(proc: subprocess.Popen) -> None:
with _procs_lock:
_procs.pop(proc.pid, None)
def _kill_proc(proc: subprocess.Popen) -> None:
"""Terminate a process and everything it spawned.
Three strategies, in order of how the process was started:
* a custom kill_fn (e.g. ``docker kill <container>``) registered on the proc;
* a new-session process (e.g. ros2 launch) — every node shares the session, so
``pkill -s <sid>`` reaches all of them (killpg would only hit the launcher);
* otherwise the process group (SIGTERM then SIGKILL), or the bare process.
Завершает процесс и всё, что он породил. Три стратегии по способу запуска:
пользовательский kill_fn (например ``docker kill``); процесс в новой сессии
(ros2 launch — все узлы делят сессию, поэтому ``pkill -s`` достаёт каждый);
иначе группа процессов (SIGTERM→SIGKILL) или сам процесс.
"""
if proc.poll() is not None:
return
kill_fn = getattr(proc, "_cobot_kill_fn", None)
if kill_fn is not None:
try:
kill_fn()
try:
proc.wait(timeout=5)
return
except subprocess.TimeoutExpired:
pass
except Exception:
pass
if getattr(proc, "_cobot_new_session", False):
try:
sid = os.getsid(proc.pid)
subprocess.run(["pkill", "-TERM", "-s", str(sid)],
stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
try:
proc.wait(timeout=3)
return
except subprocess.TimeoutExpired:
subprocess.run(["pkill", "-KILL", "-s", str(sid)],
stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
return
except Exception:
pass
try:
pgid = os.getpgid(proc.pid)
os.killpg(pgid, signal.SIGTERM)
try:
proc.wait(timeout=3)
except subprocess.TimeoutExpired:
os.killpg(pgid, signal.SIGKILL)
except Exception:
try:
proc.terminate()
except Exception:
pass
def kill_all() -> None:
"""Kill every registered subprocess. Used by the signal handler and atexit.
Убивает каждый зарегистрированный подпроцесс. Используется обработчиком сигнала и atexit.
"""
with _procs_lock:
procs = list(_procs.values())
for proc in procs:
_kill_proc(proc)
def _on_sigint(signum, frame): # noqa: ANN001
"""SIGINT handler: stop all children, print a cancel note, and exit non-zero.
Обработчик SIGINT: останавливает всех потомков, печатает заметку об отмене и выходит с ненулём.
"""
kill_all()
console.print("\n[yellow]Прервано пользователем (Ctrl-C).[/yellow]")
raise SystemExit(130)
def install_signal_handlers() -> None:
"""Install the SIGINT handler and atexit cleanup exactly once.
Устанавливает обработчик SIGINT и очистку atexit ровно один раз.
"""
global _handlers_installed
if _handlers_installed:
return
_handlers_installed = True
signal.signal(signal.SIGINT, _on_sigint)
atexit.register(kill_all)
def spawn(
cmd: Sequence[str],
*,
env: Optional[dict] = None,
cwd: Optional[str] = None,
new_session: bool = False,
shell: bool = False,
kill_fn: Optional[Callable] = None,
) -> subprocess.Popen:
"""Start a subprocess with merged stdout/stderr as text, register it, and return it.
new_session=True puts the process in its own session/process-group so the whole
tree (e.g. all ros2 launch nodes) can be torn down with one signal. kill_fn is an
optional custom teardown (e.g. ``docker kill``) used by the cleanup logic.
Запускает подпроцесс с объединённым stdout/stderr в текстовом режиме, регистрирует
его и возвращает. new_session=True помещает процесс в собственную сессию/группу.
kill_fn — опциональная функция завершения (например ``docker kill``) для очистки.
"""
proc = subprocess.Popen(
cmd,
stdout=subprocess.PIPE,
stderr=subprocess.STDOUT,
text=True,
env=env,
cwd=cwd,
start_new_session=new_session,
shell=shell,
)
proc._cobot_new_session = new_session # type: ignore[attr-defined]
proc._cobot_kill_fn = kill_fn # type: ignore[attr-defined]
_register(proc)
return proc
def stream(
cmd: Sequence[str],
*,
env: Optional[dict] = None,
cwd: Optional[str] = None,
on_line: Optional[LineHook] = None,
new_session: bool = False,
shell: bool = False,
echo: bool = True,
kill_fn: Optional[Callable] = None,
) -> int:
"""Run a command and stream every output line to the console (and on_line hook).
Returns the process exit code. SIGKILL (-9) / SIGTERM (-15) are returned as-is so
callers can treat user cancellation differently from real failures.
Запускает команду и транслирует каждую строку вывода в консоль (и в колбэк on_line).
Возвращает код возврата процесса. SIGKILL (-9) / SIGTERM (-15) возвращаются как есть,
чтобы вызывающий код мог отличать отмену пользователем от реальных ошибок.
"""
proc = spawn(cmd, env=env, cwd=cwd, new_session=new_session, shell=shell, kill_fn=kill_fn)
try:
for line in proc.stdout:
s = line.rstrip()
if on_line is not None:
on_line(s)
elif echo and s:
console.print(f" [dim]{_escape(s)}[/dim]")
proc.wait()
finally:
_unregister(proc)
return proc.returncode
def _escape(s: str) -> str:
"""Escape Rich markup so raw command output is never interpreted as markup.
Экранирует разметку Rich, чтобы сырой вывод команды не интерпретировался как разметка.
"""
return s.replace("[", "\\[")
# A progress bar that sticks to the bottom while log lines scroll above it.
# Прогресс-бар, "прилипающий" к низу, пока строки лога прокручиваются над ним.
def make_progress() -> Progress:
"""Create a Progress with a spinner, bar, percentage, and description column.
Создаёт Progress со спиннером, баром, процентами и колонкой описания.
"""
return Progress(
SpinnerColumn(),
BarColumn(bar_width=30),
TextColumn("[progress.percentage]{task.percentage:>3.0f}%"),
TextColumn("[dim]{task.description}[/dim]"),
console=console,
transient=True,
)
def run_step(
title: str,
cmd: Sequence[str],
*,
env: Optional[dict] = None,
cwd: Optional[str] = None,
new_session: bool = False,
shell: bool = False,
show_progress: bool = True,
total: float = 100.0,
parse_progress: Optional[Callable[[str], Optional[tuple]]] = None,
on_line: Optional[LineHook] = None,
success_msg: str = "",
fail_msg: str = "",
finish: bool = True,
) -> int:
"""Run one command as a self-contained "block": header, live log + progress, status.
parse_progress(line) may return (pct, label) to advance the bar, or None to ignore.
Returns the exit code. Prints a ✓/✗ line unless finish=False (used when chaining
several commands under one header).
Запускает одну команду как самодостаточный "блок": заголовок, живой лог + прогресс,
статус. parse_progress(line) может вернуть (pct, label) для продвижения бара или None.
Возвращает код возврата. Печатает строку ✓/✗, если finish=True (иначе — при цепочке
нескольких команд под одним заголовком).
"""
if title:
header(title)
if not show_progress:
rc = stream(cmd, env=env, cwd=cwd, on_line=on_line,
new_session=new_session, shell=shell)
else:
progress = make_progress()
with progress:
task = progress.add_task("", total=total)
def _line(s: str) -> None:
if parse_progress is not None:
parsed = parse_progress(s)
if parsed is not None:
pct, label = parsed
progress.update(task, completed=pct,
description=label or "")
if on_line is not None:
on_line(s)
elif s:
progress.console.print(f" [dim]{_escape(s)}[/dim]")
rc = stream(cmd, env=env, cwd=cwd, on_line=_line,
new_session=new_session, shell=shell)
progress.update(task, completed=total)
ok = rc in (0, -9, -15)
if finish:
if ok:
done(True, success_msg or "Готово")
else:
done(False, fail_msg or f"Команда завершилась с кодом {rc}")
return rc
# A live progress context for tasks that run several commands or Python work and
# need to drive the bar manually. Yields a small controller with .log()/.set().
# Живой контекст прогресса для задач, выполняющих несколько команд или Python-работу
# и управляющих баром вручную. Отдаёт небольшой контроллер с .log()/.set().
class StepProgress:
"""Manual progress controller used as a context manager.
Usage:
with StepProgress("Building") as p:
p.set(10, "step one")
p.log("some output")
Ручной контроллер прогресса, используемый как менеджер контекста.
"""
def __init__(self, title: str, total: float = 100.0, show: bool = True):
if title:
header(title)
self._total = total
self._show = show
self._progress: Optional[Progress] = None
self._task = None
def __enter__(self) -> "StepProgress":
if self._show:
self._progress = make_progress()
self._progress.__enter__()
self._task = self._progress.add_task("", total=self._total)
return self
def set(self, pct: float, label: str = "") -> None:
if self._progress is not None:
self._progress.update(self._task, completed=pct, description=label or "")
def log(self, line: str, style: str = "dim") -> None:
out = self._progress.console if self._progress is not None else console
if line == "":
out.print()
else:
out.print(f" [{style}]{_escape(line)}[/{style}]" if style else f" {line}")
def raw(self, renderable) -> None:
"""Print a pre-built Rich renderable/markup string without escaping.
Печатает готовый Rich-объект/строку с разметкой без экранирования.
"""
out = self._progress.console if self._progress is not None else console
out.print(renderable)
def __exit__(self, exc_type, exc, tb) -> None:
if self._progress is not None:
self._progress.__exit__(exc_type, exc, tb)
self._progress = None
cobot/tui.py
-463
View File
@@ -1,463 +0,0 @@
from __future__ import annotations
import os
import signal
from typing import Callable, List, Optional
from textual import on
from textual.app import ComposeResult
from textual.binding import Binding
from textual.screen import Screen
from textual.widgets import Footer, Input, LoadingIndicator, ProgressBar, RadioButton, RadioSet, RichLog, SelectionList, Static
from textual.widgets.selection_list import Selection
# Shared CSS applied to every screen in the app.
# Общий CSS, применяемый ко всем экранам приложения.
SCREEN_CSS = """
Screen {
padding: 2 4;
}
#step {
color: $text-muted;
text-style: dim;
}
#question {
text-style: bold;
color: $accent;
margin-top: 1;
margin-bottom: 1;
}
#note {
color: $text-muted;
text-style: dim;
margin-bottom: 1;
}
RadioSet {
height: auto;
border: none;
padding: 0;
margin-bottom: 1;
}
Input {
margin-bottom: 1;
}
LogScreen #progress {
margin-top: 1;
height: 1;
}
LogScreen #step-label {
color: $text-muted;
text-style: dim;
margin-bottom: 1;
}
LogScreen #log {
height: 1fr;
border: none;
padding: 0 1;
margin-top: 1;
}
LogScreen #loading {
height: 1;
margin-top: 1;
}
LogScreen #hint {
margin-top: 1;
color: $text;
}
RunScreen #log {
height: 1fr;
border: none;
padding: 0 1;
margin-top: 1;
}
RunScreen #loading {
height: 1;
margin-top: 1;
}
RunScreen #hint {
margin-top: 1;
color: $text-muted;
text-style: dim;
}
MultiPickScreen SelectionList {
height: auto;
border: none;
padding: 0;
margin-bottom: 1;
}
"""
# A screen that shows a question and a list of radio button options.
# The user picks one and presses Enter - the chosen string is returned as the result.
# Экран с вопросом и списком вариантов в виде радио-кнопок.
# Пользователь выбирает один и нажимает Enter - выбранная строка возвращается как результат.
class PickScreen(Screen[Optional[str]]):
"""Single-choice radio button screen. Returns the selected option string, or None on Escape.
Экран выбора одного варианта с радио-кнопками. Возвращает выбранную строку или None при Escape.
"""
BINDINGS = [
Binding("enter", "submit", "Confirm", priority=True),
Binding("escape", "abort", "Cancel"),
]
def __init__(self, step: str, question: str, options: List[str], default: str, note: str = ""):
super().__init__()
self._step = step
self._question = question
self._options = options
self._default = default
self._note = note
def compose(self) -> ComposeResult:
yield Static(self._step, id="step")
yield Static(self._question, id="question")
if self._note:
yield Static(self._note, id="note")
with RadioSet(id="choices"):
for opt in self._options:
# Pre-select the default option so the user can just press Enter to accept it.
# Заранее выделяем вариант по умолчанию, чтобы пользователь мог просто нажать Enter.
yield RadioButton(opt, value=(opt == self._default))
yield Footer()
def on_mount(self) -> None:
self.query_one(RadioSet).focus()
def action_submit(self) -> None:
radio_set = self.query_one("#choices", RadioSet)
buttons = list(radio_set.query(RadioButton))
idx = getattr(radio_set, "_selected", None)
if idx is not None and 0 <= idx < len(buttons):
self.dismiss(str(buttons[idx].label))
else:
btn = radio_set.pressed_button
self.dismiss(str(btn.label) if btn else self._default)
def action_abort(self) -> None:
# Exit the whole app, not just this screen, so the calling code knows the user cancelled.
# Выходим из всего приложения, а не только из этого экрана, чтобы вызывающий код знал об отмене.
self.app.exit(None)
# A screen that shows a question with a free-text input field.
# The user types a value, presses Enter, and the text is returned as the result.
# Экран с вопросом и полем для ввода произвольного текста.
# Пользователь вводит значение, нажимает Enter, и текст возвращается как результат.
class InputScreen(Screen[Optional[str]]):
"""Free-text input screen. Returns the trimmed value on Enter, or None on Escape.
Экран свободного ввода текста. Возвращает обрезанное значение при Enter или None при Escape.
"""
BINDINGS = [
Binding("enter", "submit", "Confirm", priority=True),
Binding("escape", "abort", "Cancel"),
]
def __init__(self, step: str, question: str, default: str, note: str = ""):
super().__init__()
self._step = step
self._question = question
self._default = default
self._note = note
def compose(self) -> ComposeResult:
yield Static(self._step, id="step")
yield Static(self._question, id="question")
if self._note:
yield Static(self._note, id="note")
yield Input(id="value", value=self._default)
yield Footer()
def on_mount(self) -> None:
self.query_one(Input).focus()
@on(Input.Submitted)
def _submitted(self, event: Input.Submitted) -> None:
self.dismiss(event.value.strip() or self._default)
def action_submit(self) -> None:
val = self.query_one(Input).value.strip()
self.dismiss(val or self._default)
def action_abort(self) -> None:
self.app.exit(None)
# A screen that streams output from a background task into a scrollable log.
# Used for long-running operations like installs and builds.
# Press Enter or Escape to close once the task finishes.
# Экран, который транслирует вывод фоновой задачи в прокручиваемый лог.
# Используется для долгих операций, таких как установка и сборка.
# После завершения задачи закрывается по нажатию Enter или Escape.
class LogScreen(Screen[bool]):
"""Log screen for long-running background tasks. Shows a scrollable log and optional
progress bar. Returns True on success, False on failure after the task finishes.
Экран лога для долгих фоновых задач. Показывает прокручиваемый лог и опциональный
прогресс-бар. Возвращает True при успехе, False при ошибке после завершения задачи.
"""
BINDINGS = [Binding("enter,escape", "close", "Close", show=False)]
def __init__(self, title: str, task: Callable[[LogScreen], None], show_progress: bool = False):
super().__init__()
self._title = title
self._run_fn = task
self._finished = False
self._success = False
self._show_progress = show_progress
# All subprocesses registered via set_proc() - every one gets killed on unmount.
# Все подпроцессы зарегистрированные через set_proc() - каждый убивается при выходе.
self._active_proc = None
self._procs: list = []
self._stopped = False
def compose(self) -> ComposeResult:
yield Static(self._title, id="step")
if self._show_progress:
yield ProgressBar(id="progress", total=100, show_eta=False)
yield Static("", id="step-label")
yield RichLog(id="log", highlight=True, markup=True, wrap=True)
yield LoadingIndicator(id="loading")
yield Static("", id="hint")
yield Footer()
def on_mount(self) -> None:
self.query_one(RichLog).focus()
# Run the task in a worker thread so the UI stays responsive.
# Запускаем задачу в отдельном потоке, чтобы интерфейс не зависал.
self.app.run_worker(lambda: self._run_fn(self), thread=True)
def set_proc(self, proc) -> None:
# Register the subprocess that is currently running. Added to _procs so on_unmount
# can kill it even if another proc is registered afterwards.
# Регистрируем текущий subprocess. Добавляем в _procs, чтобы on_unmount мог его убить
# даже если после него будет зарегистрирован другой процесс.
self._active_proc = proc
self._procs.append(proc)
def is_stopped(self) -> bool:
# Return True if the user has closed the screen before the task finished.
# Возвращает True если пользователь закрыл экран до завершения задачи.
return self._stopped
def on_unmount(self) -> None:
# Kill every registered subprocess so nothing keeps running in the background after exit.
# Use SIGKILL on the process group to also terminate any children spawned by the process
# (e.g. dpkg or apt subprocesses spawned under sudo). Falls back to proc.kill() if the
# process group is not available (e.g. already exited).
# Убиваем все зарегистрированные подпроцессы, чтобы ничего не висело в фоне после выхода.
# Используем SIGKILL по группе процессов, чтобы завершить и дочерние процессы
# (например dpkg или apt запущенные под sudo). Откат на proc.kill() если группа недоступна.
self._stopped = True
for proc in list(self._procs):
try:
pgid = os.getpgid(proc.pid)
os.killpg(pgid, signal.SIGKILL)
except Exception:
try:
proc.kill()
except Exception:
pass
self._procs.clear()
def set_progress(self, pct: float, label: str = "") -> None:
# Thread-safe - this is called from the worker thread, not the UI thread.
# Потокобезопасно - вызывается из рабочего потока, а не из потока интерфейса.
if self._show_progress:
self.app.call_from_thread(self._do_set_progress, pct, label)
def _do_set_progress(self, pct: float, label: str) -> None:
self.query_one("#progress", ProgressBar).progress = pct
if label:
self.query_one("#step-label", Static).update(label)
def write(self, line: str) -> None:
# Thread-safe - append a line to the log from a worker thread.
# Потокобезопасно - добавляет строку в лог из рабочего потока.
self.app.call_from_thread(self._append, line)
def _append(self, line: str) -> None:
self.query_one(RichLog).write(line)
def finish(self, success: bool) -> None:
# Thread-safe - called by the task when it is done to show the close hint.
# Потокобезопасно - вызывается задачей по завершении, чтобы показать подсказку о закрытии.
self.app.call_from_thread(self._do_finish, success)
def _do_finish(self, success: bool) -> None:
self._finished = True
self._success = success
self.query_one("#loading", LoadingIndicator).display = False
msg = (
"[green]Done![/green] Press [bold]Enter[/bold] to close."
if success
else "[red]Failed.[/red] Press [bold]Enter[/bold] to close."
)
self.query_one("#hint", Static).update(msg)
def action_close(self) -> None:
# Only allow closing after the task has finished, not while it is still running.
# Разрешаем закрытие только после завершения задачи, а не во время её работы.
if self._finished:
self.dismiss(self._success)
# A screen that shows a list of checkboxes for multi-selection.
# The user toggles items with Space, confirms with Enter, cancels with Escape.
# Экран с чекбоксами для множественного выбора.
# Пользователь переключает пункты пробелом, подтверждает Enter, отменяет Escape.
class MultiPickScreen(Screen[Optional[List[str]]]):
"""Multi-choice screen using SelectionList. Navigate with arrows, toggle with Space,
confirm with Enter, cancel with Escape. Returns selected option strings or None.
Экран множественного выбора через SelectionList. Стрелки — навигация, пробел — выбор,
Enter — подтверждение, Escape — отмена. Возвращает выбранные строки или None.
"""
BINDINGS = [
Binding("enter", "submit", "Confirm", priority=True),
Binding("escape", "abort", "Cancel"),
]
def __init__(self, step: str, question: str, options: List[str],
defaults: Optional[List[str]] = None, note: str = ""):
super().__init__()
self._step = step
self._question = question
self._options = options
# All options are selected by default when defaults is None.
# Все пункты выбраны по умолчанию если defaults не передан.
self._defaults = set(defaults) if defaults is not None else set(options)
self._note = note
def compose(self) -> ComposeResult:
yield Static(self._step, id="step")
yield Static(self._question, id="question")
if self._note:
yield Static(self._note, id="note")
yield SelectionList(
*[Selection(opt, opt, opt in self._defaults) for opt in self._options]
)
yield Footer()
def on_mount(self) -> None:
self.query_one(SelectionList).focus()
def action_submit(self) -> None:
self.dismiss(list(self.query_one(SelectionList).selected))
def action_abort(self) -> None:
self.app.exit(None)
# A screen for a long-running process that the user can stop at any time.
# Shows a live log and offers S / Enter / Escape to stop or close.
# Экран для долго работающего процесса, который пользователь может остановить в любой момент.
# Показывает живой лог и предлагает S / Enter / Escape для остановки или закрытия.
class RunScreen(Screen[None]):
"""Run screen for a persistent process (e.g. ROS2 launch). Shows a live log and allows
the user to stop the process with S or close after it exits with Enter/Escape.
Экран запуска для постоянно работающего процесса (например ros2 launch). Показывает живой
лог и позволяет остановить процесс клавишей S или закрыть после завершения через Enter/Escape.
"""
BINDINGS = [
Binding("s", "stop_close", "Stop", show=True, priority=True),
Binding("enter", "stop_close", "Close", show=False),
Binding("escape", "stop_close", "Close", show=False),
]
def __init__(self, title: str, task: Callable[[RunScreen], None]):
super().__init__()
self._title = title
self._run_fn = task
self._proc = None # the subprocess, set via set_proc()
self._kill_fn = None # optional custom kill callable, set via set_kill_fn()
self._finished = False
self._stopped = False
self._procs: list = [] # all registered procs for cleanup on forced exit
def compose(self) -> ComposeResult:
yield Static(self._title, id="step")
yield RichLog(id="log", highlight=True, markup=True, wrap=True)
yield LoadingIndicator(id="loading")
yield Static(" Press [bold]S[/bold] to stop the process", id="hint")
yield Footer()
def on_mount(self) -> None:
self.query_one(RichLog).focus()
# Run the process task in a worker thread so the UI stays responsive.
# Запускаем задачу с процессом в отдельном потоке, чтобы интерфейс не зависал.
self.app.run_worker(lambda: self._run_fn(self), thread=True)
def set_proc(self, proc) -> None:
# Register the subprocess so the Stop button knows what to terminate.
# Регистрируем subprocess, чтобы кнопка Stop знала что завершать.
self._proc = proc
self._procs.append(proc)
def set_kill_fn(self, fn: Callable) -> None:
# Override the default proc.terminate() with a custom kill function.
# For example, docker kill or os.killpg for process groups.
# Заменяем стандартный proc.terminate() кастомной функцией завершения.
# Например, docker kill или os.killpg для групп процессов.
self._kill_fn = fn
def write(self, line: str) -> None:
# Thread-safe - called from the worker thread to append a log line.
# Потокобезопасно - вызывается из рабочего потока для добавления строки в лог.
self.app.call_from_thread(self._append, line)
def _append(self, line: str) -> None:
self.query_one(RichLog).write(line)
def finish(self, stopped: bool = False) -> None:
# Thread-safe - called by the task when the process exits naturally.
# Потокобезопасно - вызывается задачей когда процесс завершается естественным образом.
self.app.call_from_thread(self._do_finish, stopped)
def _do_finish(self, stopped: bool) -> None:
self._finished = True
self.query_one("#loading", LoadingIndicator).display = False
if stopped:
msg = "[yellow]Process stopped.[/yellow] Press [bold]Enter[/bold] to close."
else:
msg = "[green]Process exited.[/green] Press [bold]Enter[/bold] to close."
self.query_one("#hint", Static).update(msg)
def on_unmount(self) -> None:
# Kill all registered subprocesses when the screen is forcibly closed (e.g. Ctrl+Q).
# Убиваем все зарегистрированные подпроцессы при принудительном закрытии экрана (Ctrl+Q).
self._stopped = True
if self._kill_fn is not None:
try:
self._kill_fn()
except Exception:
pass
for proc in list(self._procs):
try:
pgid = os.getpgid(proc.pid)
os.killpg(pgid, signal.SIGKILL)
except Exception:
try:
proc.kill()
except Exception:
pass
self._procs.clear()
def action_stop_close(self) -> None:
# This runs in the UI thread, so we call _append() directly instead of write()
# because write() uses call_from_thread() which only works from other threads.
# Выполняется в потоке UI, поэтому вызываем _append() напрямую, а не write(),
# потому что write() использует call_from_thread(), который работает только из других потоков.
if self._finished:
self.dismiss(None)
return
self._stopped = True
if self._kill_fn is not None:
try:
self._kill_fn()
except Exception:
pass
elif self._proc is not None and self._proc.poll() is None:
try:
self._proc.terminate()
except Exception:
pass
self._append("\n[yellow]Stopping process...[/yellow]")
cobot/ui.py
+358
View File
@@ -0,0 +1,358 @@
from __future__ import annotations
import os
import select as _select
import sys
from typing import List, Optional, Sequence, Tuple
from rich.console import Console, Group
from rich.live import Live
from rich.panel import Panel
from rich.text import Text
# Raw terminal control is POSIX-only; the project targets Linux/ROS so this is fine.
# Сырой режим терминала только для POSIX; проект под Linux/ROS, так что всё в порядке.
try:
import termios
import tty
_HAS_TERMIOS = True
except ImportError: # pragma: no cover - Windows fallback
_HAS_TERMIOS = False
# Single shared console used everywhere so styling and width stay consistent.
# Единый общий console, используемый везде, чтобы стиль и ширина были согласованы.
console = Console(highlight=False)
# Glyphs used across the UI. Kept here so the whole look can be retuned in one place.
# Глифы, используемые в интерфейсе. Собраны здесь, чтобы весь вид настраивался в одном месте.
_CURSOR = "❯"
_OK = "✓"
_FAIL = "✗"
_CHECK_ON = "◼"
_CHECK_OFF = "◻"
def is_interactive() -> bool:
"""Return True if both stdin and stdout are real terminals.
Arrow-key selection needs a real TTY to read raw key presses. When that is
not available (piped input, CI) callers should fall back to defaults.
Возвращает True, если и stdin, и stdout являются настоящими терминалами.
Выбор стрелками требует реального TTY для чтения нажатий клавиш. Если его нет
(перенаправленный ввод, CI), вызывающий код должен использовать значения по умолчанию.
"""
try:
return sys.stdin.isatty() and sys.stdout.isatty()
except Exception:
return False
# Low-level key reader
# Низкоуровневое чтение клавиш
# How long to wait (seconds) after a lone ESC byte before deciding it is really the
# Escape key and not the start of an arrow escape sequence (\x1b[A etc.).
# Сколько ждать (секунд) после одиночного байта ESC, прежде чем решить, что это
# именно клавиша Escape, а не начало escape-последовательности стрелок (\x1b[A и т.п.).
_ESC_TIMEOUT = 0.05
def _read_key() -> Tuple[str, str]:
"""Read one key press in raw mode and classify it.
Returns a (kind, char) tuple where kind is one of: "up", "down", "enter", "esc",
"space", "backspace", "char", "interrupt", "other". This is used instead of
readchar because readchar blocks after a lone ESC (waiting to see whether it is an
arrow sequence); here a short select() timeout distinguishes a real Escape press.
UTF-8 multibyte input (e.g. Cyrillic in a password) is decoded fully.
Читает одно нажатие в сыром режиме и классифицирует его. Возвращает кортеж
(kind, char). Используется вместо readchar, потому что readchar зависает после
одиночного ESC (ожидая, не последовательность ли это стрелок); здесь короткий
таймаут select() отличает настоящий Escape. UTF-8 (например кириллица в пароле)
декодируется полностью.
"""
if not _HAS_TERMIOS: # pragma: no cover
ch = sys.stdin.read(1)
return ("char", ch)
fd = sys.stdin.fileno()
old = termios.tcgetattr(fd)
try:
tty.setraw(fd)
b = os.read(fd, 1)
if not b:
return ("other", "")
c = b[0]
if c == 0x1B: # ESC — could be a lone Escape or an arrow/escape sequence
ready, _, _ = _select.select([fd], [], [], _ESC_TIMEOUT)
if not ready:
return ("esc", "")
seq = os.read(fd, 3)
last = seq[-1:] if seq else b""
if last == b"A":
return ("up", "")
if last == b"B":
return ("down", "")
if last in (b"C", b"D"):
return ("other", "")
return ("esc", "")
if c in (0x0D, 0x0A): # Enter
return ("enter", "")
if c == 0x03: # Ctrl-C (raw mode swallows SIGINT)
return ("interrupt", "")
if c == 0x20: # Space
return ("space", " ")
if c in (0x7F, 0x08): # Backspace / Delete
return ("backspace", "")
if c < 0x20: # other control char — ignore
return ("other", "")
# Printable byte — read any UTF-8 continuation bytes so multibyte chars decode.
# Печатный байт — дочитываем продолжения UTF-8, чтобы многобайтовые символы декодировались.
extra = 0
if c >= 0xF0:
extra = 3
elif c >= 0xE0:
extra = 2
elif c >= 0xC0:
extra = 1
if extra:
b += os.read(fd, extra)
return ("char", b.decode("utf-8", errors="ignore"))
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, old)
# Block headers and footers
# Заголовки и завершения блоков
def header(title: str, subtitle: str = "") -> None:
"""Print a styled header block that marks the start of a task or wizard step.
Печатает стилизованный блок-заголовок, обозначающий начало задачи или шага мастера.
"""
console.print()
bar = Text("▌ ", style="bold cyan")
bar.append(title, style="bold")
if subtitle:
bar.append(f" {subtitle}", style="dim")
console.print(bar)
def done(success: bool, message: str = "") -> None:
"""Print the final status line of a task (green ✓ on success, red ✗ on failure).
Печатает финальную строку статуса задачи (зелёная ✓ при успехе, красная ✗ при ошибке).
"""
if success:
line = Text(f"{_OK} ", style="bold green")
line.append(message or "Done", style="green")
else:
line = Text(f"{_FAIL} ", style="bold red")
line.append(message or "Failed", style="red")
console.print(line)
def note(message: str) -> None:
"""Print a dim helper/info line.
Печатает приглушённую вспомогательную/информационную строку.
"""
console.print(Text(f" {message}", style="dim"))
def info(message: str) -> None:
"""Print a plain message through the shared console (Rich markup allowed).
Печатает обычное сообщение через общий console (разрешена разметка Rich).
"""
console.print(message)
def error(message: str) -> None:
"""Print an error line.
Печатает строку ошибки.
"""
console.print(f"[bold red]Error:[/bold red] {message}")
# A collapsed answer line, printed after an interactive block is resolved.
# Свёрнутая строка-ответ, печатается после разрешения интерактивного блока.
def _print_answer(question: str, answer: str) -> None:
line = Text(f"{_OK} ", style="bold green")
line.append(f"{question} ", style="dim")
line.append("· ", style="dim")
line.append(answer, style="bold")
console.print(line)
def _print_cancelled(question: str) -> None:
line = Text(f"{_FAIL} ", style="bold red")
line.append(f"{question} ", style="dim")
line.append("· cancelled", style="red")
console.print(line)
def _render_choices(question: str, options: Sequence[str], cursor: int,
note_text: str = "") -> Panel:
"""Build the renderable shown while the user is navigating a single-choice list.
Строит отрисовываемый объект, показываемый пока пользователь навигирует по списку выбора.
"""
rows: List[Text] = []
for i, opt in enumerate(options):
if i == cursor:
row = Text(f" {_CURSOR} ", style="bold cyan")
row.append(opt, style="bold")
else:
row = Text(f" {opt}", style="dim")
rows.append(row)
body = Group(*rows)
title = Text(question, style="bold")
sub = "↑/↓ — выбор · Enter — подтвердить · Esc — отмена"
if note_text:
sub = f"{note_text}\n{sub}"
return Panel(body, title=title, title_align="left", subtitle=Text(sub, style="dim"),
subtitle_align="left", border_style="cyan", padding=(0, 1))
def select(question: str, options: Sequence[str], default: Optional[str] = None,
note: str = "") -> Optional[str]:
"""Show an arrow-key single-choice block and return the chosen option string.
Returns None if the user pressed Escape / Ctrl-C. When the terminal is not
interactive the default (or first option) is returned without prompting.
Показывает блок выбора одного варианта со стрелками и возвращает выбранную строку.
Возвращает None, если пользователь нажал Escape / Ctrl-C. Если терминал не
интерактивный, возвращается значение по умолчанию (или первый вариант) без запроса.
"""
options = list(options)
if not options:
return None
cursor = options.index(default) if default in options else 0
if not is_interactive():
chosen = options[cursor]
_print_answer(question, chosen)
return chosen
with Live(_render_choices(question, options, cursor, note), console=console,
auto_refresh=False, transient=True) as live:
while True:
live.update(_render_choices(question, options, cursor, note), refresh=True)
kind, ch = _read_key()
if kind == "up" or (kind == "char" and ch == "k"):
cursor = (cursor - 1) % len(options)
elif kind == "down" or (kind == "char" and ch == "j"):
cursor = (cursor + 1) % len(options)
elif kind == "enter":
break
elif kind in ("esc", "interrupt"):
_print_cancelled(question)
return None
chosen = options[cursor]
_print_answer(question, chosen)
return chosen
def _render_multi(question: str, options: Sequence[str], cursor: int,
selected: set, note_text: str = "") -> Panel:
"""Build the renderable for a multi-choice checkbox list.
Строит отрисовываемый объект для списка множественного выбора с чекбоксами.
"""
rows: List[Text] = []
for i, opt in enumerate(options):
box = _CHECK_ON if i in selected else _CHECK_OFF
if i == cursor:
row = Text(f" {_CURSOR} {box} ", style="bold cyan")
row.append(opt, style="bold")
else:
row = Text(f" {box} ", style="green" if i in selected else "dim")
row.append(opt, style="" if i in selected else "dim")
rows.append(row)
body = Group(*rows)
title = Text(question, style="bold")
sub = "↑/↓ — навигация · Space — отметить · Enter — подтвердить · Esc — отмена"
if note_text:
sub = f"{note_text}\n{sub}"
return Panel(body, title=title, title_align="left", subtitle=Text(sub, style="dim"),
subtitle_align="left", border_style="cyan", padding=(0, 1))
def multiselect(question: str, options: Sequence[str],
defaults: Optional[Sequence[str]] = None,
note: str = "") -> Optional[List[str]]:
"""Show an arrow-key multi-choice block. Space toggles, Enter confirms.
Returns the list of selected option strings, or None if cancelled.
Показывает блок множественного выбора со стрелками. Space переключает, Enter подтверждает.
Возвращает список выбранных строк или None при отмене.
"""
options = list(options)
if not options:
return []
if defaults is None:
selected = set(range(len(options)))
else:
selected = {i for i, o in enumerate(options) if o in defaults}
cursor = 0
if not is_interactive():
chosen = [options[i] for i in sorted(selected)]
_print_answer(question, ", ".join(chosen) or "—")
return chosen
with Live(_render_multi(question, options, cursor, selected, note), console=console,
auto_refresh=False, transient=True) as live:
while True:
live.update(_render_multi(question, options, cursor, selected, note), refresh=True)
kind, ch = _read_key()
if kind == "up" or (kind == "char" and ch == "k"):
cursor = (cursor - 1) % len(options)
elif kind == "down" or (kind == "char" and ch == "j"):
cursor = (cursor + 1) % len(options)
elif kind == "space":
selected.symmetric_difference_update({cursor})
elif kind == "enter":
break
elif kind in ("esc", "interrupt"):
_print_cancelled(question)
return None
chosen = [options[i] for i in sorted(selected)]
_print_answer(question, ", ".join(chosen) or "—")
return chosen
def text(question: str, default: str = "", note: str = "") -> Optional[str]:
"""Prompt for a single line of free text, pre-filled with default.
Returns the entered value (or default if left empty), or None on Ctrl-C / EOF.
Запрашивает одну строку произвольного текста, предзаполненную значением по умолчанию.
Возвращает введённое значение (или default, если пусто), либо None при Ctrl-C / EOF.
"""
prompt = Text()
prompt.append(f"{_CURSOR} ", style="bold cyan")
prompt.append(question, style="bold")
if default:
prompt.append(f" [{default}]", style="dim")
console.print(prompt)
if note:
console.print(Text(f" {note}", style="dim"))
try:
raw = input(" > ").strip()
except (EOFError, KeyboardInterrupt):
console.print()
_print_cancelled(question)
return None
value = raw or default
return value
def confirm(question: str, default: bool = True) -> bool:
"""Yes/No selection block. Returns True for yes, False for no or cancel.
Блок выбора Да/Нет. Возвращает True для да, False для нет или отмены.
"""
yes, no = "Да", "Нет"
choice = select(question, [yes, no], default=yes if default else no)
return choice == yes
docker/jazzy/ros-iiwa7-webots/Dockerfile
+9 -2
View File
@@ -5,6 +5,7 @@ ARG BUILD_TYPE=release
ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
ENV WEBOTS_HOME=/usr/local/webots
ENV PIP_BREAK_SYSTEM_PACKAGES=1
# X11 utilities and Mesa software renderer
RUN apt-get update && apt-get install -y --no-install-recommends \
@@ -35,7 +36,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends \
ros-${ROS_DISTRO}-ament-cmake-clang-format \
ros-${ROS_DISTRO}-rosbag2-storage-mcap \
python3-pil \
&& rm -rf /var/lib/apt/lists/* \
python3-pip \
&& rm -rf /var/lib/apt/lists/*
WORKDIR /ros2_ws
@@ -47,8 +48,14 @@ COPY src/iiwa_description src/iiwa_description
COPY src/iiwa_msgs src/iiwa_msgs
COPY src/iiwa_planning src/iiwa_planning
COPY src/iiwa_utils src/iiwa_utils
COPY src/iiwa_web src/iiwa_web
COPY rosdep.yaml rosdep.yaml
RUN apt-get update && \
RUN echo "yaml file:///ros2_ws/rosdep.yaml" \
> /etc/ros/rosdep/sources.list.d/50-kuka-local.list && \
rosdep update && \
apt-get update && \
pip3 install --ignore-installed "fastapi>=0.100.0" "starlette>=0.27.0" fastmcp && \
rosdep install --from-paths src -i -r -y && \
rm -rf /var/lib/apt/lists/*
docker/jazzy/ros-iiwa7/Dockerfile
+9 -1
View File
@@ -4,6 +4,7 @@ FROM ${IMAGE}
ARG BUILD_TYPE=release
ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
ENV PIP_BREAK_SYSTEM_PACKAGES=1
RUN apt-get update && apt-get install -y --no-install-recommends \
ros-${ROS_DISTRO}-ros2-control \
@@ -13,6 +14,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends \
ros-${ROS_DISTRO}-moveit-py \
ros-${ROS_DISTRO}-ament-cmake-clang-format \
ros-${ROS_DISTRO}-rosbag2-storage-mcap \
python3-pip \
&& rm -rf /var/lib/apt/lists/*
WORKDIR /ros2_ws
@@ -24,8 +26,14 @@ COPY src/iiwa_description src/iiwa_description
COPY src/iiwa_msgs src/iiwa_msgs
COPY src/iiwa_planning src/iiwa_planning
COPY src/iiwa_utils src/iiwa_utils
COPY src/iiwa_web src/iiwa_web
COPY rosdep.yaml rosdep.yaml
RUN apt-get update && \
RUN echo "yaml file:///ros2_ws/rosdep.yaml" \
> /etc/ros/rosdep/sources.list.d/50-kuka-local.list && \
rosdep update && \
apt-get update && \
pip3 install --ignore-installed "fastapi>=0.100.0" "starlette>=0.27.0" fastmcp && \
rosdep install --from-paths src -i -r -y && \
rm -rf /var/lib/apt/lists/*
rosdep.yaml
+20
View File
@@ -0,0 +1,20 @@
python3-fastapi:
ubuntu:
pip:
packages: ["fastapi>=0.100.0"]
python3-uvicorn:
ubuntu:
pip:
packages: ["uvicorn[standard]"]
python3-multipart:
ubuntu:
pip:
packages: [python-multipart]
python3-fastmcp:
ubuntu:
pip:
packages: [fastmcp]
scripts/setup_ros2_desktop.sh
+20
View File
@@ -51,10 +51,30 @@ PROGRESS 78 "Installing ROS2 dev tools..."
echo "Installing ROS2 dev tools..."
sudo apt-get install -y ros-dev-tools
PROGRESS 85 "Installing Python build tools..."
echo "Installing Python build tools..."
sudo apt-get install -y python3-pip python3-venv python3-dev
PROGRESS 90 "Initializing rosdep..."
echo "Initializing rosdep..."
sudo rosdep init 2>/dev/null || true
rosdep update
PROGRESS 95 "Configuring pip for system installs..."
echo "Configuring pip for system-wide installs (PEP 668 override)..."
sudo mkdir -p /root/.config/pip
if ! sudo grep -qs 'break-system-packages' /root/.config/pip/pip.conf 2>/dev/null; then
printf '[global]\nbreak-system-packages = true\n' | sudo tee -a /root/.config/pip/pip.conf > /dev/null
fi
# rosdep calls `pip install -U <pkg>` which upgrades every transitive dependency,
# including packages installed by apt that have no pip RECORD file, causing an
# uninstall failure. Pre-installing these packages with --ignore-installed creates
# pip RECORD files for all their transitive deps so the later rosdep upgrade succeeds.
# fastapi>=0.100.0 + starlette>=0.27.0 are pinned together to avoid the
# "Router.__init__() got an unexpected keyword argument 'on_startup'" error that
# occurs when the apt-installed fastapi (old) is mixed with a newer pip starlette.
sudo pip3 install --break-system-packages --ignore-installed \
"fastapi>=0.100.0" "starlette>=0.27.0" fastmcp
PROGRESS 100 "Done"
echo "ROS2 Jazzy Desktop installed successfully."
scripts/setup_ros2_ros_base.sh
+20
View File
@@ -51,10 +51,30 @@ PROGRESS 78 "Installing ROS2 dev tools..."
echo "Installing ROS2 dev tools..."
sudo apt-get install -y ros-dev-tools
PROGRESS 85 "Installing Python build tools..."
echo "Installing Python build tools..."
sudo apt-get install -y python3-pip python3-venv python3-dev
PROGRESS 90 "Initializing rosdep..."
echo "Initializing rosdep..."
sudo rosdep init 2>/dev/null || true
rosdep update
PROGRESS 95 "Configuring pip for system installs..."
echo "Configuring pip for system-wide installs (PEP 668 override)..."
sudo mkdir -p /root/.config/pip
if ! sudo grep -qs 'break-system-packages' /root/.config/pip/pip.conf 2>/dev/null; then
printf '[global]\nbreak-system-packages = true\n' | sudo tee -a /root/.config/pip/pip.conf > /dev/null
fi
# rosdep calls `pip install -U <pkg>` which upgrades every transitive dependency,
# including packages installed by apt that have no pip RECORD file, causing an
# uninstall failure. Pre-installing these packages with --ignore-installed creates
# pip RECORD files for all their transitive deps so the later rosdep upgrade succeeds.
# fastapi>=0.100.0 + starlette>=0.27.0 are pinned together to avoid the
# "Router.__init__() got an unexpected keyword argument 'on_startup'" error that
# occurs when the apt-installed fastapi (old) is mixed with a newer pip starlette.
sudo pip3 install --break-system-packages --ignore-installed \
"fastapi>=0.100.0" "starlette>=0.27.0" fastmcp
PROGRESS 100 "Done"
echo "ROS2 Jazzy (ros-base) installed successfully."
setup.py
+2 -2
View File
@@ -2,12 +2,12 @@ from setuptools import setup, find_packages
setup(
name="lightweight-cobot",
version="2026.05.31",
version="2026.06.11",
description="CLI tool for installing, configuring and managing the ROS 2 cobot workspace",
packages=find_packages(),
python_requires=">=3.11",
install_requires=[
"textual",
"rich",
"ruamel.yaml",
],
entry_points={
src/realsense2_camera/scripts/importRosbag/__init__.py → src/ai_agent/ai_agent/__init__.py
View File
src/ai_agent/package.xml
+18
View File
@@ -0,0 +1,18 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>ai_agent</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="grabardm@ml-dev.ru">daniel</maintainer>
<license>TODO: License declaration</license>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>
src/realsense2_camera/AMENT_IGNORE → src/ai_agent/resource/ai_agent
View File
src/ai_agent/setup.cfg
+4
View File
@@ -0,0 +1,4 @@
[develop]
script_dir=$base/lib/ai_agent
[install]
install_scripts=$base/lib/ai_agent
src/ai_agent/setup.py
+29
View File
@@ -0,0 +1,29 @@
from setuptools import find_packages, setup
package_name = 'ai_agent'
setup(
name=package_name,
version='0.0.0',
packages=find_packages(exclude=['test']),
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='daniel',
maintainer_email='grabardm@ml-dev.ru',
description='TODO: Package description',
license='TODO: License declaration',
extras_require={
'test': [
'pytest',
],
},
entry_points={
'console_scripts': [
],
},
)
src/ai_agent/test/test_copyright.py
+25
View File
@@ -0,0 +1,25 @@
# Copyright 2015 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_copyright.main import main
import pytest
# Remove the `skip` decorator once the source file(s) have a copyright header
@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
@pytest.mark.copyright
@pytest.mark.linter
def test_copyright():
rc = main(argv=['.', 'test'])
assert rc == 0, 'Found errors'
src/realsense2_description/CMakeLists.txt → src/ai_agent/test/test_flake8.py
+10 -13
View File
@@ -1,4 +1,4 @@
# Copyright 2023 RealSense, Inc. All Rights Reserved.
# Copyright 2017 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
@@ -12,17 +12,14 @@
# See the License for the specific language governing permissions and
# limitations under the License.
cmake_minimum_required(VERSION 3.10)
project(realsense2_description)
from ament_flake8.main import main_with_errors
import pytest
find_package(ament_cmake REQUIRED)
# Install files
install(DIRECTORY
launch
meshes
rviz
urdf
DESTINATION share/${PROJECT_NAME})
ament_package()
@pytest.mark.flake8
@pytest.mark.linter
def test_flake8():
rc, errors = main_with_errors(argv=[])
assert rc == 0, \
'Found %d code style errors / warnings:\n' % len(errors) + \
'\n'.join(errors)
src/realsense2_ros_mqtt_bridge/src/__init__.py → src/ai_agent/test/test_pep257.py
+11 -1
View File
@@ -1,4 +1,4 @@
# Copyright 2024 RealSense, Inc. All Rights Reserved.
# Copyright 2015 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
@@ -11,3 +11,13 @@
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_pep257.main import main
import pytest
@pytest.mark.linter
@pytest.mark.pep257
def test_pep257():
rc = main(argv=['.', 'test'])
assert rc == 0, 'Found code style errors / warnings'
src/iiwa_bringup/launch/digital_twin.launch.py
-160
View File
@@ -1,160 +0,0 @@
from launch import LaunchDescription
from launch.actions import (
DeclareLaunchArgument,
EmitEvent,
IncludeLaunchDescription,
OpaqueFunction,
RegisterEventHandler,
)
from launch.conditions import IfCondition
from launch.event_handlers import OnProcessExit
from launch.events import Shutdown
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 moveit_configs_utils import MoveItConfigsBuilder
from iiwa_utils import converter, setting_loader
def _runtime_setup(context, *args, **kwatgs):
setup = []
settings = setting_loader.build_settings(
settings_path=LaunchConfiguration("setting").perform(context), check_files=True
)
robot_description = converter.load_robot_description(
model_path=settings.robot.description,
robot_name=settings.robot.name,
xacro_args={
"initial_positions_file": settings.controller.moveit.initial_positions
},
)
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": True}],
)
webots_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
PathJoinSubstitution(
[
FindPackageShare("iiwa_bringup"),
"launch",
"supported",
"webots_spawn.launch.py",
]
)
),
launch_arguments={
"robot_name": str(settings.robot.name),
"description": str(settings.robot.description),
"world": str(settings.digital_twin.webots.world),
"transform": str(settings.digital_twin.webots.transform),
"rotation": str(settings.digital_twin.webots.rotation),
"controller_timer": str(settings.digital_twin.webots.controller_timer),
"controller": str(settings.controller.controller_path),
"initial_positions_file": str(settings.controller.moveit.initial_positions),
}.items(),
)
moveit_configs = (
MoveItConfigsBuilder("iiwa7", package_name="iiwa_config")
.robot_description(
file_path=settings.robot.description,
mappings={
"initial_positions_file": settings.controller.moveit.initial_positions
},
)
.robot_description_semantic(file_path=settings.controller.moveit.srdf)
.robot_description_kinematics(file_path=settings.controller.moveit.kinematics)
.joint_limits(file_path=settings.controller.moveit.joint_limits)
.pilz_cartesian_limits(file_path=settings.controller.moveit.pilz_limits)
.trajectory_execution(file_path=settings.controller.moveit.moveit_controllers)
.moveit_cpp(file_path=settings.controller.moveit.moveit_cpp)
.to_moveit_configs()
)
move_group = Node(
package="moveit_ros_move_group",
executable="move_group",
output="screen",
parameters=[
moveit_configs.to_dict(),
{"robot_description": robot_description},
{"use_sim_time": True},
],
)
# TODO: не забудь поменять правильное название и имя пакета
# moveit_py_node = Node(
# # name="motion_planning_node",
# package="iiwa_planning",
# executable="motion_planning",
# output="both",
# parameters=[moveit_configs.to_dict()],
# )
rviz_launch = Node(
condition=IfCondition(LaunchConfiguration("rviz")),
package="rviz2",
executable="rviz2",
name="rviz2",
arguments=["-d", settings.digital_twin.rviz.config],
output="log",
parameters=[
moveit_configs.robot_description,
moveit_configs.robot_description_semantic,
moveit_configs.planning_pipelines,
moveit_configs.planning_scene_monitor,
{"use_sim_time": True},
],
)
shutdown_on_rviz_exit = RegisterEventHandler(
OnProcessExit(target_action=rviz_launch, on_exit=[EmitEvent(event=Shutdown())])
)
setup += [
rsp_node,
webots_launch,
move_group,
# moveit_py_node,
rviz_launch,
shutdown_on_rviz_exit,
]
return setup
def generate_launch_description():
declare_rviz = DeclareLaunchArgument(
name="rviz",
default_value="0",
description="If true|1|yes then launch RViz/MoveIt branch (instead of controllers branch)",
)
declacre_setting = DeclareLaunchArgument(
name="setting",
default_value=PathJoinSubstitution(
[FindPackageShare("iiwa_config"), "config", "setting.yaml"]
),
description="Absolute path to settings file",
)
runtime_setup = OpaqueFunction(function=_runtime_setup)
return LaunchDescription(
[
declare_rviz,
declacre_setting,
runtime_setup,
]
)
src/iiwa_bringup/launch/iiwa.launch.py
+39 -177
View File
@@ -1,40 +1,26 @@
import json
from dataclasses import asdict
import os
import sys
sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
from launch import LaunchDescription
from launch.actions import (
DeclareLaunchArgument,
EmitEvent,
IncludeLaunchDescription,
OpaqueFunction,
RegisterEventHandler,
)
from launch.conditions import IfCondition
from launch.event_handlers import OnProcessExit
from launch.events import Shutdown
from launch.actions import DeclareLaunchArgument, 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 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:
params = asdict(fg)
params.pop("enabled")
params["use_sim_time"] = use_sim_time
return params
from supported.moveit_nodes import make_moveit_nodes
from supported.rviz_nodes import make_rviz_nodes
from supported.simulation_nodes import make_simulation_nodes
from supported.optional_nodes import make_foxglove_node, make_web_server_node
def _runtime_setup(context, *args, **kwargs):
setup = []
# Настройка параметров
simulate = LaunchConfiguration("simulate").perform(context) in ("true", "1", "yes")
settings = setting_loader.build_settings(
@@ -52,23 +38,21 @@ def _runtime_setup(context, *args, **kwargs):
)
description_path = settings.robot.description
use_sim_time = simulate
if simulate:
xacro_args = {
"initial_positions_file": settings.controller.moveit.initial_positions,
"simulate": "true",
}
use_sim_time = True
else:
xacro_args = {
"initial_positions_file": settings.controller.moveit.initial_positions,
"robot_ip": settings.robot.ip,
"fri_port": str(settings.robot.port),
"simulate": "false",
"command_mode": settings.robot.command_mode,
"joint_position_tau": str(settings.robot.joint_position_tau),
}
use_sim_time = False
robot_description = converter.load_robot_description(
model_path=description_path,
@@ -76,8 +60,8 @@ def _runtime_setup(context, *args, **kwargs):
xacro_args=xacro_args,
)
# Вызов нод
rsp_node = Node(
# Robot State Publisher
setup.append(Node(
package="robot_state_publisher",
executable="robot_state_publisher",
name="robot_state_publisher",
@@ -86,22 +70,15 @@ def _runtime_setup(context, *args, **kwargs):
{"robot_description": robot_description},
{"use_sim_time": use_sim_time},
],
)
))
setup += [rsp_node]
# webots spawn
# Webots симуляция
if simulate:
webots_launch = IncludeLaunchDescription(
setup.append(IncludeLaunchDescription(
PythonLaunchDescriptionSource(
PathJoinSubstitution(
[
FindPackageShare("iiwa_bringup"),
"launch",
"supported",
"webots_spawn.launch.py",
]
)
PathJoinSubstitution([
FindPackageShare("iiwa_bringup"), "launch", "supported", "webots_spawn.launch.py",
])
),
launch_arguments={
"robot_name": str(settings.robot.name),
@@ -113,11 +90,11 @@ def _runtime_setup(context, *args, **kwargs):
"controller": str(settings.controller.controller_path),
"initial_positions_file": str(settings.controller.moveit.initial_positions),
}.items(),
)
))
setup += [webots_launch]
setup += make_simulation_nodes(settings)
# Controller launch
# Controllers
if simulate:
controller_args = {
"robot_name": settings.robot.name,
@@ -129,164 +106,52 @@ def _runtime_setup(context, *args, **kwargs):
"rotation": str(settings.digital_twin.webots.rotation),
"controller_timer": str(settings.digital_twin.webots.controller_timer),
}
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)
# WebotsController для каждой камеры
for cam_path in settings.digital_twin.webots.cameras:
cam_cfg = load_camera_config(cam_path)
urdf = build_ros_urdf(cam_cfg)
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 = {
"robot_name": settings.robot.name,
"description": description_path,
"initial_positions_file": settings.controller.moveit.initial_positions,
"controller_path": settings.controller.controller_path,
"simulate": "false",
"transform": str(settings.digital_twin.webots.transform),
"rotation": str(settings.digital_twin.webots.rotation),
"simulate": "false",
"command_mode": settings.robot.command_mode,
"controller_timer": str(settings.digital_twin.webots.controller_timer),
"fri_cycle_ms": str(settings.robot.fri_cycle_ms),
"joint_position_tau": str(settings.robot.joint_position_tau),
"controller": settings.robot.active_controller,
}
controllers_launch = IncludeLaunchDescription(
setup.append(IncludeLaunchDescription(
PythonLaunchDescriptionSource(
PathJoinSubstitution(
[
FindPackageShare("iiwa_bringup"),
"launch",
"supported",
"controllers.launch.py",
]
)
PathJoinSubstitution([
FindPackageShare("iiwa_bringup"), "launch", "supported", "controllers.launch.py",
])
),
launch_arguments={k: str(v) for k, v in controller_args.items()}.items(),
)
))
# Moveit launch
moveit_configs = (
MoveItConfigsBuilder("iiwa7", package_name="iiwa_config")
.robot_description(
file_path=description_path,
mappings={
"initial_positions_file": settings.controller.moveit.initial_positions
},
)
.robot_description_semantic(file_path=settings.controller.moveit.srdf)
.robot_description_kinematics(file_path=settings.controller.moveit.kinematics)
.joint_limits(file_path=settings.controller.moveit.joint_limits)
.pilz_cartesian_limits(file_path=settings.controller.moveit.pilz_limits)
.trajectory_execution(file_path=settings.controller.moveit.moveit_controllers)
.moveit_cpp(file_path=settings.controller.moveit.moveit_cpp)
.to_moveit_configs()
)
# MoveIt
moveit_configs, moveit_nodes = make_moveit_nodes(settings, robot_description, use_sim_time)
setup += moveit_nodes
move_group = Node(
package="moveit_ros_move_group",
executable="move_group",
output="screen",
parameters=[
moveit_configs.to_dict(),
{"robot_description": robot_description},
{"use_sim_time": use_sim_time},
],
)
move_to_pose_server = Node(
package="iiwa_planning",
executable="move_to_pose_server",
output="screen",
parameters=[
moveit_configs.to_dict(),
{"robot_description": robot_description},
{"use_sim_time": use_sim_time},
{
"pose_link": settings.planning.pose_link,
"planning_group": settings.planning.planning_group,
"default_frame": settings.planning.default_frame,
"default_planner": settings.planning.default_planner,
"planning_attempts": settings.planning.planning_attempts,
},
],
)
# Rviz launch
rviz_launch = Node(
condition=IfCondition(LaunchConfiguration("rviz")),
package="rviz2",
executable="rviz2",
name="rviz2",
arguments=["-d", settings.digital_twin.rviz.config],
output="log",
parameters=[
moveit_configs.robot_description,
moveit_configs.robot_description_semantic,
moveit_configs.planning_pipelines,
joint_limits_ros2,
kinematics_ros2,
{"use_sim_time": use_sim_time},
],
)
shutdown_on_rviz_exit = RegisterEventHandler(
OnProcessExit(
target_action=rviz_launch,
on_exit=[EmitEvent(event=Shutdown())],
)
)
setup += [
controllers_launch,
move_group,
move_to_pose_server,
rviz_launch,
shutdown_on_rviz_exit,
]
# RViz
setup += make_rviz_nodes(settings, moveit_configs, joint_limits_ros2, kinematics_ros2, use_sim_time)
# Опциональные сервисы
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)]
)
setup.append(make_foxglove_node(settings, use_sim_time))
setup += [foxglove_bridge]
if settings.web.enabled:
setup.append(make_web_server_node(settings, use_sim_time))
return setup
def generate_launch_description():
declare_simulate = DeclareLaunchArgument(
name="simulate",
default_value="false",
description="true = Gazebo симуляция, false = реальный робот через FRI",
description="true = Webots симуляция, false = реальный робот через FRI",
)
declare_rviz = DeclareLaunchArgument(
@@ -303,12 +168,9 @@ def generate_launch_description():
description="Путь к файлу настроек",
)
runtime_setup = OpaqueFunction(function=_runtime_setup)
return LaunchDescription([
declare_simulate,
declare_rviz,
declare_setting,
runtime_setup,
OpaqueFunction(function=_runtime_setup),
])
src/iiwa_bringup/launch/supported/controllers.launch.py
+3 -26
View File
@@ -19,7 +19,6 @@ def _setup_controllers(context, *args, **kwargs):
controller_timer = LaunchConfiguration("controller_timer").perform(context)
controller_path = LaunchConfiguration("controller_path").perform(context)
simulate = LaunchConfiguration("simulate").perform(context).lower() in ("true", "1", "yes")
command_mode = LaunchConfiguration("command_mode").perform(context)
controller = LaunchConfiguration("controller").perform(context) # "jtc" | "forward"
fri_cycle_ms = int(LaunchConfiguration("fri_cycle_ms").perform(context))
joint_position_tau = LaunchConfiguration("joint_position_tau").perform(context)
@@ -65,18 +64,10 @@ def _setup_controllers(context, *args, **kwargs):
parameters=[{"use_sim_time": True}],
)
torque_controller_spawner = Node(
package="controller_manager",
executable="spawner",
output="screen",
arguments=["iiwa_arm_torque_controller", "--inactive"] + tmo,
parameters=[{"use_sim_time": True}]
)
jtc_after_jsb = RegisterEventHandler(
OnProcessExit(
target_action=jsb,
on_exit=[jtc, torque_controller_spawner],
on_exit=[jtc],
)
)
@@ -107,9 +98,8 @@ def _setup_controllers(context, *args, **kwargs):
cm = ["--controller-manager", "/controller_manager"]
# JTC: активен если controller=jtc (и command_mode=position), иначе --inactive
jtc_args = ["iiwa_arm_controller"] + cm
if command_mode == "torque" or controller == "forward":
if controller == "forward":
jtc_args += ["--inactive"]
# ForwardCommandController: активен если controller=forward, иначе --inactive
@@ -117,11 +107,6 @@ def _setup_controllers(context, *args, **kwargs):
if controller != "forward":
forward_args += ["--inactive"]
# TorqueController: активен если command_mode=torque и controller=jtc
torque_args = ["iiwa_arm_torque_controller"] + cm
if not (command_mode == "torque" and controller == "jtc"):
torque_args += ["--inactive"]
jtc = Node(
package="controller_manager",
executable="spawner",
@@ -136,17 +121,10 @@ def _setup_controllers(context, *args, **kwargs):
arguments=forward_args,
)
torque_controller = Node(
package="controller_manager",
executable="spawner",
output="screen",
arguments=torque_args,
)
jtc_after_jsb = RegisterEventHandler(
OnProcessExit(
target_action=jsb,
on_exit=[jtc, forward_controller, torque_controller],
on_exit=[jtc, forward_controller],
)
)
@@ -159,7 +137,6 @@ def _setup_controllers(context, *args, **kwargs):
def generate_launch_description():
return LaunchDescription([
DeclareLaunchArgument("command_mode", default_value="position"),
DeclareLaunchArgument("fri_cycle_ms", default_value="5"),
DeclareLaunchArgument("joint_position_tau", default_value="0.04"),
DeclareLaunchArgument("controller", default_value="jtc"),
src/iiwa_bringup/launch/supported/moveit_nodes.py
+53
View File
@@ -0,0 +1,53 @@
from launch_ros.actions import Node
from moveit_configs_utils import MoveItConfigsBuilder
def make_moveit_nodes(settings, robot_description: str, use_sim_time: bool):
"""Возвращает (moveit_configs, [move_group, move_to_pose_server])."""
moveit_configs = (
MoveItConfigsBuilder("iiwa7", package_name="iiwa_config")
.robot_description(
file_path=settings.robot.description,
mappings={
"initial_positions_file": settings.controller.moveit.initial_positions
},
)
.robot_description_semantic(file_path=settings.controller.moveit.srdf)
.robot_description_kinematics(file_path=settings.controller.moveit.kinematics)
.joint_limits(file_path=settings.controller.moveit.joint_limits)
.pilz_cartesian_limits(file_path=settings.controller.moveit.pilz_limits)
.trajectory_execution(file_path=settings.controller.moveit.moveit_controllers)
.moveit_cpp(file_path=settings.controller.moveit.moveit_cpp)
.to_moveit_configs()
)
common_params = [
moveit_configs.to_dict(),
{"robot_description": robot_description},
{"use_sim_time": use_sim_time},
]
move_group = Node(
package="moveit_ros_move_group",
executable="move_group",
output="screen",
parameters=common_params,
)
move_to_pose_server = Node(
package="iiwa_planning",
executable="move_to_pose_server",
output="screen",
parameters=[
*common_params,
{
"pose_link": settings.planning.pose_link,
"planning_group": settings.planning.planning_group,
"default_frame": settings.planning.default_frame,
"default_planner": settings.planning.default_planner,
"planning_attempts": settings.planning.planning_attempts,
},
],
)
return moveit_configs, [move_group, move_to_pose_server]
src/iiwa_bringup/launch/supported/optional_nodes.py
+33
View File
@@ -0,0 +1,33 @@
from dataclasses import asdict
from launch_ros.actions import Node
def make_foxglove_node(settings, use_sim_time: bool) -> Node:
params = asdict(settings.foxglove)
params.pop("enabled")
params["use_sim_time"] = use_sim_time
return Node(
package="foxglove_bridge",
executable="foxglove_bridge",
output="screen",
name="foxglove_bridge",
parameters=[params],
)
def make_web_server_node(settings, use_sim_time: bool) -> Node:
return Node(
package="iiwa_web",
executable="iiwa_web_server",
output="screen",
name="iiwa_web_server",
parameters=[{
"host": settings.web.host,
"port": settings.web.port,
"endpoints_path": settings.web.endpoints,
"joint_limits_path": settings.web.joint_limits,
"use_sim_time": use_sim_time,
}],
)
src/iiwa_bringup/launch/supported/rviz_nodes.py
+35
View File
@@ -0,0 +1,35 @@
from launch.actions import EmitEvent, RegisterEventHandler
from launch.conditions import IfCondition
from launch.event_handlers import OnProcessExit
from launch.events import Shutdown
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
def make_rviz_nodes(settings, moveit_configs, joint_limits_ros2, kinematics_ros2, use_sim_time: bool):
"""Возвращает [rviz_launch, shutdown_on_rviz_exit]."""
rviz_launch = Node(
condition=IfCondition(LaunchConfiguration("rviz")),
package="rviz2",
executable="rviz2",
name="rviz2",
arguments=["-d", settings.digital_twin.rviz.config],
output="log",
parameters=[
moveit_configs.robot_description,
moveit_configs.robot_description_semantic,
moveit_configs.planning_pipelines,
joint_limits_ros2,
kinematics_ros2,
{"use_sim_time": use_sim_time},
],
)
shutdown_on_rviz_exit = RegisterEventHandler(
OnProcessExit(
target_action=rviz_launch,
on_exit=[EmitEvent(event=Shutdown())],
)
)
return [rviz_launch, shutdown_on_rviz_exit]
src/iiwa_bringup/launch/supported/simulation_nodes.py
+40
View File
@@ -0,0 +1,40 @@
import json
from launch_ros.actions import Node
from webots_ros2_driver.webots_controller import WebotsController
from iiwa_utils.camera_spawner import load_camera_config, build_ros_urdf # type: ignore
def make_simulation_nodes(settings) -> list:
"""Возвращает ноды камер для симуляции: [camera_spawner, *camera_controllers].
Если камеры не заданы в настройках — возвращает пустой список.
"""
if not settings.digital_twin.webots.cameras:
return []
camera_spawner = Node(
package="iiwa_utils",
executable="camera_spawner",
name="camera_spawner",
output="screen",
parameters=[{
"camera_configs": json.dumps(settings.digital_twin.webots.cameras)
}],
)
camera_controllers = []
for cam_path in settings.digital_twin.webots.cameras:
cam_cfg = load_camera_config(cam_path)
camera_controllers.append(WebotsController(
robot_name=f"{cam_cfg.name}_robot",
parameters=[{
"robot_description": build_ros_urdf(cam_cfg),
"use_sim_time": True,
"set_robot_state_publisher": False,
}],
respawn=True,
))
return [camera_spawner, *camera_controllers]
src/iiwa_bringup/package.xml
+1 -1
View File
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>iiwa_bringup</name>
<version>2026.05.31</version>
<version>2026.5.31</version>
<description>Launch files for cobot: Webots simulation, real robot via FRI, MoveIt motion planning and RViz visualization</description>
<maintainer email="grabardm@ml-dev.ru">daniel</maintainer>
<license>Apache-2.0</license>
src/iiwa_config/config/api_endpoints.yaml
+144
View File
@@ -0,0 +1,144 @@
endpoints:
- path: /robot/joint_states
method: GET
type: topic
ros_name: /joint_states
msg_type: sensor_msgs/msg/JointState
summary: "Текущее состояние суставов"
description: "Возвращает имена, позиции, скорости и усилия всех суставов."
tags: [robot]
fields: [name, position, velocity, effort]
timeout: 2.0
enabled: true
- path: /robot/pose
method: GET
type: fk
ros_name: /compute_fk
parent_frame: base_link
child_frame: tcp
summary: "Текущая декартова поза TCP"
description: "Позиция (м) и ориентация TCP относительно base_link через /compute_fk. Углы Эйлера в конвенции KUKA ABC (ZYX): A=рыскание, B=тангаж, C=крен."
tags: [robot]
timeout: 5.0
enabled: true
- path: /robot/stop
method: POST
type: service
ros_name: cobot/stop
msg_type: std_srvs/srv/Trigger
summary: "Немедленно остановить движение"
description: "Вызывает сервис экстренной остановки — движение прерывается немедленно."
tags: [motion]
response_fields: [success, message]
timeout: 5.0
enabled: true
- path: /robot/move/named
method: POST
type: service
ros_name: cobot/move_to_named
msg_type: iiwa_msgs/srv/MoveToNamedPose
summary: "Переместить в именованную позу из SRDF"
description: "Перемещает робота в позу, определённую по имени в SRDF-файле. Список доступных имён и значения суставов для каждой позиции возвращает GET /robot/positions."
tags: [motion]
timeout: 30.0
request_fields:
- name: name
type: string
required: true
description: "Имя позиции из SRDF"
- name: speed
type: float
default: 0.1
min: 0.01
max: 1.0
description: "Скорость [0.01–1.0]"
- name: accel_scale
type: float
default: 0.0
min: 0.0
max: 1.0
description: "Масштаб ускорения (0 = равно speed)"
response_fields: [success, message]
enabled: true
- path: /robot/move/pose
method: POST
type: action
ros_name: cobot/move_to_pose
msg_type: iiwa_msgs/action/MoveToPose
summary: "Переместить в декартову позу"
description: "Перемещает TCP робота в заданную декартову позицию и ориентацию."
tags: [motion]
timeout: 30.0
request_fields:
- name: x
type: float
required: true
description: "Позиция X в метрах"
- name: y
type: float
required: true
description: "Позиция Y в метрах"
- name: z
type: float
required: true
description: "Позиция Z в метрах"
- name: a
type: float
default: 0.0
description: "Угол A (ZYX Эйлер, KUKA ABC) в радианах"
- name: b
type: float
default: 0.0
description: "Угол B в радианах"
- name: c
type: float
default: 0.0
description: "Угол C в радианах"
- name: speed
type: float
default: 0.1
min: 0.01
max: 1.0
description: "Скорость [0.01–1.0]"
- name: planner
type: string
default: "ptp"
choices: [ompl, ptp, lin, circ, chomp]
normalize: lower
description: "Планировщик движения: ompl, ptp, lin, circ, chomp"
- name: frame_id
type: string
default: ""
description: "Целевой фрейм (пусто = default_frame)"
response_fields: [success, message]
enabled: true
- path: /robot/move/joints
method: POST
type: action
ros_name: cobot/move_to_joints
msg_type: iiwa_msgs/action/MoveToJoints
summary: "Переместить в позиции суставов"
description: "Перемещает все суставы робота в заданные угловые позиции (радианы). Лимиты читаются из joint_limits.yaml."
tags: [motion]
timeout: 30.0
request_fields:
- name: joints
type: float_array
required: true
length: 7
joint_limits: true
description: "Позиции суставов в радианах [j1..j7]"
- name: speed
type: float
default: 0.1
min: 0.01
max: 1.0
description: "Скорость [0.01–1.0]"
response_fields: [success, message]
enabled: true
src/iiwa_config/config/moveit/iiwa7.srdf
+20 -31
View File
@@ -1,32 +1,11 @@
<?xml version="1.0" encoding="UTF-8"?>
<!--This does not replace URDF, and is not an extension of URDF.
This is a format for representing semantic information about the robot structure.
A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
-->
<!-- AUTO-GENERATED by cobot robot-setup — do not edit manually -->
<robot name="iiwa7">
<!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
<!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
<!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
<!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
<!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
<group name="iiwa_arm">
<!-- <joint name="world_base_joint"/>
<joint name="joint1"/>
<joint name="joint2"/>
<joint name="joint3"/>
<joint name="joint4"/>
<joint name="joint5"/>
<joint name="joint6"/>
<joint name="joint7"/>
<joint name="tools_joint"/>
<joint name="tool"/>
<joint name="camera_holder_patron"/>
<joint name="camera_holder_corner"/>
<joint name="camera_corner_camera"/>
<joint name="camera_hand_to_optical"/> -->
<chain base_link="base_link" tip_link="patron"/>
</group>
<!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
<group_state name="home" group="iiwa_arm">
<joint name="joint1" value="0"/>
<joint name="joint2" value="0"/>
@@ -45,9 +24,18 @@
<joint name="joint6" value="1.57"/>
<joint name="joint7" value="0"/>
</group_state>
<!--END EFFECTOR: Purpose: Represent information about an end effector.-->
<group_state name="transport" group="iiwa_arm">
<joint name="joint1" value="0"/>
<joint name="joint2" value="0.436"/>
<joint name="joint3" value="0"/>
<joint name="joint4" value="1.57"/>
<joint name="joint5" value="0"/>
<joint name="joint6" value="0"/>
<joint name="joint7" value="0"/>
</group_state>
<end_effector name="patron" parent_link="patron" group="iiwa_arm"/>
<!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
<disable_collisions link1="base_link" link2="link1" reason="Adjacent"/>
<disable_collisions link1="base_link" link2="link2" reason="Never"/>
<disable_collisions link1="base_link" link2="link3" reason="Never"/>
@@ -58,26 +46,27 @@
<disable_collisions link1="link1" link2="link5" reason="Never"/>
<disable_collisions link1="link1" link2="link6" reason="Never"/>
<disable_collisions link1="link1" link2="link7" reason="Never"/>
<disable_collisions link1="link1" link2="patron" reason="Never"/>
<disable_collisions link1="link2" link2="link3" reason="Adjacent"/>
<disable_collisions link1="link2" link2="link4" reason="Never"/>
<disable_collisions link1="link2" link2="link5" reason="Never"/>
<disable_collisions link1="link2" link2="link6" reason="Never"/>
<disable_collisions link1="link2" link2="link7" reason="Never"/>
<disable_collisions link1="link2" link2="patron" reason="Never"/>
<disable_collisions link1="link3" link2="link4" reason="Adjacent"/>
<disable_collisions link1="link3" link2="link5" reason="Never"/>
<disable_collisions link1="link3" link2="link6" reason="Never"/>
<disable_collisions link1="link3" link2="link7" reason="Never"/>
<disable_collisions link1="link3" link2="patron" reason="Never"/>
<disable_collisions link1="link4" link2="link5" reason="Adjacent"/>
<disable_collisions link1="link4" link2="link6" reason="Never"/>
<disable_collisions link1="link4" link2="link7" reason="Never"/>
<disable_collisions link1="link4" link2="patron" reason="Never"/>
<disable_collisions link1="link5" link2="link6" reason="Adjacent"/>
<disable_collisions link1="link5" link2="link7" reason="Never"/>
<disable_collisions link1="link5" link2="patron" reason="Never"/>
<disable_collisions link1="link6" link2="link7" reason="Adjacent"/>
<disable_collisions link1="link1" link2="patron" reason="Never"/>
<disable_collisions link1="link2" link2="patron" reason="Never"/>
<disable_collisions link1="link3" link2="patron" reason="Never"/>
<disable_collisions link1="link4" link2="patron" reason="Never"/>
<disable_collisions link1="link5" link2="patron" reason="Never"/>
<disable_collisions link1="link6" link2="patron" reason="Never"/>
<disable_collisions link1="link7" link2="patron" reason="Adjacent"/>
<disable_collisions link1="link7" link2="camera_holder" reason="Adjacent"/>
src/iiwa_config/config/moveit/joint_limits.yaml
+6 -6
View File
@@ -20,8 +20,8 @@ joint_limits:
max_jerk: 85.0
joint2:
has_position_limits: true
min_position: -2.09
max_position: 2.09
min_position: -2.10
max_position: 2.10
has_velocity_limits: true
max_velocity: 1.71
has_acceleration_limits: true
@@ -40,8 +40,8 @@ joint_limits:
max_jerk: 87.0
joint4:
has_position_limits: true
min_position: -2.09
max_position: 2.09
min_position: -2.10
max_position: 2.10
has_velocity_limits: true
max_velocity: 2.27
has_acceleration_limits: true
@@ -60,8 +60,8 @@ joint_limits:
max_jerk: 122.0
joint6:
has_position_limits: true
min_position: -2.09
max_position: 2.09
min_position: -2.10
max_position: 2.10
has_velocity_limits: true
max_velocity: 3.14
has_acceleration_limits: true
src/iiwa_config/config/moveit/named_positions_meta.yaml
+9
View File
@@ -0,0 +1,9 @@
# Метаданные для именованных позиций из iiwa7.srdf
# Ключ совпадает с атрибутом name тега <group_state>
named_positions:
home:
description: "Нулевое положение всех суставов - домашняя позиция робота"
work:
description: "Рабочее положение для выполнения задач"
transport:
description: "Позиция для транспортировки робота"
src/iiwa_config/config/setting.yaml
+10 -1
View File
@@ -2,7 +2,6 @@ robot:
name: "iiwa7"
ip: "192.170.10.2"
port: 30200
command_mode: "position" # torque, position
fri_cycle_ms: 10 # период FRI-цикла: 5 мс (200 Гц) или 10 мс (100 Гц)
joint_position_tau: 0.04 # EMA фильтр позиций [с]: сглаживает команды перед отправкой в FRI
joint_velocity_tau: 0.01 # EMA фильтр скорости [с]: убирает выбросы конечных разностей
@@ -37,6 +36,9 @@ controller:
moveit_cpp: pkg://iiwa_config/config/moveit/moveit_cpp.yaml
tool:
active: "patron" # Активный инструмент: none | patron | ... (из tools.yaml)
planning:
pose_link: "tcp" # TCP-линк для декартовых целей
planning_group: "iiwa_arm" # Группа планирования из SRDF
@@ -44,6 +46,13 @@ planning:
default_planner: "ompl" # Планировщик по умолчанию
planning_attempts: 3 # Число попыток планирования
web:
enabled: true
host: "0.0.0.0"
port: 8007
endpoints: pkg://iiwa_config/config/api_endpoints.yaml
joint_limits: pkg://iiwa_config/config/moveit/joint_limits.yaml
foxglove:
enabled: true # Запускать ли foxglove_bridge вместе с роботом
port: 8765 # WebSocket-порт, к которому подключается Foxglove Studio (по умолчанию 8765)
src/iiwa_config/config/tools.yaml
+64
View File
@@ -0,0 +1,64 @@
# Реестр инструментов / захватов для KUKA iiwa7.
#
# Поля каждого инструмента:
# label — название, отображаемое в меню robot-setup
# xacro — путь до xacro-файла инструмента (xacro $(find ...) синтаксис);
# null = без захвата (голый фланец)
# tip_link — конец кинематической цепочки в SRDF (<chain tip_link="..."/>)
# tcp_link — фрейм TCP для Декартовых целей (pose_link в planning)
# collisions — пары disable_collisions специфичные для этого инструмента;
# базовые пары робота (link1..link7) добавляются автоматически
tools:
none:
label: "Без захвата"
xacro: null
tip_link: "link_ee"
tcp_link: "link_ee"
collisions: []
patron:
label: "patron"
xacro: "$(find iiwa_description)/urdf/tools/patron.xacro"
tip_link: "patron"
tcp_link: "tcp"
collisions:
- [link1, patron, Never]
- [link2, patron, Never]
- [link3, patron, Never]
- [link4, patron, Never]
- [link5, patron, Never]
- [link6, patron, Never]
- [link7, patron, Adjacent]
- [link7, camera_holder, Adjacent]
- [link6, camera_holder, Never]
- [link5, camera_holder, Never]
- [link4, camera_holder, Never]
- [link3, camera_holder, Never]
- [link2, camera_holder, Never]
- [link1, camera_holder, Never]
- [base_link, camera_holder, Never]
- [camera_holder, patron, Adjacent]
- [camera_holder, camera_corner, Adjacent]
- [camera_corner, patron, Never]
- [camera_corner, link7, Never]
- [camera_corner, link6, Never]
- [camera_corner, link5, Never]
- [camera_corner, link4, Never]
- [camera_corner, link3, Never]
- [camera_corner, link2, Never]
- [camera_corner, link1, Never]
- [camera_corner, base_link, Never]
- [camera_corner, camera_hand, Adjacent]
- [camera_hand, camera_holder, Never]
- [camera_hand, patron, Never]
- [camera_hand, link7, Never]
- [camera_hand, link6, Never]
- [camera_hand, link5, Never]
- [camera_hand, link4, Never]
- [camera_hand, link3, Never]
- [camera_hand, link2, Never]
- [camera_hand, link1, Never]
- [camera_hand, base_link, Never]
src/iiwa_config/package.xml
+1 -1
View File
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>iiwa_config</name>
<version>2026.05.31</version>
<version>2026.5.31</version>
<description>Configuration files for cobot: MoveIt, ros2_control controllers, kinematics and general system settings</description>
<maintainer email="grabardm@ml-dev.ru">daniel</maintainer>
<license>Apache-2.0</license>
src/iiwa_controller/iiwa_hardware_interface_plugin.xml
+1 -1
View File
@@ -5,7 +5,7 @@
base_class_type="hardware_interface::SystemInterface">
<description>
ROS2 hardware interface для KUKA iiwa 7 через FRI (Fast Robot Interface).
Поддерживает режимы управления: position, torque.
Поддерживает управление в режиме position через FRI.
</description>
</class>
</library>
src/iiwa_controller/include/iiwa_controller/FRIClient.h
+1 -9
View File
@@ -11,11 +11,6 @@
namespace iiwa_controller
{
enum class CommandMode
{
POSITION,
TORQUE
};
// Снимок состояния робота захватывается атомарно за один lock в FRI-потоке
// и так же за один lock читается из read() в потоке управления.
@@ -40,7 +35,7 @@ public:
// joint_position_tau — постоянная времени экспоненциального фильтра позиций [с].
// Аналог joint_position_tau из lbr_fri_ros2_stack (по умолчанию 0.04 с = 40 мс).
// Сглаживает скачки команд перед отправкой роботу → убирает писк и стук суставов.
explicit FRIClient(CommandMode mode = CommandMode::POSITION, double joint_position_tau = 0.04);
explicit FRIClient(double joint_position_tau = 0.04);
~FRIClient() override = default;
// Коллбэки FRI SDK, вызываются из friThreadFunc через ClientApplication::step()
@@ -52,19 +47,16 @@ public:
// Потокобезопасное API для ros2_control, вызывается из read() и write()
void setTargetJointPositions(const std::array<double, N_JOINTS> & q);
void setTargetJointTorques(const std::array<double, N_JOINTS> & tau);
IIWAStateSnapshot getStateSnapshot() const;
bool isCommandingActive() const;
KUKA::FRI::ESessionState getSessionState() const;
private:
CommandMode cmd_mode_;
double joint_position_tau_;
std::atomic<KUKA::FRI::ESessionState> session_state_{KUKA::FRI::IDLE};
mutable std::mutex data_mutex_;
std::array<double, N_JOINTS> target_pos_{};
std::array<double, N_JOINTS> target_tau_{};
// Сглаженная позиция, которую реально отправляем роботу.
// Инициализируется IPO-позицией в waitForCommand(), чтобы не было скачка при старте.
std::array<double, N_JOINTS> filtered_pos_{};
src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp
-4
View File
@@ -59,7 +59,6 @@ private:
std::string robot_ip_;
int fri_port_{30200};
bool simulate_{false};
std::string cmd_mode_str_{"position"};
double joint_position_tau_{0.04};
// EMA-фильтр скорости: сглаживает одиночные выбросы конечных разностей.
// joint_velocity_tau = 0 отключает фильтр (raw finite difference).
@@ -82,9 +81,7 @@ private:
std::array<hardware_interface::StateInterface::SharedPtr, N_JOINTS> h_eff_;
std::array<hardware_interface::StateInterface::SharedPtr, N_JOINTS> h_ext_;
// Хэндлы командных интерфейсов
std::array<hardware_interface::CommandInterface::SharedPtr, N_JOINTS> h_cmd_pos_;
std::array<hardware_interface::CommandInterface::SharedPtr, N_JOINTS> h_cmd_eff_;
// Вычисление скорости: конечные разности + EMA-фильтр
std::array<double, N_JOINTS> prev_pos_{};
@@ -100,7 +97,6 @@ private:
rclcpp::Clock throttle_clock_{RCL_STEADY_TIME};
CommandMode parseCommandMode(const std::string & mode_str) const;
};
} // namespace iiwa_controller
src/iiwa_controller/package.xml
+1 -1
View File
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>iiwa_controller</name>
<version>2026.05.31</version>
<version>2026.5.31</version>
<description>Hardware interface for cobot: real-time joint control via FRI (Fast Robot Interface) within the ros2_control ecosystem</description>
<maintainer email="grabardm@ml-dev.ru">daniel</maintainer>
<license>Apache-2.0</license>
src/iiwa_controller/src/FRIClient.cpp
+2 -29
View File
@@ -20,11 +20,10 @@ static const char * friStateName(KUKA::FRI::ESessionState s)
}
}
FRIClient::FRIClient(CommandMode mode, double joint_position_tau)
: cmd_mode_(mode), joint_position_tau_(joint_position_tau)
FRIClient::FRIClient(double joint_position_tau)
: joint_position_tau_(joint_position_tau)
{
target_pos_.fill(0.0);
target_tau_.fill(0.0);
filtered_pos_.fill(0.0);
}
@@ -86,12 +85,6 @@ void FRIClient::waitForCommand()
std::memcpy(filtered_pos_.data(), snapshot_.ipo_pos.data(), N_JOINTS * sizeof(double));
robotCommand().setJointPosition(filtered_pos_.data());
if (cmd_mode_ == CommandMode::TORQUE) {
// Пока контроллер не синхронизирован, момент держим на нуле
target_tau_.fill(0.0);
robotCommand().setTorque(target_tau_.data());
}
}
// Вызывается в COMMANDING_ACTIVE, основной цикл управления
@@ -110,10 +103,6 @@ void FRIClient::command()
robotCommand().setJointPosition(filtered_pos_.data());
if (cmd_mode_ == CommandMode::TORQUE) {
robotCommand().setTorque(target_tau_.data());
}
// Захватываем снимок ПОСЛЕ EMA: measured_pos = filtered_pos_ = что робот только что получил.
captureCommandingData();
}
@@ -131,11 +120,6 @@ void FRIClient::onStateChange(
newState == KUKA::FRI::MONITORING_WAIT ||
newState == KUKA::FRI::MONITORING_READY)
{
std::lock_guard<std::mutex> lock(data_mutex_);
target_tau_.fill(0.0);
RCLCPP_WARN(
rclcpp::get_logger("FRIClient"),
"FRI сессия неактивна, моменты обнулены");
}
}
@@ -152,17 +136,6 @@ void FRIClient::setTargetJointPositions(const std::array<double, N_JOINTS> & q)
target_pos_ = q;
}
void FRIClient::setTargetJointTorques(const std::array<double, N_JOINTS> & tau)
{
for (const auto & v : tau) {
if (!std::isfinite(v)) {
return;
}
}
std::lock_guard<std::mutex> lock(data_mutex_);
target_tau_ = tau;
}
IIWAStateSnapshot FRIClient::getStateSnapshot() const
{
std::lock_guard<std::mutex> lock(data_mutex_);
src/iiwa_controller/src/IIWAHardwareInterface.cpp
+5 -18
View File
@@ -46,15 +46,13 @@ CallbackReturn IIWAHardwareInterface::on_init(
robot_ip_ = getParam(info, "robot_ip", "192.170.10.2");
fri_port_ = std::stoi(getParam(info, "fri_port", "30200"));
simulate_ = (getParam(info, "simulate", "false") == "true");
cmd_mode_str_ = getParam(info, "command_mode", "position");
joint_position_tau_ = std::stod(getParam(info, "joint_position_tau", "0.04"));
joint_velocity_tau_ = std::stod(getParam(info, "joint_velocity_tau", "0.01"));
RCLCPP_INFO(rclcpp::get_logger(LOG),
"on_init: ip=%s port=%d simulate=%s mode=%s pos_tau=%.3f vel_tau=%.3f",
"on_init: ip=%s port=%d simulate=%s pos_tau=%.3f vel_tau=%.3f",
robot_ip_.c_str(), fri_port_,
simulate_ ? "true" : "false",
cmd_mode_str_.c_str(),
joint_position_tau_,
joint_velocity_tau_);
@@ -99,7 +97,7 @@ CallbackReturn IIWAHardwareInterface::on_configure(const rclcpp_lifecycle::State
return CallbackReturn::SUCCESS;
}
fri_client_ = std::make_unique<FRIClient>(parseCommandMode(cmd_mode_str_), joint_position_tau_);
fri_client_ = std::make_unique<FRIClient>(joint_position_tau_);
// 100 мс таймаут recvfrom — поток корректно завершится после disconnect().
connection_ = std::make_unique<KUKA::FRI::UdpConnection>(100);
app_ = std::make_unique<KUKA::FRI::ClientApplication>(*connection_, *fri_client_);
@@ -132,10 +130,8 @@ CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State
h_eff_[i] = get_state_interface_handle(jn + "/" + hardware_interface::HW_IF_EFFORT);
h_ext_[i] = get_state_interface_handle(jn + "/external_torque");
h_cmd_pos_[i] = get_command_interface_handle(jn + "/" + hardware_interface::HW_IF_POSITION);
h_cmd_eff_[i] = get_command_interface_handle(jn + "/" + hardware_interface::HW_IF_EFFORT);
if (!h_pos_[i] || !h_vel_[i] || !h_eff_[i] || !h_ext_[i] ||
!h_cmd_pos_[i] || !h_cmd_eff_[i])
if (!h_pos_[i] || !h_vel_[i] || !h_eff_[i] || !h_ext_[i] || !h_cmd_pos_[i])
{
RCLCPP_FATAL(rclcpp::get_logger(LOG),
"Не удалось получить хэндл интерфейса для сустава '%s'. "
@@ -199,7 +195,7 @@ CallbackReturn IIWAHardwareInterface::on_deactivate(const rclcpp_lifecycle::Stat
for (size_t i = 0; i < N_JOINTS; ++i) {
h_pos_[i] = h_vel_[i] = h_eff_[i] = h_ext_[i] = nullptr;
h_cmd_pos_[i] = h_cmd_eff_[i] = nullptr;
h_cmd_pos_[i] = nullptr;
}
velocity_initialized_ = false;
@@ -342,23 +338,14 @@ hardware_interface::return_type IIWAHardwareInterface::write(
return hardware_interface::return_type::OK;
}
std::array<double, N_JOINTS> pos_cmd{}, tau_cmd{};
std::array<double, N_JOINTS> pos_cmd{};
for (size_t i = 0; i < N_JOINTS; ++i) {
get_command(h_cmd_pos_[i], pos_cmd[i], false);
get_command(h_cmd_eff_[i], tau_cmd[i], false);
}
fri_client_->setTargetJointPositions(pos_cmd);
fri_client_->setTargetJointTorques(tau_cmd);
return hardware_interface::return_type::OK;
}
// ── parseCommandMode ───────────────────────────────────────────────────────────
CommandMode IIWAHardwareInterface::parseCommandMode(const std::string & mode_str) const
{
return (mode_str == "torque") ? CommandMode::TORQUE : CommandMode::POSITION;
}
} // namespace iiwa_controller
src/iiwa_description/package.xml
+1 -1
View File
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>iiwa_description</name>
<version>2026.05.31</version>
<version>2026.5.31</version>
<description>URDF/XACRO robot description for cobot and Webots world configuration</description>
<maintainer email="grabardm@ml-dev.ru">daniel</maintainer>
<license>Apache-2.0</license>
src/iiwa_description/urdf/gripper/___gripper.urdf
-176
View File
@@ -1,176 +0,0 @@
<?xml version="1.0"?>
<robot name="gripper">
<link name="base_frame">
<visual>
<origin xyz="-21 -21 6" rpy="0 0 0"/>
<geometry>
<mesh filename="/home/daniel/dev/kuka_iiwa7_ros2/src/iiwa_description/resource/meshes/gripper/base_frame.stl" />
</geometry>
<material name="black_plastic">
<color rgba="0.05 0.05 0.05 1.0"/>
</material>
</visual>
<collision>
<origin xyz="-21 -21 6" rpy="0 0 0"/>
<geometry>
<mesh filename="/home/daniel/dev/kuka_iiwa7_ros2/src/iiwa_description/resource/meshes/gripper/base_frame.stl" />
</geometry>
</collision>
<inertial>
<mass value="0.0"/>
<origin xyz=" 0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
</inertial>
</link>
<link name="plate">
<visual>
<geometry>
<mesh filename="/home/daniel/dev/kuka_iiwa7_ros2/src/iiwa_description/resource/meshes/gripper/plate.stl"/>
</geometry>
<material name="alum_plastic">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
<mesh filename="/home/daniel/dev/kuka_iiwa7_ros2/src/iiwa_description/resource/meshes/gripper/plate.stl"/>
</geometry>
</collision>
<inertial>
<mass value="0.0"/>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
</inertial>
</link>
<link name="screw">
<inertial>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<mass value="0.0"/>
<inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
</inertial>
<visual name="">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<mesh filename="/home/daniel/dev/kuka_iiwa7_ros2/src/iiwa_description/resource/meshes/gripper/screw.stl"/>
</geometry>
<material name="alum_plastic">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
</link>
<link name="finger1">
<inertial>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<mass value="0.0"/>
<inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
</inertial>
<visual name="">
<origin xyz="9 -25 5" rpy="0.0 -1.57 0.0"/>
<geometry>
<mesh filename="/home/daniel/dev/kuka_iiwa7_ros2/src/iiwa_description/resource/meshes/gripper/finger_plates.stl"/>
</geometry>
<material name="">
<color rgba="0.0 0.8 1.0 1.0"/>
</material>
</visual>
<collision>
<origin xyz="9 -25 5" rpy="0.0 -1.57 0.0"/>
<geometry>
<mesh filename="/home/daniel/dev/kuka_iiwa7_ros2/src/iiwa_description/resource/meshes/gripper/finger_plates.stl"/>
</geometry>
</collision>
</link>
<link name="finger2">
<inertial>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<mass value="0.0"/>
<inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
</inertial>
<visual name="">
<origin xyz="9 -25 5" rpy="0.0 -1.57 0.0"/>
<geometry>
<mesh filename="/home/daniel/dev/kuka_iiwa7_ros2/src/iiwa_description/resource/meshes/gripper/finger_plates.stl"/>
</geometry>
<material name="">
<color rgba="0.0 0.8 1.0 1.0"/>
</material>
</visual>
<collision>
<origin xyz="9 -25 5" rpy="0.0 -1.57 0.0"/>
<geometry>
<mesh filename="/home/daniel/dev/kuka_iiwa7_ros2/src/iiwa_description/resource/meshes/gripper/finger_plates.stl"/>
</geometry>
</collision>
</link>
<link name="finger3">
<inertial>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<mass value="0.0"/>
<inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
</inertial>
<visual name="">
<origin xyz="9 -25 5" rpy="0.0 -1.57 0.0"/>
<geometry>
<mesh filename="/home/daniel/dev/kuka_iiwa7_ros2/src/iiwa_description/resource/meshes/gripper/finger_plates.stl"/>
</geometry>
<material name="">
<color rgba="0.0 0.8 1.0 1.0"/>
</material>
</visual>
<collision>
<origin xyz="9 -25 5" rpy="0.0 -1.57 0.0"/>
<geometry>
<mesh filename="/home/daniel/dev/kuka_iiwa7_ros2/src/iiwa_description/resource/meshes/gripper/finger_plates.stl"/>
</geometry>
</collision>
</link>
<joint name="base" type="fixed">
<origin xyz="0.0 0.0 61.3" rpy="0.0 0.0 0.0"/>
<parent link="base_frame"/>
<child link="plate"/>
</joint>
<joint name="base_screw" type="prismatic">
<origin xyz="0.0 0.0 72" rpy="0.0 0.0 0.0"/>
<parent link="base_frame"/>
<child link="screw"/>
<axis xyz="0.0 0.0 1"/>
<limit lower="-7" upper="0.0" effort="0.0" velocity="0.0"/>
</joint>
<joint name="finger1_joint" type="revolute">
<origin xyz="31.5 -18.5 28" rpy="0.0 0.0 -2.12"/>
<parent link="plate"/>
<child link="finger1"/>
<axis xyz="1 0.0 0"/>
<limit lower="-0.2" upper="0.07" effort="0.0" velocity="0.0"/>
<mimic joint="base_screw" multiplier="-0.05" offset="-0.2"/>
</joint>
<joint name="finger2_joint" type="revolute">
<origin xyz="-31.5 -18.5 28" rpy="0.0 0.0 2.12"/>
<parent link="plate"/>
<child link="finger2"/>
<axis xyz="1 0.0 0"/>
<limit lower="-0.2" upper="0.07" effort="0.0" velocity="0.0"/>
<mimic joint="base_screw" multiplier="-0.05" offset="-0.2"/>
</joint>
<joint name="finger3_joint" type="revolute">
<origin xyz="0 37 28" rpy="0.0 0.0 0.0"/>
<parent link="plate"/>
<child link="finger3"/>
<axis xyz="1 0.0 0"/>
<limit lower="-0.2" upper="0.07" effort="0.0" velocity="0.0"/>
<mimic joint="base_screw" multiplier="-0.05" offset="-0.2"/>
</joint>
</robot>
src/iiwa_description/urdf/gripper/gripper.xacro
-13
View File
@@ -1,13 +0,0 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" xacro:version="1.0">
<xacro:include filename="$(find iiwa_description)/urdf/gripper/gripper_links.xacro"/>
<xacro:include filename="$(find iiwa_description)/urdf/gripper/gripper_joints.xacro"/>
<joint name="gripper_attach_joint" type="fixed">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<parent link="link_ee"/>
<child link="base_frame"/>
</joint>
</robot>
src/iiwa_description/urdf/gripper/gripper_joints.xacro
-41
View File
@@ -1,41 +0,0 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" xacro:version="1.0">
<xacro:include filename="$(find iiwa_description)/urdf/gripper/gripper_macros.xacro"/>
<xacro:arg name="simulate" default="false"/>
<joint name="base_joint" type="fixed">
<origin xyz="0.0 0.0 0.0613" rpy="0.0 0.0 0.0"/>
<parent link="base_frame"/>
<child link="plate"/>
</joint>
<xacro:prismatic_joint jname="base_screw"
parent="base_frame" child="screw"
xyz="0.0 0.0 0.072" rpy="0.0 0.0 0.0"
axis="0.0 0.0 1"
lower="-0.007" upper="0.0"
effort="50.0" velocity="0.05"/>
<xacro:mimic_revolute_joint jname="finger1_joint"
parent="plate" child="finger1"
xyz="0.0315 -0.0185 0.028" rpy="0.0 0.0 -2.12"
axis="1 0.0 0"
lower="-0.2" upper="0.07" effort="5.0" velocity="1.0" damping="0.0"
mimic_joint="base_screw" multiplier="-50" mimic_offset="-0.2"/>
<xacro:mimic_revolute_joint jname="finger2_joint"
parent="plate" child="finger2"
xyz="-0.0315 -0.0185 0.028" rpy="0.0 0.0 2.12"
axis="1 0.0 0"
lower="-0.2" upper="0.07" effort="5.0" velocity="1.0" damping="0.0"
mimic_joint="base_screw" multiplier="-50" mimic_offset="-0.2"/>
<xacro:mimic_revolute_joint jname="finger3_joint"
parent="plate" child="finger3"
xyz="0 0.037 0.028" rpy="0.0 0.0 0.0"
axis="1 0.0 0"
lower="-0.2" upper="0.07" effort="5.0" velocity="1.0" damping="0.0"
mimic_joint="base_screw" multiplier="-50" mimic_offset="-0.2"/>
</robot>
src/iiwa_description/urdf/gripper/gripper_links.xacro
-67
View File
@@ -1,67 +0,0 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" xacro:version="1.0">
<xacro:include filename="$(find iiwa_description)/urdf/gripper/gripper_macros.xacro"/>
<xacro:include filename="$(find iiwa_description)/urdf/gripper/gripper_meshes.xacro"/>
<xacro:property name="gripper_dark_r" value="0.05"/>
<xacro:property name="gripper_dark_g" value="0.05"/>
<xacro:property name="gripper_dark_b" value="0.05"/>
<xacro:property name="gripper_alum_r" value="0.7"/>
<xacro:property name="gripper_alum_g" value="0.7"/>
<xacro:property name="gripper_alum_b" value="0.7"/>
<xacro:property name="gripper_finger_r" value="0.0"/>
<xacro:property name="gripper_finger_g" value="0.8"/>
<xacro:property name="gripper_finger_b" value="1.0"/>
<xacro:gripper_link name="base_frame"
mass="0.3" xyz_iner="0 0 0.035"
ixx="0.00024" ixy="0.0" ixz="0.0"
iyy="0.00024" iyz="0.0" izz="0.00016"
mesh="${mesh_gripper_base_frame}"
visual_xyz="-0.021 -0.021 0.006" visual_rpy="0 0 0"
r="${gripper_dark_r}" g="${gripper_dark_g}" b="${gripper_dark_b}" a="1.0"/>
<xacro:gripper_link name="plate"
mass="0.15" xyz_iner="0 0 0"
ixx="0.00012" ixy="0.0" ixz="0.0"
iyy="0.00012" iyz="0.0" izz="0.00018"
mesh="${mesh_gripper_plate}"
visual_xyz="0 0 0" visual_rpy="0 0 0"
r="${gripper_alum_r}" g="${gripper_alum_g}" b="${gripper_alum_b}" a="1.0"/>
<xacro:gripper_link name="screw"
mass="0.03" xyz_iner="0 0 0"
ixx="0.000024" ixy="0.0" ixz="0.0"
iyy="0.000024" iyz="0.0" izz="0.000012"
mesh="${mesh_gripper_screw}"
visual_xyz="0 0 0" visual_rpy="0 0 0"
r="${gripper_alum_r}" g="${gripper_alum_g}" b="${gripper_alum_b}" a="1.0"/>
<xacro:gripper_link name="finger1"
mass="0.015" xyz_iner="0 0 0"
ixx="0.000012" ixy="0.0" ixz="0.0"
iyy="0.000012" iyz="0.0" izz="0.000004"
mesh="${mesh_gripper_finger}"
visual_xyz="0.009 -0.025 0.005" visual_rpy="0 -1.57 0"
r="${gripper_finger_r}" g="${gripper_finger_g}" b="${gripper_finger_b}" a="1.0"/>
<xacro:gripper_link name="finger2"
mass="0.015" xyz_iner="0 0 0"
ixx="0.000012" ixy="0.0" ixz="0.0"
iyy="0.000012" iyz="0.0" izz="0.000004"
mesh="${mesh_gripper_finger}"
visual_xyz="0.009 -0.025 0.005" visual_rpy="0 -1.57 0"
r="${gripper_finger_r}" g="${gripper_finger_g}" b="${gripper_finger_b}" a="1.0"/>
<xacro:gripper_link name="finger3"
mass="0.015" xyz_iner="0 0 0"
ixx="0.000012" ixy="0.0" ixz="0.0"
iyy="0.000012" iyz="0.0" izz="0.000004"
mesh="${mesh_gripper_finger}"
visual_xyz="0.009 -0.025 0.005" visual_rpy="0 -1.57 0"
r="${gripper_finger_r}" g="${gripper_finger_g}" b="${gripper_finger_b}" a="1.0"/>
</robot>
src/iiwa_description/urdf/gripper/gripper_macros.xacro
-65
View File
@@ -1,65 +0,0 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" xacro:version="1.0">
<xacro:macro name="gripper_link"
params="name mass
ixx ixy ixz iyy iyz izz xyz_iner
mesh visual_xyz visual_rpy
r g b a">
<link name="${name}">
<inertial>
<origin xyz="${xyz_iner}" rpy="0 0 0"/>
<mass value="${mass}"/>
<inertia ixx="${ixx}" ixy="${ixy}" ixz="${ixz}"
iyy="${iyy}" iyz="${iyz}" izz="${izz}"/>
</inertial>
<visual>
<origin xyz="${visual_xyz}" rpy="${visual_rpy}"/>
<geometry>
<mesh filename="${mesh}" scale="0.001 0.001 0.001"/>
</geometry>
<material name="${name}_material">
<color rgba="${r} ${g} ${b} ${a}"/>
</material>
</visual>
<collision>
<origin xyz="${visual_xyz}" rpy="${visual_rpy}"/>
<geometry>
<mesh filename="${mesh}" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
</xacro:macro>
<xacro:macro name="prismatic_joint"
params="jname parent child xyz rpy axis lower upper effort velocity">
<joint name="${jname}" type="prismatic">
<origin xyz="${xyz}" rpy="${rpy}"/>
<parent link="${parent}"/>
<child link="${child}"/>
<axis xyz="${axis}"/>
<limit lower="${lower}" upper="${upper}"
effort="${effort}" velocity="${velocity}"/>
<dynamics damping="0.5" friction="0.1"/>
</joint>
</xacro:macro>
<xacro:macro name="mimic_revolute_joint"
params="jname parent child xyz rpy axis
lower upper effort velocity damping
mimic_joint multiplier mimic_offset">
<joint name="${jname}" type="revolute">
<origin xyz="${xyz}" rpy="${rpy}"/>
<parent link="${parent}"/>
<child link="${child}"/>
<axis xyz="${axis}"/>
<limit lower="${lower}" upper="${upper}"
effort="${effort}" velocity="${velocity}"/>
<dynamics damping="${damping}" friction="0.05"/>
<mimic joint="${mimic_joint}"
multiplier="${multiplier}"
offset="${mimic_offset}"/>
</joint>
</xacro:macro>
</robot>
src/iiwa_description/urdf/gripper/gripper_meshes.xacro
-12
View File
@@ -1,12 +0,0 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" xacro:version="1.0">
<xacro:property name="gripper_pkg"
value="package://iiwa_description/resource/meshes/gripper"/>
<xacro:property name="mesh_gripper_base_frame" value="${gripper_pkg}/base_frame.stl"/>
<xacro:property name="mesh_gripper_plate" value="${gripper_pkg}/plate.stl"/>
<xacro:property name="mesh_gripper_screw" value="${gripper_pkg}/screw.stl"/>
<xacro:property name="mesh_gripper_finger" value="${gripper_pkg}/finger_plates.stl"/>
</robot>
src/iiwa_description/urdf/iiwa7.urdf.xacro
+8 -31
View File
@@ -6,7 +6,6 @@
<xacro:arg name="simulate" default="false"/>
<xacro:arg name="robot_ip" default="192.170.10.2"/>
<xacro:arg name="fri_port" default="30200"/>
<xacro:arg name="command_mode" default="position"/>
<xacro:arg name="joint_position_tau" default="0.04"/>
<xacro:arg name="joint_velocity_tau" default="0.01"/>
@@ -18,7 +17,7 @@
<xacro:include filename="$(find iiwa_description)/urdf/links.xacro"/>
<xacro:include filename="$(find iiwa_description)/urdf/joints.xacro"/>
<xacro:include filename="$(find iiwa_description)/urdf/tools/patron.xacro"/>
<xacro:include filename="$(find iiwa_description)/urdf/tools/tool_active.xacro"/>
<xacro:if value="$(arg simulate)">
@@ -147,7 +146,6 @@
<param name="robot_ip">$(arg robot_ip)</param>
<param name="fri_port">$(arg fri_port)</param>
<param name="simulate">false</param>
<param name="command_mode">$(arg command_mode)</param>
<param name="joint_position_tau">$(arg joint_position_tau)</param>
<param name="joint_velocity_tau">$(arg joint_velocity_tau)</param>
</hardware>
@@ -157,10 +155,7 @@
<param name="min">-2.97</param>
<param name="max"> 2.97</param>
</command_interface>
<command_interface name="effort">
<param name="min">-200</param>
<param name="max"> 200</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">${initial_positions['joint1']}</param>
</state_interface>
@@ -173,10 +168,7 @@
<param name="min">-2.09</param>
<param name="max"> 2.09</param>
</command_interface>
<command_interface name="effort">
<param name="min">-200</param>
<param name="max"> 200</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">${initial_positions['joint2']}</param>
</state_interface>
@@ -189,10 +181,7 @@
<param name="min">-2.97</param>
<param name="max"> 2.97</param>
</command_interface>
<command_interface name="effort">
<param name="min">-200</param>
<param name="max"> 200</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">${initial_positions['joint3']}</param>
</state_interface>
@@ -205,10 +194,7 @@
<param name="min">-2.09</param>
<param name="max"> 2.09</param>
</command_interface>
<command_interface name="effort">
<param name="min">-200</param>
<param name="max"> 200</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">${initial_positions['joint4']}</param>
</state_interface>
@@ -221,10 +207,7 @@
<param name="min">-2.97</param>
<param name="max"> 2.97</param>
</command_interface>
<command_interface name="effort">
<param name="min">-200</param>
<param name="max"> 200</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">${initial_positions['joint5']}</param>
</state_interface>
@@ -237,10 +220,7 @@
<param name="min">-2.09</param>
<param name="max"> 2.09</param>
</command_interface>
<command_interface name="effort">
<param name="min">-200</param>
<param name="max"> 200</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">${initial_positions['joint6']}</param>
</state_interface>
@@ -253,10 +233,7 @@
<param name="min">-3.05</param>
<param name="max"> 3.05</param>
</command_interface>
<command_interface name="effort">
<param name="min">-200</param>
<param name="max"> 200</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">${initial_positions['joint7']}</param>
</state_interface>
src/iiwa_description/urdf/joints.xacro
+3 -3
View File
@@ -11,7 +11,7 @@
<!-- joint1: ±170° hard, ±165° soft, k_velocity=10 -->
<xacro:revolute_joint jname="joint1" parent="base_link" child="link1"
xyz="0 0 0.3375" rpy="0 0 0" axis="0 0 -1"
xyz="0 0 0.3375" rpy="0 0 0" axis="0 0 1"
lower="-2.97" upper="2.97" effort="200"
velocity="1.71" damping="10.0" friction="0.1"
soft_lower="-2.8827" soft_upper="2.8827" k_velocity="10"/>
@@ -27,7 +27,7 @@
<!-- joint3: ±170° hard, ±165° soft -->
<xacro:revolute_joint jname="joint3" parent="link2" child="link3"
xyz="0 -0.3993 0" rpy="1.5708 0 0" axis="0 0 -1"
xyz="0 -0.3993 0" rpy="1.5708 0 0" axis="0 0 1"
lower="-2.97" upper="2.97" effort="200"
velocity="1.75" damping="10.0" friction="0.1"
soft_lower="-2.8827" soft_upper="2.8827" k_velocity="10"/>
@@ -43,7 +43,7 @@
<!-- joint5: ±170° hard, ±165° soft -->
<xacro:revolute_joint jname="joint5" parent="link4" child="link5"
xyz="0 -0.3993 0" rpy="1.5708 0 0" axis="0 0 -1"
xyz="0 -0.3993 0" rpy="1.5708 0 0" axis="0 0 1"
lower="-2.97" upper="2.97" effort="200"
velocity="2.44" damping="10.0" friction="0.1"
soft_lower="-2.8827" soft_upper="2.8827" k_velocity="10"/>
src/iiwa_description/urdf/tools/tool_active.xacro
+5
View File
@@ -0,0 +1,5 @@
<?xml version="1.0"?>
<!-- AUTO-GENERATED by cobot robot-setup — do not edit manually -->
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="tool_active">
<xacro:include filename="$(find iiwa_description)/urdf/tools/patron.xacro"/>
</robot>
src/iiwa_msgs/package.xml
+1 -1
View File
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>iiwa_msgs</name>
<version>2026.05.31</version>
<version>2026.5.31</version>
<description>ROS 2 interfaces for cobot: action messages for joint-space and Cartesian motion, services for transitioning to named poses defined in SRDF</description>
<maintainer email="grabardm@ml-dev.ru">daniel</maintainer>
<license>Apache-2.0</license>
src/iiwa_planning/CMakeLists.txt
+11
View File
@@ -43,5 +43,16 @@ install(PROGRAMS
RENAME move_to_pose_server
)
install(PROGRAMS
scripts/motion_sequence_runner.py
DESTINATION lib/${PROJECT_NAME}
RENAME motion_sequence_runner
)
install(FILES
scripts/motion_sequence_config.json
DESTINATION share/${PROJECT_NAME}
)
ament_package()
src/iiwa_planning/package.xml
+1 -1
View File
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>iiwa_planning</name>
<version>2026.05.31</version>
<version>2026.5.31</version>
<description>Motion planning for cobot: C++ and Python nodes built on MoveIt 2 with OMPL, Pilz Industrial Motion and moveit_py support</description>
<maintainer email="grabardm@ml-dev.ru">daniel</maintainer>
<license>Apache-2.0</license>
src/iiwa_planning/scripts/motion_sequence_config.json
+42
View File
@@ -0,0 +1,42 @@
{
"home": {
"joints": [0.0, 0.0, 0.0, -1.57, 0.0, 1.57, 0.0],
"speed": 0.1
},
"waypoints": [
{
"x": 0.6,
"y": 0.1,
"z": 0.55,
"a": 3.14,
"b": 0.3142,
"c": 2.7916,
"speed": 0.4,
"planner": "ptp"
},
{
"x": 0.6,
"y": -0.1,
"z": 0.55,
"a": 3.14,
"b": 0.3142,
"c": 2.7916,
"speed": 0.3,
"planner": "lin"
},
{
"joints": [0.5, 0.3, 0.0, -1.2, 0.0, 1.4, 0.0],
"speed": 0.2
},
{
"x": 0.58,
"y": 0.0,
"z": 0.5,
"a": 3.14,
"b": 0.3665,
"c": 2.8791,
"speed": 0.3,
"planner": "lin"
}
]
}
src/iiwa_utils/iiwa_utils/test_motion_sequence.py → src/iiwa_planning/scripts/motion_sequence_runner.py
+73 -45
View File
@@ -1,4 +1,16 @@
#!/usr/bin/env python3
"""
Запуск заданной последовательности движений манипулятора iiwa.
Каждая точка в конфиге может быть либо суставной (type: joints),
либо декартовой (type: pose). Тип определяется автоматически по наличию
ключа "joints" или координат "x/y/z".
Запуск:
ros2 run iiwa_planning motion_sequence_runner \
--ros-args -p config_path:=/path/to/config.json -p n_iterations:=2
"""
import json
import shutil
import threading
@@ -18,37 +30,45 @@ from rosidl_runtime_py.utilities import get_message
from iiwa_msgs.action import MoveToJoints, MoveToPose
class IiwaTestRunner(Node):
def _is_joints_waypoint(wp: dict) -> bool:
return "joints" in wp
class MotionSequenceRunner(Node):
def __init__(self):
super().__init__('iiwa_test_runner')
super().__init__('motion_sequence_runner')
self.declare_parameter('n_iterations', 3)
self.declare_parameter('bag_path', '')
self.declare_parameter('config_path', '')
self.declare_parameter('topics', [''])
self.declare_parameter("delay_between_iterations", 5.0)
self.declare_parameter('delay_between_iterations', 5.0)
self.declare_parameter('joints_action', 'cobot/move_to_joints')
self.declare_parameter('pose_action', 'cobot/move_to_pose')
self._n_iter = self.get_parameter('n_iterations').value
self._delay_between_iterations = self.get_parameter("delay_between_iterations").value
self._delay = self.get_parameter('delay_between_iterations').value
self._bag_path = self.get_parameter('bag_path').value
topics_param = self.get_parameter('topics').value
# [''] means not specified — record all topics
self._topics_param: list[str] = [t for t in topics_param if t]
config_path = self.get_parameter('config_path').value
joints_action = self.get_parameter('joints_action').value
pose_action = self.get_parameter('pose_action').value
cfg = self._load_config(config_path)
self._home_joints: list[float] = cfg['home_joints']
self._poses: list[dict] = cfg['poses']
self._home: dict = cfg['home']
self._waypoints: list[dict] = cfg['waypoints']
self._cb_group = ReentrantCallbackGroup()
self._joints_client = ActionClient(
self, MoveToJoints, '/iiwa/move_to_joints',
self, MoveToJoints, joints_action,
callback_group=self._cb_group,
)
self._pose_client = ActionClient(
self, MoveToPose, '/iiwa/move_to_pose',
self, MoveToPose, pose_action,
callback_group=self._cb_group,
)
self.get_logger().info(f'joints_action={joints_action} pose_action={pose_action}')
self._writer: rosbag2_py.SequentialWriter | None = None
self._registered_topics: set[str] = set()
@@ -60,14 +80,17 @@ class IiwaTestRunner(Node):
else:
self.get_logger().info('bag_path not set — recording disabled')
# Config
def _load_config(self, config_path: str) -> dict:
path = Path(config_path) if config_path else Path(__file__).parent / 'motion_config.json'
path = (
Path(config_path) if config_path
else Path(__file__).parent / 'motion_sequence_config.json'
)
self.get_logger().info(f'Loading config from {path}')
with open(path) as f:
return json.load(f)
# Bag files
def _init_bag(self):
bag_dir = Path(self._bag_path)
if bag_dir.exists():
@@ -90,7 +113,9 @@ class IiwaTestRunner(Node):
topics = self._topics_param if self._topics_param else list(available.keys())
if not self._topics_param:
self.get_logger().info(f'topics not set — recording all {len(topics)} available topics')
self.get_logger().info(
f'topics not set — recording all {len(topics)} available topics'
)
for topic in topics:
if topic not in available:
@@ -132,26 +157,25 @@ class IiwaTestRunner(Node):
if self._writer:
del self._writer
self._writer = None
self.get_logger().info(f'[BAG] closed → {self._bag_path}')
self.get_logger().info(f'Bag closed → {self._bag_path}')
# Action helpers
def _send_joints_goal(self, joints: list[float], speed: float = 0.1) -> bool:
def _send_joints_goal(self, wp: dict) -> bool:
goal = MoveToJoints.Goal()
goal.joints = joints
goal.speed = speed
goal.joints = wp['joints']
goal.speed = float(wp.get('speed', 0.1))
self.get_logger().info(f'[MOVE] joints → {joints}')
self.get_logger().info(f'[JOINTS] → {goal.joints}')
if not self._joints_client.wait_for_server(timeout_sec=10.0):
self.get_logger().error('MoveToJoints server unavailable')
return False
done = threading.Event()
success_holder: list[bool] = [False]
result_holder: list[bool] = [False]
def _on_result(future):
res = future.result().result
success_holder[0] = res.success
self.get_logger().info(f'[MOVE] joints done: success={res.success}')
result_holder[0] = future.result().result.success
self.get_logger().info(f'[JOINTS] done: success={result_holder[0]}')
done.set()
def _on_goal(future):
@@ -164,30 +188,29 @@ class IiwaTestRunner(Node):
self._joints_client.send_goal_async(goal).add_done_callback(_on_goal)
done.wait()
return success_holder[0]
return result_holder[0]
def _send_pose_goal(self, *, x, y, z, a, b, c,
speed: float = 0.1,
planner: str = 'lin',
id: int = None) -> bool:
def _send_pose_goal(self, wp: dict, idx: int | None = None) -> bool:
goal = MoveToPose.Goal()
goal.x, goal.y, goal.z = x, y, z
goal.a, goal.b, goal.c = a, b, c
goal.speed = speed
goal.planner = planner
goal.x, goal.y, goal.z = wp['x'], wp['y'], wp['z']
goal.a, goal.b, goal.c = wp['a'], wp['b'], wp['c']
goal.speed = float(wp.get('speed', 0.1))
goal.planner = wp.get('planner', 'lin')
self.get_logger().info(f'[MOVE] {id if id is not None else ""} pose → x={x} y={y} z={z} planner={planner}')
label = f'#{idx} ' if idx is not None else ''
self.get_logger().info(
f'[POSE] {label}→ x={goal.x} y={goal.y} z={goal.z} planner={goal.planner}'
)
if not self._pose_client.wait_for_server(timeout_sec=10.0):
self.get_logger().error('MoveToPose server unavailable')
return False
done = threading.Event()
success_holder: list[bool] = [False]
result_holder: list[bool] = [False]
def _on_result(future):
res = future.result().result
success_holder[0] = res.success
self.get_logger().info(f'[MOVE] pose done: success={res.success}')
result_holder[0] = future.result().result.success
self.get_logger().info(f'[POSE] done: success={result_holder[0]}')
done.set()
def _on_goal(future):
@@ -200,21 +223,26 @@ class IiwaTestRunner(Node):
self._pose_client.send_goal_async(goal).add_done_callback(_on_goal)
done.wait()
return success_holder[0]
return result_holder[0]
def _send_waypoint(self, wp: dict, idx: int | None = None) -> bool:
if _is_joints_waypoint(wp):
return self._send_joints_goal(wp)
return self._send_pose_goal(wp, idx=idx)
# Main sequence
def run(self, done_event: threading.Event):
try:
for i in range(self._n_iter):
self.get_logger().info(f'======= Iteration {i + 1}/{self._n_iter} =======')
self._send_joints_goal(self._home_joints)
self._send_waypoint(self._home)
for i, pose in enumerate(self._poses):
self._send_pose_goal(id=i, **pose)
for idx, wp in enumerate(self._waypoints):
self._send_waypoint(wp, idx=idx)
self._send_joints_goal(self._home_joints)
time.sleep(self._delay_between_iterations)
self._send_waypoint(self._home)
time.sleep(self._delay)
self.get_logger().info('======= Sequence complete =======')
finally:
@@ -224,7 +252,7 @@ class IiwaTestRunner(Node):
def main():
rclpy.init()
node = IiwaTestRunner()
node = MotionSequenceRunner()
executor = MultiThreadedExecutor()
executor.add_node(node)
src/iiwa_planning/scripts/move_to_pose_server.py
+8 -8
View File
@@ -38,10 +38,10 @@ class IiwaMotionServer(Node):
"""Сервер управления движением манипулятора iiwa.
Предоставляет:
- action iiwa/move_to_pose — перемещение в декартову позу
- action iiwa/move_to_joints — перемещение по суставным координатам
- service iiwa/move_to_named — перемещение в именованную позу из SRDF
- service iiwa/stop — немедленная остановка движения
- action cobot/move_to_pose — перемещение в декартову позу
- action cobot/move_to_joints — перемещение по суставным координатам
- service cobot/move_to_named — перемещение в именованную позу из SRDF
- service cobot/stop — немедленная остановка движения
"""
def __init__(self):
@@ -75,19 +75,19 @@ class IiwaMotionServer(Node):
cb = ReentrantCallbackGroup()
ActionServer(
self, MoveToPose, "iiwa/move_to_pose", self._execute_pose,
self, MoveToPose, "cobot/move_to_pose", self._execute_pose,
callback_group=cb,
goal_callback=lambda _: GoalResponse.ACCEPT,
cancel_callback=lambda _: CancelResponse.ACCEPT,
)
ActionServer(
self, MoveToJoints, "iiwa/move_to_joints", self._execute_joints,
self, MoveToJoints, "cobot/move_to_joints", self._execute_joints,
callback_group=cb,
goal_callback=lambda _: GoalResponse.ACCEPT,
cancel_callback=lambda _: CancelResponse.ACCEPT,
)
self.create_service(MoveToNamedPose, "iiwa/move_to_named", self._handle_named, callback_group=cb)
self.create_service(Trigger, "iiwa/stop", self._handle_stop, callback_group=cb)
self.create_service(MoveToNamedPose, "cobot/move_to_named", self._handle_named, callback_group=cb)
self.create_service(Trigger, "cobot/stop", self._handle_stop, callback_group=cb)
def _make_plan_params(self, pipeline: str, planner_id: str, plan_time: float, velocity_scale: float, accel_scale: float | None = None) -> PlanRequestParameters:
params = PlanRequestParameters(self._moveit, self._planning_group)
src/iiwa_sunrise/src/RobotPowerControl.java
Executable
+123
View File
@@ -0,0 +1,123 @@
package backgroundTask;
import java.io.IOException;
import javax.inject.Inject;
import com.kuka.common.ThreadUtil;
import com.kuka.roboticsAPI.applicationModel.tasks.RoboticsAPIBackgroundTask;
import com.kuka.roboticsAPI.controllerModel.Controller;
import com.kuka.roboticsAPI.uiModel.userKeys.IUserKey;
import com.kuka.roboticsAPI.uiModel.userKeys.IUserKeyBar;
import com.kuka.roboticsAPI.uiModel.userKeys.IUserKeyListener;
import com.kuka.roboticsAPI.uiModel.userKeys.UserKeyAlignment;
import com.kuka.roboticsAPI.uiModel.userKeys.UserKeyEvent;
import com.kuka.roboticsAPI.uiModel.userKeys.UserKeyLED;
import com.kuka.roboticsAPI.uiModel.userKeys.UserKeyLEDSize;
import com.kuka.task.ITaskLogger;
/**
* Background task: управление питанием контроллера KUKA через кнопки SmartPAD.
*
* Панель "System" содержит две safety-critical кнопки:
* [0] REBOOT — запускает D:\Programmes\reboot.cmd
* [1] SHUTDOWN — запускает D:\Programmes\shutdown.cmd
*
* Защита реализована через setCriticalText():
* - 1-е нажатие, smartHMI показывает окно "Critical operation" с текстом предупреждения
* - Кнопка деактивируется на ~5 с
* - 2-е нажатие в течение 5 с, onKeyEvent(KeyDown) выполняется и скрипт запускается
* - Нет нажатия / тап вне окна, окно закрывается, кнопка сбрасывается
*/
public class RobotPowerControl extends RoboticsAPIBackgroundTask {
@Inject
private Controller kUKA_Sunrise_Cabinet;
@Inject
private ITaskLogger logger;
private static final String REBOOT_SCRIPT = "D:\\Programme\\reboot.cmd";
private static final String SHUTDOWN_SCRIPT = "D:\\Programme\\shutdown.cmd";
@Override
public void initialize() {
// инициализация не требуется
}
@Override
public void run() {
IUserKeyBar powerBar = getApplicationUI().createUserKeyBar("System");
// Кнопка REBOOT
IUserKeyListener rebootListener = new IUserKeyListener() {
@Override
public void onKeyEvent(IUserKey key, UserKeyEvent event) {
if (event == UserKeyEvent.KeyDown) {
// Подтверждение через setCriticalText уже получено
key.setLED(UserKeyAlignment.BottomMiddle, UserKeyLED.Yellow, UserKeyLEDSize.Small);
boolean success = executeScript(REBOOT_SCRIPT);
if (!success) {
key.setLED(UserKeyAlignment.BottomMiddle, UserKeyLED.Red, UserKeyLEDSize.Small);
ThreadUtil.milliSleep(2000);
}
key.setLED(UserKeyAlignment.BottomMiddle, UserKeyLED.Grey, UserKeyLEDSize.Small);
}
}
};
//Кнопка SHUTDOWN
IUserKeyListener shutdownListener = new IUserKeyListener() {
@Override
public void onKeyEvent(IUserKey key, UserKeyEvent event) {
if (event == UserKeyEvent.KeyDown) {
key.setLED(UserKeyAlignment.BottomMiddle, UserKeyLED.Yellow, UserKeyLEDSize.Small);
boolean success = executeScript(SHUTDOWN_SCRIPT);
if (!success) {
key.setLED(UserKeyAlignment.BottomMiddle, UserKeyLED.Red, UserKeyLEDSize.Small);
ThreadUtil.milliSleep(2000);
}
key.setLED(UserKeyAlignment.BottomMiddle, UserKeyLED.Grey, UserKeyLEDSize.Small);
}
}
};
//Регистрация и настройка кнопок
IUserKey rebootKey = powerBar.addUserKey(0, rebootListener, true);
rebootKey.setText(UserKeyAlignment.TopLeft, "REBOOT");
rebootKey.setLED(UserKeyAlignment.BottomMiddle, UserKeyLED.Grey, UserKeyLEDSize.Small);
rebootKey.setCriticalText("Controller will REBOOT! Press again to confirm.");
IUserKey shutdownKey = powerBar.addUserKey(1, shutdownListener, true);
shutdownKey.setText(UserKeyAlignment.TopLeft, "SHUTDOWN");
shutdownKey.setLED(UserKeyAlignment.BottomMiddle, UserKeyLED.Grey, UserKeyLEDSize.Small);
shutdownKey.setCriticalText("Controller will SHUTDOWN! Press again to confirm.");
powerBar.publish();
// Держим задачу живой
while (!Thread.currentThread().isInterrupted()) {
ThreadUtil.milliSleep(500);
}
}
/**
* Запускает .cmd-скрипт через cmd.exe.
* Не ждём завершения (waitFor не вызывается): контроллер уйдёт
* в reboot/shutdown сам, ожидание привело бы к зависанию задачи.
*
* @param scriptPath абсолютный путь к .cmd файлу
* @return true если процесс запущен успешно, false при IOException
*/
private boolean executeScript(String scriptPath) {
try {
ProcessBuilder pb = new ProcessBuilder("cmd.exe", "/c", scriptPath);
pb.redirectErrorStream(true);
pb.start();
return true;
} catch (IOException e) {
logger.error("Failed to execute: " + scriptPath + " | " + e.getMessage());
return false;
}
}
}
src/iiwa_sunrise/src/ServerFriRos2.java
+225 -198
View File
@@ -1,19 +1,30 @@
package ros;
// Sunrise OS 1.16 | FRI 1.16 | KUKA iiwa 7
//
// Режимы команды FRI:
// POSITION - ROS2 задаёт целевые углы суставов
// TORQUE - ROS2 задаёт добавочные моменты суставов
// NO_COMMAND_MODE - FRI только читает состояние, ведение рукой
//
// Режимы управления Sunrise (только для POSITION и TORQUE):
// POSITION_CONTROL - жёсткое позиционирование
// JOINT_IMPEDANCE_CONTROL - упругое позиционирование с заданной жёсткостью
//
// Сетевые интерфейсы:
// KONI - 192.170.10.10 (рекомендуется)
// KLI - 192.168.21.31
/**
* FRI bridge between KUKA iiwa 7 and ROS 2 via lbr_ros2_control.
* FRI-мост между KUKA iiwa 7 и ROS 2 через lbr_ros2_control.
*
* Tested on: Sunrise OS 1.16 / FRI 1.16 / iiwa 7 R800
* Проверено на: Sunrise OS 1.16 / FRI 1.16 / iiwa 7 R800
*
* Two network interfaces are supported:
* Поддерживаются два сетевых интерфейса:
*
* KONI (X66) — dedicated high-speed FRI network, recommended.
* Recommended send period: 5–10 ms.
* Поддерживает все режимы включая Monitor (ведение рукой).
*
* KLI (X6) — shared KRC control network, fallback option.
* Send period fixed at 10 ms to avoid packet loss on a shared bus.
* Monitor mode not available: KLI latency is too high for gravity
* compensation to be safe without a dedicated FRI stream.
* KLI latency слишком высока для безопасной гравкомпенсации.
*
* Control modes / Режимы управления:
* Position
* JointImpedance
* Monitor
*/
import java.util.concurrent.TimeUnit;
import java.util.concurrent.TimeoutException;
@@ -42,46 +53,71 @@ import com.kuka.roboticsAPI.uiModel.ApplicationDialogType;
public class ServerFriRos2 extends RoboticsAPIApplication {
// Позиции для движения перед запуском FRI
private static final double[] ZERO_POSITION = {0, 0, 0, 0, 0, 0, 0};
private static final double[] MONITOR_WORKING_POSITION = {0, 0, 0, -1.57, 0, 1.57, 0};
// CONFIGURE BEFORE DEPLOYMENT — проверить перед запуском на новом стенде
// Сетевые адреса
private static final String KONI_IP = "192.170.10.10";
private static final String KLI_IP = "192.168.21.31";
// IP address of the PC running the ROS 2 FRI node, as seen from the robot.
// IP-адрес ПК с ROS 2 FRI-узлом со стороны робота.
// KONI (X66 connector): default subnet is 192.170.10.x — change the last octet to match your PC.
// KLI (X6 connector): depends on your KRC network config.
private static final String KONI_IP = "192.170.10.10"; // <<< CHANGE THIS / ИЗМЕНИТЬ
private static final String KLI_IP = "192.168.21.31"; // <<< CHANGE THIS / ИЗМЕНИТЬ
private static final int FRI_CONNECT_TIMEOUT_SEC = 30;
// Tool name as defined in Sunrise Workbench -> Object Templates.
// Имя инструмента из Sunrise Workbench -> Object Templates.
// Must have valid Load Data (mass, CoM, inertia) for Monitor mode gravity compensation.
// Для Monitor режима обязательно заполните Load Data (масса, ЦМ, инерция).
// @Named is set below on the _tool field
// Safe joint-space pose the robot moves to before FRI starts.
// Безопасная поза (в пространстве суставов) куда робот едет перед запуском FRI.
// Adjust to avoid collisions with your cell layout / инструментом / оснасткой.
private static final double[] ZERO_POSITION = {0, 0, 0, 0, 0, 0, 0}; // <<< CHECK / ПРОВЕРИТЬ
private static final double[] MONITOR_WORKING_POSITION = {0, 0, 0, -1.57, 0, 1.57, 0}; // <<< CHECK / ПРОВЕРИТЬ
// TUNING — fine-tune if needed / настройки при необходимости
// How long to wait for the ROS 2 client to connect before giving up.
// Время ожидания подключения FRI-клиента до отмены.
private static final int FRI_CONNECT_TIMEOUT_SEC = 30;
// Relative joint velocity used for approach moves (0.0–1.0 of rated speed).
// Относительная скорость для подъездных движений (0.0–1.0 от номинальной).
private static final double APPROACH_VEL = 0.30;
// Параметры для NO_COMMAND_MODE: нулевая жёсткость позволяет свободно вести робота рукой
// Stiffness/damping for Monitor (gravity-comp, zero-stiffness hand guiding).
// Жёсткость/демпфирование для Monitor: нулевая жёсткость = робот не сопротивляется руке.
private static final double MONITOR_JOINT_STIFFNESS = 0.0;
private static final double MONITOR_JOINT_DAMPING = 0.7;
// Режим команды FRI - что именно отправляет ROS2 в каждом цикле
private enum CommandMode {
POSITION,
TORQUE,
NO_COMMAND_MODE
}
// Режим управления Sunrise - как контроллер обрабатывает команды
private enum ControlMode {
POSITION_CONTROL,
JOINT_IMPEDANCE_CONTROL
}
// Сетевой интерфейс для подключения FRI
/**
* Encapsulates a network interface choice together with the IP address
* that will be passed to FRIConfiguration.
* Хранит выбор сетевого интерфейса и соответствующий IP для FRIConfiguration.
*
* The label is built from the IP constant so the dialog button always
* reflects the actual address without manual string maintenance.
* Label строится из IP-константы - при изменении IP адрес в кнопке обновится сам.
*/
private enum NetworkInterface {
KONI("KONI (192.170.10.10) - выделенная FRI-сеть", KONI_IP),
KLI("KLI (192.168.21.31) - основная сеть KRC", KLI_IP);
KONI(KONI_IP),
KLI (KLI_IP);
final String label;
final String ip;
NetworkInterface(String label, String ip) {
this.label = label;
NetworkInterface(String ip) {
this.ip = ip;
this.label = name() + " — " + ip;
}
}
@@ -89,10 +125,10 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
private LBR _lbr;
private Controller _lbrController;
// Инструмент из Sunrise WB - Object Templates tool1
// Масса и CoM берутся из Load Data этого объекта
// Must match the Object Template name in Sunrise Workbench.
// Должно совпадать с именем объекта в Sunrise Workbench -> Object Templates.
@Inject
@Named("tool1")
@Named("tool1") // <<< CHANGE THIS / ИЗМЕНИТЬ
private Tool _tool;
private NetworkInterface _selectedNetwork;
@@ -113,10 +149,11 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
_lbrController = (Controller) getContext().getControllers().toArray()[0];
_lbr = (LBR) _lbrController.getDevices().toArray()[0];
// Прикрепляем инструмент к фланцу для корректной гравкомпенсации
// Attach tool so Sunrise accounts for its mass in all motion planning.
// Крепим инструмент, чтобы Sunrise учитывал его массу при всех движениях.
_tool.attachTo(_lbr.getFlange());
getLogger().info("ServerFriRos2 | KUKA iiwa 7 + ROS2 FRI");
getLogger().info("ServerFriRos2 | KUKA iiwa 7 | ROS2 FRI");
getLogger().info("Sunrise OS 1.16 | FRI 1.16");
getLogger().info("Робот: " + _lbr.getName());
getLogger().info("Инструмент: " + _tool.getName());
@@ -131,9 +168,8 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
moveToInitialPosition();
switch (_selectedCommandMode) {
case POSITION: runPositionMode(); break;
case TORQUE: runTorqueMode(); break;
case NO_COMMAND_MODE: runMonitorMode(); break;
case POSITION: runPositionMode(); break;
case NO_COMMAND_MODE: runMonitorMode(); break;
}
getLogger().info("Программа завершена.");
@@ -142,6 +178,8 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
@Override
public void dispose() {
// Sunrise calls dispose() even if run() threw - make sure the session is always released.
// Sunrise вызывает dispose() даже при исключении в run() - сессия должна быть освобождена.
if (_friSession != null) {
getLogger().info("Закрытие FRI-сессии...");
try {
@@ -156,153 +194,142 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
}
// Последовательный опрос конфигурации - каждый шаг зависит от предыдущего
private void requestUserConfig() {
// Шаг 1: сетевой интерфейс
int netChoice = getApplicationUI().displayModalDialog(
ApplicationDialogType.QUESTION,
"Шаг 1 - Сетевой интерфейс FRI\n\n"
+ "KONI: выделенная высокоскоростная сеть (рекомендуется)\n"
+ "KLI: основная сеть KRC",
"Шаг 1 — Сетевой интерфейс FRI",
NetworkInterface.KONI.label,
NetworkInterface.KLI.label
);
_selectedNetwork = (netChoice == 0) ? NetworkInterface.KONI : NetworkInterface.KLI;
getLogger().info("Сетевой интерфейс: " + _selectedNetwork.label);
// Шаг 2: режим команды FRI
int modeChoice = getApplicationUI().displayModalDialog(
ApplicationDialogType.QUESTION,
"Шаг 2 - Режим команды FRI\n\n"
+ "Position: ROS2 задаёт угловые позиции суставов\n"
+ "Torque: ROS2 задаёт добавочные моменты суставов\n"
+ "Monitor: только чтение, ведение рукой (NO_COMMAND_MODE)",
"Position",
"Torque",
"Monitor"
);
if (modeChoice == 0) {
_selectedCommandMode = CommandMode.POSITION;
selectControlMode();
selectSendPeriodForPosition();
} else if (modeChoice == 1) {
_selectedCommandMode = CommandMode.TORQUE;
// TORQUE всегда требует JointImpedanceControlMode на стороне Sunrise
_selectedControlMode = ControlMode.JOINT_IMPEDANCE_CONTROL;
selectJointStiffness();
selectSendPeriodForTorque();
if (_selectedNetwork == NetworkInterface.KLI) {
configureKli();
} else {
_selectedCommandMode = CommandMode.NO_COMMAND_MODE;
// Нулевая жёсткость задана константой, пользователю выбирать нечего
_selectedControlMode = ControlMode.JOINT_IMPEDANCE_CONTROL;
_sendPeriodMs = 2;
_jointStiffness = 0.0;
getLogger().info("Monitor (NO_COMMAND_MODE): период = " + _sendPeriodMs + " мс");
configureKoni();
}
logConfiguration();
}
// Шаг 3a (только для POSITION): выбор режима управления Sunrise
private void selectControlMode() {
// KLI: Position and JointImpedance only; send period fixed at 10 ms.
// KLI: доступны только Position и JointImpedance; период зафиксирован на 10 мс.
// Monitor is excluded because KLI latency makes zero-stiffness guiding unsafe.
// Monitor исключён: задержки KLI делают ведение рукой с нулевой жёсткостью небезопасным.
private void configureKli() {
int ctrlChoice = getApplicationUI().displayModalDialog(
int modeChoice = getApplicationUI().displayModalDialog(
ApplicationDialogType.QUESTION,
"Шаг 3 - Режим управления Sunrise (Position)\n\n"
+ "PositionControl: жёсткое позиционирование, максимальная точность следования\n"
+ "JointImpedance: упругое позиционирование, задаётся жёсткость суставов",
"PositionControl",
"Шаг 2 — Режим управления",
"Position",
"JointImpedance"
);
if (ctrlChoice == 0) {
_selectedCommandMode = CommandMode.POSITION;
if (modeChoice == 0) {
_selectedControlMode = ControlMode.POSITION_CONTROL;
_jointStiffness = 0.0;
getLogger().info("Режим управления: PositionControlMode");
} else {
_selectedControlMode = ControlMode.JOINT_IMPEDANCE_CONTROL;
selectJointStiffness();
}
// KLI shared bus cannot reliably sustain 5 ms cycles; 10 ms is the safe floor.
// Общая шина KLI не выдерживает стабильные циклы 5 мс; 10 мс — безопасный минимум.
_sendPeriodMs = 10;
getLogger().info("KLI: период зафиксирован 10 мс");
}
// KONI: all three modes available; operator picks the send period.
// KONI: доступны все три режима; оператор выбирает период отправки.
private void configureKoni() {
int modeChoice = getApplicationUI().displayModalDialog(
ApplicationDialogType.QUESTION,
"Шаг 2 — Режим управления",
"Position",
"JointImpedance",
"Monitor"
);
if (modeChoice == 0) {
_selectedCommandMode = CommandMode.POSITION;
_selectedControlMode = ControlMode.POSITION_CONTROL;
_jointStiffness = 0.0;
selectSendPeriod();
} else if (modeChoice == 1) {
_selectedCommandMode = CommandMode.POSITION;
_selectedControlMode = ControlMode.JOINT_IMPEDANCE_CONTROL;
selectJointStiffness();
selectSendPeriod();
} else {
_selectedCommandMode = CommandMode.NO_COMMAND_MODE;
_selectedControlMode = ControlMode.JOINT_IMPEDANCE_CONTROL;
// Monitor doesn't drive joints, so 2 ms gives the densest state stream to ROS 2.
// Monitor не командует суставами, поэтому 2 мс дают максимально плотный поток в ROS 2.
_sendPeriodMs = 2;
_jointStiffness = 0.0;
getLogger().info("Monitor: период = " + _sendPeriodMs + " мс");
}
}
// Выбор жёсткости суставов для JointImpedanceControlMode
private void selectJointStiffness() {
int sChoice = getApplicationUI().displayModalDialog(
ApplicationDialogType.QUESTION,
"Жёсткость суставов [Нм/рад]\n\n"
+ "Высокая жёсткость: точное следование, меньше отклонение от траектории\n"
+ "Низкая жёсткость: мягкое взаимодействие со средой\n\n"
+ "Внимание: 1500 Нм/рад только в производственном режиме без людей в зоне!",
"1500 - жёсткий / производственный",
"1000 - стандарт",
"800 - средний",
"500 - мягкий / взаимодействие",
"300 - очень мягкий"
"Жёсткость суставов [Нм/рад]",
"1500",
"1000",
"800",
"500"
);
_jointStiffness = new double[]{1500, 1000, 800, 500, 300}[sChoice];
_jointStiffness = new double[]{1500, 1000, 800, 500}[sChoice];
getLogger().info("Жёсткость суставов: " + _jointStiffness + " Нм/рад");
}
private void selectSendPeriodForPosition() {
private void selectSendPeriod() {
int pChoice = getApplicationUI().displayModalDialog(
ApplicationDialogType.QUESTION,
"Шаг 4 - Период отправки FRI [мс] (Position)\n\n"
+ "10 мс: стабильно, подходит для большинства сетей\n"
+ "5 мс: стандарт для ros2_control на 200 Гц\n"
+ "2 мс: быстро, требует низкого джиттера сети",
"Шаг 3 — Период отправки FRI",
"10 мс",
"5 мс",
"2 мс"
"5 мс"
);
_sendPeriodMs = new int[]{10, 5, 2}[pChoice];
getLogger().info("Период отправки: " + _sendPeriodMs + " мс");
}
private void selectSendPeriodForTorque() {
int pChoice = getApplicationUI().displayModalDialog(
ApplicationDialogType.QUESTION,
"Шаг 4 - Период отправки FRI [мс] (Torque)\n\n"
+ "При пропуске пакета Sunrise автоматически переходит в PositionHold.\n"
+ "Рекомендуется 1-2 мс при стабильной KONI-сети.",
"5 мс",
"2 мс (рекомендуется)",
"1 мс (максимальная частота)"
);
_sendPeriodMs = new int[]{5, 2, 1}[pChoice];
_sendPeriodMs = (pChoice == 0) ? 10 : 5;
getLogger().info("Период отправки: " + _sendPeriodMs + " мс");
}
private void logConfiguration() {
getLogger().info("Итоговая конфигурация:");
getLogger().info(" Сеть: " + _selectedNetwork.label);
getLogger().info(" IP: " + _selectedNetwork.ip);
getLogger().info(" Режим команды FRI: " + _selectedCommandMode.name());
getLogger().info(" Режим управления Sunrise: " + _selectedControlMode.name());
getLogger().info(" Период отправки: " + _sendPeriodMs + " мс");
getLogger().info(" Инструмент: " + _tool.getName());
getLogger().info("── Конфигурация ──────────────────");
getLogger().info(" Network : " + _selectedNetwork.label);
getLogger().info(" FRI mode : " + _selectedCommandMode.name());
getLogger().info(" Ctrl mode : " + _selectedControlMode.name());
getLogger().info(" Period : " + _sendPeriodMs + " мс");
getLogger().info(" Tool : " + _tool.getName());
if (_selectedControlMode == ControlMode.JOINT_IMPEDANCE_CONTROL && _jointStiffness > 0) {
getLogger().info(" Жёсткость суставов: " + _jointStiffness + " Нм/рад");
getLogger().info(" Stiffness : " + _jointStiffness + " Нм/рад");
}
getLogger().info("──────────────────────");
}
private void moveToInitialPosition() {
if (_selectedCommandMode == CommandMode.NO_COMMAND_MODE) {
getLogger().info("Движение в рабочую позицию Monitor (через нулевую)...");
// Move through zero first to avoid large single-joint swings.
// Сначала едем через ноль, чтобы не было больших движений по одному суставу.
getLogger().info("Движение в рабочую позицию Monitor...");
_lbr.move(
BasicMotions.batch(
BasicMotions.ptp(ZERO_POSITION).setBlendingRel(0.5),
@@ -332,61 +359,44 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
PositionHold posHold = new PositionHold(ctrlMode, -1, TimeUnit.SECONDS);
getLogger().info("Position режим активен. Ожидаю команды от ROS2...");
_lbr.move(posHold.addMotionOverlay(_friOverlay));
_friSession.close();
_friSession = null;
getLogger().info("Position режим завершён. FRI закрыт.");
}
private void runTorqueMode() {
getLogger().info("Запуск Torque режима, жёсткость: " + _jointStiffness + " Нм/рад");
if (!setupFriSession(ClientCommandMode.TORQUE)) {
return;
try {
_lbr.move(posHold.addMotionOverlay(_friOverlay));
} catch (Exception e) {
// Normal exit path when the ROS 2 client closes the FRI session.
// Штатный путь выхода при закрытии FRI-сессии со стороны ROS 2.
getLogger().info("FRI сеанс закрыт.");
}
JointImpedanceControlMode ctrlMode = new JointImpedanceControlMode(
_jointStiffness, _jointStiffness, _jointStiffness,
_jointStiffness, _jointStiffness, _jointStiffness,
_jointStiffness
);
ctrlMode.setDampingForAllJoints(0.7);
PositionHold posHold = new PositionHold(ctrlMode, -1, TimeUnit.SECONDS);
getLogger().info("Torque режим активен. Ожидаю команды от ROS2...");
_lbr.move(posHold.addMotionOverlay(_friOverlay));
_friSession.close();
_friSession = null;
getLogger().info("Torque режим завершён. FRI закрыт.");
closeFriSession();
getLogger().info("Position режим завершён.");
}
private void runMonitorMode() {
getLogger().info("Запуск Monitor режима (NO_COMMAND_MODE).");
getLogger().info("Запуск Monitor режима...");
// Фаза A: валидация Load Data инструмента из Sunrise WB
// Validate tool Load Data before enabling zero-stiffness guiding —
// incorrect inertia will make gravity compensation fight the operator.
// Проверяем Load Data до включения нулевой жёсткости:
// некорректная инерция заставит гравкомпенсацию работать против оператора.
validateLoadModel();
// Фаза B: ждём подтверждения оператора, что ROS2 FRI-узел запущен
getApplicationUI().displayModalDialog(
ApplicationDialogType.INFORMATION,
"Запустите ROS2 FRI-узел на ПК (" + _selectedNetwork.ip + ").\n\n"
+ "Нажмите OK когда ros2_control_node активен.",
"OK - ROS2 готов"
"OK — ROS2 готов"
);
// Фаза C: FRI в режиме только чтения (NO_COMMAND_MODE)
if (!setupFriSession(ClientCommandMode.NO_COMMAND_MODE)) {
return;
}
// Фаза D: PositionHold с нулевой жёсткостью - свободное ведение рукой
// Zero stiffness + moderate damping = the robot doesn't resist hand guiding
// but also doesn't flop around. Gravity is compensated by the controller.
// Нулевая жёсткость + умеренное демпфирование: робот не сопротивляется руке,
// но и не болтается. Гравитация компенсируется контроллером.
JointImpedanceControlMode guidingMode = new JointImpedanceControlMode(
MONITOR_JOINT_STIFFNESS, MONITOR_JOINT_STIFFNESS, MONITOR_JOINT_STIFFNESS,
MONITOR_JOINT_STIFFNESS, MONITOR_JOINT_STIFFNESS, MONITOR_JOINT_STIFFNESS,
@@ -397,19 +407,33 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
PositionHold posHold = new PositionHold(guidingMode, -1, TimeUnit.SECONDS);
getLogger().info("Monitor режим активен.");
getLogger().info("Ведите робота рукой - он не сопротивляется.");
getLogger().info("Ведите робота рукой, команды будут транслироваться в ROS2.");
getLogger().info("Данные суставов транслируются в ROS2 каждые " + _sendPeriodMs + " мс.");
getLogger().info("Остановите FRI-клиент для завершения.");
_lbr.move(posHold);
try {
_lbr.move(posHold);
} catch (Exception e) {
getLogger().info("FRI сеанс закрыт.");
}
_friSession.close();
_friSession = null;
getLogger().info("Monitor режим завершён. FRI закрыт.");
closeFriSession();
getLogger().info("Monitor режим завершён.");
}
// Silently closes the session — the client may have already torn it down.
// Тихо закрывает сессию — клиент мог уже закрыть её со своей стороны.
private void closeFriSession() {
if (_friSession != null) {
try {
_friSession.close();
} catch (Exception ignored) {}
_friSession = null;
}
}
// Создаёт объект режима управления на основе выбора пользователя
private AbstractMotionControlMode buildControlMode() {
if (_selectedControlMode == ControlMode.POSITION_CONTROL) {
@@ -417,7 +441,10 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
return new PositionControlMode();
}
// JointImpedanceControlMode - одинаковая жёсткость для всех суставов
// Uniform stiffness across all joints is a reasonable default; tune per-joint
// if the task requires asymmetric compliance (e.g. soft wrist, stiff elbow).
// Одинаковая жёсткость по всем суставам — разумный старт; при необходимости
// настройте каждый сустав отдельно (напр. мягкое запястье, жёсткий локоть).
JointImpedanceControlMode mode = new JointImpedanceControlMode(
_jointStiffness, _jointStiffness, _jointStiffness,
_jointStiffness, _jointStiffness, _jointStiffness,
@@ -429,8 +456,8 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
}
// Проверяет Load Data инструмента из Sunrise WB через SmartServo.validateForImpedanceMode.
// Корректные данные нагрузки обязательны для точной гравкомпенсации в Monitor режиме.
// SmartServo.validateForImpedanceMode checks that mass/CoM/inertia are non-zero.
// SmartServo.validateForImpedanceMode проверяет, что масса/ЦМ/инерция заданы ненулевыми.
private void validateLoadModel() {
getLogger().info("Валидация нагрузки инструмента: " + _tool.getName());
@@ -440,27 +467,26 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
if (valid) {
getLogger().info("Load Data валидны. Гравкомпенсация будет работать точно.");
} else {
getLogger().warn("Валидация Load Data не прошла для инструмента: " + _tool.getName());
getLogger().warn("Задайте данные в Sunrise WB -> Object Templates -> "
+ _tool.getName() + " -> Load Data");
getLogger().warn("(Mass [кг], Centre of Mass [мм], Inertia [кг/м2])");
getLogger().warn("Гравкомпенсация в Monitor режиме может работать некорректно.");
getLogger().warn("Load Data не заданы для: " + _tool.getName());
getLogger().warn("Sunrise WB → Object Templates → " + _tool.getName() + " → Load Data");
getLogger().warn("Требуются: Mass [кг], Centre of Mass [мм], Inertia [кг·м²]");
getLogger().warn("Без них гравкомпенсация в Monitor режиме будет неточной.");
int choice = getApplicationUI().displayModalDialog(
ApplicationDialogType.QUESTION,
"Load Data инструмента '" + _tool.getName() + "' не заданы.\n\n"
+ "Без корректных данных нагрузки гравкомпенсация\n"
+ "будет работать с ошибкой.\n\n"
+ "Задайте данные в Sunrise WB -> Object Templates -> "
+ _tool.getName() + " -> Load Data\nи перезапустите программу.\n\n"
+ "Или продолжите без корректной нагрузки (на свой риск).",
+ "Без них гравитационная компенсация работает некорректно.\n\n"
+ "Задайте данные:\n"
+ "Sunrise WB → Object Templates → " + _tool.getName() + " → Load Data\n"
+ "и перезапустите программу.\n\n"
+ "Или продолжите на свой риск.",
"Продолжить",
"Остановить"
);
if (choice == 1) {
throw new RuntimeException(
"Остановлено оператором: Load Data не заданы для " + _tool.getName());
"Остановлено: Load Data не заданы для " + _tool.getName());
}
}
}
@@ -474,9 +500,9 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
getLogger().info("Создание FRI-сессии...");
getLogger().info("Хост: " + _friConfig.getHostName()
+ ", порт: " + _friConfig.getPortOnRemote());
getLogger().info("Режим команды: " + commandMode.name());
getLogger().info("Период отправки: " + _friConfig.getSendPeriodMilliSec() + " мс");
+ " | Порт: " + _friConfig.getPortOnRemote()
+ " | Режим: " + commandMode.name()
+ " | Период: " + _friConfig.getSendPeriodMilliSec() + " мс");
_friSession = new FRISession(_friConfig);
_friSession.addFRISessionListener(_friListener);
@@ -488,8 +514,7 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
} catch (TimeoutException e) {
getLogger().error("Таймаут FRI! Клиент не ответил за " + FRI_CONNECT_TIMEOUT_SEC + " с.");
getLogger().error("Убедитесь, что ROS2 FRI-узел запущен на " + _selectedNetwork.ip);
_friSession.close();
_friSession = null;
closeFriSession();
return false;
}
@@ -500,8 +525,9 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
_friOverlay = new FRIJointOverlay(_friSession, commandMode);
getLogger().info("FRIJointOverlay создан для режима: " + commandMode.name());
} else {
// In NO_COMMAND_MODE the robot state is streamed but no overlay is needed.
// В NO_COMMAND_MODE состояние транслируется, но overlay не нужен.
_friOverlay = null;
getLogger().info("Monitor: FRIJointOverlay не создаётся (только чтение данных).");
}
return true;
@@ -513,26 +539,27 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
@Override
public void onFRIConnectionQualityChanged(FRIChannelInformation info) {
getLogger().info("FRI качество изменилось: " + info.getQuality()
+ ", jitter=" + info.getJitter() + " мс"
+ ", latency=" + info.getLatency() + " мс");
getLogger().info("FRI quality: " + info.getQuality()
+ " | jitter=" + info.getJitter() + " мс"
+ " | latency=" + info.getLatency() + " мс");
}
@Override
public void onFRISessionStateChanged(FRIChannelInformation info) {
getLogger().info("FRI состояние изменилось: " + info.getFRISessionState()
+ ", jitter=" + info.getJitter() + " мс"
+ ", latency=" + info.getLatency() + " мс");
getLogger().info("FRI state: " + info.getFRISessionState()
+ " | jitter=" + info.getJitter() + " мс"
+ " | latency=" + info.getLatency() + " мс");
}
};
}
private void logFriChannelInfo() {
FRIChannelInformation info = _friSession.getFRIChannelInformation();
getLogger().info("FRI состояние: " + info.getFRISessionState());
getLogger().info("FRI качество: " + info.getQuality());
getLogger().info("FRI jitter: " + info.getJitter() + " мс");
getLogger().info("FRI latency: " + info.getLatency() + " мс");
getLogger().info("FRI state : " + info.getFRISessionState());
getLogger().info("FRI quality : " + info.getQuality());
getLogger().info("FRI jitter : " + info.getJitter() + " мс");
getLogger().info("FRI latency : " + info.getLatency() + " мс");
}
src/iiwa_utils/iiwa_utils/motion_config.json
-913
View File
@@ -1,913 +0,0 @@
{
"home_joints": [
0.0,
0.0,
0.0,
-1.57,
0.0,
1.57,
0.0
],
"poses": [
{
"x": 0.6,
"y": 0.1,
"z": 0.55,
"a": 3.14,
"b": 0.3142,
"c": 2.7916,
"speed": 0.4,
"planner": "ptp"
},
{
"x": 0.6,
"y": -0.1,
"z": 0.55,
"a": 3.14,
"b": 0.3142,
"c": 2.7916,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.58,
"y": -0.1,
"z": 0.55,
"a": 3.14,
"b": 0.3142,
"c": 2.8791,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.58,
"y": 0.1,
"z": 0.55,
"a": 3.14,
"b": 0.3142,
"c": 2.8791,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.56,
"y": 0.1,
"z": 0.55,
"a": 3.14,
"b": 0.3142,
"c": 2.9666,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.56,
"y": -0.1,
"z": 0.55,
"a": 3.14,
"b": 0.3142,
"c": 2.9666,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.54,
"y": -0.1,
"z": 0.55,
"a": 3.14,
"b": 0.3142,
"c": 3.0541,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.54,
"y": 0.1,
"z": 0.55,
"a": 3.14,
"b": 0.3142,
"c": 3.0541,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.52,
"y": 0.1,
"z": 0.55,
"a": 3.14,
"b": 0.3142,
"c": 3.1416,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.52,
"y": -0.1,
"z": 0.55,
"a": 3.14,
"b": 0.3142,
"c": 3.1416,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.5,
"y": -0.1,
"z": 0.55,
"a": 3.14,
"b": 0.3142,
"c": 3.2291,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.5,
"y": 0.1,
"z": 0.55,
"a": 3.14,
"b": 0.3142,
"c": 3.2291,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.48,
"y": 0.1,
"z": 0.55,
"a": 3.14,
"b": 0.3142,
"c": 3.3166,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.48,
"y": -0.1,
"z": 0.55,
"a": 3.14,
"b": 0.3142,
"c": 3.3166,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.46,
"y": -0.1,
"z": 0.55,
"a": 3.14,
"b": 0.3142,
"c": 3.4041,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.46,
"y": 0.1,
"z": 0.55,
"a": 3.14,
"b": 0.3142,
"c": 3.4041,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.44,
"y": 0.1,
"z": 0.55,
"a": 3.14,
"b": 0.3142,
"c": 3.4916,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.44,
"y": -0.1,
"z": 0.55,
"a": 3.14,
"b": 0.3142,
"c": 3.4916,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.44,
"y": -0.1,
"z": 0.5,
"a": 3.14,
"b": 0.3665,
"c": 3.4916,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.44,
"y": 0.1,
"z": 0.5,
"a": 3.14,
"b": 0.3665,
"c": 3.4916,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.46,
"y": 0.1,
"z": 0.5,
"a": 3.14,
"b": 0.3665,
"c": 3.4041,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.46,
"y": -0.1,
"z": 0.5,
"a": 3.14,
"b": 0.3665,
"c": 3.4041,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.48,
"y": -0.1,
"z": 0.5,
"a": 3.14,
"b": 0.3665,
"c": 3.3166,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.48,
"y": 0.1,
"z": 0.5,
"a": 3.14,
"b": 0.3665,
"c": 3.3166,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.5,
"y": 0.1,
"z": 0.5,
"a": 3.14,
"b": 0.3665,
"c": 3.2291,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.5,
"y": -0.1,
"z": 0.5,
"a": 3.14,
"b": 0.3665,
"c": 3.2291,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.52,
"y": -0.1,
"z": 0.5,
"a": 3.14,
"b": 0.3665,
"c": 3.1416,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.52,
"y": 0.1,
"z": 0.5,
"a": 3.14,
"b": 0.3665,
"c": 3.1416,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.54,
"y": 0.1,
"z": 0.5,
"a": 3.14,
"b": 0.3665,
"c": 3.0541,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.54,
"y": -0.1,
"z": 0.5,
"a": 3.14,
"b": 0.3665,
"c": 3.0541,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.56,
"y": -0.1,
"z": 0.5,
"a": 3.14,
"b": 0.3665,
"c": 2.9666,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.56,
"y": 0.1,
"z": 0.5,
"a": 3.14,
"b": 0.3665,
"c": 2.9666,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.58,
"y": 0.1,
"z": 0.5,
"a": 3.14,
"b": 0.3665,
"c": 2.8791,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.58,
"y": -0.1,
"z": 0.5,
"a": 3.14,
"b": 0.3665,
"c": 2.8791,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.6,
"y": -0.1,
"z": 0.5,
"a": 3.14,
"b": 0.3665,
"c": 2.7916,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.6,
"y": 0.1,
"z": 0.5,
"a": 3.14,
"b": 0.3665,
"c": 2.7916,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.6,
"y": 0.1,
"z": 0.45,
"a": 3.14,
"b": 0.4189,
"c": 2.7916,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.6,
"y": -0.1,
"z": 0.45,
"a": 3.14,
"b": 0.4189,
"c": 2.7916,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.58,
"y": -0.1,
"z": 0.45,
"a": 3.14,
"b": 0.4189,
"c": 2.8791,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.58,
"y": 0.1,
"z": 0.45,
"a": 3.14,
"b": 0.4189,
"c": 2.8791,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.56,
"y": 0.1,
"z": 0.45,
"a": 3.14,
"b": 0.4189,
"c": 2.9666,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.56,
"y": -0.1,
"z": 0.45,
"a": 3.14,
"b": 0.4189,
"c": 2.9666,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.54,
"y": -0.1,
"z": 0.45,
"a": 3.14,
"b": 0.4189,
"c": 3.0541,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.54,
"y": 0.1,
"z": 0.45,
"a": 3.14,
"b": 0.4189,
"c": 3.0541,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.52,
"y": 0.1,
"z": 0.45,
"a": 3.14,
"b": 0.4189,
"c": 3.1416,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.52,
"y": -0.1,
"z": 0.45,
"a": 3.14,
"b": 0.4189,
"c": 3.1416,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.5,
"y": -0.1,
"z": 0.45,
"a": 3.14,
"b": 0.4189,
"c": 3.2291,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.5,
"y": 0.1,
"z": 0.45,
"a": 3.14,
"b": 0.4189,
"c": 3.2291,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.48,
"y": 0.1,
"z": 0.45,
"a": 3.14,
"b": 0.4189,
"c": 3.3166,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.48,
"y": -0.1,
"z": 0.45,
"a": 3.14,
"b": 0.4189,
"c": 3.3166,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.46,
"y": -0.1,
"z": 0.45,
"a": 3.14,
"b": 0.4189,
"c": 3.4041,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.46,
"y": 0.1,
"z": 0.45,
"a": 3.14,
"b": 0.4189,
"c": 3.4041,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.44,
"y": 0.1,
"z": 0.45,
"a": 3.14,
"b": 0.4189,
"c": 3.4916,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.44,
"y": -0.1,
"z": 0.45,
"a": 3.14,
"b": 0.4189,
"c": 3.4916,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.44,
"y": -0.1,
"z": 0.4,
"a": 3.14,
"b": 0.4712,
"c": 3.4916,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.44,
"y": 0.1,
"z": 0.4,
"a": 3.14,
"b": 0.4712,
"c": 3.4916,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.46,
"y": 0.1,
"z": 0.4,
"a": 3.14,
"b": 0.4712,
"c": 3.4041,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.46,
"y": -0.1,
"z": 0.4,
"a": 3.14,
"b": 0.4712,
"c": 3.4041,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.48,
"y": -0.1,
"z": 0.4,
"a": 3.14,
"b": 0.4712,
"c": 3.3166,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.48,
"y": 0.1,
"z": 0.4,
"a": 3.14,
"b": 0.4712,
"c": 3.3166,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.5,
"y": 0.1,
"z": 0.4,
"a": 3.14,
"b": 0.4712,
"c": 3.2291,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.5,
"y": -0.1,
"z": 0.4,
"a": 3.14,
"b": 0.4712,
"c": 3.2291,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.52,
"y": -0.1,
"z": 0.4,
"a": 3.14,
"b": 0.4712,
"c": 3.1416,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.52,
"y": 0.1,
"z": 0.4,
"a": 3.14,
"b": 0.4712,
"c": 3.1416,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.54,
"y": 0.1,
"z": 0.4,
"a": 3.14,
"b": 0.4712,
"c": 3.0541,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.54,
"y": -0.1,
"z": 0.4,
"a": 3.14,
"b": 0.4712,
"c": 3.0541,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.56,
"y": -0.1,
"z": 0.4,
"a": 3.14,
"b": 0.4712,
"c": 2.9666,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.56,
"y": 0.1,
"z": 0.4,
"a": 3.14,
"b": 0.4712,
"c": 2.9666,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.58,
"y": 0.1,
"z": 0.4,
"a": 3.14,
"b": 0.4712,
"c": 2.8791,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.58,
"y": -0.1,
"z": 0.4,
"a": 3.14,
"b": 0.4712,
"c": 2.8791,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.6,
"y": -0.1,
"z": 0.4,
"a": 3.14,
"b": 0.4712,
"c": 2.7916,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.6,
"y": 0.1,
"z": 0.4,
"a": 3.14,
"b": 0.4712,
"c": 2.7916,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.6,
"y": 0.1,
"z": 0.35,
"a": 3.14,
"b": 0.5236,
"c": 2.7916,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.6,
"y": -0.1,
"z": 0.35,
"a": 3.14,
"b": 0.5236,
"c": 2.7916,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.58,
"y": -0.1,
"z": 0.35,
"a": 3.14,
"b": 0.5236,
"c": 2.8791,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.58,
"y": 0.1,
"z": 0.35,
"a": 3.14,
"b": 0.5236,
"c": 2.8791,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.56,
"y": 0.1,
"z": 0.35,
"a": 3.14,
"b": 0.5236,
"c": 2.9666,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.56,
"y": -0.1,
"z": 0.35,
"a": 3.14,
"b": 0.5236,
"c": 2.9666,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.54,
"y": -0.1,
"z": 0.35,
"a": 3.14,
"b": 0.5236,
"c": 3.0541,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.54,
"y": 0.1,
"z": 0.35,
"a": 3.14,
"b": 0.5236,
"c": 3.0541,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.52,
"y": 0.1,
"z": 0.35,
"a": 3.14,
"b": 0.5236,
"c": 3.1416,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.52,
"y": -0.1,
"z": 0.35,
"a": 3.14,
"b": 0.5236,
"c": 3.1416,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.5,
"y": -0.1,
"z": 0.35,
"a": 3.14,
"b": 0.5236,
"c": 3.2291,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.5,
"y": 0.1,
"z": 0.35,
"a": 3.14,
"b": 0.5236,
"c": 3.2291,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.48,
"y": 0.1,
"z": 0.35,
"a": 3.14,
"b": 0.5236,
"c": 3.3166,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.48,
"y": -0.1,
"z": 0.35,
"a": 3.14,
"b": 0.5236,
"c": 3.3166,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.46,
"y": -0.1,
"z": 0.35,
"a": 3.14,
"b": 0.5236,
"c": 3.4041,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.46,
"y": 0.1,
"z": 0.35,
"a": 3.14,
"b": 0.5236,
"c": 3.4041,
"speed": 0.3,
"planner": "lin"
},
{
"x": 0.44,
"y": 0.1,
"z": 0.35,
"a": 3.14,
"b": 0.5236,
"c": 3.4916,
"speed": 0.4,
"planner": "lin"
},
{
"x": 0.44,
"y": -0.1,
"z": 0.35,
"a": 3.14,
"b": 0.5236,
"c": 3.4916,
"speed": 0.3,
"planner": "lin"
}
]
}
src/iiwa_utils/iiwa_utils/setting_loader.py
+56 -2
View File
@@ -13,7 +13,6 @@ class RobotCfg:
name: str
ip: str
port: int
command_mode: str
description: str
fri_cycle_ms: int
joint_position_tau: float
@@ -66,6 +65,20 @@ class PlanningCfg:
planning_attempts: int
@dataclass(frozen=True)
class ToolCfg:
active: str # ключ из tools.yaml: "none" | "patron" | ...
@dataclass(frozen=True)
class WebCfg:
enabled: bool
host: str
port: int
endpoints: str # resolved absolute path to api_endpoints.yaml
joint_limits: str # resolved absolute path to joint_limits.yaml
@dataclass(frozen=True)
class FoxgloveCfg:
enabled: bool # Запускать ли foxglove_bridge
@@ -100,7 +113,9 @@ class Settings:
digital_twin: DigitalTwinCfg
controller: ControllerCfg
planning: PlanningCfg
tool: ToolCfg
foxglove: FoxgloveCfg
web: WebCfg
def to_dict(self) -> Dict[str, Any]:
def _convert(obj: Any) -> Any:
@@ -169,6 +184,32 @@ def assert_file(path: str, key: str) -> None:
raise SettingsError(f"path for '{key}' is not a file: {path}")
# Web defaults
_WEB_DEFAULTS: Dict[str, Any] = {
"enabled": False,
"host": "0.0.0.0",
"port": 8007,
"endpoints": "pkg://iiwa_config/config/api_endpoints.yaml",
"joint_limits": "pkg://iiwa_config/config/moveit/joint_limits.yaml",
}
def _parse_web(raw: Optional[Dict[str, Any]], settings_dir: str) -> WebCfg:
if raw is None:
raw = {}
def get(key: str) -> Any:
return raw.get(key, _WEB_DEFAULTS[key])
return WebCfg(
enabled=bool(get("enabled")),
host=str(get("host")),
port=int(get("port")),
endpoints=resolve_path(str(get("endpoints")), settings_dir),
joint_limits=resolve_path(str(get("joint_limits")), settings_dir),
)
# Foxglove defaults
_FOXGLOVE_DEFAULTS: Dict[str, Any] = {
"enabled": False,
@@ -247,7 +288,6 @@ def build_settings(settings_path: str, check_files: bool = True) -> Settings:
name=str(require(robot_raw, "name")),
ip=str(require(robot_raw, "ip")),
port=int(require(robot_raw, "port")),
command_mode=str(require(robot_raw, "command_mode")),
description=resolve_path(str(require(robot_raw, "description")), settings_dir),
fri_cycle_ms=int(robot_raw.get("fri_cycle_ms", 5)),
joint_position_tau=float(robot_raw.get("joint_position_tau", 0.04)),
@@ -302,15 +342,26 @@ def build_settings(settings_path: str, check_files: bool = True) -> Settings:
planning_attempts=int(planning_raw.get("planning_attempts", 3)),
)
# tool
tool_raw = raw.get("tool", {})
tool = ToolCfg(
active=str(tool_raw.get("active", "patron")),
)
# foxglove
foxglove = _parse_foxglove(raw.get("foxglove"))
# web
web = _parse_web(raw.get("web"), settings_dir)
s = Settings(
robot=robot,
digital_twin=digital_twin,
controller=controller,
planning=planning,
tool=tool,
foxglove=foxglove,
web=web,
)
if check_files:
@@ -328,5 +379,8 @@ def build_settings(settings_path: str, check_files: bool = True) -> Settings:
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")
if s.web.enabled:
assert_file(s.web.endpoints, "web.endpoints")
assert_file(s.web.joint_limits, "web.joint_limits")
return s
src/iiwa_utils/iiwa_utils/tool_manager.py
+157
View File
@@ -0,0 +1,157 @@
"""
Управление активным инструментом робота.
Применяет выбранный инструмент из реестра tools.yaml:
1. Перезаписывает tool_active.xacro — URDF подхватывает его при следующем запуске.
2. Перегенерирует iiwa7.srdf — сохраняет текущие group_state, обновляет
кинематическую цепочку, end_effector и пары disable_collisions.
"""
from __future__ import annotations
import xml.etree.ElementTree as ET
from pathlib import Path
from typing import Any
# ---------------------------------------------------------------------------
# Базовые пары столкновений робота (не зависят от инструмента).
# ---------------------------------------------------------------------------
_BASE_COLLISIONS: list[tuple[str, str, str]] = [
("base_link", "link1", "Adjacent"),
("base_link", "link2", "Never"),
("base_link", "link3", "Never"),
("base_link", "link4", "Never"),
("link1", "link2", "Adjacent"),
("link1", "link3", "Never"),
("link1", "link4", "Never"),
("link1", "link5", "Never"),
("link1", "link6", "Never"),
("link1", "link7", "Never"),
("link2", "link3", "Adjacent"),
("link2", "link4", "Never"),
("link2", "link5", "Never"),
("link2", "link6", "Never"),
("link2", "link7", "Never"),
("link3", "link4", "Adjacent"),
("link3", "link5", "Never"),
("link3", "link6", "Never"),
("link3", "link7", "Never"),
("link4", "link5", "Adjacent"),
("link4", "link6", "Never"),
("link4", "link7", "Never"),
("link5", "link6", "Adjacent"),
("link5", "link7", "Never"),
("link6", "link7", "Adjacent"),
]
def load_registry(tools_yaml_path: Path) -> dict[str, Any]:
"""Загружает реестр инструментов из tools.yaml.
Поддерживает PyYAML и ruamel.yaml (что доступно в окружении)."""
try:
import yaml as _yaml
with open(tools_yaml_path, encoding="utf-8") as f:
data = _yaml.safe_load(f)
except ImportError:
from ruamel.yaml import YAML as _RYAML
_y = _RYAML()
with open(tools_yaml_path, encoding="utf-8") as f:
data = _y.load(f)
return dict(data.get("tools", {}))
def _read_group_states(srdf_path: Path) -> list[dict]:
"""Читает group_state из существующего SRDF, чтобы сохранить их при регенерации."""
if not srdf_path.exists():
return []
tree = ET.parse(srdf_path)
root = tree.getroot()
states = []
for gs in root.findall("group_state"):
joints = {j.get("name"): j.get("value") for j in gs.findall("joint")}
states.append({
"name": gs.get("name"),
"group": gs.get("group"),
"joints": joints,
})
return states
def _write_xacro(tool_cfg: dict, output_path: Path) -> None:
"""Записывает tool_active.xacro с include нужного инструмента (или пустой)."""
xacro_include = tool_cfg.get("xacro")
if xacro_include:
body = f' <xacro:include filename="{xacro_include}"/>\n'
else:
body = " <!-- no tool attached -->\n"
content = (
'<?xml version="1.0"?>\n'
'<!-- AUTO-GENERATED by cobot robot-setup — do not edit manually -->\n'
'<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="tool_active">\n'
+ body +
'</robot>\n'
)
output_path.write_text(content, encoding="utf-8")
def _write_srdf(
tool_cfg: dict,
group_states: list[dict],
output_path: Path,
) -> None:
"""Генерирует iiwa7.srdf с учётом выбранного инструмента."""
tip_link = tool_cfg.get("tip_link", "link_ee")
tool_collisions = [tuple(c) for c in tool_cfg.get("collisions", [])]
lines: list[str] = [
'<?xml version="1.0" encoding="UTF-8"?>',
'<!-- AUTO-GENERATED by cobot robot-setup — do not edit manually -->',
'<robot name="iiwa7">',
'',
' <group name="iiwa_arm">',
f' <chain base_link="base_link" tip_link="{tip_link}"/>',
' </group>',
]
if group_states:
lines.append('')
for gs in group_states:
lines.append(f' <group_state name="{gs["name"]}" group="{gs["group"]}">')
for jname, jval in gs["joints"].items():
lines.append(f' <joint name="{jname}" value="{jval}"/>')
lines.append(' </group_state>')
if tip_link != "link_ee":
lines += [
'',
f' <end_effector name="{tip_link}" parent_link="{tip_link}" group="iiwa_arm"/>',
]
lines.append('')
for l1, l2, reason in _BASE_COLLISIONS:
lines.append(f' <disable_collisions link1="{l1}" link2="{l2}" reason="{reason}"/>')
if tool_collisions:
lines.append('')
for l1, l2, reason in tool_collisions:
lines.append(f' <disable_collisions link1="{l1}" link2="{l2}" reason="{reason}"/>')
lines += ['</robot>', '']
output_path.write_text('\n'.join(lines), encoding="utf-8")
def apply_tool(
tool_cfg: dict,
xacro_out_path: Path,
srdf_path: Path,
) -> None:
"""
Применяет инструмент по его конфигу из реестра:
- перезаписывает xacro_out_path (tool_active.xacro)
- регенерирует srdf_path (iiwa7.srdf), сохраняя group_state
"""
group_states = _read_group_states(srdf_path)
_write_xacro(tool_cfg, xacro_out_path)
_write_srdf(tool_cfg, group_states, srdf_path)
src/iiwa_utils/package.xml
+1 -1
View File
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>iiwa_utils</name>
<version>2026.05.31</version>
<version>2026.5.31</version>
<description>Utilities for the cobot system: configuration loading, URDF/XACRO processing, object and camera spawning in Webots, geometric data conversion</description>
<maintainer email="grabardm@ml-dev.ru">daniel</maintainer>
<license>Apache-2.0</license>
src/iiwa_utils/setup.py
+1 -3
View File
@@ -4,13 +4,12 @@ package_name = 'iiwa_utils'
setup(
name=package_name,
version='2026.05.31',
version='2026.5.31',
packages=find_packages(exclude=['test']),
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
('share/' + package_name, ['iiwa_utils/motion_config.json']),
],
install_requires=['setuptools'],
zip_safe=True,
@@ -22,7 +21,6 @@ setup(
'console_scripts': [
"object_spawner = iiwa_utils.object_spawner:main",
"camera_spawner = iiwa_utils.camera_spawner:main",
"test_motion_sequence = iiwa_utils.test_motion_sequence:main",
],
},
)
src/iiwa_web/iiwa_web/config_loader.py
+191
View File
@@ -0,0 +1,191 @@
from __future__ import annotations
import xml.etree.ElementTree as ET
import yaml
from dataclasses import dataclass, field
from pathlib import Path
from typing import Any, Optional
@dataclass
class FieldDef:
name: str
type: str # string | float | int | bool | float_array
required: bool = False
default: Any = None
description: str = ""
min: Optional[float] = None
max: Optional[float] = None
length: Optional[int] = None
choices: Optional[list] = None
normalize: Optional[str] = None # "lower" | "upper"
joint_limits: bool = False
@dataclass
class EndpointDef:
path: str
method: str # GET | POST
type: str # topic | service | action | tf
ros_name: str = ""
msg_type: str = ""
summary: str = ""
description: str = ""
tags: list = field(default_factory=list)
fields: list = field(default_factory=list) # topic: поля ответа
response_fields: list = field(default_factory=list) # service/action: поля ответа
request_fields: list[FieldDef] = field(default_factory=list)
timeout: float = 5.0
deprecated: bool = False
enabled: bool = True
parent_frame: str = "" # tf: родительский фрейм
child_frame: str = "" # tf: дочерний фрейм
def _resolve_path(package: str, relative: str) -> Path:
"""Ищет файл сначала через ament_index, затем по пути относительно src/."""
try:
from ament_index_python.packages import get_package_share_directory
return Path(get_package_share_directory(package)) / relative
except Exception:
src = Path(__file__).parents[3] # .../src/iiwa_web/iiwa_web/ -> .../src/
return src / package / relative
def _parse_joint_limits_data(data: dict) -> tuple[list[str], list[tuple[float, float]]]:
joints = data["joint_limits"]
names: list[str] = []
limits: list[tuple[float, float]] = []
i = 1
while f"joint{i}" in joints:
j = joints[f"joint{i}"]
names.append(f"joint{i}")
limits.append((j["min_position"], j["max_position"]))
i += 1
return names, limits
def load_joint_names(
path: str | None = None,
package: str = "iiwa_config",
relative: str = "config/moveit/joint_limits.yaml",
) -> list[str]:
"""Возвращает упорядоченный список имён суставов из joint_limits.yaml."""
resolved = Path(path) if path else _resolve_path(package, relative)
with open(resolved) as f:
data = yaml.safe_load(f)
names, _ = _parse_joint_limits_data(data)
return names
def load_joint_limits(
path: str | None = None,
package: str = "iiwa_config",
relative: str = "config/moveit/joint_limits.yaml",
) -> list[tuple[float, float]]:
"""Возвращает список (min, max) для каждого сустава по порядку joint1..jointN."""
resolved = Path(path) if path else _resolve_path(package, relative)
with open(resolved) as f:
data = yaml.safe_load(f)
_, limits = _parse_joint_limits_data(data)
return limits
def load_api_config(
path: str | None = None,
package: str = "iiwa_config",
relative: str = "config/api_endpoints.yaml",
) -> list[EndpointDef]:
"""Загружает описания эндпоинтов из YAML и возвращает список EndpointDef."""
resolved = Path(path) if path else _resolve_path(package, relative)
with open(resolved) as f:
data = yaml.safe_load(f)
endpoints: list[EndpointDef] = []
for ep in data.get("endpoints", []):
if not ep.get("enabled", True):
continue
request_fields = [
FieldDef(
name=rf["name"],
type=rf["type"],
required=rf.get("required", False),
default=rf.get("default"),
description=rf.get("description", ""),
min=rf.get("min"),
max=rf.get("max"),
length=rf.get("length"),
choices=rf.get("choices"),
normalize=rf.get("normalize"),
joint_limits=rf.get("joint_limits", False),
)
for rf in ep.get("request_fields", [])
]
endpoints.append(EndpointDef(
path=ep["path"],
method=ep["method"].upper(),
type=ep["type"],
ros_name=ep.get("ros_name", ""),
msg_type=ep.get("msg_type", ""),
summary=ep.get("summary", ""),
description=ep.get("description", ""),
tags=ep.get("tags", []),
fields=ep.get("fields", []),
response_fields=ep.get("response_fields", []),
request_fields=request_fields,
timeout=ep.get("timeout", 5.0),
deprecated=ep.get("deprecated", False),
parent_frame=ep.get("parent_frame", ""),
child_frame=ep.get("child_frame", ""),
))
return endpoints
@dataclass
class NamedPosition:
name: str
group: str
joints: dict[str, float]
description: str = ""
def load_named_positions(
srdf_path: str | None = None,
meta_path: str | None = None,
package: str = "iiwa_config",
srdf_relative: str = "config/moveit/iiwa7.srdf",
meta_relative: str = "config/moveit/named_positions_meta.yaml",
) -> list[NamedPosition]:
"""Загружает именованные позиции из SRDF и объединяет с описаниями из YAML."""
resolved_srdf = Path(srdf_path) if srdf_path else _resolve_path(package, srdf_relative)
resolved_meta = Path(meta_path) if meta_path else _resolve_path(package, meta_relative)
tree = ET.parse(resolved_srdf)
root = tree.getroot()
descriptions: dict[str, str] = {}
if resolved_meta.exists():
with open(resolved_meta) as f:
meta = yaml.safe_load(f) or {}
for name, attrs in meta.get("named_positions", {}).items():
descriptions[name] = attrs.get("description", "")
positions: list[NamedPosition] = []
for gs in root.findall("group_state"):
name = gs.get("name", "")
group = gs.get("group", "")
joints = {
j.get("name"): float(j.get("value", 0))
for j in gs.findall("joint")
}
positions.append(NamedPosition(
name=name,
group=group,
joints=joints,
description=descriptions.get(name, ""),
))
return positions
src/iiwa_web/iiwa_web/dynamic_router.py
+316
View File
@@ -0,0 +1,316 @@
import importlib
import math
import time
from typing import Any
from fastapi import APIRouter, HTTPException
from fastapi.responses import JSONResponse
from pydantic import Field, create_model
from .config_loader import EndpointDef, FieldDef, load_api_config, load_joint_limits
from .ros_node import get_bridge
_POLL_INTERVAL = 0.05
_TYPE_MAP: dict[str, type] = {
"string": str,
"float": float,
"int": int,
"bool": bool,
"float_array": list[float],
}
def _import_ros_type(type_str: str):
"""'sensor_msgs/msg/JointState' → класс JointState."""
parts = type_str.split("/")
module = importlib.import_module(".".join(parts[:-1]))
return getattr(module, parts[-1])
def _to_python(val: Any) -> Any:
"""Конвертирует ROS-значение в JSON-сериализуемый Python-тип."""
if isinstance(val, float):
return None if math.isnan(val) else val
if hasattr(val, "__iter__") and not isinstance(val, (str, bytes)):
return [None if (isinstance(v, float) and math.isnan(v)) else v for v in val]
return val
def _extract(msg, fields: list[str]) -> dict:
return {f: _to_python(getattr(msg, f)) for f in fields}
def _build_model(name: str, field_defs: list[FieldDef]) -> type:
"""Динамически создаёт Pydantic-модель из списка FieldDef."""
definitions: dict[str, tuple] = {}
for fd in field_defs:
py_type = _TYPE_MAP[fd.type]
kwargs: dict[str, Any] = {"description": fd.description}
if fd.min is not None:
kwargs["ge"] = fd.min
if fd.max is not None:
kwargs["le"] = fd.max
if fd.length is not None:
kwargs["min_length"] = fd.length
kwargs["max_length"] = fd.length
kwargs["default"] = ... if fd.required else fd.default
definitions[fd.name] = (py_type, Field(**kwargs))
return create_model(name, **definitions)
def _make_topic_handler(ep: EndpointDef):
ros_type = _import_ros_type(ep.msg_type)
ros_name = ep.ros_name
fields = ep.fields
timeout = ep.timeout
def handler():
bridge = get_bridge()
bridge.subscribe(ros_name, ros_type)
deadline = time.monotonic() + timeout
while time.monotonic() < deadline:
msg = bridge.get_latest(ros_name)
if msg is not None:
return JSONResponse(_extract(msg, fields))
time.sleep(_POLL_INTERVAL)
raise HTTPException(503, f"Timeout waiting for topic '{ros_name}'")
return handler
def _make_service_handler(ep: EndpointDef, Body: type | None):
ros_type = _import_ros_type(ep.msg_type)
ros_name = ep.ros_name
response_fields = ep.response_fields
timeout = ep.timeout
field_defs = ep.request_fields
if Body is None:
def handler():
try:
resp = get_bridge().call_service(ros_type, ros_name, ros_type.Request(), timeout)
except (RuntimeError, TimeoutError) as e:
raise HTTPException(503, str(e))
return _extract(resp, response_fields) if response_fields else {"success": resp.success}
else:
def handler(body: Body): # type: ignore[valid-type]
req = ros_type.Request()
for fd in field_defs:
setattr(req, fd.name, getattr(body, fd.name))
try:
resp = get_bridge().call_service(ros_type, ros_name, req, timeout)
except (RuntimeError, TimeoutError) as e:
raise HTTPException(503, str(e))
return _extract(resp, response_fields) if response_fields else {"success": resp.success}
return handler
def _make_action_handler(ep: EndpointDef, Body: type, joint_limits: list[tuple[float, float]]):
ros_type = _import_ros_type(ep.msg_type)
ros_name = ep.ros_name
response_fields = ep.response_fields
timeout = ep.timeout
field_defs = ep.request_fields
choice_fields = [(fd.name, fd.choices, fd.normalize) for fd in field_defs if fd.choices]
jl_fields = [fd.name for fd in field_defs if fd.joint_limits]
def handler(body: Body): # type: ignore[valid-type]
values: dict[str, Any] = {fd.name: getattr(body, fd.name) for fd in field_defs}
# Нормализация и валидация choices
for fname, choices, normalize in choice_fields:
val = values[fname]
if normalize == "lower" and isinstance(val, str):
val = val.lower()
elif normalize == "upper" and isinstance(val, str):
val = val.upper()
if val not in choices:
raise HTTPException(422, f"Поле '{fname}' должно быть одним из {choices}, получено '{val}'")
values[fname] = val
# Валидация лимитов суставов из joint_limits.yaml
for fname in jl_fields:
joints = values[fname]
if len(joints) != len(joint_limits):
raise HTTPException(
422,
f"Ожидалось {len(joint_limits)} суставов, получено {len(joints)}",
)
for i, (pos, (lo, hi)) in enumerate(zip(joints, joint_limits)):
if not (lo <= pos <= hi):
raise HTTPException(
422,
f"Сустав {i + 1}: {pos:.4f} рад вне диапазона [{lo:.3f}, {hi:.3f}]",
)
goal = ros_type.Goal()
for fd in field_defs:
setattr(goal, fd.name, values[fd.name])
try:
result = get_bridge().send_action(ros_type, ros_name, goal, timeout)
except (RuntimeError, TimeoutError) as e:
raise HTTPException(503, str(e))
return _extract(result.result, response_fields) if response_fields else {}
return handler
def _quat_to_euler_zyx(x: float, y: float, z: float, w: float) -> tuple[float, float, float]:
"""Quaternion → ZYX Euler (KUKA ABC: A=yaw, B=pitch, C=roll)."""
sinr = 2 * (w * x + y * z)
cosr = 1 - 2 * (x * x + y * y)
roll = math.atan2(sinr, cosr)
sinp = 2 * (w * y - z * x)
pitch = math.copysign(math.pi / 2, sinp) if abs(sinp) >= 1 else math.asin(sinp)
siny = 2 * (w * z + x * y)
cosy = 1 - 2 * (y * y + z * z)
yaw = math.atan2(siny, cosy)
return roll, pitch, yaw # C, B, A
def _make_fk_handler(ep: EndpointDef):
ros_name = ep.ros_name
fk_link = ep.child_frame
base_frame = ep.parent_frame
timeout = ep.timeout
def handler():
from moveit_msgs.srv import GetPositionFK
bridge = get_bridge()
deadline = time.monotonic() + timeout
while time.monotonic() < deadline:
js = bridge.get_latest("/joint_states")
if js is not None:
break
time.sleep(_POLL_INTERVAL)
else:
raise HTTPException(503, "Timeout waiting for /joint_states")
req = GetPositionFK.Request()
req.header.frame_id = base_frame
req.fk_link_names = [fk_link]
req.robot_state.joint_state = js
try:
resp = bridge.call_service(GetPositionFK, ros_name, req, timeout)
except (RuntimeError, TimeoutError) as e:
raise HTTPException(503, str(e))
if not resp.pose_stamped:
raise HTTPException(503, f"FK вернул пустой результат для '{fk_link}'")
pose = resp.pose_stamped[0].pose
t = pose.position
r = pose.orientation
roll, pitch, yaw = _quat_to_euler_zyx(r.x, r.y, r.z, r.w)
return {
"position": {"x": t.x, "y": t.y, "z": t.z},
"orientation": {
"quaternion": {"x": r.x, "y": r.y, "z": r.z, "w": r.w},
"euler_rad": {"a": yaw, "b": pitch, "c": roll},
"euler_deg": {
"a": math.degrees(yaw),
"b": math.degrees(pitch),
"c": math.degrees(roll),
},
},
"frame": {"parent": base_frame, "child": fk_link},
}
return handler
def _make_tf_handler(ep: EndpointDef):
parent = ep.parent_frame
child = ep.child_frame
timeout = ep.timeout
def handler():
try:
tf = get_bridge().lookup_transform(parent, child, timeout)
except RuntimeError as e:
raise HTTPException(503, str(e))
t = tf.transform.translation
r = tf.transform.rotation
roll, pitch, yaw = _quat_to_euler_zyx(r.x, r.y, r.z, r.w)
return {
"position": {"x": t.x, "y": t.y, "z": t.z},
"orientation": {
"quaternion": {"x": r.x, "y": r.y, "z": r.z, "w": r.w},
"euler_rad": {"a": yaw, "b": pitch, "c": roll},
"euler_deg": {
"a": math.degrees(yaw),
"b": math.degrees(pitch),
"c": math.degrees(roll),
},
},
"frame": {"parent": parent, "child": child},
}
return handler
def build_dynamic_router(
endpoints_path: str | None = None,
joint_limits_path: str | None = None,
) -> APIRouter:
"""Читает api_endpoints.yaml и joint_limits.yaml, возвращает готовый APIRouter."""
endpoints = load_api_config(path=endpoints_path)
joint_limits = load_joint_limits(path=joint_limits_path)
router = APIRouter()
for ep in endpoints:
# Имя модели — CamelCase из пути (/robot/move/joints → RobotMoveJoints)
model_name = "".join(p.title() for p in ep.path.strip("/").split("/"))
Body: type | None = None
if ep.request_fields:
Body = _build_model(f"{model_name}Request", ep.request_fields)
if ep.type == "topic":
handler = _make_topic_handler(ep)
elif ep.type == "service":
handler = _make_service_handler(ep, Body)
elif ep.type == "action":
if Body is None:
raise ValueError(f"Action-эндпоинт '{ep.path}' не имеет request_fields")
handler = _make_action_handler(ep, Body, joint_limits)
elif ep.type == "tf":
if not ep.parent_frame or not ep.child_frame:
raise ValueError(f"TF-эндпоинт '{ep.path}' требует parent_frame и child_frame")
handler = _make_tf_handler(ep)
elif ep.type == "fk":
if not ep.ros_name or not ep.parent_frame or not ep.child_frame:
raise ValueError(f"FK-эндпоинт '{ep.path}' требует ros_name, parent_frame и child_frame")
handler = _make_fk_handler(ep)
else:
raise ValueError(f"Неизвестный тип эндпоинта: '{ep.type}'")
operation_id = ep.path.strip("/").replace("/", "_") + "_" + ep.method.lower()
router.add_api_route(
ep.path,
handler,
methods=[ep.method],
operation_id=operation_id,
summary=ep.summary or ep.description,
description=ep.description,
tags=ep.tags,
deprecated=ep.deprecated,
)
return router
src/iiwa_web/iiwa_web/main.py
+54
View File
@@ -0,0 +1,54 @@
import threading
from contextlib import asynccontextmanager
import rclpy
import uvicorn
from fastapi import FastAPI
from fastmcp import FastMCP
from sensor_msgs.msg import JointState
from .dynamic_router import build_dynamic_router
from .ros_node import CobotWebNode, get_bridge, set_bridge
from . import runner, trajectory, positions
def main():
rclpy.init()
node = CobotWebNode()
set_bridge(node)
threading.Thread(target=rclpy.spin, args=(node,), daemon=True).start()
host = node.get_parameter('host').value
port = node.get_parameter('port').value
endpoints_path = node.get_parameter('endpoints_path').value or None
joint_limits_path = node.get_parameter('joint_limits_path').value or None
positions.init()
_schema_app = FastAPI()
_schema_app.include_router(build_dynamic_router(endpoints_path, joint_limits_path))
_schema_app.include_router(runner.router)
_schema_app.include_router(trajectory.router)
_schema_app.include_router(positions.router)
mcp = FastMCP.from_fastapi(app=_schema_app)
mcp_http = mcp.http_app(path='/mcp')
@asynccontextmanager
async def lifespan(_: FastAPI):
async with mcp_http.router.lifespan_context(_):
get_bridge().subscribe("/joint_states", JointState)
yield
app = FastAPI(lifespan=lifespan)
app.include_router(build_dynamic_router(endpoints_path, joint_limits_path))
app.include_router(runner.router)
app.include_router(trajectory.router)
app.include_router(positions.router)
app.mount("/mcp", mcp_http)
uvicorn.run(app, host=host, port=port)
if __name__ == "__main__":
main()
src/iiwa_web/iiwa_web/positions.py
+68
View File
@@ -0,0 +1,68 @@
from fastapi import APIRouter
from pydantic import BaseModel, Field
from .config_loader import load_named_positions
router = APIRouter(tags=["robot"])
class NamedPositionResponse(BaseModel):
name: str = Field(
description=(
"Идентификатор позиции. Передайте это значение в поле `name` запроса "
"`POST /robot/move/named`, чтобы переместить робота в эту позу."
)
)
group: str = Field(
description="Группа планирования MoveIt, к которой относится позиция (обычно `iiwa_arm`)."
)
joints: dict[str, float] = Field(
description=(
"Целевые углы суставов в радианах. Ключи: joint1..joint7. "
"Используйте эти значения для оценки конфигурации перед движением "
"или как основу для `POST /robot/move/joints`."
)
)
description: str = Field(
description="Человекочитаемое описание назначения позиции."
)
_srdf_path: str | None = None
_meta_path: str | None = None
def init(srdf_path: str | None = None, meta_path: str | None = None) -> None:
global _srdf_path, _meta_path
_srdf_path = srdf_path
_meta_path = meta_path
@router.get(
"/robot/positions",
response_model=list[NamedPositionResponse],
summary="Список заготовленных именованных позиций робота",
description=(
"Возвращает все именованные позиции (`group_state`) из SRDF-файла конфигурации робота. "
"\n\n"
"**Типичный рабочий процесс для агента:**\n"
"1. Вызовите этот эндпоинт, чтобы узнать доступные позиции и их суставные значения.\n"
"2. Выберите подходящую позицию по полю `description` и значениям `joints`.\n"
"3. Передайте поле `name` выбранной позиции в `POST /robot/move/named`, "
"чтобы переместить робота туда.\n"
"\n"
"Позиции определены статически в SRDF и гарантированно безопасны с точки зрения "
"столкновений и кинематики."
),
)
def get_named_positions() -> list[NamedPositionResponse]:
positions = load_named_positions(srdf_path=_srdf_path, meta_path=_meta_path)
return [
NamedPositionResponse(
name=p.name,
group=p.group,
joints=p.joints,
description=p.description,
)
for p in positions
]
src/iiwa_web/iiwa_web/ros_node.py
+132
View File
@@ -0,0 +1,132 @@
import time
import rclpy
import threading
from rclpy.node import Node
from rclpy.action import ActionClient
import tf2_ros
class CobotWebNode(Node):
def __init__(self):
super().__init__('cobot_web_node')
self.declare_parameter('host', '0.0.0.0')
self.declare_parameter('port', 8007)
self.declare_parameter('endpoints_path', '')
self.declare_parameter('joint_limits_path', '')
self._topic_cache: dict = {}
self._pub_registry: dict = {}
self._service_clients: dict = {}
self._action_clients: dict = {}
self._lock = threading.Lock()
self._tf_buffer = tf2_ros.Buffer()
self._tf_listener = tf2_ros.TransformListener(self._tf_buffer, self)
def subscribe(self, topic_name: str, msg_type):
if topic_name not in self._topic_cache:
self._topic_cache[topic_name] = None
self.create_subscription(
msg_type,
topic_name,
lambda msg, t=topic_name: self._handle_message(t, msg),
10
)
self.get_logger().info(f'Subscribed to topic: {topic_name}')
def publish(self, topic_name: str, message_type, msg):
if topic_name not in self._pub_registry:
self._pub_registry[topic_name] = self.create_publisher(message_type, topic_name, 10)
self.get_logger().info(f'Created publisher for topic: {topic_name}')
self._pub_registry[topic_name].publish(msg)
def get_latest(self, topic_name: str):
with self._lock:
return self._topic_cache.get(topic_name)
def _handle_message(self, topic_name: str, msg):
with self._lock:
self._topic_cache[topic_name] = msg
def call_service(self, srv_type, srv_name: str, request, timeout: float = 5.0):
if srv_name not in self._service_clients:
self._service_clients[srv_name] = self.create_client(srv_type, srv_name)
client = self._service_clients[srv_name]
if not client.wait_for_service(timeout_sec=timeout):
raise RuntimeError(f"Сервис '{srv_name}' недоступен")
future = client.call_async(request)
deadline = time.monotonic() + timeout
while not future.done():
if time.monotonic() > deadline:
raise TimeoutError(f"Таймаут вызова сервиса '{srv_name}'")
time.sleep(0.01)
return future.result()
def lookup_transform(self, parent_frame: str, child_frame: str, timeout: float = 1.0):
try:
return self._tf_buffer.lookup_transform(
parent_frame,
child_frame,
rclpy.time.Time(),
timeout=rclpy.duration.Duration(seconds=timeout),
)
except Exception as e:
raise RuntimeError(f"TF lookup {parent_frame} → {child_frame}: {e}")
def send_action(self, action_type, action_name: str, goal, timeout: float = 30.0):
if action_name not in self._action_clients:
self._action_clients[action_name] = ActionClient(self, action_type, action_name)
client = self._action_clients[action_name]
if not client.wait_for_server(timeout_sec=10.0):
raise RuntimeError(f"Action сервер '{action_name}' недоступен")
goal_future = client.send_goal_async(goal)
deadline = time.monotonic() + 10.0
while not goal_future.done():
if time.monotonic() > deadline:
raise TimeoutError(f"Таймаут принятия goal '{action_name}'")
time.sleep(0.01)
goal_handle = goal_future.result()
if not goal_handle.accepted:
raise RuntimeError(f"Goal отклонён сервером '{action_name}'")
result_future = goal_handle.get_result_async()
deadline = time.monotonic() + timeout
while not result_future.done():
if time.monotonic() > deadline:
raise TimeoutError(f"Таймаут выполнения action '{action_name}'")
time.sleep(0.05)
return result_future.result()
_bridge: CobotWebNode = None
def set_bridge(node: CobotWebNode) -> None:
global _bridge
_bridge = node
def init_ros_node() -> None:
global _bridge
rclpy.init()
_bridge = CobotWebNode()
thread = threading.Thread(target=rclpy.spin, args=(_bridge,), daemon=True)
thread.start()
def get_bridge() -> CobotWebNode:
global _bridge
if _bridge is None:
raise RuntimeError("ROS node not initialized. Call init_ros_node() first.")
return _bridge
src/iiwa_web/iiwa_web/runner.py
+130
View File
@@ -0,0 +1,130 @@
import os
import signal
import subprocess
import threading
import tempfile
from collections import deque
from pathlib import Path
from typing import Optional
import yaml
from fastapi import APIRouter, Form, HTTPException, Query, UploadFile, File
from std_srvs.srv import Trigger
from .ros_node import get_bridge
router = APIRouter(prefix="/sequences", tags=["sequences"])
_UPLOAD_DIR = Path(tempfile.gettempdir()) / "iiwa_configs"
_UPLOAD_DIR.mkdir(exist_ok=True)
_LOG_BUFFER = 300
_process: Optional[subprocess.Popen] = None
_log_lines: deque[str] = deque(maxlen=_LOG_BUFFER)
_lock = threading.Lock()
def _stream_output(proc: subprocess.Popen) -> None:
for line in proc.stdout:
_log_lines.append(line.rstrip("\n"))
def _build_cmd(config_path: str, n_iterations: int, delay: float,
bag_path: str, topics: list[str],
joints_action: str, pose_action: str) -> list[str]:
cmd = [
"ros2", "run", "iiwa_planning", "motion_sequence_runner",
"--ros-args",
"-p", f"config_path:={config_path}",
"-p", f"n_iterations:={n_iterations}",
"-p", f"delay_between_iterations:={delay}",
"-p", f"joints_action:={joints_action}",
"-p", f"pose_action:={pose_action}",
]
if bag_path:
cmd += ["-p", f"bag_path:={bag_path}"]
if topics:
topics_yaml = yaml.dump(topics, default_flow_style=True).strip()
cmd += ["-p", f"topics:={topics_yaml}"]
return cmd
@router.post("/start", summary="Загрузить конфиг и запустить motion_sequence_runner")
async def start_runner(
config: UploadFile = File(..., description="JSON-файл конфигурации последовательности"),
n_iterations: int = Form(3, ge=1, description="Число повторений"),
delay_between_iterations: float = Form(5.0, ge=0.0, description="Пауза между итерациями [с]"),
bag_path: str = Form("", description="Путь для записи rosbag (пусто = не записывать)"),
topics: str = Form("", description="Топики для bag через запятую (пусто = все)"),
joints_action: str = Form("cobot/move_to_joints", description="Action для суставного движения"),
pose_action: str = Form("cobot/move_to_pose", description="Action для декартова движения"),
):
global _process
with _lock:
if _process and _process.poll() is None:
raise HTTPException(409, f"Runner уже запущен (pid={_process.pid})")
filename = config.filename or "config.json"
dest = _UPLOAD_DIR / filename
dest.write_bytes(await config.read())
topics_list = [t.strip() for t in topics.split(",") if t.strip()]
_log_lines.clear()
cmd = _build_cmd(
config_path=str(dest),
n_iterations=n_iterations,
delay=delay_between_iterations,
bag_path=bag_path,
topics=topics_list,
joints_action=joints_action,
pose_action=pose_action,
)
_process = subprocess.Popen(
cmd,
stdout=subprocess.PIPE,
stderr=subprocess.STDOUT,
text=True,
bufsize=1,
start_new_session=True,
)
threading.Thread(target=_stream_output, args=(_process,), daemon=True).start()
return {"status": "started", "pid": _process.pid, "config": filename}
@router.post("/stop", summary="Остановить motion_sequence_runner и послать cobot/stop")
def stop_runner():
global _process
with _lock:
if not _process or _process.poll() is not None:
raise HTTPException(404, "Runner не запущен")
pgid = os.getpgid(_process.pid)
os.killpg(pgid, signal.SIGTERM)
try:
_process.wait(timeout=5.0)
except subprocess.TimeoutExpired:
os.killpg(pgid, signal.SIGKILL)
_process.wait()
code = _process.returncode
result = get_bridge().call_service(Trigger, "cobot/stop", Trigger.Request())
return {"status": "stopped", "returncode": code, "success": result.success, "message": result.message}
@router.get("/status", summary="Статус motion_sequence_runner")
def runner_status():
if not _process:
return {"status": "idle"}
code = _process.poll()
if code is None:
return {"status": "running", "pid": _process.pid}
return {"status": "finished", "returncode": code}
@router.get("/logs", summary="Последние строки вывода motion_sequence_runner")
def runner_logs(n: int = Query(50, ge=1, le=_LOG_BUFFER, description="Количество последних строк")):
lines = list(_log_lines)
return {"lines": lines[-n:], "total_buffered": len(lines)}
src/iiwa_web/iiwa_web/trajectory.py
+201
View File
@@ -0,0 +1,201 @@
import csv
import io
from collections import deque
from datetime import datetime
from builtin_interfaces.msg import Duration
from fastapi import APIRouter, File, HTTPException, Query, UploadFile
from pydantic import BaseModel, Field
from std_srvs.srv import Trigger
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from .config_loader import load_joint_limits, load_joint_names
from .ros_node import get_bridge
router = APIRouter(prefix="/trajectory", tags=["trajectory"])
TOPIC = "/iiwa_arm_controller/joint_trajectory"
JOINT_NAMES = load_joint_names()
N_JOINTS = len(JOINT_NAMES)
_log_lines: deque[str] = deque(maxlen=300)
def _log(msg: str) -> None:
_log_lines.append(f"[{datetime.now().strftime('%H:%M:%S.%f')[:-3]}] {msg}")
def _to_duration(seconds: float) -> Duration:
sec = int(seconds)
nanosec = int(round((seconds - sec) * 1e9))
return Duration(sec=sec, nanosec=nanosec)
def _validate_limits(points: list[list[float]]) -> None:
limits = load_joint_limits()
for row_idx, positions in enumerate(points):
for j, (pos, (lo, hi)) in enumerate(zip(positions, limits)):
if not (lo <= pos <= hi):
raise HTTPException(
422,
f"Точка {row_idx + 1}, сустав {j + 1}: "
f"{pos:.4f} рад вне диапазона [{lo:.3f}, {hi:.3f}]",
)
def _build_msg(rows: list[tuple[list[float], float]]) -> JointTrajectory:
msg = JointTrajectory()
msg.joint_names = JOINT_NAMES
for positions, t in rows:
pt = JointTrajectoryPoint()
pt.positions = positions
pt.time_from_start = _to_duration(t)
msg.points.append(pt)
return msg
def _publish(msg: JointTrajectory) -> None:
get_bridge().publish(TOPIC, JointTrajectory, msg)
class Waypoint(BaseModel):
positions: list[float] = Field(
..., min_length=N_JOINTS, max_length=N_JOINTS,
description="Позиции суставов [j1..j7] в радианах",
)
time_from_start: float = Field(..., ge=0.0, description="Время от начала траектории [с]")
class SendRequest(BaseModel):
points: list[Waypoint] = Field(..., min_length=1, description="Точки траектории")
validate_limits: bool = Field(True, description="Проверять лимиты суставов")
@router.post("/send", summary="Отправить траекторию вручную (JSON)")
def send_trajectory(req: SendRequest):
"""
Принимает список точек с позициями суставов и временем от начала.
Публикует `JointTrajectory` в `/iiwa_arm_controller/joint_trajectory`.
"""
rows = [(wp.positions, wp.time_from_start) for wp in req.points]
if req.validate_limits:
_validate_limits([r[0] for r in rows])
_publish(_build_msg(rows))
_log(f"[send] {len(rows)} точек, t_end={rows[-1][1]:.2f}с")
return {"status": "sent", "points": len(rows)}
@router.post("/send_csv", summary="Загрузить CSV и отправить траекторию")
async def send_csv_trajectory(
file: UploadFile = File(
...,
description="CSV с заголовком. Колонки суставов: joint_1..joint_7 (или joint1..joint7). Колонка времени: t.",
),
separator: str = Query(",", description="Разделитель колонок (например: ',' ';' '\\t')"),
validate_limits: bool = Query(True, description="Проверять лимиты суставов"),
):
"""
Ожидаемый формат (первая строка — обязательный заголовок):
joint1,joint2,joint3,joint4,joint5,joint6,joint7,t
-2.55,-0.71,-0.77,0.028,0.0,-2.09,-0.10,0.0
-2.54,-0.71,-0.77,0.029,0.0,-2.09,-0.10,0.01
Порядок и имена колонок произвольны — сопоставление идёт по заголовку.
Имена суставов нормализуются: `joint_1` = `joint1` = `JOINT1`.
Колонка времени определяется по заголовку `t`, `time` или `time_from_start`.
"""
sep = separator.replace("\\t", "\t")
content = (await file.read()).decode("utf-8")
reader = csv.reader(io.StringIO(content), delimiter=sep)
try:
raw_headers = next(reader)
except StopIteration:
raise HTTPException(422, "Файл пуст")
headers = [h.strip() for h in raw_headers]
def _norm(s: str) -> str:
return s.lower().replace("_", "").replace(" ", "")
TIME_ALIASES = {"t", "time", "timefromstart"}
norm_joint_to_idx = {_norm(j): i for i, j in enumerate(JOINT_NAMES)}
col_joint: dict[int, int] = {} # col_index -> joint_index
col_time: int | None = None
for col_idx, h in enumerate(headers):
n = _norm(h)
if n in TIME_ALIASES:
col_time = col_idx
elif n in norm_joint_to_idx:
col_joint[col_idx] = norm_joint_to_idx[n]
if col_time is None:
raise HTTPException(422, f"Колонка времени не найдена. Ожидалось одно из: t, time, time_from_start. Заголовки: {headers}")
missing = sorted(set(range(N_JOINTS)) - set(col_joint.values()))
if missing:
raise HTTPException(422, f"Не найдены колонки для суставов: {[JOINT_NAMES[i] for i in missing]}")
joint_to_col = {j_idx: c_idx for c_idx, j_idx in col_joint.items()}
rows: list[tuple[list[float], float]] = []
for line_no, row in enumerate(reader, start=2):
row = [c.strip() for c in row]
if not any(row):
continue
if len(row) != len(headers):
raise HTTPException(
422,
f"Строка {line_no}: ожидалось {len(headers)} столбцов, получено {len(row)}",
)
try:
positions = [float(row[joint_to_col[i]]) for i in range(N_JOINTS)]
t = float(row[col_time])
except ValueError as e:
raise HTTPException(422, f"Строка {line_no}: не удалось распарсить число — {e}")
if t < 0:
raise HTTPException(422, f"Строка {line_no}: t не может быть отрицательным")
rows.append((positions, t))
if not rows:
raise HTTPException(422, "CSV не содержит точек траектории")
if validate_limits:
_validate_limits([r[0] for r in rows])
_publish(_build_msg(rows))
_log(f"[csv] {file.filename} → {len(rows)} точек, t_end={rows[-1][1]:.2f}с")
return {"status": "sent", "points": len(rows), "filename": file.filename}
@router.post("/stop", summary="Остановить выполнение траектории")
def stop_trajectory():
bridge = get_bridge()
# Replace ongoing trajectory with single point at current position
joint_states = bridge.get_latest("/joint_states")
if joint_states is not None and len(joint_states.position) >= N_JOINTS:
current_positions = list(joint_states.position[:N_JOINTS])
hold_msg = _build_msg([(current_positions, 0.5)])
_publish(hold_msg)
_log("[stop] отправлена точка удержания текущей позиции")
else:
msg = JointTrajectory()
msg.joint_names = JOINT_NAMES
_publish(msg)
_log("[stop] joint_states недоступны, отправлена пустая траектория")
# cobot/stop cancels MoveIt action-based motion
result = bridge.call_service(Trigger, "cobot/stop", Trigger.Request())
_log(f"[stop] cobot/stop -> success={result.success}, message={result.message}")
return {"status": "stopped", "success": result.success, "message": result.message}
@router.get("/logs", summary="Последние лог-записи траекторного модуля")
def trajectory_logs(n: int = Query(50, ge=1, le=300, description="Количество последних строк")):
lines = list(_log_lines)
return {"lines": lines[-n:], "total_buffered": len(lines)}
src/iiwa_web/package.xml
+9 -1
View File
@@ -2,11 +2,19 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>iiwa_web</name>
<version>2026.05.31</version>
<version>2026.5.31</version>
<description>Web interface for monitoring and remote control of the cobot via browser</description>
<maintainer email="grabardm@ml-dev.ru">daniel</maintainer>
<license>Apache-2.0</license>
<exec_depend>python3-fastapi</exec_depend>
<exec_depend>python3-uvicorn</exec_depend>
<exec_depend>python3-multipart</exec_depend>
<exec_depend>python3-fastmcp</exec_depend>
<exec_depend>tf2_ros</exec_depend>
<exec_depend>tf2_py</exec_depend>
<exec_depend>moveit_msgs</exec_depend>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
src/iiwa_web/setup.py
+10 -2
View File
@@ -4,14 +4,21 @@ package_name = 'iiwa_web'
setup(
name=package_name,
version='2026.05.31',
version='2026.5.31',
packages=find_packages(exclude=['test']),
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
],
install_requires=['setuptools'],
install_requires=[
'setuptools',
'fastapi>=0.100.0',
'starlette>=0.27.0',
'uvicorn[standard]',
'python-multipart',
'fastmcp',
],
zip_safe=True,
maintainer='daniel',
maintainer_email='grabardm@ml-dev.ru',
@@ -21,6 +28,7 @@ setup(
},
entry_points={
'console_scripts': [
'iiwa_web_server = iiwa_web.main:main',
],
},
)
src/realsense2_camera/.gitignore
-1
View File
@@ -1 +0,0 @@
CMakeLists.txt.user
src/realsense2_camera/CHANGELOG.rst
-283
View File
@@ -1,283 +0,0 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package realsense2_camera
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
4.57.4 (2025-11-02)
-------------------
* Update realsense2_camera package.xml to 4.57.4
* PR `#3441 <https://github.com/IntelRealSense/realsense-ros/issues/3441>`_ from remibettan/ros2-development: merging 4.57.3 to ros2-development
* Merge tag '4.57.3' into ros2-development
* PR `#3437 <https://github.com/IntelRealSense/realsense-ros/issues/3437>`_ from Gilaadb: bug fix(rs_node_setup): Fix topic names that were improperly marked as rect (rectified)
* bug fix(rs_node_setup): Fix topic names that were improperly marked as rect (rectified)
* Contributors: Gilad Bretter, Nir Azkiel, Remi Bettan
4.57.3 (2025-09-15)
-------------------
* PR `#3430 <https://github.com/realsenseai/realsense-ros/issues/3430>`_ from Gilaadb: Create a singleton wrapper to rs2::context
* PR `#3429 <https://github.com/realsenseai/realsense-ros/issues/3429>`_ from remibettan: intel removed, realsense added
* PR `#3421 <https://github.com/realsenseai/realsense-ros/issues/3421>`_ from ynyBonfennil: Fix argument names (`_usb_port_id` and `_device_type`)
* PR `#3417 <https://github.com/realsenseai/realsense-ros/issues/3417>`_ from remibettan: Merging ros2 hkr to ros2 dev final
* PR `#3410 <https://github.com/realsenseai/realsense-ros/issues/3410>`_ from Nir-Az: Update copyrights
* PR `#3356 <https://github.com/realsenseai/realsense-ros/issues/3356>`_ from ashrafk93: Ashraf/glsl pointcloud
* PR `#3374 <https://github.com/realsenseai/realsense-ros/issues/3374>`_ from remibettan: kilted added to wrapper
* PR `#3392 <https://github.com/realsenseai/realsense-ros/issues/3392>`_ from remibettan: adding D436
* PR `#3385 <https://github.com/realsenseai/realsense-ros/issues/3385>`_ from Gilaadb: Fix RGBD camera_info and frame_id
* PR `#3371 <https://github.com/realsenseai/realsense-ros/issues/3371>`_ from Gilaadb: replace posix argument with suffix which is what it was meant to be
* PR `#3347 <https://github.com/realsenseai/realsense-ros/issues/3347>`_ from remibettan: few logs and exceptions catches added
* PR `#41 <https://github.com/realsenseai/realsense-ros/issues/41>`_ from remibettan: Merge dev to hkr 2025 05 04
* PR `#3352 <https://github.com/realsenseai/realsense-ros/issues/3352>`_ from ashrafk93: Fix unit test of Support TF Prefixing
* PR `#3332 <https://github.com/realsenseai/realsense-ros/issues/3332>`_ from pondersome: Support TF Prefixing
* PR `#3340 <https://github.com/realsenseai/realsense-ros/issues/3340>`_ from Gilaadb: Support rgbd type and fix bug for FPS lower than 1
* PR `#3325 <https://github.com/realsenseai/realsense-ros/issues/3325>`_ from ashrafk93: use ykush to switch ports
* PR `#3319 <https://github.com/realsenseai/realsense-ros/issues/3319>`_ from ashrafk93: Add LifeCycle Node support at compile time
* PR `#3303 <https://github.com/realsenseai/realsense-ros/issues/3303>`_ from noacoohen: Enable rotation filter for color and depth sensors
* PR `#3293 <https://github.com/realsenseai/realsense-ros/issues/3293>`_ from remibettan: align_depth_to_infra2 enabled, pointcloud and align_depth filters to own files
* PR `#3284 <https://github.com/realsenseai/realsense-ros/issues/3284>`_ from noacoohen: Add color format to depth module in the launch file
* PR `#3274 <https://github.com/realsenseai/realsense-ros/issues/3274>`_ from noacoohen: Enable rotation filter ROS2
* PR `#3276 <https://github.com/realsenseai/realsense-ros/issues/3276>`_ from remibettan: removing dead code in RosSensor class
* PR `#3214 <https://github.com/realsenseai/realsense-ros/issues/3214>`_ from acornaglia: Add ROS bag loop option
* PR `#3239 <https://github.com/realsenseai/realsense-ros/issues/3239>`_ from SamerKhshiboun: Update CMakeLists.txt - remove find_package(fastrtps REQUIRED)
* PR `#3225 <https://github.com/realsenseai/realsense-ros/issues/3225>`_ from SamerKhshiboun: Use new APIs for motion, accel and gryo streams
* PR `#3222 <https://github.com/realsenseai/realsense-ros/issues/3222>`_ from SamerKhshiboun: Support D555 and its motion profiles
* PR `#3221 <https://github.com/realsenseai/realsense-ros/issues/3221>`_ from patrickwasp: fix config typo
* PR `#33 <https://github.com/realsenseai/realsense-ros/issues/33>`_ from PrasRsRos: add reset service tests
* PR `#35 <https://github.com/realsenseai/realsense-ros/issues/35>`_ from PrasRsRos: align private to public 30.9.2024
* PR `#32 <https://github.com/realsenseai/realsense-ros/issues/32>`_ from SamerKhshiboun: Support HWM command as ROS2 service and in the ROS-MQTT bridge node
* PR `#3216 <https://github.com/realsenseai/realsense-ros/issues/3216>`_ from PrasRsRos: hw_reset implementation
* PR `#30 <https://github.com/realsenseai/realsense-ros/issues/30>`_ from SamerKhshiboun: Use new apis of SIC and SP that works directly with JSON inputs/outputs
* PR `#28 <https://github.com/realsenseai/realsense-ros/issues/28>`_ from SamerKhshiboun: Fix MQTT Demo and update values for and update TC consecutives failures threshold
* PR `#27 <https://github.com/realsenseai/realsense-ros/issues/27>`_ from SamerKhshiboun: Add new flash 0.93 fields to app config
* PR `#3200 <https://github.com/realsenseai/realsense-ros/issues/3200>`_ from kadiredd: retry thrice finding devices with Ykush reset
* PR `#23 <https://github.com/realsenseai/realsense-ros/issues/23>`_ from PrasRsRos: Ros tc implementation
* PR `#20 <https://github.com/realsenseai/realsense-ros/issues/20>`_ from SamerKhshiboun: Revert switching to service mode in order to set depth params
* PR `#21 <https://github.com/realsenseai/realsense-ros/issues/21>`_ from SamerKhshiboun: Fix l4 threshold to l2 threshold
* PR `#19 <https://github.com/realsenseai/realsense-ros/issues/19>`_ from PrasRsRos: DeviceInfo and TC Mqtt tests
* PR `#3178 <https://github.com/realsenseai/realsense-ros/issues/3178>`_ from kadiredd: disabling FPS & TF tests for ROS-CI
* PR `#16 <https://github.com/realsenseai/realsense-ros/issues/16>`_ from PrasRsRos: RS ROS Mqtt bridge unit tests
* PR `#3166 <https://github.com/realsenseai/realsense-ros/issues/3166>`_ from SamerKhshiboun: Update Calibration Config API
* PR `#13 <https://github.com/realsenseai/realsense-ros/issues/13>`_ from SamerKhshiboun: Sٍupport set/get application config as ROS service and in ROS-MQTT bridge
* PR `#3159 <https://github.com/realsenseai/realsense-ros/issues/3159>`_ from noacoohen: Add D421 PID
* PR `#10 <https://github.com/realsenseai/realsense-ros/issues/10>`_ from SamerKhshiboun: Add ROS MQTT Bridge (Python) Node Into realsense-ros-private
* PR `#3153 <https://github.com/realsenseai/realsense-ros/issues/3153>`_ from SamerKhshiboun: TC | Fix feedback and update readme
* fix feedback and update readme for TC
* PR `#3138 <https://github.com/realsenseai/realsense-ros/issues/3138>`_ from SamerKhshiboun: Support Triggered Calibration as ROS2 Action
* implement Triggered Calibration action
* PR `#3135 <https://github.com/realsenseai/realsense-ros/issues/3135>`_ from kadiredd: Casefolding device name instead of strict case sensitive comparison
* Casefolding device name instead os strict case sensitive comparison
* PR `#3133 <https://github.com/realsenseai/realsense-ros/issues/3133>`_ from SamerKhshiboun: update librealsense2 version to 2.56.0
* update librealsense2 version to 2.56.0
since it includes new API that need for ros2-development
* PR `#3124 <https://github.com/realsenseai/realsense-ros/issues/3124>`_ from kadiredd: Support testing ROS2 service call device_info
* PR `#3125 <https://github.com/realsenseai/realsense-ros/issues/3125>`_ from SamerKhshiboun: Support calibration config read/write services
* PR `#5 <https://github.com/realsenseai/realsense-ros/issues/5>`_ from SamerKhshiboun: Update README and fix SIC fields in the examples
* PR `#3114 <https://github.com/realsenseai/realsense-ros/issues/3114>`_ from Arun-Prasad-V: Ubuntu 24.04 support for Rolling and Jazzy distros
* PR `#3 <https://github.com/realsenseai/realsense-ros/issues/3>`_ from SamerKhshiboun: Support sic read write services
* PR `#2 <https://github.com/realsenseai/realsense-ros/issues/2>`_ from SamerKhshiboun: Support Safety Preset Read/Write Services
* PR `#3102 <https://github.com/realsenseai/realsense-ros/issues/3102>`_ from fortizcuesta: Allow hw synchronization of several realsense using a synchonization cable
* PR `#3096 <https://github.com/realsenseai/realsense-ros/issues/3096>`_ from anisotropicity: Update rs_launch.py to add depth_module.color_profile
* PR `#1 <https://github.com/realsenseai/realsense-ros/issues/1>`_ from Arun-Prasad-V: Set Safety mode to SERVICE when loading preset
* PR `#3061 <https://github.com/realsenseai/realsense-ros/issues/3061>`_ from Arun-Prasad-V: Updated rs_launch.py for LPC and Occupancy stream profile names
* rs-launch.py update
* PR `#3038 <https://github.com/realsenseai/realsense-ros/issues/3038>`_ from Arun-Prasad-V: Set Safety mode to service before updating Depth controls during launch
* PR `#3032 <https://github.com/realsenseai/realsense-ros/issues/3032>`_ from SamerKhshiboun: Support occupancy grid cells
* PR `#2971 <https://github.com/realsenseai/realsense-ros/issues/2971>`_ from SamerKhshiboun: Occupancy Height Fix
* PR `#2952 <https://github.com/realsenseai/realsense-ros/issues/2952>`_ from Nir-Az: Support 2 res for LPC
* PR `#2827 <https://github.com/realsenseai/realsense-ros/issues/2827>`_ from SamerKhshiboun: Fix empty frames of rgbd
* PR `#2821 <https://github.com/realsenseai/realsense-ros/issues/2821>`_ from SamerKhshiboun: fix missing else due to merge from ros2-development
* PR `#2813 <https://github.com/realsenseai/realsense-ros/issues/2813>`_ from SamerKhshiboun: Fix URDF and LPCL for SC
* PR `#2815 <https://github.com/realsenseai/realsense-ros/issues/2815>`_ from SamerKhshiboun: fix labeled point cloud publisher reset condition
* PR `#2802 <https://github.com/realsenseai/realsense-ros/issues/2802>`_ from SamerKhshiboun: add new RGBD topic
* PR `#2800 <https://github.com/realsenseai/realsense-ros/issues/2800>`_ from SamerKhshiboun: Fix overriding frames on same topics/CV-images due to a bug in PR2759
* PR `#2776 <https://github.com/realsenseai/realsense-ros/issues/2776>`_ from SamerKhshiboun: Fix LPCL in SC
* PR `#2757 <https://github.com/realsenseai/realsense-ros/issues/2757>`_ from SamerKhshiboun: Support Depth Mapping Streams
* PR `#2659 <https://github.com/realsenseai/realsense-ros/issues/2659>`_ from SamerKhshiboun: Warn instead of error for undefined sensor callbacks
* PR `#2590 <https://github.com/realsenseai/realsense-ros/issues/2590>`_ from SamerKhshiboun: Add SC to ROS
* Contributors: Aman Chulawala, Arun-Prasad-V, Ashraf Kattoura, AviaAv, Cornaglia, Alessandro, Gilad Bretter, Madhukar Reddy Kadireddy, Nir Azkiel, Ortiz Cuesta, Fernando, Patrick Wspanialy, PrasRsRos, Remi Bettan, Samer Khshiboun, acornaglia, administrator, anisotropicity, louislelay, noacoohen, pondersome, ynyBonfennil
4.55.1 (2024-05-28)
-------------------
* PR `#3106 <https://github.com/realsenseai/realsense-ros/issues/3106>`_ from SamerKhshiboun: Remove unused parameter _is_profile_exist
* PR `#3098 <https://github.com/realsenseai/realsense-ros/issues/3098>`_ from kadiredd: ROS live cam test fixes
* PR `#3094 <https://github.com/realsenseai/realsense-ros/issues/3094>`_ from kadiredd: ROSCI infra for live camera testing
* PR `#3066 <https://github.com/realsenseai/realsense-ros/issues/3066>`_ from SamerKhshiboun: Revert Foxy Build Support (From Source)
* PR `#3052 <https://github.com/realsenseai/realsense-ros/issues/3052>`_ from Arun-Prasad-V: Support for selecting profile for each stream_type
* PR `#3056 <https://github.com/realsenseai/realsense-ros/issues/3056>`_ from SamerKhshiboun: Add documentation for RealSense ROS2 Wrapper Windows installation
* PR `#3049 <https://github.com/realsenseai/realsense-ros/issues/3049>`_ from Arun-Prasad-V: Applying Colorizer filter to Aligned-Depth image
* PR `#3053 <https://github.com/realsenseai/realsense-ros/issues/3053>`_ from Nir-Az: Fix Coverity issues + remove empty warning log
* PR `#3007 <https://github.com/realsenseai/realsense-ros/issues/3007>`_ from Arun-Prasad-V: Skip updating Exp 1,2 & Gain 1,2 when HDR is disabled
* PR `#3042 <https://github.com/realsenseai/realsense-ros/issues/3042>`_ from kadiredd: Assert Fail if camera not found
* PR `#3008 <https://github.com/realsenseai/realsense-ros/issues/3008>`_ from Arun-Prasad-V: Renamed GL GPU enable param
* PR `#2989 <https://github.com/realsenseai/realsense-ros/issues/2989>`_ from Arun-Prasad-V: Dynamically switching b/w CPU & GPU processing
* PR `#3001 <https://github.com/realsenseai/realsense-ros/issues/3001>`_ from deep0294: Update ReadMe to run ROS2 Unit Test
* PR `#2998 <https://github.com/realsenseai/realsense-ros/issues/2998>`_ from SamerKhshiboun: fix calibration intrinsic fail
* PR `#2987 <https://github.com/realsenseai/realsense-ros/issues/2987>`_ from SamerKhshiboun: Remove D465 SKU
* PR `#2984 <https://github.com/realsenseai/realsense-ros/issues/2984>`_ from deep0294: Fix All Profiles Test
* PR `#2956 <https://github.com/realsenseai/realsense-ros/issues/2956>`_ from Arun-Prasad-V: Extending LibRS's GL support to RS ROS2
* PR `#2953 <https://github.com/realsenseai/realsense-ros/issues/2953>`_ from Arun-Prasad-V: Added urdf & mesh files for D405 model
* PR `#2940 <https://github.com/realsenseai/realsense-ros/issues/2940>`_ from Arun-Prasad-V: Fixing the data_type of ROS Params exposure & gain
* PR `#2948 <https://github.com/realsenseai/realsense-ros/issues/2948>`_ from Arun-Prasad-V: Disabling HDR during INIT
* PR `#2934 <https://github.com/realsenseai/realsense-ros/issues/2934>`_ from Arun-Prasad-V: Disabling hdr while updating exposure & gain values
* PR `#2946 <https://github.com/realsenseai/realsense-ros/issues/2946>`_ from gwen2018: fix ros random crash with error hw monitor command for asic temperature failed
* PR `#2865 <https://github.com/realsenseai/realsense-ros/issues/2865>`_ from PrasRsRos: add live camera tests
* PR `#2891 <https://github.com/realsenseai/realsense-ros/issues/2891>`_ from Arun-Prasad-V: revert PR2872
* PR `#2853 <https://github.com/realsenseai/realsense-ros/issues/2853>`_ from Arun-Prasad-V: Frame latency for the '/topic' provided by user
* PR `#2872 <https://github.com/realsenseai/realsense-ros/issues/2872>`_ from Arun-Prasad-V: Updating _camera_name with RS node's name
* PR `#2878 <https://github.com/realsenseai/realsense-ros/issues/2878>`_ from Arun-Prasad-V: Updated ros2 examples and readme
* PR `#2841 <https://github.com/realsenseai/realsense-ros/issues/2841>`_ from SamerKhshiboun: Remove Dashing, Eloquent, Foxy, L500 and SR300 support
* PR `#2868 <https://github.com/realsenseai/realsense-ros/issues/2868>`_ from Arun-Prasad-V: Fix Pointcloud topic frame_id
* PR `#2849 <https://github.com/realsenseai/realsense-ros/issues/2849>`_ from Arun-Prasad-V: Create /imu topic only when motion streams enabled
* PR `#2847 <https://github.com/realsenseai/realsense-ros/issues/2847>`_ from Arun-Prasad-V: Updated rs_launch param names
* PR `#2839 <https://github.com/realsenseai/realsense-ros/issues/2839>`_ from Arun-Prasad: Added ros2 examples
* PR `#2861 <https://github.com/realsenseai/realsense-ros/issues/2861>`_ from SamerKhshiboun: fix readme and nodefactory for ros2 run
* PR `#2859 <https://github.com/realsenseai/realsense-ros/issues/2859>`_ from PrasRsRos: Fix tests (topic now has camera name)
* PR `#2857 <https://github.com/realsenseai/realsense-ros/issues/2857>`_ from lge-ros2: Apply camera name in topics
* PR `#2840 <https://github.com/realsenseai/realsense-ros/issues/2840>`_ from SamerKhshiboun: Support Depth, IR and Color formats in ROS2
* PR `#2764 <https://github.com/realsenseai/realsense-ros/issues/2764>`_ from lge-ros2 : support modifiable camera namespace
* PR `#2830 <https://github.com/realsenseai/realsense-ros/issues/2830>`_ from SamerKhshiboun: Add RGBD + reduce changes between hkr and development
* PR `#2811 <https://github.com/realsenseai/realsense-ros/issues/2811>`_ from Arun-Prasad-V: Exposing stream formats params to user
* PR `#2825 <https://github.com/realsenseai/realsense-ros/issues/2825>`_ from SamerKhshiboun: Fix align_depth + add test
* PR `#2822 <https://github.com/realsenseai/realsense-ros/issues/2822>`_ from Arun-Prasad-V: Updated rs_launch configurations
* PR `#2726 <https://github.com/realsenseai/realsense-ros/issues/2726>`_ from PrasRsRos: Integration test template
* PR `#2742 <https://github.com/realsenseai/realsense-ros/issues/2742>`_ from danielhonies:Update rs_launch.py
* PR `#2806 <https://github.com/realsenseai/realsense-ros/issues/2806>`_ from Arun-Prasad-V: Enabling RGB8 Infrared stream
* PR `#2799 <https://github.com/realsenseai/realsense-ros/issues/2799>`_ from SamerKhshiboun: Fix overriding frames on same topics/CV-images due to a bug in PR2759
* PR `#2759 <https://github.com/realsenseai/realsense-ros/issues/2759>`_ from SamerKhshiboun: Cleanups and name fixes
* Contributors: (=YG=) Hyunseok Yang, Arun Prasad, Arun-Prasad-V, Daniel Honies, Hyunseok, Madhukar Reddy Kadireddy, Nir, Nir Azkiel, PrasRsRos, Samer Khshiboun, SamerKhshiboun, deep0294, gwen2018, nairps
4.54.1 (2023-06-27)
-------------------
* Applying AlignDepth filter after Pointcloud
* Publish /aligned_depth_to_color topic only when color frame present
* Support Iron distro
* Protect empty string dereference
* Fix: /tf and /static_tf topics' inconsistencies
* Revamped the TF related code
* Fixing TF frame links b/w multi camera nodes when using custom names
* Updated TF descriptions in launch py and readme
* Fixing /tf topic has only TFs of last started sensor
* add D430i support
* Fix Swapped TFs Axes
* replace stereo module with depth module
* use rs2_to_ros to replace stereo module with depth moudle
* calculate extriniscs twice in two opposite ways to save inverting rotation matrix
* fix matrix rotation
* Merge branch 'ros2-development' into readme_fix
* invert translation
* Added 'publish_tf' param in rs launch files
* Indentation corrections
* Fix: Don't publish /tf when publish_tf is false
* use playback device for rosbags
* Avoid configuring dynamic_tf_broadcaster within tf_publish_rate param's callback
* Fix lower FPS in D405, D455
* update rs_launch.py to support enable_auto_exposure and manual exposure
* fix timestamp calculation metadata header to be aligned with metadata json timestamp
* Expose USB port in DeviceInfo service
* Use latched QoS for Extrinsic topic when intra-process is used
* add cppcheck to GHA
* Fix Apache License Header and Intel Copyrights
* apply copyrights and license on project
* Enable intra-process communication for point clouds
* Fix ros2 parameter descriptions and range values
* T265 clean up
* fix float_to_double method
* realsense2_camera/src/sensor_params.cpp
* remove T265 device from ROS Wrapper - step1
* Enable D457
* Fix hdr_merge filter initialization in ros2 launch
* if default profile is not defined, take the first available profile as default
* changed to static_cast and added descriptor name and type
* remove extra ';'
* remove unused variable format_str
* publish point cloud via unique shared pointer
* make source backward compatible to older versions of cv_bridge and rclcpp
* add hdr_merge.enable and depth_module.hdr_enabled to rs_launch.py
* fix compilation errors
* fix tabs
* if default profile is not defined, take the first available profile as default
* Fix ros2 sensor controls steps and add control default value to param description
* Publish static transforms when intra porocess communication is enabled
* Properly read camera config files in rs_launch.py
* fix deprecated API
* Add D457
* Windows bring-up
* publish actual IMU optical frame ID in IMU messages
* Publish static tf for IMU frames
* fix extrinsics calculation
* fix ordered_pc arg prefix
* publish IMU frames only if unite/sync imu method is not none
* Publish static tf for IMU frames
* add D430i support
* Contributors: Arun Prasad, Arun Prasad V, Arun-Prasad-V, Christian Rauch, Daniel Honies, Gilad Bretter, Nir Azkiel, NirAz, Pranav Dhulipala, Samer Khshiboun, SamerKhshiboun, Stephan Wirth, Xiangyu, Yadunund, nvidia
4.51.1 (2022-09-13)
-------------------
* Fix crash when activating IMU & aligned depth together
* Fix rosbag device loading by preventing set_option to HDR/Gain/Exposure
* Support ROS2 Humble
* Publish real frame rate of realsense camera node topics/publishers
* No need to start/stop sensors for align depth changes
* Fix colorizer filter which returns null reference ptr
* Fix align_depth enable/disable
* Add colorizer.enable to rs_launch.py
* Add copyright and license to all ROS2-beta source files
* Fix CUDA suffix for pointcloud and align_depth topics
* Add ROS build farm pre-release to ci
* Contributors: Eran, NirAz, SamerKhshiboun
4.0.4 (2022-03-20)
------------------
* fix required packages for building debians for ros2-beta branch
* Contributors: NirAz
4.0.3 (2022-03-16)
------------------
* Support intra-process zero-copy
* Update README
* Fix Galactic deprecated-declarations compilation warning
* Fix Eloquent compilation error
* Contributors: Eran, Nir-Az, SamerKhshiboun
4.0.2 (2022-02-24)
------------------
* version 4.4.0 changed to 4.0.0 in CHANGELOG
* add frequency monitoring to /diagnostics topic.
* fix topic_hz.py to recognize message type from topic name. (Naive)
* move diagnostic updater for stream frequencies into the RosSensor class.
* add frequency monitoring to /diagnostics topic.
* fix galactic issue with undeclaring parameters
* fix to support Rolling.
* fix dynamic_params syntax.
* fix issue with Galactic parameters set by default to static which prevents them from being undeclared.
* Contributors: Haowei Wen, doronhi, remibettan
4.0.1 (2022-02-01)
------------------
* fix reset issue when multiple devices are connected
* fix /rosout issue
* fix PID for D405 device
* fix bug: frame_id is based on camera_name
* unite_imu_method is now changeable in runtime.
* fix motion module default values.
* add missing extrinsics topics
* fix crash when camera disconnects.
* fix header timestamp for metadata messages.
* Contributors: nomumu, JamesChooWK, benlev, doronhi
4.0.0 (2021-11-17)
-------------------
* changed parameters:
- "stereo_module", "l500_depth_sensor" are replaced by "depth_module"
- for video streams: <module>.profile replaces <stream>_width, <stream>_height, <stream>_fps
- removed paramets <stream>_frame_id, <stream>_optical_frame_id. frame_ids are defined by camera_name
- "filters" is removed. All filters (or post-processing blocks) are enabled/disabled using "<filter>.enable"
- "align_depth" is replaced with "align_depth.enable"
- "allow_no_texture_points", "ordered_pc" replaced by "pointcloud.allow_no_texture_points", "pointcloud.ordered_pc"
- "pointcloud_texture_stream", "pointcloud_texture_index" are replaced by "pointcloud.stream_filter", "pointcloud.stream_index_filter"
* Allow enable/disable of sensors in runtime.
* Allow enable/disable of filters in runtime.
src/realsense2_camera/CMakeLists.txt
-469
View File
@@ -1,469 +0,0 @@
# Copyright 2024 RealSense, Inc. All Rights Reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
cmake_minimum_required(VERSION 3.10)
project(realsense2_camera)
# Default to C99
if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
option(BUILD_WITH_OPENMP "Use OpenMP" OFF)
option(SET_USER_BREAK_AT_STARTUP "Set user wait point in startup (for debug)" OFF)
# Define an option to enable or disable lifecycle nodes
option(USE_LIFECYCLE_NODE "Enable lifecycle nodes (ON/OFF)" OFF)
# Compiler Defense Flags
if(UNIX OR APPLE)
# Linker flags.
if(${CMAKE_CXX_COMPILER_ID} STREQUAL "GNU" OR ${CMAKE_CXX_COMPILER_ID} STREQUAL "Intel")
# GCC specific flags. ICC is compatible with them.
set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -z noexecstack -z relro -z now")
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -z noexecstack -z relro -z now")
elseif(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang")
# In Clang, -z flags are not compatible, they need to be passed to linker via -Wl.
set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,-z,noexecstack -Wl,-z,relro -Wl,-z,now")
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,-z,noexecstack -Wl,-z,relro -Wl,-z,now")
endif()
# Compiler flags.
if(${CMAKE_CXX_COMPILER_ID} STREQUAL "GNU")
# GCC specific flags.
if(CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 4.9 OR CMAKE_CXX_COMPILER_VERSION VERSION_EQUAL 4.9)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIE -fstack-protector-strong")
else()
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIE -fstack-protector")
endif()
elseif(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang")
# Clang is compatbile with some of the flags.
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIE -fstack-protector")
elseif(${CMAKE_CXX_COMPILER_ID} STREQUAL "Intel")
# Same as above, with exception that ICC compilation crashes with -fPIE option, even
# though it uses -pie linker option that require -fPIE during compilation. Checksec
# shows that it generates correct PIE anyway if only -pie is provided.
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fstack-protector")
endif()
# Generic flags.
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC -fno-operator-names -Wformat -Wformat-security -Wall")
# Dot not forward c++ flag to GPU beucause it is not supported
set( CUDA_PROPAGATE_HOST_FLAGS OFF )
set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -D_FORTIFY_SOURCE=2")
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -pie")
endif()
if(WIN32)
add_definitions(-D_USE_MATH_DEFINES)
endif()
if(NOT CMAKE_BUILD_TYPE)
set(CMAKE_BUILD_TYPE Release)
endif()
string(TOUPPER "${CMAKE_BUILD_TYPE}" uppercase_CMAKE_BUILD_TYPE)
if (${uppercase_CMAKE_BUILD_TYPE} STREQUAL "RELEASE")
message(STATUS "Create Release Build.")
set(CMAKE_CXX_FLAGS "-O2 ${CMAKE_CXX_FLAGS}")
else()
message(STATUS "Create Debug Build.")
endif()
if(BUILD_WITH_OPENMP)
find_package(OpenMP)
if(NOT OpenMP_FOUND)
message(FATAL_ERROR "\n\n OpenMP is missing!\n\n")
else()
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS} -fopenmp")
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
endif()
endif()
if(SET_USER_BREAK_AT_STARTUP)
message("GOT FLAG IN CmakeLists.txt")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DBPDEBUG")
endif()
find_package(ament_cmake REQUIRED)
find_package(builtin_interfaces REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(realsense2_camera_msgs REQUIRED)
find_package(std_srvs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2 REQUIRED)
find_package(diagnostic_updater REQUIRED)
find_package(OpenCV REQUIRED COMPONENTS core)
find_package(rclcpp_components REQUIRED)
if(TARGET rclcpp_components::component)
set(RCLCPP_COMPONENT_TARGET rclcpp_components::component)
else()
# Foxy fallback
set(RCLCPP_COMPONENT_TARGET)
endif()
find_package(image_transport REQUIRED)
if(TARGET image_transport::image_transport)
set(IMAGE_TRANSPORT_TARGET image_transport::image_transport)
else()
# Foxy fallback
set(IMAGE_TRANSPORT_TARGET ${image_transport_LIBRARIES})
endif()
find_package(cv_bridge REQUIRED)
if(TARGET cv_bridge::cv_bridge)
set(CV_BRIDGE_TARGET cv_bridge::cv_bridge)
else()
# Foxy fallback
set(CV_BRIDGE_TARGET ${cv_bridge_LIBRARIES})
endif()
find_package(sensor_msgs REQUIRED)
if(TARGET sensor_msgs::sensor_msgs_library)
set(SENSOR_MSGS_TARGET sensor_msgs::sensor_msgs_library)
else()
# Foxy fallback
set(SENSOR_MSGS_TARGET ${sensor_msgs_LIBRARIES})
endif()
find_package(realsense2 2.56.6)
if (BUILD_ACCELERATE_GPU_WITH_GLSL)
find_package(realsense2-gl 2.56.6)
endif()
if(NOT realsense2_FOUND)
message(FATAL_ERROR "\n\n RealSense SDK 2.0 is missing, please install it from https://github.com/realsenseai/librealsense/releases\n\n")
endif()
#set(CMAKE_NO_SYSTEM_FROM_IMPORTED true)
include_directories(include)
include_directories(${OpenCV_INCLUDE_DIRS}) # add OpenCV includes to the included dirs
set(node_plugins "")
set(SOURCES
src/realsense_node_factory.cpp
src/base_realsense_node.cpp
src/parameters.cpp
src/rs_node_setup.cpp
src/ros_sensor.cpp
src/ros_utils.cpp
src/dynamic_params.cpp
src/sensor_params.cpp
src/named_filter.cpp
src/pointcloud_filter.cpp
src/align_depth_filter.cpp
src/profile_manager.cpp
src/image_publisher.cpp
src/tfs.cpp
src/actions.cpp
src/safety.cpp
)
if (BUILD_ACCELERATE_GPU_WITH_GLSL)
list(APPEND SOURCES src/gl_gpu_processing.cpp)
endif()
if(NOT DEFINED ENV{ROS_DISTRO})
message(FATAL_ERROR "ROS_DISTRO is not defined." )
endif()
if("$ENV{ROS_DISTRO}" STREQUAL "foxy")
message(STATUS "Build for ROS2 Foxy")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DFOXY")
set(SOURCES "${SOURCES}" src/ros_param_backend.cpp)
elseif("$ENV{ROS_DISTRO}" STREQUAL "humble")
message(STATUS "Build for ROS2 Humble")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DHUMBLE")
set(SOURCES "${SOURCES}" src/ros_param_backend.cpp)
elseif("$ENV{ROS_DISTRO}" STREQUAL "iron")
message(STATUS "Build for ROS2 Iron")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DIRON")
set(SOURCES "${SOURCES}" src/ros_param_backend.cpp)
elseif("$ENV{ROS_DISTRO}" STREQUAL "rolling")
message(STATUS "Build for ROS2 Rolling")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DROLLING")
set(SOURCES "${SOURCES}" src/ros_param_backend.cpp)
elseif("$ENV{ROS_DISTRO}" STREQUAL "jazzy")
message(STATUS "Build for ROS2 Jazzy")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DJAZZY")
set(SOURCES "${SOURCES}" src/ros_param_backend.cpp)
elseif("$ENV{ROS_DISTRO}" STREQUAL "kilted")
message(STATUS "Build for ROS2 Kilted")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DKILTED")
set(SOURCES "${SOURCES}" src/ros_param_backend.cpp)
else()
message(FATAL_ERROR "Unsupported ROS Distribution: " "$ENV{ROS_DISTRO}")
endif()
# The header 'cv_bridge/cv_bridge.hpp' was added in version 3.3.0. For older
# cv_bridge versions, we have to use the header 'cv_bridge/cv_bridge.h'.
if(${cv_bridge_VERSION} VERSION_GREATER_EQUAL "3.3.0")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DCV_BRDIGE_HAS_HPP")
endif()
# 'OnSetParametersCallbackType' is only defined for rclcpp 17 and onward.
if(${rclcpp_VERSION} VERSION_GREATER_EQUAL "17.0")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DRCLCPP_HAS_OnSetParametersCallbackType")
endif()
if (BUILD_ACCELERATE_GPU_WITH_GLSL)
add_definitions(-DACCELERATE_GPU_WITH_GLSL)
endif()
set(INCLUDES
include/context_singleton_wrapper.h
include/constants.h
include/realsense_node_factory.h
include/base_realsense_node.h
include/ros_sensor.h
include/ros_utils.h
include/dynamic_params.h
include/sensor_params.h
include/named_filter.h
include/pointcloud_filter.h
include/align_depth_filter.h
include/ros_param_backend.h
include/profile_manager.h
include/image_publisher.h)
if (BUILD_ACCELERATE_GPU_WITH_GLSL)
list(APPEND INCLUDES include/gl_window.h)
endif()
if (BUILD_TOOLS)
include_directories(tools)
set(INCLUDES ${INCLUDES}
tools/frame_latency/frame_latency.h)
set(SOURCES ${SOURCES}
tools/frame_latency/frame_latency.cpp)
endif()
add_library(${PROJECT_NAME} SHARED
${INCLUDES}
${SOURCES}
)
if (BUILD_ACCELERATE_GPU_WITH_GLSL)
set(link_libraries ${realsense2-gl_LIBRARY})
else()
set(link_libraries ${realsense2_LIBRARY})
endif()
list(APPEND link_libraries ${OpenCV_LIBS}) # add OpenCV libs to link_libraries
target_link_libraries(${PROJECT_NAME}
${link_libraries}
)
set(dependencies
cv_bridge
image_transport
rclcpp
rclcpp_components
realsense2_camera_msgs
std_srvs
std_msgs
sensor_msgs
nav_msgs
tf2
tf2_ros
diagnostic_updater
)
set (targets
${CV_BRIDGE_TARGET}
${IMAGE_TRANSPORT_TARGET}
rclcpp::rclcpp
${RCLCPP_COMPONENT_TARGET}
${realsense2_camera_msgs_TARGETS}
${std_srvs_TARGETS}
${std_msgs_TARGETS}
${SENSOR_MSGS_TARGET}
${nav_msgs_TARGETS}
tf2::tf2
tf2_ros::tf2_ros
diagnostic_updater::diagnostic_updater
)
if (BUILD_ACCELERATE_GPU_WITH_GLSL)
list(APPEND dependencies realsense2-gl)
list(APPEND targets realsense2-gl::realsense2-gl)
else()
list(APPEND dependencies realsense2)
list(APPEND targets realsense2::realsense2)
endif()
# If the flag is enabled, define the macro
if(USE_LIFECYCLE_NODE)
find_package(rclcpp_lifecycle REQUIRED)
find_package(lifecycle_msgs REQUIRED)
list(APPEND dependencies rclcpp_lifecycle lifecycle_msgs)
list(APPEND targets
rclcpp_lifecycle::rclcpp_lifecycle
lifecycle_msgs::lifecycle_msgs__rosidl_typesupport_cpp)
add_definitions(-DUSE_LIFECYCLE_NODE)
message("🚀 USE_LIFECYCLE_NODE is ENABLED")
# Create a configuration file for the launch file
file(WRITE "${CMAKE_BINARY_DIR}/global_settings.yaml"
"use_lifecycle_node: true\n")
else()
file(WRITE "${CMAKE_BINARY_DIR}/global_settings.yaml"
"use_lifecycle_node: false\n")
endif()
install(FILES "${CMAKE_BINARY_DIR}/global_settings.yaml"
DESTINATION share/${PROJECT_NAME}/config)
target_link_libraries(${PROJECT_NAME}
${targets}
)
rclcpp_components_register_node(${PROJECT_NAME}
PLUGIN "realsense2_camera::RealSenseNodeFactory"
EXECUTABLE realsense2_camera_node
)
if(BUILD_TOOLS)
rclcpp_components_register_node(${PROJECT_NAME}
PLUGIN "rs2_ros::tools::frame_latency::FrameLatencyNode"
EXECUTABLE realsense2_frame_latency_node
)
endif()
# Install binaries
install(TARGETS ${PROJECT_NAME}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
# Install headers
install(
DIRECTORY include/
DESTINATION include
)
# Install launch files
install(DIRECTORY
launch
DESTINATION share/${PROJECT_NAME}
)
# Install example files
install(DIRECTORY
examples
DESTINATION share/${PROJECT_NAME}
)
# Test
if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
set(_gtest_folders
test
)
foreach(test_folder ${_gtest_folders})
file(GLOB files "${test_folder}/gtest_*.cpp")
foreach(file ${files})
get_filename_component(_test_name ${file} NAME_WE)
ament_add_gtest(${_test_name} ${file})
target_include_directories(${_test_name} PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
target_link_libraries(${_test_name}
std_srvs::std_srvs__rosidl_typesupport_cpp
std_msgs::std_msgs__rosidl_typesupport_cpp
)
#target_link_libraries(${_test_name} name_of_local_library)
endforeach()
endforeach()
find_package(ament_cmake_pytest REQUIRED)
set(_pytest_folders
test
test/templates
test/rosbag
test/post_processing_filters
)
foreach(test_folder ${_pytest_folders})
file(GLOB files "${test_folder}/test_*.py")
foreach(file ${files})
get_filename_component(_test_name ${file} NAME_WE)
ament_add_pytest_test(${_test_name} ${file}
APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR}:${CMAKE_SOURCE_DIR}/test/utils:${CMAKE_SOURCE_DIR}/launch:${CMAKE_SOURCE_DIR}/scripts
TIMEOUT 120
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
)
endforeach()
endforeach()
unset(_pytest_folders)
set(rs_query_cmd "rs-enumerate-devices -s")
execute_process(COMMAND bash -c ${rs_query_cmd}
WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}
RESULT_VARIABLE rs_result
OUTPUT_VARIABLE RS_DEVICE_INFO)
message(STATUS "rs_device_info:")
message(STATUS "${RS_DEVICE_INFO}")
if((RS_DEVICE_INFO MATCHES "D455") OR (RS_DEVICE_INFO MATCHES "D415") OR (RS_DEVICE_INFO MATCHES "D435"))
message(STATUS "D455 device found")
set(_pytest_live_folders
test/live_camera
)
endif()
foreach(test_folder ${_pytest_live_folders})
file(GLOB files "${test_folder}/test_*.py")
foreach(file ${files})
get_filename_component(_test_name ${file} NAME_WE)
ament_add_pytest_test(${_test_name} ${file}
APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR}:${CMAKE_SOURCE_DIR}/test/utils:${CMAKE_SOURCE_DIR}/launch:${CMAKE_SOURCE_DIR}/scripts
TIMEOUT 500
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
)
endforeach()
endforeach()
endif()
# Ament exports
ament_export_include_directories(include)
ament_export_libraries(${PROJECT_NAME})
ament_export_dependencies(${dependencies})
ament_package()
src/realsense2_camera/examples/align_depth/README.md
-12
View File
@@ -1,12 +0,0 @@
# Align Depth to Color
This example shows how to start the camera node and align depth stream to color stream.
```
ros2 launch realsense2_camera rs_align_depth_launch.py
```
The aligned image will be published to the topic "/aligned_depth_to_color/image_raw"
Also, align depth to color can enabled by following cmd:
```
ros2 launch realsense2_camera rs_launch.py align_depth.enable:=true
```
src/realsense2_camera/examples/align_depth/rs_align_depth_launch.py
-56
View File
@@ -1,56 +0,0 @@
# Copyright 2023 RealSense, Inc. All Rights Reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# DESCRIPTION #
# ----------- #
# Use this launch file to launch a device and align depth to color.
# The Parameters available for definition in the command line for the camera are described in rs_launch.configurable_parameters
# command line example:
# ros2 launch realsense2_camera rs_align_depth_launch.py
"""Launch realsense2_camera node."""
from launch import LaunchDescription
import launch_ros.actions
from launch.actions import OpaqueFunction
from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir
import sys
import pathlib
sys.path.append(str(pathlib.Path(__file__).parent.absolute()))
import os
from ament_index_python.packages import get_package_share_directory
sys.path.append(os.path.join(get_package_share_directory('realsense2_camera'), 'launch'))
import rs_launch
local_parameters = [{'name': 'camera_name', 'default': 'camera', 'description': 'camera unique name'},
{'name': 'camera_namespace', 'default': 'camera', 'description': 'camera namespace'},
{'name': 'enable_color', 'default': 'true', 'description': 'enable color stream'},
{'name': 'enable_depth', 'default': 'true', 'description': 'enable depth stream'},
{'name': 'align_depth.enable', 'default': 'true', 'description': 'enable align depth filter'},
{'name': 'enable_sync', 'default': 'true', 'description': 'enable sync mode'},
]
def set_configurable_parameters(local_params):
return dict([(param['name'], LaunchConfiguration(param['name'])) for param in local_params])
def generate_launch_description():
params = rs_launch.configurable_parameters
return LaunchDescription(
rs_launch.declare_configurable_parameters(local_parameters) +
rs_launch.declare_configurable_parameters(params) +
[
OpaqueFunction(function=rs_launch.launch_setup,
kwargs = {'params' : set_configurable_parameters(params)}
)
])
src/realsense2_camera/examples/d500_tables/application_config_example.json
-54
View File
@@ -1,54 +0,0 @@
{
"application_config":
{
"sip":
{
"immediate_mode_safety_features_selection": 0,
"temporal_safety_features_selection": 0,
"mechanisms_thresholds": [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
"mechanisms_sampling_interval": [0, 0, 0, 0, 0, 0, 0, 0],
"tc_consecutives_failures_threshold": 3
},
"dev_rules_selection": 0,
"depth_pipe_safety_checks_override": 0,
"triggered_calib_safety_checks_override": 0,
"smcu_bypass_directly_to_maintenance_mode": 0,
"smcu_skip_spi_error": 0,
"temp_thresholds":
{
"ir_right": [0, 0, 0, 0],
"ir_left": [0, 0, 0, 0],
"apm_left": [0, 0, 0, 0],
"apm_right": [0, 0, 0, 0],
"hkr_core": [0, 0, 0, 0],
"smcu_right": [0, 0, 0, 0],
"sht4x": [0, 0, 0, 0],
"imu": [0, 0, 0, 0]
},
"sht4x_humidity_threshold": 23,
"voltage_thresholds":
{
"vdd3v3": 0,
"vdd1v8": 0,
"vdd1v2": 0,
"vdd1v1": 0,
"vdd0v8": 0,
"vdd0v6": 0,
"vdd5vo_u": 0,
"vdd5vo_l": 0,
"vdd0v8_ddr": 0
},
"developer_mode":
{
"hkr": 0,
"smcu": 0,
"hkr_simulated_lock_state": 0,
"sc": 0
},
"depth_pipeline_config": 0,
"depth_roi": 0,
"ir_for_sip": 0,
"peripherals_sensors_disable_mask": 0,
"digital_signature": [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
}
}
src/realsense2_camera/examples/d500_tables/calib_config_example.json
-45
View File
@@ -1,45 +0,0 @@
{
"calibration_config":
{
"roi_num_of_segments": 2,
"roi_0":
{
"vertex_0": [ 0, 36 ],
"vertex_1": [ 640, 144 ],
"vertex_2": [ 640, 576 ],
"vertex_3": [ 0, 684 ]
},
"roi_1":
{
"vertex_0": [ 640, 144 ],
"vertex_1": [ 1280, 35 ],
"vertex_2": [ 1280, 684 ],
"vertex_3": [ 640, 576 ]
},
"roi_2":
{
"vertex_0": [ 0, 0 ],
"vertex_1": [ 0, 0 ],
"vertex_2": [ 0, 0 ],
"vertex_3": [ 0, 0 ]
},
"roi_3":
{
"vertex_0": [ 0, 0 ],
"vertex_1": [ 0, 0 ],
"vertex_2": [ 0, 0 ],
"vertex_3": [ 0, 0 ]
},
"camera_position":
{
"rotation":
[
[ 0.0, 0.0, 1.0],
[-1.0, 0.0, 0.0],
[ 0.0, -1.0, 0.0]
],
"translation": [0.0, 0.0, 0.27]
},
"crypto_signature": [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
}
}
src/realsense2_camera/examples/d500_tables/safety_interface_config_example.json
-124
View File
@@ -1,124 +0,0 @@
{
"safety_interface_config":
{
"m12_safety_pins_configuration":
{
"power":
{
"direction": "In",
"functionality": "p24VDC"
},
"ossd1_b":
{
"direction": "Out",
"functionality": "pOSSD1_B"
},
"ossd1_a":
{
"direction": "Out",
"functionality": "pOSSD1_A"
},
"preset3_a":
{
"direction": "In",
"functionality": "pPresetSelect3_A"
},
"preset3_b":
{
"direction": "In",
"functionality": "pPresetSelect3_B"
},
"preset4_a":
{
"direction": "In",
"functionality": "pPresetSelect4_A"
},
"preset1_b":
{
"direction": "In",
"functionality": "pPresetSelect1_B"
},
"preset1_a":
{
"direction": "In",
"functionality": "pPresetSelect1_A"
},
"gpio_0":
{
"direction": "In",
"functionality": "pPresetSelect5_A"
},
"gpio_1":
{
"direction": "In",
"functionality": "pPresetSelect5_B"
},
"gpio_3":
{
"direction": "In",
"functionality": "pPresetSelect6_B"
},
"gpio_2":
{
"direction": "In",
"functionality": "pPresetSelect6_A"
},
"preset2_b":
{
"direction": "In",
"functionality": "pPresetSelect2_B"
},
"gpio_4":
{
"direction": "Out",
"functionality": "pDeviceReady"
},
"preset2_a":
{
"direction": "In",
"functionality": "pPresetSelect2_A"
},
"preset4_b":
{
"direction": "In",
"functionality": "pPresetSelect4_B"
},
"ground":
{
"direction": "In",
"functionality": "pGND"
}
},
"gpio_stabilization_interval" : 150,
"camera_position":
{
"rotation":
[
[ 0.0, 0.0, 1.0],
[-1.0, 0.0, 0.0],
[ 0.0, -1.0, 0.0]
],
"translation": [0.0, 0.0, 0.27]
},
"occupancy_grid_params":
{
"grid_cell_seed" : 20,
"close_range_quorum" : 12 ,
"mid_range_quorum" : 6,
"long_range_quorum" : 4
},
"smcu_arbitration_params":
{
"l_0_total_threshold": 100,
"l_0_sustained_rate_threshold": 20,
"l_1_total_threshold": 100,
"l_1_sustained_rate_threshold": 20,
"l_2_total_threshold": 10,
"hkr_stl_timeout": 15,
"mcu_stl_timeout": 10,
"sustained_aicv_frame_drops": 95,
"ossd_self_test_pulse_width": 23
},
"crypto_signature": [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
}
}
src/realsense2_camera/examples/d500_tables/safety_preset_example.json
-165
View File
@@ -1,165 +0,0 @@
{
"safety_preset":
{
"platform_config":
{
"transformation_link":
{
"rotation":
[
[ 0.0, 0.0, 1.0],
[-1.0, 0.0, 0.0],
[ 0.0, -1.0, 0.0]
],
"translation": [0.0, 0.0, 0.27]
},
"robot_height": 1.0,
"reserved": [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
},
"safety_zones":
{
"danger_zone":
{
"zone_polygon":
{
"p0": {"x": 0.5, "y": 0.1},
"p1": {"x": 0.8, "y": 0.1},
"p2": {"x": 0.8, "y": -0.1},
"p3": {"x": 0.5, "y": -0.1}
},
"safety_trigger_confidence": 3,
"reserved": [0, 0, 0, 0, 0, 0, 0]
},
"warning_zone":
{
"zone_polygon":
{
"p0": {"x": 0.8, "y": 0.1},
"p1": {"x": 1.2, "y": 0.1},
"p2": {"x": 1.2, "y": -0.1},
"p3": {"x": 0.8, "y": -0.1}
},
"safety_trigger_confidence": 3,
"reserved": [0, 0, 0, 0, 0, 0, 0]
}
},
"masking_zones":
{
"0":
{
"attributes": 0,
"minimal_range": 0.5,
"region_of_interests":
{
"vertex_0": [0, 0],
"vertex_1": [0, 320],
"vertex_2": [200, 320],
"vertex_3": [200, 0]
}
},
"1":
{
"attributes": 0,
"minimal_range": 0.5,
"region_of_interests":
{
"vertex_0": [0, 0],
"vertex_1": [0, 320],
"vertex_2": [200, 320],
"vertex_3": [200, 0]
}
},
"2":
{
"attributes": 0,
"minimal_range": 0.5,
"region_of_interests":
{
"vertex_0": [0, 0],
"vertex_1": [0, 320],
"vertex_2": [200, 320],
"vertex_3": [200, 0]
}
},
"3":
{
"attributes": 0,
"minimal_range": 0.5,
"region_of_interests":
{
"vertex_0": [0, 0],
"vertex_1": [0, 320],
"vertex_2": [200, 320],
"vertex_3": [200, 0]
}
},
"4":
{
"attributes": 0,
"minimal_range": 0.5,
"region_of_interests":
{
"vertex_0": [0, 0],
"vertex_1": [0, 320],
"vertex_2": [200, 320],
"vertex_3": [200, 0]
}
},
"5":
{
"attributes": 0,
"minimal_range": 0.5,
"region_of_interests":
{
"vertex_0": [0, 0],
"vertex_1": [0, 320],
"vertex_2": [200, 320],
"vertex_3": [200, 0]
}
},
"6":
{
"attributes": 0,
"minimal_range": 0.5,
"region_of_interests":
{
"vertex_0": [0, 0],
"vertex_1": [0, 320],
"vertex_2": [200, 320],
"vertex_3": [200, 0]
}
},
"7":
{
"attributes": 1,
"minimal_range": 0,
"region_of_interests":
{
"vertex_0": [500, 3300],
"vertex_1": [800, 3300],
"vertex_2": [800, 3100],
"vertex_3": [500, 3100]
}
}
},
"reserved": [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
"environment":
{
"safety_trigger_duration": 1.0,
"zero_safety_monitoring": 0,
"hara_history_continuation": 0,
"reserved1": [0, 0],
"angular_velocity": 0.0,
"payload_weight": 0.0,
"surface_inclination": 15.0,
"surface_height": 0.05,
"diagnostic_zone_fill_rate_threshold": 255,
"floor_fill_threshold": 255,
"depth_fill_threshold": 255,
"diagnostic_zone_height_median_threshold": 255,
"vision_hara_persistency": 1,
"crypto_signature": [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
"reserved2": [0, 0, 0]
}
}
}
src/realsense2_camera/examples/dual_camera/README.md
-123
View File
@@ -1,123 +0,0 @@
# Launching Dual RS ROS2 nodes
The following example lanches two RS ROS2 nodes.
```
ros2 launch realsense2_camera rs_dual_camera_launch.py serial_no1:=<serial number of the first camera> serial_no2:=<serial number of the second camera>
```
## Example:
Let's say the serial numbers of two RS cameras are 207322251310 and 234422060144.
```
ros2 launch realsense2_camera rs_dual_camera_launch.py serial_no1:="'207322251310'" serial_no2:="'234422060144'"
```
or
```
ros2 launch realsense2_camera rs_dual_camera_launch.py serial_no1:=_207322251310 serial_no2:=_234422060144
```
## How to know the serial number?
Method 1: Using the rs-enumerate-devices tool
```
rs-enumerate-devices | grep "Serial Number"
```
Method 2: Connect single camera and run
```
ros2 launch realsense2_camera rs_launch.py
```
and look for the serial number in the log printed to screen under "[INFO][...] Device Serial No:".
# Using Multiple RS camera by launching each in differnet terminals
Make sure you set a different name and namespace for each camera.
Terminal 1:
```
ros2 launch realsense2_camera rs_launch.py serial_no:="'207322251310'" camera_name:='camera1' camera_namespace:='camera1'
```
Terminal 2:
```
ros2 launch realsense2_camera rs_launch.py serial_no:="'234422060144'" camera_name:='camera2' camera_namespace:='camera2'
```
# Multiple cameras showing a semi-unified pointcloud
The D430 series of RealSense cameras use stereo based algorithm to calculate depth. This mean, a couple of cameras can operate on the same scene. For the purpose of this demonstration, let's say 2 cameras can be coupled to look at the same scene from 2 different points of view. See image:
![multi_cameras](https://user-images.githubusercontent.com/127019120/268692789-1b3d5d8b-a41f-4a97-995d-81d44b4bcacb.jpg)
The schematic settings could be described as:
X--------------------------------->cam_2
|&emsp;&emsp;&emsp;&emsp;(70 cm)
|
|
|&ensp;(60 cm)
|
|
/
cam_1
The cameras have no data regarding their relative position. That’s up to a third party program to set. To simplify things, the coordinate system of cam_1 can be considered as the refernce coordinate system for the whole scene.
The estimated translation of cam_2 from cam_1 is 70(cm) on X-axis and 60(cm) on Y-axis. Also, the estimated yaw angle of cam_2 relative to cam_1 as 90(degrees) clockwise. These are the initial parameters to be set for setting the transformation between the 2 cameras as follows:
```
ros2 launch realsense2_camera rs_dual_camera_launch.py serial_no1:=_207322251310 serial_no2:=_234422060144 tf.translation.x:=0.7 tf.translation.y:=0.6 tf.translation.z:=0.0 tf.rotation.yaw:=-90.0 tf.rotation.pitch:=0.0 tf.rotation.roll:=0.0
```
If the unified pointcloud result is not good, follow the below steps to fine-tune the calibaration.
## Visualizing the pointclouds and fine-tune the camera calibration
Launch 2 cameras in separate terminals:
**Terminal 1:**
```
ros2 launch realsense2_camera rs_launch.py serial_no:="'207322251310'" camera_name:='camera1' camera_namespace:='camera1'
```
**Terminal 2:**
```
ros2 launch realsense2_camera rs_launch.py serial_no:="'234422060144'" camera_name:='camera2' camera_namespace:='camera2'
```
**Terminal 3:**
```
rviz2
```
Open rviz and set '“Fixed Frame'” to “camera1_link”
Add Pointcloud2-> By topic -> /camera1/camera1/depth/color/points
Add Pointcloud2 -> By topic -> /camera2/camera2/depth/color/points
**Terminal 4:**
Run the 'set_cams_transforms.py' tool. It can be used to fine-tune the calibaration.
```
python src/realsense-ros/realsense2_camera/scripts/set_cams_transforms.py camera1_link camera2_link 0.7 0.6 0 -90 0 0
```
**Instructions printed by the tool:**
```
Using default file /home/user_name/ros2_ws/src/realsense-ros/realsense2_camera/scripts/_set_cams_info_file.txt
Use given initial values.
Press the following keys to change mode: x, y, z, (a)zimuth, (p)itch, (r)oll
For each mode, press 6 to increase by step and 4 to decrease
Press + to multiply step by 2 or - to divide
Press Q to quit
```
Note that the tool prints the path of the current configuration file. It saves its last configuration automatically, all the time, to be used on the next run.
After a lot of fiddling around, unified pointcloud looked better with the following calibaration:
```
x = 0.75
y = 0.575
z = 0
azimuth = -91.25
pitch = 0.75
roll = 0
```
Now, use the above results in the launch file:
```
ros2 launch realsense2_camera rs_dual_camera_launch.py serial_no1:=_207322251310 serial_no2:=_234422060144 tf.translation.x:=0.75 tf.translation.y:=0.575 tf.translation.z:=0.0 tf.rotation.yaw:=-91.25 tf.rotation.pitch:=0.75 tf.rotation.roll:=0.0
```
src/realsense2_camera/examples/dual_camera/rs_dual_camera_launch.py
-109
View File
@@ -1,109 +0,0 @@
# Copyright 2023 RealSense, Inc. All Rights Reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# DESCRIPTION #
# ----------- #
# Use this launch file to launch 2 devices.
# The Parameters available for definition in the command line for each camera are described in rs_launch.configurable_parameters
# For each device, the parameter name was changed to include an index.
# For example: to set camera_name for device1 set parameter camera_name1.
# command line example:
# ros2 launch realsense2_camera rs_dual_camera_launch.py serial_no1:=<serial number of 1st camera> serial_no2:=<serial number of 2nd camera>
"""Launch realsense2_camera node."""
import copy
from launch import LaunchDescription, LaunchContext
import launch_ros.actions
from launch.actions import OpaqueFunction
from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir
import sys
import pathlib
sys.path.append(str(pathlib.Path(__file__).parent.absolute()))
import os
from ament_index_python.packages import get_package_share_directory
sys.path.append(os.path.join(get_package_share_directory('realsense2_camera'), 'launch'))
import rs_launch
local_parameters = [{'name': 'camera_name1', 'default': 'camera1', 'description': 'camera unique name'},
{'name': 'camera_name2', 'default': 'camera2', 'description': 'camera unique name'},
{'name': 'camera_namespace1', 'default': 'camera1', 'description': 'camera1 namespace'},
{'name': 'camera_namespace2', 'default': 'camera2', 'description': 'camera2 namespace'},
{'name': 'enable_color1', 'default': 'true', 'description': 'enable color stream'},
{'name': 'enable_color2', 'default': 'true', 'description': 'enable color stream'},
{'name': 'enable_depth1', 'default': 'true', 'description': 'enable depth stream'},
{'name': 'enable_depth2', 'default': 'true', 'description': 'enable depth stream'},
{'name': 'pointcloud.enable1', 'default': 'true', 'description': 'enable pointcloud'},
{'name': 'pointcloud.enable2', 'default': 'true', 'description': 'enable pointcloud'},
{'name': 'spatial_filter.enable1', 'default': 'true', 'description': 'enable_spatial_filter'},
{'name': 'spatial_filter.enable2', 'default': 'true', 'description': 'enable_spatial_filter'},
{'name': 'temporal_filter.enable1', 'default': 'true', 'description': 'enable_temporal_filter'},
{'name': 'temporal_filter.enable2', 'default': 'true', 'description': 'enable_temporal_filter'},
{'name': 'tf.translation.x', 'default': '0.0', 'description': 'x'},
{'name': 'tf.translation.y', 'default': '0.0', 'description': 'y'},
{'name': 'tf.translation.z', 'default': '0.0', 'description': 'z'},
{'name': 'tf.rotation.yaw', 'default': '0.0', 'description': 'yaw'},
{'name': 'tf.rotation.pitch', 'default': '0.0', 'description': 'pitch'},
{'name': 'tf.rotation.roll', 'default': '0.0', 'description': 'roll'},
]
def set_configurable_parameters(local_params):
return dict([(param['original_name'], LaunchConfiguration(param['name'])) for param in local_params])
def duplicate_params(general_params, posix):
local_params = copy.deepcopy(general_params)
for param in local_params:
param['original_name'] = param['name']
param['name'] += posix
return local_params
def launch_static_transform_publisher_node(context : LaunchContext):
# Static transformation from camera1 to camera2
node = launch_ros.actions.Node(
name = "my_static_transform_publisher",
package = "tf2_ros",
executable = "static_transform_publisher",
arguments = [context.launch_configurations['tf.translation.x'],
context.launch_configurations['tf.translation.y'],
context.launch_configurations['tf.translation.z'],
context.launch_configurations['tf.rotation.yaw'],
context.launch_configurations['tf.rotation.pitch'],
context.launch_configurations['tf.rotation.roll'],
context.launch_configurations['camera_name1'] + "_link",
context.launch_configurations['camera_name2'] + "_link"]
)
return [node]
def generate_launch_description():
params1 = duplicate_params(rs_launch.configurable_parameters, '1')
params2 = duplicate_params(rs_launch.configurable_parameters, '2')
return LaunchDescription(
rs_launch.declare_configurable_parameters(local_parameters) +
rs_launch.declare_configurable_parameters(params1) +
rs_launch.declare_configurable_parameters(params2) +
[
OpaqueFunction(function=rs_launch.launch_setup,
kwargs = {'params' : set_configurable_parameters(params1),
'param_name_suffix': '1'}),
OpaqueFunction(function=rs_launch.launch_setup,
kwargs = {'params' : set_configurable_parameters(params2),
'param_name_suffix': '2'}),
OpaqueFunction(function=launch_static_transform_publisher_node),
launch_ros.actions.Node(
package='rviz2',
namespace='',
executable='rviz2',
name='rviz2',
arguments=['-d', [ThisLaunchFileDir(), '/rviz/dual_camera_pointcloud.rviz']]
)
])
src/realsense2_camera/examples/dual_camera/rviz/dual_camera_pointcloud.rviz
-194
View File
@@ -1,194 +0,0 @@
Panels:
- Class: rviz_common/Displays
Help Height: 0
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /Grid1
- /PointCloud21
- /PointCloud22
Splitter Ratio: 0.5
Tree Height: 865
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
Expanded:
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz_common/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz_default_plugins/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 255; 255; 255
Color Transformer: RGB8
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: PointCloud2
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /camera1/camera1/depth/color/points
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 255; 255; 255
Color Transformer: RGB8
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: PointCloud2
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /camera2/camera2/depth/color/points
Use Fixed Frame: true
Use rainbow: true
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: camera1_link
Frame Rate: 30
Name: root
Tools:
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
- Class: rviz_default_plugins/FocusCamera
- Class: rviz_default_plugins/Measure
Line color: 128; 128; 0
- Class: rviz_default_plugins/SetInitialPose
Covariance x: 0.25
Covariance y: 0.25
Covariance yaw: 0.06853891909122467
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /initialpose
- Class: rviz_default_plugins/SetGoal
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /move_base_simple/goal
- Class: rviz_default_plugins/PublishPoint
Single click: true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /clicked_point
Transformation:
Current:
Class: rviz_default_plugins/TF
Value: true
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 8.93685531616211
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: -0.18814913928508759
Y: -0.17941315472126007
Z: 0.14549313485622406
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: -1.5697963237762451
Target Frame: <Fixed Frame>
Value: Orbit (rviz_default_plugins)
Yaw: 4.730405330657959
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1016
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 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
Selection:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1846
X: 74
Y: 27
src/realsense2_camera/examples/launch_from_rosbag/README.md
-22
View File
@@ -1,22 +0,0 @@
# Launching RS ROS2 node from rosbag File
The following example allows streaming a rosbag file, saved by RealSense Viewer, instead of streaming live with a camera. It can be used for testing and repetition of the same sequence.
```
ros2 launch realsense2_camera rs_launch_from_rosbag.py
```
By default, the 'rs_launch_from_rosbag.py' launch file uses the "/rosbag/D435i_Depth_and_IMU_Stands_still.bag" rosbag file.
User can also provide a different rosbag file through cmd line as follows:
```
ros2 launch realsense2_camera rs_launch_from_rosbag.py rosbag_filename:="/full/path/to/rosbag/file"
```
or
```
ros2 launch realsense2_camera rs_launch.py rosbag_filename:="/full/path/to/rosbag/file"
```
Additionally, the 'rosbag_loop' cmd line argument enables the looped playback of the rosbag file:
```
ros2 launch realsense2_camera rs_launch_from_rosbag.py rosbag_filename:="/full/path/to/rosbag/file" rosbag_loop:="true"
```
Check-out [sample-recordings](https://github.com/realsenseai/librealsense/blob/master/doc/sample-data.md) for a few recorded samples.
src/realsense2_camera/examples/launch_from_rosbag/rs_launch_from_rosbag.py
-57
View File
@@ -1,57 +0,0 @@
# Copyright 2023 RealSense, Inc. All Rights Reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# DESCRIPTION #
# ----------- #
# Use this launch file to launch a device from rosbag file.
# The Parameters available for definition in the command line for the camera are described in rs_launch.configurable_parameters
# command line example:
# ros2 launch realsense2_camera rs_launch_from_rosbag.py
"""Launch realsense2_camera node."""
from launch import LaunchDescription
import launch_ros.actions
from launch.actions import OpaqueFunction
from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir
import sys
import pathlib
sys.path.append(str(pathlib.Path(__file__).parent.absolute()))
import os
from ament_index_python.packages import get_package_share_directory
sys.path.append(os.path.join(get_package_share_directory('realsense2_camera'), 'launch'))
import rs_launch
local_parameters = [{'name': 'camera_name', 'default': 'camera', 'description': 'camera unique name'},
{'name': 'camera_namespace', 'default': 'camera', 'description': 'camera namespace'},
{'name': 'enable_depth', 'default': 'true', 'description': 'enable depth stream'},
{'name': 'enable_gyro', 'default': 'true', 'description': "'enable gyro stream'"},
{'name': 'enable_accel', 'default': 'true', 'description': "'enable accel stream'"},
{'name': 'rosbag_filename', 'default': [ThisLaunchFileDir(), "/rosbag/D435i_Depth_and_IMU_Stands_still.bag"], 'description': 'A realsense bagfile to run from as a device'},
{'name': 'rosbag_loop', 'default': 'false', 'description': 'enable realsense bagfile loop playback'},
]
def set_configurable_parameters(local_params):
return dict([(param['name'], LaunchConfiguration(param['name'])) for param in local_params])
def generate_launch_description():
params = rs_launch.configurable_parameters
return LaunchDescription(
rs_launch.declare_configurable_parameters(local_parameters) +
rs_launch.declare_configurable_parameters(params) +
[
OpaqueFunction(function=rs_launch.launch_setup,
kwargs = {'params' : set_configurable_parameters(params)}
)
])
src/realsense2_camera/examples/launch_params_from_file/README.md
-33
View File
@@ -1,33 +0,0 @@
# Get RS ROS2 node params from YAML file
The following example gets the RS ROS2 node params from YAML file.
```
ros2 launch realsense2_camera rs_launch_get_params_from_yaml.py
```
By default, 'rs_launch_get_params_from_yaml.py' launch file uses the "/config/config.yaml" YAML file.
User can provide a different YAML file through cmd line as follows:
```
ros2 launch realsense2_camera rs_launch_get_params_from_yaml.py config_file:="/full/path/to/config/file"
```
or
```
ros2 launch realsense2_camera rs_launch.py config_file:="/full/path/to/config/file"
```
## Syntax for defining params in YAML file
```
param1: value
param2: value
```
Example:
```
enable_color: true
rgb_camera.color_profile: 1280x720x15
enable_depth: true
align_depth.enable: true
enable_sync: true
publish_tf: true
tf_publish_rate: 1.0
```

Some files were not shown because too many files have changed in this diff Show More