Support for multiple drones in Unity

Unity/AirLibWrapper/AirsimWrapper/Source/PInvokeWrapper.cpp

@@ -19,10 +19,10 @@ bool (*SetSegmentationObjectId)(const char* meshName, int objectId, bool isNameR
 int (*GetSegmentationObjectId)(const char* meshName);
 bool (*PrintLogMessage)(const char* message, const char* messageParam, const char* vehicleName, int severity);
 UnityTransform (*GetTransformFromUnity)(const char* vehicleName);
-bool (*Reset)(const char* vehicleName);
+bool (*Reset)();
 AirSimVector (*GetVelocity)(const char* vehicleName);
 RayCastHitResult (*GetRayCastHit)(AirSimVector startVec, AirSimVector endVec, const char* vehicleName);
-bool (*Pause)(const char* vehicleName, float timeScale);
+bool (*Pause)(float timeScale);
 
 void InitVehicleManager(
     bool (*setPose)(AirSimPose pose, bool ignoreCollision, const char* vehicleName),
@@ -43,10 +43,10 @@ void InitVehicleManager(
     int (*getSegmentationObjectId)(const char* meshName),
     bool (*printLogMessage)(const char* message, const char* messageParam, const char* vehicleName, int severity),
     UnityTransform (*getTransformFromUnity)(const char* vehicleName),
-    bool (*reset)(const char* vehicleName),
+    bool (*reset)(),
     AirSimVector (*getVelocity)(const char* vehicleName),
     RayCastHitResult (*getRayCastHit)(AirSimVector startVec, AirSimVector endVec, const char* vehicleName),
-    bool (*pause)(const char* vehicleName, float timeScale))
+    bool (*pause)(float timeScale))
 {
     SetPose = setPose;
     GetPose = getPose;

Unity/AirLibWrapper/AirsimWrapper/Source/PInvokeWrapper.h

@@ -32,10 +32,10 @@ extern bool (*SetSegmentationObjectId)(const char* meshName, int objectId, bool
 extern int (*GetSegmentationObjectId)(const char* meshName);
 extern bool (*PrintLogMessage)(const char* message, const char* messageParam, const char* vehicleName, int severity);
 extern UnityTransform (*GetTransformFromUnity)(const char* vehicleName);
-extern bool (*Reset)(const char* vehicleName);
+extern bool (*Reset)();
 extern AirSimVector (*GetVelocity)(const char* vehicleName);
 extern RayCastHitResult (*GetRayCastHit)(AirSimVector startVec, AirSimVector endVec, const char* vehicleName);
-extern bool (*Pause)(const char* vehicleName, float timeScale);
+extern bool (*Pause)(float timeScale);
 
 // PInvoke call to initialize the function pointers. This function is called from Unity.
 
@@ -58,7 +58,7 @@ extern "C" EXPORT void InitVehicleManager(
     int (*getSegmentationObjectId)(const char* meshName),
     bool (*printLogMessage)(const char* message, const char* messageParam, const char* vehicleName, int severity),
     UnityTransform (*getTransformFromUnity)(const char* vehicleName),
-    bool (*reset)(const char* vehicleName),
+    bool (*reset)(),
     AirSimVector (*getVelocity)(const char* vehicleName),
     RayCastHitResult (*getRayCastHit)(AirSimVector startVec, AirSimVector endVec, const char* vehicleName),
-    bool (*pause)(const char* vehicleName, float timeScale));
+    bool (*pause)(float timeScale));

Unity/AirLibWrapper/AirsimWrapper/Source/SimHUD/SimHUD.cpp

@@ -3,8 +3,8 @@
 #include "../Vehicles/Car/SimModeCar.h"
 #include "../Vehicles/Multirotor/SimModeWorldMultiRotor.h"
 
-SimHUD::SimHUD(std::string vehicle_name, std::string sim_mode_name, int port_number)
-    : vehicle_name_(vehicle_name), sim_mode_name_(sim_mode_name), port_number_(port_number)
+SimHUD::SimHUD(std::string sim_mode_name, int port_number)
+    : sim_mode_name_(sim_mode_name), port_number_(port_number)
 {
     server_started_Successfully_ = false;
 }
@@ -20,7 +20,7 @@ void SimHUD::BeginPlay()
         server_started_Successfully_ = true;
     }
     catch (std::exception& ex) {
-        PrintLogMessage("Error at startup: ", ex.what(), vehicle_name_.c_str(), ErrorLogSeverity::Error);
+        PrintLogMessage("Error at startup: ", ex.what(), "", ErrorLogSeverity::Error);
     }
 }
 
