From 0021bfe07132f81d68291705f7eb710da4e8cbfa Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Mon, 11 Jan 2021 20:49:10 +0530 Subject: [PATCH 01/29] PawnSimApi: Change member init to initializer list --- Unreal/Plugins/AirSim/Source/PawnSimApi.h | 17 ++++++----------- 1 file changed, 6 insertions(+), 11 deletions(-) diff --git a/Unreal/Plugins/AirSim/Source/PawnSimApi.h b/Unreal/Plugins/AirSim/Source/PawnSimApi.h index a9f1d364c3..a1c2f28c88 100644 --- a/Unreal/Plugins/AirSim/Source/PawnSimApi.h +++ b/Unreal/Plugins/AirSim/Source/PawnSimApi.h @@ -54,18 +54,13 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase } Params(APawn* pawn_val, const NedTransform* global_transform_val, PawnEvents* pawn_events_val, - const common_utils::UniqueValueMap cameras_val, UClass* pip_camera_class_val, - UParticleSystem* collision_display_template_val, const msr::airlib::GeoPoint home_geopoint_val, - std::string vehicle_name_val) + const common_utils::UniqueValueMap& cameras_val, UClass* pip_camera_class_val, + UParticleSystem* collision_display_template_val, const msr::airlib::GeoPoint& home_geopoint_val, + const std::string& vehicle_name_val) + : pawn(pawn_val), global_transform(global_transform_val), pawn_events(pawn_events_val), cameras(cameras_val), + pip_camera_class(pip_camera_class_val), collision_display_template(collision_display_template_val), + home_geopoint(home_geopoint_val), vehicle_name(vehicle_name_val) { - pawn = pawn_val; - global_transform = global_transform_val; - pawn_events = pawn_events_val; - cameras = cameras_val; - pip_camera_class = pip_camera_class_val; - collision_display_template = collision_display_template_val; - home_geopoint = home_geopoint_val; - vehicle_name = vehicle_name_val; } }; From 7cb9b74cdb24244d3c1d280f5bf6b3a8b6065452 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Mon, 11 Jan 2021 21:17:25 +0530 Subject: [PATCH 02/29] Move some Camera API implementations to APIPCamera Plus cleanup --- AirLib/include/api/VehicleSimApiBase.hpp | 2 +- .../AirsimWrapper/Source/PawnSimApi.cpp | 2 +- .../AirsimWrapper/Source/PawnSimApi.h | 4 +- Unreal/Plugins/AirSim/Source/PIPCamera.cpp | 37 ++++++++++++++++- Unreal/Plugins/AirSim/Source/PIPCamera.h | 15 +++---- Unreal/Plugins/AirSim/Source/PawnSimApi.cpp | 40 ++++++------------- Unreal/Plugins/AirSim/Source/PawnSimApi.h | 2 +- 7 files changed, 61 insertions(+), 41 deletions(-) diff --git a/AirLib/include/api/VehicleSimApiBase.hpp b/AirLib/include/api/VehicleSimApiBase.hpp index 410393d58c..1476e1918c 100644 --- a/AirLib/include/api/VehicleSimApiBase.hpp +++ b/AirLib/include/api/VehicleSimApiBase.hpp @@ -63,7 +63,7 @@ namespace airlib 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 void setDistortionParam(const std::string& camera_name, const std::string& param_name, float value) = 0; - virtual std::vector getDistortionParams(const std::string& camera_name) = 0; + virtual std::vector getDistortionParams(const std::string& camera_name) const = 0; virtual CollisionInfo getCollisionInfo() const = 0; virtual int getRemoteControlID() const = 0; //which RC to use, 0 is first one, -1 means disable RC (use keyborad) diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp b/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp index c48a995b36..3a63f8c287 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp @@ -279,7 +279,7 @@ void PawnSimApi::setDistortionParam(const std::string& camera_name, const std::s // not implemented } -std::vector PawnSimApi::getDistortionParams(const std::string& camera_name) +std::vector PawnSimApi::getDistortionParams(const std::string& camera_name) const { // not implemented std::vector params(5, 0.0); diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.h b/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.h index e9a9528a9b..a505fd7432 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.h +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.h @@ -78,7 +78,7 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase 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 void setDistortionParam(const std::string& camera_name, const std::string& param_name, float value) override; - virtual std::vector getDistortionParams(const std::string& camera_name) override; + virtual std::vector getDistortionParams(const std::string& camera_name) const override; virtual CollisionInfo getCollisionInfo() const override; virtual int getRemoteControlID() const override; virtual msr::airlib::RCData getRCData() const override; @@ -129,4 +129,4 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase std::unique_ptr kinematics_; std::unique_ptr environment_; -}; \ No newline at end of file +}; diff --git a/Unreal/Plugins/AirSim/Source/PIPCamera.cpp b/Unreal/Plugins/AirSim/Source/PIPCamera.cpp index ec2080cf43..12a8a831b5 100644 --- a/Unreal/Plugins/AirSim/Source/PIPCamera.cpp +++ b/Unreal/Plugins/AirSim/Source/PIPCamera.cpp @@ -270,8 +270,10 @@ void APIPCamera::setCameraTypeEnabled(ImageType type, bool enabled) enableCaptureComponent(type, enabled); } -void APIPCamera::setCameraPose(const FTransform& pose) +void APIPCamera::setCameraPose(const msr::airlib::Pose& relative_pose) { + FTransform pose = ned_transform_->fromRelativeNed(relative_pose); + FVector position = pose.GetLocation(); this->SetActorRelativeLocation(pose.GetLocation()); @@ -294,6 +296,39 @@ void APIPCamera::setCameraFoV(float fov_degrees) camera_->SetFieldOfView(fov_degrees); } +msr::airlib::CameraInfo APIPCamera::getCameraInfo() const +{ + msr::airlib::CameraInfo camera_info; + + camera_info.pose.position = ned_transform_->toLocalNed(this->GetActorLocation()); + camera_info.pose.orientation = ned_transform_->toNed(this->GetActorRotation().Quaternion()); + camera_info.fov = camera_->FieldOfView; + camera_info.proj_mat = getProjectionMatrix(ImageType::Scene); + return camera_info; +} + +std::vector APIPCamera::getDistortionParams() const +{ + std::vector param_values(5, 0.0); + + auto getParamValue = [this](const auto &name, float &val) { + distortion_param_instance_->GetScalarParameterValue(FName(name), val); + }; + + getParamValue(TEXT("K1"), param_values[0]); + getParamValue(TEXT("K2"), param_values[1]); + getParamValue(TEXT("K3"), param_values[2]); + getParamValue(TEXT("P1"), param_values[3]); + getParamValue(TEXT("P2"), param_values[4]); + + return param_values; +} + +void APIPCamera::setDistortionParam(const std::string& param_name, float value) +{ + distortion_param_instance_->SetScalarParameterValue(FName(param_name.c_str()), value); +} + void APIPCamera::setupCameraFromSettings(const APIPCamera::CameraSetting& camera_setting, const NedTransform& ned_transform) { //TODO: should we be ignoring position and orientation settings here? diff --git a/Unreal/Plugins/AirSim/Source/PIPCamera.h b/Unreal/Plugins/AirSim/Source/PIPCamera.h index a85fe80217..b9e2e8643f 100644 --- a/Unreal/Plugins/AirSim/Source/PIPCamera.h +++ b/Unreal/Plugins/AirSim/Source/PIPCamera.h @@ -42,8 +42,11 @@ 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 setCameraPose(const FTransform& pose); + void setCameraPose(const msr::airlib::Pose& relative_pose); void setCameraFoV(float fov_degrees); + msr::airlib::CameraInfo getCameraInfo() const; + std::vector getDistortionParams() const; + void setDistortionParam(const std::string& param_name, float value); msr::airlib::ProjectionMatrix getProjectionMatrix(const APIPCamera::ImageType image_type) const; @@ -52,13 +55,11 @@ class AIRSIM_API APIPCamera : public ACameraActor UDetectionComponent* getDetectionComponent(const ImageType type, bool if_active) const; msr::airlib::Pose getPose() const; - - UPROPERTY() - UMaterialParameterCollection* distortion_param_collection_; - UPROPERTY() - UMaterialParameterCollectionInstance* distortion_param_instance_; - + private: //members + UPROPERTY() UMaterialParameterCollection* distortion_param_collection_; + UPROPERTY() UMaterialParameterCollectionInstance* distortion_param_instance_; + UPROPERTY() TArray captures_; UPROPERTY() diff --git a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp b/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp index 24a46b7ecb..1a893b793c 100644 --- a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp @@ -503,24 +503,16 @@ void PawnSimApi::plot(std::istream& s, FColor color, const Vector3r& offset) msr::airlib::CameraInfo PawnSimApi::getCameraInfo(const std::string& camera_name) const { - msr::airlib::CameraInfo camera_info; - const APIPCamera* camera = getCamera(camera_name); - camera_info.pose.position = ned_transform_.toLocalNed(camera->GetActorLocation()); - camera_info.pose.orientation = ned_transform_.toNed(camera->GetActorRotation().Quaternion()); - camera_info.fov = camera->GetCameraComponent()->FieldOfView; - camera_info.proj_mat = camera->getProjectionMatrix(APIPCamera::ImageType::Scene); - return camera_info; + return camera->getCameraInfo(); } void PawnSimApi::setCameraPose(const std::string& camera_name, const msr::airlib::Pose& pose) { UAirBlueprintLib::RunCommandOnGameThread([this, camera_name, pose]() { APIPCamera* camera = getCamera(camera_name); - FTransform pose_unreal = ned_transform_.fromRelativeNed(pose); - camera->setCameraPose(pose_unreal); - }, - true); + camera->setCameraPose(pose); + }, true); } void PawnSimApi::setCameraFoV(const std::string& camera_name, float fov_degrees) @@ -534,25 +526,17 @@ void PawnSimApi::setCameraFoV(const std::string& camera_name, float fov_degrees) void PawnSimApi::setDistortionParam(const std::string& camera_name, const std::string& param_name, float value) { - UAirBlueprintLib::RunCommandOnGameThread([this, camera_name, param_name, value]() { - APIPCamera* camera = getCamera(camera_name); - camera->distortion_param_instance_->SetScalarParameterValue(FName(param_name.c_str()), value); - }, - true); + UAirBlueprintLib::RunCommandOnGameThread([this, &camera_name, ¶m_name, &value]() { + getCamera(camera_name)->setDistortionParam(param_name, value); + }, true); } -std::vector PawnSimApi::getDistortionParams(const std::string& camera_name) +std::vector PawnSimApi::getDistortionParams(const std::string& camera_name) const { - std::vector param_values(5, 0.0); - UAirBlueprintLib::RunCommandOnGameThread([this, camera_name, ¶m_values]() { - APIPCamera* camera = getCamera(camera_name); - camera->distortion_param_instance_->GetScalarParameterValue(FName(TEXT("K1")), param_values[0]); - camera->distortion_param_instance_->GetScalarParameterValue(FName(TEXT("K2")), param_values[1]); - camera->distortion_param_instance_->GetScalarParameterValue(FName(TEXT("K3")), param_values[2]); - camera->distortion_param_instance_->GetScalarParameterValue(FName(TEXT("P1")), param_values[3]); - camera->distortion_param_instance_->GetScalarParameterValue(FName(TEXT("P2")), param_values[4]); - }, - true); + std::vector param_values; + UAirBlueprintLib::RunCommandOnGameThread([this, &camera_name, ¶m_values]() { + param_values = getCamera(camera_name)->getDistortionParams(); + }, true); return param_values; } @@ -705,4 +689,4 @@ std::string PawnSimApi::getRecordFileLine(bool is_header_line) const msr::airlib::VehicleApiBase* PawnSimApi::getVehicleApiBase() const { return nullptr; -} \ No newline at end of file +} diff --git a/Unreal/Plugins/AirSim/Source/PawnSimApi.h b/Unreal/Plugins/AirSim/Source/PawnSimApi.h index a1c2f28c88..a527e0d7bc 100644 --- a/Unreal/Plugins/AirSim/Source/PawnSimApi.h +++ b/Unreal/Plugins/AirSim/Source/PawnSimApi.h @@ -79,7 +79,7 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase 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 void setDistortionParam(const std::string& camera_name, const std::string& param_name, float value) override; - virtual std::vector getDistortionParams(const std::string& camera_name) override; + virtual std::vector getDistortionParams(const std::string& camera_name) const override; virtual CollisionInfo getCollisionInfo() const override; virtual int getRemoteControlID() const override; From 0b18b7d8e7dece4cd8c34fa74f258e3ae1260b94 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Tue, 12 Jan 2021 16:09:22 +0530 Subject: [PATCH 03/29] Add initial External Cameras implementation --- AirLib/include/common/AirSimSettings.hpp | 20 +++++++++++ .../AirSim/Source/SimMode/SimModeBase.cpp | 34 +++++++++++++++++++ .../AirSim/Source/SimMode/SimModeBase.h | 2 ++ 3 files changed, 56 insertions(+) diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index 366a8a2841..ef30c7fd35 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/AirLib/include/common/AirSimSettings.hpp @@ -393,6 +393,7 @@ namespace airlib std::string speed_unit_label = "m\\s"; std::map> sensor_defaults; Vector3r wind = Vector3r::Zero(); + std::map external_cameras; std::string settings_text_ = ""; @@ -1327,7 +1328,26 @@ namespace airlib else createDefaultSensorSettings(simmode_name, sensors); } + + static void loadExternalCameraSettings(const Settings& settings_json, std::map& external_cameras) + { + external_cameras.clear(); + + Settings json_parent; + if (settings_json.getChild("ExternalCameras", json_parent)) { + std::vector keys; + json_parent.getChildNames(keys); + + for (const auto& key : keys) { + Settings child; + json_parent.getChild(key, child); + external_cameras[key] = createCameraSetting(child); + } + } + } + }; } } //namespace + #endif diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index f374a0a45c..1c49e1e3a7 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -459,6 +459,37 @@ void ASimModeBase::initializeCameraDirector(const FTransform& camera_transform, } } +void ASimModeBase::initializeExternalCameras() +{ + //UStaticMeshComponent* bodyMesh = UAirBlueprintLib::GetActorComponent(this, TEXT("BodyMesh")); + // USceneComponent* bodyMesh = params_.pawn->GetRootComponent(); + FActorSpawnParameters camera_spawn_params; + camera_spawn_params.SpawnCollisionHandlingOverride = ESpawnActorCollisionHandlingMethod::AdjustIfPossibleButAlwaysSpawn; + const auto& transform = getGlobalNedTransform(); + + //for each camera in settings + for (const auto& camera_setting_pair : getSettings().external_cameras) { + const auto& setting = camera_setting_pair.second; + + //get pose + FVector position = transform.fromLocalNed( + NedTransform::Vector3r(setting.position.x(), setting.position.y(), setting.position.z())) + - transform.fromLocalNed(NedTransform::Vector3r(0.0, 0.0, 0.0)); + FTransform camera_transform(FRotator(setting.rotation.pitch, setting.rotation.yaw, setting.rotation.roll), + position, FVector(1., 1., 1.)); + + //spawn and attach camera to pawn + camera_spawn_params.Name = FName(("external_" + camera_setting_pair.first).c_str()); + APIPCamera* camera = this->GetWorld()->SpawnActor(pip_camera_class, camera_transform, camera_spawn_params); + // camera->AttachToComponent(bodyMesh, FAttachmentTransformRules::KeepRelativeTransform); + + camera->setupCameraFromSettings(setting, transform); + + //add on to our collection + external_cameras_.insert_or_assign(camera_setting_pair.first, camera); + } +} + bool ASimModeBase::toggleRecording() { if (isRecording()) @@ -693,6 +724,9 @@ void ASimModeBase::setupVehiclesAndCamera() } } + // Create External Cameras + initializeExternalCameras(); + if (getApiProvider()->hasDefaultVehicle()) { //TODO: better handle no FPV vehicles scenario getVehicleSimApi()->possess(); diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h index 1ec7126a9a..38c1077389 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h @@ -121,6 +121,7 @@ class AIRSIM_API ASimModeBase : public AActor void initializeCameraDirector(const FTransform& camera_transform, float follow_distance); void checkVehicleReady(); //checks if vehicle is available to use virtual void updateDebugReport(msr::airlib::StateReporterWrapper& debug_reporter); + virtual void initializeExternalCameras(); protected: //Utility methods for derived classes virtual const msr::airlib::AirSimSettings& getSettings() const; @@ -174,6 +175,7 @@ class AIRSIM_API ASimModeBase : public AActor msr::airlib::StateReporterWrapper debug_reporter_; std::vector> vehicle_sim_apis_; + common_utils::UniqueValueMap external_cameras_; UPROPERTY() TArray spawned_actors_; //keep refs alive from Unreal GC From 603712e1a434147e89e9ec54f12d83d61aba8d64 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Fri, 15 Jan 2021 21:02:35 +0530 Subject: [PATCH 04/29] Add getCameraInfo API for external cameras --- AirLib/include/api/RpcLibClientBase.hpp | 3 ++- AirLib/include/api/WorldSimApiBase.hpp | 4 ++++ AirLib/src/api/RpcLibClientBase.cpp | 4 ++-- AirLib/src/api/RpcLibServerBase.cpp | 4 ++-- PythonClient/airsim/client.py | 4 ++-- Unreal/Plugins/AirSim/Source/PawnSimApi.cpp | 3 +-- .../Plugins/AirSim/Source/SimMode/SimModeBase.cpp | 8 ++++++++ Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h | 13 +++++++++++++ Unreal/Plugins/AirSim/Source/WorldSimApi.cpp | 13 ++++++++++++- Unreal/Plugins/AirSim/Source/WorldSimApi.h | 6 +++++- 10 files changed, 51 insertions(+), 11 deletions(-) diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index febc97574e..96fa961134 100644 --- a/AirLib/include/api/RpcLibClientBase.hpp +++ b/AirLib/include/api/RpcLibClientBase.hpp @@ -120,7 +120,7 @@ namespace airlib CollisionInfo simGetCollisionInfo(const std::string& vehicle_name = "") const; - CameraInfo simGetCameraInfo(const std::string& camera_name, const std::string& vehicle_name = "") const; + CameraInfo simGetCameraInfo(const std::string& camera_name, const std::string& vehicle_name = "", bool external = false) const; void simSetDistortionParam(const std::string& camera_name, const std::string& param_name, float value, const std::string& vehicle_name = ""); std::vector simGetDistortionParams(const std::string& camera_name, const std::string& vehicle_name = ""); void simSetCameraPose(const std::string& camera_name, const Pose& pose, const std::string& vehicle_name = ""); @@ -152,4 +152,5 @@ namespace airlib }; } } //namespace + #endif diff --git a/AirLib/include/api/WorldSimApiBase.hpp b/AirLib/include/api/WorldSimApiBase.hpp index 0ea0e3ed19..0bc57ad958 100644 --- a/AirLib/include/api/WorldSimApiBase.hpp +++ b/AirLib/include/api/WorldSimApiBase.hpp @@ -88,7 +88,11 @@ namespace airlib virtual bool testLineOfSightBetweenPoints(const msr::airlib::GeoPoint& point1, const msr::airlib::GeoPoint& point2) const = 0; virtual vector getWorldExtents() const = 0; + + // Image APIs + virtual CameraInfo getCameraInfo(const std::string& camera_name, const std::string& vehicle_name = "", bool external = false) const = 0; }; } } //namespace + #endif diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index e0b0c7ad9e..0cc816e205 100644 --- a/AirLib/src/api/RpcLibClientBase.cpp +++ b/AirLib/src/api/RpcLibClientBase.cpp @@ -435,9 +435,9 @@ __pragma(warning(disable : 4239)) return pimpl_->client.call("simSetObjectScale", object_name, RpcLibAdaptorsBase::Vector3r(scale)).as(); } - CameraInfo RpcLibClientBase::simGetCameraInfo(const std::string& camera_name, const std::string& vehicle_name) const + CameraInfo RpcLibClientBase::simGetCameraInfo(const std::string& camera_name, const std::string& vehicle_name, bool external) const { - return pimpl_->client.call("simGetCameraInfo", camera_name, vehicle_name).as().to(); + return pimpl_->client.call("simGetCameraInfo", camera_name, vehicle_name, external).as().to(); } void RpcLibClientBase::simSetCameraPose(const std::string& camera_name, const Pose& pose, const std::string& vehicle_name) diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index 6860aaa584..2fba661085 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -268,8 +268,8 @@ namespace airlib return RpcLibAdaptorsBase::DistanceSensorData(distance_sensor_data); }); - pimpl_->server.bind("simGetCameraInfo", [&](const std::string& camera_name, const std::string& vehicle_name) -> RpcLibAdaptorsBase::CameraInfo { - const auto& camera_info = getVehicleSimApi(vehicle_name)->getCameraInfo(camera_name); + pimpl_->server.bind("simGetCameraInfo", [&](const std::string& camera_name, const std::string& vehicle_name, bool external) -> RpcLibAdaptorsBase::CameraInfo { + const auto& camera_info = getWorldSimApi()->getCameraInfo(camera_name, vehicle_name, external); return RpcLibAdaptorsBase::CameraInfo(camera_info); }); diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index e3e81125bc..ed571740bb 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -579,7 +579,7 @@ def simPrintLogMessage(self, message, message_param = "", severity = 0): """ self.client.call('simPrintLogMessage', message, message_param, severity) - def simGetCameraInfo(self, camera_name, vehicle_name = ''): + def simGetCameraInfo(self, camera_name, vehicle_name = '', external=False): """ Get details about the camera @@ -591,7 +591,7 @@ def simGetCameraInfo(self, camera_name, vehicle_name = ''): CameraInfo: """ # 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)) + return CameraInfo.from_msgpack(self.client.call('simGetCameraInfo', str(camera_name), vehicle_name, external)) def simGetDistortionParams(self, camera_name, vehicle_name = ''): """ diff --git a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp b/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp index 1a893b793c..b4748fb182 100644 --- a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp @@ -503,8 +503,7 @@ void PawnSimApi::plot(std::istream& s, FColor color, const Vector3r& offset) msr::airlib::CameraInfo PawnSimApi::getCameraInfo(const std::string& camera_name) const { - const APIPCamera* camera = getCamera(camera_name); - return camera->getCameraInfo(); + return getCamera(camera_name)->getCameraInfo(); } void PawnSimApi::setCameraPose(const std::string& camera_name, const msr::airlib::Pose& pose) diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index 1c49e1e3a7..d3649d4e54 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -515,6 +515,14 @@ bool ASimModeBase::isRecording() const return FRecordingThread::isRecording(); } +msr::airlib::CameraInfo ASimModeBase::getCameraInfo(const std::string& camera_name, const std::string& vehicle_name, bool external) const +{ + if (external) + return getExternalCamera(camera_name)->getCameraInfo(); + else + return getVehicleSimApi(vehicle_name)->getCameraInfo(camera_name); +} + //API server start/stop void ASimModeBase::startApiServer() { diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h index 38c1077389..b218cd8777 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h @@ -70,6 +70,8 @@ class AIRSIM_API ASimModeBase : public AActor virtual void stopRecording(); virtual bool isRecording() const; + virtual msr::airlib::CameraInfo getCameraInfo(const std::string& camera_name, const std::string& vehicle_name, bool external) const; + void startApiServer(); void stopApiServer(); bool isApiServerStarted(); @@ -92,6 +94,17 @@ class AIRSIM_API ASimModeBase : public AActor return static_cast(api_provider_->getVehicleSimApi(vehicle_name)); } + const APIPCamera* getExternalCamera(const std::string& camera_name) const + { + return external_cameras_.findOrDefault(camera_name, nullptr); + } + + APIPCamera* getExternalCamera(const std::string& camera_name) + { + return const_cast( + static_cast(this)->getExternalCamera(camera_name)); + } + TMap asset_map; TMap scene_object_map; diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index 841630531e..b6335c2c39 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -729,4 +729,15 @@ std::vector WorldSimApi::getWorldExtents() const result.push_back(lla_max_out); return result; -} \ No newline at end of file +} + +CameraInfo WorldSimApi::getCameraInfo(const std::string& camera_name, const std::string& vehicle_name, bool external) const +{ + CameraInfo info; + UAirBlueprintLib::RunCommandOnGameThread([this, &camera_name, &vehicle_name, external, &info]() { + info = simmode_->getCameraInfo(camera_name, vehicle_name, external); + }, + true); + + return info; +} diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.h b/Unreal/Plugins/AirSim/Source/WorldSimApi.h index c5e7e466d4..ee986baa82 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.h +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.h @@ -78,6 +78,10 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase virtual bool testLineOfSightBetweenPoints(const msr::airlib::GeoPoint& point1, const msr::airlib::GeoPoint& point2) const override; virtual std::vector getWorldExtents() const override; + // Image APIs + virtual msr::airlib::CameraInfo getCameraInfo(const std::string& camera_name, const std::string& vehicle_name = "", + bool external = false) const override; + private: AActor* createNewActor(const FActorSpawnParameters& spawn_params, const FTransform& actor_transform, const Vector3r& scale, UStaticMesh* static_mesh); void spawnPlayer(); @@ -86,4 +90,4 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase ASimModeBase* simmode_; ULevelStreamingDynamic* current_level_; std::vector voxel_grid_; -}; \ No newline at end of file +}; From cf4a1cc67b687244f8a57cf836dfd976095fe8f4 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Thu, 21 Jan 2021 16:59:42 +0530 Subject: [PATCH 05/29] Move getCameraInfo implementation to WorldSimApi --- AirLib/include/api/VehicleSimApiBase.hpp | 1 - Unreal/Plugins/AirSim/Source/PawnSimApi.cpp | 5 ----- Unreal/Plugins/AirSim/Source/PawnSimApi.h | 1 - Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp | 6 +++--- Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h | 8 +------- Unreal/Plugins/AirSim/Source/WorldSimApi.cpp | 6 ++++-- 6 files changed, 8 insertions(+), 19 deletions(-) diff --git a/AirLib/include/api/VehicleSimApiBase.hpp b/AirLib/include/api/VehicleSimApiBase.hpp index 1476e1918c..041a52c163 100644 --- a/AirLib/include/api/VehicleSimApiBase.hpp +++ b/AirLib/include/api/VehicleSimApiBase.hpp @@ -59,7 +59,6 @@ namespace airlib virtual const Kinematics::State* getGroundTruthKinematics() const = 0; virtual const msr::airlib::Environment* getGroundTruthEnvironment() const = 0; - virtual CameraInfo getCameraInfo(const std::string& camera_name) const = 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 void setDistortionParam(const std::string& camera_name, const std::string& param_name, float value) = 0; diff --git a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp b/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp index b4748fb182..d778fd8ff5 100644 --- a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp @@ -501,11 +501,6 @@ void PawnSimApi::plot(std::istream& s, FColor color, const Vector3r& offset) } } -msr::airlib::CameraInfo PawnSimApi::getCameraInfo(const std::string& camera_name) const -{ - return getCamera(camera_name)->getCameraInfo(); -} - void PawnSimApi::setCameraPose(const std::string& camera_name, const msr::airlib::Pose& pose) { UAirBlueprintLib::RunCommandOnGameThread([this, camera_name, pose]() { diff --git a/Unreal/Plugins/AirSim/Source/PawnSimApi.h b/Unreal/Plugins/AirSim/Source/PawnSimApi.h index a527e0d7bc..12e3acc71d 100644 --- a/Unreal/Plugins/AirSim/Source/PawnSimApi.h +++ b/Unreal/Plugins/AirSim/Source/PawnSimApi.h @@ -75,7 +75,6 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase virtual std::vector getImage(const std::string& camera_name, ImageCaptureBase::ImageType image_type) const override; 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 setCameraPose(const std::string& camera_name, const Pose& pose) override; virtual void setCameraFoV(const std::string& camera_name, float fov_degrees) override; virtual void setDistortionParam(const std::string& camera_name, const std::string& param_name, float value) override; diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index d3649d4e54..afedd68d90 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -515,12 +515,12 @@ bool ASimModeBase::isRecording() const return FRecordingThread::isRecording(); } -msr::airlib::CameraInfo ASimModeBase::getCameraInfo(const std::string& camera_name, const std::string& vehicle_name, bool external) const +const APIPCamera* ASimModeBase::getCamera(const std::string& camera_name, const std::string& vehicle_name, bool external) const { if (external) - return getExternalCamera(camera_name)->getCameraInfo(); + return getExternalCamera(camera_name); else - return getVehicleSimApi(vehicle_name)->getCameraInfo(camera_name); + return getVehicleSimApi(vehicle_name)->getCamera(camera_name); } //API server start/stop diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h index b218cd8777..856bf7ebd4 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h @@ -70,8 +70,6 @@ class AIRSIM_API ASimModeBase : public AActor virtual void stopRecording(); virtual bool isRecording() const; - virtual msr::airlib::CameraInfo getCameraInfo(const std::string& camera_name, const std::string& vehicle_name, bool external) const; - void startApiServer(); void stopApiServer(); bool isApiServerStarted(); @@ -99,11 +97,7 @@ class AIRSIM_API ASimModeBase : public AActor return external_cameras_.findOrDefault(camera_name, nullptr); } - APIPCamera* getExternalCamera(const std::string& camera_name) - { - return const_cast( - static_cast(this)->getExternalCamera(camera_name)); - } + const APIPCamera* getCamera(const std::string& camera_name, const std::string& vehicle_name = "", bool external = false) const; TMap asset_map; TMap scene_object_map; diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index b6335c2c39..e86cfc6abf 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -734,8 +734,10 @@ std::vector WorldSimApi::getWorldExtents() const CameraInfo WorldSimApi::getCameraInfo(const std::string& camera_name, const std::string& vehicle_name, bool external) const { CameraInfo info; - UAirBlueprintLib::RunCommandOnGameThread([this, &camera_name, &vehicle_name, external, &info]() { - info = simmode_->getCameraInfo(camera_name, vehicle_name, external); + + UAirBlueprintLib::RunCommandOnGameThread([this, &camera_name, &vehicle_name, &external, &info]() { + auto camera = simmode_->getCamera(camera_name, vehicle_name, external); + info = camera->getCameraInfo(); }, true); From 710379e3dbdcbb8bf59c97c2eaf4905baf5a135f Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Thu, 21 Jan 2021 18:37:02 +0530 Subject: [PATCH 06/29] Move most Camera APIs to WorldSimApi --- AirLib/include/api/RpcLibClientBase.hpp | 11 +++--- AirLib/include/api/VehicleSimApiBase.hpp | 5 --- AirLib/include/api/WorldSimApiBase.hpp | 8 ++++ AirLib/src/api/RpcLibClientBase.cpp | 20 +++++----- AirLib/src/api/RpcLibServerBase.cpp | 21 ++++++---- PythonClient/airsim/client.py | 25 ++++++------ Unreal/Plugins/AirSim/Source/PawnSimApi.cpp | 34 ---------------- Unreal/Plugins/AirSim/Source/PawnSimApi.h | 4 -- .../AirSim/Source/SimMode/SimModeBase.h | 14 ++++++- Unreal/Plugins/AirSim/Source/WorldSimApi.cpp | 39 +++++++++++++++++++ Unreal/Plugins/AirSim/Source/WorldSimApi.h | 11 +++++- 11 files changed, 111 insertions(+), 81 deletions(-) diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index 96fa961134..81bea76896 100644 --- a/AirLib/include/api/RpcLibClientBase.hpp +++ b/AirLib/include/api/RpcLibClientBase.hpp @@ -121,12 +121,13 @@ namespace airlib CollisionInfo simGetCollisionInfo(const std::string& vehicle_name = "") const; CameraInfo simGetCameraInfo(const std::string& camera_name, const std::string& vehicle_name = "", bool external = false) const; - void simSetDistortionParam(const std::string& camera_name, const std::string& param_name, float value, const std::string& vehicle_name = ""); - std::vector simGetDistortionParams(const std::string& camera_name, 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 = ""); + void simSetDistortionParam(const std::string& camera_name, const std::string& param_name, float value, const std::string& vehicle_name = "", bool external = false); + std::vector simGetDistortionParams(const std::string& camera_name, const std::string& vehicle_name = "", bool external = false); + void simSetCameraPose(const std::string& camera_name, const Pose& pose, const std::string& vehicle_name = "", bool external = false); + void simSetCameraFov(const std::string& camera_name, float fov_degrees, const std::string& vehicle_name = "", bool external = false); // This is a backwards-compatibility wrapper over simSetCameraPose, and can be removed in future major releases - void simSetCameraOrientation(const std::string& camera_name, const Quaternionr& orientation, const std::string& vehicle_name = ""); + void simSetCameraOrientation(const std::string& camera_name, const Quaternionr& orientation, const std::string& vehicle_name = "", bool external = false); + bool simCreateVoxelGrid(const Vector3r& position, const int& x_size, const int& y_size, const int& z_size, const float& res, const std::string& output_file); msr::airlib::Kinematics::State simGetGroundTruthKinematics(const std::string& vehicle_name = "") const; msr::airlib::Environment::State simGetGroundTruthEnvironment(const std::string& vehicle_name = "") const; diff --git a/AirLib/include/api/VehicleSimApiBase.hpp b/AirLib/include/api/VehicleSimApiBase.hpp index 041a52c163..a5dc1bf414 100644 --- a/AirLib/include/api/VehicleSimApiBase.hpp +++ b/AirLib/include/api/VehicleSimApiBase.hpp @@ -59,11 +59,6 @@ namespace airlib virtual const Kinematics::State* getGroundTruthKinematics() const = 0; virtual const msr::airlib::Environment* getGroundTruthEnvironment() const = 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 void setDistortionParam(const std::string& camera_name, const std::string& param_name, float value) = 0; - virtual std::vector getDistortionParams(const std::string& camera_name) const = 0; - virtual CollisionInfo getCollisionInfo() const = 0; virtual int getRemoteControlID() const = 0; //which RC to use, 0 is first one, -1 means disable RC (use keyborad) virtual RCData getRCData() const = 0; //get reading from RC from simulator's host OS diff --git a/AirLib/include/api/WorldSimApiBase.hpp b/AirLib/include/api/WorldSimApiBase.hpp index 0bc57ad958..8d5189668b 100644 --- a/AirLib/include/api/WorldSimApiBase.hpp +++ b/AirLib/include/api/WorldSimApiBase.hpp @@ -91,6 +91,14 @@ namespace airlib // Image APIs virtual CameraInfo getCameraInfo(const std::string& camera_name, const std::string& vehicle_name = "", bool external = false) const = 0; + virtual void setCameraPose(const std::string& camera_name, const msr::airlib::Pose& pose, + const std::string& vehicle_name = "", bool external = false) = 0; + virtual void setCameraFoV(const std::string& camera_name, float fov_degrees, + const std::string& vehicle_name = "", bool external = false) = 0; + virtual void setDistortionParam(const std::string& camera_name, const std::string& param_name, float value, + const std::string& vehicle_name = "", bool external = false) = 0; + virtual std::vector getDistortionParams(const std::string& camera_name, const std::string& vehicle_name = "", + bool external = false) const = 0; }; } } //namespace diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index 0cc816e205..05f51e5be4 100644 --- a/AirLib/src/api/RpcLibClientBase.cpp +++ b/AirLib/src/api/RpcLibClientBase.cpp @@ -440,31 +440,31 @@ __pragma(warning(disable : 4239)) return pimpl_->client.call("simGetCameraInfo", camera_name, vehicle_name, external).as().to(); } - void RpcLibClientBase::simSetCameraPose(const std::string& camera_name, const Pose& pose, const std::string& vehicle_name) + void RpcLibClientBase::simSetCameraPose(const std::string& camera_name, const Pose& pose, const std::string& vehicle_name, bool external) { - pimpl_->client.call("simSetCameraPose", camera_name, RpcLibAdaptorsBase::Pose(pose), vehicle_name); + pimpl_->client.call("simSetCameraPose", camera_name, RpcLibAdaptorsBase::Pose(pose), vehicle_name, external); } - void RpcLibClientBase::simSetCameraOrientation(const std::string& camera_name, const Quaternionr& orientation, const std::string& vehicle_name) + void RpcLibClientBase::simSetCameraOrientation(const std::string& camera_name, const Quaternionr& orientation, const std::string& vehicle_name, bool external) { std::cout << "`simSetCameraOrientation` API has been upgraded to `simSetCameraPose`. Please update your code." << std::endl; Pose pose{ Vector3r::Zero(), orientation }; - RpcLibClientBase::simSetCameraPose(camera_name, pose, vehicle_name); + RpcLibClientBase::simSetCameraPose(camera_name, pose, vehicle_name, external); } - void RpcLibClientBase::simSetCameraFov(const std::string& camera_name, float fov_degrees, const std::string& vehicle_name) + void RpcLibClientBase::simSetCameraFov(const std::string& camera_name, float fov_degrees, const std::string& vehicle_name, bool external) { - pimpl_->client.call("simSetCameraFov", camera_name, fov_degrees, vehicle_name); + pimpl_->client.call("simSetCameraFov", camera_name, fov_degrees, vehicle_name, external); } - void RpcLibClientBase::simSetDistortionParam(const std::string& camera_name, const std::string& param_name, float value, const std::string& vehicle_name) + void RpcLibClientBase::simSetDistortionParam(const std::string& camera_name, const std::string& param_name, float value, const std::string& vehicle_name, bool external) { - pimpl_->client.call("simSetDistortionParam", camera_name, param_name, value, vehicle_name); + pimpl_->client.call("simSetDistortionParam", camera_name, param_name, value, vehicle_name, external); } - std::vector RpcLibClientBase::simGetDistortionParams(const std::string& camera_name, const std::string& vehicle_name) + std::vector RpcLibClientBase::simGetDistortionParams(const std::string& camera_name, const std::string& vehicle_name, bool external) { - return pimpl_->client.call("simGetDistortionParams", camera_name, vehicle_name).as>(); + return pimpl_->client.call("simGetDistortionParams", camera_name, vehicle_name, external).as>(); } msr::airlib::Kinematics::State RpcLibClientBase::simGetGroundTruthKinematics(const std::string& vehicle_name) const diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index 2fba661085..7595d5d1cf 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -147,6 +147,7 @@ namespace airlib pimpl_->server.bind("simGetImages", [&](const std::vector& request_adapter, const std::string& vehicle_name) -> vector { const auto& response = getVehicleSimApi(vehicle_name)->getImages(RpcLibAdaptorsBase::ImageRequest::to(request_adapter)); return RpcLibAdaptorsBase::ImageResponse::from(response); +<<<<<<< HEAD }); pimpl_->server.bind("simGetImage", [&](const std::string& camera_name, ImageCaptureBase::ImageType type, const std::string& vehicle_name) -> vector { @@ -273,20 +274,24 @@ namespace airlib return RpcLibAdaptorsBase::CameraInfo(camera_info); }); - pimpl_->server.bind("simSetDistortionParam", [&](const std::string& camera_name, const std::string& param_name, float value, const std::string& vehicle_name) -> void { - getVehicleSimApi(vehicle_name)->setDistortionParam(camera_name, param_name, value); + pimpl_->server.bind("simSetDistortionParam", [&](const std::string& camera_name, const std::string& param_name, float value, + const std::string& vehicle_name, bool external) -> void { + getWorldSimApi()->setDistortionParam(camera_name, param_name, value, vehicle_name, external); }); - pimpl_->server.bind("simGetDistortionParams", [&](const std::string& camera_name, const std::string& vehicle_name) -> std::vector { - return getVehicleSimApi(vehicle_name)->getDistortionParams(camera_name); + pimpl_->server.bind("simGetDistortionParams", [&](const std::string& camera_name, const std::string& vehicle_name, + bool external) -> std::vector { + return getWorldSimApi()->getDistortionParams(camera_name, vehicle_name, external); }); - pimpl_->server.bind("simSetCameraPose", [&](const std::string& camera_name, const RpcLibAdaptorsBase::Pose& pose, const std::string& vehicle_name) -> void { - getVehicleSimApi(vehicle_name)->setCameraPose(camera_name, pose.to()); + pimpl_->server.bind("simSetCameraPose", [&](const std::string& camera_name, const RpcLibAdaptorsBase::Pose& pose, + const std::string& vehicle_name, bool external) -> void { + getWorldSimApi()->setCameraPose(camera_name, pose.to(), external); }); - pimpl_->server.bind("simSetCameraFov", [&](const std::string& camera_name, float fov_degrees, const std::string& vehicle_name) -> void { - getVehicleSimApi(vehicle_name)->setCameraFoV(camera_name, fov_degrees); + pimpl_->server.bind("simSetCameraFov", [&](const std::string& camera_name, float fov_degrees, + const std::string& vehicle_name, bool external) -> void { + getWorldSimApi()->setCameraFoV(camera_name, fov_degrees, vehicle_name, external); }); pimpl_->server.bind("simGetCollisionInfo", [&](const std::string& vehicle_name) -> RpcLibAdaptorsBase::CollisionInfo { diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index ed571740bb..56945b201c 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -593,7 +593,7 @@ def simGetCameraInfo(self, camera_name, vehicle_name = '', external=False): # 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, external)) - def simGetDistortionParams(self, camera_name, vehicle_name = ''): + def simGetDistortionParams(self, camera_name, vehicle_name = '', external = False): """ Get camera distortion parameters @@ -605,9 +605,9 @@ def simGetDistortionParams(self, camera_name, vehicle_name = ''): List (float): List of distortion parameter values corresponding to K1, K2, K3, P1, P2 respectively. """ - return self.client.call('simGetDistortionParams', str(camera_name), vehicle_name) + return self.client.call('simGetDistortionParams', str(camera_name), vehicle_name, external) - def simSetDistortionParams(self, camera_name, distortion_params, vehicle_name = ''): + def simSetDistortionParams(self, camera_name, distortion_params, vehicle_name = '', external = False): """ Set camera distortion parameters @@ -619,9 +619,10 @@ def simSetDistortionParams(self, camera_name, distortion_params, vehicle_name = """ for param_name, value in distortion_params.items(): - self.client.call('simSetDistortionParam', str(camera_name), param_name, value, vehicle_name) + # self.client.call('simSetDistortionParam', str(camera_name), param_name, value, vehicle_name, external) + self.simSetDistortionParam(camera_name, param_name, value, vehicle_name, external) - def simSetDistortionParam(self, camera_name, param_name, value, vehicle_name = ''): + def simSetDistortionParam(self, camera_name, param_name, value, vehicle_name = '', external = False): """ Set single camera distortion parameter @@ -631,9 +632,9 @@ def simSetDistortionParam(self, camera_name, param_name, value, vehicle_name = ' value (float): Value of distortion parameter vehicle_name (str, optional): Vehicle which the camera is associated with """ - self.client.call('simSetDistortionParam', str(camera_name), param_name, value, vehicle_name) + self.client.call('simSetDistortionParam', str(camera_name), param_name, value, vehicle_name, external) - def simSetCameraPose(self, camera_name, pose, vehicle_name = ''): + def simSetCameraPose(self, camera_name, pose, vehicle_name = '', external = False): """ - Control the pose of a selected camera @@ -643,9 +644,9 @@ def simSetCameraPose(self, camera_name, pose, vehicle_name = ''): 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('simSetCameraPose', str(camera_name), pose, vehicle_name) + self.client.call('simSetCameraPose', str(camera_name), pose, vehicle_name, external) - def simSetCameraOrientation(self, camera_name, orientation, vehicle_name = ''): + def simSetCameraOrientation(self, camera_name, orientation, vehicle_name = '', external = False): """ .. note:: @@ -660,9 +661,9 @@ def simSetCameraOrientation(self, camera_name, orientation, vehicle_name = ''): """ logging.warning("`simSetCameraOrientation` API has been upgraded to `simSetCameraPose`. Please update your code.") pose = Pose(orientation_val=orientation) - self.simSetCameraPose(camera_name, pose, vehicle_name) + self.simSetCameraPose(camera_name, pose, vehicle_name, external) - def simSetCameraFov(self, camera_name, fov_degrees, vehicle_name = ''): + def simSetCameraFov(self, camera_name, fov_degrees, vehicle_name = '', external = False): """ - Control the field of view of a selected camera @@ -672,7 +673,7 @@ def simSetCameraFov(self, camera_name, fov_degrees, vehicle_name = ''): 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('simSetCameraFov', str(camera_name), fov_degrees, vehicle_name) + self.client.call('simSetCameraFov', str(camera_name), fov_degrees, vehicle_name, external) def simGetGroundTruthKinematics(self, vehicle_name = ''): """ diff --git a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp b/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp index d778fd8ff5..784cb29c57 100644 --- a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp @@ -501,40 +501,6 @@ void PawnSimApi::plot(std::istream& s, FColor color, const Vector3r& offset) } } -void PawnSimApi::setCameraPose(const std::string& camera_name, const msr::airlib::Pose& pose) -{ - UAirBlueprintLib::RunCommandOnGameThread([this, camera_name, pose]() { - APIPCamera* camera = getCamera(camera_name); - camera->setCameraPose(pose); - }, true); -} - -void PawnSimApi::setCameraFoV(const std::string& camera_name, float fov_degrees) -{ - UAirBlueprintLib::RunCommandOnGameThread([this, camera_name, fov_degrees]() { - APIPCamera* camera = getCamera(camera_name); - camera->setCameraFoV(fov_degrees); - }, - true); -} - -void PawnSimApi::setDistortionParam(const std::string& camera_name, const std::string& param_name, float value) -{ - UAirBlueprintLib::RunCommandOnGameThread([this, &camera_name, ¶m_name, &value]() { - getCamera(camera_name)->setDistortionParam(param_name, value); - }, true); -} - -std::vector PawnSimApi::getDistortionParams(const std::string& camera_name) const -{ - std::vector param_values; - UAirBlueprintLib::RunCommandOnGameThread([this, &camera_name, ¶m_values]() { - param_values = getCamera(camera_name)->getDistortionParams(); - }, true); - - return param_values; -} - //parameters in NED frame PawnSimApi::Pose PawnSimApi::getPose() const { diff --git a/Unreal/Plugins/AirSim/Source/PawnSimApi.h b/Unreal/Plugins/AirSim/Source/PawnSimApi.h index 12e3acc71d..60d8204b78 100644 --- a/Unreal/Plugins/AirSim/Source/PawnSimApi.h +++ b/Unreal/Plugins/AirSim/Source/PawnSimApi.h @@ -75,10 +75,6 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase virtual std::vector getImage(const std::string& camera_name, ImageCaptureBase::ImageType image_type) const override; virtual Pose getPose() const override; virtual void setPose(const Pose& pose, bool ignore_collision) 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 void setDistortionParam(const std::string& camera_name, const std::string& param_name, float value) override; - virtual std::vector getDistortionParams(const std::string& camera_name) const override; virtual CollisionInfo getCollisionInfo() const override; virtual int getRemoteControlID() const override; diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h index 856bf7ebd4..a91846eba4 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h @@ -97,7 +97,19 @@ class AIRSIM_API ASimModeBase : public AActor return external_cameras_.findOrDefault(camera_name, nullptr); } - const APIPCamera* getCamera(const std::string& camera_name, const std::string& vehicle_name = "", bool external = false) const; + APIPCamera* getExternalCamera(const std::string& camera_name) + { + return const_cast( + static_cast(this)->getExternalCamera(camera_name)); + } + + const APIPCamera* getCamera(const std::string& camera_name, const std::string& vehicle_name = "", bool external = false) const; + + APIPCamera* getCamera(const std::string& camera_name, const std::string& vehicle_name = "", bool external = false) + { + return const_cast( + static_cast(this)->getCamera(camera_name, vehicle_name, external)); + } TMap asset_map; TMap scene_object_map; diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index e86cfc6abf..f27310b119 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -743,3 +743,42 @@ CameraInfo WorldSimApi::getCameraInfo(const std::string& camera_name, const std: return info; } + +void WorldSimApi::setCameraPose(const std::string& camera_name, const msr::airlib::Pose& pose, + const std::string& vehicle_name, bool external) +{ + UAirBlueprintLib::RunCommandOnGameThread([this, &camera_name, &vehicle_name, &external, &pose]() { + auto camera = simmode_->getCamera(camera_name, vehicle_name, external); + camera->setCameraPose(pose); + }, true); +} + +void WorldSimApi::setCameraFoV(const std::string& camera_name, float fov_degrees, + const std::string& vehicle_name, bool external) +{ + UAirBlueprintLib::RunCommandOnGameThread([this, &camera_name, &vehicle_name, &external, &fov_degrees]() { + auto camera = simmode_->getCamera(camera_name, vehicle_name, external); + camera->setCameraFoV(fov_degrees); + }, true); +} + +void WorldSimApi::setDistortionParam(const std::string& camera_name, const std::string& param_name, float value, + const std::string& vehicle_name, bool external) +{ + UAirBlueprintLib::RunCommandOnGameThread([this, &camera_name, &vehicle_name, &external, + ¶m_name, &value]() { + auto camera = simmode_->getCamera(camera_name, vehicle_name, external); + camera->setDistortionParam(param_name, value); + }, true); +} + +std::vector WorldSimApi::getDistortionParams(const std::string& camera_name, const std::string& vehicle_name, bool external) const +{ + std::vector param_values; + UAirBlueprintLib::RunCommandOnGameThread([this, &camera_name, &vehicle_name, &external, ¶m_values]() { + auto camera = simmode_->getCamera(camera_name, vehicle_name, external); + param_values = camera->getDistortionParams(); + }, true); + + return param_values; +} diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.h b/Unreal/Plugins/AirSim/Source/WorldSimApi.h index ee986baa82..5221e1fa3f 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.h +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.h @@ -79,8 +79,15 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase virtual std::vector getWorldExtents() const override; // Image APIs - virtual msr::airlib::CameraInfo getCameraInfo(const std::string& camera_name, const std::string& vehicle_name = "", - bool external = false) const override; + virtual msr::airlib::CameraInfo getCameraInfo(const std::string& camera_name, const std::string& vehicle_name = "", bool external = false) const override; + virtual void setCameraPose(const std::string& camera_name, const msr::airlib::Pose& pose, + const std::string& vehicle_name = "", bool external = false) override; + virtual void setCameraFoV(const std::string& camera_name, float fov_degrees, + const std::string& vehicle_name = "", bool external = false) override; + virtual void setDistortionParam(const std::string& camera_name, const std::string& param_name, float value, + const std::string& vehicle_name = "", bool external = false) override; + virtual std::vector getDistortionParams(const std::string& camera_name, const std::string& vehicle_name = "", + bool external = false) const override; private: AActor* createNewActor(const FActorSpawnParameters& spawn_params, const FTransform& actor_transform, const Vector3r& scale, UStaticMesh* static_mesh); From 22a9deedf1e5e5fed2d2b786e8478a5e3394c764 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Fri, 22 Jan 2021 13:17:02 +0530 Subject: [PATCH 07/29] Update APIs for getting external images --- AirLib/include/api/WorldSimApiBase.hpp | 5 +++++ AirLib/src/api/RpcLibServerBase.cpp | 12 +++++----- .../AirSim/Source/SimMode/SimModeBase.cpp | 10 +++++++++ .../AirSim/Source/SimMode/SimModeBase.h | 14 +++++++++--- Unreal/Plugins/AirSim/Source/WorldSimApi.cpp | 22 +++++++++++++++++++ Unreal/Plugins/AirSim/Source/WorldSimApi.h | 6 +++++ 6 files changed, 60 insertions(+), 9 deletions(-) diff --git a/AirLib/include/api/WorldSimApiBase.hpp b/AirLib/include/api/WorldSimApiBase.hpp index 8d5189668b..97649908b8 100644 --- a/AirLib/include/api/WorldSimApiBase.hpp +++ b/AirLib/include/api/WorldSimApiBase.hpp @@ -99,6 +99,11 @@ namespace airlib const std::string& vehicle_name = "", bool external = false) = 0; virtual std::vector getDistortionParams(const std::string& camera_name, const std::string& vehicle_name = "", bool external = false) const = 0; + + virtual std::vector getImages(const std::vector& requests, + const std::string& vehicle_name = "", bool external = false) const = 0; + virtual std::vector getImage(const std::string& camera_name, ImageCaptureBase::ImageType image_type, + const std::string& vehicle_name = "", bool external = false) const = 0; }; } } //namespace diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index 7595d5d1cf..023b8576ac 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -144,14 +144,14 @@ namespace airlib return getWorldSimApi()->runConsoleCommand(command); }); - pimpl_->server.bind("simGetImages", [&](const std::vector& request_adapter, const std::string& vehicle_name) -> vector { - const auto& response = getVehicleSimApi(vehicle_name)->getImages(RpcLibAdaptorsBase::ImageRequest::to(request_adapter)); - return RpcLibAdaptorsBase::ImageResponse::from(response); -<<<<<<< HEAD + pimpl_->server.bind("simGetImages", [&](const std::vector& request_adapter, const std::string& vehicle_name, bool external) -> + vector { + const auto& response = getWorldSimApi()->getImages(RpcLibAdaptorsBase::ImageRequest::to(request_adapter), vehicle_name, external); + return RpcLibAdaptorsBase::ImageResponse::from(response); }); - pimpl_->server.bind("simGetImage", [&](const std::string& camera_name, ImageCaptureBase::ImageType type, const std::string& vehicle_name) -> vector { - return getVehicleSimApi(vehicle_name)->getImage(camera_name, type); + pimpl_->server.bind("simGetImage", [&](const std::string& camera_name, ImageCaptureBase::ImageType type, const std::string& vehicle_name, bool external) -> vector { + return getWorldSimApi()->getImage(camera_name, type, vehicle_name, external); }); pimpl_->server.bind("simTestLineOfSightToPoint", [&](const RpcLibAdaptorsBase::GeoPoint& point, const std::string& vehicle_name) -> bool { diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index afedd68d90..ef46ad3494 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -523,6 +523,14 @@ const APIPCamera* ASimModeBase::getCamera(const std::string& camera_name, const return getVehicleSimApi(vehicle_name)->getCamera(camera_name); } +const UnrealImageCapture* ASimModeBase::getImageCapture(const std::string& vehicle_name, bool external) const +{ + if (external) + return external_image_capture_.get(); + else + getVehicleSimApi(vehicle_name)->getImageCapture(); +} + //API server start/stop void ASimModeBase::startApiServer() { @@ -734,6 +742,8 @@ void ASimModeBase::setupVehiclesAndCamera() // Create External Cameras initializeExternalCameras(); + // external_image_capture_.reset(new UnrealImageCapture(&external_cameras_)); + external_image_capture_ = std::make_unique(&external_cameras_); if (getApiProvider()->hasDefaultVehicle()) { //TODO: better handle no FPV vehicles scenario diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h index a91846eba4..089ccb2aba 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h @@ -15,6 +15,7 @@ #include "PawnSimApi.h" #include "common/StateReporterWrapper.hpp" #include "LoadingScreenWidget.h" +#include "UnrealImageCapture.h" #include "SimModeBase.generated.h" DECLARE_DYNAMIC_MULTICAST_DELEGATE(FLevelLoaded); @@ -102,14 +103,20 @@ class AIRSIM_API ASimModeBase : public AActor return const_cast( static_cast(this)->getExternalCamera(camera_name)); } - - const APIPCamera* getCamera(const std::string& camera_name, const std::string& vehicle_name = "", bool external = false) const; APIPCamera* getCamera(const std::string& camera_name, const std::string& vehicle_name = "", bool external = false) { return const_cast( static_cast(this)->getCamera(camera_name, vehicle_name, external)); - } + } + + const UnrealImageCapture* getExternalImageCapture() const + { + return external_image_capture_.get(); + } + + const APIPCamera* getCamera(const std::string& camera_name, const std::string& vehicle_name = "", bool external = false) const; + const UnrealImageCapture* getImageCapture(const std::string& vehicle_name = "", bool external = false) const; TMap asset_map; TMap scene_object_map; @@ -195,6 +202,7 @@ class AIRSIM_API ASimModeBase : public AActor std::vector> vehicle_sim_apis_; common_utils::UniqueValueMap external_cameras_; + std::unique_ptr external_image_capture_; UPROPERTY() TArray spawned_actors_; //keep refs alive from Unreal GC diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index f27310b119..e5114a670e 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -782,3 +782,25 @@ std::vector WorldSimApi::getDistortionParams(const std::string& camera_na return param_values; } + +std::vector WorldSimApi::getImages( + const std::vector& requests, const std::string& vehicle_name, bool external) const +{ + std::vector responses; + + const auto* camera = simmode_->getImageCapture(vehicle_name, external); + camera->getImages(requests, responses); + + return responses; +} + +std::vector WorldSimApi::getImage(const std::string& camera_name, ImageCaptureBase::ImageType image_type, + const std::string& vehicle_name, bool external) const +{ + std::vector request = { ImageCaptureBase::ImageRequest(camera_name, image_type) }; + const auto& response = getImages(request); + if (response.size() > 0) + return response.at(0).image_data_uint8; + else + return std::vector(); +} diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.h b/Unreal/Plugins/AirSim/Source/WorldSimApi.h index 5221e1fa3f..d1457993b5 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.h +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.h @@ -16,6 +16,7 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase typedef msr::airlib::Pose Pose; typedef msr::airlib::Vector3r Vector3r; typedef msr::airlib::MeshPositionVertexBuffersResponse MeshPositionVertexBuffersResponse; + typedef msr::airlib::ImageCaptureBase ImageCaptureBase; WorldSimApi(ASimModeBase* simmode); virtual ~WorldSimApi() = default; @@ -89,6 +90,11 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase virtual std::vector getDistortionParams(const std::string& camera_name, const std::string& vehicle_name = "", bool external = false) const override; + std::vector getImages(const std::vector& requests, + const std::string& vehicle_name = "", bool external = false) const; + std::vector getImage(const std::string& camera_name, ImageCaptureBase::ImageType image_type, + const std::string& vehicle_name = "", bool external = false) const; + private: AActor* createNewActor(const FActorSpawnParameters& spawn_params, const FTransform& actor_transform, const Vector3r& scale, UStaticMesh* static_mesh); void spawnPlayer(); From 77421a5ec85306f52d95d7f15a5d0406f00e3223 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Mon, 1 Feb 2021 01:08:05 +0530 Subject: [PATCH 08/29] Windows fixes and small cleanups --- Unreal/Plugins/AirSim/Source/PawnSimApi.cpp | 4 +--- Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp | 10 ++-------- Unreal/Plugins/AirSim/Source/WorldSimApi.cpp | 2 +- 3 files changed, 4 insertions(+), 12 deletions(-) diff --git a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp b/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp index 784cb29c57..69698a223a 100644 --- a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp @@ -130,9 +130,7 @@ void PawnSimApi::createCamerasFromSettings() const auto& setting = camera_setting_pair.second; //get pose - FVector position = transform.fromLocalNed( - NedTransform::Vector3r(setting.position.x(), setting.position.y(), setting.position.z())) - - transform.fromLocalNed(NedTransform::Vector3r(0.0, 0.0, 0.0)); + FVector position = transform.fromLocalNed(setting.position) - transform.fromLocalNed(Vector3r::Zero()); FTransform camera_transform(FRotator(setting.rotation.pitch, setting.rotation.yaw, setting.rotation.roll), position, FVector(1., 1., 1.)); diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index ef46ad3494..32e4cbb8b8 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -461,8 +461,6 @@ void ASimModeBase::initializeCameraDirector(const FTransform& camera_transform, void ASimModeBase::initializeExternalCameras() { - //UStaticMeshComponent* bodyMesh = UAirBlueprintLib::GetActorComponent(this, TEXT("BodyMesh")); - // USceneComponent* bodyMesh = params_.pawn->GetRootComponent(); FActorSpawnParameters camera_spawn_params; camera_spawn_params.SpawnCollisionHandlingOverride = ESpawnActorCollisionHandlingMethod::AdjustIfPossibleButAlwaysSpawn; const auto& transform = getGlobalNedTransform(); @@ -472,16 +470,13 @@ void ASimModeBase::initializeExternalCameras() const auto& setting = camera_setting_pair.second; //get pose - FVector position = transform.fromLocalNed( - NedTransform::Vector3r(setting.position.x(), setting.position.y(), setting.position.z())) - - transform.fromLocalNed(NedTransform::Vector3r(0.0, 0.0, 0.0)); + FVector position = transform.fromLocalNed(setting.position) - transform.fromLocalNed(Vector3r::Zero()); FTransform camera_transform(FRotator(setting.rotation.pitch, setting.rotation.yaw, setting.rotation.roll), position, FVector(1., 1., 1.)); //spawn and attach camera to pawn camera_spawn_params.Name = FName(("external_" + camera_setting_pair.first).c_str()); APIPCamera* camera = this->GetWorld()->SpawnActor(pip_camera_class, camera_transform, camera_spawn_params); - // camera->AttachToComponent(bodyMesh, FAttachmentTransformRules::KeepRelativeTransform); camera->setupCameraFromSettings(setting, transform); @@ -528,7 +523,7 @@ const UnrealImageCapture* ASimModeBase::getImageCapture(const std::string& vehic if (external) return external_image_capture_.get(); else - getVehicleSimApi(vehicle_name)->getImageCapture(); + return getVehicleSimApi(vehicle_name)->getImageCapture(); } //API server start/stop @@ -742,7 +737,6 @@ void ASimModeBase::setupVehiclesAndCamera() // Create External Cameras initializeExternalCameras(); - // external_image_capture_.reset(new UnrealImageCapture(&external_cameras_)); external_image_capture_ = std::make_unique(&external_cameras_); if (getApiProvider()->hasDefaultVehicle()) { diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index e5114a670e..be0f58bf05 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -797,7 +797,7 @@ std::vector WorldSimApi::getImages std::vector WorldSimApi::getImage(const std::string& camera_name, ImageCaptureBase::ImageType image_type, const std::string& vehicle_name, bool external) const { - std::vector request = { ImageCaptureBase::ImageRequest(camera_name, image_type) }; + std::vector request{ ImageCaptureBase::ImageRequest(camera_name, image_type) }; const auto& response = getImages(request); if (response.size() > 0) return response.at(0).image_data_uint8; From fbf84bff7488a81d9f3fb7f2d27eadf8d1f91d71 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Wed, 10 Feb 2021 23:34:53 +0530 Subject: [PATCH 09/29] Add external field to simGetImage/s APIs --- PythonClient/airsim/client.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index 56945b201c..d8aa35ae5e 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -220,7 +220,7 @@ def simSetWeatherParameter(self, param, val): # camera control # simGetImage returns compressed png in array of bytes # image_type uses one of the ImageType members - def simGetImage(self, camera_name, image_type, vehicle_name = ''): + def simGetImage(self, camera_name, image_type, vehicle_name = '', external = False): """ Get a single image @@ -240,7 +240,7 @@ def simGetImage(self, camera_name, image_type, vehicle_name = ''): camera_name = str(camera_name) # because this method returns std::vector, msgpack decides to encode it as a string unfortunately. - result = self.client.call('simGetImage', camera_name, image_type, vehicle_name) + result = self.client.call('simGetImage', camera_name, image_type, vehicle_name, external) if (result == "" or result == "\0"): return None return result @@ -248,7 +248,7 @@ def simGetImage(self, camera_name, image_type, vehicle_name = ''): # camera control # simGetImage returns compressed png in array of bytes # image_type uses one of the ImageType members - def simGetImages(self, requests, vehicle_name = ''): + def simGetImages(self, requests, vehicle_name = '', external = False): """ Get multiple images @@ -261,7 +261,7 @@ def simGetImages(self, requests, vehicle_name = ''): Returns: list[ImageResponse]: """ - responses_raw = self.client.call('simGetImages', requests, vehicle_name) + responses_raw = self.client.call('simGetImages', requests, vehicle_name, external) return [ImageResponse.from_msgpack(response_raw) for response_raw in responses_raw] def simTestLineOfSightToPoint(self, point, vehicle_name = ''): From 1bc9ef88707897389f149537cdec48970abe90f3 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Thu, 11 Feb 2021 00:10:05 +0530 Subject: [PATCH 10/29] Remove unused PawnSimApi getImages APIs --- AirLib/include/api/VehicleSimApiBase.hpp | 3 --- Unreal/Plugins/AirSim/Source/PawnSimApi.cpp | 21 --------------------- Unreal/Plugins/AirSim/Source/PawnSimApi.h | 2 -- 3 files changed, 26 deletions(-) diff --git a/AirLib/include/api/VehicleSimApiBase.hpp b/AirLib/include/api/VehicleSimApiBase.hpp index a5dc1bf414..7f9e549558 100644 --- a/AirLib/include/api/VehicleSimApiBase.hpp +++ b/AirLib/include/api/VehicleSimApiBase.hpp @@ -49,9 +49,6 @@ namespace airlib virtual void initialize() = 0; - virtual std::vector getImages(const std::vector& request) const = 0; - virtual std::vector getImage(const std::string& camera_name, ImageCaptureBase::ImageType image_type) const = 0; - virtual bool testLineOfSightToPoint(const GeoPoint& point) const = 0; virtual Pose getPose() const = 0; diff --git a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp b/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp index 69698a223a..ff673ce469 100644 --- a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp @@ -184,27 +184,6 @@ APawn* PawnSimApi::getPawn() return params_.pawn; } -std::vector PawnSimApi::getImages( - const std::vector& requests) const -{ - std::vector responses; - - const ImageCaptureBase* camera = getImageCapture(); - camera->getImages(requests, responses); - - return responses; -} - -std::vector PawnSimApi::getImage(const std::string& camera_name, ImageCaptureBase::ImageType image_type) const -{ - std::vector request = { ImageCaptureBase::ImageRequest(camera_name, image_type) }; - const std::vector& response = getImages(request); - if (response.size() > 0) - return response.at(0).image_data_uint8; - else - return std::vector(); -} - void PawnSimApi::setRCForceFeedback(float rumble_strength, float auto_center) { if (joystick_state_.is_initialized) { diff --git a/Unreal/Plugins/AirSim/Source/PawnSimApi.h b/Unreal/Plugins/AirSim/Source/PawnSimApi.h index 60d8204b78..aced239297 100644 --- a/Unreal/Plugins/AirSim/Source/PawnSimApi.h +++ b/Unreal/Plugins/AirSim/Source/PawnSimApi.h @@ -71,8 +71,6 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase virtual void update() override; virtual const UnrealImageCapture* getImageCapture() const override; - virtual std::vector getImages(const std::vector& request) const override; - virtual std::vector getImage(const std::string& camera_name, ImageCaptureBase::ImageType image_type) const override; virtual Pose getPose() const override; virtual void setPose(const Pose& pose, bool ignore_collision) override; From 32d22e07e17a99e612793b4288b1892b997fff00 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Thu, 11 Feb 2021 18:25:30 +0530 Subject: [PATCH 11/29] Add External field to Subwindow settings --- AirLib/include/common/AirSimSettings.hpp | 15 +++++++----- .../Plugins/AirSim/Source/SimHUD/SimHUD.cpp | 24 ++++++------------- 2 files changed, 16 insertions(+), 23 deletions(-) diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index ef30c7fd35..1e59a859e9 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/AirLib/include/common/AirSimSettings.hpp @@ -50,10 +50,12 @@ namespace airlib bool visible; std::string camera_name; std::string vehicle_name; + bool external; - SubwindowSetting(int window_index_val = 0, ImageType image_type_val = ImageType::Scene, - bool visible_val = false, const std::string& camera_name_val = "", const std::string& vehicle_name_val = "") - : window_index(window_index_val), image_type(image_type_val), visible(visible_val), camera_name(camera_name_val), vehicle_name(vehicle_name_val) + SubwindowSetting(int window_index_val = 0, ImageType image_type_val = ImageType::Scene, bool visible_val = false, + const std::string& camera_name_val = "", const std::string& vehicle_name_val = "", bool external_val = false) + : window_index(window_index_val), image_type(image_type_val), visible(visible_val), + camera_name(camera_name_val), vehicle_name(vehicle_name_val), external(external_val) { } }; @@ -1089,6 +1091,7 @@ namespace airlib subwindow_setting.visible = json_settings_child.getBool("Visible", false); subwindow_setting.camera_name = getCameraName(json_settings_child); subwindow_setting.vehicle_name = json_settings_child.getString("VehicleName", ""); + subwindow_setting.external = json_settings_child.getBool("External", false); } } } @@ -1097,9 +1100,9 @@ namespace airlib static void initializeSubwindowSettings(std::vector& subwindow_settings) { subwindow_settings.clear(); - subwindow_settings.push_back(SubwindowSetting(0, ImageType::DepthVis, false, "", "")); //depth - subwindow_settings.push_back(SubwindowSetting(1, ImageType::Segmentation, false, "", "")); //seg - subwindow_settings.push_back(SubwindowSetting(2, ImageType::Scene, false, "", "")); //vis + subwindow_settings.push_back(SubwindowSetting(0, ImageType::DepthVis, false, "", "", false)); //depth + subwindow_settings.push_back(SubwindowSetting(1, ImageType::Segmentation, false, "", "", false)); //seg + subwindow_settings.push_back(SubwindowSetting(2, ImageType::Scene, false, "", "", false)); //vis } void loadOtherSettings(const Settings& settings_json) diff --git a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp index 94f3500d07..5589b3abc9 100644 --- a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp +++ b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp @@ -332,23 +332,13 @@ void ASimHUD::initializeSubWindows() subwindow_cameras_[0] = subwindow_cameras_[1] = subwindow_cameras_[2] = nullptr; } - for (size_t window_index = 0; window_index < AirSimSettings::kSubwindowCount; ++window_index) { - - const auto& subwindow_setting = AirSimSettings::singleton().subwindow_settings.at(window_index); - auto vehicle_sim_api = simmode_->getVehicleSimApi(subwindow_setting.vehicle_name); - - if (vehicle_sim_api) { - if (vehicle_sim_api->getCamera(subwindow_setting.camera_name) != nullptr) - subwindow_cameras_[subwindow_setting.window_index] = vehicle_sim_api->getCamera(subwindow_setting.camera_name); - else - UAirBlueprintLib::LogMessageString("CameraID in element in settings.json is invalid", - std::to_string(window_index), - LogDebugLevel::Failure); - } + for (const auto& setting : getSubWindowSettings()) { + auto camera = simmode_->getCamera(setting.camera_name, setting.vehicle_name, setting.external); + if (camera) + subwindow_cameras_[setting.window_index] = camera; else - UAirBlueprintLib::LogMessageString("Vehicle in element in settings.json is invalid", - std::to_string(window_index), - LogDebugLevel::Failure); + UAirBlueprintLib::LogMessageString("Invalid Camera settings in element", + std::to_string(setting.window_index), LogDebugLevel::Failure); } } @@ -420,4 +410,4 @@ bool ASimHUD::readSettingsTextFromFile(const FString& settingsFilepath, std::str } return found; -} \ No newline at end of file +} From dd24b1e51b95eb50214f17bd8497d637366b2ae2 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Thu, 11 Feb 2021 22:02:50 +0530 Subject: [PATCH 12/29] Update Python API docstrings --- PythonClient/airsim/client.py | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index d8aa35ae5e..5a65dc964a 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -232,6 +232,7 @@ def simGetImage(self, camera_name, image_type, vehicle_name = '', external = Fal camera_name (str): Name of the camera, for backwards compatibility, ID numbers such as 0,1,etc. can also be used image_type (ImageType): Type of image required vehicle_name (str, optional): Name of the vehicle with the camera + external (bool, optional): Whether the camera is an External Camera Returns: Binary string literal of compressed png image @@ -257,6 +258,7 @@ def simGetImages(self, requests, vehicle_name = '', external = False): Args: requests (list[ImageRequest]): Images required vehicle_name (str, optional): Name of vehicle associated with the camera + external (bool, optional): Whether the camera is an External Camera Returns: list[ImageResponse]: @@ -586,6 +588,7 @@ def simGetCameraInfo(self, camera_name, vehicle_name = '', external=False): Args: camera_name (str): Name of the camera, for backwards compatibility, ID numbers such as 0,1,etc. can also be used vehicle_name (str, optional): Vehicle which the camera is associated with + external (bool, optional): Whether the camera is an External Camera Returns: CameraInfo: @@ -600,6 +603,7 @@ def simGetDistortionParams(self, camera_name, vehicle_name = '', external = Fals Args: camera_name (str): Name of the camera, for backwards compatibility, ID numbers such as 0,1,etc. can also be used vehicle_name (str, optional): Vehicle which the camera is associated with + external (bool, optional): Whether the camera is an External Camera Returns: List (float): List of distortion parameter values corresponding to K1, K2, K3, P1, P2 respectively. @@ -616,6 +620,7 @@ def simSetDistortionParams(self, camera_name, distortion_params, vehicle_name = distortion_params (dict): Dictionary of distortion param names and corresponding values {"K1": 0.0, "K2": 0.0, "K3": 0.0, "P1": 0.0, "P2": 0.0} vehicle_name (str, optional): Vehicle which the camera is associated with + external (bool, optional): Whether the camera is an External Camera """ for param_name, value in distortion_params.items(): @@ -631,6 +636,7 @@ def simSetDistortionParam(self, camera_name, param_name, value, vehicle_name = ' param_name (str): Name of distortion parameter value (float): Value of distortion parameter vehicle_name (str, optional): Vehicle which the camera is associated with + external (bool, optional): Whether the camera is an External Camera """ self.client.call('simSetDistortionParam', str(camera_name), param_name, value, vehicle_name, external) @@ -642,6 +648,7 @@ def simSetCameraPose(self, camera_name, pose, vehicle_name = '', external = Fals camera_name (str): Name of the camera to be controlled pose (Pose): Pose representing the desired position and orientation of the camera vehicle_name (str, optional): Name of vehicle which the camera corresponds to + external (bool, optional): Whether the camera is an External Camera """ # TODO: below str() conversion is only needed for legacy reason and should be removed in future self.client.call('simSetCameraPose', str(camera_name), pose, vehicle_name, external) @@ -658,6 +665,7 @@ def simSetCameraOrientation(self, camera_name, orientation, vehicle_name = '', e camera_name (str): Name of the camera to be controlled orientation (Quaternionr): Quaternion representing the desired orientation of the camera vehicle_name (str, optional): Name of vehicle which the camera corresponds to + external (bool, optional): Whether the camera is an External Camera """ logging.warning("`simSetCameraOrientation` API has been upgraded to `simSetCameraPose`. Please update your code.") pose = Pose(orientation_val=orientation) @@ -671,6 +679,7 @@ def simSetCameraFov(self, camera_name, fov_degrees, vehicle_name = '', external camera_name (str): Name of the camera to be controlled fov_degrees (float): Value of field of view in degrees vehicle_name (str, optional): Name of vehicle which the camera corresponds to + external (bool, optional): Whether the camera is an External Camera """ # TODO: below str() conversion is only needed for legacy reason and should be removed in future self.client.call('simSetCameraFov', str(camera_name), fov_degrees, vehicle_name, external) From 21a7a392f282d28a8e2f3f41fcd367940f1dbf59 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Thu, 11 Feb 2021 22:34:28 +0530 Subject: [PATCH 13/29] Docs: Add info about external cameras --- docs/image_apis.md | 5 +++++ docs/settings.md | 25 +++++++++++++++++++++---- 2 files changed, 26 insertions(+), 4 deletions(-) diff --git a/docs/image_apis.md b/docs/image_apis.md index d48442db49..69126c4495 100644 --- a/docs/image_apis.md +++ b/docs/image_apis.md @@ -172,6 +172,11 @@ camera_pose = airsim.Pose(airsim.Vector3r(0, 0, 0), airsim.to_quaternion(0.26179 client.simSetCameraPose(0, camera_pose); ``` +- `simSetCameraFov` allows changing the Field-of-View of the camera at runtime. +- `simSetDistortionParams`, `simGetDistortionParams` allow setting and fetching the distortion parameters K1, K2, K3, P1, P2 + +All Camera APIs take in 3 common parameters apart from the API-specific ones, `camera_name`(str), `vehicle_name`(str) and `external`(bool, to indicate [External Camera](settings.md#external-cameras)). Camera and vehicle name is used to get the specific camera, if `external` is set to `true`, then the vehicle name is ignored. Also see [external_camera.py](https://github.com/microsoft/AirSim/blob/master/PythonClient/computer_vision/external_camera.py) for example usage of these APIs. + ### Gimbal You can set stabilization for pitch, roll or yaw for any camera [using settings](settings.md#gimbal). diff --git a/docs/settings.md b/docs/settings.md index 63fa1533e8..b819de129a 100644 --- a/docs/settings.md +++ b/docs/settings.md @@ -132,9 +132,9 @@ Below are complete list of settings available along with their default values. I "UpdateIntervalSecs": 60 }, "SubWindows": [ - {"WindowID": 0, "CameraName": "0", "ImageType": 3, "VehicleName": "", "Visible": false}, - {"WindowID": 1, "CameraName": "0", "ImageType": 5, "VehicleName": "", "Visible": false}, - {"WindowID": 2, "CameraName": "0", "ImageType": 0, "VehicleName": "", "Visible": false} + {"WindowID": 0, "CameraName": "0", "ImageType": 3, "VehicleName": "", "Visible": false, "External": false}, + {"WindowID": 1, "CameraName": "0", "ImageType": 5, "VehicleName": "", "Visible": false, "External": false}, + {"WindowID": 2, "CameraName": "0", "ImageType": 0, "VehicleName": "", "Visible": false, "External": false} ], "SegmentationSettings": { "InitMethod": "", @@ -188,6 +188,14 @@ Below are complete list of settings available along with their default values. I "X": NaN, "Y": NaN, "Z": NaN, "Pitch": NaN, "Roll": NaN, "Yaw": NaN } + }, + "ExternalCameras": { + "FixedCamera1": { + // same elements as in CameraDefaults above + }, + "FixedCamera2": { + // same elements as in CameraDefaults above + } } } ``` @@ -218,7 +226,13 @@ Also see [Time of Day API](apis.md#time-of-day-api). This setting specifies the latitude, longitude and altitude of the Player Start component placed in the Unreal environment. The vehicle's home point is computed using this transformation. Note that all coordinates exposed via APIs are using NED system in SI units which means each vehicle starts at (0, 0, 0) in NED system. Time of Day settings are computed for geographical coordinates specified in `OriginGeopoint`. ## SubWindows -This setting determines what is shown in each of 3 subwindows which are visible when you press 0,1,2 keys. The `WindowID` can be 0 to 2, `CameraName` is any [available camera](image_apis.md#available_cameras) on the vehicle. `ImageType` integer value determines what kind of image gets shown according to [ImageType enum](image_apis.md#available-imagetype). `VehicleName` string allows you to specify the vehicle to use the camera from, used when multiple vehicles are specified in the settings. First vehicle's camera will be used if there are any mistakes such as incorrect vehicle name, or only a single vehicle. +This setting determines what is shown in each of 3 subwindows which are visible when you press 1,2,3 keys. + +* `WindowID`: Can be 0 to 2 +* `CameraName` is any [available camera](image_apis.md#available_cameras) on the vehicle. +* `ImageType` integer value determines what kind of image gets shown according to [ImageType enum](image_apis.md#available-imagetype). +* `VehicleName` string allows you to specify the vehicle to use the camera from, used when multiple vehicles are specified in the settings. First vehicle's camera will be used if there are any mistakes such as incorrect vehicle name, or only a single vehicle. +* `External`: Set it to `true` if the camera is an external camera. If true, then the `VehicleName` parameter is ignored For example, for a single car vehicle, below shows driver view, front bumper view and rear view as scene, depth and surface normals respectively. ```json @@ -330,6 +344,9 @@ This adds fluctuations on horizontal line. ### Gimbal The `Gimbal` element allows to freeze camera orientation for pitch, roll and/or yaw. This setting is ignored unless `ImageType` is -1. The `Stabilization` is defaulted to 0 meaning no gimbal i.e. camera orientation changes with body orientation on all axis. The value of 1 means full stabilization. The value between 0 to 1 acts as a weight for fixed angles specified (in degrees, in world-frame) in `Pitch`, `Roll` and `Yaw` elements and orientation of the vehicle body. When any of the angles is omitted from json or set to NaN, that angle is not stabilized (i.e. it moves along with vehicle body). +## External Cameras +This element allows specifying cameras which are separate from the cameras attached to the vehicle, such as a CCTV camera. These are fixed cameras, and don't move along with the vehicles. The key in the element is the name of the camera, and the value i.e. settings are the same as `CameraDefaults` described above. All the camera APIs work with external cameras, including capturing images, changing the pose, etc by passing the parameter `external=True` in the API call. + ## Vehicles Settings Each simulation mode will go through the list of vehicles specified in this setting and create the ones that has `"AutoCreate": true`. Each vehicle specified in this setting has key which becomes the name of the vehicle. If `"Vehicles"` element is missing then this list is populated with default car named "PhysXCar" and default multirotor named "SimpleFlight". From 4276935c2f8f21dd8c17cba4f23b411a4673e66f Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Sat, 13 Feb 2021 00:10:05 +0530 Subject: [PATCH 14/29] Unity: Update to new Image APIs --- .../AirsimWrapper/Source/PawnSimApi.cpp | 56 +--------------- .../AirsimWrapper/Source/PawnSimApi.h | 9 +-- .../AirsimWrapper/Source/WorldSimApi.cpp | 64 +++++++++++++++++++ .../AirsimWrapper/Source/WorldSimApi.h | 19 +++++- 4 files changed, 85 insertions(+), 63 deletions(-) diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp b/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp index 3a63f8c287..6f36a665fd 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp @@ -135,24 +135,6 @@ const NedTransform& PawnSimApi::getNedTransform() const return ned_transform_; } -std::vector PawnSimApi::getImages( - const std::vector& requests) const -{ - std::vector responses; - const ImageCaptureBase* camera = getImageCapture(); - camera->getImages(requests, responses); - return responses; -} - -std::vector PawnSimApi::getImage(const std::string& camera_name, ImageCaptureBase::ImageType image_type) const -{ - std::vector request = { ImageCaptureBase::ImageRequest(camera_name, image_type) }; - const std::vector& response = getImages(request); - if (response.size() > 0) - return response.at(0).image_data_uint8; - else - return std::vector(); -} msr::airlib::RCData PawnSimApi::getRCData() const { @@ -249,42 +231,6 @@ void PawnSimApi::allowPassthroughToggleInput() PrintLogMessage("enable_passthrough_on_collisions: ", state_.passthrough_enabled ? "true" : "false", params_.vehicle_name.c_str(), ErrorLogSeverity::Information); } -msr::airlib::CameraInfo PawnSimApi::getCameraInfo(const std::string& camera_name) const -{ - AirSimCameraInfo airsim_camera_info = GetCameraInfo(camera_name.c_str(), params_.vehicle_name.c_str()); // Into Unity - msr::airlib::CameraInfo camera_info; - camera_info.pose.position.x() = airsim_camera_info.pose.position.x; - camera_info.pose.position.y() = airsim_camera_info.pose.position.y; - camera_info.pose.position.z() = airsim_camera_info.pose.position.z; - camera_info.pose.orientation.x() = airsim_camera_info.pose.orientation.x; - camera_info.pose.orientation.y() = airsim_camera_info.pose.orientation.y; - camera_info.pose.orientation.z() = airsim_camera_info.pose.orientation.z; - camera_info.pose.orientation.w() = airsim_camera_info.pose.orientation.w; - camera_info.fov = airsim_camera_info.fov; - return camera_info; -} - -void PawnSimApi::setCameraPose(const std::string& camera_name, const msr::airlib::Pose& pose) -{ - 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) -{ - SetCameraFoV(camera_name.c_str(), fov_degrees, params_.vehicle_name.c_str()); -} - -void PawnSimApi::setDistortionParam(const std::string& camera_name, const std::string& param_name, float value) -{ - // not implemented -} - -std::vector PawnSimApi::getDistortionParams(const std::string& camera_name) const -{ - // not implemented - std::vector params(5, 0.0); - return params; -} //parameters in NED frame PawnSimApi::Pose PawnSimApi::getPose() const @@ -371,4 +317,4 @@ void PawnSimApi::updateKinematics(float dt) next_kinematics.accelerations.angular = (next_kinematics.twist.angular - kinematics_->getTwist().angular) / dt; kinematics_->setState(next_kinematics); kinematics_->update(); -} \ No newline at end of file +} diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.h b/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.h index a505fd7432..aecaee70dc 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.h +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.h @@ -70,15 +70,10 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase virtual void resetImplementation() override; virtual void update() override; virtual const UnityImageCapture* getImageCapture() const override; - virtual std::vector getImages(const std::vector& request) const override; - virtual std::vector getImage(const std::string& camera_name, ImageCaptureBase::ImageType image_type) const override; + 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 setCameraPose(const std::string& camera_name, const Pose& pose) override; - virtual void setCameraFoV(const std::string& camera_name, float fov_degrees) override; - virtual void setDistortionParam(const std::string& camera_name, const std::string& param_name, float value) override; - virtual std::vector getDistortionParams(const std::string& camera_name) const override; + virtual CollisionInfo getCollisionInfo() const override; virtual int getRemoteControlID() const override; virtual msr::airlib::RCData getRCData() const override; diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp index 29cf286d6f..8597d60788 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp @@ -266,4 +266,68 @@ std::vector WorldSimApi::getWorldExtents() const return result; } +msr::airlib::CameraInfo WorldSimApi::getCameraInfo(const std::string& camera_name, const std::string& vehicle_name, bool external) const +{ + if (external) + throw std::invalid_argument(common_utils::Utils::stringf("external field is not supported on Unity Image APIs").c_str()); + + AirSimCameraInfo airsim_camera_info = GetCameraInfo(camera_name.c_str(), vehicle_name.c_str()); // Into Unity + msr::airlib::CameraInfo camera_info; + camera_info.pose = UnityUtilities::Convert_to_Pose(airsim_camera_info.pose); + camera_info.fov = airsim_camera_info.fov; + return camera_info; +} + +void WorldSimApi::setCameraPose(const std::string& camera_name, const msr::airlib::Pose& pose, + const std::string& vehicle_name, bool external) +{ + if (external) + throw std::invalid_argument(common_utils::Utils::stringf("external field is not supported on Unity Image APIs").c_str()); + + SetCameraPose(camera_name.c_str(), UnityUtilities::Convert_to_AirSimPose(pose), vehicle_name.c_str()); +} + +void WorldSimApi::setCameraFoV(const std::string& camera_name, float fov_degrees, + const std::string& vehicle_name, bool external) +{ + if (external) + throw std::invalid_argument(common_utils::Utils::stringf("external field is not supported on Unity Image APIs").c_str()); + + SetCameraFoV(camera_name.c_str(), fov_degrees, vehicle_name.c_str()); +} + +void WorldSimApi::setDistortionParam(const std::string& camera_name, const std::string& param_name, float value, + const std::string& vehicle_name, bool external) +{ + throw std::invalid_argument(common_utils::Utils::stringf("setDistortionParam is not supported on unity").c_str()); +} + +std::vector WorldSimApi::getDistortionParams(const std::string& camera_name, const std::string& vehicle_name, bool external) const +{ + throw std::invalid_argument(common_utils::Utils::stringf("getDistortionParams is not supported on unity").c_str()); + + std::vector params(5, 0.0); + return params; +} + +std::vector WorldSimApi::getImages( + const std::vector& requests, const std::string& vehicle_name, bool external) const +{ + std::vector responses; + const ImageCaptureBase* camera = simmode_->getVehicleSimApi(vehicle_name)->getImageCapture(); + camera->getImages(requests, responses); + return responses; +} + +std::vector WorldSimApi::getImage(const std::string& camera_name, ImageCaptureBase::ImageType image_type, + const std::string& vehicle_name, bool external) const +{ + std::vector request{ ImageCaptureBase::ImageRequest(camera_name, image_type) }; + const auto& response = getImages(request); + if (response.size() > 0) + return response.at(0).image_data_uint8; + else + return std::vector(); +} + #pragma endregion diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h index 45d0f678b8..47d7cbb7be 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h @@ -1,7 +1,7 @@ #pragma once #include "api/WorldSimApiBase.hpp" -#include "./SimMode/SimModeBase.h" +#include "SimMode/SimModeBase.h" #include "AirSimStructs.hpp" class WorldSimApi : public msr::airlib::WorldSimApiBase @@ -10,6 +10,7 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase typedef msr::airlib::Pose Pose; typedef msr::airlib::Vector3r Vector3r; typedef msr::airlib::MeshPositionVertexBuffersResponse MeshPositionVertexBuffersResponse; + typedef msr::airlib::ImageCaptureBase ImageCaptureBase; WorldSimApi(SimModeBase* simmode, std::string vehicle_name); virtual ~WorldSimApi(); @@ -72,6 +73,22 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase virtual bool testLineOfSightBetweenPoints(const msr::airlib::GeoPoint& point1, const msr::airlib::GeoPoint& point2) const override; virtual std::vector getWorldExtents() const override; + // Image APIs + virtual msr::airlib::CameraInfo getCameraInfo(const std::string& camera_name, const std::string& vehicle_name = "", bool external = false) const override; + virtual void setCameraPose(const std::string& camera_name, const msr::airlib::Pose& pose, + const std::string& vehicle_name = "", bool external = false) override; + virtual void setCameraFoV(const std::string& camera_name, float fov_degrees, + const std::string& vehicle_name = "", bool external = false) override; + virtual void setDistortionParam(const std::string& camera_name, const std::string& param_name, float value, + const std::string& vehicle_name = "", bool external = false) override; + virtual std::vector getDistortionParams(const std::string& camera_name, const std::string& vehicle_name = "", + bool external = false) const override; + + virtual std::vector getImages(const std::vector& requests, + const std::string& vehicle_name = "", bool external = false) const override; + virtual std::vector getImage(const std::string& camera_name, msr::airlib::ImageCaptureBase::ImageType image_type, + const std::string& vehicle_name = "", bool external = false) const override; + private: SimModeBase* simmode_; std::string vehicle_name_; From 2ae8ffd22b4e322cf57742c6b2e92cf32aa77fd0 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Sat, 13 Feb 2021 00:11:33 +0530 Subject: [PATCH 15/29] Fix some warnings, cleanup --- AirLib/include/api/WorldSimApiBase.hpp | 2 +- PythonClient/airsim/client.py | 1 - Unreal/Plugins/AirSim/Source/PIPCamera.h | 4 +- .../AirSim/Source/SimMode/SimModeBase.cpp | 12 ++--- .../AirSim/Source/SimMode/SimModeBase.h | 2 +- Unreal/Plugins/AirSim/Source/WorldSimApi.cpp | 44 ++++++++++--------- Unreal/Plugins/AirSim/Source/WorldSimApi.h | 8 ++-- 7 files changed, 35 insertions(+), 38 deletions(-) diff --git a/AirLib/include/api/WorldSimApiBase.hpp b/AirLib/include/api/WorldSimApiBase.hpp index 97649908b8..59355a6471 100644 --- a/AirLib/include/api/WorldSimApiBase.hpp +++ b/AirLib/include/api/WorldSimApiBase.hpp @@ -5,7 +5,7 @@ #define air_WorldSimApiBase_hpp #include "common/CommonStructs.hpp" -#include "common/AirSimSettings.hpp" +#include "common/ImageCaptureBase.hpp" namespace msr { diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index 5a65dc964a..b431e244dc 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -624,7 +624,6 @@ def simSetDistortionParams(self, camera_name, distortion_params, vehicle_name = """ for param_name, value in distortion_params.items(): - # self.client.call('simSetDistortionParam', str(camera_name), param_name, value, vehicle_name, external) self.simSetDistortionParam(camera_name, param_name, value, vehicle_name, external) def simSetDistortionParam(self, camera_name, param_name, value, vehicle_name = '', external = False): diff --git a/Unreal/Plugins/AirSim/Source/PIPCamera.h b/Unreal/Plugins/AirSim/Source/PIPCamera.h index b9e2e8643f..0d85951b74 100644 --- a/Unreal/Plugins/AirSim/Source/PIPCamera.h +++ b/Unreal/Plugins/AirSim/Source/PIPCamera.h @@ -41,14 +41,14 @@ 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 setupCameraFromSettings(const CameraSetting& camera_setting, const NedTransform& ned_transform); void setCameraPose(const msr::airlib::Pose& relative_pose); void setCameraFoV(float fov_degrees); msr::airlib::CameraInfo getCameraInfo() const; std::vector getDistortionParams() const; void setDistortionParam(const std::string& param_name, float value); - msr::airlib::ProjectionMatrix getProjectionMatrix(const APIPCamera::ImageType image_type) const; + msr::airlib::ProjectionMatrix getProjectionMatrix(const ImageType image_type) const; USceneCaptureComponent2D* getCaptureComponent(const ImageType type, bool if_active); UTextureRenderTarget2D* getRenderTarget(const ImageType type, bool if_active); diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index 32e4cbb8b8..e915ca1382 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -512,18 +512,14 @@ bool ASimModeBase::isRecording() const const APIPCamera* ASimModeBase::getCamera(const std::string& camera_name, const std::string& vehicle_name, bool external) const { - if (external) - return getExternalCamera(camera_name); - else - return getVehicleSimApi(vehicle_name)->getCamera(camera_name); + return external ? getExternalCamera(camera_name) : + getVehicleSimApi(vehicle_name)->getCamera(camera_name); } const UnrealImageCapture* ASimModeBase::getImageCapture(const std::string& vehicle_name, bool external) const { - if (external) - return external_image_capture_.get(); - else - return getVehicleSimApi(vehicle_name)->getImageCapture(); + return external ? external_image_capture_.get() : + getVehicleSimApi(vehicle_name)->getImageCapture(); } //API server start/stop diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h index 089ccb2aba..3965fb1c76 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h @@ -150,7 +150,7 @@ class AIRSIM_API ASimModeBase : public AActor virtual void initializeExternalCameras(); protected: //Utility methods for derived classes - virtual const msr::airlib::AirSimSettings& getSettings() const; + virtual const AirSimSettings& getSettings() const; FRotator toFRotator(const AirSimSettings::Rotation& rotation, const FRotator& default_val); protected: diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index be0f58bf05..e0429803e5 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -731,12 +731,11 @@ std::vector WorldSimApi::getWorldExtents() const return result; } -CameraInfo WorldSimApi::getCameraInfo(const std::string& camera_name, const std::string& vehicle_name, bool external) const +msr::airlib::CameraInfo WorldSimApi::getCameraInfo(const std::string& camera_name, const std::string& vehicle_name, bool external) const { - CameraInfo info; - - UAirBlueprintLib::RunCommandOnGameThread([this, &camera_name, &vehicle_name, &external, &info]() { - auto camera = simmode_->getCamera(camera_name, vehicle_name, external); + msr::airlib::CameraInfo info; + const auto* camera = simmode_->getCamera(camera_name, vehicle_name, external); + UAirBlueprintLib::RunCommandOnGameThread([camera, &info]() { info = camera->getCameraInfo(); }, true); @@ -745,40 +744,43 @@ CameraInfo WorldSimApi::getCameraInfo(const std::string& camera_name, const std: } void WorldSimApi::setCameraPose(const std::string& camera_name, const msr::airlib::Pose& pose, - const std::string& vehicle_name, bool external) + const std::string& vehicle_name, bool external) { - UAirBlueprintLib::RunCommandOnGameThread([this, &camera_name, &vehicle_name, &external, &pose]() { - auto camera = simmode_->getCamera(camera_name, vehicle_name, external); + auto* camera = simmode_->getCamera(camera_name, vehicle_name, external); + UAirBlueprintLib::RunCommandOnGameThread([camera, &pose]() { camera->setCameraPose(pose); - }, true); + }, + true); } void WorldSimApi::setCameraFoV(const std::string& camera_name, float fov_degrees, const std::string& vehicle_name, bool external) { - UAirBlueprintLib::RunCommandOnGameThread([this, &camera_name, &vehicle_name, &external, &fov_degrees]() { - auto camera = simmode_->getCamera(camera_name, vehicle_name, external); + auto* camera = simmode_->getCamera(camera_name, vehicle_name, external); + UAirBlueprintLib::RunCommandOnGameThread([camera, &fov_degrees]() { camera->setCameraFoV(fov_degrees); - }, true); + }, + true); } void WorldSimApi::setDistortionParam(const std::string& camera_name, const std::string& param_name, float value, - const std::string& vehicle_name, bool external) + const std::string& vehicle_name, bool external) { - UAirBlueprintLib::RunCommandOnGameThread([this, &camera_name, &vehicle_name, &external, - ¶m_name, &value]() { - auto camera = simmode_->getCamera(camera_name, vehicle_name, external); + auto* camera = simmode_->getCamera(camera_name, vehicle_name, external); + UAirBlueprintLib::RunCommandOnGameThread([camera, ¶m_name, &value]() { camera->setDistortionParam(param_name, value); - }, true); + }, + true); } std::vector WorldSimApi::getDistortionParams(const std::string& camera_name, const std::string& vehicle_name, bool external) const { std::vector param_values; - UAirBlueprintLib::RunCommandOnGameThread([this, &camera_name, &vehicle_name, &external, ¶m_values]() { - auto camera = simmode_->getCamera(camera_name, vehicle_name, external); + const auto* camera = simmode_->getCamera(camera_name, vehicle_name, external); + UAirBlueprintLib::RunCommandOnGameThread([camera, ¶m_values]() { param_values = camera->getDistortionParams(); - }, true); + }, + true); return param_values; } @@ -795,7 +797,7 @@ std::vector WorldSimApi::getImages } std::vector WorldSimApi::getImage(const std::string& camera_name, ImageCaptureBase::ImageType image_type, - const std::string& vehicle_name, bool external) const + const std::string& vehicle_name, bool external) const { std::vector request{ ImageCaptureBase::ImageRequest(camera_name, image_type) }; const auto& response = getImages(request); diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.h b/Unreal/Plugins/AirSim/Source/WorldSimApi.h index d1457993b5..e523af39a5 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.h +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.h @@ -90,10 +90,10 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase virtual std::vector getDistortionParams(const std::string& camera_name, const std::string& vehicle_name = "", bool external = false) const override; - std::vector getImages(const std::vector& requests, - const std::string& vehicle_name = "", bool external = false) const; - std::vector getImage(const std::string& camera_name, ImageCaptureBase::ImageType image_type, - const std::string& vehicle_name = "", bool external = false) const; + virtual std::vector getImages(const std::vector& requests, + const std::string& vehicle_name = "", bool external = false) const override; + virtual std::vector getImage(const std::string& camera_name, ImageCaptureBase::ImageType image_type, + const std::string& vehicle_name = "", bool external = false) const override; private: AActor* createNewActor(const FActorSpawnParameters& spawn_params, const FTransform& actor_transform, const Vector3r& scale, UStaticMesh* static_mesh); From 61e15c954c72f6af1ef2b42d3c6ee1cb008eee3b Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Wed, 9 Jun 2021 13:15:49 +0530 Subject: [PATCH 16/29] Fix missing vehicle_name parameter --- AirLib/src/api/RpcLibServerBase.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index 023b8576ac..f6213414fd 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -286,7 +286,7 @@ namespace airlib pimpl_->server.bind("simSetCameraPose", [&](const std::string& camera_name, const RpcLibAdaptorsBase::Pose& pose, const std::string& vehicle_name, bool external) -> void { - getWorldSimApi()->setCameraPose(camera_name, pose.to(), external); + getWorldSimApi()->setCameraPose(camera_name, pose.to(), vehicle_name, external); }); pimpl_->server.bind("simSetCameraFov", [&](const std::string& camera_name, float fov_degrees, From f39db3009a2f2281322bce48da75e03735310842 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Wed, 10 Feb 2021 09:31:46 +0530 Subject: [PATCH 17/29] Pythonclient: example script for testing external cameras --- .../computer_vision/external_camera.py | 101 ++++++++++++++++++ 1 file changed, 101 insertions(+) create mode 100644 PythonClient/computer_vision/external_camera.py diff --git a/PythonClient/computer_vision/external_camera.py b/PythonClient/computer_vision/external_camera.py new file mode 100644 index 0000000000..de1b74524c --- /dev/null +++ b/PythonClient/computer_vision/external_camera.py @@ -0,0 +1,101 @@ +import airsim +import os +import tempfile + +""" +A simple script to test all the camera APIs. Change the cmaera name and whether it's an external camera + +Example Settings for external camera - + +{ + "SettingsVersion": 1.2, + "SimMode": "Car", + "ExternalCameras": { + "fixed1": { + "X": 0, "Y": 0, "Z": -5, + "Pitch": -90, "Roll": 0, "Yaw": 0 + } + } +} +""" + +# Just change the below to test different cameras easily! +CAM_NAME = "fixed1" +IS_EXTERNAL_CAM = True + + +client = airsim.VehicleClient() +client.confirmConnection() + +tmp_dir = os.path.join(tempfile.gettempdir(), "airsim_cv_mode") +print ("Saving images to %s" % tmp_dir) +try: + os.makedirs(tmp_dir) +except OSError: + if not os.path.isdir(tmp_dir): + raise + +print(f"Camera: {CAM_NAME}, External = {IS_EXTERNAL_CAM}") + +# Test Camera info +cam_info = client.simGetCameraInfo(CAM_NAME, external=IS_EXTERNAL_CAM) +print(cam_info) + +# Test Image APIs +airsim.wait_key('Press any key to get images') + +requests = [airsim.ImageRequest(CAM_NAME, airsim.ImageType.Scene), + airsim.ImageRequest(CAM_NAME, airsim.ImageType.DepthPlanner), + airsim.ImageRequest(CAM_NAME, airsim.ImageType.DepthVis), + airsim.ImageRequest(CAM_NAME, airsim.ImageType.Segmentation), + airsim.ImageRequest(CAM_NAME, airsim.ImageType.SurfaceNormals)] + +def save_images(responses, prefix = ""): + for i, response in enumerate(responses): + filename = os.path.join(tmp_dir, prefix + "_" + str(i)) + + if response.pixels_as_float: + print(f"Type {response.image_type}, size {len(response.image_data_float)}, pos {response.camera_position}") + airsim.write_pfm(os.path.normpath(filename + '.pfm'), airsim.get_pfm_array(response)) + else: + print(f"Type {response.image_type}, size {len(response.image_data_uint8)}, pos {response.camera_position}") + airsim.write_file(os.path.normpath(filename + '.png'), response.image_data_uint8) + + +responses = client.simGetImages(requests, external=IS_EXTERNAL_CAM) +save_images(responses, "old_fov") + + +# Test FoV API +airsim.wait_key('Press any key to change FoV and get images') + +client.simSetCameraFov(CAM_NAME, 120, external=IS_EXTERNAL_CAM) + +responses = client.simGetImages(requests, external = IS_EXTERNAL_CAM) +save_images(responses, "new_fov") + +new_cam_info = client.simGetCameraInfo(CAM_NAME, external=IS_EXTERNAL_CAM) +print(f"Old FOV: {cam_info.fov}, New FOV: {new_cam_info.fov}") + + +# Test Pose APIs +new_pose = airsim.Pose(airsim.Vector3r(-10, -5, -5), airsim.to_quaternion(0.1, 0, 0.1)) +client.simSetCameraPose(CAM_NAME, new_pose, external=IS_EXTERNAL_CAM) + +responses = client.simGetImages(requests, external=IS_EXTERNAL_CAM) +save_images(responses, "new_pose") + +new_cam_info = client.simGetCameraInfo(CAM_NAME, external=IS_EXTERNAL_CAM) +print(f"Old Pose: {cam_info.pose}, New Pose: {new_cam_info.pose}") + + +# Test Distortion params APIs +dist_params = client.simGetDistortionParams(CAM_NAME, external=IS_EXTERNAL_CAM) +print(f"Distortion Params: {dist_params}") + +new_params_dict = {"K1": 0.1, "K2": 0.01, "K3": 0.0, "P1": 0.0, "P2": 0.0} +print(f"Setting distortion params as {new_params_dict}") +client.simSetDistortionParams(CAM_NAME, new_params_dict, external=IS_EXTERNAL_CAM) + +dist_params = client.simGetDistortionParams(CAM_NAME, external=IS_EXTERNAL_CAM) +print(f"Updated Distortion Params: {dist_params}") From 8812b08ec8730f820df6ecfc279e7a529cd52a12 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Wed, 9 Jun 2021 15:46:44 +0530 Subject: [PATCH 18/29] Apply clang-format --- AirLib/include/common/AirSimSettings.hpp | 11 +++++++---- AirLib/src/api/RpcLibServerBase.cpp | 19 +++++++------------ .../AirsimWrapper/Source/PawnSimApi.cpp | 2 -- Unreal/Plugins/AirSim/Source/PIPCamera.cpp | 2 +- Unreal/Plugins/AirSim/Source/PIPCamera.h | 8 +++++--- Unreal/Plugins/AirSim/Source/PawnSimApi.h | 17 +++++++++++------ .../Plugins/AirSim/Source/SimHUD/SimHUD.cpp | 3 ++- .../AirSim/Source/SimMode/SimModeBase.cpp | 9 ++++----- .../AirSim/Source/SimMode/SimModeBase.h | 2 +- 9 files changed, 38 insertions(+), 35 deletions(-) diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index 1e59a859e9..eb15c27106 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/AirLib/include/common/AirSimSettings.hpp @@ -53,9 +53,13 @@ namespace airlib bool external; SubwindowSetting(int window_index_val = 0, ImageType image_type_val = ImageType::Scene, bool visible_val = false, - const std::string& camera_name_val = "", const std::string& vehicle_name_val = "", bool external_val = false) - : window_index(window_index_val), image_type(image_type_val), visible(visible_val), - camera_name(camera_name_val), vehicle_name(vehicle_name_val), external(external_val) + const std::string& camera_name_val = "", const std::string& vehicle_name_val = "", bool external_val = false) + : window_index(window_index_val) + , image_type(image_type_val) + , visible(visible_val) + , camera_name(camera_name_val) + , vehicle_name(vehicle_name_val) + , external(external_val) { } }; @@ -1348,7 +1352,6 @@ namespace airlib } } } - }; } } //namespace diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index f6213414fd..dd51660657 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -144,10 +144,9 @@ namespace airlib return getWorldSimApi()->runConsoleCommand(command); }); - pimpl_->server.bind("simGetImages", [&](const std::vector& request_adapter, const std::string& vehicle_name, bool external) -> - vector { - const auto& response = getWorldSimApi()->getImages(RpcLibAdaptorsBase::ImageRequest::to(request_adapter), vehicle_name, external); - return RpcLibAdaptorsBase::ImageResponse::from(response); + pimpl_->server.bind("simGetImages", [&](const std::vector& request_adapter, const std::string& vehicle_name, bool external) -> vector { + const auto& response = getWorldSimApi()->getImages(RpcLibAdaptorsBase::ImageRequest::to(request_adapter), vehicle_name, external); + return RpcLibAdaptorsBase::ImageResponse::from(response); }); pimpl_->server.bind("simGetImage", [&](const std::string& camera_name, ImageCaptureBase::ImageType type, const std::string& vehicle_name, bool external) -> vector { @@ -274,23 +273,19 @@ namespace airlib return RpcLibAdaptorsBase::CameraInfo(camera_info); }); - pimpl_->server.bind("simSetDistortionParam", [&](const std::string& camera_name, const std::string& param_name, float value, - const std::string& vehicle_name, bool external) -> void { + pimpl_->server.bind("simSetDistortionParam", [&](const std::string& camera_name, const std::string& param_name, float value, const std::string& vehicle_name, bool external) -> void { getWorldSimApi()->setDistortionParam(camera_name, param_name, value, vehicle_name, external); }); - pimpl_->server.bind("simGetDistortionParams", [&](const std::string& camera_name, const std::string& vehicle_name, - bool external) -> std::vector { + pimpl_->server.bind("simGetDistortionParams", [&](const std::string& camera_name, const std::string& vehicle_name, bool external) -> std::vector { return getWorldSimApi()->getDistortionParams(camera_name, vehicle_name, external); }); - pimpl_->server.bind("simSetCameraPose", [&](const std::string& camera_name, const RpcLibAdaptorsBase::Pose& pose, - const std::string& vehicle_name, bool external) -> void { + pimpl_->server.bind("simSetCameraPose", [&](const std::string& camera_name, const RpcLibAdaptorsBase::Pose& pose, const std::string& vehicle_name, bool external) -> void { getWorldSimApi()->setCameraPose(camera_name, pose.to(), vehicle_name, external); }); - pimpl_->server.bind("simSetCameraFov", [&](const std::string& camera_name, float fov_degrees, - const std::string& vehicle_name, bool external) -> void { + pimpl_->server.bind("simSetCameraFov", [&](const std::string& camera_name, float fov_degrees, const std::string& vehicle_name, bool external) -> void { getWorldSimApi()->setCameraFoV(camera_name, fov_degrees, vehicle_name, external); }); diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp b/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp index 6f36a665fd..bb05d5f0aa 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp @@ -135,7 +135,6 @@ const NedTransform& PawnSimApi::getNedTransform() const return ned_transform_; } - msr::airlib::RCData PawnSimApi::getRCData() const { AirSimRCData rcDataFromUnity = GetRCData(getVehicleName().c_str()); @@ -231,7 +230,6 @@ void PawnSimApi::allowPassthroughToggleInput() PrintLogMessage("enable_passthrough_on_collisions: ", state_.passthrough_enabled ? "true" : "false", params_.vehicle_name.c_str(), ErrorLogSeverity::Information); } - //parameters in NED frame PawnSimApi::Pose PawnSimApi::getPose() const { diff --git a/Unreal/Plugins/AirSim/Source/PIPCamera.cpp b/Unreal/Plugins/AirSim/Source/PIPCamera.cpp index 12a8a831b5..a4646b03dc 100644 --- a/Unreal/Plugins/AirSim/Source/PIPCamera.cpp +++ b/Unreal/Plugins/AirSim/Source/PIPCamera.cpp @@ -311,7 +311,7 @@ std::vector APIPCamera::getDistortionParams() const { std::vector param_values(5, 0.0); - auto getParamValue = [this](const auto &name, float &val) { + auto getParamValue = [this](const auto& name, float& val) { distortion_param_instance_->GetScalarParameterValue(FName(name), val); }; diff --git a/Unreal/Plugins/AirSim/Source/PIPCamera.h b/Unreal/Plugins/AirSim/Source/PIPCamera.h index 0d85951b74..17780bcdb2 100644 --- a/Unreal/Plugins/AirSim/Source/PIPCamera.h +++ b/Unreal/Plugins/AirSim/Source/PIPCamera.h @@ -55,10 +55,12 @@ class AIRSIM_API APIPCamera : public ACameraActor UDetectionComponent* getDetectionComponent(const ImageType type, bool if_active) const; msr::airlib::Pose getPose() const; - + private: //members - UPROPERTY() UMaterialParameterCollection* distortion_param_collection_; - UPROPERTY() UMaterialParameterCollectionInstance* distortion_param_instance_; + UPROPERTY() + UMaterialParameterCollection* distortion_param_collection_; + UPROPERTY() + UMaterialParameterCollectionInstance* distortion_param_instance_; UPROPERTY() TArray captures_; diff --git a/Unreal/Plugins/AirSim/Source/PawnSimApi.h b/Unreal/Plugins/AirSim/Source/PawnSimApi.h index aced239297..f0fae2fd53 100644 --- a/Unreal/Plugins/AirSim/Source/PawnSimApi.h +++ b/Unreal/Plugins/AirSim/Source/PawnSimApi.h @@ -54,12 +54,17 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase } Params(APawn* pawn_val, const NedTransform* global_transform_val, PawnEvents* pawn_events_val, - const common_utils::UniqueValueMap& cameras_val, UClass* pip_camera_class_val, - UParticleSystem* collision_display_template_val, const msr::airlib::GeoPoint& home_geopoint_val, - const std::string& vehicle_name_val) - : pawn(pawn_val), global_transform(global_transform_val), pawn_events(pawn_events_val), cameras(cameras_val), - pip_camera_class(pip_camera_class_val), collision_display_template(collision_display_template_val), - home_geopoint(home_geopoint_val), vehicle_name(vehicle_name_val) + const common_utils::UniqueValueMap& cameras_val, UClass* pip_camera_class_val, + UParticleSystem* collision_display_template_val, const msr::airlib::GeoPoint& home_geopoint_val, + const std::string& vehicle_name_val) + : pawn(pawn_val) + , global_transform(global_transform_val) + , pawn_events(pawn_events_val) + , cameras(cameras_val) + , pip_camera_class(pip_camera_class_val) + , collision_display_template(collision_display_template_val) + , home_geopoint(home_geopoint_val) + , vehicle_name(vehicle_name_val) { } }; diff --git a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp index 5589b3abc9..f6164ce9e8 100644 --- a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp +++ b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp @@ -338,7 +338,8 @@ void ASimHUD::initializeSubWindows() subwindow_cameras_[setting.window_index] = camera; else UAirBlueprintLib::LogMessageString("Invalid Camera settings in element", - std::to_string(setting.window_index), LogDebugLevel::Failure); + std::to_string(setting.window_index), + LogDebugLevel::Failure); } } diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index e915ca1382..8d9494a8d5 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -472,7 +472,8 @@ void ASimModeBase::initializeExternalCameras() //get pose FVector position = transform.fromLocalNed(setting.position) - transform.fromLocalNed(Vector3r::Zero()); FTransform camera_transform(FRotator(setting.rotation.pitch, setting.rotation.yaw, setting.rotation.roll), - position, FVector(1., 1., 1.)); + position, + FVector(1., 1., 1.)); //spawn and attach camera to pawn camera_spawn_params.Name = FName(("external_" + camera_setting_pair.first).c_str()); @@ -512,14 +513,12 @@ bool ASimModeBase::isRecording() const const APIPCamera* ASimModeBase::getCamera(const std::string& camera_name, const std::string& vehicle_name, bool external) const { - return external ? getExternalCamera(camera_name) : - getVehicleSimApi(vehicle_name)->getCamera(camera_name); + return external ? getExternalCamera(camera_name) : getVehicleSimApi(vehicle_name)->getCamera(camera_name); } const UnrealImageCapture* ASimModeBase::getImageCapture(const std::string& vehicle_name, bool external) const { - return external ? external_image_capture_.get() : - getVehicleSimApi(vehicle_name)->getImageCapture(); + return external ? external_image_capture_.get() : getVehicleSimApi(vehicle_name)->getImageCapture(); } //API server start/stop diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h index 3965fb1c76..022763e067 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h @@ -103,7 +103,7 @@ class AIRSIM_API ASimModeBase : public AActor return const_cast( static_cast(this)->getExternalCamera(camera_name)); } - + APIPCamera* getCamera(const std::string& camera_name, const std::string& vehicle_name = "", bool external = false) { return const_cast( From c1025af431db82a5b723cac20376e2326131ad37 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Wed, 9 Jun 2021 23:51:27 +0530 Subject: [PATCH 19/29] Remove deprecated simSetCameraOrientation API --- AirLib/include/api/RpcLibClientBase.hpp | 2 -- AirLib/src/api/RpcLibClientBase.cpp | 7 ------- PythonClient/airsim/client.py | 18 ------------------ 3 files changed, 27 deletions(-) diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index 81bea76896..139cb675be 100644 --- a/AirLib/include/api/RpcLibClientBase.hpp +++ b/AirLib/include/api/RpcLibClientBase.hpp @@ -125,8 +125,6 @@ namespace airlib std::vector simGetDistortionParams(const std::string& camera_name, const std::string& vehicle_name = "", bool external = false); void simSetCameraPose(const std::string& camera_name, const Pose& pose, const std::string& vehicle_name = "", bool external = false); void simSetCameraFov(const std::string& camera_name, float fov_degrees, const std::string& vehicle_name = "", bool external = false); - // This is a backwards-compatibility wrapper over simSetCameraPose, and can be removed in future major releases - void simSetCameraOrientation(const std::string& camera_name, const Quaternionr& orientation, const std::string& vehicle_name = "", bool external = false); bool simCreateVoxelGrid(const Vector3r& position, const int& x_size, const int& y_size, const int& z_size, const float& res, const std::string& output_file); msr::airlib::Kinematics::State simGetGroundTruthKinematics(const std::string& vehicle_name = "") const; diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index 05f51e5be4..5ef3ef5d7d 100644 --- a/AirLib/src/api/RpcLibClientBase.cpp +++ b/AirLib/src/api/RpcLibClientBase.cpp @@ -445,13 +445,6 @@ __pragma(warning(disable : 4239)) pimpl_->client.call("simSetCameraPose", camera_name, RpcLibAdaptorsBase::Pose(pose), vehicle_name, external); } - void RpcLibClientBase::simSetCameraOrientation(const std::string& camera_name, const Quaternionr& orientation, const std::string& vehicle_name, bool external) - { - std::cout << "`simSetCameraOrientation` API has been upgraded to `simSetCameraPose`. Please update your code." << std::endl; - Pose pose{ Vector3r::Zero(), orientation }; - RpcLibClientBase::simSetCameraPose(camera_name, pose, vehicle_name, external); - } - void RpcLibClientBase::simSetCameraFov(const std::string& camera_name, float fov_degrees, const std::string& vehicle_name, bool external) { pimpl_->client.call("simSetCameraFov", camera_name, fov_degrees, vehicle_name, external); diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index b431e244dc..1881ba47b1 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -652,24 +652,6 @@ def simSetCameraPose(self, camera_name, pose, vehicle_name = '', external = Fals # TODO: below str() conversion is only needed for legacy reason and should be removed in future self.client.call('simSetCameraPose', str(camera_name), pose, vehicle_name, external) - def simSetCameraOrientation(self, camera_name, orientation, vehicle_name = '', external = False): - """ - .. note:: - - This API has been upgraded to `simSetCameraPose` - - - Control the Orientation 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 - vehicle_name (str, optional): Name of vehicle which the camera corresponds to - external (bool, optional): Whether the camera is an External Camera - """ - logging.warning("`simSetCameraOrientation` API has been upgraded to `simSetCameraPose`. Please update your code.") - pose = Pose(orientation_val=orientation) - self.simSetCameraPose(camera_name, pose, vehicle_name, external) - def simSetCameraFov(self, camera_name, fov_degrees, vehicle_name = '', external = False): """ - Control the field of view of a selected camera From 7c368b2597b7175b15bc32026864f8e62f7d47b0 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Thu, 24 Jun 2021 01:03:40 +0530 Subject: [PATCH 20/29] Move Detection APIs to WorldSimApi, add external field --- AirLib/include/api/RpcLibClientBase.hpp | 8 +-- AirLib/include/api/VehicleSimApiBase.hpp | 5 -- AirLib/include/api/WorldSimApiBase.hpp | 15 +++- AirLib/src/api/RpcLibClientBase.cpp | 16 ++--- AirLib/src/api/RpcLibServerBase.cpp | 16 ++--- .../AirSim/Source/DetectionComponent.cpp | 19 +++++ .../AirSim/Source/DetectionComponent.h | 12 ++-- Unreal/Plugins/AirSim/Source/PawnSimApi.cpp | 63 ----------------- Unreal/Plugins/AirSim/Source/PawnSimApi.h | 5 -- Unreal/Plugins/AirSim/Source/WorldSimApi.cpp | 70 +++++++++++++++++++ Unreal/Plugins/AirSim/Source/WorldSimApi.h | 11 ++- 11 files changed, 139 insertions(+), 101 deletions(-) diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index 139cb675be..8bd3af9c04 100644 --- a/AirLib/include/api/RpcLibClientBase.hpp +++ b/AirLib/include/api/RpcLibClientBase.hpp @@ -74,10 +74,10 @@ namespace airlib int simGetSegmentationObjectID(const std::string& mesh_name) const; void simPrintLogMessage(const std::string& message, std::string message_param = "", unsigned char severity = 0); - void simAddDetectionFilterMeshName(const std::string& camera_name, const std::string& mesh_name, const std::string& vehicle_name = ""); - void simSetDetectionFilterRadius(const std::string& camera_name, const float radius_cm, const std::string& vehicle_name = ""); - void simClearDetectionMeshNames(const std::string& camera_name, const std::string& vehicle_name = ""); - vector simGetDetections(const std::string& camera_name, ImageCaptureBase::ImageType image_type, const std::string& vehicle_name = ""); + void simAddDetectionFilterMeshName(const std::string& camera_name, const std::string& mesh_name, const std::string& vehicle_name = "", bool external = false); + void simSetDetectionFilterRadius(const std::string& camera_name, const float radius_cm, const std::string& vehicle_name = "", bool external = false); + void simClearDetectionMeshNames(const std::string& camera_name, const std::string& vehicle_name = "", bool external = false); + vector simGetDetections(const std::string& camera_name, ImageCaptureBase::ImageType image_type, const std::string& vehicle_name = "", bool external = false); void simFlushPersistentMarkers(); void simPlotPoints(const vector& points, const vector& color_rgba, float size, float duration, bool is_persistent); diff --git a/AirLib/include/api/VehicleSimApiBase.hpp b/AirLib/include/api/VehicleSimApiBase.hpp index 7f9e549558..ffa329ec6e 100644 --- a/AirLib/include/api/VehicleSimApiBase.hpp +++ b/AirLib/include/api/VehicleSimApiBase.hpp @@ -64,11 +64,6 @@ namespace airlib virtual void toggleTrace() = 0; virtual void setTraceLine(const std::vector& color_rgba, float thickness) = 0; - virtual void addDetectionFilterMeshName(const std::string& camera_name, ImageCaptureBase::ImageType image_type, const std::string& mesh_name) = 0; - virtual void setDetectionFilterRadius(const std::string& camera_name, ImageCaptureBase::ImageType image_type, const float radius_cm) = 0; - virtual void clearDetectionMeshNames(const std::string& camera_name, ImageCaptureBase::ImageType image_type) = 0; - virtual std::vector getDetections(const std::string& camera_name, ImageCaptureBase::ImageType image_type) const = 0; - //use pointer here because of derived classes for VehicleSetting const AirSimSettings::VehicleSetting* getVehicleSetting() const { diff --git a/AirLib/include/api/WorldSimApiBase.hpp b/AirLib/include/api/WorldSimApiBase.hpp index 59355a6471..e8d7b52b9e 100644 --- a/AirLib/include/api/WorldSimApiBase.hpp +++ b/AirLib/include/api/WorldSimApiBase.hpp @@ -89,7 +89,7 @@ namespace airlib virtual bool testLineOfSightBetweenPoints(const msr::airlib::GeoPoint& point1, const msr::airlib::GeoPoint& point2) const = 0; virtual vector getWorldExtents() const = 0; - // Image APIs + // Camera APIs virtual CameraInfo getCameraInfo(const std::string& camera_name, const std::string& vehicle_name = "", bool external = false) const = 0; virtual void setCameraPose(const std::string& camera_name, const msr::airlib::Pose& pose, const std::string& vehicle_name = "", bool external = false) = 0; @@ -97,13 +97,22 @@ namespace airlib const std::string& vehicle_name = "", bool external = false) = 0; virtual void setDistortionParam(const std::string& camera_name, const std::string& param_name, float value, const std::string& vehicle_name = "", bool external = false) = 0; - virtual std::vector getDistortionParams(const std::string& camera_name, const std::string& vehicle_name = "", - bool external = false) const = 0; + virtual std::vector getDistortionParams(const std::string& camera_name, + const std::string& vehicle_name = "", bool external = false) const = 0; virtual std::vector getImages(const std::vector& requests, const std::string& vehicle_name = "", bool external = false) const = 0; virtual std::vector getImage(const std::string& camera_name, ImageCaptureBase::ImageType image_type, const std::string& vehicle_name = "", bool external = false) const = 0; + + virtual void addDetectionFilterMeshName(const std::string& camera_name, ImageCaptureBase::ImageType image_type, const std::string& mesh_name, + const std::string& vehicle_name = "", bool external = false) = 0; + virtual void setDetectionFilterRadius(const std::string& camera_name, ImageCaptureBase::ImageType image_type, float radius_cm, + const std::string& vehicle_name = "", bool external = false) = 0; + virtual void clearDetectionMeshNames(const std::string& camera_name, ImageCaptureBase::ImageType image_type, + const std::string& vehicle_name = "", bool external = false) = 0; + virtual std::vector getDetections(const std::string& camera_name, ImageCaptureBase::ImageType image_type, + const std::string& vehicle_name = "", bool external = false) = 0; }; } } //namespace diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index 5ef3ef5d7d..06921206a9 100644 --- a/AirLib/src/api/RpcLibClientBase.cpp +++ b/AirLib/src/api/RpcLibClientBase.cpp @@ -213,21 +213,21 @@ __pragma(warning(disable : 4239)) return pimpl_->client.call("simGetSegmentationObjectID", mesh_name).as(); } - void RpcLibClientBase::simAddDetectionFilterMeshName(const std::string& camera_name, const std::string& mesh_name, const std::string& vehicle_name) + void RpcLibClientBase::simAddDetectionFilterMeshName(const std::string& camera_name, const std::string& mesh_name, const std::string& vehicle_name, bool external) { - pimpl_->client.call("simAddDetectionFilterMeshName", camera_name, mesh_name, vehicle_name); + pimpl_->client.call("simAddDetectionFilterMeshName", camera_name, mesh_name, vehicle_name, external); } - void RpcLibClientBase::simSetDetectionFilterRadius(const std::string& camera_name, const float radius_cm, const std::string& vehicle_name) + void RpcLibClientBase::simSetDetectionFilterRadius(const std::string& camera_name, const float radius_cm, const std::string& vehicle_name, bool external) { - pimpl_->client.call("simSetDetectionFilterRadius", camera_name, radius_cm, vehicle_name); + pimpl_->client.call("simSetDetectionFilterRadius", camera_name, radius_cm, vehicle_name, external); } - void RpcLibClientBase::simClearDetectionMeshNames(const std::string& camera_name, const std::string& vehicle_name) + void RpcLibClientBase::simClearDetectionMeshNames(const std::string& camera_name, const std::string& vehicle_name, bool external) { - pimpl_->client.call("simClearDetectionMeshNames", camera_name, vehicle_name); + pimpl_->client.call("simClearDetectionMeshNames", camera_name, vehicle_name, external); } - vector RpcLibClientBase::simGetDetections(const std::string& camera_name, ImageCaptureBase::ImageType image_type, const std::string& vehicle_name) + vector RpcLibClientBase::simGetDetections(const std::string& camera_name, ImageCaptureBase::ImageType image_type, const std::string& vehicle_name, bool external) { - const auto& result = pimpl_->client.call("simGetDetections", camera_name, image_type, vehicle_name).as>(); + const auto& result = pimpl_->client.call("simGetDetections", camera_name, image_type, vehicle_name, external).as>(); return RpcLibAdaptorsBase::DetectionInfo::to(result); } diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index dd51660657..bc8ebd08d1 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -198,17 +198,17 @@ namespace airlib return getWorldSimApi()->getSegmentationObjectID(mesh_name); }); - pimpl_->server.bind("simAddDetectionFilterMeshName", [&](const std::string& camera_name, ImageCaptureBase::ImageType type, const std::string& mesh_name, const std::string& vehicle_name) -> void { - getVehicleSimApi(vehicle_name)->addDetectionFilterMeshName(camera_name, type, mesh_name); + pimpl_->server.bind("simAddDetectionFilterMeshName", [&](const std::string& camera_name, ImageCaptureBase::ImageType type, const std::string& mesh_name, const std::string& vehicle_name, bool external) -> void { + getWorldSimApi()->addDetectionFilterMeshName(camera_name, type, mesh_name, vehicle_name, external); }); - pimpl_->server.bind("simSetDetectionFilterRadius", [&](const std::string& camera_name, ImageCaptureBase::ImageType type, const float radius_cm, const std::string& vehicle_name) -> void { - getVehicleSimApi(vehicle_name)->setDetectionFilterRadius(camera_name, type, radius_cm); + pimpl_->server.bind("simSetDetectionFilterRadius", [&](const std::string& camera_name, ImageCaptureBase::ImageType type, const float radius_cm, const std::string& vehicle_name, bool external) -> void { + getWorldSimApi()->setDetectionFilterRadius(camera_name, type, radius_cm, vehicle_name, external); }); - pimpl_->server.bind("simClearDetectionMeshNames", [&](const std::string& camera_name, ImageCaptureBase::ImageType type, const std::string& vehicle_name) -> void { - getVehicleSimApi(vehicle_name)->clearDetectionMeshNames(camera_name, type); + pimpl_->server.bind("simClearDetectionMeshNames", [&](const std::string& camera_name, ImageCaptureBase::ImageType type, const std::string& vehicle_name, bool external) -> void { + getWorldSimApi()->clearDetectionMeshNames(camera_name, type, vehicle_name, external); }); - pimpl_->server.bind("simGetDetections", [&](const std::string& camera_name, ImageCaptureBase::ImageType type, const std::string& vehicle_name) -> vector { - const auto& response = getVehicleSimApi(vehicle_name)->getDetections(camera_name, type); + pimpl_->server.bind("simGetDetections", [&](const std::string& camera_name, ImageCaptureBase::ImageType type, const std::string& vehicle_name, bool external) -> vector { + const auto& response = getWorldSimApi()->getDetections(camera_name, type, vehicle_name, external); return RpcLibAdaptorsBase::DetectionInfo::from(response); }); diff --git a/Unreal/Plugins/AirSim/Source/DetectionComponent.cpp b/Unreal/Plugins/AirSim/Source/DetectionComponent.cpp index adb3582594..2e1def652a 100644 --- a/Unreal/Plugins/AirSim/Source/DetectionComponent.cpp +++ b/Unreal/Plugins/AirSim/Source/DetectionComponent.cpp @@ -186,3 +186,22 @@ FRotator UDetectionComponent::getRelativeRotation(FVector in_location, FRotator FTransform relative_object_transform = camera_transform.GetRelativeTransform(FTransform(in_rotation, in_location)); return relative_object_transform.Rotator(); } + +void UDetectionComponent::addMeshName(const std::string& mesh_name) +{ + FString name(mesh_name.c_str()); + + if (!object_filter_.wildcard_mesh_names_.Contains(name)) { + object_filter_.wildcard_mesh_names_.Add(name); + } +} + +void UDetectionComponent::setFilterRadius(const float radius_cm) +{ + max_distance_to_camera_ = radius_cm; +} + +void UDetectionComponent::clearMeshNames() +{ + object_filter_.wildcard_mesh_names_.Empty(); +} diff --git a/Unreal/Plugins/AirSim/Source/DetectionComponent.h b/Unreal/Plugins/AirSim/Source/DetectionComponent.h index 392447b6a7..6f0899af15 100644 --- a/Unreal/Plugins/AirSim/Source/DetectionComponent.h +++ b/Unreal/Plugins/AirSim/Source/DetectionComponent.h @@ -44,6 +44,10 @@ class AIRSIM_API UDetectionComponent : public USceneComponent const TArray& getDetections(); + void addMeshName(const std::string& mesh_name); + void setFilterRadius(const float radius_cm); + void clearMeshNames(); + private: bool calcBoundingFromViewInfo(AActor* actor, FBox2D& box_out); @@ -52,16 +56,16 @@ class AIRSIM_API UDetectionComponent : public USceneComponent FRotator getRelativeRotation(FVector in_location, FRotator in_rotation); public: + UPROPERTY() + UTextureRenderTarget2D* texture_target_; + +private: UPROPERTY() FObjectFilter object_filter_; UPROPERTY(EditAnywhere, Category = "Tracked Actors") float max_distance_to_camera_; - UPROPERTY() - UTextureRenderTarget2D* texture_target_; - -private: UPROPERTY() USceneCaptureComponent2D* scene_capture_component_2D_; diff --git a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp b/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp index ff673ce469..f5c6a71adb 100644 --- a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp @@ -337,69 +337,6 @@ void PawnSimApi::reportState(msr::airlib::StateReporter& reporter) reporter.writeValue("unreal pos", Vector3r(unrealPosition.X, unrealPosition.Y, unrealPosition.Z)); } -void PawnSimApi::addDetectionFilterMeshName(const std::string& camera_name, ImageCaptureBase::ImageType image_type, const std::string& mesh_name) -{ - UAirBlueprintLib::RunCommandOnGameThread([this, camera_name, image_type, mesh_name]() { - const APIPCamera* camera = getCamera(camera_name); - UDetectionComponent* detection_comp = camera->getDetectionComponent(image_type, false); - - FString name = FString(mesh_name.c_str()); - - if (!detection_comp->object_filter_.wildcard_mesh_names_.Contains(name)) { - detection_comp->object_filter_.wildcard_mesh_names_.Add(name); - } - }, - true); -} - -void PawnSimApi::clearDetectionMeshNames(const std::string& camera_name, ImageCaptureBase::ImageType image_type) -{ - UAirBlueprintLib::RunCommandOnGameThread([this, camera_name, image_type]() { - const APIPCamera* camera = getCamera(camera_name); - camera->getDetectionComponent(image_type, false)->object_filter_.wildcard_mesh_names_.Empty(); - }, - true); -} - -void PawnSimApi::setDetectionFilterRadius(const std::string& camera_name, ImageCaptureBase::ImageType image_type, const float radius_cm) -{ - UAirBlueprintLib::RunCommandOnGameThread([this, camera_name, image_type, radius_cm]() { - const APIPCamera* camera = getCamera(camera_name); - camera->getDetectionComponent(image_type, false)->max_distance_to_camera_ = radius_cm; - }, - true); -} - -std::vector PawnSimApi::getDetections(const std::string& camera_name, ImageCaptureBase::ImageType image_type) const -{ - std::vector result; - - UAirBlueprintLib::RunCommandOnGameThread([this, camera_name, image_type, &result]() { - const APIPCamera* camera = getCamera(camera_name); - const TArray& detections = camera->getDetectionComponent(image_type, false)->getDetections(); - result.resize(detections.Num()); - - for (int i = 0; i < detections.Num(); i++) { - result[i].name = std::string(TCHAR_TO_UTF8(*(detections[i].Actor->GetFName().ToString()))); - - Vector3r nedWrtOrigin = ned_transform_.toGlobalNed(detections[i].Actor->GetActorLocation()); - result[i].geo_point = msr::airlib::EarthUtils::nedToGeodetic(nedWrtOrigin, - AirSimSettings::singleton().origin_geopoint); - - result[i].box2D.min = Vector2r(detections[i].Box2D.Min.X, detections[i].Box2D.Min.Y); - result[i].box2D.max = Vector2r(detections[i].Box2D.Max.X, detections[i].Box2D.Max.Y); - - result[i].box3D.min = ned_transform_.toLocalNed(detections[i].Box3D.Min); - result[i].box3D.max = ned_transform_.toLocalNed(detections[i].Box3D.Max); - - result[i].relative_pose = toPose(detections[i].RelativeTransform.GetTranslation(), detections[i].RelativeTransform.GetRotation()); - } - }, - true); - - return result; -} - //void playBack() //{ //if (params_.pawn->GetRootPrimitiveComponent()->IsAnySimulatingPhysics()) { diff --git a/Unreal/Plugins/AirSim/Source/PawnSimApi.h b/Unreal/Plugins/AirSim/Source/PawnSimApi.h index f0fae2fd53..00a51f7bc6 100644 --- a/Unreal/Plugins/AirSim/Source/PawnSimApi.h +++ b/Unreal/Plugins/AirSim/Source/PawnSimApi.h @@ -96,11 +96,6 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase virtual std::string getRecordFileLine(bool is_header_line) const override; virtual void reportState(msr::airlib::StateReporter& reporter) override; - virtual void addDetectionFilterMeshName(const std::string& camera_name, ImageCaptureBase::ImageType image_type, const std::string& mesh_name) override; - virtual void setDetectionFilterRadius(const std::string& camera_name, ImageCaptureBase::ImageType image_type, const float radius_cm) override; - virtual void clearDetectionMeshNames(const std::string& camera_name, ImageCaptureBase::ImageType image_type) override; - virtual std::vector getDetections(const std::string& camera_name, ImageCaptureBase::ImageType image_type) const override; - protected: //additional interface for derived class virtual void pawnTick(float dt); void setPoseInternal(const Pose& pose, bool ignore_collision); diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index e0429803e5..de623dbfe3 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -806,3 +806,73 @@ std::vector WorldSimApi::getImage(const std::string& camera_name, Image else return std::vector(); } + +void WorldSimApi::addDetectionFilterMeshName(const std::string& camera_name, ImageCaptureBase::ImageType image_type, const std::string& mesh_name, + const std::string& vehicle_name, bool external) +{ + const APIPCamera* camera = simmode_->getCamera(camera_name, vehicle_name, external); + + UAirBlueprintLib::RunCommandOnGameThread([camera, image_type, &mesh_name]() { + camera->getDetectionComponent(image_type, false)->addMeshName(mesh_name); + }, + true); +} + +void WorldSimApi::setDetectionFilterRadius(const std::string& camera_name, ImageCaptureBase::ImageType image_type, float radius_cm, + const std::string& vehicle_name, bool external) +{ + const APIPCamera* camera = simmode_->getCamera(camera_name, vehicle_name, external); + + UAirBlueprintLib::RunCommandOnGameThread([camera, image_type, radius_cm]() { + camera->getDetectionComponent(image_type, false)->setFilterRadius(radius_cm); + }, + true); +} + +void WorldSimApi::clearDetectionMeshNames(const std::string& camera_name, ImageCaptureBase::ImageType image_type, + const std::string& vehicle_name, bool external) +{ + const APIPCamera* camera = simmode_->getCamera(camera_name, vehicle_name, external); + + UAirBlueprintLib::RunCommandOnGameThread([camera, image_type]() { + camera->getDetectionComponent(image_type, false)->clearMeshNames(); + }, + true); +} + +std::vector WorldSimApi::getDetections(const std::string& camera_name, ImageCaptureBase::ImageType image_type, + const std::string& vehicle_name, bool external) +{ + std::vector result; + + const APIPCamera* camera = simmode_->getCamera(camera_name, vehicle_name, external); + const NedTransform& ned_transform = external ? simmode_->getGlobalNedTransform() + : simmode_->getVehicleSimApi(vehicle_name)->getNedTransform(); + + UAirBlueprintLib::RunCommandOnGameThread([camera, image_type, &result, &ned_transform]() { + const TArray& detections = camera->getDetectionComponent(image_type, false)->getDetections(); + result.resize(detections.Num()); + + for (int i = 0; i < detections.Num(); i++) { + result[i].name = std::string(TCHAR_TO_UTF8(*(detections[i].Actor->GetFName().ToString()))); + + Vector3r nedWrtOrigin = ned_transform.toGlobalNed(detections[i].Actor->GetActorLocation()); + result[i].geo_point = msr::airlib::EarthUtils::nedToGeodetic(nedWrtOrigin, + AirSimSettings::singleton().origin_geopoint); + + result[i].box2D.min = Vector2r(detections[i].Box2D.Min.X, detections[i].Box2D.Min.Y); + result[i].box2D.max = Vector2r(detections[i].Box2D.Max.X, detections[i].Box2D.Max.Y); + + result[i].box3D.min = ned_transform.toLocalNed(detections[i].Box3D.Min); + result[i].box3D.max = ned_transform.toLocalNed(detections[i].Box3D.Max); + + const Vector3r& position = ned_transform.toLocalNed(detections[i].RelativeTransform.GetTranslation()); + const Quaternionr& orientation = ned_transform.toNed(detections[i].RelativeTransform.GetRotation()); + + result[i].relative_pose = Pose(position, orientation); + } + }, + true); + + return result; +} diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.h b/Unreal/Plugins/AirSim/Source/WorldSimApi.h index e523af39a5..48cfc00e2d 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.h +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.h @@ -79,7 +79,7 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase virtual bool testLineOfSightBetweenPoints(const msr::airlib::GeoPoint& point1, const msr::airlib::GeoPoint& point2) const override; virtual std::vector getWorldExtents() const override; - // Image APIs + // Camera APIs virtual msr::airlib::CameraInfo getCameraInfo(const std::string& camera_name, const std::string& vehicle_name = "", bool external = false) const override; virtual void setCameraPose(const std::string& camera_name, const msr::airlib::Pose& pose, const std::string& vehicle_name = "", bool external = false) override; @@ -95,6 +95,15 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase virtual std::vector getImage(const std::string& camera_name, ImageCaptureBase::ImageType image_type, const std::string& vehicle_name = "", bool external = false) const override; + virtual void addDetectionFilterMeshName(const std::string& camera_name, ImageCaptureBase::ImageType image_type, const std::string& mesh_name, + const std::string& vehicle_name = "", bool external = false) override; + virtual void setDetectionFilterRadius(const std::string& camera_name, ImageCaptureBase::ImageType image_type, float radius_cm, + const std::string& vehicle_name = "", bool external = false) override; + virtual void clearDetectionMeshNames(const std::string& camera_name, ImageCaptureBase::ImageType image_type, + const std::string& vehicle_name = "", bool external = false) override; + virtual std::vector getDetections(const std::string& camera_name, ImageCaptureBase::ImageType image_type, + const std::string& vehicle_name = "", bool external = false) override; + private: AActor* createNewActor(const FActorSpawnParameters& spawn_params, const FTransform& actor_transform, const Vector3r& scale, UStaticMesh* static_mesh); void spawnPlayer(); From 0c2475352910a546bc49fabad0e3f4c6631ec94a Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Thu, 24 Jun 2021 01:04:08 +0530 Subject: [PATCH 21/29] Update detection Python APIs --- PythonClient/airsim/client.py | 20 ++++++++++++-------- PythonClient/detection/detection.py | 4 ++-- 2 files changed, 14 insertions(+), 10 deletions(-) diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index 1881ba47b1..d62054add8 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -510,7 +510,7 @@ def simGetSegmentationObjectID(self, mesh_name): """ return self.client.call('simGetSegmentationObjectID', mesh_name) - def simAddDetectionFilterMeshName(self, camera_name, image_type, mesh_name, vehicle_name = ''): + def simAddDetectionFilterMeshName(self, camera_name, image_type, mesh_name, vehicle_name = '', external = False): """ Add mesh name to detect in wild card format @@ -521,11 +521,12 @@ def simAddDetectionFilterMeshName(self, camera_name, image_type, mesh_name, vehi image_type (ImageType): Type of image required mesh_name (str): mesh name in wild card format vehicle_name (str, optional): Vehicle which the camera is associated with + external (bool, optional): Whether the camera is an External Camera """ - self.client.call('simAddDetectionFilterMeshName', camera_name, image_type, mesh_name, vehicle_name) + self.client.call('simAddDetectionFilterMeshName', camera_name, image_type, mesh_name, vehicle_name, external) - def simSetDetectionFilterRadius(self, camera_name, image_type, radius_cm, vehicle_name = ''): + def simSetDetectionFilterRadius(self, camera_name, image_type, radius_cm, vehicle_name = '', external = False): """ Set detection radius for all cameras @@ -534,10 +535,11 @@ def simSetDetectionFilterRadius(self, camera_name, image_type, radius_cm, vehicl image_type (ImageType): Type of image required radius_cm (int): Radius in [cm] vehicle_name (str, optional): Vehicle which the camera is associated with + external (bool, optional): Whether the camera is an External Camera """ - self.client.call('simSetDetectionFilterRadius', camera_name, image_type, radius_cm, vehicle_name) + self.client.call('simSetDetectionFilterRadius', camera_name, image_type, radius_cm, vehicle_name, external) - def simClearDetectionMeshNames(self, camera_name, image_type, vehicle_name = ''): + def simClearDetectionMeshNames(self, camera_name, image_type, vehicle_name = '', external = False): """ Clear all mesh names from detection filter @@ -545,11 +547,12 @@ def simClearDetectionMeshNames(self, camera_name, image_type, vehicle_name = '') camera_name (str): Name of the camera, for backwards compatibility, ID numbers such as 0,1,etc. can also be used image_type (ImageType): Type of image required vehicle_name (str, optional): Vehicle which the camera is associated with + external (bool, optional): Whether the camera is an External Camera """ - self.client.call('simClearDetectionMeshNames', camera_name, image_type, vehicle_name) + self.client.call('simClearDetectionMeshNames', camera_name, image_type, vehicle_name, external) - def simGetDetections(self, camera_name, image_type, vehicle_name = ''): + def simGetDetections(self, camera_name, image_type, vehicle_name = '', external = False): """ Get current detections @@ -557,11 +560,12 @@ def simGetDetections(self, camera_name, image_type, vehicle_name = ''): camera_name (str): Name of the camera, for backwards compatibility, ID numbers such as 0,1,etc. can also be used image_type (ImageType): Type of image required vehicle_name (str, optional): Vehicle which the camera is associated with + external (bool, optional): Whether the camera is an External Camera Returns: DetectionInfo array """ - responses_raw = self.client.call('simGetDetections', camera_name, image_type, vehicle_name) + responses_raw = self.client.call('simGetDetections', camera_name, image_type, vehicle_name, external) return [DetectionInfo.from_msgpack(response_raw) for response_raw in responses_raw] def simPrintLogMessage(self, message, message_param = "", severity = 0): diff --git a/PythonClient/detection/detection.py b/PythonClient/detection/detection.py index 528941ca9a..388228826a 100644 --- a/PythonClient/detection/detection.py +++ b/PythonClient/detection/detection.py @@ -5,7 +5,7 @@ import pprint # connect to the AirSim simulator -client = airsim.MultirotorClient() +client = airsim.VehicleClient() client.confirmConnection() # set camera name and image type to request images and detections @@ -40,4 +40,4 @@ client.simClearDetectionMeshNames(camera_name, image_type) elif cv2.waitKey(1) & 0xFF == ord('a'): client.simAddDetectionFilterMeshName(camera_name, image_type, "Cylinder*") -cv2.destroyAllWindows() \ No newline at end of file +cv2.destroyAllWindows() From 3a1052ac3312ecb73608c878324da279c6f32d95 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Thu, 24 Jun 2021 23:09:36 +0530 Subject: [PATCH 22/29] Unity: Move detection APIs --- .../AirsimWrapper/Source/PawnSimApi.cpp | 44 ---------------- .../AirsimWrapper/Source/PawnSimApi.h | 6 --- .../AirsimWrapper/Source/WorldSimApi.cpp | 51 +++++++++++++++++++ .../AirsimWrapper/Source/WorldSimApi.h | 11 +++- 4 files changed, 61 insertions(+), 51 deletions(-) diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp b/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp index bb05d5f0aa..b51084e70d 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp @@ -59,50 +59,6 @@ void PawnSimApi::pawnTick(float dt) updateRendering(dt); } -void PawnSimApi::addDetectionFilterMeshName(const std::string& camera_name, ImageCaptureBase::ImageType image_type, const std::string& mesh_name) -{ - unused(camera_name); - unused(image_type); - unused(mesh_name); - - throw std::invalid_argument(common_utils::Utils::stringf( - "addDetectionFilterMeshName is not supported on unity") - .c_str()); -} - -void PawnSimApi::setDetectionFilterRadius(const std::string& camera_name, ImageCaptureBase::ImageType image_type, const float radius_cm) -{ - unused(camera_name); - unused(image_type); - unused(radius_cm); - - throw std::invalid_argument(common_utils::Utils::stringf( - "setDetectionFilterRadius is not supported on unity") - .c_str()); -} - -void PawnSimApi::clearDetectionMeshNames(const std::string& camera_name, ImageCaptureBase::ImageType image_type) -{ - unused(camera_name); - unused(image_type); - - throw std::invalid_argument(common_utils::Utils::stringf( - "clearDetectionMeshNames is not supported on unity") - .c_str()); -} - -std::vector PawnSimApi::getDetections(const std::string& camera_name, ImageCaptureBase::ImageType image_type) const -{ - unused(camera_name); - unused(image_type); - - throw std::invalid_argument(common_utils::Utils::stringf( - "getDetections is not supported on unity") - .c_str()); - - return std::vector(); -} - bool PawnSimApi::testLineOfSightToPoint(const msr::airlib::GeoPoint& point) const { unused(point); diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.h b/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.h index aecaee70dc..83cb525bf8 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.h +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.h @@ -29,7 +29,6 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase typedef msr::airlib::Utils Utils; typedef msr::airlib::AirSimSettings::VehicleSetting VehicleSetting; typedef msr::airlib::ImageCaptureBase ImageCaptureBase; - typedef msr::airlib::DetectionInfo DetectionInfo; public: struct Params @@ -93,11 +92,6 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase const NedTransform& getNedTransform() const; virtual void pawnTick(float dt); - virtual void addDetectionFilterMeshName(const std::string& camera_name, ImageCaptureBase::ImageType image_type, const std::string& mesh_name) override; - virtual void setDetectionFilterRadius(const std::string& camera_name, ImageCaptureBase::ImageType image_type, const float radius_cm) override; - virtual void clearDetectionMeshNames(const std::string& camera_name, ImageCaptureBase::ImageType image_type) override; - virtual std::vector getDetections(const std::string& camera_name, ImageCaptureBase::ImageType image_type) const override; - virtual bool testLineOfSightToPoint(const msr::airlib::GeoPoint& point) const override; private: diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp index 8597d60788..e430010609 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp @@ -330,4 +330,55 @@ std::vector WorldSimApi::getImage(const std::string& camera_name, Image return std::vector(); } +void WorldSimApi::addDetectionFilterMeshName(const std::string& camera_name, ImageCaptureBase::ImageType image_type, const std::string& mesh_name, + const std::string& vehicle_name, bool external) +{ + unused(camera_name); + unused(image_type); + unused(mesh_name); + + throw std::invalid_argument(common_utils::Utils::stringf( + "addDetectionFilterMeshName is not supported on unity") + .c_str()); +} + +void WorldSimApi::setDetectionFilterRadius(const std::string& camera_name, ImageCaptureBase::ImageType image_type, float radius_cm, + const std::string& vehicle_name, bool external) +{ + unused(camera_name); + unused(image_type); + unused(radius_cm); + unused(external); + + throw std::invalid_argument(common_utils::Utils::stringf( + "setDetectionFilterRadius is not supported on unity") + .c_str()); +} + +void WorldSimApi::clearDetectionMeshNames(const std::string& camera_name, ImageCaptureBase::ImageType image_type, + const std::string& vehicle_name, bool external) +{ + unused(camera_name); + unused(image_type); + unused(external); + + throw std::invalid_argument(common_utils::Utils::stringf( + "clearDetectionMeshNames is not supported on unity") + .c_str()); +} + +std::vector WorldSimApi::getDetections(const std::string& camera_name, ImageCaptureBase::ImageType image_type, + const std::string& vehicle_name, bool external) +{ + unused(camera_name); + unused(image_type); + unused(external); + + throw std::invalid_argument(common_utils::Utils::stringf( + "getDetections is not supported on unity") + .c_str()); + + return std::vector(); +} + #pragma endregion diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h index 47d7cbb7be..c22ddba94a 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h @@ -73,7 +73,7 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase virtual bool testLineOfSightBetweenPoints(const msr::airlib::GeoPoint& point1, const msr::airlib::GeoPoint& point2) const override; virtual std::vector getWorldExtents() const override; - // Image APIs + // Camera APIs virtual msr::airlib::CameraInfo getCameraInfo(const std::string& camera_name, const std::string& vehicle_name = "", bool external = false) const override; virtual void setCameraPose(const std::string& camera_name, const msr::airlib::Pose& pose, const std::string& vehicle_name = "", bool external = false) override; @@ -89,6 +89,15 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase virtual std::vector getImage(const std::string& camera_name, msr::airlib::ImageCaptureBase::ImageType image_type, const std::string& vehicle_name = "", bool external = false) const override; + virtual void addDetectionFilterMeshName(const std::string& camera_name, ImageCaptureBase::ImageType image_type, const std::string& mesh_name, + const std::string& vehicle_name = "", bool external = false) override; + virtual void setDetectionFilterRadius(const std::string& camera_name, ImageCaptureBase::ImageType image_type, float radius_cm, + const std::string& vehicle_name = "", bool external = false) override; + virtual void clearDetectionMeshNames(const std::string& camera_name, ImageCaptureBase::ImageType image_type, + const std::string& vehicle_name = "", bool external = false) override; + virtual std::vector getDetections(const std::string& camera_name, ImageCaptureBase::ImageType image_type, + const std::string& vehicle_name = "", bool external = false) override; + private: SimModeBase* simmode_; std::string vehicle_name_; From 2e84c97cf36b762a0f0d9b4a86b5953c53b409aa Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Fri, 25 Jun 2021 00:40:40 +0530 Subject: [PATCH 23/29] Add CameraDetails struct --- AirLib/include/api/WorldSimApiBase.hpp | 26 ++++------ AirLib/include/common/CommonStructs.hpp | 14 ++++++ AirLib/src/api/RpcLibServerBase.cpp | 18 +++---- .../AirsimWrapper/Source/WorldSimApi.cpp | 48 +++++++----------- .../AirsimWrapper/Source/WorldSimApi.h | 27 ++++------ .../Plugins/AirSim/Source/SimHUD/SimHUD.cpp | 2 +- .../AirSim/Source/SimMode/SimModeBase.cpp | 5 +- .../AirSim/Source/SimMode/SimModeBase.h | 8 +-- Unreal/Plugins/AirSim/Source/WorldSimApi.cpp | 50 ++++++++----------- Unreal/Plugins/AirSim/Source/WorldSimApi.h | 27 ++++------ 10 files changed, 101 insertions(+), 124 deletions(-) diff --git a/AirLib/include/api/WorldSimApiBase.hpp b/AirLib/include/api/WorldSimApiBase.hpp index e8d7b52b9e..bd35143d98 100644 --- a/AirLib/include/api/WorldSimApiBase.hpp +++ b/AirLib/include/api/WorldSimApiBase.hpp @@ -90,29 +90,21 @@ namespace airlib virtual vector getWorldExtents() const = 0; // Camera APIs - virtual CameraInfo getCameraInfo(const std::string& camera_name, const std::string& vehicle_name = "", bool external = false) const = 0; - virtual void setCameraPose(const std::string& camera_name, const msr::airlib::Pose& pose, - const std::string& vehicle_name = "", bool external = false) = 0; - virtual void setCameraFoV(const std::string& camera_name, float fov_degrees, - const std::string& vehicle_name = "", bool external = false) = 0; - virtual void setDistortionParam(const std::string& camera_name, const std::string& param_name, float value, - const std::string& vehicle_name = "", bool external = false) = 0; - virtual std::vector getDistortionParams(const std::string& camera_name, - const std::string& vehicle_name = "", bool external = false) const = 0; + virtual CameraInfo getCameraInfo(const CameraDetails& camera_details) const = 0; + virtual void setCameraPose(const msr::airlib::Pose& pose, const CameraDetails& camera_details) = 0; + virtual void setCameraFoV(float fov_degrees, const CameraDetails& camera_details) = 0; + virtual void setDistortionParam(const std::string& param_name, float value, const CameraDetails& camera_details) = 0; + virtual std::vector getDistortionParams(const CameraDetails& camera_details) const = 0; virtual std::vector getImages(const std::vector& requests, const std::string& vehicle_name = "", bool external = false) const = 0; virtual std::vector getImage(const std::string& camera_name, ImageCaptureBase::ImageType image_type, const std::string& vehicle_name = "", bool external = false) const = 0; - virtual void addDetectionFilterMeshName(const std::string& camera_name, ImageCaptureBase::ImageType image_type, const std::string& mesh_name, - const std::string& vehicle_name = "", bool external = false) = 0; - virtual void setDetectionFilterRadius(const std::string& camera_name, ImageCaptureBase::ImageType image_type, float radius_cm, - const std::string& vehicle_name = "", bool external = false) = 0; - virtual void clearDetectionMeshNames(const std::string& camera_name, ImageCaptureBase::ImageType image_type, - const std::string& vehicle_name = "", bool external = false) = 0; - virtual std::vector getDetections(const std::string& camera_name, ImageCaptureBase::ImageType image_type, - const std::string& vehicle_name = "", bool external = false) = 0; + virtual void addDetectionFilterMeshName(ImageCaptureBase::ImageType image_type, const std::string& mesh_name, const CameraDetails& camera_details) = 0; + virtual void setDetectionFilterRadius(ImageCaptureBase::ImageType image_type, float radius_cm, const CameraDetails& camera_details) = 0; + virtual void clearDetectionMeshNames(ImageCaptureBase::ImageType image_type, const CameraDetails& camera_details) = 0; + virtual std::vector getDetections(ImageCaptureBase::ImageType image_type, const CameraDetails& camera_details) = 0; }; } } //namespace diff --git a/AirLib/include/common/CommonStructs.hpp b/AirLib/include/common/CommonStructs.hpp index aa4b7fa2f9..ba7bbc564d 100644 --- a/AirLib/include/common/CommonStructs.hpp +++ b/AirLib/include/common/CommonStructs.hpp @@ -421,6 +421,20 @@ namespace airlib std::vector indices; std::string name; }; + + // This is a small helper struct to keep camera details together + // Not currently exposed to the client, just for cleaner codebase internally + struct CameraDetails + { + std::string camera_name; + std::string vehicle_name; + bool external; + + CameraDetails(const std::string& camera_name_val, const std::string& vehicle_name_val, bool external_val) + : camera_name(camera_name_val), vehicle_name(vehicle_name_val), external(external_val) + { + } + }; } } //namespace #endif diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index bc8ebd08d1..ef1d3a8fd9 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -199,16 +199,16 @@ namespace airlib }); pimpl_->server.bind("simAddDetectionFilterMeshName", [&](const std::string& camera_name, ImageCaptureBase::ImageType type, const std::string& mesh_name, const std::string& vehicle_name, bool external) -> void { - getWorldSimApi()->addDetectionFilterMeshName(camera_name, type, mesh_name, vehicle_name, external); + getWorldSimApi()->addDetectionFilterMeshName(type, mesh_name, CameraDetails(camera_name, vehicle_name, external)); }); pimpl_->server.bind("simSetDetectionFilterRadius", [&](const std::string& camera_name, ImageCaptureBase::ImageType type, const float radius_cm, const std::string& vehicle_name, bool external) -> void { - getWorldSimApi()->setDetectionFilterRadius(camera_name, type, radius_cm, vehicle_name, external); + getWorldSimApi()->setDetectionFilterRadius(type, radius_cm, CameraDetails(camera_name, vehicle_name, external)); }); pimpl_->server.bind("simClearDetectionMeshNames", [&](const std::string& camera_name, ImageCaptureBase::ImageType type, const std::string& vehicle_name, bool external) -> void { - getWorldSimApi()->clearDetectionMeshNames(camera_name, type, vehicle_name, external); + getWorldSimApi()->clearDetectionMeshNames(type, CameraDetails(camera_name, vehicle_name, external)); }); pimpl_->server.bind("simGetDetections", [&](const std::string& camera_name, ImageCaptureBase::ImageType type, const std::string& vehicle_name, bool external) -> vector { - const auto& response = getWorldSimApi()->getDetections(camera_name, type, vehicle_name, external); + const auto& response = getWorldSimApi()->getDetections(type, CameraDetails(camera_name, vehicle_name, external)); return RpcLibAdaptorsBase::DetectionInfo::from(response); }); @@ -269,24 +269,24 @@ namespace airlib }); pimpl_->server.bind("simGetCameraInfo", [&](const std::string& camera_name, const std::string& vehicle_name, bool external) -> RpcLibAdaptorsBase::CameraInfo { - const auto& camera_info = getWorldSimApi()->getCameraInfo(camera_name, vehicle_name, external); + const auto& camera_info = getWorldSimApi()->getCameraInfo(CameraDetails(camera_name, vehicle_name, external)); return RpcLibAdaptorsBase::CameraInfo(camera_info); }); pimpl_->server.bind("simSetDistortionParam", [&](const std::string& camera_name, const std::string& param_name, float value, const std::string& vehicle_name, bool external) -> void { - getWorldSimApi()->setDistortionParam(camera_name, param_name, value, vehicle_name, external); + getWorldSimApi()->setDistortionParam(param_name, value, CameraDetails(camera_name, vehicle_name, external)); }); pimpl_->server.bind("simGetDistortionParams", [&](const std::string& camera_name, const std::string& vehicle_name, bool external) -> std::vector { - return getWorldSimApi()->getDistortionParams(camera_name, vehicle_name, external); + return getWorldSimApi()->getDistortionParams(CameraDetails(camera_name, vehicle_name, external)); }); pimpl_->server.bind("simSetCameraPose", [&](const std::string& camera_name, const RpcLibAdaptorsBase::Pose& pose, const std::string& vehicle_name, bool external) -> void { - getWorldSimApi()->setCameraPose(camera_name, pose.to(), vehicle_name, external); + getWorldSimApi()->setCameraPose(pose.to(), CameraDetails(camera_name, vehicle_name, external)); }); pimpl_->server.bind("simSetCameraFov", [&](const std::string& camera_name, float fov_degrees, const std::string& vehicle_name, bool external) -> void { - getWorldSimApi()->setCameraFoV(camera_name, fov_degrees, vehicle_name, external); + getWorldSimApi()->setCameraFoV(fov_degrees, CameraDetails(camera_name, vehicle_name, external)); }); pimpl_->server.bind("simGetCollisionInfo", [&](const std::string& vehicle_name) -> RpcLibAdaptorsBase::CollisionInfo { diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp index e430010609..9148995fa5 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp @@ -266,43 +266,40 @@ std::vector WorldSimApi::getWorldExtents() const return result; } -msr::airlib::CameraInfo WorldSimApi::getCameraInfo(const std::string& camera_name, const std::string& vehicle_name, bool external) const +msr::airlib::CameraInfo WorldSimApi::getCameraInfo(const CameraDetails& camera_details) const { - if (external) + if (camera_details.external) throw std::invalid_argument(common_utils::Utils::stringf("external field is not supported on Unity Image APIs").c_str()); - AirSimCameraInfo airsim_camera_info = GetCameraInfo(camera_name.c_str(), vehicle_name.c_str()); // Into Unity + AirSimCameraInfo airsim_camera_info = GetCameraInfo(camera_details.camera_name.c_str(), camera_details.vehicle_name.c_str()); // Into Unity msr::airlib::CameraInfo camera_info; camera_info.pose = UnityUtilities::Convert_to_Pose(airsim_camera_info.pose); camera_info.fov = airsim_camera_info.fov; return camera_info; } -void WorldSimApi::setCameraPose(const std::string& camera_name, const msr::airlib::Pose& pose, - const std::string& vehicle_name, bool external) +void WorldSimApi::setCameraPose(const msr::airlib::Pose& pose, const CameraDetails& camera_details) { - if (external) + if (camera_details.external) throw std::invalid_argument(common_utils::Utils::stringf("external field is not supported on Unity Image APIs").c_str()); - SetCameraPose(camera_name.c_str(), UnityUtilities::Convert_to_AirSimPose(pose), vehicle_name.c_str()); + SetCameraPose(camera_details.camera_name.c_str(), UnityUtilities::Convert_to_AirSimPose(pose), camera_details.vehicle_name.c_str()); } -void WorldSimApi::setCameraFoV(const std::string& camera_name, float fov_degrees, - const std::string& vehicle_name, bool external) +void WorldSimApi::setCameraFoV(float fov_degrees, const CameraDetails& camera_details) { - if (external) + if (camera_details.external) throw std::invalid_argument(common_utils::Utils::stringf("external field is not supported on Unity Image APIs").c_str()); - SetCameraFoV(camera_name.c_str(), fov_degrees, vehicle_name.c_str()); + SetCameraFoV(camera_details.camera_name.c_str(), fov_degrees, camera_details.vehicle_name.c_str()); } -void WorldSimApi::setDistortionParam(const std::string& camera_name, const std::string& param_name, float value, - const std::string& vehicle_name, bool external) +void WorldSimApi::setDistortionParam(const std::string& param_name, float value, const CameraDetails& camera_details) { throw std::invalid_argument(common_utils::Utils::stringf("setDistortionParam is not supported on unity").c_str()); } -std::vector WorldSimApi::getDistortionParams(const std::string& camera_name, const std::string& vehicle_name, bool external) const +std::vector WorldSimApi::getDistortionParams(const CameraDetails& camera_details) const { throw std::invalid_argument(common_utils::Utils::stringf("getDistortionParams is not supported on unity").c_str()); @@ -330,10 +327,9 @@ std::vector WorldSimApi::getImage(const std::string& camera_name, Image return std::vector(); } -void WorldSimApi::addDetectionFilterMeshName(const std::string& camera_name, ImageCaptureBase::ImageType image_type, const std::string& mesh_name, - const std::string& vehicle_name, bool external) +void WorldSimApi::addDetectionFilterMeshName(ImageCaptureBase::ImageType image_type, const std::string& mesh_name, const CameraDetails& camera_details) { - unused(camera_name); + unused(camera_details); unused(image_type); unused(mesh_name); @@ -342,37 +338,31 @@ void WorldSimApi::addDetectionFilterMeshName(const std::string& camera_name, Ima .c_str()); } -void WorldSimApi::setDetectionFilterRadius(const std::string& camera_name, ImageCaptureBase::ImageType image_type, float radius_cm, - const std::string& vehicle_name, bool external) +void WorldSimApi::setDetectionFilterRadius(ImageCaptureBase::ImageType image_type, float radius_cm, const CameraDetails& camera_details) { - unused(camera_name); + unused(camera_details); unused(image_type); unused(radius_cm); - unused(external); throw std::invalid_argument(common_utils::Utils::stringf( "setDetectionFilterRadius is not supported on unity") .c_str()); } -void WorldSimApi::clearDetectionMeshNames(const std::string& camera_name, ImageCaptureBase::ImageType image_type, - const std::string& vehicle_name, bool external) +void WorldSimApi::clearDetectionMeshNames(ImageCaptureBase::ImageType image_type, const CameraDetails& camera_details) { - unused(camera_name); + unused(camera_details); unused(image_type); - unused(external); throw std::invalid_argument(common_utils::Utils::stringf( "clearDetectionMeshNames is not supported on unity") .c_str()); } -std::vector WorldSimApi::getDetections(const std::string& camera_name, ImageCaptureBase::ImageType image_type, - const std::string& vehicle_name, bool external) +std::vector WorldSimApi::getDetections(ImageCaptureBase::ImageType image_type, const CameraDetails& camera_details) { - unused(camera_name); + unused(camera_details); unused(image_type); - unused(external); throw std::invalid_argument(common_utils::Utils::stringf( "getDetections is not supported on unity") diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h index c22ddba94a..e2efd2c52e 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h @@ -11,6 +11,7 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase typedef msr::airlib::Vector3r Vector3r; typedef msr::airlib::MeshPositionVertexBuffersResponse MeshPositionVertexBuffersResponse; typedef msr::airlib::ImageCaptureBase ImageCaptureBase; + typedef msr::airlib::CameraDetails CameraDetails; WorldSimApi(SimModeBase* simmode, std::string vehicle_name); virtual ~WorldSimApi(); @@ -74,29 +75,21 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase virtual std::vector getWorldExtents() const override; // Camera APIs - virtual msr::airlib::CameraInfo getCameraInfo(const std::string& camera_name, const std::string& vehicle_name = "", bool external = false) const override; - virtual void setCameraPose(const std::string& camera_name, const msr::airlib::Pose& pose, - const std::string& vehicle_name = "", bool external = false) override; - virtual void setCameraFoV(const std::string& camera_name, float fov_degrees, - const std::string& vehicle_name = "", bool external = false) override; - virtual void setDistortionParam(const std::string& camera_name, const std::string& param_name, float value, - const std::string& vehicle_name = "", bool external = false) override; - virtual std::vector getDistortionParams(const std::string& camera_name, const std::string& vehicle_name = "", - bool external = false) const override; + virtual msr::airlib::CameraInfo getCameraInfo(const CameraDetails& camera_details) const override; + virtual void setCameraPose(const msr::airlib::Pose& pose, const CameraDetails& camera_details) override; + virtual void setCameraFoV(float fov_degrees, const CameraDetails& camera_details) override; + virtual void setDistortionParam(const std::string& param_name, float value, const CameraDetails& camera_details) override; + virtual std::vector getDistortionParams(const CameraDetails& camera_details) const override; virtual std::vector getImages(const std::vector& requests, const std::string& vehicle_name = "", bool external = false) const override; virtual std::vector getImage(const std::string& camera_name, msr::airlib::ImageCaptureBase::ImageType image_type, const std::string& vehicle_name = "", bool external = false) const override; - virtual void addDetectionFilterMeshName(const std::string& camera_name, ImageCaptureBase::ImageType image_type, const std::string& mesh_name, - const std::string& vehicle_name = "", bool external = false) override; - virtual void setDetectionFilterRadius(const std::string& camera_name, ImageCaptureBase::ImageType image_type, float radius_cm, - const std::string& vehicle_name = "", bool external = false) override; - virtual void clearDetectionMeshNames(const std::string& camera_name, ImageCaptureBase::ImageType image_type, - const std::string& vehicle_name = "", bool external = false) override; - virtual std::vector getDetections(const std::string& camera_name, ImageCaptureBase::ImageType image_type, - const std::string& vehicle_name = "", bool external = false) override; + virtual void addDetectionFilterMeshName(ImageCaptureBase::ImageType image_type, const std::string& mesh_name, const CameraDetails& camera_details) override; + virtual void setDetectionFilterRadius(ImageCaptureBase::ImageType image_type, float radius_cm, const CameraDetails& camera_details) override; + virtual void clearDetectionMeshNames(ImageCaptureBase::ImageType image_type, const CameraDetails& camera_details) override; + virtual std::vector getDetections(ImageCaptureBase::ImageType image_type, const CameraDetails& camera_details) override; private: SimModeBase* simmode_; diff --git a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp index f6164ce9e8..71000329c0 100644 --- a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp +++ b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp @@ -333,7 +333,7 @@ void ASimHUD::initializeSubWindows() } for (const auto& setting : getSubWindowSettings()) { - auto camera = simmode_->getCamera(setting.camera_name, setting.vehicle_name, setting.external); + APIPCamera* camera = simmode_->getCamera(msr::airlib::CameraDetails(setting.camera_name, setting.vehicle_name, setting.external)); if (camera) subwindow_cameras_[setting.window_index] = camera; else diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index 8d9494a8d5..7502bee85b 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -511,9 +511,10 @@ bool ASimModeBase::isRecording() const return FRecordingThread::isRecording(); } -const APIPCamera* ASimModeBase::getCamera(const std::string& camera_name, const std::string& vehicle_name, bool external) const +const APIPCamera* ASimModeBase::getCamera(const msr::airlib::CameraDetails& camera_details) const { - return external ? getExternalCamera(camera_name) : getVehicleSimApi(vehicle_name)->getCamera(camera_name); + return camera_details.external ? getExternalCamera(camera_details.camera_name) + : getVehicleSimApi(camera_details.vehicle_name)->getCamera(camera_details.camera_name); } const UnrealImageCapture* ASimModeBase::getImageCapture(const std::string& vehicle_name, bool external) const diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h index 022763e067..756f227d34 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h @@ -104,10 +104,11 @@ class AIRSIM_API ASimModeBase : public AActor static_cast(this)->getExternalCamera(camera_name)); } - APIPCamera* getCamera(const std::string& camera_name, const std::string& vehicle_name = "", bool external = false) + const APIPCamera* getCamera(const msr::airlib::CameraDetails& camera_details) const; + + APIPCamera* getCamera(const msr::airlib::CameraDetails& camera_details) { - return const_cast( - static_cast(this)->getCamera(camera_name, vehicle_name, external)); + return const_cast(static_cast(this)->getCamera(camera_details)); } const UnrealImageCapture* getExternalImageCapture() const @@ -115,7 +116,6 @@ class AIRSIM_API ASimModeBase : public AActor return external_image_capture_.get(); } - const APIPCamera* getCamera(const std::string& camera_name, const std::string& vehicle_name = "", bool external = false) const; const UnrealImageCapture* getImageCapture(const std::string& vehicle_name = "", bool external = false) const; TMap asset_map; diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index de623dbfe3..49c61cc767 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -731,10 +731,10 @@ std::vector WorldSimApi::getWorldExtents() const return result; } -msr::airlib::CameraInfo WorldSimApi::getCameraInfo(const std::string& camera_name, const std::string& vehicle_name, bool external) const +msr::airlib::CameraInfo WorldSimApi::getCameraInfo(const CameraDetails& camera_details) const { msr::airlib::CameraInfo info; - const auto* camera = simmode_->getCamera(camera_name, vehicle_name, external); + const APIPCamera* camera = simmode_->getCamera(camera_details); UAirBlueprintLib::RunCommandOnGameThread([camera, &info]() { info = camera->getCameraInfo(); }, @@ -743,40 +743,37 @@ msr::airlib::CameraInfo WorldSimApi::getCameraInfo(const std::string& camera_nam return info; } -void WorldSimApi::setCameraPose(const std::string& camera_name, const msr::airlib::Pose& pose, - const std::string& vehicle_name, bool external) +void WorldSimApi::setCameraPose(const msr::airlib::Pose& pose, const CameraDetails& camera_details) { - auto* camera = simmode_->getCamera(camera_name, vehicle_name, external); + APIPCamera* camera = simmode_->getCamera(camera_details); UAirBlueprintLib::RunCommandOnGameThread([camera, &pose]() { camera->setCameraPose(pose); }, true); } -void WorldSimApi::setCameraFoV(const std::string& camera_name, float fov_degrees, - const std::string& vehicle_name, bool external) +void WorldSimApi::setCameraFoV(float fov_degrees, const CameraDetails& camera_details) { - auto* camera = simmode_->getCamera(camera_name, vehicle_name, external); + APIPCamera* camera = simmode_->getCamera(camera_details); UAirBlueprintLib::RunCommandOnGameThread([camera, &fov_degrees]() { camera->setCameraFoV(fov_degrees); }, true); } -void WorldSimApi::setDistortionParam(const std::string& camera_name, const std::string& param_name, float value, - const std::string& vehicle_name, bool external) +void WorldSimApi::setDistortionParam(const std::string& param_name, float value, const CameraDetails& camera_details) { - auto* camera = simmode_->getCamera(camera_name, vehicle_name, external); + APIPCamera* camera = simmode_->getCamera(camera_details); UAirBlueprintLib::RunCommandOnGameThread([camera, ¶m_name, &value]() { camera->setDistortionParam(param_name, value); }, true); } -std::vector WorldSimApi::getDistortionParams(const std::string& camera_name, const std::string& vehicle_name, bool external) const +std::vector WorldSimApi::getDistortionParams(const CameraDetails& camera_details) const { std::vector param_values; - const auto* camera = simmode_->getCamera(camera_name, vehicle_name, external); + const APIPCamera* camera = simmode_->getCamera(camera_details); UAirBlueprintLib::RunCommandOnGameThread([camera, ¶m_values]() { param_values = camera->getDistortionParams(); }, @@ -790,7 +787,7 @@ std::vector WorldSimApi::getImages { std::vector responses; - const auto* camera = simmode_->getImageCapture(vehicle_name, external); + const UnrealImageCapture* camera = simmode_->getImageCapture(vehicle_name, external); camera->getImages(requests, responses); return responses; @@ -807,10 +804,9 @@ std::vector WorldSimApi::getImage(const std::string& camera_name, Image return std::vector(); } -void WorldSimApi::addDetectionFilterMeshName(const std::string& camera_name, ImageCaptureBase::ImageType image_type, const std::string& mesh_name, - const std::string& vehicle_name, bool external) +void WorldSimApi::addDetectionFilterMeshName(ImageCaptureBase::ImageType image_type, const std::string& mesh_name, const CameraDetails& camera_details) { - const APIPCamera* camera = simmode_->getCamera(camera_name, vehicle_name, external); + const APIPCamera* camera = simmode_->getCamera(camera_details); UAirBlueprintLib::RunCommandOnGameThread([camera, image_type, &mesh_name]() { camera->getDetectionComponent(image_type, false)->addMeshName(mesh_name); @@ -818,10 +814,9 @@ void WorldSimApi::addDetectionFilterMeshName(const std::string& camera_name, Ima true); } -void WorldSimApi::setDetectionFilterRadius(const std::string& camera_name, ImageCaptureBase::ImageType image_type, float radius_cm, - const std::string& vehicle_name, bool external) +void WorldSimApi::setDetectionFilterRadius(ImageCaptureBase::ImageType image_type, float radius_cm, const CameraDetails& camera_details) { - const APIPCamera* camera = simmode_->getCamera(camera_name, vehicle_name, external); + const APIPCamera* camera = simmode_->getCamera(camera_details); UAirBlueprintLib::RunCommandOnGameThread([camera, image_type, radius_cm]() { camera->getDetectionComponent(image_type, false)->setFilterRadius(radius_cm); @@ -829,10 +824,9 @@ void WorldSimApi::setDetectionFilterRadius(const std::string& camera_name, Image true); } -void WorldSimApi::clearDetectionMeshNames(const std::string& camera_name, ImageCaptureBase::ImageType image_type, - const std::string& vehicle_name, bool external) +void WorldSimApi::clearDetectionMeshNames(ImageCaptureBase::ImageType image_type, const CameraDetails& camera_details) { - const APIPCamera* camera = simmode_->getCamera(camera_name, vehicle_name, external); + const APIPCamera* camera = simmode_->getCamera(camera_details); UAirBlueprintLib::RunCommandOnGameThread([camera, image_type]() { camera->getDetectionComponent(image_type, false)->clearMeshNames(); @@ -840,14 +834,14 @@ void WorldSimApi::clearDetectionMeshNames(const std::string& camera_name, ImageC true); } -std::vector WorldSimApi::getDetections(const std::string& camera_name, ImageCaptureBase::ImageType image_type, - const std::string& vehicle_name, bool external) +std::vector WorldSimApi::getDetections(ImageCaptureBase::ImageType image_type, const CameraDetails& camera_details) { std::vector result; - const APIPCamera* camera = simmode_->getCamera(camera_name, vehicle_name, external); - const NedTransform& ned_transform = external ? simmode_->getGlobalNedTransform() - : simmode_->getVehicleSimApi(vehicle_name)->getNedTransform(); + const APIPCamera* camera = simmode_->getCamera(camera_details); + const NedTransform& ned_transform = camera_details.external + ? simmode_->getGlobalNedTransform() + : simmode_->getVehicleSimApi(camera_details.vehicle_name)->getNedTransform(); UAirBlueprintLib::RunCommandOnGameThread([camera, image_type, &result, &ned_transform]() { const TArray& detections = camera->getDetectionComponent(image_type, false)->getDetections(); diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.h b/Unreal/Plugins/AirSim/Source/WorldSimApi.h index 48cfc00e2d..b6bbc40d24 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.h +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.h @@ -17,6 +17,7 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase typedef msr::airlib::Vector3r Vector3r; typedef msr::airlib::MeshPositionVertexBuffersResponse MeshPositionVertexBuffersResponse; typedef msr::airlib::ImageCaptureBase ImageCaptureBase; + typedef msr::airlib::CameraDetails CameraDetails; WorldSimApi(ASimModeBase* simmode); virtual ~WorldSimApi() = default; @@ -80,29 +81,21 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase virtual std::vector getWorldExtents() const override; // Camera APIs - virtual msr::airlib::CameraInfo getCameraInfo(const std::string& camera_name, const std::string& vehicle_name = "", bool external = false) const override; - virtual void setCameraPose(const std::string& camera_name, const msr::airlib::Pose& pose, - const std::string& vehicle_name = "", bool external = false) override; - virtual void setCameraFoV(const std::string& camera_name, float fov_degrees, - const std::string& vehicle_name = "", bool external = false) override; - virtual void setDistortionParam(const std::string& camera_name, const std::string& param_name, float value, - const std::string& vehicle_name = "", bool external = false) override; - virtual std::vector getDistortionParams(const std::string& camera_name, const std::string& vehicle_name = "", - bool external = false) const override; + virtual msr::airlib::CameraInfo getCameraInfo(const CameraDetails& camera_details) const override; + virtual void setCameraPose(const msr::airlib::Pose& pose, const CameraDetails& camera_details) override; + virtual void setCameraFoV(float fov_degrees, const CameraDetails& camera_details) override; + virtual void setDistortionParam(const std::string& param_name, float value, const CameraDetails& camera_details) override; + virtual std::vector getDistortionParams(const CameraDetails& camera_details) const override; virtual std::vector getImages(const std::vector& requests, const std::string& vehicle_name = "", bool external = false) const override; virtual std::vector getImage(const std::string& camera_name, ImageCaptureBase::ImageType image_type, const std::string& vehicle_name = "", bool external = false) const override; - virtual void addDetectionFilterMeshName(const std::string& camera_name, ImageCaptureBase::ImageType image_type, const std::string& mesh_name, - const std::string& vehicle_name = "", bool external = false) override; - virtual void setDetectionFilterRadius(const std::string& camera_name, ImageCaptureBase::ImageType image_type, float radius_cm, - const std::string& vehicle_name = "", bool external = false) override; - virtual void clearDetectionMeshNames(const std::string& camera_name, ImageCaptureBase::ImageType image_type, - const std::string& vehicle_name = "", bool external = false) override; - virtual std::vector getDetections(const std::string& camera_name, ImageCaptureBase::ImageType image_type, - const std::string& vehicle_name = "", bool external = false) override; + virtual void addDetectionFilterMeshName(ImageCaptureBase::ImageType image_type, const std::string& mesh_name, const CameraDetails& camera_details) override; + virtual void setDetectionFilterRadius(ImageCaptureBase::ImageType image_type, float radius_cm, const CameraDetails& camera_details) override; + virtual void clearDetectionMeshNames(ImageCaptureBase::ImageType image_type, const CameraDetails& camera_details) override; + virtual std::vector getDetections(ImageCaptureBase::ImageType image_type, const CameraDetails& camera_details) override; private: AActor* createNewActor(const FActorSpawnParameters& spawn_params, const FTransform& actor_transform, const Vector3r& scale, UStaticMesh* static_mesh); From 49bfda36bdda0d13182d58109bc23ae0237b5aee Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Fri, 2 Jul 2021 00:18:39 +0530 Subject: [PATCH 24/29] Fix missing call to parse External Camera settings --- AirLib/include/common/AirSimSettings.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index eb15c27106..a54334bc9f 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/AirLib/include/common/AirSimSettings.hpp @@ -435,6 +435,7 @@ namespace airlib loadOtherSettings(settings_json); loadDefaultSensorSettings(simmode_name, settings_json, sensor_defaults); loadVehicleSettings(simmode_name, settings_json, vehicles, sensor_defaults); + loadExternalCameraSettings(settings_json, external_cameras); //this should be done last because it depends on vehicles (and/or their type) we have loadRecordingSetting(settings_json); From 991d35c1de28cafca7ad5fed3eb33385c462aa78 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Fri, 2 Jul 2021 00:20:01 +0530 Subject: [PATCH 25/29] Update C++ Image APIs to include external field --- AirLib/include/api/RpcLibClientBase.hpp | 4 ++-- AirLib/src/api/RpcLibClientBase.cpp | 9 +++++---- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index 8bd3af9c04..9262120796 100644 --- a/AirLib/include/api/RpcLibClientBase.hpp +++ b/AirLib/include/api/RpcLibClientBase.hpp @@ -108,8 +108,8 @@ namespace airlib void simSetVehiclePose(const Pose& pose, bool ignore_collision, const std::string& vehicle_name = ""); void simSetTraceLine(const std::vector& color_rgba, float thickness = 3.0f, const std::string& vehicle_name = ""); - vector simGetImages(vector request, const std::string& vehicle_name = ""); - vector simGetImage(const std::string& camera_name, ImageCaptureBase::ImageType type, const std::string& vehicle_name = ""); + vector simGetImages(vector request, const std::string& vehicle_name = "", bool external = false); + vector simGetImage(const std::string& camera_name, ImageCaptureBase::ImageType type, const std::string& vehicle_name = "", bool external = false); bool simTestLineOfSightToPoint(const msr::airlib::GeoPoint& point, const std::string& vehicle_name = ""); bool simTestLineOfSightBetweenPoints(const msr::airlib::GeoPoint& point1, const msr::airlib::GeoPoint& point2); diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index 06921206a9..294ed98a6d 100644 --- a/AirLib/src/api/RpcLibClientBase.cpp +++ b/AirLib/src/api/RpcLibClientBase.cpp @@ -251,18 +251,19 @@ __pragma(warning(disable : 4239)) pimpl_->client.call("simSetTraceLine", color_rgba, thickness, vehicle_name); } - vector RpcLibClientBase::simGetImages(vector request, const std::string& vehicle_name) + vector RpcLibClientBase::simGetImages(vector request, const std::string& vehicle_name, bool external) { const auto& response_adaptor = pimpl_->client.call("simGetImages", RpcLibAdaptorsBase::ImageRequest::from(request), - vehicle_name) + vehicle_name, + external) .as>(); return RpcLibAdaptorsBase::ImageResponse::to(response_adaptor); } - vector RpcLibClientBase::simGetImage(const std::string& camera_name, ImageCaptureBase::ImageType type, const std::string& vehicle_name) + vector RpcLibClientBase::simGetImage(const std::string& camera_name, ImageCaptureBase::ImageType type, const std::string& vehicle_name, bool external) { - vector result = pimpl_->client.call("simGetImage", camera_name, type, vehicle_name).as>(); + vector result = pimpl_->client.call("simGetImage", camera_name, type, vehicle_name, external).as>(); if (result.size() == 1) { // rpclib has a bug with serializing empty vectors, so we return a 1 byte vector instead. result.clear(); From ba3bc1c8ad67b17bacb9c0b00d35ba608eba9157 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Fri, 2 Jul 2021 00:21:07 +0530 Subject: [PATCH 26/29] Update getImage API to use CameraDetails Also fix missing vehicle, external field --- AirLib/include/api/WorldSimApiBase.hpp | 5 ++--- AirLib/src/api/RpcLibServerBase.cpp | 2 +- .../AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp | 10 ++++++---- Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h | 5 ++--- Unreal/Plugins/AirSim/Source/WorldSimApi.cpp | 10 ++++++---- Unreal/Plugins/AirSim/Source/WorldSimApi.h | 5 ++--- 6 files changed, 19 insertions(+), 18 deletions(-) diff --git a/AirLib/include/api/WorldSimApiBase.hpp b/AirLib/include/api/WorldSimApiBase.hpp index bd35143d98..fdd1d184b7 100644 --- a/AirLib/include/api/WorldSimApiBase.hpp +++ b/AirLib/include/api/WorldSimApiBase.hpp @@ -97,9 +97,8 @@ namespace airlib virtual std::vector getDistortionParams(const CameraDetails& camera_details) const = 0; virtual std::vector getImages(const std::vector& requests, - const std::string& vehicle_name = "", bool external = false) const = 0; - virtual std::vector getImage(const std::string& camera_name, ImageCaptureBase::ImageType image_type, - const std::string& vehicle_name = "", bool external = false) const = 0; + const std::string& vehicle_name, bool external) const = 0; + virtual std::vector getImage(ImageCaptureBase::ImageType image_type, const CameraDetails& camera_details) const = 0; virtual void addDetectionFilterMeshName(ImageCaptureBase::ImageType image_type, const std::string& mesh_name, const CameraDetails& camera_details) = 0; virtual void setDetectionFilterRadius(ImageCaptureBase::ImageType image_type, float radius_cm, const CameraDetails& camera_details) = 0; diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index ef1d3a8fd9..b566001a8d 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -150,7 +150,7 @@ namespace airlib }); pimpl_->server.bind("simGetImage", [&](const std::string& camera_name, ImageCaptureBase::ImageType type, const std::string& vehicle_name, bool external) -> vector { - return getWorldSimApi()->getImage(camera_name, type, vehicle_name, external); + return getWorldSimApi()->getImage(type, CameraDetails(camera_name, vehicle_name, external)); }); pimpl_->server.bind("simTestLineOfSightToPoint", [&](const RpcLibAdaptorsBase::GeoPoint& point, const std::string& vehicle_name) -> bool { diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp index 9148995fa5..66a7e021b3 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp @@ -316,11 +316,13 @@ std::vector WorldSimApi::getImages return responses; } -std::vector WorldSimApi::getImage(const std::string& camera_name, ImageCaptureBase::ImageType image_type, - const std::string& vehicle_name, bool external) const +std::vector WorldSimApi::getImage(ImageCaptureBase::ImageType image_type, const CameraDetails& camera_details) const { - std::vector request{ ImageCaptureBase::ImageRequest(camera_name, image_type) }; - const auto& response = getImages(request); + std::vector request{ + ImageCaptureBase::ImageRequest(camera_details.camera_name, image_type) + }; + + const auto& response = getImages(request, camera_details.vehicle_name, camera_details.external); if (response.size() > 0) return response.at(0).image_data_uint8; else diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h index e2efd2c52e..02f7b64881 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h @@ -82,9 +82,8 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase virtual std::vector getDistortionParams(const CameraDetails& camera_details) const override; virtual std::vector getImages(const std::vector& requests, - const std::string& vehicle_name = "", bool external = false) const override; - virtual std::vector getImage(const std::string& camera_name, msr::airlib::ImageCaptureBase::ImageType image_type, - const std::string& vehicle_name = "", bool external = false) const override; + const std::string& vehicle_name, bool external) const override; + virtual std::vector getImage(ImageCaptureBase::ImageType image_type, const CameraDetails& camera_details) const override; virtual void addDetectionFilterMeshName(ImageCaptureBase::ImageType image_type, const std::string& mesh_name, const CameraDetails& camera_details) override; virtual void setDetectionFilterRadius(ImageCaptureBase::ImageType image_type, float radius_cm, const CameraDetails& camera_details) override; diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index 49c61cc767..d1f1b2f001 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -793,11 +793,13 @@ std::vector WorldSimApi::getImages return responses; } -std::vector WorldSimApi::getImage(const std::string& camera_name, ImageCaptureBase::ImageType image_type, - const std::string& vehicle_name, bool external) const +std::vector WorldSimApi::getImage(ImageCaptureBase::ImageType image_type, const CameraDetails& camera_details) const { - std::vector request{ ImageCaptureBase::ImageRequest(camera_name, image_type) }; - const auto& response = getImages(request); + std::vector request{ + ImageCaptureBase::ImageRequest(camera_details.camera_name, image_type) + }; + + const auto& response = getImages(request, camera_details.vehicle_name, camera_details.external); if (response.size() > 0) return response.at(0).image_data_uint8; else diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.h b/Unreal/Plugins/AirSim/Source/WorldSimApi.h index b6bbc40d24..5e38548787 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.h +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.h @@ -88,9 +88,8 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase virtual std::vector getDistortionParams(const CameraDetails& camera_details) const override; virtual std::vector getImages(const std::vector& requests, - const std::string& vehicle_name = "", bool external = false) const override; - virtual std::vector getImage(const std::string& camera_name, ImageCaptureBase::ImageType image_type, - const std::string& vehicle_name = "", bool external = false) const override; + const std::string& vehicle_name, bool external) const override; + virtual std::vector getImage(ImageCaptureBase::ImageType image_type, const CameraDetails& camera_details) const override; virtual void addDetectionFilterMeshName(ImageCaptureBase::ImageType image_type, const std::string& mesh_name, const CameraDetails& camera_details) override; virtual void setDetectionFilterRadius(ImageCaptureBase::ImageType image_type, float radius_cm, const CameraDetails& camera_details) override; From 5b310e1821123fff548e31531ee97e89c2523117 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Fri, 2 Jul 2021 00:22:08 +0530 Subject: [PATCH 27/29] Add CameraDetails::to_string helper method --- AirLib/include/common/CommonStructs.hpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/AirLib/include/common/CommonStructs.hpp b/AirLib/include/common/CommonStructs.hpp index ba7bbc564d..bda9f85840 100644 --- a/AirLib/include/common/CommonStructs.hpp +++ b/AirLib/include/common/CommonStructs.hpp @@ -434,6 +434,14 @@ namespace airlib : camera_name(camera_name_val), vehicle_name(vehicle_name_val), external(external_val) { } + + std::string to_string() const + { + return Utils::stringf("CameraDetails: camera_name=%s, vehicle_name=%s, external=%d", + camera_name.c_str(), + vehicle_name.c_str(), + external); + } }; } } //namespace From d6becacc6041430af54c946f02de2da2488f86e9 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Fri, 2 Jul 2021 00:22:37 +0530 Subject: [PATCH 28/29] Update external_camera.py script --- PythonClient/computer_vision/external_camera.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/PythonClient/computer_vision/external_camera.py b/PythonClient/computer_vision/external_camera.py index de1b74524c..b41a072cba 100644 --- a/PythonClient/computer_vision/external_camera.py +++ b/PythonClient/computer_vision/external_camera.py @@ -1,9 +1,10 @@ +import setup_path import airsim import os import tempfile """ -A simple script to test all the camera APIs. Change the cmaera name and whether it's an external camera +A simple script to test all the camera APIs. Change the camera name and whether it's an external camera Example Settings for external camera - @@ -45,7 +46,7 @@ airsim.wait_key('Press any key to get images') requests = [airsim.ImageRequest(CAM_NAME, airsim.ImageType.Scene), - airsim.ImageRequest(CAM_NAME, airsim.ImageType.DepthPlanner), + airsim.ImageRequest(CAM_NAME, airsim.ImageType.DepthPlanar), airsim.ImageRequest(CAM_NAME, airsim.ImageType.DepthVis), airsim.ImageRequest(CAM_NAME, airsim.ImageType.Segmentation), airsim.ImageRequest(CAM_NAME, airsim.ImageType.SurfaceNormals)] From 79ee34204efb0e5f6a26b32347cb40115e7c6814 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Sat, 24 Jul 2021 08:03:40 +0530 Subject: [PATCH 29/29] Update docs after review --- docs/image_apis.md | 3 +++ docs/settings.md | 7 ++++--- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/docs/image_apis.md b/docs/image_apis.md index 69126c4495..21158d3c3a 100644 --- a/docs/image_apis.md +++ b/docs/image_apis.md @@ -133,6 +133,9 @@ For a more complete ready to run sample code please see [sample code in HelloDro See also [other example code](https://github.com/Microsoft/AirSim/tree/master/Examples/DataCollection/StereoImageGenerator.hpp) that generates specified number of stereo images along with ground truth depth and disparity and saving it to [pfm format](pfm.md). ## Available Cameras + +These are the default cameras already available in each vehicle. Apart from these, you can add more cameras to the vehicles and external cameras which are not attached to any vehicle through the [settings](settings.md). + ### Car The cameras on car can be accessed by following names in API calls: `front_center`, `front_right`, `front_left`, `fpv` and `back_center`. Here FPV camera is driver's head position in the car. ### Multirotor diff --git a/docs/settings.md b/docs/settings.md index b819de129a..3a9c8fee99 100644 --- a/docs/settings.md +++ b/docs/settings.md @@ -229,9 +229,9 @@ This setting specifies the latitude, longitude and altitude of the Player Start This setting determines what is shown in each of 3 subwindows which are visible when you press 1,2,3 keys. * `WindowID`: Can be 0 to 2 -* `CameraName` is any [available camera](image_apis.md#available_cameras) on the vehicle. -* `ImageType` integer value determines what kind of image gets shown according to [ImageType enum](image_apis.md#available-imagetype). -* `VehicleName` string allows you to specify the vehicle to use the camera from, used when multiple vehicles are specified in the settings. First vehicle's camera will be used if there are any mistakes such as incorrect vehicle name, or only a single vehicle. +* `CameraName`: is any [available camera](image_apis.md#available-cameras) on the vehicle or external camera +* `ImageType`: integer value determines what kind of image gets shown according to [ImageType enum](image_apis.md#available-imagetype-values). +* `VehicleName`: string allows you to specify the vehicle to use the camera from, used when multiple vehicles are specified in the settings. First vehicle's camera will be used if there are any mistakes such as incorrect vehicle name, or only a single vehicle. * `External`: Set it to `true` if the camera is an external camera. If true, then the `VehicleName` parameter is ignored For example, for a single car vehicle, below shows driver view, front bumper view and rear view as scene, depth and surface normals respectively. @@ -264,6 +264,7 @@ The recording feature allows you to record data such as position, orientation, v * When `PixelsAsFloat` is true, image is saved as [pfm](pfm.md) file instead of png file. * `VehicleName` option allows you to specify separate cameras for individual vehicles. If the `Cameras` element isn't present, `Scene` image from the default camera of each vehicle will be recorded. * If you don't want to record any images and just the vehicle's physics data, then specify the `Cameras` element but leave it empty, like this: `"Cameras": []` + * External cameras are currently not supported in recording For example, the `Cameras` element below records scene & segmentation images for `Car1` & scene for `Car2`-