Ue5.4.4 pick from devel

.gitattributes

@@ -2,5 +2,4 @@ Content/Robots/** filter=lfs diff=lfs merge=lfs -text
 Content/SkeletalRobots/** filter=lfs diff=lfs merge=lfs -text
 Content/Textures/** filter=lfs diff=lfs merge=lfs -text
 Content/ExternalDevices/** filter=lfs diff=lfs merge=lfs -text
-Content/Character/SKM_Manny_CtrlRig1.uasset filter=lfs diff=lfs merge=lfs -text
-Content/Character/SKM_Manny_CtrlRig2.uasset filter=lfs diff=lfs merge=lfs -text
+docs/source/_static/videos/** filter=lfs diff=lfs merge=lfs -text

Source/RapyutaSimulationPlugins/Private/Robots/RRAIRobotROSController.cpp

@@ -14,10 +14,49 @@
 #include "Robots/RRBaseRobot.h"
 #include "Robots/RRRobotROS2Interface.h"
 
+void URRAIRobotROSControllerParam::SetParametersFromPawn(ARRAIRobotROSController* InController) const
+{
+    if (!InController)
+    {
+        UE_LOG_WITH_INFO_NAMED(LogRapyutaCore, Error, TEXT("FCoontroller is null"));
+        return;
+    }
+    InController->bDebug = bDebug;
+    InController->OrientationTolerance = OrientationTolerance;
+    InController->LinearMotionTolerance = LinearMotionTolerance;
+    InController->bTeleportOnFail = bTeleportOnFail;
+    InController->Mode = Mode;
+    InController->RandomMoveBoundingBox = RandomMoveBoundingBox;
+    InController->GoalSequence = GoalSequence;
+    InController->GoalSequenceActor = GoalSequenceActor;
+    InController->OriginActor = OriginActor;
+    InController->Origin = Origin;
+
+    // ROS related parameters
+    InController->NavStatusPublicationFrequencyHz = NavStatusPublicationFrequencyHz;
+    InController->SetSpeedTopicName = SetSpeedTopicName;
+    InController->SetAngularVelTopicName = SetAngularVelTopicName;
+    InController->SetModeTopicName = SetModeTopicName;
+    InController->NavStatusTopicName = NavStatusTopicName;
+    InController->PoseGoalTopicName = PoseGoalTopicName;
+    InController->ActorGoalTopicName = ActorGoalTopicName;
+
+    BPSetParametersFromPawn(InController);
+}
+
 void ARRAIRobotROSController::OnPossess(APawn* InPawn)
 {
     Super::OnPossess(InPawn);
 
+    // Get param from Pawn
+    URRAIRobotROSControllerParam* param = InPawn->FindComponentByClass<URRAIRobotROSControllerParam>();
+    if (param)
+    {
+        param->SetParametersFromPawn(this);
+    }
+
+    InitParameters();
+
     /**
      * @todo this won't work with client-server
     */
@@ -29,6 +68,26 @@ void ARRAIRobotROSController::OnUnPossess()
     Super::OnUnPossess();
 }
 
