Merge pull request #213 from andreschreiber/latch-and-magnetic-field

Sanic · web-flow · commit 3db9f269084a · 2025-02-11T11:10:58.000+01:00
Add latched topics and magnetic field
diff --git a/Source/ROSIntegration/Classes/RI/Topic.h b/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
diff --git a/Source/ROSIntegration/Private/Conversion/Messages/sensor_msgs/SensorMsgsImuConverter.h b/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);
 	}
diff --git a/Source/ROSIntegration/Private/Conversion/Messages/sensor_msgs/SensorMsgsMagneticFieldConverter.cpp b/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;
+}
diff --git a/Source/ROSIntegration/Private/Conversion/Messages/sensor_msgs/SensorMsgsMagneticFieldConverter.h b/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);
+	}
+};
diff --git a/Source/ROSIntegration/Private/Topic.cpp b/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
diff --git a/Source/ROSIntegration/Private/rosbridge2cpp/ros_topic.h b/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)
 		{
 		}
diff --git a/Source/ROSIntegration/Public/sensor_msgs/MagneticField.h b/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;
+
+		};
+	}
+}

Original file line number	Diff line number	Diff line change
`@@ -59,7 +59,7 @@ class ROSINTEGRATION_API USensorMsgsImuConverter : public UBaseMessageConverter`
`59`	`59`	`UGeometryMsgsQuaternionConverter::_bson_append_child_quaternion(b, "orientation", &msg->orientation);`
`60`	`60`	`_bson_append_double_tarray(b, "orientation_covariance", msg->orientation_covariance);`
`61`	`61`	`UGeometryMsgsVector3Converter::_bson_append_child_vector3(b, "angular_velocity", &msg->angular_velocity);`
`62`		`- _bson_append_double_tarray(b, "angular_velocity_covariance", msg->orientation_covariance);`
	`62`	`+ _bson_append_double_tarray(b, "angular_velocity_covariance", msg->angular_velocity_covariance);`
`63`	`63`	`UGeometryMsgsVector3Converter::_bson_append_child_vector3(b, "linear_acceleration", &msg->linear_acceleration);`
`64`	`64`	`_bson_append_double_tarray(b, "linear_acceleration_covariance", msg->linear_acceleration_covariance);`
`65`	`65`	`}`
Original file line number	Diff line number	Diff line change
`@@ -40,6 +40,7 @@ class UTopic::Impl {`
`40`	`40`	`FString _Topic;`
`41`	`41`	`FString _MessageType;`
`42`	`42`	`int32 _QueueSize;`
	`43`	`+ bool _Latch;`
`43`	`44`	`rosbridge2cpp::ROSTopic* _ROSTopic = nullptr;`
`44`	`45`	`UBaseMessageConverter* _Converter;`
`45`	`46`	`rosbridge2cpp::ROSCallbackHandle<rosbridge2cpp::FunVrROSPublishMsg> _CallbackHandle;`
`@@ -116,7 +117,7 @@ class UTopic::Impl {`
`116`	`117`	`}`
`117`	`118`	`}`
`118`	`119`
`119`		`- void Init(UROSIntegrationCore *Ric, const FString& Topic, const FString& MessageType, int32 QueueSize)`
	`120`	`+ void Init(UROSIntegrationCore *Ric, const FString& Topic, const FString& MessageType, int32 QueueSize, bool Latch)`
`120`	`121`	`{`
`121`	`122`	`// Construct static ConverterMap`
`122`	`123`	`if (TypeConverterMap.Num() == 0)`
`@@ -138,6 +139,7 @@ class UTopic::Impl {`
`138`	`139`	`_Topic = Topic;`
`139`	`140`	`_MessageType = MessageType;`
`140`	`141`	`_QueueSize = QueueSize;`
	`142`	`+ _Latch = Latch;`
`141`	`143`
`142`	`144`	`UBaseMessageConverter** Converter = TypeConverterMap.Find(MessageType);`
`143`	`145`	`if (!Converter)`
`@@ -152,7 +154,7 @@ class UTopic::Impl {`
`152`	`154`	`}`
`153`	`155`	`_Converter = *Converter;`
`154`	`156`
`155`		`- _ROSTopic = new rosbridge2cpp::ROSTopic(Ric->_Implementation->Get()->GetBridge(), TCHAR_TO_UTF8(Topic), TCHAR_TO_UTF8(MessageType), QueueSize);`
	`157`	`+ _ROSTopic = new rosbridge2cpp::ROSTopic(Ric->_Implementation->Get()->GetBridge(), TCHAR_TO_UTF8(Topic), TCHAR_TO_UTF8(MessageType), QueueSize, Latch);`
