Add latched topics and magnetic field

Source/ROSIntegration/Classes/RI/Topic.h

@@ -52,7 +52,7 @@ class ROSINTEGRATION_API UTopic: public UObject
 
 	void BeginDestroy() override;
 
-	void Init(UROSIntegrationCore *Ric, FString Topic, FString MessageType, int32 QueueSize = 10);
+	void Init(UROSIntegrationCore *Ric, FString Topic, FString MessageType, int32 QueueSize = 10, bool Latch = false);
 
 	FString GetROSBridgeHost() const;
 
@@ -133,7 +133,7 @@ class ROSINTEGRATION_API UTopic: public UObject
 
 
 	UFUNCTION(BlueprintCallable, Category = "ROS|Topic")
-	void Init(const FString& TopicName, EMessageType MessageType, int32 QueueSize = 1);
+	void Init(const FString& TopicName, EMessageType MessageType, int32 QueueSize = 1, bool Latch = false);
 
 	/**
 	 * Subscribe to the given topic

Source/ROSIntegration/Private/Conversion/Messages/sensor_msgs/SensorMsgsImuConverter.h

@@ -59,7 +59,7 @@ class ROSINTEGRATION_API USensorMsgsImuConverter : public UBaseMessageConverter
 		UGeometryMsgsQuaternionConverter::_bson_append_child_quaternion(b, "orientation", &msg->orientation);
 		_bson_append_double_tarray(b, "orientation_covariance", msg->orientation_covariance);
 		UGeometryMsgsVector3Converter::_bson_append_child_vector3(b, "angular_velocity", &msg->angular_velocity);
-		_bson_append_double_tarray(b, "angular_velocity_covariance", msg->orientation_covariance);
+		_bson_append_double_tarray(b, "angular_velocity_covariance", msg->angular_velocity_covariance);
 		UGeometryMsgsVector3Converter::_bson_append_child_vector3(b, "linear_acceleration", &msg->linear_acceleration);
 		_bson_append_double_tarray(b, "linear_acceleration_covariance", msg->linear_acceleration_covariance);
 	}

Source/ROSIntegration/Private/Conversion/Messages/sensor_msgs/SensorMsgsMagneticFieldConverter.cpp

@@ -0,0 +1,21 @@
+#include "Conversion/Messages/sensor_msgs/SensorMsgsMagneticFieldConverter.h"
+
+USensorMsgsMagneticFieldConverter::USensorMsgsMagneticFieldConverter()
+{
+	_MessageType = "sensor_msgs/MagneticField";
+}
+
+bool USensorMsgsMagneticFieldConverter::ConvertIncomingMessage(const ROSBridgePublishMsg* message, TSharedPtr<FROSBaseMsg>& BaseMsg)
+{
+	auto msg = new ROSMessages::sensor_msgs::MagneticField();
+	BaseMsg = TSharedPtr<FROSBaseMsg>(msg);
+	return _bson_extract_child_magnetic_field(message->full_msg_bson_, "msg", msg);
+}
+
+bool USensorMsgsMagneticFieldConverter::ConvertOutgoingMessage(TSharedPtr<FROSBaseMsg> BaseMsg, bson_t** message)
+{
+	auto CastMsg = StaticCastSharedPtr<ROSMessages::sensor_msgs::MagneticField>(BaseMsg);
+	*message = bson_new();
+	_bson_append_magnetic_field(*message, CastMsg.Get());
+	return true;
+}

Source/ROSIntegration/Private/Conversion/Messages/sensor_msgs/SensorMsgsMagneticFieldConverter.h

@@ -0,0 +1,49 @@
+#pragma once
+
+#include "CoreMinimal.h"
+#include "Conversion/Messages/BaseMessageConverter.h"
+#include "Conversion/Messages/std_msgs/StdMsgsHeaderConverter.h"
+#include "Conversion/Messages/geometry_msgs/GeometryMsgsVector3Converter.h"
+#include "sensor_msgs/MagneticField.h"
+#include "SensorMsgsMagneticFieldConverter.generated.h"
+
+
+UCLASS()
+class ROSINTEGRATION_API USensorMsgsMagneticFieldConverter : public UBaseMessageConverter
+{
+	GENERATED_BODY()
+
+public:
+	USensorMsgsMagneticFieldConverter();
+	virtual bool ConvertIncomingMessage(const ROSBridgePublishMsg* message, TSharedPtr<FROSBaseMsg>& BaseMsg);
+	virtual bool ConvertOutgoingMessage(TSharedPtr<FROSBaseMsg> BaseMsg, bson_t** message);
+
+	static bool _bson_extract_child_magnetic_field(bson_t* b, FString key, ROSMessages::sensor_msgs::MagneticField* msg, bool LogOnErrors = true)
+	{
+		bool KeyFound = false;
+
+		if (!UStdMsgsHeaderConverter::_bson_extract_child_header(b, key + ".header", &msg->header)) return false;
+
+		if (!UGeometryMsgsVector3Converter::_bson_extract_child_vector3(b, key + ".magnetic_field", &msg->magnetic_field)) return false;
+		msg->magnetic_field_covariance = GetDoubleTArrayFromBSON(key + ".magnetic_field_covariance", b, KeyFound);
+		if (!KeyFound || msg->magnetic_field_covariance.Num() != 9) // Size of covariance, 3x3 -> array of 9 see ROS magnetic field msg definition at above link
+			return false;
+
+		return true;
+	}
+
+	static void _bson_append_child_magnetic_field(bson_t* b, const char* key, const ROSMessages::sensor_msgs::MagneticField* msg)
+	{
+		bson_t child;
+		BSON_APPEND_DOCUMENT_BEGIN(b, key, &child);
+		_bson_append_magnetic_field(&child, msg);
+		bson_append_document_end(b, &child);
+	}
+
+	static void _bson_append_magnetic_field(bson_t* b, const ROSMessages::sensor_msgs::MagneticField* msg)
+	{
+		UStdMsgsHeaderConverter::_bson_append_child_header(b, "header", &msg->header);
+		UGeometryMsgsVector3Converter::_bson_append_child_vector3(b, "magnetic_field", &msg->magnetic_field);
+		_bson_append_double_tarray(b, "magnetic_field_covariance", msg->magnetic_field_covariance);
+	}
+};

Source/ROSIntegration/Private/Topic.cpp

@@ -40,6 +40,7 @@ class UTopic::Impl {
 	FString _Topic;
 	FString _MessageType;
 	int32 _QueueSize;
+	bool _Latch;
 	rosbridge2cpp::ROSTopic* _ROSTopic = nullptr;
 	UBaseMessageConverter* _Converter;
 	rosbridge2cpp::ROSCallbackHandle<rosbridge2cpp::FunVrROSPublishMsg> _CallbackHandle;
@@ -116,7 +117,7 @@ class UTopic::Impl {
 		}
 	}
 
-	void Init(UROSIntegrationCore *Ric, const FString& Topic, const FString& MessageType, int32 QueueSize)
+	void Init(UROSIntegrationCore *Ric, const FString& Topic, const FString& MessageType, int32 QueueSize, bool Latch)
 	{
 		// Construct static ConverterMap
 		if (TypeConverterMap.Num() == 0)
@@ -138,6 +139,7 @@ class UTopic::Impl {
 		_Topic = Topic;
 		_MessageType = MessageType;
 		_QueueSize = QueueSize;
+		_Latch = Latch;
 
 		UBaseMessageConverter** Converter = TypeConverterMap.Find(MessageType);
 		if (!Converter)
@@ -152,7 +154,7 @@ class UTopic::Impl {
 		}
 		_Converter = *Converter;
 
-		_ROSTopic = new rosbridge2cpp::ROSTopic(Ric->_Implementation->Get()->GetBridge(), TCHAR_TO_UTF8(*Topic), TCHAR_TO_UTF8(*MessageType), QueueSize);
+		_ROSTopic = new rosbridge2cpp::ROSTopic(Ric->_Implementation->Get()->GetBridge(), TCHAR_TO_UTF8(*Topic), TCHAR_TO_UTF8(*MessageType), QueueSize, Latch);
 	}
 
 	void MessageCallback(const ROSBridgePublishMsg &message)
@@ -252,14 +254,14 @@ bool UTopic::Publish(TSharedPtr<FROSBaseMsg> msg)
 	return _State.Connected && _Implementation->Publish(msg);
 }
 
-void UTopic::Init(UROSIntegrationCore *Ric, FString Topic, FString MessageType, int32 QueueSize)
+void UTopic::Init(UROSIntegrationCore *Ric, FString Topic, FString MessageType, int32 QueueSize, bool Latch)
 {
 	_ROSIntegrationCore = Ric;
 	_ROSBridgeHost = Ric->GetROSBridgeHost();
 	_ROSBridgePort = Ric->GetROSBridgePort();
 	_Topic = Topic;
 	_MessageType = MessageType;
-	_Implementation->Init(Ric, Topic, MessageType, QueueSize);
+	_Implementation->Init(Ric, Topic, MessageType, QueueSize, Latch);
 }
 
 FString UTopic::GetROSBridgeHost() const
@@ -296,7 +298,7 @@ bool UTopic::Reconnect(UROSIntegrationCore* ROSIntegrationCore)
 
 	Impl* oldImplementation = _Implementation;
 	_Implementation = new UTopic::Impl();
-	_Implementation->Init(ROSIntegrationCore, oldImplementation->_Topic, oldImplementation->_MessageType, oldImplementation->_QueueSize);
+	_Implementation->Init(ROSIntegrationCore, oldImplementation->_Topic, oldImplementation->_MessageType, oldImplementation->_QueueSize, oldImplementation->_Latch);
 
 	_State.Connected = true;
 	if (_State.Subscribed)
@@ -327,7 +329,7 @@ FString UTopic::GetDetailedInfoInternal() const
 	return _Implementation->_Topic;
 }
 
-void UTopic::Init(const FString& TopicName, EMessageType MessageType, int32 QueueSize)
+void UTopic::Init(const FString& TopicName, EMessageType MessageType, int32 QueueSize, bool Latch)
 {
 	_State.Blueprint = true;
 	_State.BlueprintMessageType = MessageType;
@@ -337,7 +339,7 @@ void UTopic::Init(const FString& TopicName, EMessageType MessageType, int32 Queu
 	{
 		if (ROSInstance->bConnectToROS && _State.Connected)
 		{
-			Init(ROSInstance->ROSIntegrationCore, TopicName, SupportedMessageTypes[MessageType], QueueSize);
+			Init(ROSInstance->ROSIntegrationCore, TopicName, SupportedMessageTypes[MessageType], QueueSize, Latch);
 		}
 	}
 	else

Source/ROSIntegration/Private/rosbridge2cpp/ros_topic.h

@@ -19,10 +19,11 @@ namespace rosbridge2cpp{
 
 	class ROSTopic {
 	public:
-		ROSTopic(ROSBridge &ros, std::string topic_name, std::string message_type, int queue_size = 10)
+		ROSTopic(ROSBridge &ros, std::string topic_name, std::string message_type, int queue_size = 10, bool latch = false)
 		: ros_(ros)
 		, topic_name_(topic_name)
 		, message_type_(message_type)
+		, latch_(latch)
 		, queue_size_(queue_size)
 		{
 		}

Source/ROSIntegration/Public/sensor_msgs/MagneticField.h

@@ -0,0 +1,24 @@
+#pragma once 
+
+#include "ROSBaseMsg.h"
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Vector3.h"
+
+// https://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/MagneticField.html
+namespace ROSMessages {
+	namespace sensor_msgs {
+		class MagneticField : public FROSBaseMsg {
+		public:
+			MagneticField() {
+				_MessageType = "sensor_msgs/MagneticField";
+			}
+
+			// Header header
+			std_msgs::Header header;
+
+			geometry_msgs::Vector3 magnetic_field;
+			TArray<double> magnetic_field_covariance;
+
+		};
+	}
+}