diff --git a/Source/RapyutaSimulationPlugins/Private/Drives/DifferentialDriveComponent.cpp b/Source/RapyutaSimulationPlugins/Private/Drives/DifferentialDriveComponent.cpp index f10badade..9c9e3fe61 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,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)) { diff --git a/Source/RapyutaSimulationPlugins/Private/Drives/RobotVehicleMovementComponent.cpp b/Source/RapyutaSimulationPlugins/Private/Drives/RobotVehicleMovementComponent.cpp index cbb969970..cff3f3dd6 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::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(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, @@ -155,7 +158,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 5e9505aab..9c9a2df56 100644 --- a/Source/RapyutaSimulationPlugins/Private/Robots/RobotVehicle.cpp +++ b/Source/RapyutaSimulationPlugins/Private/Robots/RobotVehicle.cpp @@ -2,11 +2,13 @@ #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")); } 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 c99c6ce81..fbb9deb60 100644 --- a/Source/RapyutaSimulationPlugins/Private/Robots/Turtlebot3/TurtlebotBurger.cpp +++ b/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(this, TEXT("DifferentialDriveComponent")); } void ATurtlebotBurger::Init() { if (!IsInitialized) { + InitializeMoveComponent(); Base = CreateDefaultSubobject(TEXT("Base")); LidarSensor = CreateDefaultSubobject(TEXT("LidarSensor")); WheelLeft = CreateDefaultSubobject(TEXT("WheelLeft")); @@ -56,8 +62,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/Private/Tools/StatePublisher.cpp b/Source/RapyutaSimulationPlugins/Private/Tools/StatePublisher.cpp new file mode 100644 index 000000000..7a9051df9 --- /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/Drives/DifferentialDriveComponent.h b/Source/RapyutaSimulationPlugins/Public/Drives/DifferentialDriveComponent.h index 1f0d87604..f0e6b6eb5 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 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; }; diff --git a/Source/RapyutaSimulationPlugins/Public/Drives/RobotVehicleMovementComponent.h b/Source/RapyutaSimulationPlugins/Public/Drives/RobotVehicleMovementComponent.h index b48adbd58..d0f4d717d 100644 --- a/Source/RapyutaSimulationPlugins/Public/Drives/RobotVehicleMovementComponent.h +++ b/Source/RapyutaSimulationPlugins/Public/Drives/RobotVehicleMovementComponent.h @@ -1,65 +1,69 @@ // Copyright 2020-2021 Rapyuta Robotics Co., Ltd. #pragma once - -#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 = FVector::ZeroVector; - UPROPERTY(Transient) - FVector DesiredMovement; - - UPROPERTY(Transient) - FQuat DesiredRotation; + UPROPERTY(Transient) + FQuat DesiredRotation = FQuat::Identity; public: - URobotVehicleMovementComponent(); + URobotVehicleMovementComponent(); - UPROPERTY(VisibleAnywhere, BlueprintReadWrite, Category = Velocity) - FVector AngularVelocity; + UPROPERTY(VisibleAnywhere, BlueprintReadWrite, Category = Velocity) + FVector AngularVelocity = FVector::ZeroVector; - 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 = FTransform::Identity; - UFUNCTION(BlueprintCallable) - FTransform GetOdomTF(); + UFUNCTION(BlueprintCallable) + FTransform GetOdomTF(); - UFUNCTION(BlueprintCallable) - virtual void InitMovementComponent(); + 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() + int InversionFactor = 1; - UPROPERTY(EditAnywhere) - FTransform PreviousTransform; +protected: + virtual void BeginPlay() override; + virtual void InitOdom(); + virtual void UpdateMovement(float DeltaTime); + virtual void UpdateOdom(float DeltaTime); + bool IsOdomInitialized = false; + UPROPERTY() + FTransform PreviousTransform = FTransform::Identity; - 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 +79,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 67442f6bb..c9780a7dc 100644 --- a/Source/RapyutaSimulationPlugins/Public/Robots/RobotVehicle.h +++ b/Source/RapyutaSimulationPlugins/Public/Robots/RobotVehicle.h @@ -3,35 +3,31 @@ #pragma once #include "CoreMinimal.h" -#include "GameFramework/Pawn.h" #include "Drives/RobotVehicleMovementComponent.h" +#include "GameFramework/Pawn.h" #include "RobotVehicle.generated.h" +class URobotVehicleMovementComponent; UCLASS() class RAPYUTASIMULATIONPLUGINS_API ARobotVehicle : public APawn { - GENERATED_BODY() + GENERATED_BODY() public: + UPROPERTY(EditAnywhere, BlueprintReadWrite) + URobotVehicleMovementComponent* RobotVehicleMoveComponent = nullptr; - UPROPERTY(EditAnywhere, BlueprintReadWrite) - URobotVehicleMovementComponent *MoveComponent; - -public: - ARobotVehicle(const FObjectInitializer& ObjectInitializer); + virtual void InitializeMoveComponent(); - virtual void Tick(float DeltaSeconds) override; + UFUNCTION(BlueprintCallable) + virtual void SetLinearVel(const FVector& InLinearVelocity); - UFUNCTION(BlueprintCallable) - virtual void SetLinearVel(FVector velocity); - - UFUNCTION(BlueprintCallable) - virtual void SetAngularVel(FVector velocity); + UFUNCTION(BlueprintCallable) + virtual void SetAngularVel(const FVector& InAngularVelocity); protected: - - virtual void BeginPlay() override; - - virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override; + virtual void BeginPlay() override; + virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override; + virtual void Tick(float DeltaSeconds) override; }; diff --git a/Source/RapyutaSimulationPlugins/Public/Robots/Turtlebot3/TurtlebotBurger.h b/Source/RapyutaSimulationPlugins/Public/Robots/Turtlebot3/TurtlebotBurger.h index ca2dd32ac..c60918ca7 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,71 +14,70 @@ DECLARE_LOG_CATEGORY_EXTERN(LogTurtlebotBurger, Log, All); UCLASS() class RAPYUTASIMULATIONPLUGINS_API ATurtlebotBurger : public ARobotVehicle { - GENERATED_BODY() - -public: - ATurtlebotBurger(const FObjectInitializer& ObjectInitializer); + GENERATED_BODY() -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; +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; + + virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override; + + // Called every frame + virtual void Tick(float DeltaTime) override; + + UFUNCTION(BlueprintCallable) + virtual void Init(); + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + UStaticMeshComponent* Base = nullptr; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + UStaticMeshComponent* LidarSensor = nullptr; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + UStaticMeshComponent* WheelLeft = nullptr; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + UStaticMeshComponent* WheelRight = nullptr; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + UStaticMeshComponent* CasterBack = nullptr; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + UPhysicsConstraintComponent* Base_LidarSensor = nullptr; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + UPhysicsConstraintComponent* Base_WheelLeft = nullptr; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + UPhysicsConstraintComponent* Base_WheelRight = nullptr; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + UPhysicsConstraintComponent* Base_CasterBack = nullptr; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + float MaxForce = 1000.f; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + UMaterial* VehicleMaterial = nullptr; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + UMaterial* BallMaterial = nullptr; + + UPROPERTY(VisibleAnywhere) + bool IsInitialized = false; - UFUNCTION() - void SetupConstraintsAndPhysics(); + UFUNCTION() + void SetupConstraintsAndPhysics(); - UFUNCTION() - void SetupWheels(); - - + UFUNCTION() + void SetupWheels(); }; diff --git a/Source/RapyutaSimulationPlugins/Public/Tools/StatePublisher.h b/Source/RapyutaSimulationPlugins/Public/Tools/StatePublisher.h new file mode 100644 index 000000000..eede9dd1e --- /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; +};