From 05b818782b788fdf04989844ccda2b030c9d418a Mon Sep 17 00:00:00 2001 From: phuicy Date: Sat, 10 Sep 2022 19:58:05 +0100 Subject: [PATCH 1/2] feature: added std_msgs and geometry_msgs to BP interfaces. --- Source/ROSIntegration/Classes/RI/Topic.h | 48 +++++ .../std_msgs/StdMsgsEmptyConverter.cpp | 21 +++ .../Messages/std_msgs/StdMsgsEmptyConverter.h | 21 +++ .../std_msgs/StdMsgsInt64Converter.cpp | 25 +++ .../Messages/std_msgs/StdMsgsInt64Converter.h | 18 ++ Source/ROSIntegration/Private/Topic.cpp | 170 +++++++++++++++++- Source/ROSIntegration/Public/std_msgs/Empty.h | 20 +++ Source/ROSIntegration/Public/std_msgs/Int64.h | 20 +++ 8 files changed, 341 insertions(+), 2 deletions(-) create mode 100644 Source/ROSIntegration/Private/Conversion/Messages/std_msgs/StdMsgsEmptyConverter.cpp create mode 100644 Source/ROSIntegration/Private/Conversion/Messages/std_msgs/StdMsgsEmptyConverter.h create mode 100644 Source/ROSIntegration/Private/Conversion/Messages/std_msgs/StdMsgsInt64Converter.cpp create mode 100644 Source/ROSIntegration/Private/Conversion/Messages/std_msgs/StdMsgsInt64Converter.h create mode 100644 Source/ROSIntegration/Public/std_msgs/Empty.h create mode 100644 Source/ROSIntegration/Public/std_msgs/Int64.h diff --git a/Source/ROSIntegration/Classes/RI/Topic.h b/Source/ROSIntegration/Classes/RI/Topic.h index 1ba76b4..1748205 100644 --- a/Source/ROSIntegration/Classes/RI/Topic.h +++ b/Source/ROSIntegration/Classes/RI/Topic.h @@ -5,6 +5,7 @@ #include #include #include +#include "Math/Vector.h" #include "ROSBaseMsg.h" #include "ROSIntegrationCore.h" @@ -19,6 +20,21 @@ enum class EMessageType : uint8 { String = 0, Float32 = 1, + Bool = 3, + Float64 = 7, + Header = 8, + Int16 = 9, + Int32 = 11, + Int64 = 12, + UInt16 = 13, + UInt32 = 14, + UInt64 = 15, + + Vector3 = 2, + Point = 17, + Pose = 16, + Quaternion = 18, + Twist = 19, }; UCLASS(Blueprintable) @@ -56,12 +72,44 @@ class ROSINTEGRATION_API UTopic: public UObject UFUNCTION(BlueprintImplementableEvent, Category = ROS) void OnConstruct(); + // Std msgs + UFUNCTION(BlueprintImplementableEvent, Category = ROS) void OnStringMessage(const FString& Data); UFUNCTION(BlueprintImplementableEvent, Category = ROS) void OnFloat32Message(const float& Data); + UFUNCTION(BlueprintImplementableEvent, Category = ROS) + void OnBoolMessage(const int& Data); + + UFUNCTION(BlueprintImplementableEvent, Category = ROS) + void OnFloat64Message(const double& Data); + + UFUNCTION(BlueprintImplementableEvent, Category = ROS) + void OnInt32Message(const int32& Data); + + UFUNCTION(BlueprintImplementableEvent, Category = ROS) + void OnInt64Message(const int64& Data); + + + // Geometry msgs + + UFUNCTION(BlueprintImplementableEvent, Category = ROS) + void OnVector3Message(const FVector& Data); + + UFUNCTION(BlueprintImplementableEvent, Category = ROS) + void OnPointMessage(const FVector& Data); + + UFUNCTION(BlueprintImplementableEvent, Category = ROS) + void OnPoseMessage(const FVector& position, const FVector4& orientation); + + UFUNCTION(BlueprintImplementableEvent, Category = ROS) + void OnQuaternionMessage( const FVector4& quaternion); + + UFUNCTION(BlueprintImplementableEvent, Category = ROS) + void OnTwistMessage(const FVector& linear, const FVector& angular); + UPROPERTY() UROSIntegrationCore* _ROSIntegrationCore = nullptr; diff --git a/Source/ROSIntegration/Private/Conversion/Messages/std_msgs/StdMsgsEmptyConverter.cpp b/Source/ROSIntegration/Private/Conversion/Messages/std_msgs/StdMsgsEmptyConverter.cpp new file mode 100644 index 0000000..6278509 --- /dev/null +++ b/Source/ROSIntegration/Private/Conversion/Messages/std_msgs/StdMsgsEmptyConverter.cpp @@ -0,0 +1,21 @@ +// Fill out your copyright notice in the Description page of Project Settings. + + +#include "Conversion/Messages/std_msgs/StdMsgsEmptyConverter.h" + + +UStdMsgsEmptyConverter::UStdMsgsEmptyConverter() +{ + _MessageType = "std_msgs/Empty"; +} + +bool UStdMsgsEmptyConverter::ConvertIncomingMessage(const ROSBridgePublishMsg* message, TSharedPtr &BaseMsg) { + + + return true; +} + +bool UStdMsgsEmptyConverter::ConvertOutgoingMessage(TSharedPtr BaseMsg, bson_t** message) { + + return true; +} \ No newline at end of file diff --git a/Source/ROSIntegration/Private/Conversion/Messages/std_msgs/StdMsgsEmptyConverter.h b/Source/ROSIntegration/Private/Conversion/Messages/std_msgs/StdMsgsEmptyConverter.h new file mode 100644 index 0000000..11c247f --- /dev/null +++ b/Source/ROSIntegration/Private/Conversion/Messages/std_msgs/StdMsgsEmptyConverter.h @@ -0,0 +1,21 @@ +// Fill out your copyright notice in the Description page of Project Settings. + +#pragma once + +#include "CoreMinimal.h" +#include "Conversion/Messages/BaseMessageConverter.h" +#include "std_msgs/Bool.h" +#include "StdMsgsEmptyConverter.generated.h" + + +UCLASS() +class ROSINTEGRATION_API UStdMsgsEmptyConverter : public UBaseMessageConverter +{ + GENERATED_BODY() + +public: + UStdMsgsEmptyConverter(); + virtual bool ConvertIncomingMessage(const ROSBridgePublishMsg* message, TSharedPtr &BaseMsg); + virtual bool ConvertOutgoingMessage(TSharedPtr BaseMsg, bson_t** message); + +}; \ No newline at end of file diff --git a/Source/ROSIntegration/Private/Conversion/Messages/std_msgs/StdMsgsInt64Converter.cpp b/Source/ROSIntegration/Private/Conversion/Messages/std_msgs/StdMsgsInt64Converter.cpp new file mode 100644 index 0000000..ca91992 --- /dev/null +++ b/Source/ROSIntegration/Private/Conversion/Messages/std_msgs/StdMsgsInt64Converter.cpp @@ -0,0 +1,25 @@ +#include "Conversion/Messages/std_msgs/StdMsgsInt64Converter.h" + + +UStdMsgsInt64Converter::UStdMsgsInt64Converter() +{ + _MessageType = "std_msgs/Int64"; +} + +bool UStdMsgsInt64Converter::ConvertIncomingMessage(const ROSBridgePublishMsg* message, TSharedPtr &BaseMsg) { + bool KeyFound = false; + + int64 Data = GetInt64FromBSON(TEXT("msg.data"), message->full_msg_bson_, KeyFound); + if (!KeyFound) return false; + + BaseMsg = TSharedPtr(new ROSMessages::std_msgs::Int64(Data)); + return true; +} + +bool UStdMsgsInt64Converter::ConvertOutgoingMessage(TSharedPtr BaseMsg, bson_t** message) { + auto Int64Message = StaticCastSharedPtr(BaseMsg); + *message = BCON_NEW( + "data", BCON_INT32(Int64Message->_Data) + ); + return true; +} diff --git a/Source/ROSIntegration/Private/Conversion/Messages/std_msgs/StdMsgsInt64Converter.h b/Source/ROSIntegration/Private/Conversion/Messages/std_msgs/StdMsgsInt64Converter.h new file mode 100644 index 0000000..a8ec173 --- /dev/null +++ b/Source/ROSIntegration/Private/Conversion/Messages/std_msgs/StdMsgsInt64Converter.h @@ -0,0 +1,18 @@ +#pragma once + +#include "CoreMinimal.h" +#include "Conversion/Messages/BaseMessageConverter.h" +#include "std_msgs/Int64.h" +#include "StdMsgsInt64Converter.generated.h" + + +UCLASS() +class ROSINTEGRATION_API UStdMsgsInt64Converter: public UBaseMessageConverter +{ + GENERATED_BODY() + +public: + UStdMsgsInt64Converter(); + virtual bool ConvertIncomingMessage(const ROSBridgePublishMsg* message, TSharedPtr &BaseMsg); + virtual bool ConvertOutgoingMessage(TSharedPtr BaseMsg, bson_t** message); +}; diff --git a/Source/ROSIntegration/Private/Topic.cpp b/Source/ROSIntegration/Private/Topic.cpp index a118aa7..f91ccf8 100644 --- a/Source/ROSIntegration/Private/Topic.cpp +++ b/Source/ROSIntegration/Private/Topic.cpp @@ -4,6 +4,13 @@ #include "rosbridge2cpp/ros_topic.h" #include "Conversion/Messages/BaseMessageConverter.h" #include "Conversion/Messages/std_msgs/StdMsgsStringConverter.h" +#include "Conversion/Messages/std_msgs/StdMsgsBoolConverter.h" +#include "Conversion/Messages/geometry_msgs/GeometryMsgsVector3Converter.h" +#include "Conversion/Messages/geometry_msgs/GeometryMsgsPointConverter.h" +#include "Conversion/Messages/geometry_msgs/GeometryMsgsPoseConverter.h" + + + static TMap TypeConverterMap; static TMap SupportedMessageTypes; @@ -174,8 +181,19 @@ UTopic::UTopic(const FObjectInitializer& ObjectInitializer) if (SupportedMessageTypes.Num() == 0) { - SupportedMessageTypes.Add(EMessageType::String, TEXT("std_msgs/String")); - SupportedMessageTypes.Add(EMessageType::Float32, TEXT("std_msgs/Float32")); + SupportedMessageTypes.Add(EMessageType::String, TEXT("std_msgs/String")); + SupportedMessageTypes.Add(EMessageType::Float32, TEXT("std_msgs/Float32")); + SupportedMessageTypes.Add(EMessageType::Bool, TEXT("std_msgs/Bool")); + SupportedMessageTypes.Add(EMessageType::Float64, TEXT("std_msgs/Float64")); + SupportedMessageTypes.Add(EMessageType::Int32, TEXT("std_msgs/Int32")); + SupportedMessageTypes.Add(EMessageType::Int64, TEXT("std_msgs/Int64")); + + SupportedMessageTypes.Add(EMessageType::Vector3, TEXT("geometry_msgs/Vector3")); + SupportedMessageTypes.Add(EMessageType::Point, TEXT("geometry_msgs/Point")); + SupportedMessageTypes.Add(EMessageType::Pose, TEXT("geometry_msgs/Pose")); + SupportedMessageTypes.Add(EMessageType::Quaternion, TEXT("geometry_msgs/Quaternion")); + SupportedMessageTypes.Add(EMessageType::Twist, TEXT("geometry_msgs/Twist")); + } } @@ -345,6 +363,154 @@ bool UTopic::Subscribe() } break; } + case EMessageType::Bool: + { + auto ConcreteBoolMessage = StaticCastSharedPtr(msg); + if (ConcreteBoolMessage.IsValid()) + { + const bool Data = ConcreteBoolMessage->_Data; + TWeakPtr SelfPtr(_SelfPtr); + AsyncTask(ENamedThreads::GameThread, [this, Data, SelfPtr]() + { + if (!SelfPtr.IsValid()) return; + OnBoolMessage(Data); + }); + } + break; + } + case EMessageType::Float64: + { + auto ConcreteDoubleMessage = StaticCastSharedPtr(msg); + if (ConcreteDoubleMessage.IsValid()) + { + const double Data = ConcreteDoubleMessage->_Data; + TWeakPtr SelfPtr(_SelfPtr); + AsyncTask(ENamedThreads::GameThread, [this, Data, SelfPtr]() + { + if (!SelfPtr.IsValid()) return; + OnFloat64Message(Data); + }); + } + break; + } + case EMessageType::Int32: + { + auto ConcreteInt32Message = StaticCastSharedPtr(msg); + if (ConcreteInt32Message.IsValid()) + { + const int32 Data = ConcreteInt32Message->_Data; + TWeakPtr SelfPtr(_SelfPtr); + AsyncTask(ENamedThreads::GameThread, [this, Data, SelfPtr]() + { + if (!SelfPtr.IsValid()) return; + OnInt32Message(Data); + }); + } + break; + } + case EMessageType::Int64: + { + auto ConcreteInt64Message = StaticCastSharedPtr(msg); + if (ConcreteInt64Message.IsValid()) + { + const int64 Data = ConcreteInt64Message->_Data; + TWeakPtr SelfPtr(_SelfPtr); + AsyncTask(ENamedThreads::GameThread, [this, Data, SelfPtr]() + { + if (!SelfPtr.IsValid()) return; + OnInt64Message(Data); + }); + } + break; + } + case EMessageType::Vector3: + { + auto ConcreteVectorMessage = StaticCastSharedPtr(msg); + if (ConcreteVectorMessage.IsValid()) + { + const float x = ConcreteVectorMessage->x; + const float y = ConcreteVectorMessage->y; + const float z = ConcreteVectorMessage->z; + FVector Data(x,y,z); + TWeakPtr SelfPtr(_SelfPtr); + AsyncTask(ENamedThreads::GameThread, [this, Data, SelfPtr]() + { + if (!SelfPtr.IsValid()) return; + OnVector3Message(Data); + }); + } + break; + } + case EMessageType::Point: + { + auto ConcreteVectorMessage = StaticCastSharedPtr(msg); + if (ConcreteVectorMessage.IsValid()) + { + const float x = ConcreteVectorMessage->x; + const float y = ConcreteVectorMessage->y; + const float z = ConcreteVectorMessage->z; + FVector Data(x, y, z); + TWeakPtr SelfPtr(_SelfPtr); + AsyncTask(ENamedThreads::GameThread, [this, Data, SelfPtr]() + { + if (!SelfPtr.IsValid()) return; + OnPointMessage(Data); + }); + } + break; + } + case EMessageType::Pose: + { + auto ConcreteVectorMessage = StaticCastSharedPtr(msg); + if (ConcreteVectorMessage.IsValid()) + { + const auto p = ConcreteVectorMessage->position; + const auto q = ConcreteVectorMessage->orientation; + const FVector position(p.x, p.y, p.z); + const FVector4 orientation(q.x,q.y,q.z,q.w); + TWeakPtr SelfPtr(_SelfPtr); + AsyncTask(ENamedThreads::GameThread, [this, position, orientation, SelfPtr]() + { + if (!SelfPtr.IsValid()) return; + OnPoseMessage(position, orientation); + }); + } + break; + } + case EMessageType::Quaternion: + { + auto ConcreteVectorMessage = StaticCastSharedPtr(msg); + if (ConcreteVectorMessage.IsValid()) + { + const auto q = ConcreteVectorMessage; + const FVector4 orientation(q->x, q->y, q->z, q->w); + TWeakPtr SelfPtr(_SelfPtr); + AsyncTask(ENamedThreads::GameThread, [this, orientation, SelfPtr]() + { + if (!SelfPtr.IsValid()) return; + OnQuaternionMessage(orientation); + }); + } + break; + } + case EMessageType::Twist: + { + auto ConcreteVectorMessage = StaticCastSharedPtr(msg); + if (ConcreteVectorMessage.IsValid()) + { + const auto p = ConcreteVectorMessage->linear; + const auto q = ConcreteVectorMessage->angular; + const FVector linear(p.x, p.y, p.z); + const FVector angular(q.x, q.y, q.z); + TWeakPtr SelfPtr(_SelfPtr); + AsyncTask(ENamedThreads::GameThread, [this, linear, angular, SelfPtr]() + { + if (!SelfPtr.IsValid()) return; + OnTwistMessage(linear, angular); + }); + } + break; + } default: unimplemented(); break; diff --git a/Source/ROSIntegration/Public/std_msgs/Empty.h b/Source/ROSIntegration/Public/std_msgs/Empty.h new file mode 100644 index 0000000..6a8b102 --- /dev/null +++ b/Source/ROSIntegration/Public/std_msgs/Empty.h @@ -0,0 +1,20 @@ +#pragma once + +#include "ROSBaseMsg.h" + +namespace ROSMessages{ + namespace std_msgs { + class String : public FROSBaseMsg { + public: + String() { + _MessageType = "std_msgs/Empty"; + } + + String(FString data) { + _MessageType = "std_msgs/Empty"; + } + + //private: + }; + } +} diff --git a/Source/ROSIntegration/Public/std_msgs/Int64.h b/Source/ROSIntegration/Public/std_msgs/Int64.h new file mode 100644 index 0000000..6f70308 --- /dev/null +++ b/Source/ROSIntegration/Public/std_msgs/Int64.h @@ -0,0 +1,20 @@ +#pragma once + +#include "ROSBaseMsg.h" + +namespace ROSMessages{ + namespace std_msgs { + class Int64 : public FROSBaseMsg { + public: + Int64() : Int64(0.f) {} + + Int64(int64 data) { + _MessageType = "std_msgs/Int64"; + _Data = data; + } + + //private: + int64 _Data; + }; + } +} From ca8dd905960ba227c93070d177d02f751b7f64fa Mon Sep 17 00:00:00 2001 From: phuicy Date: Sat, 10 Sep 2022 20:02:14 +0100 Subject: [PATCH 2/2] bugfix removed unimplemented bp types --- Source/ROSIntegration/Classes/RI/Topic.h | 4 ---- 1 file changed, 4 deletions(-) diff --git a/Source/ROSIntegration/Classes/RI/Topic.h b/Source/ROSIntegration/Classes/RI/Topic.h index 1748205..7f8380a 100644 --- a/Source/ROSIntegration/Classes/RI/Topic.h +++ b/Source/ROSIntegration/Classes/RI/Topic.h @@ -23,12 +23,8 @@ enum class EMessageType : uint8 Bool = 3, Float64 = 7, Header = 8, - Int16 = 9, Int32 = 11, Int64 = 12, - UInt16 = 13, - UInt32 = 14, - UInt64 = 15, Vector3 = 2, Point = 17,