RRRobotROS2Interface.h

Go to the documentation of this file.

 
#pragma once
 
 
 
// UE
 
#include "CoreMinimal.h"
 
 
 
// rclUE
 
#include "ROS2NodeComponent.h"
 
#include "ROS2ServiceClient.h"
 
#include "Tools/ROS2Spawnable.h"
 
#include "Tools/RRROS2OdomPublisher.h"
 
#include "Tools/RRROS2TFPublisher.h"
 
 
 
// RapyutaSimulationPlugins
 
#include "Core/RRUObjectUtils.h"
 
#include "Robots/RRBaseROS2Interface.h"
 
#include "Sensors/RRBaseOdomComponent.h"
 
 
 
#include "RRRobotROS2Interface.generated.h"
 
 
 
class ARRBaseRobot;
 
class RAPYUTASIMULATIONPLUGINS_API URRRobotROS2Interface : public URRBaseROS2Interface
 
{
 
 
 
public:
 
 
 
    TObjectPtr<ARRBaseRobot> Robot = nullptr;
 
 
 
    virtual void GetLifetimeReplicatedProps(TArray<FLifetimeProperty>& OutLifetimeProps) const override;
 
 
 
    virtual void Initialize(AActor* Owner) override;
 
 
 
    virtual void InitInterfaces() override;
 
 
 
    virtual void DeInitialize() override;
 
 
 
    virtual void InitROS2NodeParam(AActor* Owner) override;
 
 
 
 
    //Mobile
 
 
 
 
 
 
    TObjectPtr<URRBaseOdomComponent> OdomComponent = nullptr;
 
 
 
 
    bool bPublishOdom = true;
 
 
 
 
    bool bPublishOdomTf = false;
 
 
 
 
    float OdomPublicationFrequencyHz = 30;
 
 
 
 
 
    FString CmdVelTopicName = TEXT("cmd_vel");
 
 
 
 
    //Joint
 
 
 
 
    virtual void JointCmdCallback(const UROS2GenericMsg* Msg);
 
 
 
 
 
    FString JointCmdTopicName = TEXT("ue_joint_commands");
 
 
 
    void UpdateJointState(UROS2GenericMsg* InMessage);
 
 
 
 
 
    TObjectPtr<UROS2Publisher> JointStatePublisher = nullptr;
 
 
 
 
 
    float JointStatePublicationFrequencyHz = 30.f;
 
 
 
 
 
    FString JointStateTopicName = TEXT("ue_joint_states");
 
 
 
 
    bool bWarnAboutMissingLink = true;
 
 
 
 
    TObjectPtr<URRROS2TFsPublisher> JointsTFPublisher = nullptr;
 
 
 
 
    bool bPublishJointTf = false;
 
 
 
 
    float JointTfPublicationFrequencyHz = 1.f;
 
 
 
 
 
    TObjectPtr<URRROS2OdomPublisher> OdomPublisher = nullptr;
 
 
 
    virtual bool InitPublishers() override;
 
 
 
    virtual bool InitSubscriptions() override;
 
 
 
    virtual void MovementCallback(const UROS2GenericMsg* Msg);
 
 
 
protected:
 
    template<typename TROS2Message,
 
             typename TROS2MessageData,
 
             typename TRobot,
 
             typename TRobotMemFuncType = void (TRobot::*)(const TROS2MessageData&)>
 
    FORCEINLINE void OnMessageReceived(const TROS2Message* InMsg, const TRobotMemFuncType& InMemFunc)
 
    {
 
        const auto* msg = Cast<TROS2Message>(InMsg);
 
        if (IsValid(msg))
 
        {
 
            TROS2MessageData msgData;
 
            msg->GetMsg(msgData);
 
 
 
            // (Note) In this callback, which could be invoked from a ROS working thread,
 
            // thus any direct referencing to its member in this GameThread lambda needs to be verified.
 
            AsyncTask(ENamedThreads::GameThread,
 
                      [this, InMemFunc, msgData]
 
                      {
 
                          auto* robot = Cast<TRobot>(Robot);
 
                          if (IsValid(Cast<UObject>(robot)))
 
                          {
 
                              ::Invoke(InMemFunc, robot, msgData);
 
                          }
 
                      });
 
        }
 
    }
 
 
 
    template<typename TROS2Message, typename TROS2MessageData, typename TRobot>
 
    FORCEINLINE void OnMessageReceivedFunc(const TROS2Message* InMsg, const TFunction<void(const TROS2MessageData&)>& InFunc)
 
    {
 
        if (IsValid(InMsg))
 
        {
 
            TROS2MessageData msgData;
 
            InMsg->GetMsg(msgData);
 
 
 
            // (Note) In this callback, which could be invoked from a ROS working thread,
 
            // thus any direct referencing to its member in this GameThread lambda needs to be verified.
 
            AsyncTask(ENamedThreads::GameThread,
 
                      [this, InFunc, msgData]
 
                      {
 
                          auto* robot = Cast<TRobot>(Robot);
 
                          if (IsValid(Cast<UObject>(robot)))
 
                          {
 
                              InFunc(msgData);
 
                          }
 
                      });
 
        }
 
    }
 
 
 
    template<typename TService, typename TServiceRequest>
 
    void MakeServiceRequest(const FString& InServiceName, const TServiceRequest& InRequest)
 
    {
 
        // Create and update request
 
        if (auto* client = ServiceClients.FindRef(InServiceName))
 
        {
 
            TService* service = CastChecked<TService>(client->Service);
 
            client->SendRequest(service, InRequest);
 
 
 
            UE_LOG_WITH_INFO(LogTemp, Warning, TEXT("%s [%s] Request made"), *InServiceName, *GetName());
 
        }
 
        else
 
        {
 
            UE_LOG_WITH_INFO(LogTemp, Error, TEXT("[MakeServiceRequest] [%s] srv client not found"), *InServiceName);
 
        }
 
    }
 
};
 
 
 
class RAPYUTASIMULATIONPLUGINS_API URRRobotROS2InterfaceComponent : public URRBaseROS2InterfaceComponent
 
{
 
public:
 
    URRRobotROS2InterfaceComponent()
 
    {
 
        ROS2InterfaceClass = URRRobotROS2Interface::StaticClass();
 
    };
 
};