728x90
반응형
이번 단계에서는 본격적 언리얼 씬을 위한 UI를 계획하고 작업하는 과정을 기록한다.
1단계: 기반 설정하기 - API 표면 + 위젯 인프라
- 앞선 단계에서 구현했던 E-stop/Sync/Record/Replay 기능을 위젯 버튼에서 호출할 수 있게 BlueprintCallable로 노출하고, 상태(worker 상태, 연결/heartbeat, follower/leader 에러, 관절 각도)를 읽는 BlueprintPure 게터, 그리고 상태가 바뀔 때 위젯에 알리는 BlueprintAssignable 델리게이트를 추가할 것이다.
- 액터의 API를 호출하고 델리게이트에 바인딩하는 베이스 위젯 클래스(UUserWidget)를 새로 작성한다.
- PIE 시작 시 위젯을 뷰포트에 띄우는 코드를 작성한다. 이게 깔리면 다음 단계부터는 화면에 위젯을 올리고 버튼만 연결하면 된다.
Step1 : Source/SO101_Twin/SO101_Twin.Build.cs 파일 수정
모듈 의존성에 "UMG", "Slate", "SlateCore"를 추가하고, 새 UI 폴더를 include 경로에 넣는다. PublicDependencyModuleNames에 들어있는 배열에 세 개를 더하고, 파일 안 PublicIncludePaths 블록에 한 줄 추가
// PublicDependencyModuleNames 의 { ... } 안에 추가:
"UMG", "Slate", "SlateCore",
// 기존 PublicIncludePaths 블록에 한 줄 추가 (RosBridge/Robot 옆에):
PublicIncludePaths.Add(Path.Combine(ModuleDirectory, "UI"));
전체 코드:
더보기
// Copyright Epic Games, Inc. All Rights Reserved.
using System.IO;
using UnrealBuildTool;
public class SO101_Twin : ModuleRules
{
public SO101_Twin(ReadOnlyTargetRules Target) : base(Target)
{
PCHUsage = PCHUsageMode.UseExplicitOrSharedPCHs;
PublicDependencyModuleNames.AddRange(new string[] { "Core", "CoreUObject", "Engine", "InputCore", "EnhancedInput", "UMG", "Slate", "SlateCore" });
PrivateDependencyModuleNames.AddRange(new string[] {
"WebSockets",
"Json",
"JsonUtilities"
});
PublicIncludePaths.Add(Path.Combine(ModuleDirectory, "RosBridge"));
PublicIncludePaths.Add(Path.Combine(ModuleDirectory, "Robot"));
PublicIncludePaths.Add(Path.Combine(ModuleDirectory, "UI"));
// Uncomment if you are using Slate UI
// PrivateDependencyModuleNames.AddRange(new string[] { "Slate", "SlateCore" });
// Uncomment if you are using online features
// PrivateDependencyModuleNames.Add("OnlineSubsystem");
// To include OnlineSubsystemSteam, add it to the plugins section in your uproject file with the Enabled attribute set to true
}
}
Step2 : RobotVisualizer.h 파일 수정
- 상단 전방 선언 - class URosBridgeSubsystem; 바로 아래에 추가
class URobotControlWidget;
- include - #include "GameFramework/Actor.h" 바로 아래에 추가
#include "Templates/SubclassOf.h"
- 생성자 ARobotVisualizer(); 와 protected: 사이에 Widget API 삽입
// =================================================================
// 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;
- private 섹션 끝부분, device error bool들(bLeaderDeviceError) 근처에 런타임 포인터 추가
/** The viewport control UI widget instance (Phase 10). */
UPROPERTY(Transient)
TObjectPtr<URobotControlWidget> ControlWidget;
전체 코드:
더보기
#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
*/
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;
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;
// =================================================================
// 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);
};
Step3 : RobotVisualizer.cpp 파일 수정
- include 블록 끝(#include "UObject/ConstructorHelpers.h" 아래)에 추가
#include "GameFramework/PlayerController.h"
#include "Blueprint/UserWidget.h"
#include "RobotControlWidget.h"
- BeginPlay() 맨 끝, 닫는 } 바로 앞(이미 있는 "Initiate connection" if/else 블록 다음)에 추가
// --- 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() 안, Super::EndPlay(EndPlayReason); 바로 앞에 추가
if (ControlWidget)
{
ControlWidget->RemoveFromParent();
ControlWidget = nullptr;
}
전체 코드:
더보기
#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));
}
}
// 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"));
}
}
}
Step4 : Source/SO101_Twin/UI/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();
TWeakObjectPtr<ARobotVisualizer> Robot;
TWeakObjectPtr<URosBridgeSubsystem> Ros;
};
Step5 : Source/SO101_Twin/UI/RobotControlWidget.cpp 파일 생성
#include "RobotControlWidget.h"
#include "RobotVisualizer.h"
#include "RosBridgeSubsystem.h"
#include "Kismet/GameplayStatics.h"
#include "Engine/World.h"
#include "Engine/GameInstance.h"
void URobotControlWidget::NativeConstruct()
{
Super::NativeConstruct();
ResolveRobot();
}
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();
}
}
// --- 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(); }
Step6 : 빌드 & 검증
- .uproject 프로젝트 파일 우클릭 → "Generate Visual Studio project files" → VS에서 빌드(Development Editor)
- WBP 생성: Content Browser에 Content/UI 폴더 생성 → 우클릭 → User Interface → Widget Blueprint. 이름 WBP_RobotControl. 열고 → Graph 탭 → Class Settings → Details의 Parent Class를 RobotControlWidget으로 변경.
- 임시 표시 하나: Designer 탭에서 Text 위젯 하나를 캔버스에 끌어다 놓고 글자를 "SO101 Control"로 설정 (확인용. 이후 다른 UI로 변경)
- 액터에 연결: 레벨의 RobotVisualizer 액터 선택 → Details → ROS|UI 섹션 → ControlWidgetClass를 WBP_RobotControl로 지정.
- PIE 실행: 화면에 "SO101 Control" 글자가 뜨고, 기존 로봇 3D 모델/연결 동작이 그대로면 성공
2단계: 안전 UI 설정
Step1 : RobotControlWidget.h에 추가
protected 섹션, 기존 Robot/Ros 위쪽에 추가
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;
전체 코드:
더보기
#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;
};
Step2 : RobotControlWidget.cpp 에 추가
- 맨 위 include에 두 줄 추가
#include "Components/Button.h"
#include "Components/TextBlock.h"
- NativeConstruct() 안, ResolveRobot(); 다음에 버튼 바인딩 추가
if (EStopButton)
{
EStopButton->OnClicked.AddDynamic(this, &URobotControlWidget::HandleEStopClicked);
}
RefreshSafetyUI();
- void URobotControlWidget::CmdStartReplay 아래 세 함수 추가
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();
}
}
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
{
StatusStr = FString::Printf(TEXT("Connected | %s"), *GetWorkerState());
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);
}
}
}
전체 코드:
더보기
#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"
void URobotControlWidget::NativeConstruct()
{
Super::NativeConstruct();
ResolveRobot();
if (EStopButton)
{
EStopButton->OnClicked.AddDynamic(this, &URobotControlWidget::HandleEStopClicked);
}
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();
}
}
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
{
StatusStr = FString::Printf(TEXT("Connected | %s"), *GetWorkerState());
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);
}
}
}
// --- 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에 위젯 올리기 (UMG 디자이너)
WBP_RobotControl을 열고 Designer 탭으로 간다. 이전에 테스트로 만들어 놓은 "SO101 Control" 텍스트는 지운다.
[VerticalBox]를 하나 추가하고, Hierarchy 창에서 [Wrap With...] - [Canvas Panel]로 캔버스 아래에 위치하도록 수정한다.
Anchors는 상단 가운데로 설정하고, Postion Y는 20정도 줬다.
[Vertical Box]안에 Text와 Button을 배치한다. 각각의 이름은 정확히 "ConnectionStatusText" / "EStopButton" / "DeviceErrorText" 와 같아야 C++에서 인식할 수 있다.
상단에 Compile 버튼으로 컴파일 한다.
WSL 켜지 않은 상태로 실행했을 때 빨간 글씨로 DISCONNECTED가 떠 있으면 성공.
처음 연결했을 때는 bridge lost, Sync를 누르니 idle 표시가 뜬다.
USB를 강제 제거한 상태도 잘 뜬다.
연결 계층 문제로 박은 WorkerState 문자열이, 연결이 회복돼도 초기화되지 않는 문제( "Connected | bridge lost", "Connected | disconnected" 표시)가 생겨서 해당 부분을 수정한다. 처음 연결 되었을 때 worker의 상태를 못 받은 상태라면 "awaiting status"로 표시되게끔 수정한다.
RobotControlWidget.cpp 수정
- RefreshSafetyUI() 부분 수정
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;
}
전체 코드:
더보기
#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"
void URobotControlWidget::NativeConstruct()
{
Super::NativeConstruct();
ResolveRobot();
if (EStopButton)
{
EStopButton->OnClicked.AddDynamic(this, &URobotControlWidget::HandleEStopClicked);
}
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();
}
}
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);
}
}
}
// --- 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(); }수정했으면 다시 빌드한다.
worker의 상태가 오기 전에 awaiting status가 잘 뜨는 것을 확인했다.
728x90
반응형
