[cherry-pick to UE5.2 from devel] Spline path

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/Robots/RRAIRobotROSController.h

@@ -47,9 +47,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.
@@ -61,6 +144,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
@@ -143,6 +227,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;
@@ -815,6 +903,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

docs/source/conf.py

@@ -1,76 +1,82 @@
 # Configuration file for the Sphinx documentation builder.
 
-import sys
 import os
 import subprocess
 
-print('****************************************')
-print('conf.py')
-print('****************************************')
+# import sys
 
-read_the_docs_build = os.environ.get('READTHEDOCS', None) == 'True'
-#if read_the_docs_build:
+print("****************************************")
+print("conf.py")
+print("****************************************")
+
+read_the_docs_build = os.environ.get("READTHEDOCS", None) == "True"
+# if read_the_docs_build:
+#    pass
+# else :
 #    pass
-#else :
-#    pass 
-    #sys.path.append( "/usr/local/lib/python3.9/site-packages/breathe/" )
+# sys.path.append( "/usr/local/lib/python3.9/site-packages/breathe/" )
 
 # -- Doxygen and breath
-subprocess.call('mkdir -p ../../_readthedocs/html/; cd ..; doxygen', shell=True)
-breathe_projects = { "RapyutaSimulationPlugins": "../../_readthedocs/html/doxygen_generated/xml" }
+subprocess.call("mkdir -p ../../_readthedocs/html/; cd ..; doxygen", shell=True)
+breathe_projects = {
+    "RapyutaSimulationPlugins": "../../_readthedocs/html/doxygen_generated/xml"
+}
 # breathe_projects_source = {
 #      "auto" : ( "../Private/RapyutaSimulationPlugins", ["*.h"] )
 #      }
 breathe_default_project = "RapyutaSimulationPlugins"
 
 # -- Project information
 
-project = 'RapyutaSimulationPlugins'
-copyright = '2022, Rapyuta Robotics'
-author = 'Rapyuta Robotics'
+project = "RapyutaSimulationPlugins"
+copyright = "2022, Rapyuta Robotics"
+author = "Rapyuta Robotics"
 
-release = '0.1'
-version = '0.1.0'
+release = "0.1"
+version = "0.1.0"
 
 # -- General configuration
 
 extensions = [
-    'sphinx.ext.duration',
-    'sphinx.ext.doctest',
-    'sphinx.ext.autodoc',
-    'sphinx.ext.autosummary',
-    'sphinx.ext.intersphinx',
-    'sphinx.ext.imgmath', 
-    'sphinx.ext.todo', 
-    'breathe', 
-    'sphinx.ext.graphviz',
-    'myst_parser',
-    'sphinxcontrib.video'
+    "sphinx.ext.duration",
+    "sphinx.ext.doctest",
+    "sphinx.ext.autodoc",
+    "sphinx.ext.autosummary",
+    "sphinx.ext.intersphinx",
+    "sphinx.ext.imgmath",
+    "sphinx.ext.todo",
+    "breathe",
+    "sphinx.ext.graphviz",
+    "myst_parser",
+    "sphinxcontrib.video",
+    "sphinxcontrib.youtube",
 ]
 
 intersphinx_mapping = {
-    'python': ('https://docs.python.org/3/', None),
-    'sphinx': ('https://www.sphinx-doc.org/en/master/', None),
+    "python": ("https://docs.python.org/3/", None),
+    "sphinx": ("https://www.sphinx-doc.org/en/master/", None),
 }
-intersphinx_disabled_domains = ['std']
+intersphinx_disabled_domains = ["std"]
 
-templates_path = ['_templates']
+templates_path = ["_templates"]
 
-exclude_patterns = ['api']
+exclude_patterns = ["api"]
 # -- Options for HTML output
 
-html_theme = 'sphinx_rtd_theme'
+html_theme = "sphinx_rtd_theme"
 # html_style = 'css/style.css'
-html_static_path = ['_static'] 
+html_static_path = ["_static"]
 
 # -- Options for EPUB output
-epub_show_urls = 'footnote'
+epub_show_urls = "footnote"
 
 # html_extra_path = ['../html']
 
 source_suffix = {
-    '.rst': 'restructuredtext',
-    '.md': 'markdown',
+    ".rst": "restructuredtext",
+    ".md": "markdown",
 }
+
+
 def setup(app):
-    app.add_css_file('css/style.css')
+    app.add_css_file("css/style.css")