Spline path (#335) (#337)

* add spline following mode to AIController

* update document

---------

Co-authored-by: yuokamoto <[email protected]>
Co-authored-by: torea Foissotte <[email protected]>

Author: 3 people

Content/AI/BP_RRROS2AIController.uasset

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:50ececfab355ad73ee1ea07f899c6867942e23b03e665430c8b7b3f55577c221
-size 78795
+oid sha256:ede98bed97b6b96e6150d1e72c7efa1cbea2d8017939d3a5ef91c048fbf152e9
+size 81034

Content/AI/BP_RRROS2AIControllerParam.uasset

@@ -14,10 +14,49 @@
 #include "Robots/RRBaseRobot.h"
 #include "Robots/RRRobotROS2Interface.h"
 
+void URRAIRobotROSControllerParam::SetParametersFromPawn(ARRAIRobotROSController* InController) const
+{
+    if (!InController)
+    {
+        UE_LOG_WITH_INFO_NAMED(LogRapyutaCore, Error, TEXT("FCoontroller is null"));
+        return;
+    }
+    InController->bDebug = bDebug;
+    InController->OrientationTolerance = OrientationTolerance;
+    InController->LinearMotionTolerance = LinearMotionTolerance;
+    InController->bTeleportOnFail = bTeleportOnFail;
+    InController->Mode = Mode;
+    InController->RandomMoveBoundingBox = RandomMoveBoundingBox;
+    InController->GoalSequence = GoalSequence;
+    InController->GoalSequenceActor = GoalSequenceActor;
+    InController->OriginActor = OriginActor;
+    InController->Origin = Origin;
+
+    // ROS related parameters
+    InController->NavStatusPublicationFrequencyHz = NavStatusPublicationFrequencyHz;
+    InController->SetSpeedTopicName = SetSpeedTopicName;
+    InController->SetAngularVelTopicName = SetAngularVelTopicName;
+    InController->SetModeTopicName = SetModeTopicName;
+    InController->NavStatusTopicName = NavStatusTopicName;
+    InController->PoseGoalTopicName = PoseGoalTopicName;
+    InController->ActorGoalTopicName = ActorGoalTopicName;
+
+    BPSetParametersFromPawn(InController);
+}
+
 void ARRAIRobotROSController::OnPossess(APawn* InPawn)
 {
     Super::OnPossess(InPawn);
 
+    // Get param from Pawn
+    URRAIRobotROSControllerParam* param = InPawn->FindComponentByClass<URRAIRobotROSControllerParam>();
+    if (param)
+    {
+        param->SetParametersFromPawn(this);
+    }
+
+    InitParameters();
+
     /**
      * @todo this won't work with client-server
     */
@@ -29,6 +68,26 @@ void ARRAIRobotROSController::OnUnPossess()
     Super::OnUnPossess();
 }
 
+void ARRAIRobotROSController::InitParameters()
+{
+    if (OriginActor)
+    {
+        Origin = OriginActor->GetTransform();
+    }
+
+    // Transform to world transform
+    for (int i = 0; i < GoalSequence.Num(); i++)
+    {
+        GoalSequence[i] = URRGeneralUtils::GetWorldTransform(Origin, GoalSequence[i], true);
+    }
+
+    // Transform GoalSequenceActor to GoalSequence
+    for (AActor* actor : GoalSequenceActor)
+    {
+        GoalSequence.Add(actor->GetTransform());
+    }
+}
+
 void ARRAIRobotROSController::InitROS2Interface()
 {
     InitPropertiesFromJSON();
@@ -1096,7 +1155,8 @@ FTransform ARRAIRobotROSController::GetOrigin()
 {
     if (OriginActor)
     {
-        return OriginActor->GetTransform();
+        Origin = OriginActor->GetTransform();
+        return Origin;
     }
     else
     {