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

Update logic work system

Browse Source
This commit is contained in:
Даниил Грабарь
2026-05-25 07:38:07 +03:00
parent fff46f0ade
commit 2f079df087
1 changed files with 225 additions and 198 deletions
Download Patch File Download Diff File Expand all files Collapse all files
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() + " мс");
}