Merge pull request #180 from phuicy/master

Adding more BP topic types

Source/ROSIntegration/Classes/RI/Topic.h

@@ -5,6 +5,7 @@
 #include <CoreMinimal.h>
 #include <UObject/ObjectMacros.h>
 #include <UObject/Object.h>
+#include "Math/Vector.h"
 #include "ROSBaseMsg.h"
 #include "ROSIntegrationCore.h"
 
@@ -19,6 +20,17 @@ enum class EMessageType : uint8
 {
 	String = 0,
 	Float32 = 1,
+	Bool = 3,
+	Float64 = 7,
+	Header = 8,
+	Int32 = 11,
+	Int64 = 12,
+
+	Vector3 = 2,
+	Point = 17,
+	Pose = 16,
+	Quaternion = 18,
+	Twist = 19,
 };
 
 UCLASS(Blueprintable)
@@ -56,12 +68,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;
 

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<FROSBaseMsg> &BaseMsg) {
+
+
+	return true;
+}
+
+bool UStdMsgsEmptyConverter::ConvertOutgoingMessage(TSharedPtr<FROSBaseMsg> BaseMsg, bson_t** message) {
+
+	return true;
+}

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<FROSBaseMsg> &BaseMsg);
+	virtual bool ConvertOutgoingMessage(TSharedPtr<FROSBaseMsg> BaseMsg, bson_t** message);
+
+};

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<FROSBaseMsg> &BaseMsg) {
+	bool KeyFound = false;
+
+	int64 Data = GetInt64FromBSON(TEXT("msg.data"), message->full_msg_bson_, KeyFound);
+	if (!KeyFound) return false;
+
+	BaseMsg = TSharedPtr<FROSBaseMsg>(new ROSMessages::std_msgs::Int64(Data));
+	return true;
+}
+
+bool UStdMsgsInt64Converter::ConvertOutgoingMessage(TSharedPtr<FROSBaseMsg> BaseMsg, bson_t** message) {
+	auto Int64Message = StaticCastSharedPtr<ROSMessages::std_msgs::Int64>(BaseMsg);
+	*message = BCON_NEW(
+		"data", BCON_INT32(Int64Message->_Data)
+	);
+	return true;
+}

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<FROSBaseMsg> &BaseMsg);
+	virtual bool ConvertOutgoingMessage(TSharedPtr<FROSBaseMsg> BaseMsg, bson_t** message);
+};

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<FString, UBaseMessageConverter*> TypeConverterMap;
 static TMap<EMessageType, FString> 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<ROSMessages::std_msgs::Bool>(msg);
