Movement Components refactor

Source/RapyutaSimulationPlugins/Private/Drives/DifferentialDriveComponent.cpp

@@ -53,7 +53,7 @@ void UDifferentialDriveComponent::UpdateMovement(float DeltaTime)
 void UDifferentialDriveComponent::UpdateOdom(float DeltaTime)
 {
     // need to add noise!
-    
+
     if (!IsOdomInitialized)
     {
         InitOdom();
@@ -68,20 +68,20 @@ void UDifferentialDriveComponent::UpdateOdom(float DeltaTime)
     uint64 ns = (uint64)(TimeNow * 1e+09f);
     OdomData.header_stamp_nanosec = static_cast<uint32>(ns - (OdomData.header_stamp_sec * 1e+09));
 
-	OdomData.header_frame_id = FString("odom");
-	OdomData.child_frame_id = FString("base_footprint");
+    OdomData.header_frame_id = FString("odom");
+    OdomData.child_frame_id = FString("base_footprint");
 
     // vl and vr as computed here is ok for kinematics
-    // for physics, vl and vr should be computed based on the change in wheel orientation (i.e. the velocity term to be used is wheel rotations per unit time [rad/s])
-    // together with the wheel radius or perimeter, the displacement can be computed:
+    // for physics, vl and vr should be computed based on the change in wheel orientation (i.e. the velocity term to be used is
+    // wheel rotations per unit time [rad/s]) together with the wheel radius or perimeter, the displacement can be computed:
     //  vl = (left_wheel_orientation_rad_now - left_wheel_orientation_rad_previous) * perimeter / (2pi)
     //  vr = (right_wheel_orientation_rad_now - right_wheel_orientation_rad_previous) * perimeter / (2pi)
     // in the kinematics case, (dx,dy,dtheta) can be simplified considerably
     // but as this is not a performance bottleneck, for the moment we leave the full general formulation,
     // at least until the odom for the physics version of the agent is implemented, so that we have a reference
     float vl = Velocity.X + AngularVelocity.Z * WheelSeparationHalf;
     float vr = Velocity.X - AngularVelocity.Z * WheelSeparationHalf;
-    
+
     // noise added as a component of vl, vr
     // Gazebo links this Book here: Sigwart 2011 Autonomous Mobile Robots page:337
     //  seems to be Introduction to Autonomous Mobile Robots (Sigwart, Nourbakhsh, Scaramuzza)
@@ -119,34 +119,34 @@ void UDifferentialDriveComponent::UpdateOdom(float DeltaTime)
     OdomData.twist_twist_linear.Y = 0;
     OdomData.twist_twist_linear.Z = 0;
 
-
-	OdomData.pose_covariance.Init(0,36);
-	OdomData.pose_covariance[0] = 0.01;
-	OdomData.pose_covariance[7] = 0.01;
-	OdomData.pose_covariance[14] = 1000000000000.0;
-	OdomData.pose_covariance[21] = 1000000000000.0;
-	OdomData.pose_covariance[28] = 1000000000000.0;
-	OdomData.pose_covariance[35] = 0.01;
-	OdomData.twist_covariance.Init(0,36);
-	OdomData.twist_covariance[0] = 0.01;
-	OdomData.twist_covariance[7] = 0.01;
-	OdomData.twist_covariance[14] = 1000000000000.0;
-	OdomData.twist_covariance[21] = 1000000000000.0;
-	OdomData.twist_covariance[28] = 1000000000000.0;
-	OdomData.twist_covariance[35] = 0.01;
+    OdomData.pose_covariance.Init(0, 36);
+    OdomData.pose_covariance[0] = 0.01;
+    OdomData.pose_covariance[7] = 0.01;
+    OdomData.pose_covariance[14] = 1000000000000.0;
+    OdomData.pose_covariance[21] = 1000000000000.0;
+    OdomData.pose_covariance[28] = 1000000000000.0;
+    OdomData.pose_covariance[35] = 0.01;
+    OdomData.twist_covariance.Init(0, 36);
+    OdomData.twist_covariance[0] = 0.01;
+    OdomData.twist_covariance[7] = 0.01;
+    OdomData.twist_covariance[14] = 1000000000000.0;
+    OdomData.twist_covariance[21] = 1000000000000.0;
+    OdomData.twist_covariance[28] = 1000000000000.0;
+    OdomData.twist_covariance[35] = 0.01;
 
     // UE_LOG(LogTemp, Warning, TEXT("Input:"));
     // UE_LOG(LogTemp, Warning, TEXT("\tVel: %s, %s"), *Velocity.ToString(), *AngularVelocity.ToString());
     // UE_LOG(LogTemp, Warning, TEXT("Odometry:"));
-    // UE_LOG(LogTemp, Warning, TEXT("\tOdom Positon:\t\t\t\t%f %f from %f %f (%f)"), PoseEncoderX, PoseEncoderY, dx, dy, Velocity.X);
-    // UE_LOG(LogTemp, Warning, TEXT("\tOdom Orientation:\t\t\t%s (%f)"), *OdomData.pose_pose_orientation.ToString(), PoseEncoderTheta);
-    // UE_LOG(LogTemp, Warning, TEXT("\tOdom TwistLin:\t\t\t\t%s - %f"), *OdomData.twist_twist_linear.ToString(), OdomData.twist_twist_linear.Size());
-    // UE_LOG(LogTemp, Warning, TEXT("\tOdom TwistAng:\t\t\t\t%s"), *OdomData.twist_twist_angular.ToString());
+    // UE_LOG(LogTemp, Warning, TEXT("\tOdom Positon:\t\t\t\t%f %f from %f %f (%f)"), PoseEncoderX, PoseEncoderY, dx, dy,
+    // Velocity.X); UE_LOG(LogTemp, Warning, TEXT("\tOdom Orientation:\t\t\t%s (%f)"), *OdomData.pose_pose_orientation.ToString(),
+    // PoseEncoderTheta); UE_LOG(LogTemp, Warning, TEXT("\tOdom TwistLin:\t\t\t\t%s - %f"), *OdomData.twist_twist_linear.ToString(),
+    // OdomData.twist_twist_linear.Size()); UE_LOG(LogTemp, Warning, TEXT("\tOdom TwistAng:\t\t\t\t%s"),
+    // *OdomData.twist_twist_angular.ToString());
 }
 
-void UDifferentialDriveComponent::InitMovementComponent()
+void UDifferentialDriveComponent::Initialize()
 {
-    Super::InitMovementComponent();
+    Super::Initialize();
 
     if (!IsValid(WheelLeft) || !IsValid(WheelRight))
     {

Source/RapyutaSimulationPlugins/Private/Drives/RobotVehicleMovementComponent.cpp

@@ -2,9 +2,9 @@
 
 #include "Drives/RobotVehicleMovementComponent.h"
 
-#include "Kismet/KismetMathLibrary.h"
 #include "GameFramework/Pawn.h"
 #include "GameFramework/PlayerController.h"
+#include "Kismet/KismetMathLibrary.h"
 
 URobotVehicleMovementComponent::URobotVehicleMovementComponent()
 {
@@ -23,7 +23,7 @@ void URobotVehicleMovementComponent::UpdateMovement(float DeltaTime)
     const FQuat OldRotation = UpdatedComponent->GetComponentQuat();
 
     FVector position = UpdatedComponent->ComponentVelocity * DeltaTime;
-    FQuat DeltaRotation(FVector::ZAxisVector, AngularVelocity.Z * DeltaTime);
+    FQuat DeltaRotation(FVector::ZAxisVector, InversionFactor * AngularVelocity.Z * DeltaTime);
 
     DesiredRotation = OldRotation * DeltaRotation;
     DesiredMovement = (OldRotation * position);
@@ -46,12 +46,12 @@ void URobotVehicleMovementComponent::InitOdom()
     InitialTransform.SetTranslation(PawnOwner->GetActorLocation());
     InitialTransform.SetRotation(FQuat(PawnOwner->GetActorRotation()));
 
-    PreviousTransform = FTransform(FQuat(0,0,0,1), FVector(0,0,0), FVector(1,1,1));
+    PreviousTransform = FTransform::Identity;
 
     OdomData.pose_pose_position_x = 0;
     OdomData.pose_pose_position_y = 0;
     OdomData.pose_pose_position_z = 0;
-    OdomData.pose_pose_orientation = FQuat(0,0,0,1);
+    OdomData.pose_pose_orientation = FQuat::Identity;
 
     // todo temporary hardcoded
     OdomData.pose_covariance.Init(0, 36);
@@ -87,7 +87,8 @@ void URobotVehicleMovementComponent::UpdateOdom(float DeltaTime)
     OdomData.header_stamp_nanosec = static_cast<uint32>(ns - (OdomData.header_stamp_sec * 1e+09));
 
     // previous estimated data
-    FVector PreviousEstimatedPos = FVector(OdomData.pose_pose_position_x, OdomData.pose_pose_position_y, OdomData.pose_pose_position_z);
+    FVector PreviousEstimatedPos =
+        FVector(OdomData.pose_pose_position_x, OdomData.pose_pose_position_y, OdomData.pose_pose_position_z);
     FQuat PreviousEstimatedRot = OdomData.pose_pose_orientation;
 
     // position
@@ -100,16 +101,17 @@ void URobotVehicleMovementComponent::UpdateOdom(float DeltaTime)
     FQuat Rot = InitialTransform.GetRotation().Inverse() * PawnOwner->GetActorRotation().Quaternion();
     FQuat PreviousRot = PreviousTransform.GetRotation();
     PreviousTransform.SetRotation(Rot);
-    Rot =  NoiseRot.Quaternion() * PreviousEstimatedRot * PreviousRot.Inverse() * Rot;
+    Rot = NoiseRot.Quaternion() * PreviousEstimatedRot * PreviousRot.Inverse() * Rot;
     Rot.Normalize();
 
     OdomData.pose_pose_position_x = Pos.X;
     OdomData.pose_pose_position_y = Pos.Y;
     OdomData.pose_pose_position_z = Pos.Z;
     OdomData.pose_pose_orientation = Rot;
 
-    OdomData.twist_twist_linear = OdomData.pose_pose_orientation.UnrotateVector( Pos-PreviousEstimatedPos ) / DeltaTime;
-    OdomData.twist_twist_angular = FMath::DegreesToRadians((PreviousEstimatedRot * Rot.Inverse()).GetNormalized().Euler())/DeltaTime;
+    OdomData.twist_twist_linear = OdomData.pose_pose_orientation.UnrotateVector(Pos - PreviousEstimatedPos) / DeltaTime;
+    OdomData.twist_twist_angular =
+        FMath::DegreesToRadians((PreviousEstimatedRot * Rot.Inverse()).GetNormalized().Euler()) / DeltaTime;
 
     // UE_LOG(LogTemp, Warning, TEXT("Odometry:"));
     // UE_LOG(LogTemp, Warning, TEXT("\tCurrent Positon:\t\t\t%s"), *PawnOwner->GetActorLocation().ToString());
@@ -123,8 +125,9 @@ void URobotVehicleMovementComponent::UpdateOdom(float DeltaTime)
     // UE_LOG(LogTemp, Warning, TEXT("\tPrevious Estimated Orientation:\t%s"), *PreviousEstimatedRot.Rotator().ToString());
     // UE_LOG(LogTemp, Warning, TEXT("\tOdom Orientation:\t\t\t%s"), *Rot.Rotator().ToString());
     // UE_LOG(LogTemp, Warning, TEXT("\tOdom Orientation:\t\t\t%s"), *Rot.ToString());
-    // UE_LOG(LogTemp, Warning, TEXT("\tOdom TwistLin:\t\t\t\t%s - %f"), *OdomData.twist_twist_linear.ToString(), OdomData.twist_twist_linear.Size());
-    // UE_LOG(LogTemp, Warning, TEXT("\tOdom TwistAng:\t\t\t\t%s"), *OdomData.twist_twist_angular.ToString());
+    // UE_LOG(LogTemp, Warning, TEXT("\tOdom TwistLin:\t\t\t\t%s - %f"), *OdomData.twist_twist_linear.ToString(),
+    // OdomData.twist_twist_linear.Size()); UE_LOG(LogTemp, Warning, TEXT("\tOdom TwistAng:\t\t\t\t%s"),
+    // *OdomData.twist_twist_angular.ToString());
 }
 
 void URobotVehicleMovementComponent::TickComponent(float DeltaTime,
@@ -155,7 +158,7 @@ FTransform URobotVehicleMovementComponent::GetOdomTF()
     return TF;
 }
 
-void URobotVehicleMovementComponent::InitMovementComponent()
+void URobotVehicleMovementComponent::Initialize()
 {
     InitOdom();
-}
+}

Source/RapyutaSimulationPlugins/Private/Robots/RobotVehicle.cpp

@@ -2,33 +2,35 @@
 
 #include "Robots/RobotVehicle.h"
 
+#include "Drives/RobotVehicleMovementComponent.h"
 #include "Msgs/ROS2TFMsg.h"
 #include "ROS2Node.h"
 
-ARobotVehicle::ARobotVehicle(const FObjectInitializer& ObjectInitializer) : Super(ObjectInitializer)
+void ARobotVehicle::InitializeMoveComponent()
 {
+    RobotVehicleMoveComponent = NewObject<URobotVehicleMovementComponent>(this, TEXT("RobotVehicleMoveComponent"));
 }
 
 void ARobotVehicle::Tick(float DeltaSeconds)
 {
     Super::Tick(DeltaSeconds);
 }
 
-void ARobotVehicle::SetLinearVel(FVector Velocity)
+void ARobotVehicle::SetLinearVel(const FVector& InLinearVelocity)
 {
     // We're assuming input is in meters, so convert to centimeters.
-    MoveComponent->Velocity = Velocity;
+    RobotVehicleMoveComponent->Velocity = InLinearVelocity;    // 0
 }
 
-void ARobotVehicle::SetAngularVel(FVector Velocity)
+void ARobotVehicle::SetAngularVel(const FVector& InAngularVelocity)
 {
-    MoveComponent->AngularVelocity = Velocity;
+    RobotVehicleMoveComponent->AngularVelocity = InAngularVelocity;
 }
 
 void ARobotVehicle::BeginPlay()
 {
     Super::BeginPlay();
-    MoveComponent->InitMovementComponent();
+    RobotVehicleMoveComponent->Initialize();
 }
 
 void ARobotVehicle::EndPlay(const EEndPlayReason::Type EndPlayReason)

Source/RapyutaSimulationPlugins/Private/Robots/Turtlebot3/TurtlebotBurger.cpp

@@ -14,13 +14,19 @@ ATurtlebotBurger::ATurtlebotBurger(const FObjectInitializer& ObjectInitializer)
     PrimaryActorTick.bCanEverTick = true;
 
     Init();
-
+}
+
+void ATurtlebotBurger::InitializeMoveComponent() 
+{
+    Super::InitializeMoveComponent();
+    DifferentialDriveComponent = NewObject<UDifferentialDriveComponent>(this, TEXT("DifferentialDriveComponent"));
 }
 
 void ATurtlebotBurger::Init()
 {
     if (!IsInitialized)
     {
+        InitializeMoveComponent();
         Base = CreateDefaultSubobject<UStaticMeshComponent>(TEXT("Base"));
         LidarSensor = CreateDefaultSubobject<UStaticMeshComponent>(TEXT("LidarSensor"));
         WheelLeft = CreateDefaultSubobject<UStaticMeshComponent>(TEXT("WheelLeft"));
@@ -56,8 +62,6 @@ void ATurtlebotBurger::SetupWheels()
 {
     if (IsInitialized)
     {
-        MoveComponent = CreateDefaultSubobject<UDifferentialDriveComponent>(TEXT("MoveComponent"));
-        UDifferentialDriveComponent* DifferentialDriveComponent = Cast<UDifferentialDriveComponent>(MoveComponent);
         DifferentialDriveComponent->SetWheels(Base_WheelLeft, Base_WheelRight);
         DifferentialDriveComponent->SetPerimeter();
     }

Source/RapyutaSimulationPlugins/Private/Tools/StatePublisher.cpp

@@ -0,0 +1,51 @@
+// Copyright 2020-2021 Rapyuta Robotics Co., Ltd.
+
+#include "Tools/StatePublisher.h"
+
+// rclUE
+#include "ROS2Node.h"
+
+// RapyutaSimulationPlugins
+#include "Tools/SimulationState.h"
+
+void UStatePublisher::RegisterPublisher(AROS2Node* Node)
+{
+    for (auto i = 0; i < StatesToPublish.Num(); i++)
+    {
+        Node->AddPublisher(this);
+    }
+}
+
+void UStatePublisher::PublishState(UROS2GenericMsg* Msg)
+{
+    UROS2EntityStateMsg* StateMsg = Cast<UROS2EntityStateMsg>(Msg);
+    if ((StateMsg != nullptr) && StatesToPublish.IsValidIndex(Idx))
+    {
+        StateMsg->SetMsg(StatesToPublish[Idx]);
+        Idx = (Idx + 1 == StatesToPublish.Num()) ? 0 : Idx + 1;    // ue4 does not seem to have a modulo operator for integers
+                                                                   // (it does have one for floats: FGenericPlatformMath::FMod)
+        check(Idx < StatesToPublish.Num());
+    }
+}
+
+void UStatePublisher::Bind()
+{
+    UpdateDelegate.BindDynamic(this, &UStatePublisher::PublishState);
+}
+
+void UStatePublisher::AddEntityToPublish(const FString& InName,
+                                         const FVector& InPosition,
+                                         const FRotator& InOrientation,
+                                         const FString& InRefFrame)
+{
+    FROSEntityState BodyState;
+    BodyState.name = InName;
+    BodyState.pose_position_x = InPosition.X;
+    BodyState.pose_position_y = -InPosition.Y;
+    BodyState.pose_position_z = InPosition.Z;
+    BodyState.pose_orientation = InOrientation.Quaternion();
+    BodyState.pose_orientation.X = -BodyState.pose_orientation.X;
+    BodyState.pose_orientation.Z = -BodyState.pose_orientation.Z;
+    BodyState.reference_frame = InRefFrame;
+    StatesToPublish.Add(BodyState);
+}

Source/RapyutaSimulationPlugins/Public/Drives/DifferentialDriveComponent.h

@@ -2,54 +2,53 @@
 
 #pragma once
 
-#include <random>
-
 #include "CoreMinimal.h"
 #include "Drives/RobotVehicleMovementComponent.h"
-
 #include "PhysicsEngine/PhysicsConstraintComponent.h"
 
+#include <random>
+
 #include "DifferentialDriveComponent.generated.h"
 
 DECLARE_LOG_CATEGORY_EXTERN(LogDifferentialDriveComponent, Log, All);
 
-UCLASS( ClassGroup=(Custom), meta=(BlueprintSpawnableComponent) )
+UCLASS(ClassGroup = (Custom), meta = (BlueprintSpawnableComponent))
 class RAPYUTASIMULATIONPLUGINS_API UDifferentialDriveComponent : public URobotVehicleMovementComponent
 {
-	GENERATED_BODY()
+    GENERATED_BODY()
 
 public:
-	UDifferentialDriveComponent();
-	virtual void UpdateMovement(float DeltaTime) override;
-	virtual void UpdateOdom(float DeltaTime) override;
-	
-	UFUNCTION(BlueprintCallable)
-	void SetWheels(UPhysicsConstraintComponent* InWheelLeft, UPhysicsConstraintComponent* InWheelRight);
+    UDifferentialDriveComponent();
+    virtual void UpdateMovement(float DeltaTime) override;
+    virtual void UpdateOdom(float DeltaTime) override;
+
+    UFUNCTION(BlueprintCallable)
+    void SetWheels(UPhysicsConstraintComponent* InWheelLeft, UPhysicsConstraintComponent* InWheelRight);
 
-	virtual void InitMovementComponent() override;
+    virtual void Initialize() override;
 
-	UFUNCTION(BlueprintCallable)
-	void SetPerimeter();
+    UFUNCTION(BlueprintCallable)
+    void SetPerimeter();
 
-	UPROPERTY(EditAnywhere,BlueprintReadWrite)
-	UPhysicsConstraintComponent* WheelLeft;
+    UPROPERTY(EditAnywhere, BlueprintReadWrite)
+    UPhysicsConstraintComponent* WheelLeft;
 
-	UPROPERTY(EditAnywhere,BlueprintReadWrite)
-	UPhysicsConstraintComponent* WheelRight;
+    UPROPERTY(EditAnywhere, BlueprintReadWrite)
+    UPhysicsConstraintComponent* WheelRight;
 
-	UPROPERTY(EditAnywhere,BlueprintReadWrite)
-	float WheelRadius = 1.0f;
+    UPROPERTY(EditAnywhere, BlueprintReadWrite)
+    float WheelRadius = 1.0f;
 
-	UPROPERTY(EditAnywhere,BlueprintReadWrite)
-	float WheelSeparationHalf = 1.0f; //todo get data from links
+    UPROPERTY(EditAnywhere, BlueprintReadWrite)
+    float WheelSeparationHalf = 1.0f;    // todo get data from links
 
-	UPROPERTY(EditAnywhere,BlueprintReadWrite)
-	float MaxForce = 1000; //todo get data from physics constraints
+    UPROPERTY(EditAnywhere, BlueprintReadWrite)
+    float MaxForce = 1000;    // todo get data from physics constraints
 
 private:
-	float WheelPerimeter = 6.28f;
+    float WheelPerimeter = 6.28f;
 
-	float PoseEncoderX = 0;
-	float PoseEncoderY = 0;
-	float PoseEncoderTheta = 0;
+    float PoseEncoderX = 0;
+    float PoseEncoderY = 0;
+    float PoseEncoderTheta = 0;
 };