diff --git a/.gitattributes b/.gitattributes index 70f1c53b..bc69bc28 100644 --- a/.gitattributes +++ b/.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 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/ExternalDevices/BP_ConveyorCollisionMeshAddon.uasset b/Content/ExternalDevices/BP_ConveyorCollisionMeshAddon.uasset index 74217a5f..dc628aa7 100644 --- a/Content/ExternalDevices/BP_ConveyorCollisionMeshAddon.uasset +++ b/Content/ExternalDevices/BP_ConveyorCollisionMeshAddon.uasset @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:5a82315f6b954509610f128e26d6327db02c177be34f9b46ea3f7a05161c6158 -size 359496 +oid sha256:f1432666856f4eae099b8b9659f35cff00e883edb1b5183fe49dfcca51703628 +size 357065 diff --git a/Content/ExternalDevices/BP_ExternalDeviceBase.uasset b/Content/ExternalDevices/BP_ExternalDeviceBase.uasset index 046bf952..1d5d1f1c 100644 Binary files a/Content/ExternalDevices/BP_ExternalDeviceBase.uasset and b/Content/ExternalDevices/BP_ExternalDeviceBase.uasset differ diff --git a/Content/ExternalDevices/BP_MobileRack.uasset b/Content/ExternalDevices/BP_MobileRack.uasset new file mode 100644 index 00000000..9491b692 --- /dev/null +++ b/Content/ExternalDevices/BP_MobileRack.uasset @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b36f05553f874f8b8274ecc9cd0e28c3b13095da5e43b1fd327b6b1e18eebc58 +size 1215758 diff --git a/Content/ExternalDevices/BP_PayloadHandlerBase.uasset b/Content/ExternalDevices/BP_PayloadHandlerBase.uasset index 2fefed5c..e19cd396 100644 --- a/Content/ExternalDevices/BP_PayloadHandlerBase.uasset +++ b/Content/ExternalDevices/BP_PayloadHandlerBase.uasset @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:76f1c26582306017d3dfa0368088323353ea927668275d9a731c411741a3a585 -size 243552 +oid sha256:3a6b8b40415aa87abb26ec2b98618c893edca5185e026d9b3cdb155cfb1e0126 +size 242537 diff --git a/Content/ExternalDevices/BP_RackParam.uasset b/Content/ExternalDevices/BP_RackParam.uasset new file mode 100644 index 00000000..f7e57094 --- /dev/null +++ b/Content/ExternalDevices/BP_RackParam.uasset @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4d89db18ca5c913ba53a77eeddd3ef134aa66abff6cf4089ce76dfd5c13e1ef6 +size 19549 diff --git a/Content/ExternalDevices/BP_RackUnit.uasset b/Content/ExternalDevices/BP_RackUnit.uasset new file mode 100644 index 00000000..51ee7641 --- /dev/null +++ b/Content/ExternalDevices/BP_RackUnit.uasset @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8bfe22eeec8fff5ea97f20fcbfd69ab90d88adbd35f4f87feb811d0318a36b0f +size 1724943 diff --git a/Content/ExternalDevices/BP_SourceArea.uasset b/Content/ExternalDevices/BP_SourceArea.uasset index ff7fc6dc..c70233ae 100644 --- a/Content/ExternalDevices/BP_SourceArea.uasset +++ b/Content/ExternalDevices/BP_SourceArea.uasset @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:3d266bfa5a89ae6022d269697d9fc8983894238ccb251d95db0fee0cbdc4485e -size 385663 +oid sha256:61a431b841c082ee1e6465a57f948ac7577eb5aebd79adb389caee71eb7c4273 +size 381993 diff --git a/Content/ExternalDevices/BP_SplineConveyor.uasset b/Content/ExternalDevices/BP_SplineConveyor.uasset index fe49ba6c..35e50fa1 100644 --- a/Content/ExternalDevices/BP_SplineConveyor.uasset +++ b/Content/ExternalDevices/BP_SplineConveyor.uasset @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:36ee42e1d1aa2dec05c812169952ea92fc1875718f01809968bc1e08334d20d1 -size 544308 +oid sha256:4d26f3205cc262e1080e2d770e28113b3dd624d7c96862c677125f29eeee8b63 +size 544638 diff --git a/Content/ExternalDevices/BP_VerticalConveyor.uasset b/Content/ExternalDevices/BP_VerticalConveyor.uasset index 3b8d3f99..93c0c865 100644 --- a/Content/ExternalDevices/BP_VerticalConveyor.uasset +++ b/Content/ExternalDevices/BP_VerticalConveyor.uasset @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:9016df7e5217a56bc973aa9015dbd07f6b3b3ebb37afb5eeb06aeecaa9aa7372 -size 630682 +oid sha256:c689c7247b6a2b08222204437ff96d280f7927e6ee8a8285c7359ef24a0f56c2 +size 625509 diff --git a/Content/ExternalDevices/Legacy/BP_SimpleConveyor.uasset b/Content/ExternalDevices/Legacy/BP_SimpleConveyor.uasset index c5772f29..f7375c74 100644 --- a/Content/ExternalDevices/Legacy/BP_SimpleConveyor.uasset +++ b/Content/ExternalDevices/Legacy/BP_SimpleConveyor.uasset @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:a2195beb16153841ce414cda55fd1fc6945d4d4c88458498e85a25dd701e0fb2 -size 807080 +oid sha256:dc276b47b0d1a906e099fd9be63cab81e28bfff5d70d5573a29af9cdd4d8bcd9 +size 806515 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_PayloadBase.uasset b/Content/Tools/BP_PayloadBase.uasset index 41f29870..847c2a18 100644 Binary files a/Content/Tools/BP_PayloadBase.uasset and b/Content/Tools/BP_PayloadBase.uasset differ 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/Content/Tools/RRGeneralUtility.uasset b/Content/Tools/RRGeneralUtility.uasset index 7888b13c..a034d976 100644 Binary files a/Content/Tools/RRGeneralUtility.uasset and b/Content/Tools/RRGeneralUtility.uasset differ diff --git a/Content/Tools/RRGeneralUtilityMacro.uasset b/Content/Tools/RRGeneralUtilityMacro.uasset index 8e7d7bdb..9f7361dc 100644 Binary files a/Content/Tools/RRGeneralUtilityMacro.uasset and b/Content/Tools/RRGeneralUtilityMacro.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/Private/Robots/RRBaseRobot.cpp b/Source/RapyutaSimulationPlugins/Private/Robots/RRBaseRobot.cpp index a1cf5fac..b93aefae 100644 --- a/Source/RapyutaSimulationPlugins/Private/Robots/RRBaseRobot.cpp +++ b/Source/RapyutaSimulationPlugins/Private/Robots/RRBaseRobot.cpp @@ -458,13 +458,15 @@ void ARRBaseRobot::StopMovement() void ARRBaseRobot::SetLinearVel(const FVector& InLinearVel) { - SyncServerLinearMovement(GetWorld()->GetGameState()->GetServerWorldTimeSeconds(), GetTransform(), InLinearVel); + LastCmdVelUpdateTime = GetWorld()->GetGameState()->GetServerWorldTimeSeconds(); + SyncServerLinearMovement(LastCmdVelUpdateTime, GetTransform(), InLinearVel); SetLocalLinearVel(InLinearVel); } void ARRBaseRobot::SetAngularVel(const FVector& InAngularVel) { - SyncServerAngularMovement(GetWorld()->GetGameState()->GetServerWorldTimeSeconds(), GetActorRotation(), InAngularVel); + LastCmdVelUpdateTime = GetWorld()->GetGameState()->GetServerWorldTimeSeconds(); + SyncServerAngularMovement(LastCmdVelUpdateTime, GetActorRotation(), InAngularVel); SetLocalAngularVel(InAngularVel); } @@ -593,6 +595,11 @@ void ARRBaseRobot::Tick(float DeltaSeconds) { CheckJointsInitialization(); } + + if (CmdVelTimeout > 0 && CmdVelTimeout <= GetWorld()->GetGameState()->GetServerWorldTimeSeconds() - LastCmdVelUpdateTime) + { + StopMovement(); + } } bool ARRBaseRobot::InitPropertiesFromJSON() diff --git a/Source/RapyutaSimulationPlugins/Private/Sensors/RR3DLidarComponent.cpp b/Source/RapyutaSimulationPlugins/Private/Sensors/RR3DLidarComponent.cpp index 2924f812..06673b62 100644 --- a/Source/RapyutaSimulationPlugins/Private/Sensors/RR3DLidarComponent.cpp +++ b/Source/RapyutaSimulationPlugins/Private/Sensors/RR3DLidarComponent.cpp @@ -392,7 +392,7 @@ FROSPointCloud2 URR3DLidarComponent::GetROS2Data() } // Convert pose to local coordinate, ROS unit and double -> float - FVector posInDouble = RecordedHits.Last(index).ImpactPoint + BWithNoise * PositionNoise->Get(); + FVector posInDouble = RecordedHits.Last(index).ImpactPoint; // + BWithNoise * PositionNoise->Get(); posInDouble = URRGeneralUtils::GetRelativeTransform( FTransform(GetComponentQuat(), GetComponentLocation(), FVector::OneVector), FTransform(posInDouble)) .GetTranslation(); diff --git a/Source/RapyutaSimulationPlugins/Private/Sensors/RRROS2BaseSensorComponent.cpp b/Source/RapyutaSimulationPlugins/Private/Sensors/RRROS2BaseSensorComponent.cpp index e7e9ce5c..21c68efd 100644 --- a/Source/RapyutaSimulationPlugins/Private/Sensors/RRROS2BaseSensorComponent.cpp +++ b/Source/RapyutaSimulationPlugins/Private/Sensors/RRROS2BaseSensorComponent.cpp @@ -81,5 +81,19 @@ void URRROS2BaseSensorComponent::Run() void URRROS2BaseSensorComponent::Stop() { + if (SensorPublisher) + { + SensorPublisher->StopPublishTimer(); + } GetWorld()->GetTimerManager().ClearTimer(TimerHandle); } + +void URRROS2BaseSensorComponent::DestroyComponent(bool bPromoteChildren) +{ + Stop(); + if (SensorPublisher) + { + SensorPublisher->Destroy(); + } + Super::DestroyComponent(); +} diff --git a/Source/RapyutaSimulationPlugins/Private/Sensors/RRROS2CameraComponent.cpp b/Source/RapyutaSimulationPlugins/Private/Sensors/RRROS2CameraComponent.cpp index fc1d0a18..e0f4d62f 100644 --- a/Source/RapyutaSimulationPlugins/Private/Sensors/RRROS2CameraComponent.cpp +++ b/Source/RapyutaSimulationPlugins/Private/Sensors/RRROS2CameraComponent.cpp @@ -49,8 +49,10 @@ void URRROS2CameraComponent::PreInitializePublisher(UROS2NodeComponent* InROS2No void URRROS2CameraComponent::SensorUpdate() { - SceneCaptureComponent->CaptureScene(); - CaptureNonBlocking(); + if (Render) { + SceneCaptureComponent->CaptureScene(); + CaptureNonBlocking(); + } } // reference https://github.com/TimmHess/UnrealImageCapture diff --git a/Source/RapyutaSimulationPlugins/Private/Sensors/RRROS2IMUComponent.cpp b/Source/RapyutaSimulationPlugins/Private/Sensors/RRROS2IMUComponent.cpp index 055184e0..e4ba0d9b 100644 --- a/Source/RapyutaSimulationPlugins/Private/Sensors/RRROS2IMUComponent.cpp +++ b/Source/RapyutaSimulationPlugins/Private/Sensors/RRROS2IMUComponent.cpp @@ -57,22 +57,38 @@ FROSImu URRROS2IMUComponent::GetROS2Data() void URRROS2IMUComponent::SensorUpdate() { const float currentTime = UGameplayStatics::GetTimeSeconds(GetWorld()); - const FTransform currentTransform = K2_GetComponentToWorld() * InitialTransform.Inverse(); + const FTransform worldTransform = K2_GetComponentToWorld(); + const FTransform relTransform = worldTransform * InitialTransform.Inverse(); const float dt = currentTime - LastSensorUpdateTime; if (dt > 1e-10) { const float _dt = 1.0 / dt; + Data.Header.FrameId = FrameId; Data.Header.Stamp = URRConversionUtils::FloatToROSStamp(currentTime); - const FTransform dT = currentTransform * LastTransform.Inverse(); + // Transform difference in reference(initial) transform + const FTransform dT = relTransform * LastTransform.Inverse(); - FQuat orientation = currentTransform.GetRotation(); - FVector angularVelocity = dT.GetRotation().GetNormalized().Euler() * _dt; + FQuat orientation = relTransform.GetRotation(); // Rotation from reference(initial) transform + FVector angularVelocity = + orientation.UnrotateVector(dT.GetRotation().GetNormalized().Euler() * _dt); //in sensor local transform const FVector linearVel = dT.GetTranslation() * _dt; - FVector linearAcc = (linearVel - LastLinearVel) * _dt; + FVector linearAcc = orientation.UnrotateVector((linearVel - LastLinearVel) * _dt); //in sensor local transform + + // post process, add gravity and scale + if (bAddGravity) + { + FVector GravityAcc = + FVector(0.0, 0.0, -UnitConversion::ForceUnificationFactor(EUnit::KilogramsForce) * 100); // cm/ss + // Gravity vector points towards Z(+) axis + // reference: https://base.movella.com/s/article/Why-does-an-accelerometer-measure-gravity-with-positive-sign? + linearAcc -= worldTransform.GetRotation().UnrotateVector(GravityAcc); + } + linearAcc *= AccGain; + // noise LinearAcceleration = linearAcc + FVector(LinearAccelerationNoise->Get(), LinearAccelerationNoise->Get(), LinearAccelerationNoise->Get()); AngularVelocity = @@ -80,12 +96,13 @@ void URRROS2IMUComponent::SensorUpdate() OrientationNoiseSum = OrientationNoiseSum + OrientationNoiseDriftCoefficient * OrientationNoise->Get(); Orientation = FQuat::MakeFromEuler(orientation.Euler() + OrientationNoiseSum + OffsetOrientation); - // noise + // UE -> ROS conversion Data.LinearAcceleration = URRConversionUtils::VectorUEToROS(LinearAcceleration); Data.AngularVelocity = URRConversionUtils::VectorUEToROS(AngularVelocity); Data.Orientation = URRConversionUtils::QuatUEToROS(Orientation); - LastTransform = currentTransform; + // update + LastTransform = relTransform; LastdT = dT; LastLinearVel = linearVel; LastSensorUpdateTime = currentTime; diff --git a/Source/RapyutaSimulationPlugins/Public/Core/RRConversionUtils.h b/Source/RapyutaSimulationPlugins/Public/Core/RRConversionUtils.h index 6b7f3e0b..b3a6393f 100644 --- a/Source/RapyutaSimulationPlugins/Public/Core/RRConversionUtils.h +++ b/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 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 diff --git a/Source/RapyutaSimulationPlugins/Public/Robots/RRAIRobotROSController.h b/Source/RapyutaSimulationPlugins/Public/Robots/RRAIRobotROSController.h index e6c599b4..318abf8c 100644 --- a/Source/RapyutaSimulationPlugins/Public/Robots/RRAIRobotROSController.h +++ b/Source/RapyutaSimulationPlugins/Public/Robots/RRAIRobotROSController.h @@ -53,9 +53,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. @@ -67,6 +150,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 @@ -149,6 +233,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; @@ -821,6 +909,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/Source/RapyutaSimulationPlugins/Public/Robots/RRBaseRobot.h b/Source/RapyutaSimulationPlugins/Public/Robots/RRBaseRobot.h index 52301d9d..2714aeb1 100644 --- a/Source/RapyutaSimulationPlugins/Public/Robots/RRBaseRobot.h +++ b/Source/RapyutaSimulationPlugins/Public/Robots/RRBaseRobot.h @@ -573,6 +573,11 @@ class RAPYUTASIMULATIONPLUGINS_API ARRBaseRobot : public ARRBaseActor */ bool CheckUIUserWidget() const; + //! Allowed period between two successive velocity commands. After this delay, a zero speed command will be set. + //! If this is <= 0, time out won't happen. + UPROPERTY(EditAnywhere, BlueprintReadWrite) + float CmdVelTimeout = 0.5; + protected: /** * @brief Instantiate default child components @@ -611,6 +616,9 @@ class RAPYUTASIMULATIONPLUGINS_API ARRBaseRobot : public ARRBaseActor UFUNCTION(BlueprintImplementableEvent, BlueprintCallable) void BPPostInitializeComponents(); + //! last time when SetVel is called. + float LastCmdVelUpdateTime = 0; + public: /** * @brief Parse Json parameters in #ROSSpawnParameters diff --git a/Source/RapyutaSimulationPlugins/Public/Sensors/RR3DLidarComponent.h b/Source/RapyutaSimulationPlugins/Public/Sensors/RR3DLidarComponent.h index db4a4980..da3bcbee 100644 --- a/Source/RapyutaSimulationPlugins/Public/Sensors/RR3DLidarComponent.h +++ b/Source/RapyutaSimulationPlugins/Public/Sensors/RR3DLidarComponent.h @@ -53,7 +53,7 @@ class RAPYUTASIMULATIONPLUGINS_API URR3DLidarComponent : public URRBaseLidarComp * sync: Initialize #FHitResult * async: Initialize #FTraceHandle */ - void Run() override; + virtual void Run() override; /** * @brief Get Lidar data, add noise, draw lidar rays. @@ -63,7 +63,7 @@ class RAPYUTASIMULATIONPLUGINS_API URR3DLidarComponent : public URRBaseLidarComp * @sa [LineTraceSingleByChannel](https://docs.unrealengine.com/5.1/en-US/API/Runtime/Engine/Engine/UWorld/LineTraceSingleByChannel/) * @sa [AsyncLineTraceByChannel](https://docs.unrealengine.com/5.1/en-US/API/Runtime/Engine/Engine/UWorld/AsyncLineTraceSingleByChannel/) */ - void SensorUpdate() override; + virtual void SensorUpdate() override; /** * @brief Return true if laser hits the target actor. @@ -71,7 +71,7 @@ class RAPYUTASIMULATIONPLUGINS_API URR3DLidarComponent : public URRBaseLidarComp * @return true * @return false */ - bool Visible(AActor* TargetActor) override; + virtual bool Visible(AActor* TargetActor) override; /** * @brief Create ROS 2 Msg structure from #RecordedHits @@ -79,7 +79,7 @@ class RAPYUTASIMULATIONPLUGINS_API URR3DLidarComponent : public URRBaseLidarComp * @return FROSPointCloud2 */ UFUNCTION(BlueprintCallable) - FROSPointCloud2 GetROS2Data(); + virtual FROSPointCloud2 GetROS2Data(); /** * @brief Set result of #GetROS2Data to InMessage. @@ -97,7 +97,7 @@ class RAPYUTASIMULATIONPLUGINS_API URR3DLidarComponent : public URRBaseLidarComp * * @return uint64 #NSamplesPerScan * #NChannelsPerScan */ - uint64 GetTotalScan() const + virtual uint64 GetTotalScan() const { return NSamplesPerScan * NChannelsPerScan; } diff --git a/Source/RapyutaSimulationPlugins/Public/Sensors/RRBaseOdomComponent.h b/Source/RapyutaSimulationPlugins/Public/Sensors/RRBaseOdomComponent.h index 15abd52d..73096d5e 100644 --- a/Source/RapyutaSimulationPlugins/Public/Sensors/RRBaseOdomComponent.h +++ b/Source/RapyutaSimulationPlugins/Public/Sensors/RRBaseOdomComponent.h @@ -121,26 +121,26 @@ class RAPYUTASIMULATIONPLUGINS_API URRBaseOdomComponent : public URRROS2BaseSens UPROPERTY(EditAnywhere, BlueprintReadWrite) FTransform RootOffset = FTransform::Identity; - UPROPERTY(EditAnywhere, Category = "Noise") + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Noise") TObjectPtr PositionNoise; - UPROPERTY(EditAnywhere, Category = "Noise") + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Noise") TObjectPtr RotNoise; - UPROPERTY(EditAnywhere, Category = "Noise") + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Noise") float NoiseMeanPosition = 0.f; - UPROPERTY(EditAnywhere, Category = "Noise") + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Noise") float NoiseVariancePosition = 0.001f; - UPROPERTY(EditAnywhere, Category = "Noise") + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Noise") float NoiseMeanRot = 0.f; - UPROPERTY(EditAnywhere, Category = "Noise") + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Noise") float NoiseVarianceRot = 0.005f; //! Add noise or not - UPROPERTY(EditAnywhere, Category = "Noise") + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Noise") bool bWithNoise = true; protected: diff --git a/Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2BaseSensorComponent.h b/Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2BaseSensorComponent.h index 8525b2fc..616a2a6c 100644 --- a/Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2BaseSensorComponent.h +++ b/Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2BaseSensorComponent.h @@ -88,6 +88,13 @@ class RAPYUTASIMULATIONPLUGINS_API URRROS2BaseSensorComponent : public USceneCom */ URRROS2BaseSensorComponent(); + /** + * @brief Destroy after stop timers and destroy publisher + * + * @param bPromoteChildren + */ + virtual void DestroyComponent(bool bPromoteChildren = false) override; + /** * @brief Create and initialize publisher and start sensor update by calling * #CreatePublisher, #PreInitializePublisher, #InitializePublisher and #Run. diff --git a/Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2CameraComponent.h b/Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2CameraComponent.h index 566c3cdd..89420d82 100644 --- a/Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2CameraComponent.h +++ b/Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2CameraComponent.h @@ -114,6 +114,9 @@ class RAPYUTASIMULATIONPLUGINS_API URRROS2CameraComponent : public URRROS2BaseSe UPROPERTY(EditAnywhere, BlueprintReadWrite) EROS2CameraType CameraType = EROS2CameraType::RGB; + UPROPERTY(EditAnywhere, BlueprintReadWrite) + bool Render = true; + // ROS /** * @brief Update ROS 2 Msg structure from #RenderRequestQueue diff --git a/Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2IMUComponent.h b/Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2IMUComponent.h index 97b5c7c6..4b6d852c 100644 --- a/Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2IMUComponent.h +++ b/Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2IMUComponent.h @@ -9,6 +9,7 @@ // UE #include "CoreMinimal.h" #include "EngineUtils.h" +#include "Math/UnitConversion.h" // rclUE #include "RRROS2BaseSensorComponent.h" @@ -128,4 +129,10 @@ class RAPYUTASIMULATIONPLUGINS_API URRROS2IMUComponent : public URRROS2BaseSenso UPROPERTY(VisibleAnywhere, BlueprintReadOnly) float LastSensorUpdateTime; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + bool bAddGravity = true; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + float AccGain = 1.0; }; 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/_static/videos/mobile_rack_demo.mp4 b/docs/source/_static/videos/mobile_rack_demo.mp4 new file mode 100644 index 00000000..aeb984e4 Binary files /dev/null and b/docs/source/_static/videos/mobile_rack_demo.mp4 differ diff --git a/docs/source/_static/videos/mobile_rack_param.mp4 b/docs/source/_static/videos/mobile_rack_param.mp4 new file mode 100644 index 00000000..cf37908f Binary files /dev/null and b/docs/source/_static/videos/mobile_rack_param.mp4 differ diff --git a/docs/source/_static/videos/mobile_rack_ros_api.mp4 b/docs/source/_static/videos/mobile_rack_ros_api.mp4 new file mode 100644 index 00000000..3b01a569 Binary files /dev/null and b/docs/source/_static/videos/mobile_rack_ros_api.mp4 differ diff --git a/docs/source/_static/videos/unit_rack_param.mp4 b/docs/source/_static/videos/unit_rack_param.mp4 new file mode 100644 index 00000000..49b835e0 Binary files /dev/null and b/docs/source/_static/videos/unit_rack_param.mp4 differ 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 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ diff --git a/docs/source/external_devices/external_devices.rst b/docs/source/external_devices/external_devices.rst index 2eec0b91..7c949d61 100644 --- a/docs/source/external_devices/external_devices.rst +++ b/docs/source/external_devices/external_devices.rst @@ -6,14 +6,14 @@ External Devices :height: 450 External devices are non-robot device such as conveyor, elevator, etc. -External devices are implemented using BP (Blueprint) to allow easy editing -by non-engineers. Each external device is a child class of `BP_ExternalDeviceBase`, -which is a child class of `RRBaseRobot`. This setup provides ROS2Node +External devices are implemented using BP (Blueprint) to allow easy editing +by non-engineers. Each external device is a child class of `BP_ExternalDeviceBase`, +which is a child class of `RRBaseRobot`. This setup provides ROS2Node and ROS2Interface functionalities. External devices can be place from editor or spawned from ROS 2 `/SpawnEntity `_ . -Following devices only has basic setting such as primitive meshes as visual. +Following devices only has basic setting such as primitive meshes as visual. It is expected that user create child class and change visual mesh, collision size and etc. @@ -30,46 +30,46 @@ Base ROS Interfaces UE external components have ROS 2 Interface which provide - ROS2 Topic Interface: Two main interface based on modes - - **Low level interface** + - **Low level interface** e.g. *velocity input and sensor* It allow user to create custom behavior of devices - - **Preset action** + - **Preset action** e.g. *move pallet from entrance A to entrance B* It allow user to test integration with just trigger action. It is also useful to speed up simulation since all logic inside UE. -- Parameters: - - **Normal Parameters** +- Parameters: + - **Normal Parameters** e.g. *velocity* - + Parameters which can be changed at runtime. - - **Spawn Parameters** - e.g. *size, floor height* - + - **Spawn Parameters** + e.g. *size, floor height* + Parameters which can’t be changed after spawn. Pose and ROS namespace are includes as ue_msgs/SpawnEntity.srv. Base UE functions ^^^^^^^^^^^^^^^^^^ -- ParamParser and ParamParserImpl: - These handle JSON parameter parsing. - When spawned from ROS, `ParamParser` is called from `BPInitParamParser`. - When spawned from Unreal Engine (UE), such as when placed from the editor, - `ParamParser` is called from the `Initialize` function. It is expected that +- ParamParser and ParamParserImpl: + These handle JSON parameter parsing. + When spawned from ROS, `ParamParser` is called from `BPInitParamParser`. + When spawned from Unreal Engine (UE), such as when placed from the editor, + `ParamParser` is called from the `Initialize` function. It is expected that parameters will be overwritten by the editor as normal UE Actors. -- InitializeChildActor: - Some external devices contain other devices as - child actors. For example, a vertical conveyor may have an elevator and - conveyors as child actors. The initialization of these child actors, such +- InitializeChildActor: + Some external devices contain other devices as + child actors. For example, a vertical conveyor may have an elevator and + conveyors as child actors. The initialization of these child actors, such as passing parameters and setting relative positions, is performed here. -- Construction Script: - The construction script calls three functions: - 1. `InitializeChildActor` - 2. `Initialize` +- Construction Script: + The construction script calls three functions: + 1. `InitializeChildActor` + 2. `Initialize` 3. `PostInitialize` Parameters for BP_ExternalDeviceBase @@ -82,8 +82,8 @@ Parameters for BP_ExternalDeviceBase - Type (Default) - Note * - **ROS JSON SPAWN PARAMETER** - - - - + - + - * - /debug - bool (false) - Mainly used to print debug logs. @@ -97,8 +97,8 @@ Parameters for BP_ExternalDeviceBase - dict {x:1, y:1, z:1} - Scale of the external device. * - **BP PARAMETER** - - - - + - + - * - ParseParamFromJSON - bool (true) - Parse parameters from JSON. @@ -112,10 +112,10 @@ Parameters for BP_ExternalDeviceBase Conveyor -------- -Overview +Overview ^^^^^^^^ -The conveyor has collision meshes set to `OverlapAll`. It moves objects +The conveyor has collision meshes set to `OverlapAll`. It moves objects with a given velocity that overlap with the meshes. There are two main conveyor types: @@ -142,17 +142,17 @@ Conveyor Parameters - Type (Default) - Note * - **ROS JSON SPAWN PARAMETER** - - - - + - + - * - /mode - int32 (0) - - - 0. Move until payload exits the area + - + 0. Move until payload exits the area 1. Move until it hits the entrance sensor. * - /sensor1_transform - - transform + - transform .. code-block:: json - + { "position": {"x": 97.5, "y": 0, "z": 0}, "rotation": {"x": 0, "y": 0, "z": 0}, @@ -162,7 +162,7 @@ Conveyor Parameters * - /sensor2_transform - transform .. code-block:: json - + { "position": {"x": -97.5, "y": 0, "z": 0}, "rotation": {"x": 0, "y": 0, "z": 0}, @@ -181,7 +181,7 @@ Conveyor Parameters (BP_SplineConveyor) - json .. code-block:: json - + { "position": null, "rotation": null, @@ -191,8 +191,8 @@ Conveyor Parameters } - Spline points for curve creation. * - **BP PARAMETER** - - - - + - + - * - Tag - string ('Payload') - Actors with this tag are conveyed. @@ -207,19 +207,19 @@ ROS 2 API for Conveyor - Msg Type - Note * - **SUBSCRIBE** - - - - + - + - * - /set_vel - `example_interfaces/msg/Float32 `_ - Conveyor speed. Can be positive or negative. * - /set_mode - `example_interfaces/msg/Int32 `_ - - Sets mode: - 0: Move until payload exits area + - Sets mode: + 0: Move until payload exits area 1: Move until it hits the entrance sensor. * - **PUBLISH** - - - - + - + - * - /entrance - `example_interfaces/msg/Int32MultiArray `_ - Size = 2. 0: No object, 1: Object detected. @@ -227,10 +227,10 @@ ROS 2 API for Conveyor Elevator -------- -Overview +Overview ^^^^^^^^ -Elevators consist of containers that move between floors. +Elevators consist of containers that move between floors. The containers can have doors and conveyors inside to automatically move payloads. .. figure:: ../images/elevator_ue.png @@ -250,11 +250,11 @@ Elevator Parameters - Type (Default) - Note * - **ROS JSON SPAWN PARAMETER** - - - - + - + - * - /mode - int (1) - - + - 0. Manual mode: Control elevator and doorby velocity input 1. Normal mode: Control elevator and door by /move_to and /open_door 2. Auto Move Return mode: Elevator moves to another floor specified in /auto_target_floors when the payload enters and returns to the original floor when objects go outside of the elevator. @@ -300,32 +300,32 @@ ROS 2 API for Elevator - Msg Type - Note * - **SUBSCRIBE** - - - - + - + - * - /set_mode - `example_interfaces/msg/Int32 `_ - - + - 0. Manual mode: Control elevator and doorby velocity input 1. Normal mode: Control elevator and door by /move_to and /open_door 2. Auto Move Return mode: Elevator moves to another floor specified in /auto_target_floors when the payload enters and returns to the original floor when objects go outside of the elevator. * - /set_vel - `example_interfaces/msg/Float32 `_ - - + - - Mode==0: Move the container. can be +- - Mode > 0: set vel which is used to /move_to and automovement. * - /set_front_door_vel - `example_interfaces/msg/Float32 `_ - - + - - Mode==0: Move the door. can be +- - Mode > 0: set vel which is used to /move_to and automovement. * - /set_back_door_vel - `example_interfaces/msg/Float32 `_ - - + - - Mode==0: Move the door. can be +- - Mode > 0: set vel which is used to /move_to and automovement. * - /set_door_vel - `example_interfaces/msg/Float32 `_ - - + - - Mode==0: Move the door. can be +- - Mode > 0: set vel which is used to /move_to and automovement. * - /open_door @@ -338,18 +338,18 @@ ROS 2 API for Elevator - `example_interfaces/msg/Int32MultiArray `_ [2] - Set the target floors for auto mode. * - **PUBLISH** - - - - + - + - * - /current_floor - `example_interfaces/msg/Int32 `_ - - Current floor. + - Current floor. If the floor is moving, it will show the last known floor. * - /door_status - `example_interfaces/msg/Bool `_ - Door status. True = open, False = closed. * - **SERVICE** - - - - + - + - * - /get_door_status - `ue_msgs/srv/GetBoolFromId `_ - Returns whether the door is open or not. @@ -363,7 +363,7 @@ ROS 2 API for Elevator - `ue_msgs/srv/SetInt32 `_ - Opens or closes the doors. -**Todo** +**Todo** - Support multiple type of doors - Multiple door location for each floor - Test spawning from ROS 2. @@ -375,8 +375,8 @@ Vertical Conveyor Overview ^^^^^^^^ -The vertical conveyor is a combination of a conveyor and an elevator. It allows -for the vertical transport of payloads between multiple levels, and each level +The vertical conveyor is a combination of a conveyor and an elevator. It allows +for the vertical transport of payloads between multiple levels, and each level can have its own entrance and exit conveyors. .. figure:: ../images/vertical_conveyor_ue.png @@ -394,15 +394,15 @@ Vertical Conveyor Parameters - Type (Default) - Note * - **ROS JSON SPAWN PARAMETER** - - - - + - + - * - /floor_height - float (10 m) - Distance between floors. * - /entrances - int32 [4]([false, true, false, true]) - - Define if each entrance is active. - 0. no entrance, + - Define if each entrance is active. + 0. no entrance, 1. entrance active. * - /target_entrance - int8 [2]([0, 1]) @@ -414,12 +414,12 @@ Vertical Conveyor Parameters - string ('') - JSON spawn parameter to pass to child entrance conveyor actors. * - **BP PARAMETER** - - - - + - + - * - EntranceActorClass - ActorClass(BP_Conveyor) - Entrance conveyor class - + ROS 2 API for Vertical Conveyor ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ @@ -433,6 +433,170 @@ ROS 2 API for Vertical Conveyor - `example_interfaces/msg/Int32MultiArray `_ [2] - Sets the target entrance, [in, out]. -**Todo** +**Todo** - Support different parameter settings for each entrance. - Test spawning from ROS 2. + + + +Mobile Rack +----------------- + +Overview +^^^^^^^^ + +The mobile rack is a combination of rack units. +User can move unit racks to make path for robot. + +.. video:: ../_static/videos/mobile_rack_demo.mp4 + :width: 750 + :height: 450 + + +.. figure:: ../images/mobile_rack_ue.png + :align: center + + Figure : Mobile Rack + + +Rack Unit Parameters +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +.. video:: ../_static/videos/unit_rack_param.mp4 + :width: 750 + :height: 450 + +.. list-table:: + :header-rows: 1 + + * - Param Name + - Type (Default) + - Note + * - **ROS JSON SPAWN PARAMETER** + - + - + * - /layer_num + - int (4) + - Number of Rack layers. + * - /layer_height + - float (2m) + - Height of each layer. + * - /plate_num + - vector2d ({x: 1, y: 2}) + - Number of plate in each layer. + * - /plate_dim + - vector ({x: 1.5, y: 2.5, z: 0.1}) + - Plate dimension. + * - /plate_spacing + - vector2d ({x: 0.2, y: 0.0}) + - Spacing between plates. + * - /spot_num_per_plate + - int (2) + - Number of spots in each plate. Spot can be used as a target for pick/drop. + * - /spot_spacing_from_plate_edge + - float (0.65m) + - Spacing between spot and plate edge. + * - /has_approach_spot + - dict {Front: true, Rear: true} + - Approach spot presence. Spot can be used as a target for robot movement. + * - /approach_distance + - dict {Front: 1.5, Rear: 1.5} + - Approach spot distance from the rack. + * - /has_mesh + - dict {Foot: true, Side: true, Back: true} + - Mesh presence. + * - /meshe + - dict {Plate: None, Pillar: None, Foot: None, Side: None, Back: None} + - Meshes for each part. If it is None, default shape is created with primitives. + (\* not supported to set from ROS yet.) + * - /material + - string('None') + - Material for each part. If it is None, default material is used. + (\* not supported to set from ROS yet.) + * - /has_bottom_layer + - bool (true) + - Bottom layer presence. + * - /has_collision + - bool (true) + - Collision presence. This collision is used to move payload when rack is moving. This should be true for unit rack of mobile rack. + * - /allow_path_bottom + - bool (false) + - Allow path from bottom. If true, robot can pass through the bottom of the rack. + +Mobile Rack Parameters +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +.. video:: ../_static/videos/mobile_rack_param.mp4 + :width: 750 + :height: 450 + +.. list-table:: + :header-rows: 1 + + * - Param Name + - Type (Default) + - Note + * - **ROS JSON SPAWN PARAMETER** + - + - + * - /unit_num + - int (3) + - Number of Rack Units. + * - /unit_spacing + - float (0.2m) + - Spacing between rack units. + * - /path_width + - float (3.5m) + - Path width + * - /begin_edge + - bool (true) + - Begin edge presence + * - /end_edge + - bool (true) + - End edge presence + * - /initial_path_index + - int (0) + - Initial path index. + * - /speed + - float (m/s) + - moving speed of rack units. + * - /unit_rack_param + - Json(Rack Unit Parameters) + - Rack parameter + * - /begine_edge_rack_param + - Json(Rack Unit Parameters) + - Begin Edge Rack parameter + * - /end_edge_rack_param + - Json(Rack Unit Parameters) + - End Edge Rack parameter + + +ROS 2 API for Mobile Rack +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +.. video:: ../_static/videos/mobile_rack_ros_api.mp4 + :width: 750 + :height: 450 + +.. list-table:: + :header-rows: 1 + + * - Topic Name + - Msg Type + - Note + * - **SUBSCRIBE** + - + - + * - /open + - `example_interfaces/msg/Int32 `_ + - Open path at index. + * - **PUBLISH** + - + - + * - /path + - `example_interfaces/msg/Int32 `_ + - Current path index. + If racks are moving, it will be -1 + +**Todo** + - Test spawning from ROS 2. diff --git a/docs/source/images/mobile_rack_ue.png b/docs/source/images/mobile_rack_ue.png new file mode 100644 index 00000000..5da7f07d Binary files /dev/null and b/docs/source/images/mobile_rack_ue.png differ