@@ -75,10 +75,10 @@ void SimHUD::initializeSettings()
     AirSimSettings::singleton().load(std::bind(&SimHUD::getSimModeFromUser, this));
 
     for (const auto& warning : AirSimSettings::singleton().warning_messages) {
-        PrintLogMessage(warning.c_str(), "LogDebugLevel::Failure", vehicle_name_.c_str(), ErrorLogSeverity::Error);
+        PrintLogMessage(warning.c_str(), "LogDebugLevel::Failure", "", ErrorLogSeverity::Error);
     }
     for (const auto& error : AirSimSettings::singleton().error_messages) {
-        PrintLogMessage(error.c_str(), "settings.json", vehicle_name_.c_str(), ErrorLogSeverity::Error);
+        PrintLogMessage(error.c_str(), "settings.json", "", ErrorLogSeverity::Error);
     }
 }
 
@@ -107,9 +107,9 @@ void SimHUD::createSimMode()
     std::string simmode_name = AirSimSettings::singleton().simmode_name;
 
     if (simmode_name == "Multirotor")
-        simmode_ = new SimModeWorldMultiRotor(vehicle_name_, port_number_);
+        simmode_ = new SimModeWorldMultiRotor(port_number_);
     else if (simmode_name == "Car")
-        simmode_ = new SimModeCar(vehicle_name_, port_number_);
+        simmode_ = new SimModeCar(port_number_);
 
     simmode_->BeginPlay();
 }
@@ -134,11 +134,11 @@ bool SimHUD::readSettingsTextFromFile(std::string settingsFilepath, std::string&
         std::stringstream buffer;
         buffer << file.rdbuf();
         settingsText = buffer.str();
-        PrintLogMessage("Loaded settings from ", settingsFilepath.c_str(), vehicle_name_.c_str(), ErrorLogSeverity::Information);
+        PrintLogMessage("Loaded settings from ", settingsFilepath.c_str(), "", ErrorLogSeverity::Information);
         return true;
     }
     else {
-        PrintLogMessage("Cannot read settings file ", settingsFilepath.c_str(), vehicle_name_.c_str(), ErrorLogSeverity::Error);
+        PrintLogMessage("Cannot read settings file ", settingsFilepath.c_str(), "", ErrorLogSeverity::Error);
     }
     return false;
 }

Unity/AirLibWrapper/AirsimWrapper/Source/SimHUD/SimHUD.h

@@ -18,7 +18,7 @@ class SimHUD
     std::string getSimModeFromUser();
 
 public:
-    SimHUD(std::string vehicle_name, std::string sime_mode_name, int port_number);
+    SimHUD(std::string sime_mode_name, int port_number);
     SimModeBase* GetSimMode();
     ImageType getSubwindowCameraType(int window_index);
     void setSubwindowCameraType(int window_index, ImageType type);
@@ -31,7 +31,6 @@ class SimHUD
 private:
     typedef common_utils::Utils Utils;
     SimModeBase* simmode_;
-    std::string vehicle_name_;
     std::string sim_mode_name_;
     int port_number_;
 

Unity/AirLibWrapper/AirsimWrapper/Source/SimMode/SimModeBase.cpp

@@ -5,8 +5,8 @@
 #include "../PInvokeWrapper.h"
 #include "../WorldSimApi.h"
 
-SimModeBase::SimModeBase(std::string vehicle_name, int port_number)
-    : vehicle_name_(vehicle_name), port_number_(port_number)
+SimModeBase::SimModeBase(int port_number)
+    : port_number_(port_number)
 {
 }
 
@@ -15,7 +15,7 @@ void SimModeBase::BeginPlay()
     debug_reporter_.initialize(false);
     debug_reporter_.reset();
     global_ned_transform_.reset(new NedTransform(GetVehicleStartTransform()));
