From 046356b481767132ebd03e34a18d63ed15c9a0a9 Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Wed, 28 Nov 2018 20:09:19 -0800
Subject: [PATCH] fix problem with multirotor sim on PX4 drones.

---
 PythonClient/multirotor/land.py             |  2 +-
 PythonClient/multirotor/path.py             |  2 +-
 PythonClient/multirotor/setup_path.py       |  4 ++--
 PythonClient/multirotor/survey.py           | 22 +++++++++++++++++----
 Unreal/Plugins/AirSim/Source/PawnSimApi.cpp |  7 +++++++
 5 files changed, 29 insertions(+), 8 deletions(-)

diff --git a/PythonClient/multirotor/land.py b/PythonClient/multirotor/land.py
index 6e3cd3f12b..5a591c87d2 100644
--- a/PythonClient/multirotor/land.py
+++ b/PythonClient/multirotor/land.py
@@ -11,7 +11,7 @@
     print("already landed...")
 else:
     print("landing...")
-    client.land()
+    client.landAsync()
 
 client.armDisarm(False)
 client.enableApiControl(False)
diff --git a/PythonClient/multirotor/path.py b/PythonClient/multirotor/path.py
index c192039613..b0cbdb271e 100644
--- a/PythonClient/multirotor/path.py
+++ b/PythonClient/multirotor/path.py
@@ -28,6 +28,6 @@
                         12, 120, 
                         airsim.DrivetrainType.ForwardOnly, airsim.YawMode(False,0), 20, 1).join()
 client.moveToPositionAsync(0,0,z,1).join()
-client.land()
+client.landAsync()
 client.armDisarm(False)
 client.enableApiControl(False)
diff --git a/PythonClient/multirotor/setup_path.py b/PythonClient/multirotor/setup_path.py
index 362113fea8..a33b94789c 100644
--- a/PythonClient/multirotor/setup_path.py
+++ b/PythonClient/multirotor/setup_path.py
@@ -4,7 +4,7 @@
 # Else we look up grand-parent folder to see if it has airsim folder
 #    and if it does then we add that in sys.path
 
-import os,sys,inspect,logging
+import os,sys,logging
 
 #this class simply tries to see if airsim 
 class SetupPath:
@@ -15,7 +15,7 @@ def getDirLevels(path):
 
     @staticmethod
     def getCurrentPath():
-        cur_filepath = os.path.abspath(inspect.getfile(inspect.currentframe()))
+        cur_filepath = __file__
         return os.path.dirname(cur_filepath)
 
     @staticmethod
diff --git a/PythonClient/multirotor/survey.py b/PythonClient/multirotor/survey.py
index 0801d95c10..e1a7d9f46a 100644
--- a/PythonClient/multirotor/survey.py
+++ b/PythonClient/multirotor/survey.py
@@ -15,6 +15,19 @@ def __init__(self, args):
         self.client.confirmConnection()
         self.client.enableApiControl(True)
 
+    def move_to_position(self, x, y, z, threshold=2):
+        self.client.moveToPositionAsync(0, 0, z, self.velocity).join()
+        # bufbuf: seems moveToPositionAsync isn't working right...
+        while True:
+            k = self.client.simGetGroundTruthKinematics()
+            pos = self.client.getPosition()
+            dx = abs(pos.x_val - x)
+            dy = abs(pos.y_val - y)
+            dz = abs(pos.z_val - z)
+            if dx+dy+dz < threshold:
+                return
+            time.sleep(0.1)  # 100ms should be fine        
+
     def start(self):
         print("arming the drone...")
         self.client.armDisarm(True)
@@ -29,10 +42,10 @@ def start(self):
         z = -self.altitude
 
         print("climbing to altitude: " + str(self.altitude))
-        self.client.moveToPositionAsync(0, 0, z, self.velocity).join()
+        self.move_to_position(0, 0, z)
 
         print("flying to first corner of survey box")
-        self.client.moveToPositionAsync(x, -self.boxsize, z, self.velocity).join()
+        self.move_to_position(x, -self.boxsize, z)
         
         # let it settle there a bit.
         self.client.hoverAsync().join()
@@ -66,11 +79,12 @@ def start(self):
             pass
 
         print("flying back home")
-        self.client.moveToPositionAsync(0, 0, z, self.velocity).join()
+        self.move_to_position(0, 0, z)
         
         if z < -5:
             print("descending")
-            self.client.moveToPositionAsync(0, 0, -5, 2).join()
+            self.velocity = 2
+            self.move_to_position(0, 0, -5)
 
         print("landing...")
         self.client.landAsync().join()
diff --git a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp b/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp
index f52cd6e207..f766fe7f08 100644
--- a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp
+++ b/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp
@@ -295,6 +295,12 @@ void PawnSimApi::update()
     environment_->update();
     //kinematics_->update();
 
+    // and in the case of subclasses that are overriding getGroundTruthEnvironment, make sure those 
+    // are also updated.
+    msr::airlib::Environment* vehicleEnvironment = const_cast<msr::airlib::Environment*>(getGroundTruthEnvironment());
+    vehicleEnvironment->setPosition(kinematics_.pose.position);
+    vehicleEnvironment->update();
+
     VehicleSimApiBase::update();
 }
 
@@ -418,6 +424,7 @@ void PawnSimApi::setPoseInternal(const Pose& pose, bool ignore_collision)
     //translate to new PawnSimApi position & orientation from NED to NEU
     FVector position = ned_transform_.fromLocalNed(pose.position);
     state_.current_position = position;
+    kinematics_.pose = pose;
 
     //quaternion formula comes from http://stackoverflow.com/a/40334755/207661
     FQuat orientation = ned_transform_.fromNed(pose.orientation);