Spline path

.gitattributes

@@ -2,5 +2,4 @@ Content/Robots/** filter=lfs diff=lfs merge=lfs -text
 Content/SkeletalRobots/** filter=lfs diff=lfs merge=lfs -text
 Content/Textures/** filter=lfs diff=lfs merge=lfs -text
 Content/ExternalDevices/** filter=lfs diff=lfs merge=lfs -text
-Content/Character/SKM_Manny_CtrlRig1.uasset filter=lfs diff=lfs merge=lfs -text
-Content/Character/SKM_Manny_CtrlRig2.uasset filter=lfs diff=lfs merge=lfs -text
+docs/source/_static/videos/** filter=lfs diff=lfs merge=lfs -text

Source/RapyutaSimulationPlugins/Private/Robots/RRAIRobotROSController.cpp

@@ -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
     {

Source/RapyutaSimulationPlugins/Public/Core/RRConversionUtils.h

@@ -34,6 +34,12 @@ class URRConversionUtils : public UBlueprintFunctionLibrary
         return FVector(InLocation.X, -InLocation.Y, InLocation.Z);
     }
 
+    UFUNCTION(BlueprintCallable, Category = "Conversion")
+    static FVector2D ConvertHandedness2D(const FVector2D& InLocation)
+    {
+        return FVector2D(InLocation.X, -InLocation.Y);
+    }
+
     // UE: cm, ROS: m
     template<typename T>
     static T DistanceROSToUE(const T& InROSDistance)
@@ -77,6 +83,18 @@ class URRConversionUtils : public UBlueprintFunctionLibrary
         OutputZ = InputZ * 0.01f;
     }
 
+    UFUNCTION(BlueprintCallable, Category = "Conversion")
+    static FVector2D Vector2DUEToROS(const FVector2D& Input)
+    {
+        return 0.01f * ConvertHandedness2D(Input);
+    }
+
+    FORCEINLINE static void Vector2DUEToROS(const double& InputX, const double& InputY, double& OutputX, double& OutputY)
+    {
+        OutputX = InputX * 0.01f;
+        OutputY = -InputY * 0.01f;
+    }
+
     /**
      * @brief Convert UE Rotation to ROS Vector with optional Deg->Rad conversion
      * @param Input
@@ -168,6 +186,23 @@ class URRConversionUtils : public UBlueprintFunctionLibrary
         OutputZ = InputZ * 100.f;
     }
 
+    UFUNCTION(BlueprintCallable, Category = "Conversion")
+    static FVector2D Vector2DROSToUE(const FVector2D& Input)
+    {
+        return 100.f * ConvertHandedness2D(Input);
+    }
+
+    FORCEINLINE static void Vector2DROSToUE(const FVector2D& Input, FVector2D& Output)
+    {
+        Output.Set(Input.X * 100.f, -Input.Y * 100.f);
+    }
+
+    FORCEINLINE static void Vector2DROSToUE(const double& InputX, const double& InputY, double& OutputX, double& OutputY)
+    {
+        OutputX = InputX * 100.f;
+        OutputY = -InputY * 100.f;
+    }
+
     /**
      * @brief Convert ROS Rotation Euler (rad) to UE Vector with optional Rad->Deg conversion
      * @param Input

Source/RapyutaSimulationPlugins/Public/Robots/RRAIRobotROSController.h

@@ -46,9 +46,92 @@ enum class ERRAIRobotMode : uint8
     RANDOM_SEQUENCE = 3 UMETA(DisplayName = "Random Sequence"),
     RANDOM_AREA = 4 UMETA(DisplayName = "Random Area"),
 
+    SPLINE = 10 UMETA(DisplayName = "Spline following"),
+
     END = 100 UMETA(DisplayName = "End")
 };
 
+UCLASS(ClassGroup = (Custom), Blueprintable, meta = (BlueprintSpawnableComponent))
+class RAPYUTASIMULATIONPLUGINS_API URRAIRobotROSControllerParam : public UActorComponent
+{
+    GENERATED_BODY()
+
+public:
+    //! debug flat
+    UPROPERTY(EditAnywhere, BlueprintReadWrite)
+    bool bDebug = false;
+
+    //! [degree] tolerance for orientation control
+    UPROPERTY(EditAnywhere, BlueprintReadWrite)
+    float OrientationTolerance = 5.f;
+
+    UPROPERTY(EditAnywhere, BlueprintReadWrite)
+    float LinearMotionTolerance = 10.f;
+
+    //! Teleport to target pose if robot can't reach target pose
+    UPROPERTY(EditAnywhere, BlueprintReadWrite)
+    bool bTeleportOnFail = true;
+
+    UPROPERTY(EditAnywhere, BlueprintReadWrite)
+    ERRAIRobotMode Mode = ERRAIRobotMode::MANUAL;
+
+    //! Bounding box for random move. This is used when #Mode is ERRAIRobotMode::RANDOM_AREA
+    UPROPERTY(EditAnywhere, BlueprintReadWrite)
+    FVector RandomMoveBoundingBox = FVector::OneVector;
+
+    //! Goal sequence. This is used when #Mode is ERRAIRobotMode::SEQUENCE and ERRAIRobotMode::RANDOM_SEQUENCE
+    UPROPERTY(EditAnywhere, BlueprintReadWrite)
+    TArray<FTransform> GoalSequence;
+
+    //! Goal sequence. Transform of the Actors are set to #GoalSequence
+    UPROPERTY(EditAnywhere, BlueprintReadWrite)
+    TArray<AActor*> GoalSequenceActor;
+
+    //! Origin actor for move. This is used when #Mode is ERRAIRobotMode::RANDOM_AREA, ERRAIRobotMode::SEQUENCE and ERRAIRobotMode::RANDOM_SEQUENCE
+    UPROPERTY(EditAnywhere, BlueprintReadWrite)
+    AActor* OriginActor = nullptr;
+
+    //! Origin transform for move. This is used when #Mode is ERRAIRobotMode::RANDOM_AREA, ERRAIRobotMode::SEQUENCE and ERRAIRobotMode::RANDOM_SEQUENCE
+    UPROPERTY(EditAnywhere, BlueprintReadWrite)
+    FTransform Origin = FTransform::Identity;
+
+    // ROS related parameters
+    UPROPERTY(EditAnywhere, BlueprintReadWrite)
+    float NavStatusPublicationFrequencyHz = 1;
+
+    UPROPERTY(EditAnywhere, BlueprintReadWrite)
+    FString SetSpeedTopicName = TEXT("set_speed");
+
+    UPROPERTY(EditAnywhere, BlueprintReadWrite)
+    FString SetAngularVelTopicName = TEXT("set_angular_vel");
+
+    UPROPERTY(EditAnywhere, BlueprintReadWrite)
+    FString SetModeTopicName = TEXT("set_mode");
+
+    UPROPERTY(EditAnywhere, BlueprintReadWrite)
+    FString NavStatusTopicName = TEXT("nav_status");
+
+    UPROPERTY(EditAnywhere, BlueprintReadWrite)
+    FString PoseGoalTopicName = TEXT("pose_goal");
+
+    UPROPERTY(EditAnywhere, BlueprintReadWrite)
+    FString ActorGoalTopicName = TEXT("actor_goal");
+
+    /**
+     * @brief Set the Parameters From Pawn to the Controller
+     *
+     * @param InController
+     */
+    UFUNCTION(BlueprintCallable)
+    void SetParametersFromPawn(ARRAIRobotROSController* InController) const;
+
+    /**
+     * @brief AdditionalInitialization implemented in BP.
+     */
+    UFUNCTION(BlueprintImplementableEvent, BlueprintCallable)
+    void BPSetParametersFromPawn(ARRAIRobotROSController* InController) const;
+};
+
 /**
  * @brief  Base Robot ROS controller class with AI movement by UE navigation system.
  * This class has authority to start ROS 2 Component in pausses robot.
@@ -60,6 +143,7 @@ enum class ERRAIRobotMode : uint8
  * @sa https://answers.unrealengine.com/questions/239159/how-many-ai-controllers-should-i-have.html
  *
  * @todo not work in client-server
+ * @todo replace parameters with #FRRAIRobotROSControllerParam
  */
 UCLASS(ClassGroup = (Custom), meta = (BlueprintSpawnableComponent))
 class RAPYUTASIMULATIONPLUGINS_API ARRAIRobotROSController : public ARRBaseRobotROSController
