diff --git a/Content/Tools/ROS2GameMode.uasset b/Content/Tools/ROS2GameMode.uasset index ab1e5aad1..6b993d791 100644 Binary files a/Content/Tools/ROS2GameMode.uasset and b/Content/Tools/ROS2GameMode.uasset differ diff --git a/Source/RapyutaSimulationPlugins/Private/Tools/ROS2Spawnable.cpp b/Source/RapyutaSimulationPlugins/Private/Tools/ROS2Spawnable.cpp new file mode 100644 index 000000000..46fd60476 --- /dev/null +++ b/Source/RapyutaSimulationPlugins/Private/Tools/ROS2Spawnable.cpp @@ -0,0 +1,33 @@ +// Copyright 2020-2021 Rapyuta Robotics Co., Ltd. + +#include "Tools/ROS2Spawnable.h" + +// rclUE +#include "Srvs/ROS2SpawnEntitySrv.h" + +void UROS2Spawnable::InitializeParameters(FROSSpawnEntity_Request Request) +{ + SetName(Request.state_name); + SetNamespace(Request.robot_namespace); + +} + +void UROS2Spawnable::SetName(FString Name) +{ + ActorName = Name; +} + +void UROS2Spawnable::SetNamespace(FString Namespace) +{ + ActorNamespace = Namespace; +} + +FString UROS2Spawnable::GetName() +{ + return ActorName; +} + +FString UROS2Spawnable::GetNamespace() +{ + return ActorNamespace; +} \ No newline at end of file diff --git a/Source/RapyutaSimulationPlugins/Private/Tools/SimulationState.cpp b/Source/RapyutaSimulationPlugins/Private/Tools/SimulationState.cpp index 555f02d09..66374be4b 100644 --- a/Source/RapyutaSimulationPlugins/Private/Tools/SimulationState.cpp +++ b/Source/RapyutaSimulationPlugins/Private/Tools/SimulationState.cpp @@ -1,9 +1,10 @@ // Copyright 2020-2021 Rapyuta Robotics Co., Ltd. #include "Tools/SimulationState.h" - +#include "Tools/ROS2Spawnable.h" // UE #include "EngineUtils.h" +#include "Engine/World.h" #include "Kismet/GameplayStatics.h" // rclUE @@ -126,7 +127,6 @@ bool ASimulationState::ReferenceFrameToInertiaFrame(const FString& InReferenceFr FQuat& OutOrientation) { bool bSuccess = false; - LeftToRight(OutPositionX, OutPositionY, OutPositionZ, OutOrientation); if (InReferenceFrame.IsEmpty()) { @@ -186,7 +186,7 @@ void ASimulationState::SetEntityStateSrv(UROS2GenericSrv* Service) { Response.success = false; UE_LOG(LogTemp, Warning, - TEXT("Entity %s not exit or not under SimulationState control. Please call AddEntity to make Actors under SimulationState contorl."), + TEXT("Entity %s not exit or not under SimulationState control. Please call AddEntity to make Actors under SimulationState control."), *Request.state_name); } } @@ -222,7 +222,7 @@ void ASimulationState::AttachSrv(UROS2GenericSrv* Service) else { UE_LOG(LogTemp, Warning, - TEXT("Entity %s and/or %s not exit or not under SimulationState Actor control. Please call AddEntity to make Actors under SimulationState contorl."), + TEXT("Entity %s and/or %s not exit or not under SimulationState Actor control. Please call AddEntity to make Actors under SimulationState control."), *Request.name1, *Request.name2); } @@ -236,14 +236,14 @@ void ASimulationState::SpawnEntitySrv(UROS2GenericSrv* Service) FROSSpawnEntity_Request Request; SpawnEntityService->GetRequest(Request); - UE_LOG(LogTemp, Warning, TEXT("SpawnEntityService called")); - FROSSpawnEntity_Response Response; + Response.success = ReferenceFrameToInertiaFrame(Request.state_reference_frame, Request.state_pose_position_x, Request.state_pose_position_y, Request.state_pose_position_z, Request.state_pose_orientation); + if (Response.success) { if (SpawnableEntities.Contains(Request.xml)) @@ -254,17 +254,31 @@ void ASimulationState::SpawnEntitySrv(UROS2GenericSrv* Service) UE_LOG(LogTemp, Warning, TEXT("Spawning %s"), *Request.xml); Response.success = true; - FActorSpawnParameters SpawnParameters; - SpawnParameters.Name = FName(Request.state_name); FRotator Rotator = Request.state_pose_orientation.Rotator(); FVector Position(Request.state_pose_position_x, Request.state_pose_position_y, Request.state_pose_position_z); - AActor* NewEntity = GetWorld()->SpawnActor(SpawnableEntities[Request.xml], &Position, &Rotator, SpawnParameters); + FVector Scale(1, 1, 1); + FTransform Transform(Rotator, Position, Scale); + + AActor* NewEntity = GetWorld()->SpawnActorDeferred(SpawnableEntities[Request.xml], Transform); + UROS2Spawnable* SpawnableComponent = NewObject(NewEntity, FName("ROS2 Spawn Parameters")); + + SpawnableComponent->RegisterComponent(); + SpawnableComponent->InitializeParameters(Request); + NewEntity->AddInstanceComponent(SpawnableComponent); +#if WITH_EDITOR + NewEntity->SetActorLabel(*Request.state_name); +#endif + NewEntity->Rename(*Request.state_name); + + UGameplayStatics::FinishSpawningActor(NewEntity, Transform); AddEntity(NewEntity); + + UE_LOG(LogTemp, Warning, TEXT("New Spawned Entity Name: %s"), *NewEntity->GetName()); } else { UE_LOG(LogTemp, Warning, - TEXT("Entity %s not exit or not under SimulationState Actor control. Please call AddEntity to make Actors under SimulationState contorl."), + TEXT("Entity %s not exit or not under SimulationState Actor control. Please call AddEntity to make Actors under SimulationState control."), *Request.xml); } } @@ -272,6 +286,7 @@ void ASimulationState::SpawnEntitySrv(UROS2GenericSrv* Service) SpawnEntityService->SetResponse(Response); } + void ASimulationState::DeleteEntitySrv(UROS2GenericSrv* Service) { UROS2DeleteEntitySrv* DeleteEntityService = Cast(Service); diff --git a/Source/RapyutaSimulationPlugins/Public/Tools/ROS2Spawnable.h b/Source/RapyutaSimulationPlugins/Public/Tools/ROS2Spawnable.h new file mode 100644 index 000000000..4b82afe0a --- /dev/null +++ b/Source/RapyutaSimulationPlugins/Public/Tools/ROS2Spawnable.h @@ -0,0 +1,42 @@ +// Copyright 2020-2021 Rapyuta Robotics Co., Ltd. + +#pragma once +#include "CoreMinimal.h" +#include "GameFramework/Actor.h" + +// rclUE +#include "Srvs/ROS2SpawnEntitySrv.h" + +#include "ROS2Spawnable.generated.h" + +UCLASS(ClassGroup = (Custom), Blueprintable, meta = (BlueprintSpawnableComponent)) +class RAPYUTASIMULATIONPLUGINS_API UROS2Spawnable : public UActorComponent +{ + GENERATED_BODY() + +public: + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + FString ActorName; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + FString ActorNamespace; + +public: + + UFUNCTION(BlueprintCallable) + virtual void InitializeParameters(FROSSpawnEntity_Request Request); + + UFUNCTION(BlueprintCallable) + virtual void SetName(FString Name); + + UFUNCTION(BlueprintCallable) + virtual void SetNamespace(FString Namespace); + + UFUNCTION(BlueprintCallable) + virtual FString GetName(); + + UFUNCTION(BlueprintCallable) + virtual FString GetNamespace(); + +};