728x90
반응형
이전 글에 이어서 UI 작업을 한다.
3단계: Record/Replay 조작 UI
Step1 : RobotControlWidget.h 추가 (protected 섹션)
UFUNCTION() void HandleSyncOnClicked();
UFUNCTION() void HandleSyncOffClicked();
UFUNCTION() void HandleStartRecordClicked();
UFUNCTION() void HandleStopRecordClicked();
UFUNCTION() void HandleStartReplayClicked();
UFUNCTION() void HandleStopReplayClicked();
/** Recompute the control panel (worker state label + button enable states). */
void RefreshControlUI();
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UButton> SyncOnButton;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UButton> SyncOffButton;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UButton> StartRecordButton;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UButton> StopRecordButton;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UButton> StartReplayButton;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UButton> StopReplayButton;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UEditableTextBox> ReplayFilenameTextBox;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UCheckBox> LoopCheckBox;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class USpinBox> ApproachSpeedSpinBox;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UTextBlock> WorkerStateText;
전체 코드:
더보기
#pragma once
#include "CoreMinimal.h"
#include "Blueprint/UserWidget.h"
#include "RobotControlWidget.generated.h"
class ARobotVisualizer;
class URosBridgeSubsystem;
/**
* Base class for the in-viewport robot control UI (Phase 10).
*
* This C++ base owns the logic: it finds the ARobotVisualizer in the level,
* exposes the robot's commands as BlueprintCallable functions (bind WBP
* buttons to these), and exposes its state as BlueprintPure getters (bind
* WBP text / visibility to these).
*
* Create a Widget Blueprint (WBP_RobotControl) reparented to this class,
* lay out the visuals in the UMG designer, and wire buttons/text to the
* functions below. No Blueprint scripting required — just bindings.
*/
UCLASS()
class SO101_TWIN_API URobotControlWidget : public UUserWidget
{
GENERATED_BODY()
public:
// --- Commands (bind WBP buttons' OnClicked to these) ---
UFUNCTION(BlueprintCallable, Category = "ROS|UI")
void CmdSyncOn();
UFUNCTION(BlueprintCallable, Category = "ROS|UI")
void CmdSyncOff();
UFUNCTION(BlueprintCallable, Category = "ROS|UI")
void CmdStartRecord();
UFUNCTION(BlueprintCallable, Category = "ROS|UI")
void CmdStopRecord();
UFUNCTION(BlueprintCallable, Category = "ROS|UI")
void CmdStartReplay(const FString& Filename, bool bLoop, float ApproachSpeed);
UFUNCTION(BlueprintCallable, Category = "ROS|UI")
void CmdStopReplay();
UFUNCTION(BlueprintCallable, Category = "ROS|UI")
void CmdEStop();
// --- State getters (bind WBP text / visibility to these) ---
UFUNCTION(BlueprintPure, Category = "ROS|UI")
FString GetWorkerState() const;
UFUNCTION(BlueprintPure, Category = "ROS|UI")
bool IsSyncActive() const;
UFUNCTION(BlueprintPure, Category = "ROS|UI")
bool IsRosBridgeConnected() const;
UFUNCTION(BlueprintPure, Category = "ROS|UI")
bool IsBridgeNodeAlive() const;
UFUNCTION(BlueprintPure, Category = "ROS|UI")
bool IsWorkerAlive() const;
UFUNCTION(BlueprintPure, Category = "ROS|UI")
bool HasFollowerError() const;
UFUNCTION(BlueprintPure, Category = "ROS|UI")
bool HasLeaderError() const;
/** True once the robot actor has been located in the level. */
UFUNCTION(BlueprintPure, Category = "ROS|UI")
bool HasRobot() const;
protected:
virtual void NativeConstruct() override;
/** Find and cache the ARobotVisualizer + subsystem. Safe to call repeatedly. */
ARobotVisualizer* ResolveRobot();
virtual void NativeTick(const FGeometry& MyGeometry, float InDeltaTime) override;
UFUNCTION()
void HandleEStopClicked();
/** Recompute the safety panel from the robot's current state. */
void RefreshSafetyUI();
// --- Bound widgets: names MUST match the WBP widget names exactly ---
UPROPERTY(meta = (BindWidget))
TObjectPtr<class UButton> EStopButton;
UPROPERTY(meta = (BindWidget))
TObjectPtr<class UTextBlock> ConnectionStatusText;
UPROPERTY(meta = (BindWidget))
TObjectPtr<class UTextBlock> DeviceErrorText;
float RefreshAccum = 0.0f;
TWeakObjectPtr<ARobotVisualizer> Robot;
TWeakObjectPtr<URosBridgeSubsystem> Ros;
UFUNCTION() void HandleSyncOnClicked();
UFUNCTION() void HandleSyncOffClicked();
UFUNCTION() void HandleStartRecordClicked();
UFUNCTION() void HandleStopRecordClicked();
UFUNCTION() void HandleStartReplayClicked();
UFUNCTION() void HandleStopReplayClicked();
/** Recompute the control panel (worker state label + button enable states). */
void RefreshControlUI();
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UButton> SyncOnButton;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UButton> SyncOffButton;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UButton> StartRecordButton;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UButton> StopRecordButton;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UButton> StartReplayButton;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UButton> StopReplayButton;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UEditableTextBox> ReplayFilenameTextBox;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UCheckBox> LoopCheckBox;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class USpinBox> ApproachSpeedSpinBox;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UTextBlock> WorkerStateText;
};
Step2 : RobotControlWidget.cpp 수정
- include에 세 줄 추가
#include "Components/EditableTextBox.h"
#include "Components/CheckBox.h"
#include "Components/SpinBox.h"
- NativeConstruct() 안, 기존 EStop 바인딩 다음(RefreshSafetyUI(); 앞)에
if (SyncOnButton) SyncOnButton->OnClicked.AddDynamic(this, &URobotControlWidget::HandleSyncOnClicked);
if (SyncOffButton) SyncOffButton->OnClicked.AddDynamic(this, &URobotControlWidget::HandleSyncOffClicked);
if (StartRecordButton) StartRecordButton->OnClicked.AddDynamic(this, &URobotControlWidget::HandleStartRecordClicked);
if (StopRecordButton) StopRecordButton->OnClicked.AddDynamic(this, &URobotControlWidget::HandleStopRecordClicked);
if (StartReplayButton) StartReplayButton->OnClicked.AddDynamic(this, &URobotControlWidget::HandleStartReplayClicked);
if (StopReplayButton) StopReplayButton->OnClicked.AddDynamic(this, &URobotControlWidget::HandleStopReplayClicked);
if (ApproachSpeedSpinBox)
{
ApproachSpeedSpinBox->SetMinValue(5.0f);
ApproachSpeedSpinBox->SetMaxValue(300.0f);
ApproachSpeedSpinBox->SetMinSliderValue(5.0f);
ApproachSpeedSpinBox->SetMaxSliderValue(300.0f);
ApproachSpeedSpinBox->SetValue(45.0f);
}
RefreshControlUI();
- NativeTick의 throttle 블록 안에 한 줄 추가 — RefreshSafetyUI(); 다음 추가
RefreshControlUI();
- 함수 추가
void URobotControlWidget::HandleSyncOnClicked() { CmdSyncOn(); }
void URobotControlWidget::HandleSyncOffClicked() { CmdSyncOff(); }
void URobotControlWidget::HandleStartRecordClicked() { CmdStartRecord(); }
void URobotControlWidget::HandleStopRecordClicked() { CmdStopRecord(); }
void URobotControlWidget::HandleStopReplayClicked() { CmdStopReplay(); }
void URobotControlWidget::HandleStartReplayClicked()
{
FString Filename;
if (RecordingComboBox && !RecordingComboBox->GetSelectedOption().IsEmpty())
Filename = RecordingComboBox->GetSelectedOption();
else if (ReplayFilenameTextBox)
Filename = ReplayFilenameTextBox->GetText().ToString();
const bool bLoop = LoopCheckBox ? LoopCheckBox->IsChecked() : false;
const float Speed = ApproachSpeedSpinBox ? ApproachSpeedSpinBox->GetValue() : 45.0f;
CmdStartReplay(Filename, bLoop, Speed);
}
void URobotControlWidget::RefreshControlUI()
{
const FString WS = GetWorkerState();
const bool bIdle = (WS == TEXT("idle"));
const bool bSyncing = (WS == TEXT("syncing"));
const bool bRecording = (WS == TEXT("recording"));
const bool bReplaying = (WS == TEXT("replaying"));
if (WorkerStateText)
{
FString Label;
FLinearColor Color;
if (bRecording) { Label = TEXT("RECORDING"); Color = FLinearColor::Red; }
else if (bReplaying) { Label = TEXT("REPLAYING"); Color = FLinearColor(0.0f, 1.0f, 1.0f); }
else if (bSyncing) { Label = TEXT("SYNCING"); Color = FLinearColor::Green; }
else if (bIdle) { Label = TEXT("IDLE"); Color = FLinearColor::Gray; }
else { Label = TEXT("--"); Color = FLinearColor::Gray; }
WorkerStateText->SetText(FText::FromString(Label));
WorkerStateText->SetColorAndOpacity(FSlateColor(Color));
}
// Workflow guards — only enable actions that make sense in the current state.
const bool bAlive = IsWorkerAlive();
if (SyncOnButton) SyncOnButton->SetIsEnabled(bAlive && !bRecording && !bReplaying);
if (SyncOffButton) SyncOffButton->SetIsEnabled(bAlive && IsSyncActive() && !bRecording);
if (StartRecordButton) StartRecordButton->SetIsEnabled(bAlive && IsSyncActive() && !bRecording && !bReplaying);
if (StopRecordButton) StopRecordButton->SetIsEnabled(bAlive && bRecording);
if (StartReplayButton) StartReplayButton->SetIsEnabled(bAlive && !bRecording && !bReplaying);
if (StopReplayButton) StopReplayButton->SetIsEnabled(bAlive && bReplaying);
}
전체 코드:
더보기
#include "RobotControlWidget.h"
#include "RobotVisualizer.h"
#include "RosBridgeSubsystem.h"
#include "Kismet/GameplayStatics.h"
#include "Engine/World.h"
#include "Engine/GameInstance.h"
#include "Components/Button.h"
#include "Components/TextBlock.h"
#include "Components/EditableTextBox.h"
#include "Components/CheckBox.h"
#include "Components/SpinBox.h"
void URobotControlWidget::NativeConstruct()
{
Super::NativeConstruct();
ResolveRobot();
if (EStopButton)
{
EStopButton->OnClicked.AddDynamic(this, &URobotControlWidget::HandleEStopClicked);
}
if (SyncOnButton) SyncOnButton->OnClicked.AddDynamic(this, &URobotControlWidget::HandleSyncOnClicked);
if (SyncOffButton) SyncOffButton->OnClicked.AddDynamic(this, &URobotControlWidget::HandleSyncOffClicked);
if (StartRecordButton) StartRecordButton->OnClicked.AddDynamic(this, &URobotControlWidget::HandleStartRecordClicked);
if (StopRecordButton) StopRecordButton->OnClicked.AddDynamic(this, &URobotControlWidget::HandleStopRecordClicked);
if (StartReplayButton) StartReplayButton->OnClicked.AddDynamic(this, &URobotControlWidget::HandleStartReplayClicked);
if (StopReplayButton) StopReplayButton->OnClicked.AddDynamic(this, &URobotControlWidget::HandleStopReplayClicked);
if (ApproachSpeedSpinBox)
{
ApproachSpeedSpinBox->SetMinValue(5.0f);
ApproachSpeedSpinBox->SetMaxValue(300.0f);
ApproachSpeedSpinBox->SetMinSliderValue(5.0f);
ApproachSpeedSpinBox->SetMaxSliderValue(300.0f);
ApproachSpeedSpinBox->SetValue(45.0f);
}
RefreshControlUI();
RefreshSafetyUI();
}
ARobotVisualizer* URobotControlWidget::ResolveRobot()
{
if (!Robot.IsValid())
{
if (UWorld* World = GetWorld())
{
Robot = Cast<ARobotVisualizer>(
UGameplayStatics::GetActorOfClass(World, ARobotVisualizer::StaticClass()));
}
}
if (!Ros.IsValid())
{
if (UGameInstance* GI = GetGameInstance())
{
Ros = GI->GetSubsystem<URosBridgeSubsystem>();
}
}
return Robot.Get();
}
// --- Commands ---
void URobotControlWidget::CmdSyncOn() { if (ARobotVisualizer* R = ResolveRobot()) { R->SyncOn(); } }
void URobotControlWidget::CmdSyncOff() { if (ARobotVisualizer* R = ResolveRobot()) { R->SyncOff(); } }
void URobotControlWidget::CmdStartRecord() { if (ARobotVisualizer* R = ResolveRobot()) { R->StartRecord(); } }
void URobotControlWidget::CmdStopRecord() { if (ARobotVisualizer* R = ResolveRobot()) { R->StopRecord(); } }
void URobotControlWidget::CmdStopReplay() { if (ARobotVisualizer* R = ResolveRobot()) { R->StopReplay(); } }
void URobotControlWidget::CmdEStop() { if (ARobotVisualizer* R = ResolveRobot()) { R->EStop(); } }
void URobotControlWidget::CmdStartReplay(const FString& Filename, bool bLoop, float ApproachSpeed)
{
if (ARobotVisualizer* R = ResolveRobot())
{
R->ReplayFilename = Filename;
R->bReplayLoop = bLoop;
R->ApproachSpeed = ApproachSpeed;
R->StartReplay();
}
}
void URobotControlWidget::NativeTick(const FGeometry& MyGeometry, float InDeltaTime)
{
Super::NativeTick(MyGeometry, InDeltaTime);
// Poll the robot state ~5x/sec (cheap, no delegates needed).
RefreshAccum += InDeltaTime;
if (RefreshAccum >= 0.2f)
{
RefreshAccum = 0.0f;
RefreshSafetyUI();
RefreshControlUI();
}
}
void URobotControlWidget::HandleEStopClicked()
{
CmdEStop();
}
void URobotControlWidget::RefreshSafetyUI()
{
// --- Connection status text + color (priority order) ---
if (ConnectionStatusText)
{
FString StatusStr;
FLinearColor Color;
if (!IsRosBridgeConnected())
{
StatusStr = TEXT("DISCONNECTED");
Color = FLinearColor::Red;
}
else if (!IsBridgeNodeAlive())
{
StatusStr = TEXT("Bridge node: DOWN");
Color = FLinearColor::Red;
}
else if (!IsWorkerAlive())
{
StatusStr = TEXT("Worker: DOWN");
Color = FLinearColor(1.0f, 0.5f, 0.0f); // orange
}
else
{
const FString WS = GetWorkerState();
// WorkerState may still hold a stale connection-layer string
// ("disconnected", "bridge lost", "worker lost", "unknown") until
// the worker sends its first real /robot_status. Filter those out.
const bool bRealState =
(WS == TEXT("idle") || WS == TEXT("syncing") ||
WS == TEXT("recording") || WS == TEXT("replaying"));
StatusStr = bRealState
? FString::Printf(TEXT("Connected | %s"), *WS)
: TEXT("Connected | (awaiting status)");
Color = FLinearColor::Green;
}
ConnectionStatusText->SetText(FText::FromString(StatusStr));
ConnectionStatusText->SetColorAndOpacity(FSlateColor(Color));
}
// --- Device error panel (hidden when no errors) ---
if (DeviceErrorText)
{
TArray<FString> Errors;
if (HasFollowerError()) { Errors.Add(TEXT("Follower: USB/Serial ERROR")); }
if (HasLeaderError()) { Errors.Add(TEXT("Leader: USB/Serial ERROR")); }
if (Errors.Num() > 0)
{
DeviceErrorText->SetText(FText::FromString(FString::Join(Errors, TEXT("\n"))));
DeviceErrorText->SetColorAndOpacity(FSlateColor(FLinearColor::Red));
DeviceErrorText->SetVisibility(ESlateVisibility::HitTestInvisible);
}
else
{
DeviceErrorText->SetVisibility(ESlateVisibility::Collapsed);
}
}
}
void URobotControlWidget::HandleSyncOnClicked() { CmdSyncOn(); }
void URobotControlWidget::HandleSyncOffClicked() { CmdSyncOff(); }
void URobotControlWidget::HandleStartRecordClicked() { CmdStartRecord(); }
void URobotControlWidget::HandleStopRecordClicked() { CmdStopRecord(); }
void URobotControlWidget::HandleStopReplayClicked() { CmdStopReplay(); }
void URobotControlWidget::HandleStartReplayClicked()
{
FString Filename;
if (RecordingComboBox && !RecordingComboBox->GetSelectedOption().IsEmpty())
Filename = RecordingComboBox->GetSelectedOption();
else if (ReplayFilenameTextBox)
Filename = ReplayFilenameTextBox->GetText().ToString();
const bool bLoop = LoopCheckBox ? LoopCheckBox->IsChecked() : false;
const float Speed = ApproachSpeedSpinBox ? ApproachSpeedSpinBox->GetValue() : 45.0f;
CmdStartReplay(Filename, bLoop, Speed);
}
void URobotControlWidget::RefreshControlUI()
{
const FString WS = GetWorkerState();
const bool bIdle = (WS == TEXT("idle"));
const bool bSyncing = (WS == TEXT("syncing"));
const bool bRecording = (WS == TEXT("recording"));
const bool bReplaying = (WS == TEXT("replaying"));
if (WorkerStateText)
{
FString Label;
FLinearColor Color;
if (bRecording) { Label = TEXT("RECORDING"); Color = FLinearColor::Red; }
else if (bReplaying) { Label = TEXT("REPLAYING"); Color = FLinearColor(0.0f, 1.0f, 1.0f); }
else if (bSyncing) { Label = TEXT("SYNCING"); Color = FLinearColor::Green; }
else if (bIdle) { Label = TEXT("IDLE"); Color = FLinearColor::Gray; }
else { Label = TEXT("--"); Color = FLinearColor::Gray; }
WorkerStateText->SetText(FText::FromString(Label));
WorkerStateText->SetColorAndOpacity(FSlateColor(Color));
}
// Workflow guards — only enable actions that make sense in the current state.
const bool bAlive = IsWorkerAlive();
if (SyncOnButton) SyncOnButton->SetIsEnabled(bAlive && !bRecording && !bReplaying);
if (SyncOffButton) SyncOffButton->SetIsEnabled(bAlive && IsSyncActive() && !bRecording);
if (StartRecordButton) StartRecordButton->SetIsEnabled(bAlive && IsSyncActive() && !bRecording && !bReplaying);
if (StopRecordButton) StopRecordButton->SetIsEnabled(bAlive && bRecording);
if (StartReplayButton) StartReplayButton->SetIsEnabled(bAlive && !bRecording && !bReplaying);
if (StopReplayButton) StopReplayButton->SetIsEnabled(bAlive && bReplaying);
}
// --- State getters ---
FString URobotControlWidget::GetWorkerState() const
{
return Robot.IsValid() ? Robot->GetWorkerState() : TEXT("no robot");
}
bool URobotControlWidget::IsSyncActive() const { return Robot.IsValid() && Robot->IsSyncActive(); }
bool URobotControlWidget::IsRosBridgeConnected() const { return Robot.IsValid() && Robot->IsRosBridgeConnected(); }
bool URobotControlWidget::IsBridgeNodeAlive() const { return Robot.IsValid() && Robot->IsBridgeNodeAlive(); }
bool URobotControlWidget::IsWorkerAlive() const { return Robot.IsValid() && Robot->IsWorkerAlive(); }
bool URobotControlWidget::HasFollowerError() const { return Robot.IsValid() && Robot->HasFollowerError(); }
bool URobotControlWidget::HasLeaderError() const { return Robot.IsValid() && Robot->HasLeaderError(); }
bool URobotControlWidget::HasRobot() const { return Robot.IsValid(); }
수정했다면 빌드한다.
Step3 : WBP_RobotControl에 조작 패널 올리기
WBP Designer에서 UI 패널 설정
WBP에서 Designer 탭으로 가서 새 [Vertical Box]를 하나 만들고 버튼을 배치했다.
WorkerStateText, SyncOnButton / SyncOffButton, StartRecordButton / StopRecordButton, StartReplayButton / StopReplayButton, ReplayFilenameTextBox, LoopCheckBox, ApproachSpeedSpinBox 모든 이름은 동일해야 한다.
잘 설정했다면 WBP Compile.
버튼이 잘 작동되는 것을 확인했다.
Step4: Replay 목록과 진행률 가져오기
추가로, Replay 목록을 가져오고, Replay 진행률을 보여주는 UI를 넣기 위해서는 WSL/Python쪽의 접근이 필요하다.
- worker (lerobot_worker.py)
- list_recordings 명령: ~/recordings/의 recording_*.json을 스캔해 파일명·프레임수/길이/시각을 최신순으로 반환.
- replay 진행률: 재생 루프에서 (현재 프레임/총 프레임)을 PUB 메시지에 실어 보냄(replay_frame/replay_total).
- bridge (ros_bridge_node.py)
- list_recordings 응답을 /robot_status로 전달(Unreal이 목록 UI에 뿌릴 수 있게).
- PUB의 진행률 필드를 /robot_status로 주기 전달(상태 변화 때만이 아니라 replay 중 지속).
이미 대부분은 이전에 만들어놨기 때문에 지금은 bridge에 "replay 중 진행률을 주기적으로(5Hz) 발행" 한다는 부분만 수정해주면 된다.
ros_bridge_node.py 수정
- __init__에서 self.last_device_errors = {} 바로 다음에 추가
self.last_progress_pub_time = self.get_clock().now()
self.progress_pub_interval_ns = 200_000_000 # 5 Hz throttle for replay progress
- poll_zmq 안, if state_changed: 블록이 끝나는 곳을 참고하여 if state_changed:와 같은 레벨에 추가
self.last_worker_state = worker_state
self.last_worker_teleop = worker_teleop
self.get_logger().info(
f"Worker state: {worker_state}, teleop: {worker_teleop}")
# Continuously publish replay progress (throttled to 5 Hz) so the
# Unreal progress bar updates during replay. state_changed is False
# while replaying (state stays "replaying"), so this is separate.
if worker_state == "replaying" and "replay_progress" in msg:
now_t = self.get_clock().now()
if (now_t - self.last_progress_pub_time).nanoseconds > self.progress_pub_interval_ns:
self.last_progress_pub_time = now_t
prog_msg = String()
prog_msg.data = json.dumps({
"state": worker_state,
"teleop": worker_teleop,
"replay_progress": msg["replay_progress"],
})
self.robot_status_pub.publish(prog_msg)
전체 코드:
더보기
#!/usr/bin/env python3
"""
ros_bridge_node.py — ZMQ ↔ ROS2 bridge node
Runs in .venv-ros-bridge (Python 3.10) with rclpy + pyzmq.
Subscribes to joint states from lerobot_worker via ZMQ,
converts degrees→radians, and publishes sensor_msgs/JointState.
Also subscribes to /follower_joint_commands and forwards to worker via ZMQ REQ.
Relays record/replay/estop commands:
- /robot_command (std_msgs/String) → ZMQ REQ → worker
- Worker state published on /robot_status (std_msgs/String) every tick
Usage:
export PATH=$(echo $PATH | tr ':' '\\n' | grep -v miniforge | tr '\\n' ':' | sed 's/:$//')
cd ~/UnrealRobotics
source .venv-ros-bridge/bin/activate
source /opt/ros/humble/setup.bash
python src/lerobot_ros2_bridge/lerobot_ros2_bridge/ros_bridge_node.py \\
[--sub-addr tcp://127.0.0.1:5555] \\
[--req-addr tcp://127.0.0.1:5556]
"""
import argparse
import json
import math
import sys
import traceback
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
from std_msgs.msg import String
import zmq
# ---------------------------------------------------------------------------
# Joint names — must match URDF, LeRobot calibration, and worker output
# (Phase 3.2 verified: all 6 names identical across all three sources)
# ---------------------------------------------------------------------------
JOINT_NAMES = [
"shoulder_pan",
"shoulder_lift",
"elbow_flex",
"wrist_flex",
"wrist_roll",
"gripper",
]
def deg2rad(deg: float) -> float:
return deg * math.pi / 180.0
def rad2deg(rad: float) -> float:
return rad * 180.0 / math.pi
class RosBridgeNode(Node):
def __init__(self, args):
super().__init__("lerobot_ros2_bridge")
self.get_logger().info("Initializing lerobot_ros2_bridge node")
# --- ZMQ setup ---
self.zmq_ctx = zmq.Context()
# SUB: receive joint states from worker
self.zmq_sub = self.zmq_ctx.socket(zmq.SUB)
self.zmq_sub.connect(args.sub_addr)
self.zmq_sub.setsockopt_string(zmq.SUBSCRIBE, "")
# Don't block waiting for messages — use RCVTIMEO
self.zmq_sub.setsockopt(zmq.RCVTIMEO, 0)
self.get_logger().info(f"ZMQ SUB connected to {args.sub_addr}")
# REQ: send commands to worker
self.req_addr = args.req_addr
self.zmq_req = self._create_req_socket()
self.req_consecutive_timeouts = 0
self.req_max_timeouts = 3 # recreate socket after this many consecutive timeouts
# --- ROS2 publishers ---
# Follower joint states (for robot_state_publisher + rosbridge → Unreal)
self.follower_pub = self.create_publisher(JointState, "/joint_states", 10)
# Leader joint states (separate topic)
self.leader_pub = self.create_publisher(JointState, "/leader_joint_states", 10)
# --- ROS2 subscriber for commands from Unreal (via rosbridge) ---
self.cmd_sub = self.create_subscription(
JointState,
"/follower_joint_commands",
self.on_joint_command,
10,
)
# --- Record/Replay/E-Stop command relay ---
# Unreal publishes JSON command strings to /robot_command,
# bridge relays them to worker via ZMQ REQ.
self.robot_cmd_sub = self.create_subscription(
String,
"/robot_command",
self.on_robot_command,
10,
)
# Worker state feedback → Unreal
self.robot_status_pub = self.create_publisher(String, "/robot_status", 10)
self.last_worker_state = ""
self.last_worker_teleop = None
self.last_device_errors = {}
self.last_progress_pub_time = self.get_clock().now()
self.progress_pub_interval_ns = 200_000_000 # 5 Hz throttle for replay progress
# Bridge heartbeat → Unreal (so Unreal can distinguish bridge vs worker death)
self.bridge_heartbeat_pub = self.create_publisher(String, "/bridge_heartbeat", 10)
# --- Timer: poll ZMQ at ~100Hz (faster than worker's 30Hz to avoid buffering) ---
self.poll_timer = self.create_timer(0.01, self.poll_zmq)
# --- Timer: heartbeat ping to worker every 2s ---
self.heartbeat_timer = self.create_timer(2.0, self.send_heartbeat)
# --- Timer: bridge heartbeat to Unreal every 1s ---
self.bridge_heartbeat_timer = self.create_timer(1.0, self.publish_bridge_heartbeat)
# Stats
self.recv_count = 0
self.last_log_time = self.get_clock().now()
def poll_zmq(self):
"""Non-blocking poll of ZMQ SUB for worker messages."""
# Drain all available messages (in case we're slightly behind)
messages_this_tick = 0
while messages_this_tick < 5: # cap per tick to avoid starving ROS callbacks
try:
raw = self.zmq_sub.recv_string(zmq.NOBLOCK)
except zmq.Again:
break
messages_this_tick += 1
self.recv_count += 1
try:
msg = json.loads(raw)
except json.JSONDecodeError as e:
self.get_logger().warn(f"JSON parse error: {e}")
continue
now = self.get_clock().now().to_msg()
# Publish follower joint states
if "follower" in msg:
js = JointState()
js.header.stamp = now
js.header.frame_id = ""
js.name = list(JOINT_NAMES)
js.position = [
deg2rad(msg["follower"].get(name, 0.0))
for name in JOINT_NAMES
]
# velocity and effort left empty (not available from LeRobot API)
self.follower_pub.publish(js)
# Publish leader joint states
if "leader" in msg:
js = JointState()
js.header.stamp = now
js.header.frame_id = ""
js.name = list(JOINT_NAMES)
js.position = [
deg2rad(msg["leader"].get(name, 0.0))
for name in JOINT_NAMES
]
self.leader_pub.publish(js)
# Publish worker state to /robot_status (only on change)
worker_state = msg.get("state", "idle")
worker_teleop = msg.get("teleop", False)
device_errors = msg.get("device_errors", {})
state_changed = (worker_state != self.last_worker_state or
worker_teleop != self.last_worker_teleop)
# Also publish on device error changes
if device_errors != self.last_device_errors:
state_changed = True
self.last_device_errors = dict(device_errors)
if device_errors:
self.get_logger().warn(f"Device errors: {device_errors}")
if state_changed:
status_msg = String()
status_data = {"state": worker_state, "teleop": worker_teleop}
# Include recording/replay metadata if present
if "recording_frames" in msg:
status_data["recording_frames"] = msg["recording_frames"]
if "replay_progress" in msg:
status_data["replay_progress"] = msg["replay_progress"]
if device_errors:
status_data["device_errors"] = device_errors
status_msg.data = json.dumps(status_data)
self.robot_status_pub.publish(status_msg)
self.last_worker_state = worker_state
self.last_worker_teleop = worker_teleop
self.get_logger().info(
f"Worker state: {worker_state}, teleop: {worker_teleop}")
# Continuously publish replay progress (throttled to 5 Hz) so the
# Unreal progress bar updates during replay. state_changed is False
# while replaying (state stays "replaying"), so this is separate.
if worker_state == "replaying" and "replay_progress" in msg:
now_t = self.get_clock().now()
if (now_t - self.last_progress_pub_time).nanoseconds > self.progress_pub_interval_ns:
self.last_progress_pub_time = now_t
prog_msg = String()
prog_msg.data = json.dumps({
"state": worker_state,
"teleop": worker_teleop,
"replay_progress": msg["replay_progress"],
})
self.robot_status_pub.publish(prog_msg)
# Periodic logging (every 10 seconds)
now_time = self.get_clock().now()
if (now_time - self.last_log_time).nanoseconds > 10_000_000_000:
self.get_logger().info(
f"ZMQ recv total: {self.recv_count} messages"
)
self.last_log_time = now_time
def on_joint_command(self, msg: JointState):
"""
Receive joint commands from ROS2 (e.g. from Unreal via rosbridge),
convert radians→degrees, and forward to worker via ZMQ REQ.
"""
if len(msg.name) == 0 or len(msg.position) == 0:
self.get_logger().warn("Empty joint command received, ignoring")
return
# Build command dict (degrees)
joint_args = {}
for name, pos_rad in zip(msg.name, msg.position):
if name in JOINT_NAMES:
joint_args[name] = rad2deg(pos_rad)
if not joint_args:
self.get_logger().warn("No recognized joint names in command")
return
cmd = {
"cmd": "send_follower_action",
"args": joint_args,
}
try:
self.zmq_req.send_string(json.dumps(cmd))
reply_raw = self.zmq_req.recv_string()
reply = json.loads(reply_raw)
if reply.get("status") != "ok":
self.get_logger().warn(
f"Worker command error: {reply.get('reason', 'unknown')}"
)
except zmq.Again:
self.get_logger().error("Worker REQ timeout — is lerobot_worker running?")
except Exception as e:
self.get_logger().error(f"Worker command exception: {e}")
def on_robot_command(self, msg: String):
"""
Receive record/replay/estop commands from Unreal (via rosbridge),
relay to worker via ZMQ REQ, and publish response on /robot_status.
Expected msg.data formats:
'{"cmd": "start_record"}'
'{"cmd": "stop_record"}'
'{"cmd": "start_replay", "args": {"filename": "...", "loop": false}}'
'{"cmd": "start_replay"}' (plays most recent)
'{"cmd": "stop_replay"}'
'{"cmd": "estop"}'
'{"cmd": "list_recordings"}'
"""
try:
cmd = json.loads(msg.data)
except json.JSONDecodeError:
# Support simple string commands: "estop", "start_record", etc.
cmd = {"cmd": msg.data.strip()}
cmd_name = cmd.get("cmd", "")
self.get_logger().info(f"Robot command received: {cmd_name}")
try:
self.zmq_req.send_string(json.dumps(cmd))
reply_raw = self.zmq_req.recv_string()
reply = json.loads(reply_raw)
# Publish response on /robot_status
status_msg = String()
status_msg.data = reply_raw
self.robot_status_pub.publish(status_msg)
status = reply.get("status", "unknown")
if status == "ok":
new_state = reply.get("state", "")
if new_state:
self.last_worker_state = new_state
self.get_logger().info(f"Command '{cmd_name}' OK: {reply}")
else:
self.get_logger().warn(
f"Command '{cmd_name}' error: {reply.get('reason', 'unknown')}"
)
except zmq.Again:
self.get_logger().error(
f"Worker REQ timeout for '{cmd_name}' — is lerobot_worker running?"
)
# Publish timeout error on /robot_status
err_msg = String()
err_msg.data = json.dumps({
"status": "error", "reason": "worker timeout", "cmd": cmd_name
})
self.robot_status_pub.publish(err_msg)
except Exception as e:
self.get_logger().error(f"Robot command exception: {e}")
err_msg = String()
err_msg.data = json.dumps({
"status": "error", "reason": str(e), "cmd": cmd_name
})
self.robot_status_pub.publish(err_msg)
def _create_req_socket(self):
"""Create a fresh ZMQ REQ socket connected to the worker."""
sock = self.zmq_ctx.socket(zmq.REQ)
sock.setsockopt(zmq.RCVTIMEO, 1000) # 1s timeout for replies
sock.setsockopt(zmq.LINGER, 0) # don't block on close
sock.connect(self.req_addr)
self.get_logger().info(f"ZMQ REQ connected to {self.req_addr}")
return sock
def _reset_req_socket(self):
"""Close and recreate the REQ socket to recover from stuck state."""
self.get_logger().warn("Resetting ZMQ REQ socket (worker may have restarted)")
try:
self.zmq_req.close()
except Exception:
pass
self.zmq_req = self._create_req_socket()
self.req_consecutive_timeouts = 0
def publish_bridge_heartbeat(self):
"""Publish a lightweight heartbeat so Unreal knows the bridge is alive."""
msg = String()
msg.data = '{"bridge":"alive"}'
self.bridge_heartbeat_pub.publish(msg)
def send_heartbeat(self):
"""Send periodic ping to worker so it knows the bridge is alive."""
try:
self.zmq_req.send_string('{"cmd":"ping"}')
self.zmq_req.recv_string() # discard reply, just keeping the link alive
self.req_consecutive_timeouts = 0 # reset on success
except zmq.Again:
self.req_consecutive_timeouts += 1
self.get_logger().warn(
f"Heartbeat ping timeout ({self.req_consecutive_timeouts}/"
f"{self.req_max_timeouts})")
if self.req_consecutive_timeouts >= self.req_max_timeouts:
self._reset_req_socket()
except Exception as e:
self.get_logger().warn(f"Heartbeat ping error: {e}")
self.req_consecutive_timeouts += 1
if self.req_consecutive_timeouts >= self.req_max_timeouts:
self._reset_req_socket()
def destroy_node(self):
"""Clean shutdown of ZMQ resources."""
self.get_logger().info("Shutting down ZMQ...")
self.zmq_sub.close()
self.zmq_req.close()
self.zmq_ctx.term()
super().destroy_node()
def main():
parser = argparse.ArgumentParser(description="ROS2 ↔ ZMQ bridge node")
parser.add_argument("--sub-addr", default="tcp://127.0.0.1:5555",
help="ZMQ SUB address (worker PUB)")
parser.add_argument("--req-addr", default="tcp://127.0.0.1:5556",
help="ZMQ REQ address (worker REP)")
# ROS2 may pass extra args — use parse_known_args
args, unknown = parser.parse_known_args()
rclpy.init(args=unknown if unknown else None)
node = RosBridgeNode(args)
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()
5Hz 마다 찍히는 지를 테스트 하기 위해서 다른 터미널 하나를 더 연다.
export PATH=$(echo $PATH | tr ':' '\n' | grep -v miniforge | tr '\n' ':' | sed 's/:$//')
source /opt/ros/humble/setup.bash
source ~/UnrealRobotics/install/setup.bash
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export ROS_LOCALHOST_ONLY=1
daemon 재시작
#한 줄씩 실행
ros2 daemon start
ros2 daemon stop
ros2 daemon start
echo
ros2 topic echo /robot_status
이 상태로 Start Replay를 걸면 5Hz 마다 로그가 찍히게 된다.
언리얼에서 목록 + 진행률 바 UI 설정
- list_recordings 응답의 recordings 배열 → 목록에 표시 + 클릭하면 파일명 자동 입력
- replay_progress → 프로그레스 바 + "12 / 52" 텍스트
- 액터(ARobotVisualizer)의 OnRobotStatus에 recordings/replay_progress 파싱 추가 → 멤버에 저장 + 게터 노출. 목록은 갱신될 때만 바뀌도록 "버전 카운터"를 둬서 위젯이 매 틱 리스트를 재생성하지 않게 한다.
- 위젯에서 ListView(목록) + ProgressBar + 텍스트를 바인딩하고, "Refresh" 버튼으로 list_recordings 명령을 보냄
RobotVisualizer.h 수정
- UCLASS() 위에(기존 forward 선언들 아래) recording 정보 구조체 추가
USTRUCT(BlueprintType)
struct FRecordingInfo
{
GENERATED_BODY()
UPROPERTY(BlueprintReadOnly, Category = "ROS|UI") FString Filename;
UPROPERTY(BlueprintReadOnly, Category = "ROS|UI") int32 Frames = 0;
UPROPERTY(BlueprintReadOnly, Category = "ROS|UI") float DurationSec = 0.0f;
UPROPERTY(BlueprintReadOnly, Category = "ROS|UI") FString RecordedAt;
};
- public API 블록(getter들 모여있는 곳)에 plain 게터 추가
const TArray<FRecordingInfo>& GetRecordings() const { return Recordings; }
int32 GetRecordingsVersion() const { return RecordingsVersion; }
int32 GetReplayIndex() const { return ReplayIndex; }
int32 GetReplayTotal() const { return ReplayTotal; }
bool IsReplayApproaching() const { return bReplayApproaching; }
- private 섹션(예: ControlWidget 포인터 근처)에 멤버 추가
TArray<FRecordingInfo> Recordings;
int32 RecordingsVersion = 0;
int32 ReplayIndex = 0;
int32 ReplayTotal = 0;
FString ReplayProgFilename;
bool bReplayApproaching = false;
전체 코드
더보기
#pragma once
#include "CoreMinimal.h"
#include "GameFramework/Actor.h"
#include "Templates/SubclassOf.h"
#include "RobotVisualizer.generated.h"
class UStaticMesh;
class UStaticMeshComponent;
class USceneComponent;
class URosBridgeSubsystem;
class URobotControlWidget;
/**
* Visualizes the SO-ARM-101 follower arm in Unreal Engine and provides
* MoveIt command interface via rosbridge.
*
* The component hierarchy mirrors the URDF link/joint structure:
* BaseLink -> ShoulderPanJoint -> ShoulderLink -> ShoulderLiftJoint -> ...
*
* Each "Joint" SceneComponent is where the ROS joint angle gets applied
* as a local Z-axis rotation. All child links/meshes rotate with it.
*
* Phase 8 additions:
* - SendNamedTarget(): publish to /moveit_goal_named (std_msgs/String)
* - SendJointGoal(): publish to /moveit_goal_joints (sensor_msgs/JointState)
* - SendPoseGoal(): publish to /moveit_goal_pose (geometry_msgs/PoseStamped)
* - Blueprint-callable + editor-testable via UPROPERTY buttons
*
* Phase 9 additions (Record/Replay/E-Stop):
* - StartRecord(): begin teleop recording on worker
* - StopRecord(): stop recording, save trajectory
* - StartReplay(): replay most recent (or named) recording
* - StopReplay(): stop replay
* - EStop(): emergency stop all motion
* - All commands publish JSON to /robot_command topic
* - Worker state feedback via /robot_status subscription
*/
USTRUCT(BlueprintType)
struct FRecordingInfo
{
GENERATED_BODY()
UPROPERTY(BlueprintReadOnly, Category = "ROS|UI") FString Filename;
UPROPERTY(BlueprintReadOnly, Category = "ROS|UI") int32 Frames = 0;
UPROPERTY(BlueprintReadOnly, Category = "ROS|UI") float DurationSec = 0.0f;
UPROPERTY(BlueprintReadOnly, Category = "ROS|UI") FString RecordedAt;
};
UCLASS()
class SO101_TWIN_API ARobotVisualizer : public AActor
{
GENERATED_BODY()
public:
ARobotVisualizer();
// =================================================================
// Phase 10 — Widget-facing API
// =================================================================
/** Widget Blueprint to spawn into the viewport on BeginPlay.
* Set this to WBP_RobotControl in this actor's Details panel. */
UPROPERTY(EditAnywhere, BlueprintReadOnly, Category = "ROS|UI")
TSubclassOf<URobotControlWidget> ControlWidgetClass;
UFUNCTION(BlueprintPure, Category = "ROS|UI")
FString GetWorkerState() const { return WorkerState; }
UFUNCTION(BlueprintPure, Category = "ROS|UI")
bool IsSyncActive() const { return bSyncActive; }
UFUNCTION(BlueprintPure, Category = "ROS|UI")
bool IsRosBridgeConnected() const { return bRosBridgeConnected; }
UFUNCTION(BlueprintPure, Category = "ROS|UI")
bool IsBridgeNodeAlive() const { return bRosBridgeConnected && !bBridgeHeartbeatLost; }
UFUNCTION(BlueprintPure, Category = "ROS|UI")
bool IsWorkerAlive() const { return bRosBridgeConnected && !bBridgeHeartbeatLost && !bWorkerDataLost; }
UFUNCTION(BlueprintPure, Category = "ROS|UI")
bool HasFollowerError() const { return bFollowerDeviceError; }
UFUNCTION(BlueprintPure, Category = "ROS|UI")
bool HasLeaderError() const { return bLeaderDeviceError; }
/** The control widget may read protected state and call commands directly. */
friend class URobotControlWidget;
const TArray<FRecordingInfo>& GetRecordings() const { return Recordings; }
int32 GetRecordingsVersion() const { return RecordingsVersion; }
int32 GetReplayIndex() const { return ReplayIndex; }
int32 GetReplayTotal() const { return ReplayTotal; }
bool IsReplayApproaching() const { return bReplayApproaching; }
protected:
virtual void BeginPlay() override;
virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override;
// =================================================================
// Configuration
// =================================================================
UPROPERTY(EditAnywhere, Category = "ROS|Bridge")
FString RosBridgeUrl = TEXT("ws://127.0.0.1:9090/?x=1");
UPROPERTY(EditAnywhere, Category = "ROS|Topics")
FString JointStateTopic = TEXT("/joint_states");
UPROPERTY(EditAnywhere, Category = "ROS|Topics")
FString JointStateType = TEXT("sensor_msgs/JointState");
// =================================================================
// MoveIt Command Interface (Phase 8)
// =================================================================
// --- Named target ---
/** Named target to send (e.g. "home", "ready"). Set in Details panel, then call SendNamedTarget(). */
UPROPERTY(EditAnywhere, Category = "ROS|MoveIt")
FString MoveItNamedTarget = TEXT("home");
/** Send the named target to MoveIt via /moveit_goal_named topic. */
UFUNCTION(BlueprintCallable, CallInEditor, Category = "ROS|MoveIt")
void SendNamedTarget();
// --- Joint goal ---
/** Joint goal values in radians. Set in Details panel, then call SendJointGoal(). */
UPROPERTY(EditAnywhere, Category = "ROS|MoveIt|Joints")
float GoalShoulderPan = 0.0f;
UPROPERTY(EditAnywhere, Category = "ROS|MoveIt|Joints")
float GoalShoulderLift = 0.0f;
UPROPERTY(EditAnywhere, Category = "ROS|MoveIt|Joints")
float GoalElbowFlex = 0.0f;
UPROPERTY(EditAnywhere, Category = "ROS|MoveIt|Joints")
float GoalWristFlex = 0.0f;
UPROPERTY(EditAnywhere, Category = "ROS|MoveIt|Joints")
float GoalWristRoll = 0.0f;
/** Send joint goal to MoveIt via /moveit_goal_joints topic. Values are in radians. */
UFUNCTION(BlueprintCallable, CallInEditor, Category = "ROS|MoveIt|Joints")
void SendJointGoal();
// --- Pose goal (Cartesian, position-only for 5DOF) ---
/** Target position in UE coordinates (cm). Converted to ROS (meters) on send. */
UPROPERTY(EditAnywhere, Category = "ROS|MoveIt|Pose")
FVector GoalPositionUE = FVector(10.0f, 0.0f, 15.0f);
/** Send position-only goal to MoveIt via /moveit_goal_pose topic.
* GoalPositionUE is in Unreal cm, auto-converted to ROS meters with Y flip. */
UFUNCTION(BlueprintCallable, CallInEditor, Category = "ROS|MoveIt|Pose")
void SendPoseGoal();
// =================================================================
// Teleop Sync (Phase 9)
// =================================================================
/** Activate leader→follower sync (teleop). Must be ON before recording. */
UFUNCTION(BlueprintCallable, CallInEditor, Category = "ROS|Sync")
void SyncOn();
/** Deactivate leader→follower sync. */
UFUNCTION(BlueprintCallable, CallInEditor, Category = "ROS|Sync")
void SyncOff();
/** Whether teleop (sync) is currently active. Updated from /robot_status. */
UPROPERTY(VisibleAnywhere, Category = "ROS|Status")
bool bSyncActive = false;
// =================================================================
// Record / Replay / E-Stop (Phase 9)
// =================================================================
/** Start recording: activates teleop on worker, buffers joint trajectory. */
UFUNCTION(BlueprintCallable, CallInEditor, Category = "ROS|Record")
void StartRecord();
/** Stop recording: saves trajectory to file on worker. */
UFUNCTION(BlueprintCallable, CallInEditor, Category = "ROS|Record")
void StopRecord();
/** Replay filename (empty = most recent recording). */
UPROPERTY(EditAnywhere, Category = "ROS|Replay")
FString ReplayFilename;
/** Whether to loop the replay continuously. */
UPROPERTY(EditAnywhere, Category = "ROS|Replay")
bool bReplayLoop = false;
/** Approach speed in degrees/sec. Controls how fast the robot moves
* to the start position before replay begins. Lower = smoother. */
UPROPERTY(EditAnywhere, Category = "ROS|Replay", meta = (ClampMin = "5.0", ClampMax = "300.0"))
float ApproachSpeed = 45.0f;
/** Start replaying a recorded trajectory on the follower arm. */
UFUNCTION(BlueprintCallable, CallInEditor, Category = "ROS|Replay")
void StartReplay();
/** Stop replay immediately. */
UFUNCTION(BlueprintCallable, CallInEditor, Category = "ROS|Replay")
void StopReplay();
/** Emergency stop: abort ALL motion immediately (recording, replay, teleop). */
UFUNCTION(BlueprintCallable, CallInEditor, Category = "ROS|Safety")
void EStop();
/** Current worker state (idle/recording/replaying). Updated from /robot_status. */
UPROPERTY(VisibleAnywhere, Category = "ROS|Status")
FString WorkerState = TEXT("unknown");
private:
// =================================================================
// Component hierarchy
// =================================================================
UPROPERTY(VisibleAnywhere, Category = "Robot")
TObjectPtr<USceneComponent> RobotRoot;
// Link SceneComponents
UPROPERTY(VisibleAnywhere, Category = "Robot|Links")
TObjectPtr<USceneComponent> BaseLink;
UPROPERTY(VisibleAnywhere, Category = "Robot|Links")
TObjectPtr<USceneComponent> ShoulderLink;
UPROPERTY(VisibleAnywhere, Category = "Robot|Links")
TObjectPtr<USceneComponent> UpperArmLink;
UPROPERTY(VisibleAnywhere, Category = "Robot|Links")
TObjectPtr<USceneComponent> LowerArmLink;
UPROPERTY(VisibleAnywhere, Category = "Robot|Links")
TObjectPtr<USceneComponent> WristLink;
UPROPERTY(VisibleAnywhere, Category = "Robot|Links")
TObjectPtr<USceneComponent> GripperLink;
UPROPERTY(VisibleAnywhere, Category = "Robot|Links")
TObjectPtr<USceneComponent> MovingJawLink;
// Joint SceneComponents
UPROPERTY(VisibleAnywhere, Category = "Robot|Joints")
TObjectPtr<USceneComponent> ShoulderPanJoint;
UPROPERTY(VisibleAnywhere, Category = "Robot|Joints")
TObjectPtr<USceneComponent> ShoulderLiftJoint;
UPROPERTY(VisibleAnywhere, Category = "Robot|Joints")
TObjectPtr<USceneComponent> ElbowFlexJoint;
UPROPERTY(VisibleAnywhere, Category = "Robot|Joints")
TObjectPtr<USceneComponent> WristFlexJoint;
UPROPERTY(VisibleAnywhere, Category = "Robot|Joints")
TObjectPtr<USceneComponent> WristRollJoint;
UPROPERTY(VisibleAnywhere, Category = "Robot|Joints")
TObjectPtr<USceneComponent> GripperJoint;
// Joint name -> component mapping
UPROPERTY()
TMap<FName, TObjectPtr<USceneComponent>> JointComponentMap;
// Mesh components
UPROPERTY()
TArray<TObjectPtr<UStaticMeshComponent>> AllMeshComponents;
// =================================================================
// ROS connection
// =================================================================
UFUNCTION()
void OnRosBridgeConnected();
UFUNCTION()
void OnRosBridgeDisconnected();
UFUNCTION()
void OnRosMessage(const FString& Topic, const FString& MessageJson);
/** Tracks rosbridge connection state for viewport warnings. */
bool bRosBridgeConnected = false;
void ParseAndApplyJointStates(const FString& MessageJson);
// =================================================================
// MoveIt publish helpers
// =================================================================
/** Advertise MoveIt command topics. Called once on connect. */
void AdvertiseMoveItTopics();
/** Whether MoveIt topics have been advertised in this connection session. */
bool bMoveItTopicsAdvertised = false;
// =================================================================
// Record / Replay / E-Stop helpers
// =================================================================
/** Advertise /robot_command and subscribe /robot_status. Called once on connect. */
void SetupRecordReplayTopics();
/** Whether record/replay topics have been set up. */
bool bRecordReplayTopicsSetup = false;
/** Send a JSON command to /robot_command topic. */
void PublishRobotCommand(const FString& JsonCmd);
/** Handle /robot_status messages from the bridge node. */
UFUNCTION()
void OnRobotStatus(const FString& Topic, const FString& MessageJson);
// =================================================================
// Connection health monitoring (Unreal-side)
// =================================================================
/** Called periodically to check bridge and worker heartbeats. */
void CheckConnectionHealth();
/** Timer handle for health check. */
FTimerHandle ConnectionHealthTimerHandle;
/** Last time we received /bridge_heartbeat. */
double LastBridgeHeartbeatTime = 0.0;
/** Last time we received /joint_states (worker data via bridge). */
double LastJointStatesTime = 0.0;
/** Timeout in seconds for bridge heartbeat (bridge publishes every 1s). */
float BridgeHeartbeatTimeoutSec = 4.0f;
/** Timeout in seconds for worker data (/joint_states at 30Hz). */
float WorkerDataTimeoutSec = 3.0f;
/** Whether bridge heartbeat has been lost. */
bool bBridgeHeartbeatLost = false;
/** Whether worker data has been lost. */
bool bWorkerDataLost = false;
/** Tracks device-level USB/serial error state for recovery messages. */
bool bFollowerDeviceError = false;
bool bLeaderDeviceError = false;
/** The viewport control UI widget instance (Phase 10). */
UPROPERTY(Transient)
TObjectPtr<URobotControlWidget> ControlWidget;
TArray<FRecordingInfo> Recordings;
int32 RecordingsVersion = 0;
int32 ReplayIndex = 0;
int32 ReplayTotal = 0;
FString ReplayProgFilename;
bool bReplayApproaching = false;
// =================================================================
// Helpers (declared in original header, kept for compatibility)
// =================================================================
USceneComponent* CreateJointComponent(const FName& Name, USceneComponent* Parent,
const FVector& Location, const FRotator& Rotation);
USceneComponent* CreateLinkComponent(const FName& Name, USceneComponent* Parent);
UStaticMeshComponent* AttachMesh(USceneComponent* Parent, UStaticMesh* Mesh,
const FName& Name, const FVector& Location, const FRotator& Rotation,
bool bIsMotor);
};
RobotVisualizer.cpp 수정
OnRobotStatus()에 파싱 추가: device error 처리 블록 앞쪽(예: recording saved 처리 다음)에 이 두 블록 추가
// --- Recordings list (reply to list_recordings) ---
const TArray<TSharedPtr<FJsonValue>>* RecArray = nullptr;
if (StatusJson->TryGetArrayField(TEXT("recordings"), RecArray) && RecArray)
{
Recordings.Reset();
for (const TSharedPtr<FJsonValue>& V : *RecArray)
{
const TSharedPtr<FJsonObject> RecObj = V->AsObject();
if (RecObj.IsValid())
{
FRecordingInfo Info;
RecObj->TryGetStringField(TEXT("filename"), Info.Filename);
RecObj->TryGetNumberField(TEXT("frames"), Info.Frames);
double Dur = 0.0;
RecObj->TryGetNumberField(TEXT("duration_sec"), Dur);
Info.DurationSec = static_cast<float>(Dur);
RecObj->TryGetStringField(TEXT("recorded_at"), Info.RecordedAt);
Recordings.Add(Info);
}
}
++RecordingsVersion;
UE_LOG(LogRosBridge, Log, TEXT("Recordings list updated: %d files"), Recordings.Num());
}
// --- Replay progress ---
const TSharedPtr<FJsonObject>* ProgObj = nullptr;
if (StatusJson->TryGetObjectField(TEXT("replay_progress"), ProgObj) && ProgObj)
{
(*ProgObj)->TryGetNumberField(TEXT("index"), ReplayIndex);
(*ProgObj)->TryGetNumberField(TEXT("total"), ReplayTotal);
(*ProgObj)->TryGetStringField(TEXT("filename"), ReplayProgFilename);
(*ProgObj)->TryGetBoolField(TEXT("approaching"), bReplayApproaching);
}
전체 코드
더보기
#include "RobotVisualizer.h"
#include "RosCoordConv.h"
#include "RosBridgeSubsystem.h"
#include "RosBridgeLog.h"
#include "Components/SceneComponent.h"
#include "Components/StaticMeshComponent.h"
#include "Engine/StaticMesh.h"
#include "Engine/Engine.h"
#include "Engine/World.h"
#include "Kismet/GameplayStatics.h"
#include "Dom/JsonObject.h"
#include "Dom/JsonValue.h"
#include "Serialization/JsonReader.h"
#include "Serialization/JsonSerializer.h"
#include "Serialization/JsonWriter.h"
#include "UObject/ConstructorHelpers.h"
#include "GameFramework/PlayerController.h"
#include "Blueprint/UserWidget.h"
#include "RobotControlWidget.h"
// =============================================================================
// Mesh asset path helper
// =============================================================================
static UStaticMesh* LoadMeshAsset(const TCHAR* AssetName)
{
// All meshes live under /Game/Robot/Meshes/
FString Path = FString::Printf(TEXT("/Game/Robot/Meshes/%s.%s"), AssetName, AssetName);
UStaticMesh* Mesh = Cast<UStaticMesh>(StaticLoadObject(UStaticMesh::StaticClass(), nullptr, *Path));
if (!Mesh)
{
UE_LOG(LogRosBridge, Warning, TEXT("Failed to load mesh: %s"), *Path);
}
return Mesh;
}
// =============================================================================
// Constructor — build the entire component hierarchy
// =============================================================================
ARobotVisualizer::ARobotVisualizer()
{
PrimaryActorTick.bCanEverTick = false;
// --- Root ---
RobotRoot = CreateDefaultSubobject<USceneComponent>(TEXT("RobotRoot"));
RootComponent = RobotRoot;
// =========================================================================
// URDF data converted to UE coordinates:
// Position: meters * 100 = cm, Y flipped
// Rotation: RPY radians -> FRotator degrees, pitch & yaw negated
//
// All values below are pre-computed from so101_follower.urdf.
// =========================================================================
// --- base_link (attached directly to root, no joint) ---
BaseLink = CreateDefaultSubobject<USceneComponent>(TEXT("BaseLink"));
BaseLink->SetupAttachment(RobotRoot);
// --- shoulder_pan joint ---
// URDF origin: xyz(0.0388353, 0, 0.0624) rpy(3.14159, 0, -3.14159)
ShoulderPanJoint = CreateDefaultSubobject<USceneComponent>(TEXT("ShoulderPanJoint"));
ShoulderPanJoint->SetupAttachment(BaseLink);
ShoulderPanJoint->SetRelativeLocation(RosCoordConv::RosToUePosition(0.0388353, 0.0, 0.0624));
ShoulderPanJoint->SetRelativeRotation(RosCoordConv::RosRpyToUeRotator(3.14159, 0.0, -3.14159));
ShoulderLink = CreateDefaultSubobject<USceneComponent>(TEXT("ShoulderLink"));
ShoulderLink->SetupAttachment(ShoulderPanJoint);
// --- shoulder_lift joint ---
// URDF origin: xyz(-0.0303992, -0.0182778, -0.0542) rpy(-1.5708, -1.5708, 0)
ShoulderLiftJoint = CreateDefaultSubobject<USceneComponent>(TEXT("ShoulderLiftJoint"));
ShoulderLiftJoint->SetupAttachment(ShoulderLink);
ShoulderLiftJoint->SetRelativeLocation(RosCoordConv::RosToUePosition(-0.0303992, -0.0182778, -0.0542));
ShoulderLiftJoint->SetRelativeRotation(RosCoordConv::RosRpyToUeRotator(-1.5708, -1.5708, 0.0));
UpperArmLink = CreateDefaultSubobject<USceneComponent>(TEXT("UpperArmLink"));
UpperArmLink->SetupAttachment(ShoulderLiftJoint);
// --- elbow_flex joint ---
// URDF origin: xyz(-0.11257, -0.028, 0) rpy(0, 0, 1.5708)
ElbowFlexJoint = CreateDefaultSubobject<USceneComponent>(TEXT("ElbowFlexJoint"));
ElbowFlexJoint->SetupAttachment(UpperArmLink);
ElbowFlexJoint->SetRelativeLocation(RosCoordConv::RosToUePosition(-0.11257, -0.028, 0.0));
ElbowFlexJoint->SetRelativeRotation(RosCoordConv::RosRpyToUeRotator(0.0, 0.0, 1.5708));
LowerArmLink = CreateDefaultSubobject<USceneComponent>(TEXT("LowerArmLink"));
LowerArmLink->SetupAttachment(ElbowFlexJoint);
// --- wrist_flex joint ---
// URDF origin: xyz(-0.1349, 0.0052, 0) rpy(0, 0, -1.5708)
WristFlexJoint = CreateDefaultSubobject<USceneComponent>(TEXT("WristFlexJoint"));
WristFlexJoint->SetupAttachment(LowerArmLink);
WristFlexJoint->SetRelativeLocation(RosCoordConv::RosToUePosition(-0.1349, 0.0052, 0.0));
WristFlexJoint->SetRelativeRotation(RosCoordConv::RosRpyToUeRotator(0.0, 0.0, -1.5708));
WristLink = CreateDefaultSubobject<USceneComponent>(TEXT("WristLink"));
WristLink->SetupAttachment(WristFlexJoint);
// --- wrist_roll joint ---
// URDF origin: xyz(0, -0.0611, 0.0181) rpy(1.5708, 0.0486795, 3.14159)
WristRollJoint = CreateDefaultSubobject<USceneComponent>(TEXT("WristRollJoint"));
WristRollJoint->SetupAttachment(WristLink);
WristRollJoint->SetRelativeLocation(RosCoordConv::RosToUePosition(0.0, -0.0611, 0.0181));
WristRollJoint->SetRelativeRotation(RosCoordConv::RosRpyToUeRotator(1.5708, 0.0486795, 3.14159));
GripperLink = CreateDefaultSubobject<USceneComponent>(TEXT("GripperLink"));
GripperLink->SetupAttachment(WristRollJoint);
// --- gripper joint ---
// URDF origin: xyz(0.0202, 0.0188, -0.0234) rpy(1.5708, 0, 0)
GripperJoint = CreateDefaultSubobject<USceneComponent>(TEXT("GripperJoint"));
GripperJoint->SetupAttachment(GripperLink);
GripperJoint->SetRelativeLocation(RosCoordConv::RosToUePosition(0.0202, 0.0188, -0.0234));
GripperJoint->SetRelativeRotation(RosCoordConv::RosRpyToUeRotator(1.5708, 0.0, 0.0));
MovingJawLink = CreateDefaultSubobject<USceneComponent>(TEXT("MovingJawLink"));
MovingJawLink->SetupAttachment(GripperJoint);
// --- Joint name mapping (matches ROS /joint_states names) ---
JointComponentMap.Add(FName("shoulder_pan"), ShoulderPanJoint);
JointComponentMap.Add(FName("shoulder_lift"), ShoulderLiftJoint);
JointComponentMap.Add(FName("elbow_flex"), ElbowFlexJoint);
JointComponentMap.Add(FName("wrist_flex"), WristFlexJoint);
JointComponentMap.Add(FName("wrist_roll"), WristRollJoint);
JointComponentMap.Add(FName("gripper"), GripperJoint);
}
// =============================================================================
// BeginPlay — load meshes and attach, connect to ROS
// =============================================================================
void ARobotVisualizer::BeginPlay()
{
Super::BeginPlay();
// --- Load meshes and attach to links ---
// Meshes are loaded at runtime (not in constructor) because
// StaticLoadObject is safer to call here and allows hot-reload.
// Helper lambda to reduce repetition
auto Attach = [this](USceneComponent* Parent, const TCHAR* MeshName,
double RosX, double RosY, double RosZ,
double RosRoll, double RosPitch, double RosYaw,
bool bIsMotor = false)
{
UStaticMesh* Mesh = LoadMeshAsset(MeshName);
if (!Mesh) return;
UStaticMeshComponent* SMC = NewObject<UStaticMeshComponent>(this);
SMC->SetStaticMesh(Mesh);
SMC->SetRelativeLocation(RosCoordConv::RosToUePosition(RosX, RosY, RosZ));
SMC->SetRelativeRotation(RosCoordConv::RosRpyToUeRotator(RosRoll, RosPitch, RosYaw));
SMC->SetCollisionEnabled(ECollisionEnabled::NoCollision);
SMC->AttachToComponent(Parent, FAttachmentTransformRules::KeepRelativeTransform);
SMC->RegisterComponent();
AllMeshComponents.Add(SMC);
};
// === base_link meshes ===
Attach(BaseLink, TEXT("base_motor_holder_so101_v1"),
-0.00636471, -0.0000994414, -0.0024,
1.5708, 0.0, 1.5708);
Attach(BaseLink, TEXT("base_so101_v2"),
-0.00636471, 0.0, -0.0024,
1.5708, 0.0, 1.5708);
Attach(BaseLink, TEXT("sts3215_03a_v1"),
0.0263353, 0.0, 0.0437,
0.0, 0.0, 0.0, true);
Attach(BaseLink, TEXT("waveshare_mounting_plate_so101_v2"),
-0.0309827, -0.000199441, 0.0474,
1.5708, 0.0, 1.5708);
// === shoulder_link meshes ===
Attach(ShoulderLink, TEXT("sts3215_03a_v1"),
-0.0303992, 0.000422241, -0.0417,
1.5708, 1.5708, 0.0, true);
Attach(ShoulderLink, TEXT("motor_holder_so101_base_v1"),
-0.0675992, -0.000177759, 0.0158499,
1.5708, -1.5708, 0.0);
Attach(ShoulderLink, TEXT("rotation_pitch_so101_v1"),
0.0122008, 0.0000222413, 0.0464,
-1.5708, 0.0, 0.0);
// === upper_arm_link meshes ===
Attach(UpperArmLink, TEXT("sts3215_03a_v1"),
-0.11257, -0.0155, 0.0187,
-3.14159, 0.0, -1.5708, true);
Attach(UpperArmLink, TEXT("upper_arm_so101_v1"),
-0.065085, 0.012, 0.0182,
3.14159, 0.0, 0.0);
// === lower_arm_link meshes ===
Attach(LowerArmLink, TEXT("under_arm_so101_v1"),
-0.0648499, -0.032, 0.0182,
3.14159, 0.0, 0.0);
Attach(LowerArmLink, TEXT("motor_holder_so101_wrist_v1"),
-0.0648499, -0.032, 0.018,
-3.14159, 0.0, 0.0);
Attach(LowerArmLink, TEXT("sts3215_03a_v1"),
-0.1224, 0.0052, 0.0187,
-3.14159, 0.0, -3.14159, true);
// === wrist_link meshes ===
Attach(WristLink, TEXT("sts3215_03a_no_horn_v1"),
0.0, -0.0424, 0.0306,
1.5708, 1.5708, 0.0, true);
Attach(WristLink, TEXT("wrist_roll_pitch_so101_v2"),
0.0, -0.028, 0.0181,
-1.5708, -1.5708, 0.0);
// === gripper_link meshes ===
Attach(GripperLink, TEXT("sts3215_03a_v1"),
0.0077, 0.0001, -0.0234,
-1.5708, 0.0, 0.0, true);
Attach(GripperLink, TEXT("wrist_roll_follower_so101_v1"),
0.0, -0.000218214, 0.000949706,
-3.14159, 0.0, 0.0);
// === moving_jaw_link meshes ===
Attach(MovingJawLink, TEXT("moving_jaw_so101_v1"),
0.0, 0.0, 0.0189,
0.0, 0.0, 0.0);
UE_LOG(LogRosBridge, Log, TEXT("RobotVisualizer: %d mesh components created"), AllMeshComponents.Num());
// --- Connect to ROS via Subsystem ---
UGameInstance* GI = UGameplayStatics::GetGameInstance(this);
if (!GI) return;
URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>();
if (!Ros) return;
// Bind delegates.
Ros->OnTopicMessage.AddDynamic(this, &ARobotVisualizer::OnRosMessage);
Ros->OnConnected.AddDynamic(this, &ARobotVisualizer::OnRosBridgeConnected);
Ros->OnDisconnected.AddDynamic(this, &ARobotVisualizer::OnRosBridgeDisconnected);
// Subscribe is now queued even before connection — the subsystem will
// send it automatically when connected (including on reconnect).
Ros->Subscribe(JointStateTopic, JointStateType);
// Queue MoveIt topic advertisements (sent on connect).
AdvertiseMoveItTopics();
// Queue record/replay/estop topics.
SetupRecordReplayTopics();
// Subscribe to bridge heartbeat for connection health monitoring.
Ros->Subscribe(TEXT("/bridge_heartbeat"), TEXT("std_msgs/String"));
// Start connection health monitor (checks every 2 seconds).
const double Now = FPlatformTime::Seconds();
LastBridgeHeartbeatTime = Now;
LastJointStatesTime = Now;
GetWorldTimerManager().SetTimer(
ConnectionHealthTimerHandle, this,
&ARobotVisualizer::CheckConnectionHealth, 2.0f, true);
// Initiate connection if not already connected.
if (!Ros->IsConnected())
{
Ros->Connect(RosBridgeUrl);
}
else
{
// Already connected (e.g. another actor already called Connect).
UE_LOG(LogRosBridge, Log, TEXT("RobotVisualizer: already connected, subscription sent."));
}
// --- Spawn the in-viewport control UI (Phase 10) ---
if (ControlWidgetClass)
{
if (APlayerController* PC = UGameplayStatics::GetPlayerController(this, 0))
{
ControlWidget = CreateWidget<URobotControlWidget>(PC, ControlWidgetClass);
if (ControlWidget)
{
ControlWidget->AddToViewport();
UE_LOG(LogRosBridge, Log, TEXT("RobotVisualizer: control widget added to viewport."));
}
}
}
}
// =============================================================================
// EndPlay
// =============================================================================
void ARobotVisualizer::EndPlay(const EEndPlayReason::Type EndPlayReason)
{
if (UGameInstance* GI = UGameplayStatics::GetGameInstance(this))
{
if (URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>())
{
Ros->OnTopicMessage.RemoveDynamic(this, &ARobotVisualizer::OnRosMessage);
Ros->OnConnected.RemoveDynamic(this, &ARobotVisualizer::OnRosBridgeConnected);
Ros->OnDisconnected.RemoveDynamic(this, &ARobotVisualizer::OnRosBridgeDisconnected);
}
}
bMoveItTopicsAdvertised = false;
bRecordReplayTopicsSetup = false;
GetWorldTimerManager().ClearTimer(ConnectionHealthTimerHandle);
if (ControlWidget)
{
ControlWidget->RemoveFromParent();
ControlWidget = nullptr;
}
Super::EndPlay(EndPlayReason);
}
// =============================================================================
// ROS connection callback
// =============================================================================
void ARobotVisualizer::OnRosBridgeConnected()
{
bRosBridgeConnected = true;
UE_LOG(LogRosBridge, Log,
TEXT("RobotVisualizer: rosbridge connected — subscriptions restored by subsystem."));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Green,
TEXT("ROS Bridge: Connected"));
}
}
// =============================================================================
// ROS disconnection callback
// =============================================================================
void ARobotVisualizer::OnRosBridgeDisconnected()
{
bRosBridgeConnected = false;
WorkerState = TEXT("disconnected");
UE_LOG(LogRosBridge, Warning,
TEXT("RobotVisualizer: rosbridge DISCONNECTED — cannot send commands. Auto-reconnect active."));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 10.0f, FColor::Red,
TEXT("*** ROS Bridge DISCONNECTED *** Cannot send commands. Reconnecting..."));
}
}
// =============================================================================
// Connection health monitoring
// =============================================================================
void ARobotVisualizer::CheckConnectionHealth()
{
// Only check when WebSocket is connected — if WebSocket is down,
// OnRosBridgeDisconnected already shows a warning for that.
if (!bRosBridgeConnected)
{
return;
}
const double Now = FPlatformTime::Seconds();
// Check bridge heartbeat (published every 1s by bridge_node)
const double BridgeElapsed = Now - LastBridgeHeartbeatTime;
if (BridgeElapsed > BridgeHeartbeatTimeoutSec && !bBridgeHeartbeatLost)
{
bBridgeHeartbeatLost = true;
bWorkerDataLost = true; // if bridge is down, worker data can't reach us either
WorkerState = TEXT("bridge lost");
UE_LOG(LogRosBridge, Warning,
TEXT("Bridge heartbeat lost (%.1fs). bridge_node may be down."), BridgeElapsed);
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 10.0f, FColor::Red,
TEXT("*** Bridge Node: DOWN *** Check bridge_node terminal (terminal 2)."));
}
return; // no need to check worker separately
}
// Check worker data (/joint_states at ~30Hz, flows through bridge)
// Only meaningful if bridge is alive.
if (!bBridgeHeartbeatLost)
{
const double WorkerElapsed = Now - LastJointStatesTime;
if (WorkerElapsed > WorkerDataTimeoutSec && !bWorkerDataLost)
{
bWorkerDataLost = true;
WorkerState = TEXT("worker lost");
UE_LOG(LogRosBridge, Warning,
TEXT("Worker data lost (%.1fs). lerobot_worker may be down."), WorkerElapsed);
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 10.0f, FColor::Red,
TEXT("*** Worker: DOWN *** Check lerobot_worker terminal (terminal 1)."));
}
}
}
}
// =============================================================================
// MoveIt topic advertisements
// =============================================================================
void ARobotVisualizer::AdvertiseMoveItTopics()
{
UGameInstance* GI = UGameplayStatics::GetGameInstance(this);
if (!GI) return;
URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>();
if (!Ros) return;
// These are queued and sent automatically when connected.
Ros->Advertise(TEXT("/moveit_goal_named"), TEXT("std_msgs/String"));
Ros->Advertise(TEXT("/moveit_goal_joints"), TEXT("sensor_msgs/JointState"));
Ros->Advertise(TEXT("/moveit_goal_pose"), TEXT("geometry_msgs/PoseStamped"));
bMoveItTopicsAdvertised = true;
UE_LOG(LogRosBridge, Log, TEXT("RobotVisualizer: MoveIt command topics advertised."));
}
// =============================================================================
// MoveIt commands — SendNamedTarget
// =============================================================================
void ARobotVisualizer::SendNamedTarget()
{
UGameInstance* GI = UGameplayStatics::GetGameInstance(this);
if (!GI) return;
URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>();
if (!Ros || !Ros->IsConnected())
{
UE_LOG(LogRosBridge, Warning, TEXT("SendNamedTarget: not connected to rosbridge."));
return;
}
// std_msgs/String: {"data": "home"}
FString MsgJson = FString::Printf(TEXT("{\"data\":\"%s\"}"), *MoveItNamedTarget);
Ros->Publish(TEXT("/moveit_goal_named"), MsgJson);
UE_LOG(LogRosBridge, Log, TEXT("SendNamedTarget: published '%s' to /moveit_goal_named"), *MoveItNamedTarget);
}
// =============================================================================
// MoveIt commands — SendJointGoal
// =============================================================================
void ARobotVisualizer::SendJointGoal()
{
UGameInstance* GI = UGameplayStatics::GetGameInstance(this);
if (!GI) return;
URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>();
if (!Ros || !Ros->IsConnected())
{
UE_LOG(LogRosBridge, Warning, TEXT("SendJointGoal: not connected to rosbridge."));
return;
}
// sensor_msgs/JointState:
// {
// "header": {"stamp": {"sec": 0, "nanosec": 0}, "frame_id": ""},
// "name": ["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll"],
// "position": [0.0, -0.5, 0.5, 0.0, 0.0],
// "velocity": [],
// "effort": []
// }
FString MsgJson = FString::Printf(
TEXT("{\"header\":{\"stamp\":{\"sec\":0,\"nanosec\":0},\"frame_id\":\"\"},")
TEXT("\"name\":[\"shoulder_pan\",\"shoulder_lift\",\"elbow_flex\",\"wrist_flex\",\"wrist_roll\"],")
TEXT("\"position\":[%f,%f,%f,%f,%f],")
TEXT("\"velocity\":[],\"effort\":[]}"),
GoalShoulderPan, GoalShoulderLift, GoalElbowFlex, GoalWristFlex, GoalWristRoll
);
Ros->Publish(TEXT("/moveit_goal_joints"), MsgJson);
UE_LOG(LogRosBridge, Log,
TEXT("SendJointGoal: [%.3f, %.3f, %.3f, %.3f, %.3f] to /moveit_goal_joints"),
GoalShoulderPan, GoalShoulderLift, GoalElbowFlex, GoalWristFlex, GoalWristRoll);
}
// =============================================================================
// MoveIt commands — SendPoseGoal
// =============================================================================
void ARobotVisualizer::SendPoseGoal()
{
UGameInstance* GI = UGameplayStatics::GetGameInstance(this);
if (!GI) return;
URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>();
if (!Ros || !Ros->IsConnected())
{
UE_LOG(LogRosBridge, Warning, TEXT("SendPoseGoal: not connected to rosbridge."));
return;
}
// Convert UE position (cm, left-handed) to ROS (meters, right-handed)
double RosX, RosY, RosZ;
RosCoordConv::UeToRosPosition(GoalPositionUE, RosX, RosY, RosZ);
// geometry_msgs/PoseStamped (orientation defaults to identity — position-only goal)
FString MsgJson = FString::Printf(
TEXT("{\"header\":{\"stamp\":{\"sec\":0,\"nanosec\":0},\"frame_id\":\"base_link\"},")
TEXT("\"pose\":{\"position\":{\"x\":%f,\"y\":%f,\"z\":%f},")
TEXT("\"orientation\":{\"x\":0.0,\"y\":0.0,\"z\":0.0,\"w\":1.0}}}"),
RosX, RosY, RosZ
);
Ros->Publish(TEXT("/moveit_goal_pose"), MsgJson);
UE_LOG(LogRosBridge, Log,
TEXT("SendPoseGoal: UE(%.1f, %.1f, %.1f)cm -> ROS(%.4f, %.4f, %.4f)m to /moveit_goal_pose"),
GoalPositionUE.X, GoalPositionUE.Y, GoalPositionUE.Z,
RosX, RosY, RosZ);
}
// =============================================================================
// Message handling
// =============================================================================
void ARobotVisualizer::OnRosMessage(const FString& Topic, const FString& MessageJson)
{
if (Topic == JointStateTopic)
{
// /joint_states arrives at ~30Hz — proves both bridge AND worker are alive.
LastJointStatesTime = FPlatformTime::Seconds();
LastBridgeHeartbeatTime = LastJointStatesTime; // joint_states flows through bridge
if (bWorkerDataLost)
{
bWorkerDataLost = false;
UE_LOG(LogRosBridge, Log, TEXT("Worker data restored."));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Green,
TEXT("Worker: Connection restored"));
}
}
if (bBridgeHeartbeatLost)
{
bBridgeHeartbeatLost = false;
UE_LOG(LogRosBridge, Log, TEXT("Bridge heartbeat restored (via joint_states)."));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Green,
TEXT("Bridge: Connection restored"));
}
}
ParseAndApplyJointStates(MessageJson);
}
else if (Topic == TEXT("/bridge_heartbeat"))
{
// Bridge heartbeat arrives every 1s — proves bridge is alive
// (but worker may still be dead if /joint_states is not arriving).
LastBridgeHeartbeatTime = FPlatformTime::Seconds();
if (bBridgeHeartbeatLost)
{
bBridgeHeartbeatLost = false;
UE_LOG(LogRosBridge, Log, TEXT("Bridge heartbeat restored."));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Green,
TEXT("Bridge: Connection restored"));
}
}
}
else if (Topic == TEXT("/robot_status"))
{
OnRobotStatus(Topic, MessageJson);
}
}
void ARobotVisualizer::ParseAndApplyJointStates(const FString& MessageJson)
{
// Parse sensor_msgs/JointState JSON:
// { "name": ["shoulder_pan", ...], "position": [0.1, ...], ... }
TSharedPtr<FJsonObject> Json;
TSharedRef<TJsonReader<>> Reader = TJsonReaderFactory<>::Create(MessageJson);
if (!FJsonSerializer::Deserialize(Reader, Json) || !Json.IsValid())
{
return;
}
const TArray<TSharedPtr<FJsonValue>>* NameArray = nullptr;
const TArray<TSharedPtr<FJsonValue>>* PosArray = nullptr;
if (!Json->TryGetArrayField(TEXT("name"), NameArray) ||
!Json->TryGetArrayField(TEXT("position"), PosArray))
{
return;
}
const int32 Count = FMath::Min(NameArray->Num(), PosArray->Num());
for (int32 i = 0; i < Count; ++i)
{
const FName JointName(*(*NameArray)[i]->AsString());
const double AngleRad = (*PosArray)[i]->AsNumber();
TObjectPtr<USceneComponent>* JointComp = JointComponentMap.Find(JointName);
if (JointComp && *JointComp)
{
const float AngleDeg = RosCoordConv::RosJointAngleToUeDegrees(AngleRad);
FRotator BaseRot;
if (JointName == FName("shoulder_pan"))
BaseRot = RosCoordConv::RosRpyToUeRotator(3.14159, 0.0, -3.14159);
else if (JointName == FName("shoulder_lift"))
BaseRot = RosCoordConv::RosRpyToUeRotator(-1.5708, -1.5708, 0.0);
else if (JointName == FName("elbow_flex"))
BaseRot = RosCoordConv::RosRpyToUeRotator(0.0, 0.0, 1.5708);
else if (JointName == FName("wrist_flex"))
BaseRot = RosCoordConv::RosRpyToUeRotator(0.0, 0.0, -1.5708);
else if (JointName == FName("wrist_roll"))
BaseRot = RosCoordConv::RosRpyToUeRotator(1.5708, 0.0486795, 3.14159);
else if (JointName == FName("gripper"))
BaseRot = RosCoordConv::RosRpyToUeRotator(1.5708, 0.0, 0.0);
const FQuat BaseQuat = BaseRot.Quaternion();
const FQuat JointQuat = FQuat(FVector::UpVector, FMath::DegreesToRadians(AngleDeg));
const FQuat FinalQuat = BaseQuat * JointQuat;
(*JointComp)->SetRelativeRotation(FinalQuat.Rotator());
}
}
}
// =============================================================================
// Record / Replay / E-Stop — Topic setup
// =============================================================================
void ARobotVisualizer::SetupRecordReplayTopics()
{
UGameInstance* GI = UGameplayStatics::GetGameInstance(this);
if (!GI) return;
URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>();
if (!Ros) return;
// Advertise the command topic
Ros->Advertise(TEXT("/robot_command"), TEXT("std_msgs/String"));
// Subscribe to status feedback
Ros->Subscribe(TEXT("/robot_status"), TEXT("std_msgs/String"));
bRecordReplayTopicsSetup = true;
UE_LOG(LogRosBridge, Log, TEXT("RobotVisualizer: Record/Replay topics set up."));
}
// =============================================================================
// Record / Replay / E-Stop — Command publisher helper
// =============================================================================
void ARobotVisualizer::PublishRobotCommand(const FString& JsonCmd)
{
UGameInstance* GI = UGameplayStatics::GetGameInstance(this);
if (!GI) return;
URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>();
if (!Ros || !Ros->IsConnected())
{
UE_LOG(LogRosBridge, Warning, TEXT("PublishRobotCommand: not connected to rosbridge."));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 3.0f, FColor::Red,
TEXT("Robot: Not connected to rosbridge!"));
}
return;
}
// std_msgs/String: {"data": "<json_cmd>"}
// The JSON command is nested inside the "data" field, with quotes escaped.
FString EscapedCmd = JsonCmd.Replace(TEXT("\""), TEXT("\\\""));
FString MsgJson = FString::Printf(TEXT("{\"data\":\"%s\"}"), *EscapedCmd);
Ros->Publish(TEXT("/robot_command"), MsgJson);
UE_LOG(LogRosBridge, Log, TEXT("PublishRobotCommand: %s"), *JsonCmd);
}
// =============================================================================
// Record / Replay / E-Stop — Button handlers
// =============================================================================
void ARobotVisualizer::StartRecord()
{
PublishRobotCommand(TEXT("{\"cmd\":\"start_record\"}"));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 3.0f, FColor::Green,
TEXT("Robot: Recording started (teleop active)"));
}
}
void ARobotVisualizer::StopRecord()
{
PublishRobotCommand(TEXT("{\"cmd\":\"stop_record\"}"));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 3.0f, FColor::Yellow,
TEXT("Robot: Recording stopped, saving..."));
}
}
void ARobotVisualizer::StartReplay()
{
FString ArgsJson;
if (ReplayFilename.IsEmpty())
{
ArgsJson = FString::Printf(
TEXT("{\"cmd\":\"start_replay\",\"args\":{\"loop\":%s,\"approach_speed\":%f}}"),
bReplayLoop ? TEXT("true") : TEXT("false"),
ApproachSpeed);
}
else
{
ArgsJson = FString::Printf(
TEXT("{\"cmd\":\"start_replay\",\"args\":{\"filename\":\"%s\",\"loop\":%s,\"approach_speed\":%f}}"),
*ReplayFilename,
bReplayLoop ? TEXT("true") : TEXT("false"),
ApproachSpeed);
}
PublishRobotCommand(ArgsJson);
if (GEngine)
{
FString DisplayName = ReplayFilename.IsEmpty() ? TEXT("(most recent)") : ReplayFilename;
GEngine->AddOnScreenDebugMessage(-1, 3.0f, FColor::Cyan,
FString::Printf(TEXT("Robot: Replaying %s (loop=%s, speed=%.0f°/s)"),
*DisplayName, bReplayLoop ? TEXT("yes") : TEXT("no"), ApproachSpeed));
}
}
void ARobotVisualizer::StopReplay()
{
PublishRobotCommand(TEXT("{\"cmd\":\"stop_replay\"}"));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 3.0f, FColor::Yellow,
TEXT("Robot: Replay stopped"));
}
}
void ARobotVisualizer::EStop()
{
PublishRobotCommand(TEXT("{\"cmd\":\"estop\"}"));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Red,
TEXT("*** E-STOP *** All motion halted"));
}
}
// =============================================================================
// Teleop Sync — SyncOn / SyncOff
// =============================================================================
void ARobotVisualizer::SyncOn()
{
PublishRobotCommand(TEXT("{\"cmd\":\"start_teleop\"}"));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 3.0f, FColor::Green,
TEXT("Sync ON: leader -> follower active"));
}
}
void ARobotVisualizer::SyncOff()
{
PublishRobotCommand(TEXT("{\"cmd\":\"stop_teleop\"}"));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 3.0f, FColor::Yellow,
TEXT("Sync OFF: leader -> follower deactivated"));
}
}
// =============================================================================
// Record / Replay — Status feedback handler
// =============================================================================
void ARobotVisualizer::OnRobotStatus(const FString& Topic, const FString& MessageJson)
{
// /robot_status flows through bridge from worker — both are alive.
const double Now = FPlatformTime::Seconds();
LastBridgeHeartbeatTime = Now;
LastJointStatesTime = Now;
if (bBridgeHeartbeatLost)
{
bBridgeHeartbeatLost = false;
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Green,
TEXT("Bridge: Connection restored"));
}
}
if (bWorkerDataLost)
{
bWorkerDataLost = false;
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Green,
TEXT("Worker: Connection restored"));
}
}
// rosbridge wraps std_msgs/String as: {"data": "..."}
TSharedPtr<FJsonObject> OuterJson;
TSharedRef<TJsonReader<>> OuterReader = TJsonReaderFactory<>::Create(MessageJson);
if (!FJsonSerializer::Deserialize(OuterReader, OuterJson) || !OuterJson.IsValid())
{
return;
}
FString DataStr;
if (!OuterJson->TryGetStringField(TEXT("data"), DataStr))
{
return;
}
// Parse the inner JSON status
TSharedPtr<FJsonObject> StatusJson;
TSharedRef<TJsonReader<>> StatusReader = TJsonReaderFactory<>::Create(DataStr);
if (!FJsonSerializer::Deserialize(StatusReader, StatusJson) || !StatusJson.IsValid())
{
return;
}
FString State;
if (StatusJson->TryGetStringField(TEXT("state"), State))
{
if (State != WorkerState)
{
WorkerState = State;
UE_LOG(LogRosBridge, Log, TEXT("Worker state: %s"), *WorkerState);
if (GEngine)
{
FColor Color = FColor::White;
if (State == TEXT("recording")) Color = FColor::Green;
else if (State == TEXT("replaying")) Color = FColor::Cyan;
else if (State == TEXT("idle")) Color = FColor::Silver;
GEngine->AddOnScreenDebugMessage(-1, 3.0f, Color,
FString::Printf(TEXT("Robot state: %s"), *WorkerState));
}
}
}
// Update sync (teleop) status
bool bTeleop = false;
if (StatusJson->TryGetBoolField(TEXT("teleop"), bTeleop))
{
if (bTeleop != bSyncActive)
{
bSyncActive = bTeleop;
UE_LOG(LogRosBridge, Log, TEXT("Sync (teleop): %s"),
bSyncActive ? TEXT("ON") : TEXT("OFF"));
}
}
// Log errors from commands
FString Status;
if (StatusJson->TryGetStringField(TEXT("status"), Status) && Status == TEXT("error"))
{
FString Reason;
StatusJson->TryGetStringField(TEXT("reason"), Reason);
UE_LOG(LogRosBridge, Warning, TEXT("Robot command error: %s"), *Reason);
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Red,
FString::Printf(TEXT("Robot error: %s"), *Reason));
}
}
// Log recording saved info
FString Filename;
if (StatusJson->TryGetStringField(TEXT("filename"), Filename) && !Filename.IsEmpty())
{
int32 Frames = 0;
StatusJson->TryGetNumberField(TEXT("frames"), Frames);
double Duration = 0.0;
StatusJson->TryGetNumberField(TEXT("duration_sec"), Duration);
UE_LOG(LogRosBridge, Log, TEXT("Recording saved: %s (%d frames, %.1fs)"),
*Filename, Frames, Duration);
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Green,
FString::Printf(TEXT("Recording saved: %s (%d frames, %.1fs)"),
*Filename, Frames, Duration));
}
}
// --- Recordings list (reply to list_recordings) ---
const TArray<TSharedPtr<FJsonValue>>* RecArray = nullptr;
if (StatusJson->TryGetArrayField(TEXT("recordings"), RecArray) && RecArray)
{
Recordings.Reset();
for (const TSharedPtr<FJsonValue>& V : *RecArray)
{
const TSharedPtr<FJsonObject> RecObj = V->AsObject();
if (RecObj.IsValid())
{
FRecordingInfo Info;
RecObj->TryGetStringField(TEXT("filename"), Info.Filename);
RecObj->TryGetNumberField(TEXT("frames"), Info.Frames);
double Dur = 0.0;
RecObj->TryGetNumberField(TEXT("duration_sec"), Dur);
Info.DurationSec = static_cast<float>(Dur);
RecObj->TryGetStringField(TEXT("recorded_at"), Info.RecordedAt);
Recordings.Add(Info);
}
}
++RecordingsVersion;
UE_LOG(LogRosBridge, Log, TEXT("Recordings list updated: %d files"), Recordings.Num());
}
// --- Replay progress ---
const TSharedPtr<FJsonObject>* ProgObj = nullptr;
if (StatusJson->TryGetObjectField(TEXT("replay_progress"), ProgObj) && ProgObj)
{
(*ProgObj)->TryGetNumberField(TEXT("index"), ReplayIndex);
(*ProgObj)->TryGetNumberField(TEXT("total"), ReplayTotal);
(*ProgObj)->TryGetStringField(TEXT("filename"), ReplayProgFilename);
(*ProgObj)->TryGetBoolField(TEXT("approaching"), bReplayApproaching);
}
// Display device-level errors (USB disconnection, serial errors)
const TSharedPtr<FJsonObject>* DeviceErrors = nullptr;
bool bHasDeviceErrors = StatusJson->TryGetObjectField(TEXT("device_errors"), DeviceErrors) && DeviceErrors;
// Check follower error
FString FollowerErr;
if (bHasDeviceErrors)
{
(*DeviceErrors)->TryGetStringField(TEXT("follower"), FollowerErr);
}
if (!FollowerErr.IsEmpty() && !bFollowerDeviceError)
{
bFollowerDeviceError = true;
UE_LOG(LogRosBridge, Error, TEXT("Follower USB error: %s"), *FollowerErr);
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 10.0f, FColor::Red,
TEXT("*** Follower: USB/Serial ERROR *** Check USB connection."));
}
}
else if (FollowerErr.IsEmpty() && bFollowerDeviceError)
{
bFollowerDeviceError = false;
UE_LOG(LogRosBridge, Log, TEXT("Follower USB restored."));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Green,
TEXT("Follower: USB connection restored"));
}
}
// Check leader error
FString LeaderErr;
if (bHasDeviceErrors)
{
(*DeviceErrors)->TryGetStringField(TEXT("leader"), LeaderErr);
}
if (!LeaderErr.IsEmpty() && !bLeaderDeviceError)
{
bLeaderDeviceError = true;
UE_LOG(LogRosBridge, Error, TEXT("Leader USB error: %s"), *LeaderErr);
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 10.0f, FColor::Red,
TEXT("*** Leader: USB/Serial ERROR *** Check USB connection."));
}
}
else if (LeaderErr.IsEmpty() && bLeaderDeviceError)
{
bLeaderDeviceError = false;
UE_LOG(LogRosBridge, Log, TEXT("Leader USB restored."));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Green,
TEXT("Leader: USB connection restored"));
}
}
}
Source/SO101_Twin/UI/RecordingEntryWidget.h 새 파일 생성
#pragma once
#include "CoreMinimal.h"
#include "Blueprint/UserWidget.h"
#include "Blueprint/IUserObjectListEntry.h"
#include "RecordingEntryWidget.generated.h"
/** Data backing one recording row in the ListView. */
UCLASS(BlueprintType)
class SO101_TWIN_API URecordingEntryData : public UObject
{
GENERATED_BODY()
public:
UPROPERTY(BlueprintReadOnly, Category = "ROS|UI") FString Filename;
UPROPERTY(BlueprintReadOnly, Category = "ROS|UI") int32 Frames = 0;
UPROPERTY(BlueprintReadOnly, Category = "ROS|UI") float DurationSec = 0.0f;
UPROPERTY(BlueprintReadOnly, Category = "ROS|UI") FString RecordedAt;
};
/** Visual row for the recordings ListView. WBP_RecordingEntry reparents to this. */
UCLASS()
class SO101_TWIN_API URecordingEntryWidget : public UUserWidget, public IUserObjectListEntry
{
GENERATED_BODY()
protected:
virtual void NativeOnListItemObjectSet(UObject* ListItemObject) override;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UTextBlock> FilenameText;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UTextBlock> MetaText;
};
Source/SO101_Twin/UI/RecordingEntryWidget.cpp 새 파일 생성
#include "RecordingEntryWidget.h"
#include "Components/TextBlock.h"
void URecordingEntryWidget::NativeOnListItemObjectSet(UObject* ListItemObject)
{
URecordingEntryData* Data = Cast<URecordingEntryData>(ListItemObject);
if (!Data) return;
if (FilenameText)
{
FilenameText->SetText(FText::FromString(Data->Filename));
}
if (MetaText)
{
const FString Meta = FString::Printf(TEXT("%d frames | %.1fs"), Data->Frames, Data->DurationSec);
MetaText->SetText(FText::FromString(Meta));
}
}
RobotControlWidget.h 수정
protected에 추가
UFUNCTION() void HandleRefreshRecordingsClicked();
UFUNCTION() void HandleRecordingSelected(FString SelectedItem, ESelectInfo::Type SelectionType);
void RefreshRecordingsList();
void RefreshReplayProgress();
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UButton> RefreshRecordingsButton;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UComboBoxString> RecordingComboBox;
bool bInitialListRequested = false;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UProgressBar> ReplayProgressBar;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UTextBlock> ReplayProgressText;
int32 CachedRecordingsVersion = -1;
전체 코드
#pragma once
#include "CoreMinimal.h"
#include "Blueprint/UserWidget.h"
#include "RobotControlWidget.generated.h"
class ARobotVisualizer;
class URosBridgeSubsystem;
/**
* Base class for the in-viewport robot control UI (Phase 10).
*
* This C++ base owns the logic: it finds the ARobotVisualizer in the level,
* exposes the robot's commands as BlueprintCallable functions (bind WBP
* buttons to these), and exposes its state as BlueprintPure getters (bind
* WBP text / visibility to these).
*
* Create a Widget Blueprint (WBP_RobotControl) reparented to this class,
* lay out the visuals in the UMG designer, and wire buttons/text to the
* functions below. No Blueprint scripting required — just bindings.
*/
UCLASS()
class SO101_TWIN_API URobotControlWidget : public UUserWidget
{
GENERATED_BODY()
public:
// --- Commands (bind WBP buttons' OnClicked to these) ---
UFUNCTION(BlueprintCallable, Category = "ROS|UI")
void CmdSyncOn();
UFUNCTION(BlueprintCallable, Category = "ROS|UI")
void CmdSyncOff();
UFUNCTION(BlueprintCallable, Category = "ROS|UI")
void CmdStartRecord();
UFUNCTION(BlueprintCallable, Category = "ROS|UI")
void CmdStopRecord();
UFUNCTION(BlueprintCallable, Category = "ROS|UI")
void CmdStartReplay(const FString& Filename, bool bLoop, float ApproachSpeed);
UFUNCTION(BlueprintCallable, Category = "ROS|UI")
void CmdStopReplay();
UFUNCTION(BlueprintCallable, Category = "ROS|UI")
void CmdEStop();
// --- State getters (bind WBP text / visibility to these) ---
UFUNCTION(BlueprintPure, Category = "ROS|UI")
FString GetWorkerState() const;
UFUNCTION(BlueprintPure, Category = "ROS|UI")
bool IsSyncActive() const;
UFUNCTION(BlueprintPure, Category = "ROS|UI")
bool IsRosBridgeConnected() const;
UFUNCTION(BlueprintPure, Category = "ROS|UI")
bool IsBridgeNodeAlive() const;
UFUNCTION(BlueprintPure, Category = "ROS|UI")
bool IsWorkerAlive() const;
UFUNCTION(BlueprintPure, Category = "ROS|UI")
bool HasFollowerError() const;
UFUNCTION(BlueprintPure, Category = "ROS|UI")
bool HasLeaderError() const;
/** True once the robot actor has been located in the level. */
UFUNCTION(BlueprintPure, Category = "ROS|UI")
bool HasRobot() const;
protected:
virtual void NativeConstruct() override;
/** Find and cache the ARobotVisualizer + subsystem. Safe to call repeatedly. */
ARobotVisualizer* ResolveRobot();
virtual void NativeTick(const FGeometry& MyGeometry, float InDeltaTime) override;
UFUNCTION()
void HandleEStopClicked();
/** Recompute the safety panel from the robot's current state. */
void RefreshSafetyUI();
// --- Bound widgets: names MUST match the WBP widget names exactly ---
UPROPERTY(meta = (BindWidget))
TObjectPtr<class UButton> EStopButton;
UPROPERTY(meta = (BindWidget))
TObjectPtr<class UTextBlock> ConnectionStatusText;
UPROPERTY(meta = (BindWidget))
TObjectPtr<class UTextBlock> DeviceErrorText;
float RefreshAccum = 0.0f;
TWeakObjectPtr<ARobotVisualizer> Robot;
TWeakObjectPtr<URosBridgeSubsystem> Ros;
UFUNCTION() void HandleSyncOnClicked();
UFUNCTION() void HandleSyncOffClicked();
UFUNCTION() void HandleStartRecordClicked();
UFUNCTION() void HandleStopRecordClicked();
UFUNCTION() void HandleStartReplayClicked();
UFUNCTION() void HandleStopReplayClicked();
/** Recompute the control panel (worker state label + button enable states). */
void RefreshControlUI();
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UButton> SyncOnButton;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UButton> SyncOffButton;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UButton> StartRecordButton;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UButton> StopRecordButton;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UButton> StartReplayButton;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UButton> StopReplayButton;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UEditableTextBox> ReplayFilenameTextBox;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UCheckBox> LoopCheckBox;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class USpinBox> ApproachSpeedSpinBox;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UTextBlock> WorkerStateText;
UFUNCTION() void HandleRefreshRecordingsClicked();
UFUNCTION() void HandleRecordingSelected(FString SelectedItem, ESelectInfo::Type SelectionType);
void RefreshRecordingsList();
void RefreshReplayProgress();
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UButton> RefreshRecordingsButton;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UComboBoxString> RecordingComboBox;
bool bInitialListRequested = false;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UProgressBar> ReplayProgressBar;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UTextBlock> ReplayProgressText;
int32 CachedRecordingsVersion = -1;
};
RobotControlWidget.cpp 수정
- include
#include "Components/ListView.h"
#include "Components/ComboBoxString.h"
#include "Components/ProgressBar.h"
#include "RecordingEntryWidget.h"
- NativeConstruct()의 바인딩 구역에 추가
if (RefreshRecordingsButton)
{
RefreshRecordingsButton->OnClicked.AddDynamic(this, &URobotControlWidget::HandleRefreshRecordingsClicked);
}
if (RecordingComboBox)
{
RecordingComboBox->OnSelectionChanged.AddDynamic(this, &URobotControlWidget::HandleRecordingSelected);
}
- NativeTick의 throttle 블록에 두 줄 추가 (RefreshControlUI(); 다음)
RefreshRecordingsList();
RefreshReplayProgress();
- 함수 구현 추가
void URobotControlWidget::HandleRefreshRecordingsClicked()
{
if (ARobotVisualizer* R = ResolveRobot())
{
R->PublishRobotCommand(TEXT("{\"cmd\":\"list_recordings\"}"));
}
}
void URobotControlWidget::HandleRecordingSelected(FString SelectedItem, ESelectInfo::Type SelectionType)
{
if (ReplayFilenameTextBox && !SelectedItem.IsEmpty())
{
ReplayFilenameTextBox->SetText(FText::FromString(SelectedItem));
}
}
void URobotControlWidget::RefreshRecordingsList()
{
if (!RecordingComboBox) return;
ARobotVisualizer* R = Robot.Get();
if (!R) return;
// Auto-request the list once when the worker first comes alive (self-heals
// the "sometimes empty" case without needing a manual Refresh click).
if (!bInitialListRequested && IsWorkerAlive())
{
bInitialListRequested = true;
R->PublishRobotCommand(TEXT("{\"cmd\":\"list_recordings\"}"));
}
if (!IsWorkerAlive())
{
bInitialListRequested = false; // allow re-request after a reconnect
}
// Only rebuild when the actor's recordings actually changed.
if (R->GetRecordingsVersion() == CachedRecordingsVersion) return;
CachedRecordingsVersion = R->GetRecordingsVersion();
const FString Prev = RecordingComboBox->GetSelectedOption();
RecordingComboBox->ClearOptions();
for (const FRecordingInfo& Info : R->GetRecordings())
{
RecordingComboBox->AddOption(Info.Filename);
}
// Keep the old selection if still present, otherwise pick the most recent.
if (!Prev.IsEmpty() && RecordingComboBox->FindOptionIndex(Prev) != INDEX_NONE)
{
RecordingComboBox->SetSelectedOption(Prev);
}
else if (RecordingComboBox->GetOptionCount() > 0)
{
RecordingComboBox->SetSelectedIndex(RecordingComboBox->GetOptionCount() - 1);
}
}
void URobotControlWidget::RefreshReplayProgress()
{
const bool bReplaying = (GetWorkerState() == TEXT("replaying"));
ARobotVisualizer* R = Robot.Get();
if (ReplayProgressBar)
{
float Pct = 0.0f;
if (bReplaying && R && R->GetReplayTotal() > 0)
{
Pct = static_cast<float>(R->GetReplayIndex()) / static_cast<float>(R->GetReplayTotal());
}
ReplayProgressBar->SetPercent(Pct);
}
if (ReplayProgressText)
{
if (bReplaying && R)
{
const FString T = R->IsReplayApproaching()
? TEXT("approaching...")
: FString::Printf(TEXT("%d / %d"), R->GetReplayIndex(), R->GetReplayTotal());
ReplayProgressText->SetText(FText::FromString(T));
}
else
{
ReplayProgressText->SetText(FText::GetEmpty());
}
}
}
전체 코드
더보기
#include "RobotControlWidget.h"
#include "RobotVisualizer.h"
#include "RosBridgeSubsystem.h"
#include "Kismet/GameplayStatics.h"
#include "Engine/World.h"
#include "Engine/GameInstance.h"
#include "Components/Button.h"
#include "Components/TextBlock.h"
#include "Components/EditableTextBox.h"
#include "Components/CheckBox.h"
#include "Components/SpinBox.h"
#include "Components/ListView.h"
#include "Components/ComboBoxString.h"
#include "Components/ProgressBar.h"
#include "RecordingEntryWidget.h"
void URobotControlWidget::NativeConstruct()
{
Super::NativeConstruct();
ResolveRobot();
if (EStopButton)
{
EStopButton->OnClicked.AddDynamic(this, &URobotControlWidget::HandleEStopClicked);
}
if (SyncOnButton) SyncOnButton->OnClicked.AddDynamic(this, &URobotControlWidget::HandleSyncOnClicked);
if (SyncOffButton) SyncOffButton->OnClicked.AddDynamic(this, &URobotControlWidget::HandleSyncOffClicked);
if (StartRecordButton) StartRecordButton->OnClicked.AddDynamic(this, &URobotControlWidget::HandleStartRecordClicked);
if (StopRecordButton) StopRecordButton->OnClicked.AddDynamic(this, &URobotControlWidget::HandleStopRecordClicked);
if (StartReplayButton) StartReplayButton->OnClicked.AddDynamic(this, &URobotControlWidget::HandleStartReplayClicked);
if (StopReplayButton) StopReplayButton->OnClicked.AddDynamic(this, &URobotControlWidget::HandleStopReplayClicked);
if (ApproachSpeedSpinBox)
{
ApproachSpeedSpinBox->SetMinValue(5.0f);
ApproachSpeedSpinBox->SetMaxValue(300.0f);
ApproachSpeedSpinBox->SetMinSliderValue(5.0f);
ApproachSpeedSpinBox->SetMaxSliderValue(300.0f);
ApproachSpeedSpinBox->SetValue(45.0f);
}
RefreshControlUI();
RefreshSafetyUI();
if (RefreshRecordingsButton)
{
RefreshRecordingsButton->OnClicked.AddDynamic(this, &URobotControlWidget::HandleRefreshRecordingsClicked);
}
if (RecordingComboBox)
{
RecordingComboBox->OnSelectionChanged.AddDynamic(this, &URobotControlWidget::HandleRecordingSelected);
}
}
ARobotVisualizer* URobotControlWidget::ResolveRobot()
{
if (!Robot.IsValid())
{
if (UWorld* World = GetWorld())
{
Robot = Cast<ARobotVisualizer>(
UGameplayStatics::GetActorOfClass(World, ARobotVisualizer::StaticClass()));
}
}
if (!Ros.IsValid())
{
if (UGameInstance* GI = GetGameInstance())
{
Ros = GI->GetSubsystem<URosBridgeSubsystem>();
}
}
return Robot.Get();
}
// --- Commands ---
void URobotControlWidget::CmdSyncOn() { if (ARobotVisualizer* R = ResolveRobot()) { R->SyncOn(); } }
void URobotControlWidget::CmdSyncOff() { if (ARobotVisualizer* R = ResolveRobot()) { R->SyncOff(); } }
void URobotControlWidget::CmdStartRecord() { if (ARobotVisualizer* R = ResolveRobot()) { R->StartRecord(); } }
void URobotControlWidget::CmdStopRecord() { if (ARobotVisualizer* R = ResolveRobot()) { R->StopRecord(); } }
void URobotControlWidget::CmdStopReplay() { if (ARobotVisualizer* R = ResolveRobot()) { R->StopReplay(); } }
void URobotControlWidget::CmdEStop() { if (ARobotVisualizer* R = ResolveRobot()) { R->EStop(); } }
void URobotControlWidget::CmdStartReplay(const FString& Filename, bool bLoop, float ApproachSpeed)
{
if (ARobotVisualizer* R = ResolveRobot())
{
R->ReplayFilename = Filename;
R->bReplayLoop = bLoop;
R->ApproachSpeed = ApproachSpeed;
R->StartReplay();
}
}
void URobotControlWidget::NativeTick(const FGeometry& MyGeometry, float InDeltaTime)
{
Super::NativeTick(MyGeometry, InDeltaTime);
// Poll the robot state ~5x/sec (cheap, no delegates needed).
RefreshAccum += InDeltaTime;
if (RefreshAccum >= 0.2f)
{
RefreshAccum = 0.0f;
RefreshSafetyUI();
RefreshControlUI();
RefreshRecordingsList();
RefreshReplayProgress();
}
}
void URobotControlWidget::HandleEStopClicked()
{
CmdEStop();
}
void URobotControlWidget::RefreshSafetyUI()
{
// --- Connection status text + color (priority order) ---
if (ConnectionStatusText)
{
FString StatusStr;
FLinearColor Color;
if (!IsRosBridgeConnected())
{
StatusStr = TEXT("DISCONNECTED");
Color = FLinearColor::Red;
}
else if (!IsBridgeNodeAlive())
{
StatusStr = TEXT("Bridge node: DOWN");
Color = FLinearColor::Red;
}
else if (!IsWorkerAlive())
{
StatusStr = TEXT("Worker: DOWN");
Color = FLinearColor(1.0f, 0.5f, 0.0f); // orange
}
else
{
const FString WS = GetWorkerState();
// WorkerState may still hold a stale connection-layer string
// ("disconnected", "bridge lost", "worker lost", "unknown") until
// the worker sends its first real /robot_status. Filter those out.
const bool bRealState =
(WS == TEXT("idle") || WS == TEXT("syncing") ||
WS == TEXT("recording") || WS == TEXT("replaying"));
StatusStr = bRealState
? FString::Printf(TEXT("Connected | %s"), *WS)
: TEXT("Connected | (awaiting status)");
Color = FLinearColor::Green;
}
ConnectionStatusText->SetText(FText::FromString(StatusStr));
ConnectionStatusText->SetColorAndOpacity(FSlateColor(Color));
}
// --- Device error panel (hidden when no errors) ---
if (DeviceErrorText)
{
TArray<FString> Errors;
if (HasFollowerError()) { Errors.Add(TEXT("Follower: USB/Serial ERROR")); }
if (HasLeaderError()) { Errors.Add(TEXT("Leader: USB/Serial ERROR")); }
if (Errors.Num() > 0)
{
DeviceErrorText->SetText(FText::FromString(FString::Join(Errors, TEXT("\n"))));
DeviceErrorText->SetColorAndOpacity(FSlateColor(FLinearColor::Red));
DeviceErrorText->SetVisibility(ESlateVisibility::HitTestInvisible);
}
else
{
DeviceErrorText->SetVisibility(ESlateVisibility::Collapsed);
}
}
}
void URobotControlWidget::HandleSyncOnClicked() { CmdSyncOn(); }
void URobotControlWidget::HandleSyncOffClicked() { CmdSyncOff(); }
void URobotControlWidget::HandleStartRecordClicked() { CmdStartRecord(); }
void URobotControlWidget::HandleStopRecordClicked() { CmdStopRecord(); }
void URobotControlWidget::HandleStopReplayClicked() { CmdStopReplay(); }
void URobotControlWidget::HandleStartReplayClicked()
{
FString Filename;
if (RecordingComboBox && !RecordingComboBox->GetSelectedOption().IsEmpty())
Filename = RecordingComboBox->GetSelectedOption();
else if (ReplayFilenameTextBox)
Filename = ReplayFilenameTextBox->GetText().ToString();
const bool bLoop = LoopCheckBox ? LoopCheckBox->IsChecked() : false;
const float Speed = ApproachSpeedSpinBox ? ApproachSpeedSpinBox->GetValue() : 45.0f;
CmdStartReplay(Filename, bLoop, Speed);
}
void URobotControlWidget::RefreshControlUI()
{
const FString WS = GetWorkerState();
const bool bIdle = (WS == TEXT("idle"));
const bool bSyncing = (WS == TEXT("syncing"));
const bool bRecording = (WS == TEXT("recording"));
const bool bReplaying = (WS == TEXT("replaying"));
if (WorkerStateText)
{
FString Label;
FLinearColor Color;
if (bRecording) { Label = TEXT("RECORDING"); Color = FLinearColor::Red; }
else if (bReplaying) { Label = TEXT("REPLAYING"); Color = FLinearColor(0.0f, 1.0f, 1.0f); }
else if (bSyncing) { Label = TEXT("SYNCING"); Color = FLinearColor::Green; }
else if (bIdle) { Label = TEXT("IDLE"); Color = FLinearColor::Gray; }
else { Label = TEXT("--"); Color = FLinearColor::Gray; }
WorkerStateText->SetText(FText::FromString(Label));
WorkerStateText->SetColorAndOpacity(FSlateColor(Color));
}
// Workflow guards — only enable actions that make sense in the current state.
const bool bAlive = IsWorkerAlive();
if (SyncOnButton) SyncOnButton->SetIsEnabled(bAlive && !bRecording && !bReplaying);
if (SyncOffButton) SyncOffButton->SetIsEnabled(bAlive && IsSyncActive() && !bRecording);
if (StartRecordButton) StartRecordButton->SetIsEnabled(bAlive && IsSyncActive() && !bRecording && !bReplaying);
if (StopRecordButton) StopRecordButton->SetIsEnabled(bAlive && bRecording);
if (StartReplayButton) StartReplayButton->SetIsEnabled(bAlive && !bRecording && !bReplaying);
if (StopReplayButton) StopReplayButton->SetIsEnabled(bAlive && bReplaying);
}
void URobotControlWidget::HandleRefreshRecordingsClicked()
{
if (ARobotVisualizer* R = ResolveRobot())
{
R->PublishRobotCommand(TEXT("{\"cmd\":\"list_recordings\"}"));
}
}
void URobotControlWidget::HandleRecordingSelected(FString SelectedItem, ESelectInfo::Type SelectionType)
{
if (ReplayFilenameTextBox && !SelectedItem.IsEmpty())
{
ReplayFilenameTextBox->SetText(FText::FromString(SelectedItem));
}
}
void URobotControlWidget::RefreshRecordingsList()
{
if (!RecordingComboBox) return;
ARobotVisualizer* R = Robot.Get();
if (!R) return;
// Auto-request the list once when the worker first comes alive (self-heals
// the "sometimes empty" case without needing a manual Refresh click).
if (!bInitialListRequested && IsWorkerAlive())
{
bInitialListRequested = true;
R->PublishRobotCommand(TEXT("{\"cmd\":\"list_recordings\"}"));
}
if (!IsWorkerAlive())
{
bInitialListRequested = false; // allow re-request after a reconnect
}
// Only rebuild when the actor's recordings actually changed.
if (R->GetRecordingsVersion() == CachedRecordingsVersion) return;
CachedRecordingsVersion = R->GetRecordingsVersion();
const FString Prev = RecordingComboBox->GetSelectedOption();
RecordingComboBox->ClearOptions();
for (const FRecordingInfo& Info : R->GetRecordings())
{
RecordingComboBox->AddOption(Info.Filename);
}
// Keep the old selection if still present, otherwise pick the most recent.
if (!Prev.IsEmpty() && RecordingComboBox->FindOptionIndex(Prev) != INDEX_NONE)
{
RecordingComboBox->SetSelectedOption(Prev);
}
else if (RecordingComboBox->GetOptionCount() > 0)
{
RecordingComboBox->SetSelectedIndex(RecordingComboBox->GetOptionCount() - 1);
}
}
void URobotControlWidget::RefreshReplayProgress()
{
const bool bReplaying = (GetWorkerState() == TEXT("replaying"));
ARobotVisualizer* R = Robot.Get();
if (ReplayProgressBar)
{
float Pct = 0.0f;
if (bReplaying && R && R->GetReplayTotal() > 0)
{
Pct = static_cast<float>(R->GetReplayIndex()) / static_cast<float>(R->GetReplayTotal());
}
ReplayProgressBar->SetPercent(Pct);
}
if (ReplayProgressText)
{
if (bReplaying && R)
{
const FString T = R->IsReplayApproaching()
? TEXT("approaching...")
: FString::Printf(TEXT("%d / %d"), R->GetReplayIndex(), R->GetReplayTotal());
ReplayProgressText->SetText(FText::FromString(T));
}
else
{
ReplayProgressText->SetText(FText::GetEmpty());
}
}
}
// --- State getters ---
FString URobotControlWidget::GetWorkerState() const
{
return Robot.IsValid() ? Robot->GetWorkerState() : TEXT("no robot");
}
bool URobotControlWidget::IsSyncActive() const { return Robot.IsValid() && Robot->IsSyncActive(); }
bool URobotControlWidget::IsRosBridgeConnected() const { return Robot.IsValid() && Robot->IsRosBridgeConnected(); }
bool URobotControlWidget::IsBridgeNodeAlive() const { return Robot.IsValid() && Robot->IsBridgeNodeAlive(); }
bool URobotControlWidget::IsWorkerAlive() const { return Robot.IsValid() && Robot->IsWorkerAlive(); }
bool URobotControlWidget::HasFollowerError() const { return Robot.IsValid() && Robot->HasFollowerError(); }
bool URobotControlWidget::HasLeaderError() const { return Robot.IsValid() && Robot->HasLeaderError(); }
bool URobotControlWidget::HasRobot() const { return Robot.IsValid(); }
여기까지 했으면 빌드한다.
Step5: 언리얼 에디터에서 행 위젯 WBP 생성
행 위젯 WBP 만들기
[Content Browser]에서 마우스 우클릭, [User Interface] - [Widget Bluprint] 생성, 이름은 "WBP_RecordingEntry"로 지었다.
더블 클릭하여 들어가서 Graph 탭으로 진입한다.
[Class Settings] - [Parent Class]를 "RecordingEntryWidget"으로 설정한다.
Designer로 돌아가서 Horizontal Box 하나 놓고 "FilenameText", "MetaText" 이름의 Text 두 개를 배치한다.
메인 UI인 WBP_RobotControl에서, [List View] 하나 추가, 이름은 "RecordingListView"
Details 창에서 Entry Widget Class 항목을 찾아 "WBP_RecordingEntry"로 지정.
버튼 하나 추가해서 "RefreshRecordingsButton"으로 지정.
Progress Bar 추가해서 "ReplayProgressBar" 이름 지정.
*그런데 드롭다운에 녹화된 리스트가 나오지 않아서 원인을 분석했다.
ComboBox 드롭다운 리스트가 많아지면 리스트를 가져올 수 없는 것으로 확인. 결국 녹화된 총 용량 문제이며 bridge에서는 전체 항목을 한 응답에 다 담지 않고, 항목 하나씩 발행. Unreal에서는 지금은 recordings 배열을 통째로 파싱하는데 이걸 recording_item을 하나씩 받아 임시 버퍼에 쌓다가 다 모이면 Recordings에 반영하도록 변경한다.
ros_bridge_node.py 수정
- on_robot_command의 try: 블록 수정
# Publish response on /robot_status.
# list_recordings can be large; split into one small message per
# recording so each stays under the WebSocket frame limit.
if cmd_name == "list_recordings" and "recordings" in reply:
recs = reply.get("recordings", [])
total = len(recs)
if total == 0:
clear_msg = String()
clear_msg.data = json.dumps({"recordings_clear": True})
self.robot_status_pub.publish(clear_msg)
else:
for i, rec in enumerate(recs):
item_msg = String()
item_msg.data = json.dumps({
"recording_item": rec,
"index": i,
"total": total,
})
self.robot_status_pub.publish(item_msg)
self.get_logger().info(f"Published {total} recording item(s)")
else:
status_msg = String()
status_msg.data = reply_raw
self.robot_status_pub.publish(status_msg)
ros_bridge_node.py 전체 코드
더보기
#!/usr/bin/env python3
"""
ros_bridge_node.py — ZMQ ↔ ROS2 bridge node
Runs in .venv-ros-bridge (Python 3.10) with rclpy + pyzmq.
Subscribes to joint states from lerobot_worker via ZMQ,
converts degrees→radians, and publishes sensor_msgs/JointState.
Also subscribes to /follower_joint_commands and forwards to worker via ZMQ REQ.
Relays record/replay/estop commands:
- /robot_command (std_msgs/String) → ZMQ REQ → worker
- Worker state published on /robot_status (std_msgs/String) every tick
Usage:
export PATH=$(echo $PATH | tr ':' '\\n' | grep -v miniforge | tr '\\n' ':' | sed 's/:$//')
cd ~/UnrealRobotics
source .venv-ros-bridge/bin/activate
source /opt/ros/humble/setup.bash
python src/lerobot_ros2_bridge/lerobot_ros2_bridge/ros_bridge_node.py \\
[--sub-addr tcp://127.0.0.1:5555] \\
[--req-addr tcp://127.0.0.1:5556]
"""
import argparse
import json
import math
import sys
import traceback
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
from std_msgs.msg import String
import zmq
# ---------------------------------------------------------------------------
# Joint names — must match URDF, LeRobot calibration, and worker output
# (Phase 3.2 verified: all 6 names identical across all three sources)
# ---------------------------------------------------------------------------
JOINT_NAMES = [
"shoulder_pan",
"shoulder_lift",
"elbow_flex",
"wrist_flex",
"wrist_roll",
"gripper",
]
def deg2rad(deg: float) -> float:
return deg * math.pi / 180.0
def rad2deg(rad: float) -> float:
return rad * 180.0 / math.pi
class RosBridgeNode(Node):
def __init__(self, args):
super().__init__("lerobot_ros2_bridge")
self.get_logger().info("Initializing lerobot_ros2_bridge node")
# --- ZMQ setup ---
self.zmq_ctx = zmq.Context()
# SUB: receive joint states from worker
self.zmq_sub = self.zmq_ctx.socket(zmq.SUB)
self.zmq_sub.connect(args.sub_addr)
self.zmq_sub.setsockopt_string(zmq.SUBSCRIBE, "")
# Don't block waiting for messages — use RCVTIMEO
self.zmq_sub.setsockopt(zmq.RCVTIMEO, 0)
self.get_logger().info(f"ZMQ SUB connected to {args.sub_addr}")
# REQ: send commands to worker
self.req_addr = args.req_addr
self.zmq_req = self._create_req_socket()
self.req_consecutive_timeouts = 0
self.req_max_timeouts = 3 # recreate socket after this many consecutive timeouts
# --- ROS2 publishers ---
# Follower joint states (for robot_state_publisher + rosbridge → Unreal)
self.follower_pub = self.create_publisher(JointState, "/joint_states", 10)
# Leader joint states (separate topic)
self.leader_pub = self.create_publisher(JointState, "/leader_joint_states", 10)
# --- ROS2 subscriber for commands from Unreal (via rosbridge) ---
self.cmd_sub = self.create_subscription(
JointState,
"/follower_joint_commands",
self.on_joint_command,
10,
)
# --- Record/Replay/E-Stop command relay ---
# Unreal publishes JSON command strings to /robot_command,
# bridge relays them to worker via ZMQ REQ.
self.robot_cmd_sub = self.create_subscription(
String,
"/robot_command",
self.on_robot_command,
10,
)
# Worker state feedback → Unreal
self.robot_status_pub = self.create_publisher(String, "/robot_status", 10)
self.last_worker_state = ""
self.last_worker_teleop = None
self.last_device_errors = {}
self.last_progress_pub_time = self.get_clock().now()
self.progress_pub_interval_ns = 200_000_000 # 5 Hz throttle for replay progress
# Bridge heartbeat → Unreal (so Unreal can distinguish bridge vs worker death)
self.bridge_heartbeat_pub = self.create_publisher(String, "/bridge_heartbeat", 10)
# --- Timer: poll ZMQ at ~100Hz (faster than worker's 30Hz to avoid buffering) ---
self.poll_timer = self.create_timer(0.01, self.poll_zmq)
# --- Timer: heartbeat ping to worker every 2s ---
self.heartbeat_timer = self.create_timer(2.0, self.send_heartbeat)
# --- Timer: bridge heartbeat to Unreal every 1s ---
self.bridge_heartbeat_timer = self.create_timer(1.0, self.publish_bridge_heartbeat)
# Stats
self.recv_count = 0
self.last_log_time = self.get_clock().now()
def poll_zmq(self):
"""Non-blocking poll of ZMQ SUB for worker messages."""
# Drain all available messages (in case we're slightly behind)
messages_this_tick = 0
while messages_this_tick < 5: # cap per tick to avoid starving ROS callbacks
try:
raw = self.zmq_sub.recv_string(zmq.NOBLOCK)
except zmq.Again:
break
messages_this_tick += 1
self.recv_count += 1
try:
msg = json.loads(raw)
except json.JSONDecodeError as e:
self.get_logger().warn(f"JSON parse error: {e}")
continue
now = self.get_clock().now().to_msg()
# Publish follower joint states
if "follower" in msg:
js = JointState()
js.header.stamp = now
js.header.frame_id = ""
js.name = list(JOINT_NAMES)
js.position = [
deg2rad(msg["follower"].get(name, 0.0))
for name in JOINT_NAMES
]
# velocity and effort left empty (not available from LeRobot API)
self.follower_pub.publish(js)
# Publish leader joint states
if "leader" in msg:
js = JointState()
js.header.stamp = now
js.header.frame_id = ""
js.name = list(JOINT_NAMES)
js.position = [
deg2rad(msg["leader"].get(name, 0.0))
for name in JOINT_NAMES
]
self.leader_pub.publish(js)
# Publish worker state to /robot_status (only on change)
worker_state = msg.get("state", "idle")
worker_teleop = msg.get("teleop", False)
device_errors = msg.get("device_errors", {})
state_changed = (worker_state != self.last_worker_state or
worker_teleop != self.last_worker_teleop)
# Also publish on device error changes
if device_errors != self.last_device_errors:
state_changed = True
self.last_device_errors = dict(device_errors)
if device_errors:
self.get_logger().warn(f"Device errors: {device_errors}")
if state_changed:
status_msg = String()
status_data = {"state": worker_state, "teleop": worker_teleop}
# Include recording/replay metadata if present
if "recording_frames" in msg:
status_data["recording_frames"] = msg["recording_frames"]
if "replay_progress" in msg:
status_data["replay_progress"] = msg["replay_progress"]
if device_errors:
status_data["device_errors"] = device_errors
status_msg.data = json.dumps(status_data)
self.robot_status_pub.publish(status_msg)
self.last_worker_state = worker_state
self.last_worker_teleop = worker_teleop
self.get_logger().info(
f"Worker state: {worker_state}, teleop: {worker_teleop}")
# Continuously publish replay progress (throttled to 5 Hz) so the
# Unreal progress bar updates during replay. state_changed is False
# while replaying (state stays "replaying"), so this is separate.
if worker_state == "replaying" and "replay_progress" in msg:
now_t = self.get_clock().now()
if (now_t - self.last_progress_pub_time).nanoseconds > self.progress_pub_interval_ns:
self.last_progress_pub_time = now_t
prog_msg = String()
prog_msg.data = json.dumps({
"state": worker_state,
"teleop": worker_teleop,
"replay_progress": msg["replay_progress"],
})
self.robot_status_pub.publish(prog_msg)
# Periodic logging (every 10 seconds)
now_time = self.get_clock().now()
if (now_time - self.last_log_time).nanoseconds > 10_000_000_000:
self.get_logger().info(
f"ZMQ recv total: {self.recv_count} messages"
)
self.last_log_time = now_time
def on_joint_command(self, msg: JointState):
"""
Receive joint commands from ROS2 (e.g. from Unreal via rosbridge),
convert radians→degrees, and forward to worker via ZMQ REQ.
"""
if len(msg.name) == 0 or len(msg.position) == 0:
self.get_logger().warn("Empty joint command received, ignoring")
return
# Build command dict (degrees)
joint_args = {}
for name, pos_rad in zip(msg.name, msg.position):
if name in JOINT_NAMES:
joint_args[name] = rad2deg(pos_rad)
if not joint_args:
self.get_logger().warn("No recognized joint names in command")
return
cmd = {
"cmd": "send_follower_action",
"args": joint_args,
}
try:
self.zmq_req.send_string(json.dumps(cmd))
reply_raw = self.zmq_req.recv_string()
reply = json.loads(reply_raw)
if reply.get("status") != "ok":
self.get_logger().warn(
f"Worker command error: {reply.get('reason', 'unknown')}"
)
except zmq.Again:
self.get_logger().error("Worker REQ timeout — is lerobot_worker running?")
except Exception as e:
self.get_logger().error(f"Worker command exception: {e}")
def on_robot_command(self, msg: String):
"""
Receive record/replay/estop commands from Unreal (via rosbridge),
relay to worker via ZMQ REQ, and publish response on /robot_status.
Expected msg.data formats:
'{"cmd": "start_record"}'
'{"cmd": "stop_record"}'
'{"cmd": "start_replay", "args": {"filename": "...", "loop": false}}'
'{"cmd": "start_replay"}' (plays most recent)
'{"cmd": "stop_replay"}'
'{"cmd": "estop"}'
'{"cmd": "list_recordings"}'
"""
try:
cmd = json.loads(msg.data)
except json.JSONDecodeError:
# Support simple string commands: "estop", "start_record", etc.
cmd = {"cmd": msg.data.strip()}
cmd_name = cmd.get("cmd", "")
self.get_logger().info(f"Robot command received: {cmd_name}")
try:
self.zmq_req.send_string(json.dumps(cmd))
reply_raw = self.zmq_req.recv_string()
reply = json.loads(reply_raw)
# Publish response on /robot_status.
# list_recordings can be large; split into one small message per
# recording so each stays under the WebSocket frame limit.
if cmd_name == "list_recordings" and "recordings" in reply:
recs = reply.get("recordings", [])
total = len(recs)
if total == 0:
clear_msg = String()
clear_msg.data = json.dumps({"recordings_clear": True})
self.robot_status_pub.publish(clear_msg)
else:
for i, rec in enumerate(recs):
item_msg = String()
item_msg.data = json.dumps({
"recording_item": rec,
"index": i,
"total": total,
})
self.robot_status_pub.publish(item_msg)
self.get_logger().info(f"Published {total} recording item(s)")
else:
status_msg = String()
status_msg.data = reply_raw
self.robot_status_pub.publish(status_msg)
status = reply.get("status", "unknown")
if status == "ok":
new_state = reply.get("state", "")
if new_state:
self.last_worker_state = new_state
self.get_logger().info(f"Command '{cmd_name}' OK: {reply}")
else:
self.get_logger().warn(
f"Command '{cmd_name}' error: {reply.get('reason', 'unknown')}"
)
except zmq.Again:
self.get_logger().error(
f"Worker REQ timeout for '{cmd_name}' — is lerobot_worker running?"
)
# Publish timeout error on /robot_status
err_msg = String()
err_msg.data = json.dumps({
"status": "error", "reason": "worker timeout", "cmd": cmd_name
})
self.robot_status_pub.publish(err_msg)
except Exception as e:
self.get_logger().error(f"Robot command exception: {e}")
err_msg = String()
err_msg.data = json.dumps({
"status": "error", "reason": str(e), "cmd": cmd_name
})
self.robot_status_pub.publish(err_msg)
def _create_req_socket(self):
"""Create a fresh ZMQ REQ socket connected to the worker."""
sock = self.zmq_ctx.socket(zmq.REQ)
sock.setsockopt(zmq.RCVTIMEO, 1000) # 1s timeout for replies
sock.setsockopt(zmq.LINGER, 0) # don't block on close
sock.connect(self.req_addr)
self.get_logger().info(f"ZMQ REQ connected to {self.req_addr}")
return sock
def _reset_req_socket(self):
"""Close and recreate the REQ socket to recover from stuck state."""
self.get_logger().warn("Resetting ZMQ REQ socket (worker may have restarted)")
try:
self.zmq_req.close()
except Exception:
pass
self.zmq_req = self._create_req_socket()
self.req_consecutive_timeouts = 0
def publish_bridge_heartbeat(self):
"""Publish a lightweight heartbeat so Unreal knows the bridge is alive."""
msg = String()
msg.data = '{"bridge":"alive"}'
self.bridge_heartbeat_pub.publish(msg)
def send_heartbeat(self):
"""Send periodic ping to worker so it knows the bridge is alive."""
try:
self.zmq_req.send_string('{"cmd":"ping"}')
self.zmq_req.recv_string() # discard reply, just keeping the link alive
self.req_consecutive_timeouts = 0 # reset on success
except zmq.Again:
self.req_consecutive_timeouts += 1
self.get_logger().warn(
f"Heartbeat ping timeout ({self.req_consecutive_timeouts}/"
f"{self.req_max_timeouts})")
if self.req_consecutive_timeouts >= self.req_max_timeouts:
self._reset_req_socket()
except Exception as e:
self.get_logger().warn(f"Heartbeat ping error: {e}")
self.req_consecutive_timeouts += 1
if self.req_consecutive_timeouts >= self.req_max_timeouts:
self._reset_req_socket()
def destroy_node(self):
"""Clean shutdown of ZMQ resources."""
self.get_logger().info("Shutting down ZMQ...")
self.zmq_sub.close()
self.zmq_req.close()
self.zmq_ctx.term()
super().destroy_node()
def main():
parser = argparse.ArgumentParser(description="ROS2 ↔ ZMQ bridge node")
parser.add_argument("--sub-addr", default="tcp://127.0.0.1:5555",
help="ZMQ SUB address (worker PUB)")
parser.add_argument("--req-addr", default="tcp://127.0.0.1:5556",
help="ZMQ REQ address (worker REP)")
# ROS2 may pass extra args — use parse_known_args
args, unknown = parser.parse_known_args()
rclpy.init(args=unknown if unknown else None)
node = RosBridgeNode(args)
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()
- RobotVisualizer.h 수정
TArray<FRecordingInfo> Recordings; 아래에 추가
TArray<FRecordingInfo> PendingRecordings;
RobotVisualizer.h 전체 코드
더보기
#pragma once
#include "CoreMinimal.h"
#include "GameFramework/Actor.h"
#include "Templates/SubclassOf.h"
#include "RobotVisualizer.generated.h"
class UStaticMesh;
class UStaticMeshComponent;
class USceneComponent;
class URosBridgeSubsystem;
class URobotControlWidget;
/**
* Visualizes the SO-ARM-101 follower arm in Unreal Engine and provides
* MoveIt command interface via rosbridge.
*
* The component hierarchy mirrors the URDF link/joint structure:
* BaseLink -> ShoulderPanJoint -> ShoulderLink -> ShoulderLiftJoint -> ...
*
* Each "Joint" SceneComponent is where the ROS joint angle gets applied
* as a local Z-axis rotation. All child links/meshes rotate with it.
*
* Phase 8 additions:
* - SendNamedTarget(): publish to /moveit_goal_named (std_msgs/String)
* - SendJointGoal(): publish to /moveit_goal_joints (sensor_msgs/JointState)
* - SendPoseGoal(): publish to /moveit_goal_pose (geometry_msgs/PoseStamped)
* - Blueprint-callable + editor-testable via UPROPERTY buttons
*
* Phase 9 additions (Record/Replay/E-Stop):
* - StartRecord(): begin teleop recording on worker
* - StopRecord(): stop recording, save trajectory
* - StartReplay(): replay most recent (or named) recording
* - StopReplay(): stop replay
* - EStop(): emergency stop all motion
* - All commands publish JSON to /robot_command topic
* - Worker state feedback via /robot_status subscription
*/
USTRUCT(BlueprintType)
struct FRecordingInfo
{
GENERATED_BODY()
UPROPERTY(BlueprintReadOnly, Category = "ROS|UI") FString Filename;
UPROPERTY(BlueprintReadOnly, Category = "ROS|UI") int32 Frames = 0;
UPROPERTY(BlueprintReadOnly, Category = "ROS|UI") float DurationSec = 0.0f;
UPROPERTY(BlueprintReadOnly, Category = "ROS|UI") FString RecordedAt;
};
UCLASS()
class SO101_TWIN_API ARobotVisualizer : public AActor
{
GENERATED_BODY()
public:
ARobotVisualizer();
// =================================================================
// Phase 10 — Widget-facing API
// =================================================================
/** Widget Blueprint to spawn into the viewport on BeginPlay.
* Set this to WBP_RobotControl in this actor's Details panel. */
UPROPERTY(EditAnywhere, BlueprintReadOnly, Category = "ROS|UI")
TSubclassOf<URobotControlWidget> ControlWidgetClass;
UFUNCTION(BlueprintPure, Category = "ROS|UI")
FString GetWorkerState() const { return WorkerState; }
UFUNCTION(BlueprintPure, Category = "ROS|UI")
bool IsSyncActive() const { return bSyncActive; }
UFUNCTION(BlueprintPure, Category = "ROS|UI")
bool IsRosBridgeConnected() const { return bRosBridgeConnected; }
UFUNCTION(BlueprintPure, Category = "ROS|UI")
bool IsBridgeNodeAlive() const { return bRosBridgeConnected && !bBridgeHeartbeatLost; }
UFUNCTION(BlueprintPure, Category = "ROS|UI")
bool IsWorkerAlive() const { return bRosBridgeConnected && !bBridgeHeartbeatLost && !bWorkerDataLost; }
UFUNCTION(BlueprintPure, Category = "ROS|UI")
bool HasFollowerError() const { return bFollowerDeviceError; }
UFUNCTION(BlueprintPure, Category = "ROS|UI")
bool HasLeaderError() const { return bLeaderDeviceError; }
/** The control widget may read protected state and call commands directly. */
friend class URobotControlWidget;
const TArray<FRecordingInfo>& GetRecordings() const { return Recordings; }
int32 GetRecordingsVersion() const { return RecordingsVersion; }
int32 GetReplayIndex() const { return ReplayIndex; }
int32 GetReplayTotal() const { return ReplayTotal; }
bool IsReplayApproaching() const { return bReplayApproaching; }
protected:
virtual void BeginPlay() override;
virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override;
// =================================================================
// Configuration
// =================================================================
UPROPERTY(EditAnywhere, Category = "ROS|Bridge")
FString RosBridgeUrl = TEXT("ws://127.0.0.1:9090/?x=1");
UPROPERTY(EditAnywhere, Category = "ROS|Topics")
FString JointStateTopic = TEXT("/joint_states");
UPROPERTY(EditAnywhere, Category = "ROS|Topics")
FString JointStateType = TEXT("sensor_msgs/JointState");
// =================================================================
// MoveIt Command Interface (Phase 8)
// =================================================================
// --- Named target ---
/** Named target to send (e.g. "home", "ready"). Set in Details panel, then call SendNamedTarget(). */
UPROPERTY(EditAnywhere, Category = "ROS|MoveIt")
FString MoveItNamedTarget = TEXT("home");
/** Send the named target to MoveIt via /moveit_goal_named topic. */
UFUNCTION(BlueprintCallable, CallInEditor, Category = "ROS|MoveIt")
void SendNamedTarget();
// --- Joint goal ---
/** Joint goal values in radians. Set in Details panel, then call SendJointGoal(). */
UPROPERTY(EditAnywhere, Category = "ROS|MoveIt|Joints")
float GoalShoulderPan = 0.0f;
UPROPERTY(EditAnywhere, Category = "ROS|MoveIt|Joints")
float GoalShoulderLift = 0.0f;
UPROPERTY(EditAnywhere, Category = "ROS|MoveIt|Joints")
float GoalElbowFlex = 0.0f;
UPROPERTY(EditAnywhere, Category = "ROS|MoveIt|Joints")
float GoalWristFlex = 0.0f;
UPROPERTY(EditAnywhere, Category = "ROS|MoveIt|Joints")
float GoalWristRoll = 0.0f;
/** Send joint goal to MoveIt via /moveit_goal_joints topic. Values are in radians. */
UFUNCTION(BlueprintCallable, CallInEditor, Category = "ROS|MoveIt|Joints")
void SendJointGoal();
// --- Pose goal (Cartesian, position-only for 5DOF) ---
/** Target position in UE coordinates (cm). Converted to ROS (meters) on send. */
UPROPERTY(EditAnywhere, Category = "ROS|MoveIt|Pose")
FVector GoalPositionUE = FVector(10.0f, 0.0f, 15.0f);
/** Send position-only goal to MoveIt via /moveit_goal_pose topic.
* GoalPositionUE is in Unreal cm, auto-converted to ROS meters with Y flip. */
UFUNCTION(BlueprintCallable, CallInEditor, Category = "ROS|MoveIt|Pose")
void SendPoseGoal();
// =================================================================
// Teleop Sync (Phase 9)
// =================================================================
/** Activate leader→follower sync (teleop). Must be ON before recording. */
UFUNCTION(BlueprintCallable, CallInEditor, Category = "ROS|Sync")
void SyncOn();
/** Deactivate leader→follower sync. */
UFUNCTION(BlueprintCallable, CallInEditor, Category = "ROS|Sync")
void SyncOff();
/** Whether teleop (sync) is currently active. Updated from /robot_status. */
UPROPERTY(VisibleAnywhere, Category = "ROS|Status")
bool bSyncActive = false;
// =================================================================
// Record / Replay / E-Stop (Phase 9)
// =================================================================
/** Start recording: activates teleop on worker, buffers joint trajectory. */
UFUNCTION(BlueprintCallable, CallInEditor, Category = "ROS|Record")
void StartRecord();
/** Stop recording: saves trajectory to file on worker. */
UFUNCTION(BlueprintCallable, CallInEditor, Category = "ROS|Record")
void StopRecord();
/** Replay filename (empty = most recent recording). */
UPROPERTY(EditAnywhere, Category = "ROS|Replay")
FString ReplayFilename;
/** Whether to loop the replay continuously. */
UPROPERTY(EditAnywhere, Category = "ROS|Replay")
bool bReplayLoop = false;
/** Approach speed in degrees/sec. Controls how fast the robot moves
* to the start position before replay begins. Lower = smoother. */
UPROPERTY(EditAnywhere, Category = "ROS|Replay", meta = (ClampMin = "5.0", ClampMax = "300.0"))
float ApproachSpeed = 45.0f;
/** Start replaying a recorded trajectory on the follower arm. */
UFUNCTION(BlueprintCallable, CallInEditor, Category = "ROS|Replay")
void StartReplay();
/** Stop replay immediately. */
UFUNCTION(BlueprintCallable, CallInEditor, Category = "ROS|Replay")
void StopReplay();
/** Emergency stop: abort ALL motion immediately (recording, replay, teleop). */
UFUNCTION(BlueprintCallable, CallInEditor, Category = "ROS|Safety")
void EStop();
/** Current worker state (idle/recording/replaying). Updated from /robot_status. */
UPROPERTY(VisibleAnywhere, Category = "ROS|Status")
FString WorkerState = TEXT("unknown");
private:
// =================================================================
// Component hierarchy
// =================================================================
UPROPERTY(VisibleAnywhere, Category = "Robot")
TObjectPtr<USceneComponent> RobotRoot;
// Link SceneComponents
UPROPERTY(VisibleAnywhere, Category = "Robot|Links")
TObjectPtr<USceneComponent> BaseLink;
UPROPERTY(VisibleAnywhere, Category = "Robot|Links")
TObjectPtr<USceneComponent> ShoulderLink;
UPROPERTY(VisibleAnywhere, Category = "Robot|Links")
TObjectPtr<USceneComponent> UpperArmLink;
UPROPERTY(VisibleAnywhere, Category = "Robot|Links")
TObjectPtr<USceneComponent> LowerArmLink;
UPROPERTY(VisibleAnywhere, Category = "Robot|Links")
TObjectPtr<USceneComponent> WristLink;
UPROPERTY(VisibleAnywhere, Category = "Robot|Links")
TObjectPtr<USceneComponent> GripperLink;
UPROPERTY(VisibleAnywhere, Category = "Robot|Links")
TObjectPtr<USceneComponent> MovingJawLink;
// Joint SceneComponents
UPROPERTY(VisibleAnywhere, Category = "Robot|Joints")
TObjectPtr<USceneComponent> ShoulderPanJoint;
UPROPERTY(VisibleAnywhere, Category = "Robot|Joints")
TObjectPtr<USceneComponent> ShoulderLiftJoint;
UPROPERTY(VisibleAnywhere, Category = "Robot|Joints")
TObjectPtr<USceneComponent> ElbowFlexJoint;
UPROPERTY(VisibleAnywhere, Category = "Robot|Joints")
TObjectPtr<USceneComponent> WristFlexJoint;
UPROPERTY(VisibleAnywhere, Category = "Robot|Joints")
TObjectPtr<USceneComponent> WristRollJoint;
UPROPERTY(VisibleAnywhere, Category = "Robot|Joints")
TObjectPtr<USceneComponent> GripperJoint;
// Joint name -> component mapping
UPROPERTY()
TMap<FName, TObjectPtr<USceneComponent>> JointComponentMap;
// Mesh components
UPROPERTY()
TArray<TObjectPtr<UStaticMeshComponent>> AllMeshComponents;
// =================================================================
// ROS connection
// =================================================================
UFUNCTION()
void OnRosBridgeConnected();
UFUNCTION()
void OnRosBridgeDisconnected();
UFUNCTION()
void OnRosMessage(const FString& Topic, const FString& MessageJson);
/** Tracks rosbridge connection state for viewport warnings. */
bool bRosBridgeConnected = false;
void ParseAndApplyJointStates(const FString& MessageJson);
// =================================================================
// MoveIt publish helpers
// =================================================================
/** Advertise MoveIt command topics. Called once on connect. */
void AdvertiseMoveItTopics();
/** Whether MoveIt topics have been advertised in this connection session. */
bool bMoveItTopicsAdvertised = false;
// =================================================================
// Record / Replay / E-Stop helpers
// =================================================================
/** Advertise /robot_command and subscribe /robot_status. Called once on connect. */
void SetupRecordReplayTopics();
/** Whether record/replay topics have been set up. */
bool bRecordReplayTopicsSetup = false;
/** Send a JSON command to /robot_command topic. */
void PublishRobotCommand(const FString& JsonCmd);
/** Handle /robot_status messages from the bridge node. */
UFUNCTION()
void OnRobotStatus(const FString& Topic, const FString& MessageJson);
// =================================================================
// Connection health monitoring (Unreal-side)
// =================================================================
/** Called periodically to check bridge and worker heartbeats. */
void CheckConnectionHealth();
/** Timer handle for health check. */
FTimerHandle ConnectionHealthTimerHandle;
/** Last time we received /bridge_heartbeat. */
double LastBridgeHeartbeatTime = 0.0;
/** Last time we received /joint_states (worker data via bridge). */
double LastJointStatesTime = 0.0;
/** Timeout in seconds for bridge heartbeat (bridge publishes every 1s). */
float BridgeHeartbeatTimeoutSec = 4.0f;
/** Timeout in seconds for worker data (/joint_states at 30Hz). */
float WorkerDataTimeoutSec = 3.0f;
/** Whether bridge heartbeat has been lost. */
bool bBridgeHeartbeatLost = false;
/** Whether worker data has been lost. */
bool bWorkerDataLost = false;
/** Tracks device-level USB/serial error state for recovery messages. */
bool bFollowerDeviceError = false;
bool bLeaderDeviceError = false;
/** The viewport control UI widget instance (Phase 10). */
UPROPERTY(Transient)
TObjectPtr<URobotControlWidget> ControlWidget;
TArray<FRecordingInfo> Recordings;
TArray<FRecordingInfo> PendingRecordings;
int32 RecordingsVersion = 0;
int32 ReplayIndex = 0;
int32 ReplayTotal = 0;
FString ReplayProgFilename;
bool bReplayApproaching = false;
// =================================================================
// Helpers (declared in original header, kept for compatibility)
// =================================================================
USceneComponent* CreateJointComponent(const FName& Name, USceneComponent* Parent,
const FVector& Location, const FRotator& Rotation);
USceneComponent* CreateLinkComponent(const FName& Name, USceneComponent* Parent);
UStaticMeshComponent* AttachMesh(USceneComponent* Parent, UStaticMesh* Mesh,
const FName& Name, const FVector& Location, const FRotator& Rotation,
bool bIsMotor);
};
RobotVisualizer.cpp 수정
- 기존 recordings-array 블록이 끝나는 }(938번째 줄)과 // --- Replay progress ---(940번째 줄) 사이에 삽입
// --- Recordings list, per-item (split to avoid large-frame drops) ---
const TSharedPtr<FJsonObject>* RecItemObj = nullptr;
if (StatusJson->TryGetObjectField(TEXT("recording_item"), RecItemObj) && RecItemObj && RecItemObj->IsValid())
{
int32 RecIndex = 0, RecTotal = 0;
StatusJson->TryGetNumberField(TEXT("index"), RecIndex);
StatusJson->TryGetNumberField(TEXT("total"), RecTotal);
if (RecIndex == 0) { PendingRecordings.Reset(); }
FRecordingInfo Info;
(*RecItemObj)->TryGetStringField(TEXT("filename"), Info.Filename);
(*RecItemObj)->TryGetNumberField(TEXT("frames"), Info.Frames);
double Dur = 0.0;
(*RecItemObj)->TryGetNumberField(TEXT("duration_sec"), Dur);
Info.DurationSec = static_cast<float>(Dur);
(*RecItemObj)->TryGetStringField(TEXT("recorded_at"), Info.RecordedAt);
PendingRecordings.Add(Info);
if (RecTotal > 0 && PendingRecordings.Num() >= RecTotal)
{
Recordings = PendingRecordings;
PendingRecordings.Reset();
++RecordingsVersion;
UE_LOG(LogRosBridge, Log, TEXT("Recordings list updated: %d files"), Recordings.Num());
}
}
// --- Recordings list cleared (empty result) ---
bool bRecClear = false;
if (StatusJson->TryGetBoolField(TEXT("recordings_clear"), bRecClear) && bRecClear)
{
Recordings.Reset();
PendingRecordings.Reset();
++RecordingsVersion;
UE_LOG(LogRosBridge, Log, TEXT("Recordings list cleared"));
}
RobotVisualizer.cpp 전체 코드
더보기
#include "RobotVisualizer.h"
#include "RosCoordConv.h"
#include "RosBridgeSubsystem.h"
#include "RosBridgeLog.h"
#include "Components/SceneComponent.h"
#include "Components/StaticMeshComponent.h"
#include "Engine/StaticMesh.h"
#include "Engine/Engine.h"
#include "Engine/World.h"
#include "Kismet/GameplayStatics.h"
#include "Dom/JsonObject.h"
#include "Dom/JsonValue.h"
#include "Serialization/JsonReader.h"
#include "Serialization/JsonSerializer.h"
#include "Serialization/JsonWriter.h"
#include "UObject/ConstructorHelpers.h"
#include "GameFramework/PlayerController.h"
#include "Blueprint/UserWidget.h"
#include "RobotControlWidget.h"
// =============================================================================
// Mesh asset path helper
// =============================================================================
static UStaticMesh* LoadMeshAsset(const TCHAR* AssetName)
{
// All meshes live under /Game/Robot/Meshes/
FString Path = FString::Printf(TEXT("/Game/Robot/Meshes/%s.%s"), AssetName, AssetName);
UStaticMesh* Mesh = Cast<UStaticMesh>(StaticLoadObject(UStaticMesh::StaticClass(), nullptr, *Path));
if (!Mesh)
{
UE_LOG(LogRosBridge, Warning, TEXT("Failed to load mesh: %s"), *Path);
}
return Mesh;
}
// =============================================================================
// Constructor — build the entire component hierarchy
// =============================================================================
ARobotVisualizer::ARobotVisualizer()
{
PrimaryActorTick.bCanEverTick = false;
// --- Root ---
RobotRoot = CreateDefaultSubobject<USceneComponent>(TEXT("RobotRoot"));
RootComponent = RobotRoot;
// =========================================================================
// URDF data converted to UE coordinates:
// Position: meters * 100 = cm, Y flipped
// Rotation: RPY radians -> FRotator degrees, pitch & yaw negated
//
// All values below are pre-computed from so101_follower.urdf.
// =========================================================================
// --- base_link (attached directly to root, no joint) ---
BaseLink = CreateDefaultSubobject<USceneComponent>(TEXT("BaseLink"));
BaseLink->SetupAttachment(RobotRoot);
// --- shoulder_pan joint ---
// URDF origin: xyz(0.0388353, 0, 0.0624) rpy(3.14159, 0, -3.14159)
ShoulderPanJoint = CreateDefaultSubobject<USceneComponent>(TEXT("ShoulderPanJoint"));
ShoulderPanJoint->SetupAttachment(BaseLink);
ShoulderPanJoint->SetRelativeLocation(RosCoordConv::RosToUePosition(0.0388353, 0.0, 0.0624));
ShoulderPanJoint->SetRelativeRotation(RosCoordConv::RosRpyToUeRotator(3.14159, 0.0, -3.14159));
ShoulderLink = CreateDefaultSubobject<USceneComponent>(TEXT("ShoulderLink"));
ShoulderLink->SetupAttachment(ShoulderPanJoint);
// --- shoulder_lift joint ---
// URDF origin: xyz(-0.0303992, -0.0182778, -0.0542) rpy(-1.5708, -1.5708, 0)
ShoulderLiftJoint = CreateDefaultSubobject<USceneComponent>(TEXT("ShoulderLiftJoint"));
ShoulderLiftJoint->SetupAttachment(ShoulderLink);
ShoulderLiftJoint->SetRelativeLocation(RosCoordConv::RosToUePosition(-0.0303992, -0.0182778, -0.0542));
ShoulderLiftJoint->SetRelativeRotation(RosCoordConv::RosRpyToUeRotator(-1.5708, -1.5708, 0.0));
UpperArmLink = CreateDefaultSubobject<USceneComponent>(TEXT("UpperArmLink"));
UpperArmLink->SetupAttachment(ShoulderLiftJoint);
// --- elbow_flex joint ---
// URDF origin: xyz(-0.11257, -0.028, 0) rpy(0, 0, 1.5708)
ElbowFlexJoint = CreateDefaultSubobject<USceneComponent>(TEXT("ElbowFlexJoint"));
ElbowFlexJoint->SetupAttachment(UpperArmLink);
ElbowFlexJoint->SetRelativeLocation(RosCoordConv::RosToUePosition(-0.11257, -0.028, 0.0));
ElbowFlexJoint->SetRelativeRotation(RosCoordConv::RosRpyToUeRotator(0.0, 0.0, 1.5708));
LowerArmLink = CreateDefaultSubobject<USceneComponent>(TEXT("LowerArmLink"));
LowerArmLink->SetupAttachment(ElbowFlexJoint);
// --- wrist_flex joint ---
// URDF origin: xyz(-0.1349, 0.0052, 0) rpy(0, 0, -1.5708)
WristFlexJoint = CreateDefaultSubobject<USceneComponent>(TEXT("WristFlexJoint"));
WristFlexJoint->SetupAttachment(LowerArmLink);
WristFlexJoint->SetRelativeLocation(RosCoordConv::RosToUePosition(-0.1349, 0.0052, 0.0));
WristFlexJoint->SetRelativeRotation(RosCoordConv::RosRpyToUeRotator(0.0, 0.0, -1.5708));
WristLink = CreateDefaultSubobject<USceneComponent>(TEXT("WristLink"));
WristLink->SetupAttachment(WristFlexJoint);
// --- wrist_roll joint ---
// URDF origin: xyz(0, -0.0611, 0.0181) rpy(1.5708, 0.0486795, 3.14159)
WristRollJoint = CreateDefaultSubobject<USceneComponent>(TEXT("WristRollJoint"));
WristRollJoint->SetupAttachment(WristLink);
WristRollJoint->SetRelativeLocation(RosCoordConv::RosToUePosition(0.0, -0.0611, 0.0181));
WristRollJoint->SetRelativeRotation(RosCoordConv::RosRpyToUeRotator(1.5708, 0.0486795, 3.14159));
GripperLink = CreateDefaultSubobject<USceneComponent>(TEXT("GripperLink"));
GripperLink->SetupAttachment(WristRollJoint);
// --- gripper joint ---
// URDF origin: xyz(0.0202, 0.0188, -0.0234) rpy(1.5708, 0, 0)
GripperJoint = CreateDefaultSubobject<USceneComponent>(TEXT("GripperJoint"));
GripperJoint->SetupAttachment(GripperLink);
GripperJoint->SetRelativeLocation(RosCoordConv::RosToUePosition(0.0202, 0.0188, -0.0234));
GripperJoint->SetRelativeRotation(RosCoordConv::RosRpyToUeRotator(1.5708, 0.0, 0.0));
MovingJawLink = CreateDefaultSubobject<USceneComponent>(TEXT("MovingJawLink"));
MovingJawLink->SetupAttachment(GripperJoint);
// --- Joint name mapping (matches ROS /joint_states names) ---
JointComponentMap.Add(FName("shoulder_pan"), ShoulderPanJoint);
JointComponentMap.Add(FName("shoulder_lift"), ShoulderLiftJoint);
JointComponentMap.Add(FName("elbow_flex"), ElbowFlexJoint);
JointComponentMap.Add(FName("wrist_flex"), WristFlexJoint);
JointComponentMap.Add(FName("wrist_roll"), WristRollJoint);
JointComponentMap.Add(FName("gripper"), GripperJoint);
}
// =============================================================================
// BeginPlay — load meshes and attach, connect to ROS
// =============================================================================
void ARobotVisualizer::BeginPlay()
{
Super::BeginPlay();
// --- Load meshes and attach to links ---
// Meshes are loaded at runtime (not in constructor) because
// StaticLoadObject is safer to call here and allows hot-reload.
// Helper lambda to reduce repetition
auto Attach = [this](USceneComponent* Parent, const TCHAR* MeshName,
double RosX, double RosY, double RosZ,
double RosRoll, double RosPitch, double RosYaw,
bool bIsMotor = false)
{
UStaticMesh* Mesh = LoadMeshAsset(MeshName);
if (!Mesh) return;
UStaticMeshComponent* SMC = NewObject<UStaticMeshComponent>(this);
SMC->SetStaticMesh(Mesh);
SMC->SetRelativeLocation(RosCoordConv::RosToUePosition(RosX, RosY, RosZ));
SMC->SetRelativeRotation(RosCoordConv::RosRpyToUeRotator(RosRoll, RosPitch, RosYaw));
SMC->SetCollisionEnabled(ECollisionEnabled::NoCollision);
SMC->AttachToComponent(Parent, FAttachmentTransformRules::KeepRelativeTransform);
SMC->RegisterComponent();
AllMeshComponents.Add(SMC);
};
// === base_link meshes ===
Attach(BaseLink, TEXT("base_motor_holder_so101_v1"),
-0.00636471, -0.0000994414, -0.0024,
1.5708, 0.0, 1.5708);
Attach(BaseLink, TEXT("base_so101_v2"),
-0.00636471, 0.0, -0.0024,
1.5708, 0.0, 1.5708);
Attach(BaseLink, TEXT("sts3215_03a_v1"),
0.0263353, 0.0, 0.0437,
0.0, 0.0, 0.0, true);
Attach(BaseLink, TEXT("waveshare_mounting_plate_so101_v2"),
-0.0309827, -0.000199441, 0.0474,
1.5708, 0.0, 1.5708);
// === shoulder_link meshes ===
Attach(ShoulderLink, TEXT("sts3215_03a_v1"),
-0.0303992, 0.000422241, -0.0417,
1.5708, 1.5708, 0.0, true);
Attach(ShoulderLink, TEXT("motor_holder_so101_base_v1"),
-0.0675992, -0.000177759, 0.0158499,
1.5708, -1.5708, 0.0);
Attach(ShoulderLink, TEXT("rotation_pitch_so101_v1"),
0.0122008, 0.0000222413, 0.0464,
-1.5708, 0.0, 0.0);
// === upper_arm_link meshes ===
Attach(UpperArmLink, TEXT("sts3215_03a_v1"),
-0.11257, -0.0155, 0.0187,
-3.14159, 0.0, -1.5708, true);
Attach(UpperArmLink, TEXT("upper_arm_so101_v1"),
-0.065085, 0.012, 0.0182,
3.14159, 0.0, 0.0);
// === lower_arm_link meshes ===
Attach(LowerArmLink, TEXT("under_arm_so101_v1"),
-0.0648499, -0.032, 0.0182,
3.14159, 0.0, 0.0);
Attach(LowerArmLink, TEXT("motor_holder_so101_wrist_v1"),
-0.0648499, -0.032, 0.018,
-3.14159, 0.0, 0.0);
Attach(LowerArmLink, TEXT("sts3215_03a_v1"),
-0.1224, 0.0052, 0.0187,
-3.14159, 0.0, -3.14159, true);
// === wrist_link meshes ===
Attach(WristLink, TEXT("sts3215_03a_no_horn_v1"),
0.0, -0.0424, 0.0306,
1.5708, 1.5708, 0.0, true);
Attach(WristLink, TEXT("wrist_roll_pitch_so101_v2"),
0.0, -0.028, 0.0181,
-1.5708, -1.5708, 0.0);
// === gripper_link meshes ===
Attach(GripperLink, TEXT("sts3215_03a_v1"),
0.0077, 0.0001, -0.0234,
-1.5708, 0.0, 0.0, true);
Attach(GripperLink, TEXT("wrist_roll_follower_so101_v1"),
0.0, -0.000218214, 0.000949706,
-3.14159, 0.0, 0.0);
// === moving_jaw_link meshes ===
Attach(MovingJawLink, TEXT("moving_jaw_so101_v1"),
0.0, 0.0, 0.0189,
0.0, 0.0, 0.0);
UE_LOG(LogRosBridge, Log, TEXT("RobotVisualizer: %d mesh components created"), AllMeshComponents.Num());
// --- Connect to ROS via Subsystem ---
UGameInstance* GI = UGameplayStatics::GetGameInstance(this);
if (!GI) return;
URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>();
if (!Ros) return;
// Bind delegates.
Ros->OnTopicMessage.AddDynamic(this, &ARobotVisualizer::OnRosMessage);
Ros->OnConnected.AddDynamic(this, &ARobotVisualizer::OnRosBridgeConnected);
Ros->OnDisconnected.AddDynamic(this, &ARobotVisualizer::OnRosBridgeDisconnected);
// Subscribe is now queued even before connection — the subsystem will
// send it automatically when connected (including on reconnect).
Ros->Subscribe(JointStateTopic, JointStateType);
// Queue MoveIt topic advertisements (sent on connect).
AdvertiseMoveItTopics();
// Queue record/replay/estop topics.
SetupRecordReplayTopics();
// Subscribe to bridge heartbeat for connection health monitoring.
Ros->Subscribe(TEXT("/bridge_heartbeat"), TEXT("std_msgs/String"));
// Start connection health monitor (checks every 2 seconds).
const double Now = FPlatformTime::Seconds();
LastBridgeHeartbeatTime = Now;
LastJointStatesTime = Now;
GetWorldTimerManager().SetTimer(
ConnectionHealthTimerHandle, this,
&ARobotVisualizer::CheckConnectionHealth, 2.0f, true);
// Initiate connection if not already connected.
if (!Ros->IsConnected())
{
Ros->Connect(RosBridgeUrl);
}
else
{
// Already connected (e.g. another actor already called Connect).
UE_LOG(LogRosBridge, Log, TEXT("RobotVisualizer: already connected, subscription sent."));
}
// --- Spawn the in-viewport control UI (Phase 10) ---
if (ControlWidgetClass)
{
if (APlayerController* PC = UGameplayStatics::GetPlayerController(this, 0))
{
ControlWidget = CreateWidget<URobotControlWidget>(PC, ControlWidgetClass);
if (ControlWidget)
{
ControlWidget->AddToViewport();
UE_LOG(LogRosBridge, Log, TEXT("RobotVisualizer: control widget added to viewport."));
}
}
}
}
// =============================================================================
// EndPlay
// =============================================================================
void ARobotVisualizer::EndPlay(const EEndPlayReason::Type EndPlayReason)
{
if (UGameInstance* GI = UGameplayStatics::GetGameInstance(this))
{
if (URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>())
{
Ros->OnTopicMessage.RemoveDynamic(this, &ARobotVisualizer::OnRosMessage);
Ros->OnConnected.RemoveDynamic(this, &ARobotVisualizer::OnRosBridgeConnected);
Ros->OnDisconnected.RemoveDynamic(this, &ARobotVisualizer::OnRosBridgeDisconnected);
}
}
bMoveItTopicsAdvertised = false;
bRecordReplayTopicsSetup = false;
GetWorldTimerManager().ClearTimer(ConnectionHealthTimerHandle);
if (ControlWidget)
{
ControlWidget->RemoveFromParent();
ControlWidget = nullptr;
}
Super::EndPlay(EndPlayReason);
}
// =============================================================================
// ROS connection callback
// =============================================================================
void ARobotVisualizer::OnRosBridgeConnected()
{
bRosBridgeConnected = true;
UE_LOG(LogRosBridge, Log,
TEXT("RobotVisualizer: rosbridge connected — subscriptions restored by subsystem."));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Green,
TEXT("ROS Bridge: Connected"));
}
}
// =============================================================================
// ROS disconnection callback
// =============================================================================
void ARobotVisualizer::OnRosBridgeDisconnected()
{
bRosBridgeConnected = false;
WorkerState = TEXT("disconnected");
UE_LOG(LogRosBridge, Warning,
TEXT("RobotVisualizer: rosbridge DISCONNECTED — cannot send commands. Auto-reconnect active."));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 10.0f, FColor::Red,
TEXT("*** ROS Bridge DISCONNECTED *** Cannot send commands. Reconnecting..."));
}
}
// =============================================================================
// Connection health monitoring
// =============================================================================
void ARobotVisualizer::CheckConnectionHealth()
{
// Only check when WebSocket is connected — if WebSocket is down,
// OnRosBridgeDisconnected already shows a warning for that.
if (!bRosBridgeConnected)
{
return;
}
const double Now = FPlatformTime::Seconds();
// Check bridge heartbeat (published every 1s by bridge_node)
const double BridgeElapsed = Now - LastBridgeHeartbeatTime;
if (BridgeElapsed > BridgeHeartbeatTimeoutSec && !bBridgeHeartbeatLost)
{
bBridgeHeartbeatLost = true;
bWorkerDataLost = true; // if bridge is down, worker data can't reach us either
WorkerState = TEXT("bridge lost");
UE_LOG(LogRosBridge, Warning,
TEXT("Bridge heartbeat lost (%.1fs). bridge_node may be down."), BridgeElapsed);
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 10.0f, FColor::Red,
TEXT("*** Bridge Node: DOWN *** Check bridge_node terminal (terminal 2)."));
}
return; // no need to check worker separately
}
// Check worker data (/joint_states at ~30Hz, flows through bridge)
// Only meaningful if bridge is alive.
if (!bBridgeHeartbeatLost)
{
const double WorkerElapsed = Now - LastJointStatesTime;
if (WorkerElapsed > WorkerDataTimeoutSec && !bWorkerDataLost)
{
bWorkerDataLost = true;
WorkerState = TEXT("worker lost");
UE_LOG(LogRosBridge, Warning,
TEXT("Worker data lost (%.1fs). lerobot_worker may be down."), WorkerElapsed);
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 10.0f, FColor::Red,
TEXT("*** Worker: DOWN *** Check lerobot_worker terminal (terminal 1)."));
}
}
}
}
// =============================================================================
// MoveIt topic advertisements
// =============================================================================
void ARobotVisualizer::AdvertiseMoveItTopics()
{
UGameInstance* GI = UGameplayStatics::GetGameInstance(this);
if (!GI) return;
URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>();
if (!Ros) return;
// These are queued and sent automatically when connected.
Ros->Advertise(TEXT("/moveit_goal_named"), TEXT("std_msgs/String"));
Ros->Advertise(TEXT("/moveit_goal_joints"), TEXT("sensor_msgs/JointState"));
Ros->Advertise(TEXT("/moveit_goal_pose"), TEXT("geometry_msgs/PoseStamped"));
bMoveItTopicsAdvertised = true;
UE_LOG(LogRosBridge, Log, TEXT("RobotVisualizer: MoveIt command topics advertised."));
}
// =============================================================================
// MoveIt commands — SendNamedTarget
// =============================================================================
void ARobotVisualizer::SendNamedTarget()
{
UGameInstance* GI = UGameplayStatics::GetGameInstance(this);
if (!GI) return;
URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>();
if (!Ros || !Ros->IsConnected())
{
UE_LOG(LogRosBridge, Warning, TEXT("SendNamedTarget: not connected to rosbridge."));
return;
}
// std_msgs/String: {"data": "home"}
FString MsgJson = FString::Printf(TEXT("{\"data\":\"%s\"}"), *MoveItNamedTarget);
Ros->Publish(TEXT("/moveit_goal_named"), MsgJson);
UE_LOG(LogRosBridge, Log, TEXT("SendNamedTarget: published '%s' to /moveit_goal_named"), *MoveItNamedTarget);
}
// =============================================================================
// MoveIt commands — SendJointGoal
// =============================================================================
void ARobotVisualizer::SendJointGoal()
{
UGameInstance* GI = UGameplayStatics::GetGameInstance(this);
if (!GI) return;
URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>();
if (!Ros || !Ros->IsConnected())
{
UE_LOG(LogRosBridge, Warning, TEXT("SendJointGoal: not connected to rosbridge."));
return;
}
// sensor_msgs/JointState:
// {
// "header": {"stamp": {"sec": 0, "nanosec": 0}, "frame_id": ""},
// "name": ["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll"],
// "position": [0.0, -0.5, 0.5, 0.0, 0.0],
// "velocity": [],
// "effort": []
// }
FString MsgJson = FString::Printf(
TEXT("{\"header\":{\"stamp\":{\"sec\":0,\"nanosec\":0},\"frame_id\":\"\"},")
TEXT("\"name\":[\"shoulder_pan\",\"shoulder_lift\",\"elbow_flex\",\"wrist_flex\",\"wrist_roll\"],")
TEXT("\"position\":[%f,%f,%f,%f,%f],")
TEXT("\"velocity\":[],\"effort\":[]}"),
GoalShoulderPan, GoalShoulderLift, GoalElbowFlex, GoalWristFlex, GoalWristRoll
);
Ros->Publish(TEXT("/moveit_goal_joints"), MsgJson);
UE_LOG(LogRosBridge, Log,
TEXT("SendJointGoal: [%.3f, %.3f, %.3f, %.3f, %.3f] to /moveit_goal_joints"),
GoalShoulderPan, GoalShoulderLift, GoalElbowFlex, GoalWristFlex, GoalWristRoll);
}
// =============================================================================
// MoveIt commands — SendPoseGoal
// =============================================================================
void ARobotVisualizer::SendPoseGoal()
{
UGameInstance* GI = UGameplayStatics::GetGameInstance(this);
if (!GI) return;
URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>();
if (!Ros || !Ros->IsConnected())
{
UE_LOG(LogRosBridge, Warning, TEXT("SendPoseGoal: not connected to rosbridge."));
return;
}
// Convert UE position (cm, left-handed) to ROS (meters, right-handed)
double RosX, RosY, RosZ;
RosCoordConv::UeToRosPosition(GoalPositionUE, RosX, RosY, RosZ);
// geometry_msgs/PoseStamped (orientation defaults to identity — position-only goal)
FString MsgJson = FString::Printf(
TEXT("{\"header\":{\"stamp\":{\"sec\":0,\"nanosec\":0},\"frame_id\":\"base_link\"},")
TEXT("\"pose\":{\"position\":{\"x\":%f,\"y\":%f,\"z\":%f},")
TEXT("\"orientation\":{\"x\":0.0,\"y\":0.0,\"z\":0.0,\"w\":1.0}}}"),
RosX, RosY, RosZ
);
Ros->Publish(TEXT("/moveit_goal_pose"), MsgJson);
UE_LOG(LogRosBridge, Log,
TEXT("SendPoseGoal: UE(%.1f, %.1f, %.1f)cm -> ROS(%.4f, %.4f, %.4f)m to /moveit_goal_pose"),
GoalPositionUE.X, GoalPositionUE.Y, GoalPositionUE.Z,
RosX, RosY, RosZ);
}
// =============================================================================
// Message handling
// =============================================================================
void ARobotVisualizer::OnRosMessage(const FString& Topic, const FString& MessageJson)
{
if (Topic == JointStateTopic)
{
// /joint_states arrives at ~30Hz — proves both bridge AND worker are alive.
LastJointStatesTime = FPlatformTime::Seconds();
LastBridgeHeartbeatTime = LastJointStatesTime; // joint_states flows through bridge
if (bWorkerDataLost)
{
bWorkerDataLost = false;
UE_LOG(LogRosBridge, Log, TEXT("Worker data restored."));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Green,
TEXT("Worker: Connection restored"));
}
}
if (bBridgeHeartbeatLost)
{
bBridgeHeartbeatLost = false;
UE_LOG(LogRosBridge, Log, TEXT("Bridge heartbeat restored (via joint_states)."));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Green,
TEXT("Bridge: Connection restored"));
}
}
ParseAndApplyJointStates(MessageJson);
}
else if (Topic == TEXT("/bridge_heartbeat"))
{
// Bridge heartbeat arrives every 1s — proves bridge is alive
// (but worker may still be dead if /joint_states is not arriving).
LastBridgeHeartbeatTime = FPlatformTime::Seconds();
if (bBridgeHeartbeatLost)
{
bBridgeHeartbeatLost = false;
UE_LOG(LogRosBridge, Log, TEXT("Bridge heartbeat restored."));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Green,
TEXT("Bridge: Connection restored"));
}
}
}
else if (Topic == TEXT("/robot_status"))
{
OnRobotStatus(Topic, MessageJson);
}
}
void ARobotVisualizer::ParseAndApplyJointStates(const FString& MessageJson)
{
// Parse sensor_msgs/JointState JSON:
// { "name": ["shoulder_pan", ...], "position": [0.1, ...], ... }
TSharedPtr<FJsonObject> Json;
TSharedRef<TJsonReader<>> Reader = TJsonReaderFactory<>::Create(MessageJson);
if (!FJsonSerializer::Deserialize(Reader, Json) || !Json.IsValid())
{
return;
}
const TArray<TSharedPtr<FJsonValue>>* NameArray = nullptr;
const TArray<TSharedPtr<FJsonValue>>* PosArray = nullptr;
if (!Json->TryGetArrayField(TEXT("name"), NameArray) ||
!Json->TryGetArrayField(TEXT("position"), PosArray))
{
return;
}
const int32 Count = FMath::Min(NameArray->Num(), PosArray->Num());
for (int32 i = 0; i < Count; ++i)
{
const FName JointName(*(*NameArray)[i]->AsString());
const double AngleRad = (*PosArray)[i]->AsNumber();
TObjectPtr<USceneComponent>* JointComp = JointComponentMap.Find(JointName);
if (JointComp && *JointComp)
{
const float AngleDeg = RosCoordConv::RosJointAngleToUeDegrees(AngleRad);
FRotator BaseRot;
if (JointName == FName("shoulder_pan"))
BaseRot = RosCoordConv::RosRpyToUeRotator(3.14159, 0.0, -3.14159);
else if (JointName == FName("shoulder_lift"))
BaseRot = RosCoordConv::RosRpyToUeRotator(-1.5708, -1.5708, 0.0);
else if (JointName == FName("elbow_flex"))
BaseRot = RosCoordConv::RosRpyToUeRotator(0.0, 0.0, 1.5708);
else if (JointName == FName("wrist_flex"))
BaseRot = RosCoordConv::RosRpyToUeRotator(0.0, 0.0, -1.5708);
else if (JointName == FName("wrist_roll"))
BaseRot = RosCoordConv::RosRpyToUeRotator(1.5708, 0.0486795, 3.14159);
else if (JointName == FName("gripper"))
BaseRot = RosCoordConv::RosRpyToUeRotator(1.5708, 0.0, 0.0);
const FQuat BaseQuat = BaseRot.Quaternion();
const FQuat JointQuat = FQuat(FVector::UpVector, FMath::DegreesToRadians(AngleDeg));
const FQuat FinalQuat = BaseQuat * JointQuat;
(*JointComp)->SetRelativeRotation(FinalQuat.Rotator());
}
}
}
// =============================================================================
// Record / Replay / E-Stop — Topic setup
// =============================================================================
void ARobotVisualizer::SetupRecordReplayTopics()
{
UGameInstance* GI = UGameplayStatics::GetGameInstance(this);
if (!GI) return;
URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>();
if (!Ros) return;
// Advertise the command topic
Ros->Advertise(TEXT("/robot_command"), TEXT("std_msgs/String"));
// Subscribe to status feedback
Ros->Subscribe(TEXT("/robot_status"), TEXT("std_msgs/String"));
bRecordReplayTopicsSetup = true;
UE_LOG(LogRosBridge, Log, TEXT("RobotVisualizer: Record/Replay topics set up."));
}
// =============================================================================
// Record / Replay / E-Stop — Command publisher helper
// =============================================================================
void ARobotVisualizer::PublishRobotCommand(const FString& JsonCmd)
{
UGameInstance* GI = UGameplayStatics::GetGameInstance(this);
if (!GI) return;
URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>();
if (!Ros || !Ros->IsConnected())
{
UE_LOG(LogRosBridge, Warning, TEXT("PublishRobotCommand: not connected to rosbridge."));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 3.0f, FColor::Red,
TEXT("Robot: Not connected to rosbridge!"));
}
return;
}
// std_msgs/String: {"data": "<json_cmd>"}
// The JSON command is nested inside the "data" field, with quotes escaped.
FString EscapedCmd = JsonCmd.Replace(TEXT("\""), TEXT("\\\""));
FString MsgJson = FString::Printf(TEXT("{\"data\":\"%s\"}"), *EscapedCmd);
Ros->Publish(TEXT("/robot_command"), MsgJson);
UE_LOG(LogRosBridge, Log, TEXT("PublishRobotCommand: %s"), *JsonCmd);
}
// =============================================================================
// Record / Replay / E-Stop — Button handlers
// =============================================================================
void ARobotVisualizer::StartRecord()
{
PublishRobotCommand(TEXT("{\"cmd\":\"start_record\"}"));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 3.0f, FColor::Green,
TEXT("Robot: Recording started (teleop active)"));
}
}
void ARobotVisualizer::StopRecord()
{
PublishRobotCommand(TEXT("{\"cmd\":\"stop_record\"}"));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 3.0f, FColor::Yellow,
TEXT("Robot: Recording stopped, saving..."));
}
}
void ARobotVisualizer::StartReplay()
{
FString ArgsJson;
if (ReplayFilename.IsEmpty())
{
ArgsJson = FString::Printf(
TEXT("{\"cmd\":\"start_replay\",\"args\":{\"loop\":%s,\"approach_speed\":%f}}"),
bReplayLoop ? TEXT("true") : TEXT("false"),
ApproachSpeed);
}
else
{
ArgsJson = FString::Printf(
TEXT("{\"cmd\":\"start_replay\",\"args\":{\"filename\":\"%s\",\"loop\":%s,\"approach_speed\":%f}}"),
*ReplayFilename,
bReplayLoop ? TEXT("true") : TEXT("false"),
ApproachSpeed);
}
PublishRobotCommand(ArgsJson);
if (GEngine)
{
FString DisplayName = ReplayFilename.IsEmpty() ? TEXT("(most recent)") : ReplayFilename;
GEngine->AddOnScreenDebugMessage(-1, 3.0f, FColor::Cyan,
FString::Printf(TEXT("Robot: Replaying %s (loop=%s, speed=%.0f°/s)"),
*DisplayName, bReplayLoop ? TEXT("yes") : TEXT("no"), ApproachSpeed));
}
}
void ARobotVisualizer::StopReplay()
{
PublishRobotCommand(TEXT("{\"cmd\":\"stop_replay\"}"));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 3.0f, FColor::Yellow,
TEXT("Robot: Replay stopped"));
}
}
void ARobotVisualizer::EStop()
{
PublishRobotCommand(TEXT("{\"cmd\":\"estop\"}"));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Red,
TEXT("*** E-STOP *** All motion halted"));
}
}
// =============================================================================
// Teleop Sync — SyncOn / SyncOff
// =============================================================================
void ARobotVisualizer::SyncOn()
{
PublishRobotCommand(TEXT("{\"cmd\":\"start_teleop\"}"));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 3.0f, FColor::Green,
TEXT("Sync ON: leader -> follower active"));
}
}
void ARobotVisualizer::SyncOff()
{
PublishRobotCommand(TEXT("{\"cmd\":\"stop_teleop\"}"));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 3.0f, FColor::Yellow,
TEXT("Sync OFF: leader -> follower deactivated"));
}
}
// =============================================================================
// Record / Replay — Status feedback handler
// =============================================================================
void ARobotVisualizer::OnRobotStatus(const FString& Topic, const FString& MessageJson)
{
UE_LOG(LogRosBridge, Warning, TEXT("OnRobotStatus HIT len=%d"), MessageJson.Len());
// /robot_status flows through bridge from worker — both are alive.
const double Now = FPlatformTime::Seconds();
LastBridgeHeartbeatTime = Now;
LastJointStatesTime = Now;
if (bBridgeHeartbeatLost)
{
bBridgeHeartbeatLost = false;
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Green,
TEXT("Bridge: Connection restored"));
}
}
if (bWorkerDataLost)
{
bWorkerDataLost = false;
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Green,
TEXT("Worker: Connection restored"));
}
}
// rosbridge wraps std_msgs/String as: {"data": "..."}
TSharedPtr<FJsonObject> OuterJson;
TSharedRef<TJsonReader<>> OuterReader = TJsonReaderFactory<>::Create(MessageJson);
if (!FJsonSerializer::Deserialize(OuterReader, OuterJson) || !OuterJson.IsValid())
{
return;
}
FString DataStr;
if (!OuterJson->TryGetStringField(TEXT("data"), DataStr))
{
return;
}
// Parse the inner JSON status
TSharedPtr<FJsonObject> StatusJson;
TSharedRef<TJsonReader<>> StatusReader = TJsonReaderFactory<>::Create(DataStr);
if (!FJsonSerializer::Deserialize(StatusReader, StatusJson) || !StatusJson.IsValid())
{
return;
}
FString State;
if (StatusJson->TryGetStringField(TEXT("state"), State))
{
if (State != WorkerState)
{
WorkerState = State;
UE_LOG(LogRosBridge, Log, TEXT("Worker state: %s"), *WorkerState);
if (GEngine)
{
FColor Color = FColor::White;
if (State == TEXT("recording")) Color = FColor::Green;
else if (State == TEXT("replaying")) Color = FColor::Cyan;
else if (State == TEXT("idle")) Color = FColor::Silver;
GEngine->AddOnScreenDebugMessage(-1, 3.0f, Color,
FString::Printf(TEXT("Robot state: %s"), *WorkerState));
}
}
}
// Update sync (teleop) status
bool bTeleop = false;
if (StatusJson->TryGetBoolField(TEXT("teleop"), bTeleop))
{
if (bTeleop != bSyncActive)
{
bSyncActive = bTeleop;
UE_LOG(LogRosBridge, Log, TEXT("Sync (teleop): %s"),
bSyncActive ? TEXT("ON") : TEXT("OFF"));
}
}
// Log errors from commands
FString Status;
if (StatusJson->TryGetStringField(TEXT("status"), Status) && Status == TEXT("error"))
{
FString Reason;
StatusJson->TryGetStringField(TEXT("reason"), Reason);
UE_LOG(LogRosBridge, Warning, TEXT("Robot command error: %s"), *Reason);
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Red,
FString::Printf(TEXT("Robot error: %s"), *Reason));
}
}
// Log recording saved info
FString Filename;
if (StatusJson->TryGetStringField(TEXT("filename"), Filename) && !Filename.IsEmpty())
{
int32 Frames = 0;
StatusJson->TryGetNumberField(TEXT("frames"), Frames);
double Duration = 0.0;
StatusJson->TryGetNumberField(TEXT("duration_sec"), Duration);
UE_LOG(LogRosBridge, Log, TEXT("Recording saved: %s (%d frames, %.1fs)"),
*Filename, Frames, Duration);
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Green,
FString::Printf(TEXT("Recording saved: %s (%d frames, %.1fs)"),
*Filename, Frames, Duration));
}
}
// --- Recordings list (reply to list_recordings) ---
const TArray<TSharedPtr<FJsonValue>>* RecArray = nullptr;
if (StatusJson->TryGetArrayField(TEXT("recordings"), RecArray) && RecArray)
{
Recordings.Reset();
for (const TSharedPtr<FJsonValue>& V : *RecArray)
{
const TSharedPtr<FJsonObject> RecObj = V->AsObject();
if (RecObj.IsValid())
{
FRecordingInfo Info;
RecObj->TryGetStringField(TEXT("filename"), Info.Filename);
RecObj->TryGetNumberField(TEXT("frames"), Info.Frames);
double Dur = 0.0;
RecObj->TryGetNumberField(TEXT("duration_sec"), Dur);
Info.DurationSec = static_cast<float>(Dur);
RecObj->TryGetStringField(TEXT("recorded_at"), Info.RecordedAt);
Recordings.Add(Info);
}
}
++RecordingsVersion;
UE_LOG(LogRosBridge, Log, TEXT("Recordings list updated: %d files"), Recordings.Num());
}
// --- Recordings list, per-item (split to avoid large-frame drops) ---
const TSharedPtr<FJsonObject>* RecItemObj = nullptr;
if (StatusJson->TryGetObjectField(TEXT("recording_item"), RecItemObj) && RecItemObj && RecItemObj->IsValid())
{
int32 RecIndex = 0, RecTotal = 0;
StatusJson->TryGetNumberField(TEXT("index"), RecIndex);
StatusJson->TryGetNumberField(TEXT("total"), RecTotal);
if (RecIndex == 0) { PendingRecordings.Reset(); }
FRecordingInfo Info;
(*RecItemObj)->TryGetStringField(TEXT("filename"), Info.Filename);
(*RecItemObj)->TryGetNumberField(TEXT("frames"), Info.Frames);
double Dur = 0.0;
(*RecItemObj)->TryGetNumberField(TEXT("duration_sec"), Dur);
Info.DurationSec = static_cast<float>(Dur);
(*RecItemObj)->TryGetStringField(TEXT("recorded_at"), Info.RecordedAt);
PendingRecordings.Add(Info);
if (RecTotal > 0 && PendingRecordings.Num() >= RecTotal)
{
Recordings = PendingRecordings;
PendingRecordings.Reset();
++RecordingsVersion;
UE_LOG(LogRosBridge, Log, TEXT("Recordings list updated: %d files"), Recordings.Num());
}
}
// --- Recordings list cleared (empty result) ---
bool bRecClear = false;
if (StatusJson->TryGetBoolField(TEXT("recordings_clear"), bRecClear) && bRecClear)
{
Recordings.Reset();
PendingRecordings.Reset();
++RecordingsVersion;
UE_LOG(LogRosBridge, Log, TEXT("Recordings list cleared"));
}
// --- Replay progress ---
const TSharedPtr<FJsonObject>* ProgObj = nullptr;
if (StatusJson->TryGetObjectField(TEXT("replay_progress"), ProgObj) && ProgObj)
{
(*ProgObj)->TryGetNumberField(TEXT("index"), ReplayIndex);
(*ProgObj)->TryGetNumberField(TEXT("total"), ReplayTotal);
(*ProgObj)->TryGetStringField(TEXT("filename"), ReplayProgFilename);
(*ProgObj)->TryGetBoolField(TEXT("approaching"), bReplayApproaching);
}
// Display device-level errors (USB disconnection, serial errors)
const TSharedPtr<FJsonObject>* DeviceErrors = nullptr;
bool bHasDeviceErrors = StatusJson->TryGetObjectField(TEXT("device_errors"), DeviceErrors) && DeviceErrors;
// Check follower error
FString FollowerErr;
if (bHasDeviceErrors)
{
(*DeviceErrors)->TryGetStringField(TEXT("follower"), FollowerErr);
}
if (!FollowerErr.IsEmpty() && !bFollowerDeviceError)
{
bFollowerDeviceError = true;
UE_LOG(LogRosBridge, Error, TEXT("Follower USB error: %s"), *FollowerErr);
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 10.0f, FColor::Red,
TEXT("*** Follower: USB/Serial ERROR *** Check USB connection."));
}
}
else if (FollowerErr.IsEmpty() && bFollowerDeviceError)
{
bFollowerDeviceError = false;
UE_LOG(LogRosBridge, Log, TEXT("Follower USB restored."));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Green,
TEXT("Follower: USB connection restored"));
}
}
// Check leader error
FString LeaderErr;
if (bHasDeviceErrors)
{
(*DeviceErrors)->TryGetStringField(TEXT("leader"), LeaderErr);
}
if (!LeaderErr.IsEmpty() && !bLeaderDeviceError)
{
bLeaderDeviceError = true;
UE_LOG(LogRosBridge, Error, TEXT("Leader USB error: %s"), *LeaderErr);
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 10.0f, FColor::Red,
TEXT("*** Leader: USB/Serial ERROR *** Check USB connection."));
}
}
else if (LeaderErr.IsEmpty() && bLeaderDeviceError)
{
bLeaderDeviceError = false;
UE_LOG(LogRosBridge, Log, TEXT("Leader USB restored."));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Green,
TEXT("Leader: USB connection restored"));
}
}
}
*기능 보완
추가 디버깅으로, Refresh 버튼 한 번으로는 리스트가 제대로 안뜨는 문제와 리스트 이름 순 정렬 부분을 보완한다.
Refresh 버튼 한 번으로 리스트 잘 뜨게 하기
현재는 RefreshRecordingsList()가 매 틱(5Hz) 돌면서 worker가 살아있으면 자동으로 list_recordings를 한 번 쏘고, Refresh 버튼 클릭까지 겹쳐서 들어올 수 있다. 이러면 버퍼가 깨지게 된다. 리스트 완성 판정을 index==0 리셋에 의존하지 말고 항목이 자기 자리 index에 들어가게 만들면 된다.
- RobotVisualizer.cpp 수정
recording_item 블록 교체
// --- Recordings list, per-item (split to avoid large-frame drops) ---
const TSharedPtr<FJsonObject>* RecItemObj = nullptr;
if (StatusJson->TryGetObjectField(TEXT("recording_item"), RecItemObj) && RecItemObj && RecItemObj->IsValid())
{
int32 RecIndex = 0, RecTotal = 0;
StatusJson->TryGetNumberField(TEXT("index"), RecIndex);
StatusJson->TryGetNumberField(TEXT("total"), RecTotal);
// Size the buffer to `total` and drop each item into its own slot.
// This is robust even if two list requests overlap (same list, so
// slots just get refilled) — no reset-in-the-middle corruption.
if (RecTotal > 0)
{
if (PendingRecordings.Num() != RecTotal)
{
PendingRecordings.Reset();
PendingRecordings.SetNum(RecTotal);
}
if (PendingRecordings.IsValidIndex(RecIndex))
{
FRecordingInfo Info;
(*RecItemObj)->TryGetStringField(TEXT("filename"), Info.Filename);
(*RecItemObj)->TryGetNumberField(TEXT("frames"), Info.Frames);
double Dur = 0.0;
(*RecItemObj)->TryGetNumberField(TEXT("duration_sec"), Dur);
Info.DurationSec = static_cast<float>(Dur);
(*RecItemObj)->TryGetStringField(TEXT("recorded_at"), Info.RecordedAt);
PendingRecordings[RecIndex] = Info;
}
// Complete only when every slot is filled (filename non-empty).
bool bComplete = true;
for (const FRecordingInfo& R : PendingRecordings)
{
if (R.Filename.IsEmpty()) { bComplete = false; break; }
}
if (bComplete)
{
Recordings = PendingRecordings;
++RecordingsVersion;
UE_LOG(LogRosBridge, Log, TEXT("Recordings list updated: %d files"), Recordings.Num());
}
}
}
리스트 이름 순 정렬
정렬은 Unreal에서 완성 시점에 한 번 하는 게 깔끔하다.
Recordings = PendingRecordings; 다음 줄에 정렬 한 줄을 추가
Recordings = PendingRecordings;
Recordings.Sort([](const FRecordingInfo& A, const FRecordingInfo& B)
{
return A.Filename > B.Filename;
});
++RecordingsVersion;
RobotVisualizer.cpp 전체 코드
더보기
#include "RobotVisualizer.h"
#include "RosCoordConv.h"
#include "RosBridgeSubsystem.h"
#include "RosBridgeLog.h"
#include "Components/SceneComponent.h"
#include "Components/StaticMeshComponent.h"
#include "Engine/StaticMesh.h"
#include "Engine/Engine.h"
#include "Engine/World.h"
#include "Kismet/GameplayStatics.h"
#include "Dom/JsonObject.h"
#include "Dom/JsonValue.h"
#include "Serialization/JsonReader.h"
#include "Serialization/JsonSerializer.h"
#include "Serialization/JsonWriter.h"
#include "UObject/ConstructorHelpers.h"
#include "GameFramework/PlayerController.h"
#include "Blueprint/UserWidget.h"
#include "RobotControlWidget.h"
// =============================================================================
// Mesh asset path helper
// =============================================================================
static UStaticMesh* LoadMeshAsset(const TCHAR* AssetName)
{
// All meshes live under /Game/Robot/Meshes/
FString Path = FString::Printf(TEXT("/Game/Robot/Meshes/%s.%s"), AssetName, AssetName);
UStaticMesh* Mesh = Cast<UStaticMesh>(StaticLoadObject(UStaticMesh::StaticClass(), nullptr, *Path));
if (!Mesh)
{
UE_LOG(LogRosBridge, Warning, TEXT("Failed to load mesh: %s"), *Path);
}
return Mesh;
}
// =============================================================================
// Constructor — build the entire component hierarchy
// =============================================================================
ARobotVisualizer::ARobotVisualizer()
{
PrimaryActorTick.bCanEverTick = false;
// --- Root ---
RobotRoot = CreateDefaultSubobject<USceneComponent>(TEXT("RobotRoot"));
RootComponent = RobotRoot;
// =========================================================================
// URDF data converted to UE coordinates:
// Position: meters * 100 = cm, Y flipped
// Rotation: RPY radians -> FRotator degrees, pitch & yaw negated
//
// All values below are pre-computed from so101_follower.urdf.
// =========================================================================
// --- base_link (attached directly to root, no joint) ---
BaseLink = CreateDefaultSubobject<USceneComponent>(TEXT("BaseLink"));
BaseLink->SetupAttachment(RobotRoot);
// --- shoulder_pan joint ---
// URDF origin: xyz(0.0388353, 0, 0.0624) rpy(3.14159, 0, -3.14159)
ShoulderPanJoint = CreateDefaultSubobject<USceneComponent>(TEXT("ShoulderPanJoint"));
ShoulderPanJoint->SetupAttachment(BaseLink);
ShoulderPanJoint->SetRelativeLocation(RosCoordConv::RosToUePosition(0.0388353, 0.0, 0.0624));
ShoulderPanJoint->SetRelativeRotation(RosCoordConv::RosRpyToUeRotator(3.14159, 0.0, -3.14159));
ShoulderLink = CreateDefaultSubobject<USceneComponent>(TEXT("ShoulderLink"));
ShoulderLink->SetupAttachment(ShoulderPanJoint);
// --- shoulder_lift joint ---
// URDF origin: xyz(-0.0303992, -0.0182778, -0.0542) rpy(-1.5708, -1.5708, 0)
ShoulderLiftJoint = CreateDefaultSubobject<USceneComponent>(TEXT("ShoulderLiftJoint"));
ShoulderLiftJoint->SetupAttachment(ShoulderLink);
ShoulderLiftJoint->SetRelativeLocation(RosCoordConv::RosToUePosition(-0.0303992, -0.0182778, -0.0542));
ShoulderLiftJoint->SetRelativeRotation(RosCoordConv::RosRpyToUeRotator(-1.5708, -1.5708, 0.0));
UpperArmLink = CreateDefaultSubobject<USceneComponent>(TEXT("UpperArmLink"));
UpperArmLink->SetupAttachment(ShoulderLiftJoint);
// --- elbow_flex joint ---
// URDF origin: xyz(-0.11257, -0.028, 0) rpy(0, 0, 1.5708)
ElbowFlexJoint = CreateDefaultSubobject<USceneComponent>(TEXT("ElbowFlexJoint"));
ElbowFlexJoint->SetupAttachment(UpperArmLink);
ElbowFlexJoint->SetRelativeLocation(RosCoordConv::RosToUePosition(-0.11257, -0.028, 0.0));
ElbowFlexJoint->SetRelativeRotation(RosCoordConv::RosRpyToUeRotator(0.0, 0.0, 1.5708));
LowerArmLink = CreateDefaultSubobject<USceneComponent>(TEXT("LowerArmLink"));
LowerArmLink->SetupAttachment(ElbowFlexJoint);
// --- wrist_flex joint ---
// URDF origin: xyz(-0.1349, 0.0052, 0) rpy(0, 0, -1.5708)
WristFlexJoint = CreateDefaultSubobject<USceneComponent>(TEXT("WristFlexJoint"));
WristFlexJoint->SetupAttachment(LowerArmLink);
WristFlexJoint->SetRelativeLocation(RosCoordConv::RosToUePosition(-0.1349, 0.0052, 0.0));
WristFlexJoint->SetRelativeRotation(RosCoordConv::RosRpyToUeRotator(0.0, 0.0, -1.5708));
WristLink = CreateDefaultSubobject<USceneComponent>(TEXT("WristLink"));
WristLink->SetupAttachment(WristFlexJoint);
// --- wrist_roll joint ---
// URDF origin: xyz(0, -0.0611, 0.0181) rpy(1.5708, 0.0486795, 3.14159)
WristRollJoint = CreateDefaultSubobject<USceneComponent>(TEXT("WristRollJoint"));
WristRollJoint->SetupAttachment(WristLink);
WristRollJoint->SetRelativeLocation(RosCoordConv::RosToUePosition(0.0, -0.0611, 0.0181));
WristRollJoint->SetRelativeRotation(RosCoordConv::RosRpyToUeRotator(1.5708, 0.0486795, 3.14159));
GripperLink = CreateDefaultSubobject<USceneComponent>(TEXT("GripperLink"));
GripperLink->SetupAttachment(WristRollJoint);
// --- gripper joint ---
// URDF origin: xyz(0.0202, 0.0188, -0.0234) rpy(1.5708, 0, 0)
GripperJoint = CreateDefaultSubobject<USceneComponent>(TEXT("GripperJoint"));
GripperJoint->SetupAttachment(GripperLink);
GripperJoint->SetRelativeLocation(RosCoordConv::RosToUePosition(0.0202, 0.0188, -0.0234));
GripperJoint->SetRelativeRotation(RosCoordConv::RosRpyToUeRotator(1.5708, 0.0, 0.0));
MovingJawLink = CreateDefaultSubobject<USceneComponent>(TEXT("MovingJawLink"));
MovingJawLink->SetupAttachment(GripperJoint);
// --- Joint name mapping (matches ROS /joint_states names) ---
JointComponentMap.Add(FName("shoulder_pan"), ShoulderPanJoint);
JointComponentMap.Add(FName("shoulder_lift"), ShoulderLiftJoint);
JointComponentMap.Add(FName("elbow_flex"), ElbowFlexJoint);
JointComponentMap.Add(FName("wrist_flex"), WristFlexJoint);
JointComponentMap.Add(FName("wrist_roll"), WristRollJoint);
JointComponentMap.Add(FName("gripper"), GripperJoint);
}
// =============================================================================
// BeginPlay — load meshes and attach, connect to ROS
// =============================================================================
void ARobotVisualizer::BeginPlay()
{
Super::BeginPlay();
// --- Load meshes and attach to links ---
// Meshes are loaded at runtime (not in constructor) because
// StaticLoadObject is safer to call here and allows hot-reload.
// Helper lambda to reduce repetition
auto Attach = [this](USceneComponent* Parent, const TCHAR* MeshName,
double RosX, double RosY, double RosZ,
double RosRoll, double RosPitch, double RosYaw,
bool bIsMotor = false)
{
UStaticMesh* Mesh = LoadMeshAsset(MeshName);
if (!Mesh) return;
UStaticMeshComponent* SMC = NewObject<UStaticMeshComponent>(this);
SMC->SetStaticMesh(Mesh);
SMC->SetRelativeLocation(RosCoordConv::RosToUePosition(RosX, RosY, RosZ));
SMC->SetRelativeRotation(RosCoordConv::RosRpyToUeRotator(RosRoll, RosPitch, RosYaw));
SMC->SetCollisionEnabled(ECollisionEnabled::NoCollision);
SMC->AttachToComponent(Parent, FAttachmentTransformRules::KeepRelativeTransform);
SMC->RegisterComponent();
AllMeshComponents.Add(SMC);
};
// === base_link meshes ===
Attach(BaseLink, TEXT("base_motor_holder_so101_v1"),
-0.00636471, -0.0000994414, -0.0024,
1.5708, 0.0, 1.5708);
Attach(BaseLink, TEXT("base_so101_v2"),
-0.00636471, 0.0, -0.0024,
1.5708, 0.0, 1.5708);
Attach(BaseLink, TEXT("sts3215_03a_v1"),
0.0263353, 0.0, 0.0437,
0.0, 0.0, 0.0, true);
Attach(BaseLink, TEXT("waveshare_mounting_plate_so101_v2"),
-0.0309827, -0.000199441, 0.0474,
1.5708, 0.0, 1.5708);
// === shoulder_link meshes ===
Attach(ShoulderLink, TEXT("sts3215_03a_v1"),
-0.0303992, 0.000422241, -0.0417,
1.5708, 1.5708, 0.0, true);
Attach(ShoulderLink, TEXT("motor_holder_so101_base_v1"),
-0.0675992, -0.000177759, 0.0158499,
1.5708, -1.5708, 0.0);
Attach(ShoulderLink, TEXT("rotation_pitch_so101_v1"),
0.0122008, 0.0000222413, 0.0464,
-1.5708, 0.0, 0.0);
// === upper_arm_link meshes ===
Attach(UpperArmLink, TEXT("sts3215_03a_v1"),
-0.11257, -0.0155, 0.0187,
-3.14159, 0.0, -1.5708, true);
Attach(UpperArmLink, TEXT("upper_arm_so101_v1"),
-0.065085, 0.012, 0.0182,
3.14159, 0.0, 0.0);
// === lower_arm_link meshes ===
Attach(LowerArmLink, TEXT("under_arm_so101_v1"),
-0.0648499, -0.032, 0.0182,
3.14159, 0.0, 0.0);
Attach(LowerArmLink, TEXT("motor_holder_so101_wrist_v1"),
-0.0648499, -0.032, 0.018,
-3.14159, 0.0, 0.0);
Attach(LowerArmLink, TEXT("sts3215_03a_v1"),
-0.1224, 0.0052, 0.0187,
-3.14159, 0.0, -3.14159, true);
// === wrist_link meshes ===
Attach(WristLink, TEXT("sts3215_03a_no_horn_v1"),
0.0, -0.0424, 0.0306,
1.5708, 1.5708, 0.0, true);
Attach(WristLink, TEXT("wrist_roll_pitch_so101_v2"),
0.0, -0.028, 0.0181,
-1.5708, -1.5708, 0.0);
// === gripper_link meshes ===
Attach(GripperLink, TEXT("sts3215_03a_v1"),
0.0077, 0.0001, -0.0234,
-1.5708, 0.0, 0.0, true);
Attach(GripperLink, TEXT("wrist_roll_follower_so101_v1"),
0.0, -0.000218214, 0.000949706,
-3.14159, 0.0, 0.0);
// === moving_jaw_link meshes ===
Attach(MovingJawLink, TEXT("moving_jaw_so101_v1"),
0.0, 0.0, 0.0189,
0.0, 0.0, 0.0);
UE_LOG(LogRosBridge, Log, TEXT("RobotVisualizer: %d mesh components created"), AllMeshComponents.Num());
// --- Connect to ROS via Subsystem ---
UGameInstance* GI = UGameplayStatics::GetGameInstance(this);
if (!GI) return;
URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>();
if (!Ros) return;
// Bind delegates.
Ros->OnTopicMessage.AddDynamic(this, &ARobotVisualizer::OnRosMessage);
Ros->OnConnected.AddDynamic(this, &ARobotVisualizer::OnRosBridgeConnected);
Ros->OnDisconnected.AddDynamic(this, &ARobotVisualizer::OnRosBridgeDisconnected);
// Subscribe is now queued even before connection — the subsystem will
// send it automatically when connected (including on reconnect).
Ros->Subscribe(JointStateTopic, JointStateType);
// Queue MoveIt topic advertisements (sent on connect).
AdvertiseMoveItTopics();
// Queue record/replay/estop topics.
SetupRecordReplayTopics();
// Subscribe to bridge heartbeat for connection health monitoring.
Ros->Subscribe(TEXT("/bridge_heartbeat"), TEXT("std_msgs/String"));
// Start connection health monitor (checks every 2 seconds).
const double Now = FPlatformTime::Seconds();
LastBridgeHeartbeatTime = Now;
LastJointStatesTime = Now;
GetWorldTimerManager().SetTimer(
ConnectionHealthTimerHandle, this,
&ARobotVisualizer::CheckConnectionHealth, 2.0f, true);
// Initiate connection if not already connected.
if (!Ros->IsConnected())
{
Ros->Connect(RosBridgeUrl);
}
else
{
// Already connected (e.g. another actor already called Connect).
UE_LOG(LogRosBridge, Log, TEXT("RobotVisualizer: already connected, subscription sent."));
}
// --- Spawn the in-viewport control UI (Phase 10) ---
if (ControlWidgetClass)
{
if (APlayerController* PC = UGameplayStatics::GetPlayerController(this, 0))
{
ControlWidget = CreateWidget<URobotControlWidget>(PC, ControlWidgetClass);
if (ControlWidget)
{
ControlWidget->AddToViewport();
UE_LOG(LogRosBridge, Log, TEXT("RobotVisualizer: control widget added to viewport."));
}
}
}
}
// =============================================================================
// EndPlay
// =============================================================================
void ARobotVisualizer::EndPlay(const EEndPlayReason::Type EndPlayReason)
{
if (UGameInstance* GI = UGameplayStatics::GetGameInstance(this))
{
if (URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>())
{
Ros->OnTopicMessage.RemoveDynamic(this, &ARobotVisualizer::OnRosMessage);
Ros->OnConnected.RemoveDynamic(this, &ARobotVisualizer::OnRosBridgeConnected);
Ros->OnDisconnected.RemoveDynamic(this, &ARobotVisualizer::OnRosBridgeDisconnected);
}
}
bMoveItTopicsAdvertised = false;
bRecordReplayTopicsSetup = false;
GetWorldTimerManager().ClearTimer(ConnectionHealthTimerHandle);
if (ControlWidget)
{
ControlWidget->RemoveFromParent();
ControlWidget = nullptr;
}
Super::EndPlay(EndPlayReason);
}
// =============================================================================
// ROS connection callback
// =============================================================================
void ARobotVisualizer::OnRosBridgeConnected()
{
bRosBridgeConnected = true;
UE_LOG(LogRosBridge, Log,
TEXT("RobotVisualizer: rosbridge connected — subscriptions restored by subsystem."));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Green,
TEXT("ROS Bridge: Connected"));
}
}
// =============================================================================
// ROS disconnection callback
// =============================================================================
void ARobotVisualizer::OnRosBridgeDisconnected()
{
bRosBridgeConnected = false;
WorkerState = TEXT("disconnected");
UE_LOG(LogRosBridge, Warning,
TEXT("RobotVisualizer: rosbridge DISCONNECTED — cannot send commands. Auto-reconnect active."));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 10.0f, FColor::Red,
TEXT("*** ROS Bridge DISCONNECTED *** Cannot send commands. Reconnecting..."));
}
}
// =============================================================================
// Connection health monitoring
// =============================================================================
void ARobotVisualizer::CheckConnectionHealth()
{
// Only check when WebSocket is connected — if WebSocket is down,
// OnRosBridgeDisconnected already shows a warning for that.
if (!bRosBridgeConnected)
{
return;
}
const double Now = FPlatformTime::Seconds();
// Check bridge heartbeat (published every 1s by bridge_node)
const double BridgeElapsed = Now - LastBridgeHeartbeatTime;
if (BridgeElapsed > BridgeHeartbeatTimeoutSec && !bBridgeHeartbeatLost)
{
bBridgeHeartbeatLost = true;
bWorkerDataLost = true; // if bridge is down, worker data can't reach us either
WorkerState = TEXT("bridge lost");
UE_LOG(LogRosBridge, Warning,
TEXT("Bridge heartbeat lost (%.1fs). bridge_node may be down."), BridgeElapsed);
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 10.0f, FColor::Red,
TEXT("*** Bridge Node: DOWN *** Check bridge_node terminal (terminal 2)."));
}
return; // no need to check worker separately
}
// Check worker data (/joint_states at ~30Hz, flows through bridge)
// Only meaningful if bridge is alive.
if (!bBridgeHeartbeatLost)
{
const double WorkerElapsed = Now - LastJointStatesTime;
if (WorkerElapsed > WorkerDataTimeoutSec && !bWorkerDataLost)
{
bWorkerDataLost = true;
WorkerState = TEXT("worker lost");
UE_LOG(LogRosBridge, Warning,
TEXT("Worker data lost (%.1fs). lerobot_worker may be down."), WorkerElapsed);
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 10.0f, FColor::Red,
TEXT("*** Worker: DOWN *** Check lerobot_worker terminal (terminal 1)."));
}
}
}
}
// =============================================================================
// MoveIt topic advertisements
// =============================================================================
void ARobotVisualizer::AdvertiseMoveItTopics()
{
UGameInstance* GI = UGameplayStatics::GetGameInstance(this);
if (!GI) return;
URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>();
if (!Ros) return;
// These are queued and sent automatically when connected.
Ros->Advertise(TEXT("/moveit_goal_named"), TEXT("std_msgs/String"));
Ros->Advertise(TEXT("/moveit_goal_joints"), TEXT("sensor_msgs/JointState"));
Ros->Advertise(TEXT("/moveit_goal_pose"), TEXT("geometry_msgs/PoseStamped"));
bMoveItTopicsAdvertised = true;
UE_LOG(LogRosBridge, Log, TEXT("RobotVisualizer: MoveIt command topics advertised."));
}
// =============================================================================
// MoveIt commands — SendNamedTarget
// =============================================================================
void ARobotVisualizer::SendNamedTarget()
{
UGameInstance* GI = UGameplayStatics::GetGameInstance(this);
if (!GI) return;
URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>();
if (!Ros || !Ros->IsConnected())
{
UE_LOG(LogRosBridge, Warning, TEXT("SendNamedTarget: not connected to rosbridge."));
return;
}
// std_msgs/String: {"data": "home"}
FString MsgJson = FString::Printf(TEXT("{\"data\":\"%s\"}"), *MoveItNamedTarget);
Ros->Publish(TEXT("/moveit_goal_named"), MsgJson);
UE_LOG(LogRosBridge, Log, TEXT("SendNamedTarget: published '%s' to /moveit_goal_named"), *MoveItNamedTarget);
}
// =============================================================================
// MoveIt commands — SendJointGoal
// =============================================================================
void ARobotVisualizer::SendJointGoal()
{
UGameInstance* GI = UGameplayStatics::GetGameInstance(this);
if (!GI) return;
URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>();
if (!Ros || !Ros->IsConnected())
{
UE_LOG(LogRosBridge, Warning, TEXT("SendJointGoal: not connected to rosbridge."));
return;
}
// sensor_msgs/JointState:
// {
// "header": {"stamp": {"sec": 0, "nanosec": 0}, "frame_id": ""},
// "name": ["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll"],
// "position": [0.0, -0.5, 0.5, 0.0, 0.0],
// "velocity": [],
// "effort": []
// }
FString MsgJson = FString::Printf(
TEXT("{\"header\":{\"stamp\":{\"sec\":0,\"nanosec\":0},\"frame_id\":\"\"},")
TEXT("\"name\":[\"shoulder_pan\",\"shoulder_lift\",\"elbow_flex\",\"wrist_flex\",\"wrist_roll\"],")
TEXT("\"position\":[%f,%f,%f,%f,%f],")
TEXT("\"velocity\":[],\"effort\":[]}"),
GoalShoulderPan, GoalShoulderLift, GoalElbowFlex, GoalWristFlex, GoalWristRoll
);
Ros->Publish(TEXT("/moveit_goal_joints"), MsgJson);
UE_LOG(LogRosBridge, Log,
TEXT("SendJointGoal: [%.3f, %.3f, %.3f, %.3f, %.3f] to /moveit_goal_joints"),
GoalShoulderPan, GoalShoulderLift, GoalElbowFlex, GoalWristFlex, GoalWristRoll);
}
// =============================================================================
// MoveIt commands — SendPoseGoal
// =============================================================================
void ARobotVisualizer::SendPoseGoal()
{
UGameInstance* GI = UGameplayStatics::GetGameInstance(this);
if (!GI) return;
URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>();
if (!Ros || !Ros->IsConnected())
{
UE_LOG(LogRosBridge, Warning, TEXT("SendPoseGoal: not connected to rosbridge."));
return;
}
// Convert UE position (cm, left-handed) to ROS (meters, right-handed)
double RosX, RosY, RosZ;
RosCoordConv::UeToRosPosition(GoalPositionUE, RosX, RosY, RosZ);
// geometry_msgs/PoseStamped (orientation defaults to identity — position-only goal)
FString MsgJson = FString::Printf(
TEXT("{\"header\":{\"stamp\":{\"sec\":0,\"nanosec\":0},\"frame_id\":\"base_link\"},")
TEXT("\"pose\":{\"position\":{\"x\":%f,\"y\":%f,\"z\":%f},")
TEXT("\"orientation\":{\"x\":0.0,\"y\":0.0,\"z\":0.0,\"w\":1.0}}}"),
RosX, RosY, RosZ
);
Ros->Publish(TEXT("/moveit_goal_pose"), MsgJson);
UE_LOG(LogRosBridge, Log,
TEXT("SendPoseGoal: UE(%.1f, %.1f, %.1f)cm -> ROS(%.4f, %.4f, %.4f)m to /moveit_goal_pose"),
GoalPositionUE.X, GoalPositionUE.Y, GoalPositionUE.Z,
RosX, RosY, RosZ);
}
// =============================================================================
// Message handling
// =============================================================================
void ARobotVisualizer::OnRosMessage(const FString& Topic, const FString& MessageJson)
{
if (Topic == JointStateTopic)
{
// /joint_states arrives at ~30Hz — proves both bridge AND worker are alive.
LastJointStatesTime = FPlatformTime::Seconds();
LastBridgeHeartbeatTime = LastJointStatesTime; // joint_states flows through bridge
if (bWorkerDataLost)
{
bWorkerDataLost = false;
UE_LOG(LogRosBridge, Log, TEXT("Worker data restored."));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Green,
TEXT("Worker: Connection restored"));
}
}
if (bBridgeHeartbeatLost)
{
bBridgeHeartbeatLost = false;
UE_LOG(LogRosBridge, Log, TEXT("Bridge heartbeat restored (via joint_states)."));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Green,
TEXT("Bridge: Connection restored"));
}
}
ParseAndApplyJointStates(MessageJson);
}
else if (Topic == TEXT("/bridge_heartbeat"))
{
// Bridge heartbeat arrives every 1s — proves bridge is alive
// (but worker may still be dead if /joint_states is not arriving).
LastBridgeHeartbeatTime = FPlatformTime::Seconds();
if (bBridgeHeartbeatLost)
{
bBridgeHeartbeatLost = false;
UE_LOG(LogRosBridge, Log, TEXT("Bridge heartbeat restored."));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Green,
TEXT("Bridge: Connection restored"));
}
}
}
else if (Topic == TEXT("/robot_status"))
{
OnRobotStatus(Topic, MessageJson);
}
}
void ARobotVisualizer::ParseAndApplyJointStates(const FString& MessageJson)
{
// Parse sensor_msgs/JointState JSON:
// { "name": ["shoulder_pan", ...], "position": [0.1, ...], ... }
TSharedPtr<FJsonObject> Json;
TSharedRef<TJsonReader<>> Reader = TJsonReaderFactory<>::Create(MessageJson);
if (!FJsonSerializer::Deserialize(Reader, Json) || !Json.IsValid())
{
return;
}
const TArray<TSharedPtr<FJsonValue>>* NameArray = nullptr;
const TArray<TSharedPtr<FJsonValue>>* PosArray = nullptr;
if (!Json->TryGetArrayField(TEXT("name"), NameArray) ||
!Json->TryGetArrayField(TEXT("position"), PosArray))
{
return;
}
const int32 Count = FMath::Min(NameArray->Num(), PosArray->Num());
for (int32 i = 0; i < Count; ++i)
{
const FName JointName(*(*NameArray)[i]->AsString());
const double AngleRad = (*PosArray)[i]->AsNumber();
TObjectPtr<USceneComponent>* JointComp = JointComponentMap.Find(JointName);
if (JointComp && *JointComp)
{
const float AngleDeg = RosCoordConv::RosJointAngleToUeDegrees(AngleRad);
FRotator BaseRot;
if (JointName == FName("shoulder_pan"))
BaseRot = RosCoordConv::RosRpyToUeRotator(3.14159, 0.0, -3.14159);
else if (JointName == FName("shoulder_lift"))
BaseRot = RosCoordConv::RosRpyToUeRotator(-1.5708, -1.5708, 0.0);
else if (JointName == FName("elbow_flex"))
BaseRot = RosCoordConv::RosRpyToUeRotator(0.0, 0.0, 1.5708);
else if (JointName == FName("wrist_flex"))
BaseRot = RosCoordConv::RosRpyToUeRotator(0.0, 0.0, -1.5708);
else if (JointName == FName("wrist_roll"))
BaseRot = RosCoordConv::RosRpyToUeRotator(1.5708, 0.0486795, 3.14159);
else if (JointName == FName("gripper"))
BaseRot = RosCoordConv::RosRpyToUeRotator(1.5708, 0.0, 0.0);
const FQuat BaseQuat = BaseRot.Quaternion();
const FQuat JointQuat = FQuat(FVector::UpVector, FMath::DegreesToRadians(AngleDeg));
const FQuat FinalQuat = BaseQuat * JointQuat;
(*JointComp)->SetRelativeRotation(FinalQuat.Rotator());
}
}
}
// =============================================================================
// Record / Replay / E-Stop — Topic setup
// =============================================================================
void ARobotVisualizer::SetupRecordReplayTopics()
{
UGameInstance* GI = UGameplayStatics::GetGameInstance(this);
if (!GI) return;
URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>();
if (!Ros) return;
// Advertise the command topic
Ros->Advertise(TEXT("/robot_command"), TEXT("std_msgs/String"));
// Subscribe to status feedback
Ros->Subscribe(TEXT("/robot_status"), TEXT("std_msgs/String"));
bRecordReplayTopicsSetup = true;
UE_LOG(LogRosBridge, Log, TEXT("RobotVisualizer: Record/Replay topics set up."));
}
// =============================================================================
// Record / Replay / E-Stop — Command publisher helper
// =============================================================================
void ARobotVisualizer::PublishRobotCommand(const FString& JsonCmd)
{
UGameInstance* GI = UGameplayStatics::GetGameInstance(this);
if (!GI) return;
URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>();
if (!Ros || !Ros->IsConnected())
{
UE_LOG(LogRosBridge, Warning, TEXT("PublishRobotCommand: not connected to rosbridge."));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 3.0f, FColor::Red,
TEXT("Robot: Not connected to rosbridge!"));
}
return;
}
// std_msgs/String: {"data": "<json_cmd>"}
// The JSON command is nested inside the "data" field, with quotes escaped.
FString EscapedCmd = JsonCmd.Replace(TEXT("\""), TEXT("\\\""));
FString MsgJson = FString::Printf(TEXT("{\"data\":\"%s\"}"), *EscapedCmd);
Ros->Publish(TEXT("/robot_command"), MsgJson);
UE_LOG(LogRosBridge, Log, TEXT("PublishRobotCommand: %s"), *JsonCmd);
}
// =============================================================================
// Record / Replay / E-Stop — Button handlers
// =============================================================================
void ARobotVisualizer::StartRecord()
{
PublishRobotCommand(TEXT("{\"cmd\":\"start_record\"}"));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 3.0f, FColor::Green,
TEXT("Robot: Recording started (teleop active)"));
}
}
void ARobotVisualizer::StopRecord()
{
PublishRobotCommand(TEXT("{\"cmd\":\"stop_record\"}"));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 3.0f, FColor::Yellow,
TEXT("Robot: Recording stopped, saving..."));
}
}
void ARobotVisualizer::StartReplay()
{
FString ArgsJson;
if (ReplayFilename.IsEmpty())
{
ArgsJson = FString::Printf(
TEXT("{\"cmd\":\"start_replay\",\"args\":{\"loop\":%s,\"approach_speed\":%f}}"),
bReplayLoop ? TEXT("true") : TEXT("false"),
ApproachSpeed);
}
else
{
ArgsJson = FString::Printf(
TEXT("{\"cmd\":\"start_replay\",\"args\":{\"filename\":\"%s\",\"loop\":%s,\"approach_speed\":%f}}"),
*ReplayFilename,
bReplayLoop ? TEXT("true") : TEXT("false"),
ApproachSpeed);
}
PublishRobotCommand(ArgsJson);
if (GEngine)
{
FString DisplayName = ReplayFilename.IsEmpty() ? TEXT("(most recent)") : ReplayFilename;
GEngine->AddOnScreenDebugMessage(-1, 3.0f, FColor::Cyan,
FString::Printf(TEXT("Robot: Replaying %s (loop=%s, speed=%.0f°/s)"),
*DisplayName, bReplayLoop ? TEXT("yes") : TEXT("no"), ApproachSpeed));
}
}
void ARobotVisualizer::StopReplay()
{
PublishRobotCommand(TEXT("{\"cmd\":\"stop_replay\"}"));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 3.0f, FColor::Yellow,
TEXT("Robot: Replay stopped"));
}
}
void ARobotVisualizer::EStop()
{
PublishRobotCommand(TEXT("{\"cmd\":\"estop\"}"));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Red,
TEXT("*** E-STOP *** All motion halted"));
}
}
// =============================================================================
// Teleop Sync — SyncOn / SyncOff
// =============================================================================
void ARobotVisualizer::SyncOn()
{
PublishRobotCommand(TEXT("{\"cmd\":\"start_teleop\"}"));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 3.0f, FColor::Green,
TEXT("Sync ON: leader -> follower active"));
}
}
void ARobotVisualizer::SyncOff()
{
PublishRobotCommand(TEXT("{\"cmd\":\"stop_teleop\"}"));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 3.0f, FColor::Yellow,
TEXT("Sync OFF: leader -> follower deactivated"));
}
}
// =============================================================================
// Record / Replay — Status feedback handler
// =============================================================================
void ARobotVisualizer::OnRobotStatus(const FString& Topic, const FString& MessageJson)
{
// /robot_status flows through bridge from worker — both are alive.
const double Now = FPlatformTime::Seconds();
LastBridgeHeartbeatTime = Now;
LastJointStatesTime = Now;
if (bBridgeHeartbeatLost)
{
bBridgeHeartbeatLost = false;
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Green,
TEXT("Bridge: Connection restored"));
}
}
if (bWorkerDataLost)
{
bWorkerDataLost = false;
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Green,
TEXT("Worker: Connection restored"));
}
}
// rosbridge wraps std_msgs/String as: {"data": "..."}
TSharedPtr<FJsonObject> OuterJson;
TSharedRef<TJsonReader<>> OuterReader = TJsonReaderFactory<>::Create(MessageJson);
if (!FJsonSerializer::Deserialize(OuterReader, OuterJson) || !OuterJson.IsValid())
{
return;
}
FString DataStr;
if (!OuterJson->TryGetStringField(TEXT("data"), DataStr))
{
return;
}
// Parse the inner JSON status
TSharedPtr<FJsonObject> StatusJson;
TSharedRef<TJsonReader<>> StatusReader = TJsonReaderFactory<>::Create(DataStr);
if (!FJsonSerializer::Deserialize(StatusReader, StatusJson) || !StatusJson.IsValid())
{
return;
}
FString State;
if (StatusJson->TryGetStringField(TEXT("state"), State))
{
if (State != WorkerState)
{
WorkerState = State;
UE_LOG(LogRosBridge, Log, TEXT("Worker state: %s"), *WorkerState);
if (GEngine)
{
FColor Color = FColor::White;
if (State == TEXT("recording")) Color = FColor::Green;
else if (State == TEXT("replaying")) Color = FColor::Cyan;
else if (State == TEXT("idle")) Color = FColor::Silver;
GEngine->AddOnScreenDebugMessage(-1, 3.0f, Color,
FString::Printf(TEXT("Robot state: %s"), *WorkerState));
}
}
}
// Update sync (teleop) status
bool bTeleop = false;
if (StatusJson->TryGetBoolField(TEXT("teleop"), bTeleop))
{
if (bTeleop != bSyncActive)
{
bSyncActive = bTeleop;
UE_LOG(LogRosBridge, Log, TEXT("Sync (teleop): %s"),
bSyncActive ? TEXT("ON") : TEXT("OFF"));
}
}
// Log errors from commands
FString Status;
if (StatusJson->TryGetStringField(TEXT("status"), Status) && Status == TEXT("error"))
{
FString Reason;
StatusJson->TryGetStringField(TEXT("reason"), Reason);
UE_LOG(LogRosBridge, Warning, TEXT("Robot command error: %s"), *Reason);
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Red,
FString::Printf(TEXT("Robot error: %s"), *Reason));
}
}
// Log recording saved info
FString Filename;
if (StatusJson->TryGetStringField(TEXT("filename"), Filename) && !Filename.IsEmpty())
{
int32 Frames = 0;
StatusJson->TryGetNumberField(TEXT("frames"), Frames);
double Duration = 0.0;
StatusJson->TryGetNumberField(TEXT("duration_sec"), Duration);
UE_LOG(LogRosBridge, Log, TEXT("Recording saved: %s (%d frames, %.1fs)"),
*Filename, Frames, Duration);
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Green,
FString::Printf(TEXT("Recording saved: %s (%d frames, %.1fs)"),
*Filename, Frames, Duration));
}
}
// --- Recordings list (reply to list_recordings) ---
const TArray<TSharedPtr<FJsonValue>>* RecArray = nullptr;
if (StatusJson->TryGetArrayField(TEXT("recordings"), RecArray) && RecArray)
{
Recordings.Reset();
for (const TSharedPtr<FJsonValue>& V : *RecArray)
{
const TSharedPtr<FJsonObject> RecObj = V->AsObject();
if (RecObj.IsValid())
{
FRecordingInfo Info;
RecObj->TryGetStringField(TEXT("filename"), Info.Filename);
RecObj->TryGetNumberField(TEXT("frames"), Info.Frames);
double Dur = 0.0;
RecObj->TryGetNumberField(TEXT("duration_sec"), Dur);
Info.DurationSec = static_cast<float>(Dur);
RecObj->TryGetStringField(TEXT("recorded_at"), Info.RecordedAt);
Recordings.Add(Info);
}
}
++RecordingsVersion;
UE_LOG(LogRosBridge, Log, TEXT("Recordings list updated: %d files"), Recordings.Num());
}
// --- Recordings list, per-item (split to avoid large-frame drops) ---
const TSharedPtr<FJsonObject>* RecItemObj = nullptr;
if (StatusJson->TryGetObjectField(TEXT("recording_item"), RecItemObj) && RecItemObj && RecItemObj->IsValid())
{
int32 RecIndex = 0, RecTotal = 0;
StatusJson->TryGetNumberField(TEXT("index"), RecIndex);
StatusJson->TryGetNumberField(TEXT("total"), RecTotal);
// Size the buffer to `total` and drop each item into its own slot.
// This is robust even if two list requests overlap (same list, so
// slots just get refilled) — no reset-in-the-middle corruption.
if (RecTotal > 0)
{
if (PendingRecordings.Num() != RecTotal)
{
PendingRecordings.Reset();
PendingRecordings.SetNum(RecTotal);
}
if (PendingRecordings.IsValidIndex(RecIndex))
{
FRecordingInfo Info;
(*RecItemObj)->TryGetStringField(TEXT("filename"), Info.Filename);
(*RecItemObj)->TryGetNumberField(TEXT("frames"), Info.Frames);
double Dur = 0.0;
(*RecItemObj)->TryGetNumberField(TEXT("duration_sec"), Dur);
Info.DurationSec = static_cast<float>(Dur);
(*RecItemObj)->TryGetStringField(TEXT("recorded_at"), Info.RecordedAt);
PendingRecordings[RecIndex] = Info;
}
// Complete only when every slot is filled (filename non-empty).
bool bComplete = true;
for (const FRecordingInfo& R : PendingRecordings)
{
if (R.Filename.IsEmpty()) { bComplete = false; break; }
}
if (bComplete)
{
Recordings = PendingRecordings;
Recordings.Sort([](const FRecordingInfo& A, const FRecordingInfo& B)
{
return A.Filename > B.Filename;
});
++RecordingsVersion;
}
}
}
// --- Recordings list cleared (empty result) ---
bool bRecClear = false;
if (StatusJson->TryGetBoolField(TEXT("recordings_clear"), bRecClear) && bRecClear)
{
Recordings.Reset();
PendingRecordings.Reset();
++RecordingsVersion;
UE_LOG(LogRosBridge, Log, TEXT("Recordings list cleared"));
}
// --- Replay progress ---
const TSharedPtr<FJsonObject>* ProgObj = nullptr;
if (StatusJson->TryGetObjectField(TEXT("replay_progress"), ProgObj) && ProgObj)
{
(*ProgObj)->TryGetNumberField(TEXT("index"), ReplayIndex);
(*ProgObj)->TryGetNumberField(TEXT("total"), ReplayTotal);
(*ProgObj)->TryGetStringField(TEXT("filename"), ReplayProgFilename);
(*ProgObj)->TryGetBoolField(TEXT("approaching"), bReplayApproaching);
}
// Display device-level errors (USB disconnection, serial errors)
const TSharedPtr<FJsonObject>* DeviceErrors = nullptr;
bool bHasDeviceErrors = StatusJson->TryGetObjectField(TEXT("device_errors"), DeviceErrors) && DeviceErrors;
// Check follower error
FString FollowerErr;
if (bHasDeviceErrors)
{
(*DeviceErrors)->TryGetStringField(TEXT("follower"), FollowerErr);
}
if (!FollowerErr.IsEmpty() && !bFollowerDeviceError)
{
bFollowerDeviceError = true;
UE_LOG(LogRosBridge, Error, TEXT("Follower USB error: %s"), *FollowerErr);
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 10.0f, FColor::Red,
TEXT("*** Follower: USB/Serial ERROR *** Check USB connection."));
}
}
else if (FollowerErr.IsEmpty() && bFollowerDeviceError)
{
bFollowerDeviceError = false;
UE_LOG(LogRosBridge, Log, TEXT("Follower USB restored."));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Green,
TEXT("Follower: USB connection restored"));
}
}
// Check leader error
FString LeaderErr;
if (bHasDeviceErrors)
{
(*DeviceErrors)->TryGetStringField(TEXT("leader"), LeaderErr);
}
if (!LeaderErr.IsEmpty() && !bLeaderDeviceError)
{
bLeaderDeviceError = true;
UE_LOG(LogRosBridge, Error, TEXT("Leader USB error: %s"), *LeaderErr);
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 10.0f, FColor::Red,
TEXT("*** Leader: USB/Serial ERROR *** Check USB connection."));
}
}
else if (LeaderErr.IsEmpty() && bLeaderDeviceError)
{
bLeaderDeviceError = false;
UE_LOG(LogRosBridge, Log, TEXT("Leader USB restored."));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Green,
TEXT("Leader: USB connection restored"));
}
}
}
이래도 잘 되지 않아서 또 수정... 리스트 요청이 계속 겹치는 것이 원인인 듯.
RobotVisualizer.cpp의 recording_item 블록을 이걸로 교체
요청이 이제 겹치지 않으니, 슬롯 방식 말고 index==0에서 비우고 순서대로 쌓는 단순 방식이 제일 안전
// --- Recordings list, per-item (split to avoid large-frame drops) ---
const TSharedPtr<FJsonObject>* RecItemObj = nullptr;
if (StatusJson->TryGetObjectField(TEXT("recording_item"), RecItemObj) && RecItemObj && RecItemObj->IsValid())
{
int32 RecIndex = 0, RecTotal = 0;
StatusJson->TryGetNumberField(TEXT("index"), RecIndex);
StatusJson->TryGetNumberField(TEXT("total"), RecTotal);
if (RecIndex == 0) { PendingRecordings.Reset(); }
FRecordingInfo Info;
(*RecItemObj)->TryGetStringField(TEXT("filename"), Info.Filename);
(*RecItemObj)->TryGetNumberField(TEXT("frames"), Info.Frames);
double Dur = 0.0;
(*RecItemObj)->TryGetNumberField(TEXT("duration_sec"), Dur);
Info.DurationSec = static_cast<float>(Dur);
(*RecItemObj)->TryGetStringField(TEXT("recorded_at"), Info.RecordedAt);
PendingRecordings.Add(Info);
if (RecTotal > 0 && PendingRecordings.Num() >= RecTotal)
{
Recordings = PendingRecordings;
Recordings.Sort([](const FRecordingInfo& A, const FRecordingInfo& B)
{
return A.Filename > B.Filename; // newest first (top)
});
PendingRecordings.Reset();
++RecordingsVersion;
UE_LOG(LogRosBridge, Log, TEXT("Recordings list updated: %d files"), Recordings.Num());
}
}
RobotVisualizer.cpp 전체 코드
더보기
#include "RobotVisualizer.h"
#include "RosCoordConv.h"
#include "RosBridgeSubsystem.h"
#include "RosBridgeLog.h"
#include "Components/SceneComponent.h"
#include "Components/StaticMeshComponent.h"
#include "Engine/StaticMesh.h"
#include "Engine/Engine.h"
#include "Engine/World.h"
#include "Kismet/GameplayStatics.h"
#include "Dom/JsonObject.h"
#include "Dom/JsonValue.h"
#include "Serialization/JsonReader.h"
#include "Serialization/JsonSerializer.h"
#include "Serialization/JsonWriter.h"
#include "UObject/ConstructorHelpers.h"
#include "GameFramework/PlayerController.h"
#include "Blueprint/UserWidget.h"
#include "RobotControlWidget.h"
// =============================================================================
// Mesh asset path helper
// =============================================================================
static UStaticMesh* LoadMeshAsset(const TCHAR* AssetName)
{
// All meshes live under /Game/Robot/Meshes/
FString Path = FString::Printf(TEXT("/Game/Robot/Meshes/%s.%s"), AssetName, AssetName);
UStaticMesh* Mesh = Cast<UStaticMesh>(StaticLoadObject(UStaticMesh::StaticClass(), nullptr, *Path));
if (!Mesh)
{
UE_LOG(LogRosBridge, Warning, TEXT("Failed to load mesh: %s"), *Path);
}
return Mesh;
}
// =============================================================================
// Constructor — build the entire component hierarchy
// =============================================================================
ARobotVisualizer::ARobotVisualizer()
{
PrimaryActorTick.bCanEverTick = false;
// --- Root ---
RobotRoot = CreateDefaultSubobject<USceneComponent>(TEXT("RobotRoot"));
RootComponent = RobotRoot;
// =========================================================================
// URDF data converted to UE coordinates:
// Position: meters * 100 = cm, Y flipped
// Rotation: RPY radians -> FRotator degrees, pitch & yaw negated
//
// All values below are pre-computed from so101_follower.urdf.
// =========================================================================
// --- base_link (attached directly to root, no joint) ---
BaseLink = CreateDefaultSubobject<USceneComponent>(TEXT("BaseLink"));
BaseLink->SetupAttachment(RobotRoot);
// --- shoulder_pan joint ---
// URDF origin: xyz(0.0388353, 0, 0.0624) rpy(3.14159, 0, -3.14159)
ShoulderPanJoint = CreateDefaultSubobject<USceneComponent>(TEXT("ShoulderPanJoint"));
ShoulderPanJoint->SetupAttachment(BaseLink);
ShoulderPanJoint->SetRelativeLocation(RosCoordConv::RosToUePosition(0.0388353, 0.0, 0.0624));
ShoulderPanJoint->SetRelativeRotation(RosCoordConv::RosRpyToUeRotator(3.14159, 0.0, -3.14159));
ShoulderLink = CreateDefaultSubobject<USceneComponent>(TEXT("ShoulderLink"));
ShoulderLink->SetupAttachment(ShoulderPanJoint);
// --- shoulder_lift joint ---
// URDF origin: xyz(-0.0303992, -0.0182778, -0.0542) rpy(-1.5708, -1.5708, 0)
ShoulderLiftJoint = CreateDefaultSubobject<USceneComponent>(TEXT("ShoulderLiftJoint"));
ShoulderLiftJoint->SetupAttachment(ShoulderLink);
ShoulderLiftJoint->SetRelativeLocation(RosCoordConv::RosToUePosition(-0.0303992, -0.0182778, -0.0542));
ShoulderLiftJoint->SetRelativeRotation(RosCoordConv::RosRpyToUeRotator(-1.5708, -1.5708, 0.0));
UpperArmLink = CreateDefaultSubobject<USceneComponent>(TEXT("UpperArmLink"));
UpperArmLink->SetupAttachment(ShoulderLiftJoint);
// --- elbow_flex joint ---
// URDF origin: xyz(-0.11257, -0.028, 0) rpy(0, 0, 1.5708)
ElbowFlexJoint = CreateDefaultSubobject<USceneComponent>(TEXT("ElbowFlexJoint"));
ElbowFlexJoint->SetupAttachment(UpperArmLink);
ElbowFlexJoint->SetRelativeLocation(RosCoordConv::RosToUePosition(-0.11257, -0.028, 0.0));
ElbowFlexJoint->SetRelativeRotation(RosCoordConv::RosRpyToUeRotator(0.0, 0.0, 1.5708));
LowerArmLink = CreateDefaultSubobject<USceneComponent>(TEXT("LowerArmLink"));
LowerArmLink->SetupAttachment(ElbowFlexJoint);
// --- wrist_flex joint ---
// URDF origin: xyz(-0.1349, 0.0052, 0) rpy(0, 0, -1.5708)
WristFlexJoint = CreateDefaultSubobject<USceneComponent>(TEXT("WristFlexJoint"));
WristFlexJoint->SetupAttachment(LowerArmLink);
WristFlexJoint->SetRelativeLocation(RosCoordConv::RosToUePosition(-0.1349, 0.0052, 0.0));
WristFlexJoint->SetRelativeRotation(RosCoordConv::RosRpyToUeRotator(0.0, 0.0, -1.5708));
WristLink = CreateDefaultSubobject<USceneComponent>(TEXT("WristLink"));
WristLink->SetupAttachment(WristFlexJoint);
// --- wrist_roll joint ---
// URDF origin: xyz(0, -0.0611, 0.0181) rpy(1.5708, 0.0486795, 3.14159)
WristRollJoint = CreateDefaultSubobject<USceneComponent>(TEXT("WristRollJoint"));
WristRollJoint->SetupAttachment(WristLink);
WristRollJoint->SetRelativeLocation(RosCoordConv::RosToUePosition(0.0, -0.0611, 0.0181));
WristRollJoint->SetRelativeRotation(RosCoordConv::RosRpyToUeRotator(1.5708, 0.0486795, 3.14159));
GripperLink = CreateDefaultSubobject<USceneComponent>(TEXT("GripperLink"));
GripperLink->SetupAttachment(WristRollJoint);
// --- gripper joint ---
// URDF origin: xyz(0.0202, 0.0188, -0.0234) rpy(1.5708, 0, 0)
GripperJoint = CreateDefaultSubobject<USceneComponent>(TEXT("GripperJoint"));
GripperJoint->SetupAttachment(GripperLink);
GripperJoint->SetRelativeLocation(RosCoordConv::RosToUePosition(0.0202, 0.0188, -0.0234));
GripperJoint->SetRelativeRotation(RosCoordConv::RosRpyToUeRotator(1.5708, 0.0, 0.0));
MovingJawLink = CreateDefaultSubobject<USceneComponent>(TEXT("MovingJawLink"));
MovingJawLink->SetupAttachment(GripperJoint);
// --- Joint name mapping (matches ROS /joint_states names) ---
JointComponentMap.Add(FName("shoulder_pan"), ShoulderPanJoint);
JointComponentMap.Add(FName("shoulder_lift"), ShoulderLiftJoint);
JointComponentMap.Add(FName("elbow_flex"), ElbowFlexJoint);
JointComponentMap.Add(FName("wrist_flex"), WristFlexJoint);
JointComponentMap.Add(FName("wrist_roll"), WristRollJoint);
JointComponentMap.Add(FName("gripper"), GripperJoint);
}
// =============================================================================
// BeginPlay — load meshes and attach, connect to ROS
// =============================================================================
void ARobotVisualizer::BeginPlay()
{
Super::BeginPlay();
// --- Load meshes and attach to links ---
// Meshes are loaded at runtime (not in constructor) because
// StaticLoadObject is safer to call here and allows hot-reload.
// Helper lambda to reduce repetition
auto Attach = [this](USceneComponent* Parent, const TCHAR* MeshName,
double RosX, double RosY, double RosZ,
double RosRoll, double RosPitch, double RosYaw,
bool bIsMotor = false)
{
UStaticMesh* Mesh = LoadMeshAsset(MeshName);
if (!Mesh) return;
UStaticMeshComponent* SMC = NewObject<UStaticMeshComponent>(this);
SMC->SetStaticMesh(Mesh);
SMC->SetRelativeLocation(RosCoordConv::RosToUePosition(RosX, RosY, RosZ));
SMC->SetRelativeRotation(RosCoordConv::RosRpyToUeRotator(RosRoll, RosPitch, RosYaw));
SMC->SetCollisionEnabled(ECollisionEnabled::NoCollision);
SMC->AttachToComponent(Parent, FAttachmentTransformRules::KeepRelativeTransform);
SMC->RegisterComponent();
AllMeshComponents.Add(SMC);
};
// === base_link meshes ===
Attach(BaseLink, TEXT("base_motor_holder_so101_v1"),
-0.00636471, -0.0000994414, -0.0024,
1.5708, 0.0, 1.5708);
Attach(BaseLink, TEXT("base_so101_v2"),
-0.00636471, 0.0, -0.0024,
1.5708, 0.0, 1.5708);
Attach(BaseLink, TEXT("sts3215_03a_v1"),
0.0263353, 0.0, 0.0437,
0.0, 0.0, 0.0, true);
Attach(BaseLink, TEXT("waveshare_mounting_plate_so101_v2"),
-0.0309827, -0.000199441, 0.0474,
1.5708, 0.0, 1.5708);
// === shoulder_link meshes ===
Attach(ShoulderLink, TEXT("sts3215_03a_v1"),
-0.0303992, 0.000422241, -0.0417,
1.5708, 1.5708, 0.0, true);
Attach(ShoulderLink, TEXT("motor_holder_so101_base_v1"),
-0.0675992, -0.000177759, 0.0158499,
1.5708, -1.5708, 0.0);
Attach(ShoulderLink, TEXT("rotation_pitch_so101_v1"),
0.0122008, 0.0000222413, 0.0464,
-1.5708, 0.0, 0.0);
// === upper_arm_link meshes ===
Attach(UpperArmLink, TEXT("sts3215_03a_v1"),
-0.11257, -0.0155, 0.0187,
-3.14159, 0.0, -1.5708, true);
Attach(UpperArmLink, TEXT("upper_arm_so101_v1"),
-0.065085, 0.012, 0.0182,
3.14159, 0.0, 0.0);
// === lower_arm_link meshes ===
Attach(LowerArmLink, TEXT("under_arm_so101_v1"),
-0.0648499, -0.032, 0.0182,
3.14159, 0.0, 0.0);
Attach(LowerArmLink, TEXT("motor_holder_so101_wrist_v1"),
-0.0648499, -0.032, 0.018,
-3.14159, 0.0, 0.0);
Attach(LowerArmLink, TEXT("sts3215_03a_v1"),
-0.1224, 0.0052, 0.0187,
-3.14159, 0.0, -3.14159, true);
// === wrist_link meshes ===
Attach(WristLink, TEXT("sts3215_03a_no_horn_v1"),
0.0, -0.0424, 0.0306,
1.5708, 1.5708, 0.0, true);
Attach(WristLink, TEXT("wrist_roll_pitch_so101_v2"),
0.0, -0.028, 0.0181,
-1.5708, -1.5708, 0.0);
// === gripper_link meshes ===
Attach(GripperLink, TEXT("sts3215_03a_v1"),
0.0077, 0.0001, -0.0234,
-1.5708, 0.0, 0.0, true);
Attach(GripperLink, TEXT("wrist_roll_follower_so101_v1"),
0.0, -0.000218214, 0.000949706,
-3.14159, 0.0, 0.0);
// === moving_jaw_link meshes ===
Attach(MovingJawLink, TEXT("moving_jaw_so101_v1"),
0.0, 0.0, 0.0189,
0.0, 0.0, 0.0);
UE_LOG(LogRosBridge, Log, TEXT("RobotVisualizer: %d mesh components created"), AllMeshComponents.Num());
// --- Connect to ROS via Subsystem ---
UGameInstance* GI = UGameplayStatics::GetGameInstance(this);
if (!GI) return;
URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>();
if (!Ros) return;
// Bind delegates.
Ros->OnTopicMessage.AddDynamic(this, &ARobotVisualizer::OnRosMessage);
Ros->OnConnected.AddDynamic(this, &ARobotVisualizer::OnRosBridgeConnected);
Ros->OnDisconnected.AddDynamic(this, &ARobotVisualizer::OnRosBridgeDisconnected);
// Subscribe is now queued even before connection — the subsystem will
// send it automatically when connected (including on reconnect).
Ros->Subscribe(JointStateTopic, JointStateType);
// Queue MoveIt topic advertisements (sent on connect).
AdvertiseMoveItTopics();
// Queue record/replay/estop topics.
SetupRecordReplayTopics();
// Subscribe to bridge heartbeat for connection health monitoring.
Ros->Subscribe(TEXT("/bridge_heartbeat"), TEXT("std_msgs/String"));
// Start connection health monitor (checks every 2 seconds).
const double Now = FPlatformTime::Seconds();
LastBridgeHeartbeatTime = Now;
LastJointStatesTime = Now;
GetWorldTimerManager().SetTimer(
ConnectionHealthTimerHandle, this,
&ARobotVisualizer::CheckConnectionHealth, 2.0f, true);
// Initiate connection if not already connected.
if (!Ros->IsConnected())
{
Ros->Connect(RosBridgeUrl);
}
else
{
// Already connected (e.g. another actor already called Connect).
UE_LOG(LogRosBridge, Log, TEXT("RobotVisualizer: already connected, subscription sent."));
}
// --- Spawn the in-viewport control UI (Phase 10) ---
if (ControlWidgetClass)
{
if (APlayerController* PC = UGameplayStatics::GetPlayerController(this, 0))
{
ControlWidget = CreateWidget<URobotControlWidget>(PC, ControlWidgetClass);
if (ControlWidget)
{
ControlWidget->AddToViewport();
UE_LOG(LogRosBridge, Log, TEXT("RobotVisualizer: control widget added to viewport."));
}
}
}
}
// =============================================================================
// EndPlay
// =============================================================================
void ARobotVisualizer::EndPlay(const EEndPlayReason::Type EndPlayReason)
{
if (UGameInstance* GI = UGameplayStatics::GetGameInstance(this))
{
if (URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>())
{
Ros->OnTopicMessage.RemoveDynamic(this, &ARobotVisualizer::OnRosMessage);
Ros->OnConnected.RemoveDynamic(this, &ARobotVisualizer::OnRosBridgeConnected);
Ros->OnDisconnected.RemoveDynamic(this, &ARobotVisualizer::OnRosBridgeDisconnected);
}
}
bMoveItTopicsAdvertised = false;
bRecordReplayTopicsSetup = false;
GetWorldTimerManager().ClearTimer(ConnectionHealthTimerHandle);
if (ControlWidget)
{
ControlWidget->RemoveFromParent();
ControlWidget = nullptr;
}
Super::EndPlay(EndPlayReason);
}
// =============================================================================
// ROS connection callback
// =============================================================================
void ARobotVisualizer::OnRosBridgeConnected()
{
bRosBridgeConnected = true;
UE_LOG(LogRosBridge, Log,
TEXT("RobotVisualizer: rosbridge connected — subscriptions restored by subsystem."));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Green,
TEXT("ROS Bridge: Connected"));
}
}
// =============================================================================
// ROS disconnection callback
// =============================================================================
void ARobotVisualizer::OnRosBridgeDisconnected()
{
bRosBridgeConnected = false;
WorkerState = TEXT("disconnected");
UE_LOG(LogRosBridge, Warning,
TEXT("RobotVisualizer: rosbridge DISCONNECTED — cannot send commands. Auto-reconnect active."));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 10.0f, FColor::Red,
TEXT("*** ROS Bridge DISCONNECTED *** Cannot send commands. Reconnecting..."));
}
}
// =============================================================================
// Connection health monitoring
// =============================================================================
void ARobotVisualizer::CheckConnectionHealth()
{
// Only check when WebSocket is connected — if WebSocket is down,
// OnRosBridgeDisconnected already shows a warning for that.
if (!bRosBridgeConnected)
{
return;
}
const double Now = FPlatformTime::Seconds();
// Check bridge heartbeat (published every 1s by bridge_node)
const double BridgeElapsed = Now - LastBridgeHeartbeatTime;
if (BridgeElapsed > BridgeHeartbeatTimeoutSec && !bBridgeHeartbeatLost)
{
bBridgeHeartbeatLost = true;
bWorkerDataLost = true; // if bridge is down, worker data can't reach us either
WorkerState = TEXT("bridge lost");
UE_LOG(LogRosBridge, Warning,
TEXT("Bridge heartbeat lost (%.1fs). bridge_node may be down."), BridgeElapsed);
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 10.0f, FColor::Red,
TEXT("*** Bridge Node: DOWN *** Check bridge_node terminal (terminal 2)."));
}
return; // no need to check worker separately
}
// Check worker data (/joint_states at ~30Hz, flows through bridge)
// Only meaningful if bridge is alive.
if (!bBridgeHeartbeatLost)
{
const double WorkerElapsed = Now - LastJointStatesTime;
if (WorkerElapsed > WorkerDataTimeoutSec && !bWorkerDataLost)
{
bWorkerDataLost = true;
WorkerState = TEXT("worker lost");
UE_LOG(LogRosBridge, Warning,
TEXT("Worker data lost (%.1fs). lerobot_worker may be down."), WorkerElapsed);
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 10.0f, FColor::Red,
TEXT("*** Worker: DOWN *** Check lerobot_worker terminal (terminal 1)."));
}
}
}
}
// =============================================================================
// MoveIt topic advertisements
// =============================================================================
void ARobotVisualizer::AdvertiseMoveItTopics()
{
UGameInstance* GI = UGameplayStatics::GetGameInstance(this);
if (!GI) return;
URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>();
if (!Ros) return;
// These are queued and sent automatically when connected.
Ros->Advertise(TEXT("/moveit_goal_named"), TEXT("std_msgs/String"));
Ros->Advertise(TEXT("/moveit_goal_joints"), TEXT("sensor_msgs/JointState"));
Ros->Advertise(TEXT("/moveit_goal_pose"), TEXT("geometry_msgs/PoseStamped"));
bMoveItTopicsAdvertised = true;
UE_LOG(LogRosBridge, Log, TEXT("RobotVisualizer: MoveIt command topics advertised."));
}
// =============================================================================
// MoveIt commands — SendNamedTarget
// =============================================================================
void ARobotVisualizer::SendNamedTarget()
{
UGameInstance* GI = UGameplayStatics::GetGameInstance(this);
if (!GI) return;
URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>();
if (!Ros || !Ros->IsConnected())
{
UE_LOG(LogRosBridge, Warning, TEXT("SendNamedTarget: not connected to rosbridge."));
return;
}
// std_msgs/String: {"data": "home"}
FString MsgJson = FString::Printf(TEXT("{\"data\":\"%s\"}"), *MoveItNamedTarget);
Ros->Publish(TEXT("/moveit_goal_named"), MsgJson);
UE_LOG(LogRosBridge, Log, TEXT("SendNamedTarget: published '%s' to /moveit_goal_named"), *MoveItNamedTarget);
}
// =============================================================================
// MoveIt commands — SendJointGoal
// =============================================================================
void ARobotVisualizer::SendJointGoal()
{
UGameInstance* GI = UGameplayStatics::GetGameInstance(this);
if (!GI) return;
URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>();
if (!Ros || !Ros->IsConnected())
{
UE_LOG(LogRosBridge, Warning, TEXT("SendJointGoal: not connected to rosbridge."));
return;
}
// sensor_msgs/JointState:
// {
// "header": {"stamp": {"sec": 0, "nanosec": 0}, "frame_id": ""},
// "name": ["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll"],
// "position": [0.0, -0.5, 0.5, 0.0, 0.0],
// "velocity": [],
// "effort": []
// }
FString MsgJson = FString::Printf(
TEXT("{\"header\":{\"stamp\":{\"sec\":0,\"nanosec\":0},\"frame_id\":\"\"},")
TEXT("\"name\":[\"shoulder_pan\",\"shoulder_lift\",\"elbow_flex\",\"wrist_flex\",\"wrist_roll\"],")
TEXT("\"position\":[%f,%f,%f,%f,%f],")
TEXT("\"velocity\":[],\"effort\":[]}"),
GoalShoulderPan, GoalShoulderLift, GoalElbowFlex, GoalWristFlex, GoalWristRoll
);
Ros->Publish(TEXT("/moveit_goal_joints"), MsgJson);
UE_LOG(LogRosBridge, Log,
TEXT("SendJointGoal: [%.3f, %.3f, %.3f, %.3f, %.3f] to /moveit_goal_joints"),
GoalShoulderPan, GoalShoulderLift, GoalElbowFlex, GoalWristFlex, GoalWristRoll);
}
// =============================================================================
// MoveIt commands — SendPoseGoal
// =============================================================================
void ARobotVisualizer::SendPoseGoal()
{
UGameInstance* GI = UGameplayStatics::GetGameInstance(this);
if (!GI) return;
URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>();
if (!Ros || !Ros->IsConnected())
{
UE_LOG(LogRosBridge, Warning, TEXT("SendPoseGoal: not connected to rosbridge."));
return;
}
// Convert UE position (cm, left-handed) to ROS (meters, right-handed)
double RosX, RosY, RosZ;
RosCoordConv::UeToRosPosition(GoalPositionUE, RosX, RosY, RosZ);
// geometry_msgs/PoseStamped (orientation defaults to identity — position-only goal)
FString MsgJson = FString::Printf(
TEXT("{\"header\":{\"stamp\":{\"sec\":0,\"nanosec\":0},\"frame_id\":\"base_link\"},")
TEXT("\"pose\":{\"position\":{\"x\":%f,\"y\":%f,\"z\":%f},")
TEXT("\"orientation\":{\"x\":0.0,\"y\":0.0,\"z\":0.0,\"w\":1.0}}}"),
RosX, RosY, RosZ
);
Ros->Publish(TEXT("/moveit_goal_pose"), MsgJson);
UE_LOG(LogRosBridge, Log,
TEXT("SendPoseGoal: UE(%.1f, %.1f, %.1f)cm -> ROS(%.4f, %.4f, %.4f)m to /moveit_goal_pose"),
GoalPositionUE.X, GoalPositionUE.Y, GoalPositionUE.Z,
RosX, RosY, RosZ);
}
// =============================================================================
// Message handling
// =============================================================================
void ARobotVisualizer::OnRosMessage(const FString& Topic, const FString& MessageJson)
{
if (Topic == JointStateTopic)
{
// /joint_states arrives at ~30Hz — proves both bridge AND worker are alive.
LastJointStatesTime = FPlatformTime::Seconds();
LastBridgeHeartbeatTime = LastJointStatesTime; // joint_states flows through bridge
if (bWorkerDataLost)
{
bWorkerDataLost = false;
UE_LOG(LogRosBridge, Log, TEXT("Worker data restored."));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Green,
TEXT("Worker: Connection restored"));
}
}
if (bBridgeHeartbeatLost)
{
bBridgeHeartbeatLost = false;
UE_LOG(LogRosBridge, Log, TEXT("Bridge heartbeat restored (via joint_states)."));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Green,
TEXT("Bridge: Connection restored"));
}
}
ParseAndApplyJointStates(MessageJson);
}
else if (Topic == TEXT("/bridge_heartbeat"))
{
// Bridge heartbeat arrives every 1s — proves bridge is alive
// (but worker may still be dead if /joint_states is not arriving).
LastBridgeHeartbeatTime = FPlatformTime::Seconds();
if (bBridgeHeartbeatLost)
{
bBridgeHeartbeatLost = false;
UE_LOG(LogRosBridge, Log, TEXT("Bridge heartbeat restored."));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Green,
TEXT("Bridge: Connection restored"));
}
}
}
else if (Topic == TEXT("/robot_status"))
{
OnRobotStatus(Topic, MessageJson);
}
}
void ARobotVisualizer::ParseAndApplyJointStates(const FString& MessageJson)
{
// Parse sensor_msgs/JointState JSON:
// { "name": ["shoulder_pan", ...], "position": [0.1, ...], ... }
TSharedPtr<FJsonObject> Json;
TSharedRef<TJsonReader<>> Reader = TJsonReaderFactory<>::Create(MessageJson);
if (!FJsonSerializer::Deserialize(Reader, Json) || !Json.IsValid())
{
return;
}
const TArray<TSharedPtr<FJsonValue>>* NameArray = nullptr;
const TArray<TSharedPtr<FJsonValue>>* PosArray = nullptr;
if (!Json->TryGetArrayField(TEXT("name"), NameArray) ||
!Json->TryGetArrayField(TEXT("position"), PosArray))
{
return;
}
const int32 Count = FMath::Min(NameArray->Num(), PosArray->Num());
for (int32 i = 0; i < Count; ++i)
{
const FName JointName(*(*NameArray)[i]->AsString());
const double AngleRad = (*PosArray)[i]->AsNumber();
TObjectPtr<USceneComponent>* JointComp = JointComponentMap.Find(JointName);
if (JointComp && *JointComp)
{
const float AngleDeg = RosCoordConv::RosJointAngleToUeDegrees(AngleRad);
FRotator BaseRot;
if (JointName == FName("shoulder_pan"))
BaseRot = RosCoordConv::RosRpyToUeRotator(3.14159, 0.0, -3.14159);
else if (JointName == FName("shoulder_lift"))
BaseRot = RosCoordConv::RosRpyToUeRotator(-1.5708, -1.5708, 0.0);
else if (JointName == FName("elbow_flex"))
BaseRot = RosCoordConv::RosRpyToUeRotator(0.0, 0.0, 1.5708);
else if (JointName == FName("wrist_flex"))
BaseRot = RosCoordConv::RosRpyToUeRotator(0.0, 0.0, -1.5708);
else if (JointName == FName("wrist_roll"))
BaseRot = RosCoordConv::RosRpyToUeRotator(1.5708, 0.0486795, 3.14159);
else if (JointName == FName("gripper"))
BaseRot = RosCoordConv::RosRpyToUeRotator(1.5708, 0.0, 0.0);
const FQuat BaseQuat = BaseRot.Quaternion();
const FQuat JointQuat = FQuat(FVector::UpVector, FMath::DegreesToRadians(AngleDeg));
const FQuat FinalQuat = BaseQuat * JointQuat;
(*JointComp)->SetRelativeRotation(FinalQuat.Rotator());
}
}
}
// =============================================================================
// Record / Replay / E-Stop — Topic setup
// =============================================================================
void ARobotVisualizer::SetupRecordReplayTopics()
{
UGameInstance* GI = UGameplayStatics::GetGameInstance(this);
if (!GI) return;
URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>();
if (!Ros) return;
// Advertise the command topic
Ros->Advertise(TEXT("/robot_command"), TEXT("std_msgs/String"));
// Subscribe to status feedback
Ros->Subscribe(TEXT("/robot_status"), TEXT("std_msgs/String"));
bRecordReplayTopicsSetup = true;
UE_LOG(LogRosBridge, Log, TEXT("RobotVisualizer: Record/Replay topics set up."));
}
// =============================================================================
// Record / Replay / E-Stop — Command publisher helper
// =============================================================================
void ARobotVisualizer::PublishRobotCommand(const FString& JsonCmd)
{
UGameInstance* GI = UGameplayStatics::GetGameInstance(this);
if (!GI) return;
URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>();
if (!Ros || !Ros->IsConnected())
{
UE_LOG(LogRosBridge, Warning, TEXT("PublishRobotCommand: not connected to rosbridge."));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 3.0f, FColor::Red,
TEXT("Robot: Not connected to rosbridge!"));
}
return;
}
// std_msgs/String: {"data": "<json_cmd>"}
// The JSON command is nested inside the "data" field, with quotes escaped.
FString EscapedCmd = JsonCmd.Replace(TEXT("\""), TEXT("\\\""));
FString MsgJson = FString::Printf(TEXT("{\"data\":\"%s\"}"), *EscapedCmd);
Ros->Publish(TEXT("/robot_command"), MsgJson);
UE_LOG(LogRosBridge, Log, TEXT("PublishRobotCommand: %s"), *JsonCmd);
}
// =============================================================================
// Record / Replay / E-Stop — Button handlers
// =============================================================================
void ARobotVisualizer::StartRecord()
{
PublishRobotCommand(TEXT("{\"cmd\":\"start_record\"}"));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 3.0f, FColor::Green,
TEXT("Robot: Recording started (teleop active)"));
}
}
void ARobotVisualizer::StopRecord()
{
PublishRobotCommand(TEXT("{\"cmd\":\"stop_record\"}"));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 3.0f, FColor::Yellow,
TEXT("Robot: Recording stopped, saving..."));
}
}
void ARobotVisualizer::StartReplay()
{
FString ArgsJson;
if (ReplayFilename.IsEmpty())
{
ArgsJson = FString::Printf(
TEXT("{\"cmd\":\"start_replay\",\"args\":{\"loop\":%s,\"approach_speed\":%f}}"),
bReplayLoop ? TEXT("true") : TEXT("false"),
ApproachSpeed);
}
else
{
ArgsJson = FString::Printf(
TEXT("{\"cmd\":\"start_replay\",\"args\":{\"filename\":\"%s\",\"loop\":%s,\"approach_speed\":%f}}"),
*ReplayFilename,
bReplayLoop ? TEXT("true") : TEXT("false"),
ApproachSpeed);
}
PublishRobotCommand(ArgsJson);
if (GEngine)
{
FString DisplayName = ReplayFilename.IsEmpty() ? TEXT("(most recent)") : ReplayFilename;
GEngine->AddOnScreenDebugMessage(-1, 3.0f, FColor::Cyan,
FString::Printf(TEXT("Robot: Replaying %s (loop=%s, speed=%.0f°/s)"),
*DisplayName, bReplayLoop ? TEXT("yes") : TEXT("no"), ApproachSpeed));
}
}
void ARobotVisualizer::StopReplay()
{
PublishRobotCommand(TEXT("{\"cmd\":\"stop_replay\"}"));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 3.0f, FColor::Yellow,
TEXT("Robot: Replay stopped"));
}
}
void ARobotVisualizer::EStop()
{
PublishRobotCommand(TEXT("{\"cmd\":\"estop\"}"));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Red,
TEXT("*** E-STOP *** All motion halted"));
}
}
// =============================================================================
// Teleop Sync — SyncOn / SyncOff
// =============================================================================
void ARobotVisualizer::SyncOn()
{
PublishRobotCommand(TEXT("{\"cmd\":\"start_teleop\"}"));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 3.0f, FColor::Green,
TEXT("Sync ON: leader -> follower active"));
}
}
void ARobotVisualizer::SyncOff()
{
PublishRobotCommand(TEXT("{\"cmd\":\"stop_teleop\"}"));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 3.0f, FColor::Yellow,
TEXT("Sync OFF: leader -> follower deactivated"));
}
}
// =============================================================================
// Record / Replay — Status feedback handler
// =============================================================================
void ARobotVisualizer::OnRobotStatus(const FString& Topic, const FString& MessageJson)
{
// /robot_status flows through bridge from worker — both are alive.
const double Now = FPlatformTime::Seconds();
LastBridgeHeartbeatTime = Now;
LastJointStatesTime = Now;
if (bBridgeHeartbeatLost)
{
bBridgeHeartbeatLost = false;
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Green,
TEXT("Bridge: Connection restored"));
}
}
if (bWorkerDataLost)
{
bWorkerDataLost = false;
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Green,
TEXT("Worker: Connection restored"));
}
}
// rosbridge wraps std_msgs/String as: {"data": "..."}
TSharedPtr<FJsonObject> OuterJson;
TSharedRef<TJsonReader<>> OuterReader = TJsonReaderFactory<>::Create(MessageJson);
if (!FJsonSerializer::Deserialize(OuterReader, OuterJson) || !OuterJson.IsValid())
{
return;
}
FString DataStr;
if (!OuterJson->TryGetStringField(TEXT("data"), DataStr))
{
return;
}
// Parse the inner JSON status
TSharedPtr<FJsonObject> StatusJson;
TSharedRef<TJsonReader<>> StatusReader = TJsonReaderFactory<>::Create(DataStr);
if (!FJsonSerializer::Deserialize(StatusReader, StatusJson) || !StatusJson.IsValid())
{
return;
}
FString State;
if (StatusJson->TryGetStringField(TEXT("state"), State))
{
if (State != WorkerState)
{
WorkerState = State;
UE_LOG(LogRosBridge, Log, TEXT("Worker state: %s"), *WorkerState);
if (GEngine)
{
FColor Color = FColor::White;
if (State == TEXT("recording")) Color = FColor::Green;
else if (State == TEXT("replaying")) Color = FColor::Cyan;
else if (State == TEXT("idle")) Color = FColor::Silver;
GEngine->AddOnScreenDebugMessage(-1, 3.0f, Color,
FString::Printf(TEXT("Robot state: %s"), *WorkerState));
}
}
}
// Update sync (teleop) status
bool bTeleop = false;
if (StatusJson->TryGetBoolField(TEXT("teleop"), bTeleop))
{
if (bTeleop != bSyncActive)
{
bSyncActive = bTeleop;
UE_LOG(LogRosBridge, Log, TEXT("Sync (teleop): %s"),
bSyncActive ? TEXT("ON") : TEXT("OFF"));
}
}
// Log errors from commands
FString Status;
if (StatusJson->TryGetStringField(TEXT("status"), Status) && Status == TEXT("error"))
{
FString Reason;
StatusJson->TryGetStringField(TEXT("reason"), Reason);
UE_LOG(LogRosBridge, Warning, TEXT("Robot command error: %s"), *Reason);
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Red,
FString::Printf(TEXT("Robot error: %s"), *Reason));
}
}
// Log recording saved info
FString Filename;
if (StatusJson->TryGetStringField(TEXT("filename"), Filename) && !Filename.IsEmpty())
{
int32 Frames = 0;
StatusJson->TryGetNumberField(TEXT("frames"), Frames);
double Duration = 0.0;
StatusJson->TryGetNumberField(TEXT("duration_sec"), Duration);
UE_LOG(LogRosBridge, Log, TEXT("Recording saved: %s (%d frames, %.1fs)"),
*Filename, Frames, Duration);
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Green,
FString::Printf(TEXT("Recording saved: %s (%d frames, %.1fs)"),
*Filename, Frames, Duration));
}
}
// --- Recordings list (reply to list_recordings) ---
const TArray<TSharedPtr<FJsonValue>>* RecArray = nullptr;
if (StatusJson->TryGetArrayField(TEXT("recordings"), RecArray) && RecArray)
{
Recordings.Reset();
for (const TSharedPtr<FJsonValue>& V : *RecArray)
{
const TSharedPtr<FJsonObject> RecObj = V->AsObject();
if (RecObj.IsValid())
{
FRecordingInfo Info;
RecObj->TryGetStringField(TEXT("filename"), Info.Filename);
RecObj->TryGetNumberField(TEXT("frames"), Info.Frames);
double Dur = 0.0;
RecObj->TryGetNumberField(TEXT("duration_sec"), Dur);
Info.DurationSec = static_cast<float>(Dur);
RecObj->TryGetStringField(TEXT("recorded_at"), Info.RecordedAt);
Recordings.Add(Info);
}
}
++RecordingsVersion;
UE_LOG(LogRosBridge, Log, TEXT("Recordings list updated: %d files"), Recordings.Num());
}
// --- Recordings list, per-item (split to avoid large-frame drops) ---
const TSharedPtr<FJsonObject>* RecItemObj = nullptr;
if (StatusJson->TryGetObjectField(TEXT("recording_item"), RecItemObj) && RecItemObj && RecItemObj->IsValid())
{
int32 RecIndex = 0, RecTotal = 0;
StatusJson->TryGetNumberField(TEXT("index"), RecIndex);
StatusJson->TryGetNumberField(TEXT("total"), RecTotal);
if (RecIndex == 0) { PendingRecordings.Reset(); }
FRecordingInfo Info;
(*RecItemObj)->TryGetStringField(TEXT("filename"), Info.Filename);
(*RecItemObj)->TryGetNumberField(TEXT("frames"), Info.Frames);
double Dur = 0.0;
(*RecItemObj)->TryGetNumberField(TEXT("duration_sec"), Dur);
Info.DurationSec = static_cast<float>(Dur);
(*RecItemObj)->TryGetStringField(TEXT("recorded_at"), Info.RecordedAt);
PendingRecordings.Add(Info);
if (RecTotal > 0 && PendingRecordings.Num() >= RecTotal)
{
Recordings = PendingRecordings;
Recordings.Sort([](const FRecordingInfo& A, const FRecordingInfo& B)
{
return A.Filename > B.Filename; // newest first (top)
});
PendingRecordings.Reset();
++RecordingsVersion;
UE_LOG(LogRosBridge, Log, TEXT("Recordings list updated: %d files"), Recordings.Num());
}
}
// --- Recordings list cleared (empty result) ---
bool bRecClear = false;
if (StatusJson->TryGetBoolField(TEXT("recordings_clear"), bRecClear) && bRecClear)
{
Recordings.Reset();
PendingRecordings.Reset();
++RecordingsVersion;
UE_LOG(LogRosBridge, Log, TEXT("Recordings list cleared"));
}
// --- Replay progress ---
const TSharedPtr<FJsonObject>* ProgObj = nullptr;
if (StatusJson->TryGetObjectField(TEXT("replay_progress"), ProgObj) && ProgObj)
{
(*ProgObj)->TryGetNumberField(TEXT("index"), ReplayIndex);
(*ProgObj)->TryGetNumberField(TEXT("total"), ReplayTotal);
(*ProgObj)->TryGetStringField(TEXT("filename"), ReplayProgFilename);
(*ProgObj)->TryGetBoolField(TEXT("approaching"), bReplayApproaching);
}
// Display device-level errors (USB disconnection, serial errors)
const TSharedPtr<FJsonObject>* DeviceErrors = nullptr;
bool bHasDeviceErrors = StatusJson->TryGetObjectField(TEXT("device_errors"), DeviceErrors) && DeviceErrors;
// Check follower error
FString FollowerErr;
if (bHasDeviceErrors)
{
(*DeviceErrors)->TryGetStringField(TEXT("follower"), FollowerErr);
}
if (!FollowerErr.IsEmpty() && !bFollowerDeviceError)
{
bFollowerDeviceError = true;
UE_LOG(LogRosBridge, Error, TEXT("Follower USB error: %s"), *FollowerErr);
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 10.0f, FColor::Red,
TEXT("*** Follower: USB/Serial ERROR *** Check USB connection."));
}
}
else if (FollowerErr.IsEmpty() && bFollowerDeviceError)
{
bFollowerDeviceError = false;
UE_LOG(LogRosBridge, Log, TEXT("Follower USB restored."));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Green,
TEXT("Follower: USB connection restored"));
}
}
// Check leader error
FString LeaderErr;
if (bHasDeviceErrors)
{
(*DeviceErrors)->TryGetStringField(TEXT("leader"), LeaderErr);
}
if (!LeaderErr.IsEmpty() && !bLeaderDeviceError)
{
bLeaderDeviceError = true;
UE_LOG(LogRosBridge, Error, TEXT("Leader USB error: %s"), *LeaderErr);
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 10.0f, FColor::Red,
TEXT("*** Leader: USB/Serial ERROR *** Check USB connection."));
}
}
else if (LeaderErr.IsEmpty() && bLeaderDeviceError)
{
bLeaderDeviceError = false;
UE_LOG(LogRosBridge, Log, TEXT("Leader USB restored."));
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Green,
TEXT("Leader: USB connection restored"));
}
}
}
RobotControlWidget.cpp 수정
- RefreshRecordingsList()의 처음 한 번 자동요청 블록을 이걸로 교체
// Auto-request the list until it actually arrives: retry every ~2s while the
// worker is alive and we still have no recordings. Stops once the list lands.
if (IsWorkerAlive() && R->GetRecordingsVersion() == 0)
{
AutoRequestAccum += 0.2f; // this function runs at 5 Hz
if (AutoRequestAccum >= 2.0f)
{
AutoRequestAccum = 0.0f;
UE_LOG(LogTemp, Warning, TEXT("AUTO list_recordings retry"));
R->PublishRobotCommand(TEXT("{\"cmd\":\"list_recordings\"}"));
}
}
RobotControlWidget.cpp 전체 코드
더보기
#include "RobotControlWidget.h"
#include "RobotVisualizer.h"
#include "RosBridgeSubsystem.h"
#include "Kismet/GameplayStatics.h"
#include "Engine/World.h"
#include "Engine/GameInstance.h"
#include "Components/Button.h"
#include "Components/TextBlock.h"
#include "Components/EditableTextBox.h"
#include "Components/CheckBox.h"
#include "Components/SpinBox.h"
#include "Components/ListView.h"
#include "Components/ComboBoxString.h"
#include "Components/ProgressBar.h"
#include "RecordingEntryWidget.h"
#include "TimerManager.h"
#include "Engine/World.h"
void URobotControlWidget::NativeConstruct()
{
Super::NativeConstruct();
ResolveRobot();
if (EStopButton)
{
EStopButton->OnClicked.AddDynamic(this, &URobotControlWidget::HandleEStopClicked);
}
if (SyncOnButton) SyncOnButton->OnClicked.AddDynamic(this, &URobotControlWidget::HandleSyncOnClicked);
if (SyncOffButton) SyncOffButton->OnClicked.AddDynamic(this, &URobotControlWidget::HandleSyncOffClicked);
if (StartRecordButton) StartRecordButton->OnClicked.AddDynamic(this, &URobotControlWidget::HandleStartRecordClicked);
if (StopRecordButton) StopRecordButton->OnClicked.AddDynamic(this, &URobotControlWidget::HandleStopRecordClicked);
if (StartReplayButton) StartReplayButton->OnClicked.AddDynamic(this, &URobotControlWidget::HandleStartReplayClicked);
if (StopReplayButton) StopReplayButton->OnClicked.AddDynamic(this, &URobotControlWidget::HandleStopReplayClicked);
if (ApproachSpeedSpinBox)
{
ApproachSpeedSpinBox->SetMinValue(5.0f);
ApproachSpeedSpinBox->SetMaxValue(300.0f);
ApproachSpeedSpinBox->SetMinSliderValue(5.0f);
ApproachSpeedSpinBox->SetMaxSliderValue(300.0f);
ApproachSpeedSpinBox->SetValue(45.0f);
}
RefreshControlUI();
RefreshSafetyUI();
if (RefreshRecordingsButton)
{
RefreshRecordingsButton->OnClicked.AddDynamic(this, &URobotControlWidget::HandleRefreshRecordingsClicked);
}
if (RecordingComboBox)
{
RecordingComboBox->OnSelectionChanged.AddDynamic(this, &URobotControlWidget::HandleRecordingSelected);
}
}
ARobotVisualizer* URobotControlWidget::ResolveRobot()
{
if (!Robot.IsValid())
{
if (UWorld* World = GetWorld())
{
Robot = Cast<ARobotVisualizer>(
UGameplayStatics::GetActorOfClass(World, ARobotVisualizer::StaticClass()));
}
}
if (!Ros.IsValid())
{
if (UGameInstance* GI = GetGameInstance())
{
Ros = GI->GetSubsystem<URosBridgeSubsystem>();
}
}
return Robot.Get();
}
// --- Commands ---
void URobotControlWidget::CmdSyncOn() { if (ARobotVisualizer* R = ResolveRobot()) { R->SyncOn(); } }
void URobotControlWidget::CmdSyncOff() { if (ARobotVisualizer* R = ResolveRobot()) { R->SyncOff(); } }
void URobotControlWidget::CmdStartRecord() { if (ARobotVisualizer* R = ResolveRobot()) { R->StartRecord(); } }
void URobotControlWidget::CmdStopRecord() { if (ARobotVisualizer* R = ResolveRobot()) { R->StopRecord(); } }
void URobotControlWidget::CmdStopReplay() { if (ARobotVisualizer* R = ResolveRobot()) { R->StopReplay(); } }
void URobotControlWidget::CmdEStop() { if (ARobotVisualizer* R = ResolveRobot()) { R->EStop(); } }
void URobotControlWidget::CmdStartReplay(const FString& Filename, bool bLoop, float ApproachSpeed)
{
if (ARobotVisualizer* R = ResolveRobot())
{
R->ReplayFilename = Filename;
R->bReplayLoop = bLoop;
R->ApproachSpeed = ApproachSpeed;
R->StartReplay();
}
}
void URobotControlWidget::NativeTick(const FGeometry& MyGeometry, float InDeltaTime)
{
Super::NativeTick(MyGeometry, InDeltaTime);
// Poll the robot state ~5x/sec (cheap, no delegates needed).
RefreshAccum += InDeltaTime;
if (RefreshAccum >= 0.2f)
{
RefreshAccum = 0.0f;
RefreshSafetyUI();
RefreshControlUI();
RefreshRecordingsList();
RefreshReplayProgress();
}
}
void URobotControlWidget::HandleEStopClicked()
{
CmdEStop();
}
void URobotControlWidget::RefreshSafetyUI()
{
// --- Connection status text + color (priority order) ---
if (ConnectionStatusText)
{
FString StatusStr;
FLinearColor Color;
if (!IsRosBridgeConnected())
{
StatusStr = TEXT("DISCONNECTED");
Color = FLinearColor::Red;
}
else if (!IsBridgeNodeAlive())
{
StatusStr = TEXT("Bridge node: DOWN");
Color = FLinearColor::Red;
}
else if (!IsWorkerAlive())
{
StatusStr = TEXT("Worker: DOWN");
Color = FLinearColor(1.0f, 0.5f, 0.0f); // orange
}
else
{
const FString WS = GetWorkerState();
// WorkerState may still hold a stale connection-layer string
// ("disconnected", "bridge lost", "worker lost", "unknown") until
// the worker sends its first real /robot_status. Filter those out.
const bool bRealState =
(WS == TEXT("idle") || WS == TEXT("syncing") ||
WS == TEXT("recording") || WS == TEXT("replaying"));
StatusStr = bRealState
? FString::Printf(TEXT("Connected | %s"), *WS)
: TEXT("Connected | (awaiting status)");
Color = FLinearColor::Green;
}
ConnectionStatusText->SetText(FText::FromString(StatusStr));
ConnectionStatusText->SetColorAndOpacity(FSlateColor(Color));
}
// --- Device error panel (hidden when no errors) ---
if (DeviceErrorText)
{
TArray<FString> Errors;
if (HasFollowerError()) { Errors.Add(TEXT("Follower: USB/Serial ERROR")); }
if (HasLeaderError()) { Errors.Add(TEXT("Leader: USB/Serial ERROR")); }
if (Errors.Num() > 0)
{
DeviceErrorText->SetText(FText::FromString(FString::Join(Errors, TEXT("\n"))));
DeviceErrorText->SetColorAndOpacity(FSlateColor(FLinearColor::Red));
DeviceErrorText->SetVisibility(ESlateVisibility::HitTestInvisible);
}
else
{
DeviceErrorText->SetVisibility(ESlateVisibility::Collapsed);
}
}
}
void URobotControlWidget::HandleSyncOnClicked() { CmdSyncOn(); }
void URobotControlWidget::HandleSyncOffClicked() { CmdSyncOff(); }
void URobotControlWidget::HandleStartRecordClicked() { CmdStartRecord(); }
void URobotControlWidget::HandleStopRecordClicked() { CmdStopRecord(); }
void URobotControlWidget::HandleStopReplayClicked() { CmdStopReplay(); }
void URobotControlWidget::HandleStartReplayClicked()
{
FString Filename;
if (RecordingComboBox && !RecordingComboBox->GetSelectedOption().IsEmpty())
Filename = RecordingComboBox->GetSelectedOption();
else if (ReplayFilenameTextBox)
Filename = ReplayFilenameTextBox->GetText().ToString();
const bool bLoop = LoopCheckBox ? LoopCheckBox->IsChecked() : false;
const float Speed = ApproachSpeedSpinBox ? ApproachSpeedSpinBox->GetValue() : 45.0f;
CmdStartReplay(Filename, bLoop, Speed);
}
void URobotControlWidget::RefreshControlUI()
{
const FString WS = GetWorkerState();
const bool bIdle = (WS == TEXT("idle"));
const bool bSyncing = (WS == TEXT("syncing"));
const bool bRecording = (WS == TEXT("recording"));
const bool bReplaying = (WS == TEXT("replaying"));
if (WorkerStateText)
{
FString Label;
FLinearColor Color;
if (bRecording) { Label = TEXT("RECORDING"); Color = FLinearColor::Red; }
else if (bReplaying) { Label = TEXT("REPLAYING"); Color = FLinearColor(0.0f, 1.0f, 1.0f); }
else if (bSyncing) { Label = TEXT("SYNCING"); Color = FLinearColor::Green; }
else if (bIdle) { Label = TEXT("IDLE"); Color = FLinearColor::Gray; }
else { Label = TEXT("--"); Color = FLinearColor::Gray; }
WorkerStateText->SetText(FText::FromString(Label));
WorkerStateText->SetColorAndOpacity(FSlateColor(Color));
}
// Workflow guards — only enable actions that make sense in the current state.
const bool bAlive = IsWorkerAlive();
if (SyncOnButton) SyncOnButton->SetIsEnabled(bAlive && !bRecording && !bReplaying);
if (SyncOffButton) SyncOffButton->SetIsEnabled(bAlive && IsSyncActive() && !bRecording);
if (StartRecordButton) StartRecordButton->SetIsEnabled(bAlive && IsSyncActive() && !bRecording && !bReplaying);
if (StopRecordButton) StopRecordButton->SetIsEnabled(bAlive && bRecording);
if (StartReplayButton) StartReplayButton->SetIsEnabled(bAlive && !bRecording && !bReplaying);
if (StopReplayButton) StopReplayButton->SetIsEnabled(bAlive && bReplaying);
}
void URobotControlWidget::HandleRefreshRecordingsClicked()
{
if (ARobotVisualizer* R = ResolveRobot())
{
R->PublishRobotCommand(TEXT("{\"cmd\":\"list_recordings\"}"));
}
}
void URobotControlWidget::HandleRecordingSelected(FString SelectedItem, ESelectInfo::Type SelectionType)
{
if (ReplayFilenameTextBox && !SelectedItem.IsEmpty())
{
ReplayFilenameTextBox->SetText(FText::FromString(SelectedItem));
}
}
void URobotControlWidget::RefreshRecordingsList()
{
if (!RecordingComboBox) return;
ARobotVisualizer* R = ResolveRobot();
if (!R) return;
// Auto-request the list until it actually arrives: retry every ~2s while the
// worker is alive and we still have no recordings. Stops once the list lands.
if (IsWorkerAlive() && R->GetRecordingsVersion() == 0)
{
AutoRequestAccum += 0.2f; // this function runs at 5 Hz
if (AutoRequestAccum >= 2.0f)
{
AutoRequestAccum = 0.0f;
UE_LOG(LogTemp, Warning, TEXT("AUTO list_recordings retry"));
R->PublishRobotCommand(TEXT("{\"cmd\":\"list_recordings\"}"));
}
}
// Rebuild the combo only when the actor's recordings actually changed.
if (R->GetRecordingsVersion() == CachedRecordingsVersion) return;
CachedRecordingsVersion = R->GetRecordingsVersion();
const FString Prev = RecordingComboBox->GetSelectedOption();
RecordingComboBox->ClearOptions();
for (const FRecordingInfo& Info : R->GetRecordings())
{
RecordingComboBox->AddOption(Info.Filename);
}
// Default to the most recent (top of the list, index 0).
if (!Prev.IsEmpty() && RecordingComboBox->FindOptionIndex(Prev) != INDEX_NONE)
{
RecordingComboBox->SetSelectedOption(Prev);
}
else if (RecordingComboBox->GetOptionCount() > 0)
{
RecordingComboBox->SetSelectedIndex(0);
}
}
void URobotControlWidget::RefreshReplayProgress()
{
const bool bReplaying = (GetWorkerState() == TEXT("replaying"));
ARobotVisualizer* R = Robot.Get();
if (ReplayProgressBar)
{
float Pct = 0.0f;
if (bReplaying && R && R->GetReplayTotal() > 0)
{
Pct = static_cast<float>(R->GetReplayIndex()) / static_cast<float>(R->GetReplayTotal());
}
ReplayProgressBar->SetPercent(Pct);
}
if (ReplayProgressText)
{
if (bReplaying && R)
{
const FString T = R->IsReplayApproaching()
? TEXT("approaching...")
: FString::Printf(TEXT("%d / %d"), R->GetReplayIndex(), R->GetReplayTotal());
ReplayProgressText->SetText(FText::FromString(T));
}
else
{
ReplayProgressText->SetText(FText::GetEmpty());
}
}
}
// --- State getters ---
FString URobotControlWidget::GetWorkerState() const
{
return Robot.IsValid() ? Robot->GetWorkerState() : TEXT("no robot");
}
bool URobotControlWidget::IsSyncActive() const { return Robot.IsValid() && Robot->IsSyncActive(); }
bool URobotControlWidget::IsRosBridgeConnected() const { return Robot.IsValid() && Robot->IsRosBridgeConnected(); }
bool URobotControlWidget::IsBridgeNodeAlive() const { return Robot.IsValid() && Robot->IsBridgeNodeAlive(); }
bool URobotControlWidget::IsWorkerAlive() const { return Robot.IsValid() && Robot->IsWorkerAlive(); }
bool URobotControlWidget::HasFollowerError() const { return Robot.IsValid() && Robot->HasFollowerError(); }
bool URobotControlWidget::HasLeaderError() const { return Robot.IsValid() && Robot->HasLeaderError(); }
bool URobotControlWidget::HasRobot() const { return Robot.IsValid(); }
RobotControlWidget.h에 멤버 하나 추가
- bool bInitialListRequested = false; 아래
float AutoRequestAccum = 0.0f;
RobotControlWidget.h 전체 코드
더보기
#pragma once
#include "CoreMinimal.h"
#include "Blueprint/UserWidget.h"
#include "RobotControlWidget.generated.h"
class ARobotVisualizer;
class URosBridgeSubsystem;
/**
* Base class for the in-viewport robot control UI (Phase 10).
*
* This C++ base owns the logic: it finds the ARobotVisualizer in the level,
* exposes the robot's commands as BlueprintCallable functions (bind WBP
* buttons to these), and exposes its state as BlueprintPure getters (bind
* WBP text / visibility to these).
*
* Create a Widget Blueprint (WBP_RobotControl) reparented to this class,
* lay out the visuals in the UMG designer, and wire buttons/text to the
* functions below. No Blueprint scripting required — just bindings.
*/
UCLASS()
class SO101_TWIN_API URobotControlWidget : public UUserWidget
{
GENERATED_BODY()
public:
// --- Commands (bind WBP buttons' OnClicked to these) ---
UFUNCTION(BlueprintCallable, Category = "ROS|UI")
void CmdSyncOn();
UFUNCTION(BlueprintCallable, Category = "ROS|UI")
void CmdSyncOff();
UFUNCTION(BlueprintCallable, Category = "ROS|UI")
void CmdStartRecord();
UFUNCTION(BlueprintCallable, Category = "ROS|UI")
void CmdStopRecord();
UFUNCTION(BlueprintCallable, Category = "ROS|UI")
void CmdStartReplay(const FString& Filename, bool bLoop, float ApproachSpeed);
UFUNCTION(BlueprintCallable, Category = "ROS|UI")
void CmdStopReplay();
UFUNCTION(BlueprintCallable, Category = "ROS|UI")
void CmdEStop();
// --- State getters (bind WBP text / visibility to these) ---
UFUNCTION(BlueprintPure, Category = "ROS|UI")
FString GetWorkerState() const;
UFUNCTION(BlueprintPure, Category = "ROS|UI")
bool IsSyncActive() const;
UFUNCTION(BlueprintPure, Category = "ROS|UI")
bool IsRosBridgeConnected() const;
UFUNCTION(BlueprintPure, Category = "ROS|UI")
bool IsBridgeNodeAlive() const;
UFUNCTION(BlueprintPure, Category = "ROS|UI")
bool IsWorkerAlive() const;
UFUNCTION(BlueprintPure, Category = "ROS|UI")
bool HasFollowerError() const;
UFUNCTION(BlueprintPure, Category = "ROS|UI")
bool HasLeaderError() const;
/** True once the robot actor has been located in the level. */
UFUNCTION(BlueprintPure, Category = "ROS|UI")
bool HasRobot() const;
protected:
virtual void NativeConstruct() override;
/** Find and cache the ARobotVisualizer + subsystem. Safe to call repeatedly. */
ARobotVisualizer* ResolveRobot();
virtual void NativeTick(const FGeometry& MyGeometry, float InDeltaTime) override;
UFUNCTION()
void HandleEStopClicked();
/** Recompute the safety panel from the robot's current state. */
void RefreshSafetyUI();
// --- Bound widgets: names MUST match the WBP widget names exactly ---
UPROPERTY(meta = (BindWidget))
TObjectPtr<class UButton> EStopButton;
UPROPERTY(meta = (BindWidget))
TObjectPtr<class UTextBlock> ConnectionStatusText;
UPROPERTY(meta = (BindWidget))
TObjectPtr<class UTextBlock> DeviceErrorText;
float RefreshAccum = 0.0f;
TWeakObjectPtr<ARobotVisualizer> Robot;
TWeakObjectPtr<URosBridgeSubsystem> Ros;
UFUNCTION() void HandleSyncOnClicked();
UFUNCTION() void HandleSyncOffClicked();
UFUNCTION() void HandleStartRecordClicked();
UFUNCTION() void HandleStopRecordClicked();
UFUNCTION() void HandleStartReplayClicked();
UFUNCTION() void HandleStopReplayClicked();
/** Recompute the control panel (worker state label + button enable states). */
void RefreshControlUI();
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UButton> SyncOnButton;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UButton> SyncOffButton;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UButton> StartRecordButton;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UButton> StopRecordButton;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UButton> StartReplayButton;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UButton> StopReplayButton;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UEditableTextBox> ReplayFilenameTextBox;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UCheckBox> LoopCheckBox;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class USpinBox> ApproachSpeedSpinBox;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UTextBlock> WorkerStateText;
UFUNCTION() void HandleRefreshRecordingsClicked();
UFUNCTION() void HandleRecordingSelected(FString SelectedItem, ESelectInfo::Type SelectionType);
void RefreshRecordingsList();
void RefreshReplayProgress();
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UButton> RefreshRecordingsButton;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UComboBoxString> RecordingComboBox;
bool bInitialListRequested = false;
float AutoRequestAccum = 0.0f;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UProgressBar> ReplayProgressBar;
UPROPERTY(meta = (BindWidgetOptional)) TObjectPtr<class UTextBlock> ReplayProgressText;
int32 CachedRecordingsVersion = -1;
};
현재까지 최종 화면
작업 완료 내용
- 연결 상태 4단계 표시, E-stop, USB 오류 패널
- Record/Replay: 버튼 6종 + 워크플로우 가드, worker 상태 색상 인디케이터, replay 옵션, recording 드롭다운(최신순·자동 로드), 진행률 바
- worker에 list_recordings와 replay_progress가 이미 있었고, bridge에 진행률 5Hz 발행 + 목록 청킹만 추가
- 가장 큰 걸림돌은 rosbridge가 ~1.5KB 넘는 메시지를 경고 없이 조용히 버린다는 것. echo에는 나오는데 Unreal엔 프레임 자체가 안 들어와서, WS RX len 로그로 수신부를 찍어보고서야 확정했다. max_message_size도 fragment_size도 안 먹어서, 결국 bridge에서 항목 단위로 쪼개 발행하는 게 정답이었음.
728x90
반응형