`156`	`158`	`}`
`157`	`159`
`158`	`160`	`void MessageCallback(const ROSBridgePublishMsg &message)`
`@@ -252,14 +254,14 @@ bool UTopic::Publish(TSharedPtr<FROSBaseMsg> msg)`
`252`	`254`	`return _State.Connected && _Implementation->Publish(msg);`
`253`	`255`	`}`
`254`	`256`
`255`		`-void UTopic::Init(UROSIntegrationCore *Ric, FString Topic, FString MessageType, int32 QueueSize)`
	`257`	`+void UTopic::Init(UROSIntegrationCore *Ric, FString Topic, FString MessageType, int32 QueueSize, bool Latch)`
`256`	`258`	`{`
`257`	`259`	`_ROSIntegrationCore = Ric;`
`258`	`260`	`_ROSBridgeHost = Ric->GetROSBridgeHost();`
`259`	`261`	`_ROSBridgePort = Ric->GetROSBridgePort();`
`260`	`262`	`_Topic = Topic;`
`261`	`263`	`_MessageType = MessageType;`
`262`		`- _Implementation->Init(Ric, Topic, MessageType, QueueSize);`
	`264`	`+ _Implementation->Init(Ric, Topic, MessageType, QueueSize, Latch);`
`263`	`265`	`}`
`264`	`266`
`265`	`267`	`FString UTopic::GetROSBridgeHost() const`
`@@ -296,7 +298,7 @@ bool UTopic::Reconnect(UROSIntegrationCore* ROSIntegrationCore)`
`296`	`298`
`297`	`299`	`Impl* oldImplementation = _Implementation;`
`298`	`300`	`_Implementation = new UTopic::Impl();`
`299`		`- _Implementation->Init(ROSIntegrationCore, oldImplementation->_Topic, oldImplementation->_MessageType, oldImplementation->_QueueSize);`
	`301`	`+ _Implementation->Init(ROSIntegrationCore, oldImplementation->_Topic, oldImplementation->_MessageType, oldImplementation->_QueueSize, oldImplementation->_Latch);`
`300`	`302`
`301`	`303`	`_State.Connected = true;`
`302`	`304`	`if (_State.Subscribed)`
`@@ -327,7 +329,7 @@ FString UTopic::GetDetailedInfoInternal() const`
`327`	`329`	`return _Implementation->_Topic;`
`328`	`330`	`}`
`329`	`331`
`330`		`-void UTopic::Init(const FString& TopicName, EMessageType MessageType, int32 QueueSize)`
	`332`	`+void UTopic::Init(const FString& TopicName, EMessageType MessageType, int32 QueueSize, bool Latch)`
`331`	`333`	`{`
`332`	`334`	`_State.Blueprint = true;`
`333`	`335`	`_State.BlueprintMessageType = MessageType;`
`@@ -337,7 +339,7 @@ void UTopic::Init(const FString& TopicName, EMessageType MessageType, int32 Queu`
`337`	`339`	`{`
`338`	`340`	`if (ROSInstance->bConnectToROS && _State.Connected)`
`339`	`341`	`{`
`340`		`- Init(ROSInstance->ROSIntegrationCore, TopicName, SupportedMessageTypes[MessageType], QueueSize);`
	`342`	`+ Init(ROSInstance->ROSIntegrationCore, TopicName, SupportedMessageTypes[MessageType], QueueSize, Latch);`
`341`	`343`	`}`
`342`	`344`	`}`
`343`	`345`	`else`
Original file line number	Diff line number	Diff line change
`@@ -19,10 +19,11 @@ namespace rosbridge2cpp{`
`19`	`19`
`20`	`20`	`class ROSTopic {`
`21`	`21`	`public:`
`22`		`- ROSTopic(ROSBridge &ros, std::string topic_name, std::string message_type, int queue_size = 10)`
	`22`	`+ ROSTopic(ROSBridge &ros, std::string topic_name, std::string message_type, int queue_size = 10, bool latch = false)`
`23`	`23`	`: ros_(ros)`
`24`	`24`	`, topic_name_(topic_name)`
`25`	`25`	`, message_type_(message_type)`
	`26`	`+ , latch_(latch)`
`26`	`27`	`, queue_size_(queue_size)`
`27`	`28`	`{`
`28`	`29`	`}`