Skip to content

Fixes for UE5.6 #215

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open

Fixes for UE5.6 #215

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
0874562
Fixes for UE5.6
hyfan1116 Jun 26, 2025
f0f137d
Add missing _impl.h private header
hyfan1116 Jun 27, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
323 changes: 2 additions & 321 deletions Source/ROSIntegration/Private/ROSIntegrationCore.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
#include "ROSIntegrationCore.h"
#include "ROSIntegrationCore_Impl.h"
#include "ROSIntegrationGameInstance.h"
#include "rosbridge2cpp/TCPConnection.h"
#include "rosbridge2cpp/WebsocketConnection.h"
Expand All @@ -21,328 +22,8 @@ DEFINE_LOG_CATEGORY(LogROS);


// PIMPL
class UImpl::Impl
{
// hidden implementation details
TCPConnection* _TCPConnection = nullptr;
WebsocketConnection* _WebsocketConnection = nullptr;
rosbridge2cpp::ROSBridge* _Ros = nullptr;
public:
bool _bson_test_mode;

rosbridge2cpp::ROSBridge& GetBridge() { return *_Ros; }

UWorld* _World = nullptr;

//UPROPERTY() // this UPROPERTY is completely useless and its ignored by the metacompiler which works only on headers
USpawnManager* _SpawnManager = nullptr;


std::unique_ptr<rosbridge2cpp::ROSTopic> _SpawnArrayMessageListener;

private:
FString _ROSBridgeHost;
int32 _ROSBridgePort;

public:

void SpawnArrayMessageCallback(const ROSBridgePublishMsg& message)
{
if (!rosbridge2cpp::Helper::bson_has_key(*message.full_msg_bson_, "msg.markers")) {
UE_LOG(LogROS, Warning, TEXT("msg.markers field missing from SpawnArray Message"));
return;
}

bson_iter_t iter;
bson_iter_t val;

if (bson_iter_init(&iter, message.full_msg_bson_) && bson_iter_find_descendant(&iter, "msg.markers", &val) &&
BSON_ITER_HOLDS_ARRAY(&val)) {
UE_LOG(LogROS, Verbose, TEXT("Marker is included and is an array!"));
} else {
UE_LOG(LogROS, Verbose, TEXT("Marker is not included or isn't an array!"));
}

const char* key;
bson_iter_t child;
uint32_t array_len = 0;

bson_iter_recurse(&val, &child);
while (bson_iter_next(&child)) {
key = bson_iter_key(&child);
if (BSON_ITER_HOLDS_DOCUMENT(&child)) {
array_len++;
}
}

// Construct dot notation address to fetch data
for (uint32_t i = 0; i < array_len; ++i) {
double value;
int32 ivalue;
bool key_found;
SpawnObjectMessage Message;

const std::string marker_key_prefix("msg.markers.");
std::string LookupKey;
std::stringstream LookupKeyStream;

// TODO make this more generic or use other BSON library
// Use Templates instead of different functions?
// Make a Map that contains a tuple of (key, typ,e reference_to_variable_in_message) to automate everything?
LookupKeyStream << marker_key_prefix << i << ".pose.position.x";
LookupKey = LookupKeyStream.str();
value = rosbridge2cpp::Helper::get_double_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
UNREAL_ROS_CHECK_KEY_FOUND
Message._Pose._Position.X = value;

LookupKeyStream.str("");
LookupKeyStream.clear();
LookupKeyStream << marker_key_prefix << i << ".pose.position.y";
LookupKey = LookupKeyStream.str();
value = rosbridge2cpp::Helper::get_double_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
UNREAL_ROS_CHECK_KEY_FOUND
Message._Pose._Position.Y = value;

LookupKeyStream.str("");
LookupKeyStream.clear();
LookupKeyStream << marker_key_prefix << i << ".pose.position.z";
LookupKey = LookupKeyStream.str();
value = rosbridge2cpp::Helper::get_double_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
UNREAL_ROS_CHECK_KEY_FOUND
Message._Pose._Position.Z = value;

LookupKeyStream.str("");
LookupKeyStream.clear();
LookupKeyStream << marker_key_prefix << i << ".pose.orientation.x";
LookupKey = LookupKeyStream.str();
value = rosbridge2cpp::Helper::get_double_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
UNREAL_ROS_CHECK_KEY_FOUND
Message._Pose._Orientation.X = value;

LookupKeyStream.str("");
LookupKeyStream.clear();
LookupKeyStream << marker_key_prefix << i << ".pose.orientation.y";
LookupKey = LookupKeyStream.str();
value = rosbridge2cpp::Helper::get_double_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
UNREAL_ROS_CHECK_KEY_FOUND
Message._Pose._Orientation.Y = value;

LookupKeyStream.str("");
LookupKeyStream.clear();
LookupKeyStream << marker_key_prefix << i << ".pose.orientation.z";
LookupKey = LookupKeyStream.str();
value = rosbridge2cpp::Helper::get_double_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
UNREAL_ROS_CHECK_KEY_FOUND
Message._Pose._Orientation.Z = value;

LookupKeyStream.str("");
LookupKeyStream.clear();
LookupKeyStream << marker_key_prefix << i << ".pose.orientation.w";
LookupKey = LookupKeyStream.str();
value = rosbridge2cpp::Helper::get_double_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
UNREAL_ROS_CHECK_KEY_FOUND
Message._Pose._Orientation.W = value;

LookupKeyStream.str("");
LookupKeyStream.clear();
LookupKeyStream << marker_key_prefix << i << ".type";
LookupKey = LookupKeyStream.str();
ivalue = rosbridge2cpp::Helper::get_int32_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
UNREAL_ROS_CHECK_KEY_FOUND
Message._Type = ivalue;

LookupKeyStream.str("");
LookupKeyStream.clear();
LookupKeyStream << marker_key_prefix << i << ".id";
LookupKey = LookupKeyStream.str();
ivalue = rosbridge2cpp::Helper::get_int32_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
UNREAL_ROS_CHECK_KEY_FOUND
Message._Id = ivalue;

LookupKeyStream.str("");
LookupKeyStream.clear();
LookupKeyStream << marker_key_prefix << i << ".action";
LookupKey = LookupKeyStream.str();
ivalue = rosbridge2cpp::Helper::get_int32_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
UNREAL_ROS_CHECK_KEY_FOUND
Message._Action = ivalue;

LookupKeyStream.str("");
LookupKeyStream.clear();
LookupKeyStream << marker_key_prefix << i << ".scale.x";
LookupKey = LookupKeyStream.str();
value = rosbridge2cpp::Helper::get_double_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
UNREAL_ROS_CHECK_KEY_FOUND
Message._Scale.X = value;

LookupKeyStream.str("");
LookupKeyStream.clear();
LookupKeyStream << marker_key_prefix << i << ".scale.y";
LookupKey = LookupKeyStream.str();
value = rosbridge2cpp::Helper::get_double_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
UNREAL_ROS_CHECK_KEY_FOUND
Message._Scale.Y = value;

LookupKeyStream.str("");
LookupKeyStream.clear();
LookupKeyStream << marker_key_prefix << i << ".scale.z";
LookupKey = LookupKeyStream.str();
value = rosbridge2cpp::Helper::get_double_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
UNREAL_ROS_CHECK_KEY_FOUND
Message._Scale.Z = value;

// Color
double R, G, B, CAlpha;
LookupKeyStream.str("");
LookupKeyStream.clear();
LookupKeyStream << marker_key_prefix << i << ".color.r";
LookupKey = LookupKeyStream.str();
R = rosbridge2cpp::Helper::get_double_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
UNREAL_ROS_CHECK_KEY_FOUND

LookupKeyStream.str("");
LookupKeyStream.clear();
LookupKeyStream << marker_key_prefix << i << ".color.g";
LookupKey = LookupKeyStream.str();
G = rosbridge2cpp::Helper::get_double_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
UNREAL_ROS_CHECK_KEY_FOUND

LookupKeyStream.str("");
LookupKeyStream.clear();
LookupKeyStream << marker_key_prefix << i << ".color.b";
LookupKey = LookupKeyStream.str();
B = rosbridge2cpp::Helper::get_double_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
UNREAL_ROS_CHECK_KEY_FOUND

LookupKeyStream.str("");
LookupKeyStream.clear();
LookupKeyStream << marker_key_prefix << i << ".color.a";
LookupKey = LookupKeyStream.str();
CAlpha = rosbridge2cpp::Helper::get_double_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
UNREAL_ROS_CHECK_KEY_FOUND

Message._Color = FLinearColor(R, G, B, CAlpha);

LookupKeyStream.str("");
LookupKeyStream.clear();
LookupKeyStream << marker_key_prefix << i << ".text";
LookupKey = LookupKeyStream.str();
std::string MsgText =
rosbridge2cpp::Helper::get_utf8_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
UNREAL_ROS_CHECK_KEY_FOUND
Message._Text = FString(MsgText.c_str());

LookupKeyStream.str("");
LookupKeyStream.clear();
LookupKeyStream << marker_key_prefix << i << ".mesh_resource";
LookupKey = LookupKeyStream.str();
std::string MeshResource =
rosbridge2cpp::Helper::get_utf8_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
UNREAL_ROS_CHECK_KEY_FOUND
Message._MeshResource = FString(MeshResource.c_str());

UE_LOG(LogROS, Verbose, TEXT("Enqueue Message"));
_SpawnManager->_SpawnObjectMessageQueue.Enqueue(Message);
UE_LOG(LogROS, Verbose, TEXT("Enqueue Message Done"));
}
}

std::unique_ptr<rosbridge2cpp::ROSTopic> _SpawnMessageListener;

void SpawnMessageCallback(const ROSBridgePublishMsg& message)
{
UE_LOG(LogROS, Warning, TEXT("RECEIVED SPAWN MESSAGE --- Not implemented yet. Use the SpawnArray topic instead"));
}

Impl()
{

}

~Impl()
{
UE_LOG(LogROS, Display, TEXT("UROSIntegrationCore ~Impl() "));
//_World = nullptr;
_SpawnManager = nullptr;
if(_Ros) delete _Ros;
if(_WebsocketConnection) delete _WebsocketConnection;
if (_TCPConnection) delete _TCPConnection;
}

bool IsHealthy() const
{
return ((_TCPConnection != nullptr && _TCPConnection->IsHealthy()) || (_WebsocketConnection != nullptr && _WebsocketConnection->IsHealthy())) && _Ros != nullptr && _Ros->IsHealthy();
}

void SetWorld(UWorld* World)
{
_World = World;
}

void SetImplSpawnManager(USpawnManager* SpawnManager)
{
_SpawnManager = SpawnManager;
}

bool Init(FString protocol, FString ROSBridgeHost, int32 ROSBridgePort, bool bson_test_mode)
{
_bson_test_mode = bson_test_mode;

if (protocol == "ws") {
_WebsocketConnection = new WebsocketConnection();
_Ros = new rosbridge2cpp::ROSBridge(*_WebsocketConnection);
} else if (protocol == "tcp") {
_TCPConnection = new TCPConnection();
_Ros = new rosbridge2cpp::ROSBridge(*_TCPConnection);
} else {
UE_LOG(LogROS, Error, TEXT("Protocol not supported"));
}

if (bson_test_mode) {
_Ros->enable_bson_mode();
}
_ROSBridgeHost = ROSBridgeHost;
_ROSBridgePort = ROSBridgePort;

bool ConnectionSuccessful = _Ros->Init(TCHAR_TO_UTF8(*ROSBridgeHost), ROSBridgePort);
if (!ConnectionSuccessful) {
return false;
}

UE_LOG(LogROS, Log, TEXT("rosbridge2cpp init successful"));

return true;
}

FString GetROSBridgeHost() const
{
return _ROSBridgeHost;
}

int32 GetROSBridgePort() const
{
return _ROSBridgePort;
}

void InitSpawnManager()
{
// Listen to the object spawning thread
_SpawnMessageListener = std::unique_ptr<rosbridge2cpp::ROSTopic>(
new rosbridge2cpp::ROSTopic(GetBridge(), "/unreal_ros/spawn_objects", "visualization_msgs/Marker"));
_SpawnMessageListener->Subscribe(std::bind(&UImpl::Impl::SpawnMessageCallback, this, std::placeholders::_1));

_SpawnArrayMessageListener = std::unique_ptr<rosbridge2cpp::ROSTopic>(
new rosbridge2cpp::ROSTopic(GetBridge(), "/unreal_ros/spawn_objects_array", "visualization_msgs/MarkerArray"));
_SpawnArrayMessageListener->Subscribe(
std::bind(&UImpl::Impl::SpawnArrayMessageCallback, this, std::placeholders::_1));

//_SpawnManager = NewObject<USpawnManager>();
_SpawnManager->_World = _World;
_SpawnManager->_TickingActive = true;
}
};


// Moved to ROSIntegrationCore_Impl.h

// - - - - - -

Expand Down
Loading