Stop pub when destroyed (#326)

* destroy publisher when sensor is destroyed * expose noise param * update IMU sensor, 1) acc and vel from local frame, 2) add gravity acc and acc gain
rapyuta-robotics · Nov 13, 2024 · b15e7de · b15e7de
1 parent 9f66538
commit b15e7de
Show file tree

Hide file tree

Showing 7 changed files with 56 additions and 29 deletions.
diff --git a/Source/RapyutaSimulationPlugins/Private/Robots/RRBaseRobot.cpp b/Source/RapyutaSimulationPlugins/Private/Robots/RRBaseRobot.cpp
@@ -469,12 +469,6 @@ void ARRBaseRobot::StopMovement()
     SetAngularVel(FVector::ZeroVector);
 }
 
-void ARRBaseRobot::SetVel(const FVector& InLinearVel, const FVector& InAngularVel)
-{
-    SetLinearVel(InLinearVel);
-    SetAngularVel(InAngularVel);
-}
-
 void ARRBaseRobot::SetLinearVel(const FVector& InLinearVel)
 {
     LastCmdVelUpdateTime = GetWorld()->GetGameState()->GetServerWorldTimeSeconds();

diff --git a/Source/RapyutaSimulationPlugins/Private/Sensors/RRROS2BaseSensorComponent.cpp b/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();
+}
diff --git a/Source/RapyutaSimulationPlugins/Private/Sensors/RRROS2IMUComponent.cpp b/Source/RapyutaSimulationPlugins/Private/Sensors/RRROS2IMUComponent.cpp
@@ -57,7 +57,8 @@ 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)
@@ -66,27 +67,40 @@ void URRROS2IMUComponent::SensorUpdate()
         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
+            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;

diff --git a/Source/RapyutaSimulationPlugins/Public/Robots/RRBaseRobot.h b/Source/RapyutaSimulationPlugins/Public/Robots/RRBaseRobot.h
@@ -467,15 +467,6 @@ class RAPYUTASIMULATIONPLUGINS_API ARRBaseRobot : public ARRBaseActor
     UFUNCTION(BlueprintCallable)
     virtual void SetMoveComponent(UMovementComponent* InMoveComponent);
 
-    /**
-     * @brief Set linear and angular velocity to #RobotVehicleMoveComponent.
-     * Calls #SetLinearVel and #SetAngularVel
-     * @param InLinearVel
-     * @param InAngularVel
-     */
-    UFUNCTION(BlueprintCallable)
-    virtual void SetVel(const FVector& InLinearVel, const FVector& InAngularVel);
-
     /**
      * @brief Set velocity to #RobotVehicleMoveComponent.
      * Calls #SetLocalLinearVel for setting velocity to #RobotVehicleMoveComponent and

diff --git a/Source/RapyutaSimulationPlugins/Public/Sensors/RRBaseOdomComponent.h b/Source/RapyutaSimulationPlugins/Public/Sensors/RRBaseOdomComponent.h
@@ -106,26 +106,26 @@ class RAPYUTASIMULATIONPLUGINS_API URRBaseOdomComponent : public URRROS2BaseSens
     UPROPERTY(EditAnywhere, BlueprintReadWrite)
     FTransform RootOffset = FTransform::Identity;
 
-    UPROPERTY(EditAnywhere, Category = "Noise")
+    UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Noise")
     TObjectPtr<URRGaussianNoise> PositionNoise;
 
-    UPROPERTY(EditAnywhere, Category = "Noise")
+    UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Noise")
     TObjectPtr<URRGaussianNoise> RotNoise;
 
-    UPROPERTY(EditAnywhere, Category = "Noise")
+    UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Noise")
     float NoiseMeanPosition = 0.f;
 
-    UPROPERTY(EditAnywhere, Category = "Noise")
+    UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Noise")
     float NoiseVariancePosition = 0.001f;
 
-    UPROPERTY(EditAnywhere, Category = "Noise")
+    UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Noise")
     float NoiseMeanRot = 0.f;
 
-    UPROPERTY(EditAnywhere, Category = "Noise")
+    UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Noise")
     float NoiseVarianceRot = 0.005f;
 
     //! Add noise or not
-    UPROPERTY(EditAnywhere, Category = "Noise")
+    UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Noise")
     bool bWithNoise = true;
 
 protected:

diff --git a/Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2BaseSensorComponent.h b/Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2BaseSensorComponent.h
@@ -88,6 +88,13 @@ class RAPYUTASIMULATIONPLUGINS_API URRROS2BaseSensorComponent : public USceneCom
     */
     URRROS2BaseSensorComponent();
 
+    /**
+     * @brief Destroy after stop timers and destroy publisher
+     *
+     * @param bPromoteChildren
+     */
+    virtual void DestroyComponent(bool bPromoteChildren = false) override;
+
     /**
      * @brief Create and initialize publisher and start sensor update by calling
      * #CreatePublisher, #PreInitializePublisher, #InitializePublisher and #Run.

diff --git a/Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2IMUComponent.h b/Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2IMUComponent.h
@@ -9,6 +9,7 @@
 // UE
 #include "CoreMinimal.h"
 #include "EngineUtils.h"
+#include "Math/UnitConversion.h"
 
 // rclUE
 #include "RRROS2BaseSensorComponent.h"
@@ -128,4 +129,10 @@ class RAPYUTASIMULATIONPLUGINS_API URRROS2IMUComponent : public URRROS2BaseSenso
 
     UPROPERTY(VisibleAnywhere, BlueprintReadOnly)
     float LastSensorUpdateTime;
+
+    UPROPERTY(EditAnywhere, BlueprintReadWrite)
+    bool bAddGravity = true;
+
+    UPROPERTY(EditAnywhere, BlueprintReadWrite)
+    float AccGain = 1.0;
 };