Upgrade setCameraOrientation to setCameraPose

AirLib/include/api/RpcLibClientBase.hpp

@@ -98,7 +98,7 @@ class RpcLibClientBase {
     CollisionInfo simGetCollisionInfo(const std::string& vehicle_name = "") const;
 
     CameraInfo simGetCameraInfo(const std::string& camera_name, const std::string& vehicle_name = "") const;
-    void simSetCameraOrientation(const std::string& camera_name, const Quaternionr& orientation, const std::string& vehicle_name = "");
+    void simSetCameraPose(const std::string& camera_name, const Pose& pose, const std::string& vehicle_name = "");
     void simSetCameraFov(const std::string& camera_name, float fov_degrees, const std::string& vehicle_name = "");
 
     msr::airlib::Kinematics::State simGetGroundTruthKinematics(const std::string& vehicle_name = "") const;

AirLib/include/api/VehicleSimApiBase.hpp

@@ -54,7 +54,7 @@ class VehicleSimApiBase : public msr::airlib::UpdatableObject {
     virtual const msr::airlib::Environment* getGroundTruthEnvironment() const = 0;
 
     virtual CameraInfo getCameraInfo(const std::string& camera_name) const = 0;
-    virtual void setCameraOrientation(const std::string& camera_name, const Quaternionr& orientation) = 0;
+    virtual void setCameraPose(const std::string& camera_name, const Pose& orientation) = 0;
     virtual void setCameraFoV(const std::string& camera_name, float fov_degrees) = 0;
 
     virtual CollisionInfo getCollisionInfo() const = 0;

AirLib/src/api/RpcLibClientBase.cpp

@@ -364,9 +364,9 @@ CameraInfo RpcLibClientBase::simGetCameraInfo(const std::string& camera_name, co
 {
     return pimpl_->client.call("simGetCameraInfo", camera_name, vehicle_name).as<RpcLibAdapatorsBase::CameraInfo>().to();
 }
-void RpcLibClientBase::simSetCameraOrientation(const std::string& camera_name, const Quaternionr& orientation, const std::string& vehicle_name)
+void RpcLibClientBase::simSetCameraPose(const std::string& camera_name, const Pose& pose, const std::string& vehicle_name)
 {
-    pimpl_->client.call("simSetCameraOrientation", camera_name, RpcLibAdapatorsBase::Quaternionr(orientation), vehicle_name);
+    pimpl_->client.call("simSetCameraPose", camera_name, RpcLibAdapatorsBase::Pose(pose), vehicle_name);
 }
 void RpcLibClientBase::simSetCameraFov(const std::string& camera_name, float fov_degrees, const std::string& vehicle_name)
 {

AirLib/src/api/RpcLibServerBase.cpp

@@ -228,9 +228,9 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string&
         return RpcLibAdapatorsBase::CameraInfo(camera_info);
     });
 
-    pimpl_->server.bind("simSetCameraOrientation", [&](const std::string& camera_name, const RpcLibAdapatorsBase::Quaternionr& orientation, 
+    pimpl_->server.bind("simSetCameraPose", [&](const std::string& camera_name, const RpcLibAdapatorsBase::Pose& pose, 
         const std::string& vehicle_name) -> void {
-        getVehicleSimApi(vehicle_name)->setCameraOrientation(camera_name, orientation.to());
+        getVehicleSimApi(vehicle_name)->setCameraPose(camera_name, pose.to());
     });
 
     pimpl_->server.bind("simSetCameraFov", [&](const std::string& camera_name, float fov_degrees,

PythonClient/airsim/client.py

@@ -155,17 +155,17 @@ def simGetCameraInfo(self, camera_name, vehicle_name = ''):
         # TODO: below str() conversion is only needed for legacy reason and should be removed in future
         return CameraInfo.from_msgpack(self.client.call('simGetCameraInfo', str(camera_name), vehicle_name))
 
-    def simSetCameraOrientation(self, camera_name, orientation, vehicle_name = ''):
+    def simSetCameraPose(self, camera_name, pose, vehicle_name = ''):
         """
-        - Control the orientation of a selected camera
+        - Control the pose of a selected camera
 
         Args:
             camera_name (str): Name of the camera to be controlled
-            orientation (airsim.Quaternion()): Quaternion representing the desired orientation of the camera
+            orientation (airsim.Pose()): Pose representing the desired position and orientation of the camera
             vehicle_name (str, optional): Name of vehicle which the camera corresponds to
         """
         # TODO: below str() conversion is only needed for legacy reason and should be removed in future
-        self.client.call('simSetCameraOrientation', str(camera_name), orientation, vehicle_name)
+        self.client.call('simSetCameraPose', str(camera_name), pose, vehicle_name)
 
     def simSetCameraFov(self, camera_name, fov_degrees, vehicle_name = ''):
         """

Unreal/Plugins/AirSim/Source/NedTransform.cpp

@@ -66,6 +66,14 @@ FVector NedTransform::fromLocalNed(const NedTransform::Vector3r& position) const
 {
     return NedTransform::toFVector(position, world_to_meters_, true) + local_ned_offset_;
 }
+FVector NedTransform::fromRelativeNed(const NedTransform::Vector3r& position) const
+{
+    return NedTransform::toFVector(position, world_to_meters_, true);
+}
+FTransform NedTransform::fromRelativeNed(const Pose& pose) const
+{
+    return FTransform(fromNed(pose.orientation), fromRelativeNed(pose.position));
+}
 FVector NedTransform::fromGlobalNed(const NedTransform::Vector3r& position) const
 {
     return NedTransform::toFVector(position, world_to_meters_, true) + global_transform_.GetLocation();

Unreal/Plugins/AirSim/Source/NedTransform.h

@@ -42,6 +42,8 @@ class AIRSIM_API NedTransform
     FVector fromGlobalNed(const Vector3r& position) const;
     FQuat fromNed(const Quaternionr& q) const;
     float fromNed(float length) const;
+    FVector fromRelativeNed(const Vector3r& position) const;
+    FTransform fromRelativeNed(const Pose& pose) const;
     FTransform fromLocalNed(const Pose& pose) const;
     FTransform fromGlobalNed(const Pose& pose) const;
 

Unreal/Plugins/AirSim/Source/PIPCamera.cpp

@@ -244,8 +244,12 @@ void APIPCamera::setCameraTypeEnabled(ImageType type, bool enabled)
     enableCaptureComponent(type, enabled);
 }
 
-void APIPCamera::setCameraOrientation(const FRotator& rotator)
+void APIPCamera::setCameraPose(const FTransform& pose)
 {
+    FVector position = pose.GetLocation();
+    this->SetActorRelativeLocation(pose.GetLocation());
+
+    FRotator rotator = pose.GetRotation().Rotator();
     if (gimbal_stabilization_ > 0) {
         gimbald_rotator_.Pitch = rotator.Pitch;
         gimbald_rotator_.Roll = rotator.Roll;

Unreal/Plugins/AirSim/Source/PIPCamera.h

@@ -41,7 +41,7 @@ class AIRSIM_API APIPCamera : public ACameraActor
     void setCameraTypeEnabled(ImageType type, bool enabled);
     bool getCameraTypeEnabled(ImageType type) const;
     void setupCameraFromSettings(const APIPCamera::CameraSetting& camera_setting, const NedTransform& ned_transform);
-    void setCameraOrientation(const FRotator& rotator);
+    void setCameraPose(const FTransform& pose);
     void setCameraFoV(float fov_degrees);
 
     msr::airlib::ProjectionMatrix getProjectionMatrix(const APIPCamera::ImageType image_type) const;

Unreal/Plugins/AirSim/Source/PawnSimApi.cpp

@@ -409,12 +409,12 @@ msr::airlib::CameraInfo PawnSimApi::getCameraInfo(const std::string& camera_name
     return camera_info;
 }
 
-void PawnSimApi::setCameraOrientation(const std::string& camera_name, const msr::airlib::Quaternionr& orientation)
+void PawnSimApi::setCameraPose(const std::string& camera_name, const msr::airlib::Pose& pose)
 {
-    UAirBlueprintLib::RunCommandOnGameThread([this, camera_name, orientation]() {
+    UAirBlueprintLib::RunCommandOnGameThread([this, camera_name, pose]() {
         APIPCamera* camera = getCamera(camera_name);
-        FQuat quat = ned_transform_.fromNed(orientation);
-        camera->setCameraOrientation(quat.Rotator());
+        FTransform pose_unreal = ned_transform_.fromRelativeNed(pose);
+        camera->setCameraPose(pose_unreal);
     }, true);
 }
 

Unreal/Plugins/AirSim/Source/PawnSimApi.h

@@ -77,7 +77,7 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase {
     virtual Pose getPose() const override;
     virtual void setPose(const Pose& pose, bool ignore_collision) override;
     virtual msr::airlib::CameraInfo getCameraInfo(const std::string& camera_name) const override;
-    virtual void setCameraOrientation(const std::string& camera_name, const Quaternionr& orientation) override;
+    virtual void setCameraPose(const std::string& camera_name, const Pose& pose) override;
     virtual void setCameraFoV(const std::string& camera_name, float fov_degrees) override;
     virtual CollisionInfo getCollisionInfo() const override;
     virtual int getRemoteControlID() const override;