Merge pull request #167 from tsender/ros1

Cleaned up converters, added improvements, and more

Author: Sanic

README.md

@@ -38,16 +38,31 @@ It uses http://mongoc.org/libbson/ to encode and decode the whole ROS communicat
 Since BSON is not included in Unreal Engine (yet), its code has to be added to this plugin.
 Currently, this plugin comes with a pre-compiled libbson for Windows x64 and Linux x64 which doesn't need any additional configuration.
 
-To enable the communcation between Unreal and ROS, you will need a running ROSBridge (https://github.com/RobotWebTools/rosbridge_suite) with bson_mode. As an example for ROS kinetic, you can install it with:
+To enable the communcation between Unreal and ROS, you will need a running ROSBridge (https://github.com/RobotWebTools/rosbridge_suite) with bson_mode. Note: Please use rosbridge with version=>0.8.0 to get full BSON support.
+
+The recommended way to install rosbridge is from source (requires the `rosauth` package):
+```
+sudo apt-get install ros-ROS1_DISTRO-rosauth # Replace ROS1_DISTRO with the distro you are using
+cd ~/ros_workspace/
+source devel/setup.bash
+cd src/
+git clone -b ros1 https://github.com/RobotWebTools/rosbridge_suite.git
+cd ..
+catkin_make
+source devel/setup.bash
+```
+Even though you could install rosbridge using apt via:
 ```
-sudo apt-get install ros-kinetic-rosbridge-suite
+sudo apt-get install ros-ROS1_DISTRO-rosbridge-suite # Replace ROS1_DISTRO with the distro you are using
 ```
-Note: Please use rosbridge with version=>0.8.0 to get full BSON support. After installing rosbridge, you can enable the bson_mode like this:
+there have been numersous issues where these apt packages do not reflect the code in the `ros1` branch. Hence, it is best to install from source. After installing rosbridge, you can enable the bson_mode like this:
 
 ```
 roslaunch rosbridge_server rosbridge_tcp.launch bson_only_mode:=True
 ```
-**Important**: The most recent rosbridge versions changed the transmission method. Since ROSIntegration can not handle this new method yet, please make sure to check out these issue on how to install a compatible rosbridge version: https://github.com/code-iai/ROSIntegration/issues/141 & https://github.com/code-iai/ROSIntegration/issues/139
+**How to verify the rosbridge-UE4 connection:** Make sure UE4 is configured to use the ROSIntegrationGameInstance (see below) and set the connection parameters. If UE4 and rosbridge are both running, then you should see the rosbridge node subscribe to two topics with the prefix `/unreal_ros/`. If you do NOT see this, then you likely have a problem with your rosbridge installation.
+
+<!-- **Important**: The most recent rosbridge versions changed the transmission method. Since ROSIntegration can not handle this new method yet, please make sure to check out these issue on how to install a compatible rosbridge version: https://github.com/code-iai/ROSIntegration/issues/141 & https://github.com/code-iai/ROSIntegration/issues/139 -->
 
 In our testing, we usually installed rosbridge on a Ubuntu Linux with ROS while the UE4 with ROSIntegration can be run on a Windows or Linux hosts. ROSBridge and UE4 with ROSIntegration don't need to be run on the same machine. So in order to run UE4 with ROSIntegration on Windows, you can either install a Linux Virtual Machine on your Windows Hosts or have a seperate, physical machine with Linux running in your network.
 
@@ -60,6 +75,8 @@ This plugin has previously been tested with Unreal Engine versions;
  * 4.20.3
  * **4.23** (by [Luigi Freda](http://www.luigifreda.com), fixed bugs with smart pointer management)
  * 4.24 
+ * 4.25
+ * 4.26
 
 Please note that this list is a tracker of which UE4 versions have been previously tested. It is not guaranteed that the most recent version of ROSIntegration is working with all previous UE4 versions.
 
@@ -202,6 +219,7 @@ geometry_msgs/Quaternion           | ✓          | ✓
 geometry_msgs/Transform            | ✓          | ✓
 geometry_msgs/TransformStamped     | ✓          | ✓
 geometry_msgs/Twist                | ✓          | ✓
+geometry_msgs/TwistStamped         | ✓          | ✓
 geometry_msgs/TwistWithCovariance  | ✓          | ✓
 geometry_msgs/Vector3              | ✓          | ✓
 grid_map_msgs/GridMap              | ✓          | ✓
@@ -247,6 +265,11 @@ PrivateIncludePaths.AddRange(
 
 Then you can create the message definition and the converter in your own projects source tree. You can just copy and paste the files of a similar standard message, but don't forget to replace the `ROSINTEGRATION_API` with your own API macro created by Unreal.
 
+### Overriding the rosbridge Connection Settings from Within the Level
+You can now add a `ROSBridgeParamOverride` actor to a level, allowing you to use different rosbridge connection settings for that level only (compared to what is defined in the ROSIntegrationGameInstance settings).
+
+When might this be useful? Say you have a powerful enough computer that can handle multiple independent UE4 simulations at the same time, and you want to run as many simulations as possible to collect data effeciently (perhaps you are training a reinforcement learning algorithm). If you create copies of your main level (saved with different names) and use different ROS topic names within each level, then you can launch each level in a new UE4 editor on the same computer. If using a single rosbridge node for all of these UE4 instances results in considerable delays, then you can then add the `ROSBridgeParamOverride` actor to each level so that each level uses its own rosbridge node.
+
 ### FAQ
 * Question: My Topic/Service gets closed/unadvertised or my UE4 crashes around one minute after Begin Play.
 Answer: This might be a problem relating to the Garbage Collection of UE4. Please make sure that you declare your class member attributes as UPROPERTYs. See also: https://github.com/code-iai/ROSIntegration/issues/32

Source/ROSIntegration/Classes/ROSBridgeParamOverride.h

@@ -0,0 +1,45 @@
+#pragma once
+
+#include "CoreMinimal.h"
+#include "GameFramework/Actor.h"
+#include "ROSBridgeParamOverride.generated.h"
+
+/**
+ * If you place this actor in the world, then you may override the parameters set in the ROSIntegrationGameInstance with the ones defined here.
+ * The purpose of this actor is to allow you to launch multiple UE editors on the same computer (each with a different level open) and use different rosbridge connection parameters in each level.
+ * This can be especially useful if you need to run independent parallel simulations on a single computer, each using a separate rosbridge node to distribute the load.
+ */
+UCLASS()
+class ROSINTEGRATION_API AROSBridgeParamOverride : public AActor
+{
+	GENERATED_BODY()
+	
+public:	
+	AROSBridgeParamOverride()
+    {
+        PrimaryActorTick.bCanEverTick = false; // No need to tick
+    }
+
+    // IP address of the rosbridge websocket server
+	UPROPERTY(EditAnywhere, BlueprintReadOnly, Category = "ROS")
+	FString ROSBridgeServerHost = "127.0.0.1";
+
+	// Port number to access the rosbridge websocket server
+	UPROPERTY(EditAnywhere, BlueprintReadOnly, Category = "ROS")
+	int32 ROSBridgeServerPort = 9090;
+
+	UPROPERTY(EditAnywhere, BlueprintReadOnly, Category = "ROS")
+	bool bConnectToROS = true;
+
+	UPROPERTY(EditAnywhere, Category = "ROS")
+	bool bSimulateTime = true;
+
+	UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROS")
+	bool bUseFixedUpdateInterval = false;
+
+	UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROS")
+	float FixedUpdateInterval = 0.01666666667;
+
+	UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROS")
+	bool bCheckHealth = true;
+};

Source/ROSIntegration/Private/Conversion/Messages/BaseMessageConverter.cpp

@@ -1,8 +1,7 @@
 #include "Conversion/Messages/BaseMessageConverter.h"
 
 
-UBaseMessageConverter::UBaseMessageConverter(const FObjectInitializer& ObjectInitializer)
-: Super(ObjectInitializer)
+UBaseMessageConverter::UBaseMessageConverter()
 {
 }
 

Source/ROSIntegration/Private/Conversion/Messages/BaseMessageConverter.h

@@ -1,28 +1,29 @@
 #pragma once
 
-#include <CoreMinimal.h>
+#include "CoreMinimal.h"
 
 #if PLATFORM_WINDOWS
 #include "Windows/WindowsHWrapper.h"
 #endif // PLATFORM_WINDOWS
 
 #include "ROSIntegrationCore.h"
-#include <UObject/ObjectMacros.h>
-#include <UObject/Object.h>
 #include "rosbridge2cpp/messages/rosbridge_publish_msg.h"
 #include <cstring>
 #include <functional>
 #include <bson.h>
 #include "std_msgs/Header.h"
+#include "ROSTime.h"
 
 #include "BaseMessageConverter.generated.h"
 
 UCLASS()
 class ROSINTEGRATION_API UBaseMessageConverter: public UObject
 {
-	GENERATED_UCLASS_BODY()
+	GENERATED_BODY()
 
 public:
+	UBaseMessageConverter();
+	
 	UPROPERTY()
 	FString _MessageType;
 
@@ -64,13 +65,35 @@ class ROSINTEGRATION_API UBaseMessageConverter: public UObject
 		return value;
 	}
 
+	static int64 GetInt64FromBSON(FString Key, bson_t* msg, bool &KeyFound, bool LogOnErrors = true)
+	{
+		assert(msg != nullptr);
+
+		int64 value = rosbridge2cpp::Helper::get_int64_by_key(TCHAR_TO_UTF8(*Key), *msg, KeyFound);
+		if (!KeyFound && LogOnErrors) {
+			UE_LOG(LogROS, Error, TEXT("Key %s not present in data"), *Key);
+		}
+		return value;
+	}
+
 	static bool GetBoolFromBSON(FString Key, bson_t* msg, bool &KeyFound, bool LogOnErrors = true)
 	{
 		assert(msg != nullptr);
 
 		bool value = rosbridge2cpp::Helper::get_bool_by_key(TCHAR_TO_UTF8(*Key), *msg, KeyFound);
 		if (!KeyFound && LogOnErrors) {
-			UE_LOG(LogTemp, Error, TEXT("Key %s not present in data"), *Key);
+			UE_LOG(LogROS, Error, TEXT("Key %s not present in data"), *Key);
+		}
+		return value;
+	}
+
+	static const uint8* GetBinaryFromBSON(FString Key, bson_t* msg, bool &KeyFound, bool LogOnErrors = true)
+	{
+		assert(msg != nullptr);
+		uint32_t binary_data_length = 0; // Gets populated by get_binary_by_key()
+		const uint8* value = rosbridge2cpp::Helper::get_binary_by_key(TCHAR_TO_UTF8(*Key), *msg, binary_data_length, KeyFound);
+		if (!KeyFound && LogOnErrors) {
+			UE_LOG(LogROS, Error, TEXT("Key %s not present in data"), *Key);
 		}
 		return value;
 	}
@@ -124,15 +147,11 @@ class ROSINTEGRATION_API UBaseMessageConverter: public UObject
 		return GetTArrayFromBSON<int32>(Key, msg, KeyFound, [](FString subKey, bson_t* subMsg, bool& subKeyFound) { return GetInt32FromBSON(subKey, subMsg, subKeyFound, false); }, LogOnErrors);
 	}
 
-protected:
-
-	// Helper function to append a TArray<float> to a bson_t
-	static void _bson_append_float_tarray(bson_t *b, const char *key, const TArray<float>& tarray)
+	static TArray<bool> GetBoolTArrayFromBSON(FString Key, bson_t* msg, bool &KeyFound, bool LogOnErrors = true)
 	{
-		// float -> double doesn't loose precision
-		_bson_append_double_tarray(b, key, (TArray<double>)tarray);
+		return GetTArrayFromBSON<bool>(Key, msg, KeyFound, [](FString subKey, bson_t* subMsg, bool& subKeyFound) { return GetBoolFromBSON(subKey, subMsg, subKeyFound, false); }, LogOnErrors);
 	}
-
+	
 	template<class T>
 	static void _bson_append_tarray(bson_t *b, const char *key, const TArray<T>& tarray, const std::function<void(bson_t*, const char*, const T&)>& appendT)
 	{
@@ -154,6 +173,13 @@ class ROSINTEGRATION_API UBaseMessageConverter: public UObject
 		_bson_append_tarray<double>(b, key, tarray, [](bson_t *subb, const char *subKey, double d) { BSON_APPEND_DOUBLE(subb, subKey, d); });
 	}
 
+	// Helper function to append a TArray<float> to a bson_t
+	static void _bson_append_float_tarray(bson_t *b, const char *key, const TArray<float>& tarray)
+	{
+		// float -> double doesn't loose precision
+		_bson_append_double_tarray(b, key, (TArray<double>)tarray);
+	}
+
 	// Helper function to append a TArray<uint8> to a bson_t
 	static void _bson_append_uint8_tarray(bson_t *b, const char *key, const TArray<uint8>& tarray)
 	{
@@ -172,4 +198,27 @@ class ROSINTEGRATION_API UBaseMessageConverter: public UObject
 	{
 		_bson_append_tarray<uint32>(b, key, tarray, [](bson_t *subb, const char *subKey, uint32 i) { BSON_APPEND_INT32(subb, subKey, i); });
 	}
-};
+	
+	// Helper function to append a TArray<bool> to a bson_t
+	static void _bson_append_bool_tarray(bson_t *b, const char *key, const TArray<bool>& tarray)
+	{
+		_bson_append_tarray<bool>(b, key, tarray, [](bson_t *subb, const char *subKey, bool i) { BSON_APPEND_BOOL(subb, subKey, i); });
+	}
+
+	static bool _bson_extract_child_ros_time(bson_t *b, FString key, FROSTime *time, bool LogOnErrors = true)
+	{
+		bool KeyFound = false;
+		time->_Sec = GetInt32FromBSON(key + ".secs", b, KeyFound, LogOnErrors); if (!KeyFound) return false;
+		time->_NSec = GetInt32FromBSON(key + ".nsecs", b, KeyFound, LogOnErrors); if (!KeyFound) return false;
+		return true;
+	}
+
+	static void _bson_append_child_ros_time(bson_t *b, const char* key, const FROSTime* time)
+	{
+		bson_t child;
+		BSON_APPEND_DOCUMENT_BEGIN(b, key, &child);
+		BSON_APPEND_INT32(&child, "secs", time->_Sec);
+		BSON_APPEND_INT32(&child, "nsecs", time->_NSec);
+		bson_append_document_end(b, &child);
+	}
+};

Source/ROSIntegration/Private/Conversion/Messages/actionlib_msgs/ActionlibMsgsGoalIDConverter.cpp

@@ -1,25 +1,22 @@
 #include "Conversion/Messages/actionlib_msgs/ActionlibMsgsGoalIDConverter.h"
 
 
-UActionlibMsgsGoalIDConverter::UActionlibMsgsGoalIDConverter(const FObjectInitializer& ObjectInitializer)
-	: Super(ObjectInitializer)
+UActionlibMsgsGoalIDConverter::UActionlibMsgsGoalIDConverter()
 {
 	_MessageType = "actionlib_msgs/GoalID";
 }
 
 bool UActionlibMsgsGoalIDConverter::ConvertIncomingMessage(const ROSBridgePublishMsg* message, TSharedPtr<FROSBaseMsg> &BaseMsg)
 {
-	auto g = new ROSMessages::actionlib_msgs::GoalID();
-	BaseMsg = TSharedPtr<FROSBaseMsg>(g);
-	return _bson_extract_child_goal_id(message->full_msg_bson_, "msg", g);
+	auto msg = new ROSMessages::actionlib_msgs::GoalID();
+	BaseMsg = TSharedPtr<FROSBaseMsg>(msg);
+	return _bson_extract_child_goal_id(message->full_msg_bson_, "msg", msg);
 }
 
 bool UActionlibMsgsGoalIDConverter::ConvertOutgoingMessage(TSharedPtr<FROSBaseMsg> BaseMsg, bson_t** message)
 {
-	auto GoalID = StaticCastSharedPtr<ROSMessages::actionlib_msgs::GoalID>(BaseMsg);
-
+	auto CastMSG = StaticCastSharedPtr<ROSMessages::actionlib_msgs::GoalID>(BaseMsg);
 	*message = bson_new();
-	_bson_append_goal_id(*message, GoalID.Get());
-
+	_bson_append_goal_id(*message, CastMSG.Get());
 	return true;
 }

Source/ROSIntegration/Private/Conversion/Messages/actionlib_msgs/ActionlibMsgsGoalIDConverter.h

@@ -1,8 +1,6 @@
 #pragma once
 
 #include "CoreMinimal.h"
-#include "UObject/ObjectMacros.h"
-#include "UObject/Object.h"
 #include "Conversion/Messages/BaseMessageConverter.h"
 #include "actionlib_msgs/GoalID.h"
 #include "ActionlibMsgsGoalIDConverter.generated.h"
@@ -11,38 +9,39 @@
 UCLASS()
 class ROSINTEGRATION_API UActionlibMsgsGoalIDConverter : public UBaseMessageConverter
 {
-	GENERATED_UCLASS_BODY()
+	GENERATED_BODY()
 
 public:
+	UActionlibMsgsGoalIDConverter();
 	virtual bool ConvertIncomingMessage(const ROSBridgePublishMsg* message, TSharedPtr<FROSBaseMsg> &BaseMsg);
 	virtual bool ConvertOutgoingMessage(TSharedPtr<FROSBaseMsg> BaseMsg, bson_t** message);
 
-	static bool _bson_extract_child_goal_id(bson_t *b, FString key, ROSMessages::actionlib_msgs::GoalID *g, bool LogOnErrors = true)
+	static bool _bson_extract_child_goal_id(bson_t *b, FString key, ROSMessages::actionlib_msgs::GoalID *msg, bool LogOnErrors = true)
 	{
 		bool KeyFound = false;
 		int32 Sec =   GetInt32FromBSON(key + ".stamp.secs", b, KeyFound, LogOnErrors);  if (!KeyFound) return false;
 		int32 NSec =  GetInt32FromBSON(key + ".stamp.nsecs", b, KeyFound, LogOnErrors); if (!KeyFound) return false;
-		g->id = GetFStringFromBSON(key + ".id", b, KeyFound, LogOnErrors); if (!KeyFound) return false;
-		g->stamp = FROSTime(Sec, NSec);
+		msg->id = GetFStringFromBSON(key + ".id", b, KeyFound, LogOnErrors); if (!KeyFound) return false;
+		msg->stamp = FROSTime(Sec, NSec);
 
 		return true;
 	}
 
-	static void _bson_append_child_goal_id(bson_t *b, const char *key, const ROSMessages::actionlib_msgs::GoalID *g)
+	static void _bson_append_child_goal_id(bson_t *b, const char *key, const ROSMessages::actionlib_msgs::GoalID *msg)
 	{
-		bson_t goalID;
-		BSON_APPEND_DOCUMENT_BEGIN(b, key, &goalID);
-		_bson_append_goal_id(&goalID, g);
-		bson_append_document_end(b, &goalID);
+		bson_t child;
+		BSON_APPEND_DOCUMENT_BEGIN(b, key, &child);
+		_bson_append_goal_id(&child, msg);
+		bson_append_document_end(b, &child);
 	}
 
-	static void _bson_append_goal_id(bson_t *b, const ROSMessages::actionlib_msgs::GoalID *g)
+	static void _bson_append_goal_id(bson_t *b, const ROSMessages::actionlib_msgs::GoalID *msg)
 	{
 		bson_t stamp;
 		BSON_APPEND_DOCUMENT_BEGIN(b, "stamp", &stamp);
-		BSON_APPEND_INT32(&stamp, "secs", g->stamp._Sec);
-		BSON_APPEND_INT32(&stamp, "nsecs", g->stamp._NSec);
+		BSON_APPEND_INT32(&stamp, "secs", msg->stamp._Sec);
+		BSON_APPEND_INT32(&stamp, "nsecs", msg->stamp._NSec);
 		bson_append_document_end(b, &stamp);
-		BSON_APPEND_UTF8(b, "id", TCHAR_TO_UTF8(*g->id));
+		BSON_APPEND_UTF8(b, "id", TCHAR_TO_UTF8(*msg->id));
 	}
 };

Source/ROSIntegration/Private/Conversion/Messages/actionlib_msgs/ActionlibMsgsGoalStatusArrayConverter.cpp

@@ -1,25 +1,22 @@
 #include "Conversion/Messages/actionlib_msgs/ActionlibMsgsGoalStatusArrayConverter.h"
 
 
-UActionlibMsgsGoalStatusArrayConverter::UActionlibMsgsGoalStatusArrayConverter(const FObjectInitializer& ObjectInitializer)
-	: Super(ObjectInitializer)
+UActionlibMsgsGoalStatusArrayConverter::UActionlibMsgsGoalStatusArrayConverter()
 {
 	_MessageType = "actionlib_msgs/GoalStatusArray";
 }
 
 bool UActionlibMsgsGoalStatusArrayConverter::ConvertIncomingMessage(const ROSBridgePublishMsg* message, TSharedPtr<FROSBaseMsg> &BaseMsg)
 {
-	auto g = new ROSMessages::actionlib_msgs::GoalStatusArray();
-	BaseMsg = TSharedPtr<FROSBaseMsg>(g);
-	return _bson_extract_child_goal_status_array(message->full_msg_bson_, "msg", g);
+	auto msg = new ROSMessages::actionlib_msgs::GoalStatusArray();
+	BaseMsg = TSharedPtr<FROSBaseMsg>(msg);
+	return _bson_extract_child_goal_status_array(message->full_msg_bson_, "msg", msg);
 }
 
 bool UActionlibMsgsGoalStatusArrayConverter::ConvertOutgoingMessage(TSharedPtr<FROSBaseMsg> BaseMsg, bson_t** message)
 {
-	auto GoalStatusArray = StaticCastSharedPtr<ROSMessages::actionlib_msgs::GoalStatusArray>(BaseMsg);
-
+	auto CastMSG = StaticCastSharedPtr<ROSMessages::actionlib_msgs::GoalStatusArray>(BaseMsg);
 	*message = bson_new();
-	_bson_append_goal_status_array(*message, GoalStatusArray.Get());
-
+	_bson_append_goal_status_array(*message, CastMSG.Get());
 	return true;
 }