Amr spawn with params (#21)

* added actors spawning with ROS2 param * added actors spawning with ROS2 param * removed an uneeded file for ROS2 params, ROS2SpawnableActor * fixed spawned robots pick drop crash * added copyright to ROS2Spawnable, removed emptiness check since doesnt seem useful at the moment * changed box back to using BP_AMRPayload * changed SetActorLabel to editor only and added use of Rename Co-authored-by: larryng-rapyuta <[email protected]>
rapyuta-robotics · Jan 5, 2022 · acdb584 · acdb584
1 parent b2a5be4
commit acdb584
Show file tree

Hide file tree

Showing 4 changed files with 100 additions and 10 deletions.
diff --git a/Content/Tools/ROS2GameMode.uasset b/Content/Tools/ROS2GameMode.uasset
diff --git a/Source/RapyutaSimulationPlugins/Private/Tools/ROS2Spawnable.cpp 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;
+}
diff --git 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,24 +254,39 @@ 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<AActor>(SpawnableEntities[Request.xml], Transform);
+            UROS2Spawnable* SpawnableComponent = NewObject<UROS2Spawnable>(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);
         }
     }
 
     SpawnEntityService->SetResponse(Response);
 }
 
+
 void ASimulationState::DeleteEntitySrv(UROS2GenericSrv* Service)
 {
     UROS2DeleteEntitySrv* DeleteEntityService = Cast<UROS2DeleteEntitySrv>(Service);

diff --git a/Source/RapyutaSimulationPlugins/Public/Tools/ROS2Spawnable.h 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();
+
+};