diff --git a/src/iiwa_sunrise/src/ServerFriRos2.java b/src/iiwa_sunrise/src/ServerFriRos2.java index 53a6f76..819dfed 100644 --- a/src/iiwa_sunrise/src/ServerFriRos2.java +++ b/src/iiwa_sunrise/src/ServerFriRos2.java @@ -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() + " мс"); }