Adds Robot Image Getter Functions

Private/RobofleetBPFunctionLibrary.cpp

@@ -74,6 +74,15 @@ TArray<uint8> URobofleetBPFunctionLibrary::GetRobotImage(const FString& RobotNam
 	return TArray<uint8>();
 }
 
+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<uint8> URobofleetBPFunctionLibrary::GetDetectedImage(const FString& RobotName)
+{
+	if (FRobofleetUnrealClientModule::Get()->IsSessionRunning())
+	{
+		return FRobofleetUnrealClientModule::Get()->RobofleetClient->GetDetectedImage(RobotName);
+	}
+	return TArray<uint8>();
+}
+
 // 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)

Private/RobofleetClientBase.cpp

@@ -1,5 +1,6 @@
 #include "RobofleetClientBase.h"
 #include "GameFramework/Actor.h"
+#include <typeinfo>
 
 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<fb::MsgWithMetadata>(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<Image>(Data);
+	//	OnImageReceived.Broadcast(RobotNamespace);
+	//}
+
 	else if (topic == "detected") {
 		DetectedItemMap[RobotNamespace] = DecodeMsg<DetectedItem>(Data);
 		OnDetectedItemReceived.Broadcast(RobotNamespace);
@@ -162,7 +176,6 @@ void URobofleetBase::DecodeMsg(const void* Data, FString topic, FString RobotNam
 	}
 }
 
-
 template <typename T>
 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<uint8> URobofleetBase::GetRobotImage(const FString& RobotName)
 {
-	//needs to return type that Texture expects
 	FString RobotNamestd = FString(TCHAR_TO_UTF8(*RobotName));
-	TArray<uint8> imageData;
-	imageData.Append(&DetectedItemMap[RobotNamestd].cmpr_image.data[0], DetectedItemMap[RobotNamestd].cmpr_image.data.size());
-	// you may want an TArray<FColor>
-	// FColor pixelColor = {0, &RobotImageMap[Name].data[i] : i+3}
-	return imageData;
+	//returns the constructor/ init list: TArray<type>(arrayPtr, arraySize)
+	UE_LOG(LogTemp, Log, TEXT("Creating local image message of type TArray<uint8>."));
+	return TArray<uint8>(&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<FString> URobofleetBase::GetAllRobotsAtSite(const FString& Location)
 {
 	TArray<FString> 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<uint8> URobofleetBase::GetDetectedImage(const FString& RobotName)
+{
+	FString RobotNamestd = FString(TCHAR_TO_UTF8(*RobotName));
+	//returns the constructor/ init list: TArray<type>(arrayPtr, arraySize)
+	UE_LOG(LogTemp, Log, TEXT("Creating local image message of type TArray<uint8>."));
+	return TArray<uint8>(&DetectedItemMap[RobotNamestd].cmpr_image.data[0], DetectedItemMap[RobotNamestd].cmpr_image.data.size());
+}

Private/RobofleetClientBase.h

@@ -99,6 +99,8 @@ class ROBOFLEETUNREALCLIENT_API URobofleetBase : public UObject
 
 	TArray<uint8> GetRobotImage(const FString& RobotName);
 
+	bool IsRobotImageCompressed(const FString& RobotName);
+
 	TArray<FString> 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<uint8> GetDetectedImage(const FString& RobotName);
+
 	bool IsRobotOk(const FString& RobotName);
 
 	void PrintRobotsSeen();

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,18 +56,50 @@ 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."))
 }
 
 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);
 }

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

Public/RobofleetBPFunctionLibrary.h

@@ -41,6 +41,9 @@ class ROBOFLEETUNREALCLIENT_API URobofleetBPFunctionLibrary : public UBlueprintF
 	UFUNCTION(BlueprintCallable, Category = "Robofleet")
 	static TArray<uint8> 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<uint8> GetDetectedImage(const FString& RobotName);
+
 	// Publish Messages to Robofleet
 	UFUNCTION(BlueprintCallable, Category = "Robofleet")
 	static void PublishStatusMsg(const FString& RobotName, const FRobotStatus& StatusMsg);

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"));