diff --git a/.gitignore b/.gitignore index e041dc0ba7..785b5da605 100644 --- a/.gitignore +++ b/.gitignore @@ -307,6 +307,7 @@ cmake_build/ /boost /[Ee]igen /build_debug +/build_gcc_debug /cmake/CPackSourceConfig.cmake /cmake/CPackConfig.cmake /cmake/arch.c diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index 11e7e9cd81..f0acd387cb 100644 --- a/AirLib/include/api/RpcLibClientBase.hpp +++ b/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 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 = ""); diff --git a/AirLib/include/api/RpcLibServerBase.hpp b/AirLib/include/api/RpcLibServerBase.hpp index e16f716681..9f8a0bf622 100644 --- a/AirLib/include/api/RpcLibServerBase.hpp +++ b/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; diff --git a/AirLib/include/api/VehicleApiBase.hpp b/AirLib/include/api/VehicleApiBase.hpp index 2d65ff8233..ddb6679f3c 100644 --- a/AirLib/include/api/VehicleApiBase.hpp +++ b/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(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 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(getSensors().getByType(SensorBase::SensorType::Lidar, i)); + if (current_lidar != nullptr && (current_lidar->getName() == lidar_name || lidar_name == "")) + { + lidar = current_lidar; + break; + } + } + + return lidar; + } }; diff --git a/AirLib/include/api/VehicleSimApiBase.hpp b/AirLib/include/api/VehicleSimApiBase.hpp index 766b2e696e..c913c2eac5 100644 --- a/AirLib/include/api/VehicleSimApiBase.hpp +++ b/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(); diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index 69d9a3435e..2e2dcbf1d7 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/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(), Utils::nan(), Utils::nan()); @@ -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 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); diff --git a/AirLib/include/common/Common.hpp b/AirLib/include/common/Common.hpp index 62c72b9576..8ac10fcbff 100644 --- a/AirLib/include/common/Common.hpp +++ b/AirLib/include/common/Common.hpp @@ -19,6 +19,8 @@ #define _CRT_SECURE_NO_WARNINGS 1 #endif +#define RpcLibPort 41451 + namespace msr { namespace airlib { //numericals diff --git a/AirLib/include/common/CommonStructs.hpp b/AirLib/include/common/CommonStructs.hpp index ff625d8cfe..51da55e189 100644 --- a/AirLib/include/common/CommonStructs.hpp +++ b/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) diff --git a/AirLib/include/common/DelayLine.hpp b/AirLib/include/common/DelayLine.hpp index 1c7022f66d..e0f2c475d3 100644 --- a/AirLib/include/common/DelayLine.hpp +++ b/AirLib/include/common/DelayLine.hpp @@ -11,7 +11,7 @@ namespace msr { namespace airlib { template -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; diff --git a/AirLib/include/common/FirstOrderFilter.hpp b/AirLib/include/common/FirstOrderFilter.hpp index 45bc885405..722e22b3ff 100644 --- a/AirLib/include/common/FirstOrderFilter.hpp +++ b/AirLib/include/common/FirstOrderFilter.hpp @@ -12,7 +12,7 @@ namespace msr { namespace airlib { template -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_; diff --git a/AirLib/include/common/FrequencyLimiter.hpp b/AirLib/include/common/FrequencyLimiter.hpp index 9c74e33e9e..317cf60782 100644 --- a/AirLib/include/common/FrequencyLimiter.hpp +++ b/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(), 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(); diff --git a/AirLib/include/common/GaussianMarkov.hpp b/AirLib/include/common/GaussianMarkov.hpp index e6d44dc289..b93e16197e 100644 --- a/AirLib/include/common/GaussianMarkov.hpp +++ b/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(); diff --git a/AirLib/include/common/StateReporterWrapper.hpp b/AirLib/include/common/StateReporterWrapper.hpp index c5175297f2..a4246e1ffc 100644 --- a/AirLib/include/common/StateReporterWrapper.hpp +++ b/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(); diff --git a/AirLib/include/common/UpdatableContainer.hpp b/AirLib/include/common/UpdatableContainer.hpp index 22cfb1f86d..b841d8f44b 100644 --- a/AirLib/include/common/UpdatableContainer.hpp +++ b/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(); } diff --git a/AirLib/include/common/UpdatableObject.hpp b/AirLib/include/common/UpdatableObject.hpp index aa4f370d78..579f2350fb 100644 --- a/AirLib/include/common/UpdatableObject.hpp +++ b/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 diff --git a/AirLib/include/common/WorkerThread.hpp b/AirLib/include/common/WorkerThread.hpp index 696583c55f..8ba1c501f0 100644 --- a/AirLib/include/common/WorkerThread.hpp +++ b/AirLib/include/common/WorkerThread.hpp @@ -102,6 +102,34 @@ class WorkerThreadSignal return true; } + void wait() + { + // wait for signal or timeout or cancel predicate + std::unique_lock lock(mutex_); + while (!signaled_) { + cv_.wait(lock); + } + lock.unlock(); + signaled_ = false; + } + + bool waitForRetry(double timeout_sec, int n_times) + { + std::unique_lock lock(mutex_); + while (!signaled_ && n_times > 0) { + cv_.wait_for(lock, std::chrono::milliseconds(static_cast(timeout_sec * 1000))); + --n_times; + } + lock.unlock(); + if (n_times == 0 && !signaled_) { + return false; + } + else { + signaled_ = false; + return true; + } + } + }; // This class provides a synchronized worker thread that guarantees to execute diff --git a/AirLib/include/common/common_utils/Utils.hpp b/AirLib/include/common/common_utils/Utils.hpp index fb169bdbb7..f93d193847 100644 --- a/AirLib/include/common/common_utils/Utils.hpp +++ b/AirLib/include/common/common_utils/Utils.hpp @@ -450,7 +450,7 @@ class Utils { static std::size_t length(const T(&)[N]) { return N; - }; + } static void saveToFile(string file_name, const char* data, uint size) { diff --git a/AirLib/include/physics/Environment.hpp b/AirLib/include/physics/Environment.hpp index 68e2b750bf..db610f7e4c 100644 --- a/AirLib/include/physics/Environment.hpp +++ b/AirLib/include/physics/Environment.hpp @@ -78,17 +78,25 @@ class Environment : public UpdatableObject { return current_; } - //*** Start: UpdatableState implementation ***// - virtual void reset() + virtual void update() override + { + updateState(current_, home_geo_point_); + } + +protected: + virtual void resetImplementation() override { current_ = initial_; } - virtual void update() + virtual void failResetUpdateOrdering(std::string err) override { - updateState(current_, home_geo_point_); + unused(err); + //Do nothing. + //The environment gets reset() twice without an update() inbetween, + //via MultirotorPawnSimApi::reset() and CarSimApi::reset(), because + //those functions directly reset an environment, and also call other reset()s that reset the same environment. } - //*** End: UpdatableState implementation ***// private: static void updateState(State& state, const HomeGeoPoint& home_geo_point) diff --git a/AirLib/include/physics/FastPhysicsEngine.hpp b/AirLib/include/physics/FastPhysicsEngine.hpp index 68bbdbec4b..61dc19b90c 100644 --- a/AirLib/include/physics/FastPhysicsEngine.hpp +++ b/AirLib/include/physics/FastPhysicsEngine.hpp @@ -24,10 +24,8 @@ class FastPhysicsEngine : public PhysicsEngineBase { } //*** Start: UpdatableState implementation ***// - virtual void reset() override + virtual void resetImplementation() override { - PhysicsEngineBase::reset(); - for (PhysicsBody* body_ptr : *this) { initPhysicsBody(body_ptr); } diff --git a/AirLib/include/physics/Kinematics.hpp b/AirLib/include/physics/Kinematics.hpp index c354458e8a..600b2e532c 100644 --- a/AirLib/include/physics/Kinematics.hpp +++ b/AirLib/include/physics/Kinematics.hpp @@ -38,11 +38,8 @@ class Kinematics : public UpdatableObject { initial_ = initial; } - //*** Start: UpdatableState implementation ***// - virtual void reset() override + virtual void resetImplementation() override { - UpdatableObject::reset(); - current_ = initial_; } diff --git a/AirLib/include/physics/PhysicsBody.hpp b/AirLib/include/physics/PhysicsBody.hpp index 9e47590caf..0865296bea 100644 --- a/AirLib/include/physics/PhysicsBody.hpp +++ b/AirLib/include/physics/PhysicsBody.hpp @@ -95,10 +95,8 @@ class PhysicsBody : public UpdatableObject { //*** Start: UpdatableState implementation ***// - virtual void reset() override + virtual void resetImplementation() override { - UpdatableObject::reset(); - if (environment_) environment_->reset(); wrench_ = Wrench::zero(); diff --git a/AirLib/include/physics/PhysicsBodyVertex.hpp b/AirLib/include/physics/PhysicsBodyVertex.hpp index f5a61d8a4a..e487906005 100644 --- a/AirLib/include/physics/PhysicsBodyVertex.hpp +++ b/AirLib/include/physics/PhysicsBodyVertex.hpp @@ -43,12 +43,9 @@ class PhysicsBodyVertex : public UpdatableObject { drag_factor_ = drag_factor; } - //*** Start: UpdatableState implementation ***// - virtual void reset() override + virtual void resetImplementation() override { - UpdatableObject::reset(); - position_ = initial_position_; normal_ = initial_normal_; diff --git a/AirLib/include/physics/PhysicsEngineBase.hpp b/AirLib/include/physics/PhysicsEngineBase.hpp index efdbc59ca9..b151a8d6e3 100644 --- a/AirLib/include/physics/PhysicsEngineBase.hpp +++ b/AirLib/include/physics/PhysicsEngineBase.hpp @@ -12,11 +12,6 @@ namespace msr { namespace airlib { class PhysicsEngineBase : public UpdatableObject { public: - virtual void reset() override - { - UpdatableObject::reset(); - } - virtual void update() override { UpdatableObject::update(); diff --git a/AirLib/include/physics/World.hpp b/AirLib/include/physics/World.hpp index a9dc720666..0129f575ef 100644 --- a/AirLib/include/physics/World.hpp +++ b/AirLib/include/physics/World.hpp @@ -27,9 +27,9 @@ class World : public UpdatableContainer { //override updatable interface so we can synchronize physics engine //*** Start: UpdatableState implementation ***// - virtual void reset() override + virtual void resetImplementation() override { - UpdatableContainer::reset(); + UpdatableContainer::resetImplementation(); if (physics_engine_) physics_engine_->reset(); diff --git a/AirLib/include/sensors/SensorCollection.hpp b/AirLib/include/sensors/SensorCollection.hpp index 5904b2fed1..87abcc7547 100644 --- a/AirLib/include/sensors/SensorCollection.hpp +++ b/AirLib/include/sensors/SensorCollection.hpp @@ -12,7 +12,7 @@ namespace msr { namespace airlib { -class SensorCollection : UpdatableObject { +class SensorCollection : public UpdatableObject { public: //types typedef SensorBase* SensorBasePtr; public: @@ -68,10 +68,8 @@ class SensorCollection : UpdatableObject { } //*** Start: UpdatableState implementation ***// - virtual void reset() override + virtual void resetImplementation() override { - UpdatableObject::reset(); - for (auto& pair : sensors_) { pair.second->reset(); } diff --git a/AirLib/include/sensors/barometer/BarometerSimple.hpp b/AirLib/include/sensors/barometer/BarometerSimple.hpp index 90208fb9eb..399eb86cd7 100644 --- a/AirLib/include/sensors/barometer/BarometerSimple.hpp +++ b/AirLib/include/sensors/barometer/BarometerSimple.hpp @@ -36,10 +36,8 @@ class BarometerSimple : public BarometerBase { } //*** Start: UpdatableState implementation ***// - virtual void reset() override + virtual void resetImplementation() override { - BarometerBase::reset(); - pressure_factor_.reset(); //correlated_noise_.reset(); uncorrelated_noise_.reset(); diff --git a/AirLib/include/sensors/distance/DistanceSimple.hpp b/AirLib/include/sensors/distance/DistanceSimple.hpp index 28761e9e83..74bb8069d3 100644 --- a/AirLib/include/sensors/distance/DistanceSimple.hpp +++ b/AirLib/include/sensors/distance/DistanceSimple.hpp @@ -32,14 +32,11 @@ class DistanceSimple : public DistanceBase { } //*** Start: UpdatableState implementation ***// - virtual void reset() override + virtual void resetImplementation() override { - DistanceBase::reset(); - //correlated_noise_.reset(); uncorrelated_noise_.reset(); - freq_limiter_.reset(); delay_line_.reset(); diff --git a/AirLib/include/sensors/gps/GpsSimple.hpp b/AirLib/include/sensors/gps/GpsSimple.hpp index eba1d9540a..dd32cfd22b 100644 --- a/AirLib/include/sensors/gps/GpsSimple.hpp +++ b/AirLib/include/sensors/gps/GpsSimple.hpp @@ -33,10 +33,8 @@ class GpsSimple : public GpsBase { } //*** Start: UpdatableState implementation ***// - virtual void reset() override + virtual void resetImplementation() override { - GpsBase::reset(); - freq_limiter_.reset(); delay_line_.reset(); diff --git a/AirLib/include/sensors/imu/ImuSimple.hpp b/AirLib/include/sensors/imu/ImuSimple.hpp index 10b8133ac8..fbf38c730c 100644 --- a/AirLib/include/sensors/imu/ImuSimple.hpp +++ b/AirLib/include/sensors/imu/ImuSimple.hpp @@ -25,10 +25,8 @@ class ImuSimple : public ImuBase { } //*** Start: UpdatableState implementation ***// - virtual void reset() override + virtual void resetImplementation() override { - ImuBase::reset(); - last_time_ = clock()->nowNanos(); state_.gyroscope_bias = params_.gyro.turn_on_bias; diff --git a/AirLib/include/sensors/lidar/LidarBase.hpp b/AirLib/include/sensors/lidar/LidarBase.hpp index 05a0735e58..05a286f513 100644 --- a/AirLib/include/sensors/lidar/LidarBase.hpp +++ b/AirLib/include/sensors/lidar/LidarBase.hpp @@ -45,14 +45,25 @@ class LidarBase : public SensorBase { return output_; } + const vector& getSegmentationOutput() const + { + return segmentation_output_; + } + protected: void setOutput(const LidarData& output) { output_ = output; } + void setSegmentationOutput(vector& segmentation_output) + { + segmentation_output_ = segmentation_output; + } + private: LidarData output_; + vector segmentation_output_; }; }} //namespace diff --git a/AirLib/include/sensors/lidar/LidarSimple.hpp b/AirLib/include/sensors/lidar/LidarSimple.hpp index 8c7041e766..e837abf54a 100644 --- a/AirLib/include/sensors/lidar/LidarSimple.hpp +++ b/AirLib/include/sensors/lidar/LidarSimple.hpp @@ -26,10 +26,8 @@ class LidarSimple : public LidarBase { } //*** Start: UpdatableState implementation ***// - virtual void reset() override + virtual void resetImplementation() override { - LidarBase::reset(); - freq_limiter_.reset(); last_time_ = clock()->nowNanos(); @@ -68,7 +66,7 @@ class LidarSimple : public LidarBase { protected: virtual void getPointCloud(const Pose& lidar_pose, const Pose& vehicle_pose, - TTimeDelta delta_time, vector& point_cloud) = 0; + TTimeDelta delta_time, vector& point_cloud, vector& segmentation_cloud) = 0; private: //methods @@ -91,8 +89,10 @@ class LidarSimple : public LidarBase { Pose lidar_pose = params_.relative_pose + ground_truth.kinematics->pose; getPointCloud(params_.relative_pose, // relative lidar pose ground_truth.kinematics->pose, // relative vehicle pose - delta_time, - point_cloud_); + delta_time, + point_cloud_, + segmentation_cloud_ + ); LidarData output; output.point_cloud = point_cloud_; @@ -102,11 +102,13 @@ class LidarSimple : public LidarBase { last_time_ = output.time_stamp; setOutput(output); + setSegmentationOutput(segmentation_cloud_); } private: LidarSimpleParams params_; vector point_cloud_; + vector segmentation_cloud_; FrequencyLimiter freq_limiter_; TTimePoint last_time_; diff --git a/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp b/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp index 064a8b6ec5..b2a1556611 100644 --- a/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp +++ b/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp @@ -32,10 +32,8 @@ class MagnetometerSimple : public MagnetometerBase { } //*** Start: UpdatableObject implementation ***// - virtual void reset() override + virtual void resetImplementation() override { - MagnetometerBase::reset(); - //Ground truth is reset before sensors are reset updateReference(getGroundTruth()); noise_vec_.reset(); diff --git a/AirLib/include/vehicles/car/api/CarApiBase.hpp b/AirLib/include/vehicles/car/api/CarApiBase.hpp index e83a796357..2177ee4b78 100644 --- a/AirLib/include/vehicles/car/api/CarApiBase.hpp +++ b/AirLib/include/vehicles/car/api/CarApiBase.hpp @@ -78,20 +78,13 @@ class CarApiBase : public VehicleApiBase { initialize(vehicle_setting, sensor_factory, state, environment); } - //default implementation so derived class doesn't have to call on VehicleApiBase - virtual void reset() override - { - VehicleApiBase::reset(); - - //reset sensors last after their ground truth has been reset - getSensors().reset(); - } virtual void update() override { VehicleApiBase::update(); getSensors().update(); } + void reportState(StateReporter& reporter) override { getSensors().reportState(reporter); @@ -142,6 +135,13 @@ class CarApiBase : public VehicleApiBase { std::shared_ptr sensor_factory_; SensorCollection sensors_; //maintains sensor type indexed collection of sensors vector> sensor_storage_; //RAII for created sensors + +protected: + virtual void resetImplementation() override + { + //reset sensors last after their ground truth has been reset + getSensors().reset(); + } }; diff --git a/AirLib/include/vehicles/car/api/CarRpcLibClient.hpp b/AirLib/include/vehicles/car/api/CarRpcLibClient.hpp index 47102dc3d4..d8f9f51b42 100644 --- a/AirLib/include/vehicles/car/api/CarRpcLibClient.hpp +++ b/AirLib/include/vehicles/car/api/CarRpcLibClient.hpp @@ -16,11 +16,11 @@ namespace msr { namespace airlib { class CarRpcLibClient : public RpcLibClientBase { public: - CarRpcLibClient(const string& ip_address = "localhost", uint16_t port = 41451, float timeout_sec = 60); + CarRpcLibClient(const string& ip_address = "localhost", uint16_t port = RpcLibPort, float timeout_sec = 60); void setCarControls(const CarApiBase::CarControls& controls, const std::string& vehicle_name = ""); CarApiBase::CarState getCarState(const std::string& vehicle_name = ""); - + CarApiBase::CarControls getCarControls(const std::string& vehicle_name = ""); virtual ~CarRpcLibClient(); //required for pimpl }; diff --git a/AirLib/include/vehicles/car/api/CarRpcLibServer.hpp b/AirLib/include/vehicles/car/api/CarRpcLibServer.hpp index 066638e755..ebad783aac 100644 --- a/AirLib/include/vehicles/car/api/CarRpcLibServer.hpp +++ b/AirLib/include/vehicles/car/api/CarRpcLibServer.hpp @@ -11,11 +11,12 @@ #include "api/RpcLibServerBase.hpp" #include "vehicles/car/api/CarApiBase.hpp" + namespace msr { namespace airlib { class CarRpcLibServer : public RpcLibServerBase { public: - CarRpcLibServer(ApiProvider* api_provider, string server_address, uint16_t port = 41451); + CarRpcLibServer(ApiProvider* api_provider, string server_address, uint16_t port = RpcLibPort); virtual ~CarRpcLibServer(); protected: diff --git a/AirLib/include/vehicles/multirotor/MultiRotor.hpp b/AirLib/include/vehicles/multirotor/MultiRotor.hpp index d32b5c1886..454e6559a3 100644 --- a/AirLib/include/vehicles/multirotor/MultiRotor.hpp +++ b/AirLib/include/vehicles/multirotor/MultiRotor.hpp @@ -26,10 +26,10 @@ class MultiRotor : public PhysicsBody { } //*** Start: UpdatableState implementation ***// - virtual void reset() override + virtual void resetImplementation() override { //reset rotors, kinematics and environment - PhysicsBody::reset(); + PhysicsBody::resetImplementation(); //reset sensors last after their ground truth has been reset resetSensors(); diff --git a/AirLib/include/vehicles/multirotor/MultiRotorParamsFactory.hpp b/AirLib/include/vehicles/multirotor/MultiRotorParamsFactory.hpp index 537f1f5bde..1900e8c8fe 100644 --- a/AirLib/include/vehicles/multirotor/MultiRotorParamsFactory.hpp +++ b/AirLib/include/vehicles/multirotor/MultiRotorParamsFactory.hpp @@ -8,6 +8,7 @@ #include "vehicles/multirotor/firmwares/mavlink/Px4MultiRotorParams.hpp" #include "vehicles/multirotor/firmwares/simple_flight/SimpleFlightQuadXParams.hpp" #include "vehicles/multirotor/firmwares/mavlink/ArduCopterSoloParams.hpp" +#include "vehicles/multirotor/firmwares/arducopter/ArduCopterParams.hpp" namespace msr { namespace airlib { @@ -26,6 +27,9 @@ class MultiRotorParamsFactory { else if (vehicle_setting->vehicle_type == AirSimSettings::kVehicleTypeArduCopterSolo) { config.reset(new ArduCopterSoloParams(*static_cast(vehicle_setting), sensor_factory)); } + else if (vehicle_setting->vehicle_type == AirSimSettings::kVehicleTypeArduCopter) { + config.reset(new ArduCopterParams(*static_cast(vehicle_setting), sensor_factory)); + } else if (vehicle_setting->vehicle_type == "" || //default config vehicle_setting->vehicle_type == AirSimSettings::kVehicleTypeSimpleFlight) { config.reset(new SimpleFlightQuadXParams(vehicle_setting, sensor_factory)); diff --git a/AirLib/include/vehicles/multirotor/Rotor.hpp b/AirLib/include/vehicles/multirotor/Rotor.hpp index 7124eceb81..bc749cba82 100644 --- a/AirLib/include/vehicles/multirotor/Rotor.hpp +++ b/AirLib/include/vehicles/multirotor/Rotor.hpp @@ -66,9 +66,9 @@ class Rotor : public PhysicsBodyVertex { //*** Start: UpdatableState implementation ***// - virtual void reset() override + virtual void resetImplementation() override { - PhysicsBodyVertex::reset(); + PhysicsBodyVertex::resetImplementation(); //update environmental factors before we call base updateEnvironmentalFactors(); diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp index b99c8c1489..8ec6e581f1 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp @@ -69,7 +69,7 @@ class MultirotorApiBase : public VehicleApiBase { unused(environment); } - virtual void reset() override; + virtual void resetImplementation() override; public: //these APIs uses above low level APIs diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp index b45f88cabe..b90177e2ff 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp @@ -12,11 +12,12 @@ #include "api/RpcLibClientBase.hpp" #include "vehicles/multirotor/api/MultirotorCommon.hpp" + namespace msr { namespace airlib { class MultirotorRpcLibClient : public RpcLibClientBase { public: - MultirotorRpcLibClient(const string& ip_address = "localhost", uint16_t port = 41451, float timeout_sec = 60); + MultirotorRpcLibClient(const string& ip_address = "localhost", uint16_t port = RpcLibPort, float timeout_sec = 60); MultirotorRpcLibClient* takeoffAsync(float timeout_sec = 20, const std::string& vehicle_name = ""); MultirotorRpcLibClient* landAsync(float timeout_sec = 60, const std::string& vehicle_name = ""); diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibServer.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibServer.hpp index f6e8089901..f4aaf7245f 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibServer.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibServer.hpp @@ -14,7 +14,7 @@ namespace msr { namespace airlib { class MultirotorRpcLibServer : public RpcLibServerBase { public: - MultirotorRpcLibServer(ApiProvider* api_provider, string server_address, uint16_t port = 41451); + MultirotorRpcLibServer(ApiProvider* api_provider, string server_address, uint16_t port = RpcLibPort); virtual ~MultirotorRpcLibServer(); protected: diff --git a/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp new file mode 100644 index 0000000000..c4ff3ff867 --- /dev/null +++ b/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp @@ -0,0 +1,446 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef msr_airlib_ArduCopterDroneController_hpp +#define msr_airlib_ArduCopterDroneController_hpp + +#include "vehicles/multirotor/api/MultirotorApiBase.hpp" +#include "sensors/SensorCollection.hpp" +#include "physics/Environment.hpp" +#include "physics/Kinematics.hpp" +#include "vehicles/multirotor/MultiRotorParams.hpp" +#include "common/Common.hpp" +#include "physics/PhysicsBody.hpp" +#include "common/AirSimSettings.hpp" + +// Sensors +#include "sensors/imu/ImuBase.hpp" +#include "sensors/gps/GpsBase.hpp" +#include "sensors/magnetometer/MagnetometerBase.hpp" +#include "sensors/barometer/BarometerBase.hpp" +#include "sensors/lidar/LidarBase.hpp" + +#include "UdpSocket.hpp" + +#include +#include + +#define __STDC_FORMAT_MACROS +#include + +namespace msr { namespace airlib { + +class ArduCopterApi : public MultirotorApiBase { + +public: + ArduCopterApi(const MultiRotorParams* vehicle_params, const AirSimSettings::MavLinkConnectionInfo& connection_info) + : ip(connection_info.ip_address), vehicle_params_(vehicle_params) + { + connection_info_ = connection_info; + sensors_ = &getSensors(); + + connect(); // Should we try catching exceptions here? + } + + ~ArduCopterApi() + { + closeConnections(); + } + + +public: + virtual void resetImplementation() override + { + MultirotorApiBase::resetImplementation(); + + // Reset state + } + + // Update sensor data & send to Ardupilot + virtual void update() override + { + MultirotorApiBase::update(); + + sendSensors(); + recvRotorControl(); + } + + // TODO:VehicleApiBase implementation + virtual bool isApiControlEnabled() const override + { + Utils::log("Not Implemented", Utils::kLogLevelInfo); + return false; + } + virtual void enableApiControl(bool) override + { + Utils::log("Not Implemented", Utils::kLogLevelInfo); + } + virtual bool armDisarm(bool) override + { + Utils::log("Not Implemented", Utils::kLogLevelInfo); + return false; + } + virtual GeoPoint getHomeGeoPoint() const override + { + Utils::log("Not Implemented", Utils::kLogLevelInfo); + return GeoPoint(Utils::nan(), Utils::nan(), Utils::nan()); + } + virtual void getStatusMessages(std::vector&) override + { + Utils::log("Not Implemented", Utils::kLogLevelInfo); + } + + virtual const SensorCollection& getSensors() const override + { + return vehicle_params_->getSensors(); + } + +public: //TODO:MultirotorApiBase implementation + virtual real_T getActuation(unsigned int rotor_index) const override + { + return rotor_controls_[rotor_index]; + } + virtual size_t getActuatorCount() const override + { + return vehicle_params_->getParams().rotor_count; + } + virtual void moveByRC(const RCData& rc_data) override + { + setRCData(rc_data); + } + virtual void setSimulatedGroundTruth(const Kinematics::State*, const Environment*) override + { + } + virtual bool setRCData(const RCData& rc_data) override + { + last_rcData_ = rc_data; + is_rc_connected = true; + + return true; + } + +protected: + virtual Kinematics::State getKinematicsEstimated() const override + { + Utils::log("Not Implemented", Utils::kLogLevelInfo); + Kinematics::State state; + return state; + } + + virtual Vector3r getPosition() const override + { + Utils::log("Not Implemented", Utils::kLogLevelInfo); + return Vector3r(Utils::nan(), Utils::nan(), Utils::nan()); + } + + virtual Vector3r getVelocity() const override + { + Utils::log("Not Implemented", Utils::kLogLevelInfo); + return Vector3r(Utils::nan(), Utils::nan(), Utils::nan()); + } + + virtual Quaternionr getOrientation() const override + { + Utils::log("Not Implemented", Utils::kLogLevelInfo); + return Quaternionr(Utils::nan(), Utils::nan(), Utils::nan(), Utils::nan()); + } + + virtual LandedState getLandedState() const override + { + Utils::log("Not Implemented", Utils::kLogLevelInfo); + return LandedState::Landed; + } + + virtual RCData getRCData() const override + { + //return what we received last time through setRCData + return last_rcData_; + } + + virtual GeoPoint getGpsLocation() const override + { + Utils::log("Not Implemented", Utils::kLogLevelInfo); + return GeoPoint(Utils::nan(), Utils::nan(), Utils::nan()); + } + + virtual float getCommandPeriod() const override + { + return 1.0f / 50; //50hz + } + + virtual float getTakeoffZ() const override + { + // pick a number, 3 meters is probably safe + // enough to get out of the backwash turbulence. Negative due to NED coordinate system. + // return params_.takeoff.takeoff_z; + return 3.0; + } + + virtual float getDistanceAccuracy() const override + { + return 0.5f; //measured in simulator by firing commands "MoveToLocation -x 0 -y 0" multiple times and looking at distance traveled + } + + virtual void commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) override + { + unused(pitch); + unused(roll); + unused(throttle); + unused(yaw_rate); + Utils::log("Not Implemented", Utils::kLogLevelInfo); + } + + virtual void commandRollPitchZ(float pitch, float roll, float z, float yaw) override + { + unused(pitch); + unused(roll); + unused(z); + unused(yaw); + Utils::log("Not Implemented", Utils::kLogLevelInfo); + } + + virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) override + { + unused(vx); + unused(vy); + unused(vz); + unused(yaw_mode); + Utils::log("Not Implemented", Utils::kLogLevelInfo); + } + + virtual void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) override + { + unused(vx); + unused(vy); + unused(z); + unused(yaw_mode); + Utils::log("Not Implemented", Utils::kLogLevelInfo); + } + + virtual void commandPosition(float x, float y, float z, const YawMode& yaw_mode) override + { + unused(x); + unused(y); + unused(z); + unused(yaw_mode); + Utils::log("Not Implemented", Utils::kLogLevelInfo); + } + + virtual const MultirotorApiParams& getMultirotorApiParams() const override + { + return safety_params_; + } + + //*** End: MultirotorApiBase implementation ***// + +protected: + void closeConnections() + { + if (udpSocket_ != nullptr) + udpSocket_->close(); + } + + void connect() + { + port = static_cast(connection_info_.ip_port); + + closeConnections(); + + if (ip == "") { + throw std::invalid_argument("UdpIp setting is invalid."); + } + + if (port == 0) { + throw std::invalid_argument("UdpPort setting has an invalid value."); + } + + Utils::log(Utils::stringf("Using UDP port %d, local IP %s, remote IP %s for sending sensor data", port, connection_info_.local_host_ip.c_str(), ip.c_str()), Utils::kLogLevelInfo); + Utils::log(Utils::stringf("Using UDP port %d for receiving rotor power", connection_info_.sitl_ip_port, connection_info_.local_host_ip.c_str(), ip.c_str()), Utils::kLogLevelInfo); + + udpSocket_ = std::make_shared(); + udpSocket_->bind(connection_info_.local_host_ip, connection_info_.sitl_ip_port); + } + + const GpsBase* getGps() const + { + return static_cast(sensors_->getByType(SensorBase::SensorType::Gps)); + } + const ImuBase* getImu() const + { + return static_cast(sensors_->getByType(SensorBase::SensorType::Imu)); + } + const MagnetometerBase* getMagnetometer() const + { + return static_cast(sensors_->getByType(SensorBase::SensorType::Magnetometer)); + } + const BarometerBase* getBarometer() const + { + return static_cast(sensors_->getByType(SensorBase::SensorType::Barometer)); + } + const LidarBase* getLidar() const + { + return static_cast(sensors_->getByType(SensorBase::SensorType::Lidar)); + } + +private: + virtual void normalizeRotorControls() + { + // change 1000-2000 to 0-1. + for (size_t i = 0; i < Utils::length(rotor_controls_); ++i) { + rotor_controls_[i] = (rotor_controls_[i] - 1000.0f) / 1000.0f; + } + } + + void sendSensors() + { + if (sensors_ == nullptr) + return; + + const auto& gps_output = getGps()->getOutput(); + const auto& imu_output = getImu()->getOutput(); + // const auto& baro_output = getBarometer()->getOutput(); + // const auto& mag_output = getMagnetometer()->getOutput(); + + std::ostringstream oss; + + // Send RC channels to Ardupilot if present + if (is_rc_connected && last_rcData_.is_valid) { + oss << "," + "\"rc\": {" + "\"channels\": [" + << (last_rcData_.roll + 1) * 0.5f << "," + << (last_rcData_.yaw + 1) * 0.5f << "," + << (last_rcData_.throttle + 1) * 0.5f << "," + << (-last_rcData_.pitch + 1) * 0.5f << "," + << static_cast(last_rcData_.getSwitch(0)) << "," + << static_cast(last_rcData_.getSwitch(1)) << "," + << static_cast(last_rcData_.getSwitch(2)) << "," + << static_cast(last_rcData_.getSwitch(3)) + << "]}"; + } + + const auto lidar = getLidar(); + // TODO: Add bool value in settings to check whether to send lidar data or not + // Since it's possible that we don't want to send the lidar data to Ardupilot but still have the lidar (maybe as a ROS topic) + if (lidar != nullptr) { + const auto& lidar_output = lidar->getOutput(); + oss << "," + "\"lidar\": {" + "\"point_cloud\": ["; + + std::copy(lidar_output.point_cloud.begin(), lidar_output.point_cloud.end(), std::ostream_iterator(oss, ",")); + oss << "]}"; + } + + float yaw; + float pitch; + float roll; + VectorMath::toEulerianAngle(imu_output.orientation, pitch, roll, yaw); + + char buf[65000]; + + // TODO: Split the following sensor packet formation into different parts for individual sensors + + // UDP packets have a maximum size limit of 65kB + int ret = snprintf(buf, sizeof(buf), + "{" + "\"timestamp\": %" PRIu64 "," + "\"imu\": {" + "\"angular_velocity\": [%lf, %lf, %lf]," + "\"linear_acceleration\": [%lf, %lf, %lf]" + "}," + "\"gps\": {" + "\"lat\": %lf," + "\"lon\": %lf," + "\"alt\": %lf" + "}," + "\"velocity\": {" + "\"world_linear_velocity\": [%lf, %lf, %lf]" + "}," + "\"pose\": {" + "\"roll\": %lf," + "\"pitch\": %lf," + "\"yaw\": %lf" + "}" + "%s" + "}\n", + static_cast(msr::airlib::ClockFactory::get()->nowNanos() / 1.0E3), + imu_output.angular_velocity[0], + imu_output.angular_velocity[1], + imu_output.angular_velocity[2], + imu_output.linear_acceleration[0], + imu_output.linear_acceleration[1], + imu_output.linear_acceleration[2], + gps_output.gnss.geo_point.latitude, + gps_output.gnss.geo_point.longitude, + gps_output.gnss.geo_point.altitude, + gps_output.gnss.velocity[0], + gps_output.gnss.velocity[1], + gps_output.gnss.velocity[2], + roll, pitch, yaw, + oss.str().c_str()); + + if (ret < 0) { + Utils::log("Encoding error while forming sensor message", Utils::kLogLevelInfo); + return; + } + else if (static_cast(ret) >= sizeof(buf)) { + Utils::log(Utils::stringf("Sensor message truncated, lost %d bytes", ret - sizeof(buf)), Utils::kLogLevelInfo); + } + + // Send data + if (udpSocket_ != nullptr) { + udpSocket_->sendto(buf, strlen(buf), ip, port); + } + } + + void recvRotorControl() + { + // Receive motor data + RotorControlMessage pkt; + int recv_ret = udpSocket_->recv(&pkt, sizeof(pkt), 100); + while (recv_ret != sizeof(pkt)) { + if (recv_ret <= 0) { + Utils::log(Utils::stringf("Error while receiving rotor control data - ErrorNo: %d", recv_ret), Utils::kLogLevelInfo); + } else { + Utils::log(Utils::stringf("Received %d bytes instead of %zu bytes", recv_ret, sizeof(pkt)), Utils::kLogLevelInfo); + } + + recv_ret = udpSocket_->recv(&pkt, sizeof(pkt), 100); + } + + for (auto i = 0; i < RotorControlCount && i < kArduCopterRotorControlCount; ++i) { + rotor_controls_[i] = pkt.pwm[i]; + } + + normalizeRotorControls(); + } + +private: + static const int kArduCopterRotorControlCount = 11; + + struct RotorControlMessage { + uint16_t pwm[kArduCopterRotorControlCount]; + }; + + std::shared_ptr udpSocket_; + + AirSimSettings::MavLinkConnectionInfo connection_info_; + uint16_t port; + const std::string& ip; + const SensorCollection* sensors_; + const MultiRotorParams* vehicle_params_; + + MultirotorApiParams safety_params_; + + RCData last_rcData_; + bool is_rc_connected; + + // TODO: Increase to 6 or 8 for hexa or larger frames, 11 was used in SoloAPI + static const int RotorControlCount = 4; + + float rotor_controls_[RotorControlCount]; +}; + +}} //namespace +#endif diff --git a/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterParams.hpp b/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterParams.hpp new file mode 100644 index 0000000000..c6117a64a5 --- /dev/null +++ b/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterParams.hpp @@ -0,0 +1,84 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef msr_airlib_vehicles_ArduCopter_hpp +#define msr_airlib_vehicles_ArduCopter_hpp + +#include "vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp" +#include "vehicles/multirotor/MultiRotorParams.hpp" +#include "common/AirSimSettings.hpp" +#include "sensors/SensorFactory.hpp" + + +namespace msr { namespace airlib { + +class ArduCopterParams : public MultiRotorParams { +public: + ArduCopterParams(const AirSimSettings::MavLinkVehicleSetting& vehicle_setting, std::shared_ptr sensor_factory) + : sensor_factory_(sensor_factory) + { + connection_info_ = getConnectionInfo(vehicle_setting); + } + + virtual ~ArduCopterParams() = default; + + virtual std::unique_ptr createMultirotorApi() override + { + return std::unique_ptr(new ArduCopterApi(this, connection_info_)); + } + +protected: + // TODO: Change the below function to setupGenericQuadX() which can be called by setupParams() based on connection_info.model + // This will be needed for different frames + virtual void setupParams() override + { + auto& params = getParams(); + + /******* Below is same config as PX4 generic model ********/ + + //set up arm lengths + //dimensions are for F450 frame: http://artofcircuits.com/product/quadcopter-frame-hj450-with-power-distribution + params.rotor_count = 4; + std::vector arm_lengths(params.rotor_count, 0.2275f); + + //set up mass + params.mass = 1.0f; //can be varied from 0.800 to 1.600 + real_T motor_assembly_weight = 0.055f; //weight for MT2212 motor for F450 frame + real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight; + + // using rotor_param default, but if you want to change any of the rotor_params, call calculateMaxThrust() to recompute the max_thrust + // given new thrust coefficients, motor max_rpm and propeller diameter. + params.rotor_params.calculateMaxThrust(); + + //set up dimensions of core body box or abdomen (not including arms). + params.body_box.x() = 0.180f; params.body_box.y() = 0.11f; params.body_box.z() = 0.040f; + real_T rotor_z = 2.5f / 100; + + //computer rotor poses + initializeRotorQuadX(params.rotor_poses, params.rotor_count, arm_lengths.data(), rotor_z); + + //compute inertia matrix + computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight); + + //leave everything else to defaults + } + + virtual const SensorFactory* getSensorFactory() const override + { + return sensor_factory_.get(); + } + + static const AirSimSettings::MavLinkConnectionInfo& getConnectionInfo(const AirSimSettings::MavLinkVehicleSetting& vehicle_setting) + { + return vehicle_setting.connection_info; + } + +private: + AirSimSettings::MavLinkConnectionInfo connection_info_; + vector> sensor_storage_; + // const AirSimSettings::MavlinkVehicleSetting* vehicle_setting_; //store as pointer because of derived classes + std::shared_ptr sensor_factory_; +}; + +}} //namespace +#endif diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp index 0b7a7c9830..73273359f0 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp @@ -73,9 +73,9 @@ class MavLinkMultirotorApi : public MultirotorApiBase } //reset PX4 stack - virtual void reset() override + virtual void resetImplementation() override { - MultirotorApiBase::reset(); + MultirotorApiBase::resetImplementation(); resetState(); was_reset_ = true; @@ -467,13 +467,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase endOffboardMode(); } -public: //types - typedef msr::airlib::GeoPoint GeoPoint; - typedef msr::airlib::VectorMath VectorMath; - typedef msr::airlib::Vector3r Vector3r; - typedef msr::airlib::Quaternionr Quaternionr; - typedef common_utils::Utils Utils; - typedef msr::airlib::real_T real_T; +public: class MavLinkLogViewerLog : public mavlinkcom::MavLinkLog { diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightBoard.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightBoard.hpp index 49ba0d40d1..1b301588ef 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightBoard.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightBoard.hpp @@ -65,6 +65,11 @@ class AirSimSimpleFlightBoard : public simple_flight::IBoard { return input_channels_[index]; } + virtual float getAvgMotorOutput() const override + { + return ((getMotorControlSignal(0) + getMotorControlSignal(1) + getMotorControlSignal(2) + getMotorControlSignal(3)) / 4); + } + virtual bool isRcConnected() const override { return is_connected_; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp index 7707881c53..71dfa99105 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp @@ -44,9 +44,9 @@ class SimpleFlightApi : public MultirotorApiBase { public: //VehicleApiBase implementation - virtual void reset() override + virtual void resetImplementation() override { - MultirotorApiBase::reset(); + MultirotorApiBase::resetImplementation(); firmware_->reset(); } diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp index 4da9f86787..42156bb4e8 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp @@ -318,14 +318,14 @@ class AdaptiveController : public IController { { double Fz, xddot, yddot; int i; - Fz = (zddot + 9.81)*0.9116; + Fz = (zddot + 9.81)*m; xddot = -2.0 * xdot - 2.0 * (x_current - x_desired); yddot = -2.0 * ydot - 3.0 * (y_current - y_desired); if (Fz == 0) { - Fz = 9.81*0.9116; + Fz = 9.81*m; } - result[0][0] = (xddot*sin(yaw_current) - yddot*cos(yaw_current))*0.9116 / Fz; - result[1][0] = (xddot*cos(yaw_current) + yddot*sin(yaw_current))*0.9116 / Fz; + result[0][0] = (xddot*sin(yaw_current) - yddot*cos(yaw_current))*m / Fz; + result[1][0] = (xddot*cos(yaw_current) + yddot*sin(yaw_current))*m / Fz; // Limit the angle reference values for (i = 0; i < 2; i++) { if (result[i][0] > 0.3) { diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp index 2a34860075..389108a6f2 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp @@ -219,7 +219,9 @@ class OffboardApi : // if we are not trying to move by setting motor outputs if (takenoff_) { - if (!isGreaterThanArmedThrottle(goal_.throttle())) { + //if (!isGreaterThanArmedThrottle(goal_.throttle())) { + float checkThrottle = rc_.getMotorOutput(); + if (!isGreaterThanArmedThrottle(checkThrottle)) { // and we are not currently moving (based on current velocities) auto angular = state_estimator_->getAngularVelocity(); auto velocity = state_estimator_->getLinearVelocity(); @@ -238,9 +240,10 @@ class OffboardApi : // if we are not trying to move by setting motor outputs if (!takenoff_) { + float checkThrottle = rc_.getMotorOutput(); //TODO: better handling of landed & takenoff states - if (isGreaterThanArmedThrottle(goal_.throttle()) && - std::abs(state_estimator_->getLinearVelocity().z()) > 0.01f) { + if (isGreaterThanArmedThrottle(checkThrottle) && + std::abs(state_estimator_->getLinearVelocity().z()) > 0.01f) { takenoff_ = true; landed_ = false; } diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/RemoteControl.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/RemoteControl.hpp index 7c0498d655..0ce7585204 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/RemoteControl.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/RemoteControl.hpp @@ -148,6 +148,11 @@ class RemoteControl : return allow_api_control_; } + float getMotorOutput() + { + return board_inputs_->getAvgMotorOutput(); + } + private: enum class RcRequestType { None, ArmRequest, DisarmRequest, NeutralRequest diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IBoardInputPins.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IBoardInputPins.hpp index b7dbaf8e57..1a229849a5 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IBoardInputPins.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IBoardInputPins.hpp @@ -8,6 +8,7 @@ class IBoardInputPins { public: virtual float readChannel(uint16_t index) const = 0; //output -1 to 1 virtual bool isRcConnected() const = 0; + virtual float getAvgMotorOutput() const = 0; }; } \ No newline at end of file diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index 0c5cace3ac..82ba021c9b 100644 --- a/AirLib/src/api/RpcLibClientBase.cpp +++ b/AirLib/src/api/RpcLibClientBase.cpp @@ -103,7 +103,7 @@ int RpcLibClientBase::getMinRequiredClientVersion() const } int RpcLibClientBase::getServerVersion() const { - return pimpl_->client.call("getServerVersion").as(); + return pimpl_->client.call("getServerVersion").as(); } void RpcLibClientBase::reset() @@ -189,6 +189,11 @@ msr::airlib::DistanceBase::Output RpcLibClientBase::getDistanceSensorData(const return pimpl_->client.call("getDistanceSensorData", distance_sensor_name, vehicle_name).as().to(); } +vector RpcLibClientBase::simGetLidarSegmentation(const std::string& lidar_name, const std::string& vehicle_name) const +{ + return pimpl_->client.call("simGetLidarSegmentation", lidar_name, vehicle_name).as>(); +} + bool RpcLibClientBase::simSetSegmentationObjectID(const std::string& mesh_name, int object_id, bool is_name_regex) { return pimpl_->client.call("simSetSegmentationObjectID", mesh_name, object_id, is_name_regex).as(); diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index 44f841bdee..3146d46acf 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -9,7 +9,6 @@ #include "api/RpcLibServerBase.hpp" - #include "common/Common.hpp" STRICT_MODE_OFF @@ -30,6 +29,7 @@ STRICT_MODE_OFF #include "common/common_utils/WindowsApisCommonPost.hpp" #include "api/RpcLibAdapatorsBase.hpp" +#include STRICT_MODE_ON @@ -56,10 +56,12 @@ typedef msr::airlib_rpclib::RpcLibAdapatorsBase RpcLibAdapatorsBase; RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& server_address, uint16_t port) : api_provider_(api_provider) { + if (server_address == "") pimpl_.reset(new impl(port)); else pimpl_.reset(new impl(server_address, port)); + pimpl_->server.bind("ping", [&]() -> bool { return true; }); pimpl_->server.bind("getServerVersion", []() -> int { return 1; @@ -124,6 +126,11 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& return RpcLibAdapatorsBase::Pose(pose); }); + pimpl_->server. + bind("simGetLidarSegmentation", [&](const std::string& lidar_name, const std::string& vehicle_name) -> std::vector { + return getVehicleApi(vehicle_name)->getLidarSegmentation(lidar_name); + }); + pimpl_->server. bind("simSetSegmentationObjectID", [&](const std::string& mesh_name, int object_id, bool is_name_regex) -> bool { return getWorldSimApi()->setSegmentationObjectID(mesh_name, object_id, is_name_regex); @@ -134,11 +141,19 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& }); pimpl_->server.bind("reset", [&]() -> void { + //Exit if already resetting. + static bool resetInProgress; + if (resetInProgress) + return; + + //Reset + resetInProgress = true; auto* sim_world_api = getWorldSimApi(); if (sim_world_api) sim_world_api->reset(); else getVehicleApi("")->reset(); + resetInProgress = false; }); pimpl_->server.bind("simPrintLogMessage", [&](const std::string& message, const std::string& message_param, unsigned char severity) -> void { diff --git a/AirLib/src/vehicles/car/api/CarRpcLibClient.cpp b/AirLib/src/vehicles/car/api/CarRpcLibClient.cpp index a1eefbd370..285e21c594 100644 --- a/AirLib/src/vehicles/car/api/CarRpcLibClient.cpp +++ b/AirLib/src/vehicles/car/api/CarRpcLibClient.cpp @@ -65,6 +65,11 @@ CarApiBase::CarState CarRpcLibClient::getCarState(const std::string& vehicle_nam return static_cast(getClient())-> call("getCarState", vehicle_name).as().to(); } +CarApiBase::CarControls CarRpcLibClient::getCarControls(const std::string& vehicle_name) +{ + return static_cast(getClient())-> + call("getCarControls", vehicle_name).as().to(); +} }} //namespace diff --git a/AirLib/src/vehicles/car/api/CarRpcLibServer.cpp b/AirLib/src/vehicles/car/api/CarRpcLibServer.cpp index daaa8459e1..ed1209d083 100644 --- a/AirLib/src/vehicles/car/api/CarRpcLibServer.cpp +++ b/AirLib/src/vehicles/car/api/CarRpcLibServer.cpp @@ -51,6 +51,10 @@ CarRpcLibServer::CarRpcLibServer(ApiProvider* api_provider, string server_addres bind("setCarControls", [&](const CarRpcLibAdapators::CarControls& controls, const std::string& vehicle_name) -> void { getVehicleApi(vehicle_name)->setCarControls(controls.to()); }); + (static_cast(getServer()))-> + bind("getCarControls", [&](const std::string& vehicle_name) -> CarRpcLibAdapators::CarControls { + return CarRpcLibAdapators::CarControls(getVehicleApi(vehicle_name)->getCarControls()); + }); } diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp index 906d891f21..be32d215fd 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp @@ -13,12 +13,10 @@ namespace msr { namespace airlib { -void MultirotorApiBase::reset() +void MultirotorApiBase::resetImplementation() { cancelLastTask(); SingleTaskCall lock(this); //cancel previous tasks - - VehicleApiBase::reset(); } bool MultirotorApiBase::takeoff(float timeout_sec) diff --git a/AirLibUnitTests/QuaternionTest.hpp b/AirLibUnitTests/QuaternionTest.hpp index 04f355457c..ffff74b850 100644 --- a/AirLibUnitTests/QuaternionTest.hpp +++ b/AirLibUnitTests/QuaternionTest.hpp @@ -52,15 +52,15 @@ class QuaternionTest : public TestBase if (rotAxis.squaredNorm() == 0) rotAxis = VectorMath::up(); float dot = VectorMath::front().dot(toVector); - float ang = std::acosf(dot); + float ang = std::acos(dot); return VectorMath::toQuaternion(rotAxis, ang); } Quaternionr toQuaternion(const Vector3r& axis, float angle) { - auto s = std::sinf(angle / 2); + auto s = std::sin(angle / 2.0f); auto u = axis.normalized(); - return Quaternionr(std::cosf(angle / 2), u.x() * s, u.y() * s, u.z() * s); + return Quaternionr(std::cos(angle / 2.0f), u.x() * s, u.y() * s, u.z() * s); } void lookAtTest() diff --git a/MavLinkCom/MavLinkCom.vcxproj b/MavLinkCom/MavLinkCom.vcxproj index b17f341e67..7838ec7854 100644 --- a/MavLinkCom/MavLinkCom.vcxproj +++ b/MavLinkCom/MavLinkCom.vcxproj @@ -343,6 +343,8 @@ + + @@ -374,6 +376,7 @@ + @@ -385,6 +388,7 @@ + diff --git a/MavLinkCom/MavLinkCom.vcxproj.filters b/MavLinkCom/MavLinkCom.vcxproj.filters index a1df1d0f01..4d1686b177 100644 --- a/MavLinkCom/MavLinkCom.vcxproj.filters +++ b/MavLinkCom/MavLinkCom.vcxproj.filters @@ -78,6 +78,8 @@ + + @@ -217,6 +219,8 @@ + + diff --git a/MavLinkCom/include/UdpSocket.hpp b/MavLinkCom/include/UdpSocket.hpp new file mode 100644 index 0000000000..9b032f6e1c --- /dev/null +++ b/MavLinkCom/include/UdpSocket.hpp @@ -0,0 +1,56 @@ +#ifndef MavLinkCom_UdpSocket_hpp +#define MavLinkCom_UdpSocket_hpp + +#include +#include + +namespace mavlinkcom_impl { + class UdpSocketImpl; +} + +namespace mavlinkcom { + + class UdpSocket; + + // This class represents a simple single-threaded Socket interface specific for Udp connections + // Basic socket functions are exposed through this + class UdpSocket + { + public: + UdpSocket(); + + // initiate a connection on a socket + int connect(const std::string& addr, int port); + + // bind the socket to an address + int bind(const std::string& localaddr, int port); + + void close(); + + // Send message on a socket + // Used when the socket is in a connected state (so that the intended recipient is known) + int send(const void *pkt, size_t size); + + // Send message to the specified address, port + int sendto(const void *buf, size_t size, const std::string& address, uint16_t port); + + // Receive message on socket + int recv(void *pkt, size_t size, uint32_t timeout_ms); + + // return the IP address and port of the last received packet + void last_recv_address(std::string& ip_addr, uint16_t &port); + + bool reuseaddress(); + void set_broadcast(void); + + public: + //needed for piml pattern + ~UdpSocket(); + + private: + std::unique_ptr pImpl; + friend class mavlinkcom_impl::UdpSocketImpl; + }; +} + +#endif diff --git a/MavLinkCom/src/UdpSocket.cpp b/MavLinkCom/src/UdpSocket.cpp new file mode 100644 index 0000000000..ddb5c8375c --- /dev/null +++ b/MavLinkCom/src/UdpSocket.cpp @@ -0,0 +1,60 @@ +#include "UdpSocket.hpp" +#include "impl/UdpSocketImpl.hpp" + +using namespace mavlinkcom; +using namespace mavlinkcom_impl; + +UdpSocket::UdpSocket() +{ + pImpl.reset(new UdpSocketImpl()); +} + +int UdpSocket::connect(const std::string& addr, int port) +{ + return pImpl->connect(addr, port); +} + +int UdpSocket::bind(const std::string& localaddr, int port) +{ + return pImpl->bind(localaddr, port); +} + +void UdpSocket::close() +{ + pImpl->close(); +} + +int UdpSocket::send(const void *pkt, size_t size) +{ + return pImpl->send(pkt, size); +} + +int UdpSocket::sendto(const void *buf, size_t size, const std::string& address, uint16_t port) +{ + return pImpl->sendto(buf, size, address, port); +} + +int UdpSocket::recv(void *pkt, size_t size, uint32_t timeout_ms) +{ + return pImpl->recv(pkt, size, timeout_ms); +} + +void UdpSocket::last_recv_address(std::string& ip_addr, uint16_t& port) +{ + pImpl->last_recv_address(ip_addr, port); +} + +bool UdpSocket::reuseaddress() +{ + return pImpl->reuseaddress(); +} + +void UdpSocket::set_broadcast(void) +{ + pImpl->set_broadcast(); +} + +UdpSocket::~UdpSocket() { + pImpl->close(); + pImpl = nullptr; +} diff --git a/MavLinkCom/src/impl/UdpSocketImpl.cpp b/MavLinkCom/src/impl/UdpSocketImpl.cpp new file mode 100644 index 0000000000..66439d49ec --- /dev/null +++ b/MavLinkCom/src/impl/UdpSocketImpl.cpp @@ -0,0 +1,201 @@ +#ifndef MavLinkCom_UdpSocketImpl_cpp +#define MavLinkCom_UdpSocketImpl_cpp + +#include "UdpSocketImpl.hpp" + +using namespace mavlinkcom_impl; + + +UdpSocketImpl::UdpSocketImpl() +{ + fd = socket(AF_INET, SOCK_DGRAM, 0); +} + +void UdpSocketImpl::close() +{ + if (fd != -1) { +#ifdef _WIN32 + closesocket(fd); +#else + ::close(fd); +#endif + fd = -1; + } +} + +UdpSocketImpl::~UdpSocketImpl() +{ + close(); +} + +void UdpSocketImpl::make_sockaddr(const std::string& address, uint16_t port, struct sockaddr_in &sockaddr) +{ + memset(&sockaddr, 0, sizeof(sockaddr)); + +#ifdef HAVE_SOCK_SIN_LEN + sockaddr.sin_len = sizeof(sockaddr); +#endif + sockaddr.sin_port = htons(port); + sockaddr.sin_family = AF_INET; + sockaddr.sin_addr.s_addr = inet_addr(address.c_str()); +} + +/* + connect the socket + */ +int UdpSocketImpl::connect(const std::string& address, uint16_t port) +{ + struct sockaddr_in sockaddr; + make_sockaddr(address, port, sockaddr); + + int rc = ::connect(fd, reinterpret_cast(&sockaddr), sizeof(sockaddr)); + if (rc != 0) { + int hr = WSAGetLastError(); + throw std::runtime_error(Utils::stringf("UdpSocket connect failed with error: %d\n", hr)); + return hr; + } + return 0; +} + +/* + bind the socket + */ +int UdpSocketImpl::bind(const std::string& address, uint16_t port) +{ + struct sockaddr_in sockaddr; + make_sockaddr(address, port, sockaddr); + + int rc = ::bind(fd, reinterpret_cast(&sockaddr), sizeof(sockaddr)); + if (rc != 0) + { + int hr = WSAGetLastError(); + throw std::runtime_error(Utils::stringf("UdpSocket bind failed with error: %d\n", hr)); + return hr; + } + return 0; +} + + +/* + set SO_REUSEADDR + */ +bool UdpSocketImpl::reuseaddress(void) +{ + int one = 1; + return (setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, reinterpret_cast(&one), sizeof(one)) != -1); +} + +/* + send some data + */ +int UdpSocketImpl::send(const void *buf, size_t size) +{ + int hr = ::send(fd, reinterpret_cast(buf), static_cast(size), 0); + if (hr == SOCKET_ERROR) + { + hr = WSAGetLastError(); + throw std::runtime_error(Utils::stringf("Udp socket send failed with error: %d\n", hr)); + } + return hr; +} + +/* + send some data + */ +int UdpSocketImpl::sendto(const void *buf, size_t size, const std::string& address, uint16_t port) +{ + struct sockaddr_in sockaddr; + make_sockaddr(address, port, sockaddr); + int hr = ::sendto(fd, reinterpret_cast(buf), static_cast(size), 0, reinterpret_cast(&sockaddr), sizeof(sockaddr)); + if (hr == SOCKET_ERROR) + { + hr = WSAGetLastError(); + throw std::runtime_error(Utils::stringf("Udp socket send failed with error: %d\n", hr)); + } + return hr; +} + +/* + receive some data + */ +int UdpSocketImpl::recv(void *buf, size_t size, uint32_t timeout_ms) +{ + if (!pollin(timeout_ms)) { + return -1; + } + socklen_t len = sizeof(in_addr); + int rc = ::recvfrom(fd, reinterpret_cast(buf), static_cast(size), 0, reinterpret_cast(&in_addr), &len); + if (rc < 0) + { + rc = WSAGetLastError(); + Utils::log(Utils::stringf("Udp Socket recv failed with error: %d", rc)); + } + return rc; +} + +/* + return the IP address and port of the last received packet + */ +void UdpSocketImpl::last_recv_address(std::string& ip_addr, uint16_t &port) +{ + ip_addr = inet_ntoa(in_addr.sin_addr); + port = ntohs(in_addr.sin_port); +} + +void UdpSocketImpl::set_broadcast(void) +{ + int one = 1; + setsockopt(fd,SOL_SOCKET,SO_BROADCAST,reinterpret_cast(&one),sizeof(one)); +} + +/* + return true if there is pending data for input + */ +bool UdpSocketImpl::pollin(uint32_t timeout_ms) +{ + fd_set fds; + struct timeval tv; + + FD_ZERO(&fds); + FD_SET(fd, &fds); + + tv.tv_sec = timeout_ms / 1000; + tv.tv_usec = (timeout_ms % 1000) * 1000UL; + +#ifdef _WIN32 + if (select(0, &fds, nullptr, nullptr, &tv) != 1) { +#else + if (select(fd+1, &fds, nullptr, nullptr, &tv) != 1) { +#endif + return false; + } + return true; +} + + +/* + return true if there is room for output data + */ +bool UdpSocketImpl::pollout(uint32_t timeout_ms) +{ + fd_set fds; + struct timeval tv; + + FD_ZERO(&fds); + FD_SET(fd, &fds); + + tv.tv_sec = timeout_ms / 1000; + tv.tv_usec = (timeout_ms % 1000) * 1000UL; + +#ifdef _WIN32 + if (select(0, &fds, nullptr, nullptr, &tv) != 1) { +#else + if (select(fd+1, &fds, nullptr, nullptr, &tv) != 1) { +#endif + return false; + } + return true; +} + + +#endif // MavLinkCom_UdpSocketImpl_cpp diff --git a/MavLinkCom/src/impl/UdpSocketImpl.hpp b/MavLinkCom/src/impl/UdpSocketImpl.hpp new file mode 100644 index 0000000000..125b0f93e5 --- /dev/null +++ b/MavLinkCom/src/impl/UdpSocketImpl.hpp @@ -0,0 +1,85 @@ +#ifndef MavLinkCom_UdpSocketImpl_hpp +#define MavLinkCom_UdpSocketImpl_hpp + +#include "Utils.hpp" +#include "UdpSocket.hpp" +#ifdef _WIN32 +// windows +#ifndef WIN32_LEAN_AND_MEAN +#define WIN32_LEAN_AND_MEAN +#endif +#include +#include +#include +// Need to link with Ws2_32.lib +#pragma comment (lib, "Ws2_32.lib") + +typedef int socklen_t; +static bool socket_initialized_ = false; +#else + +// posix +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +typedef int SOCKET; +const int INVALID_SOCKET = -1; +const int ERROR_ACCESS_DENIED = EACCES; + +inline int WSAGetLastError() { + return errno; +} +const int SOCKET_ERROR = -1; +#define E_NOT_SUFFICIENT_BUFFER ENOMEM + +#endif + +using namespace mavlink_utils; +using namespace mavlinkcom; + +namespace mavlinkcom_impl { + + class UdpSocketImpl + { + public: + UdpSocketImpl(); + ~UdpSocketImpl(); + + int connect(const std::string& address, uint16_t port); + int bind(const std::string& address, uint16_t port); + bool reuseaddress(); + void set_broadcast(void); + + int send(const void *pkt, size_t size); + int sendto(const void *buf, size_t size, const std::string& address, uint16_t port); + int recv(void *pkt, size_t size, uint32_t timeout_ms); + + // return the IP address and port of the last received packet + void last_recv_address(std::string& ip_addr, uint16_t &port); + + void close(); + + private: + struct sockaddr_in in_addr {}; + + SOCKET fd = -1; + + // return true if there is pending data for input + bool pollin(uint32_t timeout_ms); + + // return true if there is room for output data + bool pollout(uint32_t timeout_ms); + + void make_sockaddr(const std::string& address, uint16_t port, struct sockaddr_in &sockaddr); + }; + +} + +#endif diff --git a/PythonClient/README.md b/PythonClient/README.md index a2d8a4bdfa..a5ec115d93 100644 --- a/PythonClient/README.md +++ b/PythonClient/README.md @@ -3,7 +3,7 @@ This package contains Python APIs for [AirSim](https://github.com/microsoft/airsim). ## How to Use -See examples at [car/hello_car.py](https://github.com/Microsoft/AirSim/blob/master/PythonClient/car/hello_car.py) or [multirotor\hello_drone.py](https://github.com/Microsoft/AirSim/blob/master/PythonClient/hello_drone.py). +See examples at [car/hello_car.py](https://github.com/Microsoft/AirSim/blob/master/PythonClient/car/hello_car.py) or [multirotor/hello_drone.py](https://github.com/microsoft/AirSim/blob/master/PythonClient/multirotor/hello_drone.py). ## Dependencies This package depends on `msgpack` and would automatically install `msgpack-rpc-python` (this may need administrator/sudo prompt): diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index d7e302dc2f..fe141d3802 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -161,6 +161,9 @@ def getDistanceSensorData(self, lidar_name = '', vehicle_name = ''): def getLidarData(self, lidar_name = '', vehicle_name = ''): return LidarData.from_msgpack(self.client.call('getLidarData', lidar_name, vehicle_name)) + + def simGetLidarSegmentation(self, lidar_name = '', vehicle_name = ''): + return self.client.call('simGetLidarSegmentation', lidar_name, vehicle_name) #----------- APIs to control ACharacter in scene ----------/ def simCharSetFaceExpression(self, expression_name, value, character_name = ""): @@ -196,9 +199,9 @@ def simCharSetBonePoses(self, poses, character_name = ""): def simCharGetBonePoses(self, bone_names, character_name = ""): return self.client.call('simGetBonePoses', bone_names, character_name) - def cancelLastTask(): - self.client.call('cancelLastTask') - def waitOnLastTask(timeout_sec = float('nan')): + def cancelLastTask(self, vehicle_name = ''): + self.client.call('cancelLastTask', vehicle_name) + def waitOnLastTask(self, timeout_sec = float('nan')): return self.client.call('waitOnLastTask', timeout_sec) # legacy handling @@ -335,3 +338,6 @@ def setCarControls(self, controls, vehicle_name = ''): def getCarState(self, vehicle_name = ''): state_raw = self.client.call('getCarState', vehicle_name) return CarState.from_msgpack(state_raw) + def getCarControls(self, vehicle_name=''): + controls_raw = self.client.call('getCarControls', vehicle_name) + return CarControls.from_msgpack(controls_raw) \ No newline at end of file diff --git a/PythonClient/airsim/types.py b/PythonClient/airsim/types.py index cda3b898e1..ed1c98f4b6 100644 --- a/PythonClient/airsim/types.py +++ b/PythonClient/airsim/types.py @@ -197,7 +197,9 @@ class Pose(MsgpackMixin): position = Vector3r() orientation = Quaternionr() - def __init__(self, position_val = Vector3r(), orientation_val = Quaternionr()): + def __init__(self, position_val = None, orientation_val = None): + position_val = position_val if position_val != None else Vector3r() + orientation_val = orientation_val if orientation_val != None else Quaternionr() self.position = position_val self.orientation = orientation_val diff --git a/PythonClient/multirotor/orbit.py b/PythonClient/multirotor/orbit.py index ac5f99a112..9e6b3b996f 100644 --- a/PythonClient/multirotor/orbit.py +++ b/PythonClient/multirotor/orbit.py @@ -38,7 +38,7 @@ def __init__(self, radius = 2, altitude = 10, speed = 2, iterations = 1, center # center is just a direction vector, so normalize it to compute the actual cx,cy locations. cx = float(center[0]) cy = float(center[1]) - length = math.sqrt(cx*cx)+(cy*cy) + length = math.sqrt((cx*cx) + (cy*cy)) cx /= length cy /= length cx *= self.radius diff --git a/README.md b/README.md index 803b3a1aaa..b196622b05 100644 --- a/README.md +++ b/README.md @@ -80,7 +80,7 @@ A better way to generate training data exactly the way you want is by accessing Yet another way to use AirSim is the so-called "Computer Vision" mode. In this mode, you don't have vehicles or physics. You can use the keyboard to move around the scene, or use APIs to position available cameras in any arbitrary pose, and collect images such as depth, disparity, surface normals or object segmentation. -[More details](https://microsoft.github.io/AirSim/docs/docs/image_apis/) +[More details](https://microsoft.github.io/AirSim/docs/image_apis/) ### Weather Effects diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp b/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp index 4c8d72b00d..94f35e17f1 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp @@ -146,9 +146,8 @@ AirSimPose PawnSimApi::GetInitialPose() return AirSimPose(state_.start_location, state_.start_rotation); } -void PawnSimApi::reset() +void PawnSimApi::resetImplementation() { - VehicleSimApiBase::reset(); state_ = initial_state_; rc_data_ = msr::airlib::RCData(); kinematics_->reset(); diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.h b/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.h index 4dba5adf9a..0644653dc0 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.h +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.h @@ -64,7 +64,7 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase public: virtual void initialize() override; PawnSimApi(const Params& params); - virtual void reset() override; + virtual void resetImplementation() override; virtual void update() override; virtual const UnityImageCapture* getImageCapture() const override; virtual std::vector getImages(const std::vector& request) const override; diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Car/CarPawnApi.cpp b/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Car/CarPawnApi.cpp index a6dd398bec..dfce5b6030 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Car/CarPawnApi.cpp +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Car/CarPawnApi.cpp @@ -46,9 +46,9 @@ msr::airlib::CarApiBase::CarState CarPawnApi::getCarState() const return state; } -void CarPawnApi::reset() +void CarPawnApi::resetImplementation() { - msr::airlib::CarApiBase::reset(); + msr::airlib::CarApiBase::resetImplementation(); last_controls_ = CarControls(); setCarControls(CarControls()); diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Car/CarPawnApi.h b/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Car/CarPawnApi.h index 2dddc9e146..37897c4712 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Car/CarPawnApi.h +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Car/CarPawnApi.h @@ -16,7 +16,7 @@ class CarPawnApi : public msr::airlib::CarApiBase const msr::airlib::Kinematics::State& state, const msr::airlib::Environment& environment); virtual void setCarControls(const CarApiBase::CarControls& controls) override; virtual CarApiBase::CarState getCarState() const override; - virtual void reset() override; + virtual void resetImplementation() override; virtual void update() override; virtual msr::airlib::GeoPoint getHomeGeoPoint() const override; virtual void enableApiControl(bool is_enabled) override; diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Car/CarPawnSimApi.cpp b/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Car/CarPawnSimApi.cpp index 2bb573581a..4e92005703 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Car/CarPawnSimApi.cpp +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Car/CarPawnSimApi.cpp @@ -163,12 +163,12 @@ void CarPawnSimApi::updateCarControls() } //*** Start: UpdatableState implementation ***// -void CarPawnSimApi::reset() +void CarPawnSimApi::resetImplementation() { setPose(UnityUtilities::Convert_to_Pose(GetInitialPose()), false); Reset(getVehicleName().c_str()); - PawnSimApi::reset(); + PawnSimApi::resetImplementation(); vehicle_api_->reset(); } diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Car/CarPawnSimApi.h b/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Car/CarPawnSimApi.h index 967e074a90..48079c20e5 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Car/CarPawnSimApi.h +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Car/CarPawnSimApi.h @@ -21,7 +21,7 @@ class CarPawnSimApi : public PawnSimApi virtual void initialize() override; CarPawnSimApi(const Params& params, const CarPawnApi::CarControls& keyboard_controls, std::string car_name); virtual ~CarPawnSimApi() = default; - virtual void reset() override; + virtual void resetImplementation() override; virtual void update() override; //virtual void reportState(StateReporter& reporter) override; virtual std::string getRecordFileLine(bool is_header_line) const override; diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Multirotor/MultirotorPawnSimApi.cpp b/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Multirotor/MultirotorPawnSimApi.cpp index beedf764bf..6900983c17 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Multirotor/MultirotorPawnSimApi.cpp +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Multirotor/MultirotorPawnSimApi.cpp @@ -138,9 +138,9 @@ void MultirotorPawnSimApi::setPose(const Pose& pose, bool ignore_collision) } //*** Start: UpdatableState implementation ***// -void MultirotorPawnSimApi::reset() +void MultirotorPawnSimApi::resetImplementation() { - PawnSimApi::reset(); + PawnSimApi::resetImplementation(); vehicle_api_->reset(); phys_vehicle_->reset(); diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Multirotor/MultirotorPawnSimApi.h b/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Multirotor/MultirotorPawnSimApi.h index c7d891bb37..da727047d2 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Multirotor/MultirotorPawnSimApi.h +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Multirotor/MultirotorPawnSimApi.h @@ -25,7 +25,7 @@ class MultirotorPawnSimApi : public PawnSimApi virtual ~MultirotorPawnSimApi() = default; virtual void updateRenderedState(float dt) override; virtual void updateRendering(float dt) override; - virtual void reset() override; + virtual void resetImplementation() override; virtual void update() override; virtual void reportState(StateReporter& reporter) override; virtual UpdatableObject* getPhysicsBody() override; diff --git a/Unity/AirLibWrapper/AirsimWrapper/cmake/mav-setup.cmake b/Unity/AirLibWrapper/AirsimWrapper/cmake/mav-setup.cmake index 566467317e..7f8cb06c39 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/cmake/mav-setup.cmake +++ b/Unity/AirLibWrapper/AirsimWrapper/cmake/mav-setup.cmake @@ -18,14 +18,16 @@ LIST(APPEND MAVLINK_LIBRARY_SOURCE_FILES "${AIRSIM_ROOT}/MavLinkCom/src/MavLinkN LIST(APPEND MAVLINK_LIBRARY_SOURCE_FILES "${AIRSIM_ROOT}/MavLinkCom/src/MavLinkTcpServer.cpp") LIST(APPEND MAVLINK_LIBRARY_SOURCE_FILES "${AIRSIM_ROOT}/MavLinkCom/src/MavLinkVehicle.cpp") LIST(APPEND MAVLINK_LIBRARY_SOURCE_FILES "${AIRSIM_ROOT}/MavLinkCom/src/MavLinkVideoStream.cpp") -LIST(APPEND MAVLINK_LIBRARY_SOURCE_FILES "${AIRSIM_ROOT}/MavLinkCom/src/Semaphore.cpp") +LIST(APPEND MAVLINK_LIBRARY_SOURCE_FILES "${AIRSIM_ROOT}/MavLinkCom/src/Semaphore.cpp") +LIST(APPEND MAVLINK_LIBRARY_SOURCE_FILES "${AIRSIM_ROOT}/MavLinkCom/src/UdpSocket.cpp") LIST(APPEND MAVLINK_LIBRARY_SOURCE_FILES "${AIRSIM_ROOT}/MavLinkCom/src/impl/AdHocConnectionImpl.cpp") # LIST(APPEND MAVLINK_LIBRARY_SOURCE_FILES "${AIRSIM_ROOT}/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp") LIST(APPEND MAVLINK_LIBRARY_SOURCE_FILES "${AIRSIM_ROOT}/MavLinkCom/src/impl/MavLinkFtpClientImpl.cpp") LIST(APPEND MAVLINK_LIBRARY_SOURCE_FILES "${AIRSIM_ROOT}/MavLinkCom/src/impl/MavLinkNodeImpl.cpp") LIST(APPEND MAVLINK_LIBRARY_SOURCE_FILES "${AIRSIM_ROOT}/MavLinkCom/src/impl/MavLinkTcpServerImpl.cpp") LIST(APPEND MAVLINK_LIBRARY_SOURCE_FILES "${AIRSIM_ROOT}/MavLinkCom/src/impl/MavLinkVehicleImpl.cpp") -LIST(APPEND MAVLINK_LIBRARY_SOURCE_FILES "${AIRSIM_ROOT}/MavLinkCom/src/impl/MavLinkVideoStreamImpl.cpp") +LIST(APPEND MAVLINK_LIBRARY_SOURCE_FILES "${AIRSIM_ROOT}/MavLinkCom/src/impl/MavLinkVideoStreamImpl.cpp") +LIST(APPEND MAVLINK_LIBRARY_SOURCE_FILES "${AIRSIM_ROOT}/MavLinkCom/src/impl/UdpSocketImpl.cpp") LIST(APPEND MAVLINK_LIBRARY_SOURCE_FILES "${AIRSIM_ROOT}/MavLinkCom/src/serial_com/SerialPort.cpp") LIST(APPEND MAVLINK_LIBRARY_SOURCE_FILES "${AIRSIM_ROOT}/MavLinkCom/src/serial_com/TcpClientPort.cpp") LIST(APPEND MAVLINK_LIBRARY_SOURCE_FILES "${AIRSIM_ROOT}/MavLinkCom/src/serial_com/UdpClientPort.cpp") diff --git a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp b/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp index 0543c18faa..a9fc8c6b1e 100644 --- a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp @@ -219,6 +219,7 @@ msr::airlib::RCData PawnSimApi::getRCData() const { joystick_.getJoyStickState(getRemoteControlID(), joystick_state_); + rc_data_.is_initialized = joystick_state_.is_initialized; rc_data_.is_valid = joystick_state_.is_valid; if (rc_data_.is_valid) { @@ -287,10 +288,8 @@ int PawnSimApi::getCameraCount() return cameras_.valsSize(); } -void PawnSimApi::reset() +void PawnSimApi::resetImplementation() { - VehicleSimApiBase::reset(); - state_ = initial_state_; rc_data_ = msr::airlib::RCData(); params_.pawn->SetActorLocationAndRotation(state_.start_location, state_.start_rotation, false, nullptr, ETeleportType::TeleportPhysics); @@ -385,7 +384,7 @@ void PawnSimApi::plot(std::istream& s, FColor color, const Vector3r& offset) Vector3r current_point(x, y, z); current_point += offset; if (!VectorMath::hasNan(last_point)) { - UKismetSystemLibrary::DrawDebugLine(params_.pawn->GetWorld(), ned_transform_.fromLocalNed(last_point), ned_transform_.fromLocalNed(current_point), color, 0, 3.0F); + DrawDebugLine(params_.pawn->GetWorld(), ned_transform_.fromLocalNed(last_point), ned_transform_.fromLocalNed(current_point), color, true, -1.0F, 0, 3.0F); } last_point = current_point; } @@ -456,7 +455,7 @@ void PawnSimApi::setPoseInternal(const Pose& pose, bool ignore_collision) params_.pawn->SetActorLocationAndRotation(position, orientation, true); if (state_.tracing_enabled && (state_.last_position - position).SizeSquared() > 0.25) { - UKismetSystemLibrary::DrawDebugLine(params_.pawn->GetWorld(), state_.last_position, position, FColor::Purple, -1, 3.0f); + DrawDebugLine(params_.pawn->GetWorld(), state_.last_position, position, FColor::Purple, true, -1.0F, 0, 3.0F); state_.last_position = position; } else if (!state_.tracing_enabled) { @@ -470,7 +469,7 @@ void PawnSimApi::setDebugPose(const Pose& debug_pose) if (state_.tracing_enabled && !VectorMath::hasNan(debug_pose.position)) { FVector debug_position = state_.current_debug_position - state_.debug_position_offset; if ((state_.last_debug_position - debug_position).SizeSquared() > 0.25) { - UKismetSystemLibrary::DrawDebugLine(params_.pawn->GetWorld(), state_.last_debug_position, debug_position, FColor(0xaa, 0x33, 0x11), -1, 10.0F); + DrawDebugLine(params_.pawn->GetWorld(), state_.last_debug_position, debug_position, FColor(0xaa, 0x33, 0x11), true, -1.0F, 0, 10.0F); UAirBlueprintLib::LogMessage(FString("Debug Pose: "), debug_position.ToCompactString(), LogDebugLevel::Informational); state_.last_debug_position = debug_position; } diff --git a/Unreal/Plugins/AirSim/Source/PawnSimApi.h b/Unreal/Plugins/AirSim/Source/PawnSimApi.h index ca1bad591a..750d865065 100644 --- a/Unreal/Plugins/AirSim/Source/PawnSimApi.h +++ b/Unreal/Plugins/AirSim/Source/PawnSimApi.h @@ -68,7 +68,7 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase { public: //implementation of VehicleSimApiBase virtual void initialize() override; - virtual void reset() override; + virtual void resetImplementation() override; virtual void update() override; virtual const UnrealImageCapture* getImageCapture() const override; diff --git a/Unreal/Plugins/AirSim/Source/Recording/RecordingThread.cpp b/Unreal/Plugins/AirSim/Source/Recording/RecordingThread.cpp index 7338ccf7c6..6fb24febf5 100644 --- a/Unreal/Plugins/AirSim/Source/Recording/RecordingThread.cpp +++ b/Unreal/Plugins/AirSim/Source/Recording/RecordingThread.cpp @@ -8,7 +8,10 @@ #include "PIPCamera.h" -std::unique_ptr FRecordingThread::instance_; +std::unique_ptr FRecordingThread::running_instance_; +std::unique_ptr FRecordingThread::finishing_instance_; +msr::airlib::WorkerThreadSignal FRecordingThread::finishing_signal_; +bool FRecordingThread::first_ = true; FRecordingThread::FRecordingThread() @@ -18,44 +21,64 @@ FRecordingThread::FRecordingThread() } -void FRecordingThread::startRecording(const msr::airlib::ImageCaptureBase* image_capture, const msr::airlib::Kinematics::State* kinematics, +void FRecordingThread::startRecording(const msr::airlib::ImageCaptureBase* image_capture, const msr::airlib::Kinematics::State* kinematics, const RecordingSetting& settings, msr::airlib::VehicleSimApiBase* vehicle_sim_api) { stopRecording(); //TODO: check FPlatformProcess::SupportsMultithreading()? + assert(!isRecording()); - instance_.reset(new FRecordingThread()); - instance_->image_capture_ = image_capture; - instance_->kinematics_ = kinematics; - instance_->settings_ = settings; - instance_->vehicle_sim_api_ = vehicle_sim_api; + running_instance_.reset(new FRecordingThread()); + running_instance_->image_capture_ = image_capture; + running_instance_->kinematics_ = kinematics; + running_instance_->settings_ = settings; + running_instance_->vehicle_sim_api_ = vehicle_sim_api; - instance_->last_screenshot_on_ = 0; - instance_->last_pose_ = msr::airlib::Pose(); + running_instance_->last_screenshot_on_ = 0; + running_instance_->last_pose_ = msr::airlib::Pose(); - instance_->is_ready_ = true; + running_instance_->is_ready_ = true; - instance_->recording_file_.reset(new RecordingFile()); - instance_->recording_file_->startRecording(vehicle_sim_api); + running_instance_->recording_file_.reset(new RecordingFile()); + running_instance_->recording_file_->startRecording(vehicle_sim_api); } FRecordingThread::~FRecordingThread() { - stopRecording(); + if (this == running_instance_.get()) stopRecording(); +} + +void FRecordingThread::init() +{ + first_ = true; } bool FRecordingThread::isRecording() { - return instance_ != nullptr; + return running_instance_ != nullptr; } void FRecordingThread::stopRecording() { - if (instance_) + if (running_instance_) { - instance_->EnsureCompletion(); - instance_.reset(); + assert(finishing_instance_ == nullptr); + finishing_instance_ = std::move(running_instance_); + assert(!isRecording()); + finishing_instance_->Stop(); + } +} + +void FRecordingThread::killRecording() +{ + if (first_) return; + + stopRecording(); + bool finished = finishing_signal_.waitForRetry(1, 5); + if (!finished) { + UE_LOG(LogTemp, Log, TEXT("killing thread")); + finishing_instance_->thread_->Kill(false); } } @@ -63,6 +86,12 @@ void FRecordingThread::stopRecording() bool FRecordingThread::Init() { + if (first_) { + first_ = false; + } + else { + finishing_signal_.wait(); + } if (image_capture_ && recording_file_) { UAirBlueprintLib::LogMessage(TEXT("Initiated recording thread"), TEXT(""), LogDebugLevel::Success); @@ -85,8 +114,12 @@ uint32 FRecordingThread::Run() //TODO: should we go as fast as possible, or should we limit this to a particular number of // frames per second? - + + //BG: Workaround to get sync ground truth. See https://github.com/Microsoft/AirSim/issues/1494 for details + uint64_t timestamp_millis = static_cast(msr::airlib::ClockFactory::get()->nowNanos() / 1.0E6); + std::string gt = vehicle_sim_api_->getRecordFileLine(false); std::vector responses; + image_capture_->getImages(settings_.requests, responses); recording_file_->appendRecord(responses, vehicle_sim_api_); } @@ -107,13 +140,8 @@ void FRecordingThread::Stop() void FRecordingThread::Exit() { + assert(this == finishing_instance_.get()); if (recording_file_) recording_file_.reset(); -} - -void FRecordingThread::EnsureCompletion() -{ - Stop(); - thread_->WaitForCompletion(); - //UAirBlueprintLib::LogMessage(TEXT("Stopped recording thread"), TEXT(""), LogDebugLevel::Success); + finishing_signal_.signal(); } diff --git a/Unreal/Plugins/AirSim/Source/Recording/RecordingThread.h b/Unreal/Plugins/AirSim/Source/Recording/RecordingThread.h index 57d873e72e..538d23b9ab 100644 --- a/Unreal/Plugins/AirSim/Source/Recording/RecordingThread.h +++ b/Unreal/Plugins/AirSim/Source/Recording/RecordingThread.h @@ -10,6 +10,7 @@ #include #include "common/ClockFactory.hpp" #include "common/AirSimSettings.hpp" +#include "common/WorkerThread.hpp" class FRecordingThread : public FRunnable { @@ -19,9 +20,12 @@ class FRecordingThread : public FRunnable public: FRecordingThread(); virtual ~FRecordingThread(); + + static void init(); static void startRecording(const msr::airlib::ImageCaptureBase* camera, const msr::airlib::Kinematics::State* kinematics, const RecordingSetting& settings, msr::airlib::VehicleSimApiBase* vehicle_sim_api); - static void stopRecording(); + static void stopRecording(); + static void killRecording(); static bool isRecording(); protected: @@ -30,12 +34,14 @@ class FRecordingThread : public FRunnable virtual void Stop() override; virtual void Exit() override; -private: - void EnsureCompletion(); - private: FThreadSafeCounter stop_task_counter_; FRenderCommandFence read_pixel_fence_; + + static std::unique_ptr running_instance_; + static std::unique_ptr finishing_instance_; + static msr::airlib::WorkerThreadSignal finishing_signal_; + static bool first_; static std::unique_ptr instance_; diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index ae53d7272d..c5f7033f91 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -82,6 +82,7 @@ void ASimModeBase::BeginPlay() UAirBlueprintLib::LogMessage(TEXT("Press F1 to see help"), TEXT(""), LogDebugLevel::Informational); setupVehiclesAndCamera(); + FRecordingThread::init(); UWorld* World = GetWorld(); if (World) @@ -126,6 +127,7 @@ void ASimModeBase::setStencilIDs() void ASimModeBase::EndPlay(const EEndPlayReason::Type EndPlayReason) { FRecordingThread::stopRecording(); + FRecordingThread::killRecording(); world_sim_api_.reset(); api_provider_.reset(); api_server_.reset(); @@ -160,7 +162,8 @@ void ASimModeBase::initializeTimeOfDay() UObjectProperty* sun_prop = Cast(p); UObject* sun_obj = sun_prop->GetObjectPropertyValue_InContainer(sky_sphere_); sun_ = Cast(sun_obj); - default_sun_rotation_ = sun_->GetActorRotation(); + if (sun_) + default_sun_rotation_ = sun_->GetActorRotation(); } } diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp index 5fa2861581..d3c96e5df7 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp @@ -33,6 +33,7 @@ void ASimModeWorldBase::startAsyncUpdator() { physics_world_->startAsyncUpdator(); } + void ASimModeWorldBase::stopAsyncUpdator() { physics_world_->stopAsyncUpdator(); @@ -78,12 +79,23 @@ bool ASimModeWorldBase::isPaused() const void ASimModeWorldBase::pause(bool is_paused) { physics_world_->pause(is_paused); + UGameplayStatics::SetGamePaused(this->GetWorld(), is_paused); } void ASimModeWorldBase::continueForTime(double seconds) { - physics_world_->continueForTime(seconds); + if(physics_world_->isPaused()) + { + physics_world_->pause(false); + UGameplayStatics::SetGamePaused(this->GetWorld(), false); + } + physics_world_->continueForTime(seconds); + while(!physics_world_->isPaused()) + { + continue; + } + UGameplayStatics::SetGamePaused(this->GetWorld(), true); } void ASimModeWorldBase::updateDebugReport(msr::airlib::StateReporterWrapper& debug_reporter) diff --git a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.cpp b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.cpp index f224ebce0a..c74441c979 100644 --- a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.cpp +++ b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.cpp @@ -42,9 +42,10 @@ void UnrealLidarSensor::createLasers() // returns a point-cloud for the tick void UnrealLidarSensor::getPointCloud(const msr::airlib::Pose& lidar_pose, const msr::airlib::Pose& vehicle_pose, - const msr::airlib::TTimeDelta delta_time, msr::airlib::vector& point_cloud) + const msr::airlib::TTimeDelta delta_time, msr::airlib::vector& point_cloud, msr::airlib::vector& segmentation_cloud) { point_cloud.clear(); + segmentation_cloud.clear(); msr::airlib::LidarSimpleParams params = getParams(); const auto number_of_lasers = params.number_of_channels; @@ -89,12 +90,14 @@ void UnrealLidarSensor::getPointCloud(const msr::airlib::Pose& lidar_pose, const continue; Vector3r point; + int segmentationID = -1; // shoot laser and get the impact point, if any - if (shootLaser(lidar_pose, vehicle_pose, laser, horizontal_angle, vertical_angle, params, point)) + if (shootLaser(lidar_pose, vehicle_pose, laser, horizontal_angle, vertical_angle, params, point, segmentationID)) { point_cloud.emplace_back(point.x()); point_cloud.emplace_back(point.y()); point_cloud.emplace_back(point.z()); + segmentation_cloud.emplace_back(segmentationID); } } } @@ -107,10 +110,10 @@ void UnrealLidarSensor::getPointCloud(const msr::airlib::Pose& lidar_pose, const // simulate shooting a laser via Unreal ray-tracing. bool UnrealLidarSensor::shootLaser(const msr::airlib::Pose& lidar_pose, const msr::airlib::Pose& vehicle_pose, const uint32 laser, const float horizontal_angle, const float vertical_angle, - const msr::airlib::LidarSimpleParams params, Vector3r &point) + const msr::airlib::LidarSimpleParams params, Vector3r &point, int &segmentationID) { // start position - Vector3r start = lidar_pose.position + vehicle_pose.position; + Vector3r start = VectorMath::add(lidar_pose, vehicle_pose).position; // We need to compose rotations here rather than rotate a vector by a quaternion // Hence using coordOrientationAdd(..) rather than rotateQuaternion(..) @@ -135,6 +138,18 @@ bool UnrealLidarSensor::shootLaser(const msr::airlib::Pose& lidar_pose, const ms if (is_hit) { + //Store the segmentation id of the hit object. + auto hitActor = hit_result.GetActor(); + if (hitActor != nullptr) + { + TArray meshComponents; + hitActor->GetComponents(meshComponents); + for (int i = 0; i < meshComponents.Num(); i++) + { + segmentationID = segmentationID == -1 ? meshComponents[i]->CustomDepthStencilValue : segmentationID; + } + } + if (false && UAirBlueprintLib::IsInGameThread()) { // Debug code for very specific cases. diff --git a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.h b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.h index 2f2a46e2bd..fbbee6e521 100644 --- a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.h +++ b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.h @@ -21,7 +21,7 @@ class UnrealLidarSensor : public msr::airlib::LidarSimple { protected: virtual void getPointCloud(const msr::airlib::Pose& lidar_pose, const msr::airlib::Pose& vehicle_pose, - msr::airlib::TTimeDelta delta_time, msr::airlib::vector& point_cloud) override; + msr::airlib::TTimeDelta delta_time, msr::airlib::vector& point_cloud, msr::airlib::vector& segmentation_cloud) override; private: using Vector3r = msr::airlib::Vector3r; @@ -30,7 +30,7 @@ class UnrealLidarSensor : public msr::airlib::LidarSimple { void createLasers(); bool shootLaser(const msr::airlib::Pose& lidar_pose, const msr::airlib::Pose& vehicle_pose, const uint32 channel, const float horizontal_angle, const float vertical_angle, - const msr::airlib::LidarSimpleParams params, Vector3r &point); + const msr::airlib::LidarSimpleParams params, Vector3r &point, int &segmentationID); private: AActor* actor_; diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnApi.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnApi.cpp index b7b5688948..269e6b8898 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnApi.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnApi.cpp @@ -54,9 +54,9 @@ msr::airlib::CarApiBase::CarState CarPawnApi::getCarState() const return state; } -void CarPawnApi::reset() +void CarPawnApi::resetImplementation() { - msr::airlib::CarApiBase::reset(); + msr::airlib::CarApiBase::resetImplementation(); last_controls_ = CarControls(); auto phys_comps = UAirBlueprintLib::getPhysicsComponents(pawn_); diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnApi.h b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnApi.h index 3d12c7467d..b5db614c6f 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnApi.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnApi.h @@ -18,7 +18,6 @@ class CarPawnApi : public msr::airlib::CarApiBase { virtual CarApiBase::CarState getCarState() const override; - virtual void reset() override; virtual void update() override; virtual msr::airlib::GeoPoint getHomeGeoPoint() const override; @@ -31,6 +30,9 @@ class CarPawnApi : public msr::airlib::CarApiBase { virtual ~CarPawnApi(); +protected: + virtual void resetImplementation() override; + private: UWheeledVehicleMovementComponent* movement_; bool api_control_enabled_ = false; diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.cpp index 03b771b4b1..7e55d99d03 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.cpp @@ -159,9 +159,9 @@ void CarPawnSimApi::updateCarControls() } //*** Start: UpdatableState implementation ***// -void CarPawnSimApi::reset() +void CarPawnSimApi::resetImplementation() { - PawnSimApi::reset(); + PawnSimApi::resetImplementation(); vehicle_api_->reset(); } diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.h b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.h index eb1c694ac8..94be97408d 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.h @@ -29,7 +29,7 @@ class CarPawnSimApi : public PawnSimApi CarPawnSimApi(const Params& params, const CarPawnApi::CarControls& keyboard_controls, UWheeledVehicleMovementComponent* movement); - virtual void reset() override; + virtual void resetImplementation() override; virtual void update() override; virtual std::string getRecordFileLine(bool is_header_line) const override; diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Car/SimModeCar.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Car/SimModeCar.cpp index 9f5f70a001..cbaf3a64b6 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Car/SimModeCar.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Car/SimModeCar.cpp @@ -77,7 +77,7 @@ std::unique_ptr ASimModeCar::createApiServer() const return ASimModeBase::createApiServer(); #else return std::unique_ptr(new msr::airlib::CarRpcLibServer( - getApiProvider(), getSettings().api_server_address)); + getApiProvider(), getSettings().api_server_address, getSettings().api_port)); #endif } diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/ComputerVision/SimModeComputerVision.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/ComputerVision/SimModeComputerVision.cpp index a85a3eb3f4..b87d5170cc 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/ComputerVision/SimModeComputerVision.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/ComputerVision/SimModeComputerVision.cpp @@ -23,7 +23,7 @@ std::unique_ptr ASimModeComputerVision::createApiSer return ASimModeBase::createApiServer(); #else return std::unique_ptr(new msr::airlib::RpcLibServerBase( - getApiProvider(), getSettings().api_server_address)); + getApiProvider(), getSettings().api_server_address, getSettings().api_port)); #endif } @@ -75,4 +75,14 @@ msr::airlib::VehicleApiBase* ASimModeComputerVision::getVehicleApi(const PawnSim { //we don't have real vehicle so no vehicle API return nullptr; +} + +bool ASimModeComputerVision::isPaused() const +{ + return UGameplayStatics::IsGamePaused(this->GetWorld()); +} + +void ASimModeComputerVision::pause(bool is_paused) +{ + UGameplayStatics::SetGamePaused(this->GetWorld(), is_paused); } \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/ComputerVision/SimModeComputerVision.h b/Unreal/Plugins/AirSim/Source/Vehicles/ComputerVision/SimModeComputerVision.h index 4e2a08f74e..8fc0638055 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/ComputerVision/SimModeComputerVision.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/ComputerVision/SimModeComputerVision.h @@ -30,4 +30,6 @@ class AIRSIM_API ASimModeComputerVision : public ASimModeBase const PawnSimApi::Params& pawn_sim_api_params) const override; virtual msr::airlib::VehicleApiBase* getVehicleApi(const PawnSimApi::Params& pawn_sim_api_params, const PawnSimApi* sim_api) const override; + virtual bool isPaused() const override; + virtual void pause(bool is_paused) override; }; diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.cpp index 01adc7824d..432abc39a6 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.cpp @@ -137,9 +137,9 @@ void MultirotorPawnSimApi::setPose(const Pose& pose, bool ignore_collision) } //*** Start: UpdatableState implementation ***// -void MultirotorPawnSimApi::reset() +void MultirotorPawnSimApi::resetImplementation() { - PawnSimApi::reset(); + PawnSimApi::resetImplementation(); vehicle_api_->reset(); phys_vehicle_->reset(); diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.h b/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.h index 8bffa4d416..6254c5d6c0 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.h @@ -38,7 +38,7 @@ class MultirotorPawnSimApi : public PawnSimApi //PhysicsBody interface //this just wrapped around MultiRotor physics body - virtual void reset() override; + virtual void resetImplementation() override; virtual void update() override; virtual void reportState(StateReporter& reporter) override; virtual UpdatableObject* getPhysicsBody() override; diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/SimModeWorldMultiRotor.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/SimModeWorldMultiRotor.cpp index 27d96d8a3e..8af796f100 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/SimModeWorldMultiRotor.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/SimModeWorldMultiRotor.cpp @@ -83,7 +83,7 @@ std::unique_ptr ASimModeWorldMultiRotor::createApiSe return ASimModeBase::createApiServer(); #else return std::unique_ptr(new msr::airlib::MultirotorRpcLibServer( - getApiProvider(), getSettings().api_server_address)); + getApiProvider(), getSettings().api_server_address, getSettings().api_port)); #endif } @@ -95,8 +95,9 @@ void ASimModeWorldMultiRotor::getExistingVehiclePawns(TArray& pawns) co bool ASimModeWorldMultiRotor::isVehicleTypeSupported(const std::string& vehicle_type) const { return ((vehicle_type == AirSimSettings::kVehicleTypeSimpleFlight) || - (vehicle_type == AirSimSettings::kVehicleTypePX4) || - (vehicle_type == AirSimSettings::kVehicleTypeArduCopterSolo)); + (vehicle_type == AirSimSettings::kVehicleTypePX4) || + (vehicle_type == AirSimSettings::kVehicleTypeArduCopterSolo) || + (vehicle_type == AirSimSettings::kVehicleTypeArduCopter)); } std::string ASimModeWorldMultiRotor::getVehiclePawnPathName(const AirSimSettings::VehicleSetting& vehicle_setting) const diff --git a/UnrealPluginFiles.vcxproj b/UnrealPluginFiles.vcxproj index 0ea68469b1..9d812e2e29 100644 --- a/UnrealPluginFiles.vcxproj +++ b/UnrealPluginFiles.vcxproj @@ -95,6 +95,7 @@ + @@ -128,6 +129,7 @@ + diff --git a/UnrealPluginFiles.vcxproj.filters b/UnrealPluginFiles.vcxproj.filters index a6e87ac649..797cf97178 100644 --- a/UnrealPluginFiles.vcxproj.filters +++ b/UnrealPluginFiles.vcxproj.filters @@ -105,6 +105,9 @@ Source Files + + Source Files + @@ -203,6 +206,9 @@ Source Files + + Source Files + diff --git a/build.sh b/build.sh index 8ddf11c876..94aa1b3e6e 100755 --- a/build.sh +++ b/build.sh @@ -5,26 +5,24 @@ SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" pushd "$SCRIPT_DIR" >/dev/null set -e -# set -x - -#check for correct verion of llvm -if [[ ! -d "llvm-source-50" ]]; then - if [[ -d "llvm-source-39" ]]; then - echo "Hello there! We just upgraded AirSim to Unreal Engine 4.18." - echo "Here are few easy steps for upgrade so everything is new and shiny :)" - echo "https://github.com/Microsoft/AirSim/blob/master/docs/unreal_upgrade.md" - exit 1 - else - echo "The llvm-souce-50 folder was not found! Mystery indeed." - fi -fi +set -x -# check for libc++ -if [[ !(-d "./llvm-build/output/lib") ]]; then - echo "ERROR: clang++ and libc++ is necessary to compile AirSim and run it in Unreal engine" - echo "Please run setup.sh first." - exit 1 -fi +MIN_GCC_VERSION=6.0.0 +gccBuild=false +function version_less_than_equal_to() { test "$(printf '%s\n' "$@" | sort -V | head -n 1)" = "$1"; } + +# Parse command line arguments +while [[ $# -gt 0 ]] +do +key="$1" + +case $key in + --gcc) + gccBuild=true + shift # past argument + ;; +esac +done # check for rpclib if [ ! -d "./external/rpclib/rpclib-2.2.1" ]; then @@ -33,42 +31,73 @@ if [ ! -d "./external/rpclib/rpclib-2.2.1" ]; then exit 1 fi -# check for cmake build -if [ ! -d "./cmake_build" ]; then - echo "ERROR: cmake build was not found." - echo "please run setup.sh first and then run build.sh again." - exit 1 +# check for local cmake build created by setup.sh +if [ -d "./cmake_build" ]; then + if [ "$(uname)" == "Darwin" ]; then + CMAKE="$(greadlink -f cmake_build/bin/cmake)" + else + CMAKE="$(readlink -f cmake_build/bin/cmake)" + fi +else + CMAKE=$(which cmake) fi - # set up paths of cc and cxx compiler -if [ "$1" == "gcc" ]; then - export CC="gcc" - export CXX="g++" +if $gccBuild; then + # variable for build output + build_dir=build_gcc_debug + # gcc tools + gcc_ver=$(gcc -dumpfullversion) + gcc_path=$(which cmake) + if [[ "$gcc_path" == "" ]] ; then + echo "ERROR: run setup.sh to install a good version of gcc." + exit 1 + fi + if version_less_than_equal_to $gcc_ver $MIN_GCC_VERSION; then + export CC="gcc-6" + export CXX="g++-6" + else + export CC="gcc" + export CXX="g++" + fi else - if [ "$(uname)" == "Darwin" ]; then - CMAKE="$(greadlink -f cmake_build/bin/cmake)" + #check for correct verion of llvm + if [[ ! -d "llvm-source-50" ]]; then + if [[ -d "llvm-source-39" ]]; then + echo "Hello there! We just upgraded AirSim to Unreal Engine 4.18." + echo "Here are few easy steps for upgrade so everything is new and shiny :)" + echo "https://github.com/Microsoft/AirSim/blob/master/docs/unreal_upgrade.md" + exit 1 + else + echo "The llvm-souce-50 folder was not found! Mystery indeed." + fi + fi + # check for libc++ + if [[ !(-d "./llvm-build/output/lib") ]]; then + echo "ERROR: clang++ and libc++ is necessary to compile AirSim and run it in Unreal engine" + echo "Please run setup.sh first." + exit 1 + fi + + # variable for build output + build_dir=build_debug + if [ "$(uname)" == "Darwin" ]; then export CC=/usr/local/opt/llvm-5.0/bin/clang-5.0 export CXX=/usr/local/opt/llvm-5.0/bin/clang++-5.0 else - CMAKE="$(readlink -f cmake_build/bin/cmake)" - export CC="clang-5.0" export CXX="clang++-5.0" fi fi #install EIGEN library -if [[ !(-d "./AirLib/deps/eigen3/Eigen") ]]; then - echo "eigen is not installed. Please run setup.sh first." +if [[ !(-d "./AirLib/deps/eigen3/Eigen") ]]; then + echo "### Eigen is not installed. Please run setup.sh first." exit 1 fi - -# variable for build output -build_dir=build_debug -echo "putting build in build_debug folder, to clean, just delete the directory..." +echo "putting build in $build_dir folder, to clean, just delete the directory..." # this ensures the cmake files will be built in our $build_dir instead. if [[ -f "./cmake/CMakeCache.txt" ]]; then diff --git a/cmake/MavLinkCom/CMakeLists.txt b/cmake/MavLinkCom/CMakeLists.txt index cea0c0d250..7743eb8b9c 100644 --- a/cmake/MavLinkCom/CMakeLists.txt +++ b/cmake/MavLinkCom/CMakeLists.txt @@ -27,6 +27,7 @@ LIST(APPEND MAVLINK_SOURCES "${AIRSIM_ROOT}/MavLinkCom/src/MavLinkTcpServer.cpp" LIST(APPEND MAVLINK_SOURCES "${AIRSIM_ROOT}/MavLinkCom/src/MavLinkVehicle.cpp") LIST(APPEND MAVLINK_SOURCES "${AIRSIM_ROOT}/MavLinkCom/src/MavLinkVideoStream.cpp") LIST(APPEND MAVLINK_SOURCES "${AIRSIM_ROOT}/MavLinkCom/src/Semaphore.cpp") +LIST(APPEND MAVLINK_SOURCES "${AIRSIM_ROOT}/MavLinkCom/src/UdpSocket.cpp") LIST(APPEND MAVLINK_SOURCES "${AIRSIM_ROOT}/MavLinkCom/src/impl/AdHocConnectionImpl.cpp") LIST(APPEND MAVLINK_SOURCES "${AIRSIM_ROOT}/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp") LIST(APPEND MAVLINK_SOURCES "${AIRSIM_ROOT}/MavLinkCom/src/impl/MavLinkFtpClientImpl.cpp") @@ -34,6 +35,7 @@ LIST(APPEND MAVLINK_SOURCES "${AIRSIM_ROOT}/MavLinkCom/src/impl/MavLinkNodeImpl. LIST(APPEND MAVLINK_SOURCES "${AIRSIM_ROOT}/MavLinkCom/src/impl/MavLinkTcpServerImpl.cpp") LIST(APPEND MAVLINK_SOURCES "${AIRSIM_ROOT}/MavLinkCom/src/impl/MavLinkVehicleImpl.cpp") LIST(APPEND MAVLINK_SOURCES "${AIRSIM_ROOT}/MavLinkCom/src/impl/MavLinkVideoStreamImpl.cpp") +LIST(APPEND MAVLINK_SOURCES "${AIRSIM_ROOT}/MavLinkCom/src/impl/UdpSocketImpl.cpp") LIST(APPEND MAVLINK_SOURCES "${AIRSIM_ROOT}/MavLinkCom/src/serial_com/SerialPort.cpp") LIST(APPEND MAVLINK_SOURCES "${AIRSIM_ROOT}/MavLinkCom/src/serial_com/TcpClientPort.cpp") LIST(APPEND MAVLINK_SOURCES "${AIRSIM_ROOT}/MavLinkCom/src/serial_com/UdpClientPort.cpp") diff --git a/cmake/build.sh b/cmake/build.sh deleted file mode 100755 index 42a26f219d..0000000000 --- a/cmake/build.sh +++ /dev/null @@ -1,54 +0,0 @@ -# This script is for building AirSim with GCC-6 -# You should NOT use this with Unreal. -# If you want bits that work in Unreal you need to use ../build.sh -# For pre-requisite tools needed to run this script see -# https://github.com/Microsoft/AirSim/blob/master/docs/build_linux.md - -# First get the location of this script, it is assumed you already cloned "main" and you are running the -# script from that repo. It will then put everything else in a ROS workspace at "../catkin_ws" -SCRIPT=$(readlink -f "$0") -SCRIPTPATH=$(dirname "$SCRIPT") -pushd $SCRIPTPATH - -if [[ ! -d "$EIGEN_ROOT" ]]; then - if [[ ! -d eigen ]]; then - echo "downloading eigen..." - wget http://bitbucket.org/eigen/eigen/get/3.3.2.zip - unzip 3.3.2.zip -d eigen - pushd eigen - mv eigen* eigen3 - echo "3.3.2" > version - popd &>/dev/null - rm 3.3.2.zip - fi - export EIGEN_ROOT="$(pwd)/eigen" -fi -popd - -if [ -f "CMakeCache.txt" ]; then -rm CMakeCache.txt -fi - -if [ -d "CMakeFiles" ]; then -rm -rf CMakeFiles -fi - -GCCARGS="-D CMAKE_BUILD_TYPE=Debug" - -GCCVERSION=$(gcc -v 2>&1 | sed -n "/^gcc version/p" | sed -e "s/^gcc version \([0-9]\)/\1/g" | cut -d"." -f1) -if [ $GCCVERSION -lt 5 ]; then - GCCARGS="$GCCARGS -D CMAKE_C_COMPILER=gcc-6 -D CMAKE_CXX_COMPILER=g++-6" -fi -cmake $GCCARGS CMakeLists.txt - -make - -pushd .. -mkdir -p AirLib/lib/x64/Debug -mkdir -p AirLib/deps/rpclib -mkdir -p AirLib/deps/MavLinkCom -rsync -a --delete cmake/output/lib/ AirLib/lib/x64/Debug -rsync -a --delete external/rpclib/include AirLib/deps/rpclib -rsync -a --delete MavLinkCom/include AirLib/deps/MavLinkCom -rsync -a --delete AirLib Unreal/Plugins/AirSim/Source -popd diff --git a/cmake/clean_getlibcxx.sh b/cmake/clean_getlibcxx.sh deleted file mode 100644 index 436c709392..0000000000 --- a/cmake/clean_getlibcxx.sh +++ /dev/null @@ -1,12 +0,0 @@ -#! /bin/bash - -SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" -pushd "$SCRIPT_DIR" - -# add llvm-source to root of AirSim -cd .. - -rm -rf llvm-source -rm -rf llvm-build - -popd \ No newline at end of file diff --git a/cmake/gcc_build.sh b/cmake/gcc_build.sh deleted file mode 100644 index 1cb6b1c933..0000000000 --- a/cmake/gcc_build.sh +++ /dev/null @@ -1,55 +0,0 @@ -# This script is for building AirSim with GCC-6 -# You should NOT use this with Unreal. -# If you want bits that work in Unreal you need to use ../build.sh -# For pre-requisite tools needed to run this script see -# https://github.com/Microsoft/AirSim/blob/master/docs/build_linux.md - -# First get the location of this script, it is assumed you already cloned "main" and you are running the -# script from that repo. It will then put everything else in a ROS workspace at "../catkin_ws" -SCRIPT=$(readlink -f "$0") -SCRIPTPATH=$(dirname "$SCRIPT") -pushd $SCRIPTPATH - -# update the rpclib git submodule -pushd .. -git submodule update --init --recursive - -#install EIGEN library -if [[ ! -d './AirLib/deps/eigen3/Eigen' ]]; then - echo "downloading eigen..." - wget http://bitbucket.org/eigen/eigen/get/3.3.2.zip - unzip 3.3.2.zip -d temp_eigen - mkdir -p AirLib/deps/eigen3 - mv temp_eigen/eigen*/Eigen AirLib/deps/eigen3 - rm -rf temp_eigen - rm 3.3.2.zip -fi -popd - -if [ -f "CMakeCache.txt" ]; then -rm CMakeCache.txt -fi - -if [ -d "CMakeFiles" ]; then -rm -rf CMakeFiles -fi - -GCCARGS="-D CMAKE_BUILD_TYPE=Debug" - -GCCVERSION=$(gcc -v 2>&1 | sed -n "/^gcc version/p" | sed -e "s/^gcc version \([0-9]\)/\1/g" | cut -d"." -f1) -if [ $GCCVERSION -lt 5 ]; then - GCCARGS="$GCCARGS -D CMAKE_C_COMPILER=gcc-6 -D CMAKE_CXX_COMPILER=g++-6" -fi -cmake $GCCARGS CMakeLists.txt - -make - -pushd .. -mkdir -p AirLib/lib/x64/Debug -mkdir -p AirLib/deps/rpclib -mkdir -p AirLib/deps/MavLinkCom -rsync -a --delete cmake/output/lib/ AirLib/lib/x64/Debug -rsync -a --delete external/rpclib/include AirLib/deps/rpclib -rsync -a --delete MavLinkCom/include AirLib/deps/MavLinkCom -rsync -a --delete AirLib Unreal/Plugins/AirSim/Source -popd diff --git a/cmake/getlibcxx.sh b/cmake/getlibcxx.sh deleted file mode 100755 index dfd58ebe55..0000000000 --- a/cmake/getlibcxx.sh +++ /dev/null @@ -1,38 +0,0 @@ -#! /bin/bash - -set +x -set -e - -SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" -pushd "$SCRIPT_DIR" - -# add llvm-source to root of AirSim -cd .. - -# Checkout LLVM sources -if [[ ! -d "llvm-source" ]]; then - git clone --depth=1 -b release_39 https://github.com/llvm-mirror/llvm.git llvm-source - git clone --depth=1 -b release_39 https://github.com/llvm-mirror/libcxx.git llvm-source/projects/libcxx - git clone --depth=1 -b release_39 https://github.com/llvm-mirror/libcxxabi.git llvm-source/projects/libcxxabi -else - echo "folder llvm-source already exists, skipping git clone..." -fi - -# Build and install libc++ -rm -rf llvm-build -mkdir llvm-build && cd llvm-build - -export C_COMPILER=clang -export COMPILER=clang++ - -# Build and install libc++ -cmake -DCMAKE_C_COMPILER=${C_COMPILER} -DCMAKE_CXX_COMPILER=${COMPILER} \ - -DLIBCXX_INSTALL_EXPERIMENTAL_LIBRARY=ON \ - -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_INSTALL_PREFIX=./output \ - ../llvm-source - -make cxx - -sudo make install-libcxx install-libcxxabi - -popd \ No newline at end of file diff --git a/cmake/getlibcxx_clean.sh b/cmake/getlibcxx_clean.sh deleted file mode 100755 index 436c709392..0000000000 --- a/cmake/getlibcxx_clean.sh +++ /dev/null @@ -1,12 +0,0 @@ -#! /bin/bash - -SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" -pushd "$SCRIPT_DIR" - -# add llvm-source to root of AirSim -cd .. - -rm -rf llvm-source -rm -rf llvm-build - -popd \ No newline at end of file diff --git a/docs/build_linux.md b/docs/build_linux.md index 9b6cde3d66..17feaeb988 100644 --- a/docs/build_linux.md +++ b/docs/build_linux.md @@ -27,10 +27,19 @@ Please see instructions [here](https://github.com/Microsoft/AirSim/blob/master/d # go to the folder where you clone GitHub projects git clone https://github.com/Microsoft/AirSim.git cd AirSim + + By default AirSim recommends using clang 5 to build the binaries as those will be compatible with Unreal. The setup script + will install the right version of cmake, llvm, and eigen: ./setup.sh ./build.sh ``` + Optionally, if you need GCC binaries for some other reason, you can simply add gcc to the setup and build invocation, like this: + ```bash + ./setup.sh --gcc + ./build.sh --gcc + ``` + ### Build Unreal Environment Finally, you will need an Unreal project that hosts the environment for your vehicles. AirSim comes with a built-in "Blocks Environment" which you can use, or you can create your own. Please see [setting up Unreal Environment](unreal_proj.md). diff --git a/docs/image_apis.md b/docs/image_apis.md index b4d87a8ee5..96c9cd133f 100644 --- a/docs/image_apis.md +++ b/docs/image_apis.md @@ -64,7 +64,7 @@ responses = client.simGetImages([ If you plan to use numpy for image manipulation, you should get uncompressed RGB image and then convert to numpy like this: ```python -responses = client.simGetImages([ImageRequest("0", airsim.ImageType.Scene, False, False)]) +responses = client.simGetImages([airsim.ImageRequest("0", airsim.ImageType.Scene, False, False)]) response = responses[0] # get numpy array diff --git a/docs/settings.md b/docs/settings.md index dd8922f918..aa6f0f18cb 100644 --- a/docs/settings.md +++ b/docs/settings.md @@ -23,7 +23,7 @@ Below are complete list of settings available along with their default values. I **WARNING:** Do not copy paste all of below in your settings.json. We strongly recommend adding only those settings that you don't want default values. Only required element is `"SettingsVersion"`. -``` +```json { "SimMode": "", "ClockType": "", @@ -36,7 +36,7 @@ Below are complete list of settings available along with their default values. I "EngineSound": true, "PhysicsEngineName": "", "SpeedUnitFactor": 1.0, - "SpeedUnitLabel": "m/s", + "SpeedUnitLabel": "m/s", "Recording": { "RecordOnMove": false, "RecordInterval": 0.05, diff --git a/ros/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h b/ros/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h index 804ae13f2a..02021355e0 100644 --- a/ros/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h +++ b/ros/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h @@ -254,6 +254,8 @@ class AirsimROSWrapper bool is_vulkan_; // rosparam obtained from launch file. If vulkan is being used, we BGR encoding instead of RGB msr::airlib::MultirotorRpcLibClient airsim_client_; + msr::airlib::MultirotorRpcLibClient airsim_client_images_; + msr::airlib::MultirotorRpcLibClient airsim_client_lidar_; ros::NodeHandle nh_; ros::NodeHandle nh_private_; diff --git a/ros/src/airsim_ros_pkgs/launch/rviz.launch b/ros/src/airsim_ros_pkgs/launch/rviz.launch new file mode 100644 index 0000000000..eed8359794 --- /dev/null +++ b/ros/src/airsim_ros_pkgs/launch/rviz.launch @@ -0,0 +1,3 @@ + + + \ No newline at end of file diff --git a/ros/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp b/ros/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp index 88952836da..00eee7c6cf 100644 --- a/ros/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp +++ b/ros/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp @@ -49,6 +49,9 @@ void AirsimROSWrapper::initialize_airsim() try { airsim_client_.confirmConnection(); + airsim_client_images_.confirmConnection(); + airsim_client_lidar_.confirmConnection(); + for (const auto& vehicle_name : vehicle_names_) { airsim_client_.enableApiControl(true, vehicle_name); // todo expose as rosservice? @@ -946,7 +949,7 @@ void AirsimROSWrapper::img_response_timer_cb(const ros::TimerEvent& event) for (const auto& airsim_img_request_vehicle_name_pair : airsim_img_request_vehicle_name_pair_vec_) { std::unique_lock lck(drone_control_mutex_); - const std::vector& img_response = airsim_client_.simGetImages(airsim_img_request_vehicle_name_pair.first, airsim_img_request_vehicle_name_pair.second); + const std::vector& img_response = airsim_client_images_.simGetImages(airsim_img_request_vehicle_name_pair.first, airsim_img_request_vehicle_name_pair.second); lck.unlock(); if (img_response.size() == airsim_img_request_vehicle_name_pair.first.size()) @@ -977,7 +980,7 @@ void AirsimROSWrapper::lidar_timer_cb(const ros::TimerEvent& event) for (const auto& vehicle_lidar_pair: vehicle_lidar_map_) { std::unique_lock lck(drone_control_mutex_); - auto lidar_data = airsim_client_.getLidarData(vehicle_lidar_pair.second, vehicle_lidar_pair.first); // airsim api is imu_name, vehicle_name + auto lidar_data = airsim_client_lidar_.getLidarData(vehicle_lidar_pair.second, vehicle_lidar_pair.first); // airsim api is imu_name, vehicle_name lck.unlock(); sensor_msgs::PointCloud2 lidar_msg = get_lidar_msg_from_airsim(lidar_data); // todo make const ptr msg to avoid copy lidar_msg.header.frame_id = vehicle_lidar_pair.second; // sensor frame name. todo add to doc diff --git a/setup.sh b/setup.sh index 5ffa9750df..d74b498c1b 100755 --- a/setup.sh +++ b/setup.sh @@ -8,15 +8,78 @@ if [[ -d "llvm-source-39" ]]; then fi set -x -set -e +# set -e SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" pushd "$SCRIPT_DIR" >/dev/null -#Parse command line arguments downloadHighPolySuv=true -if [[ $1 == "--no-full-poly-car" ]]; then +gccBuild=false +MIN_CMAKE_VERSION=3.10.0 +MIN_GCC_VERSION=6.0.0 +function version_less_than_equal_to() { test "$(printf '%s\n' "$@" | sort -V | head -n 1)" = "$1"; } + +# Parse command line arguments +while [[ $# -gt 0 ]] +do +key="$1" + +case $key in + --no-full-poly-car) downloadHighPolySuv=false + shift # past value + ;; + --gcc) + gccBuild=true + shift # past argument + ;; +esac +done + +if $gccBuild; then + # gcc tools + gcc_ver=$(gcc -dumpfullversion) + gcc_path=$(which cmake) + if [[ "$gcc_path" == "" ]] ; then + gcc_ver=0 + fi + if version_less_than_equal_to $gcc_ver $MIN_GCC_VERSION; then + if [ "$(uname)" == "Darwin" ]; then # osx + brew update + brew install gcc-6 g++-6 + else + sudo add-apt-repository ppa:ubuntu-toolchain-r/test + sudo apt-get -y update + sudo apt-get install -y gcc-6 g++-6 + fi + else + echo "Already have good version of gcc: $gcc_ver" + fi +else + # llvm tools + if [ "$(uname)" == "Darwin" ]; then # osx + brew update + + # brew install llvm@3.9 + brew tap llvm-hs/homebrew-llvm + brew install llvm-5.0 + export C_COMPILER=/usr/local/opt/llvm-5.0/bin/clang-5.0 + export COMPILER=/usr/local/opt/llvm-5.0/bin/clang++-5.0 + + else #linux + #install clang and build tools + + VERSION=$(lsb_release -rs | cut -d. -f1) + # Since Ubuntu 17 clang-5.0 is part of the core repository + # See https://packages.ubuntu.com/search?keywords=clang-5.0 + if [ "$VERSION" -lt "17" ]; then + wget -O - http://apt.llvm.org/llvm-snapshot.gpg.key|sudo apt-key add - + sudo apt-get update + fi + sudo apt-get install -y clang-5.0 clang++-5.0 + export C_COMPILER=clang-5.0 + export COMPILER=clang++-5.0 + fi fi #give user perms to access USB port - this is not needed if not using PX4 HIL @@ -26,57 +89,50 @@ if [ "$(uname)" == "Darwin" ]; then # osx sudo dseditgroup -o edit -a `whoami` -t user dialout fi - #below takes way too long - # brew install llvm@3.9 - brew tap llvm-hs/homebrew-llvm - brew install llvm-5.0 - brew install wget brew install coreutils + brew install cmake # should get cmake 3.8 - export C_COMPILER=/usr/local/opt/llvm-5.0/bin/clang-5.0 - export COMPILER=/usr/local/opt/llvm-5.0/bin/clang++-5.0 else #linux if [[ ! -z "${whoami}" ]]; then #this happens when running in travis sudo /usr/sbin/useradd -G dialout $USER sudo usermod -a -G dialout $USER fi - #install clang and build tools + #install additional tools sudo apt-get install -y build-essential - VERSION=$(lsb_release -rs | cut -d. -f1) - # Since Ubuntu 17 clang-5.0 is part of the core repository - # See https://packages.ubuntu.com/search?keywords=clang-5.0 - if [ "$VERSION" -lt "17" ]; then - wget -O - http://apt.llvm.org/llvm-snapshot.gpg.key|sudo apt-key add - - sudo apt-get update - fi - sudo apt-get install -y clang-5.0 clang++-5.0 sudo apt-get install -y unzip - export C_COMPILER=clang-5.0 - export COMPILER=clang++-5.0 -fi - -#download cmake - we need v3.9+ which is not out of box in Ubuntu 16.04 -if [[ ! -d "cmake_build/bin" ]]; then - echo "Downloading cmake..." - wget https://cmake.org/files/v3.10/cmake-3.10.2.tar.gz \ - -O cmake.tar.gz - tar -xzf cmake.tar.gz - rm cmake.tar.gz - rm -rf ./cmake_build - mv ./cmake-3.10.2 ./cmake_build - pushd cmake_build - ./bootstrap - make - popd -fi + cmake_ver=$(cmake --version 2>&1 | head -n1 | cut -d ' ' -f3 | awk '{print $NF}') + cmake_path=$(which cmake) + if [[ "$cmake_path" == "" ]] ; then + cmake_ver=0 + fi -if [ "$(uname)" == "Darwin" ]; then - CMAKE="$(greadlink -f cmake_build/bin/cmake)" -else - CMAKE="$(readlink -f cmake_build/bin/cmake)" + #download cmake - v3.10.2 is not out of box in Ubuntu 16.04 + if version_less_than_equal_to $gcc_ver $MIN_GCC_VERSION; then + if [[ ! -d "cmake_build/bin" ]]; then + echo "Downloading cmake..." + wget https://cmake.org/files/v3.10/cmake-3.10.2.tar.gz \ + -O cmake.tar.gz + tar -xzf cmake.tar.gz + rm cmake.tar.gz + rm -rf ./cmake_build + mv ./cmake-3.10.2 ./cmake_build + pushd cmake_build + ./bootstrap + make + popd + fi + if [ "$(uname)" == "Darwin" ]; then + CMAKE="$(greadlink -f cmake_build/bin/cmake)" + else + CMAKE="$(readlink -f cmake_build/bin/cmake)" + fi + else + echo "Already have good version of cmake: $cmake_ver" + CMAKE=$(which cmake) + fi fi # Download rpclib @@ -96,34 +152,34 @@ if [ ! -d "external/rpclib/rpclib-2.2.1" ]; then fi # Download high-polycount SUV model -if [ ! -d "Unreal/Plugins/AirSim/Content/VehicleAdv" ]; then - mkdir -p "Unreal/Plugins/AirSim/Content/VehicleAdv" -fi -if [ ! -d "Unreal/Plugins/AirSim/Content/VehicleAdv/SUV/v1.2.0" ]; then - if $downloadHighPolySuv; then - echo "*********************************************************************************************" - echo "Downloading high-poly car assets.... The download is ~37MB and can take some time." - echo "To install without this assets, re-run setup.sh with the argument --no-full-poly-car" - echo "*********************************************************************************************" - - if [ -d "suv_download_tmp" ]; then +if $downloadHighPolySuv; then + if [ ! -d "Unreal/Plugins/AirSim/Content/VehicleAdv" ]; then + mkdir -p "Unreal/Plugins/AirSim/Content/VehicleAdv" + fi + if [ ! -d "Unreal/Plugins/AirSim/Content/VehicleAdv/SUV/v1.2.0" ]; then + echo "*********************************************************************************************" + echo "Downloading high-poly car assets.... The download is ~37MB and can take some time." + echo "To install without this assets, re-run setup.sh with the argument --no-full-poly-car" + echo "*********************************************************************************************" + + if [ -d "suv_download_tmp" ]; then + rm -rf "suv_download_tmp" + fi + mkdir -p "suv_download_tmp" + cd suv_download_tmp + wget https://github.com/Microsoft/AirSim/releases/download/v1.2.0/car_assets.zip + if [ -d "../Unreal/Plugins/AirSim/Content/VehicleAdv/SUV" ]; then + rm -rf "../Unreal/Plugins/AirSim/Content/VehicleAdv/SUV" + fi + unzip car_assets.zip -d ../Unreal/Plugins/AirSim/Content/VehicleAdv + cd .. rm -rf "suv_download_tmp" - fi - mkdir -p "suv_download_tmp" - cd suv_download_tmp - wget https://github.com/Microsoft/AirSim/releases/download/v1.2.0/car_assets.zip - if [ -d "../Unreal/Plugins/AirSim/Content/VehicleAdv/SUV" ]; then - rm -rf "../Unreal/Plugins/AirSim/Content/VehicleAdv/SUV" - fi - unzip car_assets.zip -d ../Unreal/Plugins/AirSim/Content/VehicleAdv - cd .. - rm -rf "suv_download_tmp" - else - echo "Not downloading high-poly car asset. The default unreal vehicle will be used." fi +else + echo "### Not downloading high-poly car asset (--no-full-poly-car). The default unreal vehicle will be used." fi -# Below is alternative way to get cland by downloading binaries +# Below is alternative way to get clang by downloading binaries # get clang, libc++ # sudo rm -rf llvm-build # mkdir -p llvm-build/output @@ -134,35 +190,48 @@ fi # #sudo apt-get install -y clang-3.9-doc libclang-common-3.9-dev libclang-3.9-dev libclang1-3.9 libclang1-3.9-dbg libllvm-3.9-ocaml-dev libllvm3.9 libllvm3.9-dbg lldb-3.9 llvm-3.9 llvm-3.9-dev llvm-3.9-doc llvm-3.9-examples llvm-3.9-runtime clang-format-3.9 python-clang-3.9 libfuzzer-3.9-dev #get libc++ source -if [[ ! -d "llvm-source-50" ]]; then - git clone --depth=1 -b release_50 https://github.com/llvm-mirror/llvm.git llvm-source-50 - git clone --depth=1 -b release_50 https://github.com/llvm-mirror/libcxx.git llvm-source-50/projects/libcxx - git clone --depth=1 -b release_50 https://github.com/llvm-mirror/libcxxabi.git llvm-source-50/projects/libcxxabi -else - echo "folder llvm-source-50 already exists, skipping git clone..." -fi - -#build libc++ -rm -rf llvm-build -mkdir -p llvm-build -pushd llvm-build >/dev/null - +if ! $gccBuild; then + echo "### Installing llvm 5 libc++ library..." + if [[ ! -d "llvm-source-50" ]]; then + git clone --depth=1 -b release_50 https://github.com/llvm-mirror/llvm.git llvm-source-50 + git clone --depth=1 -b release_50 https://github.com/llvm-mirror/libcxx.git llvm-source-50/projects/libcxx + git clone --depth=1 -b release_50 https://github.com/llvm-mirror/libcxxabi.git llvm-source-50/projects/libcxxabi + else + echo "folder llvm-source-50 already exists, skipping git clone..." + fi + #build libc++ + if [ "$(uname)" == "Darwin" ]; then + rm -rf llvm-build + else + sudo rm -rf llvm-build + fi + mkdir -p llvm-build + pushd llvm-build >/dev/null -"$CMAKE" -DCMAKE_C_COMPILER=${C_COMPILER} -DCMAKE_CXX_COMPILER=${COMPILER} \ - -LIBCXX_ENABLE_EXPERIMENTAL_LIBRARY=OFF -DLIBCXX_INSTALL_EXPERIMENTAL_LIBRARY=OFF \ - -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_INSTALL_PREFIX=./output \ - ../llvm-source-50 + "$CMAKE" -DCMAKE_C_COMPILER=${C_COMPILER} -DCMAKE_CXX_COMPILER=${COMPILER} \ + -LIBCXX_ENABLE_EXPERIMENTAL_LIBRARY=OFF -DLIBCXX_INSTALL_EXPERIMENTAL_LIBRARY=OFF \ + -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_INSTALL_PREFIX=./output \ + ../llvm-source-50 -make cxx + make cxx -j`nproc` -#install libc++ locally in output folder -make install-libcxx install-libcxxabi + #install libc++ locally in output folder + if [ "$(uname)" == "Darwin" ]; then + make install-libcxx install-libcxxabi + else + sudo make install-libcxx install-libcxxabi + fi -popd >/dev/null + popd >/dev/null +fi -#install EIGEN library +echo "Installing EIGEN library..." -rm -rf ./AirLib/deps/eigen3/Eigen +if [ "$(uname)" == "Darwin" ]; then + rm -rf ./AirLib/deps/eigen3/Eigen +else + sudo rm -rf ./AirLib/deps/eigen3/Eigen +fi echo "downloading eigen..." wget http://bitbucket.org/eigen/eigen/get/3.3.2.zip unzip 3.3.2.zip -d temp_eigen