@@ -142,6 +226,10 @@ class RAPYUTASIMULATIONPLUGINS_API ARRAIRobotROSController : public ARRBaseRobot
     UPROPERTY(EditAnywhere, BlueprintReadWrite)
     TArray<FTransform> GoalSequence;
 
+    //! Goal sequence. Transform of the Actors are set to #GoalSequence
+    UPROPERTY(EditAnywhere, BlueprintReadWrite)
+    TArray<AActor*> GoalSequenceActor;
+
     //! Origin actor for move. This is used when #Mode is ERRAIRobotMode::RANDOM_AREA, ERRAIRobotMode::SEQUENCE and ERRAIRobotMode::RANDOM_SEQUENCE
     UPROPERTY(EditAnywhere, BlueprintReadWrite)
     AActor* OriginActor = nullptr;
@@ -814,6 +902,9 @@ class RAPYUTASIMULATIONPLUGINS_API ARRAIRobotROSController : public ARRBaseRobot
     UPROPERTY(VisibleAnywhere, BlueprintReadOnly)
     FTimerHandle ROS2InitTimer;
 
+    UFUNCTION()
+    void InitParameters();
+
     /**
      * @brief Initialize Parameter and start timer to call #InitROS2InterfaceImpl
      *

docs/requirements.txt

@@ -1,4 +1,5 @@
 breathe
 myst-parser
 sphinxcontrib-video
-sphinx-rtd-theme
+sphinxcontrib-youtube
+sphinx-rtd-theme