changed simGetPosition,Orientation APIs to simGetPose, resolve Unreal…

… crash issues, update kinematics with thread safety

AirLib/include/common/common_utils/ProsumerQueue.hpp

@@ -6,6 +6,7 @@
 
 #include <queue>
 #include <thread>
+#include <atomic>
 #include <mutex>
 #include <condition_variable>
 
@@ -33,7 +34,10 @@ template <typename T>
 class ProsumerQueue
 {
 public:
-    ProsumerQueue()=default;
+    ProsumerQueue()
+    {
+        is_done_ = false;
+    }
 
     T pop() 
     {
@@ -86,6 +90,18 @@ class ProsumerQueue
         cond_.notify_one();
     }
 
+    //is_done_ flag is just convinience flag for external use
+    //its not used by this class
+    bool getIsDone()
+    {
+        return is_done_;
+    }
+    void setIsDone(bool val)
+    {
+        is_done_ = val;
+    }
+
+
     // non-copiable
     ProsumerQueue(const ProsumerQueue&) = delete;            
     ProsumerQueue& operator=(const ProsumerQueue&) = delete; 
@@ -94,6 +110,7 @@ class ProsumerQueue
     std::queue<T> queue_;
     std::mutex mutex_;
     std::condition_variable cond_;
+    std::atomic<bool> is_done_;
 };
 
 }

AirLib/include/controllers/DroneControllerBase.hpp

