added getCameraParameters API

senakicir · Feb 21, 2018 · d2a7726 · d2a7726
1 parent 633094b
commit d2a7726
Show file tree

Hide file tree

Showing 19 changed files with 117 additions and 13 deletions.
diff --git a/AirLib/include/api/RpcLibAdapatorsBase.hpp b/AirLib/include/api/RpcLibAdapatorsBase.hpp
@@ -197,6 +197,31 @@ class RpcLibAdapatorsBase {
             return d;
         }
     };
+
+    struct CameraInfo {
+        Pose pose;
+        float fov;
+
+        MSGPACK_DEFINE_MAP(pose, fov);
+
+        CameraInfo()
+        {}
+
+        CameraInfo(const msr::airlib::CameraInfo& s)
+        {
+            pose = s.pose;
+            fov = s.fov;
+        }
+
+        msr::airlib::CameraInfo to() const
+        {
+            msr::airlib::CameraInfo s;
+            s.pose = pose.to();
+            s.fov = fov;
+
+            return s;
+        }
+    };
 
     struct KinematicsState {
         Vector3r position;

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);
+
     virtual ~RpcLibClientBase();    //required for pimpl
 
 protected:

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

diff --git a/AirLib/include/common/CommonStructs.hpp b/AirLib/include/common/CommonStructs.hpp
@@ -198,6 +198,19 @@ struct CollisionInfo {
     }
 };
 
+struct CameraInfo {
+    Pose pose;
+    float fov;
+
+    CameraInfo()
+    {}
+
+    CameraInfo(const Pose& pose_val, float fov_val)
+        : pose(pose_val), fov(fov_val)
+    {
+    }
+};
+
 struct CollisionResponseInfo {
     unsigned int collision_count_raw = 0;
     unsigned int collision_count_non_resting = 0;

diff --git a/AirLib/include/controllers/VehicleConnectorBase.hpp b/AirLib/include/controllers/VehicleConnectorBase.hpp
@@ -34,7 +34,7 @@ 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;
 };
 
 

diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp
@@ -281,6 +281,10 @@ class MultirotorApi : public VehicleApiBase {
         return vehicle_->getSegmentationObjectID(mesh_name);
     }
 
+    virtual CameraInfo getCameraInfo(int cameta_id) const override
+    {
+        return vehicle_->getCameraInfo(cameta_id);
+    }
 
     virtual bool isApiControlEnabled() const override
     {
@@ -292,10 +296,10 @@ class MultirotorApi : public VehicleApiBase {
         vehicle_->reset();
     }
 
-	virtual void setRCData(const RCData& data)
-	{
-		controller_->setRCData(data);
-	}
+    virtual void setRCData(const RCData& data)
+    {
+        controller_->setRCData(data);
+    }
 
 
     /*** Implementation of CancelableBase ***/

diff --git a/AirLib/include/vehicles/multirotor/controllers/RealMultirotorConnector.hpp b/AirLib/include/vehicles/multirotor/controllers/RealMultirotorConnector.hpp
@@ -94,6 +94,11 @@ class RealMultirotorConnector : public VehicleConnectorBase
         return msr::airlib::Pose();
     }
 
+    virtual CameraInfo getCameraInfo(int cameta_id) const override
+    {
+        unused(cameta_id);
+        throw std::logic_error("getCameraInfo() 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,6 +152,10 @@ void* RpcLibClientBase::getClient()
     return &pimpl_->client;
 }
 
+CameraInfo RpcLibClientBase::getCameraInfo(int cameta_id)
+{
+    return pimpl_->client.call("getCameraInfo", cameta_id).as<RpcLibAdapatorsBase::CameraInfo>().to();
+}
 
 CollisionInfo RpcLibClientBase::getCollisionInfo()
 {

diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp
@@ -100,6 +100,11 @@ 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);
+        return RpcLibAdapatorsBase::CameraInfo(camera_info);
+    });
+
     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
@@ -191,6 +191,10 @@ class MultirotorState(MsgpackMixin):
     gps_location = GeoPoint()
     timestamp = np.uint64(0)
 
+class CameraInfo(MsgpackMixin):
+    pose = Pose()
+    fov = -1
+
 class AirSimClientBase:
     def __init__(self, ip, port):
         self.client = msgpackrpc.Client(msgpackrpc.Address(ip, port), timeout = 3600, pack_encoding = 'utf-8', unpack_encoding = 'utf-8')
@@ -251,6 +255,9 @@ 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))
+
     @staticmethod
     def stringToUint8Array(bstr):
         return np.fromstring(bstr, np.uint8)

diff --git a/PythonClient/PythonClient.pyproj b/PythonClient/PythonClient.pyproj
@@ -5,7 +5,7 @@
     <SchemaVersion>2.0</SchemaVersion>
     <ProjectGuid>e2049e20-b6dd-474e-8bca-1c8dc54725aa</ProjectGuid>
     <ProjectHome>.</ProjectHome>
-    <StartupFile>hello_car.py</StartupFile>
+    <StartupFile>cv_mode.py</StartupFile>
     <SearchPath>
     </SearchPath>
     <WorkingDirectory>.</WorkingDirectory>

