Merge branch 'OSI-update_and_submodule_reduction' into 'master'

Osi update and submodule reduction

See merge request tuda-fzd/perception-sensor-modeling/modular-osmp-framework!5

.gitignore

@@ -4,4 +4,6 @@ build*
 .editorconfig
 out/
 .idea/
-.vs/
+.vs/
+*.sh
+*.bat

.gitmodules

@@ -4,15 +4,6 @@
 [submodule "lib/open-simulation-interface"]
 	path = lib/open-simulation-interface
 	url = https://github.com/OpenSimulationInterface/open-simulation-interface.git
-[submodule "src/model/strategies/transformation-functions"]
-	path = src/model/strategies/transformation-functions
-	url = https://gitlab.com/tuda-fzd/perception-sensor-modeling/transformation-functions.git
-[submodule "src/model/strategies/csv-output-gtobjects-strategy"]
-	path = src/model/strategies/csv-output-gtobjects-strategy
-	url = https://gitlab.com/tuda-fzd/perception-sensor-modeling/output-strategies/csv-output-gtobjects-strategy.git
-[submodule "src/model/strategies/ros-output-gtobjects-strategy"]
-	path = src/model/strategies/ros-output-gtobjects-strategy
-	url = https://gitlab.com/tuda-fzd/perception-sensor-modeling/output-strategies/ros-output-gtobjects-strategy.git
 [submodule "lib/fmi2"]
 	path = lib/fmi2
 	url = https://github.com/modelica/fmi-standard.git

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

