Merge pull request #2390 from rajat2004/pr/1937

Spawn vehicles via RPC

.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

AirLib/include/api/RpcLibClientBase.hpp

@@ -69,7 +69,7 @@ class RpcLibClientBase {
     void simPlotLineStrip(const vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float duration, bool is_persistent);
     void simPlotLineList(const vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float duration, bool is_persistent);
     void simPlotArrows(const vector<Vector3r>& points_start, const vector<Vector3r>& points_end, const vector<float>& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent);
-    void simPlotStrings(const vector<std::string>& strings, const vector<Vector3r>& positions, float scale, const vector<float>& color_rgba, float duration); 
+    void simPlotStrings(const vector<std::string>& strings, const vector<Vector3r>& positions, float scale, const vector<float>& color_rgba, float duration);
     void simPlotTransforms(const vector<Pose>& poses, float scale, float thickness, float duration, bool is_persistent);
     void simPlotTransformsWithNames(const vector<Pose>& poses, const vector<std::string>& names, float tf_scale, float tf_thickness, float text_scale, const vector<float>& text_color_rgba, float duration);
 
@@ -97,6 +97,7 @@ class RpcLibClientBase {
     vector<uint8_t> simGetImage(const std::string& camera_name, ImageCaptureBase::ImageType type, const std::string& vehicle_name = "");
 
     vector<MeshPositionVertexBuffersResponse> simGetMeshPositionVertexBuffers();
+    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;
 

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 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;
 

AirLib/include/common/AirSimSettings.hpp

@@ -435,6 +435,22 @@ 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 std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path="")
+    {
+        auto vehicle_setting = std::unique_ptr<VehicleSetting>(new VehicleSetting());
+
+        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
     {
         auto it = vehicles.find(vehicle_name);

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_;

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,15 +125,15 @@ 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;
 
     auto server_ver = getServerVersion();
     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<float>& color_rgba, flo
 
 vector<ImageCaptureBase::ImageResponse> RpcLibClientBase::simGetImages(vector<ImageCaptureBase::ImageRequest> 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<vector<RpcLibAdapatorsBase::ImageResponse>>();
 
@@ -243,6 +243,11 @@ vector<MeshPositionVertexBuffersResponse> RpcLibClientBase::simGetMeshPositionVe
     return RpcLibAdapatorsBase::MeshPositionVertexBuffersResponse::to(response_adaptor);
 }
 
+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, RpcLibAdapatorsBase::Pose(pose), pawn_path).as<bool>();
+}
+
 void RpcLibClientBase::simPrintLogMessage(const std::string& message, std::string message_param, unsigned char  severity)
 {
     pimpl_->client.call("simPrintLogMessage", message, message_param, severity);

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,12 @@ 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 RpcLibAdapatorsBase::Pose& pose, const std::string& pawn_path) -> bool {
+        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 {
         getVehicleSimApi(vehicle_name)->setPose(pose.to(), ignore_collision);
     });
 
@@ -174,7 +178,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 +260,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 +281,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);
     });
 

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

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<ImageRequest> request = { ImageRequest("0", ImageType::Scene), ImageRequest("1", ImageType::DepthPlanner, true) };
+        std::cout << "Press Enter to get FPV image" << std::endl;
+        std::cin.get();
+
+        vector<ImageRequest> request{ ImageRequest("0", ImageType::Scene), ImageRequest("1", ImageType::DepthPlanner, true) };
         const vector<ImageResponse>& 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<double>(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<double>(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<double>(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<double>(duration));
 
         client.hoverAsync()->waitOnLastTask();