Merge branch 'master' into lidar

.gitignore

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

AirLib/include/api/RpcLibAdapatorsBase.hpp

@@ -675,7 +675,7 @@ class RpcLibAdapatorsBase {
         DistanceSensorData()
         {}
 
-        DistanceSensorData(const msr::airlib::DistanceBase::Output& s)
+        DistanceSensorData(const msr::airlib::DistanceSensorData& s)
         {
             time_stamp = s.time_stamp;
             distance = s.distance;
@@ -684,9 +684,9 @@ class RpcLibAdapatorsBase {
             relative_pose = s.relative_pose;
         }
 
-        msr::airlib::DistanceBase::Output to() const
+        msr::airlib::DistanceSensorData to() const
         {
-            msr::airlib::DistanceBase::Output d;
+            msr::airlib::DistanceSensorData d;
 
             d.time_stamp = time_stamp;
             d.distance = distance;

AirLib/include/api/RpcLibClientBase.hpp

@@ -50,7 +50,10 @@ class RpcLibClientBase {
 
     vector<string> simListSceneObjects(const string& name_regex = string(".*")) const;
     Pose simGetObjectPose(const std::string& object_name) const;
+    bool simLoadLevel(const string& level_name);
+    Vector3r simGetObjectScale(const std::string& object_name) const;
     bool simSetObjectPose(const std::string& object_name, const Pose& pose, bool teleport = true);
+    bool simSetObjectScale(const std::string& object_name, const Vector3r& scale);
 
     //task management APIs
     void cancelLastTask(const std::string& vehicle_name = "");
@@ -81,7 +84,7 @@ class RpcLibClientBase {
     msr::airlib::BarometerBase::Output getBarometerData(const std::string& barometer_name = "", const std::string& vehicle_name = "") const;
     msr::airlib::MagnetometerBase::Output getMagnetometerData(const std::string& magnetometer_name = "", const std::string& vehicle_name = "") const;
     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;
+    msr::airlib::DistanceSensorData getDistanceSensorData(const std::string& distance_sensor_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 = "");
@@ -95,14 +98,19 @@ class RpcLibClientBase {
     CollisionInfo simGetCollisionInfo(const std::string& vehicle_name = "") const;
 
     CameraInfo simGetCameraInfo(const std::string& camera_name, const std::string& vehicle_name = "") const;
-    void simSetCameraOrientation(const std::string& camera_name, const Quaternionr& orientation, const std::string& vehicle_name = "");
+    void simSetCameraPose(const std::string& camera_name, const Pose& pose, const std::string& vehicle_name = "");
     void simSetCameraFov(const std::string& camera_name, float fov_degrees, const std::string& vehicle_name = "");
 
     msr::airlib::Kinematics::State simGetGroundTruthKinematics(const std::string& vehicle_name = "") const;
     msr::airlib::Environment::State simGetGroundTruthEnvironment(const std::string& vehicle_name = "") const;
 
 	std::vector<std::string> simSwapTextures(const std::string& tags, int tex_id = 0, int component_id = 0, int material_id = 0);
 
+    // Recording APIs
+    void startRecording();
+    void stopRecording();
+    bool isRecording();
+
 protected:
     void* getClient();
     const void* getClient() const;

AirLib/include/api/VehicleApiBase.hpp

@@ -160,7 +160,7 @@ class VehicleApiBase : public UpdatableObject {
     }
 
     // Distance Sensor API
-    virtual DistanceBase::Output getDistanceSensorData(const std::string& distance_sensor_name) const
+    virtual DistanceSensorData getDistanceSensorData(const std::string& distance_sensor_name) const
     {
         auto *distance_sensor = static_cast<const DistanceBase*>(findSensorByName(distance_sensor_name, SensorBase::SensorType::Distance));
         if (distance_sensor == nullptr)

AirLib/include/api/VehicleSimApiBase.hpp

@@ -54,7 +54,7 @@ class VehicleSimApiBase : public msr::airlib::UpdatableObject {
     virtual const msr::airlib::Environment* getGroundTruthEnvironment() const = 0;
 
     virtual CameraInfo getCameraInfo(const std::string& camera_name) const = 0;
-    virtual void setCameraOrientation(const std::string& camera_name, const Quaternionr& orientation) = 0;
+    virtual void setCameraPose(const std::string& camera_name, const Pose& pose) = 0;
     virtual void setCameraFoV(const std::string& camera_name, float fov_degrees) = 0;
 
     virtual CollisionInfo getCollisionInfo() const = 0;

AirLib/include/api/WorldSimApiBase.hpp

@@ -25,6 +25,11 @@ class WorldSimApiBase {
 
     virtual ~WorldSimApiBase() = default;
 
+    // ------ Level setting apis ----- //
+    virtual bool loadLevel(const std::string& level_name) = 0;
+    virtual string spawnObject(string& object_name, const string& load_component, const Pose& pose, const Vector3r& scale) = 0;
+    virtual bool destroyObject(const string& object_name) = 0;
+
     virtual bool isPaused() const = 0;
     virtual void reset() = 0;
     virtual void pause(bool is_paused) = 0;
@@ -54,10 +59,17 @@ class WorldSimApiBase {
 
     virtual std::vector<std::string> listSceneObjects(const std::string& name_regex) const = 0;
     virtual Pose getObjectPose(const std::string& object_name) const = 0;
+    virtual Vector3r getObjectScale(const std::string& object_name) const = 0;
     virtual bool setObjectPose(const std::string& object_name, const Pose& pose, bool teleport) = 0;
+    virtual bool setObjectScale(const std::string& object_name, const Vector3r& scale) = 0;
 
 	virtual std::unique_ptr<std::vector<std::string>> swapTextures(const std::string& tag, int tex_id = 0, int component_id = 0, int material_id = 0) = 0;
     virtual vector<MeshPositionVertexBuffersResponse> getMeshPositionVertexBuffers() const = 0;
+
+    // Recording APIs
+    virtual void startRecording() = 0;
+    virtual void stopRecording() = 0;
+    virtual bool isRecording() const = 0;
 };
 
 

AirLib/include/common/AirSimSettings.hpp

@@ -25,8 +25,8 @@ struct AirSimSettings {
 public: //types
     static constexpr int kSubwindowCount = 3; //must be >= 3 for now
     static constexpr char const * kVehicleTypePX4 = "px4multirotor";
-	static constexpr char const * kVehicleTypeArduCopterSolo = "arducoptersolo";
-	static constexpr char const * kVehicleTypeSimpleFlight = "simpleflight";
+    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 * kVehicleTypeArduRover = "ardurover";
@@ -196,6 +196,12 @@ struct AirSimSettings {
     };
 
     struct DistanceSetting : SensorSetting {
+        // shared defaults
+        real_T min_distance = 20.0f / 100; //m
+        real_T max_distance = 4000.0f / 100; //m
+        Vector3r position = VectorMath::nanVector();
+        Rotation rotation = Rotation::nanRotation();
+        bool draw_debug_points = false;
     };
 
     struct LidarSetting : SensorSetting {
@@ -294,9 +300,9 @@ struct AirSimSettings {
         std::map<std::string, float> params;
     };
 
-	struct MavLinkVehicleSetting : public VehicleSetting {
-		MavLinkConnectionInfo connection_info;
-	};
+    struct MavLinkVehicleSetting : public VehicleSetting {
+        MavLinkConnectionInfo connection_info;
+    };
 
     struct SegmentationSetting {
         enum class InitMethodType {
@@ -327,6 +333,7 @@ struct AirSimSettings {
 
 public: //fields
     std::string simmode_name = "";
+    std::string level_name = "";
 
     std::vector<SubwindowSetting> subwindow_settings;
     RecordingSetting recording_setting;
@@ -340,7 +347,7 @@ struct AirSimSettings {
     int initial_view_mode = 2; //ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FLY_WITH_ME
     bool enable_rpc = true;
     std::string api_server_address = "";
-	int api_port = RpcLibPort;
+    int api_port = RpcLibPort;
     std::string physics_engine_name = "";
 
     std::string clock_type = "";
@@ -352,8 +359,8 @@ struct AirSimSettings {
     std::map<std::string, std::unique_ptr<VehicleSetting>> vehicles;
     CameraSetting camera_defaults;
     CameraDirectorSetting camera_director;
-	float speed_unit_factor =  1.0f;
-	std::string speed_unit_label = "m\\s";
+    float speed_unit_factor =  1.0f;
+    std::string speed_unit_label = "m\\s";
     std::map<std::string, std::unique_ptr<SensorSetting>> sensor_defaults;
 
 public: //methods
@@ -379,6 +386,7 @@ struct AirSimSettings {
         checkSettingsVersion(settings_json);
 
         loadCoreSimModeSettings(settings_json, simmode_getter);
+        loadLevelSettings(settings_json);
         loadDefaultCameraSetting(settings_json, camera_defaults);
         loadCameraDirectorSetting(settings_json, camera_director, simmode_name);
         loadSubWindowsSettings(settings_json, subwindow_settings);
@@ -514,6 +522,11 @@ struct AirSimSettings {
                 physics_engine_name = "PhysX"; //this value is only informational for now
         }
     }
+
+    void loadLevelSettings(const Settings& settings_json)
+    {
+        level_name = settings_json.getString("Default Environment", "");
+    }
 
     void loadViewModeSettings(const Settings& settings_json)
     {
@@ -626,7 +639,7 @@ struct AirSimSettings {
     {
         //these settings_json are expected in same section, not in another child
         std::unique_ptr<VehicleSetting> vehicle_setting_p = std::unique_ptr<VehicleSetting>(new MavLinkVehicleSetting());
-		MavLinkVehicleSetting* vehicle_setting = static_cast<MavLinkVehicleSetting*>(vehicle_setting_p.get());
+        MavLinkVehicleSetting* vehicle_setting = static_cast<MavLinkVehicleSetting*>(vehicle_setting_p.get());
 
         //TODO: we should be selecting remote if available else keyboard
         //currently keyboard is not supported so use rc as default
@@ -1021,7 +1034,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);
+        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);
@@ -1154,10 +1167,12 @@ struct AirSimSettings {
 
     static void initializeDistanceSetting(DistanceSetting& distance_setting, const Settings& settings_json)
     {
-        unused(distance_setting);
-        unused(settings_json);
+        distance_setting.min_distance = settings_json.getFloat("MinDistance", distance_setting.min_distance);
+        distance_setting.max_distance = settings_json.getFloat("MaxDistance", distance_setting.max_distance);
+        distance_setting.draw_debug_points = settings_json.getBool("DrawDebugPoints", distance_setting.draw_debug_points);
 
-        //TODO: set from json as needed
+        distance_setting.position = createVectorSetting(settings_json, distance_setting.position);
+        distance_setting.rotation = createRotationSetting(settings_json, distance_setting.rotation);
     }
 
     static void initializeLidarSetting(LidarSetting& lidar_setting, const Settings& settings_json)

AirLib/include/common/CommonStructs.hpp

@@ -311,6 +311,17 @@ struct LidarData {
     {}
 };
 
+struct DistanceSensorData {
+    TTimePoint time_stamp;
+    real_T distance;        //meters
+    real_T min_distance;    //m
+    real_T max_distance;    //m
+    Pose relative_pose;
+
+    DistanceSensorData()
+    {}
+};
+
 struct MeshPositionVertexBuffersResponse {
     Vector3r position;
     Quaternionr orientation;

AirLib/include/common/VectorMath.hpp

@@ -324,6 +324,9 @@ class VectorMathT {
 
 	static Vector3T toAngularVelocity(const QuaternionT& start, const QuaternionT& end, RealT dt)
 	{
+		if (dt == 0)
+			return Vector3T(0, 0, 0);
+
 		RealT p_s, r_s, y_s;
 		toEulerianAngle(start, p_s, r_s, y_s);
 

AirLib/include/sensors/SensorFactory.hpp

@@ -57,6 +57,8 @@ class SensorFactory {
             }
         }
     }
+
+    virtual ~SensorFactory() = default;
 };
 
 

AirLib/include/sensors/distance/DistanceBase.hpp

@@ -16,16 +16,6 @@ class DistanceBase  : public SensorBase {
         : SensorBase(sensor_name)
     {}
 
-public: //types
-    struct Output { //same fields as ROS message
-        TTimePoint time_stamp;
-        real_T distance;    //meters
-        real_T min_distance;//m
-        real_T max_distance;//m
-        Pose relative_pose;
-    };
-
-
 public:
     virtual void reportState(StateReporter& reporter) override
     {
@@ -35,20 +25,20 @@ class DistanceBase  : public SensorBase {
         reporter.writeValue("Dist-Curr", output_.distance);
     }
 
-    const Output& getOutput() const
+    const DistanceSensorData& getOutput() const
     {
         return output_;
     }
 
 protected:
-    void setOutput(const Output& output)
+    void setOutput(const DistanceSensorData& output)
     {
         output_ = output;
     }
 
 
 private:
-    Output output_;
+    DistanceSensorData output_;
 };
 
 

AirLib/include/sensors/distance/DistanceSimple.hpp

@@ -14,7 +14,7 @@
 
 namespace msr { namespace airlib {
 
-class DistanceSimple  : public DistanceBase {
+class DistanceSimple : public DistanceBase {
 public:
     DistanceSimple(const AirSimSettings::DistanceSetting& setting = AirSimSettings::DistanceSetting())
         : DistanceBase(setting.sensor_name)
@@ -62,17 +62,18 @@ class DistanceSimple  : public DistanceBase {
 
     virtual ~DistanceSimple() = default;
 
-protected:
-    virtual real_T getRayLength(const Pose& pose) = 0;
-    const DistanceSimpleParams& getParams()
+    const DistanceSimpleParams& getParams() const
     {
         return params_;
     }
 
+protected:
+    virtual real_T getRayLength(const Pose& pose) = 0;
+
 private: //methods
-    Output getOutputInternal()
+    DistanceSensorData getOutputInternal()
     {
-        Output output;
+        DistanceSensorData output;
         const GroundTruth& ground_truth = getGroundTruth();
 
         //order of Pose addition is important here because it also adds quaternions which is not commutative!
@@ -97,7 +98,7 @@ class DistanceSimple  : public DistanceBase {
     RandomGeneratorGausianR uncorrelated_noise_;
 
     FrequencyLimiter freq_limiter_;
-    DelayLine<Output> delay_line_;
+    DelayLine<DistanceSensorData> delay_line_;
 
     //start time
 };