-    world_sim_api_.reset(new WorldSimApi(this, vehicle_name_));
+    world_sim_api_.reset(new WorldSimApi(this));
     api_provider_.reset(new msr::airlib::ApiProvider(world_sim_api_.get()));
     setupClockSpeed();
     record_tick_count = 0;
@@ -24,7 +24,8 @@ void SimModeBase::BeginPlay()
 
 void SimModeBase::Tick(float DeltaSeconds)
 {
-    getVehicleSimApi(vehicle_name_)->pawnTick(DeltaSeconds);
+    for (auto& api : getApiProvider()->getVehicleSimApis())
+        static_cast<PawnSimApi*>(api)->pawnTick(DeltaSeconds);
     showClockStats();
     updateDebugReport(debug_reporter_);
 }
@@ -35,7 +36,7 @@ void SimModeBase::showClockStats()
     if (clock_speed != 1) {
         PrintLogMessage("ClockSpeed config, actual: ",
                         Utils::stringf("%f, %f", clock_speed, ClockFactory::get()->getTrueScaleWrtWallClock()).c_str(),
-                        vehicle_name_.c_str(),
+                        "",
                         ErrorLogSeverity::Information);
     }
 }
@@ -153,14 +154,14 @@ void SimModeBase::startApiServer()
         api_server_ = createApiServer();
 #endif
         try {
-            api_server_->start(false, 4); //TODO: set thread for vehicle count
+            api_server_->start(false, vehicle_sim_apis_.size() + 4); //TODO: set thread for vehicle count
         }
         catch (std::exception& ex) {
-            PrintLogMessage("Cannot start RpcLib Server", ex.what(), vehicle_name_.c_str(), ErrorLogSeverity::Error);
+            PrintLogMessage("Cannot start RpcLib Server", ex.what(), "", ErrorLogSeverity::Error);
         }
     }
     else