diff --git a/PythonClient/cv_mode.py b/PythonClient/cv_mode.py
@@ -6,9 +6,15 @@
 
 pp = pprint.PrettyPrinter(indent=4)
 
-client = MultirotorClient()
+client = CarClient()
 client.confirmConnection()
 
+AirSimClientBase.wait_key('Press any key to get camera parameters')
+for camera_id in range(5):
+    camera_info = client.getCameraInfo(camera_id)
+    print("CameraInfo %d: %s" % (camera_id, pp.pprint(camera_info)))
+
+AirSimClientBase.wait_key('Press any key to get images')
 for x in range(3): # do few times
     z = x * -20 - 5 # some random number
     client.simSetPose(Pose(Vector3r(z, z, z), AirSimClientBase.toQuaternion(x / 3.0, 0, x / 3.0)), True)

diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp b/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp
@@ -87,6 +87,11 @@ const CarApiBase::CarControls& CarPawnApi::getCarControls() const
     return last_controls_;
 }
 
+msr::airlib::CameraInfo CarPawnApi::getCameraInfo(int cameta_id) const
+{
+    return pawn_->getCameraInfo(cameta_id);
+}
+
 CarApiBase::CarState CarPawnApi::getCarState()
 {
     CarApiBase::CarState state(

diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.h b/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.h
@@ -43,10 +43,10 @@ class CarPawnApi : public msr::airlib::CarApiBase {
     virtual void enableApiControl(bool is_enabled) override;
     virtual bool isApiControlEnabled() const override;
 
-    virtual const CarApiBase::CarControls& getCarControls() const;
-
-    virtual msr::airlib::Pose simGetObjectPose(const std::string& actor_name);
+    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 ~CarPawnApi();

diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.cpp b/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.cpp
@@ -307,6 +307,11 @@ int MultiRotorConnector::getSegmentationObjectID(const std::string& mesh_name)
     return UAirBlueprintLib::GetMeshStencilID(mesh_name);
 }
 
+CameraInfo MultiRotorConnector::getCameraInfo(int cameta_id) const
+{
+    return vehicle_pawn_wrapper_->getCameraInfo(cameta_id);
+}
+
 void MultiRotorConnector::startApiServer()
 {
     if (enable_rpc_) {

diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.h b/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.h
@@ -66,7 +66,7 @@ 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;
 
 private:
     void detectUsbRc();

diff --git a/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp b/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp
@@ -154,14 +154,20 @@ const VehiclePawnWrapper::WrapperConfig& VehiclePawnWrapper::getConfig() const
     return config_;
 }
 
-APIPCamera* VehiclePawnWrapper::getCamera(int index)
+const APIPCamera* VehiclePawnWrapper::getCamera(int index) const
 {
     if (index < 0 || index >= cameras_.size())
         throw std::out_of_range("Camera id is not valid");
     //should be overridden in derived class
     return cameras_.at(index);
 }
 
+APIPCamera* VehiclePawnWrapper::getCamera(int index)
+{
+    return const_cast<APIPCamera*>(
+        static_cast<const VehiclePawnWrapper*>(this)->getCamera(index));
+}
+
 UnrealImageCapture* VehiclePawnWrapper::getImageCapture()
 {
     return image_capture_.get();
@@ -258,6 +264,17 @@ 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 camera_info;
+
+    const APIPCamera* camera = getCamera(cameta_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;
+}
+
 //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
@@ -56,6 +56,7 @@ class VehiclePawnWrapper
     void onCollision(class UPrimitiveComponent* MyComp, class AActor* Other, class UPrimitiveComponent* OtherComp, 
         bool bSelfMoved, FVector HitLocation, FVector HitNormal, FVector NormalImpulse, const FHitResult& Hit);
 
+    const APIPCamera* getCamera(int index = 0) const;
     APIPCamera* getCamera(int index = 0);
     UnrealImageCapture* getImageCapture();
     int getCameraCount();
@@ -80,6 +81,7 @@ 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;
 
     WrapperConfig& getConfig();
     const WrapperConfig& getConfig() const;

diff --git a/docs/image_apis.md b/docs/image_apis.md
@@ -173,6 +173,9 @@ To change resolution, FOV etc, you can use [settings.json](settings.md). For exa
 }
 ```
 
+### Getting Camera Parameters
+The `getCameraInfo(camera_id)` API call retuns pose (in world frame, NED coordinates, SI units) and FOV (in degrees) of specified camera. Camera ID is zer-based [index of camera](#available-cameras). Please see [example usage](https://github.com/Microsoft/AirSim/blob/master/PythonClient/cv_mode.py).
+
 ## What Does Pixel Values Mean in Different Image Types?
 ### Available ImageType
 ```
-Original file line number
+Diff line change
@@ Expand Up / @@ -32,6 +32,8 @@ class VehicleApiBase { @@
         virtual Pose simGetObjectPose(const std::string& object_name) = 0;
+        virtual CameraInfo getCameraInfo(int cameta_id) const = 0;
         virtual ~VehicleApiBase() = default;
     };
@@ Expand Down @@