Distance sensor Upgrades (microsoft#2807)

* Add DrawDebugPoint option for distance sensor

* Expose Distance Sensor settings

Plus fix for Car sensor default pose

* [Pythonclient] Fix DistanceSensorData type

Distances are float, not Quaternion

* [docs] Add info about Distance sensor settings and usage

* Use custom sensor orientation in Mavlink Distance sensor message

* [ros] Change to DistanceSensorData struct

* [Pythonclient] Example script for distance sensor with 2 cars

AirLib/include/api/RpcLibAdapatorsBase.hpp

@@ -672,7 +672,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;
@@ -681,9 +681,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

@@ -84,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;
 
     // sensor omniscient APIs
     vector<int> simGetLidarSegmentation(const std::string& lidar_name = "", const std::string& vehicle_name = "") const;

AirLib/include/api/VehicleApiBase.hpp

@@ -215,7 +215,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
     {
         const DistanceBase* distance_sensor = nullptr;
 

AirLib/include/common/AirSimSettings.hpp

@@ -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 {
@@ -1161,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

@@ -304,6 +304,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/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
 };

AirLib/include/sensors/distance/DistanceSimpleParams.hpp

@@ -14,7 +14,13 @@ namespace msr { namespace airlib {
 struct DistanceSimpleParams {
     real_T min_distance = 20.0f / 100; //m
     real_T max_distance = 4000.0f / 100; //m
-    Pose relative_pose;
+
+    Pose relative_pose {
+        Vector3r(0,0,-1),           // position - a little above vehicle (especially for cars) or Vector3r::Zero()
+        Quaternionr::Identity()     // orientation - by default Quaternionr(1, 0, 0, 0)
+    };
+
+    bool draw_debug_points = false;
 
 /*
     Ref: A Stochastic Approach to Noise Modeling for Barometric Altimeters
@@ -40,7 +46,33 @@ struct DistanceSimpleParams {
 
     void initializeFromSettings(const AirSimSettings::DistanceSetting& settings)
     {
-        unused(settings);
+        std::string simmode_name = AirSimSettings::singleton().simmode_name;
+
+        min_distance = settings.min_distance;
+        max_distance = settings.max_distance;
+
+        draw_debug_points = settings.draw_debug_points;
+
+        relative_pose.position = settings.position;
+        if (std::isnan(relative_pose.position.x()))
+            relative_pose.position.x() = 0;
+        if (std::isnan(relative_pose.position.y()))
+            relative_pose.position.y() = 0;
+        if (std::isnan(relative_pose.position.z())) {
+            if (simmode_name == "Multirotor")
+                relative_pose.position.z() = 0;
+            else
+                relative_pose.position.z() = -1;  // a little bit above for cars
+        }
+
+        float pitch, roll, yaw;
+        pitch = !std::isnan(settings.rotation.pitch) ? settings.rotation.pitch : 0;
+        roll = !std::isnan(settings.rotation.roll) ? settings.rotation.roll : 0;
+        yaw = !std::isnan(settings.rotation.yaw) ? settings.rotation.yaw : 0;
+        relative_pose.orientation = VectorMath::toQuaternion(
+            Utils::degreesToRadians(pitch),   //pitch - rotation around Y axis
+            Utils::degreesToRadians(roll),    //roll  - rotation around X axis
+            Utils::degreesToRadians(yaw));    //yaw   - rotation around Z axis
     }
 };
 

AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp

@@ -116,15 +116,13 @@ class MavLinkMultirotorApi : public MultirotorApiBase
             const uint count_distance_sensors = getSensors().size(SensorBase::SensorType::Distance);
             if (count_distance_sensors != 0) {
                 const auto& distance_output = getDistanceSensorData("");
-                float pitch, roll, yaw;
-                VectorMath::toEulerianAngle(distance_output.relative_pose.orientation, pitch, roll, yaw);
 
                 sendDistanceSensor(distance_output.min_distance / 100, //m -> cm
                     distance_output.max_distance / 100, //m -> cm
                     distance_output.distance,
                     0, //sensor type: //TODO: allow changing in settings?
                     77, //sensor id, //TODO: should this be something real?
-                    pitch); //TODO: convert from radians to degrees?
+                    distance_output.relative_pose.orientation); //TODO: convert from radians to degrees?
             }
 
             const uint count_gps_sensors = getSensors().size(SensorBase::SensorType::Gps);
@@ -1379,7 +1377,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         last_sensor_message_ = hil_sensor;
     }
 
-    void sendDistanceSensor(float min_distance, float max_distance, float current_distance, float sensor_type, float sensor_id, float orientation)
+    void sendDistanceSensor(float min_distance, float max_distance, float current_distance, float sensor_type, float sensor_id, Quaternionr orientation)
     {
         if (!is_simulation_mode_)
             throw std::logic_error("Attempt to send simulated distance sensor messages while not in simulation mode");
@@ -1392,7 +1390,14 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         distance_sensor.current_distance = static_cast<uint16_t>(current_distance);
         distance_sensor.type = static_cast<uint8_t>(sensor_type);
         distance_sensor.id = static_cast<uint8_t>(sensor_id);
-        distance_sensor.orientation = static_cast<uint8_t>(orientation);
+
+        // Use custom orientation
+        distance_sensor.orientation = 100;  // MAV_SENSOR_ROTATION_CUSTOM
+        distance_sensor.quaternion[0] = orientation.w();
+        distance_sensor.quaternion[1] = orientation.x();
+        distance_sensor.quaternion[2] = orientation.y();
+        distance_sensor.quaternion[3] = orientation.z();
+
         //TODO: use covariance parameter?
 
         if (hil_node_ != nullptr) {

AirLib/src/api/RpcLibClientBase.cpp

@@ -184,7 +184,7 @@ msr::airlib::GpsBase::Output RpcLibClientBase::getGpsData(const std::string& gps
     return pimpl_->client.call("getGpsData", gps_name, vehicle_name).as<RpcLibAdapatorsBase::GpsData>().to();
 }
 
-msr::airlib::DistanceBase::Output RpcLibClientBase::getDistanceSensorData(const std::string& distance_sensor_name, const std::string& vehicle_name) const
+msr::airlib::DistanceSensorData RpcLibClientBase::getDistanceSensorData(const std::string& distance_sensor_name, const std::string& vehicle_name) const
 {
     return pimpl_->client.call("getDistanceSensorData", distance_sensor_name, vehicle_name).as<RpcLibAdapatorsBase::DistanceSensorData>().to();
 }

PythonClient/airsim/types.py

@@ -400,9 +400,9 @@ class GpsData(MsgpackMixin):
 
 class DistanceSensorData(MsgpackMixin):
     time_stamp = np.uint64(0)
-    distance = Quaternionr()
-    min_distance = Quaternionr()
-    max_distance = Quaternionr()
+    distance = 0.0
+    min_distance = 0.0
+    max_distance = 0.0
     relative_pose = Pose()
 
 class PIDGains():

PythonClient/car/distance_sensor_multi.py

@@ -0,0 +1,49 @@
+import airsim
+import time
+
+'''
+An example script showing usage of Distance sensor to measure distance between 2 Car vehicles
+Settings -
+
+{
+  "SettingsVersion": 1.2,
+  "SimMode": "Car",
+  "Vehicles": {
+    "Car1": {
+      "VehicleType": "PhysXCar",
+      "AutoCreate": true,
+      "Sensors": {
+        "Distance": {
+            "SensorType": 5,
+            "Enabled" : true,
+            "DrawDebugPoints": true
+        }
+      }
+    },
+    "Car2": {
+        "VehicleType": "PhysXCar",
+        "AutoCreate": true,
+        "X": 10, "Y": 0, "Z": 0,
+        "Sensors": {
+            "Distance": {
+                "SensorType": 5,
+                "Enabled" : true,
+                "DrawDebugPoints": true
+            }
+        }
+    }
+  }
+}
+
+Car2 is placed in front of Car 1
+
+'''
+
+client = airsim.CarClient()
+client.confirmConnection()
+
+while True:
+    data_car1 = client.getDistanceSensorData(vehicle_name="Car1")
+    data_car2 = client.getDistanceSensorData(vehicle_name="Car2")
+    print(f"Distance sensor data: Car1: {data_car1.distance}, Car2: {data_car2.distance}")
+    time.sleep(1.0)