diff --git a/Content/AI/BP_RRROS2AIController.uasset b/Content/AI/BP_RRROS2AIController.uasset index 1b36f16d..6bb7c063 100644 Binary files a/Content/AI/BP_RRROS2AIController.uasset and b/Content/AI/BP_RRROS2AIController.uasset differ diff --git a/Content/AI/BP_RRROS2AIControllerParam.uasset b/Content/AI/BP_RRROS2AIControllerParam.uasset new file mode 100644 index 00000000..137a7a7f Binary files /dev/null and b/Content/AI/BP_RRROS2AIControllerParam.uasset differ diff --git a/Content/AI/BP_RRROS2AIControllerSplineMode.uasset b/Content/AI/BP_RRROS2AIControllerSplineMode.uasset new file mode 100644 index 00000000..bafb919f Binary files /dev/null and b/Content/AI/BP_RRROS2AIControllerSplineMode.uasset differ diff --git a/Content/AI/BP_RRROS2AIControllerSplineParam.uasset b/Content/AI/BP_RRROS2AIControllerSplineParam.uasset new file mode 100644 index 00000000..49ebd73a Binary files /dev/null and b/Content/AI/BP_RRROS2AIControllerSplineParam.uasset differ diff --git a/Content/AI/BTT_Common/BTD_RRROS2AIIsTargetMode.uasset b/Content/AI/BTT_Common/BTD_RRROS2AIIsTargetMode.uasset new file mode 100644 index 00000000..38d96593 Binary files /dev/null and b/Content/AI/BTT_Common/BTD_RRROS2AIIsTargetMode.uasset differ diff --git a/Content/AI/BT_ROS2Agent.uasset b/Content/AI/BT_ROS2Agent.uasset index 3b29cc96..f28a078d 100644 Binary files a/Content/AI/BT_ROS2Agent.uasset and b/Content/AI/BT_ROS2Agent.uasset differ diff --git a/Content/Character/BP_ROSSimpleCharacter.uasset b/Content/Character/BP_ROSSimpleCharacter.uasset index 940482a0..f83a4fbd 100644 Binary files a/Content/Character/BP_ROSSimpleCharacter.uasset and b/Content/Character/BP_ROSSimpleCharacter.uasset differ diff --git a/Content/Robots/BP_RRAIBaseRobot.uasset b/Content/Robots/BP_RRAIBaseRobot.uasset index e778111b..08900b4c 100644 --- a/Content/Robots/BP_RRAIBaseRobot.uasset +++ b/Content/Robots/BP_RRAIBaseRobot.uasset @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:50ececfab355ad73ee1ea07f899c6867942e23b03e665430c8b7b3f55577c221 -size 78795 +oid sha256:ede98bed97b6b96e6150d1e72c7efa1cbea2d8017939d3a5ef91c048fbf152e9 +size 81034 diff --git a/Content/Tools/BP_SplinePath.uasset b/Content/Tools/BP_SplinePath.uasset new file mode 100644 index 00000000..3c618ae8 Binary files /dev/null and b/Content/Tools/BP_SplinePath.uasset differ diff --git a/Source/RapyutaSimulationPlugins/Private/Robots/RRAIRobotROSController.cpp b/Source/RapyutaSimulationPlugins/Private/Robots/RRAIRobotROSController.cpp index 943093af..de0a1c35 100644 --- a/Source/RapyutaSimulationPlugins/Private/Robots/RRAIRobotROSController.cpp +++ b/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(); + 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 { diff --git a/Source/RapyutaSimulationPlugins/Public/Robots/RRAIRobotROSController.h b/Source/RapyutaSimulationPlugins/Public/Robots/RRAIRobotROSController.h index 91c5d44f..635c29cb 100644 --- a/Source/RapyutaSimulationPlugins/Public/Robots/RRAIRobotROSController.h +++ b/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 GoalSequence; + + //! Goal sequence. Transform of the Actors are set to #GoalSequence + UPROPERTY(EditAnywhere, BlueprintReadWrite) + TArray 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 GoalSequence; + //! Goal sequence. Transform of the Actors are set to #GoalSequence + UPROPERTY(EditAnywhere, BlueprintReadWrite) + TArray 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 * diff --git a/docs/requirements.txt b/docs/requirements.txt index eed9deeb..d293ca69 100644 --- a/docs/requirements.txt +++ b/docs/requirements.txt @@ -1,4 +1,5 @@ breathe myst-parser sphinxcontrib-video -sphinx-rtd-theme \ No newline at end of file +sphinxcontrib-youtube +sphinx-rtd-theme diff --git a/docs/source/_static/videos/all_character_modes.mp4 b/docs/source/_static/videos/all_character_modes.mp4 new file mode 100644 index 00000000..c2e47250 --- /dev/null +++ b/docs/source/_static/videos/all_character_modes.mp4 @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c59be56c4841f3a0e278037471bd2008e87291e6b7eecc08b12cb6988b714399 +size 6760634 diff --git a/docs/source/conf.py b/docs/source/conf.py index 087ac854..acc59d71 100644 --- a/docs/source/conf.py +++ b/docs/source/conf.py @@ -1,23 +1,26 @@ # 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"] ) # } @@ -25,52 +28,55 @@ # -- 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") diff --git a/docs/source/external_devices/character.rst b/docs/source/external_devices/character.rst index 1efcaf68..9787cf96 100644 --- a/docs/source/external_devices/character.rst +++ b/docs/source/external_devices/character.rst @@ -5,7 +5,8 @@ AI Overview ----------- AI Character/Robot is an actor controlled via `BP_ROSAIController`. It has preset -movements such as pick, drop, and navigation. The primary purpose of these actors +movements such as pick, drop, and navigation and those movement can be triggered from both BP and ROS 2. +The primary purpose of these actors is to simulate humans, non-robot components such as manually controlled forklifts, or off-the-shelf robots that you won't develop but exists in the environment. These actors help simulate robot interactions with dynamic objects in environments like warehouses. @@ -44,22 +45,13 @@ for how to control AI character/robot from ROS 2 *Video: Warehouse Sim* -.. video:: ../_static/videos/fork_pick_drop_ros.mp4 - :width: 750 - :height: 450 - -*Video: Forklift Pick/Drop From ROS* - -.. video:: ../_static/videos/character_pick_drop_ros.mp4 - :width: 750 - :height: 450 - -*Video: Character Pick/Drop From ROS* - - Basic Behaviors ^^^^^^^^^^^^^^^^ +.. youtube:: Et59pX87J5c + :width: 500 + :height: 300 + AI characters can move using two methods: - **UE Navigation System Movement**: @@ -71,12 +63,47 @@ AI characters can move using two methods: Combination Behaviors ^^^^^^^^^^^^^^^^^^^^^ +.. video:: ../_static/videos/fork_pick_drop_ros.mp4 + :width: 500 + :height: 300 + +*Video: Forklift Pick/Drop From ROS* + +.. video:: ../_static/videos/character_pick_drop_ros.mp4 + :width: 500 + :height: 300 + +*Video: Character Pick/Drop From ROS* + +.. video:: ../_static/videos/all_character_modes.mp4 + :width: 500 + :height: 300 + +*Video: Character SEQUENCE, RANDOM_SEQUENCE, RANDOM_AREA. + +.. youtube:: TRMRcbd_Xa0 + :width: 500 + :height: 300 + +*Video: SPLINE_PATH + +.. youtube:: rTq_5ab_cJ0 + :width: 500 + :height: 300 + +*Video: SPLINE_PATH from ROS + - **Auto Movement**: The robot can move randomly or sequentially through predefined goal sequences, or move randomly within a defined area. + - **MANUAL**: Not automatically move until get command from BP or ROS. - **SEQUENCE**: Moves repeatedly through a given `GoalSequence`. - **RANDOM_SEQUENCE**: Selects a random destination from `GoalSequence`. - **RANDOM_AREA**: Selects random destinations within a specified bounding box. + - **SPLINE_PATH**: Moves along a spline path. SPLINE_PATH is implemented in BP_RRROSAIController and not in RRROSAIController + + + Please check `rclUE_client_example `_ as well. - **Pick/Drop**: Combines UE navigation system movement with direct movement @@ -137,10 +164,18 @@ These are the default sub-behavior trees used for `PickImpl/DropImpl` actions. AI Controller ------------- -RRAIRobotROSController -^^^^^^^^^^^^^^^^^^^^^^ +`RRAIRobotROSControllerParam `_ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +`RRAIRobotROSControllerParam `_ +is a UActorComponent which has parameters for basic navigation functionality for RRAIRobotROSController. +If controlled pawn has child class of RRAIRobotROSControllerParam, the param is passed to the controller when the pawn is possessed. + +`RRAIRobotROSController `_ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -`RRAIRobotROSController` contains basic movement functionality in C++. It supports +`RRAIRobotROSController `_ +contains basic movement functionality in C++. It supports movement using Unreal Engine's navigation system and allows for direct linear and rotational movement via `SetActorLocation` and `SetActorRotation`. Additionally, it provides a basic ROS 2 interface for external control. @@ -234,6 +269,53 @@ ROS 2 API for RRAIRobotROSController 2. LINEAR_MOVING: linear moving without AI 3. ROTATING: Rotating without AI +BP_RRAIRobotROSControllerParam +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +BP_RRAIRobotROSControllerParam is child class of RRAIRobotROSControllerParam and has additional parameter such as BP_RRROS2AIControllerSplineParam. + + +BP_RRROS2AIControllerSplineParam +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +BP_RRROS2AIControllerSplineParam is a parameter for spline movement in BP_RRROSAIController. + +.. list-table:: + :header-rows: 1 + + * - Param Name + - Type (Default) + - Note + * - + - + - + * - Debug + - bool (false) + - Enables debug logging. + * - Mode + - int32 (0) + - Defines movement mode: + 0. OneTime: Move along spline one time and stop at the end. + 1. LoopFromStart: Move along spline and navigate/teleport to start point when it reach end of spline and repeat movement. + 2. Reverse: Move along spline and reverse direction when it reach end/start of spline. + * - FindClosestSpline + - bool (false) + - If true, it will find the closest BP_SplinePath from the current location. + * - LookAheadDistance + - float (0.1) + - Distance to look ahead on the spline. Closest spline point from Current location + LookAheadDistance is used as target to move. + * - Reverse + - bool (false) + - If true, it will move in reverse direction of spline + * - TeleportToSpline + - bool (false) + - If true, controlled pawn will teleport to the closest spline point, otherwise controller use naviagation to move to the spline point. + * - KeepInitialHeight + - bool (false) + - Keep initial controlled pawn height when moving along spline. + * - DeleteSplineAfterOneTime + - bool (false) + - If true, spline will be deleted after one time movement along spline. It is mainly used to use spline following from ROS 2. BP_RRROSAIController @@ -252,6 +334,8 @@ BP_PayloadBase can also be used. The general action is an interface to execute actions specific to the actor. The argument to the general action is a JSON string, which is parsed and the action is implemented in child classes. +BP_RRROSAIController also support spline following movement as well. + BP Parameters for BP_RRROSAIController ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^