+void ARRAIRobotROSController::InitParameters()
+{
+    if (OriginActor)
+    {
+        Origin = OriginActor->GetTransform();
+    }
+
+    // Transform to world transform
+    for (int i = 0; i < GoalSequence.Num(); i++)
+    {
+        GoalSequence[i] = URRGeneralUtils::GetWorldTransform(Origin, GoalSequence[i], true);
+    }
+
+    // Transform GoalSequenceActor to GoalSequence
+    for (AActor* actor : GoalSequenceActor)
+    {
+        GoalSequence.Add(actor->GetTransform());
+    }
+}
+
 void ARRAIRobotROSController::InitROS2Interface()
 {
     InitPropertiesFromJSON();
@@ -1096,7 +1155,8 @@ FTransform ARRAIRobotROSController::GetOrigin()
 {
     if (OriginActor)
     {
-        return OriginActor->GetTransform();
+        Origin = OriginActor->GetTransform();
+        return Origin;
     }
     else
     {

Source/RapyutaSimulationPlugins/Private/Robots/RRBaseRobot.cpp

@@ -458,13 +458,15 @@ void ARRBaseRobot::StopMovement()
 
 void ARRBaseRobot::SetLinearVel(const FVector& InLinearVel)
 {
-    SyncServerLinearMovement(GetWorld()->GetGameState()->GetServerWorldTimeSeconds(), GetTransform(), InLinearVel);
+    LastCmdVelUpdateTime = GetWorld()->GetGameState()->GetServerWorldTimeSeconds();
+    SyncServerLinearMovement(LastCmdVelUpdateTime, GetTransform(), InLinearVel);
     SetLocalLinearVel(InLinearVel);
 }
 
 void ARRBaseRobot::SetAngularVel(const FVector& InAngularVel)
 {
-    SyncServerAngularMovement(GetWorld()->GetGameState()->GetServerWorldTimeSeconds(), GetActorRotation(), InAngularVel);
+    LastCmdVelUpdateTime = GetWorld()->GetGameState()->GetServerWorldTimeSeconds();
+    SyncServerAngularMovement(LastCmdVelUpdateTime, GetActorRotation(), InAngularVel);
     SetLocalAngularVel(InAngularVel);
 }
 
@@ -593,6 +595,11 @@ void ARRBaseRobot::Tick(float DeltaSeconds)
     {
         CheckJointsInitialization();
     }
+
+    if (CmdVelTimeout > 0 && CmdVelTimeout <= GetWorld()->GetGameState()->GetServerWorldTimeSeconds() - LastCmdVelUpdateTime)
+    {
+        StopMovement();
+    }
 }
 
 bool ARRBaseRobot::InitPropertiesFromJSON()

Source/RapyutaSimulationPlugins/Private/Sensors/RR3DLidarComponent.cpp

@@ -392,7 +392,7 @@ FROSPointCloud2 URR3DLidarComponent::GetROS2Data()
             }
 
             // Convert pose to local coordinate, ROS unit and double -> float
-            FVector posInDouble = RecordedHits.Last(index).ImpactPoint + BWithNoise * PositionNoise->Get();
+            FVector posInDouble = RecordedHits.Last(index).ImpactPoint;    // + BWithNoise * PositionNoise->Get();
             posInDouble = URRGeneralUtils::GetRelativeTransform(
                               FTransform(GetComponentQuat(), GetComponentLocation(), FVector::OneVector), FTransform(posInDouble))
                               .GetTranslation();

Source/RapyutaSimulationPlugins/Private/Sensors/RRROS2BaseSensorComponent.cpp

@@ -81,5 +81,19 @@ void URRROS2BaseSensorComponent::Run()
 
 void URRROS2BaseSensorComponent::Stop()
 {
+    if (SensorPublisher)
+    {
+        SensorPublisher->StopPublishTimer();
+    }
     GetWorld()->GetTimerManager().ClearTimer(TimerHandle);
 }
+
+void URRROS2BaseSensorComponent::DestroyComponent(bool bPromoteChildren)
+{
+    Stop();
+    if (SensorPublisher)
+    {
+        SensorPublisher->Destroy();
+    }
+    Super::DestroyComponent();
+}

Source/RapyutaSimulationPlugins/Private/Sensors/RRROS2CameraComponent.cpp

@@ -49,8 +49,10 @@ void URRROS2CameraComponent::PreInitializePublisher(UROS2NodeComponent* InROS2No
 
 void URRROS2CameraComponent::SensorUpdate()
 {
-    SceneCaptureComponent->CaptureScene();
-    CaptureNonBlocking();
+    if (Render) {
+        SceneCaptureComponent->CaptureScene();
+        CaptureNonBlocking();
+    }
 }
 
 // reference https://github.com/TimmHess/UnrealImageCapture

Source/RapyutaSimulationPlugins/Private/Sensors/RRROS2IMUComponent.cpp

@@ -57,35 +57,52 @@ FROSImu URRROS2IMUComponent::GetROS2Data()
 void URRROS2IMUComponent::SensorUpdate()
 {
     const float currentTime = UGameplayStatics::GetTimeSeconds(GetWorld());
-    const FTransform currentTransform = K2_GetComponentToWorld() * InitialTransform.Inverse();
+    const FTransform worldTransform = K2_GetComponentToWorld();
+    const FTransform relTransform = worldTransform * InitialTransform.Inverse();
     const float dt = currentTime - LastSensorUpdateTime;
 
     if (dt > 1e-10)
     {
         const float _dt = 1.0 / dt;
+        Data.Header.FrameId = FrameId;
         Data.Header.Stamp = URRConversionUtils::FloatToROSStamp(currentTime);
 
-        const FTransform dT = currentTransform * LastTransform.Inverse();
+        // Transform difference in reference(initial) transform
+        const FTransform dT = relTransform * LastTransform.Inverse();
 
-        FQuat orientation = currentTransform.GetRotation();
-        FVector angularVelocity = dT.GetRotation().GetNormalized().Euler() * _dt;
+        FQuat orientation = relTransform.GetRotation();    // Rotation from reference(initial) transform
+        FVector angularVelocity =
+            orientation.UnrotateVector(dT.GetRotation().GetNormalized().Euler() * _dt);    //in sensor local transform
 
         const FVector linearVel = dT.GetTranslation() * _dt;
-        FVector linearAcc = (linearVel - LastLinearVel) * _dt;
+        FVector linearAcc = orientation.UnrotateVector((linearVel - LastLinearVel) * _dt);    //in sensor local transform
+
+        // post process, add gravity and scale
+        if (bAddGravity)
+        {
+            FVector GravityAcc =
+                FVector(0.0, 0.0, -UnitConversion::ForceUnificationFactor(EUnit::KilogramsForce) * 100);    // cm/ss
+            // Gravity vector points towards Z(+) axis
+            // reference: https://base.movella.com/s/article/Why-does-an-accelerometer-measure-gravity-with-positive-sign?
+            linearAcc -= worldTransform.GetRotation().UnrotateVector(GravityAcc);
+        }
+        linearAcc *= AccGain;
 
+        // noise
         LinearAcceleration =
             linearAcc + FVector(LinearAccelerationNoise->Get(), LinearAccelerationNoise->Get(), LinearAccelerationNoise->Get());
         AngularVelocity =
             angularVelocity + FVector(AngularVelocityNoise->Get(), AngularVelocityNoise->Get(), AngularVelocityNoise->Get());
         OrientationNoiseSum = OrientationNoiseSum + OrientationNoiseDriftCoefficient * OrientationNoise->Get();
         Orientation = FQuat::MakeFromEuler(orientation.Euler() + OrientationNoiseSum + OffsetOrientation);
 
-        // noise
+        // UE -> ROS conversion
         Data.LinearAcceleration = URRConversionUtils::VectorUEToROS(LinearAcceleration);
         Data.AngularVelocity = URRConversionUtils::VectorUEToROS(AngularVelocity);
         Data.Orientation = URRConversionUtils::QuatUEToROS(Orientation);
 
-        LastTransform = currentTransform;
+        // update
+        LastTransform = relTransform;
         LastdT = dT;
         LastLinearVel = linearVel;
         LastSensorUpdateTime = currentTime;

Source/RapyutaSimulationPlugins/Public/Core/RRConversionUtils.h

@@ -34,6 +34,12 @@ class URRConversionUtils : public UBlueprintFunctionLibrary
         return FVector(InLocation.X, -InLocation.Y, InLocation.Z);
     }
 
+    UFUNCTION(BlueprintCallable, Category = "Conversion")
+    static FVector2D ConvertHandedness2D(const FVector2D& InLocation)
+    {
+        return FVector2D(InLocation.X, -InLocation.Y);
+    }
+
     // UE: cm, ROS: m
     template<typename T>
     static T DistanceROSToUE(const T& InROSDistance)
@@ -77,6 +83,18 @@ class URRConversionUtils : public UBlueprintFunctionLibrary
         OutputZ = InputZ * 0.01f;
     }
 
+    UFUNCTION(BlueprintCallable, Category = "Conversion")
+    static FVector2D Vector2DUEToROS(const FVector2D& Input)
+    {
+        return 0.01f * ConvertHandedness2D(Input);
+    }
+
+    FORCEINLINE static void Vector2DUEToROS(const double& InputX, const double& InputY, double& OutputX, double& OutputY)
+    {
+        OutputX = InputX * 0.01f;
+        OutputY = -InputY * 0.01f;
+    }
+
     /**
      * @brief Convert UE Rotation to ROS Vector with optional Deg->Rad conversion
      * @param Input
@@ -168,6 +186,23 @@ class URRConversionUtils : public UBlueprintFunctionLibrary
         OutputZ = InputZ * 100.f;
     }
 
+    UFUNCTION(BlueprintCallable, Category = "Conversion")
+    static FVector2D Vector2DROSToUE(const FVector2D& Input)
+    {
+        return 100.f * ConvertHandedness2D(Input);
+    }
+
+    FORCEINLINE static void Vector2DROSToUE(const FVector2D& Input, FVector2D& Output)
+    {
+        Output.Set(Input.X * 100.f, -Input.Y * 100.f);
+    }
+
+    FORCEINLINE static void Vector2DROSToUE(const double& InputX, const double& InputY, double& OutputX, double& OutputY)
+    {
+        OutputX = InputX * 100.f;
+        OutputY = -InputY * 100.f;
+    }
+
     /**
      * @brief Convert ROS Rotation Euler (rad) to UE Vector with optional Rad->Deg conversion
      * @param Input