From 589c708e0ca616a2abf026a1c5577342250a7912 Mon Sep 17 00:00:00 2001 From: Kerry Moffitt Date: Sun, 28 Apr 2019 10:05:13 -0400 Subject: [PATCH 01/29] Support for adding vehicles dynamically at run-time via RPC --- AirLib/include/api/RpcLibClientBase.hpp | 3 +- AirLib/include/api/WorldSimApiBase.hpp | 4 ++ AirLib/include/common/AirSimSettings.hpp | 9 +++ AirLib/include/physics/PhysicsWorld.hpp | 7 ++ AirLib/src/api/RpcLibClientBase.cpp | 13 ++-- AirLib/src/api/RpcLibServerBase.cpp | 41 ++++++++--- .../AirSim/Source/SimMode/SimModeBase.cpp | 68 +++++++++++++++++++ .../AirSim/Source/SimMode/SimModeBase.h | 3 + .../Source/SimMode/SimModeWorldBase.cpp | 6 ++ .../AirSim/Source/SimMode/SimModeWorldBase.h | 3 + Unreal/Plugins/AirSim/Source/WorldSimApi.cpp | 16 +++++ Unreal/Plugins/AirSim/Source/WorldSimApi.h | 2 + 12 files changed, 159 insertions(+), 16 deletions(-) diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index 6c1a30db01..bb97e1164e 100644 --- a/AirLib/include/api/RpcLibClientBase.hpp +++ b/AirLib/include/api/RpcLibClientBase.hpp @@ -69,7 +69,7 @@ class RpcLibClientBase { void simPlotLineStrip(const vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent); void simPlotLineList(const vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent); void simPlotArrows(const vector& points_start, const vector& points_end, const vector& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent); - void simPlotStrings(const vector& strings, const vector& positions, float scale, const vector& color_rgba, float duration); + void simPlotStrings(const vector& strings, const vector& positions, float scale, const vector& color_rgba, float duration); void simPlotTransforms(const vector& poses, float scale, float thickness, float duration, bool is_persistent); void simPlotTransformsWithNames(const vector& poses, const vector& names, float tf_scale, float tf_thickness, float text_scale, const vector& text_color_rgba, float duration); @@ -100,6 +100,7 @@ class RpcLibClientBase { vector simGetImage(const std::string& camera_name, ImageCaptureBase::ImageType type, const std::string& vehicle_name = ""); vector simGetMeshPositionVertexBuffers(); + bool simAddVehicle(const std::string& vehicle_name, const std::string& vehicle_type, const std::string& pawn_path, float north, float east, float down); CollisionInfo simGetCollisionInfo(const std::string& vehicle_name = "") const; diff --git a/AirLib/include/api/WorldSimApiBase.hpp b/AirLib/include/api/WorldSimApiBase.hpp index e98162b306..5cd4c5257c 100644 --- a/AirLib/include/api/WorldSimApiBase.hpp +++ b/AirLib/include/api/WorldSimApiBase.hpp @@ -5,6 +5,8 @@ #define air_WorldSimApiBase_hpp #include "common/CommonStructs.hpp" +#include "common/AirSimSettings.hpp" + namespace msr { namespace airlib { @@ -45,6 +47,8 @@ class WorldSimApiBase { virtual bool setSegmentationObjectID(const std::string& mesh_name, int object_id, bool is_name_regex = false) = 0; virtual int getSegmentationObjectID(const std::string& mesh_name) const = 0; + virtual bool createVehicleAtRuntime(AirSimSettings::VehicleSetting& vehicle_setting) = 0; + virtual void printLogMessage(const std::string& message, const std::string& message_param = "", unsigned char severity = 0) = 0; diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index 34bc71dbb7..4d6553e5df 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/AirLib/include/common/AirSimSettings.hpp @@ -426,6 +426,15 @@ struct AirSimSettings { settings_json.saveJSonFile(settings_filename); } + // This is for the case when a new vehicle is made on the fly, at runtime + void addVehicleSetting(const VehicleSetting &vehicle_setting) + { + std::unique_ptr vehicle_setting_p = std::unique_ptr(new VehicleSetting()); + + // Usually we have a pointer to an entry from the json, but here we have to make a new one + vehicles[vehicle_setting.vehicle_name] = std::move(vehicle_setting_p); + } + const VehicleSetting* getVehicleSetting(const std::string& vehicle_name) const { auto it = vehicles.find(vehicle_name); diff --git a/AirLib/include/physics/PhysicsWorld.hpp b/AirLib/include/physics/PhysicsWorld.hpp index f44e334189..dbf0ec8302 100644 --- a/AirLib/include/physics/PhysicsWorld.hpp +++ b/AirLib/include/physics/PhysicsWorld.hpp @@ -43,6 +43,13 @@ class PhysicsWorld { unlock(); } + void addBody(UpdatableObject* body) + { + lock(); + world_.insert(body); + unlock(); + } + uint64_t getUpdatePeriodNanos() const { return update_period_nanos_; diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index 25a19579dc..664f3141eb 100644 --- a/AirLib/src/api/RpcLibClientBase.cpp +++ b/AirLib/src/api/RpcLibClientBase.cpp @@ -40,7 +40,7 @@ STRICT_MODE_OFF STRICT_MODE_ON #ifdef _MSC_VER __pragma(warning( disable : 4239)) -#endif +#endif namespace msr { namespace airlib { @@ -125,7 +125,7 @@ void RpcLibClientBase::confirmConnection() while (getConnectionState() != RpcLibClientBase::ConnectionState::Connected) { std::cout << "X" << std::flush; - clock->sleep_for(pause_time); + clock->sleep_for(pause_time); } std::cout << std::endl << "Connected!" << std::endl; @@ -133,7 +133,7 @@ void RpcLibClientBase::confirmConnection() auto client_ver = getClientVersion(); auto server_min_ver = getMinRequiredServerVersion(); auto client_min_ver = getMinRequiredClientVersion(); - + std::string ver_info = Utils::stringf("Client Ver:%i (Min Req:%i), Server Ver:%i (Min Req:%i)", client_ver, client_min_ver, server_ver, server_min_ver); @@ -226,7 +226,7 @@ void RpcLibClientBase::simSetTraceLine(const std::vector& color_rgba, flo vector RpcLibClientBase::simGetImages(vector request, const std::string& vehicle_name) { - const auto& response_adaptor = pimpl_->client.call("simGetImages", + const auto& response_adaptor = pimpl_->client.call("simGetImages", RpcLibAdapatorsBase::ImageRequest::from(request), vehicle_name) .as>(); @@ -248,6 +248,11 @@ vector RpcLibClientBase::simGetMeshPositionVe return RpcLibAdapatorsBase::MeshPositionVertexBuffersResponse::to(response_adaptor); } +bool RpcLibClientBase::simAddVehicle(const std::string& vehicle_name, const std::string& vehicle_type, const std::string& pawn_path, float north, float east, float down) +{ + return pimpl_->client.call("simAddVehicle", vehicle_name, vehicle_type, pawn_path, north, east, down).as(); +} + void RpcLibClientBase::simPrintLogMessage(const std::string& message, std::string message_param, unsigned char severity) { pimpl_->client.call("simPrintLogMessage", message, message_param, severity); diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index 2b41966c39..696d92bdfd 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -49,7 +49,7 @@ struct RpcLibServerBase::impl { ~impl() { } - void stop() { + void stop() { server.close_sessions(); if (!is_async_) { // this deadlocks UI thread if async_run was called while there are pending rpc calls. @@ -91,9 +91,9 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& pimpl_->server.bind("getMinRequiredClientVersion", []() -> int { return 1; }); - - pimpl_->server.bind("simPause", [&](bool is_paused) -> void { - getWorldSimApi()->pause(is_paused); + + pimpl_->server.bind("simPause", [&](bool is_paused) -> void { + getWorldSimApi()->pause(is_paused); }); pimpl_->server.bind("simIsPaused", [&]() -> bool { @@ -110,7 +110,7 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& pimpl_->server.bind("simSetTimeOfDay", [&](bool is_enabled, const string& start_datetime, bool is_start_datetime_dst, float celestial_clock_speed, float update_interval_secs, bool move_sun) -> void { - getWorldSimApi()->setTimeOfDay(is_enabled, start_datetime, is_start_datetime_dst, + getWorldSimApi()->setTimeOfDay(is_enabled, start_datetime, is_start_datetime_dst, celestial_clock_speed, update_interval_secs, move_sun); }); @@ -122,7 +122,7 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& getWorldSimApi()->setWeatherParameter(param, val); }); - pimpl_->server.bind("enableApiControl", [&](bool is_enabled, const std::string& vehicle_name) -> void { + pimpl_->server.bind("enableApiControl", [&](bool is_enabled, const std::string& vehicle_name) -> void { getVehicleApi(vehicle_name)->enableApiControl(is_enabled); }); @@ -153,8 +153,27 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& return RpcLibAdapatorsBase::MeshPositionVertexBuffersResponse::from(response); }); - pimpl_->server. - bind("simSetVehiclePose", [&](const RpcLibAdapatorsBase::Pose &pose, bool ignore_collision, const std::string& vehicle_name) -> void { + pimpl_->server.bind("simAddVehicle", [&](const std::string& vehicle_name, const std::string& vehicle_type, const std::string& pawn_path, float north, float east, float down) -> bool { + + AirSimSettings::VehicleSetting vehicle_setting; + + // TODO expose other VehicleSettings fields + vehicle_setting.vehicle_name = vehicle_name; + vehicle_setting.vehicle_type = vehicle_type; + vehicle_setting.pawn_path = pawn_path; + + vehicle_setting.position[0] = north; + vehicle_setting.position[1] = east; + vehicle_setting.position[2] = down; + + vehicle_setting.rotation.yaw = 0; + vehicle_setting.rotation.pitch = 0; + vehicle_setting.rotation.roll = 0; + + return getWorldSimApi()->createVehicleAtRuntime(vehicle_setting); + }); + + pimpl_->server.bind("simSetVehiclePose", [&](const RpcLibAdapatorsBase::Pose &pose, bool ignore_collision, const std::string& vehicle_name) -> void { getVehicleSimApi(vehicle_name)->setPose(pose.to(), ignore_collision); }); @@ -179,7 +198,7 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& pimpl_->server. bind("simGetSegmentationObjectID", [&](const std::string& mesh_name) -> int { return getWorldSimApi()->getSegmentationObjectID(mesh_name); - }); + }); pimpl_->server.bind("reset", [&]() -> void { //Exit if already resetting. @@ -261,7 +280,7 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& }); pimpl_->server.bind("simGetCollisionInfo", [&](const std::string& vehicle_name) -> RpcLibAdapatorsBase::CollisionInfo { - const auto& collision_info = getVehicleSimApi(vehicle_name)->getCollisionInfo(); + const auto& collision_info = getVehicleSimApi(vehicle_name)->getCollisionInfo(); return RpcLibAdapatorsBase::CollisionInfo(collision_info); }); @@ -282,7 +301,7 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& }); pimpl_->server.bind("simGetObjectPose", [&](const std::string& object_name) -> RpcLibAdapatorsBase::Pose { - const auto& pose = getWorldSimApi()->getObjectPose(object_name); + const auto& pose = getWorldSimApi()->getObjectPose(object_name); return RpcLibAdapatorsBase::Pose(pose); }); diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index 122fbb230f..c5bad8f2f0 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -549,6 +549,74 @@ FRotator ASimModeBase::toFRotator(const msr::airlib::AirSimSettings::Rotation& r return frotator; } +// TODO factor out commonalities with this and setupVehiclesAndCamera, below +bool ASimModeBase::createVehicleAtRuntime(const AirSimSettings::VehicleSetting &vehicle_setting) +{ + if (!isVehicleTypeSupported(vehicle_setting.vehicle_type)) { + Utils::log(Utils::stringf("Vehicle type %s is not supported in this game mode", vehicle_setting.vehicle_type.c_str()), Utils::kLogLevelWarn); + return false; + } + + // Retroactively adjust AirSimSettings, so it's like we knew about this vehicle all along + // (Other places in the code use this for reference) + AirSimSettings::singleton().addVehicleSetting(vehicle_setting); + + //get UU origin of global NED frame + const FTransform uu_origin = getGlobalNedTransform().getGlobalTransform(); + + //compute initial pose + FVector spawn_position = uu_origin.GetLocation(); + msr::airlib::Vector3r settings_position = vehicle_setting.position; + if (!msr::airlib::VectorMath::hasNan(settings_position)) + spawn_position = getGlobalNedTransform().fromGlobalNed(settings_position); + + FRotator spawn_rotation = toFRotator(vehicle_setting.rotation, uu_origin.Rotator()); + + //spawn vehicle pawn + FActorSpawnParameters pawn_spawn_params; + + std::string vehicle_name = vehicle_setting.vehicle_name; + + pawn_spawn_params.Name = FName(vehicle_name.c_str()); + pawn_spawn_params.SpawnCollisionHandlingOverride = + ESpawnActorCollisionHandlingMethod::AdjustIfPossibleButAlwaysSpawn; + auto vehicle_bp_class = UAirBlueprintLib::LoadClass( + getSettings().pawn_paths.at(getVehiclePawnPathName(vehicle_setting)).pawn_bp); + APawn* spawned_pawn = static_cast(this->GetWorld()->SpawnActor( + vehicle_bp_class, &spawn_position, &spawn_rotation, pawn_spawn_params)); + + spawned_actors_.Add(spawned_pawn); + + initializeVehiclePawn(spawned_pawn); + + //create vehicle sim api + const auto& ned_transform = getGlobalNedTransform(); + const auto& pawn_ned_pos = ned_transform.toLocalNed(spawned_pawn->GetActorLocation()); + const auto& home_geopoint = msr::airlib::EarthUtils::nedToGeodetic(pawn_ned_pos, getSettings().origin_geopoint); + + PawnSimApi::Params pawn_sim_api_params(spawned_pawn, &getGlobalNedTransform(), + getVehiclePawnEvents(spawned_pawn), getVehiclePawnCameras(spawned_pawn), pip_camera_class, + collision_display_template, home_geopoint, vehicle_name); + + auto vehicle_sim_api = createVehicleSimApi(pawn_sim_api_params); + auto vehicle_sim_api_p = vehicle_sim_api.get(); + auto vehicle_Api = getVehicleApi(pawn_sim_api_params, vehicle_sim_api_p); + getApiProvider()->insert_or_assign(vehicle_name, vehicle_Api, vehicle_sim_api_p); + + // Usually physics registration happens at init, in ASimModeWorldBase::initializeForPlay(), but not in this case + vehicle_sim_api_p->reset(); + registerPhysicsBody(vehicle_sim_api_p); + + if ((vehicle_setting.is_fpv_vehicle || !getApiProvider()->hasDefaultVehicle()) && vehicle_name != "") + { + getApiProvider()->makeDefaultVehicle(vehicle_name); + } + + vehicle_sim_apis_.push_back(std::move(vehicle_sim_api)); + + return true; +} + void ASimModeBase::setupVehiclesAndCamera() { //get UU origin of global NED frame diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h index b23be7a3dc..dd3d09c2be 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h @@ -75,6 +75,8 @@ class AIRSIM_API ASimModeBase : public AActor void stopApiServer(); bool isApiServerStarted(); + bool createVehicleAtRuntime(const msr::airlib::AirSimSettings::VehicleSetting &vehicle_setting); + const NedTransform& getGlobalNedTransform(); msr::airlib::ApiProvider* getApiProvider() const @@ -116,6 +118,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 registerPhysicsBody(msr::airlib::VehicleSimApiBase *physicsBody) {} protected: //Utility methods for derived classes virtual const msr::airlib::AirSimSettings& getSettings() const; diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp index dd157d5cf3..f325be9ace 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp @@ -21,6 +21,12 @@ void ASimModeWorldBase::initializeForPlay() vehicles, getPhysicsLoopPeriod())); } +void ASimModeWorldBase::registerPhysicsBody(msr::airlib::VehicleSimApiBase *physicsBody) +{ + physics_world_.get()->addBody(physicsBody); +} + + void ASimModeWorldBase::EndPlay(const EEndPlayReason::Type EndPlayReason) { //remove everything that we created in BeginPlay diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h index 82d65236dd..7b2febd758 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h @@ -42,6 +42,9 @@ class AIRSIM_API ASimModeWorldBase : public ASimModeBase //should be called by derived class once all api_provider_ is ready to use void initializeForPlay(); + //used for adding physics bodies on the fly + virtual void registerPhysicsBody(msr::airlib::VehicleSimApiBase *physicsBody); + long long getPhysicsLoopPeriod() const; void setPhysicsLoopPeriod(long long period); private: diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index f7679c12d7..25e29d2a00 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -268,6 +268,22 @@ void WorldSimApi::setTimeOfDay(bool is_enabled, const std::string& start_datetim celestial_clock_speed, update_interval_secs, move_sun); } +bool WorldSimApi::createVehicleAtRuntime(AirSimSettings::VehicleSetting& vehicle_setting) +{ + bool result; + + // We need to run this code on the main game thread, since it iterates over actors + FGraphEventRef task = FFunctionGraphTask::CreateAndDispatchWhenReady([&]() + { + result = simmode_->createVehicleAtRuntime(vehicle_setting); + }, TStatId(), NULL, ENamedThreads::GameThread); + + // Wait for the result + FTaskGraphInterface::Get().WaitUntilTaskCompletes(task); + + return result; +} + bool WorldSimApi::setSegmentationObjectID(const std::string& mesh_name, int object_id, bool is_name_regex) { bool success; diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.h b/Unreal/Plugins/AirSim/Source/WorldSimApi.h index 9e3ff509fc..6524538736 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.h +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.h @@ -38,6 +38,8 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase { virtual bool setSegmentationObjectID(const std::string& mesh_name, int object_id, bool is_name_regex = false) override; virtual int getSegmentationObjectID(const std::string& mesh_name) const override; + virtual bool createVehicleAtRuntime(msr::airlib::AirSimSettings::VehicleSetting& vehicle_setting); + virtual void printLogMessage(const std::string& message, const std::string& message_param = "", unsigned char severity = 0) override; From 5787dde890ab847f2468bee6fab722077b2e5619 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Sat, 13 Feb 2021 03:58:35 +0530 Subject: [PATCH 02/29] HelloSpawnedDrones sample (Windows), and note in docs --- HelloSpawnedDrones/HelloSpawnedDrones.cpp | Bin 0 -> 12766 bytes HelloSpawnedDrones/HelloSpawnedDrones.vcxproj | 161 ++++++++++++++++++ .../HelloSpawnedDrones.vcxproj.filters | 6 + docs/apis_cpp.md | 1 + 4 files changed, 168 insertions(+) create mode 100644 HelloSpawnedDrones/HelloSpawnedDrones.cpp create mode 100644 HelloSpawnedDrones/HelloSpawnedDrones.vcxproj create mode 100644 HelloSpawnedDrones/HelloSpawnedDrones.vcxproj.filters diff --git a/HelloSpawnedDrones/HelloSpawnedDrones.cpp b/HelloSpawnedDrones/HelloSpawnedDrones.cpp new file mode 100644 index 0000000000000000000000000000000000000000..26048bdfe1c1530f5bae180906f5396a42b7e65a GIT binary patch literal 12766 zcmeI2Yi}FJ6^7@l3iLm0SRkOBNV1wZA6(afV%dpP-(V>L>Y@-Rk-F3*C6J62JLq3; z^FDL-aAtSOr5z=*0~iFk%iWnX=e*}Sm;e87D{O@#+zaR7BD@Zha28I&X_)C*Hynj^ zT_5Y&k=~q#slLbY9qlLT!EaAb&U7~pn`*rihWZp4rD^;(Q15}BzS3`BE&m+;q>&l> zQKUame_HhQvn3fw!&7zXP>` z>!HTn3VXUo@*`azt2O;MHSUEzoyh5)#(pji?&*4l5+T)AN@lHSp_;8ew$Qj=!2d?WEC1uA~(xuqx;i{^H6snKaw7XQ4@?Yi{orbt_!u= z63)%H&m(`>%%QLx>WbvEn!c`L4G%;U{`TY#Q&Ap>%c?arsozl_`~y20i2qTzJGY7| z!gqx3X+(6a9&N3l){hb%wzR`PY|MVy3iSED68$q#z!&kTJ^dTYZjdcUuB*4anT74} zm8=WjH_x7h&vkF_il`ebY%KYPiVTeLbNpQsg=tB>i0?-lV=5|GDAtX|PQq<{dTKW* zrTSah8$ZSS85wkXYEkHFi2f`5TmSy9Z%6&k)q?T;Z6`RD3qGBBZ?jpXuy{1g#>}zU zkgXa;A|T_~s8#x1S($0qG?epgzUJIjBg+$62-pMu@U-PjpQ27CWED(FelwK-93EuAULO zd7H<2{`bXEUNPO#6SzE#RPL(pUxnsk96;5hzxZeR+vYZs@!~S3TWsUhxGvfC+IJc`x9#_1yNQ?Uu-lXqn(+~PecySY zUO`wa-OgfZ?rm?xFsfJI^|saAsZZVJJQD3IKD^KjfV^zk=S@T-#aJ>Ixfm+j)CU|G?Q{a|9n3Cdq#~_as7d8>UGQr)GTJDSQuI! z=}LwEldi0kFq6S{v1H3xU+8Y6bujV^Naq!Qk~JZRr?6sF&85pcfZFeksCqS4x4OlV zCiPmJFw@<{S|*1zc;9&gsOR*BcDZ-Qyv zvf6piqkQ5$acq&l&K2($c^uE{{=u_l@jQ81lIZ%@MCwwd6_Qo)w;j`NL)!`2+$uh7 zYcPvVoTJB&*1+0ZJ$A0LhNS0>EVJd7@W+S`IrmtDS;$A5_%$|B&j&fSxn(5V`4BUP zNs;q@T~VT0-+u^s!?{|PgPSv}e696`d}<^5)n>S*>I*EZ@%N8E=lWLBlWKr!ofX%s z?DVA-oVUHJ_=X`j5c$2bnGhOv`rdo6in zS~mK1>W%rbYi=$4w_IQ2EQYVocNHr*JH%YQd3xwmq026;of}!3pB%~8#lh9%jjd9# zYQnse^H?_X&QBz_IM#K?0f?!XIQ`Rb7_Z~ zGqK)eI|n=3y?a>3e%qTiec1jKd#biqeJLu~1Ti#K35j2y8G1D_(s;3q!%wd47#zzLh*?mUm#bMs7**#zqswmN}cztl?Dm&&1O z{o17%Yu#fcgsW@4tc2O>Z0-{Y%cfRccHj3&8c1(eA#JDXx+Pvm;^D5ooO>`?f1a;4 zw{h|$O2k^9$%HJqt=`uEI(O)1JDz`9#pBvs+g0SIvQe`OaKl?U73Q)y{oF^Y{e6|) zk%wcbu>W?TFWxtkt=9F8=T#{=Cuyu?yX~IIi9FWyvF^0GgB&IgcxV4S{Og%Wi2fp{C^-u@; zN;^9i(b7sVfJhl^I;jJgl;e$Cg``No`Z4KP^ zp6-brCxWme&fQ$<-fDMF!FXJ^6KQ5`mdhE3x{N%8y>mv0OmHF^sUQ2xoTLIdM(Bz@ z`&?ANwubvsG#C{WOta39N0Q{lxnxfzb2@ox5pcQXF*8Z=lv5<22tSMqXJ`5n4LA#F z|2X-#jH0#qOeg0Omnq^^*D + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 15.0 + {99CBF376-5EBA-4164-A657-E7D708C9D685} + Win32Proj + HelloSpawnedDrones + 10.0.17134.0 + + + + Application + true + v141 + Unicode + + + Application + false + v141 + true + Unicode + + + Application + true + v141 + Unicode + + + Application + false + v141 + true + Unicode + + + + + + + + + + + + + + + + + + + + + true + + + true + + + false + + + false + + + + NotUsing + Level3 + Disabled + true + _DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + $(ProjectDir)..\AirLib\deps\rpclib\include;include;$(ProjectDir)..\AirLib\deps\eigen3;$(ProjectDir)..\AirLib\include + + + Console + true + $(ProjectDir)\..\AirLib\deps\MavLinkCom\lib\$(Platform)\$(Configuration);$(ProjectDir)\..\AirLib\deps\rpclib\lib\$(Platform)\$(Configuration);$(ProjectDir)\..\AirLib\lib\$(Platform)\$(Configuration);%(AdditionalLibraryDirectories) + rpc.lib;AirLib.lib;%(AdditionalDependencies) + + + + + Use + Level3 + Disabled + true + WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + + + + + Use + Level3 + MaxSpeed + true + true + true + WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + true + true + + + + + NotUsing + Level3 + MaxSpeed + true + true + true + NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + $(ProjectDir)..\AirLib\deps\rpclib\include;include;$(ProjectDir)..\AirLib\deps\eigen3;$(ProjectDir)..\AirLib\include + + + Console + true + true + true + $(ProjectDir)\..\AirLib\deps\MavLinkCom\lib\$(Platform)\$(Configuration);$(ProjectDir)\..\AirLib\deps\rpclib\lib\$(Platform)\$(Configuration);$(ProjectDir)\..\AirLib\lib\$(Platform)\$(Configuration);%(AdditionalLibraryDirectories) + rpc.lib;AirLib.lib;%(AdditionalDependencies) + + + + + + + + + \ No newline at end of file diff --git a/HelloSpawnedDrones/HelloSpawnedDrones.vcxproj.filters b/HelloSpawnedDrones/HelloSpawnedDrones.vcxproj.filters new file mode 100644 index 0000000000..d428262af8 --- /dev/null +++ b/HelloSpawnedDrones/HelloSpawnedDrones.vcxproj.filters @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/docs/apis_cpp.md b/docs/apis_cpp.md index b9255c30f5..3d41b98cea 100644 --- a/docs/apis_cpp.md +++ b/docs/apis_cpp.md @@ -88,5 +88,6 @@ int main() ## See Also * [Examples](../Examples) of how to use internal infrastructure in AirSim in your other projects * [DroneShell](../DroneShell) app shows how to make simple interface using C++ APIs to control drones +* [HelloSpawnedDrones](./HelloSpawnedDrones) app shows how to make additional vehicles on the fly * [Python APIs](apis.md) From bffb79d80088e82e778a6067962205c5257ca462 Mon Sep 17 00:00:00 2001 From: Kerry Moffitt Date: Wed, 1 May 2019 09:33:08 -0400 Subject: [PATCH 03/29] Fix encoding --- HelloSpawnedDrones/HelloSpawnedDrones.cpp | Bin 12766 -> 6213 bytes 1 file changed, 0 insertions(+), 0 deletions(-) diff --git a/HelloSpawnedDrones/HelloSpawnedDrones.cpp b/HelloSpawnedDrones/HelloSpawnedDrones.cpp index 26048bdfe1c1530f5bae180906f5396a42b7e65a..9bfd4e4b6cb5a2cc53cbeac58489491572b6dcdb 100644 GIT binary patch literal 6213 zcmdT|>uwu27XGby3RVG%Q6kHBTla@z8*uHob!*2)tPQXU0)d*LB(7(M1Cp{L7kRS% zv5(YO=yyntJTvw>YbOP|K;39~IXsu|T>tp@e>OMSp(%g0XxVt<_;OAg%SE%q=N{TPh}zsBT}{~QlXih3wCxgs+I>)^h_2>wnzo*CMwTW zvFZOAFRaYW=B3p#v1ck3gLzqYqs!5olf%*Y?EL7}`26@7x2K>xWp6GHPfvatpI!cZ zad7xs^bBjV5UpjFWO*veac*WMPp+b8qDbYGfy-~&_t#A6GTFr6SZJ;EVBX^3b)Elf zF_%du%x1pGtklXXy~#_tdA9z1QzoZ!a+pa`STE1R(jUuAT&|22dF%A9RK{w-^Zi@> z?Iv@r3e{LKHqGRuzGUaPmhMMUbgg8{^rEHBusznzRKhd*?Bc{UQ`|Bu zbDzx91ibS9DANP1>$lcmUR~X+l`o>?+20AkN3b&2MQVv z6EMSecUkvP6;r8mk`b~LA|Y&>!C=s(ZFd%s$>^fzVPhZX=n8Iz<1y$~gkdA16@c*? zY9o#O zxpf!c_}l8Uox#?WJ!cgw;BmW$4KESgWHgtC;XlqM5{A}r+di8>c{6z}7?wE8RSs}7 ztC$hiW`e20IsHs4M1VdMnGwwLE1{-Swv>n(Q!cYgVHOXJJw%5?lL8yIzN&I|G)Pf=<)?#^!m6^MiRyVi|YDJ z*jwCRT5d(%biD!gt6K!MPVgyD?;Qv=DGzRNib7nn(3&h$R!_#SUmXGF9(GnR;{r`@ zz!>2-c&azl$U7{E@7zV_NdXa%1pxHQuCN)q52xJ+T`Al?FsmYo!$)|zsqKyZC71TR zIOWEUxViFrXuC22+>&_%*dD_a6adFYF!O3KCE;X`eGmU|-VJ@4rL@VMz#~4EnFQVt z^sbTO2tEWaJEu?=Jcjb#lxNuld510MqF{IeR0y!oVTC;BMGD8#I3^2e=9E6$;i;x+ z|I}kdx(!+GuGexM!;jPI;~9WB4*6>Uelg~&NC3-h-+ICgKC3sLihfw;06E-JU4jx} zD&RUucN1Zk0=5$zmM=4IVSt>nB+CIcZCbG5fy9x>EGH%__}s9Ky(+IF-ZswN3KB9E zUEFdvZ%s(B8L4;Ph&PC41kiq!So_VS_)os10xCu*-yjzHj*-v1)E)dCs>-M6eS-dt zck>&UJ=x>n*!0)qY()_Ss^kziXA||2mE1zBSh4JTmTww_Tp!rm>A0nLBX20tQZ^%P z)?ngx;Jc(Q+B(umTvl^gj8-K~O`jwB#P#OhaCofr5=rE|$X0YUXqBz_a!f8CqJ2n~ z6#zl!$$`D8po=PQtEpEyRcKD{!|FTal4e2B>3j``X!6oZv$3~_3zAeuxmv=ld0_P2 z@3k4|NUv?eCbnxi++#cd95o8iUQxoYwZZypLo|U+rgF?bskEqGq6(uOjoV59Z*4t% zT^-7Dzt|%{s0wb4gjCg{ z8*FQ^gLD&68}iv-m7RW}>I*sK!5Z@swNFjD|EUA0)Vl5_Pf^l+j-L!?Lc!~6>}K?O zpeB2g^>#w5)C}joUHKfVw|xx?S#zfWx+DIPD#SPU7y}1WL8W=fjJNIb(?r^G!TA%$ z51b0}7}J{Ja6Pj5mNC)uvjrz~Xvr}GLTVP&P5IGI+#7JSTRXCUUsv(!V{@}81i)(|H zSSI5Hz29yn?|vK(5e>Qob+#lsmKyDxtC=eKDkZf1u9^wv?pRFi@VCxmRVO`?D)=MVYZsQzQUc4ZJzvC|?n!x*@T$uTodTBoz@&d-K zVP>BN3&WAz#4zyVi@88CtZi&KeSATwxHZ`Jgb8~cYK3Cg?tW|4uL=#tu&MsFV19e> aFP!la2a)XmJeWltE$jk~8)mzoqCWw49^(=K literal 12766 zcmeI2Yi}FJ6^7@l3iLm0SRkOBNV1wZA6(afV%dpP-(V>L>Y@-Rk-F3*C6J62JLq3; z^FDL-aAtSOr5z=*0~iFk%iWnX=e*}Sm;e87D{O@#+zaR7BD@Zha28I&X_)C*Hynj^ zT_5Y&k=~q#slLbY9qlLT!EaAb&U7~pn`*rihWZp4rD^;(Q15}BzS3`BE&m+;q>&l> zQKUame_HhQvn3fw!&7zXP>` z>!HTn3VXUo@*`azt2O;MHSUEzoyh5)#(pji?&*4l5+T)AN@lHSp_;8ew$Qj=!2d?WEC1uA~(xuqx;i{^H6snKaw7XQ4@?Yi{orbt_!u= z63)%H&m(`>%%QLx>WbvEn!c`L4G%;U{`TY#Q&Ap>%c?arsozl_`~y20i2qTzJGY7| z!gqx3X+(6a9&N3l){hb%wzR`PY|MVy3iSED68$q#z!&kTJ^dTYZjdcUuB*4anT74} zm8=WjH_x7h&vkF_il`ebY%KYPiVTeLbNpQsg=tB>i0?-lV=5|GDAtX|PQq<{dTKW* zrTSah8$ZSS85wkXYEkHFi2f`5TmSy9Z%6&k)q?T;Z6`RD3qGBBZ?jpXuy{1g#>}zU zkgXa;A|T_~s8#x1S($0qG?epgzUJIjBg+$62-pMu@U-PjpQ27CWED(FelwK-93EuAULO zd7H<2{`bXEUNPO#6SzE#RPL(pUxnsk96;5hzxZeR+vYZs@!~S3TWsUhxGvfC+IJc`x9#_1yNQ?Uu-lXqn(+~PecySY zUO`wa-OgfZ?rm?xFsfJI^|saAsZZVJJQD3IKD^KjfV^zk=S@T-#aJ>Ixfm+j)CU|G?Q{a|9n3Cdq#~_as7d8>UGQr)GTJDSQuI! z=}LwEldi0kFq6S{v1H3xU+8Y6bujV^Naq!Qk~JZRr?6sF&85pcfZFeksCqS4x4OlV zCiPmJFw@<{S|*1zc;9&gsOR*BcDZ-Qyv zvf6piqkQ5$acq&l&K2($c^uE{{=u_l@jQ81lIZ%@MCwwd6_Qo)w;j`NL)!`2+$uh7 zYcPvVoTJB&*1+0ZJ$A0LhNS0>EVJd7@W+S`IrmtDS;$A5_%$|B&j&fSxn(5V`4BUP zNs;q@T~VT0-+u^s!?{|PgPSv}e696`d}<^5)n>S*>I*EZ@%N8E=lWLBlWKr!ofX%s z?DVA-oVUHJ_=X`j5c$2bnGhOv`rdo6in zS~mK1>W%rbYi=$4w_IQ2EQYVocNHr*JH%YQd3xwmq026;of}!3pB%~8#lh9%jjd9# zYQnse^H?_X&QBz_IM#K?0f?!XIQ`Rb7_Z~ zGqK)eI|n=3y?a>3e%qTiec1jKd#biqeJLu~1Ti#K35j2y8G1D_(s;3q!%wd47#zzLh*?mUm#bMs7**#zqswmN}cztl?Dm&&1O z{o17%Yu#fcgsW@4tc2O>Z0-{Y%cfRccHj3&8c1(eA#JDXx+Pvm;^D5ooO>`?f1a;4 zw{h|$O2k^9$%HJqt=`uEI(O)1JDz`9#pBvs+g0SIvQe`OaKl?U73Q)y{oF^Y{e6|) zk%wcbu>W?TFWxtkt=9F8=T#{=Cuyu?yX~IIi9FWyvF^0GgB&IgcxV4S{Og%Wi2fp{C^-u@; zN;^9i(b7sVfJhl^I;jJgl;e$Cg``No`Z4KP^ zp6-brCxWme&fQ$<-fDMF!FXJ^6KQ5`mdhE3x{N%8y>mv0OmHF^sUQ2xoTLIdM(Bz@ z`&?ANwubvsG#C{WOta39N0Q{lxnxfzb2@ox5pcQXF*8Z=lv5<22tSMqXJ`5n4LA#F z|2X-#jH0#qOeg0Omnq^^*D Date: Mon, 6 May 2019 12:38:14 -0400 Subject: [PATCH 04/29] Sample code, Linux-side --- .gitignore | 2 ++ cmake/CMakeLists.txt | 1 + cmake/HelloSpawnedDrones/CMakeLists.txt | 25 +++++++++++++++++++++++++ 3 files changed, 28 insertions(+) create mode 100644 cmake/HelloSpawnedDrones/CMakeLists.txt diff --git a/.gitignore b/.gitignore index c1564db29d..ec5b0897c7 100644 --- a/.gitignore +++ b/.gitignore @@ -286,12 +286,14 @@ ModelManifest.xml /cmake/MavLinkCom/Makefile /cmake/Makefile /cmake/HelloDrone/Makefile +/cmake/HelloSpawnedDrones/Makefile /cmake/DroneShell/Makefile /cmake/DroneServer/Makefile /cmake/AirLib/Makefile /cmake/AirLibUnity/Makefile /cmake/AirLibUnity/AirLibUnity /cmake/HelloDrone/HelloDrone +/cmake/HelloSpawnedDrones/HelloSpawnedDrones /cmake/DroneShell/DroneShell /cmake/DroneServer/DroneServer cmake/AirLibUnitTests/Makefile diff --git a/cmake/CMakeLists.txt b/cmake/CMakeLists.txt index 5e31535511..d5dbc29235 100644 --- a/cmake/CMakeLists.txt +++ b/cmake/CMakeLists.txt @@ -6,6 +6,7 @@ add_subdirectory("AirLib") add_subdirectory("MavLinkCom") add_subdirectory("AirLibUnitTests") add_subdirectory("HelloDrone") +add_subdirectory("HelloSpawnedDrones") add_subdirectory("HelloCar") add_subdirectory("DroneShell") add_subdirectory("DroneServer") diff --git a/cmake/HelloSpawnedDrones/CMakeLists.txt b/cmake/HelloSpawnedDrones/CMakeLists.txt new file mode 100644 index 0000000000..32906f0081 --- /dev/null +++ b/cmake/HelloSpawnedDrones/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.5.0) +project(HelloSpawnedDrones) + +LIST(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/../cmake-modules") +INCLUDE("${CMAKE_CURRENT_LIST_DIR}/../cmake-modules/CommonSetup.cmake") +CommonSetup() + +IncludeEigen() + +SetupConsoleBuild() + +## Specify additional locations of header files +include_directories( + ${AIRSIM_ROOT}/HelloSpawnedDrones + ${AIRSIM_ROOT}/AirLib/include + ${RPC_LIB_INCLUDES} + ${AIRSIM_ROOT}/MavLinkCom/include + ${AIRSIM_ROOT}/MavLinkCom/common_utils +) + +AddExecutableSource() + +CommonTargetLink() +target_link_libraries(${PROJECT_NAME} AirLib) +target_link_libraries(${PROJECT_NAME} ${RPC_LIB}) From 22a28da01ee5da2fd7d8d760e4fe004a41986157 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Sun, 19 Jan 2020 21:41:28 +0530 Subject: [PATCH 05/29] Add simAddVehicle() to Python API --- PythonClient/airsim/client.py | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index 27e6cc84b7..c303a2e747 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -854,6 +854,24 @@ def simCreateVoxelGrid(self, position, x, y, z, res, of): """ return self.client.call('simCreateVoxelGrid', position, x, y, z, res, of) + # Add new vehicle via RPC + def simAddVehicle(self, vehicle_name, vehicle_type, pawn_path, north, east, down): + """ + Create vehicle at runtime + + Args: + vehicle_name (str): Name of the vehicle being created + vehicle_type (str): Type of vehicle, e.g. "simpleflight" + pawn_path (str): Vehicle blueprint path + north (float): Metres in North direction to spawn at vehicle at + east (float): Meters in East direction to spawn vehicle at + down (float): Metres below to spawn the vehicle at + + Returns: + bool: Whether vehicle was created + """ + return self.client.call('simAddVehicle', vehicle_name, vehicle_type, pawn_path, north, east, down) + # ----------------------------------- Multirotor APIs --------------------------------------------- class MultirotorClient(VehicleClient, object): def __init__(self, ip = "", port = 41451, timeout_value = 3600): From b03ffb56266eb026d0e8bea2b5cd67bd09bb5526 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Thu, 4 Feb 2021 19:58:45 +0530 Subject: [PATCH 06/29] Use Pose instead of NED values, fix missing override --- AirLib/include/api/RpcLibClientBase.hpp | 2 +- AirLib/include/api/WorldSimApiBase.hpp | 2 +- AirLib/src/api/RpcLibClientBase.cpp | 4 ++-- AirLib/src/api/RpcLibServerBase.cpp | 21 +++----------------- PythonClient/airsim/client.py | 10 ++++------ Unreal/Plugins/AirSim/Source/WorldSimApi.cpp | 11 +++++++++- Unreal/Plugins/AirSim/Source/WorldSimApi.h | 2 +- 7 files changed, 22 insertions(+), 30 deletions(-) diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index bb97e1164e..9215087c60 100644 --- a/AirLib/include/api/RpcLibClientBase.hpp +++ b/AirLib/include/api/RpcLibClientBase.hpp @@ -100,7 +100,7 @@ class RpcLibClientBase { vector simGetImage(const std::string& camera_name, ImageCaptureBase::ImageType type, const std::string& vehicle_name = ""); vector simGetMeshPositionVertexBuffers(); - bool simAddVehicle(const std::string& vehicle_name, const std::string& vehicle_type, const std::string& pawn_path, float north, float east, float down); + bool simAddVehicle(const std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path = ""); CollisionInfo simGetCollisionInfo(const std::string& vehicle_name = "") const; diff --git a/AirLib/include/api/WorldSimApiBase.hpp b/AirLib/include/api/WorldSimApiBase.hpp index 5cd4c5257c..541aa36ca3 100644 --- a/AirLib/include/api/WorldSimApiBase.hpp +++ b/AirLib/include/api/WorldSimApiBase.hpp @@ -47,7 +47,7 @@ class WorldSimApiBase { virtual bool setSegmentationObjectID(const std::string& mesh_name, int object_id, bool is_name_regex = false) = 0; virtual int getSegmentationObjectID(const std::string& mesh_name) const = 0; - virtual bool createVehicleAtRuntime(AirSimSettings::VehicleSetting& vehicle_setting) = 0; + virtual bool createVehicleAtRuntime(const std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path = "") = 0; virtual void printLogMessage(const std::string& message, const std::string& message_param = "", unsigned char severity = 0) = 0; diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index 664f3141eb..19cf5c73f9 100644 --- a/AirLib/src/api/RpcLibClientBase.cpp +++ b/AirLib/src/api/RpcLibClientBase.cpp @@ -248,9 +248,9 @@ vector RpcLibClientBase::simGetMeshPositionVe return RpcLibAdapatorsBase::MeshPositionVertexBuffersResponse::to(response_adaptor); } -bool RpcLibClientBase::simAddVehicle(const std::string& vehicle_name, const std::string& vehicle_type, const std::string& pawn_path, float north, float east, float down) +bool RpcLibClientBase::simAddVehicle(const std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path) { - return pimpl_->client.call("simAddVehicle", vehicle_name, vehicle_type, pawn_path, north, east, down).as(); + return pimpl_->client.call("simAddVehicle", vehicle_name, vehicle_type, RpcLibAdapatorsBase::Pose(pose), pawn_path).as(); } void RpcLibClientBase::simPrintLogMessage(const std::string& message, std::string message_param, unsigned char severity) diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index 696d92bdfd..1d8b46ac0c 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -153,24 +153,9 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& return RpcLibAdapatorsBase::MeshPositionVertexBuffersResponse::from(response); }); - pimpl_->server.bind("simAddVehicle", [&](const std::string& vehicle_name, const std::string& vehicle_type, const std::string& pawn_path, float north, float east, float down) -> bool { - - AirSimSettings::VehicleSetting vehicle_setting; - - // TODO expose other VehicleSettings fields - vehicle_setting.vehicle_name = vehicle_name; - vehicle_setting.vehicle_type = vehicle_type; - vehicle_setting.pawn_path = pawn_path; - - vehicle_setting.position[0] = north; - vehicle_setting.position[1] = east; - vehicle_setting.position[2] = down; - - vehicle_setting.rotation.yaw = 0; - vehicle_setting.rotation.pitch = 0; - vehicle_setting.rotation.roll = 0; - - return getWorldSimApi()->createVehicleAtRuntime(vehicle_setting); + pimpl_->server.bind("simAddVehicle", [&](const std::string& vehicle_name, const std::string& vehicle_type, + const RpcLibAdapatorsBase::Pose& pose, const std::string& pawn_path) -> bool { + return getWorldSimApi()->createVehicleAtRuntime(vehicle_name, vehicle_type, pose.to(), pawn_path); }); pimpl_->server.bind("simSetVehiclePose", [&](const RpcLibAdapatorsBase::Pose &pose, bool ignore_collision, const std::string& vehicle_name) -> void { diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index c303a2e747..921fc50cb2 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -855,22 +855,20 @@ def simCreateVoxelGrid(self, position, x, y, z, res, of): return self.client.call('simCreateVoxelGrid', position, x, y, z, res, of) # Add new vehicle via RPC - def simAddVehicle(self, vehicle_name, vehicle_type, pawn_path, north, east, down): + def simAddVehicle(self, vehicle_name, vehicle_type, pose, pawn_path = ""): """ Create vehicle at runtime Args: vehicle_name (str): Name of the vehicle being created vehicle_type (str): Type of vehicle, e.g. "simpleflight" - pawn_path (str): Vehicle blueprint path - north (float): Metres in North direction to spawn at vehicle at - east (float): Meters in East direction to spawn vehicle at - down (float): Metres below to spawn the vehicle at + pose (Pose): Initial pose of the vehicle + pawn_path (str): Vehicle blueprint path, default empty wbich uses the default blueprint for the vehicle type Returns: bool: Whether vehicle was created """ - return self.client.call('simAddVehicle', vehicle_name, vehicle_type, pawn_path, north, east, down) + return self.client.call('simAddVehicle', vehicle_name, vehicle_type, pose, pawn_path) # ----------------------------------- Multirotor APIs --------------------------------------------- class MultirotorClient(VehicleClient, object): diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index 25e29d2a00..58ca6c6fe7 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -268,8 +268,17 @@ void WorldSimApi::setTimeOfDay(bool is_enabled, const std::string& start_datetim celestial_clock_speed, update_interval_secs, move_sun); } -bool WorldSimApi::createVehicleAtRuntime(AirSimSettings::VehicleSetting& vehicle_setting) +bool WorldSimApi::createVehicleAtRuntime(const std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path) { + // Create settings object + AirSimSettings::VehicleSetting vehicle_setting; + + vehicle_setting.vehicle_name = vehicle_name; + vehicle_setting.vehicle_type = vehicle_type; + vehicle_setting.pawn_path = pawn_path; + vehicle_setting.position = pose.position; + VectorMath::toEulerianAngle(pose.orientation, vehicle_setting.rotation.pitch, vehicle_setting.rotation.roll, vehicle_setting.rotation.yaw); + bool result; // We need to run this code on the main game thread, since it iterates over actors diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.h b/Unreal/Plugins/AirSim/Source/WorldSimApi.h index 6524538736..4072c33b3a 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.h +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.h @@ -38,7 +38,7 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase { virtual bool setSegmentationObjectID(const std::string& mesh_name, int object_id, bool is_name_regex = false) override; virtual int getSegmentationObjectID(const std::string& mesh_name) const override; - virtual bool createVehicleAtRuntime(msr::airlib::AirSimSettings::VehicleSetting& vehicle_setting); + virtual bool createVehicleAtRuntime(const std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path = "") override; virtual void printLogMessage(const std::string& message, const std::string& message_param = "", unsigned char severity = 0) override; From 4b9d6c70e566771f1b669b58d454f94ad6acb93b Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Fri, 5 Feb 2021 09:12:33 +0530 Subject: [PATCH 07/29] Consolidate vehicle creation code in SimModeBase --- .../AirSim/Source/SimMode/SimModeBase.cpp | 107 ++++++++---------- .../AirSim/Source/SimMode/SimModeBase.h | 2 + 2 files changed, 48 insertions(+), 61 deletions(-) diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index c5bad8f2f0..d44fffb3df 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -549,37 +549,27 @@ FRotator ASimModeBase::toFRotator(const msr::airlib::AirSimSettings::Rotation& r return frotator; } -// TODO factor out commonalities with this and setupVehiclesAndCamera, below -bool ASimModeBase::createVehicleAtRuntime(const AirSimSettings::VehicleSetting &vehicle_setting) +APawn* ASimModeBase::createVehiclePawn(const AirSimSettings::VehicleSetting& vehicle_setting) { - if (!isVehicleTypeSupported(vehicle_setting.vehicle_type)) { - Utils::log(Utils::stringf("Vehicle type %s is not supported in this game mode", vehicle_setting.vehicle_type.c_str()), Utils::kLogLevelWarn); - return false; - } - - // Retroactively adjust AirSimSettings, so it's like we knew about this vehicle all along - // (Other places in the code use this for reference) - AirSimSettings::singleton().addVehicleSetting(vehicle_setting); - //get UU origin of global NED frame const FTransform uu_origin = getGlobalNedTransform().getGlobalTransform(); - //compute initial pose + // compute initial pose FVector spawn_position = uu_origin.GetLocation(); - msr::airlib::Vector3r settings_position = vehicle_setting.position; - if (!msr::airlib::VectorMath::hasNan(settings_position)) + Vector3r settings_position = vehicle_setting.position; + if (!VectorMath::hasNan(settings_position)) spawn_position = getGlobalNedTransform().fromGlobalNed(settings_position); FRotator spawn_rotation = toFRotator(vehicle_setting.rotation, uu_origin.Rotator()); - //spawn vehicle pawn - FActorSpawnParameters pawn_spawn_params; - std::string vehicle_name = vehicle_setting.vehicle_name; + //spawn vehicle pawn + FActorSpawnParameters pawn_spawn_params; pawn_spawn_params.Name = FName(vehicle_name.c_str()); pawn_spawn_params.SpawnCollisionHandlingOverride = ESpawnActorCollisionHandlingMethod::AdjustIfPossibleButAlwaysSpawn; + auto vehicle_bp_class = UAirBlueprintLib::LoadClass( getSettings().pawn_paths.at(getVehiclePawnPathName(vehicle_setting)).pawn_bp); APawn* spawned_pawn = static_cast(this->GetWorld()->SpawnActor( @@ -587,30 +577,55 @@ bool ASimModeBase::createVehicleAtRuntime(const AirSimSettings::VehicleSetting & spawned_actors_.Add(spawned_pawn); - initializeVehiclePawn(spawned_pawn); + return spawned_pawn; +} + +std::unique_ptr ASimModeBase::createVehicleApi(APawn* vehicle_pawn) +{ + initializeVehiclePawn(vehicle_pawn); //create vehicle sim api const auto& ned_transform = getGlobalNedTransform(); - const auto& pawn_ned_pos = ned_transform.toLocalNed(spawned_pawn->GetActorLocation()); - const auto& home_geopoint = msr::airlib::EarthUtils::nedToGeodetic(pawn_ned_pos, getSettings().origin_geopoint); + const auto& pawn_ned_pos = ned_transform.toLocalNed(vehicle_pawn->GetActorLocation()); + const auto& home_geopoint= msr::airlib::EarthUtils::nedToGeodetic(pawn_ned_pos, getSettings().origin_geopoint); + const std::string vehicle_name = std::string(TCHAR_TO_UTF8(*(vehicle_pawn->GetName()))); - PawnSimApi::Params pawn_sim_api_params(spawned_pawn, &getGlobalNedTransform(), - getVehiclePawnEvents(spawned_pawn), getVehiclePawnCameras(spawned_pawn), pip_camera_class, + PawnSimApi::Params pawn_sim_api_params(vehicle_pawn, &getGlobalNedTransform(), + getVehiclePawnEvents(vehicle_pawn), getVehiclePawnCameras(vehicle_pawn), pip_camera_class, collision_display_template, home_geopoint, vehicle_name); auto vehicle_sim_api = createVehicleSimApi(pawn_sim_api_params); auto vehicle_sim_api_p = vehicle_sim_api.get(); - auto vehicle_Api = getVehicleApi(pawn_sim_api_params, vehicle_sim_api_p); - getApiProvider()->insert_or_assign(vehicle_name, vehicle_Api, vehicle_sim_api_p); + auto vehicle_api = getVehicleApi(pawn_sim_api_params, vehicle_sim_api_p); + getApiProvider()->insert_or_assign(vehicle_name, vehicle_api, vehicle_sim_api_p); + + return vehicle_sim_api; +} + +bool ASimModeBase::createVehicleAtRuntime(const AirSimSettings::VehicleSetting &vehicle_setting) +{ + if (!isVehicleTypeSupported(vehicle_setting.vehicle_type)) { + Utils::log(Utils::stringf("Vehicle type %s is not supported in this game mode", vehicle_setting.vehicle_type.c_str()), Utils::kLogLevelWarn); + return false; + } + + // Retroactively adjust AirSimSettings, so it's like we knew about this vehicle all along + // (Other places in the code use this for reference) + AirSimSettings::singleton().addVehicleSetting(vehicle_setting); + + auto spawned_pawn = createVehiclePawn(vehicle_setting); + + auto vehicle_sim_api = createVehicleApi(spawned_pawn); + auto vehicle_sim_api_p = vehicle_sim_api.get(); + std::string vehicle_name = vehicle_sim_api->getVehicleName(); // Usually physics registration happens at init, in ASimModeWorldBase::initializeForPlay(), but not in this case vehicle_sim_api_p->reset(); registerPhysicsBody(vehicle_sim_api_p); + // Can't be done before the vehicle apis have been created if ((vehicle_setting.is_fpv_vehicle || !getApiProvider()->hasDefaultVehicle()) && vehicle_name != "") - { getApiProvider()->makeDefaultVehicle(vehicle_name); - } vehicle_sim_apis_.push_back(std::move(vehicle_sim_api)); @@ -641,31 +656,14 @@ void ASimModeBase::setupVehiclesAndCamera() fpv_pawn = static_cast(pawns[0]); } else { //add vehicles from settings - for (auto const& vehicle_setting_pair : getSettings().vehicles) + for (const auto& vehicle_setting_pair : getSettings().vehicles) { //if vehicle is of type for derived SimMode and auto creatable const auto& vehicle_setting = *vehicle_setting_pair.second; if (vehicle_setting.auto_create && isVehicleTypeSupported(vehicle_setting.vehicle_type)) { - //compute initial pose - FVector spawn_position = uu_origin.GetLocation(); - Vector3r settings_position = vehicle_setting.position; - if (!VectorMath::hasNan(settings_position)) - spawn_position = getGlobalNedTransform().fromGlobalNed(settings_position); - FRotator spawn_rotation = toFRotator(vehicle_setting.rotation, uu_origin.Rotator()); - - //spawn vehicle pawn - FActorSpawnParameters pawn_spawn_params; - pawn_spawn_params.Name = FName(vehicle_setting.vehicle_name.c_str()); - pawn_spawn_params.SpawnCollisionHandlingOverride = - ESpawnActorCollisionHandlingMethod::AdjustIfPossibleButAlwaysSpawn; - auto vehicle_bp_class = UAirBlueprintLib::LoadClass( - getSettings().pawn_paths.at(getVehiclePawnPathName(vehicle_setting)).pawn_bp); - APawn* spawned_pawn = static_cast(this->GetWorld()->SpawnActor( - vehicle_bp_class, &spawn_position, &spawn_rotation, pawn_spawn_params)); - - spawned_actors_.Add(spawned_pawn); + auto spawned_pawn = createVehiclePawn(vehicle_setting); pawns.Add(spawned_pawn); if (vehicle_setting.is_fpv_vehicle) @@ -676,24 +674,11 @@ void ASimModeBase::setupVehiclesAndCamera() //create API objects for each pawn we have for (AActor* pawn : pawns) { - APawn* vehicle_pawn = static_cast(pawn); - - initializeVehiclePawn(vehicle_pawn); + auto vehicle_pawn = static_cast(pawn); - //create vehicle sim api - const auto& ned_transform = getGlobalNedTransform(); - const auto& pawn_ned_pos = ned_transform.toLocalNed(vehicle_pawn->GetActorLocation()); - const auto& home_geopoint= msr::airlib::EarthUtils::nedToGeodetic(pawn_ned_pos, getSettings().origin_geopoint); - const std::string vehicle_name = std::string(TCHAR_TO_UTF8(*(vehicle_pawn->GetName()))); - - PawnSimApi::Params pawn_sim_api_params(vehicle_pawn, &getGlobalNedTransform(), - getVehiclePawnEvents(vehicle_pawn), getVehiclePawnCameras(vehicle_pawn), pip_camera_class, - collision_display_template, home_geopoint, vehicle_name); + auto vehicle_sim_api = createVehicleApi(vehicle_pawn); + std::string vehicle_name = vehicle_sim_api->getVehicleName(); - auto vehicle_sim_api = createVehicleSimApi(pawn_sim_api_params); - auto vehicle_sim_api_p = vehicle_sim_api.get(); - auto vehicle_Api = getVehicleApi(pawn_sim_api_params, vehicle_sim_api_p); - getApiProvider()->insert_or_assign(vehicle_name, vehicle_Api, vehicle_sim_api_p); if ((fpv_pawn == vehicle_pawn || !getApiProvider()->hasDefaultVehicle()) && vehicle_name != "") getApiProvider()->makeDefaultVehicle(vehicle_name); diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h index dd3d09c2be..bfcd24dc75 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h @@ -111,6 +111,8 @@ class AIRSIM_API ASimModeBase : public AActor const PawnSimApi* sim_api) const; protected: //optional overrides + virtual APawn* createVehiclePawn(const AirSimSettings::VehicleSetting& vehicle_setting); + virtual std::unique_ptr createVehicleApi(APawn* vehicle_pawn); virtual void setupVehiclesAndCamera(); virtual void setupInputBindings(); //called when SimMode should handle clock speed setting From 0463bb6efa262973ee452106cd80fd389f87d7aa Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Sun, 7 Feb 2021 14:51:18 +0530 Subject: [PATCH 08/29] Store correct vehicle settings instead of default one --- AirLib/include/common/AirSimSettings.hpp | 15 +++++++++++---- .../Plugins/AirSim/Source/SimMode/SimModeBase.cpp | 6 +----- Unreal/Plugins/AirSim/Source/WorldSimApi.cpp | 13 +++++-------- 3 files changed, 17 insertions(+), 17 deletions(-) diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index 4d6553e5df..f9e9d68323 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/AirLib/include/common/AirSimSettings.hpp @@ -427,12 +427,19 @@ struct AirSimSettings { } // This is for the case when a new vehicle is made on the fly, at runtime - void addVehicleSetting(const VehicleSetting &vehicle_setting) + void addVehicleSetting(const std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path="") { - std::unique_ptr vehicle_setting_p = std::unique_ptr(new VehicleSetting()); + auto vehicle_setting = std::unique_ptr(new VehicleSetting()); - // Usually we have a pointer to an entry from the json, but here we have to make a new one - vehicles[vehicle_setting.vehicle_name] = std::move(vehicle_setting_p); + vehicle_setting->vehicle_name = vehicle_name; + vehicle_setting->vehicle_type = vehicle_type; + vehicle_setting->position = pose.position; + vehicle_setting->pawn_path = pawn_path; + + VectorMath::toEulerianAngle(pose.orientation, vehicle_setting->rotation.pitch, + vehicle_setting->rotation.roll, vehicle_setting->rotation.yaw); + + vehicles[vehicle_name] = std::move(vehicle_setting); } const VehicleSetting* getVehicleSetting(const std::string& vehicle_name) const diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index d44fffb3df..612a7bf577 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -602,17 +602,13 @@ std::unique_ptr ASimModeBase::createVehicleApi(APawn* vehicle_pawn) return vehicle_sim_api; } -bool ASimModeBase::createVehicleAtRuntime(const AirSimSettings::VehicleSetting &vehicle_setting) +bool ASimModeBase::createVehicleAtRuntime(const AirSimSettings::VehicleSetting& vehicle_setting) { if (!isVehicleTypeSupported(vehicle_setting.vehicle_type)) { Utils::log(Utils::stringf("Vehicle type %s is not supported in this game mode", vehicle_setting.vehicle_type.c_str()), Utils::kLogLevelWarn); return false; } - // Retroactively adjust AirSimSettings, so it's like we knew about this vehicle all along - // (Other places in the code use this for reference) - AirSimSettings::singleton().addVehicleSetting(vehicle_setting); - auto spawned_pawn = createVehiclePawn(vehicle_setting); auto vehicle_sim_api = createVehicleApi(spawned_pawn); diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index 58ca6c6fe7..fdb7dfe100 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -270,21 +270,18 @@ void WorldSimApi::setTimeOfDay(bool is_enabled, const std::string& start_datetim bool WorldSimApi::createVehicleAtRuntime(const std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path) { - // Create settings object - AirSimSettings::VehicleSetting vehicle_setting; + // TODO: Figure out a better way to add more fields + // Maybe allow passing a JSON string for the vehicle settings? - vehicle_setting.vehicle_name = vehicle_name; - vehicle_setting.vehicle_type = vehicle_type; - vehicle_setting.pawn_path = pawn_path; - vehicle_setting.position = pose.position; - VectorMath::toEulerianAngle(pose.orientation, vehicle_setting.rotation.pitch, vehicle_setting.rotation.roll, vehicle_setting.rotation.yaw); + // Retroactively adjust AirSimSettings, so it's like we knew about this vehicle all along + AirSimSettings::singleton().addVehicleSetting(vehicle_name, vehicle_type, pose, pawn_path); bool result; // We need to run this code on the main game thread, since it iterates over actors FGraphEventRef task = FFunctionGraphTask::CreateAndDispatchWhenReady([&]() { - result = simmode_->createVehicleAtRuntime(vehicle_setting); + result = simmode_->createVehicleAtRuntime(*(AirSimSettings::singleton().getVehicleSetting(vehicle_name))); }, TStatId(), NULL, ENamedThreads::GameThread); // Wait for the result From 83325e2b7f02ec08d7d19da983e28512f2aeb9b1 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Sun, 19 Jan 2020 21:54:26 +0530 Subject: [PATCH 09/29] Add example Python script for spawn vehicle through API --- PythonClient/multirotor/add_drone.py | 46 ++++++++++++++++++++++++++++ 1 file changed, 46 insertions(+) create mode 100644 PythonClient/multirotor/add_drone.py diff --git a/PythonClient/multirotor/add_drone.py b/PythonClient/multirotor/add_drone.py new file mode 100644 index 0000000000..fd740dd5d5 --- /dev/null +++ b/PythonClient/multirotor/add_drone.py @@ -0,0 +1,46 @@ +import setup_path +import airsim +import tempfile +import os +import numpy as np +import cv2 +import pprint + +# connect to the AirSim simulator +client = airsim.MultirotorClient() +client.confirmConnection() + +# add new vehicle +vehicle_name = "Drone2" +pose = airsim.Pose(airsim.Vector3r(0, 0, 0), airsim.to_quaternion(0, 0, 0)) + +client.simAddVehicle(vehicle_name, "simpleflight", pose) +client.enableApiControl(True, vehicle_name) +client.armDisarm(True, vehicle_name) +client.takeoffAsync(10.0, vehicle_name) + +requests = [airsim.ImageRequest("0", airsim.ImageType.DepthVis), #depth visualization image + airsim.ImageRequest("1", airsim.ImageType.DepthPerspective, True), #depth in perspective projection + airsim.ImageRequest("1", airsim.ImageType.Scene), #scene vision image in png format + airsim.ImageRequest("1", airsim.ImageType.Scene, False, False)] #scene vision image in uncompressed RGBA array + +responses = client.simGetImages(requests, vehicle_name=vehicle_name) +print('Retrieved images: %d' % len(responses)) + +tmp_dir = os.path.join(tempfile.gettempdir(), "airsim_drone") +print ("Saving images to %s" % tmp_dir) +try: + os.makedirs(tmp_dir) +except OSError: + if not os.path.isdir(tmp_dir): + raise + +for idx, response in enumerate(responses): + filename = os.path.join(tmp_dir, str(idx)) + + if response.pixels_as_float: + print("Type %d, size %d, pos %s" % (response.image_type, len(response.image_data_float), pprint.pformat(response.camera_position))) + airsim.write_pfm(os.path.normpath(filename + '.pfm'), airsim.get_pfm_array(response)) + else: + print("Type %d, size %d, pos %s" % (response.image_type, len(response.image_data_uint8), pprint.pformat(response.camera_position))) + airsim.write_file(os.path.normpath(filename + '.png'), response.image_data_uint8) \ No newline at end of file From c12929adab83f780271957ee6d4dacf51b0ee6a0 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Thu, 4 Feb 2021 20:13:00 +0530 Subject: [PATCH 10/29] Fix HelloSpawnedDrones C++ file to use new API, naming convention, convert to spaces --- HelloSpawnedDrones/HelloSpawnedDrones.cpp | 244 +++++++++++----------- 1 file changed, 123 insertions(+), 121 deletions(-) diff --git a/HelloSpawnedDrones/HelloSpawnedDrones.cpp b/HelloSpawnedDrones/HelloSpawnedDrones.cpp index 9bfd4e4b6c..a1398d3a2d 100644 --- a/HelloSpawnedDrones/HelloSpawnedDrones.cpp +++ b/HelloSpawnedDrones/HelloSpawnedDrones.cpp @@ -19,154 +19,156 @@ STRICT_MODE_ON void runSingleClient(uint16_t port, int ordinal) { - using namespace msr::airlib; - const char host[] = "localhost"; - float timeout_s = 60; + using namespace msr::airlib; + const char host[] = "localhost"; + float timeout_s = 60; - try - { - msr::airlib::MultirotorRpcLibClient *client = new msr::airlib::MultirotorRpcLibClient(host, port, timeout_s); - std::cout << "Confirming connections..." << std::endl; - client->confirmConnection(); + try + { + MultirotorRpcLibClient *client = new MultirotorRpcLibClient(host, port, timeout_s); + std::cout << "Confirming connections..." << std::endl; + client->confirmConnection(); - std::string vehicleName = "UAV_" + std::to_string(ordinal); - std::cout << "Vehicle name:" << vehicleName << std::endl; + std::string vehicle_name = "UAV_" + std::to_string(ordinal); + std::cout << "Vehicle name:" << vehicle_name << std::endl; - client->simAddVehicle(vehicleName, "simpleflight", "", 0, 5.0f * (ordinal + 1), 0); + Pose pose(Vector3r(0, 5.0f * (ordinal + 1), 0), Quaternionr(0, 0, 0, 0)); + client->simAddVehicle(vehicle_name, "simpleflight", pose, ""); - // This is a bit crude, but give it a moment to settle on the ground, else takeoff will fail - std::this_thread::sleep_for(std::chrono::duration(2)); + // This is a bit crude, but give it a moment to settle on the ground, else takeoff will fail + std::this_thread::sleep_for(std::chrono::duration(2)); - // moveByVelocityZ is an offboard operation, so we need to set offboard mode. - client->enableApiControl(true, vehicleName); - client->armDisarm(true, vehicleName); + // moveByVelocityZ is an offboard operation, so we need to set offboard mode. + client->enableApiControl(true, vehicle_name); + client->armDisarm(true, vehicle_name); - auto groundPosition = client->getMultirotorState(vehicleName).getPosition(); - float groundZ = groundPosition.z(); // current position (NED coordinate system). + auto groundPosition = client->getMultirotorState(vehicle_name).getPosition(); + float groundZ = groundPosition.z(); // current position (NED coordinate system). - float takeoffTimeout = 5; - std::cout << "Initiating takeoff for " << vehicleName << "..." << std::endl; - client->takeoffAsync(takeoffTimeout, vehicleName)->waitOnLastTask(); - std::cout << "Completed takeoff for " << vehicleName << "..." << std::endl; + float takeoffTimeout = 5; + std::cout << "Initiating takeoff for " << vehicle_name << "..." << std::endl; + client->takeoffAsync(takeoffTimeout, vehicle_name)->waitOnLastTask(); + std::cout << "Completed takeoff for " << vehicle_name << "..." << std::endl; - const float speed = 3.0f; + const float speed = 3.0f; - // switch to explicit hover mode so that this is the fallback when - // move* commands are finished. - std::cout << "Initiating hover for " << vehicleName << "..." << std::endl; - client->hoverAsync(vehicleName)->waitOnLastTask(); - std::cout << "Completed hover for " << vehicleName << "..." << std::endl; + // switch to explicit hover mode so that this is the fallback when + // move* commands are finished. + std::cout << "Initiating hover for " << vehicle_name << "..." << std::endl; + client->hoverAsync(vehicle_name)->waitOnLastTask(); + std::cout << "Completed hover for " << vehicle_name << "..." << std::endl; - auto position = client->getMultirotorState(vehicleName).getPosition(); - float duration = 1; - float z = position.z(); // current position (NED coordinate system). + auto position = client->getMultirotorState(vehicle_name).getPosition(); + float duration = 1; + float z = position.z(); // current position (NED coordinate system). - // Altitude difference between each platform, in meters - const float altitudeDelta = 1.0f; + // Altitude difference between each platform, in meters + const float altitudeDelta = 1.0f; - z -= ordinal * altitudeDelta; - float timeout = 10.0f; - client->moveToZAsync(z, speed, timeout, msr::airlib::YawMode(), -1.0f, 1.0f, vehicleName)->waitOnLastTask(); + z -= ordinal * altitudeDelta; + float timeout = 10.0f; + client->moveToZAsync(z, speed, timeout, YawMode(), -1.0f, 1.0f, vehicle_name)->waitOnLastTask(); - std::cout << "Completed move to z " << z << " for " << vehicleName << "..." << std::endl; - std::cout << "Flying in a 10m box pattern at 3 m/s velocity" << std::endl; + std::cout << "Completed move to z " << z << " for " << vehicle_name << "..." << std::endl; + std::cout << "Flying in a 10m box pattern at 3 m/s velocity" << std::endl; - const float size = 5.0f; - duration = size / speed; - DrivetrainType driveTrain = DrivetrainType::ForwardOnly; - YawMode yaw_mode(true, 0); + const float size = 5.0f; + duration = size / speed; + DrivetrainType driveTrain = DrivetrainType::ForwardOnly; + YawMode yaw_mode(true, 0); - position = client->getMultirotorState(vehicleName).getPosition(); - std::cout << "Position of " << port << ": " << position << std::endl; - z = position.z(); // current position (NED coordinate system). + position = client->getMultirotorState(vehicle_name).getPosition(); + std::cout << "Position of " << port << ": " << position << std::endl; + z = position.z(); // current position (NED coordinate system). - std::cout << "moveByVelocityZ(" << speed << ", 0, " << z << "," << duration << ")" << std::endl; - client->moveByVelocityZAsync(speed, 0, z, duration, driveTrain, yaw_mode, vehicleName); - std::this_thread::sleep_for(std::chrono::duration(duration)); - std::cout << "moveByVelocityZ(0, " << speed << "," << z << "," << duration << ")" << std::endl; - client->moveByVelocityZAsync(0, speed, z, duration, driveTrain, yaw_mode, vehicleName); - std::this_thread::sleep_for(std::chrono::duration(duration)); - std::cout << "moveByVelocityZ(" << -speed << ", 0, " << z << "," << duration << ")" << std::endl; - client->moveByVelocityZAsync(-speed, 0, z, duration, driveTrain, yaw_mode, vehicleName); - std::this_thread::sleep_for(std::chrono::duration(duration)); - std::cout << "moveByVelocityZ(0, " << -speed << "," << z << "," << duration << ")" << std::endl; - client->moveByVelocityZAsync(0, -speed, z, duration, driveTrain, yaw_mode, vehicleName); + std::cout << "moveByVelocityZ(" << speed << ", 0, " << z << "," << duration << ")" << std::endl; + client->moveByVelocityZAsync(speed, 0, z, duration, driveTrain, yaw_mode, vehicle_name); + std::this_thread::sleep_for(std::chrono::duration(duration)); + std::cout << "moveByVelocityZ(0, " << speed << "," << z << "," << duration << ")" << std::endl; + client->moveByVelocityZAsync(0, speed, z, duration, driveTrain, yaw_mode, vehicle_name); + std::this_thread::sleep_for(std::chrono::duration(duration)); + std::cout << "moveByVelocityZ(" << -speed << ", 0, " << z << "," << duration << ")" << std::endl; + client->moveByVelocityZAsync(-speed, 0, z, duration, driveTrain, yaw_mode, vehicle_name); + std::this_thread::sleep_for(std::chrono::duration(duration)); + std::cout << "moveByVelocityZ(0, " << -speed << "," << z << "," << duration << ")" << std::endl; + client->moveByVelocityZAsync(0, -speed, z, duration, driveTrain, yaw_mode, vehicle_name); - std::this_thread::sleep_for(std::chrono::duration(duration)); + std::this_thread::sleep_for(std::chrono::duration(duration)); - client->moveToZAsync(groundZ - 0.5f, speed, timeout, msr::airlib::YawMode(), -1.0f, 1.0f, vehicleName)->waitOnLastTask(); + client->moveToZAsync(groundZ - 0.5f, speed, timeout, YawMode(), -1.0f, 1.0f, vehicle_name)->waitOnLastTask(); - std::cout << "Hovering..." << std::endl; - client->hoverAsync(vehicleName)->waitOnLastTask(); + std::cout << "Hovering..." << std::endl; + client->hoverAsync(vehicle_name)->waitOnLastTask(); - client->enableApiControl(true, vehicleName); + client->enableApiControl(true, vehicle_name); - std::cout << "Landing..." << std::endl; - client->landAsync(timeout, vehicleName)->waitOnLastTask(); - std::this_thread::sleep_for(std::chrono::duration(5)); + std::cout << "Landing..." << std::endl; + client->landAsync(timeout, vehicle_name)->waitOnLastTask(); + std::this_thread::sleep_for(std::chrono::duration(5)); - std::cout << "Disarming..." << std::endl; - client->armDisarm(false, vehicleName); + std::cout << "Disarming..." << std::endl; + client->armDisarm(false, vehicle_name); - std::cout << "Done!..." << std::endl; + std::cout << "Done!..." << std::endl; - delete client; + delete client; - std::this_thread::sleep_for(std::chrono::duration(50)); - } - catch (rpc::rpc_error& e) - { - std::string msg = e.get_error().as(); - std::cout << "Exception raised by the API, something went wrong." << std::endl << msg << std::endl; - } + std::this_thread::sleep_for(std::chrono::duration(50)); + } + catch (rpc::rpc_error& e) + { + std::string msg = e.get_error().as(); + std::cout << "Exception raised by the API, something went wrong." << std::endl << msg << std::endl; + } } int main(int argc, char *argv[]) { - using namespace msr::airlib; - - uint16_t rpcPort = 41451; - int numPlatforms = 1; - - std::cout << "argc is " << argc << std::endl; - if (argc > 1) - { - std::cout << "Num plats string: " << argv[1] << std::endl; - int numPlatsInt = atoi(argv[1]); - numPlatforms = static_cast(numPlatsInt); - } - - std::cout << "First port is " << rpcPort << std::endl; - std::cout << "Num platforms: " << numPlatforms << std::endl; - std::cout << "Making clients..." << std::endl; - - - try - { - std::cout << "Press Enter to begin..." << std::endl; std::cin.get(); - - std::vector clientThreads; - - // Count down, so the first one can easily go the highest (without knowing count) - int clientOrdinal = numPlatforms - 1; - for (int i = 0; i < numPlatforms; i++) - { - clientThreads.push_back(std::thread(runSingleClient, rpcPort, clientOrdinal)); - clientOrdinal--; - - std::this_thread::sleep_for(std::chrono::duration(0.1)); - } - - for (auto &toJoin : clientThreads) - { - toJoin.join(); - } - } - catch (rpc::rpc_error& e) - { - std::string msg = e.get_error().as(); - std::cout << "Exception raised by the API, something went wrong." << std::endl << msg << std::endl; - } - - return 0; + using namespace msr::airlib; + + uint16_t rpc_port = 41451; + int num_platforms = 1; + + std::cout << "argc is " << argc << std::endl; + if (argc > 1) + { + std::cout << "Num plats string: " << argv[1] << std::endl; + int numPlatsInt = atoi(argv[1]); + num_platforms = static_cast(numPlatsInt); + } + + std::cout << "First port is " << rpc_port << std::endl; + std::cout << "Num platforms: " << num_platforms << std::endl; + std::cout << "Making clients..." << std::endl; + + + try + { + std::cout << "Press Enter to begin..." << std::endl; + std::cin.get(); + + std::vector clientThreads; + + // Count down, so the first one can easily go the highest (without knowing count) + int clientOrdinal = num_platforms - 1; + for (int i = 0; i < num_platforms; i++) + { + clientThreads.push_back(std::thread(runSingleClient, rpc_port, clientOrdinal)); + clientOrdinal--; + + std::this_thread::sleep_for(std::chrono::duration(0.1)); + } + + for (auto &toJoin : clientThreads) + { + toJoin.join(); + } + } + catch (rpc::rpc_error& e) + { + std::string msg = e.get_error().as(); + std::cout << "Exception raised by the API, something went wrong." << std::endl << msg << std::endl; + } + + return 0; } From 723d36ffcbf2f53f00e06c5e99b8e0b8b6c9a88a Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Sat, 13 Feb 2021 22:35:29 +0530 Subject: [PATCH 11/29] Adapt to AirSim naming conventions, cleanup --- AirLib/include/api/WorldSimApiBase.hpp | 2 +- AirLib/src/api/RpcLibServerBase.cpp | 2 +- .../AirSim/Source/SimMode/SimModeBase.cpp | 24 ++++++++++++------- .../AirSim/Source/SimMode/SimModeBase.h | 3 ++- Unreal/Plugins/AirSim/Source/WorldSimApi.cpp | 20 ++++------------ Unreal/Plugins/AirSim/Source/WorldSimApi.h | 2 +- 6 files changed, 24 insertions(+), 29 deletions(-) diff --git a/AirLib/include/api/WorldSimApiBase.hpp b/AirLib/include/api/WorldSimApiBase.hpp index 541aa36ca3..baaa3c6714 100644 --- a/AirLib/include/api/WorldSimApiBase.hpp +++ b/AirLib/include/api/WorldSimApiBase.hpp @@ -47,7 +47,7 @@ class WorldSimApiBase { virtual bool setSegmentationObjectID(const std::string& mesh_name, int object_id, bool is_name_regex = false) = 0; virtual int getSegmentationObjectID(const std::string& mesh_name) const = 0; - virtual bool createVehicleAtRuntime(const std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path = "") = 0; + virtual bool addVehicle(const std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path = "") = 0; virtual void printLogMessage(const std::string& message, const std::string& message_param = "", unsigned char severity = 0) = 0; diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index 1d8b46ac0c..aa86075938 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -155,7 +155,7 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& pimpl_->server.bind("simAddVehicle", [&](const std::string& vehicle_name, const std::string& vehicle_type, const RpcLibAdapatorsBase::Pose& pose, const std::string& pawn_path) -> bool { - return getWorldSimApi()->createVehicleAtRuntime(vehicle_name, vehicle_type, pose.to(), pawn_path); + return getWorldSimApi()->addVehicle(vehicle_name, vehicle_type, pose.to(), pawn_path); }); pimpl_->server.bind("simSetVehiclePose", [&](const RpcLibAdapatorsBase::Pose &pose, bool ignore_collision, const std::string& vehicle_name) -> void { diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index 612a7bf577..9135f153c4 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -602,25 +602,31 @@ std::unique_ptr ASimModeBase::createVehicleApi(APawn* vehicle_pawn) return vehicle_sim_api; } -bool ASimModeBase::createVehicleAtRuntime(const AirSimSettings::VehicleSetting& vehicle_setting) +bool ASimModeBase::createVehicleAtRuntime(const std::string& vehicle_name, const std::string& vehicle_type, + const msr::airlib::Pose& pose, const std::string& pawn_path) { - if (!isVehicleTypeSupported(vehicle_setting.vehicle_type)) { - Utils::log(Utils::stringf("Vehicle type %s is not supported in this game mode", vehicle_setting.vehicle_type.c_str()), Utils::kLogLevelWarn); + if (!isVehicleTypeSupported(vehicle_type)) { + Utils::log(Utils::stringf("Vehicle type %s is not supported in this game mode", vehicle_type.c_str()), Utils::kLogLevelWarn); return false; } - auto spawned_pawn = createVehiclePawn(vehicle_setting); + // TODO: Figure out a better way to add more fields + // Maybe allow passing a JSON string for the vehicle settings? + + // Retroactively adjust AirSimSettings, so it's like we knew about this vehicle all along + AirSimSettings::singleton().addVehicleSetting(vehicle_name, vehicle_type, pose, pawn_path); + const auto* vehicle_setting = getSettings().getVehicleSetting(vehicle_name); + + auto spawned_pawn = createVehiclePawn(*vehicle_setting); auto vehicle_sim_api = createVehicleApi(spawned_pawn); - auto vehicle_sim_api_p = vehicle_sim_api.get(); - std::string vehicle_name = vehicle_sim_api->getVehicleName(); // Usually physics registration happens at init, in ASimModeWorldBase::initializeForPlay(), but not in this case - vehicle_sim_api_p->reset(); - registerPhysicsBody(vehicle_sim_api_p); + vehicle_sim_api->reset(); + registerPhysicsBody(vehicle_sim_api.get()); // Can't be done before the vehicle apis have been created - if ((vehicle_setting.is_fpv_vehicle || !getApiProvider()->hasDefaultVehicle()) && vehicle_name != "") + if ((vehicle_setting->is_fpv_vehicle || !getApiProvider()->hasDefaultVehicle()) && vehicle_name != "") getApiProvider()->makeDefaultVehicle(vehicle_name); vehicle_sim_apis_.push_back(std::move(vehicle_sim_api)); diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h index bfcd24dc75..418f1ad87a 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h @@ -75,7 +75,8 @@ class AIRSIM_API ASimModeBase : public AActor void stopApiServer(); bool isApiServerStarted(); - bool createVehicleAtRuntime(const msr::airlib::AirSimSettings::VehicleSetting &vehicle_setting); + bool createVehicleAtRuntime(const std::string& vehicle_name, const std::string& vehicle_type, + const msr::airlib::Pose& pose, const std::string& pawn_path = ""); const NedTransform& getGlobalNedTransform(); diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index fdb7dfe100..6de0bacc01 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -268,24 +268,12 @@ void WorldSimApi::setTimeOfDay(bool is_enabled, const std::string& start_datetim celestial_clock_speed, update_interval_secs, move_sun); } -bool WorldSimApi::createVehicleAtRuntime(const std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path) +bool WorldSimApi::addVehicle(const std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path) { - // TODO: Figure out a better way to add more fields - // Maybe allow passing a JSON string for the vehicle settings? - - // Retroactively adjust AirSimSettings, so it's like we knew about this vehicle all along - AirSimSettings::singleton().addVehicleSetting(vehicle_name, vehicle_type, pose, pawn_path); - bool result; - - // We need to run this code on the main game thread, since it iterates over actors - FGraphEventRef task = FFunctionGraphTask::CreateAndDispatchWhenReady([&]() - { - result = simmode_->createVehicleAtRuntime(*(AirSimSettings::singleton().getVehicleSetting(vehicle_name))); - }, TStatId(), NULL, ENamedThreads::GameThread); - - // Wait for the result - FTaskGraphInterface::Get().WaitUntilTaskCompletes(task); + UAirBlueprintLib::RunCommandOnGameThread([&]() { + result = simmode_->createVehicleAtRuntime(vehicle_name, vehicle_type, pose, pawn_path); + }, true); return result; } diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.h b/Unreal/Plugins/AirSim/Source/WorldSimApi.h index 4072c33b3a..8ca7b8de7e 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.h +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.h @@ -38,7 +38,7 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase { virtual bool setSegmentationObjectID(const std::string& mesh_name, int object_id, bool is_name_regex = false) override; virtual int getSegmentationObjectID(const std::string& mesh_name) const override; - virtual bool createVehicleAtRuntime(const std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path = "") override; + virtual bool addVehicle(const std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path = "") override; virtual void printLogMessage(const std::string& message, const std::string& message_param = "", unsigned char severity = 0) override; From 74e5a23b8df7309507a36828891ae0728b8061c2 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Mon, 8 Feb 2021 09:16:43 +0530 Subject: [PATCH 12/29] Unity: Add unimplemented simAddVehicle API method --- Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp | 8 ++++++++ Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h | 1 + 2 files changed, 9 insertions(+) diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp index 4679fea828..7adcf58ec5 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp @@ -195,4 +195,12 @@ void WorldSimApi::setWind(const Vector3r& wind) const simmode_->setWind(wind); }; +bool addVehicle(const std::string& vehicle_name, const std::string& vehicle_type, const WorldSimApi::Pose& pose, const std::string& pawn_path) +{ + throw std::invalid_argument(common_utils::Utils::stringf( + "addVehicle is not supported on unity").c_str()); + return false; +} + + #pragma endregion diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h index d19928bd9b..548f4b2149 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h @@ -64,6 +64,7 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase virtual void setWind(const Vector3r& wind) const override; virtual bool createVoxelGrid(const Vector3r& position, const int& x_size, const int& y_size, const int& z_size, const float& res, const std::string& output_file) override; + virtual bool addVehicle(const std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path = "") override; private: SimModeBase * simmode_; From 0fa412efadbefdd35b8b6671a12bcc3f16989fe0 Mon Sep 17 00:00:00 2001 From: zimmy87 Date: Fri, 5 Mar 2021 05:21:18 -0800 Subject: [PATCH 13/29] Updating AirSim solution for new project HelloSpawnedDrones This change adds the HelloSpawnedDrones.vcxproj to AirSim.sln and also sets output and intermediate directories for the new project --- AirSim.sln | 16 +++++++++++++- HelloSpawnedDrones/HelloSpawnedDrones.vcxproj | 22 +++++++++++++------ 2 files changed, 30 insertions(+), 8 deletions(-) diff --git a/AirSim.sln b/AirSim.sln index 5d9bf8920d..2ff11e6bfe 100644 --- a/AirSim.sln +++ b/AirSim.sln @@ -1,6 +1,6 @@  Microsoft Visual Studio Solution File, Format Version 12.00 -# Visual Studio 16 +# Visual Studio Version 16 VisualStudioVersion = 16.0.27428.2043 MinimumVisualStudioVersion = 10.0.40219.1 Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "DroneShell", "DroneShell\DroneShell.vcxproj", "{9FE9234B-373A-4D5A-AD6B-FB0B593312DD}" @@ -43,6 +43,8 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "sgmstereo", "SGM\src\sgmste EndProject Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "stereoPipeline", "SGM\src\stereoPipeline\stereoPipeline.vcxproj", "{E512EB59-4EAB-49D1-9174-0CAF1B40CED0}" EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "HelloSpawnedDrones", "HelloSpawnedDrones\HelloSpawnedDrones.vcxproj", "{99CBF376-5EBA-4164-A657-E7D708C9D685}" +EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution Debug|Any CPU = Debug|Any CPU @@ -207,6 +209,18 @@ Global {E512EB59-4EAB-49D1-9174-0CAF1B40CED0}.Release|x64.Build.0 = Release|x64 {E512EB59-4EAB-49D1-9174-0CAF1B40CED0}.Release|x86.ActiveCfg = Release|Win32 {E512EB59-4EAB-49D1-9174-0CAF1B40CED0}.Release|x86.Build.0 = Release|Win32 + {99CBF376-5EBA-4164-A657-E7D708C9D685}.Debug|Any CPU.ActiveCfg = Debug|Win32 + {99CBF376-5EBA-4164-A657-E7D708C9D685}.Debug|ARM.ActiveCfg = Debug|Win32 + {99CBF376-5EBA-4164-A657-E7D708C9D685}.Debug|x64.ActiveCfg = Debug|x64 + {99CBF376-5EBA-4164-A657-E7D708C9D685}.Debug|x64.Build.0 = Debug|x64 + {99CBF376-5EBA-4164-A657-E7D708C9D685}.Debug|x86.ActiveCfg = Debug|Win32 + {99CBF376-5EBA-4164-A657-E7D708C9D685}.Debug|x86.Build.0 = Debug|Win32 + {99CBF376-5EBA-4164-A657-E7D708C9D685}.Release|Any CPU.ActiveCfg = Release|Win32 + {99CBF376-5EBA-4164-A657-E7D708C9D685}.Release|ARM.ActiveCfg = Release|Win32 + {99CBF376-5EBA-4164-A657-E7D708C9D685}.Release|x64.ActiveCfg = Release|x64 + {99CBF376-5EBA-4164-A657-E7D708C9D685}.Release|x64.Build.0 = Release|x64 + {99CBF376-5EBA-4164-A657-E7D708C9D685}.Release|x86.ActiveCfg = Release|Win32 + {99CBF376-5EBA-4164-A657-E7D708C9D685}.Release|x86.Build.0 = Release|Win32 EndGlobalSection GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE diff --git a/HelloSpawnedDrones/HelloSpawnedDrones.vcxproj b/HelloSpawnedDrones/HelloSpawnedDrones.vcxproj index 3f632f8e73..2df07271a5 100644 --- a/HelloSpawnedDrones/HelloSpawnedDrones.vcxproj +++ b/HelloSpawnedDrones/HelloSpawnedDrones.vcxproj @@ -23,32 +23,32 @@ {99CBF376-5EBA-4164-A657-E7D708C9D685} Win32Proj HelloSpawnedDrones - 10.0.17134.0 + 10.0 Application true - v141 + v142 Unicode Application false - v141 + v142 true Unicode Application true - v141 + v142 Unicode Application false - v141 + v142 true Unicode @@ -70,17 +70,25 @@ - + true + $(ProjectDir)build\$(Platform)\$(Configuration)\ + $(ProjectDir)temp\$(ProjectName)\$(Platform)\$(Configuration)\ - + true + $(ProjectDir)temp\$(ProjectName)\$(Platform)\$(Configuration)\ + $(ProjectDir)build\$(Platform)\$(Configuration)\ false + $(ProjectDir)build\$(Platform)\$(Configuration)\ + $(ProjectDir)temp\$(ProjectName)\$(Platform)\$(Configuration)\ false + $(ProjectDir)temp\$(ProjectName)\$(Platform)\$(Configuration)\ + $(ProjectDir)build\$(Platform)\$(Configuration)\ From 672a8350dc03de6a1b537d62557b0b2446762e57 Mon Sep 17 00:00:00 2001 From: Kerry Moffitt Date: Sun, 28 Apr 2019 10:05:13 -0400 Subject: [PATCH 14/29] Support for adding vehicles dynamically at run-time via RPC --- AirLib/include/api/RpcLibClientBase.hpp | 3 +- AirLib/include/api/WorldSimApiBase.hpp | 4 ++ AirLib/include/common/AirSimSettings.hpp | 9 +++ AirLib/include/physics/PhysicsWorld.hpp | 7 ++ AirLib/src/api/RpcLibClientBase.cpp | 13 ++-- AirLib/src/api/RpcLibServerBase.cpp | 41 ++++++++--- .../AirSim/Source/SimMode/SimModeBase.cpp | 68 +++++++++++++++++++ .../AirSim/Source/SimMode/SimModeBase.h | 3 + .../Source/SimMode/SimModeWorldBase.cpp | 6 ++ .../AirSim/Source/SimMode/SimModeWorldBase.h | 3 + Unreal/Plugins/AirSim/Source/WorldSimApi.cpp | 16 +++++ Unreal/Plugins/AirSim/Source/WorldSimApi.h | 2 + 12 files changed, 159 insertions(+), 16 deletions(-) diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index 1d17fe9a66..ef9cdc8415 100644 --- a/AirLib/include/api/RpcLibClientBase.hpp +++ b/AirLib/include/api/RpcLibClientBase.hpp @@ -69,7 +69,7 @@ class RpcLibClientBase { void simPlotLineStrip(const vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent); void simPlotLineList(const vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent); void simPlotArrows(const vector& points_start, const vector& points_end, const vector& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent); - void simPlotStrings(const vector& strings, const vector& positions, float scale, const vector& color_rgba, float duration); + void simPlotStrings(const vector& strings, const vector& positions, float scale, const vector& color_rgba, float duration); void simPlotTransforms(const vector& poses, float scale, float thickness, float duration, bool is_persistent); void simPlotTransformsWithNames(const vector& poses, const vector& names, float tf_scale, float tf_thickness, float text_scale, const vector& text_color_rgba, float duration); @@ -97,6 +97,7 @@ class RpcLibClientBase { vector simGetImage(const std::string& camera_name, ImageCaptureBase::ImageType type, const std::string& vehicle_name = ""); vector simGetMeshPositionVertexBuffers(); + bool simAddVehicle(const std::string& vehicle_name, const std::string& vehicle_type, const std::string& pawn_path, float north, float east, float down); CollisionInfo simGetCollisionInfo(const std::string& vehicle_name = "") const; diff --git a/AirLib/include/api/WorldSimApiBase.hpp b/AirLib/include/api/WorldSimApiBase.hpp index e98162b306..5cd4c5257c 100644 --- a/AirLib/include/api/WorldSimApiBase.hpp +++ b/AirLib/include/api/WorldSimApiBase.hpp @@ -5,6 +5,8 @@ #define air_WorldSimApiBase_hpp #include "common/CommonStructs.hpp" +#include "common/AirSimSettings.hpp" + namespace msr { namespace airlib { @@ -45,6 +47,8 @@ class WorldSimApiBase { virtual bool setSegmentationObjectID(const std::string& mesh_name, int object_id, bool is_name_regex = false) = 0; virtual int getSegmentationObjectID(const std::string& mesh_name) const = 0; + virtual bool createVehicleAtRuntime(AirSimSettings::VehicleSetting& vehicle_setting) = 0; + virtual void printLogMessage(const std::string& message, const std::string& message_param = "", unsigned char severity = 0) = 0; diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index 921da43417..9aeb937ee3 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/AirLib/include/common/AirSimSettings.hpp @@ -435,6 +435,15 @@ struct AirSimSettings { settings_json.saveJSonFile(settings_filename); } + // This is for the case when a new vehicle is made on the fly, at runtime + void addVehicleSetting(const VehicleSetting &vehicle_setting) + { + std::unique_ptr vehicle_setting_p = std::unique_ptr(new VehicleSetting()); + + // Usually we have a pointer to an entry from the json, but here we have to make a new one + vehicles[vehicle_setting.vehicle_name] = std::move(vehicle_setting_p); + } + const VehicleSetting* getVehicleSetting(const std::string& vehicle_name) const { auto it = vehicles.find(vehicle_name); diff --git a/AirLib/include/physics/PhysicsWorld.hpp b/AirLib/include/physics/PhysicsWorld.hpp index f44e334189..dbf0ec8302 100644 --- a/AirLib/include/physics/PhysicsWorld.hpp +++ b/AirLib/include/physics/PhysicsWorld.hpp @@ -43,6 +43,13 @@ class PhysicsWorld { unlock(); } + void addBody(UpdatableObject* body) + { + lock(); + world_.insert(body); + unlock(); + } + uint64_t getUpdatePeriodNanos() const { return update_period_nanos_; diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index 60f67d5c5b..448635573b 100644 --- a/AirLib/src/api/RpcLibClientBase.cpp +++ b/AirLib/src/api/RpcLibClientBase.cpp @@ -40,7 +40,7 @@ STRICT_MODE_OFF STRICT_MODE_ON #ifdef _MSC_VER __pragma(warning( disable : 4239)) -#endif +#endif namespace msr { namespace airlib { @@ -125,7 +125,7 @@ void RpcLibClientBase::confirmConnection() while (getConnectionState() != RpcLibClientBase::ConnectionState::Connected) { std::cout << "X" << std::flush; - clock->sleep_for(pause_time); + clock->sleep_for(pause_time); } std::cout << std::endl << "Connected!" << std::endl; @@ -133,7 +133,7 @@ void RpcLibClientBase::confirmConnection() auto client_ver = getClientVersion(); auto server_min_ver = getMinRequiredServerVersion(); auto client_min_ver = getMinRequiredClientVersion(); - + std::string ver_info = Utils::stringf("Client Ver:%i (Min Req:%i), Server Ver:%i (Min Req:%i)", client_ver, client_min_ver, server_ver, server_min_ver); @@ -221,7 +221,7 @@ void RpcLibClientBase::simSetTraceLine(const std::vector& color_rgba, flo vector RpcLibClientBase::simGetImages(vector request, const std::string& vehicle_name) { - const auto& response_adaptor = pimpl_->client.call("simGetImages", + const auto& response_adaptor = pimpl_->client.call("simGetImages", RpcLibAdapatorsBase::ImageRequest::from(request), vehicle_name) .as>(); @@ -243,6 +243,11 @@ vector RpcLibClientBase::simGetMeshPositionVe return RpcLibAdapatorsBase::MeshPositionVertexBuffersResponse::to(response_adaptor); } +bool RpcLibClientBase::simAddVehicle(const std::string& vehicle_name, const std::string& vehicle_type, const std::string& pawn_path, float north, float east, float down) +{ + return pimpl_->client.call("simAddVehicle", vehicle_name, vehicle_type, pawn_path, north, east, down).as(); +} + void RpcLibClientBase::simPrintLogMessage(const std::string& message, std::string message_param, unsigned char severity) { pimpl_->client.call("simPrintLogMessage", message, message_param, severity); diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index 130c409e21..f56c97b454 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -49,7 +49,7 @@ struct RpcLibServerBase::impl { ~impl() { } - void stop() { + void stop() { server.close_sessions(); if (!is_async_) { // this deadlocks UI thread if async_run was called while there are pending rpc calls. @@ -91,9 +91,9 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& pimpl_->server.bind("getMinRequiredClientVersion", []() -> int { return 1; }); - - pimpl_->server.bind("simPause", [&](bool is_paused) -> void { - getWorldSimApi()->pause(is_paused); + + pimpl_->server.bind("simPause", [&](bool is_paused) -> void { + getWorldSimApi()->pause(is_paused); }); pimpl_->server.bind("simIsPaused", [&]() -> bool { @@ -110,7 +110,7 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& pimpl_->server.bind("simSetTimeOfDay", [&](bool is_enabled, const string& start_datetime, bool is_start_datetime_dst, float celestial_clock_speed, float update_interval_secs, bool move_sun) -> void { - getWorldSimApi()->setTimeOfDay(is_enabled, start_datetime, is_start_datetime_dst, + getWorldSimApi()->setTimeOfDay(is_enabled, start_datetime, is_start_datetime_dst, celestial_clock_speed, update_interval_secs, move_sun); }); @@ -122,7 +122,7 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& getWorldSimApi()->setWeatherParameter(param, val); }); - pimpl_->server.bind("enableApiControl", [&](bool is_enabled, const std::string& vehicle_name) -> void { + pimpl_->server.bind("enableApiControl", [&](bool is_enabled, const std::string& vehicle_name) -> void { getVehicleApi(vehicle_name)->enableApiControl(is_enabled); }); @@ -153,8 +153,27 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& return RpcLibAdapatorsBase::MeshPositionVertexBuffersResponse::from(response); }); - pimpl_->server. - bind("simSetVehiclePose", [&](const RpcLibAdapatorsBase::Pose &pose, bool ignore_collision, const std::string& vehicle_name) -> void { + pimpl_->server.bind("simAddVehicle", [&](const std::string& vehicle_name, const std::string& vehicle_type, const std::string& pawn_path, float north, float east, float down) -> bool { + + AirSimSettings::VehicleSetting vehicle_setting; + + // TODO expose other VehicleSettings fields + vehicle_setting.vehicle_name = vehicle_name; + vehicle_setting.vehicle_type = vehicle_type; + vehicle_setting.pawn_path = pawn_path; + + vehicle_setting.position[0] = north; + vehicle_setting.position[1] = east; + vehicle_setting.position[2] = down; + + vehicle_setting.rotation.yaw = 0; + vehicle_setting.rotation.pitch = 0; + vehicle_setting.rotation.roll = 0; + + return getWorldSimApi()->createVehicleAtRuntime(vehicle_setting); + }); + + pimpl_->server.bind("simSetVehiclePose", [&](const RpcLibAdapatorsBase::Pose &pose, bool ignore_collision, const std::string& vehicle_name) -> void { getVehicleSimApi(vehicle_name)->setPose(pose.to(), ignore_collision); }); @@ -174,7 +193,7 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& pimpl_->server. bind("simGetSegmentationObjectID", [&](const std::string& mesh_name) -> int { return getWorldSimApi()->getSegmentationObjectID(mesh_name); - }); + }); pimpl_->server.bind("reset", [&]() -> void { //Exit if already resetting. @@ -256,7 +275,7 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& }); pimpl_->server.bind("simGetCollisionInfo", [&](const std::string& vehicle_name) -> RpcLibAdapatorsBase::CollisionInfo { - const auto& collision_info = getVehicleSimApi(vehicle_name)->getCollisionInfo(); + const auto& collision_info = getVehicleSimApi(vehicle_name)->getCollisionInfo(); return RpcLibAdapatorsBase::CollisionInfo(collision_info); }); @@ -277,7 +296,7 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& }); pimpl_->server.bind("simGetObjectPose", [&](const std::string& object_name) -> RpcLibAdapatorsBase::Pose { - const auto& pose = getWorldSimApi()->getObjectPose(object_name); + const auto& pose = getWorldSimApi()->getObjectPose(object_name); return RpcLibAdapatorsBase::Pose(pose); }); diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index e2ef6a0fae..bd35818e1e 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -556,6 +556,74 @@ FRotator ASimModeBase::toFRotator(const msr::airlib::AirSimSettings::Rotation& r return frotator; } +// TODO factor out commonalities with this and setupVehiclesAndCamera, below +bool ASimModeBase::createVehicleAtRuntime(const AirSimSettings::VehicleSetting &vehicle_setting) +{ + if (!isVehicleTypeSupported(vehicle_setting.vehicle_type)) { + Utils::log(Utils::stringf("Vehicle type %s is not supported in this game mode", vehicle_setting.vehicle_type.c_str()), Utils::kLogLevelWarn); + return false; + } + + // Retroactively adjust AirSimSettings, so it's like we knew about this vehicle all along + // (Other places in the code use this for reference) + AirSimSettings::singleton().addVehicleSetting(vehicle_setting); + + //get UU origin of global NED frame + const FTransform uu_origin = getGlobalNedTransform().getGlobalTransform(); + + //compute initial pose + FVector spawn_position = uu_origin.GetLocation(); + msr::airlib::Vector3r settings_position = vehicle_setting.position; + if (!msr::airlib::VectorMath::hasNan(settings_position)) + spawn_position = getGlobalNedTransform().fromGlobalNed(settings_position); + + FRotator spawn_rotation = toFRotator(vehicle_setting.rotation, uu_origin.Rotator()); + + //spawn vehicle pawn + FActorSpawnParameters pawn_spawn_params; + + std::string vehicle_name = vehicle_setting.vehicle_name; + + pawn_spawn_params.Name = FName(vehicle_name.c_str()); + pawn_spawn_params.SpawnCollisionHandlingOverride = + ESpawnActorCollisionHandlingMethod::AdjustIfPossibleButAlwaysSpawn; + auto vehicle_bp_class = UAirBlueprintLib::LoadClass( + getSettings().pawn_paths.at(getVehiclePawnPathName(vehicle_setting)).pawn_bp); + APawn* spawned_pawn = static_cast(this->GetWorld()->SpawnActor( + vehicle_bp_class, &spawn_position, &spawn_rotation, pawn_spawn_params)); + + spawned_actors_.Add(spawned_pawn); + + initializeVehiclePawn(spawned_pawn); + + //create vehicle sim api + const auto& ned_transform = getGlobalNedTransform(); + const auto& pawn_ned_pos = ned_transform.toLocalNed(spawned_pawn->GetActorLocation()); + const auto& home_geopoint = msr::airlib::EarthUtils::nedToGeodetic(pawn_ned_pos, getSettings().origin_geopoint); + + PawnSimApi::Params pawn_sim_api_params(spawned_pawn, &getGlobalNedTransform(), + getVehiclePawnEvents(spawned_pawn), getVehiclePawnCameras(spawned_pawn), pip_camera_class, + collision_display_template, home_geopoint, vehicle_name); + + auto vehicle_sim_api = createVehicleSimApi(pawn_sim_api_params); + auto vehicle_sim_api_p = vehicle_sim_api.get(); + auto vehicle_Api = getVehicleApi(pawn_sim_api_params, vehicle_sim_api_p); + getApiProvider()->insert_or_assign(vehicle_name, vehicle_Api, vehicle_sim_api_p); + + // Usually physics registration happens at init, in ASimModeWorldBase::initializeForPlay(), but not in this case + vehicle_sim_api_p->reset(); + registerPhysicsBody(vehicle_sim_api_p); + + if ((vehicle_setting.is_fpv_vehicle || !getApiProvider()->hasDefaultVehicle()) && vehicle_name != "") + { + getApiProvider()->makeDefaultVehicle(vehicle_name); + } + + vehicle_sim_apis_.push_back(std::move(vehicle_sim_api)); + + return true; +} + void ASimModeBase::setupVehiclesAndCamera() { //get UU origin of global NED frame diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h index b23be7a3dc..dd3d09c2be 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h @@ -75,6 +75,8 @@ class AIRSIM_API ASimModeBase : public AActor void stopApiServer(); bool isApiServerStarted(); + bool createVehicleAtRuntime(const msr::airlib::AirSimSettings::VehicleSetting &vehicle_setting); + const NedTransform& getGlobalNedTransform(); msr::airlib::ApiProvider* getApiProvider() const @@ -116,6 +118,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 registerPhysicsBody(msr::airlib::VehicleSimApiBase *physicsBody) {} protected: //Utility methods for derived classes virtual const msr::airlib::AirSimSettings& getSettings() const; diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp index dd157d5cf3..f325be9ace 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp @@ -21,6 +21,12 @@ void ASimModeWorldBase::initializeForPlay() vehicles, getPhysicsLoopPeriod())); } +void ASimModeWorldBase::registerPhysicsBody(msr::airlib::VehicleSimApiBase *physicsBody) +{ + physics_world_.get()->addBody(physicsBody); +} + + void ASimModeWorldBase::EndPlay(const EEndPlayReason::Type EndPlayReason) { //remove everything that we created in BeginPlay diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h index 82d65236dd..7b2febd758 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h @@ -42,6 +42,9 @@ class AIRSIM_API ASimModeWorldBase : public ASimModeBase //should be called by derived class once all api_provider_ is ready to use void initializeForPlay(); + //used for adding physics bodies on the fly + virtual void registerPhysicsBody(msr::airlib::VehicleSimApiBase *physicsBody); + long long getPhysicsLoopPeriod() const; void setPhysicsLoopPeriod(long long period); private: diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index f7679c12d7..25e29d2a00 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -268,6 +268,22 @@ void WorldSimApi::setTimeOfDay(bool is_enabled, const std::string& start_datetim celestial_clock_speed, update_interval_secs, move_sun); } +bool WorldSimApi::createVehicleAtRuntime(AirSimSettings::VehicleSetting& vehicle_setting) +{ + bool result; + + // We need to run this code on the main game thread, since it iterates over actors + FGraphEventRef task = FFunctionGraphTask::CreateAndDispatchWhenReady([&]() + { + result = simmode_->createVehicleAtRuntime(vehicle_setting); + }, TStatId(), NULL, ENamedThreads::GameThread); + + // Wait for the result + FTaskGraphInterface::Get().WaitUntilTaskCompletes(task); + + return result; +} + bool WorldSimApi::setSegmentationObjectID(const std::string& mesh_name, int object_id, bool is_name_regex) { bool success; diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.h b/Unreal/Plugins/AirSim/Source/WorldSimApi.h index 9e3ff509fc..6524538736 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.h +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.h @@ -38,6 +38,8 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase { virtual bool setSegmentationObjectID(const std::string& mesh_name, int object_id, bool is_name_regex = false) override; virtual int getSegmentationObjectID(const std::string& mesh_name) const override; + virtual bool createVehicleAtRuntime(msr::airlib::AirSimSettings::VehicleSetting& vehicle_setting); + virtual void printLogMessage(const std::string& message, const std::string& message_param = "", unsigned char severity = 0) override; From 2e7610e234e0a53e5e4c7f1708b4f09a9807933e Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Sat, 13 Feb 2021 03:58:35 +0530 Subject: [PATCH 15/29] HelloSpawnedDrones sample (Windows), and note in docs --- HelloSpawnedDrones/HelloSpawnedDrones.cpp | Bin 0 -> 12766 bytes HelloSpawnedDrones/HelloSpawnedDrones.vcxproj | 161 ++++++++++++++++++ .../HelloSpawnedDrones.vcxproj.filters | 6 + docs/apis_cpp.md | 1 + 4 files changed, 168 insertions(+) create mode 100644 HelloSpawnedDrones/HelloSpawnedDrones.cpp create mode 100644 HelloSpawnedDrones/HelloSpawnedDrones.vcxproj create mode 100644 HelloSpawnedDrones/HelloSpawnedDrones.vcxproj.filters diff --git a/HelloSpawnedDrones/HelloSpawnedDrones.cpp b/HelloSpawnedDrones/HelloSpawnedDrones.cpp new file mode 100644 index 0000000000000000000000000000000000000000..26048bdfe1c1530f5bae180906f5396a42b7e65a GIT binary patch literal 12766 zcmeI2Yi}FJ6^7@l3iLm0SRkOBNV1wZA6(afV%dpP-(V>L>Y@-Rk-F3*C6J62JLq3; z^FDL-aAtSOr5z=*0~iFk%iWnX=e*}Sm;e87D{O@#+zaR7BD@Zha28I&X_)C*Hynj^ zT_5Y&k=~q#slLbY9qlLT!EaAb&U7~pn`*rihWZp4rD^;(Q15}BzS3`BE&m+;q>&l> zQKUame_HhQvn3fw!&7zXP>` z>!HTn3VXUo@*`azt2O;MHSUEzoyh5)#(pji?&*4l5+T)AN@lHSp_;8ew$Qj=!2d?WEC1uA~(xuqx;i{^H6snKaw7XQ4@?Yi{orbt_!u= z63)%H&m(`>%%QLx>WbvEn!c`L4G%;U{`TY#Q&Ap>%c?arsozl_`~y20i2qTzJGY7| z!gqx3X+(6a9&N3l){hb%wzR`PY|MVy3iSED68$q#z!&kTJ^dTYZjdcUuB*4anT74} zm8=WjH_x7h&vkF_il`ebY%KYPiVTeLbNpQsg=tB>i0?-lV=5|GDAtX|PQq<{dTKW* zrTSah8$ZSS85wkXYEkHFi2f`5TmSy9Z%6&k)q?T;Z6`RD3qGBBZ?jpXuy{1g#>}zU zkgXa;A|T_~s8#x1S($0qG?epgzUJIjBg+$62-pMu@U-PjpQ27CWED(FelwK-93EuAULO zd7H<2{`bXEUNPO#6SzE#RPL(pUxnsk96;5hzxZeR+vYZs@!~S3TWsUhxGvfC+IJc`x9#_1yNQ?Uu-lXqn(+~PecySY zUO`wa-OgfZ?rm?xFsfJI^|saAsZZVJJQD3IKD^KjfV^zk=S@T-#aJ>Ixfm+j)CU|G?Q{a|9n3Cdq#~_as7d8>UGQr)GTJDSQuI! z=}LwEldi0kFq6S{v1H3xU+8Y6bujV^Naq!Qk~JZRr?6sF&85pcfZFeksCqS4x4OlV zCiPmJFw@<{S|*1zc;9&gsOR*BcDZ-Qyv zvf6piqkQ5$acq&l&K2($c^uE{{=u_l@jQ81lIZ%@MCwwd6_Qo)w;j`NL)!`2+$uh7 zYcPvVoTJB&*1+0ZJ$A0LhNS0>EVJd7@W+S`IrmtDS;$A5_%$|B&j&fSxn(5V`4BUP zNs;q@T~VT0-+u^s!?{|PgPSv}e696`d}<^5)n>S*>I*EZ@%N8E=lWLBlWKr!ofX%s z?DVA-oVUHJ_=X`j5c$2bnGhOv`rdo6in zS~mK1>W%rbYi=$4w_IQ2EQYVocNHr*JH%YQd3xwmq026;of}!3pB%~8#lh9%jjd9# zYQnse^H?_X&QBz_IM#K?0f?!XIQ`Rb7_Z~ zGqK)eI|n=3y?a>3e%qTiec1jKd#biqeJLu~1Ti#K35j2y8G1D_(s;3q!%wd47#zzLh*?mUm#bMs7**#zqswmN}cztl?Dm&&1O z{o17%Yu#fcgsW@4tc2O>Z0-{Y%cfRccHj3&8c1(eA#JDXx+Pvm;^D5ooO>`?f1a;4 zw{h|$O2k^9$%HJqt=`uEI(O)1JDz`9#pBvs+g0SIvQe`OaKl?U73Q)y{oF^Y{e6|) zk%wcbu>W?TFWxtkt=9F8=T#{=Cuyu?yX~IIi9FWyvF^0GgB&IgcxV4S{Og%Wi2fp{C^-u@; zN;^9i(b7sVfJhl^I;jJgl;e$Cg``No`Z4KP^ zp6-brCxWme&fQ$<-fDMF!FXJ^6KQ5`mdhE3x{N%8y>mv0OmHF^sUQ2xoTLIdM(Bz@ z`&?ANwubvsG#C{WOta39N0Q{lxnxfzb2@ox5pcQXF*8Z=lv5<22tSMqXJ`5n4LA#F z|2X-#jH0#qOeg0Omnq^^*D + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 15.0 + {99CBF376-5EBA-4164-A657-E7D708C9D685} + Win32Proj + HelloSpawnedDrones + 10.0.17134.0 + + + + Application + true + v141 + Unicode + + + Application + false + v141 + true + Unicode + + + Application + true + v141 + Unicode + + + Application + false + v141 + true + Unicode + + + + + + + + + + + + + + + + + + + + + true + + + true + + + false + + + false + + + + NotUsing + Level3 + Disabled + true + _DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + $(ProjectDir)..\AirLib\deps\rpclib\include;include;$(ProjectDir)..\AirLib\deps\eigen3;$(ProjectDir)..\AirLib\include + + + Console + true + $(ProjectDir)\..\AirLib\deps\MavLinkCom\lib\$(Platform)\$(Configuration);$(ProjectDir)\..\AirLib\deps\rpclib\lib\$(Platform)\$(Configuration);$(ProjectDir)\..\AirLib\lib\$(Platform)\$(Configuration);%(AdditionalLibraryDirectories) + rpc.lib;AirLib.lib;%(AdditionalDependencies) + + + + + Use + Level3 + Disabled + true + WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + + + + + Use + Level3 + MaxSpeed + true + true + true + WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + true + true + + + + + NotUsing + Level3 + MaxSpeed + true + true + true + NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + $(ProjectDir)..\AirLib\deps\rpclib\include;include;$(ProjectDir)..\AirLib\deps\eigen3;$(ProjectDir)..\AirLib\include + + + Console + true + true + true + $(ProjectDir)\..\AirLib\deps\MavLinkCom\lib\$(Platform)\$(Configuration);$(ProjectDir)\..\AirLib\deps\rpclib\lib\$(Platform)\$(Configuration);$(ProjectDir)\..\AirLib\lib\$(Platform)\$(Configuration);%(AdditionalLibraryDirectories) + rpc.lib;AirLib.lib;%(AdditionalDependencies) + + + + + + + + + \ No newline at end of file diff --git a/HelloSpawnedDrones/HelloSpawnedDrones.vcxproj.filters b/HelloSpawnedDrones/HelloSpawnedDrones.vcxproj.filters new file mode 100644 index 0000000000..d428262af8 --- /dev/null +++ b/HelloSpawnedDrones/HelloSpawnedDrones.vcxproj.filters @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/docs/apis_cpp.md b/docs/apis_cpp.md index b9255c30f5..3d41b98cea 100644 --- a/docs/apis_cpp.md +++ b/docs/apis_cpp.md @@ -88,5 +88,6 @@ int main() ## See Also * [Examples](../Examples) of how to use internal infrastructure in AirSim in your other projects * [DroneShell](../DroneShell) app shows how to make simple interface using C++ APIs to control drones +* [HelloSpawnedDrones](./HelloSpawnedDrones) app shows how to make additional vehicles on the fly * [Python APIs](apis.md) From 359354724e3c52a9cb540ab63fabd8fbe3ed7726 Mon Sep 17 00:00:00 2001 From: Kerry Moffitt Date: Wed, 1 May 2019 09:33:08 -0400 Subject: [PATCH 16/29] Fix encoding --- HelloSpawnedDrones/HelloSpawnedDrones.cpp | Bin 12766 -> 6213 bytes 1 file changed, 0 insertions(+), 0 deletions(-) diff --git a/HelloSpawnedDrones/HelloSpawnedDrones.cpp b/HelloSpawnedDrones/HelloSpawnedDrones.cpp index 26048bdfe1c1530f5bae180906f5396a42b7e65a..9bfd4e4b6cb5a2cc53cbeac58489491572b6dcdb 100644 GIT binary patch literal 6213 zcmdT|>uwu27XGby3RVG%Q6kHBTla@z8*uHob!*2)tPQXU0)d*LB(7(M1Cp{L7kRS% zv5(YO=yyntJTvw>YbOP|K;39~IXsu|T>tp@e>OMSp(%g0XxVt<_;OAg%SE%q=N{TPh}zsBT}{~QlXih3wCxgs+I>)^h_2>wnzo*CMwTW zvFZOAFRaYW=B3p#v1ck3gLzqYqs!5olf%*Y?EL7}`26@7x2K>xWp6GHPfvatpI!cZ zad7xs^bBjV5UpjFWO*veac*WMPp+b8qDbYGfy-~&_t#A6GTFr6SZJ;EVBX^3b)Elf zF_%du%x1pGtklXXy~#_tdA9z1QzoZ!a+pa`STE1R(jUuAT&|22dF%A9RK{w-^Zi@> z?Iv@r3e{LKHqGRuzGUaPmhMMUbgg8{^rEHBusznzRKhd*?Bc{UQ`|Bu zbDzx91ibS9DANP1>$lcmUR~X+l`o>?+20AkN3b&2MQVv z6EMSecUkvP6;r8mk`b~LA|Y&>!C=s(ZFd%s$>^fzVPhZX=n8Iz<1y$~gkdA16@c*? zY9o#O zxpf!c_}l8Uox#?WJ!cgw;BmW$4KESgWHgtC;XlqM5{A}r+di8>c{6z}7?wE8RSs}7 ztC$hiW`e20IsHs4M1VdMnGwwLE1{-Swv>n(Q!cYgVHOXJJw%5?lL8yIzN&I|G)Pf=<)?#^!m6^MiRyVi|YDJ z*jwCRT5d(%biD!gt6K!MPVgyD?;Qv=DGzRNib7nn(3&h$R!_#SUmXGF9(GnR;{r`@ zz!>2-c&azl$U7{E@7zV_NdXa%1pxHQuCN)q52xJ+T`Al?FsmYo!$)|zsqKyZC71TR zIOWEUxViFrXuC22+>&_%*dD_a6adFYF!O3KCE;X`eGmU|-VJ@4rL@VMz#~4EnFQVt z^sbTO2tEWaJEu?=Jcjb#lxNuld510MqF{IeR0y!oVTC;BMGD8#I3^2e=9E6$;i;x+ z|I}kdx(!+GuGexM!;jPI;~9WB4*6>Uelg~&NC3-h-+ICgKC3sLihfw;06E-JU4jx} zD&RUucN1Zk0=5$zmM=4IVSt>nB+CIcZCbG5fy9x>EGH%__}s9Ky(+IF-ZswN3KB9E zUEFdvZ%s(B8L4;Ph&PC41kiq!So_VS_)os10xCu*-yjzHj*-v1)E)dCs>-M6eS-dt zck>&UJ=x>n*!0)qY()_Ss^kziXA||2mE1zBSh4JTmTww_Tp!rm>A0nLBX20tQZ^%P z)?ngx;Jc(Q+B(umTvl^gj8-K~O`jwB#P#OhaCofr5=rE|$X0YUXqBz_a!f8CqJ2n~ z6#zl!$$`D8po=PQtEpEyRcKD{!|FTal4e2B>3j``X!6oZv$3~_3zAeuxmv=ld0_P2 z@3k4|NUv?eCbnxi++#cd95o8iUQxoYwZZypLo|U+rgF?bskEqGq6(uOjoV59Z*4t% zT^-7Dzt|%{s0wb4gjCg{ z8*FQ^gLD&68}iv-m7RW}>I*sK!5Z@swNFjD|EUA0)Vl5_Pf^l+j-L!?Lc!~6>}K?O zpeB2g^>#w5)C}joUHKfVw|xx?S#zfWx+DIPD#SPU7y}1WL8W=fjJNIb(?r^G!TA%$ z51b0}7}J{Ja6Pj5mNC)uvjrz~Xvr}GLTVP&P5IGI+#7JSTRXCUUsv(!V{@}81i)(|H zSSI5Hz29yn?|vK(5e>Qob+#lsmKyDxtC=eKDkZf1u9^wv?pRFi@VCxmRVO`?D)=MVYZsQzQUc4ZJzvC|?n!x*@T$uTodTBoz@&d-K zVP>BN3&WAz#4zyVi@88CtZi&KeSATwxHZ`Jgb8~cYK3Cg?tW|4uL=#tu&MsFV19e> aFP!la2a)XmJeWltE$jk~8)mzoqCWw49^(=K literal 12766 zcmeI2Yi}FJ6^7@l3iLm0SRkOBNV1wZA6(afV%dpP-(V>L>Y@-Rk-F3*C6J62JLq3; z^FDL-aAtSOr5z=*0~iFk%iWnX=e*}Sm;e87D{O@#+zaR7BD@Zha28I&X_)C*Hynj^ zT_5Y&k=~q#slLbY9qlLT!EaAb&U7~pn`*rihWZp4rD^;(Q15}BzS3`BE&m+;q>&l> zQKUame_HhQvn3fw!&7zXP>` z>!HTn3VXUo@*`azt2O;MHSUEzoyh5)#(pji?&*4l5+T)AN@lHSp_;8ew$Qj=!2d?WEC1uA~(xuqx;i{^H6snKaw7XQ4@?Yi{orbt_!u= z63)%H&m(`>%%QLx>WbvEn!c`L4G%;U{`TY#Q&Ap>%c?arsozl_`~y20i2qTzJGY7| z!gqx3X+(6a9&N3l){hb%wzR`PY|MVy3iSED68$q#z!&kTJ^dTYZjdcUuB*4anT74} zm8=WjH_x7h&vkF_il`ebY%KYPiVTeLbNpQsg=tB>i0?-lV=5|GDAtX|PQq<{dTKW* zrTSah8$ZSS85wkXYEkHFi2f`5TmSy9Z%6&k)q?T;Z6`RD3qGBBZ?jpXuy{1g#>}zU zkgXa;A|T_~s8#x1S($0qG?epgzUJIjBg+$62-pMu@U-PjpQ27CWED(FelwK-93EuAULO zd7H<2{`bXEUNPO#6SzE#RPL(pUxnsk96;5hzxZeR+vYZs@!~S3TWsUhxGvfC+IJc`x9#_1yNQ?Uu-lXqn(+~PecySY zUO`wa-OgfZ?rm?xFsfJI^|saAsZZVJJQD3IKD^KjfV^zk=S@T-#aJ>Ixfm+j)CU|G?Q{a|9n3Cdq#~_as7d8>UGQr)GTJDSQuI! z=}LwEldi0kFq6S{v1H3xU+8Y6bujV^Naq!Qk~JZRr?6sF&85pcfZFeksCqS4x4OlV zCiPmJFw@<{S|*1zc;9&gsOR*BcDZ-Qyv zvf6piqkQ5$acq&l&K2($c^uE{{=u_l@jQ81lIZ%@MCwwd6_Qo)w;j`NL)!`2+$uh7 zYcPvVoTJB&*1+0ZJ$A0LhNS0>EVJd7@W+S`IrmtDS;$A5_%$|B&j&fSxn(5V`4BUP zNs;q@T~VT0-+u^s!?{|PgPSv}e696`d}<^5)n>S*>I*EZ@%N8E=lWLBlWKr!ofX%s z?DVA-oVUHJ_=X`j5c$2bnGhOv`rdo6in zS~mK1>W%rbYi=$4w_IQ2EQYVocNHr*JH%YQd3xwmq026;of}!3pB%~8#lh9%jjd9# zYQnse^H?_X&QBz_IM#K?0f?!XIQ`Rb7_Z~ zGqK)eI|n=3y?a>3e%qTiec1jKd#biqeJLu~1Ti#K35j2y8G1D_(s;3q!%wd47#zzLh*?mUm#bMs7**#zqswmN}cztl?Dm&&1O z{o17%Yu#fcgsW@4tc2O>Z0-{Y%cfRccHj3&8c1(eA#JDXx+Pvm;^D5ooO>`?f1a;4 zw{h|$O2k^9$%HJqt=`uEI(O)1JDz`9#pBvs+g0SIvQe`OaKl?U73Q)y{oF^Y{e6|) zk%wcbu>W?TFWxtkt=9F8=T#{=Cuyu?yX~IIi9FWyvF^0GgB&IgcxV4S{Og%Wi2fp{C^-u@; zN;^9i(b7sVfJhl^I;jJgl;e$Cg``No`Z4KP^ zp6-brCxWme&fQ$<-fDMF!FXJ^6KQ5`mdhE3x{N%8y>mv0OmHF^sUQ2xoTLIdM(Bz@ z`&?ANwubvsG#C{WOta39N0Q{lxnxfzb2@ox5pcQXF*8Z=lv5<22tSMqXJ`5n4LA#F z|2X-#jH0#qOeg0Omnq^^*D Date: Mon, 6 May 2019 12:38:14 -0400 Subject: [PATCH 17/29] Sample code, Linux-side --- .gitignore | 2 ++ cmake/CMakeLists.txt | 1 + cmake/HelloSpawnedDrones/CMakeLists.txt | 25 +++++++++++++++++++++++++ 3 files changed, 28 insertions(+) create mode 100644 cmake/HelloSpawnedDrones/CMakeLists.txt diff --git a/.gitignore b/.gitignore index c1564db29d..ec5b0897c7 100644 --- a/.gitignore +++ b/.gitignore @@ -286,12 +286,14 @@ ModelManifest.xml /cmake/MavLinkCom/Makefile /cmake/Makefile /cmake/HelloDrone/Makefile +/cmake/HelloSpawnedDrones/Makefile /cmake/DroneShell/Makefile /cmake/DroneServer/Makefile /cmake/AirLib/Makefile /cmake/AirLibUnity/Makefile /cmake/AirLibUnity/AirLibUnity /cmake/HelloDrone/HelloDrone +/cmake/HelloSpawnedDrones/HelloSpawnedDrones /cmake/DroneShell/DroneShell /cmake/DroneServer/DroneServer cmake/AirLibUnitTests/Makefile diff --git a/cmake/CMakeLists.txt b/cmake/CMakeLists.txt index 5e31535511..d5dbc29235 100644 --- a/cmake/CMakeLists.txt +++ b/cmake/CMakeLists.txt @@ -6,6 +6,7 @@ add_subdirectory("AirLib") add_subdirectory("MavLinkCom") add_subdirectory("AirLibUnitTests") add_subdirectory("HelloDrone") +add_subdirectory("HelloSpawnedDrones") add_subdirectory("HelloCar") add_subdirectory("DroneShell") add_subdirectory("DroneServer") diff --git a/cmake/HelloSpawnedDrones/CMakeLists.txt b/cmake/HelloSpawnedDrones/CMakeLists.txt new file mode 100644 index 0000000000..32906f0081 --- /dev/null +++ b/cmake/HelloSpawnedDrones/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.5.0) +project(HelloSpawnedDrones) + +LIST(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/../cmake-modules") +INCLUDE("${CMAKE_CURRENT_LIST_DIR}/../cmake-modules/CommonSetup.cmake") +CommonSetup() + +IncludeEigen() + +SetupConsoleBuild() + +## Specify additional locations of header files +include_directories( + ${AIRSIM_ROOT}/HelloSpawnedDrones + ${AIRSIM_ROOT}/AirLib/include + ${RPC_LIB_INCLUDES} + ${AIRSIM_ROOT}/MavLinkCom/include + ${AIRSIM_ROOT}/MavLinkCom/common_utils +) + +AddExecutableSource() + +CommonTargetLink() +target_link_libraries(${PROJECT_NAME} AirLib) +target_link_libraries(${PROJECT_NAME} ${RPC_LIB}) From 4489b90c49a47f154e6681c228a9f2709945bd93 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Sun, 19 Jan 2020 21:41:28 +0530 Subject: [PATCH 18/29] Add simAddVehicle() to Python API --- PythonClient/airsim/client.py | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index 762a1269a9..b211a72c78 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -856,6 +856,24 @@ def simCreateVoxelGrid(self, position, x, y, z, res, of): """ return self.client.call('simCreateVoxelGrid', position, x, y, z, res, of) + # Add new vehicle via RPC + def simAddVehicle(self, vehicle_name, vehicle_type, pawn_path, north, east, down): + """ + Create vehicle at runtime + + Args: + vehicle_name (str): Name of the vehicle being created + vehicle_type (str): Type of vehicle, e.g. "simpleflight" + pawn_path (str): Vehicle blueprint path + north (float): Metres in North direction to spawn at vehicle at + east (float): Meters in East direction to spawn vehicle at + down (float): Metres below to spawn the vehicle at + + Returns: + bool: Whether vehicle was created + """ + return self.client.call('simAddVehicle', vehicle_name, vehicle_type, pawn_path, north, east, down) + # ----------------------------------- Multirotor APIs --------------------------------------------- class MultirotorClient(VehicleClient, object): def __init__(self, ip = "", port = 41451, timeout_value = 3600): From 208c3cbfded0e0761ff37d1490e9a50f89e72f77 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Thu, 4 Feb 2021 19:58:45 +0530 Subject: [PATCH 19/29] Use Pose instead of NED values, fix missing override --- AirLib/include/api/RpcLibClientBase.hpp | 2 +- AirLib/include/api/WorldSimApiBase.hpp | 2 +- AirLib/src/api/RpcLibClientBase.cpp | 4 ++-- AirLib/src/api/RpcLibServerBase.cpp | 21 +++----------------- PythonClient/airsim/client.py | 10 ++++------ Unreal/Plugins/AirSim/Source/WorldSimApi.cpp | 11 +++++++++- Unreal/Plugins/AirSim/Source/WorldSimApi.h | 2 +- 7 files changed, 22 insertions(+), 30 deletions(-) diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index ef9cdc8415..56e9ce567a 100644 --- a/AirLib/include/api/RpcLibClientBase.hpp +++ b/AirLib/include/api/RpcLibClientBase.hpp @@ -97,7 +97,7 @@ class RpcLibClientBase { vector simGetImage(const std::string& camera_name, ImageCaptureBase::ImageType type, const std::string& vehicle_name = ""); vector simGetMeshPositionVertexBuffers(); - bool simAddVehicle(const std::string& vehicle_name, const std::string& vehicle_type, const std::string& pawn_path, float north, float east, float down); + bool simAddVehicle(const std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path = ""); CollisionInfo simGetCollisionInfo(const std::string& vehicle_name = "") const; diff --git a/AirLib/include/api/WorldSimApiBase.hpp b/AirLib/include/api/WorldSimApiBase.hpp index 5cd4c5257c..541aa36ca3 100644 --- a/AirLib/include/api/WorldSimApiBase.hpp +++ b/AirLib/include/api/WorldSimApiBase.hpp @@ -47,7 +47,7 @@ class WorldSimApiBase { virtual bool setSegmentationObjectID(const std::string& mesh_name, int object_id, bool is_name_regex = false) = 0; virtual int getSegmentationObjectID(const std::string& mesh_name) const = 0; - virtual bool createVehicleAtRuntime(AirSimSettings::VehicleSetting& vehicle_setting) = 0; + virtual bool createVehicleAtRuntime(const std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path = "") = 0; virtual void printLogMessage(const std::string& message, const std::string& message_param = "", unsigned char severity = 0) = 0; diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index 448635573b..50beb9987e 100644 --- a/AirLib/src/api/RpcLibClientBase.cpp +++ b/AirLib/src/api/RpcLibClientBase.cpp @@ -243,9 +243,9 @@ vector RpcLibClientBase::simGetMeshPositionVe return RpcLibAdapatorsBase::MeshPositionVertexBuffersResponse::to(response_adaptor); } -bool RpcLibClientBase::simAddVehicle(const std::string& vehicle_name, const std::string& vehicle_type, const std::string& pawn_path, float north, float east, float down) +bool RpcLibClientBase::simAddVehicle(const std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path) { - return pimpl_->client.call("simAddVehicle", vehicle_name, vehicle_type, pawn_path, north, east, down).as(); + return pimpl_->client.call("simAddVehicle", vehicle_name, vehicle_type, RpcLibAdapatorsBase::Pose(pose), pawn_path).as(); } void RpcLibClientBase::simPrintLogMessage(const std::string& message, std::string message_param, unsigned char severity) diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index f56c97b454..16fcd5337e 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -153,24 +153,9 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& return RpcLibAdapatorsBase::MeshPositionVertexBuffersResponse::from(response); }); - pimpl_->server.bind("simAddVehicle", [&](const std::string& vehicle_name, const std::string& vehicle_type, const std::string& pawn_path, float north, float east, float down) -> bool { - - AirSimSettings::VehicleSetting vehicle_setting; - - // TODO expose other VehicleSettings fields - vehicle_setting.vehicle_name = vehicle_name; - vehicle_setting.vehicle_type = vehicle_type; - vehicle_setting.pawn_path = pawn_path; - - vehicle_setting.position[0] = north; - vehicle_setting.position[1] = east; - vehicle_setting.position[2] = down; - - vehicle_setting.rotation.yaw = 0; - vehicle_setting.rotation.pitch = 0; - vehicle_setting.rotation.roll = 0; - - return getWorldSimApi()->createVehicleAtRuntime(vehicle_setting); + pimpl_->server.bind("simAddVehicle", [&](const std::string& vehicle_name, const std::string& vehicle_type, + const RpcLibAdapatorsBase::Pose& pose, const std::string& pawn_path) -> bool { + return getWorldSimApi()->createVehicleAtRuntime(vehicle_name, vehicle_type, pose.to(), pawn_path); }); pimpl_->server.bind("simSetVehiclePose", [&](const RpcLibAdapatorsBase::Pose &pose, bool ignore_collision, const std::string& vehicle_name) -> void { diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index b211a72c78..0d4026261d 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -857,22 +857,20 @@ def simCreateVoxelGrid(self, position, x, y, z, res, of): return self.client.call('simCreateVoxelGrid', position, x, y, z, res, of) # Add new vehicle via RPC - def simAddVehicle(self, vehicle_name, vehicle_type, pawn_path, north, east, down): + def simAddVehicle(self, vehicle_name, vehicle_type, pose, pawn_path = ""): """ Create vehicle at runtime Args: vehicle_name (str): Name of the vehicle being created vehicle_type (str): Type of vehicle, e.g. "simpleflight" - pawn_path (str): Vehicle blueprint path - north (float): Metres in North direction to spawn at vehicle at - east (float): Meters in East direction to spawn vehicle at - down (float): Metres below to spawn the vehicle at + pose (Pose): Initial pose of the vehicle + pawn_path (str): Vehicle blueprint path, default empty wbich uses the default blueprint for the vehicle type Returns: bool: Whether vehicle was created """ - return self.client.call('simAddVehicle', vehicle_name, vehicle_type, pawn_path, north, east, down) + return self.client.call('simAddVehicle', vehicle_name, vehicle_type, pose, pawn_path) # ----------------------------------- Multirotor APIs --------------------------------------------- class MultirotorClient(VehicleClient, object): diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index 25e29d2a00..58ca6c6fe7 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -268,8 +268,17 @@ void WorldSimApi::setTimeOfDay(bool is_enabled, const std::string& start_datetim celestial_clock_speed, update_interval_secs, move_sun); } -bool WorldSimApi::createVehicleAtRuntime(AirSimSettings::VehicleSetting& vehicle_setting) +bool WorldSimApi::createVehicleAtRuntime(const std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path) { + // Create settings object + AirSimSettings::VehicleSetting vehicle_setting; + + vehicle_setting.vehicle_name = vehicle_name; + vehicle_setting.vehicle_type = vehicle_type; + vehicle_setting.pawn_path = pawn_path; + vehicle_setting.position = pose.position; + VectorMath::toEulerianAngle(pose.orientation, vehicle_setting.rotation.pitch, vehicle_setting.rotation.roll, vehicle_setting.rotation.yaw); + bool result; // We need to run this code on the main game thread, since it iterates over actors diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.h b/Unreal/Plugins/AirSim/Source/WorldSimApi.h index 6524538736..4072c33b3a 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.h +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.h @@ -38,7 +38,7 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase { virtual bool setSegmentationObjectID(const std::string& mesh_name, int object_id, bool is_name_regex = false) override; virtual int getSegmentationObjectID(const std::string& mesh_name) const override; - virtual bool createVehicleAtRuntime(msr::airlib::AirSimSettings::VehicleSetting& vehicle_setting); + virtual bool createVehicleAtRuntime(const std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path = "") override; virtual void printLogMessage(const std::string& message, const std::string& message_param = "", unsigned char severity = 0) override; From 1e3451b29d4ec5f46d4ac257198f6f193f27892d Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Fri, 5 Feb 2021 09:12:33 +0530 Subject: [PATCH 20/29] Consolidate vehicle creation code in SimModeBase --- .../AirSim/Source/SimMode/SimModeBase.cpp | 107 ++++++++---------- .../AirSim/Source/SimMode/SimModeBase.h | 2 + 2 files changed, 48 insertions(+), 61 deletions(-) diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index bd35818e1e..d3d2e31b9c 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -556,37 +556,27 @@ FRotator ASimModeBase::toFRotator(const msr::airlib::AirSimSettings::Rotation& r return frotator; } -// TODO factor out commonalities with this and setupVehiclesAndCamera, below -bool ASimModeBase::createVehicleAtRuntime(const AirSimSettings::VehicleSetting &vehicle_setting) +APawn* ASimModeBase::createVehiclePawn(const AirSimSettings::VehicleSetting& vehicle_setting) { - if (!isVehicleTypeSupported(vehicle_setting.vehicle_type)) { - Utils::log(Utils::stringf("Vehicle type %s is not supported in this game mode", vehicle_setting.vehicle_type.c_str()), Utils::kLogLevelWarn); - return false; - } - - // Retroactively adjust AirSimSettings, so it's like we knew about this vehicle all along - // (Other places in the code use this for reference) - AirSimSettings::singleton().addVehicleSetting(vehicle_setting); - //get UU origin of global NED frame const FTransform uu_origin = getGlobalNedTransform().getGlobalTransform(); - //compute initial pose + // compute initial pose FVector spawn_position = uu_origin.GetLocation(); - msr::airlib::Vector3r settings_position = vehicle_setting.position; - if (!msr::airlib::VectorMath::hasNan(settings_position)) + Vector3r settings_position = vehicle_setting.position; + if (!VectorMath::hasNan(settings_position)) spawn_position = getGlobalNedTransform().fromGlobalNed(settings_position); FRotator spawn_rotation = toFRotator(vehicle_setting.rotation, uu_origin.Rotator()); - //spawn vehicle pawn - FActorSpawnParameters pawn_spawn_params; - std::string vehicle_name = vehicle_setting.vehicle_name; + //spawn vehicle pawn + FActorSpawnParameters pawn_spawn_params; pawn_spawn_params.Name = FName(vehicle_name.c_str()); pawn_spawn_params.SpawnCollisionHandlingOverride = ESpawnActorCollisionHandlingMethod::AdjustIfPossibleButAlwaysSpawn; + auto vehicle_bp_class = UAirBlueprintLib::LoadClass( getSettings().pawn_paths.at(getVehiclePawnPathName(vehicle_setting)).pawn_bp); APawn* spawned_pawn = static_cast(this->GetWorld()->SpawnActor( @@ -594,30 +584,55 @@ bool ASimModeBase::createVehicleAtRuntime(const AirSimSettings::VehicleSetting & spawned_actors_.Add(spawned_pawn); - initializeVehiclePawn(spawned_pawn); + return spawned_pawn; +} + +std::unique_ptr ASimModeBase::createVehicleApi(APawn* vehicle_pawn) +{ + initializeVehiclePawn(vehicle_pawn); //create vehicle sim api const auto& ned_transform = getGlobalNedTransform(); - const auto& pawn_ned_pos = ned_transform.toLocalNed(spawned_pawn->GetActorLocation()); - const auto& home_geopoint = msr::airlib::EarthUtils::nedToGeodetic(pawn_ned_pos, getSettings().origin_geopoint); + const auto& pawn_ned_pos = ned_transform.toLocalNed(vehicle_pawn->GetActorLocation()); + const auto& home_geopoint= msr::airlib::EarthUtils::nedToGeodetic(pawn_ned_pos, getSettings().origin_geopoint); + const std::string vehicle_name = std::string(TCHAR_TO_UTF8(*(vehicle_pawn->GetName()))); - PawnSimApi::Params pawn_sim_api_params(spawned_pawn, &getGlobalNedTransform(), - getVehiclePawnEvents(spawned_pawn), getVehiclePawnCameras(spawned_pawn), pip_camera_class, + PawnSimApi::Params pawn_sim_api_params(vehicle_pawn, &getGlobalNedTransform(), + getVehiclePawnEvents(vehicle_pawn), getVehiclePawnCameras(vehicle_pawn), pip_camera_class, collision_display_template, home_geopoint, vehicle_name); auto vehicle_sim_api = createVehicleSimApi(pawn_sim_api_params); auto vehicle_sim_api_p = vehicle_sim_api.get(); - auto vehicle_Api = getVehicleApi(pawn_sim_api_params, vehicle_sim_api_p); - getApiProvider()->insert_or_assign(vehicle_name, vehicle_Api, vehicle_sim_api_p); + auto vehicle_api = getVehicleApi(pawn_sim_api_params, vehicle_sim_api_p); + getApiProvider()->insert_or_assign(vehicle_name, vehicle_api, vehicle_sim_api_p); + + return vehicle_sim_api; +} + +bool ASimModeBase::createVehicleAtRuntime(const AirSimSettings::VehicleSetting &vehicle_setting) +{ + if (!isVehicleTypeSupported(vehicle_setting.vehicle_type)) { + Utils::log(Utils::stringf("Vehicle type %s is not supported in this game mode", vehicle_setting.vehicle_type.c_str()), Utils::kLogLevelWarn); + return false; + } + + // Retroactively adjust AirSimSettings, so it's like we knew about this vehicle all along + // (Other places in the code use this for reference) + AirSimSettings::singleton().addVehicleSetting(vehicle_setting); + + auto spawned_pawn = createVehiclePawn(vehicle_setting); + + auto vehicle_sim_api = createVehicleApi(spawned_pawn); + auto vehicle_sim_api_p = vehicle_sim_api.get(); + std::string vehicle_name = vehicle_sim_api->getVehicleName(); // Usually physics registration happens at init, in ASimModeWorldBase::initializeForPlay(), but not in this case vehicle_sim_api_p->reset(); registerPhysicsBody(vehicle_sim_api_p); + // Can't be done before the vehicle apis have been created if ((vehicle_setting.is_fpv_vehicle || !getApiProvider()->hasDefaultVehicle()) && vehicle_name != "") - { getApiProvider()->makeDefaultVehicle(vehicle_name); - } vehicle_sim_apis_.push_back(std::move(vehicle_sim_api)); @@ -648,31 +663,14 @@ void ASimModeBase::setupVehiclesAndCamera() fpv_pawn = static_cast(pawns[0]); } else { //add vehicles from settings - for (auto const& vehicle_setting_pair : getSettings().vehicles) + for (const auto& vehicle_setting_pair : getSettings().vehicles) { //if vehicle is of type for derived SimMode and auto creatable const auto& vehicle_setting = *vehicle_setting_pair.second; if (vehicle_setting.auto_create && isVehicleTypeSupported(vehicle_setting.vehicle_type)) { - //compute initial pose - FVector spawn_position = uu_origin.GetLocation(); - Vector3r settings_position = vehicle_setting.position; - if (!VectorMath::hasNan(settings_position)) - spawn_position = getGlobalNedTransform().fromGlobalNed(settings_position); - FRotator spawn_rotation = toFRotator(vehicle_setting.rotation, uu_origin.Rotator()); - - //spawn vehicle pawn - FActorSpawnParameters pawn_spawn_params; - pawn_spawn_params.Name = FName(vehicle_setting.vehicle_name.c_str()); - pawn_spawn_params.SpawnCollisionHandlingOverride = - ESpawnActorCollisionHandlingMethod::AdjustIfPossibleButAlwaysSpawn; - auto vehicle_bp_class = UAirBlueprintLib::LoadClass( - getSettings().pawn_paths.at(getVehiclePawnPathName(vehicle_setting)).pawn_bp); - APawn* spawned_pawn = static_cast(this->GetWorld()->SpawnActor( - vehicle_bp_class, &spawn_position, &spawn_rotation, pawn_spawn_params)); - - spawned_actors_.Add(spawned_pawn); + auto spawned_pawn = createVehiclePawn(vehicle_setting); pawns.Add(spawned_pawn); if (vehicle_setting.is_fpv_vehicle) @@ -683,24 +681,11 @@ void ASimModeBase::setupVehiclesAndCamera() //create API objects for each pawn we have for (AActor* pawn : pawns) { - APawn* vehicle_pawn = static_cast(pawn); - - initializeVehiclePawn(vehicle_pawn); + auto vehicle_pawn = static_cast(pawn); - //create vehicle sim api - const auto& ned_transform = getGlobalNedTransform(); - const auto& pawn_ned_pos = ned_transform.toLocalNed(vehicle_pawn->GetActorLocation()); - const auto& home_geopoint= msr::airlib::EarthUtils::nedToGeodetic(pawn_ned_pos, getSettings().origin_geopoint); - const std::string vehicle_name = std::string(TCHAR_TO_UTF8(*(vehicle_pawn->GetName()))); - - PawnSimApi::Params pawn_sim_api_params(vehicle_pawn, &getGlobalNedTransform(), - getVehiclePawnEvents(vehicle_pawn), getVehiclePawnCameras(vehicle_pawn), pip_camera_class, - collision_display_template, home_geopoint, vehicle_name); + auto vehicle_sim_api = createVehicleApi(vehicle_pawn); + std::string vehicle_name = vehicle_sim_api->getVehicleName(); - auto vehicle_sim_api = createVehicleSimApi(pawn_sim_api_params); - auto vehicle_sim_api_p = vehicle_sim_api.get(); - auto vehicle_Api = getVehicleApi(pawn_sim_api_params, vehicle_sim_api_p); - getApiProvider()->insert_or_assign(vehicle_name, vehicle_Api, vehicle_sim_api_p); if ((fpv_pawn == vehicle_pawn || !getApiProvider()->hasDefaultVehicle()) && vehicle_name != "") getApiProvider()->makeDefaultVehicle(vehicle_name); diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h index dd3d09c2be..bfcd24dc75 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h @@ -111,6 +111,8 @@ class AIRSIM_API ASimModeBase : public AActor const PawnSimApi* sim_api) const; protected: //optional overrides + virtual APawn* createVehiclePawn(const AirSimSettings::VehicleSetting& vehicle_setting); + virtual std::unique_ptr createVehicleApi(APawn* vehicle_pawn); virtual void setupVehiclesAndCamera(); virtual void setupInputBindings(); //called when SimMode should handle clock speed setting From e574c727715d056a65e966671fd039d3839d5020 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Sun, 7 Feb 2021 14:51:18 +0530 Subject: [PATCH 21/29] Store correct vehicle settings instead of default one --- AirLib/include/common/AirSimSettings.hpp | 15 +++++++++++---- .../Plugins/AirSim/Source/SimMode/SimModeBase.cpp | 6 +----- Unreal/Plugins/AirSim/Source/WorldSimApi.cpp | 13 +++++-------- 3 files changed, 17 insertions(+), 17 deletions(-) diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index 9aeb937ee3..2f01fb22c5 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/AirLib/include/common/AirSimSettings.hpp @@ -436,12 +436,19 @@ struct AirSimSettings { } // This is for the case when a new vehicle is made on the fly, at runtime - void addVehicleSetting(const VehicleSetting &vehicle_setting) + void addVehicleSetting(const std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path="") { - std::unique_ptr vehicle_setting_p = std::unique_ptr(new VehicleSetting()); + auto vehicle_setting = std::unique_ptr(new VehicleSetting()); - // Usually we have a pointer to an entry from the json, but here we have to make a new one - vehicles[vehicle_setting.vehicle_name] = std::move(vehicle_setting_p); + vehicle_setting->vehicle_name = vehicle_name; + vehicle_setting->vehicle_type = vehicle_type; + vehicle_setting->position = pose.position; + vehicle_setting->pawn_path = pawn_path; + + VectorMath::toEulerianAngle(pose.orientation, vehicle_setting->rotation.pitch, + vehicle_setting->rotation.roll, vehicle_setting->rotation.yaw); + + vehicles[vehicle_name] = std::move(vehicle_setting); } const VehicleSetting* getVehicleSetting(const std::string& vehicle_name) const diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index d3d2e31b9c..aa5f696441 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -609,17 +609,13 @@ std::unique_ptr ASimModeBase::createVehicleApi(APawn* vehicle_pawn) return vehicle_sim_api; } -bool ASimModeBase::createVehicleAtRuntime(const AirSimSettings::VehicleSetting &vehicle_setting) +bool ASimModeBase::createVehicleAtRuntime(const AirSimSettings::VehicleSetting& vehicle_setting) { if (!isVehicleTypeSupported(vehicle_setting.vehicle_type)) { Utils::log(Utils::stringf("Vehicle type %s is not supported in this game mode", vehicle_setting.vehicle_type.c_str()), Utils::kLogLevelWarn); return false; } - // Retroactively adjust AirSimSettings, so it's like we knew about this vehicle all along - // (Other places in the code use this for reference) - AirSimSettings::singleton().addVehicleSetting(vehicle_setting); - auto spawned_pawn = createVehiclePawn(vehicle_setting); auto vehicle_sim_api = createVehicleApi(spawned_pawn); diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index 58ca6c6fe7..fdb7dfe100 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -270,21 +270,18 @@ void WorldSimApi::setTimeOfDay(bool is_enabled, const std::string& start_datetim bool WorldSimApi::createVehicleAtRuntime(const std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path) { - // Create settings object - AirSimSettings::VehicleSetting vehicle_setting; + // TODO: Figure out a better way to add more fields + // Maybe allow passing a JSON string for the vehicle settings? - vehicle_setting.vehicle_name = vehicle_name; - vehicle_setting.vehicle_type = vehicle_type; - vehicle_setting.pawn_path = pawn_path; - vehicle_setting.position = pose.position; - VectorMath::toEulerianAngle(pose.orientation, vehicle_setting.rotation.pitch, vehicle_setting.rotation.roll, vehicle_setting.rotation.yaw); + // Retroactively adjust AirSimSettings, so it's like we knew about this vehicle all along + AirSimSettings::singleton().addVehicleSetting(vehicle_name, vehicle_type, pose, pawn_path); bool result; // We need to run this code on the main game thread, since it iterates over actors FGraphEventRef task = FFunctionGraphTask::CreateAndDispatchWhenReady([&]() { - result = simmode_->createVehicleAtRuntime(vehicle_setting); + result = simmode_->createVehicleAtRuntime(*(AirSimSettings::singleton().getVehicleSetting(vehicle_name))); }, TStatId(), NULL, ENamedThreads::GameThread); // Wait for the result From 68387c68d2adf925e584cff79c7257f137f9744c Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Sun, 19 Jan 2020 21:54:26 +0530 Subject: [PATCH 22/29] Add example Python script for spawn vehicle through API --- PythonClient/multirotor/add_drone.py | 46 ++++++++++++++++++++++++++++ 1 file changed, 46 insertions(+) create mode 100644 PythonClient/multirotor/add_drone.py diff --git a/PythonClient/multirotor/add_drone.py b/PythonClient/multirotor/add_drone.py new file mode 100644 index 0000000000..fd740dd5d5 --- /dev/null +++ b/PythonClient/multirotor/add_drone.py @@ -0,0 +1,46 @@ +import setup_path +import airsim +import tempfile +import os +import numpy as np +import cv2 +import pprint + +# connect to the AirSim simulator +client = airsim.MultirotorClient() +client.confirmConnection() + +# add new vehicle +vehicle_name = "Drone2" +pose = airsim.Pose(airsim.Vector3r(0, 0, 0), airsim.to_quaternion(0, 0, 0)) + +client.simAddVehicle(vehicle_name, "simpleflight", pose) +client.enableApiControl(True, vehicle_name) +client.armDisarm(True, vehicle_name) +client.takeoffAsync(10.0, vehicle_name) + +requests = [airsim.ImageRequest("0", airsim.ImageType.DepthVis), #depth visualization image + airsim.ImageRequest("1", airsim.ImageType.DepthPerspective, True), #depth in perspective projection + airsim.ImageRequest("1", airsim.ImageType.Scene), #scene vision image in png format + airsim.ImageRequest("1", airsim.ImageType.Scene, False, False)] #scene vision image in uncompressed RGBA array + +responses = client.simGetImages(requests, vehicle_name=vehicle_name) +print('Retrieved images: %d' % len(responses)) + +tmp_dir = os.path.join(tempfile.gettempdir(), "airsim_drone") +print ("Saving images to %s" % tmp_dir) +try: + os.makedirs(tmp_dir) +except OSError: + if not os.path.isdir(tmp_dir): + raise + +for idx, response in enumerate(responses): + filename = os.path.join(tmp_dir, str(idx)) + + if response.pixels_as_float: + print("Type %d, size %d, pos %s" % (response.image_type, len(response.image_data_float), pprint.pformat(response.camera_position))) + airsim.write_pfm(os.path.normpath(filename + '.pfm'), airsim.get_pfm_array(response)) + else: + print("Type %d, size %d, pos %s" % (response.image_type, len(response.image_data_uint8), pprint.pformat(response.camera_position))) + airsim.write_file(os.path.normpath(filename + '.png'), response.image_data_uint8) \ No newline at end of file From e5abd0cdbd3fe9e53725273140ebf6a6ff5bcab5 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Thu, 4 Feb 2021 20:13:00 +0530 Subject: [PATCH 23/29] Fix HelloSpawnedDrones C++ file to use new API, naming convention, convert to spaces --- HelloSpawnedDrones/HelloSpawnedDrones.cpp | 243 +++++++++++----------- 1 file changed, 122 insertions(+), 121 deletions(-) diff --git a/HelloSpawnedDrones/HelloSpawnedDrones.cpp b/HelloSpawnedDrones/HelloSpawnedDrones.cpp index 9bfd4e4b6c..0e5b2ba78b 100644 --- a/HelloSpawnedDrones/HelloSpawnedDrones.cpp +++ b/HelloSpawnedDrones/HelloSpawnedDrones.cpp @@ -19,154 +19,155 @@ STRICT_MODE_ON void runSingleClient(uint16_t port, int ordinal) { - using namespace msr::airlib; - const char host[] = "localhost"; - float timeout_s = 60; + using namespace msr::airlib; + const char host[] = "localhost"; + float timeout_s = 60; - try - { - msr::airlib::MultirotorRpcLibClient *client = new msr::airlib::MultirotorRpcLibClient(host, port, timeout_s); - std::cout << "Confirming connections..." << std::endl; - client->confirmConnection(); + try + { + MultirotorRpcLibClient *client = new MultirotorRpcLibClient(host, port, timeout_s); + std::cout << "Confirming connections..." << std::endl; + client->confirmConnection(); - std::string vehicleName = "UAV_" + std::to_string(ordinal); - std::cout << "Vehicle name:" << vehicleName << std::endl; + std::string vehicle_name = "UAV_" + std::to_string(ordinal); + std::cout << "Vehicle name:" << vehicle_name << std::endl; - client->simAddVehicle(vehicleName, "simpleflight", "", 0, 5.0f * (ordinal + 1), 0); + Pose pose(Vector3r(0, 5.0f * (ordinal + 1), 0), Quaternionr(0, 0, 0, 0)); + client->simAddVehicle(vehicle_name, "simpleflight", pose, ""); - // This is a bit crude, but give it a moment to settle on the ground, else takeoff will fail - std::this_thread::sleep_for(std::chrono::duration(2)); + // This is a bit crude, but give it a moment to settle on the ground, else takeoff will fail + std::this_thread::sleep_for(std::chrono::duration(2)); - // moveByVelocityZ is an offboard operation, so we need to set offboard mode. - client->enableApiControl(true, vehicleName); - client->armDisarm(true, vehicleName); + // moveByVelocityZ is an offboard operation, so we need to set offboard mode. + client->enableApiControl(true, vehicle_name); + client->armDisarm(true, vehicle_name); - auto groundPosition = client->getMultirotorState(vehicleName).getPosition(); - float groundZ = groundPosition.z(); // current position (NED coordinate system). + auto ground_pos = client->getMultirotorState(vehicle_name).getPosition(); + float groundZ = ground_pos.z(); // current position (NED coordinate system). - float takeoffTimeout = 5; - std::cout << "Initiating takeoff for " << vehicleName << "..." << std::endl; - client->takeoffAsync(takeoffTimeout, vehicleName)->waitOnLastTask(); - std::cout << "Completed takeoff for " << vehicleName << "..." << std::endl; + float takeoff_timeout = 5; + std::cout << "Initiating takeoff for " << vehicle_name << "..." << std::endl; + client->takeoffAsync(takeoff_timeout, vehicle_name)->waitOnLastTask(); + std::cout << "Completed takeoff for " << vehicle_name << "..." << std::endl; - const float speed = 3.0f; + const float speed = 3.0f; - // switch to explicit hover mode so that this is the fallback when - // move* commands are finished. - std::cout << "Initiating hover for " << vehicleName << "..." << std::endl; - client->hoverAsync(vehicleName)->waitOnLastTask(); - std::cout << "Completed hover for " << vehicleName << "..." << std::endl; + // switch to explicit hover mode so that this is the fallback when + // move* commands are finished. + std::cout << "Initiating hover for " << vehicle_name << "..." << std::endl; + client->hoverAsync(vehicle_name)->waitOnLastTask(); + std::cout << "Completed hover for " << vehicle_name << "..." << std::endl; - auto position = client->getMultirotorState(vehicleName).getPosition(); - float duration = 1; - float z = position.z(); // current position (NED coordinate system). + auto position = client->getMultirotorState(vehicle_name).getPosition(); + float duration = 1; + float z = position.z(); // current position (NED coordinate system). - // Altitude difference between each platform, in meters - const float altitudeDelta = 1.0f; + // Altitude difference between each platform, in meters + const float altitude_delta = 1.0f; - z -= ordinal * altitudeDelta; - float timeout = 10.0f; - client->moveToZAsync(z, speed, timeout, msr::airlib::YawMode(), -1.0f, 1.0f, vehicleName)->waitOnLastTask(); + z -= ordinal * altitude_delta; + float timeout = 10.0f; + client->moveToZAsync(z, speed, timeout, YawMode(), -1.0f, 1.0f, vehicle_name)->waitOnLastTask(); - std::cout << "Completed move to z " << z << " for " << vehicleName << "..." << std::endl; - std::cout << "Flying in a 10m box pattern at 3 m/s velocity" << std::endl; + std::cout << "Completed move to z " << z << " for " << vehicle_name << "..." << std::endl; + std::cout << "Flying in a 10m box pattern at 3 m/s velocity" << std::endl; - const float size = 5.0f; - duration = size / speed; - DrivetrainType driveTrain = DrivetrainType::ForwardOnly; - YawMode yaw_mode(true, 0); + const float size = 5.0f; + duration = size / speed; + DrivetrainType drivetrain = DrivetrainType::ForwardOnly; + YawMode yaw_mode(true, 0); - position = client->getMultirotorState(vehicleName).getPosition(); - std::cout << "Position of " << port << ": " << position << std::endl; - z = position.z(); // current position (NED coordinate system). + position = client->getMultirotorState(vehicle_name).getPosition(); + std::cout << "Position of " << port << ": " << position << std::endl; + z = position.z(); // current position (NED coordinate system). - std::cout << "moveByVelocityZ(" << speed << ", 0, " << z << "," << duration << ")" << std::endl; - client->moveByVelocityZAsync(speed, 0, z, duration, driveTrain, yaw_mode, vehicleName); - std::this_thread::sleep_for(std::chrono::duration(duration)); - std::cout << "moveByVelocityZ(0, " << speed << "," << z << "," << duration << ")" << std::endl; - client->moveByVelocityZAsync(0, speed, z, duration, driveTrain, yaw_mode, vehicleName); - std::this_thread::sleep_for(std::chrono::duration(duration)); - std::cout << "moveByVelocityZ(" << -speed << ", 0, " << z << "," << duration << ")" << std::endl; - client->moveByVelocityZAsync(-speed, 0, z, duration, driveTrain, yaw_mode, vehicleName); - std::this_thread::sleep_for(std::chrono::duration(duration)); - std::cout << "moveByVelocityZ(0, " << -speed << "," << z << "," << duration << ")" << std::endl; - client->moveByVelocityZAsync(0, -speed, z, duration, driveTrain, yaw_mode, vehicleName); + std::cout << "moveByVelocityZ(" << speed << ", 0, " << z << "," << duration << ")" << std::endl; + client->moveByVelocityZAsync(speed, 0, z, duration, drivetrain, yaw_mode, vehicle_name); + std::this_thread::sleep_for(std::chrono::duration(duration)); + std::cout << "moveByVelocityZ(0, " << speed << "," << z << "," << duration << ")" << std::endl; + client->moveByVelocityZAsync(0, speed, z, duration, drivetrain, yaw_mode, vehicle_name); + std::this_thread::sleep_for(std::chrono::duration(duration)); + std::cout << "moveByVelocityZ(" << -speed << ", 0, " << z << "," << duration << ")" << std::endl; + client->moveByVelocityZAsync(-speed, 0, z, duration, drivetrain, yaw_mode, vehicle_name); + std::this_thread::sleep_for(std::chrono::duration(duration)); + std::cout << "moveByVelocityZ(0, " << -speed << "," << z << "," << duration << ")" << std::endl; + client->moveByVelocityZAsync(0, -speed, z, duration, drivetrain, yaw_mode, vehicle_name); - std::this_thread::sleep_for(std::chrono::duration(duration)); + std::this_thread::sleep_for(std::chrono::duration(duration)); - client->moveToZAsync(groundZ - 0.5f, speed, timeout, msr::airlib::YawMode(), -1.0f, 1.0f, vehicleName)->waitOnLastTask(); + client->moveToZAsync(groundZ - 0.5f, speed, timeout, YawMode(), -1.0f, 1.0f, vehicle_name)->waitOnLastTask(); - std::cout << "Hovering..." << std::endl; - client->hoverAsync(vehicleName)->waitOnLastTask(); + std::cout << "Hovering..." << std::endl; + client->hoverAsync(vehicle_name)->waitOnLastTask(); - client->enableApiControl(true, vehicleName); + client->enableApiControl(true, vehicle_name); - std::cout << "Landing..." << std::endl; - client->landAsync(timeout, vehicleName)->waitOnLastTask(); - std::this_thread::sleep_for(std::chrono::duration(5)); + std::cout << "Landing..." << std::endl; + client->landAsync(timeout, vehicle_name)->waitOnLastTask(); + std::this_thread::sleep_for(std::chrono::duration(5)); - std::cout << "Disarming..." << std::endl; - client->armDisarm(false, vehicleName); + std::cout << "Disarming..." << std::endl; + client->armDisarm(false, vehicle_name); - std::cout << "Done!..." << std::endl; + std::cout << "Done!..." << std::endl; - delete client; + delete client; - std::this_thread::sleep_for(std::chrono::duration(50)); - } - catch (rpc::rpc_error& e) - { - std::string msg = e.get_error().as(); - std::cout << "Exception raised by the API, something went wrong." << std::endl << msg << std::endl; - } + std::this_thread::sleep_for(std::chrono::duration(50)); + } + catch (rpc::rpc_error& e) + { + std::string msg = e.get_error().as(); + std::cout << "Exception raised by the API, something went wrong." << std::endl << msg << std::endl; + } } int main(int argc, char *argv[]) { - using namespace msr::airlib; - - uint16_t rpcPort = 41451; - int numPlatforms = 1; - - std::cout << "argc is " << argc << std::endl; - if (argc > 1) - { - std::cout << "Num plats string: " << argv[1] << std::endl; - int numPlatsInt = atoi(argv[1]); - numPlatforms = static_cast(numPlatsInt); - } - - std::cout << "First port is " << rpcPort << std::endl; - std::cout << "Num platforms: " << numPlatforms << std::endl; - std::cout << "Making clients..." << std::endl; - - - try - { - std::cout << "Press Enter to begin..." << std::endl; std::cin.get(); - - std::vector clientThreads; - - // Count down, so the first one can easily go the highest (without knowing count) - int clientOrdinal = numPlatforms - 1; - for (int i = 0; i < numPlatforms; i++) - { - clientThreads.push_back(std::thread(runSingleClient, rpcPort, clientOrdinal)); - clientOrdinal--; - - std::this_thread::sleep_for(std::chrono::duration(0.1)); - } - - for (auto &toJoin : clientThreads) - { - toJoin.join(); - } - } - catch (rpc::rpc_error& e) - { - std::string msg = e.get_error().as(); - std::cout << "Exception raised by the API, something went wrong." << std::endl << msg << std::endl; - } - - return 0; + using namespace msr::airlib; + + uint16_t rpc_port = 41451; + int num_platforms = 1; + + std::cout << "argc is " << argc << std::endl; + if (argc > 1) + { + std::cout << "Num plats string: " << argv[1] << std::endl; + num_platforms = static_cast(std::stoi(argv[1])); + } + + std::cout << "First port is " << rpc_port << std::endl; + std::cout << "Num platforms: " << num_platforms << std::endl; + std::cout << "Making clients..." << std::endl; + + + try + { + std::cout << "Press Enter to begin..." << std::endl; + std::cin.get(); + + std::vector clientThreads; + + // Count down, so the first one can easily go the highest (without knowing count) + int client_ordinal = num_platforms - 1; + for (int i = 0; i < num_platforms; i++) + { + clientThreads.push_back(std::thread(runSingleClient, rpc_port, client_ordinal)); + client_ordinal--; + + std::this_thread::sleep_for(std::chrono::duration(0.1)); + } + + for (auto &toJoin : clientThreads) + { + toJoin.join(); + } + } + catch (rpc::rpc_error& e) + { + std::string msg = e.get_error().as(); + std::cout << "Exception raised by the API, something went wrong." << std::endl << msg << std::endl; + } + + return 0; } From 2fbfb0c2814b6b2895052d6dbca933c1556387d7 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Sat, 13 Feb 2021 22:35:29 +0530 Subject: [PATCH 24/29] Adapt to AirSim naming conventions, cleanup --- AirLib/include/api/WorldSimApiBase.hpp | 2 +- AirLib/src/api/RpcLibServerBase.cpp | 2 +- .../AirSim/Source/SimMode/SimModeBase.cpp | 36 ++++++++++++------- .../AirSim/Source/SimMode/SimModeBase.h | 5 +-- .../AirSim/Source/SimMode/SimModeWorldBase.h | 2 +- Unreal/Plugins/AirSim/Source/WorldSimApi.cpp | 20 +++-------- Unreal/Plugins/AirSim/Source/WorldSimApi.h | 2 +- 7 files changed, 35 insertions(+), 34 deletions(-) diff --git a/AirLib/include/api/WorldSimApiBase.hpp b/AirLib/include/api/WorldSimApiBase.hpp index 541aa36ca3..baaa3c6714 100644 --- a/AirLib/include/api/WorldSimApiBase.hpp +++ b/AirLib/include/api/WorldSimApiBase.hpp @@ -47,7 +47,7 @@ class WorldSimApiBase { virtual bool setSegmentationObjectID(const std::string& mesh_name, int object_id, bool is_name_regex = false) = 0; virtual int getSegmentationObjectID(const std::string& mesh_name) const = 0; - virtual bool createVehicleAtRuntime(const std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path = "") = 0; + virtual bool addVehicle(const std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path = "") = 0; virtual void printLogMessage(const std::string& message, const std::string& message_param = "", unsigned char severity = 0) = 0; diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index 16fcd5337e..38a8b0cb85 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -155,7 +155,7 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& pimpl_->server.bind("simAddVehicle", [&](const std::string& vehicle_name, const std::string& vehicle_type, const RpcLibAdapatorsBase::Pose& pose, const std::string& pawn_path) -> bool { - return getWorldSimApi()->createVehicleAtRuntime(vehicle_name, vehicle_type, pose.to(), pawn_path); + return getWorldSimApi()->addVehicle(vehicle_name, vehicle_type, pose.to(), pawn_path); }); pimpl_->server.bind("simSetVehiclePose", [&](const RpcLibAdapatorsBase::Pose &pose, bool ignore_collision, const std::string& vehicle_name) -> void { diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index aa5f696441..d81d6f9780 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -594,14 +594,14 @@ std::unique_ptr ASimModeBase::createVehicleApi(APawn* vehicle_pawn) //create vehicle sim api const auto& ned_transform = getGlobalNedTransform(); const auto& pawn_ned_pos = ned_transform.toLocalNed(vehicle_pawn->GetActorLocation()); - const auto& home_geopoint= msr::airlib::EarthUtils::nedToGeodetic(pawn_ned_pos, getSettings().origin_geopoint); - const std::string vehicle_name = std::string(TCHAR_TO_UTF8(*(vehicle_pawn->GetName()))); + const auto& home_geopoint = msr::airlib::EarthUtils::nedToGeodetic(pawn_ned_pos, getSettings().origin_geopoint); + const std::string vehicle_name( TCHAR_TO_UTF8(*(vehicle_pawn->GetName())) ); PawnSimApi::Params pawn_sim_api_params(vehicle_pawn, &getGlobalNedTransform(), getVehiclePawnEvents(vehicle_pawn), getVehiclePawnCameras(vehicle_pawn), pip_camera_class, collision_display_template, home_geopoint, vehicle_name); - auto vehicle_sim_api = createVehicleSimApi(pawn_sim_api_params); + std::unique_ptr vehicle_sim_api = createVehicleSimApi(pawn_sim_api_params); auto vehicle_sim_api_p = vehicle_sim_api.get(); auto vehicle_api = getVehicleApi(pawn_sim_api_params, vehicle_sim_api_p); getApiProvider()->insert_or_assign(vehicle_name, vehicle_api, vehicle_sim_api_p); @@ -609,25 +609,31 @@ std::unique_ptr ASimModeBase::createVehicleApi(APawn* vehicle_pawn) return vehicle_sim_api; } -bool ASimModeBase::createVehicleAtRuntime(const AirSimSettings::VehicleSetting& vehicle_setting) +bool ASimModeBase::createVehicleAtRuntime(const std::string& vehicle_name, const std::string& vehicle_type, + const msr::airlib::Pose& pose, const std::string& pawn_path) { - if (!isVehicleTypeSupported(vehicle_setting.vehicle_type)) { - Utils::log(Utils::stringf("Vehicle type %s is not supported in this game mode", vehicle_setting.vehicle_type.c_str()), Utils::kLogLevelWarn); + if (!isVehicleTypeSupported(vehicle_type)) { + Utils::log(Utils::stringf("Vehicle type %s is not supported in this game mode", vehicle_type.c_str()), Utils::kLogLevelWarn); return false; } - auto spawned_pawn = createVehiclePawn(vehicle_setting); + // TODO: Figure out a better way to add more fields + // Maybe allow passing a JSON string for the vehicle settings? + + // Retroactively adjust AirSimSettings, so it's like we knew about this vehicle all along + AirSimSettings::singleton().addVehicleSetting(vehicle_name, vehicle_type, pose, pawn_path); + const auto* vehicle_setting = getSettings().getVehicleSetting(vehicle_name); + + auto spawned_pawn = createVehiclePawn(*vehicle_setting); auto vehicle_sim_api = createVehicleApi(spawned_pawn); - auto vehicle_sim_api_p = vehicle_sim_api.get(); - std::string vehicle_name = vehicle_sim_api->getVehicleName(); // Usually physics registration happens at init, in ASimModeWorldBase::initializeForPlay(), but not in this case - vehicle_sim_api_p->reset(); - registerPhysicsBody(vehicle_sim_api_p); + vehicle_sim_api->reset(); + registerPhysicsBody(vehicle_sim_api.get()); // Can't be done before the vehicle apis have been created - if ((vehicle_setting.is_fpv_vehicle || !getApiProvider()->hasDefaultVehicle()) && vehicle_name != "") + if ((vehicle_setting->is_fpv_vehicle || !getApiProvider()->hasDefaultVehicle()) && vehicle_name != "") getApiProvider()->makeDefaultVehicle(vehicle_name); vehicle_sim_apis_.push_back(std::move(vehicle_sim_api)); @@ -701,6 +707,12 @@ void ASimModeBase::setupVehiclesAndCamera() checkVehicleReady(); } +void ASimModeBase::registerPhysicsBody(msr::airlib::VehicleSimApiBase *physicsBody) +{ + // derived class shoudl override this method to add new vehicle to the physics engine +} + + void ASimModeBase::getExistingVehiclePawns(TArray& pawns) const { //derived class should override this method to retrieve types of pawns they support diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h index bfcd24dc75..6938413f20 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h @@ -75,7 +75,8 @@ class AIRSIM_API ASimModeBase : public AActor void stopApiServer(); bool isApiServerStarted(); - bool createVehicleAtRuntime(const msr::airlib::AirSimSettings::VehicleSetting &vehicle_setting); + bool createVehicleAtRuntime(const std::string& vehicle_name, const std::string& vehicle_type, + const msr::airlib::Pose& pose, const std::string& pawn_path = ""); const NedTransform& getGlobalNedTransform(); @@ -109,6 +110,7 @@ class AIRSIM_API ASimModeBase : public AActor const PawnSimApi::Params& pawn_sim_api_params) const; virtual msr::airlib::VehicleApiBase* getVehicleApi(const PawnSimApi::Params& pawn_sim_api_params, const PawnSimApi* sim_api) const; + virtual void registerPhysicsBody(msr::airlib::VehicleSimApiBase *physicsBody); protected: //optional overrides virtual APawn* createVehiclePawn(const AirSimSettings::VehicleSetting& vehicle_setting); @@ -120,7 +122,6 @@ 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 registerPhysicsBody(msr::airlib::VehicleSimApiBase *physicsBody) {} protected: //Utility methods for derived classes virtual const msr::airlib::AirSimSettings& getSettings() const; diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h index 7b2febd758..723d25fc45 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h @@ -43,7 +43,7 @@ class AIRSIM_API ASimModeWorldBase : public ASimModeBase void initializeForPlay(); //used for adding physics bodies on the fly - virtual void registerPhysicsBody(msr::airlib::VehicleSimApiBase *physicsBody); + virtual void registerPhysicsBody(msr::airlib::VehicleSimApiBase *physicsBody) override; long long getPhysicsLoopPeriod() const; void setPhysicsLoopPeriod(long long period); diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index fdb7dfe100..6de0bacc01 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -268,24 +268,12 @@ void WorldSimApi::setTimeOfDay(bool is_enabled, const std::string& start_datetim celestial_clock_speed, update_interval_secs, move_sun); } -bool WorldSimApi::createVehicleAtRuntime(const std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path) +bool WorldSimApi::addVehicle(const std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path) { - // TODO: Figure out a better way to add more fields - // Maybe allow passing a JSON string for the vehicle settings? - - // Retroactively adjust AirSimSettings, so it's like we knew about this vehicle all along - AirSimSettings::singleton().addVehicleSetting(vehicle_name, vehicle_type, pose, pawn_path); - bool result; - - // We need to run this code on the main game thread, since it iterates over actors - FGraphEventRef task = FFunctionGraphTask::CreateAndDispatchWhenReady([&]() - { - result = simmode_->createVehicleAtRuntime(*(AirSimSettings::singleton().getVehicleSetting(vehicle_name))); - }, TStatId(), NULL, ENamedThreads::GameThread); - - // Wait for the result - FTaskGraphInterface::Get().WaitUntilTaskCompletes(task); + UAirBlueprintLib::RunCommandOnGameThread([&]() { + result = simmode_->createVehicleAtRuntime(vehicle_name, vehicle_type, pose, pawn_path); + }, true); return result; } diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.h b/Unreal/Plugins/AirSim/Source/WorldSimApi.h index 4072c33b3a..8ca7b8de7e 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.h +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.h @@ -38,7 +38,7 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase { virtual bool setSegmentationObjectID(const std::string& mesh_name, int object_id, bool is_name_regex = false) override; virtual int getSegmentationObjectID(const std::string& mesh_name) const override; - virtual bool createVehicleAtRuntime(const std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path = "") override; + virtual bool addVehicle(const std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path = "") override; virtual void printLogMessage(const std::string& message, const std::string& message_param = "", unsigned char severity = 0) override; From e3b28209e26ba4f7f3a966110488e938182fc51d Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Mon, 8 Feb 2021 09:16:43 +0530 Subject: [PATCH 25/29] Unity: Add unimplemented simAddVehicle API method --- Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp | 8 ++++++++ Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h | 1 + 2 files changed, 9 insertions(+) diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp index 4679fea828..cc881cad32 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp @@ -195,4 +195,12 @@ void WorldSimApi::setWind(const Vector3r& wind) const simmode_->setWind(wind); }; +bool WorldSimApi::addVehicle(const std::string& vehicle_name, const std::string& vehicle_type, const WorldSimApi::Pose& pose, const std::string& pawn_path) +{ + throw std::invalid_argument(common_utils::Utils::stringf( + "addVehicle is not supported on unity").c_str()); + return false; +} + + #pragma endregion diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h index d19928bd9b..548f4b2149 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h @@ -64,6 +64,7 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase virtual void setWind(const Vector3r& wind) const override; virtual bool createVoxelGrid(const Vector3r& position, const int& x_size, const int& y_size, const int& z_size, const float& res, const std::string& output_file) override; + virtual bool addVehicle(const std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path = "") override; private: SimModeBase * simmode_; From f9fbc8220a8e1caf25294a673dc08271324874c5 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Sat, 6 Mar 2021 00:19:10 +0530 Subject: [PATCH 26/29] HelloDrone: Fix variable naming to align with convention --- HelloDrone/main.cpp | 26 ++++++++++++++++---------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/HelloDrone/main.cpp b/HelloDrone/main.cpp index 6e0d83574f..578ab69b90 100644 --- a/HelloDrone/main.cpp +++ b/HelloDrone/main.cpp @@ -27,8 +27,10 @@ int main() try { client.confirmConnection(); - std::cout << "Press Enter to get FPV image" << std::endl; std::cin.get(); - vector request = { ImageRequest("0", ImageType::Scene), ImageRequest("1", ImageType::DepthPlanner, true) }; + std::cout << "Press Enter to get FPV image" << std::endl; + std::cin.get(); + + vector request{ ImageRequest("0", ImageType::Scene), ImageRequest("1", ImageType::DepthPlanner, true) }; const vector& response = client.simGetImages(request); std::cout << "# of images received: " << response.size() << std::endl; @@ -56,7 +58,9 @@ int main() } } - std::cout << "Press Enter to arm the drone" << std::endl; std::cin.get(); + std::cout << "Press Enter to arm the drone" << std::endl; + std::cin.get(); + client.enableApiControl(true); client.armDisarm(true); @@ -91,8 +95,8 @@ int main() // << "magnetometer_data.magnetic_field_covariance" << magnetometer_data.magnetic_field_covariance // not implemented in sensor std::cout << "Press Enter to takeoff" << std::endl; std::cin.get(); - float takeoffTimeout = 5; - client.takeoffAsync(takeoffTimeout)->waitOnLastTask(); + float takeoff_timeout = 5; + client.takeoffAsync(takeoff_timeout)->waitOnLastTask(); // switch to explicit hover mode so that this is the fall back when // move* commands are finished. @@ -102,24 +106,26 @@ int main() std::cout << "Press Enter to fly in a 10m box pattern at 3 m/s velocity" << std::endl; std::cin.get(); // moveByVelocityZ is an offboard operation, so we need to set offboard mode. client.enableApiControl(true); + auto position = client.getMultirotorState().getPosition(); float z = position.z(); // current position (NED coordinate system). const float speed = 3.0f; const float size = 10.0f; const float duration = size / speed; - DrivetrainType driveTrain = DrivetrainType::ForwardOnly; + DrivetrainType drivetrain = DrivetrainType::ForwardOnly; YawMode yaw_mode(true, 0); + std::cout << "moveByVelocityZ(" << speed << ", 0, " << z << "," << duration << ")" << std::endl; - client.moveByVelocityZAsync(speed, 0, z, duration, driveTrain, yaw_mode); + client.moveByVelocityZAsync(speed, 0, z, duration, drivetrain, yaw_mode); std::this_thread::sleep_for(std::chrono::duration(duration)); std::cout << "moveByVelocityZ(0, " << speed << "," << z << "," << duration << ")" << std::endl; - client.moveByVelocityZAsync(0, speed, z, duration, driveTrain, yaw_mode); + client.moveByVelocityZAsync(0, speed, z, duration, drivetrain, yaw_mode); std::this_thread::sleep_for(std::chrono::duration(duration)); std::cout << "moveByVelocityZ(" << -speed << ", 0, " << z << "," << duration << ")" << std::endl; - client.moveByVelocityZAsync(-speed, 0, z, duration, driveTrain, yaw_mode); + client.moveByVelocityZAsync(-speed, 0, z, duration, drivetrain, yaw_mode); std::this_thread::sleep_for(std::chrono::duration(duration)); std::cout << "moveByVelocityZ(0, " << -speed << "," << z << "," << duration << ")" << std::endl; - client.moveByVelocityZAsync(0, -speed, z, duration, driveTrain, yaw_mode); + client.moveByVelocityZAsync(0, -speed, z, duration, drivetrain, yaw_mode); std::this_thread::sleep_for(std::chrono::duration(duration)); client.hoverAsync()->waitOnLastTask(); From 6c65de74717767723e03efd7c9de5ec5624be651 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Sat, 6 Mar 2021 00:30:10 +0530 Subject: [PATCH 27/29] Docs: Fix HelloSpawnedDrones location --- docs/apis_cpp.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/apis_cpp.md b/docs/apis_cpp.md index 3d41b98cea..0e87e2c2ac 100644 --- a/docs/apis_cpp.md +++ b/docs/apis_cpp.md @@ -88,6 +88,6 @@ int main() ## See Also * [Examples](../Examples) of how to use internal infrastructure in AirSim in your other projects * [DroneShell](../DroneShell) app shows how to make simple interface using C++ APIs to control drones -* [HelloSpawnedDrones](./HelloSpawnedDrones) app shows how to make additional vehicles on the fly +* [HelloSpawnedDrones](../HelloSpawnedDrones) app shows how to make additional vehicles on the fly * [Python APIs](apis.md) From 0536f61682eff0a1feb58b6006d8d3023caa8f61 Mon Sep 17 00:00:00 2001 From: zimmy87 Date: Tue, 9 Mar 2021 06:29:29 -0800 Subject: [PATCH 28/29] Update HelloSpawnedDrones.vcxproj add project dependencies --- HelloSpawnedDrones/HelloSpawnedDrones.vcxproj | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/HelloSpawnedDrones/HelloSpawnedDrones.vcxproj b/HelloSpawnedDrones/HelloSpawnedDrones.vcxproj index 2df07271a5..d150fadca8 100644 --- a/HelloSpawnedDrones/HelloSpawnedDrones.vcxproj +++ b/HelloSpawnedDrones/HelloSpawnedDrones.vcxproj @@ -18,6 +18,14 @@ x64 + + + {4bfb7231-077a-4671-bd21-d3ade3ea36e7} + + + {8510c7a4-bf63-41d2-94f6-d8731d137a5a} + + 15.0 {99CBF376-5EBA-4164-A657-E7D708C9D685} From 927b20e003581650d0d83a6bdd6138a88cfb1de2 Mon Sep 17 00:00:00 2001 From: zimmy87 Date: Tue, 9 Mar 2021 07:42:36 -0800 Subject: [PATCH 29/29] Update SimModeBase.h remove duplicate declaration of registerPhysicsBody() --- Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h | 1 - 1 file changed, 1 deletion(-) diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h index 7d28a4b97e..6938413f20 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h @@ -122,7 +122,6 @@ 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 registerPhysicsBody(msr::airlib::VehicleSimApiBase *physicsBody) {} protected: //Utility methods for derived classes virtual const msr::airlib::AirSimSettings& getSettings() const;