@@ -240,14 +240,15 @@ class DroneControllerBase : public VehicleControllerBase {
 
 
     //TODO: methods that are only valid for simulation mode should be prefixed with "sim"
-    virtual void simSetPosition(const Vector3r& position);
-    virtual void simSetOrientation(const Quaternionr& orientation);
+    virtual void simSetPose(const Vector3r& position, const Quaternionr& orientation);
     /// Request an image of specific type from the specified camera.  Currently default pawn is configured with only 2 cameras which
     /// have id of 0 and 1, right and left respectively.  Camera 0 is is always FPV camera view
     /// The image is return in the .png format.  This call will block until the render is complete.  
     virtual vector<VehicleCameraBase::ImageResponse> simGetImages(const vector<ImageRequest>& request);
     void simAddCamera(VehicleCameraBase* camera);
     VehicleCameraBase* simGetCamera(int index);
+    //notifies the controller that rendering was completed by simulated
+    virtual void simNotifyRender();
 
     //*********************************common pre & post for move commands***************************************************
     //TODO: make these protected

AirLib/include/controllers/DroneControllerCancelable.hpp

@@ -180,13 +180,9 @@ class DroneControllerCancelable  {
         return controller_->getVelocity();
     }
 
-    void simSetPosition(const Vector3r& position)
+    void simSetPose(const Vector3r& position, const Quaternionr& orientation)
     {
-        controller_->simSetPosition(position);
-    }
-    void simSetOrientation(const Quaternionr& orientation)
-    {
-        controller_->simSetOrientation(orientation);
+        controller_->simSetPose(position, orientation);
     }
     vector<VehicleCameraBase::ImageResponse> simGetImages(const vector<DroneControllerBase::ImageRequest>& request)
     {

AirLib/include/controllers/simple_flight/SimpleFlightDroneController.hpp

@@ -245,22 +245,38 @@ class SimpleFlightDroneController : public DroneControllerBase {
     }
 
 
-    void simSetPosition(const Vector3r& position) override
+    void simSetPose(const Vector3r& position, const Quaternionr& orientation) override
     {
-        auto kinematics = physics_body_->getKinematics();
-        kinematics.pose.position = position;
-        physics_body_->setKinematics(kinematics);
+        pending_pose_ = Pose(position, orientation);
+        waitForRender();
     }
-    void simSetOrientation(const Quaternionr& orientation) override
+
+    void simNotifyRender() override
     {
+        std::unique_lock<std::mutex> render_wait_lock(render_mutex_);
+
         auto kinematics = physics_body_->getKinematics();
-        kinematics.pose.orientation = orientation;
+        if (! VectorMath::hasNan(pending_pose_.position))
+            kinematics.pose.position = pending_pose_.position;
+        if (! VectorMath::hasNan(pending_pose_.orientation))
+            kinematics.pose.orientation = pending_pose_.orientation;
         physics_body_->setKinematics(kinematics);
+
+        is_pose_update_done_ = true;
+        render_wait_lock.unlock();
+        render_cond_.notify_all();
     }
 
     //*** End: DroneControllerBase implementation ***//
 
 private:
+    void waitForRender()
+    {
+        std::unique_lock<std::mutex> render_wait_lock(render_mutex_);
+        is_pose_update_done_ = false;
+        render_cond_.wait(render_wait_lock, [this]{return is_pose_update_done_;});
+    }
+
     //convert pitch, roll, yaw from -1 to 1 to PWM
     static uint16_t angleToPwm(float angle)
     {
@@ -286,6 +302,12 @@ class SimpleFlightDroneController : public DroneControllerBase {
     unique_ptr<AirSimSimpleFlightCommLink> comm_link_;
     unique_ptr<AirSimSimpleFlightEstimator> estimator_;
     unique_ptr<simple_flight::Firmware> firmware_;
+
+    std::mutex render_mutex_;
+    std::condition_variable render_cond_;
+    bool is_pose_update_done_;
+    Pose pending_pose_;
+
 };
 
 }} //namespace

AirLib/include/rpc/RpcLibClient.hpp

@@ -19,7 +19,7 @@ class RpcLibClient {
         Initial, Connected, Disconnected, Reset, Unknown
     };
 public:
-    RpcLibClient(const string& ip_address = "localhost", uint16_t port = 41451);
+    RpcLibClient(const string& ip_address = "localhost", uint16_t port = 41451, uint timeout_ms = 60000);
     ConnectionState getConnectionState();
     bool ping();
     bool armDisarm(bool arm);
@@ -47,8 +47,7 @@ class RpcLibClient {
     bool rotateByYawRate(float yaw_rate, float duration);
     bool hover();
 
-    void simSetPosition(const Vector3r& position);
-    void simSetOrientation(const Quaternionr& orientation);
+    void simSetPose(const Vector3r& position, const Quaternionr& orientation);
     vector<VehicleCameraBase::ImageResponse> simGetImages(vector<DroneControllerBase::ImageRequest> request);
     vector<uint8_t> simGetImage(int camera_id, VehicleCameraBase::ImageType type);
 

AirLib/src/controllers/DroneControllerBase.cpp

@@ -330,19 +330,18 @@ bool DroneControllerBase::hover(CancelableBase& cancelable_action)
     return moveToZ(getZ(), 0.5f, YawMode{ true,0 }, 1.0f, false, cancelable_action);
 }
 
-void DroneControllerBase::simSetPosition(const Vector3r& position)
-{
-    //derived flight controller class should provide implementation if they support exclusive sim*** methods
-}
-void DroneControllerBase::simSetOrientation(const Quaternionr& orientation)
+void DroneControllerBase::simSetPose(const Vector3r& position, const Quaternionr& orientation)
 {
     //derived flight controller class should provide implementation if they support exclusive sim*** methods
 }
 void DroneControllerBase::simAddCamera(VehicleCameraBase* camera)
 {
     cameras_.push_back(camera);
 }
-
+void DroneControllerBase::simNotifyRender()
+{
+    //derived class should override this if it supports sim**** methods
+}
 VehicleCameraBase* DroneControllerBase::simGetCamera(int index)
 {
     return cameras_.at(index);

AirLib/src/rpc/RpcLibClient.cpp

@@ -30,21 +30,21 @@ __pragma(warning( disable : 4239))
 namespace msr { namespace airlib {
 
 struct RpcLibClient::impl {
-    impl(const string&  ip_address, uint16_t port)
+    impl(const string&  ip_address, uint16_t port, uint timeout_ms)
         : client(ip_address, port)
     {
         // some long flight path commands can take a while, so we give it up to 1 hour max.
-        client.set_timeout(3600*1000);
+        client.set_timeout(timeout_ms);
     }
 
     rpc::client client;
 };
 
 typedef msr::airlib_rpclib::RpcLibAdapators RpcLibAdapators;
 
-RpcLibClient::RpcLibClient(const string&  ip_address, uint16_t port)
+RpcLibClient::RpcLibClient(const string&  ip_address, uint16_t port, uint timeout_ms)
 {
-    pimpl_.reset(new impl(ip_address, port));
+    pimpl_.reset(new impl(ip_address, port, timeout_ms));
 }
 
 RpcLibClient::~RpcLibClient()
@@ -159,13 +159,9 @@ bool RpcLibClient::setSafety(SafetyEval::SafetyViolationType enable_reasons, flo
 }
 
 //sim only
-void RpcLibClient::simSetPosition(const Vector3r& position)
+void RpcLibClient::simSetPose(const Vector3r& position, const Quaternionr& orientation)
 {
-    pimpl_->client.call("simSetPosition", RpcLibAdapators::Vector3r(position));
-}
-void RpcLibClient::simSetOrientation(const Quaternionr& orientation)
-{
-    pimpl_->client.call("simSetOrientation", RpcLibAdapators::Quaternionr(orientation));
+    pimpl_->client.call("simSetPose", RpcLibAdapators::Vector3r(position), RpcLibAdapators::Quaternionr(orientation));
 }
 vector<VehicleCameraBase::ImageResponse> RpcLibClient::simGetImages(vector<DroneControllerBase::ImageRequest> request)
 {

AirLib/src/rpc/RpcLibServer.cpp

@@ -86,15 +86,15 @@ RpcLibServer::RpcLibServer(DroneControllerCancelable* drone, string server_addre
         bool { return drone_->rotateByYawRate(yaw_rate, duration); });
     pimpl_->server.bind("hover", [&]() -> bool { return drone_->hover(); });
 
-    pimpl_->server.bind("setSafety", [&](uint enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy,
+    pimpl_->server.bind("setSafety", [&](uint enable_reasons, float obs_clearance, const SafetyEval::ObsAvoidanceStrategy& obs_startegy,
         float obs_avoidance_vel, const RpcLibAdapators::Vector3r& origin, float xy_length, float max_z, float min_z) -> 
         bool { return drone_->setSafety(SafetyEval::SafetyViolationType(enable_reasons), obs_clearance, obs_startegy,
             obs_avoidance_vel, origin.to(), xy_length, max_z, min_z); });
 
     //sim only
-    pimpl_->server.bind("simSetPosition", [&](RpcLibAdapators::Vector3r position) -> void { drone_->simSetPosition(position.to()); });
-    pimpl_->server.bind("simSetOrientation", [&](RpcLibAdapators::Quaternionr orientation) -> void { drone_->simSetOrientation(orientation.to()); });
-    pimpl_->server.bind("simGetImages", [&](std::vector<RpcLibAdapators::ImageRequest> request_adapter) -> vector<RpcLibAdapators::ImageResponse> { 
+    pimpl_->server.bind("simSetPose", [&](const RpcLibAdapators::Vector3r &position, const RpcLibAdapators::Quaternionr& orientation) -> 
+        void { drone_->simSetPose(position.to(), orientation.to()); });
+    pimpl_->server.bind("simGetImages", [&](const std::vector<RpcLibAdapators::ImageRequest>& request_adapter) -> vector<RpcLibAdapators::ImageResponse> { 
         const auto& response = drone_->simGetImages(RpcLibAdapators::ImageRequest::to(request_adapter)); 
         return RpcLibAdapators::ImageResponse::from(response);
     });

AirSim.sln

@@ -30,6 +30,7 @@ Project("{2150E333-8FDC-42A3-9474-1A3956D46DE8}") = "AirSim", "AirSim", "{6B6941
 		Unreal\Plugins\AirSim\Source\AirSim.Build.cs = Unreal\Plugins\AirSim\Source\AirSim.Build.cs
 		Unreal\Plugins\AirSim\Source\AirSim.cpp = Unreal\Plugins\AirSim\Source\AirSim.cpp
 		Unreal\Plugins\AirSim\Source\AirSim.h = Unreal\Plugins\AirSim\Source\AirSim.h
+		Unreal\Plugins\AirSim\AirSim.uplugin = Unreal\Plugins\AirSim\AirSim.uplugin
 		Unreal\Plugins\AirSim\Source\AirSimGameMode.cpp = Unreal\Plugins\AirSim\Source\AirSimGameMode.cpp
 		Unreal\Plugins\AirSim\Source\AirSimGameMode.h = Unreal\Plugins\AirSim\Source\AirSimGameMode.h
 		Unreal\Plugins\AirSim\Source\CameraDirector.cpp = Unreal\Plugins\AirSim\Source\CameraDirector.cpp

Examples/Examples.vcxproj

@@ -185,7 +185,7 @@
     <ClCompile Include="main.cpp" />
   </ItemGroup>
   <ItemGroup>
-    <ClInclude Include="PoseGenerators.hpp" />
+    <ClInclude Include="RandomPointPoseGenerator.hpp" />
     <ClInclude Include="StandAlonePhysics.hpp" />
     <ClInclude Include="StandAloneSensors.hpp" />
     <ClInclude Include="StereoImageGenerator.hpp" />

Examples/Examples.vcxproj.filters

@@ -29,7 +29,7 @@
     <ClInclude Include="StereoImageGenerator.hpp">
       <Filter>Header Files</Filter>
     </ClInclude>
-    <ClInclude Include="PoseGenerators.hpp">
+    <ClInclude Include="RandomPointPoseGenerator.hpp">
       <Filter>Header Files</Filter>
     </ClInclude>
   </ItemGroup>