From 9b337c9fff2692d609f4becc2dde5c340f7295c0 Mon Sep 17 00:00:00 2001 From: Matthew Foran Date: Thu, 7 Jul 2022 14:20:06 -0400 Subject: [PATCH] added HomePosition msg and some blueprint sub/pub --- README.md | 1 + Source/ROSIntegration/Classes/RI/Topic.h | 22 +++ .../GeographicMsgsGeoPointConverter.cpp | 24 +++ .../GeographicMsgsGeoPointConverter.cpp.bak | 26 +++ .../GeographicMsgsGeoPointConverter.h | 44 +++++ .../GeographicMsgsGeoPointConverter.h.bak | 44 +++++ .../MavrosMsgsHomePositionConverter.cpp | 30 ++++ .../MavrosMsgsHomePositionConverter.cpp.bak | 30 ++++ .../MavrosMsgsHomePositionConverter.h | 58 ++++++ .../MavrosMsgsHomePositionConverter.h.bak | 58 ++++++ .../Messages/std_msgs/StdMsgsBoolConverter.h | 2 +- Source/ROSIntegration/Private/Topic.cpp | 168 +++++++++++++++++- .../Public/geographic_msgs/GeoPoint.h | 20 +++ .../Public/mavros_msgs/HomePosition.h | 27 +++ 14 files changed, 552 insertions(+), 2 deletions(-) create mode 100644 Source/ROSIntegration/Private/Conversion/Messages/geographic_msgs/GeographicMsgsGeoPointConverter.cpp create mode 100644 Source/ROSIntegration/Private/Conversion/Messages/geographic_msgs/GeographicMsgsGeoPointConverter.cpp.bak create mode 100644 Source/ROSIntegration/Private/Conversion/Messages/geographic_msgs/GeographicMsgsGeoPointConverter.h create mode 100644 Source/ROSIntegration/Private/Conversion/Messages/geographic_msgs/GeographicMsgsGeoPointConverter.h.bak create mode 100644 Source/ROSIntegration/Private/Conversion/Messages/mavros_msgs/MavrosMsgsHomePositionConverter.cpp create mode 100644 Source/ROSIntegration/Private/Conversion/Messages/mavros_msgs/MavrosMsgsHomePositionConverter.cpp.bak create mode 100644 Source/ROSIntegration/Private/Conversion/Messages/mavros_msgs/MavrosMsgsHomePositionConverter.h create mode 100644 Source/ROSIntegration/Private/Conversion/Messages/mavros_msgs/MavrosMsgsHomePositionConverter.h.bak create mode 100644 Source/ROSIntegration/Public/geographic_msgs/GeoPoint.h create mode 100644 Source/ROSIntegration/Public/mavros_msgs/HomePosition.h diff --git a/README.md b/README.md index 1ab5f49..9ef32ea 100644 --- a/README.md +++ b/README.md @@ -224,6 +224,7 @@ geometry_msgs/TwistWithCovariance | ✓ | ✓ geometry_msgs/Vector3 | ✓ | ✓ grid_map_msgs/GridMap | ✓ | ✓ grid_map_msgs/GridMapInfo | ✓ | ✓ +mavros_msgs/HomePosition | ✓ | ✓ nav_msgs/Odometry | ✓ | ✓ nav_msgs/Path | ✓ | ✓ rosgraph_msgs/Clock | ✓ | ✓ diff --git a/Source/ROSIntegration/Classes/RI/Topic.h b/Source/ROSIntegration/Classes/RI/Topic.h index 1ba76b4..6b89848 100644 --- a/Source/ROSIntegration/Classes/RI/Topic.h +++ b/Source/ROSIntegration/Classes/RI/Topic.h @@ -19,6 +19,10 @@ enum class EMessageType : uint8 { String = 0, Float32 = 1, + PoseStamped = 2, + TwistStamped = 3, + Twist = 4, + HomePosition = 5 }; UCLASS(Blueprintable) @@ -62,6 +66,18 @@ class ROSINTEGRATION_API UTopic: public UObject UFUNCTION(BlueprintImplementableEvent, Category = ROS) void OnFloat32Message(const float& Data); + UFUNCTION(BlueprintImplementableEvent, Category = ROS) + void OnPoseStampedMessage(const FVector& Position, const FRotator& Rotation); + + UFUNCTION(BlueprintImplementableEvent, Category = ROS) + void OnTwistStampedMessage(const FVector& Linear, const FVector& Angular); + + UFUNCTION(BlueprintImplementableEvent, Category = ROS) + void OnTwistMessage(const FVector& Linear, const FVector& Angular); + + UFUNCTION(BlueprintImplementableEvent, Category = ROS) + void OnHomePositionMessage(const FVector& Geo, const FVector& Position, const FRotator& Orientation, const FVector& Approach); + UPROPERTY() UROSIntegrationCore* _ROSIntegrationCore = nullptr; @@ -89,6 +105,12 @@ class ROSINTEGRATION_API UTopic: public UObject UFUNCTION(BlueprintCallable, Category = "ROS|Topic") bool PublishStringMessage(const FString& Message); + UFUNCTION(BlueprintCallable, Category = "ROS|Topic") + bool PublishTwistStampedMessage(const FVector& Linear, const FVector& Angular); + + UFUNCTION(BlueprintCallable, Category = "ROS|Topic") + bool PublishTwistMessage(const FVector& Linear, const FVector& Angular); + // Helper to keep track of self-destruction for async functions TSharedPtr _SelfPtr; diff --git a/Source/ROSIntegration/Private/Conversion/Messages/geographic_msgs/GeographicMsgsGeoPointConverter.cpp b/Source/ROSIntegration/Private/Conversion/Messages/geographic_msgs/GeographicMsgsGeoPointConverter.cpp new file mode 100644 index 0000000..fdf3751 --- /dev/null +++ b/Source/ROSIntegration/Private/Conversion/Messages/geographic_msgs/GeographicMsgsGeoPointConverter.cpp @@ -0,0 +1,24 @@ +#include "Conversion/Messages/geographic_msgs/GeographicMsgsGeoPointConverter.h" + +#include "geographic_msgs/GeoPoint.h" + +UGeographicMsgsGeoPointConverter::UGeographicMsgsGeoPointConverter() +{ + _MessageType = "geographic_msgs/GeoPoint"; +} + +bool UGeographicMsgsGeoPointConverter::ConvertIncomingMessage(const ROSBridgePublishMsg* message, TSharedPtr &BaseMsg) +{ + auto msg = new ROSMessages::geographic_msgs::GeoPoint(); + BaseMsg = TSharedPtr(msg); + return _bson_extract_child_geo_point(message->full_msg_bson_, "msg", msg); +} + +bool UGeographicMsgsGeoPointConverter::ConvertOutgoingMessage(TSharedPtr BaseMsg, bson_t** message) +{ + auto CastMSG = StaticCastSharedPtr(BaseMsg); + *message = bson_new(); + _bson_append_geo_point(*message, CastMSG.Get()); + + return true; +} diff --git a/Source/ROSIntegration/Private/Conversion/Messages/geographic_msgs/GeographicMsgsGeoPointConverter.cpp.bak b/Source/ROSIntegration/Private/Conversion/Messages/geographic_msgs/GeographicMsgsGeoPointConverter.cpp.bak new file mode 100644 index 0000000..fd61245 --- /dev/null +++ b/Source/ROSIntegration/Private/Conversion/Messages/geographic_msgs/GeographicMsgsGeoPointConverter.cpp.bak @@ -0,0 +1,26 @@ +#include "Conversion/Messages/geographic_msgs/GeographicMsgsGeoPointConverter.h" + +#include "geographic_msgs/GeoPoint.h" + +UGeographicMsgsGeoPointConverter::UGeographicMsgsGeoPointConverter() +{ + _MessageType = "geographic_msgs/GeoPoint"; +} + +bool UGeographicMsgsGeoPointConverter::ConvertIncomingMessage(const ROSBridgePublishMsg* message, TSharedPtr &BaseMsg) +{ + auto msg = new ROSMessages::geographic_msgs::GeoPoint(); + BaseMsg = TSharedPtr(msg); + return _bson_extract_child_geo_point(message->full_msg_bson_, "msg", msg); +} + +bool UGeographicMsgsGeoPointConverter::ConvertOutgoingMessage(TSharedPtr BaseMsg, bson_t** message) +{ + auto CastMSG = StaticCastSharedPtr(BaseMsg); + + *message = bson_new(); + bson_init(*message); + _bson_append_geo_point(*message, CastMSG.Get()); + + return true; +} diff --git a/Source/ROSIntegration/Private/Conversion/Messages/geographic_msgs/GeographicMsgsGeoPointConverter.h b/Source/ROSIntegration/Private/Conversion/Messages/geographic_msgs/GeographicMsgsGeoPointConverter.h new file mode 100644 index 0000000..b3a074e --- /dev/null +++ b/Source/ROSIntegration/Private/Conversion/Messages/geographic_msgs/GeographicMsgsGeoPointConverter.h @@ -0,0 +1,44 @@ +#pragma once + +#include +#include "Conversion/Messages/BaseMessageConverter.h" +#include "geographic_msgs/GeoPoint.h" +#include "Conversion/Messages/std_msgs/StdMsgsFloat32Converter.h" +#include "GeographicMsgsGeoPointConverter.generated.h" + + +UCLASS() +class ROSINTEGRATION_API UGeographicMsgsGeoPointConverter : public UBaseMessageConverter +{ + GENERATED_BODY() + +public: + UGeographicMsgsGeoPointConverter(); + virtual bool ConvertIncomingMessage(const ROSBridgePublishMsg* message, TSharedPtr &BaseMsg); + virtual bool ConvertOutgoingMessage(TSharedPtr BaseMsg, bson_t** message); + + static bool _bson_extract_child_geo_point(bson_t *b, FString key, ROSMessages::geographic_msgs::GeoPoint * msg, bool LogOnErrors = true) + { + bool KeyFound = true; + + msg->latitude = GetDoubleFromBSON(key + ".latitude", b, KeyFound, LogOnErrors); if (!KeyFound) return false; + msg->longitude = GetDoubleFromBSON(key + ".longitude", b, KeyFound, LogOnErrors); if (!KeyFound) return false; + msg->altitude = GetDoubleFromBSON(key + ".altitude", b, KeyFound, LogOnErrors); if (!KeyFound) return false; + return true; + } + + static void _bson_append_child_geo_point(bson_t *b, const char *key, const ROSMessages::geographic_msgs::GeoPoint * msg) + { + bson_t child; + BSON_APPEND_DOCUMENT_BEGIN(b, key, &child); + _bson_append_geo_point(&child, msg); + bson_append_document_end(b, &child); + } + + static void _bson_append_geo_point(bson_t *b, const ROSMessages::geographic_msgs::GeoPoint * msg) + { + BSON_APPEND_DOUBLE(b, "latitude", msg->latitude); + BSON_APPEND_DOUBLE(b, "longitude", msg->longitude); + BSON_APPEND_DOUBLE(b, "altitude", msg->altitude); + } +}; diff --git a/Source/ROSIntegration/Private/Conversion/Messages/geographic_msgs/GeographicMsgsGeoPointConverter.h.bak b/Source/ROSIntegration/Private/Conversion/Messages/geographic_msgs/GeographicMsgsGeoPointConverter.h.bak new file mode 100644 index 0000000..b83e2da --- /dev/null +++ b/Source/ROSIntegration/Private/Conversion/Messages/geographic_msgs/GeographicMsgsGeoPointConverter.h.bak @@ -0,0 +1,44 @@ +#pragma once + +#include +#include "Conversion/Messages/BaseMessageConverter.h" +#include "geographic_msgs/GeoPoint.h" +#include "Conversion/Messages/std_msgs/StdMsgsFloat32Converter.h" +#include "GeographicMsgsGeoPointConverter.generated.h" + + +UCLASS() +class ROSINTEGRATION_API UGeographicMsgsGeoPointConverter : public UBaseMessageConverter +{ + GENERATED_BODY() + +public: + UGeographicMsgsGeoPointConverter(); + virtual bool ConvertIncomingMessage(const ROSBridgePublishMsg* message, TSharedPtr &BaseMsg); + virtual bool ConvertOutgoingMessage(TSharedPtr BaseMsg, bson_t** message); + + static bool _bson_extract_child_geo_point(bson_t *b, FString key, ROSMessages::geographic_msgs::GeoPoint * msg, bool LogOnErrors = true) + { + bool KeyFound = true; + + msg->latitude = GetDoubleFromBSON(key + ".latitude", b, KeyFound, LogOnErrors); if (!KeyFound) return false; + msg->longitude = GetDoubleFromBSON(key + ".longitude", b, KeyFound, LogOnErrors); if (!KeyFound) return false; + msg->altitude = GetDoubleFromBSON(key + ".altitude", b, KeyFound, LogOnErrors); if (!KeyFound) return false; + return true; + } + + static void _bson_append_child_geo_point(bson_t *b, const char *key, const ROSMessages::geographic_msgs::GeoPoint * msg) + { + bson_t child; + BSON_APPEND_DOCUMENT_BEGIN(b, key, &child); + _bson_append_geo_point(&child, ps); + bson_append_document_end(b, &child); + } + + static void _bson_append_geo_point(bson_t *b, const ROSMessages::geographic_msgs::GeoPoint * ps) + { + BSON_APPEND_DOUBLE(b, "latitude", ps->latitude); + BSON_APPEND_DOUBLE(b, "longitude", ps->longitude); + BSON_APPEND_DOUBLE(b, "altitude", ps->altitude); + } +}; diff --git a/Source/ROSIntegration/Private/Conversion/Messages/mavros_msgs/MavrosMsgsHomePositionConverter.cpp b/Source/ROSIntegration/Private/Conversion/Messages/mavros_msgs/MavrosMsgsHomePositionConverter.cpp new file mode 100644 index 0000000..ee1aa30 --- /dev/null +++ b/Source/ROSIntegration/Private/Conversion/Messages/mavros_msgs/MavrosMsgsHomePositionConverter.cpp @@ -0,0 +1,30 @@ +#include "Conversion/Messages/mavros_msgs/MavrosMsgsHomePositionConverter.h" + + +UMavrosMsgsHomePositionConverter::UMavrosMsgsHomePositionConverter() +{ + _MessageType = "mavros_msgs/HomePosition"; +} + +bool UMavrosMsgsHomePositionConverter::ConvertIncomingMessage(const ROSBridgePublishMsg* message, TSharedPtr &BaseMsg) +{ + auto msg = new ROSMessages::mavros_msgs::HomePosition(); + BaseMsg = TSharedPtr(msg); + + if (!UStdMsgsHeaderConverter::_bson_extract_child_header(message->full_msg_bson_, TEXT("msg.header"), &msg->header)) return false; + if (!UGeometryMsgsPointConverter::_bson_extract_child_point(message->full_msg_bson_, TEXT("msg.position"), &msg->position)) return false; + if (!UGeometryMsgsQuaternionConverter::_bson_extract_child_quaternion(message->full_msg_bson_, TEXT("msg.orientation"), &msg->orientation)) return false; + if (!UGeometryMsgsVector3Converter::_bson_extract_child_vector3(message->full_msg_bson_, TEXT("msg.approach"), &msg->approach)) return false; + if (!UGeographicMsgsGeoPointConverter::_bson_extract_child_geo_point(message->full_msg_bson_, TEXT("msg.geo"), &msg->geo)) return false; + + return true; +} + +bool UMavrosMsgsHomePositionConverter::ConvertOutgoingMessage(TSharedPtr BaseMsg, bson_t** message) +{ + auto CastMSG = StaticCastSharedPtr(BaseMsg); + *message = bson_new(); + _bson_append_home_position(*message, CastMSG.Get()); + + return true; +} diff --git a/Source/ROSIntegration/Private/Conversion/Messages/mavros_msgs/MavrosMsgsHomePositionConverter.cpp.bak b/Source/ROSIntegration/Private/Conversion/Messages/mavros_msgs/MavrosMsgsHomePositionConverter.cpp.bak new file mode 100644 index 0000000..ee1aa30 --- /dev/null +++ b/Source/ROSIntegration/Private/Conversion/Messages/mavros_msgs/MavrosMsgsHomePositionConverter.cpp.bak @@ -0,0 +1,30 @@ +#include "Conversion/Messages/mavros_msgs/MavrosMsgsHomePositionConverter.h" + + +UMavrosMsgsHomePositionConverter::UMavrosMsgsHomePositionConverter() +{ + _MessageType = "mavros_msgs/HomePosition"; +} + +bool UMavrosMsgsHomePositionConverter::ConvertIncomingMessage(const ROSBridgePublishMsg* message, TSharedPtr &BaseMsg) +{ + auto msg = new ROSMessages::mavros_msgs::HomePosition(); + BaseMsg = TSharedPtr(msg); + + if (!UStdMsgsHeaderConverter::_bson_extract_child_header(message->full_msg_bson_, TEXT("msg.header"), &msg->header)) return false; + if (!UGeometryMsgsPointConverter::_bson_extract_child_point(message->full_msg_bson_, TEXT("msg.position"), &msg->position)) return false; + if (!UGeometryMsgsQuaternionConverter::_bson_extract_child_quaternion(message->full_msg_bson_, TEXT("msg.orientation"), &msg->orientation)) return false; + if (!UGeometryMsgsVector3Converter::_bson_extract_child_vector3(message->full_msg_bson_, TEXT("msg.approach"), &msg->approach)) return false; + if (!UGeographicMsgsGeoPointConverter::_bson_extract_child_geo_point(message->full_msg_bson_, TEXT("msg.geo"), &msg->geo)) return false; + + return true; +} + +bool UMavrosMsgsHomePositionConverter::ConvertOutgoingMessage(TSharedPtr BaseMsg, bson_t** message) +{ + auto CastMSG = StaticCastSharedPtr(BaseMsg); + *message = bson_new(); + _bson_append_home_position(*message, CastMSG.Get()); + + return true; +} diff --git a/Source/ROSIntegration/Private/Conversion/Messages/mavros_msgs/MavrosMsgsHomePositionConverter.h b/Source/ROSIntegration/Private/Conversion/Messages/mavros_msgs/MavrosMsgsHomePositionConverter.h new file mode 100644 index 0000000..f3afc69 --- /dev/null +++ b/Source/ROSIntegration/Private/Conversion/Messages/mavros_msgs/MavrosMsgsHomePositionConverter.h @@ -0,0 +1,58 @@ +#pragma once + +#include +#include "Conversion/Messages/BaseMessageConverter.h" +#include "mavros_msgs/HomePosition.h" +#include "std_msgs/Header.h" +#include "geometry_msgs/Point.h" +#include "geometry_msgs/Quaternion.h" +#include "geometry_msgs/Vector3.h" +#include "geographic_msgs/GeoPoint.h" +#include "Conversion/Messages/std_msgs/StdMsgsHeaderConverter.h" +#include "Conversion/Messages/geometry_msgs/GeometryMsgsPointConverter.h" +#include "Conversion/Messages/geometry_msgs/GeometryMsgsQuaternionConverter.h" +#include "Conversion/Messages/geometry_msgs/GeometryMsgsVector3Converter.h" +#include "Conversion/Messages/geographic_msgs/GeographicMsgsGeoPointConverter.h" +#include "MavrosMsgsHomePositionConverter.generated.h" + + +UCLASS() +class ROSINTEGRATION_API UMavrosMsgsHomePositionConverter : public UBaseMessageConverter +{ + GENERATED_BODY() + +public: + UMavrosMsgsHomePositionConverter(); + virtual bool ConvertIncomingMessage(const ROSBridgePublishMsg* message, TSharedPtr &BaseMsg); + virtual bool ConvertOutgoingMessage(TSharedPtr BaseMsg, bson_t** message); + + static bool _bson_extract_child_home_position(bson_t *b, FString key, ROSMessages::mavros_msgs::HomePosition* msg, bool LogOnErrors = true) + { + bool KeyFound = true; + + KeyFound &= UStdMsgsHeaderConverter::_bson_extract_child_header(b, key + ".header", &msg->header); + KeyFound &= UGeometryMsgsPointConverter::_bson_extract_child_point(b, key + ".position", &msg->position); + KeyFound &= UGeometryMsgsQuaternionConverter::_bson_extract_child_quaternion(b, key + ".orientation", &msg->orientation); + KeyFound &= UGeometryMsgsVector3Converter::_bson_extract_child_vector3(b, key + ".approach", &msg->approach); + KeyFound &= UGeographicMsgsGeoPointConverter::_bson_extract_child_geo_point(b, key + ".geo", &msg->geo); + + return KeyFound; + } + + static void _bson_append_child_home_position(bson_t *b, const char *key, const ROSMessages::mavros_msgs::HomePosition* msg) + { + bson_t child; + BSON_APPEND_DOCUMENT_BEGIN(b, key, &child); + _bson_append_home_position(&child, msg); + bson_append_document_end(b, &child); + } + + static void _bson_append_home_position(bson_t *b, const ROSMessages::mavros_msgs::HomePosition* msg) + { + UStdMsgsHeaderConverter::_bson_append_header(b, &(msg->header)); + UGeometryMsgsPointConverter::_bson_append_child_point(b, "position", &(msg->position)); + UGeometryMsgsQuaternionConverter::_bson_append_child_quaternion(b, "orientation", &(msg->orientation)); + UGeometryMsgsVector3Converter::_bson_append_child_vector3(b, "approach", &(msg->approach)); + UGeographicMsgsGeoPointConverter::_bson_append_child_geo_point(b, "geo", &(msg->geo)); + } +}; diff --git a/Source/ROSIntegration/Private/Conversion/Messages/mavros_msgs/MavrosMsgsHomePositionConverter.h.bak b/Source/ROSIntegration/Private/Conversion/Messages/mavros_msgs/MavrosMsgsHomePositionConverter.h.bak new file mode 100644 index 0000000..9625b9c --- /dev/null +++ b/Source/ROSIntegration/Private/Conversion/Messages/mavros_msgs/MavrosMsgsHomePositionConverter.h.bak @@ -0,0 +1,58 @@ +#pragma once + +#include +#include "Conversion/Messages/BaseMessageConverter.h" +#include "mavros_msgs/HomePosition.h" +#include "std_msgs/Header.h" +#include "geometry_msgs/Point.h" +#include "geometry_msgs/Quaternion.h" +#include "geometry_msgs/Vector3.h" +#include "geographic_msgs/GeoPoint.h" +#include "Conversion/Messages/std_msgs/StdMsgsHeaderConverter.h" +#include "Conversion/Messages/geometry_msgs/GeometryMsgsPointConverter.h" +#include "Conversion/Messages/geometry_msgs/GeometryMsgsQuaternionConverter.h" +#include "Conversion/Messages/geometry_msgs/GeometryMsgsVector3Converter.h" +#include "Conversion/Messages/geographic_msgs/GeographicMsgsGeoPointConverter.h" +#include "MavrosMsgsHomePositionConverter.generated.h" + + +UCLASS() +class ROSINTEGRATION_API UMavrosMsgsHomePositionConverter : public UBaseMessageConverter +{ + GENERATED_BODY() + +public: + UMavrosMsgsHomePositionConverter(); + virtual bool ConvertIncomingMessage(const ROSBridgePublishMsg* message, TSharedPtr &BaseMsg); + virtual bool ConvertOutgoingMessage(TSharedPtr BaseMsg, bson_t** message); + + static bool _bson_extract_child_home_position(bson_t *b, FString key, ROSMessages::mavros_msgs::HomePosition* msg, bool LogOnErrors = true) + { + bool KeyFound = true; + + KeyFound &= UStdMsgsHeaderConverter::_bson_extract_child_header(b, key + ".header", &msg->header); + KeyFound &= UGeometryMsgsPointConverter::_bson_extract_child_point(b, key + ".position", &msg->position); + KeyFound &= UGeometryMsgsQuaternionConverter::_bson_extract_child_quaternion(b, key + ".orientation", &msg->orientation); + KeyFound &= UGeometryMsgsVector3Converter::_bson_extract_child_vector3(b, key + ".approach", &msg->approach); + KeyFound &= UGeographicMsgsGeoPointConverter::_bson_extract_child_geo_point(b, key + ".geo", &msg->geo); + + return KeyFound; + } + + static void _bson_append_child_home_position(bson_t *b, const char *key, const ROSMessages::mavros_msgs::HomePosition* ps) + { + bson_t layout; + BSON_APPEND_DOCUMENT_BEGIN(b, key, &layout); + _bson_append_home_position(&layout, ps); + bson_append_document_end(b, &layout); + } + + static void _bson_append_home_position(bson_t *b, const ROSMessages::mavros_msgs::HomePosition* ps) + { + UStdMsgsHeaderConverter::_bson_append_header(b, &(ps->header)); + UGeometryMsgsPointConverter::_bson_append_child_point(b, "position", &(ps->position)); + UGeometryMsgsQuaternionConverter::_bson_append_child_quaternion(b, "orientation", &(ps->orientation)); + UGeometryMsgsVector3Converter::_bson_append_child_vector3(b, "approach", &(ps->approach)); + UGeographicMsgsGeoPointConverter::_bson_append_child_geo_point(b, "geo", &(ps->geo)); + } +}; diff --git a/Source/ROSIntegration/Private/Conversion/Messages/std_msgs/StdMsgsBoolConverter.h b/Source/ROSIntegration/Private/Conversion/Messages/std_msgs/StdMsgsBoolConverter.h index ad1f9b7..e344ff5 100644 --- a/Source/ROSIntegration/Private/Conversion/Messages/std_msgs/StdMsgsBoolConverter.h +++ b/Source/ROSIntegration/Private/Conversion/Messages/std_msgs/StdMsgsBoolConverter.h @@ -2,7 +2,7 @@ #pragma once -#include "CoreMinimal.h" +#include #include "Conversion/Messages/BaseMessageConverter.h" #include "std_msgs/Bool.h" #include "StdMsgsBoolConverter.generated.h" diff --git a/Source/ROSIntegration/Private/Topic.cpp b/Source/ROSIntegration/Private/Topic.cpp index a118aa7..6bb7d9a 100644 --- a/Source/ROSIntegration/Private/Topic.cpp +++ b/Source/ROSIntegration/Private/Topic.cpp @@ -176,6 +176,10 @@ UTopic::UTopic(const FObjectInitializer& ObjectInitializer) { SupportedMessageTypes.Add(EMessageType::String, TEXT("std_msgs/String")); SupportedMessageTypes.Add(EMessageType::Float32, TEXT("std_msgs/Float32")); + SupportedMessageTypes.Add(EMessageType::PoseStamped, TEXT("geometry_msgs/PoseStamped")); + SupportedMessageTypes.Add(EMessageType::TwistStamped, TEXT("geometry_msgs/TwistStamped")); + SupportedMessageTypes.Add(EMessageType::Twist, TEXT("geometry_msgs/Twist")); + SupportedMessageTypes.Add(EMessageType::HomePosition, TEXT("mavros_msgs/HomePosition")); } } @@ -232,7 +236,7 @@ bool UTopic::Unadvertise() bool UTopic::Publish(TSharedPtr msg) { - return _State.Connected && _Implementation->Publish(msg); + return msg != nullptr && _State.Connected && _Implementation->Publish(msg); } void UTopic::Init(UROSIntegrationCore *Ric, FString Topic, FString MessageType, int32 QueueSize) @@ -345,6 +349,123 @@ bool UTopic::Subscribe() } break; } + case EMessageType::PoseStamped: + { + auto ConcretePoseStampedMessage = StaticCastSharedPtr(msg); + if (ConcretePoseStampedMessage.IsValid()) + { + const FVector Pos = FVector( + ConcretePoseStampedMessage->pose.position.x * 100, + ConcretePoseStampedMessage->pose.position.y * 100, + ConcretePoseStampedMessage->pose.position.z * -100); + + const FQuat Quat = FQuat( + ConcretePoseStampedMessage->pose.orientation.x, + ConcretePoseStampedMessage->pose.orientation.y * -1, + ConcretePoseStampedMessage->pose.orientation.w * -1, + ConcretePoseStampedMessage->pose.orientation.z * -1); + + TWeakPtr SelfPtr(_SelfPtr); + AsyncTask(ENamedThreads::GameThread, [this, Pos, Quat, SelfPtr]() + { + if (!SelfPtr.IsValid()) return; + FRotator Rot = Quat.Rotator(); + Rot.Yaw -= 90; + float temp = Rot.Pitch; + Rot.Pitch = Rot.Roll; + Rot.Roll = temp*-1; + OnPoseStampedMessage(Pos, Rot); + }); + } + break; + } + case EMessageType::TwistStamped: + { + auto ConcreteTwistStampedMessage = StaticCastSharedPtr(msg); + if (ConcreteTwistStampedMessage.IsValid()) + { + const FVector Linear = FVector( + ConcreteTwistStampedMessage->twist.linear.x * 100, + ConcreteTwistStampedMessage->twist.linear.y * 100, + ConcreteTwistStampedMessage->twist.linear.z * 100); + + const FVector Angular = FVector( + ConcreteTwistStampedMessage->twist.angular.x, + ConcreteTwistStampedMessage->twist.angular.y * -1, + ConcreteTwistStampedMessage->twist.angular.z); + + TWeakPtr SelfPtr(_SelfPtr); + AsyncTask(ENamedThreads::GameThread, [this, Linear, Angular, SelfPtr]() + { + if (!SelfPtr.IsValid()) return; + OnTwistStampedMessage(Linear, Angular); + }); + } + break; + } + case EMessageType::Twist: + { + auto ConcreteTwistMessage = StaticCastSharedPtr(msg); + if (ConcreteTwistMessage.IsValid()) + { + const FVector Linear = FVector( + ConcreteTwistMessage->linear.x * 100, + ConcreteTwistMessage->linear.y * -100, + ConcreteTwistMessage->linear.z * 100); + + const FVector Angular = FVector( + ConcreteTwistMessage->angular.x, + ConcreteTwistMessage->angular.y * -1, + ConcreteTwistMessage->angular.z); + + TWeakPtr SelfPtr(_SelfPtr); + AsyncTask(ENamedThreads::GameThread, [this, Linear, Angular, SelfPtr]() + { + if (!SelfPtr.IsValid()) return; + OnTwistMessage(Linear, Angular); + }); + } + break; + } + + case EMessageType::HomePosition: + { + auto ConcreteHomePositionMessage = StaticCastSharedPtr(msg); + if (ConcreteHomePositionMessage.IsValid()) + { + const FVector Geo = FVector( + ConcreteHomePositionMessage->geo.latitude, + ConcreteHomePositionMessage->geo.longitude, + ConcreteHomePositionMessage->geo.altitude); + + const FVector Position = FVector( + ConcreteHomePositionMessage->position.x * 100, + ConcreteHomePositionMessage->position.y * 100, + ConcreteHomePositionMessage->position.z * -100); + + const FQuat Orientation = FQuat( + ConcreteHomePositionMessage->orientation.x, + ConcreteHomePositionMessage->orientation.y * -1, + ConcreteHomePositionMessage->orientation.w * -1, + ConcreteHomePositionMessage->orientation.z * -1); + + const FVector Approach = FVector( + ConcreteHomePositionMessage->approach.x, + ConcreteHomePositionMessage->approach.y, + ConcreteHomePositionMessage->approach.z); + + TWeakPtr SelfPtr(_SelfPtr); + AsyncTask(ENamedThreads::GameThread, [this, Geo, Position, Orientation, Approach, SelfPtr]() + { + FRotator Rot = Orientation.Rotator(); + Rot.Yaw += 180; + + if (!SelfPtr.IsValid()) return; + OnHomePositionMessage(Geo, Position, Rot, Approach); + }); + } + break; + } default: unimplemented(); break; @@ -374,3 +495,48 @@ bool UTopic::PublishStringMessage(const FString& Message) msg->_Data = Message; return _Implementation->Publish(msg); } + + + +bool UTopic::PublishTwistStampedMessage(const FVector& Linear, const FVector& Angular) +{ + + if (!_State.Advertised) + { + if (!Advertise()) + { + return false; + } + } + + TSharedPtr msg = MakeShareable(new ROSMessages::geometry_msgs::TwistStamped); + msg->twist.linear = Linear; + msg->twist.linear.y *= -1; + msg->twist.angular = Angular; + msg->twist.angular.z *= -1; + msg->twist.angular.y *= -1; + + return _Implementation->Publish(msg); +} + + +bool UTopic::PublishTwistMessage(const FVector& Linear, const FVector& Angular) +{ + + if (!_State.Advertised) + { + if (!Advertise()) + { + return false; + } + } + + TSharedPtr msg = MakeShareable(new ROSMessages::geometry_msgs::Twist); + msg->linear = Linear; + float temp = msg->linear.y; + msg->linear.y = msg->linear.x; + msg->linear.x = temp; + msg->angular = Angular; + msg->angular.z *= -1; + return _Implementation->Publish(msg); +} \ No newline at end of file diff --git a/Source/ROSIntegration/Public/geographic_msgs/GeoPoint.h b/Source/ROSIntegration/Public/geographic_msgs/GeoPoint.h new file mode 100644 index 0000000..e42f9a4 --- /dev/null +++ b/Source/ROSIntegration/Public/geographic_msgs/GeoPoint.h @@ -0,0 +1,20 @@ +#pragma once + +#include "ROSBaseMsg.h" + +#include "std_msgs/Float32.h" + +namespace ROSMessages { + namespace geographic_msgs { + class GeoPoint : public FROSBaseMsg { + public: + GeoPoint() { + _MessageType = "geographic_msgs/GeoPoint"; + } + + double latitude; + double longitude; + double altitude; + }; + } +} diff --git a/Source/ROSIntegration/Public/mavros_msgs/HomePosition.h b/Source/ROSIntegration/Public/mavros_msgs/HomePosition.h new file mode 100644 index 0000000..e2ffe84 --- /dev/null +++ b/Source/ROSIntegration/Public/mavros_msgs/HomePosition.h @@ -0,0 +1,27 @@ +#pragma once + +#include "ROSBaseMsg.h" + +#include "std_msgs/Header.h" +#include "geographic_msgs/GeoPoint.h" +#include "geometry_msgs/Point.h" +#include "geometry_msgs/Quaternion.h" +#include "geometry_msgs/Vector3.h" + + +namespace ROSMessages { + namespace mavros_msgs { + class HomePosition : public FROSBaseMsg { + public: + HomePosition() { + _MessageType = "mavros_msgs/HomePosition"; + } + + std_msgs::Header header; + geographic_msgs::GeoPoint geo; + geometry_msgs::Point position; + geometry_msgs::Quaternion orientation; + geometry_msgs::Vector3 approach; + }; + } +}