diff --git a/AirLib/include/api/RpcLibAdapatorsBase.hpp b/AirLib/include/api/RpcLibAdapatorsBase.hpp index 2295d75710..8991be34ed 100644 --- a/AirLib/include/api/RpcLibAdapatorsBase.hpp +++ b/AirLib/include/api/RpcLibAdapatorsBase.hpp @@ -377,7 +377,7 @@ class RpcLibAdapatorsBase { return request; } }; - + struct ImageResponse { std::vector image_data_uint8; std::vector image_data_float; @@ -401,8 +401,8 @@ class RpcLibAdapatorsBase { ImageResponse(const msr::airlib::ImageCaptureBase::ImageResponse& s) { pixels_as_float = s.pixels_as_float; - - image_data_uint8 = s.image_data_uint8; + + image_data_uint8 = *(s.image_data_uint8); image_data_float = s.image_data_float; //TODO: remove bug workaround for https://github.com/rpclib/rpclib/issues/152 @@ -428,8 +428,8 @@ class RpcLibAdapatorsBase { d.pixels_as_float = pixels_as_float; - if (! pixels_as_float) - d.image_data_uint8 = image_data_uint8; + if (!pixels_as_float) + d.image_data_uint8->insert(d.image_data_uint8->begin(), image_data_uint8.front(), image_data_uint8.back()); else d.image_data_float = image_data_float; diff --git a/AirLib/include/api/VehicleSimApiBase.hpp b/AirLib/include/api/VehicleSimApiBase.hpp index 25b9579b03..cc3fb4b764 100644 --- a/AirLib/include/api/VehicleSimApiBase.hpp +++ b/AirLib/include/api/VehicleSimApiBase.hpp @@ -45,8 +45,8 @@ class VehicleSimApiBase : public msr::airlib::UpdatableObject { virtual void initialize() = 0; - virtual std::vector getImages(const std::vector& request) const = 0; - virtual std::vector getImage(const std::string& camera_name, ImageCaptureBase::ImageType image_type) const = 0; + virtual void getImages(const std::vector& request, std::vector &responses) const = 0; + virtual void getImage(const ImageCaptureBase::ImageRequest& request, ImageCaptureBase::ImageResponse &response) const = 0; virtual Pose getPose() const = 0; virtual void setPose(const Pose& pose, bool ignore_collision) = 0; diff --git a/AirLib/include/common/ImageCaptureBase.hpp b/AirLib/include/common/ImageCaptureBase.hpp index 1c16848f02..f6feb070f1 100644 --- a/AirLib/include/common/ImageCaptureBase.hpp +++ b/AirLib/include/common/ImageCaptureBase.hpp @@ -4,8 +4,10 @@ #ifndef air_ImageCaptureBase_hpp #define air_ImageCaptureBase_hpp +#pragma once #include "common/Common.hpp" #include "common/common_utils/EnumFlags.hpp" +#include namespace msr { namespace airlib { @@ -44,7 +46,7 @@ class ImageCaptureBase }; struct ImageResponse { - vector image_data_uint8; + std::unique_ptr, std::function*)>> image_data_uint8 = nullptr; vector image_data_float; std::string camera_name; @@ -56,10 +58,49 @@ class ImageCaptureBase bool compress = true; int width = 0, height = 0; ImageType image_type; + + ImageResponse() : image_data_uint8(nullptr), camera_name(""), camera_position(Vector3r::Zero()), camera_orientation(Quaternionr::Identity()), time_stamp(0), message(""), pixels_as_float(false), compress(true), width(0), height(0), image_type(ImageType::Scene) {} + + ImageResponse(const ImageResponse& other) + { + image_data_uint8 = std::unique_ptr, std::function*)>>(other.image_data_uint8.get(), std::bind(&ImageResponse::CopyDeleter, this, std::placeholders::_1)); + camera_name = other.camera_name; + camera_position = other.camera_position; + camera_orientation = other.camera_orientation; + time_stamp = other.time_stamp; + message = other.message; + pixels_as_float = other.pixels_as_float; + compress = other.compress; + width = other.width; + height = other.height; + image_type = other.image_type; + } + + ImageResponse& operator=(const ImageResponse& other) + { + image_data_uint8 = std::unique_ptr, std::function*)>>(other.image_data_uint8.get(), std::bind(&ImageResponse::CopyDeleter, this, std::placeholders::_1)); + camera_name = other.camera_name; + camera_position = other.camera_position; + camera_orientation = other.camera_orientation; + time_stamp = other.time_stamp; + message = other.message; + pixels_as_float = other.pixels_as_float; + compress = other.compress; + width = other.width; + height = other.height; + image_type = other.image_type; + return *this; + } + + private: + void CopyDeleter(vector* buf) { + buf = nullptr; //Avoid error for unreferenced formal parameter. + } //Copies of an ImageResponse effectively contain weak pointers. The original response handles deleting the buffer. Ultimately, response copying should be removed from everywhere. }; public: //methods virtual void getImages(const std::vector& requests, std::vector& responses) const = 0; + virtual void getImage(const ImageRequest& request, ImageResponse& response) const = 0; }; diff --git a/AirLib/include/common/common_utils/BufferPool.h b/AirLib/include/common/common_utils/BufferPool.h new file mode 100644 index 0000000000..cb7ec7caf3 --- /dev/null +++ b/AirLib/include/common/common_utils/BufferPool.h @@ -0,0 +1,37 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#pragma once +#include +#include +#include +#include +#include +#include + +typedef std::vector Buffer; +typedef std::function Deleter; +typedef std::unique_ptr BufferPtr; + +class BufferPool +{ +public: + BufferPtr GetBufferExactSize(size_t size); + BufferPtr GetBufferAtLeastSize(size_t size, size_t max_size = std::numeric_limits::max()); + +private: + class BufferCollection + { + public: + BufferCollection(size_t size) : Size(size) {} + BufferPtr DemandBuffer(); + BufferPtr GetBufferIfAvailable(); + + private: + size_t Size; + std::unordered_set AvailableBuffers; + BufferPtr MakeBufferPtr(Buffer *underlyingBuffer = nullptr); + void ReturnToCollection(Buffer *buffer); + }; + std::map CollectionsBySize; +}; \ No newline at end of file diff --git a/AirLib/include/common/common_utils/Utils.hpp b/AirLib/include/common/common_utils/Utils.hpp index 25149893cb..2ca8c0c485 100644 --- a/AirLib/include/common/common_utils/Utils.hpp +++ b/AirLib/include/common/common_utils/Utils.hpp @@ -333,7 +333,7 @@ class Utils { } it++; start = it; - for (; it != end; it++) { + for (; it != end; it++) { if (*it == ch) { break; } @@ -585,20 +585,20 @@ class Utils { return std::numeric_limits::epsilon(); } - //implements relative method - do not use for comparing with zero - //use this most of the time, tolerance needs to be meaningful in your context - template - static bool isApproximatelyEqual(TReal a, TReal b, TReal tolerance = epsilon()) - { - TReal diff = std::fabs(a - b); - if (diff <= tolerance) - return true; + //implements relative method - do not use for comparing with zero + //use this most of the time, tolerance needs to be meaningful in your context + template + static bool isApproximatelyEqual(TReal a, TReal b, TReal tolerance = epsilon()) + { + TReal diff = std::fabs(a - b); + if (diff <= tolerance) + return true; - if (diff < std::fmax(std::fabs(a), std::fabs(b)) * tolerance) - return true; + if (diff < std::fmax(std::fabs(a), std::fabs(b)) * tolerance) + return true; - return false; - } + return false; + } //supply tolerance that is meaningful in your context //for example, default tolerance may not work if you are comparing double with float diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index 536f392f13..6dcc97a7cf 100644 --- a/AirLib/src/api/RpcLibClientBase.cpp +++ b/AirLib/src/api/RpcLibClientBase.cpp @@ -232,6 +232,7 @@ vector RpcLibClientBase::simGetImages(vector RpcLibClientBase::simGetImage(const std::string& camera_name, ImageCaptureBase::ImageType type, const std::string& vehicle_name) { vector result = pimpl_->client.call("simGetImage", camera_name, type, vehicle_name).as>(); diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index 2cb514e0de..710e724c92 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -122,18 +122,27 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& pimpl_->server.bind("armDisarm", [&](bool arm, const std::string& vehicle_name) -> bool { return getVehicleApi(vehicle_name)->armDisarm(arm); }); - pimpl_->server.bind("simGetImages", [&](const std::vector& request_adapter, const std::string& vehicle_name) -> - vector { - const auto& response = getVehicleSimApi(vehicle_name)->getImages(RpcLibAdapatorsBase::ImageRequest::to(request_adapter)); - return RpcLibAdapatorsBase::ImageResponse::from(response); + + pimpl_->server.bind("simGetImages", [&](const std::vector& request_adapter, const std::string& vehicle_name) -> vector { + std::vector responses; + for (RpcLibAdapatorsBase::ImageRequest request : request_adapter) + responses.push_back(ImageCaptureBase::ImageResponse()); + getVehicleSimApi(vehicle_name)->getImages(RpcLibAdapatorsBase::ImageRequest::to(request_adapter), responses); + return RpcLibAdapatorsBase::ImageResponse::from(responses); }); + pimpl_->server.bind("simGetImage", [&](const std::string& camera_name, ImageCaptureBase::ImageType type, const std::string& vehicle_name) -> vector { - auto result = getVehicleSimApi(vehicle_name)->getImage(camera_name, type); - if (result.size() == 0) { - // rpclib has a bug with serializing empty vectors, so we return a 1 byte vector instead. - result.push_back(0); - } - return result; + + ImageCaptureBase::ImageRequest request(camera_name, type); + ImageCaptureBase::ImageResponse response; + + getVehicleSimApi(vehicle_name)->getImage(request, response); + + // rpclib has a bug with serializing empty vectors, so we return a 1 byte vector instead. + if (response.image_data_uint8->size() == 0) + response.image_data_uint8->push_back(0); + + return *response.image_data_uint8; }); pimpl_->server.bind("simGetMeshPositionVertexBuffers", [&]() ->vector { @@ -309,9 +318,9 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& getVehicleApi(vehicle_name)->cancelLastTask(); }); - pimpl_->server.bind("simSwapTextures", [&](const std::string tag, int tex_id, int component_id, int material_id) -> std::vector { - return *getWorldSimApi()->swapTextures(tag, tex_id, component_id, material_id); - }); + pimpl_->server.bind("simSwapTextures", [&](const std::string tag, int tex_id, int component_id, int material_id) -> std::vector { + return *getWorldSimApi()->swapTextures(tag, tex_id, component_id, material_id); + }); //if we don't suppress then server will bomb out for exceptions raised by any method pimpl_->server.suppress_exceptions(true); diff --git a/AirLib/src/common/common_utils/BufferPool.cpp b/AirLib/src/common/common_utils/BufferPool.cpp new file mode 100644 index 0000000000..5911eecad6 --- /dev/null +++ b/AirLib/src/common/common_utils/BufferPool.cpp @@ -0,0 +1,55 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#include "common/common_utils/BufferPool.h" + +BufferPtr BufferPool::GetBufferExactSize(size_t size) +{ + if (CollectionsBySize.count(size) == 0) + CollectionsBySize.insert(std::pair(size, BufferCollection(size))); + + return CollectionsBySize.at(size).DemandBuffer(); +} + +BufferPtr BufferPool::GetBufferAtLeastSize(size_t size, size_t max_size) +{ + BufferPtr buffer = nullptr; + auto closestOffering = CollectionsBySize.lower_bound(size); + if (closestOffering != CollectionsBySize.end() && closestOffering->first < max_size) + buffer = closestOffering->second.GetBufferIfAvailable(); + + if (buffer != nullptr) + return buffer; + return GetBufferExactSize(size); +} + +BufferPtr BufferPool::BufferCollection::DemandBuffer() +{ + BufferPtr buf = GetBufferIfAvailable(); + if (buf != nullptr) + return buf; + return MakeBufferPtr(); +} + +BufferPtr BufferPool::BufferCollection::GetBufferIfAvailable() +{ + if (AvailableBuffers.size() == 0) + return nullptr; + + Buffer *buffer = *AvailableBuffers.begin(); + AvailableBuffers.erase(buffer); + return MakeBufferPtr(buffer); +} + +BufferPtr BufferPool::BufferCollection::MakeBufferPtr(Buffer *underlyingBuffer) +{ + if (underlyingBuffer == nullptr) + return std::unique_ptr(new Buffer(Size), std::bind(&BufferPool::BufferCollection::ReturnToCollection, this, std::placeholders::_1)); + else + return std::unique_ptr(underlyingBuffer, std::bind(&BufferPool::BufferCollection::ReturnToCollection, this, std::placeholders::_1)); +} + +void BufferPool::BufferCollection::ReturnToCollection(Buffer *buffer) +{ + AvailableBuffers.insert(buffer); +} \ No newline at end of file diff --git a/Examples/DataCollection/DataCollectorSGM.h b/Examples/DataCollection/DataCollectorSGM.h index b5e228db67..5087f5ae52 100644 --- a/Examples/DataCollection/DataCollectorSGM.h +++ b/Examples/DataCollection/DataCollectorSGM.h @@ -204,8 +204,8 @@ class DataCollectorSGM { counter++; continue; } - left_img[idx-counter] = result->response.at(0).image_data_uint8 [idx]; - right_img[idx-counter] = result->response.at(1).image_data_uint8 [idx]; + //left_img[idx-counter] = result->response.at(0).image_data_uint8 [idx]; + //right_img[idx-counter] = result->response.at(1).image_data_uint8 [idx]; } //Get SGM disparity and confidence diff --git a/Examples/DataCollection/StereoImageGenerator.hpp b/Examples/DataCollection/StereoImageGenerator.hpp index 414ad70f0b..5a5a43c7e5 100644 --- a/Examples/DataCollection/StereoImageGenerator.hpp +++ b/Examples/DataCollection/StereoImageGenerator.hpp @@ -144,9 +144,9 @@ class StereoImageGenerator { static void processImages(common_utils::ProsumerQueue* results) { while (!results->getIsDone()) { - msr::airlib::ClockBase* clock = msr::airlib::ClockFactory::get(); + //msr::airlib::ClockBase* clock = msr::airlib::ClockFactory::get(); - ImagesResult result; + /*ImagesResult result; if (!results->tryPop(result)) { clock->sleep_for(1); continue; @@ -184,7 +184,7 @@ class StereoImageGenerator { << " ori:" << VectorMath::toString(result.orientation) << " render time " << result.render_time * 1E3f << "ms" << " process time " << clock->elapsedSince(process_time) * 1E3f << " ms" - << std::endl; + << std::endl;*/ } } diff --git a/Examples/DepthNav/DepthNav.hpp b/Examples/DepthNav/DepthNav.hpp index 3733e13abf..44286dc702 100644 --- a/Examples/DepthNav/DepthNav.hpp +++ b/Examples/DepthNav/DepthNav.hpp @@ -157,8 +157,8 @@ class DepthNav { if (response.at(1).height != params_.depth_height || response.at(1).width != params_.depth_height) throw DepthNavException("Image Dimension mismatch. Please check right camera in the AirSim config file."); */ - const std::vector& left_image = response.at(0).image_data_uint8; - const std::vector& right_image = response.at(1).image_data_uint8; + const std::vector left_image(response.at(0).image_data_uint8->begin(), response.at(0).image_data_uint8->end()); + const std::vector right_image(response.at(1).image_data_uint8->begin(), response.at(1).image_data_uint8->end()); //baseline * focal_length = depth * disparity float f = params_.depth_width / (2 * tan(params_.fov/2)); diff --git a/HelloCar/main.cpp b/HelloCar/main.cpp index 31cfea6775..bfdf5031c8 100644 --- a/HelloCar/main.cpp +++ b/HelloCar/main.cpp @@ -44,7 +44,7 @@ int main() std::getline(std::cin, path); for (const ImageResponse& image_info : response) { - std::cout << "Image uint8 size: " << image_info.image_data_uint8.size() << std::endl; + std::cout << "Image uint8 size: " << image_info.image_data_uint8->size() << std::endl; std::cout << "Image float size: " << image_info.image_data_float.size() << std::endl; if (path != "") { @@ -55,7 +55,7 @@ int main() } else { std::ofstream file(file_path + ".png", std::ios::binary); - file.write(reinterpret_cast(image_info.image_data_uint8.data()), image_info.image_data_uint8.size()); + file.write(reinterpret_cast(image_info.image_data_uint8->data()), image_info.image_data_uint8->size()); file.close(); } } diff --git a/HelloDrone/main.cpp b/HelloDrone/main.cpp index 936ec77855..6cbfa811f0 100644 --- a/HelloDrone/main.cpp +++ b/HelloDrone/main.cpp @@ -38,7 +38,7 @@ int main() std::getline(std::cin, path); for (const ImageResponse& image_info : response) { - std::cout << "Image uint8 size: " << image_info.image_data_uint8.size() << std::endl; + std::cout << "Image uint8 size: " << image_info.image_data_uint8->size() << std::endl; std::cout << "Image float size: " << image_info.image_data_float.size() << std::endl; if (path != "") { @@ -49,7 +49,7 @@ int main() } else { std::ofstream file(file_path + ".png", std::ios::binary); - file.write(reinterpret_cast(image_info.image_data_uint8.data()), image_info.image_data_uint8.size()); + file.write(reinterpret_cast(image_info.image_data_uint8->data()), image_info.image_data_uint8->size()); file.close(); } } diff --git a/PythonClient/computer_vision/high_res_camera_bm.py b/PythonClient/computer_vision/high_res_camera_bm.py new file mode 100644 index 0000000000..9564b1ee65 --- /dev/null +++ b/PythonClient/computer_vision/high_res_camera_bm.py @@ -0,0 +1,62 @@ +import setup_path +import airsim +from datetime import datetime + +''' +Example file for benchmarking and observing the affect of a high-resolution camera + +Settings used- +{ + "SettingsVersion": 1.2, + "SimMode": "Multirotor", + "Vehicles" : { + "Drone" : { + "VehicleType" : "SimpleFlight", + "AutoCreate" : true, + "Cameras" : { + "high_res": { + "CaptureSettings" : [ + { + "ImageType" : 0, + "Width" : 4320, + "Height" : 2160 + } + ], + "X": 0.50, "Y": 0.00, "Z": 0.10, + "Pitch": 0.0, "Roll": 0.0, "Yaw": 0.0 + }, + "low_res": { + "CaptureSettings" : [ + { + "ImageType" : 0, + "Width" : 256, + "Height" : 144 + } + ], + "X": 0.50, "Y": 0.00, "Z": 0.10, + "Pitch": 0.0, "Roll": 0.0, "Yaw": 0.0 + } + } + } + } +} +''' + +client = airsim.MultirotorClient() +client.confirmConnection() +framecounter = 1 + +prevtimestamp = datetime.now() + +while(framecounter <= 1500): + if framecounter%50 == 0: + now = datetime.now() + print("Time spent for 50 frames: " + str(now-prevtimestamp)) + prevtimestamp = now + + if framecounter%250 == 0: + client.simGetImages([airsim.ImageRequest("high_res", airsim.ImageType.Scene, False, False)]) + print("High resolution image captured.") + + client.simGetImages([airsim.ImageRequest("low_res", airsim.ImageType.Scene, False, False)]) + framecounter += 1 diff --git a/PythonClient/computer_vision/image_benchmarker.py b/PythonClient/computer_vision/image_benchmarker.py new file mode 100644 index 0000000000..671f6d613b --- /dev/null +++ b/PythonClient/computer_vision/image_benchmarker.py @@ -0,0 +1,105 @@ +from argparse import ArgumentParser +import airsim +import time +import threading +import numpy as np +import cv2 +import tempfile +import os + +cameraTypeMap = { + "depth": airsim.ImageType.DepthVis, + "segmentation": airsim.ImageType.Segmentation, + "seg": airsim.ImageType.Segmentation, + "scene": airsim.ImageType.Scene, + "disparity": airsim.ImageType.DisparityNormalized, + "normals": airsim.ImageType.SurfaceNormals +} + +class ImageBenchmarker(): + def __init__(self, + img_benchmark_type = 'simGetImages', + viz_image_cv2 = False, + img_type = "scene"): + self.airsim_client = airsim.VehicleClient() + self.airsim_client.confirmConnection() + self.image_benchmark_num_images = 0 + self.image_benchmark_total_time = 0.0 + self.image_callback_thread = None + self.viz_image_cv2 = viz_image_cv2 + + self.img_type = cameraTypeMap[img_type] + + if img_benchmark_type == "simGetImage": + self.image_callback_thread = threading.Thread(target=self.repeat_timer_img, args=(self.image_callback_benchmark_simGetImage, 0.001)) + if img_benchmark_type == "simGetImages": + self.image_callback_thread = threading.Thread(target=self.repeat_timer_img, args=(self.image_callback_benchmark_simGetImages, 0.001)) + self.is_image_thread_active = False + self.image_callback_thread.daemon = True + + def start_img_benchmark_thread(self): + if not self.is_image_thread_active: + self.is_image_thread_active = True + self.benchmark_start_time = time.time() + self.image_callback_thread.start() + print("Started img image_callback thread") + + def stop_img_benchmark_thread(self): + if self.is_image_thread_active: + self.is_image_thread_active = False + self.image_callback_thread.join() + print("Stopped image callback thread.") + + def repeat_timer_img(self, task, period): + while self.is_image_thread_active: + task() + time.sleep(period) + + def print_benchmark_results(self): + avg_fps = 1.0 / ((self.image_benchmark_total_time) / float(self.image_benchmark_num_images)) + print("result: {} avg_fps for {} num of images".format(avg_fps, self.image_benchmark_num_images)) + + def image_callback_benchmark_simGetImage(self): + self.image_benchmark_num_images += 1 + image = self.airsim_client.simGetImage("front_center", self.img_type) + np_arr = np.frombuffer(image, dtype=np.uint8) + img = np_arr.reshape(240, 512, 4) + + self.image_benchmark_total_time = time.time() - self.benchmark_start_time + avg_fps = self.image_benchmark_num_images / self.image_benchmark_total_time + print("result: {} avg_fps for {} num of images".format(avg_fps, self.image_benchmark_num_images)) + if self.viz_image_cv2: + cv2.imshow("img", img) + cv2.waitKey(1) + + def image_callback_benchmark_simGetImages(self): + self.image_benchmark_num_images += 1 + request = [airsim.ImageRequest("front_center", self.img_type, False, False)] + response = self.airsim_client.simGetImages(request) + np_arr = np.frombuffer(response[0].image_data_uint8, dtype=np.uint8) + img = np_arr.reshape(response[0].height, response[0].width, -1) + + self.image_benchmark_total_time = time.time() - self.benchmark_start_time + avg_fps = self.image_benchmark_num_images / self.image_benchmark_total_time + print("result + {} avg_fps for {} num of images".format(avg_fps, self.image_benchmark_num_images)) + if self.viz_image_cv2: + cv2.imshow("img", img) + cv2.waitKey(1) + +def main(args): + image_benchmarker = ImageBenchmarker(img_benchmark_type=args.img_benchmark_type, viz_image_cv2=args.viz_image_cv2, + img_type=args.img_type) + + image_benchmarker.start_img_benchmark_thread() + time.sleep(30) + image_benchmarker.stop_img_benchmark_thread() + image_benchmarker.print_benchmark_results() + +if __name__ == "__main__": + parser = ArgumentParser() + parser.add_argument('--img_benchmark_type', type=str, choices=["simGetImage", "simGetImages"], default="simGetImages") + parser.add_argument('--enable_viz_image_cv2', dest='viz_image_cv2', action='store_true', default=False) + parser.add_argument('--img_type', type=str, choices=cameraTypeMap.keys(), default="scene") + + args = parser.parse_args() + main(args) \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp b/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp index 0f3ab3946b..69d663149b 100644 --- a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp @@ -186,25 +186,15 @@ APawn* PawnSimApi::getPawn() return params_.pawn; } -std::vector PawnSimApi::getImages( - const std::vector& requests) const +void PawnSimApi::getImages(const std::vector& requests, std::vector &responses) const { - std::vector responses; - - const ImageCaptureBase* camera = getImageCapture(); - camera->getImages(requests, responses); - - return responses; + for (int i = 0; i < requests.size() && i < responses.size(); ++i) + getImage(requests[i], responses[i]); } -std::vector PawnSimApi::getImage(const std::string& camera_name, ImageCaptureBase::ImageType image_type) const +void PawnSimApi::getImage(const ImageCaptureBase::ImageRequest& request, ImageCaptureBase::ImageResponse &response) const { - std::vector request = { ImageCaptureBase::ImageRequest(camera_name, image_type) }; - const std::vector& response = getImages(request); - if (response.size() > 0) - return response.at(0).image_data_uint8; - else - return std::vector(); + getImageCapture()->getImage(request, response); } void PawnSimApi::setRCForceFeedback(float rumble_strength, float auto_center) diff --git a/Unreal/Plugins/AirSim/Source/PawnSimApi.h b/Unreal/Plugins/AirSim/Source/PawnSimApi.h index c67922abfd..55c16d5243 100644 --- a/Unreal/Plugins/AirSim/Source/PawnSimApi.h +++ b/Unreal/Plugins/AirSim/Source/PawnSimApi.h @@ -72,8 +72,8 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase { virtual void update() override; virtual const UnrealImageCapture* getImageCapture() const override; - virtual std::vector getImages(const std::vector& request) const override; - virtual std::vector getImage(const std::string& camera_name, ImageCaptureBase::ImageType image_type) const override; + virtual void getImages(const std::vector& request, std::vector &responses) const override; + virtual void getImage(const ImageCaptureBase::ImageRequest& request, ImageCaptureBase::ImageResponse &response) const override; virtual Pose getPose() const override; virtual void setPose(const Pose& pose, bool ignore_collision) override; virtual msr::airlib::CameraInfo getCameraInfo(const std::string& camera_name) const override; diff --git a/Unreal/Plugins/AirSim/Source/Recording/RecordingFile.cpp b/Unreal/Plugins/AirSim/Source/Recording/RecordingFile.cpp index c9d3d61f91..774e0049af 100644 --- a/Unreal/Plugins/AirSim/Source/Recording/RecordingFile.cpp +++ b/Unreal/Plugins/AirSim/Source/Recording/RecordingFile.cpp @@ -34,7 +34,7 @@ void RecordingFile::appendRecord(const std::vector(response.image_data_uint8.data()), response.image_data_uint8.size()); + file.write(reinterpret_cast(response.image_data_uint8->data()), response.image_data_uint8->size()); file.close(); } diff --git a/Unreal/Plugins/AirSim/Source/RenderRequest.cpp b/Unreal/Plugins/AirSim/Source/RenderRequest.cpp index 57f423272d..373610310d 100644 --- a/Unreal/Plugins/AirSim/Source/RenderRequest.cpp +++ b/Unreal/Plugins/AirSim/Source/RenderRequest.cpp @@ -3,181 +3,57 @@ #include "Engine/TextureRenderTarget2D.h" #include "Async/TaskGraphInterfaces.h" #include "ImageUtils.h" - +#include "Containers/UnrealString.h" +#include +#include +#include "common/common_utils/BufferPool.h" #include "AirBlueprintLib.h" #include "Async/Async.h" -RenderRequest::RenderRequest(UGameViewportClient * game_viewport, std::function&& query_camera_pose_cb) - : params_(nullptr), results_(nullptr), req_size_(0), - wait_signal_(new msr::airlib::WorkerThreadSignal), - game_viewport_(game_viewport), query_camera_pose_cb_(std::move(query_camera_pose_cb)) -{ -} +RenderRequest::RenderRequest(BufferPool *buffer_pool) : buffer_pool_(buffer_pool) +{} RenderRequest::~RenderRequest() -{ -} +{} -// read pixels from render target using render thread, then compress the result into PNG -// argument on the thread that calls this method. -void RenderRequest::getScreenshot(std::shared_ptr params[], std::vector>& results, unsigned int req_size, bool use_safe_method) +void RenderRequest::FastScreenshot() { - //TODO: is below really needed? - for (unsigned int i = 0; i < req_size; ++i) { - results.push_back(std::make_shared()); - - if (!params[i]->pixels_as_float) - results[i]->bmp.Reset(); - else - results[i]->bmp_float.Reset(); - results[i]->time_stamp = 0; - } - - //make sure we are not on the rendering thread - CheckNotBlockedOnRenderThread(); - - if (use_safe_method) { - for (unsigned int i = 0; i < req_size; ++i) { - //TODO: below doesn't work right now because it must be running in game thread - FIntPoint img_size; - if (!params[i]->pixels_as_float) { - //below is documented method but more expensive because it forces flush - FTextureRenderTargetResource* rt_resource = params[i]->render_target->GameThread_GetRenderTargetResource(); - auto flags = setupRenderResource(rt_resource, params[i].get(), results[i].get(), img_size); - rt_resource->ReadPixels(results[i]->bmp, flags); - } - else { - FTextureRenderTargetResource* rt_resource = params[i]->render_target->GetRenderTargetResource(); - setupRenderResource(rt_resource, params[i].get(), results[i].get(), img_size); - rt_resource->ReadFloat16Pixels(results[i]->bmp_float); - } - } - } - else { - //wait for render thread to pick up our task - params_ = params; - results_ = results.data(); - req_size_ = req_size; - - // Queue up the task of querying camera pose in the game thread and synchronizing render thread with camera pose - AsyncTask(ENamedThreads::GameThread, [this]() { - check(IsInGameThread()); - - saved_DisableWorldRendering_ = game_viewport_->bDisableWorldRendering; - game_viewport_->bDisableWorldRendering = 0; - end_draw_handle_ = game_viewport_->OnEndDraw().AddLambda([this] { - check(IsInGameThread()); - - // capture CameraPose for this frame - query_camera_pose_cb_(); - - // The completion is called immeidately after GameThread sends the - // rendering commands to RenderThread. Hence, our ExecuteTask will - // execute *immediately* after RenderThread renders the scene! - RenderRequest* This = this; - ENQUEUE_RENDER_COMMAND(SceneDrawCompletion)( - [This](FRHICommandListImmediate& RHICmdList) - { - This->ExecuteTask(); - }); - - game_viewport_->bDisableWorldRendering = saved_DisableWorldRendering_; - - assert(end_draw_handle_.IsValid()); - game_viewport_->OnEndDraw().Remove(end_draw_handle_); - }); - - // while we're still on GameThread, enqueue request for capture the scene! - for (unsigned int i = 0; i < req_size_; ++i) { - params_[i]->render_component->CaptureSceneDeferred(); - } + UAirBlueprintLib::RunCommandOnGameThread([this]() { + fast_cap_done_ = false; + fast_rt_resource_ = fast_param_.render_component->TextureTarget->GameThread_GetRenderTargetResource(); + fast_param_.render_component->CaptureScene(); + ENQUEUE_RENDER_COMMAND(SceneDrawCompletion)([this](FRHICommandListImmediate& RHICmdList) + { + this->RenderThreadScreenshotTask(this->latest_result_); }); + }, false); - // wait for this task to complete - while (!wait_signal_->waitFor(5)) { - // log a message and continue wait - // lamda function still references a few objects for which there is no refcount. - // Walking away will cause memory corruption, which is much more difficult to debug. - UE_LOG(LogTemp, Warning, TEXT("Failed: timeout waiting for screenshot")); - } - } - - for (unsigned int i = 0; i < req_size; ++i) { - if (!params[i]->pixels_as_float) { - if (results[i]->width != 0 && results[i]->height != 0) { - results[i]->image_data_uint8.SetNumUninitialized(results[i]->width * results[i]->height * 3, false); - if (params[i]->compress) - UAirBlueprintLib::CompressImageArray(results[i]->width, results[i]->height, results[i]->bmp, results[i]->image_data_uint8); - else { - uint8* ptr = results[i]->image_data_uint8.GetData(); - for (const auto& item : results[i]->bmp) { - *ptr++ = item.B; - *ptr++ = item.G; - *ptr++ = item.R; - } - } - } - } - else { - results[i]->image_data_float.SetNumUninitialized(results[i]->width * results[i]->height); - float* ptr = results[i]->image_data_float.GetData(); - for (const auto& item : results[i]->bmp_float) { - *ptr++ = item.R.GetFloat(); - } - } - } -} - -FReadSurfaceDataFlags RenderRequest::setupRenderResource(const FTextureRenderTargetResource* rt_resource, const RenderParams* params, RenderResult* result, FIntPoint& size) -{ - size = rt_resource->GetSizeXY(); - result->width = size.X; - result->height = size.Y; - FReadSurfaceDataFlags flags(RCM_UNorm, CubeFace_MAX); - flags.SetLinearToGamma(false); - - return flags; + //Try to wait just long enough for the render thread to finish the capture. + //Start with a long wait, then check for completion more frequently. + //TODO: Optimize these numbers. + for (int best_guess_cap_time_microseconds = 500; !fast_cap_done_; best_guess_cap_time_microseconds = 200) + std::this_thread::sleep_for(std::chrono::microseconds(best_guess_cap_time_microseconds)); } -void RenderRequest::ExecuteTask() +void RenderRequest::RenderThreadScreenshotTask(RenderRequest::RenderResult &result) { - if (params_ != nullptr && req_size_ > 0) - { - for (unsigned int i = 0; i < req_size_; ++i) { - FRHICommandListImmediate& RHICmdList = GetImmediateCommandList_ForRenderCommand(); - auto rt_resource = params_[i]->render_target->GetRenderTargetResource(); - if (rt_resource != nullptr) { - const FTexture2DRHIRef& rhi_texture = rt_resource->GetRenderTargetTexture(); - FIntPoint size; - auto flags = setupRenderResource(rt_resource, params_[i].get(), results_[i].get(), size); - - //should we be using ENQUEUE_UNIQUE_RENDER_COMMAND_ONEPARAMETER which was in original commit by @saihv - //https://github.com/Microsoft/AirSim/pull/162/commits/63e80c43812300a8570b04ed42714a3f6949e63f#diff-56b790f9394f7ca1949ddbb320d8456fR64 - if (!params_[i]->pixels_as_float) { - //below is undocumented method that avoids flushing, but it seems to segfault every 2000 or so calls - RHICmdList.ReadSurfaceData( - rhi_texture, - FIntRect(0, 0, size.X, size.Y), - results_[i]->bmp, - flags); - } - else { - RHICmdList.ReadSurfaceFloatData( - rhi_texture, - FIntRect(0, 0, size.X, size.Y), - results_[i]->bmp_float, - CubeFace_PosX, 0, 0 - ); - } - } - - results_[i]->time_stamp = msr::airlib::ClockFactory::get()->nowNanos(); - } - - req_size_ = 0; - params_ = nullptr; - results_ = nullptr; - - wait_signal_->signal(); - } + FRHITexture2D *fast_cap_texture = fast_rt_resource_->TextureRHI->GetTexture2D(); + EPixelFormat pixelFormat = fast_cap_texture->GetFormat(); + uint32 width = fast_cap_texture->GetSizeX(); + uint32 height = fast_cap_texture->GetSizeY(); + uint32 stride; + auto *src = (const unsigned char*)RHILockTexture2D(fast_cap_texture, 0, RLM_ReadOnly, stride, false); // needs to be on render thread + + result.time_stamp = msr::airlib::ClockFactory::get()->nowNanos(); + result.pixels = buffer_pool_->GetBufferExactSize(height*stride); + result.stride = stride; + result.width = width; + result.height = height; + + if (src) + FMemory::BigBlockMemcpy(latest_result_.pixels->data(), src, height * stride); + + RHIUnlockTexture2D(fast_cap_texture, 0, false); + + fast_cap_done_ = true; } diff --git a/Unreal/Plugins/AirSim/Source/RenderRequest.h b/Unreal/Plugins/AirSim/Source/RenderRequest.h index 3eb7fa97d3..957e36bed6 100644 --- a/Unreal/Plugins/AirSim/Source/RenderRequest.h +++ b/Unreal/Plugins/AirSim/Source/RenderRequest.h @@ -8,66 +8,60 @@ #include #include "common/Common.hpp" +class BufferPool; class RenderRequest : public FRenderCommand { public: - struct RenderParams { - USceneCaptureComponent2D * const render_component; + struct RenderParams + { + USceneCaptureComponent2D *render_component; UTextureRenderTarget2D* render_target; bool pixels_as_float; bool compress; - - RenderParams(USceneCaptureComponent2D * render_component_val, UTextureRenderTarget2D* render_target_val, bool pixels_as_float_val, bool compress_val) - : render_component(render_component_val), render_target(render_target_val), pixels_as_float(pixels_as_float_val), compress(compress_val) - { - } }; - struct RenderResult { - TArray image_data_uint8; - TArray image_data_float; - TArray bmp; - TArray bmp_float; + struct RenderResult { + RenderResult() = default; + // RenderResult(RenderResult&) = default; + RenderResult(RenderResult&&) = default; + RenderResult &operator=(RenderResult &&) = default; int width; int height; - + int stride; msr::airlib::TTimePoint time_stamp; + std::unique_ptr, std::function*)>> pixels = nullptr; }; private: - static FReadSurfaceDataFlags setupRenderResource(const FTextureRenderTargetResource* rt_resource, const RenderParams* params, RenderResult* result, FIntPoint& size); - std::shared_ptr* params_; std::shared_ptr* results_; - unsigned int req_size_; +public: + RenderParams fast_param_{ nullptr, nullptr, false, false }; + RenderResult latest_result_{}; + +private: + volatile bool fast_cap_done_ = false; + FTextureRenderTargetResource* fast_rt_resource_; std::shared_ptr wait_signal_; bool saved_DisableWorldRendering_ = false; - UGameViewportClient * const game_viewport_; + //UGameViewportClient * const game_viewport_; FDelegateHandle end_draw_handle_; std::function query_camera_pose_cb_; + BufferPool* buffer_pool_ = nullptr; public: - RenderRequest(UGameViewportClient * game_viewport, std::function&& query_camera_pose_cb); + RenderRequest(BufferPool *buffer_pool); ~RenderRequest(); - void DoTask(ENamedThreads::Type CurrentThread, const FGraphEventRef& MyCompletionGraphEvent) - { - ExecuteTask(); - } - FORCEINLINE TStatId GetStatId() const { RETURN_QUICK_DECLARE_CYCLE_STAT(RenderRequest, STATGROUP_RenderThreadCommands); } - // read pixels from render target using render thread, then compress the result into PNG - // argument on the thread that calls this method. - void getScreenshot( - std::shared_ptr params[], std::vector>& results, unsigned int req_size, bool use_safe_method); - - void ExecuteTask(); + void FastScreenshot(); + void RenderThreadScreenshotTask(RenderResult &result); }; diff --git a/Unreal/Plugins/AirSim/Source/UnrealImageCapture.cpp b/Unreal/Plugins/AirSim/Source/UnrealImageCapture.cpp index da28a1795f..b08fb448b4 100644 --- a/Unreal/Plugins/AirSim/Source/UnrealImageCapture.cpp +++ b/Unreal/Plugins/AirSim/Source/UnrealImageCapture.cpp @@ -1,7 +1,6 @@ #include "UnrealImageCapture.h" #include "Engine/World.h" #include "ImageUtils.h" - #include "RenderRequest.h" #include "common/ClockFactory.hpp" @@ -15,117 +14,46 @@ UnrealImageCapture::UnrealImageCapture(const common_utils::UniqueValueMap& requests, - std::vector& responses) const +void UnrealImageCapture::getImages(const std::vector& requests, std::vector& responses) const { - if (cameras_->valsSize() == 0) { - for (unsigned int i = 0; i < requests.size(); ++i) { - responses.push_back(ImageResponse()); - responses[responses.size() - 1].message = "camera is not set"; - } - } - else - getSceneCaptureImage(requests, responses, false); + for (int i = 0; i < requests.size() && i < responses.size(); ++i) + getImage(requests[i], responses[i]); } - -void UnrealImageCapture::getSceneCaptureImage(const std::vector& requests, - std::vector& responses, bool use_safe_method) const +void UnrealImageCapture::getImage(const msr::airlib::ImageCaptureBase::ImageRequest& request, msr::airlib::ImageCaptureBase::ImageResponse& response) const { - std::vector> render_params; - std::vector> render_results; - - bool visibilityChanged = false; - for (unsigned int i = 0; i < requests.size(); ++i) { - APIPCamera* camera = cameras_->at(requests.at(i).camera_name); - //TODO: may be we should have these methods non-const? - visibilityChanged = const_cast(this)->updateCameraVisibility(camera, requests[i]) || visibilityChanged; - } - - if (use_safe_method && visibilityChanged) { - // We don't do game/render thread synchronization for safe method. - // We just blindly sleep for 200ms (the old way) - std::this_thread::sleep_for(std::chrono::duration(0.2)); - } - - UGameViewportClient * gameViewport = nullptr; - for (unsigned int i = 0; i < requests.size(); ++i) { - APIPCamera* camera = cameras_->at(requests.at(i).camera_name); - if (gameViewport == nullptr) { - gameViewport = camera->GetWorld()->GetGameViewport(); - } - - responses.push_back(ImageResponse()); - ImageResponse& response = responses.at(i); - - UTextureRenderTarget2D* textureTarget = nullptr; - USceneCaptureComponent2D* capture = camera->getCaptureComponent(requests[i].image_type, false); - if (capture == nullptr) { - response.message = "Can't take screenshot because none camera type is not active"; - } - else if (capture->TextureTarget == nullptr) { - response.message = "Can't take screenshot because texture target is null"; - } - else - textureTarget = capture->TextureTarget; - - render_params.push_back(std::make_shared(capture, textureTarget, requests[i].pixels_as_float, requests[i].compress)); - } - - if (nullptr == gameViewport) { - return; - } - - auto query_camera_pose_cb = [this, &requests, &responses]() { - size_t count = requests.size(); - for (size_t i = 0; i < count; i++) { - const ImageRequest& request = requests.at(i); - APIPCamera* camera = cameras_->at(request.camera_name); - ImageResponse& response = responses.at(i); - auto camera_pose = camera->getPose(); - response.camera_position = camera_pose.position; - response.camera_orientation = camera_pose.orientation; - } - }; - RenderRequest render_request { gameViewport, std::move(query_camera_pose_cb) }; - - render_request.getScreenshot(render_params.data(), render_results, render_params.size(), use_safe_method); - - for (unsigned int i = 0; i < requests.size(); ++i) { - const ImageRequest& request = requests.at(i); - ImageResponse& response = responses.at(i); - - response.camera_name = request.camera_name; - response.time_stamp = render_results[i]->time_stamp; - response.image_data_uint8 = std::vector(render_results[i]->image_data_uint8.GetData(), render_results[i]->image_data_uint8.GetData() + render_results[i]->image_data_uint8.Num()); - response.image_data_float = std::vector(render_results[i]->image_data_float.GetData(), render_results[i]->image_data_float.GetData() + render_results[i]->image_data_float.Num()); - - if (use_safe_method) { - // Currently, we don't have a way to synthronize image capturing and camera pose when safe method is used, - APIPCamera* camera = cameras_->at(request.camera_name); - msr::airlib::Pose pose = camera->getPose(); - response.camera_position = pose.position; - response.camera_orientation = pose.orientation; - } - response.pixels_as_float = request.pixels_as_float; - response.compress = request.compress; - response.width = render_results[i]->width; - response.height = render_results[i]->height; - response.image_type = request.image_type; - } - + getSceneCaptureImage(request.camera_name, request.image_type, response); } - -bool UnrealImageCapture::updateCameraVisibility(APIPCamera* camera, const msr::airlib::ImageCaptureBase::ImageRequest& request) +void UnrealImageCapture::getSceneCaptureImage(const std::string& camera_name, msr::airlib::ImageCaptureBase::ImageType image_type, msr::airlib::ImageCaptureBase::ImageResponse& response) const { - bool visibilityChanged = false; - if (! camera->getCameraTypeEnabled(request.image_type)) { - camera->setCameraTypeEnabled(request.image_type, true); - visibilityChanged = true; - } - - return visibilityChanged; + APIPCamera* camera = cameras_->at(camera_name); + camera->setCameraTypeEnabled(image_type, true); + + USceneCaptureComponent2D* capture = camera->getCaptureComponent(image_type, false); + UTextureRenderTarget2D* textureTarget = capture->TextureTarget; + + RenderRequest render_request(BufferPool_); + render_request.fast_param_ = RenderRequest::RenderParams{ capture, textureTarget, false, false }; + render_request.FastScreenshot(); + + int height = capture->TextureTarget->SizeY; + int width = capture->TextureTarget->SizeX; + int stride = render_request.latest_result_.stride; + int bytes = render_request.latest_result_.pixels->size(); + int bytes_per_pixel = bytes / (height * width); + int padded_width = stride / bytes_per_pixel; + + response.camera_name = camera_name; + response.time_stamp = render_request.latest_result_.time_stamp; + response.width = padded_width; + response.height = height; + response.image_type = image_type; + response.image_data_uint8 = std::move(render_request.latest_result_.pixels); + + // Disable camera after capturing image, this reduces resource consumption when images are not being taken + // Particulary when a high-resolution camera is used occasionally + camera->setCameraTypeEnabled(image_type, false); } bool UnrealImageCapture::getScreenshotScreen(ImageType image_type, std::vector& compressedPng) @@ -146,7 +74,7 @@ void UnrealImageCapture::addScreenCaptureHandler(UWorld *world) { // Make sure that all alpha values are opaque. TArray& RefBitmap = const_cast&>(Bitmap); - for(auto& Color : RefBitmap) + for (auto& Color : RefBitmap) Color.A = 255; TArray last_compressed_png; diff --git a/Unreal/Plugins/AirSim/Source/UnrealImageCapture.h b/Unreal/Plugins/AirSim/Source/UnrealImageCapture.h index f160375bcb..b03c20b78f 100644 --- a/Unreal/Plugins/AirSim/Source/UnrealImageCapture.h +++ b/Unreal/Plugins/AirSim/Source/UnrealImageCapture.h @@ -4,7 +4,7 @@ #include "PIPCamera.h" #include "common/ImageCaptureBase.hpp" #include "common/common_utils/UniqueValueMap.hpp" - +#include "common/common_utils/BufferPool.h" class UnrealImageCapture : public msr::airlib::ImageCaptureBase { @@ -14,17 +14,17 @@ class UnrealImageCapture : public msr::airlib::ImageCaptureBase UnrealImageCapture(const common_utils::UniqueValueMap* cameras); virtual ~UnrealImageCapture(); - virtual void getImages(const std::vector& requests, std::vector& responses) const override; + virtual void getImages(const std::vector& requests, std::vector& responses) const override; + virtual void getImage(const ImageRequest& request, ImageResponse& response) const override; private: - void getSceneCaptureImage(const std::vector& requests, - std::vector& responses, bool use_safe_method) const; + BufferPool *BufferPool_ = new BufferPool(); + + void getSceneCaptureImage(const std::string& camera_name, ImageCaptureBase::ImageType image_type, ImageResponse &response) const; void addScreenCaptureHandler(UWorld *world); bool getScreenshotScreen(ImageType image_type, std::vector& compressedPng); - bool updateCameraVisibility(APIPCamera* camera, const msr::airlib::ImageCaptureBase::ImageRequest& request); - private: const common_utils::UniqueValueMap* cameras_; std::vector last_compressed_png_; diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index c9780e63a0..2c5b9c1f1b 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -1,6 +1,5 @@ #include "WorldSimApi.h" #include "AirBlueprintLib.h" -#include "TextureShuffleActor.h" #include "common/common_utils/Utils.hpp" #include "Weather/WeatherLib.h" #include "DrawDebugHelpers.h"