Skip to content

Commit c0914fb

Browse files
SanicSanic
authored
Merge pull request #180 from phuicy/master
Adding more BP topic types
2 parents e242c3e + ca8dd90 commit c0914fb

File tree

8 files changed

+337
-2
lines changed

8 files changed

+337
-2
lines changed

‎Source/ROSIntegration/Classes/RI/Topic.h

+44
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@
55
#include <CoreMinimal.h>
66
#include <UObject/ObjectMacros.h>
77
#include <UObject/Object.h>
8+
#include "Math/Vector.h"
89
#include "ROSBaseMsg.h"
910
#include "ROSIntegrationCore.h"
1011

@@ -19,6 +20,17 @@ enum class EMessageType : uint8
1920
{
2021
String = 0,
2122
Float32 = 1,
23+
Bool = 3,
24+
Float64 = 7,
25+
Header = 8,
26+
Int32 = 11,
27+
Int64 = 12,
28+
29+
Vector3 = 2,
30+
Point = 17,
31+
Pose = 16,
32+
Quaternion = 18,
33+
Twist = 19,
2234
};
2335

2436
UCLASS(Blueprintable)
@@ -56,12 +68,44 @@ class ROSINTEGRATION_API UTopic: public UObject
5668
UFUNCTION(BlueprintImplementableEvent, Category = ROS)
5769
void OnConstruct();
5870

71+
// Std msgs
72+
5973
UFUNCTION(BlueprintImplementableEvent, Category = ROS)
6074
void OnStringMessage(const FString& Data);
6175

6276
UFUNCTION(BlueprintImplementableEvent, Category = ROS)
6377
void OnFloat32Message(const float& Data);
6478

79+
UFUNCTION(BlueprintImplementableEvent, Category = ROS)
80+
void OnBoolMessage(const int& Data);
81+
82+
UFUNCTION(BlueprintImplementableEvent, Category = ROS)
83+
void OnFloat64Message(const double& Data);
84+
85+
UFUNCTION(BlueprintImplementableEvent, Category = ROS)
86+
void OnInt32Message(const int32& Data);
87+
88+
UFUNCTION(BlueprintImplementableEvent, Category = ROS)
89+
void OnInt64Message(const int64& Data);
90+
91+
92+
// Geometry msgs
93+
94+
UFUNCTION(BlueprintImplementableEvent, Category = ROS)
95+
void OnVector3Message(const FVector& Data);
96+
97+
UFUNCTION(BlueprintImplementableEvent, Category = ROS)
98+
void OnPointMessage(const FVector& Data);
99+
100+
UFUNCTION(BlueprintImplementableEvent, Category = ROS)
101+
void OnPoseMessage(const FVector& position, const FVector4& orientation);
102+
103+
UFUNCTION(BlueprintImplementableEvent, Category = ROS)
104+
void OnQuaternionMessage( const FVector4& quaternion);
105+
106+
UFUNCTION(BlueprintImplementableEvent, Category = ROS)
107+
void OnTwistMessage(const FVector& linear, const FVector& angular);
108+
65109
UPROPERTY()
66110
UROSIntegrationCore* _ROSIntegrationCore = nullptr;
67111

‎Source/ROSIntegration/Private/Conversion/Messages/std_msgs/StdMsgsEmptyConverter.cpp

+21
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
// Fill out your copyright notice in the Description page of Project Settings.
2+
3+
4+
#include "Conversion/Messages/std_msgs/StdMsgsEmptyConverter.h"
5+
6+
7+
UStdMsgsEmptyConverter::UStdMsgsEmptyConverter()
8+
{
9+
_MessageType = "std_msgs/Empty";
10+
}
11+
12+
bool UStdMsgsEmptyConverter::ConvertIncomingMessage(const ROSBridgePublishMsg* message, TSharedPtr<FROSBaseMsg> &BaseMsg) {
13+
14+
15+
return true;
16+
}
17+
18+
bool UStdMsgsEmptyConverter::ConvertOutgoingMessage(TSharedPtr<FROSBaseMsg> BaseMsg, bson_t** message) {
19+
20+
return true;
21+
}

‎Source/ROSIntegration/Private/Conversion/Messages/std_msgs/StdMsgsEmptyConverter.h

