cv mode working

AirLib/include/controllers/DroneControllerBase.hpp

@@ -229,6 +229,10 @@ class DroneControllerBase : public VehicleControllerBase {
 
     virtual void addCamera(std::shared_ptr<VehicleCamera> camera);
 
+    //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);
+
     //*********************************common pre & post for move commands***************************************************
     //TODO: make these protected
     virtual bool loopCommandPre();

AirLib/include/controllers/DroneControllerCancelable.hpp

@@ -180,6 +180,15 @@ class DroneControllerCancelable  {
         return controller_->getVelocity();
     }
 
+    void simSetPosition(const Vector3r& position)
+    {
+        controller_->simSetPosition(position);
+    }
+    void simSetOrientation(const Quaternionr& orientation)
+    {
+        controller_->simSetOrientation(orientation);
+    }
+
     Quaternionr getOrientation()
     {
         return controller_->getOrientation();

AirLib/include/controllers/VehicleControllerBase.hpp

@@ -5,6 +5,7 @@
 #define msr_airlib_VehicleControllerBase_hpp
 
 #include "controllers/ControllerBase.hpp"
+#include "physics/PhysicsBody.hpp"
 #include <exception>
 #include <string>
 
@@ -20,6 +21,13 @@ class VehicleControllerBase : public ControllerBase {
     virtual void setSimulationMode(bool is_set) = 0;
     virtual bool isOffboardMode() = 0;
     virtual bool isSimulationMode() = 0;
+
+    //TODO: below method is needed to support firmwares without state estimation. In future, we should probably remove this support.
+    virtual void initializePhysics(PhysicsBody* physics_body)
+    {
+        unused(physics_body);
+        //by default don't use it. If derived class needs this, it should override.
+    }
 };
 
 class VehicleControllerException : public ControllerException {

AirLib/include/controllers/ros_flight/RosFlightDroneController.hpp

@@ -38,10 +38,10 @@ class RosFlightDroneController : public DroneControllerBase {
         remote_control_id_ = child.getInt("RemoteControlID", 0);
     }
 
-    void initializePhysics(const Environment* environment, const Kinematics::State* kinematics)
+    void initializePhysics(PhysicsBody* physics_body) override
     {
-        environment_ = environment;
-        kinematics_ = kinematics;
+        environment_ = & physics_body->getEnvironment();
+        kinematics_ = & physics_body->getKinematics();
     }
 
 public:

AirLib/include/controllers/simple_flight/SimpleFlightDroneController.hpp

@@ -40,12 +40,12 @@ class SimpleFlightDroneController : public DroneControllerBase {
         remote_control_id_ = child.getInt("RemoteControlID", 0);
     }
 
-    void initializePhysics(const Environment* environment, const Kinematics::State* kinematics)
+    void initializePhysics(PhysicsBody* physics_body) override
     {
-        environment_ = environment;
-        kinematics_ = kinematics;
-        board_->setKinematics(kinematics_);
-        estimator_->setKinematics(kinematics_);
+        physics_body_ = physics_body;
+
+        board_->setKinematics(& physics_body_->getKinematics());
+        estimator_->setKinematics(& physics_body_->getKinematics());
     }
 
 public:
@@ -243,6 +243,21 @@ class SimpleFlightDroneController : public DroneControllerBase {
         static const VehicleParams safety_params;
         return safety_params;
     }
+
+
+    void simSetPosition(const Vector3r& position) override
+    {
+        auto kinematics = physics_body_->getKinematics();
+        kinematics.pose.position = position;
+        physics_body_->setKinematics(kinematics);
+    }
+    void simSetOrientation(const Quaternionr& orientation) override
+    {
+        auto kinematics = physics_body_->getKinematics();
+        kinematics.pose.orientation = orientation;
+        physics_body_->setKinematics(kinematics);
+    }
+
     //*** End: DroneControllerBase implementation ***//
 
 private:
@@ -264,6 +279,7 @@ class SimpleFlightDroneController : public DroneControllerBase {
     const MultiRotorParams* vehicle_params_;
     const Kinematics::State* kinematics_;
     const Environment* environment_;
+    PhysicsBody* physics_body_;
 
     int remote_control_id_ = 0;
     simple_flight::Params params_;

AirLib/include/physics/World.hpp

@@ -27,10 +27,10 @@ class World : public UpdatableContainer<UpdatableObject*> {
     {
         World::clear();
 
-        if (physics_engine) {
-            physics_engine_ = physics_engine;
+        physics_engine_ = physics_engine;
+        if (physics_engine)
             physics_engine_->clear();
-        }
+
         World::reset();
     }
 

AirLib/include/rpc/RpcLibClient.hpp

@@ -47,6 +47,9 @@ class RpcLibClient {
     bool rotateByYawRate(float yaw_rate, float duration);
     bool hover();
 
+    void simSetPosition(const Vector3r& position);
+    void simSetOrientation(const Quaternionr& orientation);
+
     Vector3r getPosition();
     CollisionInfo getCollisionInfo();
     Vector3r getVelocity();

AirLib/include/vehicles/MultiRotorParams.hpp

@@ -15,9 +15,6 @@
 #include "sensors/gps/GpsSimple.hpp"
 #include "sensors/magnetometer/MagnetometerSimple.hpp"
 
-//below includes are because of setPhysicsGroundTruth methog
-#include "physics/Environment.hpp"
-#include "physics/Kinematics.hpp"
 
 namespace msr { namespace airlib {
 
@@ -86,14 +83,6 @@ class MultiRotorParams {
         return controller_.get();
     }
 
-    //TODO: below method is needed to support firmwares without state estimation. In future, we should probably remove this support.
-    virtual void initializePhysics(const Environment* environment, const Kinematics::State* kinematics)
-    {
-        unused(environment);
-        unused(kinematics);
-        //by default don't use it. If derived class needs this, it should override.
-    }
-
 protected: //must override by derived class
     //this method must clean up any previous initializations
     virtual void setup(Params& params, SensorCollection& sensors, unique_ptr<DroneControllerBase>& controller) = 0;

AirLib/include/vehicles/configs/RosFlightQuadX.hpp

@@ -17,12 +17,6 @@ class RosFlightQuadX : public MultiRotorParams {
     {
     }
 
-    virtual void initializePhysics(const Environment* environment, const Kinematics::State* kinematics) override
-    {
-        //supply this to controller so it can use physics ground truth instead of state estimation (because ROSFlight doesn't have state estimation)
-        static_cast<RosFlightDroneController*>(getController())->initializePhysics(environment_, kinematics_);
-    }
-
 protected:
     virtual void setup(Params& params, SensorCollection& sensors, unique_ptr<DroneControllerBase>& controller) override
     {
@@ -73,8 +67,6 @@ class RosFlightQuadX : public MultiRotorParams {
 
 private:
     vector<unique_ptr<SensorBase>> sensor_storage_;
-    const Kinematics::State* kinematics_;
-    const Environment* environment_;
 };
 
 }} //namespace

AirLib/include/vehicles/configs/SimpleFlightQuadX.hpp

@@ -18,12 +18,6 @@ class SimpleFlightQuadX : public MultiRotorParams {
         unused(settings);
     }
 
-    virtual void initializePhysics(const Environment* environment, const Kinematics::State* kinematics) override
-    {
-        //supply this to controller so it can use physics ground truth instead of state estimation (because ROSFlight doesn't have state estimation)
-        static_cast<SimpleFlightDroneController*>(getController())->initializePhysics(environment, kinematics);
-    }
-
 protected:
     virtual void setup(Params& params, SensorCollection& sensors, unique_ptr<DroneControllerBase>& controller) override
     {

AirLib/src/controllers/DroneControllerBase.cpp

@@ -330,6 +330,15 @@ 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)
+{
+    //derived flight controller class should provide implementation if they support exclusive sim*** methods
+}
+
 bool DroneControllerBase::moveByVelocity(float vx, float vy, float vz, const YawMode& yaw_mode)
 {
     if (safetyCheckVelocity(Vector3r(vx, vy, vz)))

AirLib/src/rpc/RpcLibClient.cpp

@@ -158,6 +158,16 @@ bool RpcLibClient::setSafety(SafetyEval::SafetyViolationType enable_reasons, flo
         obs_avoidance_vel, RpcLibAdapators::Vector3r(origin), xy_length, max_z, min_z).as<bool>();
 }
 
+//sim only
+void RpcLibClient::simSetPosition(const Vector3r& position)
+{
+    pimpl_->client.call("simSetPosition", RpcLibAdapators::Vector3r(position));
+}
+void RpcLibClient::simSetOrientation(const Quaternionr& orientation)
+{
+    pimpl_->client.call("simSetOrientation", RpcLibAdapators::Quaternionr(orientation));
+}
+
 //status getters
 Vector3r RpcLibClient::getPosition()
 {

AirLib/src/rpc/RpcLibServer.cpp

@@ -100,6 +100,9 @@ RpcLibServer::RpcLibServer(DroneControllerCancelable* drone, string server_addre
         return result;
     });
 
+    //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()); });
 
     //getters
     pimpl_->server.bind("getPosition", [&]() -> RpcLibAdapators::Vector3r { return drone_->getPosition(); });

AirSim.sln

@@ -36,6 +36,8 @@ Project("{2150E333-8FDC-42A3-9474-1A3956D46DE8}") = "AirSim", "AirSim", "{6B6941
 		Unreal\Plugins\AirSim\Source\CameraDirector.h = Unreal\Plugins\AirSim\Source\CameraDirector.h
 		Unreal\Plugins\AirSim\Source\FlyingPawn.cpp = Unreal\Plugins\AirSim\Source\FlyingPawn.cpp
 		Unreal\Plugins\AirSim\Source\FlyingPawn.h = Unreal\Plugins\AirSim\Source\FlyingPawn.h
+		Unreal\Plugins\AirSim\Source\ManualPoseController.cpp = Unreal\Plugins\AirSim\Source\ManualPoseController.cpp
+		Unreal\Plugins\AirSim\Source\ManualPoseController.h = Unreal\Plugins\AirSim\Source\ManualPoseController.h
 		Unreal\Plugins\AirSim\Source\MultiRotorConnector.cpp = Unreal\Plugins\AirSim\Source\MultiRotorConnector.cpp
 		Unreal\Plugins\AirSim\Source\MultiRotorConnector.h = Unreal\Plugins\AirSim\Source\MultiRotorConnector.h
 		Unreal\Plugins\AirSim\Source\PIPCamera.cpp = Unreal\Plugins\AirSim\Source\PIPCamera.cpp

Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp

@@ -194,16 +194,58 @@ FInputActionBinding& UAirBlueprintLib::BindActionToKey(const FName action_name,
 
 
 template<class UserClass>
-FInputAxisBinding& UAirBlueprintLib::BindAxisToKey(const FName axis_name, const FKey in_key, UserClass* actor, 
+FInputAxisBinding& UAirBlueprintLib::BindAxisToKey(const FName axis_name, const FKey in_key, AActor* actor, UserClass* obj, 
     typename FInputAxisHandlerSignature::TUObjectMethodDelegate<UserClass>::FMethodPtr func)
 {
     FInputAxisKeyMapping axis(axis_name, in_key);
 
+    return UAirBlueprintLib::BindAxisToKey(axis, actor, obj, func);
+}
+
+template<class UserClass>
+FInputAxisBinding& UAirBlueprintLib::BindAxisToKey(const FInputAxisKeyMapping& axis, AActor* actor, UserClass* obj,
+    typename FInputAxisHandlerSignature::TUObjectMethodDelegate<UserClass>::FMethodPtr func)
+{
     APlayerController* controller = actor->GetWorld()->GetFirstPlayerController();
 
     controller->PlayerInput->AddAxisMapping(axis);
     return controller->InputComponent->
-        BindAxis(axis_name, actor, func);
+        BindAxis(axis.AxisName, obj, func);
+}
+
+
+int UAirBlueprintLib::RemoveAxisBinding(const FInputAxisKeyMapping& axis, FInputAxisBinding* axis_binding, AActor* actor)
+{
+    if (axis_binding != nullptr && actor != nullptr) {
+        APlayerController* controller = actor->GetWorld()->GetFirstPlayerController();
+
+        //remove mapping
+        int found_mapping_index = -1, cur_mapping_index = -1;
+        for (const auto& axis_arr : controller->PlayerInput->AxisMappings) {
+            ++cur_mapping_index;
+            if (axis_arr.AxisName == axis.AxisName && axis_arr.Key == axis.Key) {
+                found_mapping_index = cur_mapping_index;
+                break;
+            }
+        }
+        if (found_mapping_index >= 0)
+            controller->PlayerInput->AxisMappings.RemoveAt(found_mapping_index);
+
+        //removing binding
+        int found_binding_index = -1, cur_binding_index = -1;
+        for (const auto& axis_arr : controller->InputComponent->AxisBindings) {
+            ++cur_binding_index;
+            if (axis_arr.AxisName == axis_binding->AxisName) {
+                found_binding_index = cur_binding_index;
+                break;
+            }
+        }
+        if (found_binding_index >= 0)
+            controller->InputComponent->AxisBindings.RemoveAt(found_binding_index);
+
+        return found_binding_index;
+    }
+    else return -1;
 }
 
 void UAirBlueprintLib::EnableInput(AActor* actor)

Unreal/Plugins/AirSim/Source/AirBlueprintLib.h

@@ -56,8 +56,13 @@ class UAirBlueprintLib : public UBlueprintFunctionLibrary
         bool shift_key = false, bool control_key = false, bool alt_key = false, bool command_key = false);
 
     template<class UserClass>
-    static FInputAxisBinding& BindAxisToKey(const FName axis_name, const FKey in_key, UserClass* actor,
+    static FInputAxisBinding& BindAxisToKey(const FName axis_name, const FKey in_key, AActor* actor, UserClass* obj,
         typename FInputAxisHandlerSignature::TUObjectMethodDelegate<UserClass>::FMethodPtr func);
+    template<class UserClass>
+    static FInputAxisBinding& BindAxisToKey(const FInputAxisKeyMapping& axis, AActor* actor, UserClass* obj,
+        typename FInputAxisHandlerSignature::TUObjectMethodDelegate<UserClass>::FMethodPtr func);
+
+    static int RemoveAxisBinding(const FInputAxisKeyMapping& axis, FInputAxisBinding* axis_binding, AActor* actor);
 
     static void EnableInput(AActor* actor);