Merge pull request #1920 from madratman/PR/Baro_GPS_IMU_Magneto_APIs

Add APIs for Barometer, IMU, GPS, Magnetometer

AirLib/include/api/RpcLibAdapatorsBase.hpp

@@ -500,6 +500,200 @@ class RpcLibAdapatorsBase {
             return d;
         }
     };
+
+    struct ImuData {
+        msr::airlib::TTimePoint time_stamp;
+        Quaternionr orientation;
+        Vector3r angular_velocity;
+        Vector3r linear_acceleration;
+
+        MSGPACK_DEFINE_MAP(time_stamp, orientation, angular_velocity, linear_acceleration);
+
+        ImuData()
+        {}
+
+        ImuData(const msr::airlib::ImuBase::Output& s)
+        {
+            time_stamp = s.time_stamp;
+            orientation = s.orientation;
+            angular_velocity = s.angular_velocity;
+            linear_acceleration = s.linear_acceleration;
+        }
+
+        msr::airlib::ImuBase::Output to() const
+        {
+            msr::airlib::ImuBase::Output d;
+
+            d.time_stamp = time_stamp;
+            d.orientation = orientation.to();
+            d.angular_velocity = angular_velocity.to();
+            d.linear_acceleration = linear_acceleration.to();
+
+            return d;
+        }
+    };
+
+    struct BarometerData {
+        msr::airlib::TTimePoint time_stamp;
+        msr::airlib::real_T altitude;
+        msr::airlib::real_T pressure;
+        msr::airlib::real_T qnh;
+
+        MSGPACK_DEFINE_MAP(time_stamp, altitude, pressure, qnh);
+
+        BarometerData()
+        {}
+
+        BarometerData(const msr::airlib::BarometerBase::Output& s)
+        {
+            time_stamp = s.time_stamp;
+            altitude = s.altitude;
+            pressure = s.pressure;
+            qnh = s.qnh;
+        }
+
+        msr::airlib::BarometerBase::Output to() const
+        {
+            msr::airlib::BarometerBase::Output d;
+
+            d.time_stamp = time_stamp;
+            d.altitude = altitude;
+            d.pressure = pressure;
+            d.qnh = qnh;
+
+            return d;
+        }
+    };
+
+    struct MagnetometerData {
+        msr::airlib::TTimePoint time_stamp;
+        Vector3r magnetic_field_body;
+        std::vector<float> magnetic_field_covariance; // not implemented in MagnetometerBase.hpp
+
+        MSGPACK_DEFINE_MAP(time_stamp, magnetic_field_body, magnetic_field_covariance);
+
+        MagnetometerData()
+        {}
+
+        MagnetometerData(const msr::airlib::MagnetometerBase::Output& s)
+        {
+            time_stamp = s.time_stamp;
+            magnetic_field_body = s.magnetic_field_body;
+            magnetic_field_covariance = s.magnetic_field_covariance;
+        }
+
+        msr::airlib::MagnetometerBase::Output to() const
+        {
+            msr::airlib::MagnetometerBase::Output d;
+
+            d.time_stamp = time_stamp;
+            d.magnetic_field_body = magnetic_field_body.to();
+            d.magnetic_field_covariance = magnetic_field_covariance;
+
+            return d;
+        }
+    };
+
+    struct GnssReport {
+        GeoPoint geo_point;
+        msr::airlib::real_T eph = 0.0, epv = 0.0; 
+        Vector3r velocity;
+        msr::airlib::GpsBase::GnssFixType fix_type;
+        uint64_t time_utc = 0;
+
+        MSGPACK_DEFINE_MAP(geo_point, eph, epv, velocity, fix_type, time_utc);
+
+        GnssReport()
+        {}
+
+        GnssReport(const msr::airlib::GpsBase::GnssReport& s)
+        {
+            geo_point = s.geo_point;
+            eph = s.eph;
+            epv = s.epv;
+            velocity = s.velocity;
+            fix_type = s.fix_type;
+            time_utc = s.time_utc;
+        }
+
+        msr::airlib::GpsBase::GnssReport to() const
+        {
+            msr::airlib::GpsBase::GnssReport d;
+
+            d.geo_point = geo_point.to();
+            d.eph = eph;
+            d.epv = epv;
+            d.velocity = velocity.to();
+            d.fix_type = fix_type;
+            d.time_utc = time_utc;
+
+            return d;
+        }
+    };
+
+    struct GpsData {
+        msr::airlib::TTimePoint time_stamp;
+        GnssReport gnss;
+        bool is_valid = false;
+
+        MSGPACK_DEFINE_MAP(time_stamp, gnss, is_valid);
+
+        GpsData()
+        {}
+
+        GpsData(const msr::airlib::GpsBase::Output& s)
+        {
+            time_stamp = s.time_stamp;
+            gnss = s.gnss;
+            is_valid = s.is_valid;
+        }
+
+        msr::airlib::GpsBase::Output to() const
+        {
+            msr::airlib::GpsBase::Output d;
+
+            d.time_stamp = time_stamp;
+            d.gnss = gnss.to();
+            d.is_valid = is_valid;
+
+            return d;
+        }
+    };
+
+    struct DistanceSensorData {
+        msr::airlib::TTimePoint time_stamp;
+        msr::airlib::real_T distance;    //meters
+        msr::airlib::real_T min_distance;//m
+        msr::airlib::real_T max_distance;//m
+        Pose relative_pose;
+
+        MSGPACK_DEFINE_MAP(time_stamp, distance, min_distance, max_distance, relative_pose);
+
+        DistanceSensorData()
+        {}
+
+        DistanceSensorData(const msr::airlib::DistanceBase::Output& s)
+        {
+            time_stamp = s.time_stamp;
+            distance = s.distance;
+            min_distance = s.min_distance;
+            max_distance = s.max_distance;
+            relative_pose = s.relative_pose;
+        }
+
+        msr::airlib::DistanceBase::Output to() const
+        {
+            msr::airlib::DistanceBase::Output d;
+
+            d.time_stamp = time_stamp;
+            d.distance = distance;
+            d.min_distance = min_distance;
+            d.max_distance = max_distance;
+            d.relative_pose = relative_pose.to();
+
+            return d;
+        }
+    };
 };
 
 }} //namespace