+21
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
// Fill out your copyright notice in the Description page of Project Settings.
2+
3+
#pragma once
4+
5+
#include "CoreMinimal.h"
6+
#include "Conversion/Messages/BaseMessageConverter.h"
7+
#include "std_msgs/Bool.h"
8+
#include "StdMsgsEmptyConverter.generated.h"
9+
10+
11+
UCLASS()
12+
class ROSINTEGRATION_API UStdMsgsEmptyConverter : public UBaseMessageConverter
13+
{
14+
GENERATED_BODY()
15+
16+
public:
17+
UStdMsgsEmptyConverter();
18+
virtual bool ConvertIncomingMessage(const ROSBridgePublishMsg* message, TSharedPtr<FROSBaseMsg> &BaseMsg);
19+
virtual bool ConvertOutgoingMessage(TSharedPtr<FROSBaseMsg> BaseMsg, bson_t** message);
20+
21+
};

‎Source/ROSIntegration/Private/Conversion/Messages/std_msgs/StdMsgsInt64Converter.cpp

+25
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
#include "Conversion/Messages/std_msgs/StdMsgsInt64Converter.h"
2+
3+
4+
UStdMsgsInt64Converter::UStdMsgsInt64Converter()
5+
{
6+
_MessageType = "std_msgs/Int64";
7+
}
8+
9+
bool UStdMsgsInt64Converter::ConvertIncomingMessage(const ROSBridgePublishMsg* message, TSharedPtr<FROSBaseMsg> &BaseMsg) {
10+
bool KeyFound = false;
11+
12+
int64 Data = GetInt64FromBSON(TEXT("msg.data"), message->full_msg_bson_, KeyFound);
13+
if (!KeyFound) return false;
14+
15+
BaseMsg = TSharedPtr<FROSBaseMsg>(new ROSMessages::std_msgs::Int64(Data));
16+
return true;
17+
}
18+
19+
bool UStdMsgsInt64Converter::ConvertOutgoingMessage(TSharedPtr<FROSBaseMsg> BaseMsg, bson_t** message) {
20+
auto Int64Message = StaticCastSharedPtr<ROSMessages::std_msgs::Int64>(BaseMsg);
21+
*message = BCON_NEW(
22+
"data", BCON_INT32(Int64Message->_Data)
23+
);
24+
return true;
25+
}

‎Source/ROSIntegration/Private/Conversion/Messages/std_msgs/StdMsgsInt64Converter.h

+18
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
#pragma once
2+
3+
#include "CoreMinimal.h"
4+
#include "Conversion/Messages/BaseMessageConverter.h"
5+
#include "std_msgs/Int64.h"
6+
#include "StdMsgsInt64Converter.generated.h"
7+
8+
9+
UCLASS()
10+
class ROSINTEGRATION_API UStdMsgsInt64Converter: public UBaseMessageConverter
11+
{
12+
GENERATED_BODY()
13+
14+
public:
15+
UStdMsgsInt64Converter();
16+
virtual bool ConvertIncomingMessage(const ROSBridgePublishMsg* message, TSharedPtr<FROSBaseMsg> &BaseMsg);
17+
virtual bool ConvertOutgoingMessage(TSharedPtr<FROSBaseMsg> BaseMsg, bson_t** message);
18+
};

‎Source/ROSIntegration/Private/Topic.cpp

+168-2
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,13 @@
44
#include "rosbridge2cpp/ros_topic.h"
55
#include "Conversion/Messages/BaseMessageConverter.h"
66
#include "Conversion/Messages/std_msgs/StdMsgsStringConverter.h"
7+
#include "Conversion/Messages/std_msgs/StdMsgsBoolConverter.h"
8+
#include "Conversion/Messages/geometry_msgs/GeometryMsgsVector3Converter.h"
9+
#include "Conversion/Messages/geometry_msgs/GeometryMsgsPointConverter.h"
10+
#include "Conversion/Messages/geometry_msgs/GeometryMsgsPoseConverter.h"
11+
12+
13+
714

815
static TMap<FString, UBaseMessageConverter*> TypeConverterMap;
916
static TMap<EMessageType, FString> SupportedMessageTypes;
@@ -174,8 +181,19 @@ UTopic::UTopic(const FObjectInitializer& ObjectInitializer)
174181

