Merge branch 'feature/add_output_strategies' into 'main'

Feature/add output strategies

See merge request tuda-fzd/perception-sensor-modeling/reflection-based-radar-object-model!3

.gitignore

@@ -5,3 +5,5 @@ out/
 .idea/
 .vs/
 cmake-build*
+*.sh
+*.bat

src/model/strategies/csv-output-detectedobjects-strategy/CMakeLists.txt

@@ -0,0 +1,37 @@
+cmake_minimum_required(VERSION 3.12)
+project(CsvOutputDetectedObjects)
+
+set(CSV_DETECTEDOBJECTS_SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/src)
+set(CSV_DETECTEDOBJECTS_TARGET_DIR ${CMAKE_CURRENT_BINARY_DIR}/src)
+if (WIN32)
+    set(CSV_PATH C:/TEMP)
+elseif (${CMAKE_SYSTEM_NAME} MATCHES "Linux")
+    set(CSV_PATH /tmp)
+endif()
+configure_file(${CMAKE_CURRENT_SOURCE_DIR}/include/csvoutputdetectedobjects/set_csv_file_path_detectedobjects.cpp.in ${CMAKE_CURRENT_BINARY_DIR}/include/csvoutputdetectedobjects/set_csv_file_path_detectedobjects.cpp)
+
+# TODO MODIFY THE FOLLOWING THREE LINES AS NEEDED
+set(STRATEGY_ENTRY_POINT CsvOutputDetectedObjects)
+set(STRATEGY_SOURCES ${CSV_DETECTEDOBJECTS_SOURCE_DIR}/CsvOutputDetectedObjects.cpp)
+set(STRATEGY_EXTRA_PUBLIC_LIBS_OR_TARGETS "")
+set(STRATEGY_EXTRA_PRIVATE_LIBS_OR_TARGETS "")
+set(STRATEGY_EXTRA_INCLUDE_DIRECTORIES ${CMAKE_CURRENT_BINARY_DIR}/include)
+#set(STRATEGY_PROFILE_EXTENSION ${PROFILES})
+
+# no need to modify commands below this line
+set(CMAKE_CXX_STANDARD 17)
+
+if(NOT COMMAND add_fmu_csv_output_strategy)
+    message(FATAL_ERROR "This project directory has to be included by OSI FMU platform, it can't be build out of that context!")
+endif()
+
+add_fmu_csv_output_strategy(${PROJECT_NAME} ${STRATEGY_ENTRY_POINT})
+if(STRATEGY_PROFILE_EXTENSION)
+    add_profile_part(${STRATEGY_PROFILE_EXTENSION})
+endif()
+
+add_library(${PROJECT_NAME} STATIC ${STRATEGY_SOURCES})
+target_link_libraries(${PROJECT_NAME} PRIVATE ${STRATEGY_EXTRA_PRIVATE_LIBS_OR_TARGETS} PUBLIC model::strategy open_simulation_interface_obj ${STRATEGY_EXTRA_PUBLIC_LIBS_OR_TARGETS})
+target_include_directories(${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include ${STRATEGY_EXTRA_INCLUDE_DIRECTORIES})
+set_property(TARGET ${PROJECT_NAME} PROPERTY POSITION_INDEPENDENT_CODE ON)
+

src/model/strategies/csv-output-detectedobjects-strategy/README.md

@@ -0,0 +1,4 @@
+# CSV Output Strategy for Detected Objects
+
+This strategy writes OSI3::DetectedMovingObjects in a csv file.
+Path for the file is set via CMakeLists.

src/model/strategies/csv-output-detectedobjects-strategy/include/csvoutputdetectedobjects/CsvOutputDetectedObjects.hpp

@@ -0,0 +1,47 @@
+//
+// Copyright Institute of Automotive Engineering
+// of Technical University of Darmstadt 2020.
+// Licensed under the EUPL-1.2-or-later
+//
+// This work covered by the EUPL can be used/merged and distributed
+// in other works covered by GPL-2.0, GPL-3.0, LGPL, AGPL, CeCILL,
+// OSL, EPL, MPL and other licences listed as compatible in the EUPL
+// Appendix. This applies to the other (combined) work, while the
+// original project stays covered by the EUPL without re-licensing.
+//
+// Alternatively, the contents of this file may be used under the
+// terms of the Mozilla Public License, v. 2.0. If a copy of the MPL
+// was not distributed with this file, you can obtain one at
+// http://mozilla.org/MPL/2.0/.
+//
+
+#ifndef CSV_OUTPUT_DETECTEDOBJECTS_STRATEGY_HPP
+#define CSV_OUTPUT_DETECTEDOBJECTS_STRATEGY_HPP
+
+#include <model/include/strategy.hpp>
+#include <string>
+
+using namespace osi3;
+
+namespace model {
+
+	class CsvOutputDetectedObjects : public Strategy {
+
+        using Strategy::Strategy;
+
+        void apply(SensorData &) override;
+
+        std::string file_path_detectedobjects;
+        bool first_call = true;
+
+    public:
+
+    private:
+        static void write_first_line_to_CSV(const std::string& path);
+        static void write_data_to_CSV(const std::string &path, double timestamp, size_t tracking_id, double x, double y, double z, double roll, double pitch, double yaw, double width, double length,
+                                      double height, bool is_moving, double existence_probability);
+    };
+
+}
+
+#endif //CSV_OUTPUT_DETECTEDOBJECTS_STRATEGY_HPP

src/model/strategies/csv-output-detectedobjects-strategy/include/csvoutputdetectedobjects/set_csv_file_path_detectedobjects.cpp.in

@@ -0,0 +1,29 @@
+time_t curr_time;
+struct tm *detl;
+char buf[80];
+time( &curr_time );
+detl = localtime( &curr_time );
+// strftime(buf, 20, "%x - %I:%M%p", detl);
+strftime(buf, 20, "%Y-%m-%d_%H-%M-%S", detl);
+
+std::string start_time = std::string(buf);
+
+std::string path_string = "@CSV_PATH@/";
+size_t pos;
+
+path_string = path_string + "@MODEL_NAME@" + "_" + start_time;
+std::string filename = "DetectedObjects.csv";
+
+#if defined(_WIN32)
+    while ((pos = path_string.find("/")) != std::string::npos) {
+        path_string.replace(pos, 1, "\\");
+    }
+    _mkdir(path_string.c_str());
+    file_path_detectedobjects = path_string + "\\" + filename;
+#else
+    mkdir(path_string.c_str(), 0777);
+    file_path_detectedobjects = path_string + "/" + filename;
+#endif
+
+
+

src/model/strategies/csv-output-detectedobjects-strategy/src/CsvOutputDetectedObjects.cpp

@@ -0,0 +1,118 @@
+//
+// Copyright Institute of Automotive Engineering
+// of Technical University of Darmstadt 2020.
+// Licensed under the EUPL-1.2-or-later
+//
+// This work covered by the EUPL can be used/merged and distributed
+// in other works covered by GPL-2.0, GPL-3.0, LGPL, AGPL, CeCILL,
+// OSL, EPL, MPL and other licences listed as compatible in the EUPL
+// Appendix. This applies to the other (combined) work, while the
+// original project stays covered by the EUPL without re-licensing.
+//
+// Alternatively, the contents of this file may be used under the
+// terms of the Mozilla Public License, v. 2.0. If a copy of the MPL
+// was not distributed with this file, you can obtain one at
+// http://mozilla.org/MPL/2.0/.
+//
+
+#ifndef _USE_MATH_DEFINES
+#define _USE_MATH_DEFINES
+#endif
+
+#include "csvoutputdetectedobjects/CsvOutputDetectedObjects.hpp"
+#include <fstream>
+#include <iostream>
+#include <ctime>
+
+#ifdef _WIN32
+    #include <math.h>
+    #include <direct.h>
+#else
+    #include <cmath>
+    #include <sys/stat.h>
+#endif
+
+using namespace model;
+using namespace osi3;
+
+void model::CsvOutputDetectedObjects::apply(SensorData &sensor_data) {
+    log("Starting .csv output for detected objects");
+
+    if ((sensor_data.sensor_view_size()==0) || (!sensor_data.sensor_view(0).has_global_ground_truth())) {
+        log("No sensor view or global ground truth");
+        return;
+    }
+
+    const auto &ground_truth = sensor_data.sensor_view(0).global_ground_truth();
+
+    auto time_nanos = ground_truth.timestamp().nanos();
+    auto time_seconds = ground_truth.timestamp().seconds();
+    double timestamp = (double) time_seconds + (double) time_nanos / 1000000000;
+
+    auto no_of_moving_objects = sensor_data.moving_object_size();
+    auto no_of_stationary_objects = sensor_data.stationary_object_size();
+    auto no_of_objects = no_of_moving_objects + no_of_stationary_objects;
+
+    /// Add tracking_id, x, y, z, roll, pitch, yaw, width, length, height, is_moving, existence_probability to csv
+    if (no_of_objects > 0) {
+
+        /// Write header line of .csv on first call
+        if (first_call){
+            #include <csvoutputdetectedobjects/set_csv_file_path_detectedobjects.cpp>
+            write_first_line_to_CSV(file_path_detectedobjects);
+            first_call = false;
+        }
+
+        for (const auto &moving_object : sensor_data.moving_object()) {
+            write_data_to_CSV(file_path_detectedobjects,
+                              timestamp,
+                              moving_object.header().tracking_id().value(),
+                              std::round(moving_object.base().position().x() * 1000) / 1000,
+                              std::round(moving_object.base().position().y() * 1000) / 1000,
+                              std::round(moving_object.base().position().z() * 1000) / 1000,
+                              std::round(moving_object.base().orientation().roll() * 1000) / 1000,
+                              std::round(moving_object.base().orientation().pitch() * 1000) / 1000,
+                              std::round(moving_object.base().orientation().yaw() * 1000) / 1000,
+                              std::round(moving_object.base().dimension().width() * 1000) / 1000,
+                              std::round(moving_object.base().dimension().length() * 1000) / 1000,
+                              std::round(moving_object.base().dimension().height() * 1000) / 1000,
+                              true,
+                              std::round(moving_object.header().existence_probability() * 1000) / 1000);
+        }
+        for (const auto &stationary_object : sensor_data.stationary_object()) {
+            write_data_to_CSV(file_path_detectedobjects,
+                              timestamp,
+                              stationary_object.header().tracking_id().value(),
+                              std::round(stationary_object.base().position().x() * 1000) / 1000,
+                              std::round(stationary_object.base().position().y() * 1000) / 1000,
+                              std::round(stationary_object.base().position().z() * 1000) / 1000,
+                              std::round(stationary_object.base().orientation().roll() * 1000) / 1000,
+                              std::round(stationary_object.base().orientation().pitch() * 1000) / 1000,
+                              std::round(stationary_object.base().orientation().yaw() * 1000) / 1000,
+                              std::round(stationary_object.base().dimension().width() * 1000) / 1000,
+                              std::round(stationary_object.base().dimension().length() * 1000) / 1000,
+                              std::round(stationary_object.base().dimension().height() * 1000) / 1000,
+                              false,
+                              std::round(stationary_object.header().existence_probability() * 1000) / 1000);
+        }
+    } else {
+        log("No detected objects for .csv output at timestamp " + std::to_string(timestamp));
+        return;
+    }
+}
+
+void CsvOutputDetectedObjects::write_first_line_to_CSV(const std::string& path) {
+    std::fstream my_file;
+    my_file.open(path, std::ios::app);
+    my_file << "timestamp_in_s, tracking_id, x_in_m, y_in_m, z_in_m, roll_in_deg, pitch_in_deg, yaw_in_deg, width_in_m, length_in_m, height_in_m, is_moving, existence_probability" << std::endl;
+    my_file.close();
+}
+
+void
+CsvOutputDetectedObjects::write_data_to_CSV(const std::string &path, double timestamp, size_t tracking_id, double x, double y, double z, double roll, double pitch, double yaw, double width, double length,
+                                            double height, bool is_moving, double existence_probability) {
+    std::fstream my_file;
+    my_file.open(path, std::ios::app);
+    my_file << timestamp << ", " << tracking_id << ", " << x << ", " << y << ", " << z << ", " << roll << ", " << pitch << ", " << yaw << ", " << width << ", " << length << ", " << height << ", " << (is_moving ? "true": "false") << ", " << existence_probability << std::endl;
+    my_file.close();
+}

src/model/strategies/csv-output-detections-strategy/CMakeLists.txt

@@ -0,0 +1,38 @@
+cmake_minimum_required(VERSION 3.12)
+project(CsvOutputDetections)
+
+set(CSV_DETECTIONS_SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/src)
+set(CSV_DETECTIONS_TARGET_DIR ${CMAKE_CURRENT_BINARY_DIR}/src)
+if (WIN32)
+    set(CSV_PATH C:/TEMP)
+elseif (${CMAKE_SYSTEM_NAME} MATCHES "Linux")
+    set(CSV_PATH /tmp)
+endif()
+configure_file(${CMAKE_CURRENT_SOURCE_DIR}/include/csvoutputdetections/set_csv_file_path_detections.cpp.in ${CMAKE_CURRENT_BINARY_DIR}/include/csvoutputdetections/set_csv_file_path_detections.cpp)
+
+# TODO MODIFY THE FOLLOWING THREE LINES AS NEEDED
+set(STRATEGY_ENTRY_POINT CsvOutputDetections)
+set(STRATEGY_SOURCES ${CSV_DETECTIONS_SOURCE_DIR}/CsvOutputDetections.cpp)
+set(STRATEGY_EXTRA_PUBLIC_LIBS_OR_TARGETS "")
+set(STRATEGY_EXTRA_PRIVATE_LIBS_OR_TARGETS "")
+set(STRATEGY_EXTRA_INCLUDE_DIRECTORIES ${CMAKE_CURRENT_BINARY_DIR}/include)
+#set(STRATEGY_PROFILE_EXTENSION ${PROFILES})
+
+# no need to modify commands below this line
+set(CMAKE_CXX_STANDARD 17)
+
+if(NOT COMMAND add_fmu_csv_output_strategy)
+    message(FATAL_ERROR "This project directory has to be included by OSI FMU platform, it can't be build out of that context!")
+endif()
+
+add_fmu_csv_output_strategy(${PROJECT_NAME} ${STRATEGY_ENTRY_POINT})
+if(STRATEGY_PROFILE_EXTENSION)
+    add_profile_part(${STRATEGY_PROFILE_EXTENSION})
+endif()
+
+
+add_library(${PROJECT_NAME} STATIC ${STRATEGY_SOURCES})
+target_link_libraries(${PROJECT_NAME} PRIVATE ${STRATEGY_EXTRA_PRIVATE_LIBS_OR_TARGETS} PUBLIC model::strategy open_simulation_interface_obj ${STRATEGY_EXTRA_PUBLIC_LIBS_OR_TARGETS})
+target_include_directories(${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include ${STRATEGY_EXTRA_INCLUDE_DIRECTORIES})
+set_property(TARGET ${PROJECT_NAME} PROPERTY POSITION_INDEPENDENT_CODE ON)
+

src/model/strategies/csv-output-detections-strategy/README.md

@@ -0,0 +1,4 @@
+# RadarDetections to .csv Output Strategy
+
+This strategy writes the list of osi3::RadarDetections in a csv file.
+Path for the file is set via CMakeLists.

src/model/strategies/csv-output-detections-strategy/include/csvoutputdetections/CsvOutputDetections.hpp

@@ -0,0 +1,47 @@
+//
+// Copyright Institute of Automotive Engineering
+// of Technical University of Darmstadt 2020.
+// Licensed under the EUPL-1.2-or-later
+//
+// This work covered by the EUPL can be used/merged and distributed
+// in other works covered by GPL-2.0, GPL-3.0, LGPL, AGPL, CeCILL,
+// OSL, EPL, MPL and other licences listed as compatible in the EUPL
+// Appendix. This applies to the other (combined) work, while the
+// original project stays covered by the EUPL without re-licensing.
+//
+// Alternatively, the contents of this file may be used under the
+// terms of the Mozilla Public License, v. 2.0. If a copy of the MPL
+// was not distributed with this file, you can obtain one at
+// http://mozilla.org/MPL/2.0/.
+//
+
+#ifndef CSV_OUTPUT_DETECTIONS_STRATEGY_HPP
+#define CSV_OUTPUT_DETECTIONS_STRATEGY_HPP
+
+#include <model/include/strategy.hpp>
+#include <string>
+
+using namespace osi3;
+
+namespace model {
+
+    class CsvOutputDetections : public Strategy {
+
+        using Strategy::Strategy;
+
+        void apply(SensorData &) override;
+
+        std::string file_path_detections;
+        bool first_call = true;
+
+    public:
+
+    private:
+
+        void write_first_line_to_CSV(const std::string &path);
+        static void write_data_to_CSV(const std::string& path, double timestamp, size_t detection_idx, double azimuth_in_deg, double elevation_in_deg, double distance, double intensity);
+    };
+
+}
+
+#endif //CSV_OUTPUT_DETECTIONS_STRATEGY_HPP

src/model/strategies/csv-output-detections-strategy/include/csvoutputdetections/set_csv_file_path_detections.cpp.in

@@ -0,0 +1,26 @@
+time_t curr_time;
+struct tm *detl;
+char buf[80];
+time( &curr_time );
+detl = localtime( &curr_time );
+// strftime(buf, 20, "%x - %I:%M%p", detl);
+strftime(buf, 20, "%Y-%m-%d_%H-%M-%S", detl);
+
+std::string start_time = std::string(buf);
+
+std::string path_string = "@CSV_PATH@/";
+size_t pos;
+
+path_string = path_string + "@MODEL_NAME@" + "_" + start_time;
+std::string filename = "Detections.csv";
+
+#if defined(_WIN32)
+    while ((pos = path_string.find("/")) != std::string::npos) {
+        path_string.replace(pos, 1, "\\");
+    }
+    _mkdir(path_string.c_str());
+    file_path_detections = path_string + "\\" + filename;
+#else
+    mkdir(path_string.c_str(), 0777);
+    file_path_detections = path_string + "/" + filename;
+#endif