added gimble API

himankmaan · Feb 21, 2018 · ce85db7 · ce85db7
1 parent 8554165
commit ce85db7
Show file tree

Hide file tree

Showing 16 changed files with 89 additions and 25 deletions.
diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp
@@ -40,7 +40,8 @@ class RpcLibClientBase {
     void simPrintLogMessage(const std::string& message, std::string message_param = "", unsigned char severity = 0);
 
     Pose simGetObjectPose(const std::string& object_name);
-    CameraInfo getCameraInfo(int cameta_id);
+    CameraInfo getCameraInfo(int camera_id);
+    void setCameraOrientation(int camera_id, const Quaternionr& orientation);
 
     virtual ~RpcLibClientBase();    //required for pimpl
 

diff --git a/AirLib/include/api/VehicleApiBase.hpp b/AirLib/include/api/VehicleApiBase.hpp
@@ -32,8 +32,9 @@ class VehicleApiBase {
 
     virtual Pose simGetObjectPose(const std::string& object_name) = 0;
 
-    virtual CameraInfo getCameraInfo(int cameta_id) const = 0;
-
+    virtual CameraInfo getCameraInfo(int camera_id) const = 0;
+    virtual void setCameraOrientation(int camera_id, const Quaternionr& orientation) = 0;
+
     virtual ~VehicleApiBase() = default;
 };
 

diff --git a/AirLib/include/controllers/VehicleConnectorBase.hpp b/AirLib/include/controllers/VehicleConnectorBase.hpp
@@ -34,7 +34,8 @@ class VehicleConnectorBase : public UpdatableObject
     virtual void printLogMessage(const std::string& message, std::string message_param = "", unsigned char severity = 0) = 0;
     virtual Pose getActorPose(const std::string& actor_name) = 0;
     virtual Kinematics::State getTrueKinematics() = 0;
-    virtual CameraInfo getCameraInfo(int cameta_id) const = 0;
+    virtual CameraInfo getCameraInfo(int camera_id) const = 0;
+    virtual void setCameraOrientation(int camera_id, const Quaternionr& orientation) = 0;
 };
 
 

diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp
@@ -281,9 +281,14 @@ class MultirotorApi : public VehicleApiBase {
         return vehicle_->getSegmentationObjectID(mesh_name);
     }
 
-    virtual CameraInfo getCameraInfo(int cameta_id) const override
+    virtual CameraInfo getCameraInfo(int camera_id) const override
     {
-        return vehicle_->getCameraInfo(cameta_id);
+        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

diff --git a/AirLib/include/vehicles/multirotor/controllers/RealMultirotorConnector.hpp b/AirLib/include/vehicles/multirotor/controllers/RealMultirotorConnector.hpp
@@ -94,12 +94,20 @@ class RealMultirotorConnector : public VehicleConnectorBase
         return msr::airlib::Pose();
     }
 
-    virtual CameraInfo getCameraInfo(int cameta_id) const override
+    virtual CameraInfo getCameraInfo(int camera_id) const override
     {
-        unused(cameta_id);
+        unused(camera_id);
         throw std::logic_error("getCameraInfo() call is not implemented for this vehicle");
     }
 
+    virtual void setCameraOrientation(int camera_id, const Quaternionr& orientation) override
+    {
+        unused(camera_id);
+        unused(orientation);
+
+        throw std::logic_error("setCameraOrientation() call is not implemented for this vehicle");
+    }
+
 private:
     VehicleControllerBase* controller_;
 };

diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp
@@ -152,9 +152,14 @@ void* RpcLibClientBase::getClient()
     return &pimpl_->client;
 }
 
-CameraInfo RpcLibClientBase::getCameraInfo(int cameta_id)
+CameraInfo RpcLibClientBase::getCameraInfo(int camera_id)
 {
-    return pimpl_->client.call("getCameraInfo", cameta_id).as<RpcLibAdapatorsBase::CameraInfo>().to();
+    return pimpl_->client.call("getCameraInfo", camera_id).as<RpcLibAdapatorsBase::CameraInfo>().to();
+}
+
+void RpcLibClientBase::setCameraOrientation(int camera_id, const Quaternionr& orientation)
+{
+    pimpl_->client.call("setCameraOrientation", camera_id, RpcLibAdapatorsBase::Quaternionr(orientation));
 }
 
 CollisionInfo RpcLibClientBase::getCollisionInfo()

diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp
@@ -100,11 +100,15 @@ RpcLibServerBase::RpcLibServerBase(VehicleApiBase* vehicle, string server_addres
         return RpcLibAdapatorsBase::GeoPoint(geo_point);
     });
 
