728x90
반응형
1단계: 오류/안전 처리
- 연결 끊김 시 Unreal UI 경고 (RosBridgeSubsystem에 OnDisconnected 델리게이트가 있지만 RobotVisualizer가 바인딩하지 않음. OnRosBridgeDisconnected() 콜백을 추가하고 뷰포트에 빨간 경고 표시 + 재연결 시 복구 메시지)
예를 들어 rosbridge가 죽었는데 worker가 replay를 계속 돌리고 있으면 Unreal에서 멈출 방법이 없으니까, worker 쪽에서 "bridge node로부터 일정 시간 heartbeat가 안 오면 자동으로 동작을 멈추는" 안전장치를 넣는 것. - 플래닝 실패 시 사용자 피드백 (moveit_goal_node.py가 MoveIt 에러를 로그에만 출력하고 /robot_status로 전파하지 않음. Python 노드를 수정해서 에러를 /robot_status에 publish → Unreal의 기존 OnRobotStatus() 핸들러가 자동으로 빨간 에러 메시지 표시)
- E-stop: 연결 끊김 상태에서 E-stop 버튼이 눌리면 "Not connected" 경고만 뜸 (현재 PublishRobotCommand에서 처리). 추가로 연결이 끊기면 worker에서 진행 중인 동작이 그대로 계속될 수 있는 문제 → worker 측에서 ZMQ heartbeat timeout으로 자동 정지하는 안전장치 검토 필요.
RobotVisualizer.h 수정
더보기
#pragma once
#include "CoreMinimal.h"
#include "GameFramework/Actor.h"
#include "RobotVisualizer.generated.h"
class UStaticMesh;
class UStaticMeshComponent;
class USceneComponent;
class URosBridgeSubsystem;
/**
* 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();
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);
// =================================================================
// 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);
};수정 사항
- OnRosBridgeDisconnected() UFUNCTION 선언 추가
- bRosBridgeConnected 상태 변수 추가
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"
// =============================================================================
// 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();
// 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."));
}
}
// =============================================================================
// 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;
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..."));
}
}
// =============================================================================
// 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)
{
ParseAndApplyJointStates(MessageJson);
}
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)
{
// 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));
}
}
}수정 사항
- BeginPlay: Ros->OnDisconnected.AddDynamic 바인딩 추가
- EndPlay: Ros->OnDisconnected.RemoveDynamic 해제 추가
- OnRosBridgeConnected: bRosBridgeConnected = true + 뷰포트에 초록색 "Connected" 메시지
- OnRosBridgeDisconnected (신규): bRosBridgeConnected = false, WorkerState = "disconnected", 뷰포트에 빨간색 경고 10초간 표시
moveit_goal_node.py 수정
더보기
#!/usr/bin/env python3
"""
moveit_goal_node.py — 중간 ROS2 노드
SO-ARM-101은 5DOF arm이므로 임의의 6DOF Cartesian pose를 만족시킬 수 없음.
따라서 Joint space goal을 기본으로 사용하고, Cartesian goal은 position-only 모드로 지원.
=== 토픽 인터페이스 ===
1) /moveit_goal_joints (sensor_msgs/JointState) — Joint space goal (추천)
- name: 목표로 설정할 관절 이름 배열
- position: 각 관절의 목표 각도 (radians)
2) /moveit_goal_pose (geometry_msgs/PoseStamped) — Cartesian position-only goal
- position만 사용, orientation은 무시 (5DOF 제약)
3) /moveit_goal_named (std_msgs/String) — Named target (SRDF에 정의된 포즈)
- "home", "ready" 등
=== 테스트 명령 (ROS2 터미널) ===
# Named target (가장 간단):
ros2 topic pub --once /moveit_goal_named std_msgs/msg/String '{data: "home"}'
ros2 topic pub --once /moveit_goal_named std_msgs/msg/String '{data: "ready"}'
# Joint space goal (radians):
ros2 topic pub --once /moveit_goal_joints sensor_msgs/msg/JointState \
'{name: ["shoulder_pan","shoulder_lift","elbow_flex","wrist_flex","wrist_roll"], \
position: [0.0, -0.5, 0.5, 0.0, 0.0]}'
# Cartesian position-only goal (meters, base_link 기준):
ros2 topic pub --once /moveit_goal_pose geometry_msgs/msg/PoseStamped \
'{header: {frame_id: "base_link"}, pose: {position: {x: 0.1, y: 0.0, z: 0.15}}}'
"""
import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from rclpy.callback_groups import ReentrantCallbackGroup
from geometry_msgs.msg import PoseStamped, Pose
from sensor_msgs.msg import JointState
from std_msgs.msg import String
from moveit_msgs.action import MoveGroup
from moveit_msgs.msg import (
Constraints,
JointConstraint,
PositionConstraint,
BoundingVolume,
MoveItErrorCodes,
)
from shape_msgs.msg import SolidPrimitive
# SO-ARM-101 arm group joints (from SRDF)
ARM_JOINTS = [
'shoulder_pan',
'shoulder_lift',
'elbow_flex',
'wrist_flex',
'wrist_roll',
]
# Named targets (from SRDF — Phase 7.2)
NAMED_TARGETS = {
'home': {
'shoulder_pan': 0.0,
'shoulder_lift': 0.0,
'elbow_flex': 0.0,
'wrist_flex': 0.0,
'wrist_roll': 0.0,
},
'ready': {
'shoulder_pan': 0.0,
'shoulder_lift': -0.5,
'elbow_flex': 0.5,
'wrist_flex': 0.0,
'wrist_roll': 0.0,
},
}
class MoveItGoalNode(Node):
"""
3가지 토픽을 구독하여 MoveGroup action으로 전달하는 중간 노드.
- /moveit_goal_joints (sensor_msgs/JointState) → joint space goal
- /moveit_goal_pose (geometry_msgs/PoseStamped) → Cartesian position-only goal
- /moveit_goal_named (std_msgs/String) → named target
"""
def __init__(self):
super().__init__('moveit_goal_node')
# --- Parameters ---
self.declare_parameter('group_name', 'arm')
self.declare_parameter('end_effector_link', 'gripper_frame_link')
self.declare_parameter('planning_frame', 'base_link')
self.declare_parameter('num_planning_attempts', 5)
self.declare_parameter('allowed_planning_time', 5.0)
self.declare_parameter('max_velocity_scaling', 0.3)
self.declare_parameter('max_acceleration_scaling', 0.3)
self.declare_parameter('position_tolerance', 0.01)
self.declare_parameter('joint_tolerance', 0.01)
self.group_name = self.get_parameter('group_name').value
self.ee_link = self.get_parameter('end_effector_link').value
self.planning_frame = self.get_parameter('planning_frame').value
self.num_attempts = self.get_parameter('num_planning_attempts').value
self.planning_time = self.get_parameter('allowed_planning_time').value
self.vel_scaling = self.get_parameter('max_velocity_scaling').value
self.acc_scaling = self.get_parameter('max_acceleration_scaling').value
self.pos_tol = self.get_parameter('position_tolerance').value
self.joint_tol = self.get_parameter('joint_tolerance').value
# --- Callback group ---
self._cb_group = ReentrantCallbackGroup()
# --- Action client ---
self._action_client = ActionClient(
self, MoveGroup, 'move_action',
callback_group=self._cb_group,
)
# --- Subscribers ---
self.create_subscription(
JointState, '/moveit_goal_joints', self._on_goal_joints, 10,
callback_group=self._cb_group,
)
self.create_subscription(
PoseStamped, '/moveit_goal_pose', self._on_goal_pose, 10,
callback_group=self._cb_group,
)
self.create_subscription(
String, '/moveit_goal_named', self._on_goal_named, 10,
callback_group=self._cb_group,
)
# --- Status publisher (→ Unreal via bridge) ---
self._status_pub = self.create_publisher(String, '/robot_status', 10)
# --- State ---
self._executing = False
self.get_logger().info(
f'MoveIt Goal Node ready. group={self.group_name}, ee={self.ee_link}'
)
self.get_logger().info('Listening on:')
self.get_logger().info(' /moveit_goal_joints (sensor_msgs/JointState)')
self.get_logger().info(' /moveit_goal_pose (geometry_msgs/PoseStamped) — position only')
self.get_logger().info(' /moveit_goal_named (std_msgs/String) — "home", "ready"')
# ─────────────────────────────────────────────
# Topic callbacks
# ─────────────────────────────────────────────
def _on_goal_named(self, msg: String):
"""Named target → joint space goal로 변환."""
name = msg.data.strip().lower()
if name not in NAMED_TARGETS:
self.get_logger().error(
f'Unknown named target "{name}". Available: {list(NAMED_TARGETS.keys())}'
)
return
self.get_logger().info(f'Named target: "{name}"')
joint_values = NAMED_TARGETS[name]
self._send_joint_goal(joint_values)
def _on_goal_joints(self, msg: JointState):
"""Joint space goal — name/position 쌍으로 목표 설정."""
if len(msg.name) == 0 or len(msg.name) != len(msg.position):
self.get_logger().error('Invalid JointState: name/position length mismatch or empty')
return
joint_values = {}
for name, pos in zip(msg.name, msg.position):
if name not in ARM_JOINTS:
self.get_logger().warn(f'Ignoring unknown joint: {name}')
continue
joint_values[name] = pos
if not joint_values:
self.get_logger().error('No valid arm joints in message')
return
names_str = ', '.join(f'{k}={v:.3f}' for k, v in joint_values.items())
self.get_logger().info(f'Joint goal: {names_str}')
self._send_joint_goal(joint_values)
def _on_goal_pose(self, msg: PoseStamped):
"""Cartesian position-only goal (orientation 무시 — 5DOF 제약)."""
self.get_logger().info(
f'Cartesian goal (position only): '
f'({msg.pose.position.x:.3f}, {msg.pose.position.y:.3f}, {msg.pose.position.z:.3f})'
)
self._send_pose_goal(msg)
# ─────────────────────────────────────────────
# Goal builders
# ─────────────────────────────────────────────
def _send_joint_goal(self, joint_values: dict):
"""Joint space goal을 MoveGroup action으로 전송."""
if not self._pre_send_check():
return
goal = self._make_base_goal()
req = goal.request
constraints = Constraints()
for joint_name, position in joint_values.items():
jc = JointConstraint()
jc.joint_name = joint_name
jc.position = position
jc.tolerance_above = self.joint_tol
jc.tolerance_below = self.joint_tol
jc.weight = 1.0
constraints.joint_constraints.append(jc)
req.goal_constraints.append(constraints)
self._send_goal(goal)
def _send_pose_goal(self, msg: PoseStamped):
"""Cartesian position-only goal — orientation constraint 없음 (5DOF)."""
if not self._pre_send_check():
return
goal = self._make_base_goal()
req = goal.request
frame_id = msg.header.frame_id if msg.header.frame_id else self.planning_frame
# Position constraint only (no orientation — 5DOF arm)
pos_constraint = PositionConstraint()
pos_constraint.header.frame_id = frame_id
pos_constraint.link_name = self.ee_link
pos_constraint.target_point_offset.x = 0.0
pos_constraint.target_point_offset.y = 0.0
pos_constraint.target_point_offset.z = 0.0
bounding = BoundingVolume()
primitive = SolidPrimitive()
primitive.type = SolidPrimitive.SPHERE
primitive.dimensions = [self.pos_tol]
bounding.primitives.append(primitive)
target_pose = Pose()
target_pose.position = msg.pose.position
target_pose.orientation.w = 1.0
bounding.primitive_poses.append(target_pose)
pos_constraint.constraint_region = bounding
pos_constraint.weight = 1.0
constraints = Constraints()
constraints.position_constraints.append(pos_constraint)
req.goal_constraints.append(constraints)
self._send_goal(goal)
# ─────────────────────────────────────────────
# Helpers
# ─────────────────────────────────────────────
def _pre_send_check(self) -> bool:
"""공통 사전 검사."""
if self._executing:
self.get_logger().warn('Previous motion still executing, ignoring new goal')
return False
if not self._action_client.wait_for_server(timeout_sec=3.0):
self.get_logger().error('/move_action server not available!')
self._publish_status({
'status': 'error',
'source': 'moveit',
'reason': 'MoveIt action server not available (is move_group running?)',
})
return False
return True
def _make_base_goal(self) -> MoveGroup.Goal:
"""공통 MoveGroup goal 템플릿."""
goal = MoveGroup.Goal()
req = goal.request
req.group_name = self.group_name
req.num_planning_attempts = self.num_attempts
req.allowed_planning_time = self.planning_time
req.max_velocity_scaling_factor = self.vel_scaling
req.max_acceleration_scaling_factor = self.acc_scaling
goal.planning_options.plan_only = False
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 3
return goal
def _send_goal(self, goal: MoveGroup.Goal):
"""Action goal 전송."""
self._executing = True
self.get_logger().info('Sending MoveGroup goal...')
send_future = self._action_client.send_goal_async(
goal, feedback_callback=self._on_feedback,
)
send_future.add_done_callback(self._on_goal_response)
def _on_feedback(self, feedback_msg):
state = feedback_msg.feedback.state
if state:
self.get_logger().info(f'MoveGroup feedback: {state}')
def _on_result(self, future):
result = future.result().result
error_code = result.error_code.val
if error_code == MoveItErrorCodes.SUCCESS:
self.get_logger().info(
f'Motion SUCCEEDED! Planning time: {result.planning_time:.2f}s'
)
self._publish_status({
'status': 'ok',
'source': 'moveit',
'message': f'Motion succeeded (planning: {result.planning_time:.2f}s)',
})
else:
error_name = self._error_code_to_string(error_code)
self.get_logger().error(
f'Motion FAILED: {error_name} (code={error_code})'
)
self._publish_status({
'status': 'error',
'source': 'moveit',
'reason': f'Planning failed: {error_name} (code={error_code})',
})
self._executing = False
def _on_goal_response(self, future):
goal_handle = future.result()
if not goal_handle.accepted:
self.get_logger().error('MoveGroup goal REJECTED')
self._publish_status({
'status': 'error',
'source': 'moveit',
'reason': 'MoveGroup goal rejected by server',
})
self._executing = False
return
self.get_logger().info('MoveGroup goal accepted, waiting for result...')
result_future = goal_handle.get_result_async()
result_future.add_done_callback(self._on_result)
def _publish_status(self, data: dict):
"""Publish a status dict as JSON string to /robot_status."""
import json
msg = String()
msg.data = json.dumps(data)
self._status_pub.publish(msg)
@staticmethod
def _error_code_to_string(code: int) -> str:
code_map = {
1: 'SUCCESS',
-1: 'PLANNING_FAILED',
-2: 'INVALID_MOTION_PLAN',
-3: 'MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE',
-4: 'CONTROL_FAILED',
-5: 'UNABLE_TO_ACQUIRE_SENSOR_DATA',
-6: 'TIMED_OUT',
-7: 'PREEMPTED',
-10: 'START_STATE_IN_COLLISION',
-11: 'START_STATE_VIOLATES_PATH_CONSTRAINTS',
-12: 'GOAL_IN_COLLISION',
-13: 'GOAL_VIOLATES_PATH_CONSTRAINTS',
-14: 'GOAL_CONSTRAINTS_VIOLATED',
-15: 'INVALID_GROUP_NAME',
-16: 'INVALID_GOAL_CONSTRAINTS',
-17: 'INVALID_ROBOT_STATE',
-18: 'INVALID_LINK_NAME',
-19: 'INVALID_OBJECT_NAME',
-21: 'FRAME_TRANSFORM_FAILURE',
-22: 'COLLISION_CHECKING_UNAVAILABLE',
-23: 'ROBOT_STATE_STALE',
-24: 'SENSOR_INFO_STALE',
-25: 'COMMUNICATION_FAILURE',
-26: 'START_STATE_INVALID',
-27: 'GOAL_STATE_INVALID',
-28: 'UNRECOGNIZED_GOAL_TYPE',
-29: 'CRASH',
-30: 'ABORT',
-31: 'NO_IK_SOLUTION',
99999: 'FAILURE',
}
return code_map.get(code, f'UNKNOWN({code})')
def main():
rclpy.init()
node = MoveItGoalNode()
from rclpy.executors import MultiThreadedExecutor
executor = MultiThreadedExecutor()
executor.add_node(node)
try:
executor.spin()
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()수정 사항
- __init__에 /robot_status publisher 추가
- _on_result: 성공/실패를 JSON으로 /robot_status에 publish → Unreal의 기존 OnRobotStatus() 핸들러가 자동으로 뷰포트에 표시
- _on_goal_response: goal 거부 시에도 /robot_status에 에러 publish
- _pre_send_check: action server 불가 시 /robot_status에 에러 publish
- _publish_status() 헬퍼 메서드 추가
모두 수정 했으면 언리얼 빌드 확인
1-1단계: heartbeat timeout 구현
heartbeat timeout을 구현한다. bridge_node에서 주기적으로 ping 명령을 보내고, worker가 마지막 ping 시각을 기록하여 timeout을 감지하는 방식
- ros_bridge_node.py - 주기적 ping을 worker에 전송 (2초마다)
- lerobot_worker.py - 마지막 명령 수신 시각 기록 + timeout(5초) 시 자동 E-stop
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.zmq_req = self.zmq_ctx.socket(zmq.REQ)
self.zmq_req.connect(args.req_addr)
self.zmq_req.setsockopt(zmq.RCVTIMEO, 1000) # 1s timeout for replies
self.get_logger().info(f"ZMQ REQ connected to {args.req_addr}")
# --- 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
# --- 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)
# 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)
state_changed = (worker_state != self.last_worker_state or
worker_teleop != self.last_worker_teleop)
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"]
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}")
# 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 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
except zmq.Again:
self.get_logger().warn("Heartbeat ping timeout — worker may be unresponsive")
except Exception as e:
self.get_logger().warn(f"Heartbeat ping error: {e}")
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()
수정 내역
- __init__에 self.heartbeat_timer = self.create_timer(2.0, self.send_heartbeat) 추가
- send_heartbeat() 메서드 추가
2초마다 {"cmd":"ping"}을 worker REP에 전송. 타임아웃이나 에러 발생 시 경고 로그만 출력 (bridge_node 자체는 중단하지 않음)
lerobot_worker.py 수정
더보기
#!/usr/bin/env python3
"""
lerobot_worker.py — LeRobot ↔ ZMQ bridge worker
Runs in conda env 'lerobot' (Python 3.12).
Reads joint states from SO-ARM-101 via LeRobot API at ~30Hz,
publishes them over ZMQ PUB, and accepts commands via ZMQ REP.
Supports record/replay of joint trajectories via ZMQ commands:
- start_teleop: activate leader→follower sync (teleop)
- stop_teleop: deactivate teleop sync
- start_record: begin teleop + record joint frames
- stop_record: stop recording, save to ~/recordings/ (teleop stays on)
- start_replay: replay a saved recording on the follower arm (teleop off)
- stop_replay: stop replay immediately
- estop: emergency stop — abort all motion immediately
- list_recordings: list saved recording files
Usage:
conda activate lerobot
sudo chmod 666 /dev/ttyACM*
python scripts/lerobot_worker.py [--follower-port /dev/ttyACM0] [--leader-port /dev/ttyACM1]
[--pub-addr tcp://127.0.0.1:5555]
[--rep-addr tcp://127.0.0.1:5556]
[--rate 30]
[--follower-only | --leader-only]
[--teleop]
[--recordings-dir ~/recordings]
"""
import argparse
import json
import math
import os
import pathlib
import signal
import sys
import time
import traceback
from datetime import datetime
import zmq
# ---------------------------------------------------------------------------
# Joint name ordering (matches URDF & LeRobot calibration — Phase 3.2 verified)
# ---------------------------------------------------------------------------
JOINT_NAMES = [
"shoulder_pan",
"shoulder_lift",
"elbow_flex",
"wrist_flex",
"wrist_roll",
"gripper",
]
# ---------------------------------------------------------------------------
# Joint limits in LeRobot DEGREES (empirically measured via calibration).
# These are absolute physical limits — commands are never allowed beyond these.
# Measured by reading get_observation() degrees at calibration range_min/max
# raw encoder positions, using two-point linear interpolation.
# NOTE: These are in LeRobot's coordinate frame (0° = calibration midpoint),
# NOT URDF coordinates (which use a different zero point).
# ---------------------------------------------------------------------------
JOINT_LIMITS_DEG = {
"shoulder_pan": (-119.91, +119.91),
"shoulder_lift": (-104.62, +104.62),
"elbow_flex": ( -97.01, +97.01),
"wrist_flex": (-102.68, +102.68),
"wrist_roll": (-180.00, +180.00),
"gripper": ( +0.14, +99.45),
}
def build_args():
p = argparse.ArgumentParser(description="LeRobot ZMQ worker for SO-ARM-101")
p.add_argument("--follower-port", default="/dev/ttyACM0",
help="Serial port for follower arm")
p.add_argument("--leader-port", default="/dev/ttyACM1",
help="Serial port for leader arm")
p.add_argument("--follower-id", default="so101_twin_follower",
help="Calibration ID for follower")
p.add_argument("--leader-id", default="so101_twin_leader",
help="Calibration ID for leader")
p.add_argument("--pub-addr", default="tcp://127.0.0.1:5555",
help="ZMQ PUB bind address for joint states")
p.add_argument("--rep-addr", default="tcp://127.0.0.1:5556",
help="ZMQ REP bind address for commands")
p.add_argument("--rate", type=float, default=30.0,
help="Read loop frequency in Hz")
p.add_argument("--follower-only", action="store_true",
help="Only connect follower (no leader)")
p.add_argument("--leader-only", action="store_true",
help="Only connect leader (no follower)")
p.add_argument("--teleop", action="store_true",
help="Teleoperation mode: leader arm controls follower arm")
p.add_argument("--cmd-limit", type=float, default=5.0,
help="Safety clamp: max degrees from initial position per joint (default 5.0)")
p.add_argument("--cmd-joints", default="shoulder_pan",
help="Comma-separated joint names that accept commands "
"(default: shoulder_pan). Use 'all' for all joints.")
p.add_argument("--recordings-dir", default="~/recordings",
help="Directory to save/load recorded trajectories")
p.add_argument("--approach-speed", type=float, default=45.0,
help="Max degrees/sec for approach moves before replay "
"and loop transitions (default: 45.0)")
return p.parse_args()
class LeRobotWorker:
def __init__(self, args):
self.args = args
self.running = True
self.follower = None
self.leader = None
self.teleop = args.teleop
# Safety clamp: initial follower position recorded at startup,
# commands are clamped to ±cmd_limit degrees from this baseline.
# Only joints listed in allowed_cmd_joints accept commands;
# others are silently ignored (held at current position).
self.follower_initial_pos = None # dict: {joint_name: degrees}
self.cmd_limit_deg = args.cmd_limit # default 5.0°
self.allowed_cmd_joints = (
set(JOINT_NAMES) if args.cmd_joints.strip().lower() == "all"
else set(j.strip() for j in args.cmd_joints.split(",") if j.strip())
)
# ---------------------------------------------------------------------------
# Record / Replay state
# ---------------------------------------------------------------------------
# state: "idle", "recording", "replaying"
self.state = "idle"
# Recording buffer: list of {"ts": float, "joints": {name: deg, ...}}
self.recording_buffer = []
self.recording_start_time = None
# Replay state
self.replay_frames = [] # loaded frames from file
self.replay_index = 0 # current frame index
self.replay_start_time = None # monotonic time when replay started
self.replay_loop = False # whether to loop replay
self.replay_filename = "" # currently playing file
# Approach (smooth transition) state
# "approaching" phase runs before replay and between loop iterations
self.approaching = False # whether we're in approach phase
self.approach_start_pos = {} # joint positions at start of approach
self.approach_target_pos = {} # target joint positions (first frame)
self.approach_start_time = None # monotonic time when approach started
self.approach_duration = 0.0 # calculated duration in seconds
self.approach_speed = args.approach_speed # degrees per second
# Recordings directory
self.recordings_dir = pathlib.Path(os.path.expanduser(args.recordings_dir))
self.recordings_dir.mkdir(parents=True, exist_ok=True)
print(f"[worker] Recordings dir: {self.recordings_dir}")
# ZMQ setup
self.ctx = zmq.Context()
self.pub = self.ctx.socket(zmq.PUB)
self.pub.bind(args.pub_addr)
print(f"[worker] PUB bound on {args.pub_addr}")
self.rep = self.ctx.socket(zmq.REP)
self.rep.bind(args.rep_addr)
print(f"[worker] REP bound on {args.rep_addr}")
# Poller for non-blocking REP check inside the read loop
self.poller = zmq.Poller()
self.poller.register(self.rep, zmq.POLLIN)
# Heartbeat: track last command received from bridge node.
# If no command arrives for heartbeat_timeout_sec, auto E-stop.
# Initialized to current time so we don't trigger immediately at startup.
self.last_cmd_time = time.monotonic()
self.heartbeat_timeout_sec = 5.0
self.heartbeat_triggered = False # avoid spamming E-stop logs
def connect_robots(self):
"""Connect to follower and/or leader via LeRobot API."""
if not self.args.leader_only:
print(f"[worker] Connecting follower on {self.args.follower_port} "
f"(id={self.args.follower_id}) ...")
from lerobot.robots.so_follower import SO101Follower, SO101FollowerConfig
cfg = SO101FollowerConfig(
port=self.args.follower_port,
id=self.args.follower_id,
)
self.follower = SO101Follower(cfg)
self.follower.connect()
print("[worker] Follower connected.")
if not self.args.follower_only:
print(f"[worker] Connecting leader on {self.args.leader_port} "
f"(id={self.args.leader_id}) ...")
from lerobot.teleoperators.so_leader import SOLeader, SO101LeaderConfig
cfg = SO101LeaderConfig(
port=self.args.leader_port,
id=self.args.leader_id,
)
self.leader = SOLeader(cfg)
self.leader.connect()
print("[worker] Leader connected.")
# Validate teleop requirements
if self.teleop:
if self.follower is None or self.leader is None:
print("[worker] ERROR: --teleop requires both follower and leader connected.")
print("[worker] Cannot use --teleop with --follower-only or --leader-only.")
sys.exit(1)
print("[worker] *** TELEOP MODE ENABLED ***")
print("[worker] Leader arm will control follower arm in real-time.")
print("[worker] Move leader arm to starting position, then press Enter to begin...")
input()
print("[worker] Teleop active!")
# Record initial follower position for safety clamping
if self.follower is not None:
init_pos = self.read_follower()
# Load calibration JSON to display physical range info
calib_range = self._load_calibration_range("follower")
if init_pos is not None:
self.follower_initial_pos = dict(init_pos)
# Print calibration range info
if calib_range:
print("[worker] === CALIBRATION RANGE (raw encoder values) ===")
for name in JOINT_NAMES:
if name in calib_range:
rmin, rmax, cur_raw = calib_range[name]
total = rmax - rmin
if cur_raw is not None and total > 0:
pct = (cur_raw - rmin) / total * 100.0
print(f" {name:20s}: raw {cur_raw:4d} "
f"range [{rmin:4d} ~ {rmax:4d}] "
f"({pct:5.1f}% of range)")
else:
print(f" {name:20s}: raw ??? "
f"range [{rmin:4d} ~ {rmax:4d}]")
print("[worker] === SAFETY CLAMP BASELINE (follower initial position) ===")
for name in JOINT_NAMES:
v = init_pos.get(name, 0.0)
soft_lo = v - self.cmd_limit_deg
soft_hi = v + self.cmd_limit_deg
hard_lo, hard_hi = JOINT_LIMITS_DEG.get(name, (-180.0, 180.0))
eff_lo = max(soft_lo, hard_lo)
eff_hi = min(soft_hi, hard_hi)
marker = " <-- CMD OK" if name in self.allowed_cmd_joints else ""
print(f" {name:20s}: {v:+8.2f}° "
f"effective: [{eff_lo:+8.2f}° ~ {eff_hi:+8.2f}°] "
f"(physical: [{hard_lo:+8.2f}°~{hard_hi:+8.2f}°]){marker}")
print(f"[worker] Allowed command joints: {sorted(self.allowed_cmd_joints)}")
print(f"[worker] Clamp limit: ±{self.cmd_limit_deg}° from baseline")
else:
print("[worker] WARNING: Could not read initial follower position. "
"Commands will be REJECTED until baseline is set.")
def _load_calibration_range(self, arm_type="follower"):
"""
Load calibration JSON and read current raw positions.
Returns dict: {joint_name: (range_min, range_max, current_raw)} or None.
"""
if arm_type == "follower":
calib_path = (
pathlib.Path.home()
/ ".cache/huggingface/lerobot/calibration/robots/so_follower"
/ f"{self.args.follower_id}.json"
)
robot = self.follower
else:
calib_path = (
pathlib.Path.home()
/ ".cache/huggingface/lerobot/calibration/teleoperators/so_leader"
/ f"{self.args.leader_id}.json"
)
robot = self.leader
try:
with open(calib_path) as f:
calib = json.load(f)
print(f"[worker] Loaded calibration from {calib_path}")
# Read current raw positions (without normalization)
current_raw = {}
if robot is not None:
try:
raw = robot.bus.sync_read("Present_Position", normalize=False)
current_raw = dict(raw)
except Exception as e:
print(f"[worker] Could not read raw positions: {e}")
result = {}
for name in JOINT_NAMES:
if name in calib:
rmin = calib[name].get("range_min", 0)
rmax = calib[name].get("range_max", 4095)
cur = current_raw.get(name, None)
result[name] = (rmin, rmax, cur)
return result
except FileNotFoundError:
print(f"[worker] Calibration file not found: {calib_path}")
return None
except Exception as e:
print(f"[worker] Error loading calibration: {e}")
return None
def read_follower(self):
"""Read follower joint positions in degrees. Returns dict or None."""
if self.follower is None:
return None
try:
obs = self.follower.get_observation()
return {name: float(obs[f"{name}.pos"]) for name in JOINT_NAMES}
except Exception as e:
print(f"[worker] Follower read error: {e}")
return None
def read_leader(self):
"""Read leader joint positions in degrees. Returns dict or None."""
if self.leader is None:
return None
try:
action = self.leader.get_action()
return {name: float(action[f"{name}.pos"]) for name in JOINT_NAMES}
except Exception as e:
print(f"[worker] Leader read error: {e}")
return None
def send_follower_action(self, joint_positions_deg):
"""
Send joint positions (degrees) to follower via LeRobot send_action().
joint_positions_deg: dict like {"shoulder_pan": 10.0, ...}
"""
if self.follower is None:
return
try:
action_dict = {f"{name}.pos": float(joint_positions_deg[name])
for name in JOINT_NAMES if name in joint_positions_deg}
self.follower.send_action(action_dict)
except Exception as e:
print(f"[worker] Follower send_action error: {e}")
def handle_command(self, msg):
"""
Process a command from the ROS bridge node.
Expected format:
{"cmd": "send_follower_action", "args": {"shoulder_pan": 0.0, ...}}
{"cmd": "ping"}
{"cmd": "start_teleop"}
{"cmd": "stop_teleop"}
{"cmd": "start_record"}
{"cmd": "stop_record"}
{"cmd": "start_replay", "args": {"filename": "...", "loop": false}}
{"cmd": "stop_replay"}
{"cmd": "estop"}
{"cmd": "list_recordings"}
Returns a response dict.
"""
cmd = msg.get("cmd", "")
if cmd == "ping":
return {"status": "ok", "state": self.state,
"teleop": self.teleop, "ts": time.time()}
elif cmd == "estop":
return self._handle_estop()
elif cmd == "start_teleop":
return self._handle_start_teleop()
elif cmd == "stop_teleop":
return self._handle_stop_teleop()
elif cmd == "start_record":
return self._handle_start_record()
elif cmd == "stop_record":
return self._handle_stop_record()
elif cmd == "start_replay":
args = msg.get("args", {})
return self._handle_start_replay(args)
elif cmd == "stop_replay":
return self._handle_stop_replay()
elif cmd == "list_recordings":
return self._handle_list_recordings()
elif cmd == "send_follower_action":
if self.follower is None:
return {"status": "error", "reason": "follower not connected"}
if self.follower_initial_pos is None:
return {"status": "error", "reason": "safety baseline not set (no initial position)"}
joint_args = msg.get("args", {})
if not joint_args:
return {"status": "error", "reason": "empty args"}
try:
# Two-layer safety clamp:
# Layer 1: baseline ± cmd_limit (session safety)
# Layer 2: JOINT_LIMITS_DEG (absolute physical limits, measured empirically)
clamped = {}
rejected = []
for name, target_deg in joint_args.items():
if name not in JOINT_NAMES:
continue
if name not in self.allowed_cmd_joints:
rejected.append(name)
continue
target = float(target_deg)
# Layer 1: baseline ± cmd_limit
baseline = self.follower_initial_pos.get(name, 0.0)
soft_lo = baseline - self.cmd_limit_deg
soft_hi = baseline + self.cmd_limit_deg
# Layer 2: absolute physical limits
hard_lo, hard_hi = JOINT_LIMITS_DEG.get(name, (-180.0, 180.0))
# Effective range = intersection of both
eff_lo = max(soft_lo, hard_lo)
eff_hi = min(soft_hi, hard_hi)
clamped_val = max(eff_lo, min(eff_hi, target))
clamped[name] = clamped_val
if abs(clamped_val - target) > 0.01:
reason = ""
if target < hard_lo or target > hard_hi:
reason = " [PHYSICAL LIMIT]"
else:
reason = " [BASELINE LIMIT]"
print(f"[worker] CLAMP {name}: "
f"requested {target:.2f}° → clamped to {clamped_val:.2f}° "
f"(effective: {eff_lo:.2f}° ~ {eff_hi:.2f}°){reason}")
if not clamped:
return {"status": "error",
"reason": f"no allowed joints in command "
f"(rejected: {rejected}, allowed: {sorted(self.allowed_cmd_joints)})"}
action_dict = {f"{name}.pos": val for name, val in clamped.items()}
self.follower.send_action(action_dict)
resp = {"status": "ok", "clamped": clamped}
if rejected:
resp["rejected_joints"] = rejected
return resp
except Exception as e:
return {"status": "error", "reason": str(e)}
else:
return {"status": "error", "reason": f"unknown cmd: {cmd}"}
# -----------------------------------------------------------------------
# Teleop (Sync) handlers
# -----------------------------------------------------------------------
def _handle_start_teleop(self):
"""Activate leader→follower sync (teleop) with smooth approach."""
if self.state == "replaying":
return {"status": "error",
"reason": "cannot start teleop during replay — stop replay first"}
if self.follower is None or self.leader is None:
return {"status": "error",
"reason": "teleop requires both follower and leader connected"}
if self.teleop:
return {"status": "ok", "state": self.state, "teleop": True,
"msg": "teleop already active"}
if self.state == "syncing":
return {"status": "ok", "state": self.state, "teleop": False,
"msg": "already approaching leader position"}
# Read leader's current position as approach target
leader_pos = self.read_leader()
if leader_pos is None:
# Can't read leader — just turn on teleop directly
self.teleop = True
print("[worker] Teleop (sync) ON — leader → follower (no approach)")
return {"status": "ok", "state": self.state, "teleop": True}
# Start approach to leader position, then activate teleop
self.state = "syncing"
self._start_approach(leader_pos)
if not self.approaching:
# Already close enough — activate teleop immediately
self.state = "idle"
self.teleop = True
print("[worker] Teleop (sync) ON — already aligned")
return {"status": "ok", "state": self.state, "teleop": True}
print("[worker] Syncing: approaching leader position before teleop ON")
return {"status": "ok", "state": "syncing", "teleop": False,
"approach_sec": round(self.approach_duration, 2)}
def _handle_stop_teleop(self):
"""Deactivate leader→follower sync (teleop)."""
if self.state == "recording":
return {"status": "error",
"reason": "cannot stop teleop during recording — stop recording first"}
# Cancel syncing approach if in progress
if self.state == "syncing":
self.approaching = False
self.state = "idle"
print("[worker] Syncing cancelled")
self.teleop = False
print("[worker] Teleop (sync) OFF")
return {"status": "ok", "state": self.state, "teleop": False}
# -----------------------------------------------------------------------
# Record / Replay / E-Stop handlers
# -----------------------------------------------------------------------
def _handle_estop(self):
"""Emergency stop: abort any ongoing motion, return to idle."""
prev_state = self.state
prev_teleop = self.teleop
self.state = "idle"
self.teleop = False
self.recording_buffer = []
self.replay_frames = []
self.replay_index = 0
self.approaching = False
print(f"[worker] *** E-STOP *** (was: state={prev_state}, teleop={prev_teleop})")
return {"status": "ok", "state": "idle", "teleop": False,
"previous_state": prev_state}
def _handle_start_record(self):
"""Start recording: teleop must already be active (Sync On first)."""
if self.state != "idle":
return {"status": "error",
"reason": f"cannot start recording in state '{self.state}'"}
if self.follower is None or self.leader is None:
return {"status": "error",
"reason": "recording requires both follower and leader connected"}
if not self.teleop:
return {"status": "error",
"reason": "teleop (sync) must be active before recording — "
"press Sync On first"}
self.recording_buffer = []
self.recording_start_time = time.monotonic()
self.state = "recording"
print("[worker] *** RECORDING STARTED *** (teleop already active)")
return {"status": "ok", "state": "recording", "teleop": True}
def _handle_stop_record(self):
"""Stop recording: save buffer to file. Teleop stays ON."""
if self.state != "recording":
return {"status": "error",
"reason": f"not recording (state: '{self.state}')"}
num_frames = len(self.recording_buffer)
if num_frames == 0:
self.state = "idle"
# Teleop stays ON — user controls it via Sync button
print("[worker] Recording stopped — 0 frames, nothing saved.")
return {"status": "ok", "state": "idle", "teleop": self.teleop,
"frames": 0, "filename": None}
# Calculate duration from frame timestamps
duration = self.recording_buffer[-1]["ts"] - self.recording_buffer[0]["ts"]
# Build recording file
timestamp_str = datetime.now().strftime("%Y%m%d_%H%M%S")
filename = f"recording_{timestamp_str}.json"
filepath = self.recordings_dir / filename
recording_data = {
"version": 1,
"recorded_at": datetime.now().isoformat(),
"rate_hz": self.args.rate,
"num_frames": num_frames,
"duration_sec": round(duration, 3),
"joint_names": JOINT_NAMES,
"frames": self.recording_buffer,
}
with open(filepath, "w") as f:
json.dump(recording_data, f, indent=2)
self.state = "idle"
# Teleop stays ON — user controls it via Sync button
self.recording_buffer = []
print(f"[worker] Recording saved: {filepath}")
print(f"[worker] {num_frames} frames, {duration:.1f}s")
return {
"status": "ok",
"state": "idle",
"teleop": self.teleop,
"filename": filename,
"frames": num_frames,
"duration_sec": round(duration, 3),
}
def _handle_start_replay(self, args):
"""Load a recording file and start replaying on the follower."""
if self.state != "idle":
return {"status": "error",
"reason": f"cannot start replay in state '{self.state}'"}
if self.follower is None:
return {"status": "error",
"reason": "replay requires follower connected"}
filename = args.get("filename", "")
self.replay_loop = args.get("loop", False)
# Allow overriding approach speed per command
speed = args.get("approach_speed", self.approach_speed)
# If no filename, use the most recent recording
if not filename:
recordings = self._get_recording_files()
if not recordings:
return {"status": "error", "reason": "no recordings found"}
filename = recordings[-1] # most recent (sorted by name = sorted by time)
filepath = self.recordings_dir / filename
if not filepath.exists():
return {"status": "error",
"reason": f"file not found: {filename}"}
try:
with open(filepath) as f:
data = json.load(f)
except Exception as e:
return {"status": "error", "reason": f"failed to load: {e}"}
frames = data.get("frames", [])
if len(frames) < 2:
return {"status": "error",
"reason": f"recording too short ({len(frames)} frames)"}
self.replay_frames = frames
self.replay_index = 0
self.replay_filename = filename
self.teleop = False # disable teleop during replay
self.state = "replaying"
# Start approach phase: smoothly move from current position to first frame
first_frame_joints = frames[0].get("joints", {})
self._start_approach(first_frame_joints, speed)
duration = data.get("duration_sec", 0)
print(f"[worker] *** REPLAY STARTED *** {filename}")
print(f"[worker] {len(frames)} frames, {duration}s, "
f"loop={'yes' if self.replay_loop else 'no'}, "
f"approach={self.approach_duration:.1f}s")
return {
"status": "ok",
"state": "replaying",
"filename": filename,
"frames": len(frames),
"duration_sec": duration,
"loop": self.replay_loop,
"approach_sec": round(self.approach_duration, 2),
}
def _handle_stop_replay(self):
"""Stop replay immediately."""
if self.state != "replaying":
return {"status": "error",
"reason": f"not replaying (state: '{self.state}')"}
self.state = "idle"
self.replay_frames = []
self.replay_index = 0
print(f"[worker] Replay stopped ({self.replay_filename})")
return {"status": "ok", "state": "idle"}
def _handle_list_recordings(self):
"""List all saved recording files."""
files = self._get_recording_files()
# Add metadata summary for each file
summaries = []
for fname in files:
try:
with open(self.recordings_dir / fname) as f:
data = json.load(f)
summaries.append({
"filename": fname,
"frames": data.get("num_frames", 0),
"duration_sec": data.get("duration_sec", 0),
"recorded_at": data.get("recorded_at", ""),
})
except Exception:
summaries.append({"filename": fname})
return {"status": "ok", "recordings": summaries}
def _get_recording_files(self):
"""Get sorted list of recording JSON files."""
files = sorted(
f.name for f in self.recordings_dir.glob("recording_*.json")
if f.is_file()
)
return files
# -----------------------------------------------------------------------
# Approach (smooth transition) logic
# -----------------------------------------------------------------------
def _start_approach(self, target_joints, speed=None):
"""
Begin a smooth approach from current follower position to target_joints.
Duration is calculated from the largest joint angle difference
divided by speed (degrees/sec).
"""
if speed is None:
speed = self.approach_speed
current = self.read_follower()
if current is None:
# Can't read current position — skip approach
self.approaching = False
self.replay_start_time = time.monotonic()
return
self.approach_start_pos = dict(current)
self.approach_target_pos = dict(target_joints)
# Calculate duration based on max joint difference
max_diff = 0.0
for name in JOINT_NAMES:
if name in current and name in target_joints:
diff = abs(target_joints[name] - current[name])
max_diff = max(max_diff, diff)
if max_diff < 1.0:
# Already close enough — skip approach
self.approaching = False
self.replay_start_time = time.monotonic()
return
self.approach_duration = max_diff / speed
# Minimum 0.3s, even for small moves
self.approach_duration = max(self.approach_duration, 0.3)
self.approach_start_time = time.monotonic()
self.approaching = True
print(f"[worker] Approach: {max_diff:.1f}° max diff, "
f"{self.approach_duration:.1f}s at {speed}°/s")
def _tick_approach(self):
"""
Advance the approach interpolation by one tick.
Returns interpolated joint positions, or None if approach is finished.
"""
if not self.approaching:
return None
elapsed = time.monotonic() - self.approach_start_time
t = elapsed / self.approach_duration # 0.0 → 1.0
if t >= 1.0:
# Approach finished — send final target
self.send_follower_action(self.approach_target_pos)
self.approaching = False
self.replay_start_time = time.monotonic()
print("[worker] Approach complete")
return self.read_follower()
# Smooth interpolation using ease-in-out (cosine)
# t=0 → factor=0, t=1 → factor=1, smooth acceleration/deceleration
factor = (1.0 - math.cos(t * math.pi)) / 2.0
interpolated = {}
for name in JOINT_NAMES:
start_val = self.approach_start_pos.get(name, 0.0)
end_val = self.approach_target_pos.get(name, 0.0)
interpolated[name] = start_val + (end_val - start_val) * factor
self.send_follower_action(interpolated)
return self.read_follower()
# -----------------------------------------------------------------------
# Replay tick — called each iteration of the main loop
# -----------------------------------------------------------------------
def _tick_replay(self):
"""
Advance replay by one tick. Handles approach phase, then frame playback.
Returns the follower position after sending (for PUB).
"""
# --- Approach phase: smooth transition to start position ---
if self.approaching:
return self._tick_approach()
# --- Normal replay ---
if not self.replay_frames or self.replay_index >= len(self.replay_frames):
# Replay finished
if self.replay_loop and self.replay_frames:
# Loop: approach back to first frame before restarting
first_frame_joints = self.replay_frames[0].get("joints", {})
self.replay_index = 0
self._start_approach(first_frame_joints)
if self.approaching:
print(f"[worker] Replay loop — approaching start position")
return self.read_follower()
else:
# Already close enough, restart immediately
print(f"[worker] Replay looping ({self.replay_filename})")
else:
self.state = "idle"
print(f"[worker] Replay finished ({self.replay_filename})")
return self.read_follower()
elapsed = time.monotonic() - self.replay_start_time
first_ts = self.replay_frames[0]["ts"]
# Advance to the frame that matches current elapsed time
while self.replay_index < len(self.replay_frames):
frame = self.replay_frames[self.replay_index]
frame_time = frame["ts"] - first_ts
if frame_time <= elapsed:
# Send this frame to follower
joints = frame.get("joints", {})
if joints:
self.send_follower_action(joints)
self.replay_index += 1
else:
break # wait for next tick
# Re-read follower actual position
return self.read_follower()
def check_commands(self):
"""Non-blocking check for incoming REP commands."""
events = dict(self.poller.poll(timeout=0)) # 0 = non-blocking
if self.rep in events:
try:
raw = self.rep.recv_string(zmq.NOBLOCK)
msg = json.loads(raw)
self.last_cmd_time = time.monotonic()
self.heartbeat_triggered = False
response = self.handle_command(msg)
self.rep.send_string(json.dumps(response))
except zmq.Again:
pass
except Exception as e:
# Must always send a reply on REP socket
self.rep.send_string(json.dumps({
"status": "error", "reason": str(e)
}))
def run(self):
"""Main read loop at configured rate."""
period = 1.0 / self.args.rate
seq = 0
teleop_count = 0
print(f"[worker] Starting read loop at {self.args.rate} Hz "
f"(period={period*1000:.1f}ms)")
while self.running:
t_start = time.monotonic()
# --- Replaying mode: advance replay timeline ---
if self.state == "replaying":
follower_pos = self._tick_replay()
leader_pos = self.read_leader()
# --- Syncing mode: approach leader position, then activate teleop ---
elif self.state == "syncing":
if self.approaching:
follower_pos = self._tick_approach()
else:
# Approach finished — activate teleop
self.state = "idle"
self.teleop = True
print("[worker] Teleop (sync) ON — approach complete")
follower_pos = self.read_follower()
leader_pos = self.read_leader()
else:
# Read joint states
follower_pos = self.read_follower()
leader_pos = self.read_leader()
# --- Teleop: send leader position to follower ---
if self.teleop and leader_pos is not None:
self.send_follower_action(leader_pos)
teleop_count += 1
# Re-read follower after sending action to get updated position
follower_pos = self.read_follower()
# --- Recording mode: buffer the current frame ---
if self.state == "recording" and follower_pos is not None:
frame = {
"ts": time.monotonic() - self.recording_start_time,
"joints": dict(follower_pos),
}
self.recording_buffer.append(frame)
# Build and publish message (degrees — bridge node converts to radians)
msg = {
"seq": seq,
"ts": time.time(),
"state": self.state,
"teleop": self.teleop,
}
if follower_pos is not None:
msg["follower"] = follower_pos
if leader_pos is not None:
msg["leader"] = leader_pos
# Add recording/replay metadata
if self.state == "recording":
msg["recording_frames"] = len(self.recording_buffer)
elif self.state == "replaying":
msg["replay_progress"] = {
"index": self.replay_index,
"total": len(self.replay_frames),
"filename": self.replay_filename,
"loop": self.replay_loop,
"approaching": self.approaching,
}
self.pub.send_string(json.dumps(msg))
seq += 1
# Check for commands (non-blocking)
self.check_commands()
# Heartbeat timeout: if no command from bridge for too long,
# auto E-stop to prevent uncontrolled motion when the
# Unreal→rosbridge→bridge path is broken.
# Only triggers when actively doing something (not idle).
if self.state != "idle" or self.teleop:
since_last = time.monotonic() - self.last_cmd_time
if since_last > self.heartbeat_timeout_sec and not self.heartbeat_triggered:
self.heartbeat_triggered = True
print(f"[worker] *** HEARTBEAT TIMEOUT ({since_last:.1f}s) "
f"— auto E-STOP ***")
self._handle_estop()
# Rate limiting
elapsed = time.monotonic() - t_start
sleep_time = period - elapsed
if sleep_time > 0:
time.sleep(sleep_time)
def shutdown(self):
"""Clean shutdown."""
print("\n[worker] Shutting down...")
self.running = False
self.state = "idle"
# Save any in-progress recording
if self.recording_buffer:
try:
self._handle_stop_record()
print("[worker] Saved in-progress recording before shutdown.")
except Exception:
pass
if self.follower is not None:
try:
self.follower.disconnect()
print("[worker] Follower disconnected.")
except Exception:
pass
if self.leader is not None:
try:
self.leader.disconnect()
print("[worker] Leader disconnected.")
except Exception:
pass
self.pub.close()
self.rep.close()
self.ctx.term()
print("[worker] ZMQ cleaned up. Bye.")
def main():
args = build_args()
worker = LeRobotWorker(args)
# Graceful shutdown on Ctrl+C
def sig_handler(sig, frame):
worker.shutdown()
sys.exit(0)
signal.signal(signal.SIGINT, sig_handler)
signal.signal(signal.SIGTERM, sig_handler)
try:
worker.connect_robots()
worker.run()
except Exception:
traceback.print_exc()
worker.shutdown()
sys.exit(1)
if __name__ == "__main__":
main()수정 사항
- __init__에 3개 변수 추가: last_cmd_time, heartbeat_timeout_sec (5초), heartbeat_triggered
- check_commands()에서 명령 수신 시 last_cmd_time 갱신 + heartbeat_triggered 리셋
- run() 메인 루프에서 매 tick마다 체크: idle 상태가 아니거나 teleop이 켜져 있을 때 마지막 명령으로부터 5초 이상 경과하면 자동 E-stop 발동
동작 흐름
bridge_node (2초마다)
→ ping → worker REP
worker:
← ping 수신 → last_cmd_time 갱신
...
bridge_node 죽음 (rosbridge crash 등)
→ ping 안 옴
worker (5초 후):
→ "HEARTBEAT TIMEOUT — auto E-STOP"
→ state=idle, teleop=false, 모든 동작 중단
2단계: 테스트
로봇 연결 후 PowerShell에서
Attach-Both
터미널 1 (conda - worker)
source ~/miniforge3/etc/profile.d/conda.sh
conda activate lerobot
sudo chmod 666 /dev/ttyACM*
cd ~/UnrealRobotics/src/lerobot_ros2_bridge
python scripts/lerobot_worker.py --cmd-joints all --cmd-limit 120
터미널 2 (ROS2 - bridge node)
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
ros2 run lerobot_ros2_bridge bridge_node
터미널 3 (ROS2 - rosbridge)
export PATH=$(echo $PATH | tr ':' '\n' | grep -v miniforge | tr '\n' ':' | sed 's/:$//')
source /opt/ros/humble/setup.bash
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export ROS_LOCALHOST_ONLY=1
ros2 launch rosbridge_server rosbridge_websocket_launch.xml
테스트 1: 연결 끊김 경고
확인 방법: Unreal 뷰포트 좌상단 on-screen 메시지가 뜨는지 확인
- PIE 시작 후 뷰포트에 초록색 "ROS Bridge: Connected" 메시지가 나오는지 확인
- 터미널 3 (rosbridge)을 Ctrl+C로 죽이기
- 뷰포트에 빨간색 "* ROS Bridge DISCONNECTED *" 경고가 나오는지 확인
- 터미널 3에서 rosbridge를 다시 시작
- 몇 초 후 뷰포트에 다시 **초록색 "Connected"**가 나오는지 확인
확인 완료.
테스트 2: Heartbeat timeout 자동 E-stop
- Details 패널에서 Sync On 버튼 클릭 → follower가 leader 위치로 approach
- 뷰포트에 초록색 "Sync ON" 확인
- 터미널 2 (bridge node)를 Ctrl+C로 죽이기
- 터미널 1 (worker) 콘솔을 확인 — 약 5초 후 *** HEARTBEAT TIMEOUT (5.x초) — auto E-STOP *** 출력이 나오는지 확인
- Worker가 자동으로 idle 상태 + teleop off가 되었는지 확인
테스트 후 터미널 2 재시작.
터미널1에서 HEARTBEAT TIMEOUT 확인.
테스트 3: 일반 동작 확인 (기존 기능 깨지지 않았는지)
터미널 2, 3 모두 정상 가동 상태에서:
- Sync On → follower가 leader를 따라가는지
- Start Record → Stop Record → 파일 저장 메시지
- Start Replay → 재생 동작
- EStop → 즉시 정지
뷰포트에 각 상태에 맞는 색상 메시지가 뜨는지도 같이 확인합니다 (초록=Sync ON/저장완료, 시안=replaying, 노랑=중단, 빨강=E-stop).
모든 모드 확인 완료.
2-1단계: 테스트 후 보완점 - 추후 오류 UI를 위한 구간 별 수신 체크
위 단계 중 테스트2에서, bridge node 터미널을 죽여서 E-Stop 되는 상황에 언리얼 뷰포트에는 E-Stop 표시가 되지 않는 것을 확인. 그 이유는 상태 변화를 Unreal에 전달할 경로가 없기 때문.
worker (auto E-stop 발동) → ZMQ PUB → bridge_node (죽음) X→ /robot_status X→ Unreal
추후 로봇 상태 체크 UI 중 연결 확인 및 오류 확인에서 어떤 단계에서 문제가 있는지를 보기 위해서는 모든 단계를 감시해야 한다.
Unreal ←WebSocket→ rosbridge ←DDS→ bridge_node ←ZMQ→ worker ←USB→ 로봇
[1] [2] [3] [4]
[1] WebSocket 연결 부는 이미 OnDisconnected 델리게이트로 구현되어 있다. (rosbridge가 죽으면 감지)
[2~3]: Unreal에서 /robot_status 메시지가 일정 시간 안 오면 "Worker/Bridge 연결 끊김" 경고를 표시
[2~3]에 해당하는 부분을 추가한다. [2]와 [3] 각각에서 끊어짐 이슈가 있을 때를 가정해본다.
bridge_node → /bridge_heartbeat (1초마다 발행)
worker → /joint_states (30Hz, bridge_node 경유)
위의 두 부분이 수신되는 과정에 따라서 어떤 부분이 끊어졌는지를 유추할 수 있다.
| /bridge_heartbeat | /joint_states | 진단 |
| ✅ 수신됨 | ✅ 수신됨 | 전부 정상 |
| ✅ 수신됨 | ❌ 안 옴 | bridge는 살아있고 worker가 죽음 [3] |
| ❌ 안 옴 | ❌ 안 옴 | bridge가 죽음 [2] (worker는 알 수 없음) |
RobotVisualizer.h 수정
더보기
#pragma once
#include "CoreMinimal.h"
#include "GameFramework/Actor.h"
#include "RobotVisualizer.generated.h"
class UStaticMesh;
class UStaticMeshComponent;
class USceneComponent;
class URosBridgeSubsystem;
/**
* 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();
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;
// =================================================================
// 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);
};수정 내역
- bRosBridgeConnected - WebSocket 연결 상태 추적 변수
- CheckConnectionHealth() - 2초 주기로 bridge/worker 상태 체크하는 메서드
- ConnectionHealthTimerHandle - 타이머 핸들
- LastBridgeHeartbeatTime - 마지막 /bridge_heartbeat 수신 시각
- LastJointStatesTime - 마지막 /joint_states 수신 시각
- BridgeHeartbeatTimeoutSec - bridge 타임아웃 4초
- WorkerDataTimeoutSec - worker 데이터 타임아웃 3초
- bBridgeHeartbeatLost - bridge 끊김 경고 중복 방지 플래그
- bWorkerDataLost - worker 끊김 경고 중복 방지 플래그
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"
// =============================================================================
// 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."));
}
}
// =============================================================================
// 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);
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));
}
}
}수정 내역
- BeginPlay: OnDisconnected 델리게이트 바인딩, /bridge_heartbeat 구독, 2초 주기 CheckConnectionHealth 타이머 시작
- EndPlay: OnDisconnected 해제, ConnectionHealthTimerHandle 타이머 해제
- OnRosBridgeConnected (수정): bRosBridgeConnected = true + 뷰포트에 초록색 "Connected" 메시지
- OnRosBridgeDisconnected (신규): bRosBridgeConnected = false, 뷰포트에 빨간색 "DISCONNECTED" 경고 10초간 표시
- CheckConnectionHealth (신규): bridge heartbeat 타임아웃 → 빨간색 "Bridge Node: DOWN", worker 데이터 타임아웃 → 주황색 "Worker: DOWN"
- OnRosMessage (수정): /joint_states 수신 시 LastJointStatesTime + LastBridgeHeartbeatTime 갱신, /bridge_heartbeat 수신 시 LastBridgeHeartbeatTime 갱신, 복구 시 초록색 메시지
- OnRobotStatus (수정): 수신 시 LastBridgeHeartbeatTime + LastJointStatesTime 갱신 + 복구 메시지
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.zmq_req = self.zmq_ctx.socket(zmq.REQ)
self.zmq_req.connect(args.req_addr)
self.zmq_req.setsockopt(zmq.RCVTIMEO, 1000) # 1s timeout for replies
self.get_logger().info(f"ZMQ REQ connected to {args.req_addr}")
# --- 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
# 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)
state_changed = (worker_state != self.last_worker_state or
worker_teleop != self.last_worker_teleop)
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"]
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}")
# 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 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
except zmq.Again:
self.get_logger().warn("Heartbeat ping timeout — worker may be unresponsive")
except Exception as e:
self.get_logger().warn(f"Heartbeat ping error: {e}")
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()수정 내역
- bridge_heartbeat_pub - /bridge_heartbeat 토픽 publisher 추가
- bridge_heartbeat_timer - 1초 주기 타이머 추가
- publish_bridge_heartbeat() (신규) - {"bridge":"alive"} 메시지 매초 발행
- send_heartbeat() (신규) - 2초마다 worker에 {"cmd":"ping"} 전송 (worker heartbeat timeout용)
수정했으면 언리얼 재빌드
판단 로직
- /bridge_heartbeat가 안 오면 → bridge가 죽음 → "Bridge Node: DOWN"
- /bridge_heartbeat는 오는데 /joint_states가 안 오면 → bridge는 살아있고 worker가 죽음 → "Worker: DOWN"
- WebSocket 자체가 끊기면 → rosbridge가 죽음 → "ROS Bridge DISCONNECTED"
2-2단계: 테스트 후 보완점2 - worker 재시작시 bridge_node 소켓 재생성
여기서 또 문제가 되는 부분은 터미널 1이 끊긴 후 다시 연결하면 터미널 2도 끊고 다시 연결해야 한다는 점.
이건 ZMQ의 REQ/REP 패턴 때문이다.
bridge_node의 REQ 소켓이 worker의 REP 소켓에 연결되어 있는데, worker가 죽으면 TCP 연결이 끊어진다. Worker가 다시 시작되면 새로운 REP 소켓이 새 포트에서 bind하지만, bridge_node의 REQ 소켓은 이전 연결이 "보내고 응답 대기 중" 상태로 stuck되고 있는 상황. REQ/REP는 strict한 send-recv-send-recv 순서를 강제하기 때문에, 한 번 꼬이면 복구가 되지 않는다...
bridge_node가 worker 타임아웃을 감지하면 REQ 소켓을 닫고 새로 만드는 로직으로 수정해본다.
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
# 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)
state_changed = (worker_state != self.last_worker_state or
worker_teleop != self.last_worker_teleop)
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"]
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}")
# 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()수정 내역
- req_addr - REQ 주소 저장 (소켓 재생성 시 필요)
- req_consecutive_timeouts - 연속 타임아웃 카운터
- req_max_timeouts = 3 - 3회 연속 타임아웃 시 소켓 리셋
- _create_req_socket() (신규) - REQ 소켓 생성 헬퍼. LINGER=0 설정으로 close 시 블로킹 방지
- _reset_req_socket() (신규) - 기존 소켓 닫고 새로 생성. worker 재시작 후 stuck 상태 복구
- send_heartbeat (수정) - 성공 시 카운터 리셋, 타임아웃 시 카운터 증가 → 3회 초과하면 소켓 리셋
worker 재시작 시 동작 흐름
Worker 죽음
→ bridge heartbeat ping 타임아웃 (1/3, 2/3, 3/3)
→ 3회 연속 타임아웃
→ REQ 소켓 리셋
Worker 다시 시작
→ 새 REQ 소켓이 새 worker에 연결
→ ping 성공
→ bridge_node 재시작 불필요
잘 동작하는 것을 확인했다.
2-3단계: 테스트 후 보완점3 - 리더암 USB 강제 제거시 에러 뷰포트 전파
lerobot_worker.py 수정
더보기
#!/usr/bin/env python3
"""
lerobot_worker.py — LeRobot ↔ ZMQ bridge worker
Runs in conda env 'lerobot' (Python 3.12).
Reads joint states from SO-ARM-101 via LeRobot API at ~30Hz,
publishes them over ZMQ PUB, and accepts commands via ZMQ REP.
Supports record/replay of joint trajectories via ZMQ commands:
- start_teleop: activate leader→follower sync (teleop)
- stop_teleop: deactivate teleop sync
- start_record: begin teleop + record joint frames
- stop_record: stop recording, save to ~/recordings/ (teleop stays on)
- start_replay: replay a saved recording on the follower arm (teleop off)
- stop_replay: stop replay immediately
- estop: emergency stop — abort all motion immediately
- list_recordings: list saved recording files
Usage:
conda activate lerobot
sudo chmod 666 /dev/ttyACM*
python scripts/lerobot_worker.py [--follower-port /dev/ttyACM0] [--leader-port /dev/ttyACM1]
[--pub-addr tcp://127.0.0.1:5555]
[--rep-addr tcp://127.0.0.1:5556]
[--rate 30]
[--follower-only | --leader-only]
[--teleop]
[--recordings-dir ~/recordings]
"""
import argparse
import json
import math
import os
import pathlib
import signal
import sys
import time
import traceback
from datetime import datetime
import zmq
# ---------------------------------------------------------------------------
# Joint name ordering (matches URDF & LeRobot calibration — Phase 3.2 verified)
# ---------------------------------------------------------------------------
JOINT_NAMES = [
"shoulder_pan",
"shoulder_lift",
"elbow_flex",
"wrist_flex",
"wrist_roll",
"gripper",
]
# ---------------------------------------------------------------------------
# Joint limits in LeRobot DEGREES (empirically measured via calibration).
# These are absolute physical limits — commands are never allowed beyond these.
# Measured by reading get_observation() degrees at calibration range_min/max
# raw encoder positions, using two-point linear interpolation.
# NOTE: These are in LeRobot's coordinate frame (0° = calibration midpoint),
# NOT URDF coordinates (which use a different zero point).
# ---------------------------------------------------------------------------
JOINT_LIMITS_DEG = {
"shoulder_pan": (-119.91, +119.91),
"shoulder_lift": (-104.62, +104.62),
"elbow_flex": ( -97.01, +97.01),
"wrist_flex": (-102.68, +102.68),
"wrist_roll": (-180.00, +180.00),
"gripper": ( +0.14, +99.45),
}
def build_args():
p = argparse.ArgumentParser(description="LeRobot ZMQ worker for SO-ARM-101")
p.add_argument("--follower-port", default="/dev/ttyACM0",
help="Serial port for follower arm")
p.add_argument("--leader-port", default="/dev/ttyACM1",
help="Serial port for leader arm")
p.add_argument("--follower-id", default="so101_twin_follower",
help="Calibration ID for follower")
p.add_argument("--leader-id", default="so101_twin_leader",
help="Calibration ID for leader")
p.add_argument("--pub-addr", default="tcp://127.0.0.1:5555",
help="ZMQ PUB bind address for joint states")
p.add_argument("--rep-addr", default="tcp://127.0.0.1:5556",
help="ZMQ REP bind address for commands")
p.add_argument("--rate", type=float, default=30.0,
help="Read loop frequency in Hz")
p.add_argument("--follower-only", action="store_true",
help="Only connect follower (no leader)")
p.add_argument("--leader-only", action="store_true",
help="Only connect leader (no follower)")
p.add_argument("--teleop", action="store_true",
help="Teleoperation mode: leader arm controls follower arm")
p.add_argument("--cmd-limit", type=float, default=5.0,
help="Safety clamp: max degrees from initial position per joint (default 5.0)")
p.add_argument("--cmd-joints", default="shoulder_pan",
help="Comma-separated joint names that accept commands "
"(default: shoulder_pan). Use 'all' for all joints.")
p.add_argument("--recordings-dir", default="~/recordings",
help="Directory to save/load recorded trajectories")
p.add_argument("--approach-speed", type=float, default=45.0,
help="Max degrees/sec for approach moves before replay "
"and loop transitions (default: 45.0)")
return p.parse_args()
class LeRobotWorker:
def __init__(self, args):
self.args = args
self.running = True
self.follower = None
self.leader = None
self.teleop = args.teleop
# Safety clamp: initial follower position recorded at startup,
# commands are clamped to ±cmd_limit degrees from this baseline.
# Only joints listed in allowed_cmd_joints accept commands;
# others are silently ignored (held at current position).
self.follower_initial_pos = None # dict: {joint_name: degrees}
self.cmd_limit_deg = args.cmd_limit # default 5.0°
self.allowed_cmd_joints = (
set(JOINT_NAMES) if args.cmd_joints.strip().lower() == "all"
else set(j.strip() for j in args.cmd_joints.split(",") if j.strip())
)
# ---------------------------------------------------------------------------
# Record / Replay state
# ---------------------------------------------------------------------------
# state: "idle", "recording", "replaying"
self.state = "idle"
# Recording buffer: list of {"ts": float, "joints": {name: deg, ...}}
self.recording_buffer = []
self.recording_start_time = None
# Replay state
self.replay_frames = [] # loaded frames from file
self.replay_index = 0 # current frame index
self.replay_start_time = None # monotonic time when replay started
self.replay_loop = False # whether to loop replay
self.replay_filename = "" # currently playing file
# Approach (smooth transition) state
# "approaching" phase runs before replay and between loop iterations
self.approaching = False # whether we're in approach phase
self.approach_start_pos = {} # joint positions at start of approach
self.approach_target_pos = {} # target joint positions (first frame)
self.approach_start_time = None # monotonic time when approach started
self.approach_duration = 0.0 # calculated duration in seconds
self.approach_speed = args.approach_speed # degrees per second
# Recordings directory
self.recordings_dir = pathlib.Path(os.path.expanduser(args.recordings_dir))
self.recordings_dir.mkdir(parents=True, exist_ok=True)
print(f"[worker] Recordings dir: {self.recordings_dir}")
# ZMQ setup
self.ctx = zmq.Context()
self.pub = self.ctx.socket(zmq.PUB)
self.pub.bind(args.pub_addr)
print(f"[worker] PUB bound on {args.pub_addr}")
self.rep = self.ctx.socket(zmq.REP)
self.rep.bind(args.rep_addr)
print(f"[worker] REP bound on {args.rep_addr}")
# Poller for non-blocking REP check inside the read loop
self.poller = zmq.Poller()
self.poller.register(self.rep, zmq.POLLIN)
# Heartbeat: track last command received from bridge node.
# If no command arrives for heartbeat_timeout_sec, auto E-stop.
# Initialized to current time so we don't trigger immediately at startup.
self.last_cmd_time = time.monotonic()
self.heartbeat_timeout_sec = 5.0
self.heartbeat_triggered = False # avoid spamming E-stop logs
# Serial error tracking: consecutive read/write failures per device.
# After error_threshold consecutive failures, report in PUB message.
self.follower_error_count = 0
self.leader_error_count = 0
self.follower_error_msg = ""
self.leader_error_msg = ""
self.error_threshold = 5 # consecutive failures before reporting
def connect_robots(self):
"""Connect to follower and/or leader via LeRobot API."""
if not self.args.leader_only:
print(f"[worker] Connecting follower on {self.args.follower_port} "
f"(id={self.args.follower_id}) ...")
from lerobot.robots.so_follower import SO101Follower, SO101FollowerConfig
cfg = SO101FollowerConfig(
port=self.args.follower_port,
id=self.args.follower_id,
)
self.follower = SO101Follower(cfg)
self.follower.connect()
print("[worker] Follower connected.")
if not self.args.follower_only:
print(f"[worker] Connecting leader on {self.args.leader_port} "
f"(id={self.args.leader_id}) ...")
from lerobot.teleoperators.so_leader import SOLeader, SO101LeaderConfig
cfg = SO101LeaderConfig(
port=self.args.leader_port,
id=self.args.leader_id,
)
self.leader = SOLeader(cfg)
self.leader.connect()
print("[worker] Leader connected.")
# Validate teleop requirements
if self.teleop:
if self.follower is None or self.leader is None:
print("[worker] ERROR: --teleop requires both follower and leader connected.")
print("[worker] Cannot use --teleop with --follower-only or --leader-only.")
sys.exit(1)
print("[worker] *** TELEOP MODE ENABLED ***")
print("[worker] Leader arm will control follower arm in real-time.")
print("[worker] Move leader arm to starting position, then press Enter to begin...")
input()
print("[worker] Teleop active!")
# Record initial follower position for safety clamping
if self.follower is not None:
init_pos = self.read_follower()
# Load calibration JSON to display physical range info
calib_range = self._load_calibration_range("follower")
if init_pos is not None:
self.follower_initial_pos = dict(init_pos)
# Print calibration range info
if calib_range:
print("[worker] === CALIBRATION RANGE (raw encoder values) ===")
for name in JOINT_NAMES:
if name in calib_range:
rmin, rmax, cur_raw = calib_range[name]
total = rmax - rmin
if cur_raw is not None and total > 0:
pct = (cur_raw - rmin) / total * 100.0
print(f" {name:20s}: raw {cur_raw:4d} "
f"range [{rmin:4d} ~ {rmax:4d}] "
f"({pct:5.1f}% of range)")
else:
print(f" {name:20s}: raw ??? "
f"range [{rmin:4d} ~ {rmax:4d}]")
print("[worker] === SAFETY CLAMP BASELINE (follower initial position) ===")
for name in JOINT_NAMES:
v = init_pos.get(name, 0.0)
soft_lo = v - self.cmd_limit_deg
soft_hi = v + self.cmd_limit_deg
hard_lo, hard_hi = JOINT_LIMITS_DEG.get(name, (-180.0, 180.0))
eff_lo = max(soft_lo, hard_lo)
eff_hi = min(soft_hi, hard_hi)
marker = " <-- CMD OK" if name in self.allowed_cmd_joints else ""
print(f" {name:20s}: {v:+8.2f}° "
f"effective: [{eff_lo:+8.2f}° ~ {eff_hi:+8.2f}°] "
f"(physical: [{hard_lo:+8.2f}°~{hard_hi:+8.2f}°]){marker}")
print(f"[worker] Allowed command joints: {sorted(self.allowed_cmd_joints)}")
print(f"[worker] Clamp limit: ±{self.cmd_limit_deg}° from baseline")
else:
print("[worker] WARNING: Could not read initial follower position. "
"Commands will be REJECTED until baseline is set.")
def _load_calibration_range(self, arm_type="follower"):
"""
Load calibration JSON and read current raw positions.
Returns dict: {joint_name: (range_min, range_max, current_raw)} or None.
"""
if arm_type == "follower":
calib_path = (
pathlib.Path.home()
/ ".cache/huggingface/lerobot/calibration/robots/so_follower"
/ f"{self.args.follower_id}.json"
)
robot = self.follower
else:
calib_path = (
pathlib.Path.home()
/ ".cache/huggingface/lerobot/calibration/teleoperators/so_leader"
/ f"{self.args.leader_id}.json"
)
robot = self.leader
try:
with open(calib_path) as f:
calib = json.load(f)
print(f"[worker] Loaded calibration from {calib_path}")
# Read current raw positions (without normalization)
current_raw = {}
if robot is not None:
try:
raw = robot.bus.sync_read("Present_Position", normalize=False)
current_raw = dict(raw)
except Exception as e:
print(f"[worker] Could not read raw positions: {e}")
result = {}
for name in JOINT_NAMES:
if name in calib:
rmin = calib[name].get("range_min", 0)
rmax = calib[name].get("range_max", 4095)
cur = current_raw.get(name, None)
result[name] = (rmin, rmax, cur)
return result
except FileNotFoundError:
print(f"[worker] Calibration file not found: {calib_path}")
return None
except Exception as e:
print(f"[worker] Error loading calibration: {e}")
return None
def read_follower(self):
"""Read follower joint positions in degrees. Returns dict or None."""
if self.follower is None:
return None
try:
obs = self.follower.get_observation()
self.follower_error_count = 0
self.follower_error_msg = ""
return {name: float(obs[f"{name}.pos"]) for name in JOINT_NAMES}
except Exception as e:
self.follower_error_count += 1
self.follower_error_msg = str(e)
if self.follower_error_count <= self.error_threshold:
print(f"[worker] Follower read error: {e}")
elif self.follower_error_count == self.error_threshold + 1:
print(f"[worker] Follower read errors continuing, suppressing logs...")
return None
def read_leader(self):
"""Read leader joint positions in degrees. Returns dict or None."""
if self.leader is None:
return None
try:
action = self.leader.get_action()
self.leader_error_count = 0
self.leader_error_msg = ""
return {name: float(action[f"{name}.pos"]) for name in JOINT_NAMES}
except Exception as e:
self.leader_error_count += 1
self.leader_error_msg = str(e)
if self.leader_error_count <= self.error_threshold:
print(f"[worker] Leader read error: {e}")
elif self.leader_error_count == self.error_threshold + 1:
print(f"[worker] Leader read errors continuing, suppressing logs...")
return None
def send_follower_action(self, joint_positions_deg):
"""
Send joint positions (degrees) to follower via LeRobot send_action().
joint_positions_deg: dict like {"shoulder_pan": 10.0, ...}
"""
if self.follower is None:
return
try:
action_dict = {f"{name}.pos": float(joint_positions_deg[name])
for name in JOINT_NAMES if name in joint_positions_deg}
self.follower.send_action(action_dict)
except Exception as e:
self.follower_error_count += 1
self.follower_error_msg = str(e)
if self.follower_error_count <= self.error_threshold:
print(f"[worker] Follower send_action error: {e}")
elif self.follower_error_count == self.error_threshold + 1:
print(f"[worker] Follower send errors continuing, suppressing logs...")
def handle_command(self, msg):
"""
Process a command from the ROS bridge node.
Expected format:
{"cmd": "send_follower_action", "args": {"shoulder_pan": 0.0, ...}}
{"cmd": "ping"}
{"cmd": "start_teleop"}
{"cmd": "stop_teleop"}
{"cmd": "start_record"}
{"cmd": "stop_record"}
{"cmd": "start_replay", "args": {"filename": "...", "loop": false}}
{"cmd": "stop_replay"}
{"cmd": "estop"}
{"cmd": "list_recordings"}
Returns a response dict.
"""
cmd = msg.get("cmd", "")
if cmd == "ping":
return {"status": "ok", "state": self.state,
"teleop": self.teleop, "ts": time.time()}
elif cmd == "estop":
return self._handle_estop()
elif cmd == "start_teleop":
return self._handle_start_teleop()
elif cmd == "stop_teleop":
return self._handle_stop_teleop()
elif cmd == "start_record":
return self._handle_start_record()
elif cmd == "stop_record":
return self._handle_stop_record()
elif cmd == "start_replay":
args = msg.get("args", {})
return self._handle_start_replay(args)
elif cmd == "stop_replay":
return self._handle_stop_replay()
elif cmd == "list_recordings":
return self._handle_list_recordings()
elif cmd == "send_follower_action":
if self.follower is None:
return {"status": "error", "reason": "follower not connected"}
if self.follower_initial_pos is None:
return {"status": "error", "reason": "safety baseline not set (no initial position)"}
joint_args = msg.get("args", {})
if not joint_args:
return {"status": "error", "reason": "empty args"}
try:
# Two-layer safety clamp:
# Layer 1: baseline ± cmd_limit (session safety)
# Layer 2: JOINT_LIMITS_DEG (absolute physical limits, measured empirically)
clamped = {}
rejected = []
for name, target_deg in joint_args.items():
if name not in JOINT_NAMES:
continue
if name not in self.allowed_cmd_joints:
rejected.append(name)
continue
target = float(target_deg)
# Layer 1: baseline ± cmd_limit
baseline = self.follower_initial_pos.get(name, 0.0)
soft_lo = baseline - self.cmd_limit_deg
soft_hi = baseline + self.cmd_limit_deg
# Layer 2: absolute physical limits
hard_lo, hard_hi = JOINT_LIMITS_DEG.get(name, (-180.0, 180.0))
# Effective range = intersection of both
eff_lo = max(soft_lo, hard_lo)
eff_hi = min(soft_hi, hard_hi)
clamped_val = max(eff_lo, min(eff_hi, target))
clamped[name] = clamped_val
if abs(clamped_val - target) > 0.01:
reason = ""
if target < hard_lo or target > hard_hi:
reason = " [PHYSICAL LIMIT]"
else:
reason = " [BASELINE LIMIT]"
print(f"[worker] CLAMP {name}: "
f"requested {target:.2f}° → clamped to {clamped_val:.2f}° "
f"(effective: {eff_lo:.2f}° ~ {eff_hi:.2f}°){reason}")
if not clamped:
return {"status": "error",
"reason": f"no allowed joints in command "
f"(rejected: {rejected}, allowed: {sorted(self.allowed_cmd_joints)})"}
action_dict = {f"{name}.pos": val for name, val in clamped.items()}
self.follower.send_action(action_dict)
resp = {"status": "ok", "clamped": clamped}
if rejected:
resp["rejected_joints"] = rejected
return resp
except Exception as e:
return {"status": "error", "reason": str(e)}
else:
return {"status": "error", "reason": f"unknown cmd: {cmd}"}
# -----------------------------------------------------------------------
# Teleop (Sync) handlers
# -----------------------------------------------------------------------
def _handle_start_teleop(self):
"""Activate leader→follower sync (teleop) with smooth approach."""
if self.state == "replaying":
return {"status": "error",
"reason": "cannot start teleop during replay — stop replay first"}
if self.follower is None or self.leader is None:
return {"status": "error",
"reason": "teleop requires both follower and leader connected"}
if self.teleop:
return {"status": "ok", "state": self.state, "teleop": True,
"msg": "teleop already active"}
if self.state == "syncing":
return {"status": "ok", "state": self.state, "teleop": False,
"msg": "already approaching leader position"}
# Read leader's current position as approach target
leader_pos = self.read_leader()
if leader_pos is None:
# Can't read leader — just turn on teleop directly
self.teleop = True
print("[worker] Teleop (sync) ON — leader → follower (no approach)")
return {"status": "ok", "state": self.state, "teleop": True}
# Start approach to leader position, then activate teleop
self.state = "syncing"
self._start_approach(leader_pos)
if not self.approaching:
# Already close enough — activate teleop immediately
self.state = "idle"
self.teleop = True
print("[worker] Teleop (sync) ON — already aligned")
return {"status": "ok", "state": self.state, "teleop": True}
print("[worker] Syncing: approaching leader position before teleop ON")
return {"status": "ok", "state": "syncing", "teleop": False,
"approach_sec": round(self.approach_duration, 2)}
def _handle_stop_teleop(self):
"""Deactivate leader→follower sync (teleop)."""
if self.state == "recording":
return {"status": "error",
"reason": "cannot stop teleop during recording — stop recording first"}
# Cancel syncing approach if in progress
if self.state == "syncing":
self.approaching = False
self.state = "idle"
print("[worker] Syncing cancelled")
self.teleop = False
print("[worker] Teleop (sync) OFF")
return {"status": "ok", "state": self.state, "teleop": False}
# -----------------------------------------------------------------------
# Record / Replay / E-Stop handlers
# -----------------------------------------------------------------------
def _handle_estop(self):
"""Emergency stop: abort any ongoing motion, return to idle."""
prev_state = self.state
prev_teleop = self.teleop
self.state = "idle"
self.teleop = False
self.recording_buffer = []
self.replay_frames = []
self.replay_index = 0
self.approaching = False
print(f"[worker] *** E-STOP *** (was: state={prev_state}, teleop={prev_teleop})")
return {"status": "ok", "state": "idle", "teleop": False,
"previous_state": prev_state}
def _handle_start_record(self):
"""Start recording: teleop must already be active (Sync On first)."""
if self.state != "idle":
return {"status": "error",
"reason": f"cannot start recording in state '{self.state}'"}
if self.follower is None or self.leader is None:
return {"status": "error",
"reason": "recording requires both follower and leader connected"}
if not self.teleop:
return {"status": "error",
"reason": "teleop (sync) must be active before recording — "
"press Sync On first"}
self.recording_buffer = []
self.recording_start_time = time.monotonic()
self.state = "recording"
print("[worker] *** RECORDING STARTED *** (teleop already active)")
return {"status": "ok", "state": "recording", "teleop": True}
def _handle_stop_record(self):
"""Stop recording: save buffer to file. Teleop stays ON."""
if self.state != "recording":
return {"status": "error",
"reason": f"not recording (state: '{self.state}')"}
num_frames = len(self.recording_buffer)
if num_frames == 0:
self.state = "idle"
# Teleop stays ON — user controls it via Sync button
print("[worker] Recording stopped — 0 frames, nothing saved.")
return {"status": "ok", "state": "idle", "teleop": self.teleop,
"frames": 0, "filename": None}
# Calculate duration from frame timestamps
duration = self.recording_buffer[-1]["ts"] - self.recording_buffer[0]["ts"]
# Build recording file
timestamp_str = datetime.now().strftime("%Y%m%d_%H%M%S")
filename = f"recording_{timestamp_str}.json"
filepath = self.recordings_dir / filename
recording_data = {
"version": 1,
"recorded_at": datetime.now().isoformat(),
"rate_hz": self.args.rate,
"num_frames": num_frames,
"duration_sec": round(duration, 3),
"joint_names": JOINT_NAMES,
"frames": self.recording_buffer,
}
with open(filepath, "w") as f:
json.dump(recording_data, f, indent=2)
self.state = "idle"
# Teleop stays ON — user controls it via Sync button
self.recording_buffer = []
print(f"[worker] Recording saved: {filepath}")
print(f"[worker] {num_frames} frames, {duration:.1f}s")
return {
"status": "ok",
"state": "idle",
"teleop": self.teleop,
"filename": filename,
"frames": num_frames,
"duration_sec": round(duration, 3),
}
def _handle_start_replay(self, args):
"""Load a recording file and start replaying on the follower."""
if self.state != "idle":
return {"status": "error",
"reason": f"cannot start replay in state '{self.state}'"}
if self.follower is None:
return {"status": "error",
"reason": "replay requires follower connected"}
filename = args.get("filename", "")
self.replay_loop = args.get("loop", False)
# Allow overriding approach speed per command
speed = args.get("approach_speed", self.approach_speed)
# If no filename, use the most recent recording
if not filename:
recordings = self._get_recording_files()
if not recordings:
return {"status": "error", "reason": "no recordings found"}
filename = recordings[-1] # most recent (sorted by name = sorted by time)
filepath = self.recordings_dir / filename
if not filepath.exists():
return {"status": "error",
"reason": f"file not found: {filename}"}
try:
with open(filepath) as f:
data = json.load(f)
except Exception as e:
return {"status": "error", "reason": f"failed to load: {e}"}
frames = data.get("frames", [])
if len(frames) < 2:
return {"status": "error",
"reason": f"recording too short ({len(frames)} frames)"}
self.replay_frames = frames
self.replay_index = 0
self.replay_filename = filename
self.teleop = False # disable teleop during replay
self.state = "replaying"
# Start approach phase: smoothly move from current position to first frame
first_frame_joints = frames[0].get("joints", {})
self._start_approach(first_frame_joints, speed)
duration = data.get("duration_sec", 0)
print(f"[worker] *** REPLAY STARTED *** {filename}")
print(f"[worker] {len(frames)} frames, {duration}s, "
f"loop={'yes' if self.replay_loop else 'no'}, "
f"approach={self.approach_duration:.1f}s")
return {
"status": "ok",
"state": "replaying",
"filename": filename,
"frames": len(frames),
"duration_sec": duration,
"loop": self.replay_loop,
"approach_sec": round(self.approach_duration, 2),
}
def _handle_stop_replay(self):
"""Stop replay immediately."""
if self.state != "replaying":
return {"status": "error",
"reason": f"not replaying (state: '{self.state}')"}
self.state = "idle"
self.replay_frames = []
self.replay_index = 0
print(f"[worker] Replay stopped ({self.replay_filename})")
return {"status": "ok", "state": "idle"}
def _handle_list_recordings(self):
"""List all saved recording files."""
files = self._get_recording_files()
# Add metadata summary for each file
summaries = []
for fname in files:
try:
with open(self.recordings_dir / fname) as f:
data = json.load(f)
summaries.append({
"filename": fname,
"frames": data.get("num_frames", 0),
"duration_sec": data.get("duration_sec", 0),
"recorded_at": data.get("recorded_at", ""),
})
except Exception:
summaries.append({"filename": fname})
return {"status": "ok", "recordings": summaries}
def _get_recording_files(self):
"""Get sorted list of recording JSON files."""
files = sorted(
f.name for f in self.recordings_dir.glob("recording_*.json")
if f.is_file()
)
return files
# -----------------------------------------------------------------------
# Approach (smooth transition) logic
# -----------------------------------------------------------------------
def _start_approach(self, target_joints, speed=None):
"""
Begin a smooth approach from current follower position to target_joints.
Duration is calculated from the largest joint angle difference
divided by speed (degrees/sec).
"""
if speed is None:
speed = self.approach_speed
current = self.read_follower()
if current is None:
# Can't read current position — skip approach
self.approaching = False
self.replay_start_time = time.monotonic()
return
self.approach_start_pos = dict(current)
self.approach_target_pos = dict(target_joints)
# Calculate duration based on max joint difference
max_diff = 0.0
for name in JOINT_NAMES:
if name in current and name in target_joints:
diff = abs(target_joints[name] - current[name])
max_diff = max(max_diff, diff)
if max_diff < 1.0:
# Already close enough — skip approach
self.approaching = False
self.replay_start_time = time.monotonic()
return
self.approach_duration = max_diff / speed
# Minimum 0.3s, even for small moves
self.approach_duration = max(self.approach_duration, 0.3)
self.approach_start_time = time.monotonic()
self.approaching = True
print(f"[worker] Approach: {max_diff:.1f}° max diff, "
f"{self.approach_duration:.1f}s at {speed}°/s")
def _tick_approach(self):
"""
Advance the approach interpolation by one tick.
Returns interpolated joint positions, or None if approach is finished.
"""
if not self.approaching:
return None
elapsed = time.monotonic() - self.approach_start_time
t = elapsed / self.approach_duration # 0.0 → 1.0
if t >= 1.0:
# Approach finished — send final target
self.send_follower_action(self.approach_target_pos)
self.approaching = False
self.replay_start_time = time.monotonic()
print("[worker] Approach complete")
return self.read_follower()
# Smooth interpolation using ease-in-out (cosine)
# t=0 → factor=0, t=1 → factor=1, smooth acceleration/deceleration
factor = (1.0 - math.cos(t * math.pi)) / 2.0
interpolated = {}
for name in JOINT_NAMES:
start_val = self.approach_start_pos.get(name, 0.0)
end_val = self.approach_target_pos.get(name, 0.0)
interpolated[name] = start_val + (end_val - start_val) * factor
self.send_follower_action(interpolated)
return self.read_follower()
# -----------------------------------------------------------------------
# Replay tick — called each iteration of the main loop
# -----------------------------------------------------------------------
def _tick_replay(self):
"""
Advance replay by one tick. Handles approach phase, then frame playback.
Returns the follower position after sending (for PUB).
"""
# --- Approach phase: smooth transition to start position ---
if self.approaching:
return self._tick_approach()
# --- Normal replay ---
if not self.replay_frames or self.replay_index >= len(self.replay_frames):
# Replay finished
if self.replay_loop and self.replay_frames:
# Loop: approach back to first frame before restarting
first_frame_joints = self.replay_frames[0].get("joints", {})
self.replay_index = 0
self._start_approach(first_frame_joints)
if self.approaching:
print(f"[worker] Replay loop — approaching start position")
return self.read_follower()
else:
# Already close enough, restart immediately
print(f"[worker] Replay looping ({self.replay_filename})")
else:
self.state = "idle"
print(f"[worker] Replay finished ({self.replay_filename})")
return self.read_follower()
elapsed = time.monotonic() - self.replay_start_time
first_ts = self.replay_frames[0]["ts"]
# Advance to the frame that matches current elapsed time
while self.replay_index < len(self.replay_frames):
frame = self.replay_frames[self.replay_index]
frame_time = frame["ts"] - first_ts
if frame_time <= elapsed:
# Send this frame to follower
joints = frame.get("joints", {})
if joints:
self.send_follower_action(joints)
self.replay_index += 1
else:
break # wait for next tick
# Re-read follower actual position
return self.read_follower()
def check_commands(self):
"""Non-blocking check for incoming REP commands."""
events = dict(self.poller.poll(timeout=0)) # 0 = non-blocking
if self.rep in events:
try:
raw = self.rep.recv_string(zmq.NOBLOCK)
msg = json.loads(raw)
self.last_cmd_time = time.monotonic()
self.heartbeat_triggered = False
response = self.handle_command(msg)
self.rep.send_string(json.dumps(response))
except zmq.Again:
pass
except Exception as e:
# Must always send a reply on REP socket
self.rep.send_string(json.dumps({
"status": "error", "reason": str(e)
}))
def run(self):
"""Main read loop at configured rate."""
period = 1.0 / self.args.rate
seq = 0
teleop_count = 0
print(f"[worker] Starting read loop at {self.args.rate} Hz "
f"(period={period*1000:.1f}ms)")
while self.running:
t_start = time.monotonic()
# --- Replaying mode: advance replay timeline ---
if self.state == "replaying":
follower_pos = self._tick_replay()
leader_pos = self.read_leader()
# --- Syncing mode: approach leader position, then activate teleop ---
elif self.state == "syncing":
if self.approaching:
follower_pos = self._tick_approach()
else:
# Approach finished — activate teleop
self.state = "idle"
self.teleop = True
print("[worker] Teleop (sync) ON — approach complete")
follower_pos = self.read_follower()
leader_pos = self.read_leader()
else:
# Read joint states
follower_pos = self.read_follower()
leader_pos = self.read_leader()
# --- Teleop: send leader position to follower ---
if self.teleop and leader_pos is not None:
self.send_follower_action(leader_pos)
teleop_count += 1
# Re-read follower after sending action to get updated position
follower_pos = self.read_follower()
# --- Recording mode: buffer the current frame ---
if self.state == "recording" and follower_pos is not None:
frame = {
"ts": time.monotonic() - self.recording_start_time,
"joints": dict(follower_pos),
}
self.recording_buffer.append(frame)
# Build and publish message (degrees — bridge node converts to radians)
msg = {
"seq": seq,
"ts": time.time(),
"state": self.state,
"teleop": self.teleop,
}
if follower_pos is not None:
msg["follower"] = follower_pos
if leader_pos is not None:
msg["leader"] = leader_pos
# Report device errors if threshold exceeded
device_errors = {}
if self.follower_error_count >= self.error_threshold:
device_errors["follower"] = self.follower_error_msg
if self.leader_error_count >= self.error_threshold:
device_errors["leader"] = self.leader_error_msg
if device_errors:
msg["device_errors"] = device_errors
# Add recording/replay metadata
if self.state == "recording":
msg["recording_frames"] = len(self.recording_buffer)
elif self.state == "replaying":
msg["replay_progress"] = {
"index": self.replay_index,
"total": len(self.replay_frames),
"filename": self.replay_filename,
"loop": self.replay_loop,
"approaching": self.approaching,
}
self.pub.send_string(json.dumps(msg))
seq += 1
# Check for commands (non-blocking)
self.check_commands()
# Heartbeat timeout: if no command from bridge for too long,
# auto E-stop to prevent uncontrolled motion when the
# Unreal→rosbridge→bridge path is broken.
# Only triggers when actively doing something (not idle).
if self.state != "idle" or self.teleop:
since_last = time.monotonic() - self.last_cmd_time
if since_last > self.heartbeat_timeout_sec and not self.heartbeat_triggered:
self.heartbeat_triggered = True
print(f"[worker] *** HEARTBEAT TIMEOUT ({since_last:.1f}s) "
f"— auto E-STOP ***")
self._handle_estop()
# Rate limiting
elapsed = time.monotonic() - t_start
sleep_time = period - elapsed
if sleep_time > 0:
time.sleep(sleep_time)
def shutdown(self):
"""Clean shutdown."""
print("\n[worker] Shutting down...")
self.running = False
self.state = "idle"
# Save any in-progress recording
if self.recording_buffer:
try:
self._handle_stop_record()
print("[worker] Saved in-progress recording before shutdown.")
except Exception:
pass
if self.follower is not None:
try:
self.follower.disconnect()
print("[worker] Follower disconnected.")
except Exception:
pass
if self.leader is not None:
try:
self.leader.disconnect()
print("[worker] Leader disconnected.")
except Exception:
pass
self.pub.close()
self.rep.close()
self.ctx.term()
print("[worker] ZMQ cleaned up. Bye.")
def main():
args = build_args()
worker = LeRobotWorker(args)
# Graceful shutdown on Ctrl+C
def sig_handler(sig, frame):
worker.shutdown()
sys.exit(0)
signal.signal(signal.SIGINT, sig_handler)
signal.signal(signal.SIGTERM, sig_handler)
try:
worker.connect_robots()
worker.run()
except Exception:
traceback.print_exc()
worker.shutdown()
sys.exit(1)
if __name__ == "__main__":
main()변경 사항
- follower_error_count, leader_error_count: 연속 에러 카운터
- follower_error_msg, leader_error_msg: 최근 에러 메시지 저장
- error_threshold = 5: 5회 연속 실패 후 PUB에 에러 보고
- read_follower (수정): 성공 시 카운터 리셋, 실패 시 카운터 증가 + threshold 초과 후 로그 억제
- read_leader (수정): 동일
- send_follower_action (수정): 동일
- PUB 메시지에 device_errors 필드 추가 (예: {"device_errors": {"leader": "Port is in use!"}})
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 = {}
# 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}")
# 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()변경 사항
- last_device_errors: 이전 에러 상태 추적
- poll_zmq (수정): device_errors 변화 감지 시 /robot_status에 포함하여 publish
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"
// =============================================================================
// 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."));
}
}
// =============================================================================
// 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);
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;
if (StatusJson->TryGetObjectField(TEXT("device_errors"), DeviceErrors) && DeviceErrors)
{
FString FollowerErr, LeaderErr;
(*DeviceErrors)->TryGetStringField(TEXT("follower"), FollowerErr);
(*DeviceErrors)->TryGetStringField(TEXT("leader"), LeaderErr);
if (!FollowerErr.IsEmpty())
{
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."));
}
}
if (!LeaderErr.IsEmpty())
{
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."));
}
}
}
}변경 사항
- OnRobotStatus (수정): device_errors JSON 객체를 파싱하여 follower/leader 각각 빨간색 경고 표시
- "*** Follower: USB/Serial ERROR ***"
- "*** Leader: USB/Serial ERROR ***"
cpp 파일이 업데이트 됐기 때문에 언리얼 빌드를 다시한다.
WSL2 터미널 3개를 열고 언리얼을 실행하여 테스트한다.
리더암 USB를 강제로 뽑았을 때 에러 메시지 출력 확인 완료. 팔로워암도 마찬가지로 확인 완료.
2-4단계: 테스트 후 보완점4 - USB 재연결 시 worker에서 인식 후 재연결
lerobot_worker.py 수정
더보기
#!/usr/bin/env python3
"""
lerobot_worker.py — LeRobot ↔ ZMQ bridge worker
Runs in conda env 'lerobot' (Python 3.12).
Reads joint states from SO-ARM-101 via LeRobot API at ~30Hz,
publishes them over ZMQ PUB, and accepts commands via ZMQ REP.
Supports record/replay of joint trajectories via ZMQ commands:
- start_teleop: activate leader→follower sync (teleop)
- stop_teleop: deactivate teleop sync
- start_record: begin teleop + record joint frames
- stop_record: stop recording, save to ~/recordings/ (teleop stays on)
- start_replay: replay a saved recording on the follower arm (teleop off)
- stop_replay: stop replay immediately
- estop: emergency stop — abort all motion immediately
- list_recordings: list saved recording files
Usage:
conda activate lerobot
sudo chmod 666 /dev/ttyACM*
python scripts/lerobot_worker.py [--follower-port /dev/ttyACM0] [--leader-port /dev/ttyACM1]
[--pub-addr tcp://127.0.0.1:5555]
[--rep-addr tcp://127.0.0.1:5556]
[--rate 30]
[--follower-only | --leader-only]
[--teleop]
[--recordings-dir ~/recordings]
"""
import argparse
import json
import math
import os
import pathlib
import signal
import sys
import time
import traceback
from datetime import datetime
import zmq
# ---------------------------------------------------------------------------
# Joint name ordering (matches URDF & LeRobot calibration — Phase 3.2 verified)
# ---------------------------------------------------------------------------
JOINT_NAMES = [
"shoulder_pan",
"shoulder_lift",
"elbow_flex",
"wrist_flex",
"wrist_roll",
"gripper",
]
# ---------------------------------------------------------------------------
# Joint limits in LeRobot DEGREES (empirically measured via calibration).
# These are absolute physical limits — commands are never allowed beyond these.
# Measured by reading get_observation() degrees at calibration range_min/max
# raw encoder positions, using two-point linear interpolation.
# NOTE: These are in LeRobot's coordinate frame (0° = calibration midpoint),
# NOT URDF coordinates (which use a different zero point).
# ---------------------------------------------------------------------------
JOINT_LIMITS_DEG = {
"shoulder_pan": (-119.91, +119.91),
"shoulder_lift": (-104.62, +104.62),
"elbow_flex": ( -97.01, +97.01),
"wrist_flex": (-102.68, +102.68),
"wrist_roll": (-180.00, +180.00),
"gripper": ( +0.14, +99.45),
}
def build_args():
p = argparse.ArgumentParser(description="LeRobot ZMQ worker for SO-ARM-101")
p.add_argument("--follower-port", default="/dev/ttyACM0",
help="Serial port for follower arm")
p.add_argument("--leader-port", default="/dev/ttyACM1",
help="Serial port for leader arm")
p.add_argument("--follower-id", default="so101_twin_follower",
help="Calibration ID for follower")
p.add_argument("--leader-id", default="so101_twin_leader",
help="Calibration ID for leader")
p.add_argument("--pub-addr", default="tcp://127.0.0.1:5555",
help="ZMQ PUB bind address for joint states")
p.add_argument("--rep-addr", default="tcp://127.0.0.1:5556",
help="ZMQ REP bind address for commands")
p.add_argument("--rate", type=float, default=30.0,
help="Read loop frequency in Hz")
p.add_argument("--follower-only", action="store_true",
help="Only connect follower (no leader)")
p.add_argument("--leader-only", action="store_true",
help="Only connect leader (no follower)")
p.add_argument("--teleop", action="store_true",
help="Teleoperation mode: leader arm controls follower arm")
p.add_argument("--cmd-limit", type=float, default=5.0,
help="Safety clamp: max degrees from initial position per joint (default 5.0)")
p.add_argument("--cmd-joints", default="shoulder_pan",
help="Comma-separated joint names that accept commands "
"(default: shoulder_pan). Use 'all' for all joints.")
p.add_argument("--recordings-dir", default="~/recordings",
help="Directory to save/load recorded trajectories")
p.add_argument("--approach-speed", type=float, default=45.0,
help="Max degrees/sec for approach moves before replay "
"and loop transitions (default: 45.0)")
return p.parse_args()
class LeRobotWorker:
def __init__(self, args):
self.args = args
self.running = True
self.follower = None
self.leader = None
self.teleop = args.teleop
# Safety clamp: initial follower position recorded at startup,
# commands are clamped to ±cmd_limit degrees from this baseline.
# Only joints listed in allowed_cmd_joints accept commands;
# others are silently ignored (held at current position).
self.follower_initial_pos = None # dict: {joint_name: degrees}
self.cmd_limit_deg = args.cmd_limit # default 5.0°
self.allowed_cmd_joints = (
set(JOINT_NAMES) if args.cmd_joints.strip().lower() == "all"
else set(j.strip() for j in args.cmd_joints.split(",") if j.strip())
)
# ---------------------------------------------------------------------------
# Record / Replay state
# ---------------------------------------------------------------------------
# state: "idle", "recording", "replaying"
self.state = "idle"
# Recording buffer: list of {"ts": float, "joints": {name: deg, ...}}
self.recording_buffer = []
self.recording_start_time = None
# Replay state
self.replay_frames = [] # loaded frames from file
self.replay_index = 0 # current frame index
self.replay_start_time = None # monotonic time when replay started
self.replay_loop = False # whether to loop replay
self.replay_filename = "" # currently playing file
# Approach (smooth transition) state
# "approaching" phase runs before replay and between loop iterations
self.approaching = False # whether we're in approach phase
self.approach_start_pos = {} # joint positions at start of approach
self.approach_target_pos = {} # target joint positions (first frame)
self.approach_start_time = None # monotonic time when approach started
self.approach_duration = 0.0 # calculated duration in seconds
self.approach_speed = args.approach_speed # degrees per second
# Recordings directory
self.recordings_dir = pathlib.Path(os.path.expanduser(args.recordings_dir))
self.recordings_dir.mkdir(parents=True, exist_ok=True)
print(f"[worker] Recordings dir: {self.recordings_dir}")
# ZMQ setup
self.ctx = zmq.Context()
self.pub = self.ctx.socket(zmq.PUB)
self.pub.bind(args.pub_addr)
print(f"[worker] PUB bound on {args.pub_addr}")
self.rep = self.ctx.socket(zmq.REP)
self.rep.bind(args.rep_addr)
print(f"[worker] REP bound on {args.rep_addr}")
# Poller for non-blocking REP check inside the read loop
self.poller = zmq.Poller()
self.poller.register(self.rep, zmq.POLLIN)
# Heartbeat: track last command received from bridge node.
# If no command arrives for heartbeat_timeout_sec, auto E-stop.
# Initialized to current time so we don't trigger immediately at startup.
self.last_cmd_time = time.monotonic()
self.heartbeat_timeout_sec = 5.0
self.heartbeat_triggered = False # avoid spamming E-stop logs
# Serial error tracking: consecutive read/write failures per device.
# After error_threshold consecutive failures, report in PUB message.
self.follower_error_count = 0
self.leader_error_count = 0
self.follower_error_msg = ""
self.leader_error_msg = ""
self.error_threshold = 5 # consecutive failures before reporting
# Auto-reconnect: after reconnect_threshold consecutive errors,
# attempt disconnect() → connect(). Retry every reconnect_interval_sec.
self.reconnect_threshold = 30 # ~1 second at 30Hz
self.reconnect_interval_sec = 5.0
self.last_follower_reconnect_time = 0.0
self.last_leader_reconnect_time = 0.0
def connect_robots(self):
"""Connect to follower and/or leader via LeRobot API."""
if not self.args.leader_only:
print(f"[worker] Connecting follower on {self.args.follower_port} "
f"(id={self.args.follower_id}) ...")
from lerobot.robots.so_follower import SO101Follower, SO101FollowerConfig
cfg = SO101FollowerConfig(
port=self.args.follower_port,
id=self.args.follower_id,
)
self.follower = SO101Follower(cfg)
self.follower.connect()
print("[worker] Follower connected.")
if not self.args.follower_only:
print(f"[worker] Connecting leader on {self.args.leader_port} "
f"(id={self.args.leader_id}) ...")
from lerobot.teleoperators.so_leader import SOLeader, SO101LeaderConfig
cfg = SO101LeaderConfig(
port=self.args.leader_port,
id=self.args.leader_id,
)
self.leader = SOLeader(cfg)
self.leader.connect()
print("[worker] Leader connected.")
# Validate teleop requirements
if self.teleop:
if self.follower is None or self.leader is None:
print("[worker] ERROR: --teleop requires both follower and leader connected.")
print("[worker] Cannot use --teleop with --follower-only or --leader-only.")
sys.exit(1)
print("[worker] *** TELEOP MODE ENABLED ***")
print("[worker] Leader arm will control follower arm in real-time.")
print("[worker] Move leader arm to starting position, then press Enter to begin...")
input()
print("[worker] Teleop active!")
# Record initial follower position for safety clamping
if self.follower is not None:
init_pos = self.read_follower()
# Load calibration JSON to display physical range info
calib_range = self._load_calibration_range("follower")
if init_pos is not None:
self.follower_initial_pos = dict(init_pos)
# Print calibration range info
if calib_range:
print("[worker] === CALIBRATION RANGE (raw encoder values) ===")
for name in JOINT_NAMES:
if name in calib_range:
rmin, rmax, cur_raw = calib_range[name]
total = rmax - rmin
if cur_raw is not None and total > 0:
pct = (cur_raw - rmin) / total * 100.0
print(f" {name:20s}: raw {cur_raw:4d} "
f"range [{rmin:4d} ~ {rmax:4d}] "
f"({pct:5.1f}% of range)")
else:
print(f" {name:20s}: raw ??? "
f"range [{rmin:4d} ~ {rmax:4d}]")
print("[worker] === SAFETY CLAMP BASELINE (follower initial position) ===")
for name in JOINT_NAMES:
v = init_pos.get(name, 0.0)
soft_lo = v - self.cmd_limit_deg
soft_hi = v + self.cmd_limit_deg
hard_lo, hard_hi = JOINT_LIMITS_DEG.get(name, (-180.0, 180.0))
eff_lo = max(soft_lo, hard_lo)
eff_hi = min(soft_hi, hard_hi)
marker = " <-- CMD OK" if name in self.allowed_cmd_joints else ""
print(f" {name:20s}: {v:+8.2f}° "
f"effective: [{eff_lo:+8.2f}° ~ {eff_hi:+8.2f}°] "
f"(physical: [{hard_lo:+8.2f}°~{hard_hi:+8.2f}°]){marker}")
print(f"[worker] Allowed command joints: {sorted(self.allowed_cmd_joints)}")
print(f"[worker] Clamp limit: ±{self.cmd_limit_deg}° from baseline")
else:
print("[worker] WARNING: Could not read initial follower position. "
"Commands will be REJECTED until baseline is set.")
def _load_calibration_range(self, arm_type="follower"):
"""
Load calibration JSON and read current raw positions.
Returns dict: {joint_name: (range_min, range_max, current_raw)} or None.
"""
if arm_type == "follower":
calib_path = (
pathlib.Path.home()
/ ".cache/huggingface/lerobot/calibration/robots/so_follower"
/ f"{self.args.follower_id}.json"
)
robot = self.follower
else:
calib_path = (
pathlib.Path.home()
/ ".cache/huggingface/lerobot/calibration/teleoperators/so_leader"
/ f"{self.args.leader_id}.json"
)
robot = self.leader
try:
with open(calib_path) as f:
calib = json.load(f)
print(f"[worker] Loaded calibration from {calib_path}")
# Read current raw positions (without normalization)
current_raw = {}
if robot is not None:
try:
raw = robot.bus.sync_read("Present_Position", normalize=False)
current_raw = dict(raw)
except Exception as e:
print(f"[worker] Could not read raw positions: {e}")
result = {}
for name in JOINT_NAMES:
if name in calib:
rmin = calib[name].get("range_min", 0)
rmax = calib[name].get("range_max", 4095)
cur = current_raw.get(name, None)
result[name] = (rmin, rmax, cur)
return result
except FileNotFoundError:
print(f"[worker] Calibration file not found: {calib_path}")
return None
except Exception as e:
print(f"[worker] Error loading calibration: {e}")
return None
def _try_reconnect_follower(self):
"""Attempt to reconnect follower after serial errors."""
now = time.monotonic()
if now - self.last_follower_reconnect_time < self.reconnect_interval_sec:
return # wait before retrying
self.last_follower_reconnect_time = now
print("[worker] Attempting follower reconnect...")
try:
self.follower.disconnect()
except Exception:
pass
# Force port closed so is_connected returns False and connect() will proceed
try:
self.follower.bus.port_handler.closePort()
except Exception:
pass
try:
self.follower.connect()
self.follower_error_count = 0
self.follower_error_msg = ""
print("[worker] Follower reconnected successfully!")
except Exception as e:
print(f"[worker] Follower reconnect failed: {e}")
def _try_reconnect_leader(self):
"""Attempt to reconnect leader after serial errors."""
now = time.monotonic()
if now - self.last_leader_reconnect_time < self.reconnect_interval_sec:
return # wait before retrying
self.last_leader_reconnect_time = now
print("[worker] Attempting leader reconnect...")
try:
self.leader.disconnect()
except Exception:
pass
# Force port closed so is_connected returns False and connect() will proceed
try:
self.leader.bus.port_handler.closePort()
except Exception:
pass
try:
self.leader.connect()
self.leader_error_count = 0
self.leader_error_msg = ""
print("[worker] Leader reconnected successfully!")
except Exception as e:
print(f"[worker] Leader reconnect failed: {e}")
def read_follower(self):
"""Read follower joint positions in degrees. Returns dict or None."""
if self.follower is None:
return None
# Auto-reconnect if too many consecutive errors
if self.follower_error_count >= self.reconnect_threshold:
self._try_reconnect_follower()
try:
obs = self.follower.get_observation()
self.follower_error_count = 0
self.follower_error_msg = ""
return {name: float(obs[f"{name}.pos"]) for name in JOINT_NAMES}
except Exception as e:
self.follower_error_count += 1
self.follower_error_msg = str(e)
if self.follower_error_count <= self.error_threshold:
print(f"[worker] Follower read error: {e}")
elif self.follower_error_count == self.error_threshold + 1:
print(f"[worker] Follower read errors continuing, suppressing logs...")
return None
def read_leader(self):
"""Read leader joint positions in degrees. Returns dict or None."""
if self.leader is None:
return None
# Auto-reconnect if too many consecutive errors
if self.leader_error_count >= self.reconnect_threshold:
self._try_reconnect_leader()
try:
action = self.leader.get_action()
self.leader_error_count = 0
self.leader_error_msg = ""
return {name: float(action[f"{name}.pos"]) for name in JOINT_NAMES}
except Exception as e:
self.leader_error_count += 1
self.leader_error_msg = str(e)
if self.leader_error_count <= self.error_threshold:
print(f"[worker] Leader read error: {e}")
elif self.leader_error_count == self.error_threshold + 1:
print(f"[worker] Leader read errors continuing, suppressing logs...")
return None
def send_follower_action(self, joint_positions_deg):
"""
Send joint positions (degrees) to follower via LeRobot send_action().
joint_positions_deg: dict like {"shoulder_pan": 10.0, ...}
"""
if self.follower is None:
return
try:
action_dict = {f"{name}.pos": float(joint_positions_deg[name])
for name in JOINT_NAMES if name in joint_positions_deg}
self.follower.send_action(action_dict)
except Exception as e:
self.follower_error_count += 1
self.follower_error_msg = str(e)
if self.follower_error_count <= self.error_threshold:
print(f"[worker] Follower send_action error: {e}")
elif self.follower_error_count == self.error_threshold + 1:
print(f"[worker] Follower send errors continuing, suppressing logs...")
def handle_command(self, msg):
"""
Process a command from the ROS bridge node.
Expected format:
{"cmd": "send_follower_action", "args": {"shoulder_pan": 0.0, ...}}
{"cmd": "ping"}
{"cmd": "start_teleop"}
{"cmd": "stop_teleop"}
{"cmd": "start_record"}
{"cmd": "stop_record"}
{"cmd": "start_replay", "args": {"filename": "...", "loop": false}}
{"cmd": "stop_replay"}
{"cmd": "estop"}
{"cmd": "list_recordings"}
Returns a response dict.
"""
cmd = msg.get("cmd", "")
if cmd == "ping":
return {"status": "ok", "state": self.state,
"teleop": self.teleop, "ts": time.time()}
elif cmd == "estop":
return self._handle_estop()
elif cmd == "start_teleop":
return self._handle_start_teleop()
elif cmd == "stop_teleop":
return self._handle_stop_teleop()
elif cmd == "start_record":
return self._handle_start_record()
elif cmd == "stop_record":
return self._handle_stop_record()
elif cmd == "start_replay":
args = msg.get("args", {})
return self._handle_start_replay(args)
elif cmd == "stop_replay":
return self._handle_stop_replay()
elif cmd == "list_recordings":
return self._handle_list_recordings()
elif cmd == "send_follower_action":
if self.follower is None:
return {"status": "error", "reason": "follower not connected"}
if self.follower_initial_pos is None:
return {"status": "error", "reason": "safety baseline not set (no initial position)"}
joint_args = msg.get("args", {})
if not joint_args:
return {"status": "error", "reason": "empty args"}
try:
# Two-layer safety clamp:
# Layer 1: baseline ± cmd_limit (session safety)
# Layer 2: JOINT_LIMITS_DEG (absolute physical limits, measured empirically)
clamped = {}
rejected = []
for name, target_deg in joint_args.items():
if name not in JOINT_NAMES:
continue
if name not in self.allowed_cmd_joints:
rejected.append(name)
continue
target = float(target_deg)
# Layer 1: baseline ± cmd_limit
baseline = self.follower_initial_pos.get(name, 0.0)
soft_lo = baseline - self.cmd_limit_deg
soft_hi = baseline + self.cmd_limit_deg
# Layer 2: absolute physical limits
hard_lo, hard_hi = JOINT_LIMITS_DEG.get(name, (-180.0, 180.0))
# Effective range = intersection of both
eff_lo = max(soft_lo, hard_lo)
eff_hi = min(soft_hi, hard_hi)
clamped_val = max(eff_lo, min(eff_hi, target))
clamped[name] = clamped_val
if abs(clamped_val - target) > 0.01:
reason = ""
if target < hard_lo or target > hard_hi:
reason = " [PHYSICAL LIMIT]"
else:
reason = " [BASELINE LIMIT]"
print(f"[worker] CLAMP {name}: "
f"requested {target:.2f}° → clamped to {clamped_val:.2f}° "
f"(effective: {eff_lo:.2f}° ~ {eff_hi:.2f}°){reason}")
if not clamped:
return {"status": "error",
"reason": f"no allowed joints in command "
f"(rejected: {rejected}, allowed: {sorted(self.allowed_cmd_joints)})"}
action_dict = {f"{name}.pos": val for name, val in clamped.items()}
self.follower.send_action(action_dict)
resp = {"status": "ok", "clamped": clamped}
if rejected:
resp["rejected_joints"] = rejected
return resp
except Exception as e:
return {"status": "error", "reason": str(e)}
else:
return {"status": "error", "reason": f"unknown cmd: {cmd}"}
# -----------------------------------------------------------------------
# Teleop (Sync) handlers
# -----------------------------------------------------------------------
def _handle_start_teleop(self):
"""Activate leader→follower sync (teleop) with smooth approach."""
if self.state == "replaying":
return {"status": "error",
"reason": "cannot start teleop during replay — stop replay first"}
if self.follower is None or self.leader is None:
return {"status": "error",
"reason": "teleop requires both follower and leader connected"}
if self.teleop:
return {"status": "ok", "state": self.state, "teleop": True,
"msg": "teleop already active"}
if self.state == "syncing":
return {"status": "ok", "state": self.state, "teleop": False,
"msg": "already approaching leader position"}
# Read leader's current position as approach target
leader_pos = self.read_leader()
if leader_pos is None:
# Can't read leader — just turn on teleop directly
self.teleop = True
print("[worker] Teleop (sync) ON — leader → follower (no approach)")
return {"status": "ok", "state": self.state, "teleop": True}
# Start approach to leader position, then activate teleop
self.state = "syncing"
self._start_approach(leader_pos)
if not self.approaching:
# Already close enough — activate teleop immediately
self.state = "idle"
self.teleop = True
print("[worker] Teleop (sync) ON — already aligned")
return {"status": "ok", "state": self.state, "teleop": True}
print("[worker] Syncing: approaching leader position before teleop ON")
return {"status": "ok", "state": "syncing", "teleop": False,
"approach_sec": round(self.approach_duration, 2)}
def _handle_stop_teleop(self):
"""Deactivate leader→follower sync (teleop)."""
if self.state == "recording":
return {"status": "error",
"reason": "cannot stop teleop during recording — stop recording first"}
# Cancel syncing approach if in progress
if self.state == "syncing":
self.approaching = False
self.state = "idle"
print("[worker] Syncing cancelled")
self.teleop = False
print("[worker] Teleop (sync) OFF")
return {"status": "ok", "state": self.state, "teleop": False}
# -----------------------------------------------------------------------
# Record / Replay / E-Stop handlers
# -----------------------------------------------------------------------
def _handle_estop(self):
"""Emergency stop: abort any ongoing motion, return to idle."""
prev_state = self.state
prev_teleop = self.teleop
self.state = "idle"
self.teleop = False
self.recording_buffer = []
self.replay_frames = []
self.replay_index = 0
self.approaching = False
print(f"[worker] *** E-STOP *** (was: state={prev_state}, teleop={prev_teleop})")
return {"status": "ok", "state": "idle", "teleop": False,
"previous_state": prev_state}
def _handle_start_record(self):
"""Start recording: teleop must already be active (Sync On first)."""
if self.state != "idle":
return {"status": "error",
"reason": f"cannot start recording in state '{self.state}'"}
if self.follower is None or self.leader is None:
return {"status": "error",
"reason": "recording requires both follower and leader connected"}
if not self.teleop:
return {"status": "error",
"reason": "teleop (sync) must be active before recording — "
"press Sync On first"}
self.recording_buffer = []
self.recording_start_time = time.monotonic()
self.state = "recording"
print("[worker] *** RECORDING STARTED *** (teleop already active)")
return {"status": "ok", "state": "recording", "teleop": True}
def _handle_stop_record(self):
"""Stop recording: save buffer to file. Teleop stays ON."""
if self.state != "recording":
return {"status": "error",
"reason": f"not recording (state: '{self.state}')"}
num_frames = len(self.recording_buffer)
if num_frames == 0:
self.state = "idle"
# Teleop stays ON — user controls it via Sync button
print("[worker] Recording stopped — 0 frames, nothing saved.")
return {"status": "ok", "state": "idle", "teleop": self.teleop,
"frames": 0, "filename": None}
# Calculate duration from frame timestamps
duration = self.recording_buffer[-1]["ts"] - self.recording_buffer[0]["ts"]
# Build recording file
timestamp_str = datetime.now().strftime("%Y%m%d_%H%M%S")
filename = f"recording_{timestamp_str}.json"
filepath = self.recordings_dir / filename
recording_data = {
"version": 1,
"recorded_at": datetime.now().isoformat(),
"rate_hz": self.args.rate,
"num_frames": num_frames,
"duration_sec": round(duration, 3),
"joint_names": JOINT_NAMES,
"frames": self.recording_buffer,
}
with open(filepath, "w") as f:
json.dump(recording_data, f, indent=2)
self.state = "idle"
# Teleop stays ON — user controls it via Sync button
self.recording_buffer = []
print(f"[worker] Recording saved: {filepath}")
print(f"[worker] {num_frames} frames, {duration:.1f}s")
return {
"status": "ok",
"state": "idle",
"teleop": self.teleop,
"filename": filename,
"frames": num_frames,
"duration_sec": round(duration, 3),
}
def _handle_start_replay(self, args):
"""Load a recording file and start replaying on the follower."""
if self.state != "idle":
return {"status": "error",
"reason": f"cannot start replay in state '{self.state}'"}
if self.follower is None:
return {"status": "error",
"reason": "replay requires follower connected"}
filename = args.get("filename", "")
self.replay_loop = args.get("loop", False)
# Allow overriding approach speed per command
speed = args.get("approach_speed", self.approach_speed)
# If no filename, use the most recent recording
if not filename:
recordings = self._get_recording_files()
if not recordings:
return {"status": "error", "reason": "no recordings found"}
filename = recordings[-1] # most recent (sorted by name = sorted by time)
filepath = self.recordings_dir / filename
if not filepath.exists():
return {"status": "error",
"reason": f"file not found: {filename}"}
try:
with open(filepath) as f:
data = json.load(f)
except Exception as e:
return {"status": "error", "reason": f"failed to load: {e}"}
frames = data.get("frames", [])
if len(frames) < 2:
return {"status": "error",
"reason": f"recording too short ({len(frames)} frames)"}
self.replay_frames = frames
self.replay_index = 0
self.replay_filename = filename
self.teleop = False # disable teleop during replay
self.state = "replaying"
# Start approach phase: smoothly move from current position to first frame
first_frame_joints = frames[0].get("joints", {})
self._start_approach(first_frame_joints, speed)
duration = data.get("duration_sec", 0)
print(f"[worker] *** REPLAY STARTED *** {filename}")
print(f"[worker] {len(frames)} frames, {duration}s, "
f"loop={'yes' if self.replay_loop else 'no'}, "
f"approach={self.approach_duration:.1f}s")
return {
"status": "ok",
"state": "replaying",
"filename": filename,
"frames": len(frames),
"duration_sec": duration,
"loop": self.replay_loop,
"approach_sec": round(self.approach_duration, 2),
}
def _handle_stop_replay(self):
"""Stop replay immediately."""
if self.state != "replaying":
return {"status": "error",
"reason": f"not replaying (state: '{self.state}')"}
self.state = "idle"
self.replay_frames = []
self.replay_index = 0
print(f"[worker] Replay stopped ({self.replay_filename})")
return {"status": "ok", "state": "idle"}
def _handle_list_recordings(self):
"""List all saved recording files."""
files = self._get_recording_files()
# Add metadata summary for each file
summaries = []
for fname in files:
try:
with open(self.recordings_dir / fname) as f:
data = json.load(f)
summaries.append({
"filename": fname,
"frames": data.get("num_frames", 0),
"duration_sec": data.get("duration_sec", 0),
"recorded_at": data.get("recorded_at", ""),
})
except Exception:
summaries.append({"filename": fname})
return {"status": "ok", "recordings": summaries}
def _get_recording_files(self):
"""Get sorted list of recording JSON files."""
files = sorted(
f.name for f in self.recordings_dir.glob("recording_*.json")
if f.is_file()
)
return files
# -----------------------------------------------------------------------
# Approach (smooth transition) logic
# -----------------------------------------------------------------------
def _start_approach(self, target_joints, speed=None):
"""
Begin a smooth approach from current follower position to target_joints.
Duration is calculated from the largest joint angle difference
divided by speed (degrees/sec).
"""
if speed is None:
speed = self.approach_speed
current = self.read_follower()
if current is None:
# Can't read current position — skip approach
self.approaching = False
self.replay_start_time = time.monotonic()
return
self.approach_start_pos = dict(current)
self.approach_target_pos = dict(target_joints)
# Calculate duration based on max joint difference
max_diff = 0.0
for name in JOINT_NAMES:
if name in current and name in target_joints:
diff = abs(target_joints[name] - current[name])
max_diff = max(max_diff, diff)
if max_diff < 1.0:
# Already close enough — skip approach
self.approaching = False
self.replay_start_time = time.monotonic()
return
self.approach_duration = max_diff / speed
# Minimum 0.3s, even for small moves
self.approach_duration = max(self.approach_duration, 0.3)
self.approach_start_time = time.monotonic()
self.approaching = True
print(f"[worker] Approach: {max_diff:.1f}° max diff, "
f"{self.approach_duration:.1f}s at {speed}°/s")
def _tick_approach(self):
"""
Advance the approach interpolation by one tick.
Returns interpolated joint positions, or None if approach is finished.
"""
if not self.approaching:
return None
elapsed = time.monotonic() - self.approach_start_time
t = elapsed / self.approach_duration # 0.0 → 1.0
if t >= 1.0:
# Approach finished — send final target
self.send_follower_action(self.approach_target_pos)
self.approaching = False
self.replay_start_time = time.monotonic()
print("[worker] Approach complete")
return self.read_follower()
# Smooth interpolation using ease-in-out (cosine)
# t=0 → factor=0, t=1 → factor=1, smooth acceleration/deceleration
factor = (1.0 - math.cos(t * math.pi)) / 2.0
interpolated = {}
for name in JOINT_NAMES:
start_val = self.approach_start_pos.get(name, 0.0)
end_val = self.approach_target_pos.get(name, 0.0)
interpolated[name] = start_val + (end_val - start_val) * factor
self.send_follower_action(interpolated)
return self.read_follower()
# -----------------------------------------------------------------------
# Replay tick — called each iteration of the main loop
# -----------------------------------------------------------------------
def _tick_replay(self):
"""
Advance replay by one tick. Handles approach phase, then frame playback.
Returns the follower position after sending (for PUB).
"""
# --- Approach phase: smooth transition to start position ---
if self.approaching:
return self._tick_approach()
# --- Normal replay ---
if not self.replay_frames or self.replay_index >= len(self.replay_frames):
# Replay finished
if self.replay_loop and self.replay_frames:
# Loop: approach back to first frame before restarting
first_frame_joints = self.replay_frames[0].get("joints", {})
self.replay_index = 0
self._start_approach(first_frame_joints)
if self.approaching:
print(f"[worker] Replay loop — approaching start position")
return self.read_follower()
else:
# Already close enough, restart immediately
print(f"[worker] Replay looping ({self.replay_filename})")
else:
self.state = "idle"
print(f"[worker] Replay finished ({self.replay_filename})")
return self.read_follower()
elapsed = time.monotonic() - self.replay_start_time
first_ts = self.replay_frames[0]["ts"]
# Advance to the frame that matches current elapsed time
while self.replay_index < len(self.replay_frames):
frame = self.replay_frames[self.replay_index]
frame_time = frame["ts"] - first_ts
if frame_time <= elapsed:
# Send this frame to follower
joints = frame.get("joints", {})
if joints:
self.send_follower_action(joints)
self.replay_index += 1
else:
break # wait for next tick
# Re-read follower actual position
return self.read_follower()
def check_commands(self):
"""Non-blocking check for incoming REP commands."""
events = dict(self.poller.poll(timeout=0)) # 0 = non-blocking
if self.rep in events:
try:
raw = self.rep.recv_string(zmq.NOBLOCK)
msg = json.loads(raw)
self.last_cmd_time = time.monotonic()
self.heartbeat_triggered = False
response = self.handle_command(msg)
self.rep.send_string(json.dumps(response))
except zmq.Again:
pass
except Exception as e:
# Must always send a reply on REP socket
self.rep.send_string(json.dumps({
"status": "error", "reason": str(e)
}))
def run(self):
"""Main read loop at configured rate."""
period = 1.0 / self.args.rate
seq = 0
teleop_count = 0
print(f"[worker] Starting read loop at {self.args.rate} Hz "
f"(period={period*1000:.1f}ms)")
while self.running:
t_start = time.monotonic()
# --- Replaying mode: advance replay timeline ---
if self.state == "replaying":
follower_pos = self._tick_replay()
leader_pos = self.read_leader()
# --- Syncing mode: approach leader position, then activate teleop ---
elif self.state == "syncing":
if self.approaching:
follower_pos = self._tick_approach()
else:
# Approach finished — activate teleop
self.state = "idle"
self.teleop = True
print("[worker] Teleop (sync) ON — approach complete")
follower_pos = self.read_follower()
leader_pos = self.read_leader()
else:
# Read joint states
follower_pos = self.read_follower()
leader_pos = self.read_leader()
# --- Teleop: send leader position to follower ---
if self.teleop and leader_pos is not None:
self.send_follower_action(leader_pos)
teleop_count += 1
# Re-read follower after sending action to get updated position
follower_pos = self.read_follower()
# --- Recording mode: buffer the current frame ---
if self.state == "recording" and follower_pos is not None:
frame = {
"ts": time.monotonic() - self.recording_start_time,
"joints": dict(follower_pos),
}
self.recording_buffer.append(frame)
# Build and publish message (degrees — bridge node converts to radians)
msg = {
"seq": seq,
"ts": time.time(),
"state": self.state,
"teleop": self.teleop,
}
if follower_pos is not None:
msg["follower"] = follower_pos
if leader_pos is not None:
msg["leader"] = leader_pos
# Report device errors if threshold exceeded
device_errors = {}
if self.follower_error_count >= self.error_threshold:
device_errors["follower"] = self.follower_error_msg
if self.leader_error_count >= self.error_threshold:
device_errors["leader"] = self.leader_error_msg
if device_errors:
msg["device_errors"] = device_errors
# Add recording/replay metadata
if self.state == "recording":
msg["recording_frames"] = len(self.recording_buffer)
elif self.state == "replaying":
msg["replay_progress"] = {
"index": self.replay_index,
"total": len(self.replay_frames),
"filename": self.replay_filename,
"loop": self.replay_loop,
"approaching": self.approaching,
}
self.pub.send_string(json.dumps(msg))
seq += 1
# Check for commands (non-blocking)
self.check_commands()
# Heartbeat timeout: if no command from bridge for too long,
# auto E-stop to prevent uncontrolled motion when the
# Unreal→rosbridge→bridge path is broken.
# Only triggers when actively doing something (not idle).
if self.state != "idle" or self.teleop:
since_last = time.monotonic() - self.last_cmd_time
if since_last > self.heartbeat_timeout_sec and not self.heartbeat_triggered:
self.heartbeat_triggered = True
print(f"[worker] *** HEARTBEAT TIMEOUT ({since_last:.1f}s) "
f"— auto E-STOP ***")
self._handle_estop()
# Rate limiting
elapsed = time.monotonic() - t_start
sleep_time = period - elapsed
if sleep_time > 0:
time.sleep(sleep_time)
def shutdown(self):
"""Clean shutdown."""
print("\n[worker] Shutting down...")
self.running = False
self.state = "idle"
# Save any in-progress recording
if self.recording_buffer:
try:
self._handle_stop_record()
print("[worker] Saved in-progress recording before shutdown.")
except Exception:
pass
if self.follower is not None:
try:
self.follower.disconnect()
print("[worker] Follower disconnected.")
except Exception:
pass
if self.leader is not None:
try:
self.leader.disconnect()
print("[worker] Leader disconnected.")
except Exception:
pass
self.pub.close()
self.rep.close()
self.ctx.term()
print("[worker] ZMQ cleaned up. Bye.")
def main():
args = build_args()
worker = LeRobotWorker(args)
# Graceful shutdown on Ctrl+C
def sig_handler(sig, frame):
worker.shutdown()
sys.exit(0)
signal.signal(signal.SIGINT, sig_handler)
signal.signal(signal.SIGTERM, sig_handler)
try:
worker.connect_robots()
worker.run()
except Exception:
traceback.print_exc()
worker.shutdown()
sys.exit(1)
if __name__ == "__main__":
main()변경 사항
- reconnect_threshold = 30: 30회 연속 에러(약 1초) 후 reconnect 시도
- reconnect_interval_sec = 5.0: reconnect 실패 시 5초 간격으로 재시도
- last_follower_reconnect_time, last_leader_reconnect_time: 마지막 reconnect 시도 시각
- _try_reconnect_follower() (신규): disconnect() → connect(), 성공 시 에러 카운터 리셋
- _try_reconnect_leader() (신규): 동일
- read_follower (수정): threshold 초과 시 reconnect 호출 후 read 시도
- read_leader (수정): 동일
- port_handler.is_open이 False가 되면 bus.is_connected가 False를 반환하고, connect()가 정상적으로 진행된다.
USB 강제 제거 후 재연결 동작 흐름
USB 뽑힘
→ 에러 카운터 증가 (5회 → 뷰포트 에러 표시, 30회 → reconnect 시도)
→ disconnect() → connect() 시도 → 실패 (USB 아직 없음)
→ 5초 후 재시도 → 실패 ...
USB 다시 꽂고 detach → attach
→ /dev/ttyACM* 복구
→ 다음 reconnect 시도에서 connect() 성공
→ 에러 카운터 리셋 → 정상 동작 복귀
→ 뷰포트 에러 메시지 자동 소멸
detach - attach는 스크립트로 대체할 예정.
2-5단계: 테스트 후 보완점5 - USB 재연결 시 언리얼 뷰포트에 Leader, Follower 재연결 표시
RobotVisualizer.h 수정
더보기
#pragma once
#include "CoreMinimal.h"
#include "GameFramework/Actor.h"
#include "RobotVisualizer.generated.h"
class UStaticMesh;
class UStaticMeshComponent;
class USceneComponent;
class URosBridgeSubsystem;
/**
* 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();
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;
// =================================================================
// 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);
};변경 사항
- bFollowerDeviceError: follower USB 에러 상태 추적
- bLeaderDeviceError: leader USB 에러 상태 추적
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"
// =============================================================================
// 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."));
}
}
// =============================================================================
// 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);
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"));
}
}
}변경 사항
- OnRobotStatus의 device_errors 처리를 상태 전환 방식으로 변경:
- 에러 없음 → 에러 발생: 빨간색 "*** Follower/Leader: USB/Serial ERROR ***" (기존과 동일)
- 에러 발생 → 에러 해소: 초록색 "Follower/Leader: USB connection restored"
- 에러 지속 중: 중복 표시 안 함 (한 번만)
솔루션 빌드
복구 표시가 잘 나오는 것을 확인할 수 있다. → 어차피 두 USB 모두 detach 후 attach라서 둘 다 동시에 뜸.
이번 작업 내용
- 오류 및 안전 처리 전체 완료. 실물 테스트 완료.
- WebSocket 끊김 경고
- bridge/worker 분리 진단
- heartbeat timeout 자동 E-stop
- MoveIt 플래닝 실패 피드백
- USB 분리 시 follower/leader 개별 에러 표시
- worker 자동 reconnect(터미널 재시작 없이 USB 복구)
- ZMQ REQ 소켓 자동 복구(worker 재시작 시 bridge 재시작 불필요)
728x90
반응형
