diff --git a/Private/RobofleetBPFunctionLibrary.cpp b/Private/RobofleetBPFunctionLibrary.cpp index a563322..39d4086 100644 --- a/Private/RobofleetBPFunctionLibrary.cpp +++ b/Private/RobofleetBPFunctionLibrary.cpp @@ -74,6 +74,15 @@ TArray URobofleetBPFunctionLibrary::GetRobotImage(const FString& RobotNam return TArray(); } +bool URobofleetBPFunctionLibrary::IsRobotImageCompressed(const FString& RobotName) +{ + if (FRobofleetUnrealClientModule::Get()->IsSessionRunning()) + { + return FRobofleetUnrealClientModule::Get()->RobofleetClient->IsRobotImageCompressed(RobotName); + } + return bool(); +} + void URobofleetBPFunctionLibrary::PrintRobotsSeen() { if (FRobofleetUnrealClientModule::Get()->IsSessionRunning()) @@ -140,6 +149,15 @@ FVector URobofleetBPFunctionLibrary::GetDetectedPositionGlobal(const FString& Ro return FVector(0, 0, 0); } +TArray URobofleetBPFunctionLibrary::GetDetectedImage(const FString& RobotName) +{ + if (FRobofleetUnrealClientModule::Get()->IsSessionRunning()) + { + return FRobofleetUnrealClientModule::Get()->RobofleetClient->GetDetectedImage(RobotName); + } + return TArray(); +} + // Publish Messages to Robofleet // *TODO: These need to be rate limited somewhere in the client as there is the potential to overload the server void URobofleetBPFunctionLibrary::PublishStatusMsg(const FString& RobotName, const FRobotStatus& StatusMsg) diff --git a/Private/RobofleetClientBase.cpp b/Private/RobofleetClientBase.cpp index 74d45ae..9481914 100644 --- a/Private/RobofleetClientBase.cpp +++ b/Private/RobofleetClientBase.cpp @@ -1,5 +1,6 @@ #include "RobofleetClientBase.h" #include "GameFramework/Actor.h" +#include URobofleetBase::URobofleetBase() { @@ -26,6 +27,7 @@ bool URobofleetBase::IsConnected() { if (IsValid(SocketClient)) { + //UE_LOG(LogRobofleet, Warning, TEXT("Websocket is Valid")) return SocketClient->Socket->IsConnected(); } return false; @@ -45,7 +47,10 @@ void URobofleetBase::Initialize(FString HostUrl, const UObject* WorldContextObje RegisterRobotStatusSubscription(); RegisterRobotSubscription("localization", "*"); + //RegisterRobotSubscription("image_compressed/main", "*"); + UE_LOG(LogRobofleet, Log, TEXT("RobofleetBase initialized")); RegisterRobotSubscription("detected", "*"); + bIsInitilized = true; } @@ -77,11 +82,12 @@ void URobofleetBase::PruneInactiveRobots() { void URobofleetBase::WebsocketDataCB(const void* Data) { const fb::MsgWithMetadata* msg = flatbuffers::GetRoot(Data); + std::string MsgTopic = msg->__metadata()->topic()->c_str(); int NamespaceIndex = MsgTopic.substr(1, MsgTopic.length()).find('/'); FString RobotNamespace = FString(MsgTopic.substr(1, NamespaceIndex).c_str()); - FString TopicIsolated = FString(MsgTopic.substr(NamespaceIndex+2, MsgTopic.length()).c_str()); + FString TopicIsolated = FString(MsgTopic.substr(NamespaceIndex + 2, MsgTopic.length()).c_str()); RobotsSeenTime[RobotNamespace] = FDateTime::Now(); @@ -120,6 +126,7 @@ void URobofleetBase::RefreshRobotList() UE_LOG(LogRobofleet, Log, TEXT("Refreshing robot list")); RegisterRobotStatusSubscription(); RegisterRobotSubscription("localization", "*"); + RegisterRobotSubscription("image_raw/compressed", "*"); RegisterRobotSubscription("detected", "*"); //PruneInactiveRobots(); } @@ -150,6 +157,13 @@ void URobofleetBase::DecodeMsg(const void* Data, FString topic, FString RobotNam //UE_LOG(LogTemp,Warning,TEXT("x: %f, y:%f"), rl.x, rl.y) RobotMap[RobotNamespace]->Location = rl; } + + // Missing flatbuffer definitions + //else if (topic == "image/main") { + // RobotImageMap[RobotNamespace] = DecodeMsg(Data); + // OnImageReceived.Broadcast(RobotNamespace); + //} + else if (topic == "detected") { DetectedItemMap[RobotNamespace] = DecodeMsg(Data); OnDetectedItemReceived.Broadcast(RobotNamespace); @@ -162,7 +176,6 @@ void URobofleetBase::DecodeMsg(const void* Data, FString topic, FString RobotNam } } - template void URobofleetBase::EncodeRosMsg (const T& msg, const std::string& msg_type, std::string& from_topic, const std::string& to_topic) { // Encoding Messages for Robofleet as ROS Messages @@ -269,15 +282,24 @@ FVector URobofleetBase::GetRobotPosition(const FString& RobotName) TArray URobofleetBase::GetRobotImage(const FString& RobotName) { - //needs to return type that Texture expects FString RobotNamestd = FString(TCHAR_TO_UTF8(*RobotName)); - TArray imageData; - imageData.Append(&DetectedItemMap[RobotNamestd].cmpr_image.data[0], DetectedItemMap[RobotNamestd].cmpr_image.data.size()); - // you may want an TArray - // FColor pixelColor = {0, &RobotImageMap[Name].data[i] : i+3} - return imageData; + //returns the constructor/ init list: TArray(arrayPtr, arraySize) + UE_LOG(LogTemp, Log, TEXT("Creating local image message of type TArray.")); + return TArray(&RobotImageMap[RobotNamestd].data[0], RobotImageMap[RobotNamestd].data.size()); +} + +bool URobofleetBase::IsRobotImageCompressed(const FString& RobotName) +{ + FString RobotNamestd = FString(TCHAR_TO_UTF8(*RobotName)); + if (RobotImageMap[RobotNamestd].format.find("compressed") != std::string::npos) + { + return true; + } + else return false; + } + TArray URobofleetBase::GetAllRobotsAtSite(const FString& Location) { TArray RobotsAtSite; @@ -325,3 +347,11 @@ FVector URobofleetBase::GetDetectedPositionGlobal(const FString& RobotName) if (RobotMap.count(RobotNamestd) == 0) return FVector(-1, -1, -1); return FVector(DetectedItemMap[RobotNamestd].lat, DetectedItemMap[RobotNamestd].lon, DetectedItemMap[RobotNamestd].elv); } + +TArray URobofleetBase::GetDetectedImage(const FString& RobotName) +{ + FString RobotNamestd = FString(TCHAR_TO_UTF8(*RobotName)); + //returns the constructor/ init list: TArray(arrayPtr, arraySize) + UE_LOG(LogTemp, Log, TEXT("Creating local image message of type TArray.")); + return TArray(&DetectedItemMap[RobotNamestd].cmpr_image.data[0], DetectedItemMap[RobotNamestd].cmpr_image.data.size()); +} \ No newline at end of file diff --git a/Private/RobofleetClientBase.h b/Private/RobofleetClientBase.h index 8b35ef3..044c317 100644 --- a/Private/RobofleetClientBase.h +++ b/Private/RobofleetClientBase.h @@ -99,6 +99,8 @@ class ROBOFLEETUNREALCLIENT_API URobofleetBase : public UObject TArray GetRobotImage(const FString& RobotName); + bool IsRobotImageCompressed(const FString& RobotName); + TArray GetAllRobotsAtSite(const FString& Location); FString GetDetectedName(const FString& RobotName); @@ -111,6 +113,8 @@ class ROBOFLEETUNREALCLIENT_API URobofleetBase : public UObject FVector GetDetectedPositionGlobal(const FString& RobotName); + TArray GetDetectedImage(const FString& RobotName); + bool IsRobotOk(const FString& RobotName); void PrintRobotsSeen(); diff --git a/Private/WebsocketClient.cpp b/Private/WebsocketClient.cpp index c499a94..3d906a7 100644 --- a/Private/WebsocketClient.cpp +++ b/Private/WebsocketClient.cpp @@ -29,6 +29,7 @@ void UWebsocketClient::Initialize(FString ServerURL /*= TEXT("ws://localhost:808 Socket->OnConnected().AddUFunction(this, FName("OnConnected")); Socket->OnConnectionError().AddUFunction(this, FName("OnConnectionError")); Socket->OnMessageSent().AddUFunction(this, FName("OnMessageSent")); + Socket->OnClosed().AddUFunction(this, FName("OnClosed")); // Unreal having a problem with binding to a raw function, and a UFUNCTION doesn't like having a void* as a arg // For now, use lambda directly @@ -55,6 +56,11 @@ void UWebsocketClient::OnConnected() UE_LOG(LogTemp, Log, TEXT("Connected to websocket.")) } +void UWebsocketClient::OnClosed() +{ + UE_LOG(LogTemp, Log, TEXT("Websocket Closed.")) +} + void UWebsocketClient::OnConnectionError() { UE_LOG(LogTemp, Warning, TEXT("Encountered error while trying to connect to websocket.")) @@ -62,11 +68,38 @@ void UWebsocketClient::OnConnectionError() void UWebsocketClient::OnMessageReceived(const void* Data, SIZE_T Size, SIZE_T BytesRemaining) { - UE_LOG(LogTemp, Verbose, TEXT("Message Received")); - - if (callbackRegistered) { - OnReceivedCB(Data); + // TODO: fix it frank, actually its not too terrible atm. + + if (BytesRemaining) + { + if (!bIsBuffering) + { + DataBuffer = new char[Size + BytesRemaining]; + memcpy(DataBuffer, Data, Size); + PrevSize = Size; + bIsBuffering = true; + } + else + { + memcpy(DataBuffer+PrevSize, Data, Size); + PrevSize += Size; + } + } + else + { + if (bIsBuffering) + { + memcpy(DataBuffer + PrevSize, Data, Size); + PrevSize = 0; + bIsBuffering = false; + OnReceivedCB(DataBuffer); + delete DataBuffer; + } + else + { + OnReceivedCB(Data); + } } } @@ -77,7 +110,6 @@ void UWebsocketClient::OnMessageSent() void UWebsocketClient::Send(const void* ptr, uint32_t size, bool isBinary) { - UE_LOG(LogTemp, Verbose, TEXT("Message Sending")); Socket->Send(ptr, size, isBinary); } diff --git a/Private/WebsocketClient.h b/Private/WebsocketClient.h index e0e8549..33ac7ee 100644 --- a/Private/WebsocketClient.h +++ b/Private/WebsocketClient.h @@ -36,6 +36,9 @@ class UWebsocketClient : public UObject UFUNCTION() void OnConnected(); + + UFUNCTION() + void OnClosed(); UFUNCTION() void OnConnectionError(); @@ -53,4 +56,8 @@ class UWebsocketClient : public UObject void IsCallbackRegistered(bool val); + // TODO: Make a buffer struct and clean up the raw pointer, optimization wise the current implementation should be close to fastest but can be organized a bit better and made safer. + char* DataBuffer; + int PrevSize; + bool bIsBuffering = false; }; diff --git a/Private/robofleet_client_lib b/Private/robofleet_client_lib index 8616ad3..5f74030 160000 --- a/Private/robofleet_client_lib +++ b/Private/robofleet_client_lib @@ -1 +1 @@ -Subproject commit 8616ad3412f5a8eb2e5f63d777e9c49a5ea3b51e +Subproject commit 5f74030966af053bb6fb897de61e877f18386422 diff --git a/Public/RobofleetBPFunctionLibrary.h b/Public/RobofleetBPFunctionLibrary.h index 2cb6ea6..767720a 100644 --- a/Public/RobofleetBPFunctionLibrary.h +++ b/Public/RobofleetBPFunctionLibrary.h @@ -41,6 +41,9 @@ class ROBOFLEETUNREALCLIENT_API URobofleetBPFunctionLibrary : public UBlueprintF UFUNCTION(BlueprintCallable, Category = "Robofleet") static TArray GetRobotImage(const FString& RobotName); + UFUNCTION(BlueprintCallable, Category = "Robofleet") + static bool IsRobotImageCompressed(const FString& RobotName); + UFUNCTION(BlueprintCallable, Category = "Robofleet") static void PrintRobotsSeen(); @@ -62,6 +65,9 @@ class ROBOFLEETUNREALCLIENT_API URobofleetBPFunctionLibrary : public UBlueprintF UFUNCTION(BlueprintCallable, Category = "Robofleet") static FVector GetDetectedPositionGlobal(const FString& RobotName); + UFUNCTION(BlueprintCallable, Category = "Robofleet") + static TArray GetDetectedImage(const FString& RobotName); + // Publish Messages to Robofleet UFUNCTION(BlueprintCallable, Category = "Robofleet") static void PublishStatusMsg(const FString& RobotName, const FRobotStatus& StatusMsg); diff --git a/RobofleetUnrealClient.Build.cs b/RobofleetUnrealClient.Build.cs index f7c94cb..995cf6b 100644 --- a/RobofleetUnrealClient.Build.cs +++ b/RobofleetUnrealClient.Build.cs @@ -19,7 +19,7 @@ public RobofleetUnrealClient(ReadOnlyTargetRules Target) : base(Target) { PCHUsage = PCHUsageMode.UseExplicitOrSharedPCHs; - PublicDependencyModuleNames.AddRange(new string[] {"Core", "CoreUObject", "Engine", "WebSockets" }); + PublicDependencyModuleNames.AddRange(new string[] {"Core", "CoreUObject", "Engine", "WebSockets", "ImageWrapper" }); PublicIncludePaths.Add(Path.Combine(ModuleDirectory, "Private/robofleet_client_lib/include")); PublicIncludePaths.Add(Path.Combine(ModuleDirectory, "Private/robofleet_client_lib")); PublicIncludePaths.Add(Path.Combine(ModuleDirectory, "Private"));