175182
if (SupportedMessageTypes.Num() == 0)
176183
{
177-
SupportedMessageTypes.Add(EMessageType::String, TEXT("std_msgs/String"));
178-
SupportedMessageTypes.Add(EMessageType::Float32, TEXT("std_msgs/Float32"));
184+
SupportedMessageTypes.Add(EMessageType::String, TEXT("std_msgs/String"));
185+
SupportedMessageTypes.Add(EMessageType::Float32, TEXT("std_msgs/Float32"));
186+
SupportedMessageTypes.Add(EMessageType::Bool, TEXT("std_msgs/Bool"));
187+
SupportedMessageTypes.Add(EMessageType::Float64, TEXT("std_msgs/Float64"));
188+
SupportedMessageTypes.Add(EMessageType::Int32, TEXT("std_msgs/Int32"));
189+
SupportedMessageTypes.Add(EMessageType::Int64, TEXT("std_msgs/Int64"));
190+
191+
SupportedMessageTypes.Add(EMessageType::Vector3, TEXT("geometry_msgs/Vector3"));
192+
SupportedMessageTypes.Add(EMessageType::Point, TEXT("geometry_msgs/Point"));
193+
SupportedMessageTypes.Add(EMessageType::Pose, TEXT("geometry_msgs/Pose"));
194+
SupportedMessageTypes.Add(EMessageType::Quaternion, TEXT("geometry_msgs/Quaternion"));
195+
SupportedMessageTypes.Add(EMessageType::Twist, TEXT("geometry_msgs/Twist"));
196+
179197
}
180198
}
181199