@@ -0,0 +1,38 @@
+cmake_minimum_required(VERSION 3.12)
+project(CsvOutputGTObjects)
+
+set(CSV_GTOBJECTS_SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/src)
+set(CSV_GTOBJECTS_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/csvoutputgtobjects/set_csv_file_path_gtobjects.cpp.in ${CMAKE_CURRENT_BINARY_DIR}/include/csvoutputgtobjects/set_csv_file_path_gtobjects.cpp)
+
+# TODO MODIFY THE FOLLOWING THREE LINES AS NEEDED
+set(STRATEGY_ENTRY_POINT CsvOutputGTObjects)
+set(STRATEGY_SOURCES ${CSV_GTOBJECTS_SOURCE_DIR}/CsvOutputGTObjects.cpp ../transformation-functions/TransformationFunctions.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-gtobjects-strategy/README.md

@@ -0,0 +1,12 @@
+# Ground Truth Objects to .csv Output Strategy
+
+This strategy writes ground truth objects in a csv file.
+Path for the file is set via CMakeLists.
+
+## Usage
+You need to add the name of the strategy in a new line in the *csv_output_sequence.conf* file in the *src/model/strategies/* folder.
+
+In order for the strategy to be called during simulation, the FMI parameter *switch_for_csv_output* needs to be set to *1* and be passed to the framework or model fmu.
+
+NOTE:
+This strategy needs transformation-functions from ../transformation-functions.

src/model/strategies/csv-output-gtobjects-strategy/include/csvoutputgtobjects/CsvOutputGTObjects.hpp

@@ -0,0 +1,61 @@
+//
+// 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_GTOBJECTS_STRATEGY_HPP
+#define CSV_OUTPUT_GTOBJECTS_STRATEGY_HPP
+
+#include <model/include/strategy.hpp>
+#include <string>
+
+using namespace osi3;
+
+namespace model {
+
+	class CsvOutputGTObjects : public Strategy {
+
+        using Strategy::Strategy;
+
+        void apply(SensorData &) override;
+
+        std::string file_path_gtobjects;
+        bool first_call = true;
+
+    public:
+
+    private:
+
+		struct GT_object {
+			size_t id = 0;
+            float x = 0.0;
+            float y = 0.0;
+            float z = 0.0;
+            float roll = 0.0;
+            float pitch = 0.0;
+            float yaw = 0.0;
+            float width = 0.0;
+            float length = 0.0;
+            float height = 0.0;
+            bool is_moving = false;
+		};
+
+        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 object_idx, float x, float y, float z, float roll, float pitch, float yaw, float width, float length, float height, bool is_moving);
+    };
+
+}
+
+#endif //CSV_OUTPUT_GTOBJECTS_STRATEGY_HPP

src/model/strategies/csv-output-gtobjects-strategy/include/csvoutputgtobjects/set_csv_file_path_gtobjects.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 = "GTObjects.csv";
+
+#if defined(_WIN32)
+    while ((pos = path_string.find("/")) != std::string::npos) {
+        path_string.replace(pos, 1, "\\");
+    }
+    _mkdir(path_string.c_str());
+    file_path_gtobjects = path_string + "\\" + filename;
+#else
+    mkdir(path_string.c_str(), 0777);
+    file_path_gtobjects = path_string + "/" + filename;
+#endif

src/model/strategies/csv-output-gtobjects-strategy/src/CsvOutputGTObjects.cpp

@@ -0,0 +1,157 @@
+//
+// 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 "csvoutputgtobjects/CsvOutputGTObjects.hpp"
+#include "../../transformation-functions/TransformationFunctions.hpp"
+#include <fstream>
+#include <iostream>
+#include <vector>
+#include <ctime>
+
+#ifdef _WIN32
+    #include <math.h>
+    #include <direct.h>
+#else
+    #include <cmath>
+    #include <sys/stat.h>
+#endif
+
+using namespace model;
+using namespace osi3;
+
+static bool first_call = true;
+
+void model::CsvOutputGTObjects::apply(SensorData &sensor_data) {
+    log("Starting .csv output for GT objects");
+
+    if (sensor_data.sensor_view_size() == 0) {
+        log("No sensor view received for .csv output");
+        return;
+    }
+
+    TF::EgoData ego_data;
+    if (!TF::get_ego_info(ego_data, sensor_data.sensor_view(0))) {
+        log("Ego vehicle has no base, no id, or is not contained in GT moving objects.");
+        return;
+    }
+
+    auto time_nanos = sensor_data.sensor_view(0).global_ground_truth().timestamp().nanos();
+    auto time_seconds = sensor_data.sensor_view(0).global_ground_truth().timestamp().seconds();
+    double timestamp = (double)time_seconds + (double) time_nanos / 1000000000;
+
+    if (!sensor_data.sensor_view(0).has_global_ground_truth()) {
+        log("No GT for .csv output at timestamp " + std::to_string(timestamp));
+        return;
+    }
+
+    if ((sensor_data.sensor_view(0).global_ground_truth().moving_object_size() == 0) &&
+        (sensor_data.sensor_view(0).global_ground_truth().stationary_object_size() == 0)) {
+        log("No objects in GT for .csv output at timestamp " + std::to_string(timestamp));
+        return;
+    }
+
+    /// Write header line of .csv on first call
+    if (first_call) {
+        #include <csvoutputgtobjects/set_csv_file_path_gtobjects.cpp>
+        write_first_line_to_CSV(file_path_gtobjects);
+        first_call = false;
+    }
+
+    auto no_of_moving_objects = sensor_data.sensor_view(0).global_ground_truth().moving_object_size();
+    auto no_of_stationary_objects = sensor_data.sensor_view(0).global_ground_truth().stationary_object_size();
+
+    /// Start a vector for gt_objects with (gt_id, x, y, z, roll, pitch, yaw, width, length, height, is_moving)
+    std::vector<GT_object> gt_objects;
+    gt_objects.reserve(no_of_moving_objects + no_of_stationary_objects);
+
+    /// Collect moving objects
+    for (const auto &gt_moving_object : sensor_data.sensor_view(0).global_ground_truth().moving_object()) {
+        if (gt_moving_object.id().value() == sensor_data.sensor_view(0).global_ground_truth().host_vehicle_id().value())
+            continue;
+
+        Vector3d relative_position_ego_coordinate_system = TF::transform_position_from_world_to_ego_coordinates(gt_moving_object.base().position(), ego_data);
+        Orientation3d relative_orientation = TF::calc_relative_orientation_to_local(gt_moving_object.base().orientation(), ego_data.ego_base.orientation());
+        GT_object actual_gt_object;
+        actual_gt_object.id = gt_moving_object.id().value();
+        actual_gt_object.x = std::round(float(relative_position_ego_coordinate_system.x()) * 1000) / 1000;
+        actual_gt_object.y = std::round(float(relative_position_ego_coordinate_system.y()) * 1000) / 1000;
+        actual_gt_object.z = std::round(float(relative_position_ego_coordinate_system.z()) * 1000) / 1000;
+        actual_gt_object.roll  = (float)std::round(float(relative_orientation.roll())  * 180 / M_PI * 1000) / 1000;
+        actual_gt_object.pitch = (float)std::round(float(relative_orientation.pitch()) * 180 / M_PI * 1000) / 1000;
+        actual_gt_object.yaw   = (float)std::round(float(relative_orientation.yaw())   * 180 / M_PI * 1000) / 1000;
+        actual_gt_object.width  = float(gt_moving_object.base().dimension().width());
+        actual_gt_object.length = float(gt_moving_object.base().dimension().length());
+        actual_gt_object.height = float(gt_moving_object.base().dimension().height());
+        actual_gt_object.is_moving = true;
+        gt_objects.emplace_back(actual_gt_object);
+    }
+
+    /// Collect stationary objects
+    for (const auto &gt_stationary_object : sensor_data.sensor_view(0).global_ground_truth().stationary_object()) {
+        Vector3d relative_position_ego_coordinate_system = TF::transform_position_from_world_to_ego_coordinates(
+                gt_stationary_object.base().position(), ego_data);
+        Orientation3d relative_orientation = TF::calc_relative_orientation_to_local(
+                gt_stationary_object.base().orientation(),
+                ego_data.ego_base.orientation());
+        GT_object actual_gt_object;
+        actual_gt_object.id = gt_stationary_object.id().value();
+        actual_gt_object.x = std::round(float(relative_position_ego_coordinate_system.x()) * 1000) / 1000;
+        actual_gt_object.y = std::round(float(relative_position_ego_coordinate_system.y()) * 1000) / 1000;
+        actual_gt_object.z = std::round(float(relative_position_ego_coordinate_system.z()) * 1000) / 1000;
+        actual_gt_object.roll  = (float)std::round(float(relative_orientation.roll())  * 180 / M_PI * 1000) / 1000;
+        actual_gt_object.pitch = (float)std::round(float(relative_orientation.pitch()) * 180 / M_PI * 1000) / 1000;
+        actual_gt_object.yaw   = (float)std::round(float(relative_orientation.yaw())   * 180 / M_PI * 1000) / 1000;
+        actual_gt_object.width  = float(gt_stationary_object.base().dimension().width());
+        actual_gt_object.length = float(gt_stationary_object.base().dimension().length());
+        actual_gt_object.height = float(gt_stationary_object.base().dimension().height());
+        actual_gt_object.is_moving = false;
+        gt_objects.emplace_back(actual_gt_object);
+    }
+
+    for (const auto &gt_object : gt_objects) {
+        auto gt_id = gt_object.id;
+
+        auto roll = std::round(gt_object.roll* 180 / M_PI * 1000) / 1000;
+        auto pitch = std::round(gt_object.pitch * 180 / M_PI * 1000) / 1000;
+        auto yaw = std::round(gt_object.yaw * 180 / M_PI * 1000) / 1000;
+
+        write_data_to_CSV(file_path_gtobjects, timestamp, gt_id, gt_object.x, gt_object.y, gt_object.z, gt_object.roll, gt_object.pitch, gt_object.yaw,
+                            gt_object.width, gt_object.length, gt_object.height, gt_object.is_moving);
+    }
+}
+
+void CsvOutputGTObjects::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, gt_object_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"
+            << std::endl;
+    my_file.close();
+}
+
+void CsvOutputGTObjects::write_data_to_CSV(const std::string& path, double timestamp, size_t object_idx, float x, float y, float z, float roll, float pitch, float yaw, float width, float length, float height, bool is_moving) {
+    std::fstream my_file;
+    my_file.open(path, std::ios::app);
+    my_file << timestamp << ", " << object_idx << ", " << x << ", " << y << ", " << z << ", " << roll << ", " << pitch
+            << ", " << yaw << ", " << width << ", " << length << ", " << height << ", "
+            << (is_moving ? "true" : "false") << std::endl;
+    my_file.close();
+}

src/model/strategies/ros-output-gtobjects-strategy/CMakeLists.txt

@@ -0,0 +1,29 @@
+cmake_minimum_required(VERSION 3.12)
+project(ros_gt_objects)
+
+# TODO MODIFY THE FOLLOWING LINES AS NEEDED
+set(STRATEGY_ENTRY_POINT ros_gt_objects)
+set(STRATEGY_SOURCES src/ros_gt_objects.cpp)
+set(STRATEGY_EXTRA_PUBLIC_LIBS_OR_TARGETS ${PCL_COMMON_LIBRARIES} ${catkin_LIBRARIES})
+set(STRATEGY_EXTRA_PRIVATE_LIBS_OR_TARGETS "")
+set(STRATEGY_EXTRA_INCLUDE_DIRECTORIES ${catkin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS})
+set(STRATEGY_EXTRA_DEPENDENCIES ${catkin_EXPORTED_TARGETS})
+#set(STRATEGY_PROFILE_EXTENSION ${PROFILES})
+
+# no need to modify commands below this line
+set(CMAKE_CXX_STANDARD 17)
+
+if(NOT COMMAND add_fmu_ros_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_ros_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)
+add_dependencies(${PROJECT_NAME} ${STRATEGY_EXTRA_DEPENDENCIES})

src/model/strategies/ros-output-gtobjects-strategy/README.md

@@ -0,0 +1,12 @@
+# OSI Ground Truth Objects to ROS Output Strategy
+
+This strategy outputs OSI global_ground_truth.moving_objects and global_ground_truth.stationary_objects as ROS Marker messages.
+Using the ROS tool rviz is a convenient way to visualize OSI sensor view input and sensor data output.
+
+## Usage
+You need to add the name of the strategy in a new line in the *ros_output_sequence.conf* file in the *src/model/strategies/* folder.
+
+In order for the strategy to be called during simulation, the FMI parameter *switch_for_ros_output* needs to be set to *1* and be passed to the framework or model fmu.
+
+## ROS Distributions
+This strategy needs a full install of either ROS noetic or ROS melodic. The ROS distribution needs to be located in /opt/ros/.