Merge pull request #1 from microsoft/master

merge upstream

.gitignore

@@ -307,6 +307,7 @@ cmake_build/
 /boost
 /[Ee]igen
 /build_debug
+/build_gcc_debug
 /cmake/CPackSourceConfig.cmake
 /cmake/CPackConfig.cmake
 /cmake/arch.c

AirLib/include/api/RpcLibClientBase.hpp

@@ -25,7 +25,7 @@ class RpcLibClientBase {
         Initial = 0, Connected, Disconnected, Reset, Unknown
     };
 public:
-    RpcLibClientBase(const string& ip_address = "localhost", uint16_t port = 41451, float timeout_sec = 60);
+    RpcLibClientBase(const string& ip_address = "localhost", uint16_t port = RpcLibPort, float timeout_sec = 60);
     virtual ~RpcLibClientBase();    //required for pimpl
 
     void confirmConnection();
@@ -75,6 +75,9 @@ class RpcLibClientBase {
     msr::airlib::GpsBase::Output getGpsData(const std::string& gps_name = "", const std::string& vehicle_name = "") const;
     msr::airlib::DistanceBase::Output getDistanceSensorData(const std::string& distance_sensor_name = "", const std::string& vehicle_name = "") const;
 
+    // sensor omniscient APIs
+    vector<int> simGetLidarSegmentation(const std::string& lidar_name = "", const std::string& vehicle_name = "") const;
+
     Pose simGetVehiclePose(const std::string& vehicle_name = "") const;
     void simSetVehiclePose(const Pose& pose, bool ignore_collision, const std::string& vehicle_name = "");
 

AirLib/include/api/RpcLibServerBase.hpp

@@ -14,7 +14,7 @@ namespace msr { namespace airlib {
 
 class RpcLibServerBase : public ApiServerBase {
 public:
-    RpcLibServerBase(ApiProvider* api_provider, const std::string& server_address, uint16_t port = 41451);
+    RpcLibServerBase(ApiProvider* api_provider, const std::string& server_address, uint16_t port = RpcLibPort);
     virtual ~RpcLibServerBase() override;
 
     virtual void start(bool block, std::size_t thread_count) override;

AirLib/include/api/VehicleApiBase.hpp

@@ -37,11 +37,6 @@ class VehicleApiBase : public UpdatableObject {
     virtual bool armDisarm(bool arm) = 0;
     virtual GeoPoint getHomeGeoPoint() const = 0;
 
-    //default implementation so derived class doesn't have to call on UpdatableObject
-    virtual void reset() override
-    {
-        UpdatableObject::reset();
-    }
     virtual void update() override
     {
         UpdatableObject::update();
@@ -110,26 +105,22 @@ class VehicleApiBase : public UpdatableObject {
     // Lidar APIs
     virtual LidarData getLidarData(const std::string& lidar_name) const
     {
-        const LidarBase* lidar = nullptr;
-
-        // Find lidar with the given name (for empty input name, return the first one found)
-        // Not efficient but should suffice given small number of lidars
-        uint count_lidars = getSensors().size(SensorBase::SensorType::Lidar);
-        for (uint i = 0; i < count_lidars; i++)
-        {
-            const LidarBase* current_lidar = static_cast<const LidarBase*>(getSensors().getByType(SensorBase::SensorType::Lidar, i));
-            if (current_lidar != nullptr && (current_lidar->getName() == lidar_name || lidar_name == ""))
-            {
-                lidar = current_lidar;
-                break;
-            }
-        }
+        auto *lidar = findLidarByName(lidar_name);
         if (lidar == nullptr)
             throw VehicleControllerException(Utils::stringf("No lidar with name %s exist on vehicle", lidar_name.c_str()));
 
         return lidar->getOutput();
     }
 
+    virtual vector<int> getLidarSegmentation(const std::string& lidar_name) const
+    {
+        auto *lidar = findLidarByName(lidar_name);
+        if (lidar == nullptr)
+            throw VehicleControllerException(Utils::stringf("No lidar with name %s exist on vehicle", lidar_name.c_str()));
+
+        return lidar->getSegmentationOutput();
+    }
+
     // IMU API
     virtual ImuBase::Output getImuData(const std::string& imu_name) const
     {
@@ -260,6 +251,27 @@ class VehicleApiBase : public UpdatableObject {
             : VehicleControllerException(message) {
         }
     };
+
+    private:
+    const LidarBase* findLidarByName(const std::string& lidar_name) const
+    {
+        const LidarBase* lidar = nullptr;
+
+        // Find lidar with the given name (for empty input name, return the first one found)
+        // Not efficient but should suffice given small number of lidars
+        uint count_lidars = getSensors().size(SensorBase::SensorType::Lidar);
+        for (uint i = 0; i < count_lidars; i++)
+        {
+            const LidarBase* current_lidar = static_cast<const LidarBase*>(getSensors().getByType(SensorBase::SensorType::Lidar, i));
+            if (current_lidar != nullptr && (current_lidar->getName() == lidar_name || lidar_name == ""))
+            {
+                lidar = current_lidar;
+                break;
+            }
+        }
+
+        return lidar;
+    }
 };
 
 

AirLib/include/api/VehicleSimApiBase.hpp

@@ -17,11 +17,6 @@ class VehicleSimApiBase : public msr::airlib::UpdatableObject {
 public:
     virtual ~VehicleSimApiBase() = default;
 
-    //default implementation so derived class doesn't have to call on UpdatableObject
-    virtual void reset() override
-    {
-        UpdatableObject::reset();
-    }
     virtual void update() override
     {
         UpdatableObject::update();

AirLib/include/common/AirSimSettings.hpp

@@ -26,6 +26,7 @@ struct AirSimSettings {
     static constexpr char const * kVehicleTypePX4 = "px4multirotor";
 	static constexpr char const * kVehicleTypeArduCopterSolo = "arducoptersolo";
 	static constexpr char const * kVehicleTypeSimpleFlight = "simpleflight";
+    static constexpr char const * kVehicleTypeArduCopter = "arducopter";
     static constexpr char const * kVehicleTypePhysXCar = "physxcar";
     static constexpr char const * kVehicleTypeComputerVision = "computervision";
 
@@ -88,11 +89,6 @@ struct AirSimSettings {
         {
         }
 
-        bool hasNan()
-        {
-            return std::isnan(yaw) || std::isnan(pitch) || std::isnan(roll);
-        }
-
         static Rotation nanRotation()
         {
             static const Rotation val(Utils::nan<float>(), Utils::nan<float>(), Utils::nan<float>());
@@ -333,6 +329,7 @@ struct AirSimSettings {
     int initial_view_mode = 3; //ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FLY_WITH_ME
     bool enable_rpc = true;
     std::string api_server_address = "";
+	int api_port = RpcLibPort;
     std::string physics_engine_name = "";
 
     std::string clock_type = "";
@@ -676,7 +673,7 @@ struct AirSimSettings {
         auto vehicle_type = Utils::toLower(settings_json.getString("VehicleType", ""));
 
         std::unique_ptr<VehicleSetting> vehicle_setting;
-        if (vehicle_type == kVehicleTypePX4 || vehicle_type == kVehicleTypeArduCopterSolo)
+        if (vehicle_type == kVehicleTypePX4 || vehicle_type == kVehicleTypeArduCopterSolo || vehicle_type == kVehicleTypeArduCopter)
             vehicle_setting = createMavLinkVehicleSetting(settings_json);
         //for everything else we don't need derived class yet
         else {
@@ -708,10 +705,7 @@ struct AirSimSettings {
         vehicle_setting->is_fpv_vehicle = settings_json.getBool("IsFpvVehicle",
             vehicle_setting->is_fpv_vehicle);
 
-        Settings rc_json;
-        if (settings_json.getChild("RC", rc_json)) {
-            loadRCSetting(simmode_name, rc_json, vehicle_setting->rc);
-        }
+        loadRCSetting(simmode_name, settings_json, vehicle_setting->rc);
 
         vehicle_setting->position = createVectorSetting(settings_json, vehicle_setting->position);
         vehicle_setting->rotation = createRotationSetting(settings_json, vehicle_setting->rotation);
@@ -992,6 +986,7 @@ struct AirSimSettings {
         //because for docker container default is 0.0.0.0 and people get really confused why things
         //don't work
         api_server_address = settings_json.getString("LocalHostIp", "");
+		api_port = settings_json.getInt("ApiServerPort", RpcLibPort);
         is_record_ui_visible = settings_json.getBool("RecordUIVisible", true);
         engine_sound = settings_json.getBool("EngineSound", false);
         enable_rpc = settings_json.getBool("EnableRpc", enable_rpc);

AirLib/include/common/Common.hpp

@@ -19,6 +19,8 @@
 #define _CRT_SECURE_NO_WARNINGS 1
 #endif
 
+#define RpcLibPort 41451
+
 namespace msr { namespace airlib {
 
 //numericals

AirLib/include/common/CommonStructs.hpp

@@ -269,7 +269,7 @@ struct RCData {
 
     unsigned int getSwitch(uint16_t index) const
     {
-        return switches && (1 << index) ? 1 : 0;
+        return switches & (1 << index) ? 1 : 0;
     }
 
     void add(const RCData& other)

AirLib/include/common/DelayLine.hpp

@@ -11,7 +11,7 @@
 namespace msr { namespace airlib {
 
 template<typename T>
-class DelayLine : UpdatableObject {
+class DelayLine : public UpdatableObject {
 public:
     DelayLine()
     {}
@@ -33,10 +33,8 @@ class DelayLine : UpdatableObject {
     }
 
     //*** Start: UpdatableState implementation ***//
-    virtual void reset() override
-    {
-        UpdatableObject::reset();
-
+     virtual void resetImplementation() override
+     {
         values_.clear();
         times_.clear();
         last_time_ = 0;

AirLib/include/common/FirstOrderFilter.hpp

@@ -12,7 +12,7 @@
 namespace msr { namespace airlib {
 
 template <typename T>
-class FirstOrderFilter : UpdatableObject {
+class FirstOrderFilter : public UpdatableObject {
     /*
     This class can be used to apply a first order filter on a signal.
     It allows different acceleration and deceleration time constants.
@@ -42,10 +42,8 @@ class FirstOrderFilter : UpdatableObject {
     }
 
     //*** Start: UpdatableState implementation ***//
-    virtual void reset() override
+    virtual void resetImplementation() override
     {
-        UpdatableObject::reset();
-
         last_time_ = clock()->nowNanos();
         input_ = initial_input_;
         output_ = initial_output_;

AirLib/include/common/FrequencyLimiter.hpp

@@ -10,7 +10,7 @@
 
 namespace msr { namespace airlib {
 
-class FrequencyLimiter : UpdatableObject {
+class FrequencyLimiter : public UpdatableObject {
 public:
     FrequencyLimiter(real_T frequency = Utils::max<float>(), real_T startup_delay = 0)
     {
@@ -24,13 +24,8 @@ class FrequencyLimiter : UpdatableObject {
     }
 
     //*** Start: UpdatableState implementation ***//
-    virtual void reset() override
+    virtual void resetImplementation() override
     {
-        //disable checks for reset/update sequence because
-        //this object may get created but not used
-        clearResetUpdateAsserts();  
-        UpdatableObject::reset();
-
         last_time_ = clock()->nowNanos();
         first_time_ = last_time_;
 
@@ -47,6 +42,13 @@ class FrequencyLimiter : UpdatableObject {
         startup_complete_ = false;
     }
 
+    virtual void failResetUpdateOrdering(std::string err) override
+    {
+        // Do nothing.
+        // Disable checks for reset/update sequence because
+        // this object may get created but not used.
+    }
+
     virtual void update() override
     {
         UpdatableObject::update();

AirLib/include/common/GaussianMarkov.hpp

@@ -11,7 +11,7 @@
 
 namespace msr { namespace airlib {
 
-class GaussianMarkov : UpdatableObject {
+class GaussianMarkov : public UpdatableObject {
 public:
     GaussianMarkov()
     {}
@@ -32,10 +32,8 @@ class GaussianMarkov : UpdatableObject {
     }
 
     //*** Start: UpdatableState implementation ***//
-    virtual void reset() override
+    virtual void resetImplementation() override
     {
-        UpdatableObject::reset();
-
         last_time_ = clock()->nowNanos();
         output_ = initial_output_;
         rand_.reset();

AirLib/include/common/StateReporterWrapper.hpp

@@ -37,19 +37,21 @@ class StateReporterWrapper : public UpdatableObject {
     }
 
     //*** Start: UpdatableState implementation ***//
-    virtual void reset() override
+    virtual void resetImplementation() override
     {
-        //disable checks for reset/update sequence because
-        //this object may get created but not used
-        clearResetUpdateAsserts();  
-        UpdatableObject::reset();
-
         last_time_ = clock()->nowNanos();
         clearReport();
         dt_stats_.clear();
         report_freq_.reset();
     }
 
+    virtual void failResetUpdateOrdering(std::string err) override
+    {
+        // Do nothing.
+        // Disable checks for reset/update sequence because
+        // this object may get created but not used.
+    }
+
     virtual void update() override
     {
         UpdatableObject::update();

AirLib/include/common/UpdatableContainer.hpp

@@ -31,10 +31,8 @@ class UpdatableContainer : public UpdatableObject {
 
 public:
     //*** Start: UpdatableState implementation ***//
-    virtual void reset() override
+    virtual void resetImplementation() override
     {
-        UpdatableObject::reset();
-
         for (TUpdatableObjectPtr& member : members_)
             member->reset();
     }

AirLib/include/common/UpdatableObject.hpp

@@ -27,17 +27,27 @@ init->reset calls for base-derived class that would be incorrect.
 
 class UpdatableObject {
 public:
-    virtual void reset()
-    {
+    void reset()
+    { 
+        if (reset_in_progress)
+            return;
+
+        reset_in_progress = true;
+        //TODO: Do we need this check anymore? Maybe reset() should be idempotent. 
+
         if (reset_called && !update_called)
-            throw std::runtime_error("Multiple reset() calls detected without call to update()");
+            failResetUpdateOrdering("Multiple reset() calls detected without call to update()");
 
         reset_called = true;
+
+        resetImplementation();
+        reset_in_progress = false;
     }
+
     virtual void update()
     {
         if (!reset_called)
-            throw std::runtime_error("reset() must be called first before update()");
+            failResetUpdateOrdering("reset() must be called first before update()");
         update_called = true;
     }
 
@@ -64,15 +74,16 @@ class UpdatableObject {
     }
 
 protected:
-    void clearResetUpdateAsserts()
+    virtual void resetImplementation() = 0;
+    virtual void failResetUpdateOrdering(std::string err)
     {
-        reset_called = false;
-        update_called = false;
+        throw std::runtime_error(err);
     }
 
 private:
     bool reset_called = false;
     bool update_called = false;
+    bool reset_in_progress = false;
 };
 
 }} //namespace