+				if (ConcreteBoolMessage.IsValid())
+				{
+					const bool Data = ConcreteBoolMessage->_Data;
+					TWeakPtr<UTopic, ESPMode::ThreadSafe> SelfPtr(_SelfPtr);
+					AsyncTask(ENamedThreads::GameThread, [this, Data, SelfPtr]()
+					{
+						if (!SelfPtr.IsValid()) return;
+						OnBoolMessage(Data);
+					});
+				}
+				break;
+			}
+			case EMessageType::Float64:
+			{
+				auto ConcreteDoubleMessage = StaticCastSharedPtr<ROSMessages::std_msgs::Float32>(msg);
+				if (ConcreteDoubleMessage.IsValid())
+				{
+					const double Data = ConcreteDoubleMessage->_Data;
+					TWeakPtr<UTopic, ESPMode::ThreadSafe> SelfPtr(_SelfPtr);
+					AsyncTask(ENamedThreads::GameThread, [this, Data, SelfPtr]()
+					{
+						if (!SelfPtr.IsValid()) return;
+						OnFloat64Message(Data);
+					});
+				}
+				break;
+			}
+			case EMessageType::Int32:
+			{
+				auto ConcreteInt32Message = StaticCastSharedPtr<ROSMessages::std_msgs::Int32>(msg);
+				if (ConcreteInt32Message.IsValid())
+				{
+					const int32 Data = ConcreteInt32Message->_Data;
+					TWeakPtr<UTopic, ESPMode::ThreadSafe> SelfPtr(_SelfPtr);
+					AsyncTask(ENamedThreads::GameThread, [this, Data, SelfPtr]()
+					{
+						if (!SelfPtr.IsValid()) return;
+						OnInt32Message(Data);
+					});
+				}
+				break;
+			}
+			case EMessageType::Int64:
+			{
+				auto ConcreteInt64Message = StaticCastSharedPtr<ROSMessages::std_msgs::Int64>(msg);
+				if (ConcreteInt64Message.IsValid())
+				{
+					const int64 Data = ConcreteInt64Message->_Data;
+					TWeakPtr<UTopic, ESPMode::ThreadSafe> SelfPtr(_SelfPtr);
+					AsyncTask(ENamedThreads::GameThread, [this, Data, SelfPtr]()
+					{
+						if (!SelfPtr.IsValid()) return;
+						OnInt64Message(Data);
+					});
+				}
+				break;
+			}
+			case EMessageType::Vector3:
+			{
+				auto ConcreteVectorMessage = StaticCastSharedPtr<ROSMessages::geometry_msgs::Vector3>(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<UTopic, ESPMode::ThreadSafe> SelfPtr(_SelfPtr);
+					AsyncTask(ENamedThreads::GameThread, [this, Data, SelfPtr]()
+					{
+						if (!SelfPtr.IsValid()) return;
+						OnVector3Message(Data);
+					});
+				}
+				break;
+			}
+			case EMessageType::Point:
+			{
+				auto ConcreteVectorMessage = StaticCastSharedPtr<ROSMessages::geometry_msgs::Point>(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<UTopic, ESPMode::ThreadSafe> SelfPtr(_SelfPtr);
+					AsyncTask(ENamedThreads::GameThread, [this, Data, SelfPtr]()
+					{
+						if (!SelfPtr.IsValid()) return;
+						OnPointMessage(Data);
+					});
+				}
+				break;
+			}
+			case EMessageType::Pose:
+			{
+				auto ConcreteVectorMessage = StaticCastSharedPtr<ROSMessages::geometry_msgs::Pose>(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<UTopic, ESPMode::ThreadSafe> SelfPtr(_SelfPtr);
+					AsyncTask(ENamedThreads::GameThread, [this, position, orientation, SelfPtr]()
+					{
+						if (!SelfPtr.IsValid()) return;
+						OnPoseMessage(position, orientation);
+					});
+				}
+				break;
+			}
+			case EMessageType::Quaternion:
+			{
+				auto ConcreteVectorMessage = StaticCastSharedPtr<ROSMessages::geometry_msgs::Quaternion>(msg);
+				if (ConcreteVectorMessage.IsValid())
+				{
+					const auto q = ConcreteVectorMessage;
+					const FVector4 orientation(q->x, q->y, q->z, q->w);
+					TWeakPtr<UTopic, ESPMode::ThreadSafe> SelfPtr(_SelfPtr);
+					AsyncTask(ENamedThreads::GameThread, [this, orientation, SelfPtr]()
+					{
+						if (!SelfPtr.IsValid()) return;
+						OnQuaternionMessage(orientation);
+					});
+				}
+				break;
+			}
+			case EMessageType::Twist:
+			{
+				auto ConcreteVectorMessage = StaticCastSharedPtr<ROSMessages::geometry_msgs::Twist>(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<UTopic, ESPMode::ThreadSafe> SelfPtr(_SelfPtr);
+					AsyncTask(ENamedThreads::GameThread, [this, linear, angular, SelfPtr]()
+					{
+						if (!SelfPtr.IsValid()) return;
+						OnTwistMessage(linear, angular);
+					});
+				}
+				break;
+			}
 			default:
 				unimplemented();
 				break;

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:
+		};
+	}
+}

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;
+		};
+	}
+}