@@ -508,5 +702,6 @@ MSGPACK_ADD_ENUM(msr::airlib::SafetyEval::SafetyViolationType_);
 MSGPACK_ADD_ENUM(msr::airlib::SafetyEval::ObsAvoidanceStrategy);
 MSGPACK_ADD_ENUM(msr::airlib::ImageCaptureBase::ImageType);
 MSGPACK_ADD_ENUM(msr::airlib::WorldSimApiBase::WeatherParameter);
+MSGPACK_ADD_ENUM(msr::airlib::GpsBase::GnssFixType);
 
 #endif

AirLib/include/api/RpcLibClientBase.hpp

@@ -7,6 +7,11 @@
 #include "common/Common.hpp"
 #include "common/CommonStructs.hpp"
 #include "common/ImageCaptureBase.hpp"
+#include "sensors/imu/ImuBase.hpp"
+#include "sensors/barometer/BarometerBase.hpp"
+#include "sensors/magnetometer/MagnetometerBase.hpp"
+#include "sensors/gps/GpsBase.hpp"
+#include "sensors/distance/DistanceBase.hpp"
 #include "physics/Kinematics.hpp"
 #include "physics/Environment.hpp"
 #include "api/WorldSimApiBase.hpp"
@@ -61,7 +66,13 @@ class RpcLibClientBase {
 
     msr::airlib::GeoPoint getHomeGeoPoint(const std::string& vehicle_name = "") const;
 
+    // sensor APIs
     msr::airlib::LidarData getLidarData(const std::string& lidar_name = "", const std::string& vehicle_name = "") const;
+    msr::airlib::ImuBase::Output getImuData(const std::string& imu_name = "", const std::string& vehicle_name = "") const;
+    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;
 
     Pose simGetVehiclePose(const std::string& vehicle_name = "") const;
     void simSetVehiclePose(const Pose& pose, bool ignore_collision, const std::string& vehicle_name = "");

AirLib/include/api/VehicleApiBase.hpp

@@ -13,6 +13,11 @@
 #include "common/ImageCaptureBase.hpp"
 #include "sensors/SensorCollection.hpp"
 #include "sensors/lidar/LidarBase.hpp"
+#include "sensors/imu/ImuBase.hpp"
+#include "sensors/barometer/BarometerBase.hpp"
+#include "sensors/magnetometer/MagnetometerBase.hpp"
+#include "sensors/distance/DistanceBase.hpp"
+#include "sensors/gps/GpsBase.hpp"
 #include <exception>
 #include <string>
 
@@ -125,6 +130,113 @@ class VehicleApiBase : public UpdatableObject {
         return lidar->getOutput();
     }
 
+    // IMU API
+    virtual ImuBase::Output getImuData(const std::string& imu_name) const
+    {
+        const ImuBase* imu = nullptr;
+
+        // Find imu with the given name (for empty input name, return the first one found)
+        // Not efficient but should suffice given small number of imus
+        uint count_imus = getSensors().size(SensorBase::SensorType::Imu);
+        for (uint i = 0; i < count_imus; i++)
+        {
+            const ImuBase* current_imu = static_cast<const ImuBase*>(getSensors().getByType(SensorBase::SensorType::Imu, i));
+            if (current_imu != nullptr && (current_imu->getName() == imu_name || imu_name == ""))
+            {
+                imu = current_imu;
+                break;
+            }
+        }
+        if (imu == nullptr)
+            throw VehicleControllerException(Utils::stringf("No IMU with name %s exist on vehicle", imu_name.c_str()));
+
+        return imu->getOutput();
+    }
+
+    // Barometer API
+    virtual BarometerBase::Output getBarometerData(const std::string& barometer_name) const
+    {
+        const BarometerBase* barometer = nullptr;
+
+        uint count_barometers = getSensors().size(SensorBase::SensorType::Barometer);
+        for (uint i = 0; i < count_barometers; i++)
+        {
+            const BarometerBase* current_barometer = static_cast<const BarometerBase*>(getSensors().getByType(SensorBase::SensorType::Barometer, i));
+            if (current_barometer != nullptr && (current_barometer->getName() == barometer_name || barometer_name == ""))
+            {
+                barometer = current_barometer;
+                break;
+            }
+        }
+        if (barometer == nullptr)
+            throw VehicleControllerException(Utils::stringf("No barometer with name %s exist on vehicle", barometer_name.c_str()));
+
+        return barometer->getOutput();
+    }
+
+    // Magnetometer API
+    virtual MagnetometerBase::Output getMagnetometerData(const std::string& magnetometer_name) const
+    {
+        const MagnetometerBase* magnetometer = nullptr;
+
+        uint count_magnetometers = getSensors().size(SensorBase::SensorType::Magnetometer);
+        for (uint i = 0; i < count_magnetometers; i++)
+        {
+            const MagnetometerBase* current_magnetometer = static_cast<const MagnetometerBase*>(getSensors().getByType(SensorBase::SensorType::Magnetometer, i));
+            if (current_magnetometer != nullptr && (current_magnetometer->getName() == magnetometer_name || magnetometer_name == ""))
+            {
+                magnetometer = current_magnetometer;
+                break;
+            }
+        }
+        if (magnetometer == nullptr)
+            throw VehicleControllerException(Utils::stringf("No magnetometer with name %s exist on vehicle", magnetometer_name.c_str()));
+
+        return magnetometer->getOutput();
+    }
+
+    // Gps API
+    virtual GpsBase::Output getGpsData(const std::string& gps_name) const
+    {
+        const GpsBase* gps = nullptr;
+
+        uint count_gps = getSensors().size(SensorBase::SensorType::Gps);
+        for (uint i = 0; i < count_gps; i++)
+        {
+            const GpsBase* current_gps = static_cast<const GpsBase*>(getSensors().getByType(SensorBase::SensorType::Gps, i));
+            if (current_gps != nullptr && (current_gps->getName() == gps_name || gps_name == ""))
+            {
+                gps = current_gps;
+                break;
+            }
+        }
+        if (gps == nullptr)
+            throw VehicleControllerException(Utils::stringf("No gps with name %s exist on vehicle", gps_name.c_str()));
+
+        return gps->getOutput();
+    }
+
+    // Distance Sensor API
+    virtual DistanceBase::Output getDistanceSensorData(const std::string& distance_sensor_name) const
+    {
+        const DistanceBase* distance_sensor = nullptr;
+
+        uint count_distance_sensors = getSensors().size(SensorBase::SensorType::Distance);
+        for (uint i = 0; i < count_distance_sensors; i++)
+        {
+            const DistanceBase* current_distance_sensor = static_cast<const DistanceBase*>(getSensors().getByType(SensorBase::SensorType::Distance, i));
+            if (current_distance_sensor != nullptr && (current_distance_sensor->getName() == distance_sensor_name || distance_sensor_name == ""))
+            {
+                distance_sensor = current_distance_sensor;
+                break;
+            }
+        }
+        if (distance_sensor == nullptr)
+            throw VehicleControllerException(Utils::stringf("No distance sensor with name %s exist on vehicle", distance_sensor_name.c_str()));
+
+        return distance_sensor->getOutput();
+    }
+
     virtual ~VehicleApiBase() = default;
 
     //exceptions

AirLib/include/sensors/barometer/BarometerBase.hpp

@@ -18,6 +18,7 @@ class BarometerBase  : public SensorBase {
 
 public: //types
     struct Output { //same fields as ROS message
+        TTimePoint time_stamp;
         real_T altitude;    //meters
         real_T pressure;    //Pascal
         real_T qnh;

AirLib/include/sensors/barometer/BarometerSimple.hpp

@@ -93,6 +93,8 @@ class BarometerSimple  : public BarometerBase {
         output.altitude = (1 - pow(pressure / EarthUtils::SeaLevelPressure, 0.190284f)) * 145366.45f * 0.3048f;
         output.qnh = params_.qnh;
 
+        output.time_stamp = clock()->nowNanos();
+
         return output;
     }
 

AirLib/include/sensors/distance/DistanceBase.hpp

@@ -18,6 +18,7 @@ class DistanceBase  : public SensorBase {
 
 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

AirLib/include/sensors/distance/DistanceSimple.hpp

@@ -88,6 +88,7 @@ class DistanceSimple  : public DistanceBase {
         output.min_distance = params_.min_distance;
         output.max_distance = params_.max_distance;
         output.relative_pose = params_.relative_pose;
+        output.time_stamp = clock()->nowNanos();
 
         return output;
     }