diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index fa12e92ccf..6484578985 100644 --- a/AirLib/include/api/RpcLibClientBase.hpp +++ b/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; diff --git a/AirLib/include/api/VehicleSimApiBase.hpp b/AirLib/include/api/VehicleSimApiBase.hpp index 25b9579b03..471a2cfd85 100644 --- a/AirLib/include/api/VehicleSimApiBase.hpp +++ b/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& pose) = 0; virtual void setCameraFoV(const std::string& camera_name, float fov_degrees) = 0; virtual CollisionInfo getCollisionInfo() const = 0; diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index fa92f3b30f..b76a5a2f00 100644 --- a/AirLib/src/api/RpcLibClientBase.cpp +++ b/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().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) { diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index 913d9d677e..8ff8a90a82 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -229,9 +229,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, diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index 40896500c2..5f5793052c 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -417,17 +417,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 (Quaternionr): Quaternion representing the desired orientation of the camera + pose (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 = ''): """ diff --git a/PythonClient/computer_vision/cv_mode.py b/PythonClient/computer_vision/cv_mode.py index 188080ba2d..0691c0de7e 100644 --- a/PythonClient/computer_vision/cv_mode.py +++ b/PythonClient/computer_vision/cv_mode.py @@ -13,8 +13,9 @@ client = airsim.VehicleClient() client.confirmConnection() -airsim.wait_key('Press any key to set camera-0 gimble to 15-degree pitch') -client.simSetCameraOrientation("0", airsim.to_quaternion(0.261799, 0, 0)); #radians +airsim.wait_key('Press any key to set camera-0 gimbal to 15-degree pitch') +camera_pose = airsim.Pose(airsim.Vector3r(0, 0, 0), airsim.to_quaternion(0.261799, 0, 0)) #radians +client.simSetCameraPose("0", camera_pose) airsim.wait_key('Press any key to get camera parameters') for camera_name in range(5): @@ -48,4 +49,4 @@ time.sleep(3) # currently reset() doesn't work in CV mode. Below is the workaround -client.simSetPose(airsim.Pose(airsim.Vector3r(0, 0, 0), airsim.to_quaternion(0, 0, 0)), True) \ No newline at end of file +client.simSetPose(airsim.Pose(airsim.Vector3r(0, 0, 0), airsim.to_quaternion(0, 0, 0)), True) diff --git a/PythonClient/multirotor/gimbal.py b/PythonClient/multirotor/gimbal.py index fef1aeb527..45beb6a232 100644 --- a/PythonClient/multirotor/gimbal.py +++ b/PythonClient/multirotor/gimbal.py @@ -17,7 +17,9 @@ for i in range(5): client.moveToPositionAsync(float(-50.00), float( 50.26), float( -20.58), float( 3.5)) time.sleep(6) - client.simSetCameraOrientation("0", airsim.to_quaternion(0.5, 0.5, 0.1)) + camera_pose = airsim.Pose(airsim.Vector3r(0, 0, 0), airsim.to_quaternion(0.5, 0.5, 0.1)) + client.simSetCameraPose("0", camera_pose) client.moveToPositionAsync(float(50.00), float( -50.26), float( -10.58), float( 3.5)) time.sleep(6) - client.simSetCameraOrientation("0", airsim.to_quaternion(-0.5, -0.5, -0.1)) \ No newline at end of file + camera_pose = airsim.Pose(airsim.Vector3r(0, 0, 0), airsim.to_quaternion(-0.5, -0.5, -0.1)) + client.simSetCameraPose("0", camera_pose) diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/PInvokeWrapper.cpp b/Unity/AirLibWrapper/AirsimWrapper/Source/PInvokeWrapper.cpp index 4a698b8356..9c45287d57 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/PInvokeWrapper.cpp +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/PInvokeWrapper.cpp @@ -11,7 +11,7 @@ bool(*SetEnableApi)(bool enableApi, const char* vehicleName); bool(*SetCarApiControls)(msr::airlib::CarApiBase::CarControls controls, const char* vehicleName); AirSimCarState(*GetCarState)(const char* vehicleName); AirSimCameraInfo(*GetCameraInfo)(const char* cameraName, const char* vehicleName); -bool(*SetCameraOrientation)(const char* cameraName, AirSimQuaternion orientation, const char* vehicleName); +bool(*SetCameraPose)(const char* cameraName, AirSimPose pose, const char* vehicleName); bool(*SetCameraFoV)(const char* cameraName, const float fov_degrees, const char* vehicleName); bool(*SetSegmentationObjectId)(const char* meshName, int objectId, bool isNameRegex); int(*GetSegmentationObjectId)(const char* meshName); @@ -33,7 +33,7 @@ void InitVehicleManager( bool(*setCarApiControls)(msr::airlib::CarApiBase::CarControls controls, const char* vehicleName), AirSimCarState(*getCarState)(const char* vehicleName), AirSimCameraInfo(*getCameraInfo)(const char* cameraName, const char* vehicleName), - bool(*setCameraOrientation)(const char* cameraName, AirSimQuaternion orientation, const char* vehicleName), + bool(*setCameraPose)(const char* cameraName, AirSimPose pose, const char* vehicleName), bool(*setCameraFoV)(const char* cameraName, const float fov_degrees, const char* vehicleName), bool(*setSegmentationObjectId)(const char* meshName, int objectId, bool isNameRegex), int(*getSegmentationObjectId)(const char* meshName), @@ -55,7 +55,7 @@ void InitVehicleManager( SetCarApiControls = setCarApiControls; GetCarState = getCarState; GetCameraInfo = getCameraInfo; - SetCameraOrientation = setCameraOrientation; + SetCameraPose = setCameraPose; SetCameraFoV = setCameraFoV; SetSegmentationObjectId = setSegmentationObjectId; GetSegmentationObjectId = getSegmentationObjectId; diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/PInvokeWrapper.h b/Unity/AirLibWrapper/AirsimWrapper/Source/PInvokeWrapper.h index 9bb45c54b8..36c22c2e1e 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/PInvokeWrapper.h +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/PInvokeWrapper.h @@ -25,7 +25,7 @@ extern bool(*SetEnableApi)(bool enableApi, const char* vehicleName); extern bool(*SetCarApiControls)(msr::airlib::CarApiBase::CarControls controls, const char* vehicleName); extern AirSimCarState(*GetCarState)(const char* vehicleName); extern AirSimCameraInfo(*GetCameraInfo)(const char* cameraName, const char* vehicleName); -extern bool(*SetCameraOrientation)(const char* cameraName, AirSimQuaternion orientation, const char* vehicleName); +extern bool(*SetCameraPose)(const char* cameraName, AirSimPose pose, const char* vehicleName); extern bool(*SetCameraFoV)(const char* cameraName, const float fov_degrees, const char* vehicleName); extern bool(*SetSegmentationObjectId)(const char* meshName, int objectId, bool isNameRegex); extern int(*GetSegmentationObjectId)(const char* meshName); @@ -49,7 +49,7 @@ extern "C" EXPORT void InitVehicleManager( bool(*setCarApiControls)(msr::airlib::CarApiBase::CarControls controls, const char* vehicleName), AirSimCarState(*getCarState)(const char* vehicleName), AirSimCameraInfo(*getCameraInfo)(const char* cameraName, const char* vehicleName), - bool(*setCameraOrientation)(const char* cameraName, AirSimQuaternion orientation, const char* vehicleName), + bool(*setCameraPose)(const char* cameraName, AirSimPose pose, const char* vehicleName), bool(*setCameraFoV)(const char* cameraName, const float fov_degrees, const char* vehicleName), bool(*setSegmentationObjectId)(const char* meshName, int objectId, bool isNameRegex), int(*getSegmentationObjectId)(const char* meshName), diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp b/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp index cacadce82f..e4b5966142 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp @@ -210,15 +210,9 @@ 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) { - AirSimQuaternion airSimOrientation; - airSimOrientation.x = orientation.x(); - airSimOrientation.y = orientation.y(); - airSimOrientation.z = orientation.z(); - airSimOrientation.w = orientation.w(); - - SetCameraOrientation(camera_name.c_str(), airSimOrientation, params_.vehicle_name.c_str()); + SetCameraPose(camera_name.c_str(), UnityUtilities::Convert_to_AirSimPose(pose), params_.vehicle_name.c_str()); } void PawnSimApi::setCameraFoV(const std::string& camera_name, float fov_degrees) diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.h b/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.h index 0108cc01b5..6525666e88 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.h +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.h @@ -72,7 +72,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; diff --git a/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Utilities/DataCaptureScript.cs b/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Utilities/DataCaptureScript.cs index 41121e65cd..1a970a7227 100644 --- a/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Utilities/DataCaptureScript.cs +++ b/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Utilities/DataCaptureScript.cs @@ -23,7 +23,7 @@ public class DataCaptureScript : MonoBehaviour { private Texture2D screenShot; private Rect captureRect; private AirSimPose currentPose; - private Quaternion poseFromAirLib; + private Pose poseFromAirLib; private bool isCapturing; private bool isPoseOverride; @@ -40,7 +40,8 @@ private void FixedUpdate() { DataManager.SetToAirSim(transform.rotation, ref currentPose.orientation); if (isPoseOverride) { - transform.rotation = poseFromAirLib; + transform.position = poseFromAirLib.position; + transform.rotation = poseFromAirLib.rotation; isPoseOverride = false; } } @@ -89,8 +90,9 @@ public AirSimPose GetPose() { return currentPose; } - public void SetOrientation(AirSimQuaternion orientation) { - DataManager.SetToUnity(orientation, ref poseFromAirLib); + public void SetPose(AirSimPose pose) { + DataManager.SetToUnity(pose.position, ref poseFromAirLib.position); + DataManager.SetToUnity(pose.orientation, ref poseFromAirLib.rotation); isPoseOverride = true; } diff --git a/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Utilities/PInvokeWrapper.cs b/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Utilities/PInvokeWrapper.cs index ac1ec3deb8..8f156909ce 100644 --- a/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Utilities/PInvokeWrapper.cs +++ b/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Utilities/PInvokeWrapper.cs @@ -13,7 +13,7 @@ public static class PInvokeWrapper { [DllImport(DLL_NAME)] public static extern void InitVehicleManager(IntPtr SetPose, IntPtr GetPose, IntPtr GetCollisionInfo, IntPtr GetRCData, IntPtr GetSimImages, IntPtr SetRotorSpeed, IntPtr SetEnableApi, IntPtr SetCarApiControls, IntPtr GetCarState, - IntPtr GetCameraInfo, IntPtr SetCameraOrientation, IntPtr SetCameraFoV, IntPtr SetSegmentationObjectid, IntPtr GetSegmentationObjectId, + IntPtr GetCameraInfo, IntPtr SetCameraPose, IntPtr SetCameraFoV, IntPtr SetSegmentationObjectid, IntPtr GetSegmentationObjectId, IntPtr PrintLogMessage, IntPtr GetTransformFromUnity, IntPtr Reset, IntPtr GetVelocity, IntPtr GetRayCastHit, IntPtr Pause); [DllImport(DLL_NAME)] diff --git a/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Vehicles/IVehicleInterface.cs b/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Vehicles/IVehicleInterface.cs index 7a865ddee4..2809319985 100644 --- a/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Vehicles/IVehicleInterface.cs +++ b/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Vehicles/IVehicleInterface.cs @@ -33,7 +33,7 @@ public interface IVehicleInterface { CameraInfo GetCameraInfo(string cameraName); - bool SetCameraOrientation(string cameraName, AirSimQuaternion orientation); + bool SetCameraPose(string cameraName, AirSimPose pose); bool SetCameraFoV(string cameraName, float fov_degrees); diff --git a/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Vehicles/Vehicle.cs b/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Vehicles/Vehicle.cs index 488b80fdc3..2049d2eecd 100644 --- a/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Vehicles/Vehicle.cs +++ b/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Vehicles/Vehicle.cs @@ -264,10 +264,10 @@ public CameraInfo GetCameraInfo(string cameraName) { return info; } - public bool SetCameraOrientation(string cameraName, AirSimQuaternion orientation) { + public bool SetCameraPose(string cameraName, AirSimPose pose) { foreach (DataCaptureScript capture in captureCameras) { if (capture.GetCameraName() == cameraName) { - capture.SetOrientation(orientation); + capture.SetPose(pose); return true; } } diff --git a/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Vehicles/VehicleCompanion.cs b/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Vehicles/VehicleCompanion.cs index 20015e93ff..ce674191e7 100644 --- a/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Vehicles/VehicleCompanion.cs +++ b/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Vehicles/VehicleCompanion.cs @@ -93,7 +93,7 @@ private static void InitDelegators() { Marshal.GetFunctionPointerForDelegate(new Func(SetCarApiControls)), Marshal.GetFunctionPointerForDelegate(new Func(GetCarState)), Marshal.GetFunctionPointerForDelegate(new Func(GetCameraInfo)), - Marshal.GetFunctionPointerForDelegate(new Func(SetCameraOrientation)), + Marshal.GetFunctionPointerForDelegate(new Func(SetCameraPose)), Marshal.GetFunctionPointerForDelegate(new Func(SetCameraFoV)), Marshal.GetFunctionPointerForDelegate(new Func(SetSegmentationObjectId)), Marshal.GetFunctionPointerForDelegate(new Func(GetSegmentationObjectId)), @@ -184,9 +184,9 @@ private static CameraInfo GetCameraInfo(string cameraName, string vehicleName) { return vehicle.VehicleInterface.GetCameraInfo(cameraName); } - private static bool SetCameraOrientation(string cameraName, AirSimQuaternion orientation, string vehicleName) { + private static bool SetCameraPose(string cameraName, AirSimPose pose, string vehicleName) { var vehicle = Vehicles.Find(element => element.vehicleName == vehicleName); - return vehicle.VehicleInterface.SetCameraOrientation(cameraName, orientation); + return vehicle.VehicleInterface.SetCameraPose(cameraName, pose); } private static bool SetCameraFoV(string cameraName, float fov_degrees, string vehicleName) { diff --git a/Unreal/Plugins/AirSim/Source/NedTransform.cpp b/Unreal/Plugins/AirSim/Source/NedTransform.cpp index 112e54d355..384f74a513 100644 --- a/Unreal/Plugins/AirSim/Source/NedTransform.cpp +++ b/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(); diff --git a/Unreal/Plugins/AirSim/Source/NedTransform.h b/Unreal/Plugins/AirSim/Source/NedTransform.h index 1c3ebf644e..717b1aaa14 100644 --- a/Unreal/Plugins/AirSim/Source/NedTransform.h +++ b/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; diff --git a/Unreal/Plugins/AirSim/Source/PIPCamera.cpp b/Unreal/Plugins/AirSim/Source/PIPCamera.cpp index 03ef6c3369..e0bd8da737 100644 --- a/Unreal/Plugins/AirSim/Source/PIPCamera.cpp +++ b/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; diff --git a/Unreal/Plugins/AirSim/Source/PIPCamera.h b/Unreal/Plugins/AirSim/Source/PIPCamera.h index 99cfb54da0..311ed83268 100644 --- a/Unreal/Plugins/AirSim/Source/PIPCamera.h +++ b/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; diff --git a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp b/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp index ee89d0c3bb..9da9d1d61d 100644 --- a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp +++ b/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); } diff --git a/Unreal/Plugins/AirSim/Source/PawnSimApi.h b/Unreal/Plugins/AirSim/Source/PawnSimApi.h index c67922abfd..2148ae16cb 100644 --- a/Unreal/Plugins/AirSim/Source/PawnSimApi.h +++ b/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; diff --git a/docs/image_apis.md b/docs/image_apis.md index 8ecf384244..7c440ce66e 100644 --- a/docs/image_apis.md +++ b/docs/image_apis.md @@ -166,9 +166,10 @@ To move around the environment using APIs you can use `simSetVehiclePose` API. T ## Camera APIs The `simGetCameraInfo` returns the pose (in world frame, NED coordinates, SI units) and FOV (in degrees) for the specified camera. Please see [example usage](https://github.com/Microsoft/AirSim/tree/master/PythonClient//computer_vision/cv_mode.py). -The `simSetCameraOrientation` sets the orientation for the specified camera as quaternion in NED frame. The handy `airsim.to_quaternion()` function allows to convert pitch, roll, yaw to quaternion. For example, to set camera-0 to 15-degree pitch, you can use: +The `simSetCameraPose` sets the pose for the specified camera while taking an input pose as a combination of relative position and a quaternion in NED frame. The handy `airsim.to_quaternion()` function allows to convert pitch, roll, yaw to quaternion. For example, to set camera-0 to 15-degree pitch while maintaining the same position, you can use: ``` -client.simSetCameraOrientation(0, airsim.to_quaternion(0.261799, 0, 0)); #radians +camera_pose = airsim.Pose(airsim.Vector3r(0, 0, 0), airsim.to_quaternion(0.261799, 0, 0)) #RPY in radians +client.simSetCameraPose(0, camera_pose); ``` ### Gimbal diff --git a/ros/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h b/ros/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h index 2e96c9c6db..5ff3023c04 100644 --- a/ros/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h +++ b/ros/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h @@ -281,6 +281,7 @@ class AirsimROSWrapper nav_msgs::Odometry get_odom_msg_from_multirotor_state(const msr::airlib::MultirotorState& drone_state) const; nav_msgs::Odometry get_odom_msg_from_car_state(const msr::airlib::CarApiBase::CarState& car_state) const; airsim_ros_pkgs::CarState get_roscarstate_msg_from_car_state(const msr::airlib::CarApiBase::CarState& car_state) const; + msr::airlib::Pose get_airlib_pose(const float& x, const float& y, const float& z, const msr::airlib::Quaternionr& airlib_quat) const; airsim_ros_pkgs::GPSYaw get_gps_msg_from_airsim_geo_point(const msr::airlib::GeoPoint& geo_point) const; sensor_msgs::NavSatFix get_gps_sensor_msg_from_airsim_geo_point(const msr::airlib::GeoPoint& geo_point) const; sensor_msgs::Imu get_imu_msg_from_airsim(const msr::airlib::ImuBase::Output& imu_data) const; diff --git a/ros/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp b/ros/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp index 9a1f7eb2c3..3329a37d1e 100644 --- a/ros/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp +++ b/ros/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp @@ -482,6 +482,11 @@ void AirsimROSWrapper::car_cmd_cb(const airsim_ros_pkgs::CarControls::ConstPtr& car->has_car_cmd = true; } +msr::airlib::Pose AirsimROSWrapper::get_airlib_pose(const float& x, const float& y, const float& z, const msr::airlib::Quaternionr& airlib_quat) const +{ + return msr::airlib::Pose(msr::airlib::Vector3r(x, y, z), airlib_quat); +} + // void AirsimROSWrapper::vel_cmd_body_frame_cb(const airsim_ros_pkgs::VelCmd& msg, const std::string& vehicle_name) void AirsimROSWrapper::vel_cmd_body_frame_cb(const airsim_ros_pkgs::VelCmd::ConstPtr& msg, const std::string& vehicle_name) { @@ -626,7 +631,7 @@ void AirsimROSWrapper::gimbal_angle_quat_cmd_cb(const airsim_ros_pkgs::GimbalAng // todo support multiple gimbal commands // 1. find quaternion of default gimbal pose // 2. forward multiply with quaternion equivalent to desired euler commands (in degrees) -// 3. call airsim client's setcameraorientation which sets camera orientation wrt world (or takeoff?) ned frame. todo +// 3. call airsim client's setCameraPose which sets camera pose wrt world (or takeoff?) ned frame. todo void AirsimROSWrapper::gimbal_angle_euler_cmd_cb(const airsim_ros_pkgs::GimbalAngleEulerCmd& gimbal_angle_euler_cmd_msg) { try @@ -1181,11 +1186,11 @@ void AirsimROSWrapper::update_commands() } } - // todo add and expose a gimbal angular velocity to airlib + // Only camera rotation, no translation movement of camera if (has_gimbal_cmd_) { std::lock_guard guard(drone_control_mutex_); - airsim_client_->simSetCameraOrientation(gimbal_cmd_.camera_name, gimbal_cmd_.target_quat, gimbal_cmd_.vehicle_name); + airsim_client_.simSetCameraPose(gimbal_cmd_.camera_name, get_airlib_pose(0, 0, 0, gimbal_cmd_.target_quat), gimbal_cmd_.vehicle_name); } has_gimbal_cmd_ = false;