diff --git a/simulation/simple_sensor_simulator/test/CMakeLists.txt b/simulation/simple_sensor_simulator/test/CMakeLists.txt
index 5db9f69e675..14856674d1c 100644
--- a/simulation/simple_sensor_simulator/test/CMakeLists.txt
+++ b/simulation/simple_sensor_simulator/test/CMakeLists.txt
@@ -1,6 +1,12 @@
find_package(Protobuf REQUIRED)
include_directories(${Protobuf_INCLUDE_DIRS})
+install(
+ DIRECTORY map
+ DESTINATION share/${PROJECT_NAME})
+
add_subdirectory(src/sensor_simulation/lidar)
add_subdirectory(src/sensor_simulation/primitives)
add_subdirectory(src/sensor_simulation/occupancy_grid)
+add_subdirectory(src/sensor_simulation/detection_sensor)
+add_subdirectory(src/sensor_simulation/traffic_lights)
diff --git a/simulation/simple_sensor_simulator/test/map/lanelet2_map.osm b/simulation/simple_sensor_simulator/test/map/lanelet2_map.osm
new file mode 100644
index 00000000000..7ecba866372
--- /dev/null
+++ b/simulation/simple_sensor_simulator/test/map/lanelet2_map.osm
@@ -0,0 +1,6105 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/CMakeLists.txt b/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/CMakeLists.txt
new file mode 100644
index 00000000000..9af7c058821
--- /dev/null
+++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/CMakeLists.txt
@@ -0,0 +1,2 @@
+ament_add_gtest(test_detection_sensor test_detection_sensor.cpp)
+target_link_libraries(test_detection_sensor simple_sensor_simulator_component ${Protobuf_LIBRARIES})
diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/test_detection_sensor.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/test_detection_sensor.cpp
new file mode 100644
index 00000000000..0aefad21dc2
--- /dev/null
+++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/test_detection_sensor.cpp
@@ -0,0 +1,168 @@
+// Copyright 2015 TIER IV, Inc. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "test_detection_sensor.hpp"
+
+#include
+
+#include "../../utils/expect_eq_macros.hpp"
+
+/**
+ * @note Test basic functionality. Test update process correctness with no position noise, no delay
+ * and filter_by_range = true (in configuration), no probability of lost (configuration) and a
+ * specific entity positioned closer to Ego than the range parameter (in configuration) - the goal
+ * is to test the standard detection functionality when an entity is detected because it is close to
+ * Ego.
+ */
+TEST_P(DetectionSensorTestParameterized, update)
+{
+ const auto param = GetParam();
+ initializeEntityStatus(param.entity_name_, param.entity_type_, param.entity_subtype_);
+
+ detection_sensor_->update(
+ current_simulation_time_, status_, current_ros_time_, lidar_detected_entities_);
+
+ // Spin the node to process callbacks
+ rclcpp::spin_some(node_);
+
+ ASSERT_NE(received_msg_, nullptr);
+ ASSERT_EQ(received_msg_->objects.size(), static_cast(1));
+ EXPECT_EQ(received_msg_->objects[0].classification[0].label, param.expected_label_);
+ EXPECT_POSITION_NEAR(
+ received_msg_->objects[0].kinematics.pose_with_covariance.pose.position, entity_pose_.position,
+ std::numeric_limits::epsilon());
+}
+
+INSTANTIATE_TEST_SUITE_P(
+ DetectionSensorTests, DetectionSensorTestParameterized,
+ ::testing::Values(
+ DetectionTestParam{
+ "unknown", EntityType::VEHICLE, EntitySubtype::UNKNOWN, ObjectClassification::UNKNOWN},
+ DetectionTestParam{"car", EntityType::VEHICLE, EntitySubtype::CAR, ObjectClassification::CAR},
+ DetectionTestParam{
+ "truck", EntityType::VEHICLE, EntitySubtype::TRUCK, ObjectClassification::TRUCK},
+ DetectionTestParam{"bus", EntityType::VEHICLE, EntitySubtype::BUS, ObjectClassification::BUS},
+ DetectionTestParam{
+ "trailer", EntityType::VEHICLE, EntitySubtype::TRAILER, ObjectClassification::TRAILER},
+ DetectionTestParam{
+ "motorcycle", EntityType::VEHICLE, EntitySubtype::MOTORCYCLE,
+ ObjectClassification::MOTORCYCLE},
+ DetectionTestParam{
+ "bicycle", EntityType::VEHICLE, EntitySubtype::BICYCLE, ObjectClassification::BICYCLE},
+ DetectionTestParam{
+ "pedestrian", EntityType::PEDESTRIAN, EntitySubtype::PEDESTRIAN,
+ ObjectClassification::PEDESTRIAN}));
+
+/**
+ * @note Test basic functionality. Test update process correctness with no position noise, a
+ * substantial positive delay and filter_by_range = true (in configuration) no probability of lost
+ * (configuration) and some entity positioned closer to Ego than the range parameter (in
+ * configuration) - the goal is to test the simulated detection delay correctness
+ */
+TEST_F(DetectionSensorTest, update_detectionDelay)
+{
+ config_.set_object_recognition_delay(0.5);
+ detection_sensor_ = std::make_unique>(
+ 0.0, config_, detected_objects_publisher_, ground_truth_objects_publisher_);
+
+ initializeEntityStatus("pedestrian", EntityType::PEDESTRIAN, EntitySubtype::PEDESTRIAN);
+
+ // Initial update: before the delay period, should not detect the object
+ detection_sensor_->update(
+ current_simulation_time_, status_, current_ros_time_, lidar_detected_entities_);
+
+ // Spin the node to process callbacks
+ rclcpp::spin_some(node_);
+
+ // Check that no message has been received yet
+ EXPECT_EQ(received_msg_, nullptr);
+
+ // Advance the simulation time to after the delay period
+ current_simulation_time_ += 0.6; // 0.6 seconds, greater than the 0.5-second delay
+ current_ros_time_ = rclcpp::Time(current_simulation_time_, RCL_ROS_TIME);
+
+ // Update again: after the delay period, should detect the object
+ detection_sensor_->update(
+ current_simulation_time_, status_, current_ros_time_, lidar_detected_entities_);
+
+ // Spin the node to process callbacks
+ rclcpp::spin_some(node_);
+
+ ASSERT_NE(received_msg_, nullptr);
+ ASSERT_EQ(received_msg_->objects.size(), static_cast(1));
+ EXPECT_EQ(received_msg_->objects[0].classification[0].label, ObjectClassification::PEDESTRIAN);
+ EXPECT_POSITION_NEAR(
+ received_msg_->objects[0].kinematics.pose_with_covariance.pose.position, entity_pose_.position,
+ std::numeric_limits::epsilon());
+}
+
+/**
+ * @note Test basic functionality. Test update process correctness with no position noise, no delay
+ * and filter_by_range = true (in configuration) 100% probability of lost (configuration) and some
+ * entity positioned closer to Ego than the range parameter (in configuration) - the goal is to test
+ * the simulated malfunction when the message is not being delivered
+ */
+TEST_F(DetectionSensorTest, update_looseAllData)
+{
+ config_.set_probability_of_lost(1.0); // 100% probability of lost
+
+ detection_sensor_ = std::make_unique>(
+ 0.0, config_, detected_objects_publisher_, ground_truth_objects_publisher_);
+
+ initializeEntityStatus("pedestrian", EntityType::PEDESTRIAN, EntitySubtype::PEDESTRIAN);
+
+ detection_sensor_->update(
+ current_simulation_time_, status_, current_ros_time_, lidar_detected_entities_);
+
+ // Spin the node to process callbacks
+ rclcpp::spin_some(node_);
+
+ ASSERT_NE(received_msg_, nullptr);
+ ASSERT_EQ(received_msg_->objects.size(), static_cast(0));
+}
+
+/**
+ * @note Test basic functionality. Test update process correctness with no position noise, no delay
+ * and filter_by_range = false (in configuration) no probability of lost (configuration) and some
+ * entity in lidar_detected_entity vector - the goal is to test detection based on lidar
+ * functionality
+ */
+TEST_F(DetectionSensorTest, update_lidarBasedDetection)
+{
+ config_.set_detect_all_objects_in_range(false);
+
+ detection_sensor_ = std::make_unique>(
+ 0.0, config_, detected_objects_publisher_, ground_truth_objects_publisher_);
+
+ initializeEntityStatus("pedestrian", EntityType::PEDESTRIAN, EntitySubtype::PEDESTRIAN);
+
+ detection_sensor_->update(
+ current_simulation_time_, status_, current_ros_time_, lidar_detected_entities_);
+
+ // Spin the node to process callbacks
+ rclcpp::spin_some(node_);
+
+ ASSERT_NE(received_msg_, nullptr);
+ ASSERT_EQ(received_msg_->objects.size(), static_cast(1));
+ EXPECT_EQ(received_msg_->objects[0].classification[0].label, ObjectClassification::PEDESTRIAN);
+ EXPECT_POSITION_NEAR(
+ received_msg_->objects[0].kinematics.pose_with_covariance.pose.position, entity_pose_.position,
+ std::numeric_limits::epsilon());
+}
+
+int main(int argc, char ** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/test_detection_sensor.hpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/test_detection_sensor.hpp
new file mode 100644
index 00000000000..b16ad8f7087
--- /dev/null
+++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/test_detection_sensor.hpp
@@ -0,0 +1,116 @@
+// Copyright 2015 TIER IV, Inc. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SIMPLE_SENSOR_SIMULATOR__TEST__TEST_DETECTION_SENSOR_HPP_
+#define SIMPLE_SENSOR_SIMULATOR__TEST__TEST_DETECTION_SENSOR_HPP_
+
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include "../../utils/helper_functions.hpp"
+
+using namespace simple_sensor_simulator;
+
+using DetectedObjectsMsg = autoware_auto_perception_msgs::msg::DetectedObjects;
+using TrackedObjectsMsg = autoware_auto_perception_msgs::msg::TrackedObjects;
+using ObjectClassification = autoware_auto_perception_msgs::msg::ObjectClassification;
+
+class DetectionSensorTest : public ::testing::Test
+{
+protected:
+ DetectionSensorTest()
+ : config_(utils::constructDetectionSensorConfiguration("ego", "awf/universe", 0.1, 100, true)),
+ entity_pose_(utils::makePose(5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0)),
+ entity_dimensions_(utils::makeDimensions(4.5, 2.0, 1.5))
+ {
+ rclcpp::init(0, nullptr);
+ node_ = std::make_shared("detection_sensor_test_node");
+
+ makeRosInterface();
+ makeEgo();
+
+ detection_sensor_ = std::make_unique>(
+ 0.0, config_, detected_objects_publisher_, ground_truth_objects_publisher_);
+ }
+ ~DetectionSensorTest() { rclcpp::shutdown(); }
+
+ rclcpp::Node::SharedPtr node_;
+ rclcpp::Publisher::SharedPtr detected_objects_publisher_;
+ rclcpp::Publisher::SharedPtr ground_truth_objects_publisher_;
+ rclcpp::Subscription::SharedPtr detected_objects_subscriber_;
+
+ std::vector status_;
+ std::vector lidar_detected_entities_;
+
+ simulation_api_schema::DetectionSensorConfiguration config_;
+ std::unique_ptr detection_sensor_;
+ DetectedObjectsMsg::SharedPtr received_msg_;
+
+ double current_simulation_time_{1.0};
+ rclcpp::Time current_ros_time_{1};
+ geometry_msgs::msg::Pose entity_pose_;
+ geometry_msgs::msg::Vector3 entity_dimensions_;
+
+ auto initializeEntityStatus(
+ const std::string & name, const EntityType::Enum type, const EntitySubtype::Enum subtype)
+ -> void
+ {
+ const auto entity_status =
+ utils::makeEntity(name, type, subtype, entity_pose_, entity_dimensions_);
+ status_.push_back(entity_status);
+ lidar_detected_entities_.push_back(name);
+ }
+
+private:
+ auto makeEgo() -> void
+ {
+ const auto ego_status = utils::makeEntity(
+ "ego", EntityType::EGO, EntitySubtype::CAR,
+ utils::makePose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0), entity_dimensions_);
+ status_.push_back(ego_status);
+ }
+
+ auto makeRosInterface() -> void
+ {
+ detected_objects_publisher_ =
+ node_->create_publisher("detected_objects_output", 10);
+ ground_truth_objects_publisher_ =
+ node_->create_publisher("tracked_objects_output", 10);
+
+ detected_objects_subscriber_ = node_->create_subscription(
+ "detected_objects_output", 10,
+ [this](const DetectedObjectsMsg::SharedPtr msg) { received_msg_ = msg; });
+ }
+};
+
+struct DetectionTestParam
+{
+ std::string entity_name_;
+ EntityType::Enum entity_type_;
+ EntitySubtype::Enum entity_subtype_;
+ int expected_label_;
+};
+
+class DetectionSensorTestParameterized : public DetectionSensorTest,
+ public ::testing::WithParamInterface
+{
+};
+
+#endif // SIMPLE_SENSOR_SIMULATOR__TEST__TEST_DETECTION_SENSOR_HPP_
diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.cpp
index c7aba68ff9f..20dd82e78d4 100644
--- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.cpp
+++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.cpp
@@ -67,7 +67,7 @@ TEST_F(LidarSensorTest, update_goBackInTime)
*/
TEST_F(LidarSensorTest, getDetectedObjects)
{
- const std::set expected_objects = {other1_status_.name(), other2_status_.name()};
+ const std::set expected_objects = {status_[1].name(), status_[2].name()};
lidar_->update(current_simulation_time_, status_, current_ros_time_);
@@ -77,7 +77,7 @@ TEST_F(LidarSensorTest, getDetectedObjects)
const auto & detected_objects = lidar_->getDetectedObjects();
// LidarSensor returns duplicates. To avoid them, a set is used.
- std::set unique_objects(detected_objects.begin(), detected_objects.end());
+ const std::set unique_objects(detected_objects.begin(), detected_objects.end());
ASSERT_FALSE(unique_objects.empty());
EXPECT_EQ(unique_objects, expected_objects);
diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.hpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.hpp
index 7aef40e53b4..54bf61a03b0 100644
--- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.hpp
+++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.hpp
@@ -22,16 +22,12 @@
#include
#include
#include
-#include
#include
#include "../../utils/helper_functions.hpp"
using namespace simple_sensor_simulator;
-using EntityType = traffic_simulator_msgs::EntityType;
-using EntityStatus = traffic_simulator_msgs::EntityStatus;
-
class LidarSensorTest : public ::testing::Test
{
protected:
@@ -39,7 +35,7 @@ class LidarSensorTest : public ::testing::Test
{
rclcpp::init(0, nullptr);
node_ = std::make_shared("lidar_sensor_test_node");
- createRosInterface();
+ makeRosInterface();
initializeEntityStatuses();
lidar_ = std::make_unique>(0.0, config_, publisher_);
@@ -51,9 +47,6 @@ class LidarSensorTest : public ::testing::Test
rclcpp::Publisher::SharedPtr publisher_;
rclcpp::Subscription::SharedPtr subscription_;
- EntityStatus ego_status_;
- EntityStatus other1_status_;
- EntityStatus other2_status_;
std::vector status_;
std::unique_ptr lidar_;
@@ -66,51 +59,21 @@ class LidarSensorTest : public ::testing::Test
private:
auto initializeEntityStatuses() -> void
{
- auto dimensions = utils::makeDimensions(4.5, 2.0, 1.5);
+ const auto dimensions = utils::makeDimensions(4.5, 2.0, 1.5);
- ego_status_ = makeEntity(
+ const auto ego_status = utils::makeEntity(
"ego", EntityType::EGO, utils::makePose(5.0, 5.0, 0.0, 0.0, 0.0, 0.0, 1.0), dimensions);
- other1_status_ = makeEntity(
+ const auto other1_status = utils::makeEntity(
"other1", EntityType::VEHICLE, utils::makePose(-3.0, -3.0, 0.0, 0.0, 0.0, 0.0, 1.0),
dimensions);
- other2_status_ = makeEntity(
+ const auto other2_status = utils::makeEntity(
"other2", EntityType::VEHICLE, utils::makePose(5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0),
dimensions);
- status_ = {ego_status_, other1_status_, other2_status_};
- }
-
- auto makeEntity(
- const std::string & name, const EntityType::Enum type, const geometry_msgs::msg::Pose & pose,
- const geometry_msgs::msg::Vector3 & dimensions) -> EntityStatus
- {
- EntityStatus status;
-
- status.set_name(name);
- status.mutable_type()->set_type(type);
-
- auto new_pose = status.mutable_pose();
- auto new_position = new_pose->mutable_position();
- new_position->set_x(pose.position.x);
- new_position->set_y(pose.position.y);
- new_position->set_z(pose.position.z);
-
- auto new_orientation = new_pose->mutable_orientation();
- new_orientation->set_x(pose.orientation.x);
- new_orientation->set_y(pose.orientation.y);
- new_orientation->set_z(pose.orientation.z);
- new_orientation->set_w(pose.orientation.w);
-
- auto new_bounding_box = status.mutable_bounding_box();
- auto new_dimensions = new_bounding_box->mutable_dimensions();
- new_dimensions->set_x(dimensions.x);
- new_dimensions->set_y(dimensions.y);
- new_dimensions->set_z(dimensions.z);
-
- return status;
+ status_ = {ego_status, other1_status, other2_status};
}
- auto createRosInterface() -> void
+ auto makeRosInterface() -> void
{
publisher_ = node_->create_publisher("lidar_output", 10);
subscription_ = node_->create_subscription(
diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/CMakeLists.txt b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/CMakeLists.txt
index 8a5af2ad924..b5ae088e7a7 100644
--- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/CMakeLists.txt
+++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/CMakeLists.txt
@@ -5,4 +5,4 @@ ament_add_gtest(test_vertex test_vertex.cpp)
target_link_libraries(test_vertex simple_sensor_simulator_component)
ament_add_gtest(test_primitive test_primitive.cpp)
-target_link_libraries(test_primitive simple_sensor_simulator_component)
+target_link_libraries(test_primitive simple_sensor_simulator_component ${Protobuf_LIBRARIES})
diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/traffic_lights/CMakeLists.txt b/simulation/simple_sensor_simulator/test/src/sensor_simulation/traffic_lights/CMakeLists.txt
new file mode 100644
index 00000000000..8624ec40095
--- /dev/null
+++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/traffic_lights/CMakeLists.txt
@@ -0,0 +1,2 @@
+ament_add_gtest(test_traffic_light_detector test_traffic_light_detector.cpp)
+target_link_libraries(test_traffic_light_detector simple_sensor_simulator_component ${Protobuf_LIBRARIES})
diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/traffic_lights/test_traffic_light_detector.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/traffic_lights/test_traffic_light_detector.cpp
new file mode 100644
index 00000000000..e90cfb45146
--- /dev/null
+++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/traffic_lights/test_traffic_light_detector.cpp
@@ -0,0 +1,96 @@
+// Copyright 2015 TIER IV, Inc. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "test_traffic_light_detector.hpp"
+
+auto makeUpdateTrafficLightsRequest(const int32_t id, const TrafficLightColor color)
+ -> simulation_api_schema::UpdateTrafficLightsRequest
+{
+ simulation_api_schema::UpdateTrafficLightsRequest request;
+ auto * signal = request.add_states();
+ signal->set_id(id);
+ auto * traffic_light = signal->add_traffic_light_status();
+ traffic_light->set_color(color);
+ return request;
+}
+
+/**
+ * @note Test basic functionality. Test updating frame correctness with a vector containing some
+ * traffic light state - the goal is to verify that correct data is published on correct topic.
+ */
+TEST_F(TrafficLightDetectorTest_AutoPerceptionMsgs, updateFrame_correctState)
+{
+ traffic_lights_detector_auto_->updateFrame(
+ stamp_, makeUpdateTrafficLightsRequest(
+ static_cast(34802), TrafficLightColor::TrafficLight_Color_RED));
+ rclcpp::spin_some(node_);
+
+ ASSERT_NE(received_msg_auto_, nullptr);
+ EXPECT_EQ(received_msg_auto_->header.frame_id, "camera_link");
+ EXPECT_EQ(received_msg_auto_->signals.size(), static_cast(1));
+ EXPECT_EQ(received_msg_auto_->signals[0].map_primitive_id, 34802);
+ EXPECT_EQ(received_msg_auto_->signals[0].lights.size(), static_cast(1));
+ EXPECT_EQ(received_msg_auto_->signals[0].lights[0].color, red_light_);
+}
+
+/**
+ * @note Test function behavior when called with an empty vector.
+ */
+TEST_F(TrafficLightDetectorTest_AutoPerceptionMsgs, updateFrame_emptyVector)
+{
+ traffic_lights_detector_auto_->updateFrame(
+ stamp_, simulation_api_schema::UpdateTrafficLightsRequest());
+ rclcpp::spin_some(node_);
+
+ ASSERT_NE(received_msg_auto_, nullptr);
+ EXPECT_EQ(received_msg_auto_->header.frame_id, "camera_link");
+ EXPECT_EQ(received_msg_auto_->signals.size(), static_cast(0));
+}
+
+/**
+ * @note Test basic functionality. Test updating frame correctness with a vector containing some
+ * traffic light state - the goal is to verify that correct data is published on correct topic.
+ */
+TEST_F(TrafficLightDetectorTest_PerceptionMsgs, updateFrame_correctState)
+{
+ traffic_lights_detector_perception_->updateFrame(
+ stamp_, makeUpdateTrafficLightsRequest(
+ static_cast(34802), TrafficLightColor::TrafficLight_Color_RED));
+ rclcpp::spin_some(node_);
+
+ ASSERT_NE(received_msg_perception_, nullptr);
+ EXPECT_EQ(received_msg_perception_->signals.size(), static_cast(1));
+ EXPECT_EQ(received_msg_perception_->signals[0].traffic_signal_id, 34806);
+ EXPECT_EQ(received_msg_perception_->signals[0].elements.size(), static_cast(1));
+ EXPECT_EQ(received_msg_perception_->signals[0].elements[0].color, red_light_);
+}
+
+/**
+ * @note Test function behavior when called with an empty vector.
+ */
+TEST_F(TrafficLightDetectorTest_PerceptionMsgs, updateFrame_emptyVector)
+{
+ traffic_lights_detector_perception_->updateFrame(
+ stamp_, simulation_api_schema::UpdateTrafficLightsRequest());
+ rclcpp::spin_some(node_);
+
+ ASSERT_NE(received_msg_perception_, nullptr);
+ EXPECT_EQ(received_msg_perception_->signals.size(), static_cast(0));
+}
+
+int main(int argc, char ** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/traffic_lights/test_traffic_light_detector.hpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/traffic_lights/test_traffic_light_detector.hpp
new file mode 100644
index 00000000000..8385f09740e
--- /dev/null
+++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/traffic_lights/test_traffic_light_detector.hpp
@@ -0,0 +1,116 @@
+// Copyright 2015 TIER IV, Inc. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SIMPLE_SENSOR_SIMULATOR__TEST__TEST_TRAFFIC_LIGHT_DETECTOR_HPP_
+#define SIMPLE_SENSOR_SIMULATOR__TEST__TEST_TRAFFIC_LIGHT_DETECTOR_HPP_
+
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+using namespace simple_sensor_simulator;
+
+using AutoPerceptionTrafficSignalArray = autoware_auto_perception_msgs::msg::TrafficSignalArray;
+using PerceptionTrafficSignalArray = autoware_perception_msgs::msg::TrafficSignalArray;
+using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement;
+using TrafficLightColor = simulation_api_schema::TrafficLight_Color;
+
+class TrafficLightDetectorTestBase : public ::testing::Test
+{
+protected:
+ TrafficLightDetectorTestBase()
+ {
+ rclcpp::init(0, nullptr);
+ node_ = std::make_shared("traffic_light_detector_test_node");
+
+ hdmap_utils_ = std::make_shared(
+ ament_index_cpp::get_package_share_directory("simple_sensor_simulator") +
+ "/map/lanelet2_map.osm",
+ geographic_msgs::build()
+ .latitude(35.61836750154)
+ .longitude(139.78066608243)
+ .altitude(0.0));
+ }
+
+ ~TrafficLightDetectorTestBase() { rclcpp::shutdown(); }
+
+ rclcpp::Node::SharedPtr node_;
+ std::shared_ptr hdmap_utils_;
+ const uint8_t red_light_{TrafficSignalElement::RED};
+ const rclcpp::Time stamp_{0};
+};
+
+// awf/universe
+class TrafficLightDetectorTest_AutoPerceptionMsgs : public TrafficLightDetectorTestBase
+{
+protected:
+ TrafficLightDetectorTest_AutoPerceptionMsgs()
+ : publisher_auto_(
+ std::make_shared>(
+ perception_msgs_topic_, node_, hdmap_utils_)),
+ subscription_auto_(node_->create_subscription(
+ perception_msgs_topic_, 10,
+ [this](const AutoPerceptionTrafficSignalArray::SharedPtr msg) { received_msg_auto_ = msg; })),
+ traffic_lights_detector_auto_(
+ std::make_unique(publisher_auto_))
+ {
+ }
+
+ const std::string perception_msgs_topic_{"traffic_light_detector_output_perception"};
+
+ std::shared_ptr publisher_auto_;
+ rclcpp::Subscription::SharedPtr subscription_auto_;
+ std::unique_ptr traffic_lights_detector_auto_;
+ AutoPerceptionTrafficSignalArray::SharedPtr received_msg_auto_;
+};
+
+// awf/universe/20230906
+class TrafficLightDetectorTest_PerceptionMsgs : public TrafficLightDetectorTestBase
+{
+protected:
+ TrafficLightDetectorTest_PerceptionMsgs()
+ : publisher_perception_(
+ std::make_shared>(
+ auto_perception_msgs_topic_, node_, hdmap_utils_)),
+ subscription_perception_(node_->create_subscription(
+ auto_perception_msgs_topic_, 10,
+ [this](const PerceptionTrafficSignalArray::SharedPtr msg) {
+ received_msg_perception_ = msg;
+ })),
+ traffic_lights_detector_perception_(
+ std::make_unique(publisher_perception_))
+ {
+ }
+
+ const std::string auto_perception_msgs_topic_{"traffic_light_detector_output_auto"};
+
+ std::shared_ptr publisher_perception_;
+ rclcpp::Subscription::SharedPtr subscription_perception_;
+ std::unique_ptr traffic_lights_detector_perception_;
+ PerceptionTrafficSignalArray::SharedPtr received_msg_perception_;
+};
+
+#endif // SIMPLE_SENSOR_SIMULATOR__TEST__TEST_TRAFFIC_LIGHT_DETECTOR_HPP_
diff --git a/simulation/simple_sensor_simulator/test/src/utils/helper_functions.hpp b/simulation/simple_sensor_simulator/test/src/utils/helper_functions.hpp
index 4f8d47cc1f5..881249afd5a 100644
--- a/simulation/simple_sensor_simulator/test/src/utils/helper_functions.hpp
+++ b/simulation/simple_sensor_simulator/test/src/utils/helper_functions.hpp
@@ -17,10 +17,19 @@
#include
+#include
#include
#include
#include
#include
+#include
+#include
+#include
+#include
+
+using EntitySubtype = traffic_simulator_msgs::EntitySubtype;
+using EntityType = traffic_simulator_msgs::EntityType;
+using EntityStatus = traffic_simulator_msgs::EntityStatus;
namespace utils
{
@@ -48,6 +57,29 @@ inline auto makeDimensions(const double x, const double y, const double z)
return geometry_msgs::build().x(x).y(y).z(z);
}
+auto constructDetectionSensorConfiguration(
+ const std::string & entity, const std::string & architecture_type, const double update_duration,
+ const double range = 300.0, const bool detect_all_objects_in_range = false,
+ const double pos_noise_stddev = 0.0, const double probability_of_lost = 0.0,
+ const double object_recognition_delay = 0.0,
+ const double object_recognition_ground_truth_delay = 0.0)
+ -> const simulation_api_schema::DetectionSensorConfiguration
+{
+ simulation_api_schema::DetectionSensorConfiguration configuration;
+ configuration.set_entity(entity);
+ configuration.set_architecture_type(architecture_type);
+ configuration.set_update_duration(update_duration);
+ configuration.set_object_recognition_delay(object_recognition_delay);
+ configuration.set_object_recognition_ground_truth_delay(object_recognition_ground_truth_delay);
+ configuration.set_pos_noise_stddev(pos_noise_stddev);
+ configuration.set_probability_of_lost(probability_of_lost);
+ configuration.set_detect_all_objects_in_range(detect_all_objects_in_range);
+ configuration.set_range(range);
+ configuration.set_random_seed(1234);
+
+ return configuration;
+}
+
inline auto constructLidarConfiguration(
const std::string & entity, const std::string & architecture_type,
const double lidar_sensor_delay, const double horizontal_resolution)
@@ -79,6 +111,55 @@ inline auto constructLidarConfiguration(
return configuration;
}
+inline auto createEntityStatus(
+ const std::string & name, const EntityType::Enum type,
+ const std::optional & subtype, const geometry_msgs::msg::Pose & pose,
+ const geometry_msgs::msg::Vector3 & dimensions) -> EntityStatus
+{
+ EntityStatus status;
+ status.set_name(name);
+ status.mutable_type()->set_type(type);
+
+ if (subtype) {
+ status.mutable_subtype()->set_value(*subtype);
+ }
+
+ auto new_pose = status.mutable_pose();
+ auto new_position = new_pose->mutable_position();
+ new_position->set_x(pose.position.x);
+ new_position->set_y(pose.position.y);
+ new_position->set_z(pose.position.z);
+
+ auto new_orientation = new_pose->mutable_orientation();
+ new_orientation->set_x(pose.orientation.x);
+ new_orientation->set_y(pose.orientation.y);
+ new_orientation->set_z(pose.orientation.z);
+ new_orientation->set_w(pose.orientation.w);
+
+ auto new_bounding_box = status.mutable_bounding_box();
+ auto new_dimensions = new_bounding_box->mutable_dimensions();
+ new_dimensions->set_x(dimensions.x);
+ new_dimensions->set_y(dimensions.y);
+ new_dimensions->set_z(dimensions.z);
+
+ return status;
+}
+
+inline auto makeEntity(
+ const std::string & name, const EntityType::Enum type, const EntitySubtype::Enum subtype,
+ const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & dimensions)
+ -> EntityStatus
+{
+ return createEntityStatus(name, type, subtype, pose, dimensions);
+}
+
+inline auto makeEntity(
+ const std::string & name, const EntityType::Enum type, const geometry_msgs::msg::Pose & pose,
+ const geometry_msgs::msg::Vector3 & dimensions) -> EntityStatus
+{
+ return createEntityStatus(name, type, std::nullopt, pose, dimensions);
+}
+
} // namespace utils
#endif // SIMPLE_SENSOR_SIMULATOR__TEST__UTILS__HELPER_FUNCTIONS_HPP_