-    pimpl_->server.bind("getCameraInfo", [&](int cameta_id) -> RpcLibAdapatorsBase::CameraInfo {
-        const auto& camera_info = vehicle_->getCameraInfo(cameta_id);
+    pimpl_->server.bind("getCameraInfo", [&](int camera_id) -> RpcLibAdapatorsBase::CameraInfo {
+        const auto& camera_info = vehicle_->getCameraInfo(camera_id);
         return RpcLibAdapatorsBase::CameraInfo(camera_info);
     });
 
+    pimpl_->server.bind("setCameraOrientation", [&](int camera_id, const RpcLibAdapatorsBase::Quaternionr& orientation) -> void {
+        vehicle_->setCameraOrientation(camera_id, orientation.to());
+    });
+
     pimpl_->server.bind("enableApiControl", [&](bool is_enabled) -> void { vehicle_->enableApiControl(is_enabled); });
     pimpl_->server.bind("isApiControlEnabled", [&]() -> bool { return vehicle_->isApiControlEnabled(); });
 

diff --git a/PythonClient/AirSimClient.py b/PythonClient/AirSimClient.py
@@ -255,8 +255,11 @@ def simGetImages(self, requests):
     def getCollisionInfo(self):
         return CollisionInfo.from_msgpack(self.client.call('getCollisionInfo'))
 
-    def getCameraInfo(self, cameta_id):
-        return CameraInfo.from_msgpack(self.client.call('getCameraInfo', cameta_id))
+    def getCameraInfo(self, camera_id):
+        return CameraInfo.from_msgpack(self.client.call('getCameraInfo', camera_id))
+
+    def setCameraOrientation(self, camera_id, orientation):
+        self.client.call('setCameraOrientation', camera_id, orientation)
 
     @staticmethod
     def stringToUint8Array(bstr):

diff --git a/PythonClient/cv_mode.py b/PythonClient/cv_mode.py
@@ -6,9 +6,13 @@
 
 pp = pprint.PrettyPrinter(indent=4)
 
-client = CarClient()
+client = MultirotorClient()
 client.confirmConnection()
 
+
+AirSimClientBase.wait_key('Press any key to set camera-0 gimble to 15-degree pitch')
+client.setCameraOrientation(0, AirSimClientBase.toQuaternion(0.261799, 0, 0)); #radians
+
 AirSimClientBase.wait_key('Press any key to get camera parameters')
 for camera_id in range(5):
     camera_info = client.getCameraInfo(camera_id)

diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp b/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp
@@ -87,9 +87,16 @@ const CarApiBase::CarControls& CarPawnApi::getCarControls() const
     return last_controls_;
 }
 
-msr::airlib::CameraInfo CarPawnApi::getCameraInfo(int cameta_id) const
+msr::airlib::CameraInfo CarPawnApi::getCameraInfo(int camera_id) const
 {
-    return pawn_->getCameraInfo(cameta_id);
+    return pawn_->getCameraInfo(camera_id);
+}
+
+void CarPawnApi::setCameraOrientation(int camera_id, const msr::airlib::Quaternionr& orientation)
+{
+    UAirBlueprintLib::RunCommandOnGameThread([&camera_id, &orientation, this]() {
+        pawn_->setCameraOrientation(camera_id, orientation);
+    }, true);
 }
 
 CarApiBase::CarState CarPawnApi::getCarState()

diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.h b/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.h
@@ -46,8 +46,8 @@ class CarPawnApi : public msr::airlib::CarApiBase {
     virtual const CarApiBase::CarControls& getCarControls() const override;
 
     virtual msr::airlib::Pose simGetObjectPose(const std::string& actor_name) override;
-    virtual msr::airlib::CameraInfo getCameraInfo(int cameta_id) const override;
-
+    virtual msr::airlib::CameraInfo getCameraInfo(int camera_id) const override;
+    virtual void setCameraOrientation(int camera_id, const msr::airlib::Quaternionr& orientation) override;
 
     virtual ~CarPawnApi();
 

diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.cpp b/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.cpp
@@ -307,9 +307,16 @@ int MultiRotorConnector::getSegmentationObjectID(const std::string& mesh_name)
     return UAirBlueprintLib::GetMeshStencilID(mesh_name);
 }
 
-CameraInfo MultiRotorConnector::getCameraInfo(int cameta_id) const
+CameraInfo MultiRotorConnector::getCameraInfo(int camera_id) const
 {
-    return vehicle_pawn_wrapper_->getCameraInfo(cameta_id);
+    return vehicle_pawn_wrapper_->getCameraInfo(camera_id);
+}
+
+void MultiRotorConnector::setCameraOrientation(int camera_id, const Quaternionr& orientation) 
+{
+    UAirBlueprintLib::RunCommandOnGameThread([&camera_id, &orientation, this]() {
+        vehicle_pawn_wrapper_->setCameraOrientation(camera_id, orientation);
+    }, true);
 }
 
 void MultiRotorConnector::startApiServer()

diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.h b/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.h
@@ -66,7 +66,8 @@ class MultiRotorConnector : public msr::airlib::VehicleConnectorBase
 
     virtual void printLogMessage(const std::string& message, std::string message_param = "", unsigned char severity = 0) override;
     virtual Pose getActorPose(const std::string& actor_name) override;
-    virtual CameraInfo getCameraInfo(int cameta_id) const override;
+    virtual CameraInfo getCameraInfo(int camera_id) const override;
+    virtual void setCameraOrientation(int camera_id, const Quaternionr& orientation) override;
 
 private:
     void detectUsbRc();

diff --git a/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp b/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp
@@ -264,17 +264,24 @@ void VehiclePawnWrapper::printLogMessage(const std::string& message, const std::
     UAirBlueprintLib::LogMessageString(message, message_param, static_cast<LogDebugLevel>(severity));
 }
 
-msr::airlib::CameraInfo VehiclePawnWrapper::getCameraInfo(int cameta_id) const
+msr::airlib::CameraInfo VehiclePawnWrapper::getCameraInfo(int camera_id) const
 {
     msr::airlib::CameraInfo camera_info;
 
-    const APIPCamera* camera = getCamera(cameta_id);
+    const APIPCamera* camera = getCamera(camera_id);
     camera_info.pose.position = NedTransform::toNedMeters(camera->GetActorLocation(), true);
     camera_info.pose.orientation = NedTransform::toQuaternionr(camera->GetActorRotation().Quaternion(), true);
     camera_info.fov = camera->GetCameraComponent()->FieldOfView;
     return camera_info;
 }
 
+void VehiclePawnWrapper::setCameraOrientation(int camera_id, const msr::airlib::Quaternionr& orientation)
+{
+    APIPCamera* camera = getCamera(camera_id);
+    FQuat quat = NedTransform::toFQuat(orientation, true);
+    camera->SetActorRelativeRotation(quat);
+}
+
 //parameters in NED frame
 VehiclePawnWrapper::Pose VehiclePawnWrapper::getPose() const
 {

diff --git a/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.h b/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.h
@@ -81,7 +81,8 @@ class VehiclePawnWrapper
     std::string getLogLine();
 
     void printLogMessage(const std::string& message, const std::string& message_param = "", unsigned char severity = 0);
-    msr::airlib::CameraInfo getCameraInfo(int cameta_id) const;
+    msr::airlib::CameraInfo getCameraInfo(int camera_id) const;
+    void setCameraOrientation(int camera_id, const Quaternionr& orientation);
 
     WrapperConfig& getConfig();
     const WrapperConfig& getConfig() const;

diff --git a/docs/image_apis.md b/docs/image_apis.md
@@ -152,8 +152,17 @@ If you are only interested in this mode, you might also want to take a look at [
 
 ## How to Set Position and Orientation (Pose)?
 
+### Vehicle Pose
 To move around the environment using APIs you can use `simSetPose` API. This API takes position and orientation and sets that on the vehicle. If you don't want to change position (or orientation) then set components of position (or orientation) to floating point nan values.
 
+### Camera Orientation (Gimble)
+To change orientation of individial camera, you can use `setCameraOrientation` API. It takes camera ID which is zero-based [index of camera](#available-cameras) and quaternion relative to body in NED frame. For example, to set camera-0 to 15-degree pitch, you can use:
+```
+client.setCameraOrientation(0, AirSimClientBase.toQuaternion(0.261799, 0, 0)); #radians
+```
+
+Please see [example usage](https://github.com/Microsoft/AirSim/blob/master/PythonClient/cv_mode.py).
+
 ## Changing Resolution and Camera Parameters
 To change resolution, FOV etc, you can use [settings.json](settings.md). For example, below is the complete content of settings.json that sets parameters for scene capture and uses "Computer Vision" mode described above. If you omit any setting then below default values will be used. For more information see [settings doc](settings.md). If you are using stereo camera, currently the distance between left and right is fixed at 25 cm.