diff --git a/AirLib/AirLib.vcxproj b/AirLib/AirLib.vcxproj index 75b853bfb5..4dd04fc7ee 100644 --- a/AirLib/AirLib.vcxproj +++ b/AirLib/AirLib.vcxproj @@ -27,10 +27,12 @@ - + + + @@ -53,7 +55,7 @@ - + @@ -71,13 +73,14 @@ - + + - + @@ -137,20 +140,17 @@ - - - + - + - + - @@ -174,7 +174,6 @@ - @@ -186,7 +185,7 @@ - + diff --git a/AirLib/AirLib.vcxproj.filters b/AirLib/AirLib.vcxproj.filters index c4231bfeaa..65aa0bb0d1 100644 --- a/AirLib/AirLib.vcxproj.filters +++ b/AirLib/AirLib.vcxproj.filters @@ -165,15 +165,6 @@ Header Files - - Header Files - - - Header Files - - - Header Files - Header Files @@ -204,9 +195,6 @@ Header Files - - Header Files - Header Files @@ -405,15 +393,6 @@ Header Files - - Header Files - - - Header Files - - - Header Files - Header Files @@ -441,21 +420,9 @@ Header Files - - Header Files - Header Files - - Header Files - - - Header Files - - - Header Files - Header Files @@ -489,10 +456,40 @@ Header Files - + Header Files - + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + Header Files @@ -524,7 +521,7 @@ Source Files - + Source Files diff --git a/AirLib/include/api/ApiServerBase.hpp b/AirLib/include/api/ApiServerBase.hpp index 5ddeb9cae1..0feec316f6 100644 --- a/AirLib/include/api/ApiServerBase.hpp +++ b/AirLib/include/api/ApiServerBase.hpp @@ -4,9 +4,10 @@ #ifndef air_ApiServerBase_hpp #define air_ApiServerBase_hpp -#include "common/Common.hpp" #include - +#include "common/Common.hpp" +#include "VehicleApiBase.hpp" +#include "WorldSimApiBase.hpp" namespace msr { namespace airlib { @@ -14,6 +15,31 @@ class ApiServerBase { public: virtual void start(bool block = false) = 0; virtual void stop() = 0; + + virtual const VehicleApiBase* getVehicleApi(const std::string& vehicle_name = "") const = 0; + virtual VehicleApiBase* getVehicleApi(const std::string& vehicle_name = "") + { + return const_cast(getVehicleApi(vehicle_name)); + } + + virtual const WorldSimApiBase* getWorldSimApi() const = 0; + virtual WorldSimApiBase* getWorldSimApi() + { + return const_cast(getWorldSimApi()); + } + + virtual const VehicleSimApiBase* getVehicleSimApi(const std::string& vehicle_name = "") const = 0; + virtual VehicleSimApiBase* getVehicleSimApi(const std::string& vehicle_name = "") + { + auto* world_sim_api = getWorldSimApi(); + if (world_sim_api) { + auto* vehicle_sim_api = getWorldSimApi()->getVehicleSimApi(vehicle_name); + return const_cast(vehicle_sim_api); + } + return nullptr; + } + + virtual ~ApiServerBase() = default; }; diff --git a/AirLib/include/api/DebugApiServer.hpp b/AirLib/include/api/DebugApiServer.hpp index 5c826383f0..edca45aa4d 100644 --- a/AirLib/include/api/DebugApiServer.hpp +++ b/AirLib/include/api/DebugApiServer.hpp @@ -19,6 +19,20 @@ namespace msr { namespace airlib { { common_utils::Utils::log("Debug server stopped"); } + + virtual const VehicleApiBase* getVehicleApi() const + { + //not supported + return nullptr; + } + + virtual const WorldSimApiBase* getWorldSimApi() const + { + //not supported + return nullptr; + } + + virtual ~DebugApiServer() = default; }; diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index e28c248220..1afb45a025 100644 --- a/AirLib/include/api/RpcLibClientBase.hpp +++ b/AirLib/include/api/RpcLibClientBase.hpp @@ -11,6 +11,7 @@ namespace msr { namespace airlib { +//common methods for RCP clients of different vehicles class RpcLibClientBase { public: enum class ConnectionState : uint { @@ -18,6 +19,15 @@ class RpcLibClientBase { }; public: RpcLibClientBase(const string& ip_address = "localhost", uint16_t port = 41451, uint timeout_ms = 60000); + virtual ~RpcLibClientBase(); //required for pimpl + + void confirmConnection(); + bool isApiControlEnabled() const; + void enableApiControl(bool is_enabled); + void resetVehicle(); + void simResetWorld(); + bool armDisarm(bool arm); + ConnectionState getConnectionState(); bool ping(); int getClientVersion() const; @@ -25,38 +35,35 @@ class RpcLibClientBase { int getMinRequiredServerVersion() const; int getMinRequiredClientVersion() const; + bool simIsPaused() const; + void simPause(bool is_paused); + void simContinueForTime(double seconds); - vector simGetImages(vector request); - vector simGetImage(int camera_id, ImageCaptureBase::ImageType type); - msr::airlib::GeoPoint getHomeGeoPoint(); + msr::airlib::GeoPoint getHomeGeoPoint() const; - void simSetPose(const Pose& pose, bool ignore_collision); - Pose simGetPose(); + Pose simGetVehiclePose() const; + void simSetVehiclePose(const Pose& pose, bool ignore_collision); + Pose simGetObjectPose(const std::string& object_name) const; - void confirmConnection(); - bool isApiControlEnabled(); - void enableApiControl(bool is_enabled); - void reset(); - bool armDisarm(bool arm); + vector simGetImages(vector request); + vector simGetImage(int camera_id, ImageCaptureBase::ImageType type); - CollisionInfo getCollisionInfo(); + CollisionInfo simGetCollisionInfo() const; bool simSetSegmentationObjectID(const std::string& mesh_name, int object_id, bool is_name_regex = false); - int simGetSegmentationObjectID(const std::string& mesh_name); + int simGetSegmentationObjectID(const std::string& mesh_name) const; void simPrintLogMessage(const std::string& message, std::string message_param = "", unsigned char severity = 0); - Pose simGetObjectPose(const std::string& object_name); - CameraInfo getCameraInfo(int camera_id); + CameraInfo getCameraInfo(int camera_id) const; void setCameraOrientation(int camera_id, const Quaternionr& orientation); - bool simIsPaused(); - void simPause(bool is_paused); - void simContinueForTime(double seconds); - - virtual ~RpcLibClientBase(); //required for pimpl protected: - void* getClient(); + const void* getClient() const; + void* getClient() + { + return const_cast(getClient()); + } private: struct impl; diff --git a/AirLib/include/api/RpcLibServerBase.hpp b/AirLib/include/api/RpcLibServerBase.hpp index fcae3ed06c..548300805d 100644 --- a/AirLib/include/api/RpcLibServerBase.hpp +++ b/AirLib/include/api/RpcLibServerBase.hpp @@ -6,7 +6,7 @@ #include "common/Common.hpp" #include "api/ApiServerBase.hpp" -#include "api/SimModeApiBase.hpp" +#include "api/WorldSimApiBase.hpp" namespace msr { namespace airlib { @@ -14,20 +14,16 @@ namespace msr { namespace airlib { class RpcLibServerBase : public ApiServerBase { public: - RpcLibServerBase(SimModeApiBase* simmode_api, string server_address, uint16_t port); + RpcLibServerBase(const std::string& server_address, uint16_t port); + virtual ~RpcLibServerBase() override; + virtual void start(bool block = false) override; virtual void stop() override; - virtual ~RpcLibServerBase() override; protected: void* getServer() const; - SimModeApiBase* getSimModeApi() const; - -private: - VehicleApiBase* getVehicleApi() const; private: - SimModeApiBase* simmode_api_; struct impl; std::unique_ptr pimpl_; }; diff --git a/AirLib/include/api/SimModeApiBase.hpp b/AirLib/include/api/SimModeApiBase.hpp deleted file mode 100644 index 7bfbb57452..0000000000 --- a/AirLib/include/api/SimModeApiBase.hpp +++ /dev/null @@ -1,27 +0,0 @@ -// Copyright (c) Microsoft Corporation. All rights reserved. -// Licensed under the MIT License. - -#ifndef air_SimModeApiBase_hpp -#define air_SimModeApiBase_hpp - -#include "common/CommonStructs.hpp" -#include "VehicleApiBase.hpp" - -namespace msr { namespace airlib { - - -class SimModeApiBase { -public: - virtual VehicleApiBase* getVehicleApi() = 0; - virtual ~SimModeApiBase() = default; - - virtual bool isPaused() const = 0; - virtual void reset() = 0; - virtual void pause(bool is_paused) = 0; - virtual void continueForTime(double seconds) = 0; - virtual bool isSimulationMode() const = 0; -}; - - -}} //namespace -#endif diff --git a/AirLib/include/api/VehicleApiBase.hpp b/AirLib/include/api/VehicleApiBase.hpp index 9062c40b0b..278422bb2e 100644 --- a/AirLib/include/api/VehicleApiBase.hpp +++ b/AirLib/include/api/VehicleApiBase.hpp @@ -4,37 +4,38 @@ #ifndef air_VehicleApiBase_hpp #define air_VehicleApiBase_hpp +#include "common/CommonStructs.hpp" +#include "common/UpdatableObject.hpp" +#include "common/Common.hpp" +#include "common/Waiter.hpp" +#include "safety/SafetyEval.hpp" #include "common/CommonStructs.hpp" #include "common/ImageCaptureBase.hpp" +#include +#include namespace msr { namespace airlib { - -class VehicleApiBase { +/* +Vehicle controller allows to obtain state from vehicle and send control commands to the vehicle. +State can include many things including sensor data, logs, estimated state from onboard computer etc. +Control commands can be low level actuation commands or high level movement commands. +The base class defines usually available methods that all vehicle controllers may implement. +Some methods may not be applicable to specific vehicle in which case an exception may be raised or call may be ignored. +*/ +class VehicleApiBase : public UpdatableObject { public: - virtual GeoPoint getHomeGeoPoint() const = 0; virtual void enableApiControl(bool is_enabled) = 0; - virtual bool armDisarm(bool arm) = 0; virtual bool isApiControlEnabled() const = 0; - virtual void reset() = 0; + virtual bool armDisarm(bool arm) = 0; virtual void cancelPendingTasks() = 0; + virtual GeoPoint getHomeGeoPoint() const = 0; - - virtual vector simGetImages(const vector& request) const = 0; - virtual vector simGetImage(uint8_t camera_id, ImageCaptureBase::ImageType image_type) const = 0; - - virtual void simSetPose(const Pose& pose, bool ignore_collision) = 0; - virtual Pose simGetPose() const = 0; - - virtual bool simSetSegmentationObjectID(const std::string& mesh_name, int object_id, bool is_name_regex = false) = 0; - virtual int simGetSegmentationObjectID(const std::string& mesh_name) const = 0; - - virtual void simPrintLogMessage(const std::string& message, - const std::string& message_param = "", unsigned char severity = 0) = 0; - - virtual CollisionInfo getCollisionInfo() const = 0; - - virtual Pose simGetObjectPose(const std::string& object_name) const = 0; + virtual void getStatusMessages(std::vector& messages) + { + unused(messages); + //default implementation + } virtual CameraInfo getCameraInfo(int camera_id) const = 0; virtual void setCameraOrientation(int camera_id, const Quaternionr& orientation) = 0; @@ -42,6 +43,27 @@ class VehicleApiBase { virtual ~VehicleApiBase() = default; }; +class VehicleControllerException : public std::runtime_error { +public: + VehicleControllerException(const std::string& message) + : runtime_error(message) { + } +}; + +class VehicleCommandNotImplementedException : public VehicleControllerException { +public: + VehicleCommandNotImplementedException(const std::string& message) + : VehicleControllerException(message) { + } +}; + +class VehicleMoveException : public VehicleControllerException { +public: + VehicleMoveException(const std::string& message) + : VehicleControllerException(message) { + } +}; + }} //namespace #endif diff --git a/AirLib/include/controllers/VehicleConnectorBase.hpp b/AirLib/include/api/VehicleConnectorBase.hpp similarity index 93% rename from AirLib/include/controllers/VehicleConnectorBase.hpp rename to AirLib/include/api/VehicleConnectorBase.hpp index 3b1fd07a8c..f2f1ab973d 100644 --- a/AirLib/include/controllers/VehicleConnectorBase.hpp +++ b/AirLib/include/api/VehicleConnectorBase.hpp @@ -4,7 +4,7 @@ #ifndef air_VehicleConnectorBase_hpp #define air_VehicleConnectorBase_hpp -#include "VehicleControllerBase.hpp" +#include "VehicleApiBase.hpp" #include "common/ImageCaptureBase.hpp" #include "common/UpdatableObject.hpp" @@ -20,7 +20,7 @@ class VehicleConnectorBase : public UpdatableObject //called when render changes are required virtual void updateRendering(float dt) = 0; - virtual VehicleControllerBase* getController() = 0; + virtual VehicleApiBase* getController() = 0; virtual ImageCaptureBase* getImageCapture() = 0; virtual void setPose(const Pose& pose, bool ignore_collision) = 0; virtual Pose getPose() = 0; diff --git a/AirLib/include/api/VehicleSimApiBase.hpp b/AirLib/include/api/VehicleSimApiBase.hpp new file mode 100644 index 0000000000..b765bcc21d --- /dev/null +++ b/AirLib/include/api/VehicleSimApiBase.hpp @@ -0,0 +1,38 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef air_VehicleSimApiBase_hpp +#define air_VehicleSimApiBase_hpp + +#include "common/CommonStructs.hpp" +#include "common/ImageCaptureBase.hpp" +#include "physics/Kinematics.hpp" + +namespace msr { namespace airlib { + +class VehicleSimApiBase { +public: + virtual ~VehicleSimApiBase() = default; + + virtual void reset() = 0; + + virtual vector getImages(const vector& request) const = 0; + virtual vector getImage(uint8_t camera_id, ImageCaptureBase::ImageType image_type) const = 0; + + virtual void setPose(const Pose& pose, bool ignore_collision) = 0; + virtual Pose getPose() const = 0; + virtual CollisionInfo getCollisionInfo() const = 0; + virtual void setCollisionInfo(const CollisionInfo& collision_info) = 0; + virtual Kinematics::State getTrueKinematics() const = 0; + virtual int getRemoteControlID() const { return -1; } + virtual void setRCData(const RCData& rcData) = 0; + + //TODO: need better place for below two APIs? + //return 0 to 1 (corresponds to zero to full thrust) + virtual real_T getVertexControlSignal(unsigned int rotor_index) const = 0; + virtual size_t getVertexCount() const = 0; + +}; + +} } //namespace +#endif diff --git a/AirLib/include/api/WorldSimApiBase.hpp b/AirLib/include/api/WorldSimApiBase.hpp new file mode 100644 index 0000000000..5b86a7151f --- /dev/null +++ b/AirLib/include/api/WorldSimApiBase.hpp @@ -0,0 +1,38 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef air_WorldSimApiBase_hpp +#define air_WorldSimApiBase_hpp + +#include "common/CommonStructs.hpp" +#include "VehicleSimApiBase.hpp" + +namespace msr { namespace airlib { + + +class WorldSimApiBase { +public: + virtual ~WorldSimApiBase() = default; + + virtual bool isPaused() const = 0; + virtual void reset() = 0; + virtual void pause(bool is_paused) = 0; + virtual void continueForTime(double seconds) = 0; + + virtual const VehicleSimApiBase* getVehicleSimApi(const std::string& vehicle_name = "") const = 0; + virtual VehicleSimApiBase* getVehicleSimApi(const std::string& vehicle_name = "") + { + return const_cast(getVehicleSimApi(vehicle_name)); + } + + virtual bool setSegmentationObjectID(const std::string& mesh_name, int object_id, bool is_name_regex = false) = 0; + virtual int getSegmentationObjectID(const std::string& mesh_name) const = 0; + virtual void printLogMessage(const std::string& message, + const std::string& message_param = "", unsigned char severity = 0) = 0; + virtual Pose getObjectPose(const std::string& object_name) const = 0; + +}; + + +}} //namespace +#endif diff --git a/AirLib/include/common/CancelToken.hpp b/AirLib/include/common/CancelToken.hpp new file mode 100644 index 0000000000..47ecdaedfc --- /dev/null +++ b/AirLib/include/common/CancelToken.hpp @@ -0,0 +1,91 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef air_CancelToken_hpp +#define air_CancelToken_hpp + +#include +#include +#include "common/Common.hpp" +#include "common/common_utils/Utils.hpp" + +namespace msr { namespace airlib { + +class CancelToken { +public: + CancelToken() + : is_cancelled_(false), recursion_count_(0) + { + } + + void reset() + { + is_cancelled_ = false; + recursion_count_ = 0; + } + + bool isCancelled() const + { + return is_cancelled_; + } + + void cancel() + { + is_cancelled_ = true; + } + + bool sleep(double secs) + { + //We can pass duration directly to sleep_for however it is known that on + //some systems, sleep_for makes system call anyway even if passed duration + //is <= 0. This can cause 50us of delay due to context switch. + if (isCancelled()) { + return false; + } + + TTimePoint start = ClockFactory::get()->nowNanos(); + static constexpr std::chrono::duration MinSleepDuration(0); + + while (secs > 0 && !isCancelled() && + ClockFactory::get()->elapsedSince(start) < secs) { + + std::this_thread::sleep_for(MinSleepDuration); + } + + return !isCancelled(); + } + + int getRecursionCount() + { + return recursion_count_; + } + + bool try_lock() + { + bool result = wait_mutex_.try_lock(); + if (result) + ++recursion_count_; + return result; + } + + void unlock() + { + wait_mutex_.unlock(); + if (recursion_count_ > 0) + --recursion_count_; + } + + void lock() + { + wait_mutex_.lock(); + ++recursion_count_; + } + +private: + std::atomic is_cancelled_; + std::recursive_mutex wait_mutex_; + std::atomic recursion_count_; +}; + +}} //namespace +#endif diff --git a/AirLib/include/controllers/PidController.hpp b/AirLib/include/common/PidController.hpp similarity index 96% rename from AirLib/include/controllers/PidController.hpp rename to AirLib/include/common/PidController.hpp index c7d86c2587..f0a6ad6292 100644 --- a/AirLib/include/controllers/PidController.hpp +++ b/AirLib/include/common/PidController.hpp @@ -1,79 +1,79 @@ -// Copyright (c) Microsoft Corporation. All rights reserved. -// Licensed under the MIT License. - -#ifndef msr_airlib__PidController_hpp -#define msr_airlib__PidController_hpp - -#include - -namespace msr { namespace airlib { - - -// This class implements a pid controller. -// First call setPoint to set the desired point and the PID control variables -// then call control(x) with new observed values and it will return the updated -// control output needed to achieve the setPoint goal. Integration is done using -// dt measured using system clock. -class PidController -{ -private: - float set_point_; - float kProportional_; - float kIntegral_; - float kDerivative_; - float previous_error_; - bool previous_set; - float sum_; - std::chrono::time_point prev_time_; -public: - //set desired point. - void setPoint(float target, float kProportional, float kIntegral, float kDerivative) { - set_point_ = target; - kProportional_ = kProportional; - kIntegral_ = kIntegral; - kDerivative_ = kDerivative; - prev_time_ = std::chrono::system_clock::now(); - previous_error_ = 0; - sum_ = 0; - previous_set = false; - } - float control(float processVariable) { - auto t = std::chrono::system_clock::now(); - auto diff = std::chrono::system_clock::now() - prev_time_; - - float error = set_point_ - processVariable; - if (!previous_set) - { - previous_set = true; - previous_error_ = error; - return 0; - } - - float dt = static_cast(std::chrono::duration_cast(diff).count()) * 0.000001f; - dt = fmax(dt, 0.01f); - float proportionalGain = 0; - float derivativeGain = 0; - float integralGain = 0; - - if (kProportional_ != 0) { - proportionalGain = error * kProportional_; - } - if (kDerivative_ != 0) { - float derivative = (error - previous_error_) / dt; - derivativeGain = derivative * kDerivative_; - } - if (kIntegral_ != 0) { - sum_ += error * dt; - integralGain = sum_ * kIntegral_; - } - - previous_error_ = error; - prev_time_ = t; - - return proportionalGain + derivativeGain + integralGain; - } -}; - - -}} //namespace -#endif +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef msr_airlib__PidController_hpp +#define msr_airlib__PidController_hpp + +#include + +namespace msr { namespace airlib { + + +// This class implements a pid controller. +// First call setPoint to set the desired point and the PID control variables +// then call control(x) with new observed values and it will return the updated +// control output needed to achieve the setPoint goal. Integration is done using +// dt measured using system clock. +class PidController +{ +private: + float set_point_; + float kProportional_; + float kIntegral_; + float kDerivative_; + float previous_error_; + bool previous_set; + float sum_; + std::chrono::time_point prev_time_; +public: + //set desired point. + void setPoint(float target, float kProportional, float kIntegral, float kDerivative) { + set_point_ = target; + kProportional_ = kProportional; + kIntegral_ = kIntegral; + kDerivative_ = kDerivative; + prev_time_ = std::chrono::system_clock::now(); + previous_error_ = 0; + sum_ = 0; + previous_set = false; + } + float control(float processVariable) { + auto t = std::chrono::system_clock::now(); + auto diff = std::chrono::system_clock::now() - prev_time_; + + float error = set_point_ - processVariable; + if (!previous_set) + { + previous_set = true; + previous_error_ = error; + return 0; + } + + float dt = static_cast(std::chrono::duration_cast(diff).count()) * 0.000001f; + dt = fmax(dt, 0.01f); + float proportionalGain = 0; + float derivativeGain = 0; + float integralGain = 0; + + if (kProportional_ != 0) { + proportionalGain = error * kProportional_; + } + if (kDerivative_ != 0) { + float derivative = (error - previous_error_) / dt; + derivativeGain = derivative * kDerivative_; + } + if (kIntegral_ != 0) { + sum_ += error * dt; + integralGain = sum_ * kIntegral_; + } + + previous_error_ = error; + prev_time_ = t; + + return proportionalGain + derivativeGain + integralGain; + } +}; + + +}} //namespace +#endif diff --git a/AirLib/include/common/Waiter.hpp b/AirLib/include/common/Waiter.hpp index c6b8e97457..2c7ca79caa 100644 --- a/AirLib/include/common/Waiter.hpp +++ b/AirLib/include/common/Waiter.hpp @@ -9,17 +9,17 @@ #include "common/Common.hpp" #include "common/common_utils/Utils.hpp" #include "common/ClockFactory.hpp" -#include "common/common_utils/WorkerThread.hpp" +#include "common/CancelToken.hpp" namespace msr { namespace airlib { class Waiter { private: - TTimePoint proc_start_; TTimePoint loop_start_; TTimeDelta sleep_duration_, timeout_duration_; + public: Waiter(TTimeDelta sleep_duration_seconds, TTimeDelta timeout_duration = std::numeric_limits::max()) : sleep_duration_(sleep_duration_seconds), timeout_duration_(timeout_duration) @@ -27,7 +27,7 @@ class Waiter { proc_start_ = loop_start_ = clock()->nowNanos(); } - virtual bool sleep(CancelableBase& cancelable_action) + virtual bool sleep(CancelToken& cancelable_action) { // Sleeps for the time needed to get current running time up to the requested sleep_duration_. // So this can be used to "throttle" any loop to check something every sleep_duration_ seconds. diff --git a/AirLib/include/common/common_utils/WorkerThread.hpp b/AirLib/include/common/WorkerThread.hpp similarity index 77% rename from AirLib/include/common/common_utils/WorkerThread.hpp rename to AirLib/include/common/WorkerThread.hpp index 964b0659e0..61147ade4f 100644 --- a/AirLib/include/common/common_utils/WorkerThread.hpp +++ b/AirLib/include/common/WorkerThread.hpp @@ -1,302 +1,284 @@ -// Copyright (c) Microsoft Corporation. All rights reserved. -// Licensed under the MIT License. - -#ifndef commn_utils_WorkerThread_hpp -#define commn_utils_WorkerThread_hpp - -#include -#include -#include -#include -#include -#include -#include -#include -#include "Utils.hpp" -#include "common/ClockFactory.hpp" //TODO: move this out of common_utils - -namespace msr { namespace airlib { - -class CancelableBase { -protected: - std::atomic is_cancelled_; - std::atomic is_complete_; -public: - CancelableBase() : is_cancelled_(false), is_complete_(false) { - } - - void reset() - { - is_cancelled_ = false; - is_complete_ = false; - } - - bool isCancelled() { - return is_cancelled_; - } - - void cancel() { - is_cancelled_ = true; - } - - virtual void execute() = 0; - - virtual bool sleep(double secs) - { - //We can pass duration directly to sleep_for however it is known that on - //some systems, sleep_for makes system call anyway even if passed duration - //is <= 0. This can cause 50us of delay due to context switch. - if (isCancelled()) { - return false; - } - - TTimePoint start = ClockFactory::get()->nowNanos(); - static constexpr std::chrono::duration MinSleepDuration(0); - - while(secs > 0 && !isCancelled() && !is_complete_ && - ClockFactory::get()->elapsedSince(start) < secs) { - - std::this_thread::sleep_for(MinSleepDuration); - } - - return !isCancelled(); - } - - void complete() { - is_complete_ = true; - } - - bool isComplete() { - return is_complete_; - } - -}; - -// This wraps a condition_variable so we can handle the case where we may signal before wait -// and implement the semantics that say wait should be a noop in that case. -class WorkerThreadSignal -{ - std::condition_variable cv_; - std::mutex mutex_; - std::atomic signaled_; -public: - WorkerThreadSignal() : signaled_(false) - { - } - - void signal() - { - { - std::unique_lock lock(mutex_); - signaled_ = true; - } - cv_.notify_one(); - } - - template - void wait(_Predicate cancel) - { - // wait for signal or cancel predicate - while (!signaled_) { - std::unique_lock lock(mutex_); - cv_.wait_for(lock, std::chrono::milliseconds(1), [this, cancel] { - return cancel(); - }); - } - signaled_ = false; - } - - bool waitFor(double max_wait_seconds) - { - // wait for signal or timeout or cancel predicate - while (!signaled_) { - std::unique_lock lock(mutex_); - cv_.wait_for(lock, std::chrono::milliseconds( - static_cast(max_wait_seconds * 1000))); - } - signaled_ = false; - return true; - } - -}; - -// This class provides a synchronized worker thread that guarantees to execute -// cancelable tasks on a background thread one at a time. -// It guarantees that previous task is cancelled before new task is started. -// The queue size is 1, which means it does NOT guarantee all queued tasks are executed. -// If enqueue is called very quickly the thread will not have a chance to execute each -// task before they get cancelled, worst case in a tight loop all tasks are starved and -// nothing executes. -class WorkerThread { -public: - WorkerThread() - : thread_running_(false), cancel_request_(false) { - } - - ~WorkerThread() { - cancel_request_ = true; - cancel(); - } - void enqueue(std::shared_ptr item) { - //cancel previous item - { - std::unique_lock lock(mutex_); - std::shared_ptr pending = pending_item_; - if (pending != nullptr) { - pending->cancel(); - } - } - - bool running = false; - - { - std::unique_lock lock(mutex_); - pending_item_ = item; - running = thread_running_; - } - - if (running) { - item_arrived_.signal(); - } - else { - start(); - } - } - - bool enqueueAndWait(std::shared_ptr item, float max_wait_seconds) - { - //cancel previous item - { - std::unique_lock lock(mutex_); - std::shared_ptr pending = pending_item_; - if (pending != nullptr) { - pending->cancel(); - } - } - - bool running = false; - - //set new item to run - { - std::unique_lock lock(mutex_); - pending_item_ = item; - running = thread_running_; - } - - if (running) { - item_arrived_.signal(); - } - else { - start(); - } - - item->sleep(max_wait_seconds); - - //after the wait if item is still running then cancel it - if (!item->isCancelled() && !item->isComplete()) - item->cancel(); - - return !item->isCancelled(); - } - - void cancel() - { - std::unique_lock lock(mutex_); - std::shared_ptr pending = pending_item_; - pending_item_ = nullptr; - if (pending != nullptr) { - pending->cancel(); - } - if (thread_.joinable()) { - item_arrived_.signal(); - thread_.join(); - } - } -private: - void start() - { - //if state == not running - if (!thread_running_) { - - //make sure C++ previous thread is done - { - std::unique_lock lock(mutex_); - if (thread_.joinable()) { - thread_.join(); - } - } - Utils::cleanupThread(thread_); - - //start the thread - cancel_request_ = false; - thread_ = std::thread(&WorkerThread::run, this); - - //wait until thread tells us it has started - thread_started_.wait([this] { return static_cast(cancel_request_); }); - } - } - - void run() - { - thread_running_ = true; - - //tell the thread which started this thread that we are on now - { - std::unique_lock lock(mutex_); - thread_started_.signal(); - } - - //until we don't get stopped and have work to do, keep running - while (!cancel_request_ && pending_item_ != nullptr) { - std::shared_ptr pending; - - //get the pending item - { - std::unique_lock lock(mutex_); - pending = pending_item_; - } - - //if pending item is not yet cancelled - if (pending != nullptr && !pending->isCancelled()) { - - //execute pending item - try { - pending->execute(); - } - catch (std::exception& e) { - //Utils::DebugBreak(); - Utils::log(Utils::stringf("WorkerThread caught unhandled exception: %s", e.what()), Utils::kLogLevelError); - } - - //we use cancel here to communicate to enqueueAndWait that the task is complete. - pending->complete(); - } - - if (!cancel_request_) { - //wait for next item to arrive or thread is stopped - item_arrived_.wait([this] { return static_cast(cancel_request_); }); - } - } - - thread_running_ = false; - } - -private: - //this is used to wait until our thread actually gets started - WorkerThreadSignal thread_started_; - //when new item arrived, we signal this so waiting thread can continue - WorkerThreadSignal item_arrived_; - - // thread state - std::shared_ptr pending_item_; - std::mutex mutex_; - std::thread thread_; - //while run() is in progress this is true - std::atomic thread_running_; - //has request to stop this worker thread made? - std::atomic cancel_request_; -}; - - -}} //namespace +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef commn_utils_WorkerThread_hpp +#define commn_utils_WorkerThread_hpp + +#include +#include +#include +#include +#include +#include +#include +#include +#include "Utils.hpp" +#include "ClockFactory.hpp" //TODO: move this out of common_utils +#include "CancelToken.hpp" + +namespace msr { namespace airlib { + +class CancelableAction : public CancelToken { +protected: + virtual void executeAction() = 0; + +public: + CancelableAction() + : is_complete_(false) + { + } + + void reset() + { + is_complete_ = false; + } + + void execute() + { + is_complete_ = false; + try { + executeAction(); + } + finally{ + is_complete_ = true; + } + } + + bool isComplete() const + { + return is_complete_; + } + + private: + std::atomic is_complete_; + +}; + +// This wraps a condition_variable so we can handle the case where we may signal before wait +// and implement the semantics that say wait should be a noop in that case. +class WorkerThreadSignal +{ + std::condition_variable cv_; + std::mutex mutex_; + std::atomic signaled_; +public: + WorkerThreadSignal() : signaled_(false) + { + } + + void signal() + { + { + std::unique_lock lock(mutex_); + signaled_ = true; + } + cv_.notify_one(); + } + + template + void wait(_Predicate cancel) + { + // wait for signal or cancel predicate + while (!signaled_) { + std::unique_lock lock(mutex_); + cv_.wait_for(lock, std::chrono::milliseconds(1), [this, cancel] { + return cancel(); + }); + } + signaled_ = false; + } + + bool waitFor(double max_wait_seconds) + { + // wait for signal or timeout or cancel predicate + while (!signaled_) { + std::unique_lock lock(mutex_); + cv_.wait_for(lock, std::chrono::milliseconds( + static_cast(max_wait_seconds * 1000))); + } + signaled_ = false; + return true; + } + +}; + +// This class provides a synchronized worker thread that guarantees to execute +// cancelable tasks on a background thread one at a time. +// It guarantees that previous task is cancelled before new task is started. +// The queue size is 1, which means it does NOT guarantee all queued tasks are executed. +// If enqueue is called very quickly the thread will not have a chance to execute each +// task before they get cancelled, worst case in a tight loop all tasks are starved and +// nothing executes. +class WorkerThread { +public: + WorkerThread() + : thread_running_(false), cancel_request_(false) { + } + + ~WorkerThread() { + cancel_request_ = true; + cancel(); + } + void enqueue(std::shared_ptr item) { + //cancel previous item + { + std::unique_lock lock(mutex_); + std::shared_ptr pending = pending_item_; + if (pending != nullptr) { + pending->cancel(); + } + } + + bool running = false; + + { + std::unique_lock lock(mutex_); + pending_item_ = item; + running = thread_running_; + } + + if (running) { + item_arrived_.signal(); + } + else { + start(); + } + } + + bool enqueueAndWait(std::shared_ptr item, float max_wait_seconds) + { + //cancel previous item + { + std::unique_lock lock(mutex_); + std::shared_ptr pending = pending_item_; + if (pending != nullptr) { + pending->cancel(); + } + } + + bool running = false; + + //set new item to run + { + std::unique_lock lock(mutex_); + pending_item_ = item; + running = thread_running_; + } + + if (running) { + item_arrived_.signal(); + } + else { + start(); + } + + item->sleep(max_wait_seconds); + + //after the wait if item is still running then cancel it + if (!item->isCancelled() && !item->isComplete()) + item->cancel(); + + return !item->isCancelled(); + } + + void cancel() + { + std::unique_lock lock(mutex_); + std::shared_ptr pending = pending_item_; + pending_item_ = nullptr; + if (pending != nullptr) { + pending->cancel(); + } + if (thread_.joinable()) { + item_arrived_.signal(); + thread_.join(); + } + } +private: + void start() + { + //if state == not running + if (!thread_running_) { + + //make sure C++ previous thread is done + { + std::unique_lock lock(mutex_); + if (thread_.joinable()) { + thread_.join(); + } + } + Utils::cleanupThread(thread_); + + //start the thread + cancel_request_ = false; + thread_ = std::thread(&WorkerThread::run, this); + + //wait until thread tells us it has started + thread_started_.wait([this] { return static_cast(cancel_request_); }); + } + } + + void run() + { + thread_running_ = true; + + //tell the thread which started this thread that we are on now + { + std::unique_lock lock(mutex_); + thread_started_.signal(); + } + + //until we don't get stopped and have work to do, keep running + while (!cancel_request_ && pending_item_ != nullptr) { + std::shared_ptr pending; + + //get the pending item + { + std::unique_lock lock(mutex_); + pending = pending_item_; + } + + //if pending item is not yet cancelled + if (pending != nullptr && !pending->isCancelled()) { + + //execute pending item + try { + pending->execute(); + } + catch (std::exception& e) { + //Utils::DebugBreak(); + Utils::log(Utils::stringf("WorkerThread caught unhandled exception: %s", e.what()), Utils::kLogLevelError); + } + + //we use cancel here to communicate to enqueueAndWait that the task is complete. + pending->complete(); + } + + if (!cancel_request_) { + //wait for next item to arrive or thread is stopped + item_arrived_.wait([this] { return static_cast(cancel_request_); }); + } + } + + thread_running_ = false; + } + +private: + //this is used to wait until our thread actually gets started + WorkerThreadSignal thread_started_; + //when new item arrived, we signal this so waiting thread can continue + WorkerThreadSignal item_arrived_; + + // thread state + std::shared_ptr pending_item_; + std::mutex mutex_; + std::thread thread_; + //while run() is in progress this is true + std::atomic thread_running_; + //has request to stop this worker thread made? + std::atomic cancel_request_; +}; + + +}} //namespace #endif \ No newline at end of file diff --git a/AirLib/include/controllers/ControllerBase.hpp b/AirLib/include/controllers/ControllerBase.hpp deleted file mode 100644 index 0c4a934d3e..0000000000 --- a/AirLib/include/controllers/ControllerBase.hpp +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright (c) Microsoft Corporation. All rights reserved. -// Licensed under the MIT License. - -#ifndef msr_airlib_ControllerBase_hpp -#define msr_airlib_ControllerBase_hpp - -#include "common/UpdatableObject.hpp" -#include -#include - -namespace msr { namespace airlib { - -/* - This class defines the interface for vehicle controllers. A typical vehicle controller would consume - sensor values as inputs and return control signals for various vertices on physics body as output. The update() - method should be used to process the sensor inputs from vehicle as needed (the implementor should have vehicle object - possibly through initialization). The getVertexControlSignal() is called to retrieve the value of control signal to be applied - to physics body vertex. - While reset() may be called at any time, all other methods should be expected be called after start() and before stop(). -*/ -class ControllerBase : public UpdatableObject { -public: - //return 0 to 1 (corresponds to zero to full thrust) - virtual real_T getVertexControlSignal(unsigned int rotor_index) const = 0; - virtual size_t getVertexCount() const = 0; - - virtual void getStatusMessages(std::vector& messages) - { - unused(messages); - //default implementation - } - - virtual void reportTelemetry(float renderTime) - { - unused(renderTime); - //no default action - } - -}; - -class ControllerException : public std::runtime_error { -public: - ControllerException(const std::string& message) - : runtime_error(message) { - } -}; - -}} //namespace -#endif diff --git a/AirLib/include/controllers/VehicleControllerBase.hpp b/AirLib/include/controllers/VehicleControllerBase.hpp deleted file mode 100644 index 5caead2419..0000000000 --- a/AirLib/include/controllers/VehicleControllerBase.hpp +++ /dev/null @@ -1,58 +0,0 @@ -// Copyright (c) Microsoft Corporation. All rights reserved. -// Licensed under the MIT License. - -#ifndef msr_airlib_VehicleControllerBase_hpp -#define msr_airlib_VehicleControllerBase_hpp - -#include "controllers/ControllerBase.hpp" -#include "physics/PhysicsBody.hpp" -#include -#include - -namespace msr { namespace airlib { - -/* - Defined additional interface for vehicles -*/ -class VehicleControllerBase : public ControllerBase { -public: - //tells the controller to switch from human operated mode to computer operated mode - virtual void enableApiControl(bool is_enabled) = 0; - virtual void setSimulationMode(bool is_set) = 0; - virtual bool isApiControlEnabled() const = 0; - - //if controller connects via USB/UDP and connection fails then this - //should return false - virtual bool isAvailable(std::string& message) const = 0; - - //TODO: below method is needed to support firmwares without state estimation. In future, we should probably remove this support. - virtual void setGroundTruth(PhysicsBody* physics_body) - { - unused(physics_body); - //by default don't use it. If derived class needs this, it should override. - } -}; - -class VehicleControllerException : public ControllerException { -public: - VehicleControllerException(const std::string& message) - : ControllerException(message) { - } -}; - -class VehicleCommandNotImplementedException : public VehicleControllerException { -public: - VehicleCommandNotImplementedException(const std::string& message) - : VehicleControllerException(message) { - } -}; - -class VehicleMoveException : public VehicleControllerException { -public: - VehicleMoveException(const std::string& message) - : VehicleControllerException(message) { - } -}; - -}} //namespace -#endif diff --git a/AirLib/include/safety/SafetyEval.hpp b/AirLib/include/safety/SafetyEval.hpp index 2647bd5c7e..d0355834fc 100644 --- a/AirLib/include/safety/SafetyEval.hpp +++ b/AirLib/include/safety/SafetyEval.hpp @@ -11,7 +11,7 @@ #include "common/common_utils/Utils.hpp" #include "IGeoFence.hpp" #include "common/Common.hpp" -#include "vehicles/multirotor/controllers/DroneCommon.hpp" +#include "vehicles/multirotor/controllers/MultirotorCommon.hpp" #include "common/common_utils/EnumFlags.hpp" namespace msr { namespace airlib { @@ -72,7 +72,7 @@ class SafetyEval { }; private: - VehicleParams vehicle_params_; + MultirotorApiParams vehicle_params_; shared_ptr fence_ptr_; shared_ptr obs_xy_ptr_; SafetyViolationType enable_reasons_ = SafetyEval::SafetyViolationType_::GeoFence; @@ -86,7 +86,7 @@ class SafetyEval { void setSuggestedVelocity(SafetyEval::EvalResult& result, const Quaternionr& quaternion); float adjustClearanceForPrStl(float base_clearance, float obs_confidence); public: - SafetyEval(VehicleParams vehicle_params, shared_ptr fence_ptr, shared_ptr obs_xy); + SafetyEval(MultirotorApiParams vehicle_params, shared_ptr fence_ptr, shared_ptr obs_xy); EvalResult isSafeVelocity(const Vector3r& cur_pos, const Vector3r& velocity, const Quaternionr& quaternion); EvalResult isSafeVelocityZ(const Vector3r& cur_pos, float vx, float vy, float z, const Quaternionr& quaternion); EvalResult isSafeDestination(const Vector3r& dest,const Vector3r& cur_pos, const Quaternionr& quaternion); diff --git a/AirLib/include/vehicles/car/api/CarRpcLibServer.hpp b/AirLib/include/vehicles/car/api/CarRpcLibServer.hpp index 8f9cded19b..b4d727aa18 100644 --- a/AirLib/include/vehicles/car/api/CarRpcLibServer.hpp +++ b/AirLib/include/vehicles/car/api/CarRpcLibServer.hpp @@ -13,7 +13,7 @@ namespace msr { namespace airlib { class CarRpcLibServer : public RpcLibServerBase { public: - CarRpcLibServer(SimModeApiBase* simmode_api, string server_address, uint16_t port = 41451); + CarRpcLibServer(WorldSimApiBase* simmode_api, string server_address, uint16_t port = 41451); virtual ~CarRpcLibServer(); private: diff --git a/AirLib/include/vehicles/multirotor/MultiRotor.hpp b/AirLib/include/vehicles/multirotor/MultiRotor.hpp index 284bc4c11b..6ff7d646fe 100644 --- a/AirLib/include/vehicles/multirotor/MultiRotor.hpp +++ b/AirLib/include/vehicles/multirotor/MultiRotor.hpp @@ -7,7 +7,7 @@ #include "common/Common.hpp" #include "common/CommonStructs.hpp" #include "Rotor.hpp" -#include "controllers/ControllerBase.hpp" +#include "api/VehicleApiBase.hpp" #include "MultiRotorParams.hpp" #include #include "physics/PhysicsBody.hpp" @@ -52,7 +52,7 @@ class MultiRotor : public PhysicsBody { getController()->setGroundTruth(this); } - DroneControllerBase* getController() + MultirotorApiBase* getController() { return params_->getController(); } diff --git a/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp b/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp index 212c01de41..e4c3da0c5c 100644 --- a/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp +++ b/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp @@ -7,7 +7,7 @@ #include "common/Common.hpp" #include "RotorParams.hpp" #include "sensors/SensorCollection.hpp" -#include "controllers/DroneControllerBase.hpp" +#include "controllers/MultirotorApiBase.h" @@ -85,11 +85,11 @@ class MultiRotorParams { { return sensors_; } - DroneControllerBase* getController() + MultirotorApiBase* getController() { return controller_.get(); } - const DroneControllerBase* getController() const + const MultirotorApiBase* getController() const { return controller_.get(); } @@ -126,7 +126,7 @@ class MultiRotorParams { protected: //must override by derived class virtual void setupParams() = 0; virtual std::unique_ptr createSensor(SensorBase::SensorType sensor_type) = 0; - virtual std::unique_ptr createController() = 0; + virtual std::unique_ptr createController() = 0; protected: //static utility functions for derived classes to use @@ -249,7 +249,7 @@ class MultiRotorParams { Params params_; SensorCollection sensors_; //maintains sensor type indexed collection of sensors vector> sensor_storage_; //RAII for created sensors - std::unique_ptr controller_; + std::unique_ptr controller_; }; }} //namespace diff --git a/AirLib/include/vehicles/multirotor/MultiRotorParamsFactory.hpp b/AirLib/include/vehicles/multirotor/MultiRotorParamsFactory.hpp index 4410eb8847..87b396330f 100644 --- a/AirLib/include/vehicles/multirotor/MultiRotorParamsFactory.hpp +++ b/AirLib/include/vehicles/multirotor/MultiRotorParamsFactory.hpp @@ -5,7 +5,7 @@ #define msr_airlib_vehicles_MultiRotorParamsFactory_hpp #include "vehicles/multirotor/configs/Px4MultiRotor.hpp" -#include "vehicles/multirotor/controllers/MavLinkDroneController.hpp" +#include "vehicles/multirotor/controllers/MavLinkMultirotorApi.hpp" #include "vehicles/multirotor/configs/RosFlightQuadX.hpp" #include "vehicles/multirotor/configs/SimpleFlightQuadX.hpp" diff --git a/AirLib/include/vehicles/multirotor/controllers/MavLinkDroneController.hpp b/AirLib/include/vehicles/multirotor/api/MavLinkMultirotorApi.hpp similarity index 86% rename from AirLib/include/vehicles/multirotor/controllers/MavLinkDroneController.hpp rename to AirLib/include/vehicles/multirotor/api/MavLinkMultirotorApi.hpp index 1b64edc298..0f4cf8f340 100644 --- a/AirLib/include/vehicles/multirotor/controllers/MavLinkDroneController.hpp +++ b/AirLib/include/vehicles/multirotor/api/MavLinkMultirotorApi.hpp @@ -22,9 +22,9 @@ #include "common/CommonStructs.hpp" #include "common/VectorMath.hpp" #include "common/AirSimSettings.hpp" -#include "vehicles/multirotor//MultiRotor.hpp" -#include "vehicles/multirotor/controllers/DroneControllerBase.hpp" -#include "controllers/PidController.hpp" +#include "vehicles/multirotor/api/MultirotorApiBase.h" +#include "common/PidController.hpp" +#include "sensors/SensorCollection.hpp" //sensors #include "sensors/barometer/BarometerBase.hpp" @@ -36,7 +36,7 @@ namespace msr { namespace airlib { -class MavLinkDroneController : public DroneControllerBase +class MavLinkMultirotorApi : public MultirotorApiBase { public: typedef msr::airlib::GeoPoint GeoPoint; @@ -45,13 +45,12 @@ class MavLinkDroneController : public DroneControllerBase typedef msr::airlib::Quaternionr Quaternionr; typedef common_utils::Utils Utils; typedef msr::airlib::real_T real_T; - typedef msr::airlib::MultiRotor MultiRotor; typedef msr::airlib::AirSimSettings::MavLinkConnectionInfo MavLinkConnectionInfo; public: //required for pimpl - MavLinkDroneController(); - virtual ~MavLinkDroneController(); + MavLinkMultirotorApi(); + virtual ~MavLinkMultirotorApi(); //non-base interface specific to MavLinKDroneController void initialize(const MavLinkConnectionInfo& connection_info, const SensorCollection* sensors, bool is_simulation); @@ -62,21 +61,16 @@ class MavLinkDroneController : public DroneControllerBase void sendImage(unsigned char data[], uint32_t length, uint16_t width, uint16_t height); bool hasVideoRequest(); - //*** Start: VehicleControllerBase implementation ***// + //*** Start: VehicleApiBase implementation ***// virtual void reset() override; virtual void update() override; - virtual size_t getVertexCount() const override; - virtual real_T getVertexControlSignal(unsigned int rotor_index) const override; virtual void getStatusMessages(std::vector& messages) override; - virtual bool isAvailable(std::string& message) const override; virtual bool isApiControlEnabled() const override; virtual void enableApiControl(bool is_enabled) override; - virtual void setSimulationMode(bool is_set) override; - virtual Pose getDebugPose() const override; - //*** End: VehicleControllerBase implementation ***// + //*** End: VehicleApiBase implementation ***// - //*** Start: DroneControllerBase implementation ***// + //*** Start: MultirotorApiBase implementation ***// public: virtual Kinematics::State getKinematicsEstimated() const override; virtual Vector3r getPosition() const override; @@ -84,31 +78,28 @@ class MavLinkDroneController : public DroneControllerBase virtual Quaternionr getOrientation() const override; virtual LandedState getLandedState() const override; virtual RCData getRCData() const override; - virtual void setRCData(const RCData& rcData) override; - virtual bool armDisarm(bool arm, CancelableBase& cancelable_action) override; - virtual bool takeoff(float max_wait_seconds, CancelableBase& cancelable_action) override; - virtual bool land(float max_wait_seconds, CancelableBase& cancelable_action) override; - virtual bool goHome(CancelableBase& cancelable_action) override; - virtual bool hover(CancelableBase& cancelable_action) override; + virtual bool armDisarm(bool arm) override; + virtual bool takeoff(float max_wait_seconds) override; + virtual bool land(float max_wait_seconds) override; + virtual bool goHome() override; + virtual bool hover() override; virtual GeoPoint getHomeGeoPoint() const override; virtual GeoPoint getGpsLocation() const override; - virtual void reportTelemetry(float renderTime) override; + virtual void reportTelemetry(float renderTime); virtual float getCommandPeriod() const override; virtual float getTakeoffZ() const override; virtual float getDistanceAccuracy() const override; - virtual bool loopCommandPre() override; - virtual void loopCommandPost() override; protected: virtual void commandRollPitchZ(float pitch, float roll, float z, float yaw) override; virtual void commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) override; virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) override; virtual void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) override; virtual void commandPosition(float x, float y, float z, const YawMode& yaw_mode) override; - const VehicleParams& getVehicleParams() const override; - //*** End: DroneControllerBase implementation ***// + const MultirotorApiParams& getVehicleParams() const override; + //*** End: MultirotorApiBase implementation ***// private: //pimpl struct impl; @@ -133,1389 +124,1332 @@ class MavLinkLogViewerLog : public mavlinkcom::MavLinkLog std::shared_ptr proxy_; }; -struct MavLinkDroneController::impl { +struct MavLinkMultirotorApi::impl { public: - static const int pixhawkVendorId = 9900; ///< Vendor ID for Pixhawk board (V2 and V1) and PX4 Flow - static const int pixhawkFMUV4ProductId = 18; ///< Product ID for Pixhawk V2 board - static const int pixhawkFMUV2ProductId = 17; ///< Product ID for Pixhawk V2 board - static const int pixhawkFMUV2OldBootloaderProductId = 22; ///< Product ID for Bootloader on older Pixhawk V2 boards - static const int pixhawkFMUV1ProductId = 16; ///< Product ID for PX4 FMU V1 board - static const int RotorControlsCount = 8; - static const int messageReceivedTimeout = 10; ///< Seconds - - std::shared_ptr logviewer_proxy_, logviewer_out_proxy_, qgc_proxy_; - - size_t status_messages_MaxSize = 5000; - - std::shared_ptr hil_node_; - std::shared_ptr connection_; - std::shared_ptr video_server_; - std::shared_ptr mav_vehicle_control_; - - mavlinkcom::MavLinkAttPosMocap MocapPoseMessage; - mavlinkcom::MavLinkHeartbeat HeartbeatMessage; - mavlinkcom::MavLinkSetMode SetModeMessage; - mavlinkcom::MavLinkStatustext StatusTextMessage; - mavlinkcom::MavLinkHilControls HilControlsMessage; - mavlinkcom::MavLinkHilActuatorControls HilActuatorControlsMessage; - mavlinkcom::MavLinkCommandLong CommandLongMessage; - - mavlinkcom::MavLinkHilSensor last_sensor_message_; - mavlinkcom::MavLinkDistanceSensor last_distance_message_; - mavlinkcom::MavLinkHilGps last_gps_message_; - - std::mutex mocap_pose_mutex_, heartbeat_mutex_, set_mode_mutex_, status_text_mutex_, hil_controls_mutex_, last_message_mutex_; - MavLinkDroneController* parent_; - - impl(MavLinkDroneController* parent) + impl(MavLinkMultirotorApi* parent) : parent_(parent) { } - //variables required for VehicleControllerBase implementation - MavLinkConnectionInfo connection_info_; - bool is_any_heartbeat_, is_hil_mode_set_, is_armed_; - bool is_controls_0_1_; //Are motor controls specified in 0..1 or -1..1? - float rotor_controls_[RotorControlsCount]; - std::queue status_messages_; - int hil_state_freq_; - bool actuators_message_supported_; - const SensorCollection* sensors_; //this is optional - uint64_t last_gps_time_; - bool was_reset_; - Pose debug_pose_; - std::string is_available_message_; - bool is_available_; - - //additional variables required for DroneControllerBase implementation - //this is optional for methods that might not use vehicle commands - std::shared_ptr mav_vehicle_; - int state_version_; - mavlinkcom::VehicleState current_state; - float target_height_; - bool is_api_control_enabled_; - bool is_simulation_mode_; - PidController thrust_controller_; - common_utils::Timer hil_message_timer_; - common_utils::Timer sitl_message_timer_; - void initialize(const MavLinkConnectionInfo& connection_info, const SensorCollection* sensors, bool is_simulation) { connection_info_ = connection_info; sensors_ = sensors; is_simulation_mode_ = is_simulation; - try { - openAllConnections(); - is_available_ = true; - } - catch (std::exception& ex) { - is_available_ = false; - is_available_message_ = Utils::stringf("Failed to connect: %s", ex.what()); - } + openAllConnections(); } - bool isAvailable(std::string& message) const + MavLinkConnectionInfo getMavConnectionInfo() const { - if (!is_available_) - message = is_available_message_; - return is_available_; + return connection_info_; } - MavLinkConnectionInfo getMavConnectionInfo() const + //*** Start: VehicleApiBase implementation ***// + void reset() { - return connection_info_; + resetState(); + was_reset_ = true; + setNormalMode(); } - void normalizeRotorControls() + const ImuBase* getImu() const { - //if rotor controls are in not in 0-1 range then they are in -1 to 1 range in which case - //we normalize them to 0 to 1 for PX4 - if (!is_controls_0_1_) { - // change -1 to 1 to 0 to 1. - for (size_t i = 0; i < Utils::length(rotor_controls_); ++i) { - rotor_controls_[i] = (rotor_controls_[i] + 1.0f) / 2.0f; - } - } - else { - //this applies to PX4 and may work differently on other firmwares. - //We use 0.2 as idle rotors which leaves out range of 0.8 - for (size_t i = 0; i < Utils::length(rotor_controls_); ++i) { - rotor_controls_[i] = Utils::clip(0.8f * rotor_controls_[i] + 0.20f, 0.0f, 1.0f); - } - } + 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 DistanceBase* getDistance() const + { + return static_cast(sensors_->getByType(SensorBase::SensorType::Distance)); + } + const GpsBase* getGps() const + { + return static_cast(sensors_->getByType(SensorBase::SensorType::Gps)); } - void initializeMavSubscriptions() + void update() { - if (connection_ != nullptr && mav_vehicle_ != nullptr) { - is_any_heartbeat_ = false; - is_hil_mode_set_ = false; - is_armed_ = false; - is_controls_0_1_ = true; - Utils::setValue(rotor_controls_, 0.0f); - //TODO: main_node_->setMessageInterval(...); - connection_->subscribe([=](std::shared_ptr connection, const mavlinkcom::MavLinkMessage& msg) { - unused(connection); - processMavMessages(msg); - }); + if (sensors_ == nullptr || connection_ == nullptr || !connection_->isOpen()) + return; - // listen to the other mavlink connection also - auto mavcon = mav_vehicle_->getConnection(); - if (mavcon != connection_) { - mavcon->subscribe([=](std::shared_ptr connection, const mavlinkcom::MavLinkMessage& msg) { - unused(connection); - processMavMessages(msg); - }); - } - } - } + //send sensor updates + const auto& imu_output = getImu()->getOutput(); + const auto& mag_output = getMagnetometer()->getOutput(); + const auto& baro_output = getBarometer()->getOutput(); - bool sendTestMessage(std::shared_ptr node) { - try { - // try and send a test message. - mavlinkcom::MavLinkHeartbeat test; - test.autopilot = static_cast(mavlinkcom::MAV_AUTOPILOT::MAV_AUTOPILOT_PX4); - test.type = static_cast(mavlinkcom::MAV_TYPE::MAV_TYPE_GCS); - test.base_mode = 0; - test.custom_mode = 0; - test.mavlink_version = 3; - node->sendMessage(test); - test.system_status = 0; - return true; - } - catch (std::exception) { - return false; + sendHILSensor(imu_output.linear_acceleration, + imu_output.angular_velocity, + mag_output.magnetic_field_body, + baro_output.pressure * 0.01f /*Pa to Milibar */, baro_output.altitude); + + + const auto * distance = getDistance(); + if (distance) { + const auto& distance_output = distance->getOutput(); + float pitch, roll, yaw; + VectorMath::toEulerianAngle(distance_output.relative_pose.orientation, pitch, roll, yaw); + + sendDistanceSensor(distance_output.min_distance / 100, //m -> cm + distance_output.max_distance / 100, //m -> cm + distance_output.distance, + 0, //sensor type: //TODO: allow changing in settings? + 77, //sensor id, //TODO: should this be something real? + pitch); //TODO: convert from radians to degrees? } - } - bool connectToLogViewer() - { - //set up logviewer proxy - if (connection_info_.logviewer_ip_address.size() > 0) { - std::shared_ptr connection; - createProxy("LogViewer", connection_info_.logviewer_ip_address, connection_info_.logviewer_ip_port, connection_info_.local_host_ip, - logviewer_proxy_, connection); - if (!sendTestMessage(logviewer_proxy_)) { - // error talking to log viewer, so don't keep trying, and close the connection also. - logviewer_proxy_->getConnection()->close(); - logviewer_proxy_ = nullptr; - } + const auto gps = getGps(); + if (gps != nullptr) { + const auto& gps_output = gps->getOutput(); - std::shared_ptr out_connection; - createProxy("LogViewerOut", connection_info_.logviewer_ip_address, connection_info_.logviewer_ip_sport, connection_info_.local_host_ip, - logviewer_out_proxy_, out_connection); - if (!sendTestMessage(logviewer_out_proxy_)) { - // error talking to log viewer, so don't keep trying, and close the connection also. - logviewer_out_proxy_->getConnection()->close(); - logviewer_out_proxy_ = nullptr; - } - else if (mav_vehicle_ != nullptr) { - mav_vehicle_->getConnection()->startLoggingSendMessage(std::make_shared(logviewer_out_proxy_)); + //send GPS + if (gps_output.is_valid && gps_output.gnss.time_utc > last_gps_time_) { + last_gps_time_ = gps_output.gnss.time_utc; + Vector3r gps_velocity = gps_output.gnss.velocity; + Vector3r gps_velocity_xy = gps_velocity; + gps_velocity_xy.z() = 0; + float gps_cog; + if (Utils::isApproximatelyZero(gps_velocity.y(), 1E-2f) && Utils::isApproximatelyZero(gps_velocity.x(), 1E-2f)) + gps_cog = 0; + else + gps_cog = Utils::radiansToDegrees(atan2(gps_velocity.y(), gps_velocity.x())); + if (gps_cog < 0) + gps_cog += 360; + + sendHILGps(gps_output.gnss.geo_point, gps_velocity, gps_velocity_xy.norm(), gps_cog, + gps_output.gnss.eph, gps_output.gnss.epv, gps_output.gnss.fix_type, 10); } } - return logviewer_proxy_ != nullptr; + + //must be done at the end + if (was_reset_) + was_reset_ = false; } - bool connectToQGC() + void getStatusMessages(std::vector& messages) { - if (connection_info_.qgc_ip_address.size() > 0) { - std::shared_ptr connection; - createProxy("QGC", connection_info_.qgc_ip_address, connection_info_.qgc_ip_port, connection_info_.local_host_ip, qgc_proxy_, connection); - if (!sendTestMessage(qgc_proxy_)) { - // error talking to QGC, so don't keep trying, and close the connection also. - qgc_proxy_->getConnection()->close(); - qgc_proxy_ = nullptr; - } - else { - connection->subscribe([=](std::shared_ptr connection_val, const mavlinkcom::MavLinkMessage& msg) { - unused(connection_val); - processQgcMessages(msg); - }); - } + messages.clear(); + std::lock_guard guard(status_text_mutex_); + + while (!status_messages_.empty()) { + messages.push_back(status_messages_.front()); + status_messages_.pop(); } - return qgc_proxy_ != nullptr; } - - void createProxy(std::string name, std::string ip, int port, string local_host_ip, - std::shared_ptr& node, std::shared_ptr& connection) + void openAllConnections() { - if (connection_ == nullptr) - throw std::domain_error("MavLinkDroneController requires connection object to be set before createProxy call"); - - connection = mavlinkcom::MavLinkConnection::connectRemoteUdp("Proxy to: " + name + " at " + ip + ":" + std::to_string(port), local_host_ip, ip, port); - - // it is ok to reuse the simulator sysid and compid here because this node is only used to send a few messages directly to this endpoint - // and all other messages are funnelled through from PX4 via the Join method below. - node = std::make_shared(connection_info_.sim_sysid, connection_info_.sim_compid); - node->connect(connection); + close(); //just in case if connections were open + resetState(); //reset all variables we might have changed during last session - // now join the main connection to this one, this causes all PX4 messages to be sent to the proxy and all messages from the proxy will be - // send directly to the PX4 (using whatever sysid/compid comes from that remote node). - connection_->join(connection); + connect(); + connectToLogViewer(); + connectToQGC(); - auto mavcon = mav_vehicle_->getConnection(); - if (mavcon != connection_) { - mavcon->join(connection); - } } - - static std::string findPX4() + void closeAllConnection() { - auto result = mavlinkcom::MavLinkConnection::findSerialPorts(0, 0); - for (auto iter = result.begin(); iter != result.end(); iter++) - { - mavlinkcom::SerialPortInfo info = *iter; - if ( - ( - (info.vid == pixhawkVendorId) && - (info.pid == pixhawkFMUV4ProductId || info.pid == pixhawkFMUV2ProductId || info.pid == pixhawkFMUV2OldBootloaderProductId) - ) || - ( - (info.displayName.find(L"PX4_") != std::string::npos) - ) - ) - { - // printf("Auto Selecting COM port: %S\n", info.displayName.c_str()); - return std::string(info.portName.begin(), info.portName.end()); - } - } - return ""; + close(); } + //*** End: VehicleApiBase implementation ***// - void connect() + void getMocapPose(Vector3r& position, Quaternionr& orientation) { - createMavConnection(connection_info_); - initializeMavSubscriptions(); + position.x() = MocapPoseMessage.x; position.y() = MocapPoseMessage.y; position.z() = MocapPoseMessage.z; + orientation.w() = MocapPoseMessage.q[0]; orientation.x() = MocapPoseMessage.q[1]; + orientation.y() = MocapPoseMessage.q[2]; orientation.z() = MocapPoseMessage.q[3]; } - void createMavConnection(const MavLinkConnectionInfo& connection_info) + void sendCollision(float normalX, float normalY, float normalZ) { - if (connection_info.use_serial) { - createMavSerialConnection(connection_info.serial_port, connection_info.baud_rate); - } - else { - createMavUdpConnection(connection_info.ip_address, connection_info.ip_port); - } - //Uncomment below for sending images over MavLink - //connectToVideoServer(); + checkVehicle(); + + mavlinkcom::MavLinkCollision collision{}; + collision.src = 1; //provider of data is MavLink system in id field + collision.id = mav_vehicle_->getLocalSystemId(); + collision.action = static_cast(mavlinkcom::MAV_COLLISION_ACTION::MAV_COLLISION_ACTION_REPORT); + collision.threat_level = static_cast(mavlinkcom::MAV_COLLISION_THREAT_LEVEL::MAV_COLLISION_THREAT_LEVEL_NONE); + // we are abusing these fields, passing the angle of the object we hit, so that jMAVSim knows how to bounce off. + collision.time_to_minimum_delta = normalX; + collision.altitude_minimum_delta = normalY; + collision.horizontal_minimum_delta = normalZ; + mav_vehicle_->sendMessage(collision); } - void createMavUdpConnection(const std::string& ip, int port) + bool hasVideoRequest() { - close(); + mavlinkcom::MavLinkVideoServer::MavLinkVideoRequest image_req; + return video_server_->hasVideoRequest(image_req); + } - if (ip == "") { - throw std::invalid_argument("UdpIp setting is invalid."); - } + void sendImage(unsigned char data[], uint32_t length, uint16_t width, uint16_t height) + { + const int MAVLINK_DATA_STREAM_IMG_PNG = 6; + video_server_->sendFrame(data, length, width, height, MAVLINK_DATA_STREAM_IMG_PNG, 0); + } - if (port == 0) { - throw std::invalid_argument("UdpPort setting has an invalid value."); + void setNormalMode() + { + if (is_hil_mode_set_ && connection_ != nullptr && mav_vehicle_ != nullptr) { + + // remove MAV_MODE_FLAG_HIL_ENABLED flag from current mode + std::lock_guard guard(set_mode_mutex_); + int mode = mav_vehicle_->getVehicleState().mode; + mode &= ~static_cast(mavlinkcom::MAV_MODE_FLAG::MAV_MODE_FLAG_HIL_ENABLED); + + mavlinkcom::MavCmdDoSetMode cmd; + cmd.command = static_cast(mavlinkcom::MAV_CMD::MAV_CMD_DO_SET_MODE); + cmd.Mode = static_cast(mode); + mav_vehicle_->sendCommand(cmd); + + is_hil_mode_set_ = false; } + } - addStatusMessage(Utils::stringf("Connecting to UDP port %d, local IP %s, remote IP...", port, connection_info_.local_host_ip.c_str(), ip.c_str())); - connection_ = mavlinkcom::MavLinkConnection::connectRemoteUdp("hil", connection_info_.local_host_ip, ip, port); - hil_node_ = std::make_shared(connection_info_.sim_sysid, connection_info_.sim_compid); - hil_node_->connect(connection_); - addStatusMessage(std::string("Connected over UDP.")); + void setHILMode() + { + if (!is_simulation_mode_) + throw std::logic_error("Attempt to set device in HIL mode while not in simulation mode"); - mav_vehicle_ = std::make_shared(connection_info_.vehicle_sysid, connection_info_.vehicle_compid); - if (connection_info_.sitl_ip_address != "" && connection_info_.sitl_ip_port != 0 && connection_info_.sitl_ip_port != port) { - // bugbug: the PX4 SITL mode app cannot receive commands to control the drone over the same mavlink connection - // as the HIL_SENSOR messages, we must establish a separate mavlink channel for that so that DroneShell works. - addStatusMessage(Utils::stringf("Connecting to PX4 SITL UDP port %d, local IP %s, remote IP...", - connection_info_.sitl_ip_port, connection_info_.local_host_ip.c_str(), connection_info_.sitl_ip_address.c_str())); + checkVehicle(); - auto sitlconnection = mavlinkcom::MavLinkConnection::connectRemoteUdp("sitl", - connection_info_.local_host_ip, connection_info_.sitl_ip_address, connection_info_.sitl_ip_port); - mav_vehicle_->connect(sitlconnection); + // add MAV_MODE_FLAG_HIL_ENABLED flag to current mode + std::lock_guard guard(set_mode_mutex_); + int mode = mav_vehicle_->getVehicleState().mode; + mode |= static_cast(mavlinkcom::MAV_MODE_FLAG::MAV_MODE_FLAG_HIL_ENABLED); - addStatusMessage(std::string("Connected to SITL over UDP.")); - } - else { - mav_vehicle_->connect(connection_); - } + mavlinkcom::MavCmdDoSetMode cmd; + cmd.command = static_cast(mavlinkcom::MAV_CMD::MAV_CMD_DO_SET_MODE); + cmd.Mode = static_cast(mode); + mav_vehicle_->sendCommand(cmd); - mav_vehicle_->startHeartbeat(); + is_hil_mode_set_ = true; } - void createMavSerialConnection(const std::string& port_name, int baud_rate) + void close() { - close(); - - std::string port_name_auto = port_name; - if (port_name_auto == "" || port_name_auto == "*") { - port_name_auto = findPX4(); - if (port_name_auto == "") { - throw std::domain_error("Could not detect a connected PX4 flight controller on any USB ports. You can specify USB port in settings.json."); + if (connection_ != nullptr) { + if (is_hil_mode_set_ && mav_vehicle_ != nullptr) { + setNormalMode(); } + + connection_->close(); } - if (port_name_auto == "") { - throw std::invalid_argument("USB port for PX4 flight controller is empty. Please set it in settings.json."); + if (hil_node_ != nullptr) + hil_node_->close(); + + if (video_server_ != nullptr) + video_server_->close(); + + if (logviewer_proxy_ != nullptr) { + logviewer_proxy_->close(); + logviewer_proxy_ = nullptr; } - if (baud_rate == 0) { - throw std::invalid_argument("Baud rate specified in settings.json is 0 which is invalid"); + if (logviewer_out_proxy_ != nullptr) { + if (mav_vehicle_ != nullptr) { + mav_vehicle_->getConnection()->stopLoggingSendMessage(); + } + logviewer_out_proxy_->close(); + logviewer_out_proxy_ = nullptr; } - addStatusMessage(Utils::stringf("Connecting to PX4 over serial port: %s, baud rate %d ....", port_name_auto.c_str(), baud_rate)); - connection_ = mavlinkcom::MavLinkConnection::connectSerial("hil", port_name_auto, baud_rate); - connection_->ignoreMessage(mavlinkcom::MavLinkAttPosMocap::kMessageId); //TODO: find better way to communicate debug pose instead of using fake Mocap messages - hil_node_ = std::make_shared(connection_info_.sim_sysid, connection_info_.sim_compid); - hil_node_->connect(connection_); - addStatusMessage("Connected to PX4 over serial port."); + if (qgc_proxy_ != nullptr) { + qgc_proxy_->close(); + qgc_proxy_ = nullptr; + } + if (mav_vehicle_ != nullptr) { + mav_vehicle_->close(); + mav_vehicle_ = nullptr; + } + } - mav_vehicle_ = std::make_shared(connection_info_.vehicle_sysid, connection_info_.vehicle_compid); - mav_vehicle_->connect(connection_); // in this case we can use the same connection. - mav_vehicle_->startHeartbeat(); + //additional methods for MultirotorApiBase + void updateState() + { + StatusLock lock(parent_); + if (mav_vehicle_ != nullptr) { + int version = mav_vehicle_->getVehicleStateVersion(); + if (version != state_version_) + { + current_state = mav_vehicle_->getVehicleState(); + state_version_ = version; + } + } + } + + Kinematics::State getKinematicsEstimated() + { + updateState(); + Kinematics::State state; + //TODO: reduce code duplication below? + state.pose.position = Vector3r(current_state.local_est.pos.x, current_state.local_est.pos.y, current_state.local_est.pos.z); + state.pose.orientation = VectorMath::toQuaternion(current_state.attitude.pitch, current_state.attitude.roll, current_state.attitude.yaw); + state.twist.linear = Vector3r(current_state.local_est.lin_vel.x, current_state.local_est.lin_vel.y, current_state.local_est.lin_vel.z); + state.twist.angular = Vector3r(current_state.attitude.roll_rate, current_state.attitude.pitch_rate, current_state.attitude.yaw_rate); + state.pose.position = Vector3r(current_state.local_est.acc.x, current_state.local_est.acc.y, current_state.local_est.acc.z); + //TODO: how do we get angular acceleration? + return state; } - mavlinkcom::MavLinkHilSensor getLastSensorMessage() + Vector3r getPosition() { - std::lock_guard guard(last_message_mutex_); - return last_sensor_message_; + updateState(); + return Vector3r(current_state.local_est.pos.x, current_state.local_est.pos.y, current_state.local_est.pos.z); } - mavlinkcom::MavLinkDistanceSensor getLastDistanceMessage() + Vector3r getVelocity() { - std::lock_guard guard(last_message_mutex_); - return last_distance_message_; + updateState(); + return Vector3r(current_state.local_est.lin_vel.x, current_state.local_est.lin_vel.y, current_state.local_est.lin_vel.z); } - mavlinkcom::MavLinkHilGps getLastGpsMessage() + Quaternionr getOrientation() { - std::lock_guard guard(last_message_mutex_); - return last_gps_message_; + updateState(); + return VectorMath::toQuaternion(current_state.attitude.pitch, current_state.attitude.roll, current_state.attitude.yaw); } - void setArmed(bool armed) + GeoPoint getHomeGeoPoint() { - is_armed_ = armed; - if (!armed) { - //reset motor controls - for (size_t i = 0; i < Utils::length(rotor_controls_); ++i) { - rotor_controls_[i] = 0; - } - } + updateState(); + if (current_state.home.is_set) + return GeoPoint(current_state.home.global_pos.lat, current_state.home.global_pos.lon, current_state.home.global_pos.alt); + else + return GeoPoint(Utils::nan(), Utils::nan(), Utils::nan()); } - void processQgcMessages(const mavlinkcom::MavLinkMessage& msg) + GeoPoint getGpsLocation() { - if (msg.msgid == MocapPoseMessage.msgid) { - std::lock_guard guard(mocap_pose_mutex_); - MocapPoseMessage.decode(msg); - getMocapPose(debug_pose_.position, debug_pose_.orientation); - } - //else ignore message + updateState(); + return GeoPoint(current_state.global_est.pos.lat, current_state.global_est.pos.lon, current_state.global_est.pos.alt); } - void addStatusMessage(const std::string& message) + + LandedState getLandedState() { - std::lock_guard guard_status(status_text_mutex_); - //if queue became too large, clear it first - if (status_messages_.size() > status_messages_MaxSize) - Utils::clear(status_messages_, status_messages_MaxSize - status_messages_.size()); - status_messages_.push(message); + updateState(); + return current_state.controls.landed ? LandedState::Landed : LandedState::Flying; } - void processMavMessages(const mavlinkcom::MavLinkMessage& msg) + //administrative + + bool armDisarm(bool arm) { - if (msg.msgid == HeartbeatMessage.msgid) { - std::lock_guard guard_heartbeat(heartbeat_mutex_); + unused(arm); + checkVehicle(); + bool rc = false; + mav_vehicle_->armDisarm(arm).wait(10000, &rc); + return rc; + } - //TODO: have MavLinkNode track armed state so we don't have to re-decode message here again - HeartbeatMessage.decode(msg); - bool armed = (HeartbeatMessage.base_mode & static_cast(mavlinkcom::MAV_MODE_FLAG::MAV_MODE_FLAG_SAFETY_ARMED)) > 0; - setArmed(armed); - if (!is_any_heartbeat_) { - is_any_heartbeat_ = true; - if (HeartbeatMessage.autopilot == static_cast(mavlinkcom::MAV_AUTOPILOT::MAV_AUTOPILOT_PX4) && - HeartbeatMessage.type == static_cast(mavlinkcom::MAV_TYPE::MAV_TYPE_FIXED_WING)) { - // PX4 will scale fixed wing servo outputs to -1 to 1 - // and it scales multi rotor servo outpus to 0 to 1. - is_controls_0_1_ = false; - } - } else if (is_simulation_mode_ && !is_hil_mode_set_) { - setHILMode(); - } - } else if (msg.msgid == StatusTextMessage.msgid) { - StatusTextMessage.decode(msg); - //lock is established by below method - addStatusMessage(std::string(StatusTextMessage.text)); - } else if (msg.msgid == CommandLongMessage.msgid) { - CommandLongMessage.decode(msg); - if (CommandLongMessage.command == static_cast(mavlinkcom::MAV_CMD::MAV_CMD_SET_MESSAGE_INTERVAL)) { - int msg_id = static_cast(CommandLongMessage.param1 + 0.5); - if (msg_id == 115) { //HIL_STATE_QUATERNION - hil_state_freq_ = static_cast(CommandLongMessage.param2 + 0.5); - } - } - } else if (msg.msgid == HilControlsMessage.msgid) { - if (!actuators_message_supported_) { - std::lock_guard guard_controls(hil_controls_mutex_); - - HilControlsMessage.decode(msg); - //is_arned_ = (HilControlsMessage.mode & 128) > 0; //TODO: is this needed? - rotor_controls_[0] = HilControlsMessage.roll_ailerons; - rotor_controls_[1] = HilControlsMessage.pitch_elevator; - rotor_controls_[2] = HilControlsMessage.yaw_rudder; - rotor_controls_[3] = HilControlsMessage.throttle; - rotor_controls_[4] = HilControlsMessage.aux1; - rotor_controls_[5] = HilControlsMessage.aux2; - rotor_controls_[6] = HilControlsMessage.aux3; - rotor_controls_[7] = HilControlsMessage.aux4; + bool isApiControlEnabled() + { + return is_api_control_enabled_; + } - normalizeRotorControls(); - } + void enableApiControl(bool is_enabled) + { + checkVehicle(); + if (is_enabled) { + mav_vehicle_->requestControl(); + is_api_control_enabled_ = true; } - else if (msg.msgid == HilActuatorControlsMessage.msgid) { - actuators_message_supported_ = true; - - std::lock_guard guard_actuator(hil_controls_mutex_); //use same mutex as HIL_CONTROl - - HilActuatorControlsMessage.decode(msg); - //is_arned_ = (HilControlsMessage.mode & 128) > 0; //TODO: is this needed? - for (auto i = 0; i < 8; ++i) { - rotor_controls_[i] = HilActuatorControlsMessage.controls[i]; - } - normalizeRotorControls(); + else { + mav_vehicle_->releaseControl(); + is_api_control_enabled_ = false; } - //else ignore message } - void sendHILSensor(const Vector3r& acceleration, const Vector3r& gyro, const Vector3r& mag, float abs_pressure, float pressure_alt) + void setSimulationMode(bool is_set) { - if (!is_simulation_mode_) - throw std::logic_error("Attempt to send simulated sensor messages while not in simulation mode"); - - mavlinkcom::MavLinkHilSensor hil_sensor; - hil_sensor.time_usec = static_cast(Utils::getTimeSinceEpochNanos() / 1000.0); - - hil_sensor.xacc = acceleration.x(); - hil_sensor.yacc = acceleration.y(); - hil_sensor.zacc = acceleration.z(); - hil_sensor.xgyro = gyro.x(); - hil_sensor.ygyro = gyro.y(); - hil_sensor.zgyro = gyro.z(); - - hil_sensor.xmag = mag.x(); - hil_sensor.ymag = mag.y(); - hil_sensor.zmag = mag.z(); - - hil_sensor.abs_pressure = abs_pressure; - hil_sensor.pressure_alt = pressure_alt; - //TODO: enable temperature? diff_pressure - hil_sensor.fields_updated = was_reset_ ? (1 << 31) : 0; - - if (hil_node_ != nullptr) { - hil_node_->sendMessage(hil_sensor); + if (connection_ != nullptr) { + //TODO: can we do this? + throw std::domain_error("Cannot call setSimulationMode after connection was already established for MavLink"); } - std::lock_guard guard(last_message_mutex_); - last_sensor_message_ = hil_sensor; + is_simulation_mode_ = is_set; } - void sendDistanceSensor(float min_distance, float max_distance, float current_distance, float sensor_type, float sensor_id, float orientation) + bool takeoff(float max_wait_seconds) { - if (!is_simulation_mode_) - throw std::logic_error("Attempt to send simulated distance sensor messages while not in simulation mode"); - - mavlinkcom::MavLinkDistanceSensor distance_sensor; - distance_sensor.time_boot_ms = static_cast(Utils::getTimeSinceEpochNanos() / 1000000.0); - - distance_sensor.min_distance = static_cast(min_distance); - distance_sensor.max_distance = static_cast(max_distance); - distance_sensor.current_distance = static_cast(current_distance); - distance_sensor.type = static_cast(sensor_type); - distance_sensor.id = static_cast(sensor_id); - distance_sensor.orientation = static_cast(orientation); - //TODO: use covariance parameter? + checkVehicle(); - if (hil_node_ != nullptr) { - hil_node_->sendMessage(distance_sensor); + bool rc = false; + auto vec = getPosition(); + float z = vec.z() + getTakeoffZ(); + if (!mav_vehicle_->takeoff(z, 0.0f /* pitch */, 0.0f /* yaw */).wait(static_cast(max_wait_seconds * 1000), &rc)) + { + throw VehicleMoveException("TakeOff command - timeout waiting for response"); } + if (!rc) { + throw VehicleMoveException("TakeOff command rejected by drone"); + } + if (max_wait_seconds <= 0) + return true; // client doesn't want to wait. - std::lock_guard guard(last_message_mutex_); - last_distance_message_ = distance_sensor; + return parent_->waitForZ(max_wait_seconds, z, getDistanceAccuracy(), ); } - void sendHILGps(const GeoPoint& geo_point, const Vector3r& velocity, float velocity_xy, float cog, - float eph, float epv, int fix_type, unsigned int satellites_visible) + bool hover() { - if (!is_simulation_mode_) - throw std::logic_error("Attempt to send simulated GPS messages while not in simulation mode"); - - mavlinkcom::MavLinkHilGps hil_gps; - hil_gps.time_usec = static_cast(Utils::getTimeSinceEpochNanos() / 1000.0); - hil_gps.lat = static_cast(geo_point.latitude * 1E7); - hil_gps.lon = static_cast(geo_point.longitude* 1E7); - hil_gps.alt = static_cast(geo_point.altitude * 1000); - hil_gps.vn = static_cast(velocity.x() * 100); - hil_gps.ve = static_cast(velocity.y() * 100); - hil_gps.vd = static_cast(velocity.z() * 100); - hil_gps.eph = static_cast(eph * 100); - hil_gps.epv = static_cast(epv * 100); - hil_gps.fix_type = static_cast(fix_type); - hil_gps.vel = static_cast(velocity_xy * 100); - hil_gps.cog = static_cast(cog * 100); - hil_gps.satellites_visible = static_cast(satellites_visible); + bool rc = false; + checkVehicle(); + mavlinkcom::AsyncResult result = mav_vehicle_->loiter(); + //auto start_time = std::chrono::system_clock::now(); + while (!.isCancelled()) + { + if (result.wait(100, &rc)) + { + break; + } + } + return rc; + } - if (hil_node_ != nullptr) { - hil_node_->sendMessage(hil_gps); + bool land(float max_wait_seconds) + { + // bugbug: really need a downward pointing distance to ground sensor to do this properly, for now + // we assume the ground is relatively flat an we are landing roughly at the home altitude. + updateState(); + checkVehicle(); + if (current_state.home.is_set) + { + bool rc = false; + if (!mav_vehicle_->land(current_state.global_est.pos.lat, current_state.global_est.pos.lon, current_state.home.global_pos.alt).wait(10000, &rc)) + { + throw VehicleMoveException("Landing command - timeout waiting for response from drone"); + } + else if (!rc) { + throw VehicleMoveException("Landing command rejected by drone"); + } + } + else + { + throw VehicleMoveException("Cannot land safely with out a home position that tells us the home altitude. Could fix this if we hook up a distance to ground sensor..."); } - if (hil_gps.lat < 0.1f && hil_gps.lat > -0.1f) { - //Utils::DebugBreak(); - Utils::log("hil_gps.lat was too close to 0", Utils::kLogLevelError); + // Wait for landed state (or user cancelation) + if (!parent_->waitForFunction([&]() { + updateState(); + return current_state.controls.landed; + }, max_wait_seconds, )) + { + throw VehicleMoveException("Drone hasn't reported a landing state"); } + return true; + } - std::lock_guard guard(last_message_mutex_); - last_gps_message_ = hil_gps; + bool goHome() + { + checkVehicle(); + bool rc = false; + if (mav_vehicle_ != nullptr && !mav_vehicle_->returnToHome().wait(10000, &rc)) { + throw VehicleMoveException("goHome - timeout waiting for response from drone"); + } + return rc; } - real_T getVertexControlSignal(unsigned int rotor_index) + void commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) { - if (!is_simulation_mode_) - throw std::logic_error("Attempt to read simulated motor controls while not in simulation mode"); + checkVehicle(); + mav_vehicle_->moveByAttitude(roll, pitch, yaw_rate, 0, 0, 0, throttle); + } + void commandRollPitchZ(float pitch, float roll, float z, float yaw) + { + if (target_height_ != -z) { + // these PID values were calculated experimentally using AltHoldCommand n MavLinkTest, this provides the best + // control over thrust to achieve minimal over/under shoot in a reasonable amount of time, but it has not + // been tested on a real drone outside jMavSim, so it may need recalibrating... + thrust_controller_.setPoint(-z, .05f, .005f, 0.09f); + target_height_ = -z; + } + checkVehicle(); + auto state = mav_vehicle_->getVehicleState(); + float thrust = 0.21f + thrust_controller_.control(-state.local_est.pos.z); + mav_vehicle_->moveByAttitude(roll, pitch, yaw, 0, 0, 0, thrust); + } + void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) + { + checkVehicle(); + float yaw = yaw_mode.yaw_or_rate * M_PIf / 180; + mav_vehicle_->moveByLocalVelocity(vx, vy, vz, !yaw_mode.is_rate, yaw); + } + void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) + { + checkVehicle(); + float yaw = yaw_mode.yaw_or_rate * M_PIf / 180; + mav_vehicle_->moveByLocalVelocityWithAltHold(vx, vy, z, !yaw_mode.is_rate, yaw); + } + void commandPosition(float x, float y, float z, const YawMode& yaw_mode) + { + checkVehicle(); + float yaw = yaw_mode.yaw_or_rate * M_PIf / 180; + mav_vehicle_->moveToLocalPosition(x, y, z, !yaw_mode.is_rate, yaw); + } - std::lock_guard guard(hil_controls_mutex_); - return rotor_controls_[rotor_index]; + RCData getRCData() + { + throw VehicleCommandNotImplementedException("getRCData() function is not yet implemented"); } - void resetState() + void setRCData(const RCData& rcData) { - //reset state - is_any_heartbeat_ = is_hil_mode_set_ = is_armed_ = false; - is_controls_0_1_ = true; - hil_state_freq_ = -1; - actuators_message_supported_ = false; - last_gps_time_ = 0; - state_version_ = 0; - current_state = mavlinkcom::VehicleState(); - target_height_ = 0; - is_api_control_enabled_ = false; - thrust_controller_ = PidController(); - Utils::setValue(rotor_controls_, 0.0f); - was_reset_ = false; - debug_pose_ = Pose::nanPose(); + unused(rcData); + //TODO: use RC data to control MavLink drone } - //*** Start: VehicleControllerBase implementation ***// - void reset() + bool validateRCData(const RCData& rc_data) { - resetState(); - was_reset_ = true; - setNormalMode(); + unused(rc_data); + return true; } - const ImuBase* getImu() const + //drone parameters + float getCommandPeriod() { - return static_cast(sensors_->getByType(SensorBase::SensorType::Imu)); + return 1.0f / 50; //1 period of 50hz } - const MagnetometerBase* getMagnetometer() const + float getTakeoffZ() { - return static_cast(sensors_->getByType(SensorBase::SensorType::Magnetometer)); + // pick a number, PX4 doesn't have a fixed limit here, but 3 meters is probably safe + // enough to get out of the backwash turbulance. Negative due to NED coordinate system. + return -3.0f; } - const BarometerBase* getBarometer() const + float getDistanceAccuracy() { - return static_cast(sensors_->getByType(SensorBase::SensorType::Barometer)); + return 0.5f; //measured in simulator by firing commands "MoveToLocation -x 0 -y 0" multiple times and looking at distance travelled } - const DistanceBase* getDistance() const + const MultirotorApiParams& getVehicleParams() { - return static_cast(sensors_->getByType(SensorBase::SensorType::Distance)); + return getInternalVehicleParams(); //defaults are good for PX4 generic quadrocopter. } - const GpsBase* getGps() const + //TODO: decouple MultirotorApiBase, VehicalParams and SafetyEval + const MultirotorApiParams& getInternalVehicleParams() { - return static_cast(sensors_->getByType(SensorBase::SensorType::Gps)); + static const MultirotorApiParams vehicle_params_; + return vehicle_params_; //defaults are good for DJI Matrice 100 } - void update() + void reportTelemetry(float renderTime) { - if (sensors_ == nullptr || connection_ == nullptr || !connection_->isOpen()) + if (logviewer_proxy_ == nullptr || connection_ == nullptr || mav_vehicle_ == nullptr) { return; - - //send sensor updates - const auto& imu_output = getImu()->getOutput(); - const auto& mag_output = getMagnetometer()->getOutput(); - const auto& baro_output = getBarometer()->getOutput(); - - sendHILSensor(imu_output.linear_acceleration, - imu_output.angular_velocity, - mag_output.magnetic_field_body, - baro_output.pressure * 0.01f /*Pa to Milibar */, baro_output.altitude); - - - const auto * distance = getDistance(); - if (distance) { - const auto& distance_output = distance->getOutput(); - float pitch, roll, yaw; - VectorMath::toEulerianAngle(distance_output.relative_pose.orientation, pitch, roll, yaw); - - sendDistanceSensor(distance_output.min_distance / 100, //m -> cm - distance_output.max_distance / 100, //m -> cm - distance_output.distance, - 0, //sensor type: //TODO: allow changing in settings? - 77, //sensor id, //TODO: should this be something real? - pitch); //TODO: convert from radians to degrees? } - - const auto gps = getGps(); - if (gps != nullptr) { - const auto& gps_output = gps->getOutput(); - - //send GPS - if (gps_output.is_valid && gps_output.gnss.time_utc > last_gps_time_) { - last_gps_time_ = gps_output.gnss.time_utc; - Vector3r gps_velocity = gps_output.gnss.velocity; - Vector3r gps_velocity_xy = gps_velocity; - gps_velocity_xy.z() = 0; - float gps_cog; - if (Utils::isApproximatelyZero(gps_velocity.y(), 1E-2f) && Utils::isApproximatelyZero(gps_velocity.x(), 1E-2f)) - gps_cog = 0; - else - gps_cog = Utils::radiansToDegrees(atan2(gps_velocity.y(), gps_velocity.x())); - if (gps_cog < 0) - gps_cog += 360; - - sendHILGps(gps_output.gnss.geo_point, gps_velocity, gps_velocity_xy.norm(), gps_cog, - gps_output.gnss.eph, gps_output.gnss.epv, gps_output.gnss.fix_type, 10); + mavlinkcom::MavLinkTelemetry data; + connection_->getTelemetry(data); + if (data.messagesReceived == 0) { + if (!hil_message_timer_.started()) { + hil_message_timer_.start(); + } else if (hil_message_timer_.seconds() > messageReceivedTimeout) { + addStatusMessage("not receiving any messages from HIL, please restart your HIL node and try again"); } + } else { + hil_message_timer_.stop(); } - //must be done at the end - if (was_reset_) - was_reset_ = false; - } + // listen to the other mavlink connection also + auto mavcon = mav_vehicle_->getConnection(); + if (mavcon != connection_) { + mavlinkcom::MavLinkTelemetry sitl; + mavcon->getTelemetry(sitl); - void getStatusMessages(std::vector& messages) - { - messages.clear(); - std::lock_guard guard(status_text_mutex_); + data.handlerMicroseconds += sitl.handlerMicroseconds; + data.messagesHandled += sitl.messagesHandled; + data.messagesReceived += sitl.messagesReceived; + data.messagesSent += sitl.messagesSent; - while (!status_messages_.empty()) { - messages.push_back(status_messages_.front()); - status_messages_.pop(); + if (sitl.messagesReceived == 0) + { + if (!sitl_message_timer_.started()) { + sitl_message_timer_.start(); + } + else if (sitl_message_timer_.seconds() > messageReceivedTimeout) { + addStatusMessage("not receiving any messages from SITL, please restart your SITL node and try again"); + } + } + else { + sitl_message_timer_.stop(); + } } - } - - void openAllConnections() - { - close(); //just in case if connections were open - resetState(); //reset all variables we might have changed during last session - connect(); - connectToLogViewer(); - connectToQGC(); - - } - void closeAllConnection() - { - close(); - } - //*** End: VehicleControllerBase implementation ***// - - void getMocapPose(Vector3r& position, Quaternionr& orientation) - { - position.x() = MocapPoseMessage.x; position.y() = MocapPoseMessage.y; position.z() = MocapPoseMessage.z; - orientation.w() = MocapPoseMessage.q[0]; orientation.x() = MocapPoseMessage.q[1]; - orientation.y() = MocapPoseMessage.q[2]; orientation.z() = MocapPoseMessage.q[3]; + data.renderTime = static_cast(renderTime * 1000000);// microseconds + logviewer_proxy_->sendMessage(data); } - void sendCollision(float normalX, float normalY, float normalZ) + bool startOffboardMode() { checkVehicle(); - - mavlinkcom::MavLinkCollision collision{}; - collision.src = 1; //provider of data is MavLink system in id field - collision.id = mav_vehicle_->getLocalSystemId(); - collision.action = static_cast(mavlinkcom::MAV_COLLISION_ACTION::MAV_COLLISION_ACTION_REPORT); - collision.threat_level = static_cast(mavlinkcom::MAV_COLLISION_THREAT_LEVEL::MAV_COLLISION_THREAT_LEVEL_NONE); - // we are abusing these fields, passing the angle of the object we hit, so that jMAVSim knows how to bounce off. - collision.time_to_minimum_delta = normalX; - collision.altitude_minimum_delta = normalY; - collision.horizontal_minimum_delta = normalZ; - mav_vehicle_->sendMessage(collision); - } - - bool hasVideoRequest() - { - mavlinkcom::MavLinkVideoServer::MavLinkVideoRequest image_req; - return video_server_->hasVideoRequest(image_req); + try { + mav_vehicle_->requestControl(); + } + catch (std::exception& ex) { + ensureSafeMode(); + addStatusMessage(std::string("Request control failed: ") + ex.what()); + return false; + } + return true; } - void sendImage(unsigned char data[], uint32_t length, uint16_t width, uint16_t height) + void endOffboardMode() { - const int MAVLINK_DATA_STREAM_IMG_PNG = 6; - video_server_->sendFrame(data, length, width, height, MAVLINK_DATA_STREAM_IMG_PNG, 0); + // bugbug: I removed this releaseControl because it makes back-to-back move operations less smooth. + // The side effect of this is that with some drones (e.g. PX4 based) the drone itself will timeout + // when you stop sending move commands and the behavior on timeout is then determined by the drone itself. + // mav_vehicle_->releaseControl(); + ensureSafeMode(); } - void setNormalMode() + void ensureSafeMode() { - if (is_hil_mode_set_ && connection_ != nullptr && mav_vehicle_ != nullptr) { - - // remove MAV_MODE_FLAG_HIL_ENABLED flag from current mode - std::lock_guard guard(set_mode_mutex_); - int mode = mav_vehicle_->getVehicleState().mode; - mode &= ~static_cast(mavlinkcom::MAV_MODE_FLAG::MAV_MODE_FLAG_HIL_ENABLED); - - mavlinkcom::MavCmdDoSetMode cmd; - cmd.command = static_cast(mavlinkcom::MAV_CMD::MAV_CMD_DO_SET_MODE); - cmd.Mode = static_cast(mode); - mav_vehicle_->sendCommand(cmd); - - is_hil_mode_set_ = false; + if (mav_vehicle_ != nullptr) { + const mavlinkcom::VehicleState& state = mav_vehicle_->getVehicleState(); + if (state.controls.landed || !state.controls.armed) { + return; + } } } - void setHILMode() + bool loopCommandPre() { - if (!is_simulation_mode_) - throw std::logic_error("Attempt to set device in HIL mode while not in simulation mode"); - - - checkVehicle(); - - // add MAV_MODE_FLAG_HIL_ENABLED flag to current mode - std::lock_guard guard(set_mode_mutex_); - int mode = mav_vehicle_->getVehicleState().mode; - mode |= static_cast(mavlinkcom::MAV_MODE_FLAG::MAV_MODE_FLAG_HIL_ENABLED); + return startOffboardMode(); + } - mavlinkcom::MavCmdDoSetMode cmd; - cmd.command = static_cast(mavlinkcom::MAV_CMD::MAV_CMD_DO_SET_MODE); - cmd.Mode = static_cast(mode); - mav_vehicle_->sendCommand(cmd); + void loopCommandPost() + { + endOffboardMode(); + } - is_hil_mode_set_ = true; + void checkVehicle() { + if (mav_vehicle_ == nullptr) { + throw std::logic_error("Cannot perform operation when no vehicle is connected"); + } } - void close() +private: //methods + void normalizeRotorControls() { - if (connection_ != nullptr) { - if (is_hil_mode_set_ && mav_vehicle_ != nullptr) { - setNormalMode(); + //if rotor controls are in not in 0-1 range then they are in -1 to 1 range in which case + //we normalize them to 0 to 1 for PX4 + if (!is_controls_0_1_) { + // change -1 to 1 to 0 to 1. + for (size_t i = 0; i < Utils::length(rotor_controls_); ++i) { + rotor_controls_[i] = (rotor_controls_[i] + 1.0f) / 2.0f; } - - connection_->close(); } - - if (hil_node_ != nullptr) - hil_node_->close(); - - if (video_server_ != nullptr) - video_server_->close(); - - if (logviewer_proxy_ != nullptr) { - logviewer_proxy_->close(); - logviewer_proxy_ = nullptr; + else { + //this applies to PX4 and may work differently on other firmwares. + //We use 0.2 as idle rotors which leaves out range of 0.8 + for (size_t i = 0; i < Utils::length(rotor_controls_); ++i) { + rotor_controls_[i] = Utils::clip(0.8f * rotor_controls_[i] + 0.20f, 0.0f, 1.0f); + } } + } - if (logviewer_out_proxy_ != nullptr) { - if (mav_vehicle_ != nullptr) { - mav_vehicle_->getConnection()->stopLoggingSendMessage(); + void initializeMavSubscriptions() + { + if (connection_ != nullptr && mav_vehicle_ != nullptr) { + is_any_heartbeat_ = false; + is_hil_mode_set_ = false; + is_armed_ = false; + is_controls_0_1_ = true; + Utils::setValue(rotor_controls_, 0.0f); + //TODO: main_node_->setMessageInterval(...); + connection_->subscribe([=](std::shared_ptr connection, const mavlinkcom::MavLinkMessage& msg) { + unused(connection); + processMavMessages(msg); + }); + + // listen to the other mavlink connection also + auto mavcon = mav_vehicle_->getConnection(); + if (mavcon != connection_) { + mavcon->subscribe([=](std::shared_ptr connection, const mavlinkcom::MavLinkMessage& msg) { + unused(connection); + processMavMessages(msg); + }); } - logviewer_out_proxy_->close(); - logviewer_out_proxy_ = nullptr; } + } - if (qgc_proxy_ != nullptr) { - qgc_proxy_->close(); - qgc_proxy_ = nullptr; + bool sendTestMessage(std::shared_ptr node) { + try { + // try and send a test message. + mavlinkcom::MavLinkHeartbeat test; + test.autopilot = static_cast(mavlinkcom::MAV_AUTOPILOT::MAV_AUTOPILOT_PX4); + test.type = static_cast(mavlinkcom::MAV_TYPE::MAV_TYPE_GCS); + test.base_mode = 0; + test.custom_mode = 0; + test.mavlink_version = 3; + node->sendMessage(test); + test.system_status = 0; + return true; } - if (mav_vehicle_ != nullptr) { - mav_vehicle_->close(); - mav_vehicle_ = nullptr; + catch (std::exception) { + return false; } } - //additional methods for DroneControllerBase - void updateState() + bool connectToLogViewer() { - StatusLock lock(parent_); - if (mav_vehicle_ != nullptr) { - int version = mav_vehicle_->getVehicleStateVersion(); - if (version != state_version_) - { - current_state = mav_vehicle_->getVehicleState(); - state_version_ = version; + //set up logviewer proxy + if (connection_info_.logviewer_ip_address.size() > 0) { + std::shared_ptr connection; + createProxy("LogViewer", connection_info_.logviewer_ip_address, connection_info_.logviewer_ip_port, connection_info_.local_host_ip, + logviewer_proxy_, connection); + if (!sendTestMessage(logviewer_proxy_)) { + // error talking to log viewer, so don't keep trying, and close the connection also. + logviewer_proxy_->getConnection()->close(); + logviewer_proxy_ = nullptr; + } + + std::shared_ptr out_connection; + createProxy("LogViewerOut", connection_info_.logviewer_ip_address, connection_info_.logviewer_ip_sport, connection_info_.local_host_ip, + logviewer_out_proxy_, out_connection); + if (!sendTestMessage(logviewer_out_proxy_)) { + // error talking to log viewer, so don't keep trying, and close the connection also. + logviewer_out_proxy_->getConnection()->close(); + logviewer_out_proxy_ = nullptr; + } + else if (mav_vehicle_ != nullptr) { + mav_vehicle_->getConnection()->startLoggingSendMessage(std::make_shared(logviewer_out_proxy_)); } } - } - - Kinematics::State getKinematicsEstimated() - { - updateState(); - Kinematics::State state; - //TODO: reduce code duplication below? - state.pose.position = Vector3r(current_state.local_est.pos.x, current_state.local_est.pos.y, current_state.local_est.pos.z); - state.pose.orientation = VectorMath::toQuaternion(current_state.attitude.pitch, current_state.attitude.roll, current_state.attitude.yaw); - state.twist.linear = Vector3r(current_state.local_est.lin_vel.x, current_state.local_est.lin_vel.y, current_state.local_est.lin_vel.z); - state.twist.angular = Vector3r(current_state.attitude.roll_rate, current_state.attitude.pitch_rate, current_state.attitude.yaw_rate); - state.pose.position = Vector3r(current_state.local_est.acc.x, current_state.local_est.acc.y, current_state.local_est.acc.z); - //TODO: how do we get angular acceleration? - return state; + return logviewer_proxy_ != nullptr; } - Vector3r getPosition() + bool connectToQGC() { - updateState(); - return Vector3r(current_state.local_est.pos.x, current_state.local_est.pos.y, current_state.local_est.pos.z); + if (connection_info_.qgc_ip_address.size() > 0) { + std::shared_ptr connection; + createProxy("QGC", connection_info_.qgc_ip_address, connection_info_.qgc_ip_port, connection_info_.local_host_ip, qgc_proxy_, connection); + if (!sendTestMessage(qgc_proxy_)) { + // error talking to QGC, so don't keep trying, and close the connection also. + qgc_proxy_->getConnection()->close(); + qgc_proxy_ = nullptr; + } + else { + connection->subscribe([=](std::shared_ptr connection_val, const mavlinkcom::MavLinkMessage& msg) { + unused(connection_val); + processQgcMessages(msg); + }); + } + } + return qgc_proxy_ != nullptr; } - Vector3r getVelocity() - { - updateState(); - return Vector3r(current_state.local_est.lin_vel.x, current_state.local_est.lin_vel.y, current_state.local_est.lin_vel.z); - } - Quaternionr getOrientation() + void createProxy(std::string name, std::string ip, int port, string local_host_ip, + std::shared_ptr& node, std::shared_ptr& connection) { - updateState(); - return VectorMath::toQuaternion(current_state.attitude.pitch, current_state.attitude.roll, current_state.attitude.yaw); - } + if (connection_ == nullptr) + throw std::domain_error("MavLinkMultirotorApi requires connection object to be set before createProxy call"); - GeoPoint getHomeGeoPoint() - { - updateState(); - if (current_state.home.is_set) - return GeoPoint(current_state.home.global_pos.lat, current_state.home.global_pos.lon, current_state.home.global_pos.alt); - else - return GeoPoint(Utils::nan(), Utils::nan(), Utils::nan()); - } + connection = mavlinkcom::MavLinkConnection::connectRemoteUdp("Proxy to: " + name + " at " + ip + ":" + std::to_string(port), local_host_ip, ip, port); - GeoPoint getGpsLocation() - { - updateState(); - return GeoPoint(current_state.global_est.pos.lat, current_state.global_est.pos.lon, current_state.global_est.pos.alt); - } + // it is ok to reuse the simulator sysid and compid here because this node is only used to send a few messages directly to this endpoint + // and all other messages are funnelled through from PX4 via the Join method below. + node = std::make_shared(connection_info_.sim_sysid, connection_info_.sim_compid); + node->connect(connection); + // now join the main connection to this one, this causes all PX4 messages to be sent to the proxy and all messages from the proxy will be + // send directly to the PX4 (using whatever sysid/compid comes from that remote node). + connection_->join(connection); - LandedState getLandedState() - { - updateState(); - return current_state.controls.landed ? LandedState::Landed : LandedState::Flying; + auto mavcon = mav_vehicle_->getConnection(); + if (mavcon != connection_) { + mavcon->join(connection); + } } - //administrative - - bool armDisarm(bool arm, CancelableBase& cancelable_action) + static std::string findPX4() { - unused(arm); - unused(cancelable_action); - checkVehicle(); - bool rc = false; - mav_vehicle_->armDisarm(arm).wait(10000, &rc); - return rc; + auto result = mavlinkcom::MavLinkConnection::findSerialPorts(0, 0); + for (auto iter = result.begin(); iter != result.end(); iter++) + { + mavlinkcom::SerialPortInfo info = *iter; + if ( + ( + (info.vid == pixhawkVendorId) && + (info.pid == pixhawkFMUV4ProductId || info.pid == pixhawkFMUV2ProductId || info.pid == pixhawkFMUV2OldBootloaderProductId) + ) || + ( + (info.displayName.find(L"PX4_") != std::string::npos) + ) + ) + { + // printf("Auto Selecting COM port: %S\n", info.displayName.c_str()); + return std::string(info.portName.begin(), info.portName.end()); + } + } + return ""; } - bool isApiControlEnabled() + void connect() { - return is_api_control_enabled_; + createMavConnection(connection_info_); + initializeMavSubscriptions(); } - void enableApiControl(bool is_enabled) + void createMavConnection(const MavLinkConnectionInfo& connection_info) { - checkVehicle(); - if (is_enabled) { - mav_vehicle_->requestControl(); - is_api_control_enabled_ = true; + if (connection_info.use_serial) { + createMavSerialConnection(connection_info.serial_port, connection_info.baud_rate); } else { - mav_vehicle_->releaseControl(); - is_api_control_enabled_ = false; + createMavUdpConnection(connection_info.ip_address, connection_info.ip_port); } + //Uncomment below for sending images over MavLink + //connectToVideoServer(); } - void setSimulationMode(bool is_set) + void createMavUdpConnection(const std::string& ip, int port) { - if (connection_ != nullptr) { - //TODO: can we do this? - throw std::domain_error("Cannot call setSimulationMode after connection was already established for MavLink"); + close(); + + if (ip == "") { + throw std::invalid_argument("UdpIp setting is invalid."); } - is_simulation_mode_ = is_set; - } + if (port == 0) { + throw std::invalid_argument("UdpPort setting has an invalid value."); + } - bool takeoff(float max_wait_seconds, CancelableBase& cancelable_action) - { - unused(cancelable_action); - checkVehicle(); + addStatusMessage(Utils::stringf("Connecting to UDP port %d, local IP %s, remote IP...", port, connection_info_.local_host_ip.c_str(), ip.c_str())); + connection_ = mavlinkcom::MavLinkConnection::connectRemoteUdp("hil", connection_info_.local_host_ip, ip, port); + hil_node_ = std::make_shared(connection_info_.sim_sysid, connection_info_.sim_compid); + hil_node_->connect(connection_); + addStatusMessage(std::string("Connected over UDP.")); - bool rc = false; - auto vec = getPosition(); - float z = vec.z() + getTakeoffZ(); - if (!mav_vehicle_->takeoff(z, 0.0f /* pitch */, 0.0f /* yaw */).wait(static_cast(max_wait_seconds * 1000), &rc)) - { - throw VehicleMoveException("TakeOff command - timeout waiting for response"); + mav_vehicle_ = std::make_shared(connection_info_.vehicle_sysid, connection_info_.vehicle_compid); + + if (connection_info_.sitl_ip_address != "" && connection_info_.sitl_ip_port != 0 && connection_info_.sitl_ip_port != port) { + // bugbug: the PX4 SITL mode app cannot receive commands to control the drone over the same mavlink connection + // as the HIL_SENSOR messages, we must establish a separate mavlink channel for that so that DroneShell works. + addStatusMessage(Utils::stringf("Connecting to PX4 SITL UDP port %d, local IP %s, remote IP...", + connection_info_.sitl_ip_port, connection_info_.local_host_ip.c_str(), connection_info_.sitl_ip_address.c_str())); + + auto sitlconnection = mavlinkcom::MavLinkConnection::connectRemoteUdp("sitl", + connection_info_.local_host_ip, connection_info_.sitl_ip_address, connection_info_.sitl_ip_port); + mav_vehicle_->connect(sitlconnection); + + addStatusMessage(std::string("Connected to SITL over UDP.")); } - if (!rc) { - throw VehicleMoveException("TakeOff command rejected by drone"); + else { + mav_vehicle_->connect(connection_); } - if (max_wait_seconds <= 0) - return true; // client doesn't want to wait. - return parent_->waitForZ(max_wait_seconds, z, getDistanceAccuracy(), cancelable_action); + mav_vehicle_->startHeartbeat(); } - bool hover(CancelableBase& cancelable_action) + void createMavSerialConnection(const std::string& port_name, int baud_rate) { - bool rc = false; - checkVehicle(); - mavlinkcom::AsyncResult result = mav_vehicle_->loiter(); - //auto start_time = std::chrono::system_clock::now(); - while (!cancelable_action.isCancelled()) - { - if (result.wait(100, &rc)) - { - break; - } - } - return rc; - } + close(); - bool land(float max_wait_seconds, CancelableBase& cancelable_action) - { - unused(cancelable_action); - // bugbug: really need a downward pointing distance to ground sensor to do this properly, for now - // we assume the ground is relatively flat an we are landing roughly at the home altitude. - updateState(); - checkVehicle(); - if (current_state.home.is_set) - { - bool rc = false; - if (!mav_vehicle_->land(current_state.global_est.pos.lat, current_state.global_est.pos.lon, current_state.home.global_pos.alt).wait(10000, &rc)) - { - throw VehicleMoveException("Landing command - timeout waiting for response from drone"); - } - else if (!rc) { - throw VehicleMoveException("Landing command rejected by drone"); + std::string port_name_auto = port_name; + if (port_name_auto == "" || port_name_auto == "*") { + port_name_auto = findPX4(); + if (port_name_auto == "") { + throw std::domain_error("Could not detect a connected PX4 flight controller on any USB ports. You can specify USB port in settings.json."); } } - else - { - throw VehicleMoveException("Cannot land safely with out a home position that tells us the home altitude. Could fix this if we hook up a distance to ground sensor..."); + + if (port_name_auto == "") { + throw std::invalid_argument("USB port for PX4 flight controller is empty. Please set it in settings.json."); } - // Wait for landed state (or user cancelation) - if (!parent_->waitForFunction([&]() { - updateState(); - return current_state.controls.landed; - }, max_wait_seconds, cancelable_action)) - { - throw VehicleMoveException("Drone hasn't reported a landing state"); + if (baud_rate == 0) { + throw std::invalid_argument("Baud rate specified in settings.json is 0 which is invalid"); } - return true; + + addStatusMessage(Utils::stringf("Connecting to PX4 over serial port: %s, baud rate %d ....", port_name_auto.c_str(), baud_rate)); + connection_ = mavlinkcom::MavLinkConnection::connectSerial("hil", port_name_auto, baud_rate); + connection_->ignoreMessage(mavlinkcom::MavLinkAttPosMocap::kMessageId); //TODO: find better way to communicate debug pose instead of using fake Mocap messages + hil_node_ = std::make_shared(connection_info_.sim_sysid, connection_info_.sim_compid); + hil_node_->connect(connection_); + addStatusMessage("Connected to PX4 over serial port."); + + mav_vehicle_ = std::make_shared(connection_info_.vehicle_sysid, connection_info_.vehicle_compid); + mav_vehicle_->connect(connection_); // in this case we can use the same connection. + mav_vehicle_->startHeartbeat(); } - bool goHome(CancelableBase& cancelable_action) + mavlinkcom::MavLinkHilSensor getLastSensorMessage() { - unused(cancelable_action); - checkVehicle(); - bool rc = false; - if (mav_vehicle_ != nullptr && !mav_vehicle_->returnToHome().wait(10000, &rc)) { - throw VehicleMoveException("goHome - timeout waiting for response from drone"); - } - return rc; + std::lock_guard guard(last_message_mutex_); + return last_sensor_message_; } - void commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) + mavlinkcom::MavLinkDistanceSensor getLastDistanceMessage() { - checkVehicle(); - mav_vehicle_->moveByAttitude(roll, pitch, yaw_rate, 0, 0, 0, throttle); + std::lock_guard guard(last_message_mutex_); + return last_distance_message_; } - void commandRollPitchZ(float pitch, float roll, float z, float yaw) + + mavlinkcom::MavLinkHilGps getLastGpsMessage() { - if (target_height_ != -z) { - // these PID values were calculated experimentally using AltHoldCommand n MavLinkTest, this provides the best - // control over thrust to achieve minimal over/under shoot in a reasonable amount of time, but it has not - // been tested on a real drone outside jMavSim, so it may need recalibrating... - thrust_controller_.setPoint(-z, .05f, .005f, 0.09f); - target_height_ = -z; - } - checkVehicle(); - auto state = mav_vehicle_->getVehicleState(); - float thrust = 0.21f + thrust_controller_.control(-state.local_est.pos.z); - mav_vehicle_->moveByAttitude(roll, pitch, yaw, 0, 0, 0, thrust); + std::lock_guard guard(last_message_mutex_); + return last_gps_message_; } - void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) + + void setArmed(bool armed) { - checkVehicle(); - float yaw = yaw_mode.yaw_or_rate * M_PIf / 180; - mav_vehicle_->moveByLocalVelocity(vx, vy, vz, !yaw_mode.is_rate, yaw); + is_armed_ = armed; + if (!armed) { + //reset motor controls + for (size_t i = 0; i < Utils::length(rotor_controls_); ++i) { + rotor_controls_[i] = 0; + } + } } - void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) + + void processQgcMessages(const mavlinkcom::MavLinkMessage& msg) { - checkVehicle(); - float yaw = yaw_mode.yaw_or_rate * M_PIf / 180; - mav_vehicle_->moveByLocalVelocityWithAltHold(vx, vy, z, !yaw_mode.is_rate, yaw); + if (msg.msgid == MocapPoseMessage.msgid) { + std::lock_guard guard(mocap_pose_mutex_); + MocapPoseMessage.decode(msg); + getMocapPose(debug_pose_.position, debug_pose_.orientation); + } + //else ignore message } - void commandPosition(float x, float y, float z, const YawMode& yaw_mode) + + void addStatusMessage(const std::string& message) { - checkVehicle(); - float yaw = yaw_mode.yaw_or_rate * M_PIf / 180; - mav_vehicle_->moveToLocalPosition(x, y, z, !yaw_mode.is_rate, yaw); + std::lock_guard guard_status(status_text_mutex_); + //if queue became too large, clear it first + if (status_messages_.size() > status_messages_MaxSize) + Utils::clear(status_messages_, status_messages_MaxSize - status_messages_.size()); + status_messages_.push(message); } - RCData getRCData() + void processMavMessages(const mavlinkcom::MavLinkMessage& msg) { - throw VehicleCommandNotImplementedException("getRCData() function is not yet implemented"); + if (msg.msgid == HeartbeatMessage.msgid) { + std::lock_guard guard_heartbeat(heartbeat_mutex_); + + //TODO: have MavLinkNode track armed state so we don't have to re-decode message here again + HeartbeatMessage.decode(msg); + bool armed = (HeartbeatMessage.base_mode & static_cast(mavlinkcom::MAV_MODE_FLAG::MAV_MODE_FLAG_SAFETY_ARMED)) > 0; + setArmed(armed); + if (!is_any_heartbeat_) { + is_any_heartbeat_ = true; + if (HeartbeatMessage.autopilot == static_cast(mavlinkcom::MAV_AUTOPILOT::MAV_AUTOPILOT_PX4) && + HeartbeatMessage.type == static_cast(mavlinkcom::MAV_TYPE::MAV_TYPE_FIXED_WING)) { + // PX4 will scale fixed wing servo outputs to -1 to 1 + // and it scales multi rotor servo outpus to 0 to 1. + is_controls_0_1_ = false; + } + } else if (is_simulation_mode_ && !is_hil_mode_set_) { + setHILMode(); + } + } else if (msg.msgid == StatusTextMessage.msgid) { + StatusTextMessage.decode(msg); + //lock is established by below method + addStatusMessage(std::string(StatusTextMessage.text)); + } else if (msg.msgid == CommandLongMessage.msgid) { + CommandLongMessage.decode(msg); + if (CommandLongMessage.command == static_cast(mavlinkcom::MAV_CMD::MAV_CMD_SET_MESSAGE_INTERVAL)) { + int msg_id = static_cast(CommandLongMessage.param1 + 0.5); + if (msg_id == 115) { //HIL_STATE_QUATERNION + hil_state_freq_ = static_cast(CommandLongMessage.param2 + 0.5); + } + } + } else if (msg.msgid == HilControlsMessage.msgid) { + if (!actuators_message_supported_) { + std::lock_guard guard_controls(hil_controls_mutex_); + + HilControlsMessage.decode(msg); + //is_arned_ = (HilControlsMessage.mode & 128) > 0; //TODO: is this needed? + rotor_controls_[0] = HilControlsMessage.roll_ailerons; + rotor_controls_[1] = HilControlsMessage.pitch_elevator; + rotor_controls_[2] = HilControlsMessage.yaw_rudder; + rotor_controls_[3] = HilControlsMessage.throttle; + rotor_controls_[4] = HilControlsMessage.aux1; + rotor_controls_[5] = HilControlsMessage.aux2; + rotor_controls_[6] = HilControlsMessage.aux3; + rotor_controls_[7] = HilControlsMessage.aux4; + + normalizeRotorControls(); + } + } + else if (msg.msgid == HilActuatorControlsMessage.msgid) { + actuators_message_supported_ = true; + + std::lock_guard guard_actuator(hil_controls_mutex_); //use same mutex as HIL_CONTROl + + HilActuatorControlsMessage.decode(msg); + //is_arned_ = (HilControlsMessage.mode & 128) > 0; //TODO: is this needed? + for (auto i = 0; i < 8; ++i) { + rotor_controls_[i] = HilActuatorControlsMessage.controls[i]; + } + normalizeRotorControls(); + } + //else ignore message } - void setRCData(const RCData& rcData) - { - unused(rcData); - //TODO: use RC data to control MavLink drone - } + void sendHILSensor(const Vector3r& acceleration, const Vector3r& gyro, const Vector3r& mag, float abs_pressure, float pressure_alt) + { + if (!is_simulation_mode_) + throw std::logic_error("Attempt to send simulated sensor messages while not in simulation mode"); + + mavlinkcom::MavLinkHilSensor hil_sensor; + hil_sensor.time_usec = static_cast(Utils::getTimeSinceEpochNanos() / 1000.0); + + hil_sensor.xacc = acceleration.x(); + hil_sensor.yacc = acceleration.y(); + hil_sensor.zacc = acceleration.z(); + hil_sensor.xgyro = gyro.x(); + hil_sensor.ygyro = gyro.y(); + hil_sensor.zgyro = gyro.z(); + + hil_sensor.xmag = mag.x(); + hil_sensor.ymag = mag.y(); + hil_sensor.zmag = mag.z(); + + hil_sensor.abs_pressure = abs_pressure; + hil_sensor.pressure_alt = pressure_alt; + //TODO: enable temperature? diff_pressure + hil_sensor.fields_updated = was_reset_ ? (1 << 31) : 0; - bool validateRCData(const RCData& rc_data) - { - unused(rc_data); - return true; - } + if (hil_node_ != nullptr) { + hil_node_->sendMessage(hil_sensor); + } - //drone parameters - float getCommandPeriod() - { - return 1.0f / 50; //1 period of 50hz - } - float getTakeoffZ() - { - // pick a number, PX4 doesn't have a fixed limit here, but 3 meters is probably safe - // enough to get out of the backwash turbulance. Negative due to NED coordinate system. - return -3.0f; - } - float getDistanceAccuracy() - { - return 0.5f; //measured in simulator by firing commands "MoveToLocation -x 0 -y 0" multiple times and looking at distance travelled - } - const VehicleParams& getVehicleParams() - { - return getInternalVehicleParams(); //defaults are good for PX4 generic quadrocopter. - } - //TODO: decouple DroneControllerBase, VehicalParams and SafetyEval - const VehicleParams& getInternalVehicleParams() - { - static const VehicleParams vehicle_params_; - return vehicle_params_; //defaults are good for DJI Matrice 100 + std::lock_guard guard(last_message_mutex_); + last_sensor_message_ = hil_sensor; } - void reportTelemetry(float renderTime) + void sendDistanceSensor(float min_distance, float max_distance, float current_distance, float sensor_type, float sensor_id, float orientation) { - if (logviewer_proxy_ == nullptr || connection_ == nullptr || mav_vehicle_ == nullptr) { - return; - } - mavlinkcom::MavLinkTelemetry data; - connection_->getTelemetry(data); - if (data.messagesReceived == 0) { - if (!hil_message_timer_.started()) { - hil_message_timer_.start(); - } else if (hil_message_timer_.seconds() > messageReceivedTimeout) { - addStatusMessage("not receiving any messages from HIL, please restart your HIL node and try again"); - } - } else { - hil_message_timer_.stop(); - } + if (!is_simulation_mode_) + throw std::logic_error("Attempt to send simulated distance sensor messages while not in simulation mode"); - // listen to the other mavlink connection also - auto mavcon = mav_vehicle_->getConnection(); - if (mavcon != connection_) { - mavlinkcom::MavLinkTelemetry sitl; - mavcon->getTelemetry(sitl); + mavlinkcom::MavLinkDistanceSensor distance_sensor; + distance_sensor.time_boot_ms = static_cast(Utils::getTimeSinceEpochNanos() / 1000000.0); - data.handlerMicroseconds += sitl.handlerMicroseconds; - data.messagesHandled += sitl.messagesHandled; - data.messagesReceived += sitl.messagesReceived; - data.messagesSent += sitl.messagesSent; + distance_sensor.min_distance = static_cast(min_distance); + distance_sensor.max_distance = static_cast(max_distance); + distance_sensor.current_distance = static_cast(current_distance); + distance_sensor.type = static_cast(sensor_type); + distance_sensor.id = static_cast(sensor_id); + distance_sensor.orientation = static_cast(orientation); + //TODO: use covariance parameter? - if (sitl.messagesReceived == 0) - { - if (!sitl_message_timer_.started()) { - sitl_message_timer_.start(); - } - else if (sitl_message_timer_.seconds() > messageReceivedTimeout) { - addStatusMessage("not receiving any messages from SITL, please restart your SITL node and try again"); - } - } - else { - sitl_message_timer_.stop(); - } + if (hil_node_ != nullptr) { + hil_node_->sendMessage(distance_sensor); } - data.renderTime = static_cast(renderTime * 1000000);// microseconds - logviewer_proxy_->sendMessage(data); + std::lock_guard guard(last_message_mutex_); + last_distance_message_ = distance_sensor; } - Pose getDebugPose() + void sendHILGps(const GeoPoint& geo_point, const Vector3r& velocity, float velocity_xy, float cog, + float eph, float epv, int fix_type, unsigned int satellites_visible) { - std::lock_guard guard(mocap_pose_mutex_); - return debug_pose_; - } + if (!is_simulation_mode_) + throw std::logic_error("Attempt to send simulated GPS messages while not in simulation mode"); - bool startOffboardMode() - { - checkVehicle(); - try { - mav_vehicle_->requestControl(); + mavlinkcom::MavLinkHilGps hil_gps; + hil_gps.time_usec = static_cast(Utils::getTimeSinceEpochNanos() / 1000.0); + hil_gps.lat = static_cast(geo_point.latitude * 1E7); + hil_gps.lon = static_cast(geo_point.longitude* 1E7); + hil_gps.alt = static_cast(geo_point.altitude * 1000); + hil_gps.vn = static_cast(velocity.x() * 100); + hil_gps.ve = static_cast(velocity.y() * 100); + hil_gps.vd = static_cast(velocity.z() * 100); + hil_gps.eph = static_cast(eph * 100); + hil_gps.epv = static_cast(epv * 100); + hil_gps.fix_type = static_cast(fix_type); + hil_gps.vel = static_cast(velocity_xy * 100); + hil_gps.cog = static_cast(cog * 100); + hil_gps.satellites_visible = static_cast(satellites_visible); + + if (hil_node_ != nullptr) { + hil_node_->sendMessage(hil_gps); } - catch (std::exception& ex) { - ensureSafeMode(); - addStatusMessage(std::string("Request control failed: ") + ex.what()); - return false; + + if (hil_gps.lat < 0.1f && hil_gps.lat > -0.1f) { + //Utils::DebugBreak(); + Utils::log("hil_gps.lat was too close to 0", Utils::kLogLevelError); } - return true; - } - void endOffboardMode() - { - // bugbug: I removed this releaseControl because it makes back-to-back move operations less smooth. - // The side effect of this is that with some drones (e.g. PX4 based) the drone itself will timeout - // when you stop sending move commands and the behavior on timeout is then determined by the drone itself. - // mav_vehicle_->releaseControl(); - ensureSafeMode(); + std::lock_guard guard(last_message_mutex_); + last_gps_message_ = hil_gps; } - void ensureSafeMode() + real_T getVertexControlSignal(unsigned int rotor_index) { - if (mav_vehicle_ != nullptr) { - const mavlinkcom::VehicleState& state = mav_vehicle_->getVehicleState(); - if (state.controls.landed || !state.controls.armed) { - return; - } - } - } + if (!is_simulation_mode_) + throw std::logic_error("Attempt to read simulated motor controls while not in simulation mode"); - bool loopCommandPre() - { - return startOffboardMode(); + std::lock_guard guard(hil_controls_mutex_); + return rotor_controls_[rotor_index]; } - void loopCommandPost() + void resetState() { - endOffboardMode(); + //reset state + is_any_heartbeat_ = is_hil_mode_set_ = is_armed_ = false; + is_controls_0_1_ = true; + hil_state_freq_ = -1; + actuators_message_supported_ = false; + last_gps_time_ = 0; + state_version_ = 0; + current_state = mavlinkcom::VehicleState(); + target_height_ = 0; + is_api_control_enabled_ = false; + thrust_controller_ = PidController(); + Utils::setValue(rotor_controls_, 0.0f); + was_reset_ = false; + debug_pose_ = Pose::nanPose(); } - void checkVehicle() { - if (mav_vehicle_ == nullptr) { - throw std::logic_error("Cannot perform operation when no vehicle is connected"); - } - } + +private: //variables + static const int pixhawkVendorId = 9900; ///< Vendor ID for Pixhawk board (V2 and V1) and PX4 Flow + static const int pixhawkFMUV4ProductId = 18; ///< Product ID for Pixhawk V2 board + static const int pixhawkFMUV2ProductId = 17; ///< Product ID for Pixhawk V2 board + static const int pixhawkFMUV2OldBootloaderProductId = 22; ///< Product ID for Bootloader on older Pixhawk V2 boards + static const int pixhawkFMUV1ProductId = 16; ///< Product ID for PX4 FMU V1 board + static const int RotorControlsCount = 8; + static const int messageReceivedTimeout = 10; ///< Seconds + + std::shared_ptr logviewer_proxy_, logviewer_out_proxy_, qgc_proxy_; + + size_t status_messages_MaxSize = 5000; + + std::shared_ptr hil_node_; + std::shared_ptr connection_; + std::shared_ptr video_server_; + std::shared_ptr mav_vehicle_control_; + + mavlinkcom::MavLinkAttPosMocap MocapPoseMessage; + mavlinkcom::MavLinkHeartbeat HeartbeatMessage; + mavlinkcom::MavLinkSetMode SetModeMessage; + mavlinkcom::MavLinkStatustext StatusTextMessage; + mavlinkcom::MavLinkHilControls HilControlsMessage; + mavlinkcom::MavLinkHilActuatorControls HilActuatorControlsMessage; + mavlinkcom::MavLinkCommandLong CommandLongMessage; + + mavlinkcom::MavLinkHilSensor last_sensor_message_; + mavlinkcom::MavLinkDistanceSensor last_distance_message_; + mavlinkcom::MavLinkHilGps last_gps_message_; + + std::mutex mocap_pose_mutex_, heartbeat_mutex_, set_mode_mutex_, status_text_mutex_, hil_controls_mutex_, last_message_mutex_; + MavLinkMultirotorApi* parent_; + + //variables required for VehicleApiBase implementation + MavLinkConnectionInfo connection_info_; + bool is_any_heartbeat_, is_hil_mode_set_, is_armed_; + bool is_controls_0_1_; //Are motor controls specified in 0..1 or -1..1? + float rotor_controls_[RotorControlsCount]; + std::queue status_messages_; + int hil_state_freq_; + bool actuators_message_supported_; + const SensorCollection* sensors_; //this is optional + uint64_t last_gps_time_; + bool was_reset_; + Pose debug_pose_; + + //additional variables required for MultirotorApiBase implementation + //this is optional for methods that might not use vehicle commands + std::shared_ptr mav_vehicle_; + int state_version_; + mavlinkcom::VehicleState current_state; + float target_height_; + bool is_api_control_enabled_; + bool is_simulation_mode_; + PidController thrust_controller_; + common_utils::Timer hil_message_timer_; + common_utils::Timer sitl_message_timer_; + + }; //impl //empty constructor required for pimpl -MavLinkDroneController::MavLinkDroneController() +MavLinkMultirotorApi::MavLinkMultirotorApi() { pimpl_.reset(new impl(this)); } -MavLinkDroneController::~MavLinkDroneController() +MavLinkMultirotorApi::~MavLinkMultirotorApi() { pimpl_->closeAllConnection(); } -void MavLinkDroneController::initialize(const MavLinkConnectionInfo& connection_info, const SensorCollection* sensors, bool is_simulation) +void MavLinkMultirotorApi::initialize(const MavLinkConnectionInfo& connection_info, const SensorCollection* sensors, bool is_simulation) { pimpl_->initialize(connection_info, sensors, is_simulation); } -MavLinkDroneController::MavLinkConnectionInfo MavLinkDroneController::getMavConnectionInfo() const +MavLinkMultirotorApi::MavLinkConnectionInfo MavLinkMultirotorApi::getMavConnectionInfo() const { return pimpl_->getMavConnectionInfo(); } -void MavLinkDroneController::sendImage(unsigned char data[], uint32_t length, uint16_t width, uint16_t height) +void MavLinkMultirotorApi::sendImage(unsigned char data[], uint32_t length, uint16_t width, uint16_t height) { pimpl_->sendImage(data, length, width, height); } -bool MavLinkDroneController::hasVideoRequest() +bool MavLinkMultirotorApi::hasVideoRequest() { return pimpl_->hasVideoRequest(); } -std::string MavLinkDroneController::findPX4() +std::string MavLinkMultirotorApi::findPX4() { return impl::findPX4(); } -//*** Start: VehicleControllerBase implementation ***// -void MavLinkDroneController::reset() +//*** Start: VehicleApiBase implementation ***// +void MavLinkMultirotorApi::reset() { - DroneControllerBase::reset(); + MultirotorApiBase::reset(); pimpl_->reset(); } -void MavLinkDroneController::update() +void MavLinkMultirotorApi::update() { - DroneControllerBase::update(); + MultirotorApiBase::update(); pimpl_->update(); } -real_T MavLinkDroneController::getVertexControlSignal(unsigned int rotor_index) const -{ - return pimpl_->getVertexControlSignal(rotor_index); -} -size_t MavLinkDroneController::getVertexCount() const -{ - return impl::RotorControlsCount; -} -void MavLinkDroneController::getStatusMessages(std::vector& messages) + +void MavLinkMultirotorApi::getStatusMessages(std::vector& messages) { pimpl_->getStatusMessages(messages); } -bool MavLinkDroneController::isAvailable(std::string& message) const -{ - return pimpl_->isAvailable(message); -} -//*** End: VehicleControllerBase implementation ***// +//*** End: VehicleApiBase implementation ***// //DroneControlBase -Kinematics::State MavLinkDroneController::getKinematicsEstimated() const +Kinematics::State MavLinkMultirotorApi::getKinematicsEstimated() const { return pimpl_->getKinematicsEstimated(); } -Vector3r MavLinkDroneController::getPosition() const +Vector3r MavLinkMultirotorApi::getPosition() const { return pimpl_->getPosition(); } -Vector3r MavLinkDroneController::getVelocity() const +Vector3r MavLinkMultirotorApi::getVelocity() const { return pimpl_->getVelocity(); } -Quaternionr MavLinkDroneController::getOrientation() const +Quaternionr MavLinkMultirotorApi::getOrientation() const { return pimpl_->getOrientation(); } -GeoPoint MavLinkDroneController::getHomeGeoPoint() const +GeoPoint MavLinkMultirotorApi::getHomeGeoPoint() const { return pimpl_->getHomeGeoPoint(); } -GeoPoint MavLinkDroneController::getGpsLocation() const +GeoPoint MavLinkMultirotorApi::getGpsLocation() const { return pimpl_->getGpsLocation(); } -DroneControllerBase::LandedState MavLinkDroneController::getLandedState() const +LandedState MavLinkMultirotorApi::getLandedState() const { return pimpl_->getLandedState(); } //administrative -bool MavLinkDroneController::armDisarm(bool arm, CancelableBase& cancelable_action) +bool MavLinkMultirotorApi::armDisarm(bool arm) { - return pimpl_->armDisarm(arm, cancelable_action); + return pimpl_->armDisarm(arm); } -void MavLinkDroneController::enableApiControl(bool is_enabled) +void MavLinkMultirotorApi::enableApiControl(bool is_enabled) { pimpl_->enableApiControl(is_enabled); } -void MavLinkDroneController::setSimulationMode(bool is_set) -{ - pimpl_->setSimulationMode(is_set); -} -bool MavLinkDroneController::isApiControlEnabled() const +bool MavLinkMultirotorApi::isApiControlEnabled() const { return pimpl_->isApiControlEnabled(); } -bool MavLinkDroneController::takeoff(float max_wait_seconds, CancelableBase& cancelable_action) +bool MavLinkMultirotorApi::takeoff(float max_wait_seconds) { - return pimpl_->takeoff(max_wait_seconds, cancelable_action); + return pimpl_->takeoff(max_wait_seconds); } -bool MavLinkDroneController::hover(CancelableBase& cancelable_action) +bool MavLinkMultirotorApi::hover() { - return pimpl_->hover(cancelable_action); + return pimpl_->hover(); } -bool MavLinkDroneController::land(float max_wait_seconds, CancelableBase& cancelable_action) +bool MavLinkMultirotorApi::land(float max_wait_seconds) { - return pimpl_->land(max_wait_seconds, cancelable_action); + return pimpl_->land(max_wait_seconds); } -bool MavLinkDroneController::goHome(CancelableBase& cancelable_action) +bool MavLinkMultirotorApi::goHome() { - return pimpl_->goHome(cancelable_action); + return pimpl_->goHome(); } -void MavLinkDroneController::commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) +void MavLinkMultirotorApi::commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) { return pimpl_->commandRollPitchThrottle(pitch, roll, throttle, yaw_rate); } -void MavLinkDroneController::commandRollPitchZ(float pitch, float roll, float z, float yaw) +void MavLinkMultirotorApi::commandRollPitchZ(float pitch, float roll, float z, float yaw) { return pimpl_->commandRollPitchZ(pitch, roll, z, yaw); } -void MavLinkDroneController::commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) +void MavLinkMultirotorApi::commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) { return pimpl_->commandVelocity(vx, vy, vz, yaw_mode); } -void MavLinkDroneController::commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) +void MavLinkMultirotorApi::commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) { return pimpl_->commandVelocityZ(vx, vy, z, yaw_mode); } -void MavLinkDroneController::commandPosition(float x, float y, float z, const YawMode& yaw_mode) +void MavLinkMultirotorApi::commandPosition(float x, float y, float z, const YawMode& yaw_mode) { return pimpl_->commandPosition(x, y, z, yaw_mode); } -RCData MavLinkDroneController::getRCData() const +RCData MavLinkMultirotorApi::getRCData() const { return pimpl_->getRCData(); } -void MavLinkDroneController::setRCData(const RCData& rcData) -{ - return pimpl_->setRCData(rcData); -} - -bool MavLinkDroneController::loopCommandPre() -{ - return pimpl_->loopCommandPre(); -} - -void MavLinkDroneController::loopCommandPost() -{ - pimpl_->loopCommandPost(); -} //drone parameters -float MavLinkDroneController::getCommandPeriod() const +float MavLinkMultirotorApi::getCommandPeriod() const { return pimpl_->getCommandPeriod(); } -float MavLinkDroneController::getTakeoffZ() const +float MavLinkMultirotorApi::getTakeoffZ() const { return pimpl_->getTakeoffZ(); } -float MavLinkDroneController::getDistanceAccuracy() const +float MavLinkMultirotorApi::getDistanceAccuracy() const { return pimpl_->getDistanceAccuracy(); } -const VehicleParams& MavLinkDroneController::getVehicleParams() const +const MultirotorApiParams& MavLinkMultirotorApi::getVehicleParams() const { return pimpl_->getVehicleParams(); } -//TODO: decouple DroneControllerBase, VehicalParams and SafetyEval +//TODO: decouple MultirotorApiBase, VehicalParams and SafetyEval -void MavLinkDroneController::reportTelemetry(float renderTime) +void MavLinkMultirotorApi::reportTelemetry(float renderTime) { return pimpl_->reportTelemetry(renderTime); } -Pose MavLinkDroneController::getDebugPose() const -{ - return pimpl_->getDebugPose(); -} - - - }} //namespace #endif diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp deleted file mode 100644 index fd475b2c70..0000000000 --- a/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp +++ /dev/null @@ -1,228 +0,0 @@ -// Copyright (c) Microsoft Corporation. All rights reserved. -// Licensed under the MIT License. - -#ifndef air_DroneControlServer_hpp -#define air_DroneControlServer_hpp - -#include "common/Common.hpp" -#include "common/common_utils/WorkerThread.hpp" -#include "vehicles/multirotor/controllers/DroneControllerBase.hpp" -#include "controllers/VehicleConnectorBase.hpp" -#include "api/VehicleApiBase.hpp" -#include "common/Waiter.hpp" -#include -#include -#include - -using namespace msr::airlib; - -namespace msr { namespace airlib { - -class MultirotorApi : public VehicleApiBase { -public: - MultirotorApi(VehicleConnectorBase* vehicle) - : vehicle_(vehicle) - { - controller_ = static_cast(vehicle->getController()); - - //auto vehicle_params = controller_->getVehicleParams(); - //auto fence = std::make_shared(VectorMath::Vector3f(-1E10, -1E10, -1E10), VectorMath::Vector3f(1E10, 1E10, 1E10), vehicle_params.distance_accuracy); - //auto safety_eval = std::make_shared(vehicle_params, fence); - } - virtual ~MultirotorApi() = default; - - /************************* State APIs *********************************/ - MultirotorState getMultirotorState() const - { - MultirotorState state; - state.kinematics_estimated = controller_->getKinematicsEstimated(); - state.collision = controller_->getCollisionInfo(); - state.kinematics_true = vehicle_->getTrueKinematics(); - //TODO: add GPS health, accuracy in API - state.gps_location = getGpsLocation(); - state.timestamp = controller_->clock()->nowNanos(); - state.landed_state = controller_->getLandedState(); - state.rc_data = controller_->getRCData(); - controller_->getStatusMessages(state.controller_messages); - - return state; - } - - virtual GeoPoint getGpsLocation() const = 0; - - /******************* VehicleApiBase implementation ********************/ - virtual void cancelPendingTasks() override - { - token_.cancel(); - } - - //redefine from base - virtual void enableApiControl(bool is_enabled) override = 0; - - virtual CollisionInfo getCollisionInfo() const override - { - return controller_->getCollisionInfo(); - } - - - - - virtual vector simGetImages(const vector& requests) const override - { - vector responses; - ImageCaptureBase* image_capture = vehicle_->getImageCapture(); - image_capture->getImages(requests, responses); - return responses; - } - virtual vector simGetImage(uint8_t camera_id, ImageCaptureBase::ImageType image_type) const override - { - vector request = { ImageCaptureBase::ImageRequest(camera_id, image_type)}; - const vector& response = simGetImages(request); - if (response.size() > 0) - return response.at(0).image_data_uint8; - else - return vector(); - } - - virtual void simPrintLogMessage(const std::string& message, const std::string& message_param, unsigned char severity) override - { - vehicle_->printLogMessage(message, message_param, severity); - } - - virtual Pose simGetObjectPose(const std::string& actor_name) const override - { - return vehicle_->getActorPose(actor_name); - } - - virtual void simSetPose(const Pose& pose, bool ignore_collision) override - { - vehicle_->setPose(pose, ignore_collision); - } - virtual Pose simGetPose() const override - { - return vehicle_->getPose(); - } - - virtual bool simSetSegmentationObjectID(const std::string& mesh_name, int object_id, - bool is_name_regex = false) override - { - return vehicle_->setSegmentationObjectID(mesh_name, object_id, is_name_regex); - } - - virtual int simGetSegmentationObjectID(const std::string& mesh_name) const override - { - return vehicle_->getSegmentationObjectID(mesh_name); - } - - virtual CameraInfo getCameraInfo(int camera_id) const override - { - return vehicle_->getCameraInfo(camera_id); - } - - virtual void setCameraOrientation(int camera_id, const Quaternionr& orientation) override - { - vehicle_->setCameraOrientation(camera_id, orientation); - } - - virtual bool isApiControlEnabled() const override - { - return controller_->isApiControlEnabled(); - } - - virtual void reset() override - { - vehicle_->reset(); - } - - virtual void setRCData(const RCData& data) - { - controller_->setRCData(data); - } - - - /*** Implementation of CancelableBase ***/ - -private:// types - class CancelToken { - public: - CancelToken() - : is_cancelled_(false) - { - } - - void reset() - { - is_cancelled_ = false; - } - - bool isCancelled() const - { - return is_cancelled_; - } - - void cancel() - { - is_cancelled_ = true; - } - - bool sleep(double secs) - { - //We can pass duration directly to sleep_for however it is known that on - //some systems, sleep_for makes system call anyway even if passed duration - //is <= 0. This can cause 50us of delay due to context switch. - if (isCancelled()) { - return false; - } - - TTimePoint start = ClockFactory::get()->nowNanos(); - static constexpr std::chrono::duration MinSleepDuration(0); - - while (secs > 0 && !isCancelled() && - ClockFactory::get()->elapsedSince(start) < secs) { - - std::this_thread::sleep_for(MinSleepDuration); - } - - return !isCancelled(); - } - - std::mutex& getWaitMutex() - { - return wait_mutex_; - } - - private: - std::atomic is_cancelled_; - std::mutex wait_mutex_; - }; - - class SingleCall { - public: - SingleCall(CancelToken& token) - : token_(token) { - - if (!token_.getWaitMutex().try_lock()) { - token_.cancel(); - token_.getWaitMutex().lock(); - } - token_.reset(); - } - - ~SingleCall() - { - token_.reset(); - token_.getWaitMutex().unlock(); - } - - private: - CancelToken& token_; - }; - -private: //vars - VehicleConnectorBase* vehicle_ = nullptr; - DroneControllerBase* controller_ = nullptr; - CancelToken token_; -}; - -}} //namespace -#endif diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.h b/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.h new file mode 100644 index 0000000000..28f43619d1 --- /dev/null +++ b/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.h @@ -0,0 +1,265 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef air_DroneControlServer_hpp +#define air_DroneControlServer_hpp + +#include "common/Common.hpp" +#include "MultirotorCommon.hpp"; +#include "safety/SafetyEval.hpp" +#include "physics/Kinematics.hpp" +#include "api/VehicleApiBase.hpp" + +#include +#include +#include + +using namespace msr::airlib; + +namespace msr { namespace airlib { + +class MultirotorApiBase : public VehicleApiBase { + +public: //must be implemented + + /************************* low level move APIs *********************************/ + virtual void commandRollPitchZ(float pitch, float roll, float z, float yaw) = 0; + virtual void commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) = 0; + virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) = 0; + virtual void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) = 0; + virtual void commandPosition(float x, float y, float z, const YawMode& yaw_mode) = 0; + + /************************* State APIs *********************************/ + virtual Kinematics::State getKinematicsEstimated() const = 0; + virtual LandedState getLandedState() const = 0; + virtual GeoPoint getGpsLocation() const = 0; + virtual const MultirotorApiParams& getVehicleParams() const = 0; + virtual RCData getRCData() const = 0; + + /************************* basic config APIs *********************************/ + virtual float getCommandPeriod() const = 0; //time between two command required for drone in seconds + virtual float getTakeoffZ() const = 0; // the height above ground for the drone after successful takeoff (Z above ground is negative due to NED coordinate system). + //noise in difference of two position coordinates. This is not GPS or position accuracy which can be very low such as 1m. + //the difference between two position cancels out transitional errors. Typically this would be 0.1m or lower. + virtual float getDistanceAccuracy() const = 0; + +protected: //optional oveerides but recommanded, default values may work + virtual float getAutoLookahead(float velocity, float adaptive_lookahead, + float max_factor = 40, float min_factor = 30) const; + virtual float getObsAvoidanceVelocity(float risk_dist, float max_obs_avoidance_vel) const; + + +public: //these APIs uses above low level APIs + virtual ~MultirotorApiBase() = default; + + /************************* high level move APIs *********************************/ + //return value of these function is true if command was completed without intruption + virtual bool takeoff(float max_wait_seconds); + virtual bool land(float max_wait_seconds); + virtual bool goHome(); + virtual bool moveByAngleZ(float pitch, float roll, float z, float yaw, float duration); + /// Move by providing angles and throttles just lik ein RC + virtual bool moveByAngleThrottle(float pitch, float roll, float throttle, float yaw_rate, float duration); + virtual bool moveByVelocity(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); + virtual bool moveByVelocityZ(float vx, float vy, float z, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); + virtual bool moveOnPath(const vector& path, float velocity, DrivetrainType drivetrain, const YawMode& yaw_mode, + float lookahead, float adaptive_lookahead); + virtual bool moveToPosition(float x, float y, float z, float velocity, DrivetrainType drivetrain, + const YawMode& yaw_mode, float lookahead, float adaptive_lookahead); + virtual bool moveToZ(float z, float velocity, const YawMode& yaw_mode, + float lookahead, float adaptive_lookahead); + virtual bool moveByManual(float vx_max, float vy_max, float z_min, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); + virtual bool rotateToYaw(float yaw, float margin); + virtual bool rotateByYawRate(float yaw_rate, float duration); + virtual bool hover(); + virtual RCData estimateRCTrims(float trimduration = 1, float minCountForTrim = 10, float maxTrim = 100); + + /************************* Safety APIs *********************************/ + virtual void setSafetyEval(const shared_ptr safety_eval_ptr); + virtual bool setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy, + float obs_avoidance_vel, const Vector3r& origin, float xy_length, float max_z, float min_z); + + /************************* high level status APIs *********************************/ + MultirotorState getMultirotorState() const + { + MultirotorState state; + state.kinematics_estimated = getKinematicsEstimated(); + //TODO: add GPS health, accuracy in API + state.gps_location = getGpsLocation(); + state.timestamp = clock()->nowNanos(); + state.landed_state = getLandedState(); + state.rc_data = getRCData(); + + return state; + } + + /******************* Task management APis ********************/ + virtual void cancelPendingTasks() override + { + token_.cancel(); + } + + +public: //types + class UnsafeMoveException : public VehicleMoveException { + public: + const SafetyEval::EvalResult result; + + UnsafeMoveException(const SafetyEval::EvalResult result_val, const std::string& message = "") + : VehicleMoveException(message), result(result_val) + {} + }; + +protected: //types + class SingleCall { + public: + SingleCall(CancelToken& token) + : token_(token) { + + //if we can't get lock, cancel previous call + if (!token_.try_lock()) { + //TODO: should we worry about spurious failures in try_lock? + token_.cancel(); + token_.lock(); + } + + if (token_.getRecursionCount() == 1) + token_.reset(); + //else this is not the start of the call + } + + ~SingleCall() + { + if (token_.getRecursionCount() == 1) + token_.reset(); + //else this is not the end of the call + + token_.unlock(); + } + + private: + CancelToken& token_; + }; + + struct StatusLock { + StatusLock(MultirotorApiBase* api) + : lock_(api->status_mutex_) + { + } + + private: + std::lock_guard lock_; + }; + + + struct PathPosition { + uint seg_index; + float offset; + Vector3r position; + }; + + struct PathSegment { + Vector3r seg_normalized; + Vector3r seg; + float seg_length; + float seg_velocity; + float start_z; + float seg_path_length; + + PathSegment(const Vector3r& start, const Vector3r& end, float velocity, float path_length) + { + seg = end - start; + seg_length = seg.norm(); + seg_normalized = seg.normalized(); + start_z = start.z(); + seg_path_length = path_length; + + seg_velocity = velocity; + } + }; + + //RAII + class ObsStrategyChanger { + private: + shared_ptr safety_eval_ptr_; + SafetyEval::ObsAvoidanceStrategy old_strategy_; + public: + ObsStrategyChanger(shared_ptr safety_eval_ptr, SafetyEval::ObsAvoidanceStrategy new_startegy) + { + safety_eval_ptr_ = safety_eval_ptr; + old_strategy_ = safety_eval_ptr_->getObsAvoidanceStrategy(); + safety_eval_ptr_->setObsAvoidanceStrategy(new_startegy); + } + ~ObsStrategyChanger() + { + safety_eval_ptr_->setObsAvoidanceStrategy(old_strategy_); + } + }; + + +protected: //utility methods + typedef std::function WaitFunction; + + //*********************************safe wrapper around low level commands*************************************************** + virtual bool moveByVelocityInternal(float vx, float vy, float vz, const YawMode& yaw_mode); + virtual bool moveByVelocityZInternal(float vx, float vy, float z, const YawMode& yaw_mode); + virtual bool moveToPositionInternal(const Vector3r& dest, const YawMode& yaw_mode); + virtual bool moveByRollPitchZInternal(float pitch, float roll, float z, float yaw); + virtual bool moveByRollPitchThrottleInternal(float pitch, float roll, float throttle, float yaw_rate); + + /************* safety checks & emergency manuevers ************/ + virtual bool emergencyManeuverIfUnsafe(const SafetyEval::EvalResult& result); + virtual bool safetyCheckVelocity(const Vector3r& velocity); + virtual bool safetyCheckVelocityZ(float vx, float vy, float z); + virtual bool safetyCheckDestination(const Vector3r& dest_loc); + + /************* wait helpers ************/ + // helper function can wait for anything (as defined by the given function) up to the max_wait duration (in seconds). + // returns true if the wait function succeeded, or false if timeout occurred or the timeout is invalid. + bool waitForFunction(WaitFunction function, float max_wait); + + //useful for derived class to check after takeoff + bool waitForZ(float max_wait_seconds, float z, float margin); + + /************* other short hands ************/ + virtual Vector3r getPosition() const + { + return getKinematicsEstimated().pose.position; + } + virtual Vector3r getVelocity() const + { + return getKinematicsEstimated().twist.linear; + } + virtual Quaternionr getOrientation() const + { + return getKinematicsEstimated().pose.orientation; + } + ClockBase* clock() const + { + return ClockFactory::get(); + } + + + CancelToken& getCancelToken() + { + return token_; + } + +private: //methods + float setNextPathPosition(const vector& path, const vector& path_segs, + const PathPosition& cur_path_loc, float next_dist, PathPosition& next_path_loc); + void adjustYaw(const Vector3r& heading, DrivetrainType drivetrain, YawMode& yaw_mode); + void adjustYaw(float x, float y, DrivetrainType drivetrain, YawMode& yaw_mode); + void moveToPathPosition(const Vector3r& dest, float velocity, DrivetrainType drivetrain, /* pass by value */ YawMode yaw_mode, float last_z); + bool isYawWithinMargin(float yaw_target, float margin) const; + +private: //variables + CancelToken token_; + std::recursive_mutex status_mutex_; + RCData rc_data_trims_; + shared_ptr safety_eval_ptr_; + float obs_avoidance_vel_ = 0.5f; +}; + +}} //namespace +#endif diff --git a/AirLib/include/vehicles/multirotor/controllers/DroneCommon.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorCommon.hpp similarity index 83% rename from AirLib/include/vehicles/multirotor/controllers/DroneCommon.hpp rename to AirLib/include/vehicles/multirotor/api/MultirotorCommon.hpp index 8f5a7775d2..ae686bf88b 100644 --- a/AirLib/include/vehicles/multirotor/controllers/DroneCommon.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorCommon.hpp @@ -5,8 +5,7 @@ #include "common/CommonStructs.hpp" #include "physics/Kinematics.hpp" -namespace msr { namespace airlib { - +namespace msr { namespace airlib { enum class DrivetrainType { MaxDegreeOfFreedom = 0, @@ -45,8 +44,8 @@ struct YawMode { }; //properties of vehicle -struct VehicleParams { - VehicleParams(){}; +struct MultirotorApiParams { + MultirotorApiParams() {}; //what is the breaking distance for given velocity? //Below is just proportionality constant to convert from velocity to breaking distance float vel_to_breaking_dist = 0.5f; //ideally this should be 2X for very high speed but for testing we are keeping it 0.5 @@ -66,30 +65,27 @@ struct VehicleParams { //what is the +/-window we should check on obstacle map? //for example 2 means check from ticks -2 to 2 int obs_window = 0; -}; +}; struct MultirotorState { CollisionInfo collision; Kinematics::State kinematics_estimated; - Kinematics::State kinematics_true; GeoPoint gps_location; uint64_t timestamp; LandedState landed_state; RCData rc_data; - std::vector controller_messages; MultirotorState() {} - MultirotorState(const CollisionInfo& collision_val, const Kinematics::State& kinematics_true_val, + MultirotorState(const CollisionInfo& collision_val, const Kinematics::State& kinematics_true_val, const Kinematics::State& kinematics_estimated_val, const GeoPoint& gps_location_val, uint64_t timestamp_val, LandedState landed_state_val, const RCData& rc_data_val, const std::vector& controller_messages_val) - : collision(collision_val), kinematics_estimated(kinematics_estimated_val), - kinematics_true(kinematics_true_val), gps_location(gps_location_val), timestamp(timestamp_val), - landed_state(landed_state_val), rc_data(rc_data_val), controller_messages(controller_messages_val) + : collision(collision_val), kinematics_estimated(kinematics_estimated_val), + gps_location(gps_location_val), timestamp(timestamp_val), + landed_state(landed_state_val), rc_data(rc_data_val) { } }; }} //namespace - #endif \ No newline at end of file diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibAdapators.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibAdapators.hpp index a21305225d..792ea8f013 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibAdapators.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibAdapators.hpp @@ -7,8 +7,8 @@ #include "common/Common.hpp" #include "common/CommonStructs.hpp" #include "api/RpcLibAdapatorsBase.hpp" -#include "vehicles/multirotor/controllers/DroneCommon.hpp" -#include "vehicles/multirotor/controllers/DroneControllerBase.hpp" +#include "vehicles/multirotor/controllers/MultirotorCommon.hpp" +#include "vehicles/multirotor/controllers/MultirotorApiBase.h" #include "common/ImageCaptureBase.hpp" #include "safety/SafetyEval.hpp" #include "rpc/msgpack.hpp" diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp index a5efa58f8c..223851c71e 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp @@ -8,9 +8,9 @@ #include #include "common/CommonStructs.hpp" #include "common/ImageCaptureBase.hpp" -#include "vehicles/multirotor/controllers/DroneControllerBase.hpp" +#include "vehicles/multirotor/controllers/MultirotorApiBase.h" #include "api/RpcLibClientBase.hpp" -#include "vehicles/multirotor/controllers/DroneCommon.hpp" +#include "vehicles/multirotor/controllers/MultirotorCommon.hpp" namespace msr { namespace airlib { diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibServer.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibServer.hpp index e469d41f22..a7535b218a 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibServer.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibServer.hpp @@ -6,7 +6,7 @@ #include "common/Common.hpp" #include -#include "vehicles/multirotor/api/MultirotorApi.hpp" +#include "vehicles/multirotor/api/MultirotorApiBase.h" #include "api/RpcLibServerBase.hpp" @@ -14,11 +14,11 @@ namespace msr { namespace airlib { class MultirotorRpcLibServer : public RpcLibServerBase { public: - MultirotorRpcLibServer(SimModeApiBase* simmode_api, string server_address, uint16_t port = 41451); + MultirotorRpcLibServer(WorldSimApiBase* simmode_api, string server_address, uint16_t port = 41451); virtual ~MultirotorRpcLibServer(); private: - MultirotorApi* getDroneApi() const; + MultirotorApiBase* getDroneApi() const; }; }} //namespace diff --git a/AirLib/include/vehicles/multirotor/controllers/RealMultirotorConnector.hpp b/AirLib/include/vehicles/multirotor/api/RealMultirotorConnector.hpp similarity index 94% rename from AirLib/include/vehicles/multirotor/controllers/RealMultirotorConnector.hpp rename to AirLib/include/vehicles/multirotor/api/RealMultirotorConnector.hpp index 89af35c354..2f772a66d5 100644 --- a/AirLib/include/vehicles/multirotor/controllers/RealMultirotorConnector.hpp +++ b/AirLib/include/vehicles/multirotor/api/RealMultirotorConnector.hpp @@ -11,7 +11,7 @@ namespace msr { namespace airlib { class RealMultirotorConnector : public VehicleConnectorBase { public: - RealMultirotorConnector(VehicleControllerBase* controller) + RealMultirotorConnector(VehicleApiBase* controller) : controller_(controller) { } @@ -26,7 +26,7 @@ class RealMultirotorConnector : public VehicleConnectorBase unused(dt); } - virtual VehicleControllerBase* getController() override + virtual VehicleApiBase* getController() override { return controller_; } @@ -96,7 +96,7 @@ class RealMultirotorConnector : public VehicleConnectorBase } private: - VehicleControllerBase* controller_; + VehicleApiBase* controller_; }; diff --git a/AirLib/include/vehicles/multirotor/configs/Px4MultiRotor.hpp b/AirLib/include/vehicles/multirotor/configs/Px4MultiRotor.hpp index 29df63ae97..a7b2c3a677 100644 --- a/AirLib/include/vehicles/multirotor/configs/Px4MultiRotor.hpp +++ b/AirLib/include/vehicles/multirotor/configs/Px4MultiRotor.hpp @@ -4,7 +4,7 @@ #ifndef msr_airlib_vehicles_Px4MultiRotor_hpp #define msr_airlib_vehicles_Px4MultiRotor_hpp -#include "vehicles/multirotor/controllers/MavLinkDroneController.hpp" +#include "vehicles/multirotor/controllers/MavLinkMultirotorApi.hpp" #include "common/AirSimSettings.hpp" #include "sensors/SensorFactory.hpp" @@ -47,10 +47,10 @@ class Px4MultiRotor : public MultiRotorParams { return sensor_factory_->createSensor(sensor_type); } - virtual std::unique_ptr createController() override + virtual std::unique_ptr createController() override { - unique_ptr controller(new MavLinkDroneController()); - auto mav_controller = static_cast(controller.get()); + unique_ptr controller(new MavLinkMultirotorApi()); + auto mav_controller = static_cast(controller.get()); mav_controller->initialize(connection_info_, & getSensors(), true); return controller; @@ -244,14 +244,14 @@ class Px4MultiRotor : public MultiRotorParams { } - static const MavLinkDroneController::MavLinkConnectionInfo& getConnectionInfo(const AirSimSettings::PX4VehicleSetting& vehicle_setting) + static const MavLinkMultirotorApi::MavLinkConnectionInfo& getConnectionInfo(const AirSimSettings::PX4VehicleSetting& vehicle_setting) { return vehicle_setting.connection_info; } private: - MavLinkDroneController::MavLinkConnectionInfo connection_info_; + MavLinkMultirotorApi::MavLinkConnectionInfo connection_info_; std::shared_ptr sensor_factory_; }; diff --git a/AirLib/include/vehicles/multirotor/configs/RosFlightQuadX.hpp b/AirLib/include/vehicles/multirotor/configs/RosFlightQuadX.hpp index d741294584..f8796af879 100644 --- a/AirLib/include/vehicles/multirotor/configs/RosFlightQuadX.hpp +++ b/AirLib/include/vehicles/multirotor/configs/RosFlightQuadX.hpp @@ -65,9 +65,9 @@ class RosFlightQuadX : public MultiRotorParams { return sensor_factory_->createSensor(sensor_type); } - virtual std::unique_ptr createController() override + virtual std::unique_ptr createController() override { - return std::unique_ptr(new RosFlightDroneController(& getSensors(), this, + return std::unique_ptr(new RosFlightDroneController(& getSensors(), this, vehicle_setting_->rc.remote_control_id)); } diff --git a/AirLib/include/vehicles/multirotor/configs/SimpleFlightQuadX.hpp b/AirLib/include/vehicles/multirotor/configs/SimpleFlightQuadX.hpp index ab8dd5d287..6e77fcb1ad 100644 --- a/AirLib/include/vehicles/multirotor/configs/SimpleFlightQuadX.hpp +++ b/AirLib/include/vehicles/multirotor/configs/SimpleFlightQuadX.hpp @@ -60,9 +60,9 @@ class SimpleFlightQuadX : public MultiRotorParams { return sensor_factory_->createSensor(sensor_type); } - virtual std::unique_ptr createController() override + virtual std::unique_ptr createController() override { - return std::unique_ptr(new SimpleFlightDroneController(this, vehicle_setting_)); + return std::unique_ptr(new SimpleFlightDroneController(this, vehicle_setting_)); } diff --git a/AirLib/include/vehicles/multirotor/controllers/DroneControllerBase.hpp b/AirLib/include/vehicles/multirotor/controllers/DroneControllerBase.hpp deleted file mode 100644 index 6360e8abeb..0000000000 --- a/AirLib/include/vehicles/multirotor/controllers/DroneControllerBase.hpp +++ /dev/null @@ -1,354 +0,0 @@ -// Copyright (c) Microsoft Corporation. All rights reserved. -// Licensed under the MIT License. - -#ifndef air_DroneControlBase_hpp -#define air_DroneControlBase_hpp - - -#include "common/Common.hpp" -#include "controllers/VehicleControllerBase.hpp" -#include "common/common_utils/WorkerThread.hpp" -#include "common/Waiter.hpp" -#include "safety/SafetyEval.hpp" -#include "common/CommonStructs.hpp" -#include "DroneCommon.hpp" - -namespace msr { namespace airlib { - -/// DroneControllerBase represents a generic drone that can be controlled and queried for current state. -/// All control methods return a boolean where true means the command was completed successfully, and -/// false means the command was cancelled. -/// -/// Cancellable actions: all commands return a CancelableBase& cancelable_action object. -/// These objects can be used to cancel the command, or find out if the command has -/// been cancelled. Only one command can be active at a time, so the next command you -/// send will automatically and immediately cancel any previous command that is in progress. -/// Many movement actions also take a duration, and when that duration is up the drone -/// will automatically hover at whatever location it is at. -/// -/// NOTE: It is very important to understand the local position coordinate system. -/// All x,y,z values below are in North/East/Down NED coordinates. This means the -/// z values will be more negative the higher you go. Ground is zero, and then it is -/// negative from there up. This is the defacto standard in the drone world so it is -/// simpler to keep with that throughout the AirSim library. -/// -/// NOTE: all the movement commands, except hover, require you first call enableApiControl(true). If that succeeds -/// it means your application now has control over the drone and the user cannot control the drone unless they -/// flip a switch on their controller taking control back from your program. If the user takes control back -/// and you call one of these methods, an exception will be thrown telling your program that it no longer -/// has control. If you call enableApiControl(false) the drone will hover waiting for user RC input. - - -class DroneControllerBase : public VehicleControllerBase { -public: //types - class UnsafeMoveException : public VehicleMoveException { - public: - const SafetyEval::EvalResult result; - - UnsafeMoveException(const SafetyEval::EvalResult result_val, const std::string& message = "") - : VehicleMoveException(message), result(result_val) - {} - }; - - struct StatusLock { - StatusLock(DroneControllerBase* drone) - : drone_(drone), lock_(drone->status_mutex_) - { - unused(drone_); - } - - private: - DroneControllerBase* drone_; - std::lock_guard lock_; - }; - -public: //interface for outside world - /// The drone must be armed before it will fly. Set arm to true to arm the drone. - /// On some drones arming may cause the motors to spin on low throttle, this is normal. - /// Set arm to false to disarm the drone. This will disable the motors, so don't do that - /// unless the drone is on the ground! Arming the drone also sets the "home position" - /// This home position is local position x=0,y=0,z=0. You can also query what GPS location - /// that is via getHomeGeoPoint. - virtual bool armDisarm(bool arm, CancelableBase& cancelable_action) = 0; - - /// When armed you can tell the drone to takeoff. This will fly to a preset altitude (like 2.5 meters) - /// above the home position. Once the drone is safely in the air you can use other commands to fly from there. - /// If the drone is already flying takeoff will be ignored. Pass non-zer max_wait_seconds if you want the - /// method to also wait until the takeoff altitude is achieved. - virtual bool takeoff(float max_wait_seconds, CancelableBase& cancelable_action); - - /// At any point this command will disable offboard control and land the drone at the current GPS location. - /// How quickly the drone descends is up to the drone. Some models will descend slowly if they have no - /// lidar telling them how far it is to the ground, while others that can see the ground will descend more - /// quickly until they get near the ground. None of that behavior is defined in this API because it is - /// depends on what kind of hardware the drone has onboard. Pass non-zer max_wait_seconds if you want the - /// method to also wait until the drone reports it has landed, the timeout here is a bit tricky, depends - /// on how high you are and what the drone's configured descent velocity is. If you don't want to wait - /// pass zero. You can also periodically check getLandedState to see if it has landed. - virtual bool land(float max_wait_seconds, CancelableBase& cancelable_action); - - /// This command is a safety measure, at any point this command will cancel offboard control and send the - /// drone back to the launch point (or home position). Most drones are also configured to climb to a safe - /// altitude before doing that so they don't run into a tree on the way home. - virtual bool goHome(CancelableBase& cancelable_action); - - /// Move the drone by controlling the angles (or attitude) of the drone, if you set pitch, roll to zero - /// and z to the current z value then it is equivalent to a hover command. A little bit of pitch can - /// make the drone move forwards, a little bit of roll can make it move sideways. The yaw control can - /// make the drone spin around on the spot. The duration says how long you want to apply these settings - /// before reverting to a hover command. So you can say "fly forwards slowly for 1 second" using - /// moveByAngleZ(0.1, 0, z, yaw, 1, ...). The cancelable_action can be used to canel all actions. In fact, - /// every time you call another move* method you will automatically cancel any previous action that is - /// happening. - virtual bool moveByAngleZ(float pitch, float roll, float z, float yaw, float duration - , CancelableBase& cancelable_action); - - /// Move by providing angles and throttles just lik ein RC - virtual bool moveByAngleThrottle(float pitch, float roll, float throttle, float yaw_rate, float duration - , CancelableBase& cancelable_action); - - /// Move the drone by controlling the velocity vector of the drone. A little bit of vx can - /// make the drone move forwards, a little bit of vy can make it move sideways. A bit of vz can move - /// the drone up or down vertically. The yaw_mode can set a specific yaw target, or tell the drone - /// to move as a specified yaw rate. The yaw rate command is handy if you want to do a slow 360 - /// and capture a nice smooth panorama. The duration says how long you want to apply these settings - /// before reverting to a hover command. So you can say "fly forwards slowly for 1 second" using - /// moveByVelocity(0.1, 0, 0, 1, ...). The cancelable_action can be used to canel all actions. In fact, - /// every time you call another move* method you will automatically cancel any previous action that is - /// happening. - virtual bool moveByVelocity(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode, - CancelableBase& cancelable_action); - - - /// Move the drone by controlling the velocity x,y of the drone but with a fixed altitude z. A little bit of vx can - /// make the drone move forwards, a little bit of vy can make it move sideways. - /// The yaw_mode can set a specific yaw target, or tell the drone - /// to move as a specified yaw rate. The yaw rate command is handy if you want to do a slow 360 - /// and capture a nice smooth panorama. The duration says how long you want to apply these settings - /// before reverting to a hover command. So you can say "fly forwards slowly for 1 second" using - /// moveByVelocityZ(0.1, 0, z, 1, ...). The cancelable_action can be used to canel all actions. In fact, - /// every time you call another move* method you will automatically cancel any previous action that is - /// happening. - virtual bool moveByVelocityZ(float vx, float vy, float z, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode, - CancelableBase& cancelable_action); - - /// Move the drone along the given path at the given speed and yaw. The lookahead argument will smooth this path - /// by looking ahead from current location by a given number of meters, then it will try and move the drone to - /// that lookahead position, thereby smoothing any corners in the path. The lookahead can also ensure the drone - /// doesn't stop and start at each vertex along the path. - virtual bool moveOnPath(const vector& path, float velocity, DrivetrainType drivetrain, const YawMode& yaw_mode, - float lookahead, float adaptive_lookahead, CancelableBase& cancelable_action); - - - /// Move the drone to the absolution x, y, z local positions at given speed and yaw. Remember z is negative. - /// Positive z is under ground. Instead of moving to the yaw before starting, the drone will move to the yaw position - /// smoothly as it goes, which means for short paths it may not reach that target heading until it has already - /// reached the end point at which time the drone will continue rotating until it reaches the desired heading. - /// The lookahead argument will smooth the path that moves the drone from it's current position to the target - /// postion by looking ahead from current location by a given number of meters, it keeps doing this iteratively - /// until it reaches the target position. This ensures a smoother flight. - virtual bool moveToPosition(float x, float y, float z, float velocity, DrivetrainType drivetrain, - const YawMode& yaw_mode, float lookahead, float adaptive_lookahead, CancelableBase& cancelable_action); - - /// moveToZ is a shortcut for moveToPosition at the current x, y location. - virtual bool moveToZ(float z, float velocity, const YawMode& yaw_mode, - float lookahead, float adaptive_lookahead, CancelableBase& cancelable_action); - - virtual bool moveByManual(float vx_max, float vy_max, float z_min, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode, CancelableBase& cancelable_action); - - /// Rotate the drone to the specified fixed heading (yaw) while remaining stationery at the current x, y, and z. - virtual bool rotateToYaw(float yaw, float margin, CancelableBase& cancelable_action); - - /// Rotate the drone to the specified yaw rate while remaining stationery at the current x, y, and z. - /// bugbug: why isn't it just rotate(yaw_mode) ? - virtual bool rotateByYawRate(float yaw_rate, float duration, CancelableBase& cancelable_action); - - /// Hover at the current x, y, and z. If the drone is moving when this is called, it will try - /// and move back to the location it was at when this command was received and hover there. - virtual bool hover(CancelableBase& cancelable_action); - - /// get state from estimator - virtual Kinematics::State getKinematicsEstimated() const = 0; - - /// get the current local position in NED coordinate (x=North/y=East,z=Down) so z is negative. - virtual Vector3r getPosition() const = 0; - - - /// Get the current velocity of the drone - virtual Vector3r getVelocity() const = 0; - - /// Get the current orientation (or attitude) of the drone as a Quaternion. - virtual Quaternionr getOrientation() const = 0; - - /// Get debug pose, meaning of which is dependent on application usage. For example, - /// this could be pose of real vehicle from log playback. - virtual Pose getDebugPose() const; - - /// get the current X and Y position - Vector2r getPositionXY() const; - - /// Get the Z position (z starts at zero on the ground, and becomes more and more negative as you go up) - float getZ() const; - - /// Get current state of the drone, is it landed or in the air - virtual LandedState getLandedState() const = 0; - - /// Assigned remote control to use for this controller, - /// -1 = onboard RC, 0+ = joystick ID available on OS - virtual int getRemoteControlID() const { return -1; } - - /// Get the current RC inputs when RC transmitter is talking to to flight controller - virtual RCData getRCData() const = 0; - - virtual RCData estimateRCTrims(CancelableBase& cancelable_action, float trimduration = 1, float minCountForTrim = 10, float maxTrim = 100); - - /// Set the RC data that should be used by flight controller - virtual void setRCData(const RCData& rcData) = 0; - - - /// Get the current GPS location of the drone. - - - //below are for passing information from simulator to API layer - //in non simulation mode default would be no collision unless - //controller implements otherwise. - virtual CollisionInfo getCollisionInfo() const; - virtual void setCollisionInfo(const CollisionInfo& collision_info); - - //safety settings - virtual void setSafetyEval(const shared_ptr safety_eval_ptr); - virtual bool setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy, - float obs_avoidance_vel, const Vector3r& origin, float xy_length, float max_z, float min_z); - virtual const VehicleParams& getVehicleParams() const = 0; - - //*********************************common pre & post for move commands*************************************************** - //TODO: make these protected - virtual bool loopCommandPre(); - virtual void loopCommandPost(); - //*********************************common pre & post for move commands*************************************************** - - DroneControllerBase() = default; - virtual ~DroneControllerBase() = default; - -protected: //must implement interface by derived class - //low level commands - //all angles in degrees, lengths in meters, velocities in m/s, durations in seconds - //all coordinates systems are world NED (+x is North, +y is East, +z is down) - virtual void commandRollPitchZ(float pitch, float roll, float z, float yaw) = 0; - virtual void commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) = 0; - virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) = 0; - virtual void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) = 0; - virtual void commandPosition(float x, float y, float z, const YawMode& yaw_mode) = 0; - - //config commands - virtual float getCommandPeriod() const = 0; //time between two command required for drone in seconds - virtual float getTakeoffZ() const = 0; // the height above ground for the drone after successful takeoff (Z above ground is negative due to NED coordinate system). - //noise in difference of two position coordinates. This is not GPS or position accuracy which can be very low such as 1m. - //the difference between two position cancels out transitional errors. Typically this would be 0.1m or lower. - virtual float getDistanceAccuracy() const = 0; - -protected: //optional oveerides recommanded for any drones, default implementation may work - virtual float getAutoLookahead(float velocity, float adaptive_lookahead, - float max_factor = 40, float min_factor = 30) const; - virtual float getObsAvoidanceVelocity(float risk_dist, float max_obs_avoidance_vel) const; - -protected: //utility functions and data members for derived classes - typedef std::function WaitFunction; - - // helper function can wait for anything (as defined by the given function) up to the max_wait duration (in seconds). - // returns true if the wait function succeeded, or false if timeout occurred or the timeout is invalid. - virtual bool waitForFunction(WaitFunction function, float max_wait, CancelableBase& cancelable_action); - - //useful for derived class to check after takeoff - virtual bool waitForZ(float max_wait_seconds, float z, float margin, CancelableBase& cancelable_action); - - - //*********************************safe wrapper around low level commands*************************************************** - virtual bool moveByVelocity(float vx, float vy, float vz, const YawMode& yaw_mode); - virtual bool moveByVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode); - virtual bool moveToPosition(const Vector3r& dest, const YawMode& yaw_mode); - virtual bool moveByRollPitchZ(float pitch, float roll, float z, float yaw); - virtual bool moveByRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate); - //**************************************************************************************************************************** - - /************* safety checks & emergency manuevers ************/ - virtual bool emergencyManeuverIfUnsafe(const SafetyEval::EvalResult& result); - virtual bool safetyCheckVelocity(const Vector3r& velocity); - virtual bool safetyCheckVelocityZ(float vx, float vy, float z); - virtual bool safetyCheckDestination(const Vector3r& dest_loc); - /************* safety checks & emergency manuevers ************/ - - void logHomePoint(); - -private: //types - struct PathPosition { - uint seg_index; - float offset; - Vector3r position; - }; - - struct PathSegment { - Vector3r seg_normalized; - Vector3r seg; - float seg_length; - float seg_velocity; - float start_z; - float seg_path_length; - - PathSegment(const Vector3r& start, const Vector3r& end, float velocity, float path_length) - { - seg = end - start; - seg_length = seg.norm(); - seg_normalized = seg.normalized(); - start_z = start.z(); - seg_path_length = path_length; - - seg_velocity = velocity; - } - }; - - //RAII - class ObsStrategyChanger { - private: - shared_ptr safety_eval_ptr_; - SafetyEval::ObsAvoidanceStrategy old_strategy_; - public: - ObsStrategyChanger(shared_ptr safety_eval_ptr, SafetyEval::ObsAvoidanceStrategy new_startegy) - { - safety_eval_ptr_ = safety_eval_ptr; - old_strategy_ = safety_eval_ptr_->getObsAvoidanceStrategy(); - safety_eval_ptr_->setObsAvoidanceStrategy(new_startegy); - } - ~ObsStrategyChanger() - { - safety_eval_ptr_->setObsAvoidanceStrategy(old_strategy_); - } - }; - - RCData rc_data_trims_; - -private: //methods - float setNextPathPosition(const vector& path, const vector& path_segs, - const PathPosition& cur_path_loc, float next_dist, PathPosition& next_path_loc); - - void adjustYaw(const Vector3r& heading, DrivetrainType drivetrain, YawMode& yaw_mode); - - void adjustYaw(float x, float y, DrivetrainType drivetrain, YawMode& yaw_mode); - - void moveToPathPosition(const Vector3r& dest, float velocity, DrivetrainType drivetrain, /* pass by value */ YawMode yaw_mode, float last_z); - - bool isYawWithinMargin(float yaw_target, float margin) const; - -private:// vars - shared_ptr safety_eval_ptr_; - float obs_avoidance_vel_ = 0.5f; - - CollisionInfo collision_info_; - - // we make this recursive so that DroneControllerBase subclass can grab StatusLock then call a - // base class method on DroneControllerBase that also grabs the StatusLock. - std::recursive_mutex status_mutex_; -}; - -}} //namespace -#endif diff --git a/AirLib/include/vehicles/multirotor/firmwares/ros_flight/RosFlightDroneController.hpp b/AirLib/include/vehicles/multirotor/firmwares/ros_flight/RosFlightDroneController.hpp index 2b0f5c877f..5ab5192527 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/ros_flight/RosFlightDroneController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/ros_flight/RosFlightDroneController.hpp @@ -4,7 +4,7 @@ #ifndef msr_airlib_RosFlightDroneController_hpp #define msr_airlib_RosFlightDroneController_hpp -#include "vehicles/multirotor/controllers/DroneControllerBase.hpp" +#include "vehicles/multirotor/controllers/MultirotorApiBase.h" #include "sensors/SensorCollection.hpp" #include "physics/Environment.hpp" #include "physics/Kinematics.hpp" @@ -20,7 +20,7 @@ STRICT_MODE_ON namespace msr { namespace airlib { -class RosFlightDroneController : public DroneControllerBase { +class RosFlightDroneController : public MultirotorApiBase { public: RosFlightDroneController(const SensorCollection* sensors, const MultiRotorParams* vehicle_params, @@ -44,17 +44,17 @@ class RosFlightDroneController : public DroneControllerBase { } public: - //*** Start: VehicleControllerBase implementation ***// + //*** Start: VehicleApiBase implementation ***// virtual void reset() override { - DroneControllerBase::reset(); + MultirotorApiBase::reset(); board_->system_reset(false); } virtual void update() override { - DroneControllerBase::update(); + MultirotorApiBase::update(); board_->notifySensorUpdated(ros_flight::Board::SensorType::Imu); firmware_->loop(); @@ -65,13 +65,6 @@ class RosFlightDroneController : public DroneControllerBase { return vehicle_params_->getParams().rotor_count; } - virtual bool isAvailable(std::string& message) const override - { - unused(message); - return true; - } - - virtual real_T getVertexControlSignal(unsigned int rotor_index) const override { //convert counter clockwise index to ArduCopter's QuadX style index @@ -113,9 +106,9 @@ class RosFlightDroneController : public DroneControllerBase { if (!is_set) throw VehicleCommandNotImplementedException("setting non-simulation mode is not supported yet"); } - //*** End: VehicleControllerBase implementation ***// + //*** End: VehicleApiBase implementation ***// -//*** Start: DroneControllerBase implementation ***// +//*** Start: MultirotorApiBase implementation ***// public: virtual Kinematics::State getKinematicsEstimated() const override { @@ -172,7 +165,7 @@ class RosFlightDroneController : public DroneControllerBase { //else we don't have RC data } - virtual bool armDisarm(bool arm, CancelableBase& cancelable_action) override + virtual bool armDisarm(bool arm) override { unused(arm); unused(cancelable_action); @@ -180,7 +173,7 @@ class RosFlightDroneController : public DroneControllerBase { return true; } - virtual bool takeoff(float max_wait_seconds, CancelableBase& cancelable_action) override + virtual bool takeoff(float max_wait_seconds) override { unused(max_wait_seconds); unused(cancelable_action); @@ -188,7 +181,7 @@ class RosFlightDroneController : public DroneControllerBase { return true; } - virtual bool land(float max_wait_seconds, CancelableBase& cancelable_action) override + virtual bool land(float max_wait_seconds) override { unused(max_wait_seconds); unused(cancelable_action); @@ -196,13 +189,13 @@ class RosFlightDroneController : public DroneControllerBase { return true; } - virtual bool goHome(CancelableBase& cancelable_action) override + virtual bool goHome() override { unused(cancelable_action); return true; } - virtual bool hover(CancelableBase& cancelable_action) override + virtual bool hover() override { unused(cancelable_action); return true; @@ -218,13 +211,6 @@ class RosFlightDroneController : public DroneControllerBase { return environment_->getState().geo_point; } - virtual void reportTelemetry(float renderTime) override - { - unused(renderTime); - - //TODO: implement this - } - virtual float getCommandPeriod() const override { return 1.0f/50; //50hz @@ -293,13 +279,13 @@ class RosFlightDroneController : public DroneControllerBase { //TODO: implement this } - virtual const VehicleParams& getVehicleParams() const override + virtual const MultirotorApiParams& getVehicleParams() const override { //used for safety algos. For now just use defaults - static const VehicleParams safety_params; + static const MultirotorApiParams safety_params; return safety_params; } - //*** End: DroneControllerBase implementation ***// + //*** End: MultirotorApiBase implementation ***// private: //convert pitch, roll, yaw from -1 to 1 to PWM diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightDroneController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightDroneController.hpp index 3f37374df6..8af6db2b35 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightDroneController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightDroneController.hpp @@ -4,7 +4,7 @@ #ifndef msr_airlib_SimpleFlightDroneController_hpp #define msr_airlib_SimpleFlightDroneController_hpp -#include "vehicles/multirotor/controllers/DroneControllerBase.hpp" +#include "vehicles/multirotor/controllers/MultirotorApiBase.h" #include "sensors/SensorCollection.hpp" #include "physics/Environment.hpp" #include "physics/Kinematics.hpp" @@ -20,7 +20,7 @@ namespace msr { namespace airlib { -class SimpleFlightDroneController : public DroneControllerBase { +class SimpleFlightDroneController : public MultirotorApiBase { public: SimpleFlightDroneController(const MultiRotorParams* vehicle_params, const AirSimSettings::VehicleSetting* vehicle_setting) @@ -49,17 +49,17 @@ class SimpleFlightDroneController : public DroneControllerBase { } public: - //*** Start: VehicleControllerBase implementation ***// + //*** Start: VehicleApiBase implementation ***// virtual void reset() override { - DroneControllerBase::reset(); + MultirotorApiBase::reset(); firmware_->reset(); } virtual void update() override { - DroneControllerBase::update(); + MultirotorApiBase::update(); firmware_->update(); } @@ -69,12 +69,6 @@ class SimpleFlightDroneController : public DroneControllerBase { return vehicle_params_->getParams().rotor_count; } - virtual bool isAvailable(std::string& message) const override - { - unused(message); - return true; - } - virtual real_T getVertexControlSignal(unsigned int rotor_index) const override { auto control_signal = board_->getMotorControlSignal(rotor_index); @@ -107,9 +101,9 @@ class SimpleFlightDroneController : public DroneControllerBase { if (!is_set) throw VehicleCommandNotImplementedException("setting non-simulation mode is not supported yet"); } - //*** End: VehicleControllerBase implementation ***// + //*** End: VehicleApiBase implementation ***// -//*** Start: DroneControllerBase implementation ***// +//*** Start: MultirotorApiBase implementation ***// public: virtual Kinematics::State getKinematicsEstimated() const override { @@ -173,7 +167,7 @@ class SimpleFlightDroneController : public DroneControllerBase { } } - virtual bool armDisarm(bool arm, CancelableBase& cancelable_action) override + virtual bool armDisarm(bool arm) override { unused(cancelable_action); @@ -194,12 +188,6 @@ class SimpleFlightDroneController : public DroneControllerBase { return AirSimSimpleFlightCommon::toGeoPoint(firmware_->offboardApi().getGeoPoint()); } - virtual void reportTelemetry(float renderTime) override - { - unused(renderTime); - //TODO: implement this - } - virtual float getCommandPeriod() const override { return 1.0f/50; //50hz @@ -289,12 +277,12 @@ class SimpleFlightDroneController : public DroneControllerBase { firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); } - virtual const VehicleParams& getVehicleParams() const override + virtual const MultirotorApiParams& getVehicleParams() const override { return safety_params_; } - //*** End: DroneControllerBase implementation ***// + //*** End: MultirotorApiBase implementation ***// private: @@ -334,7 +322,7 @@ class SimpleFlightDroneController : public DroneControllerBase { unique_ptr estimator_; unique_ptr firmware_; - VehicleParams safety_params_; + MultirotorApiParams safety_params_; RCData last_rcData_; }; diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index b4d0737eae..6e4a5d8c96 100644 --- a/AirLib/src/api/RpcLibClientBase.cpp +++ b/AirLib/src/api/RpcLibClientBase.cpp @@ -68,28 +68,14 @@ RpcLibClientBase::ConnectionState RpcLibClientBase::getConnectionState() return ConnectionState::Unknown; } } -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(); -} -int RpcLibClientBase::simGetSegmentationObjectID(const std::string& mesh_name) -{ - return pimpl_->client.call("simGetSegmentationObjectID", mesh_name).as(); -} void RpcLibClientBase::enableApiControl(bool is_enabled) { pimpl_->client.call("enableApiControl", is_enabled); } -bool RpcLibClientBase::isApiControlEnabled() +bool RpcLibClientBase::isApiControlEnabled() const { return pimpl_->client.call("isApiControlEnabled").as(); } - -msr::airlib::GeoPoint RpcLibClientBase::getHomeGeoPoint() -{ - return pimpl_->client.call("getHomeGeoPoint").as().to(); -} - int RpcLibClientBase::getClientVersion() const { return 1; //sync with Python client @@ -107,11 +93,17 @@ int RpcLibClientBase::getServerVersion() const return pimpl_->client.call("getServerVersion").as(); } -void RpcLibClientBase::reset() +void RpcLibClientBase::resetVehicle() { - pimpl_->client.call("reset"); + pimpl_->client.call("resetVehicle"); } +void RpcLibClientBase::simResetWorld() +{ + pimpl_->client.call("simResetWorld"); +} + + void RpcLibClientBase::confirmConnection() { ClockBase* clock = ClockFactory::get(); @@ -150,41 +142,41 @@ void RpcLibClientBase::confirmConnection() std::cout << std::endl << ver_info << std::endl; } -void* RpcLibClientBase::getClient() -{ - return &pimpl_->client; -} - bool RpcLibClientBase::armDisarm(bool arm) { return pimpl_->client.call("armDisarm", arm).as(); } -CameraInfo RpcLibClientBase::getCameraInfo(int camera_id) +msr::airlib::GeoPoint RpcLibClientBase::getHomeGeoPoint() const { - return pimpl_->client.call("getCameraInfo", camera_id).as().to(); + return pimpl_->client.call("getHomeGeoPoint").as().to(); } -void RpcLibClientBase::setCameraOrientation(int camera_id, const Quaternionr& orientation) +bool RpcLibClientBase::simSetSegmentationObjectID(const std::string& mesh_name, int object_id, bool is_name_regex) { - pimpl_->client.call("setCameraOrientation", camera_id, RpcLibAdapatorsBase::Quaternionr(orientation)); + return pimpl_->client.call("simSetSegmentationObjectID", mesh_name, object_id, is_name_regex).as(); +} +int RpcLibClientBase::simGetSegmentationObjectID(const std::string& mesh_name) const +{ + return pimpl_->client.call("simGetSegmentationObjectID", mesh_name).as(); } -CollisionInfo RpcLibClientBase::getCollisionInfo() +CollisionInfo RpcLibClientBase::simGetCollisionInfo() const { return pimpl_->client.call("getCollisionInfo").as().to(); } //sim only -void RpcLibClientBase::simSetPose(const Pose& pose, bool ignore_collision) +Pose RpcLibClientBase::simGetVehiclePose() const { - pimpl_->client.call("simSetPose", RpcLibAdapatorsBase::Pose(pose), ignore_collision); + return pimpl_->client.call("simGetPose").as().to(); } -Pose RpcLibClientBase::simGetPose() +void RpcLibClientBase::simSetVehiclePose(const Pose& pose, bool ignore_collision) { - return pimpl_->client.call("simGetPose").as().to(); + pimpl_->client.call("simSetPose", RpcLibAdapatorsBase::Pose(pose), ignore_collision); } + vector RpcLibClientBase::simGetImages(vector request) { const auto& response_adaptor = pimpl_->client.call("simGetImages", @@ -208,8 +200,7 @@ void RpcLibClientBase::simPrintLogMessage(const std::string& message, std::strin pimpl_->client.call("simPrintLogMessage", message, message_param, severity); } - -bool RpcLibClientBase::simIsPaused() +bool RpcLibClientBase::simIsPaused() const { return pimpl_->client.call("simIsPaused").as(); } @@ -224,11 +215,30 @@ void RpcLibClientBase::simContinueForTime(double seconds) pimpl_->client.call("simContinueForTime", seconds); } -msr::airlib::Pose RpcLibClientBase::simGetObjectPose(const std::string& object_name) +msr::airlib::Pose RpcLibClientBase::simGetObjectPose(const std::string& object_name) const { return pimpl_->client.call("simGetObjectPose", object_name).as().to(); } +CameraInfo RpcLibClientBase::getCameraInfo(int camera_id) const +{ + return pimpl_->client.call("getCameraInfo", camera_id).as().to(); +} + +void RpcLibClientBase::setCameraOrientation(int camera_id, const Quaternionr& orientation) +{ + pimpl_->client.call("setCameraOrientation", camera_id, RpcLibAdapatorsBase::Quaternionr(orientation)); +} + +void* RpcLibClientBase::getClient() +{ + return &pimpl_->client; +} + +const void* RpcLibClientBase::getClient() const +{ + return &pimpl_->client; +} }} //namespace diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index c59c1e7404..dcc44edafc 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -45,23 +45,43 @@ struct RpcLibServerBase::impl { typedef msr::airlib_rpclib::RpcLibAdapatorsBase RpcLibAdapatorsBase; -RpcLibServerBase::RpcLibServerBase(SimModeApiBase* simmode_api, string server_address, uint16_t port) - : simmode_api_(simmode_api) +RpcLibServerBase::RpcLibServerBase(const std::string& server_address, uint16_t port) { + pimpl_->server.bind("getServerVersion", []() -> int { + return 1; + }); + pimpl_->server.bind("getMinRequiredClientVersion", []() -> int { + return 1; + }); + + 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("enableApiControl", [&](bool is_enabled) -> void { getVehicleApi()->enableApiControl(is_enabled); }); + pimpl_->server.bind("isApiControlEnabled", [&]() -> bool { return getVehicleApi()->isApiControlEnabled(); }); + pimpl_->server.bind("armDisarm", [&](bool arm) -> bool { return getVehicleApi()->armDisarm(arm); }); + + + pimpl_->server.bind("simPause", [&](bool is_paused) -> void { + getWorldSimApi()->pause(is_paused); + }); + pimpl_->server.bind("simIsPaused", [&]() -> bool { + return getWorldSimApi()->isPaused(); + }); + pimpl_->server.bind("simContinueForTime", [&](double seconds) -> void { + getWorldSimApi()->continueForTime(seconds); + }); - //sim only pimpl_->server.bind("simGetImages", [&](const std::vector& request_adapter) -> vector { - const auto& response = getVehicleApi()->simGetImages(RpcLibAdapatorsBase::ImageRequest::to(request_adapter)); + const auto& response = getVehicleSimApi()->getImages(RpcLibAdapatorsBase::ImageRequest::to(request_adapter)); return RpcLibAdapatorsBase::ImageResponse::from(response); }); pimpl_->server.bind("simGetImage", [&](uint8_t camera_id, ImageCaptureBase::ImageType type) -> vector { - auto result = getVehicleApi()->simGetImage(camera_id, type); + auto result = getVehicleSimApi()->getImage(camera_id, type); if (result.size() == 0) { // rpclib has a bug with serializing empty vectors, so we return a 1 byte vector instead. result.push_back(0); @@ -71,28 +91,35 @@ RpcLibServerBase::RpcLibServerBase(SimModeApiBase* simmode_api, string server_ad pimpl_->server. bind("simSetPose", [&](const RpcLibAdapatorsBase::Pose &pose, bool ignore_collision) -> void { - getVehicleApi()->simSetPose(pose.to(), ignore_collision); + getVehicleSimApi()->setPose(pose.to(), ignore_collision); }); pimpl_->server.bind("simGetPose", [&]() -> RpcLibAdapatorsBase::Pose { - const auto& pose = getVehicleApi()->simGetPose(); + const auto& pose = getVehicleSimApi()->getPose(); return RpcLibAdapatorsBase::Pose(pose); }); pimpl_->server. bind("simSetSegmentationObjectID", [&](const std::string& mesh_name, int object_id, bool is_name_regex) -> bool { - return getVehicleApi()->simSetSegmentationObjectID(mesh_name, object_id, is_name_regex); + return getWorldSimApi()->setSegmentationObjectID(mesh_name, object_id, is_name_regex); }); pimpl_->server. bind("simGetSegmentationObjectID", [&](const std::string& mesh_name) -> int { - return getVehicleApi()->simGetSegmentationObjectID(mesh_name); + return getWorldSimApi()->getSegmentationObjectID(mesh_name); }); - pimpl_->server.bind("reset", [&]() -> void { - getSimModeApi()->reset(); + pimpl_->server.bind("simResetWorld", [&]() -> void { + getWorldSimApi()->reset(); + }); + pimpl_->server.bind("resetVehicle", [&]() -> void { + auto* sim_vehicle_api = getVehicleSimApi(); + if (sim_vehicle_api) + sim_vehicle_api->reset(); + else + getVehicleApi()->reset(); }); pimpl_->server.bind("simPrintLogMessage", [&](const std::string& message, const std::string& message_param, unsigned char severity) -> void { - getVehicleApi()->simPrintLogMessage(message, message_param, severity); + getWorldSimApi()->printLogMessage(message, message_param, severity); }); pimpl_->server.bind("getHomeGeoPoint", [&]() -> RpcLibAdapatorsBase::GeoPoint { @@ -109,39 +136,18 @@ RpcLibServerBase::RpcLibServerBase(SimModeApiBase* simmode_api, string server_ad getVehicleApi()->setCameraOrientation(camera_id, orientation.to()); }); - pimpl_->server.bind("enableApiControl", [&](bool is_enabled) -> void { getVehicleApi()->enableApiControl(is_enabled); }); - pimpl_->server.bind("isApiControlEnabled", [&]() -> bool { return getVehicleApi()->isApiControlEnabled(); }); - pimpl_->server.bind("armDisarm", [&](bool arm) -> bool { return getVehicleApi()->armDisarm(arm); }); - pimpl_->server.bind("getCollisionInfo", [&]() -> RpcLibAdapatorsBase::CollisionInfo { - const auto& collision_info = getVehicleApi()->getCollisionInfo(); + pimpl_->server.bind("simGetCollisionInfo", [&]() -> RpcLibAdapatorsBase::CollisionInfo { + const auto& collision_info = getVehicleSimApi()->getCollisionInfo(); return RpcLibAdapatorsBase::CollisionInfo(collision_info); }); pimpl_->server.bind("simGetObjectPose", [&](const std::string& object_name) -> RpcLibAdapatorsBase::Pose { - const auto& pose = getVehicleApi()->simGetObjectPose(object_name); + const auto& pose = getWorldSimApi()->getObjectPose(object_name); return RpcLibAdapatorsBase::Pose(pose); }); - - pimpl_->server.bind("simPause", [&](bool is_paused) -> void { - getSimModeApi()->pause(is_paused); - }); - pimpl_->server.bind("simIsPaused", [&]() -> bool { - return getSimModeApi()->isPaused(); - }); - pimpl_->server.bind("simContinueForTime", [&](double seconds) -> void { - getSimModeApi()->continueForTime(seconds); - }); - pimpl_->server.bind("getServerVersion", []() -> int { - return 1; - }); - pimpl_->server.bind("getMinRequiredClientVersion", []() -> int { - return 1; - }); - pimpl_->server.bind("isSimulationMode", [&]() -> bool { - return getSimModeApi()->isSimulationMode(); - }); - + + //if we don't suppress then server will bomb out for exceptions raised by any method pimpl_->server.suppress_exceptions(true); } @@ -164,21 +170,11 @@ void RpcLibServerBase::stop() pimpl_->server.stop(); } -VehicleApiBase* RpcLibServerBase::getVehicleApi() const -{ - return simmode_api_->getVehicleApi(); -} - void* RpcLibServerBase::getServer() const { return &pimpl_->server; } -SimModeApiBase* RpcLibServerBase::getSimModeApi() const -{ - return simmode_api_; -} - }} //namespace #endif diff --git a/AirLib/src/safety/SafetyEval.cpp b/AirLib/src/safety/SafetyEval.cpp index 065e5e46cf..879e75d431 100644 --- a/AirLib/src/safety/SafetyEval.cpp +++ b/AirLib/src/safety/SafetyEval.cpp @@ -14,7 +14,7 @@ namespace msr { namespace airlib { //TODO: something defines max macro which interfears with code here #undef max -SafetyEval::SafetyEval(VehicleParams vehicle_params, shared_ptr fence_ptr, shared_ptr obs_xy_ptr) +SafetyEval::SafetyEval(MultirotorApiParams vehicle_params, shared_ptr fence_ptr, shared_ptr obs_xy_ptr) : vehicle_params_(vehicle_params), fence_ptr_(fence_ptr), obs_xy_ptr_(obs_xy_ptr) { Utils::log(Utils::stringf("enable_reasons: %X, obs_strategy=%X", uint(enable_reasons_), uint(obs_strategy_))); diff --git a/AirLib/src/vehicles/car/api/CarRpcLibServer.cpp b/AirLib/src/vehicles/car/api/CarRpcLibServer.cpp index 594e3d88b9..af2d38f6b3 100644 --- a/AirLib/src/vehicles/car/api/CarRpcLibServer.cpp +++ b/AirLib/src/vehicles/car/api/CarRpcLibServer.cpp @@ -30,7 +30,7 @@ namespace msr { namespace airlib { typedef msr::airlib_rpclib::CarRpcLibAdapators CarRpcLibAdapators; -CarRpcLibServer::CarRpcLibServer(SimModeApiBase* simmode_api, string server_address, uint16_t port) +CarRpcLibServer::CarRpcLibServer(WorldSimApiBase* simmode_api, string server_address, uint16_t port) : RpcLibServerBase(simmode_api, server_address, port) { (static_cast(getServer()))-> diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp index 4de68164be..da6816f355 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp @@ -141,10 +141,10 @@ Quaternionr MultirotorRpcLibClient::getOrientation() return static_cast(getClient())->call("getOrientation").as().to(); } -DroneControllerBase::LandedState MultirotorRpcLibClient::getLandedState() +MultirotorApiBase::LandedState MultirotorRpcLibClient::getLandedState() { int result = static_cast(getClient())->call("getLandedState").as(); - return static_cast(result); + return static_cast(result); } RCData MultirotorRpcLibClient::getRCData() diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp index 8077f3563c..9907ad1a76 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp @@ -30,7 +30,7 @@ namespace msr { namespace airlib { typedef msr::airlib_rpclib::MultirotorRpcLibAdapators MultirotorRpcLibAdapators; -MultirotorRpcLibServer::MultirotorRpcLibServer(SimModeApiBase* simmode_api, string server_address, uint16_t port) +MultirotorRpcLibServer::MultirotorRpcLibServer(WorldSimApiBase* simmode_api, string server_address, uint16_t port) : RpcLibServerBase(simmode_api, server_address, port) { (static_cast(getServer()))-> @@ -130,9 +130,9 @@ MultirotorRpcLibServer::~MultirotorRpcLibServer() { } -MultirotorApi* MultirotorRpcLibServer::getDroneApi() const +MultirotorApiBase* MultirotorRpcLibServer::getDroneApi() const { - return static_cast(getSimModeApi()->getVehicleApi()); + return static_cast(getSimModeApi()->getVehicleApi()); } }} //namespace diff --git a/AirLib/src/vehicles/multirotor/controllers/DroneControllerBase.cpp b/AirLib/src/vehicles/multirotor/controllers/MultirotorControllerApiBase.cpp similarity index 74% rename from AirLib/src/vehicles/multirotor/controllers/DroneControllerBase.cpp rename to AirLib/src/vehicles/multirotor/controllers/MultirotorControllerApiBase.cpp index 0ae39ebf08..99732f3571 100644 --- a/AirLib/src/vehicles/multirotor/controllers/DroneControllerBase.cpp +++ b/AirLib/src/vehicles/multirotor/controllers/MultirotorControllerApiBase.cpp @@ -4,7 +4,7 @@ //in header only mode, control library is not available #ifndef AIRLIB_HEADER_ONLY -#include "vehicles/multirotor/controllers/DroneControllerBase.hpp" +#include "vehicles/multirotor/api/MultirotorApiBase.h" #include #include #include @@ -13,63 +13,35 @@ namespace msr { namespace airlib { -float DroneControllerBase::getAutoLookahead(float velocity, float adaptive_lookahead, - float max_factor, float min_factor) const -{ - //if auto mode requested for lookahead then calculate based on velocity - float command_period_dist = velocity * getCommandPeriod(); - float lookahead = command_period_dist * (adaptive_lookahead > 0 ? min_factor : max_factor); - lookahead = std::max(lookahead, getDistanceAccuracy()*1.5f); //50% more than distance accuracy - return lookahead; -} - -float DroneControllerBase::getObsAvoidanceVelocity(float risk_dist, float max_obs_avoidance_vel) const -{ - unused(risk_dist); - return max_obs_avoidance_vel; -} - -void DroneControllerBase::setSafetyEval(const shared_ptr safety_eval_ptr) -{ - safety_eval_ptr_ = safety_eval_ptr; -} - -bool DroneControllerBase::loopCommandPre() -{ - //no-op by default. derived class can override it if needed - return true; -} -void DroneControllerBase::loopCommandPost() +bool MultirotorApiBase::moveByAngleZ(float pitch, float roll, float z, float yaw, float duration) { - //no-op by default. derived class can override it if needed -} + SingleCall lock(getCancelToken()); -bool DroneControllerBase::moveByAngleZ(float pitch, float roll, float z, float yaw, float duration - , CancelableBase& cancelable_action) -{ if (duration <= 0) return true; return !waitForFunction([&]() { - return !moveByRollPitchZ(pitch, roll, z, yaw); - }, duration, cancelable_action); + return !moveByRollPitchZInternal(pitch, roll, z, yaw); + }, duration); } -bool DroneControllerBase::moveByAngleThrottle(float pitch, float roll, float throttle, float yaw_rate, float duration - , CancelableBase& cancelable_action) +bool MultirotorApiBase::moveByAngleThrottle(float pitch, float roll, float throttle, float yaw_rate, float duration) { + SingleCall lock(getCancelToken()); + if (duration <= 0) return true; return !waitForFunction([&]() { - return !moveByRollPitchThrottle(pitch, roll, throttle, yaw_rate); - }, duration, cancelable_action); + return !moveByRollPitchThrottleInternal(pitch, roll, throttle, yaw_rate); + }, duration); } -bool DroneControllerBase::moveByVelocity(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode, - CancelableBase& cancelable_action) +bool MultirotorApiBase::moveByVelocity(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode) { + SingleCall lock(getCancelToken()); + if (duration <= 0) return true; @@ -77,13 +49,14 @@ bool DroneControllerBase::moveByVelocity(float vx, float vy, float vz, float dur adjustYaw(vx, vy, drivetrain, adj_yaw_mode); return !waitForFunction([&]() { - return !moveByVelocity(vx, vy, vz, adj_yaw_mode); - }, duration, cancelable_action); + return !moveByVelocityInternal(vx, vy, vz, adj_yaw_mode); + }, duration); } -bool DroneControllerBase::moveByVelocityZ(float vx, float vy, float z, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode, - CancelableBase& cancelable_action) +bool MultirotorApiBase::moveByVelocityZ(float vx, float vy, float z, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode) { + SingleCall lock(getCancelToken()); + if (duration <= 0) return false; @@ -91,14 +64,16 @@ bool DroneControllerBase::moveByVelocityZ(float vx, float vy, float z, float dur adjustYaw(vx, vy, drivetrain, adj_yaw_mode); return !waitForFunction([&]() { - return !moveByVelocityZ(vx, vy, z, adj_yaw_mode); - }, duration, cancelable_action); + return !moveByVelocityZInternal(vx, vy, z, adj_yaw_mode); + }, duration); } -bool DroneControllerBase::moveOnPath(const vector& path, float velocity, DrivetrainType drivetrain, const YawMode& yaw_mode, - float lookahead, float adaptive_lookahead, CancelableBase& cancelable_action) +bool MultirotorApiBase::moveOnPath(const vector& path, float velocity, DrivetrainType drivetrain, const YawMode& yaw_mode, + float lookahead, float adaptive_lookahead) { + SingleCall lock(getCancelToken()); + //validate path size if (path.size() == 0) { Utils::log("moveOnPath terminated because path has no points", Utils::kLogLevelWarn); @@ -128,7 +103,7 @@ bool DroneControllerBase::moveOnPath(const vector& path, float velocit //add current position as starting point vector path3d; vector path_segs; - path3d.push_back(getPosition()); + path3d.push_back(getKinematicsEstimated().pose.position); Vector3r point; float path_length = 0; @@ -183,7 +158,7 @@ bool DroneControllerBase::moveOnPath(const vector& path, float velocit yaw_mode, path_segs.at(cur_path_loc.seg_index).start_z); //sleep for rest of the cycle - if (!waiter.sleep(cancelable_action)) + if (!waiter.sleep(getCancelToken())) return false; /* Below, P is previous position on path, N is next goal and C is our current position. @@ -273,41 +248,48 @@ bool DroneControllerBase::moveOnPath(const vector& path, float velocit return true; } -bool DroneControllerBase::moveToPosition(float x, float y, float z, float velocity, DrivetrainType drivetrain, - const YawMode& yaw_mode, float lookahead, float adaptive_lookahead, CancelableBase& cancelable_action) +bool MultirotorApiBase::moveToPosition(float x, float y, float z, float velocity, DrivetrainType drivetrain, + const YawMode& yaw_mode, float lookahead, float adaptive_lookahead) { + SingleCall lock(getCancelToken()); + vector path { Vector3r(x, y, z) }; - return moveOnPath(path, velocity, drivetrain, yaw_mode, lookahead, adaptive_lookahead, cancelable_action); + return moveOnPath(path, velocity, drivetrain, yaw_mode, lookahead, adaptive_lookahead); } -bool DroneControllerBase::moveToZ(float z, float velocity, const YawMode& yaw_mode, - float lookahead, float adaptive_lookahead, CancelableBase& cancelable_action) +bool MultirotorApiBase::moveToZ(float z, float velocity, const YawMode& yaw_mode, + float lookahead, float adaptive_lookahead) { - Vector2r cur_xy = getPositionXY(); + SingleCall lock(getCancelToken()); + + Vector2r cur_xy(getPosition().x, getPosition().y); vector path { Vector3r(cur_xy.x(), cur_xy.y(), z) }; - return moveOnPath(path, velocity, DrivetrainType::MaxDegreeOfFreedom, yaw_mode, lookahead, adaptive_lookahead, - cancelable_action); + return moveOnPath(path, velocity, DrivetrainType::MaxDegreeOfFreedom, yaw_mode, lookahead, adaptive_lookahead); } -bool DroneControllerBase::rotateToYaw(float yaw, float margin, CancelableBase& cancelable_action) +bool MultirotorApiBase::rotateToYaw(float yaw, float margin) { + SingleCall lock(getCancelToken()); + YawMode yaw_mode(false, VectorMath::normalizeAngle(yaw)); Waiter waiter(getCommandPeriod()); auto start_pos = getPosition(); bool is_yaw_reached; while ((is_yaw_reached = isYawWithinMargin(yaw, margin)) == false) { - if (!moveToPosition(start_pos, yaw_mode)) + if (!moveToPositionInternal(start_pos, yaw_mode)) return false; - if (!waiter.sleep(cancelable_action)) + if (!waiter.sleep(getCancelToken())) return false; } return true; } -bool DroneControllerBase::rotateByYawRate(float yaw_rate, float duration, CancelableBase& cancelable_action) +bool MultirotorApiBase::rotateByYawRate(float yaw_rate, float duration) { + SingleCall lock(getCancelToken()); + if (duration <= 0) return true; @@ -315,17 +297,19 @@ bool DroneControllerBase::rotateByYawRate(float yaw_rate, float duration, Cancel YawMode yaw_mode(true, yaw_rate); Waiter waiter(getCommandPeriod(), duration); do { - if (!moveToPosition(start_pos, yaw_mode)) + if (!moveToPositionInternal(start_pos, yaw_mode)) return false; - } while (waiter.sleep(cancelable_action) && !waiter.is_timeout()); + } while (waiter.sleep(getCancelToken()) && !waiter.is_timeout()); return waiter.is_timeout(); } -bool DroneControllerBase::takeoff(float max_wait_seconds, CancelableBase& cancelable_action) +bool MultirotorApiBase::takeoff(float max_wait_seconds) { + SingleCall lock(getCancelToken()); + unused(max_wait_seconds); - bool ret = moveToPosition(0, 0, getTakeoffZ(), 0.5f, DrivetrainType::MaxDegreeOfFreedom, YawMode::Zero(), -1, 1, cancelable_action); + bool ret = moveToPosition(0, 0, getTakeoffZ(), 0.5f, DrivetrainType::MaxDegreeOfFreedom, YawMode::Zero(), -1, 1); //last command is to hold on to position //commandPosition(0, 0, getTakeoffZ(), YawMode::Zero()); @@ -333,13 +317,17 @@ bool DroneControllerBase::takeoff(float max_wait_seconds, CancelableBase& cancel return ret; } -bool DroneControllerBase::goHome(CancelableBase& cancelable_action) +bool MultirotorApiBase::goHome() { - return moveToPosition(0, 0, 0, 0.5f, DrivetrainType::MaxDegreeOfFreedom, YawMode::Zero(), -1, 1, cancelable_action); + SingleCall lock(getCancelToken()); + + return moveToPosition(0, 0, 0, 0.5f, DrivetrainType::MaxDegreeOfFreedom, YawMode::Zero(), -1, 1); } -bool DroneControllerBase::land(float max_wait_seconds, CancelableBase& cancelable_action) +bool MultirotorApiBase::land(float max_wait_seconds) { + SingleCall lock(getCancelToken()); + float land_vel = 0.2f; float near_zero_vel = land_vel / 4; int near_zero_vel_count = 0; @@ -351,16 +339,18 @@ bool DroneControllerBase::land(float max_wait_seconds, CancelableBase& cancelabl else near_zero_vel_count = 0; - return near_zero_vel_count > 10 || !moveByVelocity(0, 0, 0.2f, YawMode::Zero()); - }, max_wait_seconds, cancelable_action); + return near_zero_vel_count > 10 || !moveByVelocityInternal(0, 0, 0.2f, YawMode::Zero()); + }, max_wait_seconds); } -bool DroneControllerBase::hover(CancelableBase& cancelable_action) +bool MultirotorApiBase::hover() { - return moveToZ(getZ(), 0.5f, YawMode{ true,0 }, 1.0f, false, cancelable_action); + SingleCall lock(getCancelToken()); + + return moveToZ(getPosition().z, 0.5f, YawMode{ true,0 }, 1.0f, false); } -bool DroneControllerBase::moveByVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) +bool MultirotorApiBase::moveByVelocityInternal(float vx, float vy, float vz, const YawMode& yaw_mode) { if (safetyCheckVelocity(Vector3r(vx, vy, vz))) commandVelocity(vx, vy, vz, yaw_mode); @@ -368,7 +358,7 @@ bool DroneControllerBase::moveByVelocity(float vx, float vy, float vz, const Yaw return true; } -bool DroneControllerBase::moveByVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) +bool MultirotorApiBase::moveByVelocityZInternal(float vx, float vy, float z, const YawMode& yaw_mode) { if (safetyCheckVelocityZ(vx, vy, z)) commandVelocityZ(vx, vy, z, yaw_mode); @@ -376,7 +366,7 @@ bool DroneControllerBase::moveByVelocityZ(float vx, float vy, float z, const Yaw return true; } -bool DroneControllerBase::moveToPosition(const Vector3r& dest, const YawMode& yaw_mode) +bool MultirotorApiBase::moveToPositionInternal(const Vector3r& dest, const YawMode& yaw_mode) { if (safetyCheckDestination(dest)) commandPosition(dest.x(), dest.y(), dest.z(), yaw_mode); @@ -384,7 +374,7 @@ bool DroneControllerBase::moveToPosition(const Vector3r& dest, const YawMode& ya return true; } -bool DroneControllerBase::moveByRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) +bool MultirotorApiBase::moveByRollPitchThrottleInternal(float pitch, float roll, float throttle, float yaw_rate) { if (safetyCheckVelocity(getVelocity())) commandRollPitchThrottle(pitch, roll, throttle, yaw_rate); @@ -392,7 +382,7 @@ bool DroneControllerBase::moveByRollPitchThrottle(float pitch, float roll, float return true; } -bool DroneControllerBase::moveByRollPitchZ(float pitch, float roll, float z, float yaw) +bool MultirotorApiBase::moveByRollPitchZInternal(float pitch, float roll, float z, float yaw) { if (safetyCheckVelocity(getVelocity())) commandRollPitchZ(pitch, roll, z, yaw); @@ -400,7 +390,7 @@ bool DroneControllerBase::moveByRollPitchZ(float pitch, float roll, float z, flo return true; } -bool DroneControllerBase::setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy, +bool MultirotorApiBase::setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy, float obs_avoidance_vel, const Vector3r& origin, float xy_length, float max_z, float min_z) { if (safety_eval_ptr_ == nullptr) @@ -416,7 +406,7 @@ bool DroneControllerBase::setSafety(SafetyEval::SafetyViolationType enable_reaso } -RCData DroneControllerBase::estimateRCTrims(CancelableBase& cancelable_action, float trimduration, float minCountForTrim, float maxTrim) +RCData MultirotorApiBase::estimateRCTrims(float trimduration, float minCountForTrim, float maxTrim) { rc_data_trims_ = RCData(); @@ -431,7 +421,7 @@ RCData DroneControllerBase::estimateRCTrims(CancelableBase& cancelable_action, f count++; } - } while (waiter_trim.sleep(cancelable_action) && !waiter_trim.is_timeout()); + } while (waiter_trim.sleep(getCancelToken()) && !waiter_trim.is_timeout()); rc_data_trims_.is_valid = true; @@ -453,15 +443,17 @@ RCData DroneControllerBase::estimateRCTrims(CancelableBase& cancelable_action, f return rc_data_trims_; } -bool DroneControllerBase::moveByManual(float vx_max, float vy_max, float z_min, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode, CancelableBase& cancelable_action) +bool MultirotorApiBase::moveByManual(float vx_max, float vy_max, float z_min, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode) { + SingleCall lock(getCancelToken()); + const float kMaxMessageAge = 0.1f /* 0.1 sec */, kMaxRCValue = 10000; if (duration <= 0) return true; //freeze the quaternion - Quaternionr starting_quaternion = getOrientation(); + Quaternionr starting_quaternion = getKinematicsEstimated().pose.orientation; Waiter waiter(getCommandPeriod(), duration); do { @@ -483,34 +475,22 @@ bool DroneControllerBase::moveByManual(float vx_max, float vy_max, float z_min, //execute command try { - float vz = (rc_data.throttle / kMaxRCValue) * z_min + getZ(); - moveByVelocityZ(vel_body.x(), vel_body.y(), vz, adj_yaw_mode); + float vz = (rc_data.throttle / kMaxRCValue) * z_min + getPosition().z; + moveByVelocityZInternal(vel_body.x(), vel_body.y(), vz, adj_yaw_mode); } - catch(const DroneControllerBase::UnsafeMoveException& ex) { + catch(const MultirotorApiBase::UnsafeMoveException& ex) { Utils::log(Utils::stringf("Safety violation: %s", ex.result.message.c_str()), Utils::kLogLevelWarn); } } else Utils::log(Utils::stringf("RCData had too old timestamp: %f", age)); - } while (waiter.sleep(cancelable_action) && !waiter.is_timeout()); + } while (waiter.sleep(getCancelToken()) && !waiter.is_timeout()); return waiter.is_timeout(); } -Vector2r DroneControllerBase::getPositionXY() const -{ - const Vector3r& cur_loc3 = getPosition(); - Vector2r cur_loc(cur_loc3.x(), cur_loc3.y()); - return cur_loc; -} - -float DroneControllerBase::getZ() const -{ - return getPosition().z(); -} - -bool DroneControllerBase::waitForFunction(WaitFunction function, float max_wait_seconds, CancelableBase& cancelable_action) +bool MultirotorApiBase::waitForFunction(WaitFunction function, float max_wait_seconds) { if (max_wait_seconds < 0) { @@ -524,17 +504,17 @@ bool DroneControllerBase::waitForFunction(WaitFunction function, float max_wait_ break; } } - while (waiter.sleep(cancelable_action) && !waiter.is_timeout()); + while (waiter.sleep(getCancelToken()) && !waiter.is_timeout()); return found; } -bool DroneControllerBase::waitForZ(float max_wait_seconds, float z, float margin, CancelableBase& cancelable_action) +bool MultirotorApiBase::waitForZ(float max_wait_seconds, float z, float margin) { float cur_z = 100000; if (!waitForFunction([&]() { - cur_z = getZ(); + cur_z = getPosition().z; return (std::abs(cur_z - z) <= margin); - }, max_wait_seconds, cancelable_action)) + }, max_wait_seconds)) { //Only raise exception is time out occurred. If preempted then return status. throw VehicleMoveException(Utils::stringf("Drone hasn't came to expected z of %f within time %f sec within error margin %f (current z = %f)", @@ -544,7 +524,7 @@ bool DroneControllerBase::waitForZ(float max_wait_seconds, float z, float margin } -bool DroneControllerBase::emergencyManeuverIfUnsafe(const SafetyEval::EvalResult& result) +bool MultirotorApiBase::emergencyManeuverIfUnsafe(const SafetyEval::EvalResult& result) { if (!result.is_safe) { if (result.reason == SafetyEval::SafetyViolationType_::Obstacle) { @@ -554,7 +534,7 @@ bool DroneControllerBase::emergencyManeuverIfUnsafe(const SafetyEval::EvalResult Vector3r avoidance_vel = getObsAvoidanceVelocity(result.cur_risk_dist, obs_avoidance_vel_) * result.suggested_vec; //use the unchecked command - commandVelocityZ(avoidance_vel.x(), avoidance_vel.y(), getZ(), YawMode::Zero()); + commandVelocityZ(avoidance_vel.x(), avoidance_vel.y(), getPosition().z, YawMode::Zero()); //tell caller not to execute planned command return false; @@ -571,7 +551,7 @@ bool DroneControllerBase::emergencyManeuverIfUnsafe(const SafetyEval::EvalResult return true; } -bool DroneControllerBase::safetyCheckVelocity(const Vector3r& velocity) +bool MultirotorApiBase::safetyCheckVelocity(const Vector3r& velocity) { if (safety_eval_ptr_ == nullptr) //safety checks disabled return true; @@ -579,7 +559,7 @@ bool DroneControllerBase::safetyCheckVelocity(const Vector3r& velocity) const auto& result = safety_eval_ptr_->isSafeVelocity(getPosition(), velocity, getOrientation()); return emergencyManeuverIfUnsafe(result); } -bool DroneControllerBase::safetyCheckVelocityZ(float vx, float vy, float z) +bool MultirotorApiBase::safetyCheckVelocityZ(float vx, float vy, float z) { if (safety_eval_ptr_ == nullptr) //safety checks disabled return true; @@ -587,7 +567,7 @@ bool DroneControllerBase::safetyCheckVelocityZ(float vx, float vy, float z) const auto& result = safety_eval_ptr_->isSafeVelocityZ(getPosition(), vx, vy, z, getOrientation()); return emergencyManeuverIfUnsafe(result); } -bool DroneControllerBase::safetyCheckDestination(const Vector3r& dest_pos) +bool MultirotorApiBase::safetyCheckDestination(const Vector3r& dest_pos) { if (safety_eval_ptr_ == nullptr) //safety checks disabled return true; @@ -596,16 +576,7 @@ bool DroneControllerBase::safetyCheckDestination(const Vector3r& dest_pos) return emergencyManeuverIfUnsafe(result); } -void DroneControllerBase::logHomePoint() -{ - GeoPoint homepoint = getHomeGeoPoint(); - if (std::isnan(homepoint.longitude)) - Utils::log("Home point is not set!", Utils::kLogLevelWarn); - else - Utils::log(homepoint.to_string().c_str()); -} - -float DroneControllerBase::setNextPathPosition(const vector& path, const vector& path_segs, +float MultirotorApiBase::setNextPathPosition(const vector& path, const vector& path_segs, const PathPosition& cur_path_loc, float next_dist, PathPosition& next_path_loc) { //note: cur_path_loc and next_path_loc may both point to same object @@ -639,7 +610,7 @@ float DroneControllerBase::setNextPathPosition(const vector& path, con return next_dist; } -void DroneControllerBase::adjustYaw(const Vector3r& heading, DrivetrainType drivetrain, YawMode& yaw_mode) +void MultirotorApiBase::adjustYaw(const Vector3r& heading, DrivetrainType drivetrain, YawMode& yaw_mode) { //adjust yaw for the direction of travel in foward-only mode if (drivetrain == DrivetrainType::ForwardOnly && !yaw_mode.is_rate) { @@ -653,11 +624,11 @@ void DroneControllerBase::adjustYaw(const Vector3r& heading, DrivetrainType driv //else no adjustment needed } -void DroneControllerBase::adjustYaw(float x, float y, DrivetrainType drivetrain, YawMode& yaw_mode) { +void MultirotorApiBase::adjustYaw(float x, float y, DrivetrainType drivetrain, YawMode& yaw_mode) { adjustYaw(Vector3r(x, y, 0), drivetrain, yaw_mode); } -void DroneControllerBase::moveToPathPosition(const Vector3r& dest, float velocity, DrivetrainType drivetrain, /* pass by value */ YawMode yaw_mode, float last_z) +void MultirotorApiBase::moveToPathPosition(const Vector3r& dest, float velocity, DrivetrainType drivetrain, /* pass by value */ YawMode yaw_mode, float last_z) { unused(last_z); //validate dest @@ -693,31 +664,40 @@ void DroneControllerBase::moveToPathPosition(const Vector3r& dest, float velocit //send commands //try to maintain altitude if path was in XY plan only, velocity based control is not as good if (std::abs(cur.z() - dest.z()) <= getDistanceAccuracy()) //for paths in XY plan current code leaves z untouched, so we can compare with strict equality - moveByVelocityZ(velocity_vect.x(), velocity_vect.y(), dest.z(), yaw_mode); + moveByVelocityZInternal(velocity_vect.x(), velocity_vect.y(), dest.z(), yaw_mode); else - moveByVelocity(velocity_vect.x(), velocity_vect.y(), velocity_vect.z(), yaw_mode); + moveByVelocityInternal(velocity_vect.x(), velocity_vect.y(), velocity_vect.z(), yaw_mode); } -bool DroneControllerBase::isYawWithinMargin(float yaw_target, float margin) const +bool MultirotorApiBase::isYawWithinMargin(float yaw_target, float margin) const { const float yaw_current = VectorMath::getYaw(getOrientation()) * 180 / M_PIf; return std::abs(yaw_current - yaw_target) <= margin; -} +} -Pose DroneControllerBase::getDebugPose() const +float MultirotorApiBase::getAutoLookahead(float velocity, float adaptive_lookahead, + float max_factor, float min_factor) const { - //by default indicate that we don't have alternative pose info - return Pose::nanPose(); + //if auto mode requested for lookahead then calculate based on velocity + float command_period_dist = velocity * getCommandPeriod(); + float lookahead = command_period_dist * (adaptive_lookahead > 0 ? min_factor : max_factor); + lookahead = std::max(lookahead, getDistanceAccuracy()*1.5f); //50% more than distance accuracy + return lookahead; } -CollisionInfo DroneControllerBase::getCollisionInfo() const +float MultirotorApiBase::getObsAvoidanceVelocity(float risk_dist, float max_obs_avoidance_vel) const { - return collision_info_; + unused(risk_dist); + return max_obs_avoidance_vel; } -void DroneControllerBase::setCollisionInfo(const CollisionInfo& collision_info) + +void MultirotorApiBase::setSafetyEval(const shared_ptr safety_eval_ptr) { - collision_info_ = collision_info; + SingleCall lock(getCancelToken()); + safety_eval_ptr_ = safety_eval_ptr; } + + }} //namespace #endif diff --git a/AirLibUnitTests/PixhawkTest.hpp b/AirLibUnitTests/PixhawkTest.hpp index d8c039af30..9a79ccdcf8 100644 --- a/AirLibUnitTests/PixhawkTest.hpp +++ b/AirLibUnitTests/PixhawkTest.hpp @@ -14,7 +14,7 @@ class PixhawkTest : public TestBase { //Test PX4 based drones auto pixhawk = MultiRotorParamsFactory::createConfig("Pixhawk", std::make_shared()); - DroneControllerBase* controller = pixhawk->getController(); + MultirotorApiBase* controller = pixhawk->getController(); testAssert(controller != nullptr, "Couldn't get pixhawk controller"); try { diff --git a/AirLibUnitTests/RosFlightTest.hpp b/AirLibUnitTests/RosFlightTest.hpp index 9864e3112d..cb12643301 100644 --- a/AirLibUnitTests/RosFlightTest.hpp +++ b/AirLibUnitTests/RosFlightTest.hpp @@ -14,7 +14,7 @@ class RosFlightTest : public TestBase { { auto rosFlight = MultiRotorParamsFactory::createConfig("RosFlight", std::make_shared()); - DroneControllerBase* controller = rosFlight->getController(); + MultirotorApiBase* controller = rosFlight->getController(); testAssert(controller != nullptr, "Couldn't get controller"); std::this_thread::sleep_for(std::chrono::milliseconds(10)); diff --git a/AirLibUnitTests/SimpleFlightTest.hpp b/AirLibUnitTests/SimpleFlightTest.hpp index 0ce98babd0..29295ca3cb 100644 --- a/AirLibUnitTests/SimpleFlightTest.hpp +++ b/AirLibUnitTests/SimpleFlightTest.hpp @@ -6,7 +6,7 @@ #include "TestBase.hpp" #include "physics/PhysicsWorld.hpp" #include "physics/FastPhysicsEngine.hpp" -#include "vehicles/multirotor/api/MultirotorApi.hpp" +#include "vehicles/multirotor/api/MultirotorApiBase.h" #include "common/SteppableClock.hpp" namespace msr { namespace airlib { @@ -33,7 +33,7 @@ class SimpleFlightTest : public TestBase PhysicsWorld physics_world(physics_engine.get(), vehicles, static_cast(clock->getStepSize() * 1E9)); - DroneControllerBase* controller = params->getController(); + MultirotorApiBase* controller = params->getController(); testAssert(controller != nullptr, "Controller was null"); std::string message; testAssert(controller->isAvailable(message), message); @@ -70,10 +70,10 @@ class SimpleFlightTest : public TestBase std::vector messages_; private: - class DirectCancelableBase : public CancelableBase + class DirectCancelableBase : public CancelableAction { public: - DirectCancelableBase(DroneControllerBase* controller, const MultiRotor* vehicle) + DirectCancelableBase(MultirotorApiBase* controller, const MultiRotor* vehicle) : controller_(controller), vehicle_(vehicle) { } @@ -88,11 +88,11 @@ class SimpleFlightTest : public TestBase } messages_.clear(); - return CancelableBase::sleep(secs); + return CancelableAction::sleep(secs); }; private: - DroneControllerBase* controller_; + MultirotorApiBase* controller_; const MultiRotor* vehicle_; std::vector messages_; }; diff --git a/AirLibUnitTests/WorkerThreadTest.hpp b/AirLibUnitTests/WorkerThreadTest.hpp index f6524282be..b9a7c1d6f5 100644 --- a/AirLibUnitTests/WorkerThreadTest.hpp +++ b/AirLibUnitTests/WorkerThreadTest.hpp @@ -10,7 +10,7 @@ namespace msr { namespace airlib { class WorkerThreadTest : public TestBase { - class WorkItem : public CancelableBase { + class WorkItem : public CancelableAction { public: double runTime = 2; // seconds @@ -41,7 +41,7 @@ class WorkerThreadTest : public TestBase { }; public: - virtual void run() override + virtual void run() override { WorkerThread thread; diff --git a/DroneServer/main.cpp b/DroneServer/main.cpp index ce5812aa00..6ca510e3a9 100644 --- a/DroneServer/main.cpp +++ b/DroneServer/main.cpp @@ -4,7 +4,7 @@ #include #include #include "vehicles/multirotor/api/MultirotorRpcLibServer.hpp" -#include "vehicles/multirotor/controllers/MavLinkDroneController.hpp" +#include "vehicles/multirotor/controllers/MavLinkMultirotorApi.hpp" #include "vehicles/multirotor/controllers/RealMultirotorConnector.hpp" #include "common/Settings.hpp" @@ -16,9 +16,9 @@ void printUsage() { cout << "Start the DroneServer using the 'PX4' settings in ~/Documents/AirSim/settings.json." << endl; } -class DroneServerSimModeApi : public SimModeApiBase { +class DroneServerSimModeApi : public WorldSimApiBase { public: - DroneServerSimModeApi(MultirotorApi* api) + DroneServerSimModeApi(MultirotorApiBase* api) : api_(api) { } @@ -49,7 +49,7 @@ class DroneServerSimModeApi : public SimModeApiBase { } private: - MultirotorApi* api_; + MultirotorApiBase* api_; }; int main(int argc, const char* argv[]) @@ -66,7 +66,7 @@ int main(int argc, const char* argv[]) else std::cout << "WARNING: This is not simulation!" << std::endl; - MavLinkDroneController::ConnectionInfo connection_info; + MavLinkMultirotorApi::ConnectionInfo connection_info; // read settings and override defaults auto settings_full_filepath = Settings::getUserDirectoryFullPath("settings.json"); @@ -110,13 +110,13 @@ int main(int argc, const char* argv[]) } - MavLinkDroneController mav_drone; + MavLinkMultirotorApi mav_drone; mav_drone.initialize(connection_info, nullptr, is_simulation); mav_drone.reset(); RealMultirotorConnector connector(& mav_drone); - MultirotorApi api(& connector); + MultirotorApiBase api(& connector); DroneServerSimModeApi simmode_api(&api); msr::airlib::MultirotorRpcLibServer server(&simmode_api, connection_info.local_host_ip); diff --git a/DroneShell/src/main.cpp b/DroneShell/src/main.cpp index a51e6ea775..37353c02b5 100644 --- a/DroneShell/src/main.cpp +++ b/DroneShell/src/main.cpp @@ -14,8 +14,8 @@ #include "common/common_utils/AsyncTasker.hpp" #include "vehicles/multirotor/api/MultirotorRpcLibClient.hpp" #include "common/EarthUtils.hpp" -#include "vehicles/multirotor/controllers/DroneCommon.hpp" -#include "vehicles/multirotor/controllers/DroneControllerBase.hpp" +#include "vehicles/multirotor/controllers/MultirotorCommon.hpp" +#include "vehicles/multirotor/controllers/MultirotorApiBase.h" #include "safety/SafetyEval.hpp" #include "common/common_utils/Timer.hpp" diff --git a/Examples/StereoImageGenerator.hpp b/Examples/StereoImageGenerator.hpp index 34908e1db1..32874b8791 100644 --- a/Examples/StereoImageGenerator.hpp +++ b/Examples/StereoImageGenerator.hpp @@ -10,7 +10,7 @@ #include "common/common_utils/FileSystem.hpp" #include "common/ClockFactory.hpp" #include "vehicles/multirotor/api/MultirotorRpcLibClient.hpp" -#include "vehicles/multirotor/controllers/DroneControllerBase.hpp" +#include "vehicles/multirotor/controllers/MultirotorApiBase.h" #include "RandomPointPoseGenerator.hpp" STRICT_MODE_OFF #ifndef RPCLIB_MSGPACK diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/FlyingPawn.h b/Unreal/Plugins/AirSim/Source/Multirotor/FlyingPawn.h index bf6e777a74..24e3fc26f6 100644 --- a/Unreal/Plugins/AirSim/Source/Multirotor/FlyingPawn.h +++ b/Unreal/Plugins/AirSim/Source/Multirotor/FlyingPawn.h @@ -3,7 +3,7 @@ #include "VehiclePawnWrapper.h" #include "GameFramework/RotatingMovementComponent.h" #include -#include "vehicles/multirotor/controllers/DroneCommon.hpp" +#include "vehicles/multirotor/controllers/MultirotorCommon.hpp" #include "PIPCamera.h" #include "FlyingPawn.generated.h" diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.cpp b/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.cpp index 4b644c0714..fb14103c2d 100644 --- a/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.cpp +++ b/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.cpp @@ -24,7 +24,7 @@ MultiRotorConnector::MultiRotorConnector(VehiclePawnWrapper* wrapper, vehicle_.initialize(vehicle_params_, wrapper_->getPose(), wrapper_->getHomePoint(), environment_); - controller_ = static_cast(vehicle_.getController()); + controller_ = static_cast(vehicle_.getController()); if (controller_->getRemoteControlID() >= 0) detectUsbRc(); @@ -38,7 +38,7 @@ MultiRotorConnector::MultiRotorConnector(VehiclePawnWrapper* wrapper, did_reset_ = false; wrapper_->setApi(std::unique_ptr( - new MultirotorApi(this))); + new MultirotorApiBase(this))); std::string message; if (!vehicle_.getController()->isAvailable(message)) { @@ -61,7 +61,7 @@ MultiRotorConnector::~MultiRotorConnector() { } -msr::airlib::VehicleControllerBase* MultiRotorConnector::getController() +msr::airlib::VehicleApiBase* MultiRotorConnector::getController() { return controller_; } @@ -310,9 +310,9 @@ void MultiRotorConnector::setCameraOrientation(int camera_id, const Quaternionr& }, true); } -msr::airlib::MultirotorApi* MultiRotorConnector::getApi() const +msr::airlib::MultirotorApiBase* MultiRotorConnector::getApi() const { - return static_cast(wrapper_->getApi()); + return static_cast(wrapper_->getApi()); } diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.h b/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.h index b4e6f67eaa..4f89e9de7d 100644 --- a/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.h +++ b/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.h @@ -3,7 +3,7 @@ #include "CoreMinimal.h" //TODO: all code except setRotorSpeed requires VehiclePawnBase. //May be we should have MultiRotorPawnBase so we don't need FlyingPawn.h -#include "vehicles/multirotor/api/MultirotorApi.hpp" +#include "vehicles/multirotor/api/MultirotorApiBase.h" #include "VehiclePawnWrapper.h" #include "vehicles/multirotor/MultiRotor.hpp" #include "vehicles/multirotor/MultiRotorParams.hpp" @@ -40,7 +40,7 @@ class MultiRotorConnector : public msr::airlib::VehicleConnectorBase virtual void updateRenderedState(float dt) override; virtual void updateRendering(float dt) override; - virtual msr::airlib::VehicleControllerBase* getController() override; + virtual msr::airlib::VehicleApiBase* getController() override; //PhysicsBody interface //this just wrapped around MultiRotor physics body @@ -69,7 +69,7 @@ class MultiRotorConnector : public msr::airlib::VehicleConnectorBase void detectUsbRc(); const msr::airlib::RCData& getRCData(); void resetPrivate(); - msr::airlib::MultirotorApi* getApi() const; + msr::airlib::MultirotorApiBase* getApi() const; private: MultiRotor vehicle_; @@ -90,7 +90,7 @@ class MultiRotorConnector : public msr::airlib::VehicleConnectorBase CollisionResponseInfo collision_response_info; - msr::airlib::DroneControllerBase* controller_; + msr::airlib::MultirotorApiBase* controller_; UManualPoseController* manual_pose_controller_; SimJoyStick joystick_; diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp index ec600e4728..1724d56e24 100644 --- a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp +++ b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp @@ -5,7 +5,7 @@ #include "GameFramework/PlayerController.h" #include "AirBlueprintLib.h" -#include "vehicles/multirotor/controllers/DroneControllerBase.hpp" +#include "vehicles/multirotor/controllers/MultirotorApiBase.h" #include "physics/PhysicsBody.hpp" #include "common/ClockFactory.hpp" #include diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index 8cf4094e0c..25cb9b75ed 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -66,7 +66,7 @@ void ASimModeBase::EndPlay(const EEndPlayReason::Type EndPlayReason) Super::EndPlay(EndPlayReason); } -msr::airlib::SimModeApiBase* ASimModeBase::getSimModeApi() const +msr::airlib::WorldSimApiBase* ASimModeBase::getSimModeApi() const { return simmode_api_.get(); } diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h index 578fb99a71..8e82c4afd6 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h @@ -11,7 +11,7 @@ #include "common/ClockFactory.hpp" #include "Engine/DirectionalLight.h" #include "api/ApiServerBase.hpp" -#include "api/SimModeApiBase.hpp" +#include "api/WorldSimApiBase.hpp" #include "SimModeBase.generated.h" @@ -63,7 +63,7 @@ class AIRSIM_API ASimModeBase : public AActor virtual const AirSimSettings& getSettings() const; long long getPhysicsLoopPeriod() const; void setPhysicsLoopPeriod(long long period); - msr::airlib::SimModeApiBase* getSimModeApi() const; + msr::airlib::WorldSimApiBase* getSimModeApi() const; virtual void setupClockSpeed(); protected: //settings @@ -78,7 +78,7 @@ class AIRSIM_API ASimModeBase : public AActor typedef msr::airlib::TTimeDelta TTimeDelta; - class SimModeApi : public msr::airlib::SimModeApiBase { + class SimModeApi : public msr::airlib::WorldSimApiBase { public: SimModeApi(ASimModeBase* simmode); virtual msr::airlib::VehicleApiBase* getVehicleApi() override;