From 958b65f93c7d668d8ce9f70738e530a47feeefbc Mon Sep 17 00:00:00 2001 From: Lukas Elster Date: Wed, 14 Sep 2022 15:18:02 +0000 Subject: [PATCH] Feature/add output strategies --- .gitignore | 2 + .../CMakeLists.txt | 37 +++ .../README.md | 4 + .../CsvOutputDetectedObjects.hpp | 47 +++ .../set_csv_file_path_detectedobjects.cpp.in | 29 ++ .../src/CsvOutputDetectedObjects.cpp | 118 +++++++ .../CMakeLists.txt | 38 +++ .../csv-output-detections-strategy/README.md | 4 + .../CsvOutputDetections.hpp | 47 +++ .../set_csv_file_path_detections.cpp.in | 26 ++ .../src/CsvOutputDetections.cpp | 97 ++++++ .../CMakeLists.txt | 38 +++ .../csv-output-gtobjects-strategy/README.md | 7 + .../csvoutputgtobjects/CsvOutputGTObjects.hpp | 61 ++++ .../set_csv_file_path_gtobjects.cpp.in | 26 ++ .../src/CsvOutputGTObjects.cpp | 157 +++++++++ .../CMakeLists.txt | 38 +++ .../README.md | 4 + .../CsvOutputLogicalDetections.hpp | 46 +++ ...set_csv_file_path_logicaldetections.cpp.in | 26 ++ .../src/CsvOutputLogicalDetections.cpp | 90 +++++ .../CMakeLists.txt | 38 +++ .../csv-output-reflections-strategy/README.md | 4 + .../CsvOutputReflections.hpp | 46 +++ .../set_csv_file_path_reflections.cpp.in | 29 ++ .../src/CsvOutputReflections.cpp | 105 ++++++ src/model/strategies/csv_output_sequence.conf | 5 + .../CMakeLists.txt | 38 +++ .../pcd-output-detections-strategy/README.md | 4 + .../PcdOutputDetections.hpp | 46 +++ .../set_pcd_file_path_detections.cpp.in | 23 ++ .../src/PcdOutputDetections.cpp | 121 +++++++ .../CMakeLists.txt | 38 +++ .../README.md | 4 + .../PcdOutputLogicalDetections.hpp | 46 +++ ...set_pcd_file_path_logicaldetections.cpp.in | 23 ++ .../src/PcdOutputLogicalDetections.cpp | 99 ++++++ src/model/strategies/pcd_output_sequence.conf | 2 + .../src/DetectionSensing.cpp | 2 +- .../CMakeLists.txt | 29 ++ .../README.md | 4 + .../ros_detectedobjects.hpp | 63 ++++ .../package.xml | 20 ++ .../src/ros_detectedobjects.cpp | 139 ++++++++ .../CMakeLists.txt | 29 ++ .../ros-output-detections-strategy/README.md | 4 + .../include/ros_detections/ros_detections.hpp | 59 ++++ .../package.xml | 20 ++ .../src/ros_detections.cpp | 107 ++++++ .../CMakeLists.txt | 29 ++ .../ros-output-gtobjects-strategy/README.md | 14 + .../include/ros_gt_objects/ros_gt_objects.hpp | 64 ++++ .../ros-output-gtobjects-strategy/package.xml | 20 ++ .../src/ros_gt_objects.cpp | 208 ++++++++++++ .../CMakeLists.txt | 29 ++ .../README.md | 3 + .../ros_logicaldetections.hpp | 63 ++++ .../src/ros_logicaldetections.cpp | 86 +++++ .../CMakeLists.txt | 33 ++ .../ros-output-reflections-strategy/README.md | 4 + .../ros_reflections/ros_reflections.hpp | 59 ++++ .../package.xml | 20 ++ .../src/ros_reflections.cpp | 116 +++++++ .../TransformationFunctions.cpp | 307 ++++++++++++++++++ .../TransformationFunctions.hpp | 78 +++++ 65 files changed, 3221 insertions(+), 1 deletion(-) create mode 100644 src/model/strategies/csv-output-detectedobjects-strategy/CMakeLists.txt create mode 100644 src/model/strategies/csv-output-detectedobjects-strategy/README.md create mode 100644 src/model/strategies/csv-output-detectedobjects-strategy/include/csvoutputdetectedobjects/CsvOutputDetectedObjects.hpp create mode 100644 src/model/strategies/csv-output-detectedobjects-strategy/include/csvoutputdetectedobjects/set_csv_file_path_detectedobjects.cpp.in create mode 100644 src/model/strategies/csv-output-detectedobjects-strategy/src/CsvOutputDetectedObjects.cpp create mode 100644 src/model/strategies/csv-output-detections-strategy/CMakeLists.txt create mode 100644 src/model/strategies/csv-output-detections-strategy/README.md create mode 100644 src/model/strategies/csv-output-detections-strategy/include/csvoutputdetections/CsvOutputDetections.hpp create mode 100644 src/model/strategies/csv-output-detections-strategy/include/csvoutputdetections/set_csv_file_path_detections.cpp.in create mode 100644 src/model/strategies/csv-output-detections-strategy/src/CsvOutputDetections.cpp create mode 100644 src/model/strategies/csv-output-gtobjects-strategy/CMakeLists.txt create mode 100644 src/model/strategies/csv-output-gtobjects-strategy/README.md create mode 100644 src/model/strategies/csv-output-gtobjects-strategy/include/csvoutputgtobjects/CsvOutputGTObjects.hpp create mode 100644 src/model/strategies/csv-output-gtobjects-strategy/include/csvoutputgtobjects/set_csv_file_path_gtobjects.cpp.in create mode 100644 src/model/strategies/csv-output-gtobjects-strategy/src/CsvOutputGTObjects.cpp create mode 100644 src/model/strategies/csv-output-logicaldetections-strategy/CMakeLists.txt create mode 100644 src/model/strategies/csv-output-logicaldetections-strategy/README.md create mode 100644 src/model/strategies/csv-output-logicaldetections-strategy/include/csvoutputlogicaldetections/CsvOutputLogicalDetections.hpp create mode 100644 src/model/strategies/csv-output-logicaldetections-strategy/include/csvoutputlogicaldetections/set_csv_file_path_logicaldetections.cpp.in create mode 100644 src/model/strategies/csv-output-logicaldetections-strategy/src/CsvOutputLogicalDetections.cpp create mode 100644 src/model/strategies/csv-output-reflections-strategy/CMakeLists.txt create mode 100644 src/model/strategies/csv-output-reflections-strategy/README.md create mode 100644 src/model/strategies/csv-output-reflections-strategy/include/csvoutputreflections/CsvOutputReflections.hpp create mode 100644 src/model/strategies/csv-output-reflections-strategy/include/csvoutputreflections/set_csv_file_path_reflections.cpp.in create mode 100644 src/model/strategies/csv-output-reflections-strategy/src/CsvOutputReflections.cpp create mode 100644 src/model/strategies/pcd-output-detections-strategy/CMakeLists.txt create mode 100644 src/model/strategies/pcd-output-detections-strategy/README.md create mode 100644 src/model/strategies/pcd-output-detections-strategy/include/pcdoutputdetections/PcdOutputDetections.hpp create mode 100644 src/model/strategies/pcd-output-detections-strategy/include/pcdoutputdetections/set_pcd_file_path_detections.cpp.in create mode 100644 src/model/strategies/pcd-output-detections-strategy/src/PcdOutputDetections.cpp create mode 100644 src/model/strategies/pcd-output-logicaldetections-strategy/CMakeLists.txt create mode 100644 src/model/strategies/pcd-output-logicaldetections-strategy/README.md create mode 100644 src/model/strategies/pcd-output-logicaldetections-strategy/include/pcdoutputlogicaldetections/PcdOutputLogicalDetections.hpp create mode 100644 src/model/strategies/pcd-output-logicaldetections-strategy/include/pcdoutputlogicaldetections/set_pcd_file_path_logicaldetections.cpp.in create mode 100644 src/model/strategies/pcd-output-logicaldetections-strategy/src/PcdOutputLogicalDetections.cpp create mode 100644 src/model/strategies/ros-output-detectedobjects-strategy/CMakeLists.txt create mode 100644 src/model/strategies/ros-output-detectedobjects-strategy/README.md create mode 100644 src/model/strategies/ros-output-detectedobjects-strategy/include/ros_detectedobjects/ros_detectedobjects.hpp create mode 100644 src/model/strategies/ros-output-detectedobjects-strategy/package.xml create mode 100644 src/model/strategies/ros-output-detectedobjects-strategy/src/ros_detectedobjects.cpp create mode 100644 src/model/strategies/ros-output-detections-strategy/CMakeLists.txt create mode 100644 src/model/strategies/ros-output-detections-strategy/README.md create mode 100644 src/model/strategies/ros-output-detections-strategy/include/ros_detections/ros_detections.hpp create mode 100644 src/model/strategies/ros-output-detections-strategy/package.xml create mode 100644 src/model/strategies/ros-output-detections-strategy/src/ros_detections.cpp create mode 100644 src/model/strategies/ros-output-gtobjects-strategy/CMakeLists.txt create mode 100644 src/model/strategies/ros-output-gtobjects-strategy/README.md create mode 100644 src/model/strategies/ros-output-gtobjects-strategy/include/ros_gt_objects/ros_gt_objects.hpp create mode 100644 src/model/strategies/ros-output-gtobjects-strategy/package.xml create mode 100644 src/model/strategies/ros-output-gtobjects-strategy/src/ros_gt_objects.cpp create mode 100644 src/model/strategies/ros-output-logicaldetections-strategy/CMakeLists.txt create mode 100644 src/model/strategies/ros-output-logicaldetections-strategy/README.md create mode 100644 src/model/strategies/ros-output-logicaldetections-strategy/include/ros_logicaldetections/ros_logicaldetections.hpp create mode 100644 src/model/strategies/ros-output-logicaldetections-strategy/src/ros_logicaldetections.cpp create mode 100644 src/model/strategies/ros-output-reflections-strategy/CMakeLists.txt create mode 100644 src/model/strategies/ros-output-reflections-strategy/README.md create mode 100644 src/model/strategies/ros-output-reflections-strategy/include/ros_reflections/ros_reflections.hpp create mode 100644 src/model/strategies/ros-output-reflections-strategy/package.xml create mode 100644 src/model/strategies/ros-output-reflections-strategy/src/ros_reflections.cpp create mode 100644 src/model/strategies/transformation-functions/TransformationFunctions.cpp create mode 100644 src/model/strategies/transformation-functions/TransformationFunctions.hpp diff --git a/.gitignore b/.gitignore index 0f82f88..48d46aa 100644 --- a/.gitignore +++ b/.gitignore @@ -5,3 +5,5 @@ out/ .idea/ .vs/ cmake-build* +*.sh +*.bat \ No newline at end of file diff --git a/src/model/strategies/csv-output-detectedobjects-strategy/CMakeLists.txt b/src/model/strategies/csv-output-detectedobjects-strategy/CMakeLists.txt new file mode 100644 index 0000000..28a4145 --- /dev/null +++ b/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) + diff --git a/src/model/strategies/csv-output-detectedobjects-strategy/README.md b/src/model/strategies/csv-output-detectedobjects-strategy/README.md new file mode 100644 index 0000000..9f9c64f --- /dev/null +++ b/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. \ No newline at end of file diff --git a/src/model/strategies/csv-output-detectedobjects-strategy/include/csvoutputdetectedobjects/CsvOutputDetectedObjects.hpp b/src/model/strategies/csv-output-detectedobjects-strategy/include/csvoutputdetectedobjects/CsvOutputDetectedObjects.hpp new file mode 100644 index 0000000..e66fd14 --- /dev/null +++ b/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 +#include + +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 diff --git a/src/model/strategies/csv-output-detectedobjects-strategy/include/csvoutputdetectedobjects/set_csv_file_path_detectedobjects.cpp.in b/src/model/strategies/csv-output-detectedobjects-strategy/include/csvoutputdetectedobjects/set_csv_file_path_detectedobjects.cpp.in new file mode 100644 index 0000000..a928c12 --- /dev/null +++ b/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 + + + diff --git a/src/model/strategies/csv-output-detectedobjects-strategy/src/CsvOutputDetectedObjects.cpp b/src/model/strategies/csv-output-detectedobjects-strategy/src/CsvOutputDetectedObjects.cpp new file mode 100644 index 0000000..98404ce --- /dev/null +++ b/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 +#include +#include + +#ifdef _WIN32 + #include + #include +#else + #include + #include +#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 + 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(); +} diff --git a/src/model/strategies/csv-output-detections-strategy/CMakeLists.txt b/src/model/strategies/csv-output-detections-strategy/CMakeLists.txt new file mode 100644 index 0000000..ff3e89d --- /dev/null +++ b/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) + diff --git a/src/model/strategies/csv-output-detections-strategy/README.md b/src/model/strategies/csv-output-detections-strategy/README.md new file mode 100644 index 0000000..6a6305f --- /dev/null +++ b/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. \ No newline at end of file diff --git a/src/model/strategies/csv-output-detections-strategy/include/csvoutputdetections/CsvOutputDetections.hpp b/src/model/strategies/csv-output-detections-strategy/include/csvoutputdetections/CsvOutputDetections.hpp new file mode 100644 index 0000000..f7bd378 --- /dev/null +++ b/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 +#include + +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 diff --git a/src/model/strategies/csv-output-detections-strategy/include/csvoutputdetections/set_csv_file_path_detections.cpp.in b/src/model/strategies/csv-output-detections-strategy/include/csvoutputdetections/set_csv_file_path_detections.cpp.in new file mode 100644 index 0000000..3595028 --- /dev/null +++ b/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 diff --git a/src/model/strategies/csv-output-detections-strategy/src/CsvOutputDetections.cpp b/src/model/strategies/csv-output-detections-strategy/src/CsvOutputDetections.cpp new file mode 100644 index 0000000..e95d904 --- /dev/null +++ b/src/model/strategies/csv-output-detections-strategy/src/CsvOutputDetections.cpp @@ -0,0 +1,97 @@ +// +// 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 "csvoutputdetections/CsvOutputDetections.hpp" +#include +#include +#include + +#ifdef _WIN32 + #include + #include +#else + #include + #include +#endif + +using namespace model; +using namespace osi3; + +static bool first_call = true; + +void model::CsvOutputDetections::apply(SensorData &sensor_data) { + log("Starting .csv output for detections"); + + if ((sensor_data.sensor_view_size()==0) || (!sensor_data.has_feature_data())) { + log("No sensor view or feature data received"); + return; + } + + if (!sensor_data.sensor_view(0).has_global_ground_truth()) { + log("No global ground truth received"); + 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 + time_nanos / 1000000000.0; + +if (sensor_data.sensor_view(0).radar_sensor_view().size() > 0) { + if (first_call && (sensor_data.feature_data().radar_sensor(0).detection().size() > 0)) { + #include + + write_first_line_to_CSV(file_path_detections); + first_call = false; + } + for (int sensor_idx = 0; sensor_idx < sensor_data.sensor_view(0).radar_sensor_view_size(); sensor_idx++) { + if (sensor_data.feature_data().radar_sensor(sensor_idx).detection().size() > 0) { + /// Run through all detections + size_t detection_idx = 0; + for (const auto &detection: sensor_data.feature_data().radar_sensor(sensor_idx).detection()) { + write_data_to_CSV(file_path_detections, timestamp, detection_idx, + detection.position().azimuth() * 180 / M_PI, + detection.position().elevation() * 180 / M_PI, + detection.position().distance(), detection.rcs()); + detection_idx++; + } + } else { + log("No radar detections for .csv output at timestamp " + std::to_string(timestamp)); + } + } + } else { + log("No radar sensor view"); + return; + } +} + +void CsvOutputDetections::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, detection_id, azimuth_in_deg, elevation_in_deg, distance_in_m, rcs_in_dbm²" << std::endl; + my_file.close(); +} + +void CsvOutputDetections::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) { + std::fstream my_file; + my_file.open(path, std::ios::app); + my_file << timestamp << ", " << detection_idx << ", " << azimuth_in_deg << ", " << elevation_in_deg << ", " << distance << ", " << intensity << std::endl; + my_file.close(); +} \ No newline at end of file diff --git a/src/model/strategies/csv-output-gtobjects-strategy/CMakeLists.txt b/src/model/strategies/csv-output-gtobjects-strategy/CMakeLists.txt new file mode 100644 index 0000000..86c3a5e --- /dev/null +++ b/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) + diff --git a/src/model/strategies/csv-output-gtobjects-strategy/README.md b/src/model/strategies/csv-output-gtobjects-strategy/README.md new file mode 100644 index 0000000..585c698 --- /dev/null +++ b/src/model/strategies/csv-output-gtobjects-strategy/README.md @@ -0,0 +1,7 @@ +# 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. + +NOTE: +This strategy needs transformation-functions from ../transformation-functions. \ No newline at end of file diff --git a/src/model/strategies/csv-output-gtobjects-strategy/include/csvoutputgtobjects/CsvOutputGTObjects.hpp b/src/model/strategies/csv-output-gtobjects-strategy/include/csvoutputgtobjects/CsvOutputGTObjects.hpp new file mode 100644 index 0000000..2ff221e --- /dev/null +++ b/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 +#include + +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 diff --git a/src/model/strategies/csv-output-gtobjects-strategy/include/csvoutputgtobjects/set_csv_file_path_gtobjects.cpp.in b/src/model/strategies/csv-output-gtobjects-strategy/include/csvoutputgtobjects/set_csv_file_path_gtobjects.cpp.in new file mode 100644 index 0000000..afdae70 --- /dev/null +++ b/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 diff --git a/src/model/strategies/csv-output-gtobjects-strategy/src/CsvOutputGTObjects.cpp b/src/model/strategies/csv-output-gtobjects-strategy/src/CsvOutputGTObjects.cpp new file mode 100644 index 0000000..1d2d5f3 --- /dev/null +++ b/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 +#include +#include +#include + +#ifdef _WIN32 + #include + #include +#else + #include + #include +#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 + 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_objects; + gt_objects.reserve(no_of_moving_objects + no_of_stationary_objects); + + /// Collect moving objects + for (const auto >_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 >_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 >_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(); +} \ No newline at end of file diff --git a/src/model/strategies/csv-output-logicaldetections-strategy/CMakeLists.txt b/src/model/strategies/csv-output-logicaldetections-strategy/CMakeLists.txt new file mode 100644 index 0000000..4a3878a --- /dev/null +++ b/src/model/strategies/csv-output-logicaldetections-strategy/CMakeLists.txt @@ -0,0 +1,38 @@ +cmake_minimum_required(VERSION 3.12) +project(CsvOutputLogicalDetections) + +set(CSV_LOGICALDETECTIONS_SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/src) +set(CSV_LOGICALDETECTIONS_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/csvoutputlogicaldetections/set_csv_file_path_logicaldetections.cpp.in ${CMAKE_CURRENT_BINARY_DIR}/include/csvoutputlogicaldetections/set_csv_file_path_logicaldetections.cpp) + +# TODO MODIFY THE FOLLOWING THREE LINES AS NEEDED +set(STRATEGY_ENTRY_POINT CsvOutputLogicalDetections) +set(STRATEGY_SOURCES ${CSV_LOGICALDETECTIONS_SOURCE_DIR}/CsvOutputLogicalDetections.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) + diff --git a/src/model/strategies/csv-output-logicaldetections-strategy/README.md b/src/model/strategies/csv-output-logicaldetections-strategy/README.md new file mode 100644 index 0000000..ae66b8c --- /dev/null +++ b/src/model/strategies/csv-output-logicaldetections-strategy/README.md @@ -0,0 +1,4 @@ +# LogicalDetections to .csv Output Strategy + +This strategy writes osi3::LogicalDetectionData in a csv file. +Path for the file is set via CMakeLists. \ No newline at end of file diff --git a/src/model/strategies/csv-output-logicaldetections-strategy/include/csvoutputlogicaldetections/CsvOutputLogicalDetections.hpp b/src/model/strategies/csv-output-logicaldetections-strategy/include/csvoutputlogicaldetections/CsvOutputLogicalDetections.hpp new file mode 100644 index 0000000..2fb1de2 --- /dev/null +++ b/src/model/strategies/csv-output-logicaldetections-strategy/include/csvoutputlogicaldetections/CsvOutputLogicalDetections.hpp @@ -0,0 +1,46 @@ +// +// 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_LOGICALDETECTIOONS_STRATEGY_HPP +#define CSV_OUTPUT_LOGICALDETECTIOONS_STRATEGY_HPP + +#include +#include + +using namespace osi3; + +namespace model { + + class CsvOutputLogicalDetections : public Strategy { + + using Strategy::Strategy; + + void apply(SensorData &) override; + + std::string file_path_logicaldetections; + 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 detection_idx, float x, float y, float z, float intensity); + }; + +} + +#endif //CSV_OUTPUT_LOGICALDETECTIOONS_STRATEGY_HPP diff --git a/src/model/strategies/csv-output-logicaldetections-strategy/include/csvoutputlogicaldetections/set_csv_file_path_logicaldetections.cpp.in b/src/model/strategies/csv-output-logicaldetections-strategy/include/csvoutputlogicaldetections/set_csv_file_path_logicaldetections.cpp.in new file mode 100644 index 0000000..b101606 --- /dev/null +++ b/src/model/strategies/csv-output-logicaldetections-strategy/include/csvoutputlogicaldetections/set_csv_file_path_logicaldetections.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 = "LogicalDetections.csv"; + +#if defined(_WIN32) + while ((pos = path_string.find("/")) != std::string::npos) { + path_string.replace(pos, 1, "\\"); + } + _mkdir(path_string.c_str()); + file_path_logicaldetections = path_string + "\\" + filename; +#else + mkdir(path_string.c_str(), 0777); + file_path_logicaldetections = path_string + "/" + filename; +#endif diff --git a/src/model/strategies/csv-output-logicaldetections-strategy/src/CsvOutputLogicalDetections.cpp b/src/model/strategies/csv-output-logicaldetections-strategy/src/CsvOutputLogicalDetections.cpp new file mode 100644 index 0000000..6eccdfd --- /dev/null +++ b/src/model/strategies/csv-output-logicaldetections-strategy/src/CsvOutputLogicalDetections.cpp @@ -0,0 +1,90 @@ +// +// 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/. +// + +#include "csvoutputlogicaldetections/CsvOutputLogicalDetections.hpp" +#include +#include +#include + +#ifdef _WIN32 + #include + #include +#else + #include +#endif + +using namespace model; +using namespace osi3; + +static bool first_call = true; + +void model::CsvOutputLogicalDetections::apply(SensorData &sensor_data) { + log("Starting .csv output for logical detections"); + + if ((sensor_data.sensor_view_size() == 0) || (!sensor_data.has_feature_data())) { + log("No sensor view or feature data received"); + return; + } + + if (!sensor_data.sensor_view(0).has_global_ground_truth()) { + log("No global ground truth received"); + 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 = (float)time_seconds + (double) time_nanos / 1000000000; + + if (sensor_data.logical_detection_data().logical_detection_size() == 0) { + log("No logical detections for .csv output at timestamp " + std::to_string(timestamp)); + return; + } + + /// Write header line of .csv on first call + if (first_call){ + #include + if (sensor_data.sensor_view(0).radar_sensor_view_size() > 0) { + write_first_line_to_CSV(file_path_logicaldetections); + first_call = false; + } else { + log("RadarSensorView empty at timestamp " + std::to_string(timestamp)); + } + } + + size_t logical_detection_idx = 0; + for (const auto &logical_detection: sensor_data.logical_detection_data().logical_detection()) { + write_data_to_CSV(file_path_logicaldetections, timestamp, logical_detection_idx, + float(logical_detection.position().x()), float(logical_detection.position().y()), float(logical_detection.position().z()), + float(logical_detection.intensity())); + logical_detection_idx++; + } +} + +void CsvOutputLogicalDetections::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, detection_id, x_in_m, y_in_m, z_in_m, intensity_in_%" << std::endl; + my_file.close(); +} + +void CsvOutputLogicalDetections::write_data_to_CSV(const std::string& path, double timestamp, size_t detection_idx, float x, float y, float z, float intensity) { + // Debug output of Data + std::fstream my_file; + my_file.open(path, std::ios::app); + my_file << timestamp << ", " << detection_idx << ", " << x << ", " << y << ", " << z << ", " << intensity << std::endl; + my_file.close(); +} diff --git a/src/model/strategies/csv-output-reflections-strategy/CMakeLists.txt b/src/model/strategies/csv-output-reflections-strategy/CMakeLists.txt new file mode 100644 index 0000000..c5046ff --- /dev/null +++ b/src/model/strategies/csv-output-reflections-strategy/CMakeLists.txt @@ -0,0 +1,38 @@ +cmake_minimum_required(VERSION 3.12) +project(CsvOutputReflections) + +set(CSV_REFLECTIONS_SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/src) +set(CSV_REFLECTIONS_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/csvoutputreflections/set_csv_file_path_reflections.cpp.in ${CMAKE_CURRENT_BINARY_DIR}/include/csvoutputreflections/set_csv_file_path_reflections.cpp) + +# TODO MODIFY THE FOLLOWING THREE LINES AS NEEDED +set(STRATEGY_ENTRY_POINT CsvOutputReflections) +set(STRATEGY_SOURCES ${CSV_REFLECTIONS_SOURCE_DIR}/CsvOutputReflections.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) + diff --git a/src/model/strategies/csv-output-reflections-strategy/README.md b/src/model/strategies/csv-output-reflections-strategy/README.md new file mode 100644 index 0000000..b70c48b --- /dev/null +++ b/src/model/strategies/csv-output-reflections-strategy/README.md @@ -0,0 +1,4 @@ +# Reflections to .csv Output Strategy + +This strategy writes a list of osi3::RadarReflections in a csv file. +Path for the file is set via CMakeLists. \ No newline at end of file diff --git a/src/model/strategies/csv-output-reflections-strategy/include/csvoutputreflections/CsvOutputReflections.hpp b/src/model/strategies/csv-output-reflections-strategy/include/csvoutputreflections/CsvOutputReflections.hpp new file mode 100644 index 0000000..00129c1 --- /dev/null +++ b/src/model/strategies/csv-output-reflections-strategy/include/csvoutputreflections/CsvOutputReflections.hpp @@ -0,0 +1,46 @@ +// +// 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_REFLECTIONS_STRATEGY_HPP +#define CSV_OUTPUT_REFLECTIONS_STRATEGY_HPP + +#include +#include + +using namespace osi3; + +namespace model { + + class CsvOutputReflections : public Strategy { + + using Strategy::Strategy; + + void apply(SensorData &) override; + + std::string file_path_reflections; + 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 reflection_idx, double azimuth_in_deg, double elevation_in_deg, double distance, double signal_strength); + }; + +} + +#endif //CSV_OUTPUT_REFLECTIONS_STRATEGY_HPP diff --git a/src/model/strategies/csv-output-reflections-strategy/include/csvoutputreflections/set_csv_file_path_reflections.cpp.in b/src/model/strategies/csv-output-reflections-strategy/include/csvoutputreflections/set_csv_file_path_reflections.cpp.in new file mode 100644 index 0000000..2428f63 --- /dev/null +++ b/src/model/strategies/csv-output-reflections-strategy/include/csvoutputreflections/set_csv_file_path_reflections.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; + +std::string filename = "Reflections.csv"; +path_string = path_string + "@MODEL_NAME@" + "_" + start_time; + +#if defined(_WIN32) + while ((pos = path_string.find("/")) != std::string::npos) { + path_string.replace(pos, 1, "\\"); + } + _mkdir(path_string.c_str()); + file_path_reflections = path_string + "\\" + filename; +#else + mkdir(path_string.c_str(), 0777); + file_path_reflections = path_string + "/" + filename; +#endif + + + diff --git a/src/model/strategies/csv-output-reflections-strategy/src/CsvOutputReflections.cpp b/src/model/strategies/csv-output-reflections-strategy/src/CsvOutputReflections.cpp new file mode 100644 index 0000000..ebdac39 --- /dev/null +++ b/src/model/strategies/csv-output-reflections-strategy/src/CsvOutputReflections.cpp @@ -0,0 +1,105 @@ +// +// 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 Speed_of_Light +#define Speed_of_Light 299792458 +#endif + +#ifndef _USE_MATH_DEFINES +#define _USE_MATH_DEFINES +#endif + +#include "csvoutputreflections/CsvOutputReflections.hpp" +#include +#include +#include + +#ifdef _WIN32 + #include + #include +#else + #include + #include +#endif + +using namespace model; +using namespace osi3; + +void model::CsvOutputReflections::apply(SensorData &sensor_data) { + log("Starting .csv output for reflections"); + + if (sensor_data.sensor_view_size() == 0) { + log("No sensor view received"); + return; + } + + if (!sensor_data.sensor_view(0).has_global_ground_truth()) { + log("No global ground truth received"); + 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 = (float)time_seconds + (double) time_nanos / 1000000000; + + /// Loop over all received sensor views + for (auto &sensor_view: sensor_data.sensor_view()) { + + auto no_of_radar_sensors = sensor_view.radar_sensor_view_size(); + if (profile.sensor_view_configuration.radar_sensor_view_configuration_size() != no_of_radar_sensors) + alert("Number of RadarSensorViews different to profile/SensorViewConfiguration!"); + + /// Loop over all received lidar sensors per sensor view + if (no_of_radar_sensors > 0) { + if (first_call) { + #include + write_first_line_to_CSV(file_path_reflections); + first_call = false; + } + for (auto & radar_sensor_view : sensor_view.radar_sensor_view()) { + /// Run through all rendering results + size_t radar_reflection_idx = 0; + for (auto & reflection : radar_sensor_view.reflection()) { + auto elevation_rad = reflection.source_vertical_angle(); + auto azimuth_rad = reflection.source_horizontal_angle(); + auto distance = 0.5 * reflection.time_of_flight() * Speed_of_Light; + auto signal_strength_in_dBm = reflection.signal_strength(); + + write_data_to_CSV(file_path_reflections, timestamp, radar_reflection_idx, azimuth_rad * 180 / M_PI, + elevation_rad * 180 / M_PI, distance, signal_strength_in_dBm); + radar_reflection_idx++; + } + } + } else { + log("Lidar or radar sensor view empty at timestamp " + std::to_string(timestamp)); + } + } +} + +void CsvOutputReflections::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, reflection_id, azimuth_in_deg, elevation_in_deg, distance_in_m, signal_strength_in_dBm" << std::endl; + my_file.close(); +} + +void CsvOutputReflections::write_data_to_CSV(const std::string& path, double timestamp, size_t reflection_idx, double azimuth_in_deg, double elevation_in_deg, double distance, double signal_strength) { + std::fstream my_file; + my_file.open(path, std::ios::app); + my_file << timestamp << ", " << reflection_idx << ", " << azimuth_in_deg << ", " << elevation_in_deg << ", " << distance << ", " << signal_strength << std::endl; + my_file.close(); +} diff --git a/src/model/strategies/csv_output_sequence.conf b/src/model/strategies/csv_output_sequence.conf index e69de29..b27753f 100644 --- a/src/model/strategies/csv_output_sequence.conf +++ b/src/model/strategies/csv_output_sequence.conf @@ -0,0 +1,5 @@ +csv-output-reflections-strategy +csv-output-detections-strategy +csv-output-logicaldetections-strategy +csv-output-gtobjects-strategy +csv-output-detectedobjects-strategy diff --git a/src/model/strategies/pcd-output-detections-strategy/CMakeLists.txt b/src/model/strategies/pcd-output-detections-strategy/CMakeLists.txt new file mode 100644 index 0000000..3e21ed5 --- /dev/null +++ b/src/model/strategies/pcd-output-detections-strategy/CMakeLists.txt @@ -0,0 +1,38 @@ +cmake_minimum_required(VERSION 3.12) +project(PcdOutputDetections) + +set(PCD_DETECTIONS_SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/src) +set(PCD_DETECTIONS_TARGET_DIR ${CMAKE_CURRENT_BINARY_DIR}/src) +if (WIN32) + set(PCD_PATH C:/TEMP) +elseif (${CMAKE_SYSTEM_NAME} MATCHES "Linux") + set(PCD_PATH /tmp) +endif() +configure_file(${CMAKE_CURRENT_SOURCE_DIR}/include/pcdoutputdetections/set_pcd_file_path_detections.cpp.in ${CMAKE_CURRENT_BINARY_DIR}/include/pcdoutputdetections/set_pcd_file_path_detections.cpp) + +# TODO MODIFY THE FOLLOWING THREE LINES AS NEEDED +set(STRATEGY_ENTRY_POINT PcdOutputDetections) +set(STRATEGY_SOURCES ${PCD_DETECTIONS_SOURCE_DIR}/PcdOutputDetections.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_pcd_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_pcd_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) + diff --git a/src/model/strategies/pcd-output-detections-strategy/README.md b/src/model/strategies/pcd-output-detections-strategy/README.md new file mode 100644 index 0000000..3260170 --- /dev/null +++ b/src/model/strategies/pcd-output-detections-strategy/README.md @@ -0,0 +1,4 @@ +# RadarDetections to .pcd Output Strategy + +This strategy writes osi3::RadarDetections in a pcd file. +Path for the file is set via CMakeLists. \ No newline at end of file diff --git a/src/model/strategies/pcd-output-detections-strategy/include/pcdoutputdetections/PcdOutputDetections.hpp b/src/model/strategies/pcd-output-detections-strategy/include/pcdoutputdetections/PcdOutputDetections.hpp new file mode 100644 index 0000000..52cf123 --- /dev/null +++ b/src/model/strategies/pcd-output-detections-strategy/include/pcdoutputdetections/PcdOutputDetections.hpp @@ -0,0 +1,46 @@ +// +// Copyright Institute of Automotive Engineering +// of Technical University of Darmstadt, 2021. +// 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 PCD_OUTPUT_DETECTIOONS_STRATEGY_HPP +#define PCD_OUTPUT_DETECTIOONS_STRATEGY_HPP + +#include +#include + +using namespace osi3; + +namespace model { + + class PcdOutputDetections : public Strategy { + + using Strategy::Strategy; + + void apply(SensorData &) override; + + std::string path_string; + bool first_call = true; + + public: + + private: + static void writePcdHeader(const std::string& path, const SensorData& sensor_data, const size_t& no_of_sensors); + static void write2Pcd(const std::string& path, double x, double y, double z, double intensity); + }; + +} + +#endif //PCD_OUTPUT_DETECTIOONS_STRATEGY_HPP diff --git a/src/model/strategies/pcd-output-detections-strategy/include/pcdoutputdetections/set_pcd_file_path_detections.cpp.in b/src/model/strategies/pcd-output-detections-strategy/include/pcdoutputdetections/set_pcd_file_path_detections.cpp.in new file mode 100644 index 0000000..17a2195 --- /dev/null +++ b/src/model/strategies/pcd-output-detections-strategy/include/pcdoutputdetections/set_pcd_file_path_detections.cpp.in @@ -0,0 +1,23 @@ +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); + +path_string = "@PCD_PATH@/"; +size_t pos; + +path_string = path_string + "@MODEL_NAME@" + "_" + start_time; + +#if defined(_WIN32) + while ((pos = path_string.find("/")) != std::string::npos) { + path_string.replace(pos, 1, "\\"); + } + _mkdir(path_string.c_str()); +#else + mkdir(path_string.c_str(), 0777); +#endif diff --git a/src/model/strategies/pcd-output-detections-strategy/src/PcdOutputDetections.cpp b/src/model/strategies/pcd-output-detections-strategy/src/PcdOutputDetections.cpp new file mode 100644 index 0000000..1450549 --- /dev/null +++ b/src/model/strategies/pcd-output-detections-strategy/src/PcdOutputDetections.cpp @@ -0,0 +1,121 @@ +// +// Copyright Institute of Automotive Engineering +// of Technical University of Darmstadt, 2021. +// 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/. +// + +#include "pcdoutputdetections/PcdOutputDetections.hpp" +#include +#include +#include + +#ifdef _WIN32 + #include + #include +#else + #include + #include +#endif + +using namespace model; +using namespace osi3; + +void model::PcdOutputDetections::apply(SensorData &sensor_data) { + log("Starting .pcd output for detections"); + + if(sensor_data.sensor_view().size() == 0) + 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.has_feature_data()) + return; + + size_t no_of_detections = 0; + size_t no_of_sensors = 0; + if (sensor_data.sensor_view(0).radar_sensor_view().size() > 0) { + for (int sensor_idx = 0; sensor_idx < sensor_data.feature_data().radar_sensor_size(); sensor_idx++) { + no_of_detections = no_of_detections + sensor_data.feature_data().radar_sensor(sensor_idx).detection_size(); + no_of_sensors = sensor_data.feature_data().radar_sensor_size(); + } + } + if (no_of_detections == 0) { + log("No detections for .pcd output at timestamp " + std::to_string(timestamp)); + return; + } + + if (first_call) { + #include + first_call = false; + } + + /// Header needed in every file + std::string filename = "Detections_"; + filename.append(std::to_string(timestamp)); + filename.append(".pcd"); + #if defined(_WIN32) + std::string path = path_string + "\\" + filename; + #else + std::string path = path_string + "/" + filename; + #endif + writePcdHeader(path, sensor_data, no_of_sensors); + + for (int sensor_idx = 0; sensor_idx < no_of_sensors; sensor_idx++) { + /// Run through all detections + if (sensor_data.sensor_view(0).radar_sensor_view().size() > 0) { + for (const auto &detection: sensor_data.feature_data().radar_sensor(sensor_idx).detection()) { + Vector3d point_cartesian_sensor; + point_cartesian_sensor.set_x(detection.position().distance() * cos(detection.position().elevation()) * cos(detection.position().azimuth())); + point_cartesian_sensor.set_y(detection.position().distance() * cos(detection.position().elevation()) * sin(detection.position().azimuth())); + point_cartesian_sensor.set_z(detection.position().distance() * sin(detection.position().elevation())); + auto intensity = float(detection.rcs()); + write2Pcd(path, point_cartesian_sensor.x(), point_cartesian_sensor.y(), point_cartesian_sensor.z(), intensity); + } + } + } +} + +void PcdOutputDetections::writePcdHeader(const std::string& path, const SensorData& sensor_data, const size_t& no_of_sensors) { + size_t no_of_points = 0; + for (int sensor_idx = 0; sensor_idx < no_of_sensors; sensor_idx++) { + if (sensor_data.sensor_view(0).lidar_sensor_view().size() > 0) + no_of_points += sensor_data.feature_data().lidar_sensor(sensor_idx).detection().size(); + else if (sensor_data.sensor_view(0).radar_sensor_view().size() > 0) + no_of_points += sensor_data.feature_data().radar_sensor(sensor_idx).detection().size(); + } + + std::fstream my_file; + my_file.open(path, std::ios::app); + my_file << "# .PCD v0.7 - Point Cloud Data file format" << std::endl; + my_file << "VERSION " << "0.7" << std::endl; + my_file << "FIELDS " << "x y z intensity" << std::endl; + my_file << "SIZE " << "4 4 4 4" << std::endl; + my_file << "TYPE " << "F F F F" << std::endl; + my_file << "COUNT " << "1 1 1 1" << std::endl; + my_file << "WIDTH " << no_of_points << std::endl; + my_file << "HEIGHT " << "1" << std::endl; + my_file << "VIEWPOINT " << "0 0 0 1 0 0 0" << std::endl; + my_file << "POINTS " << no_of_points << std::endl; + my_file << "DATA " << "ascii" << std::endl; + my_file.close(); +} + +void PcdOutputDetections::write2Pcd(const std::string& path, double x, double y, double z, double intensity) { + std::fstream my_file; + my_file.open(path, std::ios::app); + my_file << x << " " << y << " " << z << " " << intensity << std::endl; + my_file.close(); +} diff --git a/src/model/strategies/pcd-output-logicaldetections-strategy/CMakeLists.txt b/src/model/strategies/pcd-output-logicaldetections-strategy/CMakeLists.txt new file mode 100644 index 0000000..13880f8 --- /dev/null +++ b/src/model/strategies/pcd-output-logicaldetections-strategy/CMakeLists.txt @@ -0,0 +1,38 @@ +cmake_minimum_required(VERSION 3.12) +project(PcdOutputLogicalDetections) + +set(PCD_LOGICALDETECTIONS_SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/src) +set(PCD_LOGICALDETECTIONS_TARGET_DIR ${CMAKE_CURRENT_BINARY_DIR}/src) +if (WIN32) + set(PCD_PATH C:/TEMP) +elseif (${CMAKE_SYSTEM_NAME} MATCHES "Linux") + set(PCD_PATH /tmp) +endif() +configure_file(${CMAKE_CURRENT_SOURCE_DIR}/include/pcdoutputlogicaldetections/set_pcd_file_path_logicaldetections.cpp.in ${CMAKE_CURRENT_BINARY_DIR}/include/pcdoutputlogicaldetections/set_pcd_file_path_logicaldetections.cpp) + +# TODO MODIFY THE FOLLOWING THREE LINES AS NEEDED +set(STRATEGY_ENTRY_POINT PcdOutputLogicalDetections) +set(STRATEGY_SOURCES ${PCD_LOGICALDETECTIONS_SOURCE_DIR}/PcdOutputLogicalDetections.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_pcd_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_pcd_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) + diff --git a/src/model/strategies/pcd-output-logicaldetections-strategy/README.md b/src/model/strategies/pcd-output-logicaldetections-strategy/README.md new file mode 100644 index 0000000..8b3be6d --- /dev/null +++ b/src/model/strategies/pcd-output-logicaldetections-strategy/README.md @@ -0,0 +1,4 @@ +# LogicalDetections to .pcd Output Strategy + +This strategy writes osi3::LogicalDetectionData in a pcd file. +Path for the file is set via CMakeLists. \ No newline at end of file diff --git a/src/model/strategies/pcd-output-logicaldetections-strategy/include/pcdoutputlogicaldetections/PcdOutputLogicalDetections.hpp b/src/model/strategies/pcd-output-logicaldetections-strategy/include/pcdoutputlogicaldetections/PcdOutputLogicalDetections.hpp new file mode 100644 index 0000000..777e8b2 --- /dev/null +++ b/src/model/strategies/pcd-output-logicaldetections-strategy/include/pcdoutputlogicaldetections/PcdOutputLogicalDetections.hpp @@ -0,0 +1,46 @@ +// +// Copyright Institute of Automotive Engineering +// of Technical University of Darmstadt, 2021. +// 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 PCD_OUTPUT_LOGICALDETECTIOONS_STRATEGY_HPP +#define PCD_OUTPUT_LOGICALDETECTIOONS_STRATEGY_HPP + +#include +#include + +using namespace osi3; + +namespace model { + + class PcdOutputLogicalDetections : public Strategy { + + using Strategy::Strategy; + + void apply(SensorData &) override; + + std::string path_string; + bool first_call = true; + + public: + + private: + static void writePcdHeader(const std::string& path, const SensorData& sensor_data); + static void write2Pcd(const std::string& path, float x, float y, float z, float intensity); + }; + +} + +#endif //PCD_OUTPUT_LOGICALDETECTIOONS_STRATEGY_HPP diff --git a/src/model/strategies/pcd-output-logicaldetections-strategy/include/pcdoutputlogicaldetections/set_pcd_file_path_logicaldetections.cpp.in b/src/model/strategies/pcd-output-logicaldetections-strategy/include/pcdoutputlogicaldetections/set_pcd_file_path_logicaldetections.cpp.in new file mode 100644 index 0000000..17a2195 --- /dev/null +++ b/src/model/strategies/pcd-output-logicaldetections-strategy/include/pcdoutputlogicaldetections/set_pcd_file_path_logicaldetections.cpp.in @@ -0,0 +1,23 @@ +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); + +path_string = "@PCD_PATH@/"; +size_t pos; + +path_string = path_string + "@MODEL_NAME@" + "_" + start_time; + +#if defined(_WIN32) + while ((pos = path_string.find("/")) != std::string::npos) { + path_string.replace(pos, 1, "\\"); + } + _mkdir(path_string.c_str()); +#else + mkdir(path_string.c_str(), 0777); +#endif diff --git a/src/model/strategies/pcd-output-logicaldetections-strategy/src/PcdOutputLogicalDetections.cpp b/src/model/strategies/pcd-output-logicaldetections-strategy/src/PcdOutputLogicalDetections.cpp new file mode 100644 index 0000000..15ec990 --- /dev/null +++ b/src/model/strategies/pcd-output-logicaldetections-strategy/src/PcdOutputLogicalDetections.cpp @@ -0,0 +1,99 @@ +// +// Copyright Institute of Automotive Engineering +// of Technical University of Darmstadt, 2021. +// 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/. +// + +#include "pcdoutputlogicaldetections/PcdOutputLogicalDetections.hpp" +#include +#include +#include + +#ifdef _WIN32 + #include + #include +#else + #include +#endif + +using namespace model; +using namespace osi3; + +void model::PcdOutputLogicalDetections::apply(SensorData &sensor_data) { + log("Starting .pcd output for logical detections"); + + if ((sensor_data.sensor_view_size() == 0) || (!sensor_data.has_feature_data())) { + log("No sensor view or feature data received"); + return; + } + + if (!sensor_data.sensor_view(0).has_global_ground_truth()) { + log("No global ground truth received"); + 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.logical_detection_data().logical_detection_size() == 0) { + log("No logical detections for .csv output at timestamp " + std::to_string(timestamp)); + return; + } + + if (first_call) { + #include + first_call = false; + } + + /// Header needed in every file + std::string filename = "LogicalDetections_"; + filename.append(std::to_string(timestamp)); + filename.append(".pcd"); + #if defined(_WIN32) + std::string path = path_string + "\\" + filename; + #else + std::string path = path_string + "/" + filename; + #endif + writePcdHeader(path, sensor_data); + + for (const auto &logical_detection: sensor_data.logical_detection_data().logical_detection()) { + write2Pcd(path, float(logical_detection.position().x()), float(logical_detection.position().y()), float(logical_detection.position().z()), + float(logical_detection.intensity())); + } +} + +void PcdOutputLogicalDetections::writePcdHeader(const std::string& path, const SensorData& sensor_data) { + std::fstream my_file; + my_file.open(path, std::ios::app); + my_file << "# .PCD v0.7 - Point Cloud Data file format" << std::endl; + my_file << "VERSION " << "0.7" << std::endl; + my_file << "FIELDS " << "x y z intensity" << std::endl; + my_file << "SIZE " << "4 4 4 4" << std::endl; + my_file << "TYPE " << "F F F F" << std::endl; + my_file << "COUNT " << "1 1 1 1" << std::endl; + my_file << "WIDTH " << sensor_data.logical_detection_data().logical_detection_size() << std::endl; + my_file << "HEIGHT " << "1" << std::endl; + my_file << "VIEWPOINT " << "0 0 0 1 0 0 0" << std::endl; + my_file << "POINTS " << sensor_data.logical_detection_data().logical_detection_size() << std::endl; + my_file << "DATA " << "ascii" << std::endl; + my_file.close(); +} + +void PcdOutputLogicalDetections::write2Pcd(const std::string& path, float x, float y, float z, float intensity) { + std::fstream my_file; + my_file.open(path, std::ios::app); + my_file << x << " " << y << " " << z << " " << intensity << std::endl; + my_file.close(); +} diff --git a/src/model/strategies/pcd_output_sequence.conf b/src/model/strategies/pcd_output_sequence.conf index e69de29..ccb0c10 100644 --- a/src/model/strategies/pcd_output_sequence.conf +++ b/src/model/strategies/pcd_output_sequence.conf @@ -0,0 +1,2 @@ +pcd-output-detections-strategy +pcd-output-logicaldetections-strategy diff --git a/src/model/strategies/radar-detection-sensing-strategy/src/DetectionSensing.cpp b/src/model/strategies/radar-detection-sensing-strategy/src/DetectionSensing.cpp index 8993f5a..7cce91d 100644 --- a/src/model/strategies/radar-detection-sensing-strategy/src/DetectionSensing.cpp +++ b/src/model/strategies/radar-detection-sensing-strategy/src/DetectionSensing.cpp @@ -294,7 +294,7 @@ void DetectionSensing::apply(SensorData &sensor_data) { /// CFAR Peakdetection for (size_t r = 0; r < number_range_bin; ++r) { - float doppler_vec[number_doppler_bin]; + float* doppler_vec = new float[number_doppler_bin]; memset(doppler_vec, 0, sizeof(doppler_vec)); for (size_t d = 0; d < number_doppler_bin; ++d) { for (size_t b = 0; b < number_azimuth_bin; ++b) { diff --git a/src/model/strategies/ros-output-detectedobjects-strategy/CMakeLists.txt b/src/model/strategies/ros-output-detectedobjects-strategy/CMakeLists.txt new file mode 100644 index 0000000..2a68985 --- /dev/null +++ b/src/model/strategies/ros-output-detectedobjects-strategy/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.12) +project(ros_detectedobjects) + +# TODO MODIFY THE FOLLOWING LINES AS NEEDED +set(STRATEGY_ENTRY_POINT ros_detectedobjects) +set(STRATEGY_SOURCES src/ros_detectedobjects.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}) diff --git a/src/model/strategies/ros-output-detectedobjects-strategy/README.md b/src/model/strategies/ros-output-detectedobjects-strategy/README.md new file mode 100644 index 0000000..88bbd4f --- /dev/null +++ b/src/model/strategies/ros-output-detectedobjects-strategy/README.md @@ -0,0 +1,4 @@ +# Detected Objects to ROS Output Strategy + +This strategy outputs osi3::DetectedMovingObjects as ROS Marker messages. + diff --git a/src/model/strategies/ros-output-detectedobjects-strategy/include/ros_detectedobjects/ros_detectedobjects.hpp b/src/model/strategies/ros-output-detectedobjects-strategy/include/ros_detectedobjects/ros_detectedobjects.hpp new file mode 100644 index 0000000..ea58c67 --- /dev/null +++ b/src/model/strategies/ros-output-detectedobjects-strategy/include/ros_detectedobjects/ros_detectedobjects.hpp @@ -0,0 +1,63 @@ +// +// 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 ROS_DETECTEDOBJECTS_HPP +#define ROS_DETECTEDOBJECTS_HPP + +#include +#include +#include +#include +#include +#include + +using namespace model; +using namespace osi3; + +namespace detectedobjects { + class WorkerMarker final { + public: + WorkerMarker(const std::string &topic, std::string frame_id); + + void inject(SensorData &in, const Log &log); + + private: + const std::string frame_id; + ros::NodeHandle node; + ros::Publisher publisher; + + static visualization_msgs::Marker set_marker(const Vector3d &position, const Orientation3d &orientation, const Dimension3d &dimension, const uint64_t id, std_msgs::ColorRGBA color, std::string frame, ros::Time time); + static std_msgs::ColorRGBA set_color(float r, float g, float b, float a); + }; +} + +namespace model { + class ros_detectedobjects : public Strategy { + public: + ros_detectedobjects(const Profile &profile, const Log &log, const Alert &alert); + using Strategy::Strategy; + + void apply(SensorData &) override; + + private: + std::unique_ptr worker = nullptr; + }; +} + + + +#endif //ROS_DETECTEDOBJECTS_HPP diff --git a/src/model/strategies/ros-output-detectedobjects-strategy/package.xml b/src/model/strategies/ros-output-detectedobjects-strategy/package.xml new file mode 100644 index 0000000..b0aecbb --- /dev/null +++ b/src/model/strategies/ros-output-detectedobjects-strategy/package.xml @@ -0,0 +1,20 @@ + + + ros_detectedobjects + 1.0.0 + Forward detected moving objects data to ROS + Clemens Linnhoff + EUPL-1.2 + https://git.rwth-aachen.de/fzd/sensormodeling/fmi-and-osi/visualization-and-output/ros-bridge-gt-objects-strategy + Clemens Linnhoff + + catkin + roscpp + roscpp + roscpp + std_msgs + sensor_msgs + tf + visualization_msgs + diff --git a/src/model/strategies/ros-output-detectedobjects-strategy/src/ros_detectedobjects.cpp b/src/model/strategies/ros-output-detectedobjects-strategy/src/ros_detectedobjects.cpp new file mode 100644 index 0000000..a662c5e --- /dev/null +++ b/src/model/strategies/ros-output-detectedobjects-strategy/src/ros_detectedobjects.cpp @@ -0,0 +1,139 @@ +// +// 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/. +// +#include "ros_detectedobjects/ros_detectedobjects.hpp" +#include +#include +#include + +using namespace model; +using namespace osi3; + +void ros_detectedobjects::apply(SensorData &sensor_data) { + log("Starting ROS output for detected objects"); + + ros::param::set("/use_sim_time", true); + + if ((sensor_data.sensor_view_size()==0) || (!sensor_data.has_feature_data())) { + log("No sensor view or feature data received"); + return; + } + + if (!sensor_data.sensor_view(0).has_global_ground_truth()) { + log("No global ground truth received"); + 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; + + if (sensor_data.moving_object_size() == 0 && sensor_data.stationary_object_size() == 0) { + log("No detected objects for ROS output at timestamp " + std::to_string(timestamp)); + } + + worker->inject(sensor_data, log); +} + +ros_detectedobjects::ros_detectedobjects(const Profile &profile, const Log &log, const Alert &alert): model::Strategy(profile, log, alert) { + + auto remapping = std::map(); + remapping.emplace("__master", "http://localhost:11311"); + ros::init(remapping, "sensor_model_fmu"); + worker = std::make_unique("tracker", "base_link"); +} + +//Objects +detectedobjects::WorkerMarker::WorkerMarker(const std::string& topic, std::string frame_id) : publisher(node.advertise(topic, 1)), frame_id(std::move(frame_id)) {} +void detectedobjects::WorkerMarker::inject(SensorData &sensor_data, const Log &log) { +// auto time = ros::Time::now(); + + + auto sim_seconds = uint32_t(sensor_data.sensor_view(0).global_ground_truth().timestamp().seconds()); + auto sim_nanos = uint32_t(sensor_data.sensor_view(0).global_ground_truth().timestamp().nanos()); + auto sim_time = ros::Time(sim_seconds, sim_nanos); + auto sim_time_behind = ros::Time(sim_seconds, sim_nanos + 500); + + //Detected Objects + visualization_msgs::MarkerArray marker_array_tracker; + std_msgs::ColorRGBA marker_color; + if (sensor_data.moving_object_size() > 0) { + for (const auto &moving_object: sensor_data.moving_object()) { + marker_color = set_color(0.0, 0.0, 1.0, float(0.6 * moving_object.header().existence_probability())); + visualization_msgs::Marker marker = set_marker(moving_object.base().position(), + moving_object.base().orientation(), + moving_object.base().dimension(), + moving_object.header().tracking_id().value(), + marker_color, + frame_id, sim_time_behind); + marker_array_tracker.markers.push_back(marker); + } + } + + if (sensor_data.stationary_object_size() > 0) { + for (const auto &stationary_object: sensor_data.stationary_object()) { + marker_color = set_color(0.0, 0.0, 1.0, float(0.5 * stationary_object.header().existence_probability())); + visualization_msgs::Marker marker = set_marker(stationary_object.base().position(), + stationary_object.base().orientation(), + stationary_object.base().dimension(), + stationary_object.header().tracking_id().value(), + marker_color, + frame_id, sim_time_behind); + marker_array_tracker.markers.push_back(marker); + } + } + + publisher.publish(marker_array_tracker); +} + + +visualization_msgs::Marker detectedobjects::WorkerMarker::set_marker(const Vector3d &position, const Orientation3d &orientation, const Dimension3d &dimension, const uint64_t id, std_msgs::ColorRGBA color, std::string frame, ros::Time time) { + visualization_msgs::Marker marker; + marker.header.frame_id = std::move(frame); + marker.header.stamp = time; + marker.id = (int)id; + marker.type = visualization_msgs::Marker::CUBE; + marker.action = visualization_msgs::Marker::ADD; + marker.pose.position.x = position.x(); + marker.pose.position.y = position.y(); + marker.pose.position.z = position.z(); + tf::Quaternion orientation_q; + orientation_q.setRPY(orientation.roll(), + orientation.pitch(), + orientation.yaw()); + marker.pose.orientation.x = orientation_q.x(); + marker.pose.orientation.y = orientation_q.y(); + marker.pose.orientation.z = orientation_q.z(); + marker.pose.orientation.w = orientation_q.w(); + marker.scale.x = dimension.length(); + marker.scale.y = dimension.width(); + marker.scale.z = dimension.height(); + marker.color = color; + marker.lifetime = ros::Duration(0.7); + marker.frame_locked = false; + return marker; +} + +std_msgs::ColorRGBA detectedobjects::WorkerMarker::set_color(float r, float g, float b, float a) { + std_msgs::ColorRGBA color; + color.r = r; + color.g = g; + color.b = b; + color.a = a; + return color; +} \ No newline at end of file diff --git a/src/model/strategies/ros-output-detections-strategy/CMakeLists.txt b/src/model/strategies/ros-output-detections-strategy/CMakeLists.txt new file mode 100644 index 0000000..e217729 --- /dev/null +++ b/src/model/strategies/ros-output-detections-strategy/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.12) +project(ros_detections) + +# TODO MODIFY THE FOLLOWING LINES AS NEEDED +set(STRATEGY_ENTRY_POINT ros_detections) +set(STRATEGY_SOURCES src/ros_detections.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}) diff --git a/src/model/strategies/ros-output-detections-strategy/README.md b/src/model/strategies/ros-output-detections-strategy/README.md new file mode 100644 index 0000000..48300b5 --- /dev/null +++ b/src/model/strategies/ros-output-detections-strategy/README.md @@ -0,0 +1,4 @@ +# Detections to ROS Output Strategy + +This strategy outputs osi3::RadarDetectionData and osi3::RadarDetectionData as ROS PointCloud2 messages. + diff --git a/src/model/strategies/ros-output-detections-strategy/include/ros_detections/ros_detections.hpp b/src/model/strategies/ros-output-detections-strategy/include/ros_detections/ros_detections.hpp new file mode 100644 index 0000000..83d1c06 --- /dev/null +++ b/src/model/strategies/ros-output-detections-strategy/include/ros_detections/ros_detections.hpp @@ -0,0 +1,59 @@ +// +// Copyright Institute of Automotive Engineering +// of Technical University of Darmstadt 2021. +// 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 ROS_DETECTIONS_HPP +#define ROS_DETECTIONS_HPP + +#include +#include +#include +#include +#include + +using namespace model; +using namespace osi3; + +namespace detections { + class WorkerPCL final { + public: + WorkerPCL(const std::string &topic, std::string frame_id); + void injectRadar(SensorData &sensor_data, int sensor_no, const Log &log); + + private: + const std::string frame_id; + ros::NodeHandle node; + ros::Publisher publisher; + }; +} + + +namespace model { + class ros_detections : public Strategy { + public: + ros_detections(const Profile &profile, const Log &log, const Alert &alert); + using Strategy::Strategy; + + void apply(SensorData &) override; + + private: + std::unique_ptr worker_pcl = nullptr; + }; +} + +std_msgs::ColorRGBA set_color(float r, float g, float b, float a); + +#endif //ROS_DETECTIONS_HPP diff --git a/src/model/strategies/ros-output-detections-strategy/package.xml b/src/model/strategies/ros-output-detections-strategy/package.xml new file mode 100644 index 0000000..50679d3 --- /dev/null +++ b/src/model/strategies/ros-output-detections-strategy/package.xml @@ -0,0 +1,20 @@ + + + ros_detections + 1.0.0 + Forward lidar detection data into ROS + Clemens Linnhoff + EUPL-1.2 + https://git.rwth-aachen.de/fzd/sensormodeling/fmi-and-osi/visualization-and-output/ros-bridge-detections-strategy + Clemens Linnhoff + + catkin + roscpp + roscpp + roscpp + std_msgs + sensor_msgs + tf + visualization_msgs + diff --git a/src/model/strategies/ros-output-detections-strategy/src/ros_detections.cpp b/src/model/strategies/ros-output-detections-strategy/src/ros_detections.cpp new file mode 100644 index 0000000..af5e5fb --- /dev/null +++ b/src/model/strategies/ros-output-detections-strategy/src/ros_detections.cpp @@ -0,0 +1,107 @@ +// +// 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/. +// + +#include "ros_detections/ros_detections.hpp" +#include +#include +#include +#include + +#ifdef _WIN32 +#include +#else + +#include + +#endif + +#define _USE_MATH_DEFINES + +using namespace model; +using namespace osi3; + +void ros_detections::apply(SensorData &sensor_data) { + log("Starting ROS output for detections"); + + ros::param::set("/use_sim_time", true); + + if ((sensor_data.sensor_view_size() == 0) || (!sensor_data.has_feature_data())) { + log("No sensor view or feature data received"); + return; + } + + if (!sensor_data.sensor_view(0).has_global_ground_truth()) { + log("No global ground truth received"); + return; + } + + if (sensor_data.feature_data().radar_sensor().size() > 0) { + for (int sensor_no = 0; sensor_no < sensor_data.feature_data().radar_sensor_size(); sensor_no++) { + worker_pcl = std::make_unique("detections_" + std::to_string(sensor_no), "sensor_" + std::to_string(sensor_no)); + worker_pcl->injectRadar(sensor_data, sensor_no, log); + } + } else { + log("No radar sensor in feature data"); + return; + } +} + +ros_detections::ros_detections(const Profile &profile, const Log &log, const Alert &alert) : model::Strategy(profile, log, alert) { + auto remapping = std::map(); + remapping.emplace("__master", "http://localhost:11311"); + ros::init(remapping, "sensor_model_fmu"); +} + +detections::WorkerPCL::WorkerPCL(const std::string &topic, std::string frame_id) : publisher(node.advertise>(topic, 1)), frame_id(std::move(frame_id)) {} + +void detections::WorkerPCL::injectRadar(SensorData &sensor_data, int sensor_no, const Log &log) { + const auto &radar_sensor = sensor_data.feature_data().radar_sensor(sensor_no); + + auto sim_seconds = uint32_t(sensor_data.sensor_view(0).global_ground_truth().timestamp().seconds()); + auto sim_nanos = uint32_t(sensor_data.sensor_view(0).global_ground_truth().timestamp().nanos()); + auto sim_time = ros::Time(sim_seconds, sim_nanos); + auto sim_time_behind = ros::Time(sim_seconds, sim_nanos + 500); + + auto no_of_detections = radar_sensor.detection_size(); + + if (no_of_detections == 0) { + auto timestamp = (double) sim_seconds + (double) sim_nanos / 1000000000; + log("No detections from sensor " + std::to_string(sensor_no) + " for ROS output at timestamp " + std::to_string(timestamp)); + return; + } + + pcl::PointCloud::Ptr cloud_tmp(new pcl::PointCloud()); + pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); + + pcl_conversions::toPCL(sim_time_behind, cloud_tmp->header.stamp); + cloud_tmp->header.frame_id = frame_id; + + /// Run through all detections + + for (const auto &detection: radar_sensor.detection()) { + auto x = float(detection.position().distance() * cos(detection.position().azimuth()) * cos(detection.position().elevation())); + auto y = float(detection.position().distance() * sin(detection.position().azimuth()) * cos(detection.position().elevation())); + auto z = float(detection.position().distance() * sin(detection.position().elevation())); + cloud_tmp->points.emplace_back(pcl::PointXYZ(x, y, z)); + } + pcl::copyPointCloud(*cloud_tmp, *cloud); + /// Add intensity to all detections + for (int detection_idx = 0; detection_idx < radar_sensor.detection_size(); detection_idx++) { + cloud->points[detection_idx].intensity = float(radar_sensor.detection(detection_idx).rcs()); + } + publisher.publish(cloud); +} \ No newline at end of file diff --git a/src/model/strategies/ros-output-gtobjects-strategy/CMakeLists.txt b/src/model/strategies/ros-output-gtobjects-strategy/CMakeLists.txt new file mode 100644 index 0000000..68123f2 --- /dev/null +++ b/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}) diff --git a/src/model/strategies/ros-output-gtobjects-strategy/README.md b/src/model/strategies/ros-output-gtobjects-strategy/README.md new file mode 100644 index 0000000..6ee5852 --- /dev/null +++ b/src/model/strategies/ros-output-gtobjects-strategy/README.md @@ -0,0 +1,14 @@ +# 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. +It can be used within the [Modular OSMP Framework](https://gitlab.com/tuda-fzd/perception-sensor-modeling/modular-osmp-framework) or any framework compliant model. Using the ROS tool rviz is a convenient way to visualize OSI sensor view input and sensor data output. There are several other [output strategies](https://gitlab.com/tuda-fzd/perception-sensor-modeling/output-strategies) available for different kinds of OSI data. + +## Usage +The easiest way to use the strategy within the [Modular OSMP Framework](https://gitlab.com/tuda-fzd/perception-sensor-modeling/modular-osmp-framework) is by including it as a git submodule in *src/model/strategies/*. Navigate to the git repository of your framework or model. + +Now you need to add the new strategy to your sequence.conf. In this case of a ROS output strategy, 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/. diff --git a/src/model/strategies/ros-output-gtobjects-strategy/include/ros_gt_objects/ros_gt_objects.hpp b/src/model/strategies/ros-output-gtobjects-strategy/include/ros_gt_objects/ros_gt_objects.hpp new file mode 100644 index 0000000..bc903a5 --- /dev/null +++ b/src/model/strategies/ros-output-gtobjects-strategy/include/ros_gt_objects/ros_gt_objects.hpp @@ -0,0 +1,64 @@ +// +// 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 ROS_GT_OBJECTS_HPP +#define ROS_GT_OBJECTS_HPP + +#include +#include +#include +#include +#include +#include + +using namespace model; +using namespace osi3; + +namespace ground_truth { + class WorkerMarker final { + public: + WorkerMarker(const std::string &topic, std::string frame_id); + + void inject(SensorData &sensor_data, const Profile &profile, const Log &log, const Alert &alert); + + private: + const std::string frame_id; + ros::NodeHandle node; + ros::Publisher publisher; + tf::TransformBroadcaster transform_broadcaster; + + static visualization_msgs::Marker set_marker(const Vector3d &position, const Orientation3d &orientation, const Dimension3d &dimension, const uint64_t id, std_msgs::ColorRGBA color, std::string frame, ros::Time time); + static std_msgs::ColorRGBA set_color(float r, float g, float b, float a); + }; +} + +namespace model { + class ros_gt_objects : public Strategy { + public: + ros_gt_objects(const Profile &profile, const Log &log, const Alert &alert); + using Strategy::Strategy; + + void apply(SensorData &) override; + + private: + std::unique_ptr worker_gt = nullptr; + }; +} + + + +#endif //ROS_GT_OBJECTS_HPP diff --git a/src/model/strategies/ros-output-gtobjects-strategy/package.xml b/src/model/strategies/ros-output-gtobjects-strategy/package.xml new file mode 100644 index 0000000..73a3754 --- /dev/null +++ b/src/model/strategies/ros-output-gtobjects-strategy/package.xml @@ -0,0 +1,20 @@ + + + ros_gt_objects + 1.0.0 + Forward ground truth object data to ROS + Clemens Linnhoff + EUPL-1.2 + https://git.rwth-aachen.de/fzd/sensormodeling/fmi-and-osi/visualization-and-output/ros-bridge-gt-objects-strategy + Clemens Linnhoff + + catkin + roscpp + roscpp + roscpp + std_msgs + sensor_msgs + tf + visualization_msgs + diff --git a/src/model/strategies/ros-output-gtobjects-strategy/src/ros_gt_objects.cpp b/src/model/strategies/ros-output-gtobjects-strategy/src/ros_gt_objects.cpp new file mode 100644 index 0000000..9e85ea9 --- /dev/null +++ b/src/model/strategies/ros-output-gtobjects-strategy/src/ros_gt_objects.cpp @@ -0,0 +1,208 @@ +// +// Copyright Institute of Automotive Engineering +// of Technical University of Darmstadt 2021. +// 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/. +// + +#include "ros_gt_objects/ros_gt_objects.hpp" +#include "visualization_msgs/MarkerArray.h" +#include "std_msgs/String.h" +#include +#include + +using namespace model; +using namespace osi3; + +void ros_gt_objects::apply(SensorData &sensor_data) { + log("Starting ROS output for GT objects"); + + ros::param::set("/use_sim_time", true); + + if (sensor_data.sensor_view_size() == 0) { + log("No sensor view received"); + return; + } + + if (!sensor_data.sensor_view(0).has_global_ground_truth()) { + log("No global ground truth received"); + 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).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 ROS output at timestamp " + std::to_string(timestamp)); + return; + } + + worker_gt->inject(sensor_data, profile, log, alert); +} + +ros_gt_objects::ros_gt_objects(const Profile &profile, const Log &log, const Alert &alert) : model::Strategy(profile, + log, + alert) { + auto remapping = std::map(); + remapping.emplace("__master", "http://localhost:11311"); + ros::init(remapping, "sensor_model_fmu"); + worker_gt = std::make_unique("ground_truth", "world"); +} + +/// Objects +ground_truth::WorkerMarker::WorkerMarker(const std::string &topic, std::string frame_id) : publisher( + node.advertise(topic, 1)), frame_id(std::move(frame_id)) {} + +void ground_truth::WorkerMarker::inject(SensorData &sensor_data, const Profile &profile, const Log &log, + const Alert &alert) { +// auto time = ros::Time::now(); + auto sim_seconds = uint32_t(sensor_data.sensor_view(0).global_ground_truth().timestamp().seconds()); + auto sim_nanos = uint32_t(sensor_data.sensor_view(0).global_ground_truth().timestamp().nanos()); + auto sim_time = ros::Time(sim_seconds, sim_nanos); + auto sim_time_behind = ros::Time(sim_seconds, sim_nanos + 500); + + /// Global Ground Truth + visualization_msgs::MarkerArray marker_array; + const MovingObject *ego_vehicle; + + /// Moving objects + std_msgs::ColorRGBA color_gt_moving; + for (const MovingObject >_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()) { + ego_vehicle = >_moving_object; + color_gt_moving = set_color(1.0, 0.0, 0.0, 0.3); + } else { + color_gt_moving = set_color(1.0, 1.0, 1.0, 0.3); + } + visualization_msgs::Marker marker = set_marker(gt_moving_object.base().position(), + gt_moving_object.base().orientation(), + gt_moving_object.base().dimension(), + gt_moving_object.id().value(), + color_gt_moving, + frame_id, sim_time_behind); + marker_array.markers.push_back(marker); + } + /// Stationary objects + std_msgs::ColorRGBA color_gt_stationary = set_color(1.0, 1.0, 1.0, 0.1); + for (const StationaryObject >_stationary_object: sensor_data.sensor_view( + 0).global_ground_truth().stationary_object()) { + visualization_msgs::Marker marker = set_marker(gt_stationary_object.base().position(), + gt_stationary_object.base().orientation(), + gt_stationary_object.base().dimension(), + gt_stationary_object.id().value(), + color_gt_stationary, + frame_id, sim_time_behind); + marker_array.markers.push_back(marker); + } + + /// Transformations + tf::Transform transform; + tf::Quaternion q; + /// Transform Bounding Box Center to world + if (ego_vehicle->has_base()) { + transform.setOrigin(tf::Vector3(ego_vehicle->base().position().x(), ego_vehicle->base().position().y(), + ego_vehicle->base().position().z())); + q.setRPY(ego_vehicle->base().orientation().roll(), ego_vehicle->base().orientation().pitch(), + ego_vehicle->base().orientation().yaw()); + transform.setRotation(q); + transform_broadcaster.sendTransform(tf::StampedTransform(transform, sim_time, "world", "bb_center")); + } else { + log("Ego vehicle has no base!"); + } + + /// Transform Bounding Box Center to center of rear axle + if (ego_vehicle->has_vehicle_attributes()) { + transform.setOrigin(tf::Vector3(ego_vehicle->vehicle_attributes().bbcenter_to_rear().x(), + ego_vehicle->vehicle_attributes().bbcenter_to_rear().y(), + ego_vehicle->vehicle_attributes().bbcenter_to_rear().z())); + q.setRPY(0, 0, 0); + transform.setRotation(q); + transform_broadcaster.sendTransform(tf::StampedTransform(transform, sim_time, "bb_center", "base_link")); + } else { + log("Ego vehicle has no vehicle_attributes!"); + } + + for (auto &sensor_view: sensor_data.sensor_view()) { + /// Transform center of rear axle to sensor('s) origin + size_t radar_sensor_no = 0; + for (auto &radar_sensor_view: sensor_view.radar_sensor_view()) { + transform.setOrigin(tf::Vector3(radar_sensor_view.view_configuration().mounting_position().position().x(), + radar_sensor_view.view_configuration().mounting_position().position().y(), + radar_sensor_view.view_configuration().mounting_position().position().z())); + q.setRPY(radar_sensor_view.view_configuration().mounting_position().orientation().roll(), + radar_sensor_view.view_configuration().mounting_position().orientation().pitch(), + radar_sensor_view.view_configuration().mounting_position().orientation().yaw()); + transform.setRotation(q); + transform_broadcaster.sendTransform( + tf::StampedTransform(transform, sim_time, "base_link", "sensor_" + std::to_string(radar_sensor_no))); + radar_sensor_no++; + } + size_t lidar_sensor_no = 0; + for (auto &lidar_sensor_view: sensor_view.lidar_sensor_view()) { + transform.setOrigin(tf::Vector3(lidar_sensor_view.view_configuration().mounting_position().position().x(), + lidar_sensor_view.view_configuration().mounting_position().position().y(), + lidar_sensor_view.view_configuration().mounting_position().position().z())); + q.setRPY(lidar_sensor_view.view_configuration().mounting_position().orientation().roll(), + lidar_sensor_view.view_configuration().mounting_position().orientation().pitch(), + lidar_sensor_view.view_configuration().mounting_position().orientation().yaw()); + transform.setRotation(q); + transform_broadcaster.sendTransform( + tf::StampedTransform(transform, sim_time, "base_link", "sensor_" + std::to_string(lidar_sensor_no))); + lidar_sensor_no++; + } + + publisher.publish(marker_array); + } +} + +visualization_msgs::Marker +ground_truth::WorkerMarker::set_marker(const Vector3d &position, const Orientation3d &orientation, + const Dimension3d &dimension, const size_t id, std_msgs::ColorRGBA color, + std::string frame, ros::Time time) { + visualization_msgs::Marker marker; + marker.header.frame_id = std::move(frame); + marker.header.stamp = time; + marker.id = (int)id; + marker.type = visualization_msgs::Marker::CUBE; + marker.action = visualization_msgs::Marker::ADD; + marker.pose.position.x = position.x(); + marker.pose.position.y = position.y(); + marker.pose.position.z = position.z(); + tf::Quaternion orientation_q; + orientation_q.setRPY(orientation.roll(), + orientation.pitch(), + orientation.yaw()); + marker.pose.orientation.x = orientation_q.x(); + marker.pose.orientation.y = orientation_q.y(); + marker.pose.orientation.z = orientation_q.z(); + marker.pose.orientation.w = orientation_q.w(); + marker.scale.x = dimension.length(); + marker.scale.y = dimension.width(); + marker.scale.z = dimension.height(); + marker.color = color; + marker.lifetime = ros::Duration(1); + marker.frame_locked = false; + return marker; +} + +std_msgs::ColorRGBA ground_truth::WorkerMarker::set_color(float r, float g, float b, float a) { + std_msgs::ColorRGBA color; + color.r = r; + color.g = g; + color.b = b; + color.a = a; + return color; +} \ No newline at end of file diff --git a/src/model/strategies/ros-output-logicaldetections-strategy/CMakeLists.txt b/src/model/strategies/ros-output-logicaldetections-strategy/CMakeLists.txt new file mode 100644 index 0000000..fac1110 --- /dev/null +++ b/src/model/strategies/ros-output-logicaldetections-strategy/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.12) +project(ros_logicaldetections) + +# TODO MODIFY THE FOLLOWING LINES AS NEEDED +set(STRATEGY_ENTRY_POINT ros_logicaldetections) +set(STRATEGY_SOURCES src/ros_logicaldetections.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}) diff --git a/src/model/strategies/ros-output-logicaldetections-strategy/README.md b/src/model/strategies/ros-output-logicaldetections-strategy/README.md new file mode 100644 index 0000000..747c55c --- /dev/null +++ b/src/model/strategies/ros-output-logicaldetections-strategy/README.md @@ -0,0 +1,3 @@ +# ROS Bridge LogicalDetections Strategy + +This strategy outputs osi3::LogicalDetectionData as ROS PointCloud2 messages. diff --git a/src/model/strategies/ros-output-logicaldetections-strategy/include/ros_logicaldetections/ros_logicaldetections.hpp b/src/model/strategies/ros-output-logicaldetections-strategy/include/ros_logicaldetections/ros_logicaldetections.hpp new file mode 100644 index 0000000..83094fc --- /dev/null +++ b/src/model/strategies/ros-output-logicaldetections-strategy/include/ros_logicaldetections/ros_logicaldetections.hpp @@ -0,0 +1,63 @@ +// +// 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 ROS_LOGICALDETECTIONS_HPP +#define ROS_LOGICALDETECTIONS_HPP + +#include +#include +#include +#include +#include +#include +#include + +using namespace model; +using namespace osi3; + +namespace logicaldetections { + class WorkerPCL final { + public: + WorkerPCL(const std::string &topic, std::string frame_id); + void inject(SensorData &sensor_data, const Log &log); + + private: + const std::string frame_id; + ros::NodeHandle node; + ros::Publisher publisher; + tf::TransformListener listener; + tf::TransformBroadcaster transform_broadcaster; + }; +} + + +namespace model { + class ros_logicaldetections : public Strategy { + public: + ros_logicaldetections(const Profile &profile, const Log &log, const Alert &alert); + using Strategy::Strategy; + + void apply(SensorData &) override; + + private: + std::unique_ptr worker_pcl = nullptr; + }; +} + +std_msgs::ColorRGBA set_color(float r, float g, float b, float a); + +#endif //ROS_LOGICALDETECTIONS_HPP diff --git a/src/model/strategies/ros-output-logicaldetections-strategy/src/ros_logicaldetections.cpp b/src/model/strategies/ros-output-logicaldetections-strategy/src/ros_logicaldetections.cpp new file mode 100644 index 0000000..ba49453 --- /dev/null +++ b/src/model/strategies/ros-output-logicaldetections-strategy/src/ros_logicaldetections.cpp @@ -0,0 +1,86 @@ +// +// 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/. +// + +#include "ros_logicaldetections/ros_logicaldetections.hpp" +#include +#include +#include +#include + +#define _USE_MATH_DEFINES + +using namespace model; +using namespace osi3; + +void ros_logicaldetections::apply(SensorData &sensor_data) { + log("Starting ROS output for logical detections"); + + ros::param::set("/use_sim_time", true); + + if ((sensor_data.sensor_view_size()==0) || (!sensor_data.has_logical_detection_data())) { + log("No sensor view or feature data received"); + return; + } + + if (!sensor_data.sensor_view(0).has_global_ground_truth()) { + log("No global ground truth received"); + return; + } + + if (sensor_data.logical_detection_data().logical_detection().size() == 0) { + log("No logical detections for ros output"); + return; + } + + worker_pcl->inject(sensor_data, log); +} + +ros_logicaldetections::ros_logicaldetections(const Profile &profile, const Log &log, const Alert &alert): model::Strategy(profile, log, alert) { + + auto remapping = std::map(); + remapping.emplace("__master", "http://localhost:11311"); + ros::init(remapping, "sensor_model_fmu"); + worker_pcl = std::make_unique("logical_detections", "base_link"); +} + +logicaldetections::WorkerPCL::WorkerPCL(const std::string& topic, std::string frame_id) : publisher(node.advertise>(topic, 1)) , frame_id(std::move(frame_id)) {} + +void logicaldetections::WorkerPCL::inject(SensorData &sensor_data, const Log &log) { + pcl::PointCloud::Ptr cloud_tmp(new pcl::PointCloud()); + pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); +// auto time = ros::Time::now(); + auto sim_seconds = uint32_t(sensor_data.sensor_view(0).global_ground_truth().timestamp().seconds()); + auto sim_nanos = uint32_t(sensor_data.sensor_view(0).global_ground_truth().timestamp().nanos()); + auto sim_time = ros::Time(sim_seconds, sim_nanos); + auto sim_time_behind = ros::Time(sim_seconds, sim_nanos + 500); + pcl_conversions::toPCL(sim_time_behind, cloud_tmp->header.stamp); + cloud_tmp->header.frame_id = frame_id; + + /// Run through all logical detections + for (const auto& logical_detection : sensor_data.logical_detection_data().logical_detection()) { + auto x = float(logical_detection.position().x()); + auto y = float(logical_detection.position().y()); + auto z = float(logical_detection.position().z()); + cloud_tmp->points.emplace_back(pcl::PointXYZ(x, y, z)); + } + pcl::copyPointCloud(*cloud_tmp, *cloud); + // add intensity to all detections + for (int logical_detection_idx = 0; logical_detection_idx < sensor_data.logical_detection_data().logical_detection().size(); logical_detection_idx++) { + cloud->points[logical_detection_idx].intensity = float(sensor_data.logical_detection_data().logical_detection(logical_detection_idx).intensity()); + } + publisher.publish(cloud); +} diff --git a/src/model/strategies/ros-output-reflections-strategy/CMakeLists.txt b/src/model/strategies/ros-output-reflections-strategy/CMakeLists.txt new file mode 100644 index 0000000..5df2cc4 --- /dev/null +++ b/src/model/strategies/ros-output-reflections-strategy/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 3.12) +project(ros_reflections) + +catkin_package(CATKIN_DEPENDS roscpp std_msgs sensor_msgs tf) + +set(CMAKE_INSTALL_PREFIX ${CMAKE_BINARY_DIR}/catkin) + +# TODO MODIFY THE FOLLOWING LINES AS NEEDED +set(STRATEGY_ENTRY_POINT ros_reflections) +set(STRATEGY_SOURCES src/ros_reflections.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}) diff --git a/src/model/strategies/ros-output-reflections-strategy/README.md b/src/model/strategies/ros-output-reflections-strategy/README.md new file mode 100644 index 0000000..218dfa9 --- /dev/null +++ b/src/model/strategies/ros-output-reflections-strategy/README.md @@ -0,0 +1,4 @@ +# Reflections to ROS Output Strategy + +This strategy outputs osi3::RadarReflections as ROS PointCloud2 messages. + diff --git a/src/model/strategies/ros-output-reflections-strategy/include/ros_reflections/ros_reflections.hpp b/src/model/strategies/ros-output-reflections-strategy/include/ros_reflections/ros_reflections.hpp new file mode 100644 index 0000000..fdac3ad --- /dev/null +++ b/src/model/strategies/ros-output-reflections-strategy/include/ros_reflections/ros_reflections.hpp @@ -0,0 +1,59 @@ +// +// 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 ROS_REFLECTIONS_HPP +#define ROS_REFLECTIONS_HPP + +#include +#include +#include +#include +#include + +using namespace model; +using namespace osi3; + +namespace reflections { + class WorkerPCL final { + public: + WorkerPCL(const std::string &topic, std::string frame_id); + void injectRadar(SensorData &sensor_data, int sensor_no, const Log &log); + + private: + const std::string frame_id; + ros::NodeHandle node; + ros::Publisher publisher; + }; +} + + +namespace model { + class ros_reflections : public Strategy { + public: + ros_reflections(const Profile &profile, const Log &log, const Alert &alert); + using Strategy::Strategy; + + void apply(SensorData &) override; + + private: + std::unique_ptr worker_pcl = nullptr; + }; +} + +std_msgs::ColorRGBA set_color(float r, float g, float b, float a); + +#endif //ROS_REFLECTIONS_HPP diff --git a/src/model/strategies/ros-output-reflections-strategy/package.xml b/src/model/strategies/ros-output-reflections-strategy/package.xml new file mode 100644 index 0000000..210a536 --- /dev/null +++ b/src/model/strategies/ros-output-reflections-strategy/package.xml @@ -0,0 +1,20 @@ + + + ros_reflections + 1.0.0 + Forward lidar reflection data into ROS + Clemens Linnhoff + EUPL-1.2 + https://git.rwth-aachen.de/fzd/sensormodeling/fmi-and-osi/visualization-and-output/ros-bridge-detections-strategy + Clemens Linnhoff + + catkin + roscpp + roscpp + roscpp + std_msgs + sensor_msgs + tf + visualization_msgs + diff --git a/src/model/strategies/ros-output-reflections-strategy/src/ros_reflections.cpp b/src/model/strategies/ros-output-reflections-strategy/src/ros_reflections.cpp new file mode 100644 index 0000000..59de288 --- /dev/null +++ b/src/model/strategies/ros-output-reflections-strategy/src/ros_reflections.cpp @@ -0,0 +1,116 @@ +// +// 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 Speed_of_Light +#define Speed_of_Light 299792458 +#endif + +#ifndef _USE_MATH_DEFINES +#define _USE_MATH_DEFINES +#endif + +#include "ros_reflections/ros_reflections.hpp" +#include +#include +#include +#include + +#ifdef _WIN32 +#include +#else +#include +#endif + +using namespace model; +using namespace osi3; + +void ros_reflections::apply(SensorData &sensor_data) { + log("Starting ROS output for reflections"); + + ros::param::set("/use_sim_time", true); + + if ((sensor_data.sensor_view_size()==0) || (!sensor_data.has_feature_data())) { + log("No sensor view or feature data received"); + return; + } + + if (!sensor_data.sensor_view(0).has_global_ground_truth()) { + log("No global ground truth received"); + return; + } + + if (sensor_data.sensor_view(0).radar_sensor_view().size() > 0) { + for (int sensor_no = 0; sensor_no < sensor_data.sensor_view(0).radar_sensor_view().size(); sensor_no++) { + worker_pcl = std::make_unique("reflections_" + std::to_string(sensor_no), "sensor_" + std::to_string(sensor_no)); + worker_pcl->injectRadar(sensor_data, sensor_no, log); + } + } else { + log("No radar sensor view"); + return; + } +} + +ros_reflections::ros_reflections(const Profile &profile, const Log &log, const Alert &alert): model::Strategy(profile, log, alert) { + auto remapping = std::map(); + remapping.emplace("__master", "http://localhost:11311"); + ros::init(remapping, "sensor_model_fmu"); +} + +reflections::WorkerPCL::WorkerPCL(const std::string& topic, std::string frame_id) : publisher(node.advertise>(topic, 1)) , frame_id(std::move(frame_id)) {} + +void reflections::WorkerPCL::injectRadar(SensorData &sensor_data, int sensor_no, const Log &log) { + //loop over all sensors and rendering results + + const auto &radar_sensor_view = sensor_data.sensor_view(0).radar_sensor_view(sensor_no); + auto no_of_rendering_results = radar_sensor_view.reflection_size(); + + auto sim_seconds = uint32_t(sensor_data.sensor_view(0).global_ground_truth().timestamp().seconds()); + auto sim_nanos = uint32_t(sensor_data.sensor_view(0).global_ground_truth().timestamp().nanos()); + auto sim_time = ros::Time(sim_seconds, sim_nanos); + auto sim_time_behind = ros::Time(sim_seconds, sim_nanos + 500); + + if (no_of_rendering_results == 0) { + auto timestamp = (double) sim_seconds + (double) sim_nanos / 1000000000; + log("No reflection from sensor " + std::to_string(sensor_no) + " for ROS output at timestamp " + + std::to_string(timestamp)); + return; + } + + pcl::PointCloud::Ptr cloud_tmp(new pcl::PointCloud()); + pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); + + pcl_conversions::toPCL(sim_time_behind, cloud_tmp->header.stamp); + cloud_tmp->header.frame_id = frame_id; + /// Loop over all received sensor views + for (const auto &reflection: radar_sensor_view.reflection()) { + auto ray_vertical_angle_rad = reflection.source_vertical_angle(); + auto ray_horizontal_angle_rad = reflection.source_horizontal_angle(); + auto distance = 0.5 * reflection.time_of_flight() * Speed_of_Light; + + double x = distance * cos(ray_horizontal_angle_rad) * cos(ray_vertical_angle_rad); + double y = distance * sin(ray_horizontal_angle_rad) * cos(ray_vertical_angle_rad); + double z = distance * sin(ray_vertical_angle_rad); + cloud_tmp->points.emplace_back(pcl::PointXYZ(float(x), float(y), float(z))); + } + pcl::copyPointCloud(*cloud_tmp, *cloud); + for (int reflection_idx = 0; reflection_idx < radar_sensor_view.reflection().size(); reflection_idx++) { + auto signal_strength_in_dB = radar_sensor_view.reflection( + reflection_idx).signal_strength(); + cloud->points[reflection_idx].intensity = float(signal_strength_in_dB); + } + publisher.publish(cloud); +} diff --git a/src/model/strategies/transformation-functions/TransformationFunctions.cpp b/src/model/strategies/transformation-functions/TransformationFunctions.cpp new file mode 100644 index 0000000..5eea5e4 --- /dev/null +++ b/src/model/strategies/transformation-functions/TransformationFunctions.cpp @@ -0,0 +1,307 @@ +// +// Copyright Clemens Linnhoff, M. Sc. +// Institute of Automotive Engineering +// of Technical University of Darmstadt, 2021. +// 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 + +#define PREV_INDEX(itr, total_num) ((itr - 1 + total_num) % total_num) +#define NEXT_INDEX(itr, total_num) ((itr + 1 ) % total_num) + +#include "TransformationFunctions.hpp" + +#ifdef _WIN32 +#include +#else +#include +#endif + +using namespace model; +using namespace osi3; + +Orientation3d TF::calc_relative_orientation_to_local(const Orientation3d& object_orientation, const Orientation3d& ego_orientation) { + + Eigen::Matrix3d ego_rotation_matrix = calc_rotation_matrix_from_euler_angles(ego_orientation); + Eigen::Matrix3d object_rotation_matrix = calc_rotation_matrix_from_euler_angles(object_orientation); + Eigen::Matrix3d resulting_rotation_matrix = object_rotation_matrix * ego_rotation_matrix.transpose(); + + Orientation3d relative_orientation = calc_euler_angles_from_rotation_matrix(resulting_rotation_matrix); + + return relative_orientation; +} + +Orientation3d TF::calc_relative_orientation_from_local(const Orientation3d& object_orientation, const Orientation3d& ego_orientation) { + + Eigen::Matrix3d ego_rotation_matrix = calc_rotation_matrix_from_euler_angles(ego_orientation); + Eigen::Matrix3d object_rotation_matrix = calc_rotation_matrix_from_euler_angles(object_orientation); + Eigen::Matrix3d resulting_rotation_matrix = object_rotation_matrix * ego_rotation_matrix; + + Orientation3d relative_orientation = calc_euler_angles_from_rotation_matrix(resulting_rotation_matrix); + + return relative_orientation; +} + +Eigen::Matrix3d TF::calc_rotation_matrix_from_euler_angles(const Orientation3d& orientation) { + Eigen::Matrix3d R; + R(0,0) = cos(orientation.pitch()) * cos(orientation.yaw()); + R(0,1) = cos(orientation.pitch()) * sin(orientation.yaw()); + R(0,2) = -sin(orientation.pitch()); + R(1,0) = sin(orientation.roll()) * sin(orientation.pitch()) * cos(orientation.yaw()) - cos(orientation.roll()) * sin(orientation.yaw()); + R(1,1) = sin(orientation.roll()) * sin(orientation.pitch()) * sin(orientation.yaw()) + cos(orientation.roll()) * cos(orientation.yaw()); + R(1,2) = sin(orientation.roll()) * cos(orientation.pitch()); + R(2,0) = cos(orientation.roll()) * sin(orientation.pitch()) * cos(orientation.yaw()) + sin(orientation.yaw()) * sin(orientation.roll()); + R(2,1) = cos(orientation.roll()) * sin(orientation.pitch()) * sin(orientation.yaw()) - sin(orientation.roll()) * cos(orientation.yaw()); + R(2,2) = cos(orientation.roll()) * cos(orientation.pitch()); + return R; +} + +Orientation3d TF::calc_euler_angles_from_rotation_matrix(Eigen::Matrix3d R) { + Orientation3d orientation; + orientation.set_pitch(-asin(R(0,2))); + if(cos(orientation.pitch()) != 0) { + orientation.set_yaw(atan2(R(0,1)/cos(orientation.pitch()),R(0,0)/cos(orientation.pitch()))); + orientation.set_roll(atan2(R(1,2)/cos(orientation.pitch()),R(2,2)/cos(orientation.pitch()))); + } else { + orientation.set_yaw(0); + orientation.set_roll(atan2(R(1,0),R(1,1))); + } + return orientation; +} + +Vector3d TF::transform_to_local_coordinates(const Vector3d& input_coordinates, const Orientation3d& new_origin_rotation, const Vector3d& new_origin_translation) { + double delta_x = input_coordinates.x() - new_origin_translation.x(); + double delta_y = input_coordinates.y() - new_origin_translation.y(); + double delta_z = input_coordinates.z() - new_origin_translation.z(); + + Eigen::Matrix3d R = calc_rotation_matrix_from_euler_angles(new_origin_rotation); + double x_rel = delta_x * R(0,0) + + delta_y * R(0,1) + + delta_z * R(0,2); + double y_rel = delta_x * R(1,0) + + delta_y * R(1,1) + + delta_z * R(1,2); + double z_rel = delta_x * R(2,0) + + delta_y * R(2,1) + + delta_z * R(2,2); + + Vector3d coordinates_new_origin; + coordinates_new_origin.set_x(x_rel); + coordinates_new_origin.set_y(y_rel); + coordinates_new_origin.set_z(z_rel); + + return coordinates_new_origin; +} + + +Vector3d TF::transform_from_local_coordinates(const Vector3d& input_coordinates, const Orientation3d& new_origin_rotation, const Vector3d& new_origin_translation) { + + Eigen::Matrix3d R = calc_rotation_matrix_from_euler_angles(new_origin_rotation); + double x_rel = input_coordinates.x() * R.transpose()(0,0) + + input_coordinates.y() * R.transpose()(0,1) + + input_coordinates.z() * R.transpose()(0,2); + double y_rel = input_coordinates.x() * R.transpose()(1,0) + + input_coordinates.y() * R.transpose()(1,1) + + input_coordinates.z() * R.transpose()(1,2); + double z_rel = input_coordinates.x() * R.transpose()(2,0) + + input_coordinates.y() * R.transpose()(2,1) + + input_coordinates.z() * R.transpose()(2,2); + + double delta_x = x_rel + new_origin_translation.x(); + double delta_y = y_rel + new_origin_translation.y(); + double delta_z = z_rel + new_origin_translation.z(); + + Vector3d coordinates_new_origin; + coordinates_new_origin.set_x(delta_x); + coordinates_new_origin.set_y(delta_y); + coordinates_new_origin.set_z(delta_z); + + return coordinates_new_origin; +} + +Vector3d TF::transform_position_from_ego_to_world_coordinates(const Vector3d &position_in_ego_coordinates, const TF::EgoData &ego_data) { + Vector3d position_to_ego_bb_center = vector_translation(position_in_ego_coordinates, ego_data.ego_vehicle_attributes.bbcenter_to_rear(), 1); + Vector3d position_in_world_coordinates = transform_from_local_coordinates(position_to_ego_bb_center, ego_data.ego_base.orientation(), ego_data.ego_base.position()); + return position_in_world_coordinates; +} + +Vector3d TF::transform_position_from_world_to_ego_coordinates(const Vector3d &input_in_world_coordinates, const TF::EgoData &ego_data) { + Vector3d position_ego_bb_center = transform_to_local_coordinates(input_in_world_coordinates, ego_data.ego_base.orientation(), ego_data.ego_base.position()); + Vector3d position_ego_coordinates = vector_translation(position_ego_bb_center, ego_data.ego_vehicle_attributes.bbcenter_to_rear(), -1); + return position_ego_coordinates; +} + +Vector3d TF::transform_position_from_world_to_sensor_coordinates(const Vector3d &input_in_world_coordinates, const TF::EgoData &ego_data, const MountingPosition &mounting_pose) { + Vector3d position_ego_bb_center = transform_to_local_coordinates(input_in_world_coordinates, ego_data.ego_base.orientation(), ego_data.ego_base.position()); + Vector3d position_ego_coordinates = vector_translation(position_ego_bb_center, ego_data.ego_vehicle_attributes.bbcenter_to_rear(), -1); + Vector3d position_cartesian_sensor_coordinates = transform_to_local_coordinates(position_ego_coordinates, mounting_pose.orientation(), mounting_pose.position()); + return position_cartesian_sensor_coordinates; +} + +Orientation3d TF::transform_orientation_from_world_to_sensor_coordinates(const Orientation3d &orientation_in_world_coordinates, const TF::EgoData &ego_data, const MountingPosition &mounting_pose) { + Orientation3d relative_orientation_ego_coordinates = calc_relative_orientation_to_local(orientation_in_world_coordinates, ego_data.ego_base.orientation()); + Orientation3d relative_orientation_sensor_coordinates = calc_relative_orientation_to_local(relative_orientation_ego_coordinates, mounting_pose.orientation()); + return relative_orientation_sensor_coordinates; +} + +Orientation3d TF::transform_orientation_from_sensor_to_world_coordinates(const Orientation3d &orientation_in_sensor_coordinates, const TF::EgoData &ego_data, const MountingPosition &mounting_pose) { + Orientation3d relative_orientation_ego_coordinates = calc_relative_orientation_from_local(orientation_in_sensor_coordinates, mounting_pose.orientation()); + Orientation3d relative_orientation_world_coordinates = calc_relative_orientation_from_local(relative_orientation_ego_coordinates, ego_data.ego_base.orientation()); + return relative_orientation_world_coordinates; +} + +Vector3d TF::transform_position_from_object_to_ego_coordinates(const Vector3d &pcl_segment_position_in_object, const TF::EgoData &ego_data, const MovingObject >_object) { + Vector3d position_in_world_coordinates = TF::transform_from_local_coordinates(pcl_segment_position_in_object, gt_object.base().orientation(), gt_object.base().position()); + Vector3d position_in_ego_coordinates = TF::transform_position_from_world_to_ego_coordinates(position_in_world_coordinates, ego_data); + return position_in_ego_coordinates; +} + +Vector3d TF::transform_position_from_ego_to_object_coordinates(const Vector3d &pcl_segment_position_in_ego, const TF::EgoData &ego_data, const MovingObject >_object) { + Vector3d position_in_world_coordinates = TF::transform_position_from_ego_to_world_coordinates(pcl_segment_position_in_ego, ego_data); + Vector3d position_in_object_coordinates = TF::transform_to_local_coordinates(position_in_world_coordinates, gt_object.base().orientation(), gt_object.base().position()); + return position_in_object_coordinates; +} + +Vector3d TF::transform_position_from_sensor_to_object_coordinates(const Vector3d &pcl_segment_position_in_sensor, const TF::EgoData &ego_data, const MountingPosition &mounting_pose, const BaseMoving &object_base) { + Vector3d pcl_segment_position_in_ego = TF::transform_from_local_coordinates(pcl_segment_position_in_sensor, mounting_pose.orientation(), mounting_pose.position()); + Vector3d position_in_world_coordinates = TF::transform_position_from_ego_to_world_coordinates(pcl_segment_position_in_ego, ego_data); + Vector3d position_in_object_coordinates = TF::transform_to_local_coordinates(position_in_world_coordinates, object_base.orientation(), object_base.position()); + return position_in_object_coordinates; +} + +Vector3d TF::transform_position_from_object_to_sensor_coordinates(const Vector3d &pcl_segment_position_in_object, const TF::EgoData &ego_data, const MountingPosition &mounting_pose, const BaseMoving &object_base) { + Vector3d position_in_world_coordinates = TF::transform_from_local_coordinates(pcl_segment_position_in_object, object_base.orientation(), object_base.position()); + Vector3d position_in_ego_coordinates = TF::transform_position_from_world_to_ego_coordinates(position_in_world_coordinates, ego_data); + Vector3d position_in_sensor_coordinates = TF::transform_to_local_coordinates(position_in_ego_coordinates, mounting_pose.orientation(), mounting_pose.position()); + return position_in_sensor_coordinates; +} + +Vector3d TF::transform_position_from_object_to_world_coordinates(const Vector3d &pcl_segment_position_in_object, const BaseMoving &object_base) { + Vector3d position_in_world_coordinates = TF::transform_from_local_coordinates(pcl_segment_position_in_object, object_base.orientation(), object_base.position()); + return position_in_world_coordinates; +} + +Spherical3d TF::transform_cartesian_to_spherical(const Vector3d &input_relative_position) { + double distance; + double azimuth; + double elevation; + distance = sqrt(input_relative_position.x() * input_relative_position.x() + + input_relative_position.y() * input_relative_position.y() + + input_relative_position.z() * input_relative_position.z()); + azimuth = atan2(input_relative_position.y() , input_relative_position.x()); + elevation = asin(input_relative_position.z() / distance); + Spherical3d spherical_coordinate; + spherical_coordinate.set_distance(distance); + spherical_coordinate.set_azimuth(azimuth); + spherical_coordinate.set_elevation(elevation); + return spherical_coordinate; +} + +double TF::projection_to_spherical_surface(std::vector &spherical_coordinates, double distance) { + // the input is in spherical coordinate system + if (spherical_coordinates.size() < 3) return 0; + double processed_prev, processed_next, ready_to_add; + double sum_of_spherical_angle = 0.0; + double elevation_a, azimuth_a, elevation_b, azimuth_b, elevation_c, azimuth_c; + auto points_number = spherical_coordinates.size(); + for (size_t itr = 0; itr != points_number; ++itr) { + azimuth_c = spherical_coordinates.at(NEXT_INDEX(itr, points_number)).azimuth(); + elevation_c = spherical_coordinates.at(NEXT_INDEX(itr, points_number)).elevation(); + azimuth_b = spherical_coordinates.at(itr).azimuth(); + elevation_b = spherical_coordinates.at(itr).elevation(); + azimuth_a = spherical_coordinates.at(PREV_INDEX(itr, points_number)).azimuth(); + elevation_a = spherical_coordinates.at(PREV_INDEX(itr, points_number)).elevation(); + processed_prev = atan2((sin(elevation_a - elevation_b) * cos(azimuth_a)), + (sin(azimuth_a) * cos(azimuth_b) - cos(azimuth_a) * sin(azimuth_b) * cos(elevation_a - elevation_b))); + processed_next = atan2((sin(elevation_c - elevation_b) * cos(azimuth_c)), + (sin(azimuth_c) * cos(azimuth_b) - cos(azimuth_c) * sin(azimuth_b) * cos(elevation_c - elevation_b))); + processed_prev < processed_next ? ready_to_add = (processed_prev - processed_next) + 2 * M_PI : ready_to_add = (processed_prev - processed_next); + sum_of_spherical_angle += ready_to_add; + } + return (sum_of_spherical_angle - double(points_number - 2) * M_PI) * distance * distance; +} + +void TF::transform_spherical_to_cartesian(const Spherical3d &input_spherical_coordinate, Vector3d &vector_xyz) { + double x; + double y; + double z; + x = input_spherical_coordinate.distance() * cos(input_spherical_coordinate.elevation()) * cos(input_spherical_coordinate.azimuth()); + y = input_spherical_coordinate.distance() * cos(input_spherical_coordinate.elevation()) * sin(input_spherical_coordinate.azimuth()); + z = input_spherical_coordinate.distance() * sin(input_spherical_coordinate.elevation()); + vector_xyz.set_x(x); + vector_xyz.set_y(y); + vector_xyz.set_z(z); +} + +void TF::projection_onto_unit_distance_cylinder(const Spherical3d &input_spherical_coordinate, Vector2d &output_position_on_plane) { + output_position_on_plane.set_x(input_spherical_coordinate.azimuth()); + output_position_on_plane.set_y(tan(input_spherical_coordinate.elevation())); +} + +bool TF::get_ego_info(TF::EgoData &ego_data, const SensorView &input_sensor_view) { + if (input_sensor_view.global_ground_truth().has_host_vehicle_id()) { + ego_data.ego_vehicle_id = input_sensor_view.global_ground_truth().host_vehicle_id(); + bool ego_vehicle_found = false; + int mov_obj_no = 0; + while (!ego_vehicle_found && mov_obj_no < input_sensor_view.global_ground_truth().moving_object_size()) { + if (input_sensor_view.global_ground_truth().moving_object(mov_obj_no).id().value() == ego_data.ego_vehicle_id.value()) { + ego_data.ego_vehicle_no = mov_obj_no; + ego_vehicle_found = true; + } + mov_obj_no++; + } + if (!ego_vehicle_found) { + return false; + } + if (input_sensor_view.global_ground_truth().moving_object_size() > ego_data.ego_vehicle_no && + input_sensor_view.global_ground_truth().moving_object(ego_data.ego_vehicle_no).has_base()) { + ego_data.ego_base.CopyFrom(input_sensor_view.global_ground_truth().moving_object(ego_data.ego_vehicle_no).base()); + ego_data.sensor_mounting_parameters.CopyFrom(input_sensor_view.mounting_position()); + ego_data.ego_vehicle_attributes.CopyFrom(input_sensor_view.global_ground_truth().moving_object(ego_data.ego_vehicle_no).vehicle_attributes()); + } else { + return false; + } + } else { + return false; + } + return true; +} + +Vector3d TF::vector_translation(const Vector3d &vector_a, const Vector3d &vector_b, double factor_for_vector_b) { + + Vector3d vector_result; + vector_result.set_x(vector_a.x() + factor_for_vector_b * vector_b.x()); + vector_result.set_y(vector_a.y() + factor_for_vector_b * vector_b.y()); + vector_result.set_z(vector_a.z() + factor_for_vector_b * vector_b.z()); + return vector_result; +} + +double TF::get_vector_abs(const Vector3d& vector) { + return sqrt(pow(vector.x(), 2) + pow(vector.y(), 2) + pow(vector.z(), 2)); +} + +double TF::dot_product(const Vector3d &vector_a, const Vector3d &vector_b) { + return vector_a.x()*vector_b.x() + vector_a.y()*vector_b.y() + vector_a.z()*vector_b.z(); +} + +Vector3d TF::cross_product(const Vector3d &vector_a, const Vector3d &vector_b) { + Vector3d vector_result; + vector_result.set_x(vector_a.y()*vector_b.z() - vector_a.z()*vector_b.y()); + vector_result.set_y(vector_a.z()*vector_b.x() - vector_a.x()*vector_b.z()); + vector_result.set_z(vector_a.x()*vector_b.y() - vector_a.y()*vector_b.x()); + return vector_result; +} diff --git a/src/model/strategies/transformation-functions/TransformationFunctions.hpp b/src/model/strategies/transformation-functions/TransformationFunctions.hpp new file mode 100644 index 0000000..82c44d9 --- /dev/null +++ b/src/model/strategies/transformation-functions/TransformationFunctions.hpp @@ -0,0 +1,78 @@ +// +// Copyright Clemens Linnhoff, M. Sc. +// Institute of Automotive Engineering +// of Technical University of Darmstadt, 2021. +// 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 TRANSFORMATIONFUNCTIONS_H +#define TRANSFORMATIONFUNCTIONS_H + +#include "osi_sensordata.pb.h" +#include + +using namespace osi3; + +namespace model { + + class TF { + public: + struct EgoData { + BaseMoving ego_base; + MountingPosition sensor_mounting_parameters; + Identifier ego_vehicle_id; + MovingObject::VehicleAttributes ego_vehicle_attributes; + int ego_vehicle_no; + }; + + static Orientation3d calc_relative_orientation_to_local(const Orientation3d& object_orientation, const Orientation3d& ego_orientation); + static Orientation3d calc_relative_orientation_from_local(const Orientation3d& object_orientation, const Orientation3d& ego_orientation); + + static Eigen::Matrix3d calc_rotation_matrix_from_euler_angles(const Orientation3d& orientation); + static Orientation3d calc_euler_angles_from_rotation_matrix(Eigen::Matrix3d R); + + static Vector3d transform_to_local_coordinates(const Vector3d& input_coordinates, const Orientation3d& new_origin_rotation, const Vector3d& new_origin_translation); + static Vector3d transform_from_local_coordinates(const Vector3d& input_coordinates, const Orientation3d& new_origin_rotation, const Vector3d& new_origin_translation); + + static Vector3d transform_position_from_world_to_ego_coordinates(const Vector3d &input_in_world_coordinates, const TF::EgoData &ego_data); + static Vector3d transform_position_from_ego_to_world_coordinates(const Vector3d &position_in_ego_coordinates, const TF::EgoData &ego_data); + + static Vector3d transform_position_from_world_to_sensor_coordinates(const Vector3d &input_in_world_coordinates, const TF::EgoData &ego_data, const MountingPosition &mounting_pose); + + static Vector3d transform_position_from_object_to_ego_coordinates(const Vector3d &pcl_segment_position_in_object, const TF::EgoData &ego_data, const MovingObject >_object); + static Vector3d transform_position_from_ego_to_object_coordinates(const Vector3d &pcl_segment_position_in_ego, const EgoData &ego_data, const MovingObject >_object); + static Vector3d transform_position_from_sensor_to_object_coordinates(const Vector3d &pcl_segment_position_in_sensor, const EgoData &ego_data, const MountingPosition &mounting_pose, const BaseMoving &object_base); + static Vector3d transform_position_from_object_to_sensor_coordinates(const Vector3d &pcl_segment_position_in_object, const TF::EgoData &ego_data, const MountingPosition &mounting_pose, const BaseMoving &object_base); + static Vector3d transform_position_from_object_to_world_coordinates(const Vector3d &pcl_segment_position_in_object, const BaseMoving &object_base); + + + static Orientation3d transform_orientation_from_world_to_sensor_coordinates(const Orientation3d &orientation_in_world_coordinates, const TF::EgoData &ego_data, const MountingPosition &mounting_pose); + static Orientation3d transform_orientation_from_sensor_to_world_coordinates(const Orientation3d &orientation_in_sensor_coordinates, const EgoData &ego_data, const MountingPosition &mounting_pose); + + static Spherical3d transform_cartesian_to_spherical(const Vector3d &input_relative_position); + static void transform_spherical_to_cartesian(const Spherical3d &input_spherical_coordinate, Vector3d &vector_xyz); + + static double projection_to_spherical_surface(std::vector &spherical_coordinates, double distance); + static void projection_onto_unit_distance_cylinder(const Spherical3d &input_spherical_coordinate, Vector2d &output_position_on_plane); + + static bool get_ego_info(TF::EgoData &ego_data, const SensorView &input_sensor_view); + + static Vector3d vector_translation(const Vector3d &vector_a, const Vector3d &vector_b, double factor_for_vector_b); + static double get_vector_abs(const Vector3d &vector); + static double dot_product(const Vector3d &vector_a, const Vector3d &vector_b); + static Vector3d cross_product(const Vector3d &vector_a, const Vector3d &vector_b); + }; +} + +#endif //TRANSFORMATIONFUNCTIONS_H