RRAIRobotROSController.h

Go to the documentation of this file.

 
#pragma once
 
 
 
// UE
 
#include "AIController.h"
 
#include "CoreMinimal.h"
 
#include "Robots/RRBaseRobotROSController.h"
 
 
 
#include "RRAIRobotROSController.generated.h"
 
 
 
class URRRobotROS2Interface;
 
 
 
DECLARE_DYNAMIC_DELEGATE(FMoveCompleteCallback);
 
DECLARE_DELEGATE(FMoveCompleteCallbackStatic);
 
 
 
enum class ERRAIRobotNavStatus : uint8
 
{
 
    IDLE , 
 
    AI_MOVING , 
 
    LINEAR_MOVING , 
 
    ROTATING , 
 
};
 
 
 
enum class ERRAIRobotMode : uint8
 
{
 
    BEGIN = 0 , 
 
 
 
    MANUAL = 1 , 
 
    SEQUENCE = 2 , 
 
    RANDOM_SEQUENCE = 3 , 
 
    RANDOM_AREA = 4 , 
 
 
 
    END = 100 , 
 
};
 
 
 
class RAPYUTASIMULATIONPLUGINS_API ARRAIRobotROSController : public ARRBaseRobotROSController
 
{
 
 
 
public:
 
 
 
    bool bDebug = false;
 
 
 
 
 
    float OrientationTolerance = 5.f;
 
 
 
 
    float LinearMotionTolerance = 10.f;
 
 
 
 
 
    FRotator OrientationTarget = FRotator::ZeroRotator;
 
 
 
 
 
    FVector LinearMotionTarget = FVector::ZeroVector;
 
 
 
 
 
    bool bTeleportOnFail = true;
 
 
 
 
 
 
 
    virtual bool SetSpeed(const float InSpeed);
 
 
 
 
 
 
    virtual bool SetRotationRate(const float InRotationRate);
 
 
 
 
 
    virtual bool SetAcceleration(const float InAcceleration);
 
 
 
    // ROS
 
 
 
    TObjectPtr<UROS2Publisher> NavStatusPublisher = nullptr;
 
 
 
 
    float NavStatusPublicationFrequencyHz = 1;
 
 
 
 
    FString SetSpeedTopicName = TEXT("set_speed");
 
 
 
 
    FString SetAngularVelTopicName = TEXT("set_angular_vel");
 
 
 
 
    FString SetModeTopicName = TEXT("set_mode");
 
 
 
 
    FString NavStatusTopicName = TEXT("nav_status");
 
 
 
 
    FString PoseGoalTopicName = TEXT("pose_goal");
 
 
 
 
    FString ActorGoalTopicName = TEXT("actor_goal");
 
 
 
 
    ERRAIRobotMode Mode = ERRAIRobotMode::MANUAL;
 
 
 
 
 
    FVector RandomMoveBoundingBox = FVector::OneVector;
 
 
 
 
 
    TArray<FTransform> GoalSequence;
 
 
 
 
 
    AActor* OriginActor = nullptr;
 
 
 
 
 
    FTransform Origin = FTransform::Identity;
 
 
 
 
 
    virtual FTransform GetOrigin();
 
 
 
 
 
    bool InitPropertiesFromJSON();
 
 
 
protected:
 
    virtual void OnPossess(APawn* InPawn) override;
 
 
 
    virtual void OnUnPossess() override;
 
 
 
    virtual void OnMoveCompleted(FAIRequestID RequestID, const FPathFollowingResult& Result) override;
 
 
 
    FVector AIMovePositionTarget;
 
 
 
 
    float RotationRate = 90.f;
 
 
 
 
    float LinearSpeed = 100.f;
 
 
 
 
    FVector AngularVelocity = FVector::ZeroVector;
 
 
 
 
    ERRAIRobotNavStatus NavStatus = ERRAIRobotNavStatus::IDLE;
 
 
 
    virtual void SetDelegates(const FMoveCompleteCallback& InOnSuccess,
 
                              const FMoveCompleteCallback& InOnFail,
 
                              const float InLinearMotionToleranc = -1.0,
 
                              const float InOrientationTolerance = -1.0,
 
                              const float InTimeOut = -1.0f,
 
                              const bool Verbose = false);
 
 
 
 
 
    FMoveCompleteCallback OnSuccess;
 
 
 
 
    FMoveCompleteCallbackStatic OnSuccessInternal;
 
 
 
 
 
    FMoveCompleteCallback OnFail;
 
 
 
    static ARRAIRobotROSController* CheckController(APawn* TargetPawn);
 
 
 