-        PrintLogMessage("API server is disabled in settings", "", vehicle_name_.c_str(), ErrorLogSeverity::Information);
+        PrintLogMessage("API server is disabled in settings", "", "", ErrorLogSeverity::Information);
 }
 void SimModeBase::stopApiServer()
 {
@@ -206,30 +207,35 @@ void SimModeBase::setupVehiclesAndCamera()
 {
     //determine camera director camera default pose and spawn it
     const auto& camera_director_setting = getSettings().camera_director;
-    UnityPawn* vehicle_pawn = GetVehiclePawn();
-    const auto& home_geopoint = msr::airlib::EarthUtils::nedToGeodetic(GetVehiclePosition(), getSettings().origin_geopoint);
+    for (auto const& vehicle_setting_pair : getSettings().vehicles) {
+        const auto& vehicle_setting = *vehicle_setting_pair.second;
+        const std::string& vehicle_name = vehicle_setting.vehicle_name;
 
-    PawnSimApi::Params pawn_sim_api_params(vehicle_pawn, &getGlobalNedTransform(), home_geopoint, vehicle_name_);
-    auto vehicle_sim_api = createVehicleSimApi(pawn_sim_api_params);
-    auto vehicle_sim_api_p = vehicle_sim_api.get();
-    auto vehicle_Api = getVehicleApi(pawn_sim_api_params, vehicle_sim_api_p);
+        UnityPawn* vehicle_pawn = GetVehiclePawn(vehicle_name);
+        const auto& home_geopoint = msr::airlib::EarthUtils::nedToGeodetic(GetVehiclePosition(vehicle_name), getSettings().origin_geopoint);
 
-    getApiProvider()->insert_or_assign(vehicle_name_, vehicle_Api, vehicle_sim_api_p);
+        PawnSimApi::Params pawn_sim_api_params(vehicle_pawn, &getGlobalNedTransform(), home_geopoint, vehicle_name);
+        auto vehicle_sim_api = createVehicleSimApi(pawn_sim_api_params);
+        auto vehicle_sim_api_p = vehicle_sim_api.get();
+        auto vehicle_Api = getVehicleApi(pawn_sim_api_params, vehicle_sim_api_p);
 
-    if ((!getApiProvider()->hasDefaultVehicle()) && vehicle_name_ != "") {
-        getApiProvider()->makeDefaultVehicle(vehicle_name_);
+        getApiProvider()->insert_or_assign(vehicle_name, vehicle_Api, vehicle_sim_api_p);
+
+        if ((!getApiProvider()->hasDefaultVehicle()) && vehicle_name != "")
+            getApiProvider()->makeDefaultVehicle(vehicle_name);
+
+        vehicle_sim_apis_.push_back(std::move(vehicle_sim_api));
     }
-    vehicle_sim_apis_.push_back(std::move(vehicle_sim_api));
 }
 
-const msr::airlib::Vector3r SimModeBase::GetVehiclePosition()
+const msr::airlib::Vector3r SimModeBase::GetVehiclePosition(const std::string& vehicle_name)
 {
-    AirSimPose airSimPose = GetPose(vehicle_name_.c_str());
+    AirSimPose airSimPose = GetPose(vehicle_name.c_str());
     msr::airlib::Vector3r vehiclePosition(airSimPose.position.x, airSimPose.position.y, airSimPose.position.z);
     return vehiclePosition;
 }
 
-UnityPawn* SimModeBase::GetVehiclePawn()
+UnityPawn* SimModeBase::GetVehiclePawn(const std::string& vehicle_name)
 {
     return nullptr;
 }
@@ -258,6 +264,7 @@ msr::airlib::VehicleApiBase* SimModeBase::getVehicleApi(const PawnSimApi::Params
 
 UnityTransform SimModeBase::GetVehicleStartTransform()
 {
-    UnityTransform unityTransform = GetTransformFromUnity(vehicle_name_.c_str());
+    //can we just take origin in Unity here?
+    UnityTransform unityTransform;
     return unityTransform;
 }

Unity/AirLibWrapper/AirsimWrapper/Source/SimMode/SimModeBase.h

@@ -21,7 +21,7 @@ class SimModeBase
     typedef msr::airlib::AirSimSettings AirSimSettings;
 
 private:
-    const msr::airlib::Vector3r GetVehiclePosition();
+    const msr::airlib::Vector3r GetVehiclePosition(const std::string& vehicle_name);
     UnityTransform GetVehicleStartTransform();
     void showClockStats();
 
@@ -30,14 +30,14 @@ class SimModeBase
     virtual bool isVehicleTypeSupported(const std::string& vehicle_type) const;
     virtual std::unique_ptr<PawnSimApi> createVehicleSimApi(const PawnSimApi::Params& pawn_sim_api_params) const;
     virtual msr::airlib::VehicleApiBase* getVehicleApi(const PawnSimApi::Params& pawn_sim_api_params, const PawnSimApi* sim_api) const;
-    virtual UnityPawn* GetVehiclePawn();
+    virtual UnityPawn* GetVehiclePawn(const std::string& vehicle_name);
     virtual void setupVehiclesAndCamera();
     virtual void setupClockSpeed(); //called when SimMode should handle clock speed setting
     virtual void updateDebugReport(msr::airlib::StateReporterWrapper& debug_reporter);
     virtual const msr::airlib::AirSimSettings& getSettings() const;
 
 public:
-    SimModeBase(std::string vehicle_name, int port_number);
+    SimModeBase(int port_number);
     virtual void BeginPlay();
     virtual void EndPlay();
     virtual void Tick(float DeltaSeconds) = 0;
@@ -85,7 +85,6 @@ class SimModeBase
     int record_tick_count;
 
 public:
-    std::string vehicle_name_;
     int port_number_;
     bool EnableReport = false;
 };

Unity/AirLibWrapper/AirsimWrapper/Source/SimMode/SimModeWorldBase.cpp

@@ -2,8 +2,8 @@
 #include "SimModeWorldBase.h"
 #include "../PInvokeWrapper.h"
 
-SimModeWorldBase::SimModeWorldBase(std::string multiRotorName, int port_number)
-    : SimModeBase(multiRotorName, port_number)
+SimModeWorldBase::SimModeWorldBase(int port_number)
+    : SimModeBase(port_number)
 {
 }
 
@@ -70,7 +70,7 @@ std::unique_ptr<SimModeWorldBase::PhysicsEngineBase> SimModeWorldBase::createPhy
     }
     else {
         physics_engine.reset();
-        PrintLogMessage("Unrecognized physics engine name: ", physics_engine_name.c_str(), vehicle_name_.c_str(), ErrorLogSeverity::Warnning);
+        PrintLogMessage("Unrecognized physics engine name: ", physics_engine_name.c_str(), "", ErrorLogSeverity::Warnning);
     }
     return physics_engine;
 }

