From 2530d9021b228d351723b83f93fb4a218ee89fb5 Mon Sep 17 00:00:00 2001 From: praphulkallakuri Date: Mon, 25 Oct 2021 17:17:26 +0530 Subject: [PATCH 1/9] Move components duplication refactored for RobotVehicle class and AMRVehicle class --- .../Drives/DifferentialDriveComponent.cpp | 4 ++-- .../Drives/RobotVehicleMovementComponent.cpp | 4 ++-- .../Private/Robots/RobotVehicle.cpp | 16 +++++++++------- .../Robots/Turtlebot3/TurtlebotBurger.cpp | 9 ++++++--- .../Public/Drives/DifferentialDriveComponent.h | 2 +- .../Drives/RobotVehicleMovementComponent.h | 4 ++-- .../Public/Robots/RobotVehicle.h | 10 ++++++---- .../Public/Robots/Turtlebot3/TurtlebotBurger.h | 9 ++++++--- 8 files changed, 34 insertions(+), 24 deletions(-) diff --git a/Source/RapyutaSimulationPlugins/Private/Drives/DifferentialDriveComponent.cpp b/Source/RapyutaSimulationPlugins/Private/Drives/DifferentialDriveComponent.cpp index f10badad..2db47404 100644 --- a/Source/RapyutaSimulationPlugins/Private/Drives/DifferentialDriveComponent.cpp +++ b/Source/RapyutaSimulationPlugins/Private/Drives/DifferentialDriveComponent.cpp @@ -144,9 +144,9 @@ void UDifferentialDriveComponent::UpdateOdom(float DeltaTime) // 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)) { diff --git a/Source/RapyutaSimulationPlugins/Private/Drives/RobotVehicleMovementComponent.cpp b/Source/RapyutaSimulationPlugins/Private/Drives/RobotVehicleMovementComponent.cpp index cbb96997..8150a456 100644 --- a/Source/RapyutaSimulationPlugins/Private/Drives/RobotVehicleMovementComponent.cpp +++ b/Source/RapyutaSimulationPlugins/Private/Drives/RobotVehicleMovementComponent.cpp @@ -155,7 +155,7 @@ FTransform URobotVehicleMovementComponent::GetOdomTF() return TF; } -void URobotVehicleMovementComponent::InitMovementComponent() -{ +void URobotVehicleMovementComponent::Initialize() +{ InitOdom(); } \ No newline at end of file diff --git a/Source/RapyutaSimulationPlugins/Private/Robots/RobotVehicle.cpp b/Source/RapyutaSimulationPlugins/Private/Robots/RobotVehicle.cpp index 5e9505aa..89b9e5ca 100644 --- a/Source/RapyutaSimulationPlugins/Private/Robots/RobotVehicle.cpp +++ b/Source/RapyutaSimulationPlugins/Private/Robots/RobotVehicle.cpp @@ -1,12 +1,14 @@ // Copyright 2020-2021 Rapyuta Robotics Co., Ltd. #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(this, TEXT("RobotVehicleMoveComponent")); + verify(RobotVehicleMoveComponent); } void ARobotVehicle::Tick(float DeltaSeconds) @@ -14,21 +16,21 @@ 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) diff --git a/Source/RapyutaSimulationPlugins/Private/Robots/Turtlebot3/TurtlebotBurger.cpp b/Source/RapyutaSimulationPlugins/Private/Robots/Turtlebot3/TurtlebotBurger.cpp index c99c6ce8..5fd5dbbc 100644 --- a/Source/RapyutaSimulationPlugins/Private/Robots/Turtlebot3/TurtlebotBurger.cpp +++ b/Source/RapyutaSimulationPlugins/Private/Robots/Turtlebot3/TurtlebotBurger.cpp @@ -14,13 +14,18 @@ ATurtlebotBurger::ATurtlebotBurger(const FObjectInitializer& ObjectInitializer) PrimaryActorTick.bCanEverTick = true; Init(); - +} + +void ATurtlebotBurger::InitializeMoveComponent() +{ + DifferentialDriveComponent = NewObject(this, TEXT("DifferentialDriveComponent")); } void ATurtlebotBurger::Init() { if (!IsInitialized) { + InitializeMoveComponent(); Base = CreateDefaultSubobject(TEXT("Base")); LidarSensor = CreateDefaultSubobject(TEXT("LidarSensor")); WheelLeft = CreateDefaultSubobject(TEXT("WheelLeft")); @@ -56,8 +61,6 @@ void ATurtlebotBurger::SetupWheels() { if (IsInitialized) { - MoveComponent = CreateDefaultSubobject(TEXT("MoveComponent")); - UDifferentialDriveComponent* DifferentialDriveComponent = Cast(MoveComponent); DifferentialDriveComponent->SetWheels(Base_WheelLeft, Base_WheelRight); DifferentialDriveComponent->SetPerimeter(); } diff --git a/Source/RapyutaSimulationPlugins/Public/Drives/DifferentialDriveComponent.h b/Source/RapyutaSimulationPlugins/Public/Drives/DifferentialDriveComponent.h index 1f0d8760..a8a2c229 100644 --- a/Source/RapyutaSimulationPlugins/Public/Drives/DifferentialDriveComponent.h +++ b/Source/RapyutaSimulationPlugins/Public/Drives/DifferentialDriveComponent.h @@ -26,7 +26,7 @@ class RAPYUTASIMULATIONPLUGINS_API UDifferentialDriveComponent : public URobotVe UFUNCTION(BlueprintCallable) void SetWheels(UPhysicsConstraintComponent* InWheelLeft, UPhysicsConstraintComponent* InWheelRight); - virtual void InitMovementComponent() override; + virtual void Initialize() override; UFUNCTION(BlueprintCallable) void SetPerimeter(); diff --git a/Source/RapyutaSimulationPlugins/Public/Drives/RobotVehicleMovementComponent.h b/Source/RapyutaSimulationPlugins/Public/Drives/RobotVehicleMovementComponent.h index b48adbd5..0ad75487 100644 --- a/Source/RapyutaSimulationPlugins/Public/Drives/RobotVehicleMovementComponent.h +++ b/Source/RapyutaSimulationPlugins/Public/Drives/RobotVehicleMovementComponent.h @@ -1,7 +1,7 @@ // Copyright 2020-2021 Rapyuta Robotics Co., Ltd. #pragma once - +#include #include #include "CoreMinimal.h" #include "GameFramework/FloatingPawnMovement.h" @@ -43,7 +43,7 @@ class RCLUE_API URobotVehicleMovementComponent : public UPawnMovementComponent FTransform GetOdomTF(); UFUNCTION(BlueprintCallable) - virtual void InitMovementComponent(); + virtual void Initialize(); protected: virtual void BeginPlay() override; diff --git a/Source/RapyutaSimulationPlugins/Public/Robots/RobotVehicle.h b/Source/RapyutaSimulationPlugins/Public/Robots/RobotVehicle.h index 67442f6b..62346352 100644 --- a/Source/RapyutaSimulationPlugins/Public/Robots/RobotVehicle.h +++ b/Source/RapyutaSimulationPlugins/Public/Robots/RobotVehicle.h @@ -8,6 +8,7 @@ #include "RobotVehicle.generated.h" +class URobotVehicleMovementComponent; UCLASS() class RAPYUTASIMULATIONPLUGINS_API ARobotVehicle : public APawn { @@ -16,18 +17,19 @@ class RAPYUTASIMULATIONPLUGINS_API ARobotVehicle : public APawn public: UPROPERTY(EditAnywhere, BlueprintReadWrite) - URobotVehicleMovementComponent *MoveComponent; + URobotVehicleMovementComponent* RobotVehicleMoveComponent = nullptr; + + virtual void InitializeMoveComponent(); public: - ARobotVehicle(const FObjectInitializer& ObjectInitializer); virtual void Tick(float DeltaSeconds) override; UFUNCTION(BlueprintCallable) - virtual void SetLinearVel(FVector velocity); + virtual void SetLinearVel(const FVector& InLinearVelocity); UFUNCTION(BlueprintCallable) - virtual void SetAngularVel(FVector velocity); + virtual void SetAngularVel(const FVector& InAngularVelocity); protected: diff --git a/Source/RapyutaSimulationPlugins/Public/Robots/Turtlebot3/TurtlebotBurger.h b/Source/RapyutaSimulationPlugins/Public/Robots/Turtlebot3/TurtlebotBurger.h index ca2dd32a..801b09e2 100644 --- a/Source/RapyutaSimulationPlugins/Public/Robots/Turtlebot3/TurtlebotBurger.h +++ b/Source/RapyutaSimulationPlugins/Public/Robots/Turtlebot3/TurtlebotBurger.h @@ -20,6 +20,11 @@ class RAPYUTASIMULATIONPLUGINS_API ATurtlebotBurger : public ARobotVehicle public: ATurtlebotBurger(const FObjectInitializer& ObjectInitializer); + UPROPERTY(EditAnywhere, BlueprintReadWrite) + UDifferentialDriveComponent* DifferentialDriveComponent = nullptr; + + void InitializeMoveComponent() override; + protected: // Called when the game starts or when spawned virtual void BeginPlay() override; @@ -79,7 +84,5 @@ class RAPYUTASIMULATIONPLUGINS_API ATurtlebotBurger : public ARobotVehicle void SetupConstraintsAndPhysics(); UFUNCTION() - void SetupWheels(); - - + void SetupWheels(); }; From 2b1cae557d7f9eca93053b41f5c5c00067980e7e Mon Sep 17 00:00:00 2001 From: praphulkallakuri Date: Wed, 27 Oct 2021 12:08:16 +0530 Subject: [PATCH 2/9] Clang formatted code --- .../Drives/DifferentialDriveComponent.cpp | 50 +++---- .../Private/Robots/RobotVehicle.cpp | 5 +- .../Drives/DifferentialDriveComponent.h | 55 ++++---- .../Drives/RobotVehicleMovementComponent.h | 78 +++++------ .../Public/Robots/RobotVehicle.h | 27 ++-- .../Robots/Turtlebot3/TurtlebotBurger.h | 127 +++++++++--------- 6 files changed, 168 insertions(+), 174 deletions(-) diff --git a/Source/RapyutaSimulationPlugins/Private/Drives/DifferentialDriveComponent.cpp b/Source/RapyutaSimulationPlugins/Private/Drives/DifferentialDriveComponent.cpp index 2db47404..9c9e3fe6 100644 --- a/Source/RapyutaSimulationPlugins/Private/Drives/DifferentialDriveComponent.cpp +++ b/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,12 +68,12 @@ void UDifferentialDriveComponent::UpdateOdom(float DeltaTime) uint64 ns = (uint64)(TimeNow * 1e+09f); OdomData.header_stamp_nanosec = static_cast(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 @@ -81,7 +81,7 @@ void UDifferentialDriveComponent::UpdateOdom(float DeltaTime) // 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,29 +119,29 @@ 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::Initialize() diff --git a/Source/RapyutaSimulationPlugins/Private/Robots/RobotVehicle.cpp b/Source/RapyutaSimulationPlugins/Private/Robots/RobotVehicle.cpp index 89b9e5ca..d052f7a2 100644 --- a/Source/RapyutaSimulationPlugins/Private/Robots/RobotVehicle.cpp +++ b/Source/RapyutaSimulationPlugins/Private/Robots/RobotVehicle.cpp @@ -1,6 +1,7 @@ // Copyright 2020-2021 Rapyuta Robotics Co., Ltd. #include "Robots/RobotVehicle.h" + #include "Drives/RobotVehicleMovementComponent.h" #include "Msgs/ROS2TFMsg.h" #include "ROS2Node.h" @@ -19,12 +20,12 @@ void ARobotVehicle::Tick(float DeltaSeconds) void ARobotVehicle::SetLinearVel(const FVector& InLinearVelocity) { // We're assuming input is in meters, so convert to centimeters. - RobotVehicleMoveComponent->Velocity = InLinearVelocity; //0 + RobotVehicleMoveComponent->Velocity = InLinearVelocity; // 0 } void ARobotVehicle::SetAngularVel(const FVector& InAngularVelocity) { - RobotVehicleMoveComponent->AngularVelocity = InAngularVelocity; + RobotVehicleMoveComponent->AngularVelocity = InAngularVelocity; } void ARobotVehicle::BeginPlay() diff --git a/Source/RapyutaSimulationPlugins/Public/Drives/DifferentialDriveComponent.h b/Source/RapyutaSimulationPlugins/Public/Drives/DifferentialDriveComponent.h index a8a2c229..f0e6b6eb 100644 --- a/Source/RapyutaSimulationPlugins/Public/Drives/DifferentialDriveComponent.h +++ b/Source/RapyutaSimulationPlugins/Public/Drives/DifferentialDriveComponent.h @@ -2,54 +2,53 @@ #pragma once -#include - #include "CoreMinimal.h" #include "Drives/RobotVehicleMovementComponent.h" - #include "PhysicsEngine/PhysicsConstraintComponent.h" +#include + #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 Initialize() 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; }; diff --git a/Source/RapyutaSimulationPlugins/Public/Drives/RobotVehicleMovementComponent.h b/Source/RapyutaSimulationPlugins/Public/Drives/RobotVehicleMovementComponent.h index 0ad75487..023d6bfe 100644 --- a/Source/RapyutaSimulationPlugins/Public/Drives/RobotVehicleMovementComponent.h +++ b/Source/RapyutaSimulationPlugins/Public/Drives/RobotVehicleMovementComponent.h @@ -1,65 +1,66 @@ // Copyright 2020-2021 Rapyuta Robotics Co., Ltd. #pragma once -#include -#include #include "CoreMinimal.h" #include "GameFramework/FloatingPawnMovement.h" #include "Kismet/GameplayStatics.h" + +#include + +#include + #include "RobotVehicleMovementComponent.generated.h" -UCLASS( ClassGroup=(Custom), meta=(BlueprintSpawnableComponent) ) +UCLASS(ClassGroup = (Custom), meta = (BlueprintSpawnableComponent)) class RCLUE_API URobotVehicleMovementComponent : public UPawnMovementComponent { - GENERATED_BODY() + GENERATED_BODY() private: + UPROPERTY(Transient) + FVector DesiredMovement; - UPROPERTY(Transient) - FVector DesiredMovement; - - UPROPERTY(Transient) - FQuat DesiredRotation; + UPROPERTY(Transient) + FQuat DesiredRotation; public: - URobotVehicleMovementComponent(); + URobotVehicleMovementComponent(); - UPROPERTY(VisibleAnywhere, BlueprintReadWrite, Category = Velocity) - FVector AngularVelocity; + UPROPERTY(VisibleAnywhere, BlueprintReadWrite, Category = Velocity) + FVector AngularVelocity; - UPROPERTY(VisibleAnywhere, BlueprintReadWrite) - FROSOdometry OdomData; + UPROPERTY(VisibleAnywhere, BlueprintReadWrite) + FROSOdometry OdomData; - UPROPERTY(EditAnywhere, BlueprintReadWrite) - FString FrameId = TEXT(""); + UPROPERTY(EditAnywhere, BlueprintReadWrite) + FString FrameId = TEXT(""); - UPROPERTY(EditAnywhere, BlueprintReadWrite) - FString ChildFrameId = TEXT(""); + UPROPERTY(EditAnywhere, BlueprintReadWrite) + FString ChildFrameId = TEXT(""); - UPROPERTY(EditAnywhere) - FTransform InitialTransform; + UPROPERTY(EditAnywhere) + FTransform InitialTransform; - UFUNCTION(BlueprintCallable) - FTransform GetOdomTF(); + UFUNCTION(BlueprintCallable) + FTransform GetOdomTF(); - UFUNCTION(BlueprintCallable) - virtual void Initialize(); + UFUNCTION(BlueprintCallable) + virtual void Initialize(); protected: - virtual void BeginPlay() override; - virtual void InitOdom(); - virtual void UpdateMovement(float DeltaTime); - virtual void UpdateOdom(float DeltaTime); - bool IsOdomInitialized = false; - - UPROPERTY(EditAnywhere) - FTransform PreviousTransform; + virtual void BeginPlay() override; + virtual void InitOdom(); + virtual void UpdateMovement(float DeltaTime); + virtual void UpdateOdom(float DeltaTime); + bool IsOdomInitialized = false; + UPROPERTY(EditAnywhere) + FTransform PreviousTransform; - std::random_device Rng; - std::mt19937 Gen; - std::normal_distribution<> GaussianRNGPosition; - std::normal_distribution<> GaussianRNGRotation; + std::random_device Rng; + std::mt19937 Gen; + std::normal_distribution<> GaussianRNGPosition; + std::normal_distribution<> GaussianRNGRotation; UPROPERTY(EditAnywhere, Category = "Noise") float NoiseMeanPos = 0.f; @@ -75,8 +76,7 @@ class RCLUE_API URobotVehicleMovementComponent : public UPawnMovementComponent UPROPERTY(EditAnywhere, Category = "Noise") bool WithNoise = true; - -public: - virtual void TickComponent(float DeltaTime, enum ELevelTick TickType, FActorComponentTickFunction *ThisTickFunction) override; +public: + virtual void TickComponent(float DeltaTime, enum ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; }; diff --git a/Source/RapyutaSimulationPlugins/Public/Robots/RobotVehicle.h b/Source/RapyutaSimulationPlugins/Public/Robots/RobotVehicle.h index 62346352..595925dc 100644 --- a/Source/RapyutaSimulationPlugins/Public/Robots/RobotVehicle.h +++ b/Source/RapyutaSimulationPlugins/Public/Robots/RobotVehicle.h @@ -3,8 +3,8 @@ #pragma once #include "CoreMinimal.h" -#include "GameFramework/Pawn.h" #include "Drives/RobotVehicleMovementComponent.h" +#include "GameFramework/Pawn.h" #include "RobotVehicle.generated.h" @@ -12,28 +12,25 @@ class URobotVehicleMovementComponent; UCLASS() class RAPYUTASIMULATIONPLUGINS_API ARobotVehicle : public APawn { - GENERATED_BODY() + GENERATED_BODY() public: + UPROPERTY(EditAnywhere, BlueprintReadWrite) + URobotVehicleMovementComponent* RobotVehicleMoveComponent = nullptr; - UPROPERTY(EditAnywhere, BlueprintReadWrite) - URobotVehicleMovementComponent* RobotVehicleMoveComponent = nullptr; - - virtual void InitializeMoveComponent(); + virtual void InitializeMoveComponent(); public: + virtual void Tick(float DeltaSeconds) override; - virtual void Tick(float DeltaSeconds) override; + UFUNCTION(BlueprintCallable) + virtual void SetLinearVel(const FVector& InLinearVelocity); - UFUNCTION(BlueprintCallable) - virtual void SetLinearVel(const FVector& InLinearVelocity); - - UFUNCTION(BlueprintCallable) - virtual void SetAngularVel(const FVector& InAngularVelocity); + UFUNCTION(BlueprintCallable) + virtual void SetAngularVel(const FVector& InAngularVelocity); protected: + virtual void BeginPlay() override; - virtual void BeginPlay() override; - - virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override; + virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override; }; diff --git a/Source/RapyutaSimulationPlugins/Public/Robots/Turtlebot3/TurtlebotBurger.h b/Source/RapyutaSimulationPlugins/Public/Robots/Turtlebot3/TurtlebotBurger.h index 801b09e2..4d74fb36 100644 --- a/Source/RapyutaSimulationPlugins/Public/Robots/Turtlebot3/TurtlebotBurger.h +++ b/Source/RapyutaSimulationPlugins/Public/Robots/Turtlebot3/TurtlebotBurger.h @@ -3,10 +3,9 @@ #pragma once #include "CoreMinimal.h" -#include "Robots/RobotVehicle.h" - #include "Drives/DifferentialDriveComponent.h" #include "PhysicsEngine/PhysicsConstraintComponent.h" +#include "Robots/RobotVehicle.h" #include "TurtlebotBurger.generated.h" @@ -15,74 +14,72 @@ DECLARE_LOG_CATEGORY_EXTERN(LogTurtlebotBurger, Log, All); UCLASS() class RAPYUTASIMULATIONPLUGINS_API ATurtlebotBurger : public ARobotVehicle { - GENERATED_BODY() - -public: - ATurtlebotBurger(const FObjectInitializer& ObjectInitializer); + GENERATED_BODY() - UPROPERTY(EditAnywhere, BlueprintReadWrite) - UDifferentialDriveComponent* DifferentialDriveComponent = nullptr; +public: + ATurtlebotBurger(const FObjectInitializer& ObjectInitializer); - void InitializeMoveComponent() override; + UPROPERTY(EditAnywhere, BlueprintReadWrite) + UDifferentialDriveComponent* DifferentialDriveComponent = nullptr; -protected: - // Called when the game starts or when spawned - virtual void BeginPlay() override; - - virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override; - -public: - // Called every frame - virtual void Tick(float DeltaTime) override; - - UFUNCTION(BlueprintCallable) - virtual void Init(); - - UPROPERTY(EditAnywhere,BlueprintReadWrite) - UStaticMeshComponent* Base; - - UPROPERTY(EditAnywhere,BlueprintReadWrite) - UStaticMeshComponent* LidarSensor; - - UPROPERTY(EditAnywhere,BlueprintReadWrite) - UStaticMeshComponent* WheelLeft; - - UPROPERTY(EditAnywhere,BlueprintReadWrite) - UStaticMeshComponent* WheelRight; - - UPROPERTY(EditAnywhere,BlueprintReadWrite) - UStaticMeshComponent* CasterBack; - - - UPROPERTY(EditAnywhere,BlueprintReadWrite) - UPhysicsConstraintComponent* Base_LidarSensor; - - UPROPERTY(EditAnywhere,BlueprintReadWrite) - UPhysicsConstraintComponent* Base_WheelLeft; - - UPROPERTY(EditAnywhere,BlueprintReadWrite) - UPhysicsConstraintComponent* Base_WheelRight; - - UPROPERTY(EditAnywhere,BlueprintReadWrite) - UPhysicsConstraintComponent* Base_CasterBack; - - UPROPERTY(EditAnywhere,BlueprintReadWrite) - float MaxForce = 1000; - - UPROPERTY(EditAnywhere,BlueprintReadWrite) - UMaterial* VehicleMaterial; - - UPROPERTY(EditAnywhere,BlueprintReadWrite) - UMaterial* BallMaterial; - - UPROPERTY(VisibleAnywhere) - bool IsInitialized = false; + void InitializeMoveComponent() override; protected: + // Called when the game starts or when spawned + virtual void BeginPlay() override; + + virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override; + +public: + // Called every frame + virtual void Tick(float DeltaTime) override; + + UFUNCTION(BlueprintCallable) + virtual void Init(); + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + UStaticMeshComponent* Base; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + UStaticMeshComponent* LidarSensor; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + UStaticMeshComponent* WheelLeft; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + UStaticMeshComponent* WheelRight; - UFUNCTION() - void SetupConstraintsAndPhysics(); + UPROPERTY(EditAnywhere, BlueprintReadWrite) + UStaticMeshComponent* CasterBack; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + UPhysicsConstraintComponent* Base_LidarSensor; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + UPhysicsConstraintComponent* Base_WheelLeft; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + UPhysicsConstraintComponent* Base_WheelRight; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + UPhysicsConstraintComponent* Base_CasterBack; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + float MaxForce = 1000; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + UMaterial* VehicleMaterial; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + UMaterial* BallMaterial; + + UPROPERTY(VisibleAnywhere) + bool IsInitialized = false; + +protected: + UFUNCTION() + void SetupConstraintsAndPhysics(); - UFUNCTION() - void SetupWheels(); + UFUNCTION() + void SetupWheels(); }; From 10feaae8ae7a48bf1df76ba8320dfcf0fbe43366 Mon Sep 17 00:00:00 2001 From: praphulkallakuri Date: Mon, 1 Nov 2021 09:08:21 +0530 Subject: [PATCH 3/9] Individual entity moved from io_amr_UE --- .../Private/Tools/StatePublisher.cpp | 51 +++++++++++++++++++ .../Public/Tools/StatePublisher.h | 50 ++++++++++++++++++ 2 files changed, 101 insertions(+) create mode 100644 Source/RapyutaSimulationPlugins/Private/Tools/StatePublisher.cpp create mode 100644 Source/RapyutaSimulationPlugins/Public/Tools/StatePublisher.h diff --git a/Source/RapyutaSimulationPlugins/Private/Tools/StatePublisher.cpp b/Source/RapyutaSimulationPlugins/Private/Tools/StatePublisher.cpp new file mode 100644 index 00000000..6ebd88ed --- /dev/null +++ b/Source/RapyutaSimulationPlugins/Private/Tools/StatePublisher.cpp @@ -0,0 +1,51 @@ +// Copyright 2020-2021 Rapyuta Robotics Co., Ltd. + +#include "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(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); +} diff --git a/Source/RapyutaSimulationPlugins/Public/Tools/StatePublisher.h b/Source/RapyutaSimulationPlugins/Public/Tools/StatePublisher.h new file mode 100644 index 00000000..db80d4b1 --- /dev/null +++ b/Source/RapyutaSimulationPlugins/Public/Tools/StatePublisher.h @@ -0,0 +1,50 @@ +// Copyright 2020-2021 Rapyuta Robotics Co., Ltd. + +#pragma once + +// UE +#include "CoreMinimal.h" + +// rclUE +#include "Msgs/ROS2EntityStateMsg.h" +#include "ROS2Publisher.h" + +#include "StatePublisher.generated.h" + +class AROS2Node; + +/** + * This class should be attached to a robot and publish its states + * It is responsibility of the owning robot to update the structs, this way the class can be kept more flexible and work with robots + * defined by multiple actors as well as robots defined by a skeletal mesh This could use a refactor once the robots are more well + * defined and could require a refactor of the main publisher class as well, to avoid the iterator Idx as it is used now Ideally, it + * should be this class that fetches all the necessary data to be published + */ +UCLASS(ClassGroup = (Custom), Blueprintable, meta = (BlueprintSpawnableComponent)) +class IO_AMR_UE_API UStatePublisher : public UROS2Publisher +{ + GENERATED_BODY() + +public: + UFUNCTION(BlueprintCallable) + virtual void RegisterPublisher(AROS2Node* Node); + + UFUNCTION() + virtual void PublishState(UROS2GenericMsg* Msg); + + UFUNCTION(BlueprintCallable) + virtual void Bind(); + + UFUNCTION(BlueprintCallable) + void AddEntityToPublish(const FString& InName, + const FVector& InPosition, + const FRotator& InOrientation, + const FString& InRefFrame); + + // it's responsibility of the owner to update this + UPROPERTY(EditAnywhere, BlueprintReadWrite) + TArray StatesToPublish; + + UPROPERTY() + int32 Idx = 0; +}; From c7df2d7b7499137e2ada22bbaa2e0fd40baa1fbd Mon Sep 17 00:00:00 2001 From: praphulkallakuri Date: Mon, 1 Nov 2021 15:46:24 +0530 Subject: [PATCH 4/9] deleted files --- .../Private/Tools/StatePublisher.cpp | 51 ------------------- .../Public/Tools/StatePublisher.h | 50 ------------------ 2 files changed, 101 deletions(-) delete mode 100644 Source/RapyutaSimulationPlugins/Private/Tools/StatePublisher.cpp delete mode 100644 Source/RapyutaSimulationPlugins/Public/Tools/StatePublisher.h diff --git a/Source/RapyutaSimulationPlugins/Private/Tools/StatePublisher.cpp b/Source/RapyutaSimulationPlugins/Private/Tools/StatePublisher.cpp deleted file mode 100644 index 6ebd88ed..00000000 --- a/Source/RapyutaSimulationPlugins/Private/Tools/StatePublisher.cpp +++ /dev/null @@ -1,51 +0,0 @@ -// Copyright 2020-2021 Rapyuta Robotics Co., Ltd. - -#include "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(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); -} diff --git a/Source/RapyutaSimulationPlugins/Public/Tools/StatePublisher.h b/Source/RapyutaSimulationPlugins/Public/Tools/StatePublisher.h deleted file mode 100644 index db80d4b1..00000000 --- a/Source/RapyutaSimulationPlugins/Public/Tools/StatePublisher.h +++ /dev/null @@ -1,50 +0,0 @@ -// Copyright 2020-2021 Rapyuta Robotics Co., Ltd. - -#pragma once - -// UE -#include "CoreMinimal.h" - -// rclUE -#include "Msgs/ROS2EntityStateMsg.h" -#include "ROS2Publisher.h" - -#include "StatePublisher.generated.h" - -class AROS2Node; - -/** - * This class should be attached to a robot and publish its states - * It is responsibility of the owning robot to update the structs, this way the class can be kept more flexible and work with robots - * defined by multiple actors as well as robots defined by a skeletal mesh This could use a refactor once the robots are more well - * defined and could require a refactor of the main publisher class as well, to avoid the iterator Idx as it is used now Ideally, it - * should be this class that fetches all the necessary data to be published - */ -UCLASS(ClassGroup = (Custom), Blueprintable, meta = (BlueprintSpawnableComponent)) -class IO_AMR_UE_API UStatePublisher : public UROS2Publisher -{ - GENERATED_BODY() - -public: - UFUNCTION(BlueprintCallable) - virtual void RegisterPublisher(AROS2Node* Node); - - UFUNCTION() - virtual void PublishState(UROS2GenericMsg* Msg); - - UFUNCTION(BlueprintCallable) - virtual void Bind(); - - UFUNCTION(BlueprintCallable) - void AddEntityToPublish(const FString& InName, - const FVector& InPosition, - const FRotator& InOrientation, - const FString& InRefFrame); - - // it's responsibility of the owner to update this - UPROPERTY(EditAnywhere, BlueprintReadWrite) - TArray StatesToPublish; - - UPROPERTY() - int32 Idx = 0; -}; From 894de72449c9e22eb5db8a40ce674e8a2b79ee63 Mon Sep 17 00:00:00 2001 From: praphulkallakuri Date: Tue, 2 Nov 2021 11:43:29 +0530 Subject: [PATCH 5/9] Moved StatePublisher class with minor changes --- .../Private/Tools/StatePublisher.cpp | 51 +++++++++++++++++++ .../Public/Tools/StatePublisher.h | 50 ++++++++++++++++++ 2 files changed, 101 insertions(+) create mode 100644 Source/RapyutaSimulationPlugins/Private/Tools/StatePublisher.cpp create mode 100644 Source/RapyutaSimulationPlugins/Public/Tools/StatePublisher.h diff --git a/Source/RapyutaSimulationPlugins/Private/Tools/StatePublisher.cpp b/Source/RapyutaSimulationPlugins/Private/Tools/StatePublisher.cpp new file mode 100644 index 00000000..7a9051df --- /dev/null +++ b/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(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); +} diff --git a/Source/RapyutaSimulationPlugins/Public/Tools/StatePublisher.h b/Source/RapyutaSimulationPlugins/Public/Tools/StatePublisher.h new file mode 100644 index 00000000..eede9dd1 --- /dev/null +++ b/Source/RapyutaSimulationPlugins/Public/Tools/StatePublisher.h @@ -0,0 +1,50 @@ +// Copyright 2020-2021 Rapyuta Robotics Co., Ltd. + +#pragma once + +// UE +#include "CoreMinimal.h" + +// rclUE +#include "Msgs/ROS2EntityStateMsg.h" +#include "ROS2Publisher.h" + +#include "StatePublisher.generated.h" + +class AROS2Node; + +/** + * This class should be attached to a robot and publish its states + * It is responsibility of the owning robot to update the structs, this way the class can be kept more flexible and work with robots + * defined by multiple actors as well as robots defined by a skeletal mesh This could use a refactor once the robots are more well + * defined and could require a refactor of the main publisher class as well, to avoid the iterator Idx as it is used now Ideally, it + * should be this class that fetches all the necessary data to be published + */ +UCLASS(ClassGroup = (Custom), Blueprintable, meta = (BlueprintSpawnableComponent)) +class RAPYUTASIMULATIONPLUGINS_API UStatePublisher : public UROS2Publisher +{ + GENERATED_BODY() + +public: + UFUNCTION(BlueprintCallable) + virtual void RegisterPublisher(AROS2Node* Node); + + UFUNCTION() + virtual void PublishState(UROS2GenericMsg* Msg); + + UFUNCTION(BlueprintCallable) + virtual void Bind(); + + UFUNCTION(BlueprintCallable) + void AddEntityToPublish(const FString& InName, + const FVector& InPosition, + const FRotator& InOrientation, + const FString& InRefFrame); + + // it's responsibility of the owner to update this + UPROPERTY(EditAnywhere, BlueprintReadWrite) + TArray StatesToPublish; + + UPROPERTY() + int32 Idx = 0; +}; From 685fed76c913524e0181eef53b24c12175935f2c Mon Sep 17 00:00:00 2001 From: praphulkallakuri Date: Tue, 2 Nov 2021 11:54:57 +0530 Subject: [PATCH 6/9] Add InversionFactor variable in RobotVehicleMovementComponent class, minor formatting --- .../Drives/RobotVehicleMovementComponent.cpp | 25 +++++++++++-------- .../Drives/RobotVehicleMovementComponent.h | 3 +++ 2 files changed, 17 insertions(+), 11 deletions(-) diff --git a/Source/RapyutaSimulationPlugins/Private/Drives/RobotVehicleMovementComponent.cpp b/Source/RapyutaSimulationPlugins/Private/Drives/RobotVehicleMovementComponent.cpp index 8150a456..2ae66b49 100644 --- a/Source/RapyutaSimulationPlugins/Private/Drives/RobotVehicleMovementComponent.cpp +++ b/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(FQuat(0, 0, 0, 1), FVector(0, 0, 0), FVector(1, 1, 1)); 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(0, 0, 0, 1); // todo temporary hardcoded OdomData.pose_covariance.Init(0, 36); @@ -87,7 +87,8 @@ void URobotVehicleMovementComponent::UpdateOdom(float DeltaTime) OdomData.header_stamp_nanosec = static_cast(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,7 +101,7 @@ 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; @@ -108,8 +109,9 @@ void URobotVehicleMovementComponent::UpdateOdom(float DeltaTime) 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, @@ -156,6 +159,6 @@ FTransform URobotVehicleMovementComponent::GetOdomTF() } void URobotVehicleMovementComponent::Initialize() -{ +{ InitOdom(); } \ No newline at end of file diff --git a/Source/RapyutaSimulationPlugins/Public/Drives/RobotVehicleMovementComponent.h b/Source/RapyutaSimulationPlugins/Public/Drives/RobotVehicleMovementComponent.h index 023d6bfe..05550f7b 100644 --- a/Source/RapyutaSimulationPlugins/Public/Drives/RobotVehicleMovementComponent.h +++ b/Source/RapyutaSimulationPlugins/Public/Drives/RobotVehicleMovementComponent.h @@ -47,6 +47,9 @@ class RCLUE_API URobotVehicleMovementComponent : public UPawnMovementComponent UFUNCTION(BlueprintCallable) virtual void Initialize(); + UPROPERTY() + int InversionFactor = 1; + protected: virtual void BeginPlay() override; virtual void InitOdom(); From d1bc7c862528d2aae5e7d06b936075e2753bf9b5 Mon Sep 17 00:00:00 2001 From: praphulkallakuri Date: Thu, 4 Nov 2021 10:20:56 +0530 Subject: [PATCH 7/9] Updated with Identity,ZeroVector and OneVector functions --- .../Private/Drives/RobotVehicleMovementComponent.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Source/RapyutaSimulationPlugins/Private/Drives/RobotVehicleMovementComponent.cpp b/Source/RapyutaSimulationPlugins/Private/Drives/RobotVehicleMovementComponent.cpp index 2ae66b49..7bf57b5d 100644 --- a/Source/RapyutaSimulationPlugins/Private/Drives/RobotVehicleMovementComponent.cpp +++ b/Source/RapyutaSimulationPlugins/Private/Drives/RobotVehicleMovementComponent.cpp @@ -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(FQuat::Identity, FVector::ZeroVector, FVector::OneVector); 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); From 3c354abcec4d3d995f24c9d4c06dd668da9ed872 Mon Sep 17 00:00:00 2001 From: duc Date: Mon, 13 Dec 2021 15:36:56 +0900 Subject: [PATCH 8/9] [ATurtlebotBurger::InitializeMoveComponent()] Call Super::, which initializes RobotVehicleMoveComponent [ATurtlebotBurger] definition: cleaning up --- .../Robots/Turtlebot3/TurtlebotBurger.cpp | 1 + .../Robots/Turtlebot3/TurtlebotBurger.h | 26 +++++++++---------- 2 files changed, 13 insertions(+), 14 deletions(-) diff --git a/Source/RapyutaSimulationPlugins/Private/Robots/Turtlebot3/TurtlebotBurger.cpp b/Source/RapyutaSimulationPlugins/Private/Robots/Turtlebot3/TurtlebotBurger.cpp index 5fd5dbbc..fbb9deb6 100644 --- a/Source/RapyutaSimulationPlugins/Private/Robots/Turtlebot3/TurtlebotBurger.cpp +++ b/Source/RapyutaSimulationPlugins/Private/Robots/Turtlebot3/TurtlebotBurger.cpp @@ -18,6 +18,7 @@ ATurtlebotBurger::ATurtlebotBurger(const FObjectInitializer& ObjectInitializer) void ATurtlebotBurger::InitializeMoveComponent() { + Super::InitializeMoveComponent(); DifferentialDriveComponent = NewObject(this, TEXT("DifferentialDriveComponent")); } diff --git a/Source/RapyutaSimulationPlugins/Public/Robots/Turtlebot3/TurtlebotBurger.h b/Source/RapyutaSimulationPlugins/Public/Robots/Turtlebot3/TurtlebotBurger.h index 4d74fb36..c60918ca 100644 --- a/Source/RapyutaSimulationPlugins/Public/Robots/Turtlebot3/TurtlebotBurger.h +++ b/Source/RapyutaSimulationPlugins/Public/Robots/Turtlebot3/TurtlebotBurger.h @@ -30,7 +30,6 @@ class RAPYUTASIMULATIONPLUGINS_API ATurtlebotBurger : public ARobotVehicle virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override; -public: // Called every frame virtual void Tick(float DeltaTime) override; @@ -38,45 +37,44 @@ class RAPYUTASIMULATIONPLUGINS_API ATurtlebotBurger : public ARobotVehicle virtual void Init(); UPROPERTY(EditAnywhere, BlueprintReadWrite) - UStaticMeshComponent* Base; + UStaticMeshComponent* Base = nullptr; UPROPERTY(EditAnywhere, BlueprintReadWrite) - UStaticMeshComponent* LidarSensor; + UStaticMeshComponent* LidarSensor = nullptr; UPROPERTY(EditAnywhere, BlueprintReadWrite) - UStaticMeshComponent* WheelLeft; + UStaticMeshComponent* WheelLeft = nullptr; UPROPERTY(EditAnywhere, BlueprintReadWrite) - UStaticMeshComponent* WheelRight; + UStaticMeshComponent* WheelRight = nullptr; UPROPERTY(EditAnywhere, BlueprintReadWrite) - UStaticMeshComponent* CasterBack; + UStaticMeshComponent* CasterBack = nullptr; UPROPERTY(EditAnywhere, BlueprintReadWrite) - UPhysicsConstraintComponent* Base_LidarSensor; + UPhysicsConstraintComponent* Base_LidarSensor = nullptr; UPROPERTY(EditAnywhere, BlueprintReadWrite) - UPhysicsConstraintComponent* Base_WheelLeft; + UPhysicsConstraintComponent* Base_WheelLeft = nullptr; UPROPERTY(EditAnywhere, BlueprintReadWrite) - UPhysicsConstraintComponent* Base_WheelRight; + UPhysicsConstraintComponent* Base_WheelRight = nullptr; UPROPERTY(EditAnywhere, BlueprintReadWrite) - UPhysicsConstraintComponent* Base_CasterBack; + UPhysicsConstraintComponent* Base_CasterBack = nullptr; UPROPERTY(EditAnywhere, BlueprintReadWrite) - float MaxForce = 1000; + float MaxForce = 1000.f; UPROPERTY(EditAnywhere, BlueprintReadWrite) - UMaterial* VehicleMaterial; + UMaterial* VehicleMaterial = nullptr; UPROPERTY(EditAnywhere, BlueprintReadWrite) - UMaterial* BallMaterial; + UMaterial* BallMaterial = nullptr; UPROPERTY(VisibleAnywhere) bool IsInitialized = false; -protected: UFUNCTION() void SetupConstraintsAndPhysics(); From 717d72856cb397afa5d2fc1616ccb2b338e60b68 Mon Sep 17 00:00:00 2001 From: duc Date: Mon, 13 Dec 2021 16:01:23 +0900 Subject: [PATCH 9/9] [RobotVehicle] Cleaning up --- .../Private/Drives/RobotVehicleMovementComponent.cpp | 4 ++-- .../Private/Robots/RobotVehicle.cpp | 1 - .../Public/Drives/RobotVehicleMovementComponent.h | 12 ++++++------ .../Public/Robots/RobotVehicle.h | 5 +---- 4 files changed, 9 insertions(+), 13 deletions(-) diff --git a/Source/RapyutaSimulationPlugins/Private/Drives/RobotVehicleMovementComponent.cpp b/Source/RapyutaSimulationPlugins/Private/Drives/RobotVehicleMovementComponent.cpp index 7bf57b5d..cff3f3dd 100644 --- a/Source/RapyutaSimulationPlugins/Private/Drives/RobotVehicleMovementComponent.cpp +++ b/Source/RapyutaSimulationPlugins/Private/Drives/RobotVehicleMovementComponent.cpp @@ -46,7 +46,7 @@ void URobotVehicleMovementComponent::InitOdom() InitialTransform.SetTranslation(PawnOwner->GetActorLocation()); InitialTransform.SetRotation(FQuat(PawnOwner->GetActorRotation())); - PreviousTransform = FTransform(FQuat::Identity, FVector::ZeroVector, FVector::OneVector); + PreviousTransform = FTransform::Identity; OdomData.pose_pose_position_x = 0; OdomData.pose_pose_position_y = 0; @@ -161,4 +161,4 @@ FTransform URobotVehicleMovementComponent::GetOdomTF() void URobotVehicleMovementComponent::Initialize() { InitOdom(); -} \ No newline at end of file +} diff --git a/Source/RapyutaSimulationPlugins/Private/Robots/RobotVehicle.cpp b/Source/RapyutaSimulationPlugins/Private/Robots/RobotVehicle.cpp index d052f7a2..9c9a2df5 100644 --- a/Source/RapyutaSimulationPlugins/Private/Robots/RobotVehicle.cpp +++ b/Source/RapyutaSimulationPlugins/Private/Robots/RobotVehicle.cpp @@ -9,7 +9,6 @@ void ARobotVehicle::InitializeMoveComponent() { RobotVehicleMoveComponent = NewObject(this, TEXT("RobotVehicleMoveComponent")); - verify(RobotVehicleMoveComponent); } void ARobotVehicle::Tick(float DeltaSeconds) diff --git a/Source/RapyutaSimulationPlugins/Public/Drives/RobotVehicleMovementComponent.h b/Source/RapyutaSimulationPlugins/Public/Drives/RobotVehicleMovementComponent.h index 05550f7b..d0f4d717 100644 --- a/Source/RapyutaSimulationPlugins/Public/Drives/RobotVehicleMovementComponent.h +++ b/Source/RapyutaSimulationPlugins/Public/Drives/RobotVehicleMovementComponent.h @@ -18,16 +18,16 @@ class RCLUE_API URobotVehicleMovementComponent : public UPawnMovementComponent private: UPROPERTY(Transient) - FVector DesiredMovement; + FVector DesiredMovement = FVector::ZeroVector; UPROPERTY(Transient) - FQuat DesiredRotation; + FQuat DesiredRotation = FQuat::Identity; public: URobotVehicleMovementComponent(); UPROPERTY(VisibleAnywhere, BlueprintReadWrite, Category = Velocity) - FVector AngularVelocity; + FVector AngularVelocity = FVector::ZeroVector; UPROPERTY(VisibleAnywhere, BlueprintReadWrite) FROSOdometry OdomData; @@ -39,7 +39,7 @@ class RCLUE_API URobotVehicleMovementComponent : public UPawnMovementComponent FString ChildFrameId = TEXT(""); UPROPERTY(EditAnywhere) - FTransform InitialTransform; + FTransform InitialTransform = FTransform::Identity; UFUNCTION(BlueprintCallable) FTransform GetOdomTF(); @@ -57,8 +57,8 @@ class RCLUE_API URobotVehicleMovementComponent : public UPawnMovementComponent virtual void UpdateOdom(float DeltaTime); bool IsOdomInitialized = false; - UPROPERTY(EditAnywhere) - FTransform PreviousTransform; + UPROPERTY() + FTransform PreviousTransform = FTransform::Identity; std::random_device Rng; std::mt19937 Gen; diff --git a/Source/RapyutaSimulationPlugins/Public/Robots/RobotVehicle.h b/Source/RapyutaSimulationPlugins/Public/Robots/RobotVehicle.h index 595925dc..c9780a7d 100644 --- a/Source/RapyutaSimulationPlugins/Public/Robots/RobotVehicle.h +++ b/Source/RapyutaSimulationPlugins/Public/Robots/RobotVehicle.h @@ -20,9 +20,6 @@ class RAPYUTASIMULATIONPLUGINS_API ARobotVehicle : public APawn virtual void InitializeMoveComponent(); -public: - virtual void Tick(float DeltaSeconds) override; - UFUNCTION(BlueprintCallable) virtual void SetLinearVel(const FVector& InLinearVelocity); @@ -31,6 +28,6 @@ class RAPYUTASIMULATIONPLUGINS_API ARobotVehicle : public APawn protected: virtual void BeginPlay() override; - virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override; + virtual void Tick(float DeltaSeconds) override; };