From 2a77fd94a58f662600a35c012ad3767faf81e5a5 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Thu, 8 Feb 2018 18:10:42 -0800 Subject: [PATCH] refactor sensor architecture for easy pluggability for sensors derived from Unreal engine --- .gitignore | 1 + AirLib/AirLib.vcxproj | 1 + AirLib/AirLib.vcxproj.filters | 3 + AirLib/include/common/VectorMath.hpp | 44 +++++++++- AirLib/include/physics/Environment.hpp | 10 --- AirLib/include/physics/PhysicsBody.hpp | 5 +- AirLib/include/sensors/SensorBase.hpp | 8 ++ AirLib/include/sensors/SensorCollection.hpp | 19 ++--- AirLib/include/sensors/SensorFactory.hpp | 38 +++++++++ .../include/sensors/distance/DistanceBase.hpp | 10 +-- .../sensors/distance/DistanceSimple.hpp | 15 ++-- .../sensors/distance/DistanceSimpleParams.hpp | 8 +- .../magnetometer/MagnetometerSimple.hpp | 1 + .../vehicles/multirotor/MultiRotor.hpp | 5 -- .../vehicles/multirotor/MultiRotorParams.hpp | 81 ++++++++++--------- .../multirotor/MultiRotorParamsFactory.hpp | 8 +- .../multirotor/configs/Px4MultiRotor.hpp | 36 ++++++--- .../multirotor/configs/RosFlightQuadX.hpp | 23 ++++-- .../multirotor/configs/SimpleFlightQuadX.hpp | 25 +++--- .../controllers/MavLinkDroneController.hpp | 23 +++--- .../ros_flight/AirSimRosFlightBoard.hpp | 6 +- AirLibUnitTests/PixhawkTest.hpp | 4 +- AirLibUnitTests/RosFlightTest.hpp | 4 +- AirLibUnitTests/SimpleFlightTest.hpp | 5 +- .../Source/Multirotor/MultiRotorConnector.cpp | 4 +- .../Multirotor/SimModeWorldMultiRotor.cpp | 10 ++- .../UnrealSensors/UnrealDistanceSensor.cpp | 31 +++++++ .../UnrealSensors/UnrealDistanceSensor.h | 24 ++++++ .../UnrealSensors/UnrealSensorFactory.cpp | 34 ++++++++ .../UnrealSensors/UnrealSensorFactory.h | 25 ++++++ .../AirSim/Source/VehiclePawnWrapper.cpp | 20 ----- .../AirSim/Source/VehiclePawnWrapper.h | 2 - 32 files changed, 367 insertions(+), 166 deletions(-) create mode 100644 AirLib/include/sensors/SensorFactory.hpp create mode 100644 Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealDistanceSensor.cpp create mode 100644 Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealDistanceSensor.h create mode 100644 Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.cpp create mode 100644 Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.h diff --git a/.gitignore b/.gitignore index 76fc62cddd..d73973acc9 100644 --- a/.gitignore +++ b/.gitignore @@ -275,6 +275,7 @@ ModelManifest.xml /AirLib/cmake_install.cmake /MavLinkCom/lib/libMavLinkCom.a /Unreal/Plugins/AirSim/Source/AirLib/ +/Unreal/Environments/Blocks/Plugins/AirSim/ /enc_temp_folder /cmake/MavLinkCom/MavLinkTest/MavLinkTest /cmake/MavLinkCom/lib/libMavLinkCom.a diff --git a/AirLib/AirLib.vcxproj b/AirLib/AirLib.vcxproj index 20eb6003b8..3540bea4d6 100644 --- a/AirLib/AirLib.vcxproj +++ b/AirLib/AirLib.vcxproj @@ -68,6 +68,7 @@ + diff --git a/AirLib/AirLib.vcxproj.filters b/AirLib/AirLib.vcxproj.filters index 1e2a330ea4..6498f76dbf 100644 --- a/AirLib/AirLib.vcxproj.filters +++ b/AirLib/AirLib.vcxproj.filters @@ -471,6 +471,9 @@ Header Files + + Header Files + diff --git a/AirLib/include/common/VectorMath.hpp b/AirLib/include/common/VectorMath.hpp index 92fd2b2ecf..5a17572194 100644 --- a/AirLib/include/common/VectorMath.hpp +++ b/AirLib/include/common/VectorMath.hpp @@ -62,6 +62,10 @@ class VectorMathT { { return VectorMathT::subtract(lhs, rhs); } + friend Pose operator+(const Pose& lhs, const Pose& rhs) + { + return VectorMathT::add(lhs, rhs); + } friend bool operator==(const Pose& lhs, const Pose& rhs) { return lhs.position == rhs.position && lhs.orientation.coeffs() == rhs.orientation.coeffs(); @@ -293,6 +297,7 @@ class VectorMathT { QuaternionT net_q(dq_unit.coeffs() * dt + orientation.coeffs()); return net_q.normalized(); } + //all angles in radians static QuaternionT toQuaternion(RealT pitch, RealT roll, RealT yaw) { QuaternionT q; @@ -310,7 +315,7 @@ class VectorMathT { return q; } - //from http://osrf-distributions.s3.amazonaws.com/gazebo/api/dev/Pose_8hh_source.html + //from https://github.com/arpg/Gazebo/blob/master/gazebo/math/Pose.cc static Vector3T coordPositionSubtract(const Pose& lhs, const Pose& rhs) { QuaternionT tmp(0, @@ -329,11 +334,28 @@ class VectorMathT { result.normalize(); return result; } + static Vector3T coordPositionAdd(const Pose& lhs, const Pose& rhs) + { + QuaternionT tmp(0, lhs.position.x(), lhs.position.y(), lhs.position.z()); + + tmp = rhs.orientation * (tmp * rhs.orientation.inverse()); + + return tmp.vec() + rhs.position; + } + static QuaternionT coordOrientationAdd(const QuaternionT& lhs, const QuaternionT& rhs) + { + QuaternionT result(rhs * lhs); + result.normalize(); + return result; + } static Pose subtract(const Pose& lhs, const Pose& rhs) { return Pose(coordPositionSubtract(lhs, rhs), coordOrientationSubtract(lhs.orientation, rhs.orientation)); } - + static Pose add(const Pose& lhs, const Pose& rhs) + { + return Pose(coordPositionAdd(lhs, rhs), coordOrientationAdd(lhs.orientation, rhs.orientation)); + } static std::string toString(const Vector3T& vect, const char* prefix = nullptr) { @@ -399,6 +421,24 @@ class VectorMathT { static QuaternionT quaternionFromYaw(RealT yaw) { return QuaternionT(Eigen::AngleAxisd(yaw, Vector3T::UnitZ())); } + + static const Vector3T front() + { + static Vector3T v(1, 0, 0); + return v; + } + + static const Vector3T down() + { + static Vector3T v(0, 0, 1); + return v; + } + + static const Vector3T right() + { + static Vector3T v(0, 1, 0); + return v; + } }; typedef VectorMathT, double> VectorMathd; typedef VectorMathT, float> VectorMathf; diff --git a/AirLib/include/physics/Environment.hpp b/AirLib/include/physics/Environment.hpp index 44fdab2e83..a3a8c115e5 100644 --- a/AirLib/include/physics/Environment.hpp +++ b/AirLib/include/physics/Environment.hpp @@ -78,15 +78,6 @@ class Environment : public UpdatableObject { return current_; } - void setDistance(const real_T distance) - { - distance_ = distance; - } - real_T getDistance() const - { - return distance_; - } - //*** Start: UpdatableState implementation ***// virtual void reset() { @@ -116,7 +107,6 @@ class Environment : public UpdatableObject { private: State initial_, current_; EarthUtils::HomeGeoPoint home_geo_point_; - real_T distance_; }; }} //namespace diff --git a/AirLib/include/physics/PhysicsBody.hpp b/AirLib/include/physics/PhysicsBody.hpp index cf402b452d..7ba721665d 100644 --- a/AirLib/include/physics/PhysicsBody.hpp +++ b/AirLib/include/physics/PhysicsBody.hpp @@ -55,10 +55,7 @@ class PhysicsBody : public UpdatableObject { { collision_info_ = collision_info; } - virtual void setDistance(const real_T distance) - { - environment_->setDistance(distance); - } + public: //methods //constructors diff --git a/AirLib/include/sensors/SensorBase.hpp b/AirLib/include/sensors/SensorBase.hpp index 1839b9c167..6646776b37 100644 --- a/AirLib/include/sensors/SensorBase.hpp +++ b/AirLib/include/sensors/SensorBase.hpp @@ -19,6 +19,14 @@ namespace msr { namespace airlib { set the sensor in good-to-use state by call to reset. */ class SensorBase : public UpdatableObject { +public: + enum class SensorType : uint { + Barometer = 1, + Imu = 2, + Gps = 3, + Magnetometer = 4, + Distance = 5 + }; protected: struct GroundTruth { const Kinematics::State* kinematics; diff --git a/AirLib/include/sensors/SensorCollection.hpp b/AirLib/include/sensors/SensorCollection.hpp index 473c09f5e7..5904b2fed1 100644 --- a/AirLib/include/sensors/SensorCollection.hpp +++ b/AirLib/include/sensors/SensorCollection.hpp @@ -14,16 +14,9 @@ namespace msr { namespace airlib { class SensorCollection : UpdatableObject { public: //types - enum class SensorType : uint { - Barometer = 1, - Imu = 2, - Gps = 3, - Magnetometer = 4, - Distance = 5 - }; typedef SensorBase* SensorBasePtr; public: - void insert(SensorBasePtr sensor, SensorType type) + void insert(SensorBasePtr sensor, SensorBase::SensorType type) { auto type_int = static_cast(type); const auto& it = sensors_.find(type_int); @@ -36,7 +29,7 @@ class SensorCollection : UpdatableObject { } } - const SensorBase* getByType(SensorType type, uint index = 0) const + const SensorBase* getByType(SensorBase::SensorType type, uint index = 0) const { auto type_int = static_cast(type); const auto& it = sensors_.find(type_int); @@ -48,7 +41,7 @@ class SensorCollection : UpdatableObject { } } - uint size(SensorType type) const + uint size(SensorBase::SensorType type) const { auto type_int = static_cast(type); const auto& it = sensors_.find(type_int); @@ -69,7 +62,11 @@ class SensorCollection : UpdatableObject { } } - + void clear() + { + sensors_.clear(); + } + //*** Start: UpdatableState implementation ***// virtual void reset() override { diff --git a/AirLib/include/sensors/SensorFactory.hpp b/AirLib/include/sensors/SensorFactory.hpp new file mode 100644 index 0000000000..5eef521c31 --- /dev/null +++ b/AirLib/include/sensors/SensorFactory.hpp @@ -0,0 +1,38 @@ +#ifndef msr_airlib_SensorFactoryBase_hpp +#define msr_airlib_SensorFactoryBase_hpp + + +#include "SensorBase.hpp" +#include + +//sensors +#include "sensors/imu/ImuSimple.hpp" +#include "sensors/magnetometer/MagnetometerSimple.hpp" +#include "sensors/gps/GpsSimple.hpp" +#include "sensors/barometer/BarometerSimple.hpp" + +namespace msr { namespace airlib { + + +class SensorFactory { +public: + virtual std::unique_ptr createSensor(SensorBase::SensorType sensor_type) const + { + switch (sensor_type) { + case SensorBase::SensorType::Imu: + return std::unique_ptr(new ImuSimple()); + case SensorBase::SensorType::Magnetometer: + return std::unique_ptr(new MagnetometerSimple()); + case SensorBase::SensorType::Gps: + return std::unique_ptr(new GpsSimple()); + case SensorBase::SensorType::Barometer: + return std::unique_ptr(new BarometerSimple()); + default: + return std::unique_ptr(); + } + } +}; + + +}} //namespace +#endif \ No newline at end of file diff --git a/AirLib/include/sensors/distance/DistanceBase.hpp b/AirLib/include/sensors/distance/DistanceBase.hpp index e46a770643..36147705f1 100644 --- a/AirLib/include/sensors/distance/DistanceBase.hpp +++ b/AirLib/include/sensors/distance/DistanceBase.hpp @@ -13,12 +13,10 @@ namespace msr { namespace airlib { class DistanceBase : public SensorBase { public: //types struct Output { //same fields as ROS message - real_T min_distance;//cm - real_T max_distance;//cm - real_T distance; //meters - real_T sensor_type; - real_T sensor_id; - real_T orientation; + real_T distance; //meters + real_T min_distance;//m + real_T max_distance;//m + Pose relative_pose; }; diff --git a/AirLib/include/sensors/distance/DistanceSimple.hpp b/AirLib/include/sensors/distance/DistanceSimple.hpp index c5c9e7a8b6..432cd7ab82 100644 --- a/AirLib/include/sensors/distance/DistanceSimple.hpp +++ b/AirLib/include/sensors/distance/DistanceSimple.hpp @@ -62,25 +62,28 @@ class DistanceSimple : public DistanceBase { virtual ~DistanceSimple() = default; +protected: + virtual real_T getRayLength(const Pose& pose) = 0; + const DistanceSimpleParams& getParams() + { + return params_; + } + private: //methods Output getOutputInternal() { Output output; const GroundTruth& ground_truth = getGroundTruth(); - auto distance = ground_truth.environment->getDistance(); + auto distance = getRayLength(ground_truth.kinematics->pose + params_.relative_pose); //add noise in distance (about 0.2m sigma) distance += uncorrelated_noise_.next(); output.distance = distance; - output.min_distance = params_.min_distance; output.max_distance = params_.max_distance; - output.sensor_type = params_.sensor_type; - output.sensor_id = params_.sensor_id; - output.orientation = params_.orientation; - + output.relative_pose = params_.relative_pose; return output; } diff --git a/AirLib/include/sensors/distance/DistanceSimpleParams.hpp b/AirLib/include/sensors/distance/DistanceSimpleParams.hpp index be9ffd9523..d5f36bec35 100644 --- a/AirLib/include/sensors/distance/DistanceSimpleParams.hpp +++ b/AirLib/include/sensors/distance/DistanceSimpleParams.hpp @@ -11,11 +11,9 @@ namespace msr { namespace airlib { struct DistanceSimpleParams { - real_T min_distance = 20; - real_T max_distance = 4000; - real_T sensor_type = 0; //TODO: allow changing in settings? - real_T sensor_id = 77; //TODO: should this be something real? - real_T orientation = 25; //Pitch270 (downwards) as defined in MavLinkMessages + real_T min_distance = 20.0f / 100; //m + real_T max_distance = 4000.0f / 100; //m + Pose relative_pose; /* Ref: A Stochastic Approach to Noise Modeling for Barometric Altimeters diff --git a/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp b/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp index 6fb86ffb31..51ef587791 100644 --- a/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp +++ b/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp @@ -10,6 +10,7 @@ #include "MagnetometerSimpleParams.hpp" #include "MagnetometerBase.hpp" #include "common/FrequencyLimiter.hpp" +#include "common/DelayLine.hpp" namespace msr { namespace airlib { diff --git a/AirLib/include/vehicles/multirotor/MultiRotor.hpp b/AirLib/include/vehicles/multirotor/MultiRotor.hpp index e75984b770..284bc4c11b 100644 --- a/AirLib/include/vehicles/multirotor/MultiRotor.hpp +++ b/AirLib/include/vehicles/multirotor/MultiRotor.hpp @@ -163,11 +163,6 @@ class MultiRotor : public PhysicsBody { getController()->setCollisionInfo(collision_info); } - virtual void setDistance(const real_T distance) override - { - PhysicsBody::setDistance(distance); - } - virtual ~MultiRotor() = default; private: //methods diff --git a/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp b/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp index 6ce0791319..bf1d9bb023 100644 --- a/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp +++ b/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp @@ -9,12 +9,6 @@ #include "sensors/SensorCollection.hpp" #include "controllers/DroneControllerBase.hpp" -//sensors -#include "sensors/barometer/BarometerSimple.hpp" -#include "sensors/imu/ImuSimple.hpp" -#include "sensors/gps/GpsSimple.hpp" -#include "sensors/magnetometer/MagnetometerSimple.hpp" -#include "sensors/distance/DistanceSimple.hpp" namespace msr { namespace airlib { @@ -66,37 +60,71 @@ class MultiRotorParams { }; public: //interface - void initialize() + virtual void initialize() { - setup(params_, sensors_, controller_); + sensor_storage_.clear(); + sensors_.clear(); + + setupParams(); + + addEnabledSensors(params_.enabled_sensors); + controller_ = createController(); } const Params& getParams() const { return params_; } - Params& getParams() { return params_; } - SensorCollection& getSensors() { return sensors_; } - - //return pointer because we might have derived class + const SensorCollection& getSensors() const + { + return sensors_; + } DroneControllerBase* getController() { return controller_.get(); } + const DroneControllerBase* getController() const + { + return controller_.get(); + } + + void addEnabledSensors(const EnabledSensors& enabled_sensors) + { + if (enabled_sensors.imu) + addSensor(SensorBase::SensorType::Imu); + if (enabled_sensors.magnetometer) + addSensor(SensorBase::SensorType::Magnetometer); + if (enabled_sensors.gps) + addSensor(SensorBase::SensorType::Gps); + if (enabled_sensors.barometer) + addSensor(SensorBase::SensorType::Barometer); + if (enabled_sensors.distance) + addSensor(SensorBase::SensorType::Distance); + } + + SensorBase* addSensor(SensorBase::SensorType sensor_type) + { + sensor_storage_.emplace_back(createSensor(sensor_type)); + auto sensor = sensor_storage_.back().get(); + sensors_.insert(sensor, sensor_type); + return sensor; + } + virtual ~MultiRotorParams() = default; protected: //must override by derived class - //this method must clean up any previous initializations - virtual void setup(Params& params, SensorCollection& sensors, unique_ptr& controller) = 0; + virtual void setupParams() = 0; + virtual std::unique_ptr createSensor(SensorBase::SensorType sensor_type) = 0; + virtual std::unique_ptr createController() = 0; protected: //static utility functions for derived classes to use @@ -215,31 +243,10 @@ class MultiRotorParams { } } - static void createStandardSensors(vector>& sensor_storage, SensorCollection& sensors, const EnabledSensors& enabled_sensors) - { - sensor_storage.clear(); - if (enabled_sensors.imu) - sensors.insert(createSensor(sensor_storage), SensorCollection::SensorType::Imu); - if (enabled_sensors.magnetometer) - sensors.insert(createSensor(sensor_storage), SensorCollection::SensorType::Magnetometer); - if (enabled_sensors.gps) - sensors.insert(createSensor(sensor_storage), SensorCollection::SensorType::Gps); - if (enabled_sensors.barometer) - sensors.insert(createSensor(sensor_storage), SensorCollection::SensorType::Barometer); - if (enabled_sensors.distance) - sensors.insert(createSensor(sensor_storage), SensorCollection::SensorType::Distance); - } - - template - static SensorBase* createSensor(vector>& sensor_storage) - { - sensor_storage.emplace_back(unique_ptr(new SensorClass())); - return sensor_storage.back().get(); - } - private: Params params_; - SensorCollection sensors_; + SensorCollection sensors_; //maintains sensor type indexed collection of sensors + vector> sensor_storage_; //RAII for created sensors std::unique_ptr controller_; }; diff --git a/AirLib/include/vehicles/multirotor/MultiRotorParamsFactory.hpp b/AirLib/include/vehicles/multirotor/MultiRotorParamsFactory.hpp index d0d12c3c2c..a3e0508480 100644 --- a/AirLib/include/vehicles/multirotor/MultiRotorParamsFactory.hpp +++ b/AirLib/include/vehicles/multirotor/MultiRotorParamsFactory.hpp @@ -15,7 +15,7 @@ namespace msr { namespace airlib { class MultiRotorParamsFactory { public: - static std::unique_ptr createConfig(const std::string& vehicle_name) + static std::unique_ptr createConfig(const std::string& vehicle_name, const SensorFactory* sensor_factory) { AirSimSettings::VehicleSettings vehicle_settings = AirSimSettings::singleton().getVehicleSettings(vehicle_name); @@ -23,11 +23,11 @@ class MultiRotorParamsFactory { std::unique_ptr config; if (vehicle_settings.firmware_name == "PX4") { - config.reset(new Px4MultiRotor(vehicle_settings)); + config.reset(new Px4MultiRotor(vehicle_settings, sensor_factory)); } else if (vehicle_settings.firmware_name == "RosFlight") { - config.reset(new RosFlightQuadX(vehicle_settings)); + config.reset(new RosFlightQuadX(vehicle_settings, sensor_factory)); } else if (vehicle_settings.firmware_name == "SimpleFlight") { - config.reset(new SimpleFlightQuadX(vehicle_settings)); + config.reset(new SimpleFlightQuadX(vehicle_settings, sensor_factory)); } else throw std::runtime_error(Utils::stringf( "Cannot create vehicle config because vehicle name '%s' is not recognized", diff --git a/AirLib/include/vehicles/multirotor/configs/Px4MultiRotor.hpp b/AirLib/include/vehicles/multirotor/configs/Px4MultiRotor.hpp index 55e0bed753..5cd8df3cbf 100644 --- a/AirLib/include/vehicles/multirotor/configs/Px4MultiRotor.hpp +++ b/AirLib/include/vehicles/multirotor/configs/Px4MultiRotor.hpp @@ -6,21 +6,25 @@ #include "vehicles/multirotor/controllers/MavLinkDroneController.hpp" #include "common/AirSimSettings.hpp" +#include "sensors/SensorFactory.hpp" namespace msr { namespace airlib { class Px4MultiRotor : public MultiRotorParams { public: - Px4MultiRotor(const AirSimSettings::VehicleSettings& vehicle_settings) + Px4MultiRotor(const AirSimSettings::VehicleSettings& vehicle_settings, const SensorFactory* sensor_factory) + : sensor_factory_(sensor_factory) { connection_info_ = getConnectionInfo(vehicle_settings); } virtual ~Px4MultiRotor() = default; - virtual void setup(Params& params, SensorCollection& sensors, unique_ptr& controller) override + virtual void setupParams() override { + auto& params = getParams(); + if (connection_info_.model == "Blacksheep") { setupFrameBlacksheep(params); } @@ -35,13 +39,24 @@ class Px4MultiRotor : public MultiRotorParams { } else //Generic setupFrameGenericQuad(params); + } - //create sensors - createStandardSensors(sensor_storage_, sensors, params.enabled_sensors); - //create MavLink controller for PX4 - createController(controller, sensors); +protected: + virtual std::unique_ptr createSensor(SensorBase::SensorType sensor_type) override + { + return sensor_factory_->createSensor(sensor_type); } + virtual std::unique_ptr createController() override + { + unique_ptr controller(new MavLinkDroneController()); + auto mav_controller = static_cast(controller.get()); + mav_controller->initialize(connection_info_, & getSensors(), true); + + return controller; + } + + private: void setupFrameGenericQuad(Params& params) { @@ -269,16 +284,11 @@ class Px4MultiRotor : public MultiRotorParams { return connection_info; } - void createController(unique_ptr& controller, SensorCollection& sensors) - { - controller.reset(new MavLinkDroneController()); - auto mav_controller = static_cast(controller.get()); - mav_controller->initialize(connection_info_, &sensors, true); - } private: - vector> sensor_storage_; MavLinkDroneController::ConnectionInfo connection_info_; + const SensorFactory* sensor_factory_; + }; }} //namespace diff --git a/AirLib/include/vehicles/multirotor/configs/RosFlightQuadX.hpp b/AirLib/include/vehicles/multirotor/configs/RosFlightQuadX.hpp index beca040f2e..7710630385 100644 --- a/AirLib/include/vehicles/multirotor/configs/RosFlightQuadX.hpp +++ b/AirLib/include/vehicles/multirotor/configs/RosFlightQuadX.hpp @@ -7,20 +7,24 @@ #include "vehicles/multirotor/firmwares/ros_flight/RosFlightDroneController.hpp" #include "vehicles/multirotor/MultiRotorParams.hpp" #include "common/AirSimSettings.hpp" +#include "sensors/SensorFactory.hpp" namespace msr { namespace airlib { class RosFlightQuadX : public MultiRotorParams { public: - RosFlightQuadX(const AirSimSettings::VehicleSettings& vehicle_settings) + RosFlightQuadX(const AirSimSettings::VehicleSettings& vehicle_settings, const SensorFactory* sensor_factory) + : sensor_factory_(sensor_factory) { unused(vehicle_settings); } protected: - virtual void setup(Params& params, SensorCollection& sensors, unique_ptr& controller) override + virtual void setupParams() override { + auto& params = getParams(); + //set up arm lengths //dimensions are for F450 frame: http://artofcircuits.com/product/quadcopter-frame-hj450-with-power-distribution params.rotor_count = 4; @@ -54,20 +58,23 @@ class RosFlightQuadX : public MultiRotorParams { params.inertia(1, 1) = 0.08f; params.inertia(2, 2) = 0.12f; - createStandardSensors(sensor_storage_, sensors, params.enabled_sensors); - createController(controller, sensors); - //leave everything else to defaults } -private: - void createController(unique_ptr& controller, SensorCollection& sensors) + virtual std::unique_ptr createSensor(SensorBase::SensorType sensor_type) override { - controller.reset(new RosFlightDroneController(&sensors, this)); + return sensor_factory_->createSensor(sensor_type); + } + + virtual std::unique_ptr createController() override + { + return std::unique_ptr(new RosFlightDroneController(& getSensors(), this)); } private: vector> sensor_storage_; + const SensorFactory* sensor_factory_; + }; }} //namespace diff --git a/AirLib/include/vehicles/multirotor/configs/SimpleFlightQuadX.hpp b/AirLib/include/vehicles/multirotor/configs/SimpleFlightQuadX.hpp index d2bc2f5988..4ddf58c614 100644 --- a/AirLib/include/vehicles/multirotor/configs/SimpleFlightQuadX.hpp +++ b/AirLib/include/vehicles/multirotor/configs/SimpleFlightQuadX.hpp @@ -7,22 +7,25 @@ #include "vehicles/multirotor/firmwares/simple_flight/SimpleFlightDroneController.hpp" #include "vehicles/multirotor/MultiRotorParams.hpp" #include "common/AirSimSettings.hpp" +#include "sensors/SensorFactory.hpp" namespace msr { namespace airlib { class SimpleFlightQuadX : public MultiRotorParams { public: - SimpleFlightQuadX(const AirSimSettings::VehicleSettings& vehicle_settings) + SimpleFlightQuadX(const AirSimSettings::VehicleSettings& vehicle_settings, const SensorFactory* sensor_factory) + : vehicle_settings_(vehicle_settings), sensor_factory_(sensor_factory) { - vehicle_settings_ = vehicle_settings; } virtual ~SimpleFlightQuadX() = default; protected: - virtual void setup(Params& params, SensorCollection& sensors, unique_ptr& controller) override + virtual void setupParams() override { + auto& params = getParams(); + /******* Below is same config as PX4 generic model ********/ //set up arm lengths @@ -49,22 +52,24 @@ class SimpleFlightQuadX : public MultiRotorParams { //compute inertia matrix computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight); - createStandardSensors(sensor_storage_, sensors, params.enabled_sensors); - createController(controller, sensors); - //leave everything else to defaults } -private: - void createController(unique_ptr& controller, SensorCollection& sensors) + virtual std::unique_ptr createSensor(SensorBase::SensorType sensor_type) override { - unused(sensors); - controller.reset(new SimpleFlightDroneController(this, vehicle_settings_)); + return sensor_factory_->createSensor(sensor_type); } + virtual std::unique_ptr createController() override + { + return std::unique_ptr(new SimpleFlightDroneController(this, vehicle_settings_)); + } + + private: vector> sensor_storage_; AirSimSettings::VehicleSettings vehicle_settings_; + const SensorFactory* sensor_factory_; }; }} //namespace diff --git a/AirLib/include/vehicles/multirotor/controllers/MavLinkDroneController.hpp b/AirLib/include/vehicles/multirotor/controllers/MavLinkDroneController.hpp index c3e5288af6..f0841104df 100644 --- a/AirLib/include/vehicles/multirotor/controllers/MavLinkDroneController.hpp +++ b/AirLib/include/vehicles/multirotor/controllers/MavLinkDroneController.hpp @@ -753,23 +753,23 @@ struct MavLinkDroneController::impl { const ImuBase* getImu() { - return static_cast(sensors_->getByType(SensorCollection::SensorType::Imu)); + return static_cast(sensors_->getByType(SensorBase::SensorType::Imu)); } const MagnetometerBase* getMagnetometer() { - return static_cast(sensors_->getByType(SensorCollection::SensorType::Magnetometer)); + return static_cast(sensors_->getByType(SensorBase::SensorType::Magnetometer)); } const BarometerBase* getBarometer() { - return static_cast(sensors_->getByType(SensorCollection::SensorType::Barometer)); + return static_cast(sensors_->getByType(SensorBase::SensorType::Barometer)); } const DistanceBase* getDistance() { - return static_cast(sensors_->getByType(SensorCollection::SensorType::Distance)); + return static_cast(sensors_->getByType(SensorBase::SensorType::Distance)); } const GpsBase* getGps() { - return static_cast(sensors_->getByType(SensorCollection::SensorType::Gps)); + return static_cast(sensors_->getByType(SensorBase::SensorType::Gps)); } void update() @@ -788,12 +788,15 @@ struct MavLinkDroneController::impl { baro_output.pressure * 0.01f /*Pa to Milibar */, baro_output.altitude); const auto& distance_output = getDistance()->getOutput(); - sendDistanceSensor(distance_output.min_distance, - distance_output.max_distance, + 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, - distance_output.sensor_type, - distance_output.sensor_id, - distance_output.orientation); + 0, //sensor type: //TODO: allow changing in settings? + 77, //sensor id, //TODO: should this be something real? + pitch); //TODO: convert from radians to degrees? const auto gps = getGps(); if (gps != nullptr) { diff --git a/AirLib/include/vehicles/multirotor/firmwares/ros_flight/AirSimRosFlightBoard.hpp b/AirLib/include/vehicles/multirotor/firmwares/ros_flight/AirSimRosFlightBoard.hpp index 7aade16dbc..df95c13a20 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/ros_flight/AirSimRosFlightBoard.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/ros_flight/AirSimRosFlightBoard.hpp @@ -53,9 +53,9 @@ class AirSimRosFlightBoard : public ros_flight::Board { //Board interface implementation -------------------------------------------------------------------------- virtual void init() override { - imu_ = static_cast(sensors_->getByType(SensorCollection::SensorType::Imu)); - baro_ = static_cast(sensors_->getByType(SensorCollection::SensorType::Barometer)); - mag_ = static_cast(sensors_->getByType(SensorCollection::SensorType::Magnetometer)); + imu_ = static_cast(sensors_->getByType(SensorBase::SensorType::Imu)); + baro_ = static_cast(sensors_->getByType(SensorBase::SensorType::Barometer)); + mag_ = static_cast(sensors_->getByType(SensorBase::SensorType::Magnetometer)); } virtual uint64_t micros() override diff --git a/AirLibUnitTests/PixhawkTest.hpp b/AirLibUnitTests/PixhawkTest.hpp index 8824713f62..0cf4df5cba 100644 --- a/AirLibUnitTests/PixhawkTest.hpp +++ b/AirLibUnitTests/PixhawkTest.hpp @@ -11,8 +11,10 @@ class PixhawkTest : public TestBase { public: virtual void run() override { + SensorFactory sensor_factory; + //Test PX4 based drones - auto pixhawk = MultiRotorParamsFactory::createConfig("Pixhawk"); + auto pixhawk = MultiRotorParamsFactory::createConfig("Pixhawk", &sensor_factory); DroneControllerBase* controller = pixhawk->getController(); testAssert(controller != nullptr, "Couldn't get pixhawk controller"); diff --git a/AirLibUnitTests/RosFlightTest.hpp b/AirLibUnitTests/RosFlightTest.hpp index 49ebf4de1e..f8c512d734 100644 --- a/AirLibUnitTests/RosFlightTest.hpp +++ b/AirLibUnitTests/RosFlightTest.hpp @@ -4,6 +4,7 @@ #include "vehicles/multirotor/MultiRotorParamsFactory.hpp" #include "TestBase.hpp" +#include "sensors/SensorFactory.hpp" namespace msr { namespace airlib { @@ -11,7 +12,8 @@ class RosFlightTest : public TestBase { public: virtual void run() override { - auto rosFlight = MultiRotorParamsFactory::createConfig("RosFlight"); + SensorFactory sensor_factory; + auto rosFlight = MultiRotorParamsFactory::createConfig("RosFlight", &sensor_factory); DroneControllerBase* controller = rosFlight->getController(); testAssert(controller != nullptr, "Couldn't get controller"); diff --git a/AirLibUnitTests/SimpleFlightTest.hpp b/AirLibUnitTests/SimpleFlightTest.hpp index 9a2c04d694..df6784dc5f 100644 --- a/AirLibUnitTests/SimpleFlightTest.hpp +++ b/AirLibUnitTests/SimpleFlightTest.hpp @@ -11,6 +11,7 @@ namespace msr { namespace airlib { + class SimpleFlightTest : public TestBase { public: @@ -19,7 +20,9 @@ class SimpleFlightTest : public TestBase auto clock = std::make_shared(3E-3f); ClockFactory::get(clock); - std::unique_ptr params = MultiRotorParamsFactory::createConfig("SimpleFlight"); + SensorFactory sensor_factory; + + std::unique_ptr params = MultiRotorParamsFactory::createConfig("SimpleFlight", &sensor_factory); MultiRotor vehicle; std::unique_ptr environment; vehicle.initialize(params.get(), Pose(), diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.cpp b/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.cpp index f4fc1fefe0..836600aabf 100644 --- a/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.cpp +++ b/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.cpp @@ -153,9 +153,7 @@ void MultiRotorConnector::updateRenderedState(float dt) //move collision info from rendering engine to vehicle const CollisionInfo& collision_info = vehicle_pawn_wrapper_->getCollisionInfo(); vehicle_.setCollisionInfo(collision_info); - //move distance reading from rendering engine to vehicle - const real_T distance = vehicle_pawn_wrapper_->getDistance(); - vehicle_.setDistance(distance); + //update ground level if (manual_pose_controller_ != nullptr && manual_pose_controller_->getActor() == vehicle_pawn_wrapper_->getPawn()) { diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp index d1e96479bc..5656efeacd 100644 --- a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp +++ b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp @@ -6,6 +6,7 @@ #include #include "Logging/MessageLog.h" #include "vehicles/multirotor/MultiRotorParamsFactory.hpp" +#include "UnrealSensors/UnrealSensorFactory.h" ASimModeWorldMultiRotor::ASimModeWorldMultiRotor() @@ -183,14 +184,15 @@ void ASimModeWorldMultiRotor::createVehicles(std::vector& vehicles) ASimModeWorldBase::VehiclePtr ASimModeWorldMultiRotor::createVehicle(VehiclePawnWrapper* wrapper) { - auto vehicle_params = MultiRotorParamsFactory::createConfig(wrapper->getVehicleConfigName()); + UnrealSensorFactory sensor_factory(wrapper->getPawn()); + auto vehicle_params = MultiRotorParamsFactory::createConfig(wrapper->getVehicleConfigName(), & sensor_factory); vehicle_params_.push_back(std::move(vehicle_params)); std::shared_ptr vehicle = std::make_shared( - - wrapper, vehicle_params_.back().get(), getSettings().enable_rpc, getSettings().api_server_address, - vehicle_params_.back()->getParams().api_server_port + vehicle_params_.size() - 1, manual_pose_controller); + wrapper, vehicle_params_.back().get(), getSettings().enable_rpc, getSettings().api_server_address, + vehicle_params_.back()->getParams().api_server_port + vehicle_params_.size() - 1, manual_pose_controller + ); if (vehicle->getPhysicsBody() != nullptr) wrapper->setKinematics(&(static_cast(vehicle->getPhysicsBody())->getKinematics())); diff --git a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealDistanceSensor.cpp b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealDistanceSensor.cpp new file mode 100644 index 0000000000..63ef753dee --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealDistanceSensor.cpp @@ -0,0 +1,31 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#include "UnrealDistanceSensor.h" +#include "AirBlueprintLib.h" +#include "common/Common.hpp" +#include "NedTransform.h" + +UnrealDistanceSensor::UnrealDistanceSensor(AActor* actor) + : actor_(actor) +{ +} + +msr::airlib::real_T UnrealDistanceSensor::getRayLength(const msr::airlib::Pose& pose) +{ + //update ray tracing + Vector3r start = pose.position; + Vector3r end = start + VectorMath::rotateVector(VectorMath::front(), pose.orientation, true) * getParams().max_distance; + + FHitResult dist_hit = FHitResult(ForceInit); + bool is_hit = UAirBlueprintLib::GetObstacle(actor_, NedTransform::toNeuUU(start), NedTransform::toNeuUU(end), dist_hit); + float distance = is_hit? dist_hit.Distance / 100.0f : getParams().max_distance; + + //FString hit_name = FString("None"); + //if (dist_hit.GetActor()) + // hit_name=dist_hit.GetActor()->GetName(); + + //UAirBlueprintLib::LogMessage(FString("Distance to "), hit_name+FString(": ")+FString::SanitizeFloat(distance), LogDebugLevel::Informational); + + return distance; +} diff --git a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealDistanceSensor.h b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealDistanceSensor.h new file mode 100644 index 0000000000..4f4028d0e0 --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealDistanceSensor.h @@ -0,0 +1,24 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#pragma once + +#include "common/Common.hpp" +#include "GameFramework/Actor.h" +#include "sensors/distance/DistanceSimple.hpp" + +class UnrealDistanceSensor : public msr::airlib::DistanceSimple { +public: + UnrealDistanceSensor(AActor* actor); + +protected: + virtual msr::airlib::real_T getRayLength(const msr::airlib::Pose& pose) override; + +private: + using Vector3r = msr::airlib::Vector3r; + using VectorMath = msr::airlib::VectorMath; + + +private: + AActor* actor_; +}; \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.cpp b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.cpp new file mode 100644 index 0000000000..b838a05d06 --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.cpp @@ -0,0 +1,34 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#pragma once + +#include "UnrealSensorFactory.h" +#include "UnrealSensors/UnrealDistanceSensor.h" + + +UnrealSensorFactory::UnrealSensorFactory(AActor* actor) +{ + setActor(actor); +} + + +std::unique_ptr UnrealSensorFactory::createSensor(msr::airlib::SensorBase::SensorType sensor_type) const +{ + using SensorBase = msr::airlib::SensorBase; + + switch (sensor_type) { + case SensorBase::SensorType::Distance: + return std::unique_ptr(new UnrealDistanceSensor(actor_)); + default: + return msr::airlib::SensorFactory::createSensor(sensor_type); + } +} + +void UnrealSensorFactory::setActor(AActor* actor) +{ + actor_ = actor; +} + + + diff --git a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.h b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.h new file mode 100644 index 0000000000..0873624835 --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.h @@ -0,0 +1,25 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#pragma once + +#include "sensors/SensorFactory.hpp" +#include +#include "GameFramework/Actor.h" + + +namespace msr { namespace airlib { + + +class UnrealSensorFactory : public msr::airlib::SensorFactory { +public: + UnrealSensorFactory(AActor* actor); + void setActor(AActor* actor); + virtual std::unique_ptr createSensor(SensorBase::SensorType sensor_type) const override; + +private: + AActor* actor_; +}; + + +}} //namespace \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp b/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp index a003256d71..334b5b5300 100644 --- a/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp +++ b/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp @@ -203,11 +203,6 @@ const VehiclePawnWrapper::CollisionInfo& VehiclePawnWrapper::getCollisionInfo() return state_.collision_info; } -const real_T VehiclePawnWrapper::getDistance() const -{ - return distance_; -} - FVector VehiclePawnWrapper::getPosition() const { return pawn_->GetActorLocation(); // - state_.mesh_origin @@ -305,21 +300,6 @@ void VehiclePawnWrapper::setPose(const Pose& pose, bool ignore_collision) else if (!state_.tracing_enabled) { state_.last_position = position; } - - //update ray tracing - FVector start = getPosition(); - FVector end = start - (pawn_->GetActorUpVector()) * 4000; - FHitResult dist_hit = FHitResult(ForceInit); - - bool is_hit = UAirBlueprintLib::GetObstacle(pawn_, start, end, dist_hit); - distance_ = is_hit? dist_hit.Distance : 4000; - - FString hit_name = FString("None"); - if (dist_hit.GetActor()) - hit_name=dist_hit.GetActor()->GetName(); - - float range_m = distance_*0.01; - UAirBlueprintLib::LogMessage(FString("Distance to "), hit_name+FString(": ")+FString::SanitizeFloat(range_m), LogDebugLevel::Informational); } void VehiclePawnWrapper::setDebugPose(const Pose& debug_pose) diff --git a/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.h b/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.h index a0c3acbae5..c52f2bf7b1 100644 --- a/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.h +++ b/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.h @@ -75,7 +75,6 @@ class VehiclePawnWrapper const GeoPoint& getHomePoint() const; const CollisionInfo& getCollisionInfo() const; - const real_T getDistance() const; void setLogLine(std::string line); std::string getLogLine(); @@ -115,7 +114,6 @@ class VehiclePawnWrapper const msr::airlib::Kinematics::State* kinematics_; std::string log_line_; WrapperConfig config_; - real_T distance_; struct State { FVector start_location;