Unity/AirLibWrapper/AirsimWrapper/Source/SimMode/SimModeWorldBase.h

@@ -26,7 +26,7 @@ class SimModeWorldBase : public SimModeBase
     void initializeForPlay();
 
 public:
-    SimModeWorldBase(std::string multiRotorName, int port_number);
+    SimModeWorldBase(int port_number);
     virtual void BeginPlay() override;
     virtual void EndPlay() override;
     virtual void Tick(float DeltaSeconds) override;

Unity/AirLibWrapper/AirsimWrapper/Source/UnityToAirSimCalls.cpp

@@ -1,7 +1,7 @@
 #include "UnityToAirSimCalls.h"
 
-void StartServerThread(std::string vehicle_name, std::string sim_mode_name, int port_number)
+void StartServerThread(std::string sim_mode_name, int port_number)
 {
-    key = new SimHUD(vehicle_name, sim_mode_name, port_number);
+    key = new SimHUD(sim_mode_name, port_number);
     key->BeginPlay();
 }

Unity/AirLibWrapper/AirsimWrapper/Source/UnityToAirSimCalls.h

@@ -13,12 +13,12 @@
 
 static SimHUD* key = nullptr;
 
-void StartServerThread(std::string vehicle_name, std::string sim_mode_name, int port_number);
+void StartServerThread(std::string sim_mode_name, int port_number);
 
-extern "C" EXPORT bool StartServer(char* vehicle_name, char* sim_mode_name, int port_number)
+extern "C" EXPORT bool StartServer(char* sim_mode_name, int port_number)
 {
     LOGGER->WriteLog("Starting server for : " + std::string(sim_mode_name));
-    std::thread server_thread(StartServerThread, vehicle_name, sim_mode_name, port_number);
+    std::thread server_thread(StartServerThread, sim_mode_name, port_number);
     server_thread.detach();
     int waitCounter = 25; // waiting for maximum 5 seconds to start a server.
     while ((key == nullptr || !key->server_started_Successfully_) && waitCounter > 0) {
@@ -28,7 +28,7 @@ extern "C" EXPORT bool StartServer(char* vehicle_name, char* sim_mode_name, int
     return key->server_started_Successfully_;
 }
 
-extern "C" EXPORT void StopServer(char* vehicle_name)
+extern "C" EXPORT void StopServer()
 {
     key->EndPlay();
     if (key != nullptr) {
@@ -43,11 +43,11 @@ extern "C" EXPORT void CallTick(float deltaSeconds)
     key->Tick(deltaSeconds);
 }
 
-extern "C" EXPORT void InvokeCollisionDetection(AirSimUnity::AirSimCollisionInfo collision_info)
+extern "C" EXPORT void InvokeCollisionDetection(char* vehicle_name, AirSimUnity::AirSimCollisionInfo collision_info)
 {
     auto simMode = key->GetSimMode();
     if (simMode) {
-        auto vehicleApi = simMode->getVehicleSimApi(simMode->vehicle_name_);
+        auto vehicleApi = simMode->getVehicleSimApi(vehicle_name);
         if (vehicleApi) {
             msr::airlib::CollisionInfo collisionInfo = UnityUtilities::Convert_to_AirSimCollisioinInfo(collision_info);
             vehicleApi->OnCollision(collisionInfo);

Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Car/CarPawnSimApi.cpp

@@ -158,7 +158,7 @@ void CarPawnSimApi::updateCarControls()
 void CarPawnSimApi::resetImplementation()
 {
     setPose(UnityUtilities::Convert_to_Pose(GetInitialPose()), false);
-    Reset(getVehicleName().c_str());
+    Reset();
 
     PawnSimApi::resetImplementation();
     pawn_api_->reset();