@@ -345,6 +363,154 @@ bool UTopic::Subscribe()
345363
}
346364
break;
347365
}
366+
case EMessageType::Bool:
367+
{
368+
auto ConcreteBoolMessage = StaticCastSharedPtr<ROSMessages::std_msgs::Bool>(msg);
369+
if (ConcreteBoolMessage.IsValid())
370+
{
371+
const bool Data = ConcreteBoolMessage->_Data;
372+
TWeakPtr<UTopic, ESPMode::ThreadSafe> SelfPtr(_SelfPtr);
373+
AsyncTask(ENamedThreads::GameThread, [this, Data, SelfPtr]()
374+
{
375+
if (!SelfPtr.IsValid()) return;
376+
OnBoolMessage(Data);
377+
});
378+
}
379+
break;
380+
}
381+
case EMessageType::Float64:
382+
{
383+
auto ConcreteDoubleMessage = StaticCastSharedPtr<ROSMessages::std_msgs::Float32>(msg);
384+
if (ConcreteDoubleMessage.IsValid())
385+
{
386+
const double Data = ConcreteDoubleMessage->_Data;
387+
TWeakPtr<UTopic, ESPMode::ThreadSafe> SelfPtr(_SelfPtr);
388+
AsyncTask(ENamedThreads::GameThread, [this, Data, SelfPtr]()
389+
{
390+
if (!SelfPtr.IsValid()) return;
391+
OnFloat64Message(Data);
392+
});
393+
}
394+
break;
395+
}
396+
case EMessageType::Int32:
397+
{
398+
auto ConcreteInt32Message = StaticCastSharedPtr<ROSMessages::std_msgs::Int32>(msg);
399+
if (ConcreteInt32Message.IsValid())
400+
{
401+
const int32 Data = ConcreteInt32Message->_Data;
402+
TWeakPtr<UTopic, ESPMode::ThreadSafe> SelfPtr(_SelfPtr);
403+
AsyncTask(ENamedThreads::GameThread, [this, Data, SelfPtr]()
404+
{
405+
if (!SelfPtr.IsValid()) return;
406+
OnInt32Message(Data);
407+
});
408+
}
409+
break;
410+
}
411+
case EMessageType::Int64:
412+
{
413+
auto ConcreteInt64Message = StaticCastSharedPtr<ROSMessages::std_msgs::Int64>(msg);
414+
if (ConcreteInt64Message.IsValid())
415+
{
416+
const int64 Data = ConcreteInt64Message->_Data;
417+
TWeakPtr<UTopic, ESPMode::ThreadSafe> SelfPtr(_SelfPtr);
418+
AsyncTask(ENamedThreads::GameThread, [this, Data, SelfPtr]()
419+
{
420+
if (!SelfPtr.IsValid()) return;
421+
OnInt64Message(Data);
422+
});
423+
}
424+
break;
425+
}
426+
case EMessageType::Vector3:
427+
{
428+
auto ConcreteVectorMessage = StaticCastSharedPtr<ROSMessages::geometry_msgs::Vector3>(msg);
429+
if (ConcreteVectorMessage.IsValid())
430+
{
431+
const float x = ConcreteVectorMessage->x;
432+
const float y = ConcreteVectorMessage->y;
433+
const float z = ConcreteVectorMessage->z;
434+
FVector Data(x,y,z);
435+
TWeakPtr<UTopic, ESPMode::ThreadSafe> SelfPtr(_SelfPtr);
436+
AsyncTask(ENamedThreads::GameThread, [this, Data, SelfPtr]()
437+
{
438+
if (!SelfPtr.IsValid()) return;
439+
OnVector3Message(Data);
440+
});
441+
}
442+
break;
443+
}
444+
case EMessageType::Point:
445+
{
446+
auto ConcreteVectorMessage = StaticCastSharedPtr<ROSMessages::geometry_msgs::Point>(msg);
447+
if (ConcreteVectorMessage.IsValid())
448+
{
449+
const float x = ConcreteVectorMessage->x;
450+
const float y = ConcreteVectorMessage->y;
451+
const float z = ConcreteVectorMessage->z;
452+
FVector Data(x, y, z);
453+
TWeakPtr<UTopic, ESPMode::ThreadSafe> SelfPtr(_SelfPtr);
454+
AsyncTask(ENamedThreads::GameThread, [this, Data, SelfPtr]()
455+
{
456+
if (!SelfPtr.IsValid()) return;
457+
OnPointMessage(Data);
458+
});
459+
}
460+
break;
461+
}
462+
case EMessageType::Pose:
463+
{
464+
auto ConcreteVectorMessage = StaticCastSharedPtr<ROSMessages::geometry_msgs::Pose>(msg);
465+
if (ConcreteVectorMessage.IsValid())
466+
{
467+
const auto p = ConcreteVectorMessage->position;
468+
const auto q = ConcreteVectorMessage->orientation;
469+
const FVector position(p.x, p.y, p.z);
470+
const FVector4 orientation(q.x,q.y,q.z,q.w);
471+
TWeakPtr<UTopic, ESPMode::ThreadSafe> SelfPtr(_SelfPtr);
472+
AsyncTask(ENamedThreads::GameThread, [this, position, orientation, SelfPtr]()
473+
{
474+
if (!SelfPtr.IsValid()) return;
475+
OnPoseMessage(position, orientation);
476+
});
477+
}
478+
break;
479+
}
480+
case EMessageType::Quaternion:
481+
{
482+
auto ConcreteVectorMessage = StaticCastSharedPtr<ROSMessages::geometry_msgs::Quaternion>(msg);
483+
if (ConcreteVectorMessage.IsValid())
484+
{
485+
const auto q = ConcreteVectorMessage;
486+
const FVector4 orientation(q->x, q->y, q->z, q->w);
487+
TWeakPtr<UTopic, ESPMode::ThreadSafe> SelfPtr(_SelfPtr);
488+
AsyncTask(ENamedThreads::GameThread, [this, orientation, SelfPtr]()
489+
{
490+
if (!SelfPtr.IsValid()) return;
491+
OnQuaternionMessage(orientation);
492+
});
493+
}
494+
break;
495+
}
496+
case EMessageType::Twist:
497+
{
498+
auto ConcreteVectorMessage = StaticCastSharedPtr<ROSMessages::geometry_msgs::Twist>(msg);
499+
if (ConcreteVectorMessage.IsValid())
500+
{
501+
const auto p = ConcreteVectorMessage->linear;
502+
const auto q = ConcreteVectorMessage->angular;
503+
const FVector linear(p.x, p.y, p.z);
504+
const FVector angular(q.x, q.y, q.z);
505+
TWeakPtr<UTopic, ESPMode::ThreadSafe> SelfPtr(_SelfPtr);
506+
AsyncTask(ENamedThreads::GameThread, [this, linear, angular, SelfPtr]()
507+
{
508+
if (!SelfPtr.IsValid()) return;
509+
OnTwistMessage(linear, angular);
510+
});
511+
}
512+
break;
513+
}
348514
default:
349515
unimplemented();
350516
break;

‎Source/ROSIntegration/Public/std_msgs/Empty.h

+20
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,20 @@
1+
#pragma once
2+
3+
#include "ROSBaseMsg.h"
4+
5+
namespace ROSMessages{
6+
namespace std_msgs {
7+
class String : public FROSBaseMsg {
8+
public:
9+
String() {
10+
_MessageType = "std_msgs/Empty";
11+
}
12+
13+
String(FString data) {
14+
_MessageType = "std_msgs/Empty";
15+
}
16+
17+
//private:
18+
};
19+
}
20+
}

‎Source/ROSIntegration/Public/std_msgs/Int64.h

+20
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,20 @@
1+
#pragma once
2+
3+
#include "ROSBaseMsg.h"
4+
5+
namespace ROSMessages{
6+
namespace std_msgs {
7+
class Int64 : public FROSBaseMsg {
8+
public:
9+
Int64() : Int64(0.f) {}
10+
11+
Int64(int64 data) {
12+
_MessageType = "std_msgs/Int64";
13+
_Data = data;
14+
}
15+
16+
//private:
17+
int64 _Data;
18+
};
19+
}
20+
}

0 commit comments

Comments
 (0)