    virtual EPathFollowingRequestResult::Type MoveToActorWithDelegates(AActor* Goal,
 
                                                                       const FMoveCompleteCallback& InOnSuccess,
 
                                                                       const FMoveCompleteCallback& InOnFail,
 
                                                                       float AcceptanceRadius = -1,
 
                                                                       bool bStopOnOverlap = true,
 
                                                                       bool bUsePathfinding = true,
 
                                                                       bool bCanStrafe = true,
 
                                                                       TSubclassOf<UNavigationQueryFilter> FilterClass = NULL,
 
                                                                       bool bAllowPartialPath = true,
 
                                                                       const float InOrientationTolerance = -1.0,
 
                                                                       const float InTimeOut = -1.0);
 
 
 
              Category = "AI|Navigation",
 
              meta = (DefaultToSelf = "TargetPawn", AdvancedDisplay = "bStopOnOverlap,bCanStrafe,bAllowPartialPath"))
 
    static EPathFollowingRequestResult::Type MoveToActorWithDelegates(APawn* TargetPawn,
 
                                                                      AActor* Goal,
 
                                                                      const FMoveCompleteCallback& InOnSuccess,
 
                                                                      const FMoveCompleteCallback& InOnFail,
 
                                                                      float AcceptanceRadius = -1,
 
                                                                      bool bStopOnOverlap = true,
 
                                                                      bool bUsePathfinding = true,
 
                                                                      bool bCanStrafe = true,
 
                                                                      TSubclassOf<UNavigationQueryFilter> FilterClass = NULL,
 
                                                                      bool bAllowPartialPath = true,
 
                                                                      const float InOrientationTolerance = -1.0,
 
                                                                      const float InTimeOut = -1.0);
 
 
 
    virtual EPathFollowingRequestResult::Type MoveToLocationWithDelegates(const FVector& Dest,
 
                                                                          const FRotator& DestRotator,
 
                                                                          const FMoveCompleteCallback& InOnSuccess,
 
                                                                          const FMoveCompleteCallback& InOnFail,
 
                                                                          float AcceptanceRadius = -1,
 
                                                                          bool bStopOnOverlap = true,
 
                                                                          bool bUsePathfinding = true,
 
                                                                          bool bProjectDestinationToNavigation = false,
 
                                                                          bool bCanStrafe = true,
 
                                                                          TSubclassOf<UNavigationQueryFilter> FilterClass = NULL,
 
                                                                          bool bAllowPartialPath = true,
 
                                                                          const float InOrientationTolerance = -1.0,
 
                                                                          const float InTimeOut = -1.0,
 
                                                                          const FVector& InOriginPosition = FVector::ZeroVector,
 
                                                                          const FRotator& InOriginRotator = FRotator::ZeroRotator);
 
 
 
 
              Category = "AI|Navigation",
 
              meta = (DefaultToSelf = "TargetPawn", AdvancedDisplay = "bStopOnOverlap,bCanStrafe,bAllowPartialPath"))
 
    static EPathFollowingRequestResult::Type MoveToLocationWithDelegates(APawn* TargetPawn,
 
                                                                         const FVector& Dest,
 
                                                                         const FRotator& DestRotator,
 
                                                                         const FMoveCompleteCallback& InOnSuccess,
 
                                                                         const FMoveCompleteCallback& InOnFail,
 
                                                                         float AcceptanceRadius = -1,
 
                                                                         bool bStopOnOverlap = true,
 
                                                                         bool bUsePathfinding = true,
 
                                                                         bool bProjectDestinationToNavigation = false,
 
                                                                         bool bCanStrafe = true,
 
                                                                         TSubclassOf<UNavigationQueryFilter> FilterClass = NULL,
 
                                                                         bool bAllowPartialPath = true,
 
                                                                         const float InOrientationTolerance = -1.0,
 
                                                                         const float InTimeOut = -1.0,
 
                                                                         const FVector& InOriginPosition = FVector::ZeroVector,
 
                                                                         const FRotator& InOriginRotator = FRotator::ZeroRotator);
 
 
 
    virtual bool LinearMoveToLocationWithDelegates(const FVector& Dest,
 
                                                   const FRotator& DestRotator,
 
                                                   const FMoveCompleteCallback& InOnSuccess,
 
                                                   const FMoveCompleteCallback& InOnFail,
 
                                                   float AcceptanceRadius = -1,
 
                                                   const float InOrientationTolerance = -1.0,
 
                                                   const float InTimeOut = -1.0,
 
                                                   const FVector& InOriginPosition = FVector::ZeroVector,
 
                                                   const FRotator& InOriginRotator = FRotator::ZeroRotator);
 
 
 
              meta = (DefaultToSelf = "TargetPawn", AdvancedDisplay = "bStopOnOverlap,bCanStrafe,bAllowPartialPath"))
 
    static bool LinearMoveToLocationWithDelegates(APawn* TargetPawn,
 
                                                  const FVector& Dest,
 
                                                  const FRotator& DestRotator,
 
                                                  const FMoveCompleteCallback& InOnSuccess,
 
                                                  const FMoveCompleteCallback& InOnFail,
 
                                                  float AcceptanceRadius = -1,
 
                                                  const float InOrientationTolerance = -1.0,
 
                                                  const float InTimeOut = -1.0,
 
                                                  const FVector& InOriginPosition = FVector::ZeroVector,
 
                                                  const FRotator& InOriginRotator = FRotator::ZeroRotator);
 
 
 
    virtual bool HasReachedOrientationTarget(const float InOrientationTolerance = -1.0);
 
 
 
    virtual bool HasReachedOrientationTarget(const FRotator InOrientationTarget, const float InOrientationTolerance = -1.0);
 
 
 
    virtual bool OrientationEquals(const FRotator InOrientationTarget1,
 
                                   const FRotator InOrientationTarget2,
 
                                   const float InOrientationTolerance = -1.0);
 
 
 
    virtual bool HasReachedLinearMotionTarget(const float InLinearMotionTolerance = -1.0);
 
 
 
    virtual bool HasReachedLinearMotionTarget(const FVector InLinearMotionTarget, const float InLinearMotionTolerance = -1.0);
 
 
 
    virtual bool PositionEquals(const FVector InLinearMotionTarget1,
 
                                const FVector InLinearMotionTarget2,
 
                                const float InLinearMotionTolerance = -1.0);
 
 
 
    virtual bool SetOrientationTarget(const FRotator& InOrientation,
 
                                      const bool InReset = true,
 
                                      const FRotator& InOriginRotator = FRotator::ZeroRotator);
 
 
 
    virtual bool AddLocalOrientationOffset(const FRotator& InOrientation, const bool InReset = true);
 
 
 
              meta = (DefaultToSelf = "TargetPawn", AdvancedDisplay = "bStopOnOverlap,bCanStrafe,bAllowPartialPath"))
 
    static bool SetOrientationTarget(APawn* TargetPawn,
 
                                     const FRotator& InOrientation,
 
                                     const bool InReset = true,
 
                                     const FRotator& InOriginRotator = FRotator::ZeroRotator);
 
 
 
              meta = (DefaultToSelf = "TargetPawn", AdvancedDisplay = "bStopOnOverlap,bCanStrafe,bAllowPartialPath"))
 
    static bool AddLocalOrientationOffset(APawn* TargetPawn, const FRotator& InOrientation, const bool InReset = true);
 
 
 
    virtual bool SetOrientationTargetWthDelegates(const FRotator& InOrientation,
 
                                                  const FMoveCompleteCallback& InOnSuccess,
 
                                                  const FMoveCompleteCallback& InOnFail,
 
                                                  const float InOrientationTolerance = -1.0,
 
                                                  const float InTimeOut = -1.0,
 
                                                  const FRotator& InOriginRotator = FRotator::ZeroRotator);
 
 
 
    virtual bool AddLocalOrientationOffsetWthDelegates(const FRotator& InOrientation,
 
                                                       const FMoveCompleteCallback& InOnSuccess,
 
                                                       const FMoveCompleteCallback& InOnFail,
 
                                                       const float InOrientationTolerance = -1.0,
 
                                                       const float InTimeOut = -1.0);
 
 
 
              meta = (DefaultToSelf = "TargetPawn", AdvancedDisplay = "bStopOnOverlap,bCanStrafe,bAllowPartialPath"))
 
    static bool SetOrientationTargetWthDelegates(APawn* TargetPawn,
 
                                                 const FRotator& InOrientation,
 
                                                 const FMoveCompleteCallback& InOnSuccess,
 
                                                 const FMoveCompleteCallback& InOnFail,
 
                                                 const float InOrientationTolerance = -1.0,
 
                                                 const float InTimeOut = -1.0,
 
                                                 const FRotator& InOriginRotator = FRotator::ZeroRotator);
 
 
 
              meta = (DefaultToSelf = "TargetPawn", AdvancedDisplay = "bStopOnOverlap,bCanStrafe,bAllowPartialPath"))
 
    static bool AddLocalOrientationOffsetWthDelegates(APawn* TargetPawn,
 
                                                      const FRotator& InOrientation,
 
                                                      const FMoveCompleteCallback& InOnSuccess,
 
                                                      const FMoveCompleteCallback& InOnFail,
 
                                                      const float InOrientationTolerance = -1.0,
 
                                                      const float InTimeOut = -1.0);
 
 
 
    virtual bool SetLinearMotionTarget(const FVector& InPosition,
 
                                       const bool InReset = true,
 
                                       const FVector& InOriginPosition = FVector::ZeroVector,
 
                                       const FRotator& InOriginRotator = FRotator::ZeroRotator);
 
 
 
    virtual bool AddLocalLinearMotionOffset(const FVector& InPosition, const bool InReset = true);
 
 
 
              meta = (DefaultToSelf = "TargetPawn", AdvancedDisplay = "bStopOnOverlap,bCanStrafe,bAllowPartialPath"))
 
    static bool SetLinearMotionTarget(APawn* TargetPawn,
 
                                      const FVector& InPosition,
 
                                      const bool InReset = true,
 
                                      const FVector& InOriginPosition = FVector::ZeroVector,
 
                                      const FRotator& InOriginRotator = FRotator::ZeroRotator);
 
 
 
              meta = (DefaultToSelf = "TargetPawn", AdvancedDisplay = "bStopOnOverlap,bCanStrafe,bAllowPartialPath"))
 
    static bool AddLocalLinearMotionOffset(APawn* TargetPawn, const FVector& InPosition, const bool InReset = true);
 
 
 
    virtual bool SetLinearMotionTargetWthDelegates(const FVector& InPosition,
 
                                                   const FMoveCompleteCallback& InOnSuccess,
 
                                                   const FMoveCompleteCallback& InOnFail,
 
                                                   const float InLinearMotionTolerancee = -1.0,
 
                                                   const float InTimeOut = -1.0,
 
                                                   const FVector& InOriginPosition = FVector::ZeroVector,
 
                                                   const FRotator& InOriginRotator = FRotator::ZeroRotator);
 
 
 
    virtual bool AddLocalLinearMotionOffsetWthDelegates(const FVector& InPosition,
 
                                                        const FMoveCompleteCallback& InOnSuccess,
 
                                                        const FMoveCompleteCallback& InOnFail,
 
                                                        const float InLinearMotionTolerancee = -1.0,
 
                                                        const float InTimeOut = -1.0);
 
 
 
              meta = (DefaultToSelf = "TargetPawn", AdvancedDisplay = "bStopOnOverlap,bCanStrafe,bAllowPartialPath"))
 
    static bool SetLinearMotionTargetWthDelegates(APawn* TargetPawn,
 
                                                  const FVector& InPosition,
 
                                                  const FMoveCompleteCallback& InOnSuccess,
 
                                                  const FMoveCompleteCallback& InOnFail,
 
                                                  const float InLinearMotionTolerancee = -1.0,
 
                                                  const float InTimeOut = -1.0,
 
                                                  const FVector& InOriginPosition = FVector::ZeroVector,
 
                                                  const FRotator& InOriginRotator = FRotator::ZeroRotator);
 
 
 
              meta = (DefaultToSelf = "TargetPawn", AdvancedDisplay = "bStopOnOverlap,bCanStrafe,bAllowPartialPath"))
 
    static bool AddLocalLinearMotionOffsetWthDelegates(APawn* TargetPawn,
 
                                                       const FVector& InPosition,
 
                                                       const FMoveCompleteCallback& InOnSuccess,
 
                                                       const FMoveCompleteCallback& InOnFail,
 
                                                       const float InLinearMotionTolerancee = -1.0,
 
                                                       const float InTimeOut = -1.0);
 
 
 
 
    float MoveStartTime = 0.f;
 
 
 
 
 
 
 
    float MoveTimeout = -1.0f;
 
 
 
    virtual void Tick(float DeltaSeconds) override;
 
 
 
    virtual void UpdateControl(float DeltaSeconds);
 
 
 
    virtual void UpdateRotation(float DeltaSeconds);
 
 
 
    virtual void UpdateLinearMotion(float DeltaSeconds);
 
 
 
    virtual void ResetControl();
 
 
 
    virtual bool InProgress();
 
 
 
    // ROS
 
 
 
    FTimerHandle ROS2InitTimer;
 
 
 
    void InitROS2Interface();
 
 
 
    bool InitROS2InterfaceImpl();
 
 
 
    void UpdateNavStatus(UROS2GenericMsg* InMessage);
 
 
 
    void PoseGoalCallback(const UROS2GenericMsg* Msg);
 
 
 
    void ActorGoalCallback(const UROS2GenericMsg* Msg);
 
 
 
    void SetSpeedCallback(const UROS2GenericMsg* Msg);
 
 
 
    void SetAngularVelCallback(const UROS2GenericMsg* Msg);
 
 
 
    void SetModeCallback(const UROS2GenericMsg* Msg);
 
 
 
    // Mode variable
 
 
 
    int GoalIndex = 0;
 
 
 
 
 
    bool bModeUpdate = true;
 
 
 
    bool UpdateGoalSequenceIndex(FTransform& OutGoal);
 
 
 
    bool RandomUpdateGoalSequenceIndex(FTransform& OutGoal);
 
 
 
    virtual void ModeUpdate();
 
};