Plugin refactoring to sync with AirLib (Part 2)

AirLib/AirLib.vcxproj

@@ -50,6 +50,7 @@
     <ClInclude Include="include\common\common_utils\ProsumerQueue.hpp" />
     <ClInclude Include="include\common\common_utils\RandomGenerator.hpp" />
     <ClInclude Include="include\common\common_utils\ScheduledExecutor.hpp" />
+    <ClInclude Include="include\common\common_utils\Signal.hpp" />
     <ClInclude Include="include\common\common_utils\sincos.hpp" />
     <ClInclude Include="include\common\common_utils\StrictMode.hpp" />
     <ClInclude Include="include\common\common_utils\Timer.hpp" />

AirLib/AirLib.vcxproj.filters

@@ -429,6 +429,9 @@
     <ClInclude Include="include\api\ApiProvider.hpp">
       <Filter>Header Files</Filter>
     </ClInclude>
+    <ClInclude Include="include\common\common_utils\Signal.hpp">
+      <Filter>Header Files</Filter>
+    </ClInclude>
   </ItemGroup>
   <ItemGroup>
     <ClCompile Include="src\safety\ObstacleMap.cpp">

AirLib/include/api/ApiProvider.hpp

@@ -53,11 +53,23 @@ class ApiProvider {
     {
         return vehicle_apis_;
     }
-    const std::map<std::string, VehicleSimApiBase*>& getVehicleSimApis()
+    const std::map<std::string, VehicleSimApiBase*>& geVehicleSimApis()
     {
         return vehicle_sim_apis_;
     }
 
+    bool hasDefaultVehicle() const
+    {
+        return Utils::findOrDefault<std::string, VehicleApiBase*>(vehicle_apis_, "") == nullptr &&
+            Utils::findOrDefault<std::string, VehicleSimApiBase*>(vehicle_sim_apis_, "") == nullptr;
+    }
+
+    void makeDefaultVehicle(const std::string& vehicle_name)
+    {
+        vehicle_apis_[""] = vehicle_apis_[vehicle_name];
+        vehicle_sim_apis_[""] = vehicle_sim_apis_[vehicle_name];
+    }
+
 private:
     WorldSimApiBase* world_sim_api_;
 

AirLib/include/api/RpcLibAdapatorsBase.hpp

@@ -264,19 +264,19 @@ class RpcLibAdapatorsBase {
     };
 
     struct ImageRequest {
-        uint8_t camera_id;
+        std::string camera_name;
         msr::airlib::ImageCaptureBase::ImageType image_type;
         bool pixels_as_float;
         bool compress;
 
-        MSGPACK_DEFINE_MAP(camera_id, image_type, pixels_as_float, compress);
+        MSGPACK_DEFINE_MAP(camera_name, image_type, pixels_as_float, compress);
 
         ImageRequest()
         {}
 
         ImageRequest(const msr::airlib::ImageCaptureBase::ImageRequest& s)
         {
-            camera_id = s.camera_id;
+            camera_name = s.camera_name;
             image_type = s.image_type;
             pixels_as_float = s.pixels_as_float;
             compress = s.compress;
@@ -285,7 +285,7 @@ class RpcLibAdapatorsBase {
         msr::airlib::ImageCaptureBase::ImageRequest to() const
         {
             msr::airlib::ImageCaptureBase::ImageRequest d;
-            d.camera_id = camera_id;
+            d.camera_name = camera_name;
             d.image_type = image_type;
             d.pixels_as_float = pixels_as_float;
             d.compress = compress;

AirLib/include/api/RpcLibClientBase.hpp

@@ -45,16 +45,16 @@ class RpcLibClientBase {
     Pose simGetObjectPose(const std::string& object_name) const;
 
     vector<ImageCaptureBase::ImageResponse> simGetImages(vector<ImageCaptureBase::ImageRequest> request);
-    vector<uint8_t> simGetImage(int camera_id, ImageCaptureBase::ImageType type);
+    vector<uint8_t> simGetImage(const std::string& camera_name, ImageCaptureBase::ImageType type);
 
     CollisionInfo simGetCollisionInfo() const;
 
     bool simSetSegmentationObjectID(const std::string& mesh_name, int object_id, bool is_name_regex = false);
     int simGetSegmentationObjectID(const std::string& mesh_name) const;
     void simPrintLogMessage(const std::string& message, std::string message_param = "", unsigned char severity = 0);
 
-    CameraInfo simGetCameraInfo(int camera_id) const;
-    void simSetCameraOrientation(int camera_id, const Quaternionr& orientation);
+    CameraInfo simGetCameraInfo(const std::string& camera_name) const;
+    void simSetCameraOrientation(const std::string& camera_name, const Quaternionr& orientation);
 
     //task management APIs
     void cancelLastTask();

AirLib/include/api/VehicleSimApiBase.hpp

@@ -50,10 +50,13 @@ class VehicleSimApiBase : public msr::airlib::UpdatableObject {
     virtual RCData getRCData() const = 0; //get reading from RC from simulator
     virtual std::string getVehicleName() const = 0;
     virtual std::string getLogLine() const = 0;
+    virtual void setLogLine(std::string line) = 0;
+    virtual void toggleTrace() = 0;
 
-    const AirSimSettings::VehicleSetting& getVehicleSetting() const
+    //use pointer here because of derived classes for VehicleSetting
+    const AirSimSettings::VehicleSetting* getVehicleSetting() const
     {
-        return *(AirSimSettings::singleton().getVehicleSetting(getVehicleName()));
+        return AirSimSettings::singleton().getVehicleSetting(getVehicleName());
     }
 
 };

AirLib/include/common/AirSimSettings.hpp

@@ -31,10 +31,10 @@ struct AirSimSettings {
         int window_index;
         ImageType image_type;
         bool visible;
-        int camera_id;
+        std::string camera_name;
 
-        SubwindowSetting(int window_index_val = 0, ImageType image_type_val = ImageType::Scene, bool visible_val = false, int camera_id_val = 0)
-            : window_index(window_index_val), image_type(image_type_val), visible(visible_val), camera_id(camera_id_val)
+        SubwindowSetting(int window_index_val = 0, ImageType image_type_val = ImageType::Scene, bool visible_val = false, const std::string& camera_name_val = 0)
+            : window_index(window_index_val), image_type(image_type_val), visible(visible_val), camera_name(camera_name_val)
         {
         }
     };
@@ -848,7 +848,7 @@ struct AirSimSettings {
                     subwindow_setting.image_type = Utils::toEnum<ImageType>(
                         json_settings_child.getInt("ImageType", 0));
                     subwindow_setting.visible = json_settings_child.getBool("Visible", false);
-                    subwindow_setting.camera_id = json_settings_child.getInt("CameraID", 0);
+                    subwindow_setting.camera_name = json_settings_child.getString("CameraName", "");
                 }
             }
         }

AirLib/include/common/ImageCaptureBase.hpp

@@ -13,7 +13,7 @@ namespace msr { namespace airlib {
 class ImageCaptureBase
 {
 public: //types
-    enum class ImageType : uint { //this indexes to array
+    enum class ImageType : int { //this indexes to array, -1 is special to indicate main camera
         Scene = 0, 
         DepthPlanner, 
         DepthPerspective,

AirLib/include/common/common_utils/Signal.hpp

@@ -0,0 +1,101 @@
+#ifndef common_utils_Signal_hpp
+#define common_utils_Signal_hpp
+
+#include <functional>
+#include <map>
+
+// A signal object may call multiple slots with the
+// same signature. You can connect functions to the signal
+// which will be called when the emit() method on the
+// signal object is invoked. Any argument passed to emit()
+// will be passed to the given functions.
+
+/*
+//USAGE:
+
+#include "Signal.hpp"
+#include <iostream>
+
+int main() {
+
+// create new signal
+Signal<std::string, int> signal;
+
+// attach a slot
+signal.connect([](std::string arg1, int arg2) {
+std::cout << arg1 << " " << arg2 << std::endl;
+});
+
+signal.emit("The answer:", 42);
+
+return 0;
+}
+
+*/
+
+namespace common_utils {
+
+
+template <typename... Args>
+class Signal {
+
+public:
+
+    Signal() : current_id_(0) {}
+
+    // copy creates new signal
+    Signal(Signal const& other) : current_id_(0) {}
+
+    // connects a member function to this Signal
+    template <typename T>
+    int connect_member(T *inst, void (T::*func)(Args...)) {
+        return connect([=](Args... args) {
+            (inst->*func)(args...);
+        });
+    }
+
+    // connects a const member function to this Signal
+    template <typename T>
+    int connect_member(T *inst, void (T::*func)(Args...) const) {
+        return connect([=](Args... args) {
+            (inst->*func)(args...);
+        });
+    }
+
+    // connects a std::function to the signal. The returned
+    // value can be used to disconnect the function again
+    int connect(std::function<void(Args...)> const& slot) const {
+        slots_.insert(std::make_pair(++current_id_, slot));
+        return current_id_;
+    }
+
+    // disconnects a previously connected function
+    void disconnect(int id) const {
+        slots_.erase(id);
+    }
+
+    // disconnects all previously connected functions
+    void disconnect_all() const {
+        slots_.clear();
+    }
+
+    // calls all connected functions
+    void emit(Args... p) {
+        for (auto it : slots_) {
+            it.second(p...);
+        }
+    }
+
+    // assignment creates new Signal
+    Signal& operator=(Signal const& other) {
+        disconnect_all();
+    }
+
+private:
+    mutable std::map<int, std::function<void(Args...)>> slots_;
+    mutable int current_id_;
+};
+
+}
+
+#endif /* common_utils_Signal_hpp */

AirLib/include/common/common_utils/Utils.hpp

@@ -127,13 +127,21 @@ class Utils {
     }
 
     template <template<class, class, class...> class TContainer, typename TKey, typename TVal, typename... Args>
-    static TVal findOrDefault(const TContainer<TKey, TVal, Args...>& m, TKey const& key, const TVal & default_val)
+    static TVal findOrDefault(const TContainer<TKey, TVal, Args...>& m, TKey const& key, const TVal& default_val)
     {
         typename TContainer<TKey, TVal, Args...>::const_iterator it = m.find(key);
         if (it == m.end())
             return default_val;
         return it->second;
     }
+    template <typename TKey, typename TVal>
+    static TVal findOrDefault(const std::map<TKey, TVal>& m, TKey const& key, const TVal& default_val = TVal())
+    {
+        typename TContainer<TKey, TVal>::const_iterator it = m.find(key);
+        if (it == m.end())
+            return default_val;
+        return it->second;
+    }
 
     static Logger* getSetLogger(Logger* logger = nullptr)
     {

AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp

@@ -28,7 +28,7 @@ class SimpleFlightApi : public MultirotorApiBase {
     SimpleFlightApi(const MultiRotorParams* vehicle_params, const AirSimSettings::VehicleSetting* vehicle_setting)
         : vehicle_params_(vehicle_params)
     {
-        readSettings(vehicle_setting);
+        readSettings(*vehicle_setting);
 
         //TODO: set below properly for better high speed safety
         safety_params_.vel_to_breaking_dist = safety_params_.min_breaking_dist = 0;
@@ -127,6 +127,8 @@ class SimpleFlightApi : public MultirotorApiBase {
         else { //else we don't have RC data
             board_->setIsRcConnected(false);
         }
+
+        return true;
     }
 
 protected: 
@@ -280,14 +282,14 @@ class SimpleFlightApi : public MultirotorApiBase {
         return static_cast<uint16_t>(1000.0f * switchVal / maxSwitchVal + 1000.0f);
     }
 
-    void readSettings(const AirSimSettings::VehicleSetting* vehicle_setting)
+    void readSettings(const AirSimSettings::VehicleSetting& vehicle_setting)
     {
         params_.default_vehicle_state = simple_flight::VehicleState::fromString(
-            vehicle_setting->default_vehicle_state == "" ? "Armed" : vehicle_setting->default_vehicle_state);
+            vehicle_setting.default_vehicle_state == "" ? "Armed" : vehicle_setting.default_vehicle_state);
 
-        remote_control_id_ = vehicle_setting->rc.remote_control_id;
-        params_.rc.allow_api_when_disconnected = vehicle_setting->rc.allow_api_when_disconnected;
-        params_.rc.allow_api_always = vehicle_setting->allow_api_always;
+        remote_control_id_ = vehicle_setting.rc.remote_control_id;
+        params_.rc.allow_api_when_disconnected = vehicle_setting.rc.allow_api_when_disconnected;
+        params_.rc.allow_api_always = vehicle_setting.allow_api_always;
     }
 
 private:

AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightQuadXParams.hpp

@@ -68,7 +68,7 @@ class SimpleFlightQuadXParams : public MultiRotorParams {
 
 private:
     vector<unique_ptr<SensorBase>> sensor_storage_;
-    const AirSimSettings::VehicleSetting* vehicle_setting_;
+    const AirSimSettings::VehicleSetting* vehicle_setting_; //store as pointer because of derived classes
     std::shared_ptr<const SensorFactory> sensor_factory_;
 };
 

AirLib/src/api/RpcLibClientBase.cpp

@@ -179,9 +179,9 @@ vector<ImageCaptureBase::ImageResponse> RpcLibClientBase::simGetImages(vector<Im
 
     return RpcLibAdapatorsBase::ImageResponse::to(response_adaptor);
 }
-vector<uint8_t> RpcLibClientBase::simGetImage(int camera_id, ImageCaptureBase::ImageType type)
+vector<uint8_t> RpcLibClientBase::simGetImage(const std::string& camera_name, ImageCaptureBase::ImageType type)
 {
-    vector<uint8_t> result = pimpl_->client.call("simGetImage", camera_id, type).as<vector<uint8_t>>();
+    vector<uint8_t> result = pimpl_->client.call("simGetImage", camera_name, type).as<vector<uint8_t>>();
     if (result.size() == 1) {
         // rpclib has a bug with serializing empty vectors, so we return a 1 byte vector instead.
         result.clear();
@@ -214,13 +214,13 @@ msr::airlib::Pose RpcLibClientBase::simGetObjectPose(const std::string& object_n
     return pimpl_->client.call("simGetObjectPose", object_name).as<RpcLibAdapatorsBase::Pose>().to();
 }
 
-CameraInfo RpcLibClientBase::simGetCameraInfo(int camera_id) const
+CameraInfo RpcLibClientBase::simGetCameraInfo(const std::string& camera_name) const
 {
-    return pimpl_->client.call("simGetCameraInfo", camera_id).as<RpcLibAdapatorsBase::CameraInfo>().to();
+    return pimpl_->client.call("simGetCameraInfo", camera_name).as<RpcLibAdapatorsBase::CameraInfo>().to();
 }
-void RpcLibClientBase::simSetCameraOrientation(int camera_id, const Quaternionr& orientation)
+void RpcLibClientBase::simSetCameraOrientation(const std::string& camera_name, const Quaternionr& orientation)
 {
-    pimpl_->client.call("simSetCameraOrientation", camera_id, RpcLibAdapatorsBase::Quaternionr(orientation));
+    pimpl_->client.call("simSetCameraOrientation", camera_name, RpcLibAdapatorsBase::Quaternionr(orientation));
 }
 
 void RpcLibClientBase::cancelLastTask()

AirLib/src/api/RpcLibServerBase.cpp

@@ -81,8 +81,8 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string&
         const auto& response = getVehicleSimApi()->getImages(RpcLibAdapatorsBase::ImageRequest::to(request_adapter));
         return RpcLibAdapatorsBase::ImageResponse::from(response);
     });
-    pimpl_->server.bind("simGetImage", [&](uint8_t camera_id, ImageCaptureBase::ImageType type) -> vector<uint8_t> {
-        auto result = getVehicleSimApi()->getImage(camera_id, type);
+    pimpl_->server.bind("simGetImage", [&](const std::string& camera_name, ImageCaptureBase::ImageType type) -> vector<uint8_t> {
+        auto result = getVehicleSimApi()->getImage(camera_name, type);
         if (result.size() == 0) {
             // rpclib has a bug with serializing empty vectors, so we return a 1 byte vector instead.
             result.push_back(0);
@@ -125,13 +125,13 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string&
         return RpcLibAdapatorsBase::GeoPoint(geo_point);
     });
 
-    pimpl_->server.bind("simGetCameraInfo", [&](int camera_id) -> RpcLibAdapatorsBase::CameraInfo {
-        const auto& camera_info = getVehicleSimApi()->getCameraInfo(camera_id);
+    pimpl_->server.bind("simGetCameraInfo", [&](const std::string& camera_name) -> RpcLibAdapatorsBase::CameraInfo {
+        const auto& camera_info = getVehicleSimApi()->getCameraInfo(camera_name);
         return RpcLibAdapatorsBase::CameraInfo(camera_info);
     });
 
-    pimpl_->server.bind("simSetCameraOrientation", [&](int camera_id, const RpcLibAdapatorsBase::Quaternionr& orientation) -> void {
-        getVehicleSimApi()->setCameraOrientation(camera_id, orientation.to());
+    pimpl_->server.bind("simSetCameraOrientation", [&](const std::string& camera_name, const RpcLibAdapatorsBase::Quaternionr& orientation) -> void {
+        getVehicleSimApi()->setCameraOrientation(camera_name, orientation.to());
     });
 
     pimpl_->server.bind("simGetCollisionInfo", [&]() -> RpcLibAdapatorsBase::CollisionInfo { 

AirLibUnitTests/SimpleFlightTest.hpp

@@ -28,9 +28,7 @@ class SimpleFlightTest : public TestBase
             std::make_shared<SensorFactory>());
         auto api = params->createMultirotorApi();
 
-        std::unique_ptr<Environment> environment;
-        MultiRotor vehicle(params.get(), api.get(), Pose(),
-            GeoPoint(), environment);
+        MultiRotor vehicle(params.get(), api.get(), Pose());
 
         std::vector<UpdatableObject*> vehicles = { &vehicle };
         std::unique_ptr<PhysicsEngineBase> physics_engine(new FastPhysicsEngine());

DroneServer/main.cpp

@@ -81,7 +81,7 @@ int main(int argc, const char* argv[])
 
 
     ApiProvider api_provider(nullptr);
-    api_provider.insert_or_assign("", &api);
+    api_provider.insert_or_assign("", &api, nullptr);
     msr::airlib::MultirotorRpcLibServer server(&api_provider, connection_info.local_host_ip);
 
     //start server in async mode