recording fix

AirLib/include/common/common_utils/FileSystem.hpp

@@ -30,7 +30,7 @@ class FileSystem
         '/';
 #endif
 
-    static std::string createDirectory(std::string fullPath);
+    static std::string createDirectory(const std::string& fullPath);
 
     static std::string getUserHomeFolder()
     {
@@ -50,14 +50,22 @@ class FileSystem
         return ensureFolder(combine(getUserDocumentsFolder(), ProductFolderName));
     }
 
-    static std::string ensureFolder(std::string fullpath) {
+    static std::string ensureFolder(const std::string& fullpath) {
         // make sure this directory exists.
         return createDirectory(fullpath);
     }
 
-    static std::string combine(const std::string parentFolder, const std::string child) {
+    static std::string ensureFolder(const std::string& parentFolder, const std::string& child) {
+        // make sure this directory exists.
+        return createDirectory(combine(parentFolder, child));
+    }
+
+    static std::string combine(const std::string& parentFolder, const std::string& child) {
+        if (child.size() == 0)
+            return parentFolder;
+
         size_t len = parentFolder.size();
-        if (len > 0 && parentFolder[len - 1] == kPathSeparator) {
+        if (parentFolder.size() > 0 && parentFolder[len - 1] == kPathSeparator) {
             // parent already ends with '/'
             return parentFolder + child;
         } 
@@ -78,7 +86,7 @@ class FileSystem
     }
 
 
-    static std::string getFileExtension(const std::string str)
+    static std::string getFileExtension(const std::string& str)
     {
         // bugbug: this is not unicode safe.
         int len = static_cast<int>(str.size());
@@ -94,12 +102,18 @@ class FileSystem
         return str.substr(ui, len - ui);
     }
 
-    static std::string getLogFileNamePath(std::string prefix, std::string suffix, std::string extension, bool add_timestamp)
+    static std::string getLogFolderPath(bool folder_timestamp)
     {
-        std::string logfolder = Utils::to_string(Utils::now(), "%Y-%m-%d");
+        std::string logfolder = folder_timestamp ? Utils::to_string(Utils::now()) : "";
         std::string fullPath = combine(getAppDataFolder(), logfolder);
-        std::string timestamp = add_timestamp ? Utils::to_string(Utils::now()) : "";
+        ensureFolder(fullPath);
 
+        return fullPath;
+    }
+
+    static std::string getLogFileNamePath(const std::string& fullPath, const std::string& prefix, const std::string& suffix, const std::string& extension, 
+        bool file_timestamp)
+    {
         //TODO: because this bug we are using alternative code with stringstream
         //https://answers.unrealengine.com/questions/664905/unreal-crashes-on-two-lines-of-extremely-simple-st.html
 
@@ -108,7 +122,7 @@ class FileSystem
             .push_back(kPathSeparator);
         filename.append(prefix)
             .append(suffix)
-            .append(timestamp)
+            .append(file_timestamp ? Utils::to_string(Utils::now()) : "")
             .append(extension);
 
         return filename;
@@ -118,7 +132,7 @@ class FileSystem
         //return filename_ss.str();
     }
 
-    static void openTextFile(std::string filepath, std::ifstream& file){
+    static void openTextFile(const std::string& filepath, std::ifstream& file){
 
 #ifdef _WIN32
         // WIN32 will create the wrong file names if we don't first convert them to UTF-16.
@@ -130,7 +144,7 @@ class FileSystem
 #endif
     }
 
-    static void createBinaryFile(std::string filepath, std::ofstream& file){
+    static void createBinaryFile(const std::string& filepath, std::ofstream& file){
 
 #ifdef _WIN32
         // WIN32 will create the wrong file names if we don't first convert them to UTF-16.
@@ -142,7 +156,7 @@ class FileSystem
 #endif
     }
 
-    static void createTextFile(std::string filepath, std::ofstream& file){
+    static void createTextFile(const std::string& filepath, std::ofstream& file){
 
 #ifdef _WIN32
         // WIN32 will create the wrong file names if we don't first convert them to UTF-16.
@@ -157,9 +171,10 @@ class FileSystem
             throw std::ios_base::failure(std::strerror(errno));
     }
 
-    static std::string createLogFile(std::string suffix, std::ofstream& flog)
+    static std::string createLogFile(const std::string& suffix, std::ofstream& flog)
     {
-        std::string filepath = getLogFileNamePath("log_", suffix, ".tsv", true);
+        std::string log_folderpath = common_utils::FileSystem::getLogFolderPath(false);
+        std::string filepath = getLogFileNamePath(log_folderpath, "log_", suffix, ".tsv", true);
         createTextFile(filepath, flog);
 
         Utils::log(Utils::stringf("log file started: %s", filepath.c_str()));
@@ -180,7 +195,7 @@ class FileSystem
         return line;
     }    
 
-    static void appendLineToFile(std::string filepath, std::string line)
+    static void appendLineToFile(const std::string& filepath, const std::string& line)
     {
         std::ofstream file;
 #ifdef _WIN32

AirLib/include/vehicles/multirotor/controllers/DroneControllerBase.hpp

@@ -352,7 +352,7 @@ class DroneControllerBase : public VehicleControllerBase {
 private:// vars
     shared_ptr<SafetyEval> safety_eval_ptr_;
     float obs_avoidance_vel_ = 0.5f;
-    bool log_to_file_ = false;
+
     CollisionInfo collision_info_;
     vector<VehicleCameraBase*> cameras_;
 

AirLib/src/common/common_utils/FileSystem.cpp

@@ -36,7 +36,7 @@ using namespace common_utils;
 
 // File names are unicode (std::wstring), because users can create folders containing unicode characters on both
 // Windows, OSX and Linux.
-std::string FileSystem::createDirectory(std::string fullPath) {
+std::string FileSystem::createDirectory(const std::string& fullPath) {
 
 #ifdef _WIN32
     std::wstring_convert<std::codecvt_utf8<wchar_t>, wchar_t> converter;

AirLib/src/controllers/DroneControllerBase.cpp

@@ -120,12 +120,6 @@ bool DroneControllerBase::moveOnPath(const vector<Vector3r>& path, float velocit
     vector<PathSegment> path_segs;
     path3d.push_back(getPosition());
 
-    std::ofstream flog;
-    if (log_to_file_) {
-        common_utils::FileSystem::createLogFile("MoveToPosition", flog);
-        flog << "seg_index\toffset\tx\ty\tz\tgoal_dist\tseg_index\toffset\tx\ty\tz\tlookahead\tlookahead_error\tseg_index\toffset\tx\ty\tz";
-    }
-
     Vector3r point;
     float path_length = 0;
     //append the input path and compute segments
@@ -251,9 +245,6 @@ bool DroneControllerBase::moveOnPath(const vector<Vector3r>& path, float velocit
         //     VectorMath::toString(getPosition()).c_str(), goal_dist, VectorMath::toString(cur_path_loc.position).c_str(),
         //     VectorMath::toString(next_path_loc.position).c_str(), lookahead_error);
 
-        if (log_to_file_)
-            flog << cur_path_loc.seg_index << "\t" << cur_path_loc.offset << "\t" << cur_path_loc.position.x() << "\t" << cur_path_loc.position.y() << "\t" << cur_path_loc.position.z() << "\t" << goal_dist << "\t";
-
         //if drone moved backward, we don't want goal to move backward as well
         //so only climb forward on the path, never back. Also note >= which means
         //we climb path even if distance was 0 to take care of duplicated points on path
@@ -267,11 +258,6 @@ bool DroneControllerBase::moveOnPath(const vector<Vector3r>& path, float velocit
 
         //compute next target on path
         overshoot = setNextPathPosition(path3d, path_segs, cur_path_loc, lookahead + lookahead_error, next_path_loc);
-
-        if (log_to_file_) {
-            flog << cur_path_loc.seg_index << "\t" << cur_path_loc.offset << "\t" << cur_path_loc.position.x() << "\t" << cur_path_loc.position.y() << "\t" << cur_path_loc.position.z() << "\t" << lookahead  << "\t" << lookahead_error  << "\t";
-            flog << next_path_loc.seg_index << "\t" << next_path_loc.offset << "\t" << next_path_loc.position.x() << "\t" << next_path_loc.position.y() << "\t" << next_path_loc.position.z() << std::endl;
-        }
     }
 
     return true;

Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp

@@ -306,6 +306,7 @@ void ACarPawn::initializeForBeginPlay(bool enable_rpc, const std::string& api_se
 
     std::vector<APIPCamera*> cameras = { InternalCamera1, InternalCamera2, InternalCamera3, InternalCamera4, InternalCamera5 };
     wrapper_->initialize(this, cameras);
+    wrapper_->setKinematics(&kinematics_);
 
     startApiServer(enable_rpc, api_server_address);
 }

Unreal/Plugins/AirSim/Source/Car/CarPawn.h

@@ -6,6 +6,7 @@
 #include "VehiclePawnWrapper.h"
 #include "WheeledVehicle.h"
 #include "vehicles/car/api/CarRpcLibServer.hpp"
+#include "Physics/Kinematics.hpp"
 #include "CarPawn.generated.h"
 
 class UPhysicalMaterial;
@@ -158,6 +159,6 @@ class ACarPawn : public AWheeledVehicle
     std::unique_ptr<msr::airlib::CarRpcLibServer> rpclib_server_;
     std::unique_ptr<msr::airlib::CarApiBase> controller_;
     std::unique_ptr<VehiclePawnWrapper> wrapper_;
-
+    msr::airlib::Kinematics::State kinematics_;
     bool api_control_enabled_ = false;
 };

Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp

@@ -126,7 +126,7 @@ void ASimModeWorldMultiRotor::setupVehiclesAndCamera(std::vector<VehiclePtr>& ve
                 fpv_vehicle_pawn_wrapper_ = wrapper;
 
             //now create the connector for each pawn
-            auto vehicle = createVehicle(wrapper);
+            VehiclePtr vehicle = createVehicle(wrapper);
             if (vehicle != nullptr) {
                 vehicles.push_back(vehicle);
 
@@ -160,9 +160,13 @@ ASimModeWorldBase::VehiclePtr ASimModeWorldMultiRotor::createVehicle(VehiclePawn
 
     vehicle_params_.push_back(std::move(vehicle_params));
 
-    auto vehicle = std::make_shared<MultiRotorConnector>(
+    std::shared_ptr<MultiRotorConnector> vehicle = std::make_shared<MultiRotorConnector>(
         wrapper, vehicle_params_.back().get(), enable_rpc, api_server_address, 
         vehicle_params_.back()->getParams().api_server_port, manual_pose_controller);
+
+    if (vehicle->getPhysicsBody() != nullptr)
+        wrapper->setKinematics(& (static_cast<PhysicsBody*>(vehicle->getPhysicsBody())->getKinematics()));
+
     return std::static_pointer_cast<VehicleConnectorBase>(vehicle);
 }
 

Unreal/Plugins/AirSim/Source/Recording/RecordingFile.cpp

@@ -7,25 +7,24 @@
 #include "common/common_utils/FileSystem.hpp"
 
 
-void RecordingFile::appendRecord(TArray<uint8>& image_data, const msr::airlib::Kinematics* kinematics)
+void RecordingFile::appendRecord(TArray<uint8>& image_data, const msr::airlib::Kinematics::State* kinematics)
 {
     if (image_data.Num() == 0)
         return;
 
     bool imageSavedOk = false;
     FString filePath;
     try {    
-        FString image_path = FString(common_utils::FileSystem::getLogFileNamePath("img_", "", "", false).c_str());
-        filePath = image_path + FString::FromInt(images_saved_) + ".png";
-        imageSavedOk = FFileHelper::SaveArrayToFile(image_data, *filePath);
+        FString image_file_path = FString(common_utils::FileSystem::combine(image_path_, "img_").c_str()) + FString::FromInt(images_saved_) + ".png";
+        imageSavedOk = FFileHelper::SaveArrayToFile(image_data, *image_file_path);
     }
     catch(std::exception& ex) {
         UAirBlueprintLib::LogMessage(TEXT("Image file save failed"), FString(ex.what()), LogDebugLevel::Failure);        
     }
     // If render command is complete, save image along with position and orientation
 
     if (imageSavedOk) {
-        writeString(getLine(kinematics->getState()));
+        writeString(getLine(*kinematics));
 
         UAirBlueprintLib::LogMessage(TEXT("Screenshot saved to:"), filePath, LogDebugLevel::Success);
         images_saved_++;
@@ -106,23 +105,14 @@ RecordingFile::~RecordingFile()
     closeFile();
 }
 
-std::string RecordingFile::getLogFileFullPath()
-{
-    try {
-        return common_utils::FileSystem::getLogFileNamePath(record_filename, "", ".txt", true);
-    }
-    catch(std::exception& ex) {
-        UAirBlueprintLib::LogMessageString(std::string("getLogFileFullPath failed: "), ex.what(), LogDebugLevel::Failure); 
-        return "";  
-    }
-}
-
 void RecordingFile::startRecording()
 {
     try {
-        std::string fullPath = getLogFileFullPath();
-        if (fullPath != "")
-            createFile(fullPath);
+        std::string log_folderpath = common_utils::FileSystem::getLogFolderPath(true);
+        image_path_ = common_utils::FileSystem::ensureFolder(log_folderpath, "images");
+        std::string log_filepath = common_utils::FileSystem::getLogFileNamePath(log_folderpath, record_filename, "", ".txt", false);
+        if (log_filepath != "")
+            createFile(log_filepath);
         else {
             UAirBlueprintLib::LogMessageString("Cannot start recording because path for log file is not available", "", LogDebugLevel::Failure);
             return;
@@ -134,17 +124,20 @@ void RecordingFile::startRecording()
             UAirBlueprintLib::LogMessage(TEXT("Recording"), TEXT("Started"), LogDebugLevel::Success);
         }
         else
-            UAirBlueprintLib::LogMessageString("Error creating log file", fullPath.c_str(), LogDebugLevel::Failure);
+            UAirBlueprintLib::LogMessageString("Error creating log file", log_filepath.c_str(), LogDebugLevel::Failure);
     }
     catch(...) {
         UAirBlueprintLib::LogMessageString("Error in startRecording", "", LogDebugLevel::Failure);
     }
 }
 
-void RecordingFile::stopRecording()
+void RecordingFile::stopRecording(bool ignore_if_stopped)
 {
     is_recording_ = false;
     if (! isFileOpen()) {
+        if (ignore_if_stopped)
+            return;
+
         UAirBlueprintLib::LogMessage(TEXT("Recording Error"), TEXT("File was not open"), LogDebugLevel::Failure);
     }
     else

Unreal/Plugins/AirSim/Source/Recording/RecordingFile.h

@@ -16,9 +16,9 @@ class RecordingFile {
 public:
     ~RecordingFile();
 
-    void appendRecord(TArray<uint8>& compressedPng, const msr::airlib::Kinematics* kinematics);
+    void appendRecord(TArray<uint8>& compressedPng, const msr::airlib::Kinematics::State* kinematics);
     void startRecording();
-    void stopRecording();
+    void stopRecording(bool ignore_if_stopped);
     bool isRecording();
 
 private:
@@ -33,7 +33,7 @@ class RecordingFile {
 private:
     std::string record_filename = "airsim_rec";     
     unsigned int images_saved_ = 0;
-    FString image_path_;
+    std::string image_path_;
     bool is_recording_ = false;
     IFileHandle* log_file_handle_ = nullptr;
 };

Unreal/Plugins/AirSim/Source/Recording/RecordingThread.cpp

@@ -16,7 +16,7 @@ FRecordingThread::FRecordingThread()
 }
 
 
-FRecordingThread* FRecordingThread::ThreadInit(msr::airlib::VehicleCameraBase* camera, RecordingFile* recording_file, const msr::airlib::Kinematics* kinematics, const RecordingSettings& settings)
+FRecordingThread* FRecordingThread::ThreadInit(msr::airlib::VehicleCameraBase* camera, RecordingFile* recording_file, const msr::airlib::Kinematics::State* kinematics, const RecordingSettings& settings)
 {
     if (!instance_ && FPlatformProcess::SupportsMultithreading())
     {
@@ -56,11 +56,11 @@ uint32 FRecordingThread::Run()
         //make sire all vars are set up
         if (is_ready_) {
             bool interval_elapsed = msr::airlib::ClockFactory::get()->elapsedSince(last_screenshot_on_) > settings_.record_interval;
-            bool is_pose_unequal = kinematics_ && last_pose_ != kinematics_->getPose();
+            bool is_pose_unequal = kinematics_ && last_pose_ != kinematics_->pose;
             if (interval_elapsed && (!settings_.record_on_move || is_pose_unequal))
             {
                 last_screenshot_on_ = msr::airlib::ClockFactory::get()->nowNanos();
-                last_pose_ = kinematics_->getPose();
+                last_pose_ = kinematics_->pose;
 
                 // todo: should we go as fast as possible, or should we limit this to a particular number of
                 // frames per second?

Unreal/Plugins/AirSim/Source/Recording/RecordingThread.h

@@ -13,7 +13,7 @@ class FRecordingThread : public FRunnable
 public:
     FRecordingThread();
     virtual ~FRecordingThread();
-    static FRecordingThread* ThreadInit(msr::airlib::VehicleCameraBase* camera, RecordingFile* recording_file, const msr::airlib::Kinematics* kinematics, const RecordingSettings& settings);
+    static FRecordingThread* ThreadInit(msr::airlib::VehicleCameraBase* camera, RecordingFile* recording_file, const msr::airlib::Kinematics::State* kinematics, const RecordingSettings& settings);
     static void Shutdown();
 
 private:
@@ -29,7 +29,7 @@ class FRecordingThread : public FRunnable
 
     msr::airlib::VehicleCameraBase* camera_;
     RecordingFile* recording_file_;
-    const msr::airlib::Kinematics* kinematics_;
+    const msr::airlib::Kinematics::State* kinematics_;
 
     FString image_path_;