Skip to content

Commit f0f137d

Browse files
hyfan1116hyfan1116
committed
Add missing _impl.h private header
1 parent 0874562 commit f0f137d

File tree

1 file changed

+343
-0
lines changed

1 file changed

+343
-0
lines changed

‎Source/ROSIntegration/Private/ROSIntegrationCore_Impl.h‎

Lines changed: 343 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,343 @@
1+
#pragma once
2+
3+
#include "ROSIntegrationCore.h"
4+
#include "ROSIntegrationGameInstance.h"
5+
#include "rosbridge2cpp/TCPConnection.h"
6+
#include "rosbridge2cpp/WebsocketConnection.h"
7+
#include "rosbridge2cpp/ros_bridge.h"
8+
#include "rosbridge2cpp/ros_topic.h"
9+
10+
#include "SpawnManager.h"
11+
#include "SpawnObjectMessage.h"
12+
13+
14+
#include <sstream>
15+
16+
#define UNREAL_ROS_CHECK_KEY_FOUND \
17+
if (!key_found) {\
18+
UE_LOG(LogROS, Warning, TEXT("%s is not present in data"), *FString(UTF8_TO_TCHAR(LookupKey.c_str())));\
19+
return;\
20+
}
21+
22+
23+
// PIMPL
24+
class UImpl::Impl
25+
{
26+
// hidden implementation details
27+
TCPConnection* _TCPConnection = nullptr;
28+
WebsocketConnection* _WebsocketConnection = nullptr;
29+
rosbridge2cpp::ROSBridge* _Ros = nullptr;
30+
public:
31+
bool _bson_test_mode;
32+
33+
rosbridge2cpp::ROSBridge& GetBridge() { return *_Ros; }
34+
35+
UWorld* _World = nullptr;
36+
37+
//UPROPERTY() // this UPROPERTY is completely useless and its ignored by the metacompiler which works only on headers
38+
USpawnManager* _SpawnManager = nullptr;
39+
40+
41+
std::unique_ptr<rosbridge2cpp::ROSTopic> _SpawnArrayMessageListener;
42+
43+
private:
44+
FString _ROSBridgeHost;
45+
int32 _ROSBridgePort;
46+
47+
public:
48+
49+
void SpawnArrayMessageCallback(const ROSBridgePublishMsg& message)
50+
{
51+
if (!rosbridge2cpp::Helper::bson_has_key(*message.full_msg_bson_, "msg.markers")) {
52+
UE_LOG(LogROS, Warning, TEXT("msg.markers field missing from SpawnArray Message"));
53+
return;
54+
}
55+
56+
bson_iter_t iter;
57+
bson_iter_t val;
58+
59+
if (bson_iter_init(&iter, message.full_msg_bson_) && bson_iter_find_descendant(&iter, "msg.markers", &val) &&
60+
BSON_ITER_HOLDS_ARRAY(&val)) {
61+
UE_LOG(LogROS, Verbose, TEXT("Marker is included and is an array!"));
62+
} else {
63+
UE_LOG(LogROS, Verbose, TEXT("Marker is not included or isn't an array!"));
64+
}
65+
66+
const char* key;
67+
bson_iter_t child;
68+
uint32_t array_len = 0;
69+
70+
bson_iter_recurse(&val, &child);
71+
while (bson_iter_next(&child)) {
72+
key = bson_iter_key(&child);
73+
if (BSON_ITER_HOLDS_DOCUMENT(&child)) {
74+
array_len++;
75+
}
76+
}
77+
78+
// Construct dot notation address to fetch data
79+
for (uint32_t i = 0; i < array_len; ++i) {
80+
double value;
81+
int32 ivalue;
82+
bool key_found;
83+
SpawnObjectMessage Message;
84+
85+
const std::string marker_key_prefix("msg.markers.");
86+
std::string LookupKey;
87+
std::stringstream LookupKeyStream;
88+
89+
// TODO make this more generic or use other BSON library
90+
// Use Templates instead of different functions?
91+
// Make a Map that contains a tuple of (key, typ,e reference_to_variable_in_message) to automate everything?
92+
LookupKeyStream << marker_key_prefix << i << ".pose.position.x";
93+
LookupKey = LookupKeyStream.str();
94+
value = rosbridge2cpp::Helper::get_double_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
95+
UNREAL_ROS_CHECK_KEY_FOUND
96+
Message._Pose._Position.X = value;
97+
98+
LookupKeyStream.str("");
99+
LookupKeyStream.clear();
100+
LookupKeyStream << marker_key_prefix << i << ".pose.position.y";
101+
LookupKey = LookupKeyStream.str();
102+
value = rosbridge2cpp::Helper::get_double_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
103+
UNREAL_ROS_CHECK_KEY_FOUND
104+
Message._Pose._Position.Y = value;
105+
106+
LookupKeyStream.str("");
107+
LookupKeyStream.clear();
108+
LookupKeyStream << marker_key_prefix << i << ".pose.position.z";
109+
LookupKey = LookupKeyStream.str();
110+
value = rosbridge2cpp::Helper::get_double_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
111+
UNREAL_ROS_CHECK_KEY_FOUND
112+
Message._Pose._Position.Z = value;
113+
114+
LookupKeyStream.str("");
115+
LookupKeyStream.clear();
116+
LookupKeyStream << marker_key_prefix << i << ".pose.orientation.x";
117+
LookupKey = LookupKeyStream.str();
118+
value = rosbridge2cpp::Helper::get_double_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
119+
UNREAL_ROS_CHECK_KEY_FOUND
120+
Message._Pose._Orientation.X = value;
121+
122+
LookupKeyStream.str("");
123+
LookupKeyStream.clear();
124+
LookupKeyStream << marker_key_prefix << i << ".pose.orientation.y";
125+
LookupKey = LookupKeyStream.str();
126+
value = rosbridge2cpp::Helper::get_double_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
127+
UNREAL_ROS_CHECK_KEY_FOUND
128+
Message._Pose._Orientation.Y = value;
129+
130+
LookupKeyStream.str("");
131+
LookupKeyStream.clear();
132+
LookupKeyStream << marker_key_prefix << i << ".pose.orientation.z";
133+
LookupKey = LookupKeyStream.str();
134+
value = rosbridge2cpp::Helper::get_double_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
135+
UNREAL_ROS_CHECK_KEY_FOUND
136+
Message._Pose._Orientation.Z = value;
137+
138+
LookupKeyStream.str("");
139+
LookupKeyStream.clear();
140+
LookupKeyStream << marker_key_prefix << i << ".pose.orientation.w";
141+
LookupKey = LookupKeyStream.str();
142+
value = rosbridge2cpp::Helper::get_double_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
143+
UNREAL_ROS_CHECK_KEY_FOUND
144+
Message._Pose._Orientation.W = value;
145+
146+
LookupKeyStream.str("");
147+
LookupKeyStream.clear();
148+
LookupKeyStream << marker_key_prefix << i << ".type";
149+
LookupKey = LookupKeyStream.str();
150+
ivalue = rosbridge2cpp::Helper::get_int32_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
151+
UNREAL_ROS_CHECK_KEY_FOUND
152+
Message._Type = ivalue;
153+
154+
LookupKeyStream.str("");
155+
LookupKeyStream.clear();
156+
LookupKeyStream << marker_key_prefix << i << ".id";
157+
LookupKey = LookupKeyStream.str();
158+
ivalue = rosbridge2cpp::Helper::get_int32_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
159+
UNREAL_ROS_CHECK_KEY_FOUND
160+
Message._Id = ivalue;
161+
162+
LookupKeyStream.str("");
163+
LookupKeyStream.clear();
164+
LookupKeyStream << marker_key_prefix << i << ".action";
165+
LookupKey = LookupKeyStream.str();
166+
ivalue = rosbridge2cpp::Helper::get_int32_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
167+
UNREAL_ROS_CHECK_KEY_FOUND
168+
Message._Action = ivalue;
169+
170+
LookupKeyStream.str("");
171+
LookupKeyStream.clear();
172+
LookupKeyStream << marker_key_prefix << i << ".scale.x";
173+
LookupKey = LookupKeyStream.str();
174+
value = rosbridge2cpp::Helper::get_double_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
175+
UNREAL_ROS_CHECK_KEY_FOUND
176+
Message._Scale.X = value;
177+
178+
LookupKeyStream.str("");
179+
LookupKeyStream.clear();
180+
LookupKeyStream << marker_key_prefix << i << ".scale.y";
181+
LookupKey = LookupKeyStream.str();
182+
value = rosbridge2cpp::Helper::get_double_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
183+
UNREAL_ROS_CHECK_KEY_FOUND
184+
Message._Scale.Y = value;
185+
186+
LookupKeyStream.str("");
187+
LookupKeyStream.clear();
188+
LookupKeyStream << marker_key_prefix << i << ".scale.z";
189+
LookupKey = LookupKeyStream.str();
190+
value = rosbridge2cpp::Helper::get_double_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
191+
UNREAL_ROS_CHECK_KEY_FOUND
192+
Message._Scale.Z = value;
193+
194+
// Color
195+
double R, G, B, CAlpha;
196+
LookupKeyStream.str("");
197+
LookupKeyStream.clear();
198+
LookupKeyStream << marker_key_prefix << i << ".color.r";
199+
LookupKey = LookupKeyStream.str();
200+
R = rosbridge2cpp::Helper::get_double_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
201+
UNREAL_ROS_CHECK_KEY_FOUND
202+
203+
LookupKeyStream.str("");
204+
LookupKeyStream.clear();
205+
LookupKeyStream << marker_key_prefix << i << ".color.g";
206+
LookupKey = LookupKeyStream.str();
207+
G = rosbridge2cpp::Helper::get_double_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
208+
UNREAL_ROS_CHECK_KEY_FOUND
209+
210+
LookupKeyStream.str("");
211+
LookupKeyStream.clear();
212+
LookupKeyStream << marker_key_prefix << i << ".color.b";
213+
LookupKey = LookupKeyStream.str();
214+
B = rosbridge2cpp::Helper::get_double_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
215+
UNREAL_ROS_CHECK_KEY_FOUND
216+
217+
LookupKeyStream.str("");
218+
LookupKeyStream.clear();
219+
LookupKeyStream << marker_key_prefix << i << ".color.a";
220+
LookupKey = LookupKeyStream.str();
221+
CAlpha = rosbridge2cpp::Helper::get_double_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
222+
UNREAL_ROS_CHECK_KEY_FOUND
223+
224+
Message._Color = FLinearColor(R, G, B, CAlpha);
225+
226+
LookupKeyStream.str("");
227+
LookupKeyStream.clear();
228+
LookupKeyStream << marker_key_prefix << i << ".text";
229+
LookupKey = LookupKeyStream.str();
230+
std::string MsgText =
231+
rosbridge2cpp::Helper::get_utf8_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
232+
UNREAL_ROS_CHECK_KEY_FOUND
233+
Message._Text = FString(MsgText.c_str());
234+
235+
LookupKeyStream.str("");
236+
LookupKeyStream.clear();
237+
LookupKeyStream << marker_key_prefix << i << ".mesh_resource";
238+
LookupKey = LookupKeyStream.str();
239+
std::string MeshResource =
240+
rosbridge2cpp::Helper::get_utf8_by_key(LookupKey.c_str(), *message.full_msg_bson_, key_found);
241+
UNREAL_ROS_CHECK_KEY_FOUND
242+
Message._MeshResource = FString(MeshResource.c_str());
243+
244+
UE_LOG(LogROS, Verbose, TEXT("Enqueue Message"));
245+
_SpawnManager->_SpawnObjectMessageQueue.Enqueue(Message);
246+
UE_LOG(LogROS, Verbose, TEXT("Enqueue Message Done"));
247+
}
248+
}
249+
250+
std::unique_ptr<rosbridge2cpp::ROSTopic> _SpawnMessageListener;
251+
252+
void SpawnMessageCallback(const ROSBridgePublishMsg& message)
253+
{
254+
UE_LOG(LogROS, Warning, TEXT("RECEIVED SPAWN MESSAGE --- Not implemented yet. Use the SpawnArray topic instead"));
255+
}
256+
257+
Impl()
258+
{
259+
260+
}
261+
262+
~Impl()
263+
{
264+
UE_LOG(LogROS, Display, TEXT("UROSIntegrationCore ~Impl() "));
265+
//_World = nullptr;
266+
_SpawnManager = nullptr;
267+
if(_Ros) delete _Ros;
268+
if(_WebsocketConnection) delete _WebsocketConnection;
269+
if (_TCPConnection) delete _TCPConnection;
270+
}
271+
272+
bool IsHealthy() const
273+
{
274+
return ((_TCPConnection != nullptr && _TCPConnection->IsHealthy()) || (_WebsocketConnection != nullptr && _WebsocketConnection->IsHealthy())) && _Ros != nullptr && _Ros->IsHealthy();
275+
}
276+
277+
void SetWorld(UWorld* World)
278+
{
279+
_World = World;
280+
}
281+
282+
void SetImplSpawnManager(USpawnManager* SpawnManager)
283+
{
284+
_SpawnManager = SpawnManager;
285+
}
286+
287+
bool Init(FString protocol, FString ROSBridgeHost, int32 ROSBridgePort, bool bson_test_mode)
288+
{
289+
_bson_test_mode = bson_test_mode;
290+
291+
if (protocol == "ws") {
292+
_WebsocketConnection = new WebsocketConnection();
293+
_Ros = new rosbridge2cpp::ROSBridge(*_WebsocketConnection);
294+
} else if (protocol == "tcp") {
295+
_TCPConnection = new TCPConnection();
296+
_Ros = new rosbridge2cpp::ROSBridge(*_TCPConnection);
297+
} else {
298+
UE_LOG(LogROS, Error, TEXT("Protocol not supported"));
299+
}
300+
301+
if (bson_test_mode) {
302+
_Ros->enable_bson_mode();
303+
}
304+
_ROSBridgeHost = ROSBridgeHost;
305+
_ROSBridgePort = ROSBridgePort;
306+
307+
bool ConnectionSuccessful = _Ros->Init(TCHAR_TO_UTF8(*ROSBridgeHost), ROSBridgePort);
308+
if (!ConnectionSuccessful) {
309+
return false;
310+
}
311+
312+
UE_LOG(LogROS, Log, TEXT("rosbridge2cpp init successful"));
313+
314+
return true;
315+
}
316+
317+
FString GetROSBridgeHost() const
318+
{
319+
return _ROSBridgeHost;
320+
}
321+
322+
int32 GetROSBridgePort() const
323+
{
324+
return _ROSBridgePort;
325+
}
326+
327+
void InitSpawnManager()
328+
{
329+
// Listen to the object spawning thread
330+
_SpawnMessageListener = std::unique_ptr<rosbridge2cpp::ROSTopic>(
331+
new rosbridge2cpp::ROSTopic(GetBridge(), "/unreal_ros/spawn_objects", "visualization_msgs/Marker"));
332+
_SpawnMessageListener->Subscribe(std::bind(&UImpl::Impl::SpawnMessageCallback, this, std::placeholders::_1));
333+
334+
_SpawnArrayMessageListener = std::unique_ptr<rosbridge2cpp::ROSTopic>(
335+
new rosbridge2cpp::ROSTopic(GetBridge(), "/unreal_ros/spawn_objects_array", "visualization_msgs/MarkerArray"));
336+
_SpawnArrayMessageListener->Subscribe(
337+
std::bind(&UImpl::Impl::SpawnArrayMessageCallback, this, std::placeholders::_1));
338+
339+
//_SpawnManager = NewObject<USpawnManager>();
340+
_SpawnManager->_World = _World;
341+
_SpawnManager->_TickingActive = true;
342+
}
343+
};

0 commit comments

Comments
 (0)